基于改進航向估計的行人航跡推算研究*

孫 艷， 樊啟高， 孫璧文， 莊祥鵬

(江南大學 物聯網工程學院，江蘇 無錫 214122)

0 引 言

隨著室內定位系統的快速發展，越來越多的技術開始興起。如WiFi技術[1]，射頻識別(radio frequency identification,RFID)技術[2,3]、超聲波技術[4]、ZigBee技術[5,6]。但無線傳輸不能保證在所有條件下的定位的穩定性和準確性。因此，基于微機電慣性測量單元(micro-electro-mecha-nical system inertial measurement unit,MEMS IMU)[7]的行人室內定位技術(pedestrian dead reckoning,PDR)[8]興起。PDR具有計算簡便,成本低，發展空間大等特點。

PDR算法大多基于零速更新(zero velocity update,ZUPT)[9]建立不同的慣性導航系統，得到定位信息。但基于ZUPT的定位算法存在缺點:零速點未被完全提取，MEMS IMU的累積誤差導致判斷零速區間錯誤等。因此，本文提出了基于改進航向估計的行人航跡算法，利用步頻、步長和航向計算出行人的位置，其中航向估計采用了基于四元數算法，并采用擴展卡爾曼濾波[10](extended Kalman filtering,EKF)對航向角的偏差進行修正，提高了定位的精度。

1 行人航跡推算

行人航跡推算由步頻估計、步長估計以及航向估計組成。如圖1所示，通過加速度計的輸出值計算出步長以及估算出行走的步數。航向估計則通過加速度計、陀螺儀和磁強計的輸出值計算得到，并利用EKF實時修正航向角的偏差，得到精確的航向角，提高了行人定位的精度。行人定位公式如下

(1)

(2)

式中ei與ni分別為行人東向和北向的步長；Ψk為每一步東向和北向的夾角；k為行走的步數。

圖1 行人航跡推算的結構框圖

1.1 步頻估計

步頻估計算法包括峰值檢測法[11]、站立測頻法[12]、零點交叉法[13，14]等。本文采用劉程的自適應計步檢測算法[15]，利用加速度輸出的Z向峰值來臨時間不同，采用自適應時間窗的檢測算法。

1.2 步幅估計

步幅估計算法通常有2種：1)根據經驗，設定行人的步幅為固定值，這種做法會存在很大的累積誤差；2)對行人的步幅進行動態的計算，本文利用加速度計輸的峰值進行計算。公式如下

(3)

式中az max為在本時段加速度計的最高值；az min為本時段的加速度計的最低值；Lk為根據行人的行走特征所設定的固定值。

1.3 航向估計

航向估計為本文提出的行人航跡算法中是最重要的步驟，對行人的定位有重要的影響。首先，俯仰角θ和橫滾角φ可以通過加速度計得到[16]

(4)

(5)

(6)

(7)

(8)

式中ax，ay，az為加速度計X，Y，Z軸的輸出值，gn為重力加速度；θ為俯仰角；φ為橫滾角；ψ為航向角；hx，hy，hz分別為磁強計X，Y，Z軸的輸出值；δD為本地的磁偏角。

2 EKF

通過加速度計和磁強計計算的俯仰角、橫滾角、航向角存在著累積誤差，影響行人定位的精確性，因此,利用EKF修正航向角的偏差。

基于四元數的姿態矩陣

(9)

(10)

四元數更新公式

(11)

(12)

式中q=[q0q1q2q3];wx,wy,wz分別為陀螺儀X，Y，Z軸的輸出值;dt為采樣時間。

卡爾曼濾波的狀態值X=[q0q1q2q3]T;量測值Z=[θφψ]T;根據文獻[10]可得狀態矩陣

(13)

量測矩陣

(14)

系統干擾方差矩陣為

(15)

量測噪聲方差矩陣為

(16)

3 實 驗

