基于改進(jìn)航向估計的行人航跡推算研究*

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

(江南大學(xué) 物聯(lián)網(wǎng)工程學(xué)院，江蘇 無錫 214122)

0 引 言

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

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

1 行人航跡推算

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

(1)

(2)

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

圖1 行人航跡推算的結(jié)構(gòu)框圖

1.1 步頻估計

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

1.2 步幅估計

步幅估計算法通常有2種：1)根據(jù)經(jīng)驗，設(shè)定行人的步幅為固定值，這種做法會存在很大的累積誤差；2)對行人的步幅進(jìn)行動態(tài)的計算，本文利用加速度計輸?shù)姆逯颠M(jìn)行計算。公式如下

(3)

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

1.3 航向估計

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

(4)

(5)

(6)

(7)

(8)

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

2 EKF

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

基于四元數(shù)的姿態(tài)矩陣

(9)

(10)

四元數(shù)更新公式

(11)

(12)

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

卡爾曼濾波的狀態(tài)值X=[q0q1q2q3]T;量測值Z=[θφψ]T;根據(jù)文獻(xiàn)[10]可得狀態(tài)矩陣

(13)

量測矩陣

(14)

系統(tǒng)干擾方差矩陣為

(15)

量測噪聲方差矩陣為

(16)

3 實 驗

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

圖2 IMU結(jié)構(gòu)及安裝方式

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

圖3 航向角對比

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

圖4 行人軌跡對比

4 結(jié) 論

通過IMU的陀螺儀、加速度計和磁強(qiáng)計的輸出值，得到步長、步數(shù)?；谒脑獢?shù)法計算航向角，并采用EKF修正航向角的偏差提高航向角精度。實驗結(jié)果表明：航向角的置信度為99.59%，定位軌跡動態(tài)誤差為-0.3059～0.2415m，置信度為98.28% ，滿足實際系統(tǒng)的要求。未來的工作，需要在更加復(fù)雜的環(huán)境下進(jìn)行實驗，并對濾波算法進(jìn)一步改進(jìn)，構(gòu)建緊耦合模型，提高定位精度。

[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的室內(nèi)定位技術(shù)評述[J].傳感器與微系統(tǒng),2009,28(2):1-3.

[3] 郝金平，張建寄.超高頻RFID發(fā)卡機(jī)射頻匹配網(wǎng)絡(luò)研究[J].自動化技術(shù)與應(yīng)用，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的無線網(wǎng)絡(luò)定位技術(shù)的研究與實現(xiàn)[J].傳感器與微系統(tǒng),2010,29(2):13-15.

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

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

[8] 殷 紅.基于foot-mounted的IMU室內(nèi)行人航跡推算研究[D].南昌：南昌大學(xué),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] 趙 斌，竇靈平，袁立軍.卡爾曼濾波算法的硬件實現(xiàn)研究[J].無線電工程，2016,46(6)：65-68.

[11] 劉 程,陽 洪.自適應(yīng)計步檢測算法研究[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和自包含傳感器的行人室內(nèi)外無縫定位算法研究[D].合肥：中國科學(xué)技術(shù)大學(xué)，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] 賈瑞才.基于四元數(shù)EKF的低成本MEMS姿態(tài)估計算法[J].傳感技術(shù)學(xué)報,2014,27(1):90-95.