多輸入融合卡爾曼濾波算法在礦井定位中的優化研究*

劉 云, 房飛翔

(昆明理工大學 信息工程與自動化學院，云南 昆明 650500)

0 引 言

礦井定位對于保障礦井安全生產是一個非常重要的研究領域。由于礦井特殊的物理環境，在目前的礦井無線定位系統中，不規則的隧道和粗糙的墻壁通常會造成多徑干擾[1]和非線性視距(non-line-of-sight，NLOS)誤差[2]。因此，提高礦井無線定位系統的抗干擾能力顯得尤為重要。

Colombo A等人[3]提出了將一個由加速度計、陀螺儀、磁力計組成的可穿戴的慣性測量單元(inertial measurement unit,IMU)融合無線定位技術,佩戴于工作人員的腰部跟蹤其行動。IMU相比普通礦井無線定位系統的關鍵優勢是其具有高度的可移植性,提供慣性數據的更新率高,以及不受信號阻塞,但僅靠IMU定位估計，由于傳感器偏差的不穩定性會導致慣性漂移的積累[4]。Fourati H[5]提出了零速度更新(zero-speed update,ZUPT)通過與地面接觸以糾正測速誤差，但基于總步行距離，漂移仍將增長。

劉明偉等人[6]提出了低功耗藍牙(Bluetooth low energy,BLE)技術，其中的接收信號強度指示(received signal strength indication,RSSI)可以被用來估計接收機相對發射機的距離。通過多組到固定信標節點(發射機)的距離測量值，三邊測量法可以用來估計目標節點(接收機)的絕對位置，但定位精度會遭受NLOS和多徑干擾的影響。Simon D等人[7]提出了IMU和BLE測量可被融合，以在一個高更新率下準確估計目標節點的無漂移位置。卡爾曼濾波是一種廣泛使用的算法，用來從帶有高斯白噪聲的多組傳感器測量值中估計一個系統的狀態。但基于來自于多徑和NLOS的頻繁異常值,無線跟蹤系統的測量噪聲不能作為高斯分布進行建模。Hassani V等人[8]已經解決了該問題，通過引入自適應卡爾曼濾波器(Kalman filter.KF),如多模自適應估計(multimodel adaptive estimation,MMAE)和基于創新的自適應估計(innovation-based adaptive estimation,IAE),容納動態條件的變化。MMAE基于有著不同過程和測量噪聲矩陣的多個KF進行狀態估計,但同時運行多個KF的高計算成本使得其不適合用于可穿戴設備中的低成本的微控制器。IAE適應基于新息序列的新的過程和測量噪聲協方差,但其估計值會發散。

本文提出了一種多輸入融合卡爾曼濾波(multiple input-fusion Kalman filtering,MIFKF)算法,在存在異常值的情況下準確估計傳感器的位置。算法中，位置通過融合IMU中的外部加速度和BLE的三邊測量進行估計。使用估計的翻滾角，俯仰角和偏航角，加速度測量值從移動傳感器旋轉到固定導航坐標系中。三邊測量估計的權值基于距離測量和三邊測量估計之間的余值自適應設置。定位精度將隨著Rauch-Tung-Striebel (RTS)平滑器進一步提高。實驗結果表明：該余值與三邊測量估計誤差有著很強的相關性，MIFKF算法相比標準KF在應對異常值時能夠更準確地估算出位置。

1 算法模型

1.1 慣性測量預處理

MIFKF算法整體框架如圖1所示，其中,預處理是通過將傳感器坐標系S中的慣性測量值輸入一個傾斜和偏航KF估計α(偏航角),β(俯仰角)和γ(翻滾角),即關于固定導航坐標系N中Z軸，Y軸和X軸的旋轉角度。算法見文獻[9]。

圖1 MIFKF算法流程

1.2 三邊測量估計

通過假設接收機與發射機具有全向天線和發射機具有恒定傳輸能量,接收功率[10,11]Pr(即RSSI)在接收機的值由式(1)決定

Pr=B-algd+χ

(1)

