唐文杰　呂志偉　張倫東　叢佃偉

1　信息工程大學(xué)導(dǎo)航與空天目標(biāo)工程學(xué)院，鄭州市科學(xué)大道62號，450001 2　地理信息工程國家重點(diǎn)實(shí)驗(yàn)室,西安市雁塔路中段1號,710054

?

基于零速修正輔助MIMU的行人導(dǎo)航算法研究

唐文杰1,2呂志偉1張倫東1叢佃偉1,2

1信息工程大學(xué)導(dǎo)航與空天目標(biāo)工程學(xué)院，鄭州市科學(xué)大道62號，450001 2地理信息工程國家重點(diǎn)實(shí)驗(yàn)室,西安市雁塔路中段1號,710054

摘要：針對微慣性測量單元(MIMU)誤差積累問題，根據(jù)行人行走過程中每一次邁步的落地階段短時(shí)間內(nèi)速度可保持為0的特性，在零速時(shí)刻利用系統(tǒng)輸出對系統(tǒng)誤差進(jìn)行估計(jì)，從而提高導(dǎo)航精度。對比分析基于加速度、角速度、加速度方差和加速度與角速度組合的4種零速檢測算法，結(jié)果表明，在步態(tài)平緩、行走距離較短的情況下，4種算法都能達(dá)到較高的精度，其中組合檢測法精度最高。

關(guān)鍵詞：MIMU；零速修正；零速檢測；行人導(dǎo)航

傳統(tǒng)導(dǎo)航模式需要接收機(jī)等導(dǎo)航設(shè)備與衛(wèi)星之間保持通視。但是，在室內(nèi)環(huán)境下信號易受遮擋，多路徑效應(yīng)增強(qiáng)，導(dǎo)致無法使用[1]。慣性導(dǎo)航具有自主性強(qiáng)且不受外界干擾等特點(diǎn)，能在復(fù)雜環(huán)境中提供連續(xù)的位置和姿態(tài)信息。 近年來，基于 MIMU的慣性導(dǎo)航技術(shù)發(fā)展迅速，相比于常規(guī)慣性器件，其具有體積小、成本低、可靠性強(qiáng)、易于集成和數(shù)字化等特點(diǎn)[1]，使其應(yīng)用于行人導(dǎo)航成為可能。但MIMU精度比傳統(tǒng)慣性器件低很多，導(dǎo)航積累誤差很大，必須采取輔助手段修正誤差。零速修正[2]利用行人行走過程中每次著地時(shí)短暫的靜止?fàn)顟B(tài)，將 MIMU的輸出作為系統(tǒng)誤差的觀測量，從而對系統(tǒng)誤差進(jìn)行估計(jì)，改善導(dǎo)航精度。本文主要介紹基于零速修正輔助 MIMU的行人導(dǎo)航算法，利用加速度信號和角速度信號的周期特性檢測零速狀態(tài)，在每個(gè)零速時(shí)刻對慣導(dǎo)誤差進(jìn)行修正，并利用4種零速檢測方法進(jìn)行實(shí)驗(yàn)。

1行人導(dǎo)航算法

1.1姿態(tài)角初始對準(zhǔn)

設(shè)i系為慣性坐標(biāo)系，e系為地球坐標(biāo)系，b系為載體坐標(biāo)系，三軸分別指向載體的前、右、下3個(gè)方向；n系為導(dǎo)航坐標(biāo)系，采用當(dāng)?shù)氐乩碜鴺?biāo)系，三軸方向分別指向北、東和當(dāng)?shù)劂U垂方向。

(1)

(2)

(3)

式中，fx、fy、fz分別為加速度計(jì)在載體坐標(biāo)系三軸上的分量。

航向角在系統(tǒng)方程中不可觀[3]，不能直接通過濾波計(jì)算出來。因此在靜態(tài)條件下，可以利用磁強(qiáng)計(jì)進(jìn)行航向估計(jì)。設(shè)三軸磁強(qiáng)計(jì)在載體坐標(biāo)系下的測量值為Mxb、Myb、Mzb,則三軸磁強(qiáng)計(jì)在導(dǎo)航坐標(biāo)系下的坐標(biāo)為：

