室內(nèi)行人自主定位算法的研究

張 博，賈浩男，趙 皎

(衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點(diǎn)實(shí)驗(yàn)室，河北 石家莊050081)

室內(nèi)行人自主定位算法的研究

張 博，賈浩男，趙 皎

(衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點(diǎn)實(shí)驗(yàn)室，河北 石家莊050081)

室內(nèi)定位與導(dǎo)航在軍事單兵系統(tǒng)、消防定位和大型場館引導(dǎo)領(lǐng)域都擁有非常大的需求。由于GNSS導(dǎo)航在復(fù)雜的室內(nèi)環(huán)境下會出現(xiàn)衰減、多徑等問題，因此針對行人的基于低成本MEMS慣性傳感器的行人航位推算(Personal Dead-reckoning，PDR)導(dǎo)航系統(tǒng)成為該領(lǐng)域的研究熱點(diǎn)。以捷聯(lián)慣性導(dǎo)航原理為基礎(chǔ)，設(shè)計(jì)了室內(nèi)行人自主定位算法并實(shí)現(xiàn)了該算法實(shí)時(shí)地解算行人位置，該系統(tǒng)模塊佩戴于行人腳部，可以完成在室內(nèi)環(huán)境中行人所在位置的實(shí)時(shí)定位。實(shí)驗(yàn)結(jié)果表明，該室內(nèi)行人自主定位算法的精度在行進(jìn)距離的1%以內(nèi)，達(dá)到了室內(nèi)定位功能的要求。

室內(nèi)定位；慣性傳感器；零速檢測；姿態(tài)更新

Abstract The fields of military pedestrian system,firefighting and large venues have a very large demand of indoor positioning and navigation.GNSS navigation will be chanllenged by attenuation,multipath and other issues in a complex indoor environment.The Personal Dead-reckoning navigation system based on low-cost MEMS inertial sensors become a hot topic in the field.In this paper,the algorithm of pedestrian autonomous positioning is designed and used to solve pedestrian location in real time based on the strapdown inertial navigation system.The system module is worn on the pedestrian foot,and can complete the pedestrian location in indoor and outdoor environment.Experiment results show that the accuracy of the indoor pedestrian localization algorithm is within 1% of the travel distance,which meets the requirement of indoor positioning function.

Key words indoor positioning;inertial sensor;zero velocity detection;attitude update

0 引言

隨著現(xiàn)代化建設(shè)的進(jìn)程不斷深入以及大型場館建筑日益增多，對于室內(nèi)的和一些未接收GPS信號的區(qū)域，自主的定位與導(dǎo)航技術(shù)的需求會越來越大[1]。在軍事方面，在一些GPS拒止的環(huán)境中，士兵可以通過穿戴能夠自主定位的單兵系統(tǒng)，來完成地面作戰(zhàn)任務(wù)或者協(xié)助記錄訓(xùn)練任務(wù)[2]。對于相關(guān)問題的研究，目前已有部分學(xué)者取得了一定的成果。Ruiz、Madrid等人提出了將基于慣性導(dǎo)航原理的系統(tǒng)與RFID定位系統(tǒng)結(jié)合一起實(shí)現(xiàn)行人室內(nèi)的定位與跟蹤[3]，具體來說是將集成了IMU的模塊固連在腳部采集運(yùn)動信息，融合預(yù)安置在室內(nèi)的若干個(gè)主動式電子標(biāo)簽發(fā)送的RSS信號，采用基于卡爾曼濾波的方法，將INS估計(jì)的位置與RSS路徑損失模型估計(jì)出的位置進(jìn)行整合。Eric Foxlin等人開發(fā)了稱為“NavSheo”的定位系統(tǒng)[4]，其中包含有微型慣性、磁力計(jì)等傳感器，將其佩戴在行人腳部以在具有和不具有GPS支持的任意環(huán)境中提供導(dǎo)航，密歇根大學(xué)的Lauro Ojeda、Johann Borenstein等人針對使用慣性傳感器實(shí)現(xiàn)行人自主定位出現(xiàn)的誤差漂移提出了地圖匹配啟發(fā)式方法(MAPHDE)，驗(yàn)證了該方法在一定程度上減小了方向的漂移，進(jìn)而降低了位置誤差[5]。

為了實(shí)現(xiàn)室內(nèi)行人的高精度定位，本文主要研究采用慣性傳感器、磁力計(jì)組合導(dǎo)航原理，優(yōu)化姿態(tài)融合解算算法，進(jìn)而通過行人航跡推算實(shí)現(xiàn)實(shí)時(shí)化的室內(nèi)定位。在最初通過慣性傳感器進(jìn)行室內(nèi)定位時(shí)，行人僅僅步行幾分鐘后便會出現(xiàn)較大的位置誤差，針對這一問題，找到了解決問題的思路并提出行人自主定位算法。

