孫 艷, 樊啟高, 孫璧文, 莊祥鵬
(江南大學(xué) 物聯(lián)網(wǎng)工程學(xué)院,江蘇 無(wú)錫 214122)
隨著室內(nèi)定位系統(tǒng)的快速發(fā)展,越來(lái)越多的技術(shù)開(kāi)始興起。如WiFi技術(shù)[1],射頻識(shí)別(radio frequency identification,RFID)技術(shù)[2,3]、超聲波技術(shù)[4]、ZigBee技術(shù)[5,6]。但無(wú)線傳輸不能保證在所有條件下的定位的穩(wěn)定性和準(zhǔn)確性。因此,基于微機(jī)電慣性測(cè)量單元(micro-electro-mecha-nical system inertial measurement unit,MEMS IMU)[7]的行人室內(nèi)定位技術(shù)(pedestrian dead reckoning,PDR)[8]興起。PDR具有計(jì)算簡(jiǎn)便,成本低,發(fā)展空間大等特點(diǎn)。
PDR算法大多基于零速更新(zero velocity update,ZUPT)[9]建立不同的慣性導(dǎo)航系統(tǒng),得到定位信息。但基于ZUPT的定位算法存在缺點(diǎn):零速點(diǎn)未被完全提取,MEMS IMU的累積誤差導(dǎo)致判斷零速區(qū)間錯(cuò)誤等。因此,本文提出了基于改進(jìn)航向估計(jì)的行人航跡算法,利用步頻、步長(zhǎng)和航向計(jì)算出行人的位置,其中航向估計(jì)采用了基于四元數(shù)算法,并采用擴(kuò)展卡爾曼濾波[10](extended Kalman filtering,EKF)對(duì)航向角的偏差進(jìn)行修正,提高了定位的精度。
行人航跡推算由步頻估計(jì)、步長(zhǎng)估計(jì)以及航向估計(jì)組成。如圖1所示,通過(guò)加速度計(jì)的輸出值計(jì)算出步長(zhǎng)以及估算出行走的步數(shù)。航向估計(jì)則通過(guò)加速度計(jì)、陀螺儀和磁強(qiáng)計(jì)的輸出值計(jì)算得到,并利用EKF實(shí)時(shí)修正航向角的偏差,得到精確的航向角,提高了行人定位的精度。行人定位公式如下
(1)
(2)
式中ei與ni分別為行人東向和北向的步長(zhǎng);Ψk為每一步東向和北向的夾角;k為行走的步數(shù)。
圖1 行人航跡推算的結(jié)構(gòu)框圖
步頻估計(jì)算法包括峰值檢測(cè)法[11]、站立測(cè)頻法[12]、零點(diǎn)交叉法[13,14]等。本文采用劉程的自適應(yīng)計(jì)步檢測(cè)算法[15],利用加速度輸出的Z向峰值來(lái)臨時(shí)間不同,采用自適應(yīng)時(shí)間窗的檢測(cè)算法。
步幅估計(jì)算法通常有2種:1)根據(jù)經(jīng)驗(yàn),設(shè)定行人的步幅為固定值,這種做法會(huì)存在很大的累積誤差;2)對(duì)行人的步幅進(jìn)行動(dòng)態(tài)的計(jì)算,本文利用加速度計(jì)輸?shù)姆逯颠M(jìn)行計(jì)算。公式如下
(3)
式中az max為在本時(shí)段加速度計(jì)的最高值;az min為本時(shí)段的加速度計(jì)的最低值;Lk為根據(jù)行人的行走特征所設(shè)定的固定值。
航向估計(jì)為本文提出的行人航跡算法中是最重要的步驟,對(duì)行人的定位有重要的影響。首先,俯仰角θ和橫滾角φ可以通過(guò)加速度計(jì)得到[16]
(4)
(5)
(6)
(7)
(8)
式中ax,ay,az為加速度計(jì)X,Y,Z軸的輸出值,gn為重力加速度;θ為俯仰角;φ為橫滾角;ψ為航向角;hx,hy,hz分別為磁強(qiáng)計(jì)X,Y,Z軸的輸出值;δD為本地的磁偏角。
通過(guò)加速度計(jì)和磁強(qiáng)計(jì)計(jì)算的俯仰角、橫滾角、航向角存在著累積誤差,影響行人定位的精確性,因此,利用EKF修正航向角的偏差。
基于四元數(shù)的姿態(tài)矩陣
(9)
(10)
四元數(shù)更新公式
(11)
(12)
式中q=[q0q1q2q3];wx,wy,wz分別為陀螺儀X,Y,Z軸的輸出值;dt為采樣時(shí)間。
卡爾曼濾波的狀態(tài)值X=[q0q1q2q3]T;量測(cè)值Z=[θφψ]T;根據(jù)文獻(xiàn)[10]可得狀態(tài)矩陣
(13)
量測(cè)矩陣
(14)
系統(tǒng)干擾方差矩陣為
(15)
量測(cè)噪聲方差矩陣為
(16)
為評(píng)估提出算法的定位效果,需采用慣性航姿系統(tǒng)Xsens Mti300進(jìn)行相關(guān)實(shí)驗(yàn),如圖2所示為IMU硬件整體結(jié)構(gòu)以及安裝方式。陀螺儀、加速度計(jì)、磁強(qiáng)計(jì)、溫度傳感器和氣壓傳感器等作為從設(shè)備掛載內(nèi)部集成電路(inter-integrated circuit,I2C)總線,再通過(guò)數(shù)字信號(hào)處理器(digital signal processor,DSP)將數(shù)據(jù)從串口中輸出到計(jì)算機(jī)上,得到方向、加速度以及轉(zhuǎn)速。
圖2 IMU結(jié)構(gòu)及安裝方式
圖3為濾波前、后的航向角與真實(shí)航向角的對(duì)比,實(shí)驗(yàn)結(jié)果表明:航向角動(dòng)態(tài)誤差為-120.012°~149.0313°,但平均誤差僅3.5699×10-6(°),說(shuō)明有極少個(gè)數(shù)據(jù)誤差特別大,其他的則較小,殘差率為0.0041,置信度為99.59%,因此,航向角的精度滿足行人定位的需求。
圖3 航向角對(duì)比
圖4為軌跡對(duì)比,實(shí)驗(yàn)表明:誤差范圍為-0.3059~0.2415m,殘差率為0.0172,置信度為98.28%,本文提出的行人定位算法具有準(zhǔn)確性和穩(wěn)定性。
圖4 行人軌跡對(duì)比
通過(guò)IMU的陀螺儀、加速度計(jì)和磁強(qiáng)計(jì)的輸出值,得到步長(zhǎng)、步數(shù)?;谒脑獢?shù)法計(jì)算航向角,并采用EKF修正航向角的偏差提高航向角精度。實(shí)驗(yàn)結(jié)果表明:航向角的置信度為99.59%,定位軌跡動(dòng)態(tài)誤差為-0.3059~0.2415m,置信度為98.28% ,滿足實(shí)際系統(tǒng)的要求。未來(lái)的工作,需要在更加復(fù)雜的環(huán)境下進(jìn)行實(shí)驗(yàn),并對(duì)濾波算法進(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ù)評(píng)述[J].傳感器與微系統(tǒng),2009,28(2):1-3.
[3] 郝金平,張建寄.超高頻RFID發(fā)卡機(jī)射頻匹配網(wǎng)絡(luò)研究[J].自動(dòng)化技術(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] 王 靜,張會(huì)清.基于ZigBee的無(wú)線網(wǎng)絡(luò)定位技術(shù)的研究與實(shí)現(xiàn)[J].傳感器與微系統(tǒng),2010,29(2):13-15.
[6] 羅 歡,張 健,席文平.基于ZigBee技術(shù)的足球機(jī)器人通信系統(tǒng)設(shè)計(jì)[J].無(wú)線電工程,2015,45(5):16-19.
[7] 錢(qián)偉行,朱欣華,蘇 巖.基于足部微慣性/地磁測(cè)量組件的個(gè)人導(dǎo)航方法[J].中國(guó)慣性技術(shù)學(xué)報(bào),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] 趙 斌,竇靈平,袁立軍.卡爾曼濾波算法的硬件實(shí)現(xiàn)研究[J].無(wú)線電工程,2016,46(6):65-68.
[11] 劉 程,陽(yáng) 洪.自適應(yīng)計(jì)步檢測(cè)算法研究[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)外無(wú)縫定位算法研究[D].合肥:中國(guó)科學(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ì)算法[J].傳感技術(shù)學(xué)報(bào),2014,27(1):90-95.