式中B為功率抵消常數;a為一個環境變量;d為發射機和接收機之間的距離；χ為隨機噪聲。在固定環境中，設a為一個常數，d根據Pr，a和B估計得到

d=10Pr-B/a

(2)

(3)

(4)

距離估計值di和到目標節點位置x的距離之間的余值可以按式(5)計算

(5)

余值ri=[ri,xri,y]T在X軸和Y軸上的距離di依次由式(6)和式(7)決定

ri=[|xi-x||y-yi|]T

(6)

(7)

總體余值r=[rxry]T在X軸和Y軸上為多組ri的平均值

(8)

2 MIFKF算法實現

在融合KF中,目標節點的位置由外部加速度、慣性測量預處理值和三邊測量融合估計。該KF由以下方程控制

xt=At-1xt-1+Bt-1ut-1+wt-1

(9)

zt=Ctxt+vt

(10)

式中xt為由目標節點的位置和速度組成的狀態向量；At-1和Bt-1為狀態轉換和輸入矩陣；ut-1為輸入向量；wt-1為過程噪聲向量；zt為測量值向量；Ct為觀測矩陣；vt為測量噪聲向量。變量定義如下

(11)

(12)

(13)

(14)

Ct-1=[I2×2O2×2]

(15)

(16)

zt=xBLE,t

(17)

(18)

(19)

為了后期處理,RTS平滑器[12]被用來提高前期多融合KF位置估計后的定位精度，如下

(20)

(21)

(22)

(23)

3 仿真與分析

為了說明MIFKF算法的優越性，本文在MATLAB軟件中對礦井環境進行模擬分析。如圖2所示，為模擬礦井巷道的一段截取部分，假設該段巷道寬為5m，長為30m，巷道中以近似正三角形的方式布有8個信標節點，信標節點兩兩之間的距離約為8.6m。假設目標節點在巷道中以平均2m/s的速度做往返變速直線運動，每次模擬，目標節點持續運動30s，模擬測試重復10次。其中，信標節點的發射功率設為23dBm。目標節點在不同的時間接收每個信標節點的RSSI采樣頻率約為80Hz，IMU采樣頻率為100Hz。每個信標節點在使用三邊測量時的RSSIs平均為10Hz。系統采樣頻率為100Hz。

圖2 礦井巷道節點分布

MIFKF算法在本次模擬的仿真結果將分別與理想值、BLE三邊測量算法以及標準KF算法進行對比分析，如圖3～圖7所示。

如圖3(a)所示，在Y軸上比較了BLE三邊測量算法、MIFKF算法與理想值。圖中顯示大部分的三邊測量估計值在t=78～80s和108～120s時偏離了參考軌跡,導致平均誤差為1.091m。表明MIFKF算法相比三邊測量估計算法處理異常值效果明顯更好。圖3(a)與圖3(b)比較表明,仿真中余值與三邊測量估計Y軸絕對誤差保持很強的相關性。且最大的三邊測量誤差在108.5s時的3.76m,余值為3.55m。

圖3 BLE三邊測量和MIFKF算法仿真結果

如圖4所示為所有10次測試中三邊測量誤差對應的余值。該圖顯示了三邊測量在X軸和Y軸上的誤差值與余值存在著很強的相關性，且對應相關系數分別為0.897和0.952。印證了圖3(b)的結論，同時表明余值可以很好地提供三邊測量的可靠性估計。

圖4 三邊測量估計關于余值在X軸和Y軸上的絕對誤差

為了表現出MIFKF算法在礦井定位中的優越性,MIFKF算法的性能將在Y軸上與標準KF進行比較。標準KF設定一個常數三邊測量的測量噪聲。噪聲方差(Rt的對角元素)在X軸Y軸上調整為9×10-4m2。

如圖5(a)所示，標準KF在t=79～82s和t=108～110s分別產生一個較大的平均誤差為1.281m和1.192m，相應時段MIFKF的平均誤差分別為0.653m和0.718m。此外，在拒絕異常值后，標準KF定位跟蹤在單位時間間隔下的平均誤差為0.774m，而MIFKF為0.531m。