1 行人自主定位算法設(shè)計(jì)

行人自主定位系統(tǒng)的信息采集與處理模塊將行人腳部運(yùn)動信息(包括三軸加速度值、三軸角速度值與三軸磁場強(qiáng)度值)輸出，通過標(biāo)定系數(shù)對傳感器輸出進(jìn)行處理，并選用低通濾波器對傳感器輸出值進(jìn)行濾波，以便得到較為精確的原始數(shù)值信息。

然后設(shè)計(jì)基于梯度下降的姿態(tài)解算算法對加速度計(jì)、陀螺儀和磁力計(jì)的導(dǎo)航作用進(jìn)行組合[6]，綜合各傳感器優(yōu)點(diǎn)獲得行人腳部姿態(tài)，尤其是獲得偏航角信息[7]；以四元數(shù)形式來表示姿態(tài)變化，可以避免出現(xiàn)奇點(diǎn)現(xiàn)象；以實(shí)現(xiàn)3個(gè)傳感器的最佳融合效果。通過四元數(shù)實(shí)現(xiàn)加速度數(shù)據(jù)由載體系向地理系轉(zhuǎn)換，積分后得到行人速度與位置[8]。關(guān)鍵算法為基于梯度下降的姿態(tài)解算算法，推導(dǎo)過程主要涉及4個(gè)算法。

1.1 角速率計(jì)算載體姿態(tài)算法

三軸陀螺儀可以分別測量MEMS-IMUx，y，z三個(gè)方向角速率ωx、ωy和ωz[9]，單位為rads-1，擴(kuò)展為四元數(shù)Sω記錄為：

(1)

(2)

(3)

(4)

1.2 加速度計(jì)、磁力計(jì)計(jì)算載體姿態(tài)算法

地理系中重力加速度是垂直向下的，磁感線方向是地理南極指向地理北極方向，在無干擾的環(huán)境下，地理系中重力與磁強(qiáng)方向是已知的[10]。傳感器系中三軸加速度與磁強(qiáng)是可以測量出的，由此可以計(jì)算出傳感器系至地理系姿態(tài)變化矩陣，且姿態(tài)變換矩陣不是唯一的，具有無窮多解，與真姿態(tài)角具有周期關(guān)系的角度均是解。通過對目標(biāo)函數(shù)最優(yōu)計(jì)算可以得到姿態(tài)矩陣：

(5)

(6)

最優(yōu)化求解技巧有許多，比如共軛梯度法、梯度下降法、牛頓法和擬牛頓法[11]，其中梯度下降法是最簡單、精度高的一種辦法，式(7)中給出n次迭代的梯度下降計(jì)算方法：

(7)

(8)

上述公式中描述了通用的計(jì)算形式，然而對于行人自主定位系統(tǒng)中，地理系的加速度與磁強(qiáng)方向往往具有1～2個(gè)分量，那么上述形式可以進(jìn)行簡化。

(9)

(10)

(11)

(12)

單獨(dú)地使用重力的測量或地球磁場的測量不能提供準(zhǔn)確的方向[12]，因此，將加速度與磁場強(qiáng)度的測量進(jìn)行合并處理，如式(13)和式(14)所示；由式(9)和式(11)建立的解的集合具有一個(gè)全局最小值，由方程式(13)定義的解具有一個(gè)單個(gè)點(diǎn)定義的最小值，條件是bx=0。

(13)

(14)

(15)

(16)

(17)

1.3 磁場擾動補(bǔ)償算法

行人行走環(huán)境中磁場擾動會嚴(yán)重影響姿態(tài)計(jì)算精度，例如包括電器、金屬家具和建筑結(jié)構(gòu)內(nèi)金屬結(jié)構(gòu)都可以引入實(shí)質(zhì)的誤差[14]。磁場擾動根據(jù)干擾源分為硬磁干擾與軟磁干擾，其中硬磁干擾可以通過傳感器校準(zhǔn)校正掉，而軟磁干擾只能消減無法消除，除非有其他參考[15]。加速度計(jì)提供了另外的參考姿態(tài)，因此可以用于消除軟磁偏差。

地理系t時(shí)刻磁場強(qiáng)度描述為：

(18)

(19)

以這種方式補(bǔ)償磁性失真來確保磁干擾被限制為僅影響航向分量[16]。該方法還消除了對預(yù)定義的地球磁場的參考方向的需要。

1.4 數(shù)據(jù)融合算法

(20)

(21)