為評估提出算法的定位效果，需采用慣性航姿系統Xsens Mti300進行相關實驗，如圖2所示為IMU硬件整體結構以及安裝方式。陀螺儀、加速度計、磁強計、溫度傳感器和氣壓傳感器等作為從設備掛載內部集成電路(inter-integrated circuit,I2C)總線，再通過數字信號處理器(digital signal processor,DSP)將數據從串口中輸出到計算機上，得到方向、加速度以及轉速。

圖2 IMU結構及安裝方式

圖3為濾波前、后的航向角與真實航向角的對比，實驗結果表明：航向角動態誤差為-120.012°～149.0313°，但平均誤差僅3.5699×10-6(°)，說明有極少個數據誤差特別大，其他的則較小，殘差率為0.0041，置信度為99.59%，因此，航向角的精度滿足行人定位的需求。

圖3 航向角對比

圖4為軌跡對比，實驗表明：誤差范圍為-0.3059～0.2415m,殘差率為0.0172，置信度為98.28%，本文提出的行人定位算法具有準確性和穩定性。

圖4 行人軌跡對比

4 結 論

通過IMU的陀螺儀、加速度計和磁強計的輸出值，得到步長、步數。基于四元數法計算航向角，并采用EKF修正航向角的偏差提高航向角精度。實驗結果表明：航向角的置信度為99.59%，定位軌跡動態誤差為-0.3059～0.2415m，置信度為98.28% ，滿足實際系統的要求。未來的工作，需要在更加復雜的環境下進行實驗，并對濾波算法進一步改進，構建緊耦合模型，提高定位精度。

[1] Ma R,Guo Q,Hu C,et al.An improved WiFi indoor positioning algorithm by weighted fusion[J].Sensors,2015,15(9):21824-21843.

[2] 王小輝,汪云甲,張 偉.基于RFID的室內定位技術評述[J].傳感器與微系統,2009,28(2):1-3.

[3] 郝金平，張建寄.超高頻RFID發卡機射頻匹配網絡研究[J].自動化技術與應用，2016,35(5)：59-63.

[4] Hazas M,Hopper A.Broadband ultrasonic location systems for improved indoor positioning[J].IEEE Transactions on Mobile Computing,2006,5(5):536-547.

[5] 王 靜,張會清.基于ZigBee的無線網絡定位技術的研究與實現[J].傳感器與微系統,2010,29(2):13-15.

[6] 羅 歡，張 健，席文平.基于ZigBee技術的足球機器人通信系統設計[J].無線電工程，2015,45(5)：16-19.

[7] 錢偉行,朱欣華,蘇 巖.基于足部微慣性/地磁測量組件的個人導航方法[J].中國慣性技術學報,2012,20(5):567-572.

[8] 殷 紅.基于foot-mounted的IMU室內行人航跡推算研究[D].南昌：南昌大學,2013.

[9] Lin T,Li L,Lachapelle G.Multiple sensors integration for pedestrian indoor navigation[C]∥2015International Conference on Indoor Positioning and Indoor Navigation(IPIN),IEEE,2015:1-9.

[10] 趙 斌，竇靈平，袁立軍.卡爾曼濾波算法的硬件實現研究[J].無線電工程，2016,46(6)：65-68.

[11] 劉 程,陽 洪.自適應計步檢測算法研究[J].壓電與聲光,2015,37(2):258-261.

[12] Levi R W,Judd T.Dead reckoning navigational system using accelerometer to measure foot impacts:US,US5583776[P].1999—12—06.

[13] Yan L,Wang J J.A robust pedestrian navigation algorithm with low cost IMU[C]∥2012IEEE International Conference on Indoor Positioning and Indoor Navigation(IPIN),Sydney,NSW,2012:1-7.

[14] 陳 偉.基于GPS和自包含傳感器的行人室內外無縫定位算法研究[D].合肥：中國科學技術大學，2010.

[15] Pratama A R,Wan W,Hidayat R.Smartphonebased pedestrian dead reckoning as an indoor positioning system[C]∥IEEE International Conference on System Engineering and Technology,Bandung,2012:1-6．

[16] 賈瑞才.基于四元數EKF的低成本MEMS姿態估計算法[J].傳感技術學報,2014,27(1):90-95.