(4)

在忽略磁偏角的情況下，航向角為：

(5)

1.2擴(kuò)展卡爾曼濾波器

擴(kuò)展卡爾曼濾波就是對非線性系統(tǒng)中的待估參數(shù)Xk進(jìn)行線性化，得到線性化的模型，然后再利用標(biāo)準(zhǔn)Kalman濾波進(jìn)行估計(jì)。盡管這種濾波是一種次優(yōu)的狀態(tài)估計(jì)方法[4]，但由于行人導(dǎo)航系統(tǒng)是非線性的，利用擴(kuò)展卡爾曼濾波能夠獲得較好的精度。捷聯(lián)慣導(dǎo)在當(dāng)?shù)氐乩碜鴺?biāo)系下的系統(tǒng)誤差模型為：

(6)

其中,

選取地理坐標(biāo)系中姿態(tài)角誤差、速度誤差、位移誤差、陀螺儀和加速度計(jì)常值零偏構(gòu)成狀態(tài)矢量，即系統(tǒng)狀態(tài)x=(δφ,δv,δr,Δb,ηb)T，觀測量z=δvobv，δvobv為系統(tǒng)速度誤差，則系統(tǒng)方程和觀測方程為：

(7)

z=Hx+V

(8)

式中,F是根據(jù)誤差模型和狀態(tài)矢量構(gòu)成的15×15維系統(tǒng)矩陣， H=[03×3I3×303×303×303×3]是3×15維觀測矩陣，W為15維系統(tǒng)隨機(jī)過程噪聲序列，V為三維系統(tǒng)隨機(jī)觀測噪聲序列。將系統(tǒng)模型線性化后，利用標(biāo)準(zhǔn)Kalman濾波可實(shí)現(xiàn)對誤差狀態(tài)的估計(jì)。

1.3零速修正算法

零速修正的思想就是在零速情況下將系統(tǒng)輸出的速度誤差作為觀測量對卡爾曼濾波器進(jìn)行更新，得到狀態(tài)的估計(jì)值，利用估計(jì)值對輸出的導(dǎo)航信息進(jìn)行修正，給出系統(tǒng)狀態(tài)的最優(yōu)估計(jì)[5]。

(9)

利用§1.2中介紹的擴(kuò)展卡爾曼估計(jì)可以得到速度誤差δvk和姿態(tài)誤差δγk、δθk、δψk，則通過零速修正后的姿態(tài)為：

(10)

(11)

2零速檢測算法

利用ZUPT進(jìn)行誤差修正的前提是能精確檢測出行人的零速狀態(tài)，現(xiàn)有的步態(tài)檢測算法大多數(shù)都是基于加速度和角速度信號進(jìn)行檢測的。本文主要介紹4種零速檢測數(shù)學(xué)模型。

假設(shè)fk=[fxkfykfzk]T、ωk=[ωxkωykωzk]T分別為k時(shí)刻加速度計(jì)和陀螺儀輸出。實(shí)際上，零速檢測的目的就是為了檢測一段時(shí)間內(nèi)MIMU處于零速狀態(tài)的時(shí)刻。假設(shè)在這段時(shí)間內(nèi)的采樣值共有N個(gè)，即窗口的大小為N。判斷MIMU是否處在零速狀態(tài)可以看作是一個(gè)假設(shè)檢驗(yàn)問題，即當(dāng)下式成立時(shí)，認(rèn)為MIMU為零速狀態(tài)：

(12)

式中，T(fk,ωk)為零速檢測的檢驗(yàn)統(tǒng)計(jì)量，L為閾值。檢驗(yàn)統(tǒng)計(jì)量形式的不同，會影響零速檢測的準(zhǔn)確率。這里總結(jié)了4種形式[6-8]。

1)加速度檢測法：

(13)

2)角速度檢測法：

(14)

3)加速度協(xié)方差檢測法：

(15)

4)組合檢測法：

(16)

式中，σf和σω是加速度計(jì)和陀螺儀隨機(jī)噪聲方差，‖·‖為二范數(shù)。

(17)

