程浪 ，楊杰 ，豐富 ，高濤，齊洪峰，邵福波

(1.江西省磁懸浮技術(shù)重點實驗室，江西 贛州 341000；2.中國科學院 贛江創(chuàng)新研究院，江西 贛州 341000；3.中車工業(yè)研究院有限公司，北京 100076)

永磁懸浮管道物流系統(tǒng)[1]采用永磁懸浮技術(shù)，可實現(xiàn)“零功率”懸浮，依托綜合管廊進行物流運輸，安全高效，不占用地面空間，發(fā)展?jié)摿薮?。永磁懸浮管道物流系統(tǒng)模型如圖1所示。測速定位技術(shù)[2]是管道物流運輸系統(tǒng)的關(guān)鍵技術(shù)，對于物流運輸系統(tǒng)的安全預(yù)警、運行控制、運輸效率具有重要意義。目前常采用多普勒雷達實現(xiàn)永磁懸浮管道車輛速度測量，并對速度數(shù)據(jù)進行積分得到列車的位移。但雷達信號設(shè)備易受強磁場干擾[3]、氣流沖擊、車輛振動以及信號覆蓋角移位等問題的影響，導致量測噪聲的統(tǒng)計特性發(fā)生變化，且易產(chǎn)生遠大于或小于列車真實運行速度的失常數(shù)據(jù)[4]。隨著發(fā)車間隔[5]的不斷壓縮，以及列車編隊技術(shù)的需要，對列車測速定位技術(shù)的測量精度以及安全性方面提出了更高的要求?；谧顑?yōu)估計理論的數(shù)據(jù)濾波、融合處理，降低了因傳感器缺陷所導致的誤差，從而提升測速定位的性能[6]?？柭鼮V波是一種基于最優(yōu)估計理論的優(yōu)化算法，它可以通過分析帶有噪聲和擾動的輸入數(shù)據(jù)對系統(tǒng)的狀態(tài)進行最優(yōu)估計[7]。在實際應(yīng)用中，量測數(shù)據(jù)的噪聲統(tǒng)計特性易受復雜工況影響發(fā)生變化，使得常規(guī)卡爾曼濾波算法受到極大限制。Saga-Husa 自適應(yīng)濾波算法可以對噪聲的統(tǒng)計特性進行實時估計和調(diào)整[8]，在量測噪聲統(tǒng)計特性發(fā)生變化時依然能夠?qū)崿F(xiàn)最優(yōu)估計，但存在計算量較大和容易發(fā)散等缺點[9-10]。針對此類問題，文海平等[11]提出一種基于新息檢測的自適應(yīng)抗差卡爾曼濾波算法，該算法通過引進抗差因子和自適應(yīng)因子對量測噪聲協(xié)方差矩陣和狀態(tài)預(yù)測協(xié)方差矩陣進行調(diào)整，提高了組合導航的精度。王廣玉等[12]利用殘差協(xié)方差對量測噪聲統(tǒng)計特性進行實時修正，進而調(diào)整卡爾曼濾波增益，獲得更好跟蹤效果。孫旭等[13]采用一種極大后驗噪聲估計器對量測噪聲進行在線修正，避免系統(tǒng)發(fā)生濾波發(fā)散的問題，并減小了量測噪聲的估計誤差。石青[14]利用自適應(yīng)卡爾曼濾波器的殘差信息結(jié)合模糊推理系統(tǒng)得出位置和姿態(tài)調(diào)節(jié)因子，對濾波器的噪聲統(tǒng)計特性進行在線修正，解決量測噪聲變化不規(guī)律的問題。針對復雜工況下測速數(shù)據(jù)的量測噪聲統(tǒng)計特性發(fā)生變化的問題，本文以永磁懸浮管道物流系統(tǒng)的測速定位技術(shù)為研究背景，提出一種新型Sage-Husa自適應(yīng)濾波算法，在Sage-Husa自適應(yīng)濾波算法的基礎(chǔ)上進行簡化，并引入模糊推理系統(tǒng)，對量測噪聲協(xié)方差進行在線修正，同時設(shè)置了失常數(shù)據(jù)判定閾值，對失常數(shù)據(jù)進行補償，進一步提高濾波器的速度估計精度，通過仿真及實測車載數(shù)據(jù)對傳統(tǒng)卡爾曼濾波、Sage-Husa 自適應(yīng)濾波以及新型Sage-Husa 自適應(yīng)濾波算法進行對比，驗證了新型Sage-Husa 自適應(yīng)濾波算法具有更高的速度估計精度及對失常數(shù)據(jù)的補償能力。

