劉 云, 房飛翔

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

0 引 言

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

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

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

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

1 算法模型

1.1 慣性測(cè)量預(yù)處理

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

圖1 MIFKF算法流程

1.2 三邊測(cè)量估計(jì)

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

Pr=B-algd+χ

(1)

式中B為功率抵消常數(shù);a為一個(gè)環(huán)境變量;d為發(fā)射機(jī)和接收機(jī)之間的距離；χ為隨機(jī)噪聲。在固定環(huán)境中，設(shè)a為一個(gè)常數(shù)，d根據(jù)Pr，a和B估計(jì)得到

d=10Pr-B/a

(2)

(3)

(4)

距離估計(jì)值di和到目標(biāo)節(jié)點(diǎn)位置x的距離之間的余值可以按式(5)計(jì)算

(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算法實(shí)現(xiàn)

在融合KF中,目標(biāo)節(jié)點(diǎn)的位置由外部加速度、慣性測(cè)量預(yù)處理值和三邊測(cè)量融合估計(jì)。該KF由以下方程控制

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

(9)

zt=Ctxt+vt

(10)

式中xt為由目標(biāo)節(jié)點(diǎn)的位置和速度組成的狀態(tài)向量；At-1和Bt-1為狀態(tài)轉(zhuǎn)換和輸入矩陣；ut-1為輸入向量；wt-1為過程噪聲向量；zt為測(cè)量值向量；Ct為觀測(cè)矩陣；vt為測(cè)量噪聲向量。變量定義如下

(11)

(12)

(13)

(14)

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

(15)

(16)

zt=xBLE,t

(17)

(18)

(19)

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

(20)

(21)

(22)

(23)

3 仿真與分析

為了說明MIFKF算法的優(yōu)越性，本文在MATLAB軟件中對(duì)礦井環(huán)境進(jìn)行模擬分析。如圖2所示，為模擬礦井巷道的一段截取部分，假設(shè)該段巷道寬為5m，長(zhǎng)為30m，巷道中以近似正三角形的方式布有8個(gè)信標(biāo)節(jié)點(diǎn)，信標(biāo)節(jié)點(diǎn)兩兩之間的距離約為8.6m。假設(shè)目標(biāo)節(jié)點(diǎn)在巷道中以平均2m/s的速度做往返變速直線運(yùn)動(dòng)，每次模擬，目標(biāo)節(jié)點(diǎn)持續(xù)運(yùn)動(dòng)30s，模擬測(cè)試重復(fù)10次。其中，信標(biāo)節(jié)點(diǎn)的發(fā)射功率設(shè)為23dBm。目標(biāo)節(jié)點(diǎn)在不同的時(shí)間接收每個(gè)信標(biāo)節(jié)點(diǎn)的RSSI采樣頻率約為80Hz，IMU采樣頻率為100Hz。每個(gè)信標(biāo)節(jié)點(diǎn)在使用三邊測(cè)量時(shí)的RSSIs平均為10Hz。系統(tǒng)采樣頻率為100Hz。

圖2 礦井巷道節(jié)點(diǎn)分布

MIFKF算法在本次模擬的仿真結(jié)果將分別與理想值、BLE三邊測(cè)量算法以及標(biāo)準(zhǔn)KF算法進(jìn)行對(duì)比分析，如圖3～圖7所示。

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

圖3 BLE三邊測(cè)量和MIFKF算法仿真結(jié)果

如圖4所示為所有10次測(cè)試中三邊測(cè)量誤差對(duì)應(yīng)的余值。該圖顯示了三邊測(cè)量在X軸和Y軸上的誤差值與余值存在著很強(qiáng)的相關(guān)性，且對(duì)應(yīng)相關(guān)系數(shù)分別為0.897和0.952。印證了圖3(b)的結(jié)論，同時(shí)表明余值可以很好地提供三邊測(cè)量的可靠性估計(jì)。

圖4 三邊測(cè)量估計(jì)關(guān)于余值在X軸和Y軸上的絕對(duì)誤差

為了表現(xiàn)出MIFKF算法在礦井定位中的優(yōu)越性,MIFKF算法的性能將在Y軸上與標(biāo)準(zhǔn)KF進(jìn)行比較。標(biāo)準(zhǔn)KF設(shè)定一個(gè)常數(shù)三邊測(cè)量的測(cè)量噪聲。噪聲方差(Rt的對(duì)角元素)在X軸Y軸上調(diào)整為9×10-4m2。

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

圖6比較了標(biāo)準(zhǔn)KF算法與MIFKF算法下的卡爾曼增益。從圖中可知：MIFKF相對(duì)于標(biāo)準(zhǔn)KF有較低的平均卡爾曼增益為0.0013，而標(biāo)準(zhǔn)KF則為0.0451，MIFKF算法下卡爾曼增益越小表明在其算法中IMU測(cè)量相比三邊測(cè)量所占權(quán)重越大。

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

圖5 標(biāo)準(zhǔn)KF和MIFKF算法仿真結(jié)果

圖6 標(biāo)準(zhǔn)KF與MIFKF算法上的卡爾曼增益

圖7 標(biāo)準(zhǔn)KF與MIFKF算法模擬礦井跟蹤軌跡

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

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

4 結(jié) 論

針對(duì)礦井無線定位異常值的處理，提出了一種優(yōu)化的用于礦井工作人員和機(jī)車準(zhǔn)確估計(jì)位置軌跡的(MIFKF)算法，算法通過對(duì)IMU測(cè)量值進(jìn)行預(yù)處理并融合BLE三邊測(cè)量值進(jìn)行濾波優(yōu)化處理，同時(shí)濾波器參數(shù)可自動(dòng)調(diào)節(jié)。實(shí)驗(yàn)結(jié)果表明：MIFKF算法相對(duì)于三邊測(cè)量和標(biāo)準(zhǔn)KF在礦井復(fù)雜環(huán)境中有效減小定位誤差，并且在使用RTS平滑處理后準(zhǔn)確性得到了進(jìn)一步提高。

表1 定位跟蹤的RMSE cm

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

[2] 蘇應(yīng)敢，張林山，姚 建.一種考慮非視距傳播的UWB定位算法[J].云南電力技術(shù),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] 吳哲明,孫振國(guó),張文增.基于慣性測(cè)量單元旋轉(zhuǎn)的陀螺漂移估計(jì)和補(bǔ)償方法[J].清華大學(xué)學(xué)報(bào)：自然科學(xué)版,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] 劉明偉,劉太君,葉 焱.基于低功耗藍(lán)牙技術(shù)的室內(nèi)定位應(yīng)用研究[J].無線通信技術(shù),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算法對(duì)礦井人員定位跟蹤的優(yōu)化研究[J].云南大學(xué)學(xué)報(bào)：自然科學(xué)版,2016,38(3)：392-398.

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

[12] 鄒 娟.無線傳感網(wǎng)絡(luò)中基于RTS平滑的目標(biāo)跟蹤技術(shù)研究[D].湘潭：湘潭大學(xué),2014.