隨機(jī)噪聲起到了定權(quán)的作用。實(shí)際情況中，加速度計(jì)的隨機(jī)誤差要大于陀螺儀的觀測誤差，當(dāng)σf/σω→時(shí)，角速度檢測法是組合檢測的一種特殊形式。

滑動窗口和閾值的大小與采樣率和實(shí)際的行走速度有關(guān)，采樣率越大，行走速度越慢，則滑動窗口取值越大，根據(jù)統(tǒng)計(jì)量的形式不同，閾值也會有相應(yīng)的調(diào)整。

利用以上4種算法進(jìn)行3min的行走實(shí)驗(yàn),圖1為不同算法檢測20s的結(jié)果。

實(shí)驗(yàn)總共步行50步，利用上述4種算法分別正確檢測出47、48、48、50個(gè)零速狀態(tài)，從效率上來看，4種方法的效率差別不大，但組合檢測法檢測的正確率最高。不同檢測方法得到的每次零速時(shí)間不同，通過圖中柱體的粗細(xì)可以明顯地區(qū)分出來。具體的檢測精度可以通過實(shí)際行走實(shí)驗(yàn)在最終的平面精度中體現(xiàn)出來。

3實(shí)驗(yàn)精度分析

實(shí)測該數(shù)據(jù)由OPENSHOE提供,MIMU為ADIS公司生產(chǎn)，設(shè)備型號為ADIS16367。實(shí)驗(yàn)采樣率為250Hz，通過串口輸出原始數(shù)據(jù)，利用Matlab進(jìn)行數(shù)據(jù)處理。實(shí)驗(yàn)路線是閉合的，行走時(shí)間約3min，行走距離為189m，速度約3km/h。起點(diǎn)和終點(diǎn)重合，通過計(jì)算起點(diǎn)和終點(diǎn)的坐標(biāo)差驗(yàn)證實(shí)驗(yàn)精度。

利用不同檢測方法對采集數(shù)據(jù)進(jìn)行處理。設(shè)定σf=0.01m·s-2、σω=0.1*π/180rad·s-1,窗口大小和閾值見表1。

圖2給出了利用組合檢測法進(jìn)行零速檢測得到的行走軌跡。

不同零速檢測方法的平面精度見表2。由表2可知，4種檢測方法均可達(dá)到較高的精度，其中組合檢測法精度最高。角速度檢測法與組合檢測法精度近似，這是因?yàn)榧铀俣扔?jì)隨機(jī)誤差較大，在最優(yōu)組合法中角速度統(tǒng)計(jì)量所占的權(quán)重較大，而單純依靠加速度進(jìn)行零速檢測誤差較大。

為驗(yàn)證長時(shí)間行走時(shí)算法的可用性，進(jìn)行時(shí)間長度為23 min的實(shí)驗(yàn)，其中包括30 s的靜態(tài)數(shù)據(jù)采集，行走距離為750 m。實(shí)驗(yàn)中使用的儀器是荷蘭Xsens公司生產(chǎn)的Mti-g-700型MIMU，采樣頻率為100 Hz，圖3為行走軌跡。

由圖3看出，起點(diǎn)和終點(diǎn)坐標(biāo)差為24.718 m，誤差百分比僅為3%。所以，利用零速修正對慣導(dǎo)誤差進(jìn)行修正，在長時(shí)間長距離的行走實(shí)驗(yàn)中也能達(dá)到很好的效果。

4結(jié)語

本文主要介紹基于零速修正的行人導(dǎo)航算法，將零速時(shí)刻的系統(tǒng)輸出作為卡爾曼濾波觀測量估計(jì)系統(tǒng)誤差，改善導(dǎo)航精度?；诩铀俣扔?jì)和陀螺儀輸出的周期特性，總結(jié)并實(shí)現(xiàn)了4種零速檢測算法，4種算法都在較大程度上提高了慣導(dǎo)精度，其中利用加速度和角速度組合的檢測法精度最高。

參考文獻(xiàn)

[1]馬曉龍.基于MIMU的單兵室內(nèi)導(dǎo)航算法研究[D].長沙:國防科技大學(xué),2013(Ma Xiaolong. Research on the Navigation Algorithm of Individual-Soldier Indoor Positioning Based on MIMU[D].Changsha:National University of Defense Technology,2013)