圖1 永磁懸浮管道物流系統(tǒng)Fig.1 Permanent magnet suspension pipeline logistics system

圖2 多普勒雷達測速原理示意圖Fig.2 Schematic diagram of Doppler radar velocity measurement

1 多普勒雷達測速原理

多普勒雷達以多普勒效應(yīng)為基礎(chǔ)，通過檢測發(fā)射波與回波之間的頻率信號差來計算列車相對于地面的速度值[15]，可實現(xiàn)無接觸式速度測量[16]，為永磁懸浮管道物流系統(tǒng)的測速定位問題提供了技術(shù)參考。

設(shè)車體行駛速度為V，雷達發(fā)射波頻率為fT，發(fā)射波與水平方向夾角為α1，反射波與水平方向夾角為α2，電磁波速度為c，則根據(jù)多普勒效應(yīng)，測速反射面O接收到的電磁波頻率為：

雷達接收到的回波頻率為：

將式(1)代入式(2)可得：

回波與發(fā)射波之間的頻率信號差為：

從而在知道多普勒雷達發(fā)射波頻率fT并檢測到發(fā)射波與回波之間的頻率信號差fe的條件下即可求出當前車體速度V：

以圖1為實驗樣車，多普勒雷達安裝在永磁懸浮管道物流系統(tǒng)運輸車體的頂部，采集小車速度數(shù)據(jù)。永磁懸浮管道物流運輸系統(tǒng)測速示意圖如圖3所示。

圖3 管道物流運輸系統(tǒng)測速示意圖Fig.3 Schematic diagram of speed measurement of pipeline logistics transportation system

選取管軌小車速度v與位移s作為系統(tǒng)的狀態(tài)變量，X=[v,s]T，設(shè)管道內(nèi)小車以初速度為v0=0 m/s，加速度為a，采樣時間間隔為Δt，k時刻系統(tǒng)的運動學方程如下：

系統(tǒng)的狀態(tài)方程與量測方程如下：

2 Sage-Husa 自適應(yīng)濾波算法及其改進

2.1 Sage-Husa自適應(yīng)濾波算法及改進方案

Sage-Husa 自適應(yīng)濾波算法是一種基于極大后驗噪聲統(tǒng)計估值器的自適應(yīng)濾波算法[17]。其在卡爾曼濾波算法的基礎(chǔ)上，通過傳感器獲取的數(shù)據(jù)和系統(tǒng)的先驗估計值對系統(tǒng)噪聲和量測噪聲統(tǒng)計特性在線修正，以解決噪聲變化不規(guī)律的問題。

Sage-Husa自適應(yīng)濾波算法如下。

先驗估計：

先驗誤差協(xié)方差：

殘差計算：

卡爾曼增益：

后驗估計：

后驗誤差協(xié)方差：

系統(tǒng)噪聲均值：

系統(tǒng)噪聲協(xié)方差：

量測噪聲均值：

量測噪聲協(xié)方差：

式(8)～式(17)中，Φk為狀態(tài)轉(zhuǎn)移矩陣，為系統(tǒng)噪聲協(xié)方差矩陣，Zk+1為k+1 時刻的量測值，+1為量測噪聲協(xié)方差矩陣，H為量測矩陣，dk(0

Sage-Husa 自適應(yīng)濾波算法雖然可以對噪聲統(tǒng)計特性進行實時修正，但其算法運算量也相應(yīng)增加，同時在濾波過程中易發(fā)生發(fā)散的問題[18]。因而，對Sage-Husa 自適應(yīng)濾波算法做如下改進：

1) Sage-Husa 自適應(yīng)濾波算法不能同時對系統(tǒng)噪聲協(xié)方差陣和量測噪聲協(xié)方差陣進行估計[19]。故采用先驗信息給出的初始設(shè)定值Q作為系統(tǒng)噪聲協(xié)方差陣，不進行自適應(yīng)修正，對算法進行簡化，以減小運算量。即去掉式(14)與式(15)，并對式(8)與式(9)分別修改為：