圖6比較了標準KF算法與MIFKF算法下的卡爾曼增益。從圖中可知：MIFKF相對于標準KF有較低的平均卡爾曼增益為0.0013，而標準KF則為0.0451，MIFKF算法下卡爾曼增益越小表明在其算法中IMU測量相比三邊測量所占權重越大。

圖7(a)給出了標準KF與MIFKF算法在模擬礦井環境下t=78～109.8s時的定位跟蹤軌跡。從圖中可以發現標準KF的漂移現象更嚴重，且MIFKF算法相較標準KF的定位精度明顯更高。

圖5 標準KF和MIFKF算法仿真結果

圖6 標準KF與MIFKF算法上的卡爾曼增益

圖7 標準KF與MIFKF算法模擬礦井跟蹤軌跡

圖5(b)和圖7(b)表明:RTS平滑器可以進一步提高定位性能。但在t=78～80s時標準KF仍然存在小漂移。

表1比較了每次測試中標準KF算法和MIFKF算法定位跟蹤的均方根誤差(root mean square error,RMSE)。MIFKF算法的定位精度相比BLE三邊測量和標準KF分別提高了44.1%和26.5%。在采用平滑器時，MIFKF算法相比有平滑器的標準KF和沒有平滑器的MIFKF算法，其定位精度分別進一步提高了13.6%和29.1%。

4 結 論

針對礦井無線定位異常值的處理，提出了一種優化的用于礦井工作人員和機車準確估計位置軌跡的(MIFKF)算法，算法通過對IMU測量值進行預處理并融合BLE三邊測量值進行濾波優化處理，同時濾波器參數可自動調節。實驗結果表明：MIFKF算法相對于三邊測量和標準KF在礦井復雜環境中有效減小定位誤差，并且在使用RTS平滑處理后準確性得到了進一步提高。

表1 定位跟蹤的RMSE cm

[1] 霍 羽，房咪咪，劉逢雪.煤礦巷道壁粗糙度對電磁波傳播的影響[J].工礦自動化，2013,39(3)：43-45.

[2] 蘇應敢，張林山，姚 建.一種考慮非視距傳播的UWB定位算法[J].云南電力技術,2015,43(6)：95-99.

[3] Colombo A,Fontanelli D,Macii D.Flexible indoor localization and tracking based on a wearable platform and sensor data fusi-on[J].IEEE Trans on Instrum Meas,2014，63(4)：864-876.

[4] 吳哲明,孫振國,張文增.基于慣性測量單元旋轉的陀螺漂移估計和補償方法[J].清華大學學報：自然科學版,2014,54(9)：1143-1147.

[5] Fourati H.Heterogeneous data fusion algorithm for pedestrian navigation via foot-mounted inertial measurement unit and complementary filter[J].IEEE Trans on Instrum Meas,2015,64(1)：221-229.

[6] 劉明偉,劉太君,葉 焱.基于低功耗藍牙技術的室內定位應用研究[J].無線通信技術,2015(3)：19-24.

[7] Simon D.Kalman filtering with state constraints:A survey of linear and nonlinear algorithms[J].IET Control Theory & Applications,2010,4(8)：1303-1318.

[8] Hassani V.Multiple model adaptive wave filtering for dynamic positioning of marine vessels[C]∥American Control Conference,2012：6222-6228.

[9] Zihajehzadeh S,Loh D,Lee M.A cascaded two-step Kalman filter for estimation of human body segment orientation using MEMS-IMU[C]∥Proc of IEEE Engineering in Medicine & Biology Society,2014：6270-6273.

[10] 劉 云,房飛翔.基于LO-EKF算法對礦井人員定位跟蹤的優化研究[J].云南大學學報：自然科學版,2016,38(3)：392-398.

[11] 熊志廣,石為人,許 磊.基于加權處理的三邊測量定位算法[J].計算機工程與應用,2010,46(22)：99-102.

[12] 鄒 娟.無線傳感網絡中基于RTS平滑的目標跟蹤技術研究[D].湘潭：湘潭大學,2014.