室內行人自主定位算法的研究

張 博，賈浩男，趙 皎

(衛星導航系統與裝備技術國家重點實驗室，河北 石家莊050081)

室內行人自主定位算法的研究

張 博，賈浩男，趙 皎

(衛星導航系統與裝備技術國家重點實驗室，河北 石家莊050081)

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

室內定位；慣性傳感器；零速檢測；姿態更新

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 引言

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

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

1 行人自主定位算法設計

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

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

1.1 角速率計算載體姿態算法

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

(1)

(2)

(3)

(4)

1.2 加速度計、磁力計計算載體姿態算法

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

(5)

(6)

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

(7)

(8)

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

(9)

(10)

(11)

(12)

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

(13)

(14)

(15)

(16)

(17)

1.3 磁場擾動補償算法

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

地理系t時刻磁場強度描述為：

(18)

(19)

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

1.4 數據融合算法

(20)

(21)

(22)

(23)

(24)

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

(25)

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

(26)

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

2 行人自主定位系統實驗

行人自主定位系統采用室內場地，地面由邊長60cm的正方形地板鋪成，方便規劃路徑以及行走時基準路線的確定，且場地內2個區域中間有4個臺階的高度差，可以分析行人自主定位算法對于垂直位移的定位精度。

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

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

圖1 行人位置坐標的估計

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

圖2 行人的水平位置軌跡

圖3 行人的高度變化軌跡

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

表1 實驗結果統計

行人自主定位系統中信息采集與處理模塊將實時解算出的信息通過串口藍牙模塊發送至顯示終端，顯示終端接收顯示行人行走軌跡與所在室內的具體位置。

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

3 結束語

為室內行人提供了一個簡單的個人航位推算定位系統，該系統不需要任何外部參考，例如WiFi或RFID等基站，能夠實現室內較長時間自主、精確的定位功能。通過對行人自主定位系統進行設計，并針對室內行人實驗中存在的問題，提出了基于梯度下降的姿態融合算法，經過多次實驗驗證滿足預期目標并且算法比較穩定，具有很好的應用前景。

[1] 尹杭.使用MEMS慣性測量單元和拓撲地圖的室內定位與導航技術研究[D].成都:電子科技大學,2015.

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

[3] 鄧仲哲.基于慣性傳感器和地圖匹配的行人室內定位算法[D].哈爾濱:哈爾濱工業大學,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] 張浩,任芊.四旋翼飛行器航姿測量系統的數據融合方法[J].兵工自動化,2004,32(1):28-31.

[8] 陳靜燕.深海環境慣性導航系統誤差補償方法的研究[D].杭州:浙江大學,2009.

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

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

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

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

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

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

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

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

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

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

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

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

[21] 趙凱.神經網絡和RFID相融合的室內定位算法[J].激光雜志,2015,36(8):138-141.

[22] 王遠哲,毛陸虹,劉輝.基于參考標簽的射頻識別定位算法研究與應用[J].通信學報,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

張博，賈浩男，趙皎.室內行人自主定位算法的研究[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

國家自然科學基金重大研究計劃基金資助項目(91638203)；國家重點研發計劃基金資助項目(2016YFB0502402，2016YFB0502101)。

TN961

A

1003-3106(2017)08-0031-05

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

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