2) 采用量測噪聲協(xié)方差陣的漸進無偏估計對其進行簡化，并引入模糊調(diào)節(jié)因子α對量測噪聲進行調(diào)節(jié)，實現(xiàn)對量測噪聲協(xié)方差陣的實時修正。此時式(17)成為

模糊推理系統(tǒng)以殘差協(xié)方差的實際值和理論值之比A作為輸入量，調(diào)節(jié)因子α為輸出量作用于量測噪聲協(xié)方差，對進行實時修正。

3) 針對多普勒雷達測速過程中可能產(chǎn)生的失常數(shù)據(jù)，設(shè)置失常數(shù)據(jù)判定閾值F，當量測值與系統(tǒng)狀態(tài)預(yù)測值差值的絕對值f小于閾值F，則認為量測數(shù)據(jù)是正常數(shù)據(jù)，卡爾曼增益K仍按式(11)計算；當f超過閾值F，則認為量測數(shù)據(jù)大概率是失常數(shù)據(jù)，此時應(yīng)自適應(yīng)調(diào)節(jié)卡爾曼增益K，式(11)改為：

式(21)中，當量測值與系統(tǒng)狀態(tài)預(yù)測值之間差值越大，卡爾曼增益K將呈指數(shù)級減小，此時對量測數(shù)據(jù)的干預(yù)程度也就越強，分配給量測值的權(quán)值也就越小，相應(yīng)的分配給狀態(tài)預(yù)測值的權(quán)值越大，系統(tǒng)估計值就越不相信量測值，越偏向于狀態(tài)預(yù)測值，從而實現(xiàn)對量測失常數(shù)據(jù)的補償。

2.2 引入模糊推理系統(tǒng)

當多普勒雷達在理想工作狀態(tài)下進行測速時，設(shè)定好噪聲參數(shù)的卡爾曼濾波器能夠濾除測量噪聲的干擾，實現(xiàn)對速度的最優(yōu)估計。但當多普勒雷達在復雜的工況下運行，導致系統(tǒng)的量測噪聲統(tǒng)計特性發(fā)生變化與設(shè)定的噪聲參數(shù)不匹配時，傳統(tǒng)卡爾曼濾波器難以預(yù)測出小車的真實速度。因此，在量測噪聲統(tǒng)計特性發(fā)生變化時，需要對卡爾曼濾波器的量測噪聲參數(shù)進行自適應(yīng)調(diào)節(jié)以匹配真實的量測噪聲狀況。選取n個時刻的實際殘差rk，求出其均值rˉ和方差Sk，如下所示：

殘差協(xié)方差理論值Lk：

模糊系統(tǒng)的輸入A：

式(22)～(25)中，Hk為量測矩陣，Pk+1,k為先驗誤差協(xié)方差矩陣，量測噪聲協(xié)方差矩陣。

模糊控制以殘差實際方差和理論方差的比值A(chǔ)為輸入量，量測噪聲調(diào)節(jié)因子α為輸出量，當A偏離1 時，通過調(diào)節(jié)因子α調(diào)整可以改變Lr的大小，從而使A回到1附近。定義模糊子集M表示在1 附近，B表示大于1，S表示小于1。A的論域為[0.5,1.5]，α的論域為[0.9,2.3]，根據(jù)經(jīng)驗與仿真調(diào)試構(gòu)建模糊規(guī)則表[20]，模糊控制規(guī)則可表示為：

A和α的隸屬度函數(shù)如圖4所示。

圖4 模糊推理系統(tǒng)輸入輸出的隸屬度函數(shù)Fig.4 Membership function of input and output of fuzzy reasoning system

新型Sage-Husa 自適應(yīng)濾波算法流程可描述為通過多普勒雷達獲取量測數(shù)據(jù)Zk+1，同時根據(jù)系統(tǒng)上一次后驗估計Xk,k，Pk,k計算系統(tǒng)的先驗估計Xk+1,k和Pk+1,k，并計算殘差vk+1和量測值與系統(tǒng)狀態(tài)預(yù)測值差值的絕對值f；通過失常數(shù)據(jù)閾值判定計算出對應(yīng)的卡爾曼增益Kk+1，然后計算殘差協(xié)方差的實際值與理論值之比A，將其作為模糊控制系統(tǒng)的輸入，得到模糊控制系統(tǒng)的輸出α，對量測噪聲協(xié)方差進行在線調(diào)節(jié)，最后得出系統(tǒng)的后驗估計Xk+1,k+1和Pk+1,k+1，并更新為下一次先驗估計的輸入。