[2]方靖,顧啟泰,丁天懷.車載慣性導(dǎo)航的動態(tài)零速修正技術(shù)[J].中國慣性技術(shù)學(xué)報(bào),2008,16(3):265-268(Fang Jing, Gu Qitai, Ding Tianhuai.Dynamic Zero Velocity Update for Vehicle Inertial Navigation System[J].Journal of Chinese Inertial Technology, 2008,16(3):265-268)

[3]El-Sheimy N. Inertial Techniques and INS/DGPS Integration[Z].ENGO 623-Course Notes, Department of Geomatics Engineering, University of Calgary, Canada, 2004

[4]Titterton D H,Weston J L.Strapdown Inertial Navigation Technology[M].Stevenage: The Institute of Electrical Engineers and the American Institute of Aeronautics and Astronautics, 2004

[5]趙小明,鄧東黎,蔣志煒.車載單軸旋轉(zhuǎn)激光捷聯(lián)慣導(dǎo)抗晃動初始對準(zhǔn)和零速修正方法[J].中國慣性技術(shù)學(xué)報(bào),2013,21(3):302-307(Zhao Xiaoming, Deng Dongli, Jiang Zhiwei.Anti-Disturbing Alignment and ZUPT Methods for Land Single-Axis Rotation Laser SINS [J].Journal of Chinese Inertial Technology,2013,21(3):302-307)

[6]Skog I, Nilsson J O, H?ndel P.Evaluation of Zero-Velocity Detectors for Foot-Mounted Inertial Navigation Systems[C].International Conference on Indoor Positioning and Indoor Navigation (IPIN), 2010

[7]Skog I, H?ndel P, Nilsson J O, et al. Zero-Velocity Detection—An Algorithm Evaluation[J]. IEEE Transactions on Biomedical Engineering, 2010, 57(11): 2 657-2 666

[8]Nilsson J O, Skog I, Handel P, et al. Foot-Mounted INS for Everybody-An Open-Source Embedded Implementation[C].Position Location and Navigation Symposium (PLANS), IEEE/ION, 2012

Foundation support:Open Fund of State Key Laboartory of Geographic Information Engineering, No.SKLGIE2015-M-2-5.

About the first author:TANG Wenjie, postgraduate, majors in pedestrian navigation algorithm, E-mail:twjhao123@sina.com.

Research of Pedestrian Navigation Algorithm Based on Zero Velocity Update Aided MIMU

TANGWenjie1LüZhiwei1ZHANGLundong1CONGDianwei1,2

1School of Navigation and Aerospace Engineering, Information Engineering University，62 Kexue Road, Zhengzhou 450001，China 2State Key Laboratory of Geo-information Engineering，1 Mid-Yanta Road, Xi’an 710054,China

Abstract:This paper studies a pedestrian navigation algorithm based on zero velocity update(ZUPT) aided MIMU. Considering the problem of the increasing error of MIMU over time,according to the assumption that the pedestrian’s velocity can maintain zero in a short time in every foot stance phase,the virtual noise observations are constructed to estimate system error. Noting that the zero-velocity detector is the key technology in ZUPT,we compare four detectors and evaluate their efficiency and accuracy.The four detectors are the acceleration moving variance detector, the acceleration detector, the angular rate detector, and a combined detector.The experimental results show that under the circumstance that the distance is short and step is slow,the four detectors can all reach high accuracy. However, the combined detector give the highest performance.

Key words:MIMU;ZUPT;zero-velocity detector;pedestrian navigation

收稿日期：2015-07-04

第一作者簡介：唐文杰，碩士生，研究方向?yàn)樾腥藢?dǎo)航算法，E-mail:twjhao123@sina.com。

DOI：10.14075/j.jgg.2016.06.018

文章編號：1671-5942(2016)06-0548-04

中圖分類號：P227

文獻(xiàn)標(biāo)識碼：A

項(xiàng)目來源：地理信息工程國家重點(diǎn)實(shí)驗(yàn)室開放基金(SKLGIE2015-M-2-5)。