(22)

(23)

(24)

將式(23)、式(24)和式(25)代入式(20)中，可得通過融合算法之后的姿態(tài)計(jì)算公式，式中由陀螺儀推算出的姿態(tài)中使用的融合權(quán)重比為γt≈0。

(25)

式中，濾波增益β為陀螺儀零輸入誤差的平均值，零輸入誤差包括非線性誤差、安裝誤差、校準(zhǔn)誤差、噪聲誤差以及信號混疊誤差，通過其微分方程描述[21]為：

(26)

通過上述算法可以由基于梯度的姿態(tài)解算算法得到行人的姿態(tài)變化四元數(shù)，可以將加速度信息轉(zhuǎn)換為地理系下的信息[22]，進(jìn)行雙重積分后即可得出行人的速度和位置信息。

2 行人自主定位系統(tǒng)實(shí)驗(yàn)

行人自主定位系統(tǒng)采用室內(nèi)場地，地面由邊長60cm的正方形地板鋪成，方便規(guī)劃路徑以及行走時(shí)基準(zhǔn)路線的確定，且場地內(nèi)2個(gè)區(qū)域中間有4個(gè)臺階的高度差，可以分析行人自主定位算法對于垂直位移的定位精度。

測試過程為：置采集頻率為123Hz，初始對準(zhǔn)時(shí)間為20s，在每次采集數(shù)據(jù)之前都應(yīng)將傳感器預(yù)熱10min，行走距離水平方向50.3m，垂直方向1.4m。重復(fù)上述步驟做10組實(shí)驗(yàn)。

通過航位推算得行人的X、Y和Z軸方向上的矢量位移估計(jì)，從而實(shí)現(xiàn)行人的定位，如圖1所示，通過終點(diǎn)與起始點(diǎn)的距離獲得行人位置定位的誤差，水平誤差為0.154 m，高度誤差為0.6 cm，由此可知，行人在室內(nèi)行走的定位精度比較高。

圖1 行人位置坐標(biāo)的估計(jì)

根據(jù)對行人速度、姿態(tài)的估計(jì)，獲得對行人航跡的估計(jì)，分別為水平位置的軌跡和高度變化的軌跡，如圖2和圖3所示。

圖2 行人的水平位置軌跡

圖3 行人的高度變化軌跡

為了更具體地驗(yàn)證算法定位效果，表1給出了以均方根誤差衡量的統(tǒng)計(jì)結(jié)果，由表中數(shù)據(jù)可以分析出行走總長度50.3 m，高度變化0.7 m的定位中水平方向誤差平均值為0.201 m，為行走總長度的0.39%，垂直方向上誤差平均值為0.010 2 m，為上升總高度的0.73%。

表1 實(shí)驗(yàn)結(jié)果統(tǒng)計(jì)

行人自主定位系統(tǒng)中信息采集與處理模塊將實(shí)時(shí)解算出的信息通過串口藍(lán)牙模塊發(fā)送至顯示終端，顯示終端接收顯示行人行走軌跡與所在室內(nèi)的具體位置。

該行人自主定位系統(tǒng)在行人線性位移的測量中較為精確，在行進(jìn)速度為1～2.5 m/s的情況下，誤差小于行進(jìn)距離的1%。并且行人自主定位系統(tǒng)對步行中的暫停、快走和上下樓梯等行為均能夠保持較好的精度。

3 結(jié)束語

為室內(nèi)行人提供了一個(gè)簡單的個(gè)人航位推算定位系統(tǒng)，該系統(tǒng)不需要任何外部參考，例如WiFi或RFID等基站，能夠?qū)崿F(xiàn)室內(nèi)較長時(shí)間自主、精確的定位功能。通過對行人自主定位系統(tǒng)進(jìn)行設(shè)計(jì)，并針對室內(nèi)行人實(shí)驗(yàn)中存在的問題，提出了基于梯度下降的姿態(tài)融合算法，經(jīng)過多次實(shí)驗(yàn)驗(yàn)證滿足預(yù)期目標(biāo)并且算法比較穩(wěn)定，具有很好的應(yīng)用前景。

[1] 尹杭.使用MEMS慣性測量單元和拓?fù)涞貓D的室內(nèi)定位與導(dǎo)航技術(shù)研究[D].成都:電子科技大學(xué),2015.

[2] 朱娟,周尚偉,馬啟平.基于RFID的室內(nèi)定位算法[J].微計(jì)算機(jī)信息,2009,25(8):160-162.

[3] 鄧仲哲.基于慣性傳感器和地圖匹配的行人室內(nèi)定位算法[D].哈爾濱:哈爾濱工業(yè)大學(xué),2015.