新型Sage-Husa 自適應(yīng)濾波算法流程框圖如圖5所示。

圖5 新型Sage-Husa自適應(yīng)濾波算法流程框圖Fig.5 Flow block diagram of the new Sage-Husa adaptive filtering algorithm

3 實驗分析

3.1 聯(lián)合仿真模型

本文在Matlab/simulink 仿真軟件中，采用SFuction 函數(shù)模塊實現(xiàn)新型Sage-Husa自適應(yīng)濾波程序開發(fā)，采用Fuzzy 模塊構(gòu)造模糊控制算法，并搭建基于新型Sage-Husa 自適應(yīng)濾波器、傳統(tǒng)卡爾曼濾波器和Sage-Husa 濾波器的仿真模型。通過仿真獲取測速數(shù)據(jù)，對數(shù)據(jù)施加設(shè)定的量測噪聲，將其作為3種卡爾曼濾波器的量測信號輸入，分別通過3種濾波器得到各自的系統(tǒng)預(yù)測值輸出，同時當前得到的系統(tǒng)預(yù)測值也將作為下一次濾波更新各自系統(tǒng)的先驗估計值輸入。

仿真模型框圖如圖6所示。

圖6 仿真實驗?zāi)Ｐ涂驁DFig.6 Block diagram of simulation experiment model

3.2 結(jié)果對比與分析

設(shè)定系統(tǒng)噪聲Q=0.2 初始量測噪聲R=0.2，仿真實驗初始條件：加速度a=0.5tm/s2，初速度v0=0 m/s，施加=5R的高斯白噪聲，3 種卡爾曼濾波器的仿真速度數(shù)據(jù)如圖7所示。

圖7 仿真速度對比Fig.7 Comparison diagram of simulation speed

仿真速度誤差對比如圖8所示。

圖8 仿真速度誤差對比Fig.8 Comparison of simulation speed errors

仿真速度最大誤差對比如圖9所示。

圖9 仿真速度最大誤差對比Fig.9 Comparison of maximum error of simulation speed

從圖7 和圖8 中可以看出，當實際量測噪聲大于系統(tǒng)設(shè)置量測噪聲時，傳統(tǒng)卡爾曼濾波不能較好地跟蹤真實數(shù)據(jù)，精度及平穩(wěn)性都較差，新型Sage-Husa自適應(yīng)濾波效果最好，Sage-Husa自適應(yīng)濾波器次之。從圖9 可知，3 種卡爾曼濾波器的估計速度與實際速度最大偏差依次為2.25，1.17 和0.64 m，最大誤差率依次為12.3%，6.39%，1.42%，相比于傳統(tǒng)卡爾曼濾波器、Sage-Husa 自適應(yīng)濾波器，新型Sage-Husa 自適應(yīng)濾波器精度分別提高10.88%和4.97%。

針對測速數(shù)據(jù)失常情況，對仿真數(shù)據(jù)施加隨機的突變干擾，并設(shè)置實際量測噪聲協(xié)方差=5R，使用3 種卡爾曼濾波仿真失常速度數(shù)據(jù)如圖10所示。

圖10 仿真失常速度對比Fig.10 Comparison of simulation aberration velocity

仿真失常速度誤差對比如圖11所示。

圖11 仿真失常速度誤差對比Fig.11 Comparison of simulation aberration velocity errors

仿真失常速度最大誤差對比如圖12所示。

圖12 仿真失常速度最大誤差對比Fig.12 Comparison of maximum error of simulation abnormal velocity

從圖10 和圖11 中可以看出，傳統(tǒng)的卡爾曼濾波器無法剔除失常數(shù)據(jù)的影響，Sage-Husa 自適應(yīng)濾波器雖然對失常的量測數(shù)據(jù)有一定的補償作用，但是估計值無法快速收斂到真實值附近，且失常數(shù)據(jù)與真實值之間差值過大時，會導致Sage-Husa自適應(yīng)濾波的發(fā)散。而新型Sage-Husa 自適應(yīng)濾波器能夠很好地剔除失常數(shù)據(jù)的影響，且能夠較快地收斂到真實值附近。

