磁浮管道物流系統智能測速算法研究

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

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

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

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

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

1 多普勒雷達測速原理

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

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

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

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

回波與發射波之間的頻率信號差為：

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

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

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

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

系統的狀態方程與量測方程如下：

2 Sage-Husa 自適應濾波算法及其改進

2.1 Sage-Husa自適應濾波算法及改進方案

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

Sage-Husa自適應濾波算法如下。

先驗估計：

先驗誤差協方差：

殘差計算：

卡爾曼增益：

后驗估計：

后驗誤差協方差：

系統噪聲均值：

系統噪聲協方差：

量測噪聲均值：

量測噪聲協方差：

式(8)～式(17)中，Φk為狀態轉移矩陣，為系統噪聲協方差矩陣，Zk+1為k+1 時刻的量測值，+1為量測噪聲協方差矩陣，H為量測矩陣，dk(0

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

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

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

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

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

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

2.2 引入模糊推理系統

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

殘差協方差理論值Lk：

模糊系統的輸入A：

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

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

A和α的隸屬度函數如圖4所示。

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

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

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

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

3 實驗分析

3.1 聯合仿真模型

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

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

圖6 仿真實驗模型框圖Fig.6 Block diagram of simulation experiment model

3.2 結果對比與分析

設定系統噪聲Q=0.2 初始量測噪聲R=0.2，仿真實驗初始條件：加速度a=0.5tm/s2，初速度v0=0 m/s，施加=5R的高斯白噪聲，3 種卡爾曼濾波器的仿真速度數據如圖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 中可以看出，當實際量測噪聲大于系統設置量測噪聲時，傳統卡爾曼濾波不能較好地跟蹤真實數據，精度及平穩性都較差，新型Sage-Husa自適應濾波效果最好，Sage-Husa自適應濾波器次之。從圖9 可知，3 種卡爾曼濾波器的估計速度與實際速度最大偏差依次為2.25，1.17 和0.64 m，最大誤差率依次為12.3%，6.39%，1.42%，相比于傳統卡爾曼濾波器、Sage-Husa 自適應濾波器，新型Sage-Husa 自適應濾波器精度分別提高10.88%和4.97%。

針對測速數據失常情況，對仿真數據施加隨機的突變干擾，并設置實際量測噪聲協方差=5R，使用3 種卡爾曼濾波仿真失常速度數據如圖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 中可以看出，傳統的卡爾曼濾波器無法剔除失常數據的影響，Sage-Husa 自適應濾波器雖然對失常的量測數據有一定的補償作用，但是估計值無法快速收斂到真實值附近，且失常數據與真實值之間差值過大時，會導致Sage-Husa自適應濾波的發散。而新型Sage-Husa 自適應濾波器能夠很好地剔除失常數據的影響，且能夠較快地收斂到真實值附近。

由于磁浮管道物流系統仍處于研發階段，尚不具備實驗條件，本文選在懸浮原理及驅動方式與磁浮管道物流系統類似且技術成熟的“紅軌”上進行測速實驗?！凹t軌”屬于中低速永磁磁浮軌道交通試驗線，線路全長800 m，載客能力88 人，最高設計運行速度80 km/h[21]。設定程序系統噪聲Q=0.2，初始量測噪聲R=0.2，對速度數據施加=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 中可以看出，在實際車載數據實驗中，相較于前2 種濾波算法，新型Sage-Husa自適應濾波器依然保持了很好的估計精度與平穩性，從圖15可知，3種卡爾曼濾波器的估計速度與實際速度最大偏差依次為2.4，1.2 和0.68 m，最大誤差率依次為14.6%，7.9%，4.3%，相比于傳統的卡爾曼濾波器和Sage-Husa 自適應濾波器，新型Sage-Husa 自適應濾波器精度分別提高10.3%和3.6%。通過實測車載測速數據濾波實驗進一步驗證了新型Sage-Husa 自適應濾波算法的優越性。

4 結論

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

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

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