[4] FOXLIN E.Inertial Head-tracker Sensor Fusion by a Complementary Separate-bias Kalman Filter[C]∥Virtual Reality Annual International Symposium the IEEE,1996:185-194.

[5] Ronan Alves da Paixao,Jacy Montenegro Magalhaes Neto,Paulo Fernando Ferreira Rosa.Prototyping a Basic MEMS Attitude Heading and Reference System[C]∥2011 IEEE International Conference on Industrial Technology,2011:13-38.

[6] GLUECKM,BUHMANN A,MANOLI Y.Autocalibration of MEMS Accelerometers[C]∥Graz,Austria.IEEE Inertional Instrumentation and Measurement Technology Conference,2012:45-48.

[7] 張浩,任芊.四旋翼飛行器航姿測量系統(tǒng)的數(shù)據(jù)融合方法[J].兵工自動化,2004,32(1):28-31.

[8] 陳靜燕.深海環(huán)境慣性導(dǎo)航系統(tǒng)誤差補(bǔ)償方法的研究[D].杭州:浙江大學(xué),2009.

[9] 黃艷輝.MTi中微機(jī)械陀螺誤差測試、建模及補(bǔ)償研究[D].哈爾濱:哈爾濱工程大學(xué),2009.

[10] 白晶,馬燕飛.無源交叉定位系統(tǒng)中的最優(yōu)交會角[J].無線電通信技術(shù),2015,41(1):31-34.

[11] 梁健.目標(biāo)定位精度評估方法研究[J].無線電工程,2015,45(4):65-69.

[12] 洪雁,王培康.基于稀疏表示多分類的室內(nèi)定位算法[J].無線電工程,2014,44(2):46-49.

[13] 江城.基于北斗終端的室內(nèi)外無縫定位系統(tǒng)設(shè)計(jì)[J].無線電工程.2016,46(8):38-42.

[14] 劉烜,胡倩影.航空光電平臺機(jī)動目標(biāo)跟蹤定位技術(shù)應(yīng)用[J].無線電通信技術(shù),2014,40(3):82-85.

[15] 劉欣然.老人跌倒監(jiān)測定位裝置的研究[J].無線電通信技術(shù),2016,42(1):82-85.

[16] 袁曉峰,陳頎.一種基于RFID的定位方法[J].無線電通信技術(shù),2016,42(6)：81-85.

[17] 段同樂,張冬寧,劉文展.基于模糊邏輯的WSN節(jié)點(diǎn)自定位算法[J].無線電通信技術(shù),2016,42(2):48-50.

[18] 任紹俊,于惠海.基于GPS的水下動態(tài)目標(biāo)定位方法研究[J].無線電工程,2015,45(7):87-90.

[19] 后茜,劉韋韋,丁進(jìn).LTE定位中非視距誤差的鑒別方法[J].無線電工程,2016,46(1):8-11.

[20] 江城.基于北斗終端的室內(nèi)外無縫定位系統(tǒng)設(shè)計(jì)[J].無線電工程,2016,46(8):38-42.

[21] 趙凱.神經(jīng)網(wǎng)絡(luò)和RFID相融合的室內(nèi)定位算法[J].激光雜志,2015,36(8):138-141.

[22] 王遠(yuǎn)哲,毛陸虹,劉輝.基于參考標(biāo)簽的射頻識別定位算法研究與應(yīng)用[J].通信學(xué)報(bào),2010,31(2):86-92.

Research on Indoor Pedestrian Autonomous Location Algorithm

ZHANG Bo，JIA Hao-nan，ZHAO Jiao

(StateKeyLaboratoryofSatelliteNavigationSystemandEquipmentTechnology,ShijiazhuangHebei050081,China)

10.3969/j.issn.1003-3106.2017.08.08

張博，賈浩男，趙皎.室內(nèi)行人自主定位算法的研究[J].無線電工程,2017,47(8):31-35.[ZHANG Bo，JIA Haonan，ZHAO Jiao.Research on Indoor Pedestrian Autonomous Location Algorithm[J].Radio Engineering,2017,47(8):31-35.]

2017-03-24

國家自然科學(xué)基金重大研究計(jì)劃基金資助項(xiàng)目(91638203)；國家重點(diǎn)研發(fā)計(jì)劃基金資助項(xiàng)目(2016YFB0502402，2016YFB0502101)。

TN961

A

1003-3106(2017)08-0031-05

張 博 男，(1986—)，碩士,工程師。主要研究方向：地面運(yùn)控系統(tǒng)和定位算法。

賈浩男 男，(1990—)，碩士,助理工程師。主要研究方向：信號處理和信息處理。