劉 云, 房飛翔
(昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院,云南 昆明 650500)
礦井定位對(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)確地估算出位置。
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算法流程
通過假設(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)
在融合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)
為了說明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%。
針對(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.