由于磁浮管道物流系統(tǒng)仍處于研發(fā)階段，尚不具備實驗條件，本文選在懸浮原理及驅(qū)動方式與磁浮管道物流系統(tǒng)類似且技術(shù)成熟的“紅軌”上進行測速實驗。“紅軌”屬于中低速永磁磁浮軌道交通試驗線，線路全長800 m，載客能力88 人，最高設(shè)計運行速度80 km/h[21]。設(shè)定程序系統(tǒng)噪聲Q=0.2，初始量測噪聲R=0.2，對速度數(shù)據(jù)施加=5R的高斯白噪聲，輸入3種卡爾曼濾波器中進行仿真，得到的實測車載速度對比如圖13所示。

圖13 實測車載速度對比Fig.13 Comparison of actual vehicle speed

實測車載速度誤差對比如圖14所示。

圖14 實測車載速度誤差對比Fig.14 Comparison of actual vehicle speed errors

實測車載速度最大誤差對比如圖15所示。

圖15 實測車載速度最大誤差對比Fig.15 Comparison of the maximum error of actual vehicle speed

從圖13 和圖14 中可以看出，在實際車載數(shù)據(jù)實驗中，相較于前2 種濾波算法，新型Sage-Husa自適應(yīng)濾波器依然保持了很好的估計精度與平穩(wěn)性，從圖15可知，3種卡爾曼濾波器的估計速度與實際速度最大偏差依次為2.4，1.2 和0.68 m，最大誤差率依次為14.6%，7.9%，4.3%，相比于傳統(tǒng)的卡爾曼濾波器和Sage-Husa 自適應(yīng)濾波器，新型Sage-Husa 自適應(yīng)濾波器精度分別提高10.3%和3.6%。通過實測車載測速數(shù)據(jù)濾波實驗進一步驗證了新型Sage-Husa 自適應(yīng)濾波算法的優(yōu)越性。

4 結(jié)論

1) 針對管道物流運輸系統(tǒng)多普勒雷達測速定位模塊在復雜工況中量測噪聲統(tǒng)計特性變化及數(shù)據(jù)失常的問題，提出一種新型Sage-Husa 自適應(yīng)濾波算法。該算法對Sage-Husa 自適應(yīng)濾波算法進一步優(yōu)化，通過引入模糊推理系統(tǒng)對量測噪聲的增益系數(shù)進行調(diào)節(jié)，實現(xiàn)對量測噪聲的實時修正；通過失常數(shù)據(jù)判定閾值調(diào)節(jié)卡爾曼增益，實現(xiàn)對失常數(shù)據(jù)的補償。

2) 搭建仿真模型對新型Sage-Husa 自適應(yīng)濾波器進行驗證，在給定實際量測噪聲=5R，數(shù)據(jù)正常的情況下，傳統(tǒng)卡爾曼濾波器、Sage-Husa 自適應(yīng)濾波器，新型Sage-Husa 自適應(yīng)濾波器的最大誤差率依次為12.3%，6.39%，1.42%，與前2 種卡爾曼濾波器相比，新型Sage-Husa 自適應(yīng)濾波器精度分別提高10.88%和4.97%；在數(shù)據(jù)失常的情況下，相較于前2 種卡爾曼濾波器，新型Sage-Husa自適應(yīng)濾波器能夠很好地剔除失常數(shù)據(jù)的影響，且能較快收斂到真實值附近。

3) 采用實測車載數(shù)據(jù)對新型Sage-Husa 自適應(yīng)濾波算法進一步驗證，傳統(tǒng)卡爾曼濾波器、Sage-Husa自適應(yīng)濾波器和新型Sage-Husa自適應(yīng)濾波器的最大誤差率依次為14.6%，7.9%和4.3%，與前2種卡爾曼濾波器相比，新型Sage-Husa 自適應(yīng)濾波器精度分別提高10.3%和3.6 %，進一步驗證了新型Sage-Husa 自適應(yīng)濾波算法具有更高的精度和對失常數(shù)據(jù)的補償能力，對于提高磁浮管道物流運輸系統(tǒng)測速定位技術(shù)的精度和可靠性具有重要的學術(shù)意義，同時，本方法對其他軌道交通制式的測速定位也具有重要的參考價值。