王立兵，楊松普，羅 巍，皮燕燕

（1. 中國(guó)人民解放軍63961部隊(duì)，北京 100012；2. 天津航海儀器研究所，天津 300131）

足部安裝MEMS-IMU個(gè)人導(dǎo)航系統(tǒng)

王立兵1，楊松普2，羅 巍2，皮燕燕2

（1. 中國(guó)人民解放軍63961部隊(duì)，北京 100012；2. 天津航海儀器研究所，天津 300131）

為了實(shí)現(xiàn)室內(nèi)外環(huán)境下個(gè)人自主導(dǎo)航，研究了足部安裝的MEMS-IMU個(gè)人導(dǎo)航系統(tǒng)。根據(jù)人行走時(shí)足部具有周期性零速的特征，以加速度計(jì)輸出矢量和、滑動(dòng)方差和陀螺儀輸出的角速度矢量和為檢測(cè)量，設(shè)計(jì)了一種多條件零速檢測(cè)算法，有效地提高了零速檢測(cè)的準(zhǔn)確性。針對(duì)MEMS慣性傳感器零漂大、精度低的問(wèn)題，導(dǎo)航定位算法以傳統(tǒng)的捷聯(lián)解算算法為基礎(chǔ)，進(jìn)行了適應(yīng)性改進(jìn)。引入零速修正（ZUPT）技術(shù)，設(shè)計(jì)了以速度信息作為偽量測(cè)的Kalman濾波器。在零速階段對(duì)系統(tǒng)速度，姿態(tài)，位置誤差進(jìn)行估計(jì)，將估計(jì)結(jié)果反饋以修正導(dǎo)航解算的累積誤差。實(shí)驗(yàn)結(jié)果表明，基于上述導(dǎo)航修正算法可以有效地消除MEMS慣性傳感器零漂引起的累積誤差，使得多組多種行走路徑下系統(tǒng)的定位誤差均小于行程的2%。

個(gè)人導(dǎo)航系統(tǒng)；MEMS；零速修正；零速檢測(cè)

個(gè)人導(dǎo)航系統(tǒng)（Pedestrian Navigation System，PNS）是指實(shí)時(shí)確定行人行進(jìn)方向、速度、位置等信息的系統(tǒng)[1]。一般采用各種傳感器模塊測(cè)量行人運(yùn)動(dòng)參數(shù)，經(jīng)過(guò)導(dǎo)航解算實(shí)時(shí)提供可視的導(dǎo)航信息。在消防救援，礦井勘探，單兵作戰(zhàn)等領(lǐng)域，能夠已知人員的位置信息，對(duì)于保證人員安全，提高救援速度，增強(qiáng)作戰(zhàn)能力等具有重要意義。

傳統(tǒng)的衛(wèi)星導(dǎo)航具有全方位實(shí)時(shí)定位功能，但在衛(wèi)星信號(hào)不可用時(shí)，無(wú)法定位，因此只適用于室外無(wú)遮擋的環(huán)境。MEMS慣性器件的快速發(fā)展及其成本低，體積小，重量輕，功耗低的特點(diǎn)，使得基于MEMS-IMU的個(gè)人導(dǎo)航系統(tǒng)成為研究熱點(diǎn)。文獻(xiàn)[2] 研究利用兩軸數(shù)字羅盤和三軸加速度計(jì)測(cè)量行人的速度和航向，采用航位推算（PDR）算法進(jìn)行定位；文獻(xiàn)[3-5]研究了將MEMS IMU固定在腳部，采用經(jīng)典捷聯(lián)慣導(dǎo)解算方案進(jìn)行定位，利用人行走時(shí)的步態(tài)特征，系統(tǒng)檢測(cè)到腳部為零速時(shí)，即進(jìn)行零速修正以校正位置誤差；文獻(xiàn)[6]介紹了一種將IMU固定在腰部，采用PDR算法，并利用人行走時(shí)的動(dòng)態(tài)模型作為虛擬速度傳感器來(lái)抑制定位誤差；文獻(xiàn)[7]根據(jù)典型計(jì)步器原理及步長(zhǎng)估算獲取位置信息，利用磁力計(jì)得到航向角進(jìn)行航位推算（PDR），室內(nèi)定位精度在5%以內(nèi)。

圖1為本文個(gè)人導(dǎo)航系統(tǒng)的功能組成。以傳統(tǒng)的捷聯(lián)慣性導(dǎo)航解算算法為基礎(chǔ)，經(jīng)過(guò)適應(yīng)性改進(jìn)完成導(dǎo)航定位解算。然后根據(jù)人行走時(shí)的步態(tài)特征設(shè)計(jì)了零速檢測(cè)（ZVD）算法，檢測(cè)行走過(guò)程中的零速區(qū)間。最后，以速度信息為偽量測(cè)建立Kalman濾波器，進(jìn)行零速修正提高系統(tǒng)定位精度。

圖1 系統(tǒng)功能框圖Fig.1 Block chart of system function

1 導(dǎo)航定位算法

以初始位置作為坐標(biāo)原點(diǎn)，建立當(dāng)?shù)厮阶鴺?biāo)系（n系）。利用加速度計(jì)輸出計(jì)算初始水平姿態(tài)角，計(jì)算公式為

選取當(dāng)?shù)厮阶鴺?biāo)系為導(dǎo)航坐標(biāo)系，b系為MEMS-IMU坐標(biāo)系。慣性導(dǎo)航微分方程如下所示：

2 零速檢測(cè)與零速區(qū)間確定

如圖2所示，人行走時(shí)的步態(tài)可以描述為：腳跟觸地（Heel strike）→腳跟和腳尖都觸地（Foot flat）→站立（Mid-stance）→腳跟離地（Push off）→腳尖離地（Toe off）的循環(huán)往復(fù)過(guò)程[7]。在站立階段，腳部與地面完全接觸，處于靜止?fàn)顟B(tài)，即在站立階段，腳部速度為零。因此可以將零速作為卡爾曼濾波器的量測(cè)信息。

圖2 人行走的步態(tài)周期Fig.2 Human gait cycle

如何有效、準(zhǔn)確地檢測(cè)出零速階段是零速修正的前提和基礎(chǔ)。固定在腳部的MEMS-IMU能夠敏感行走過(guò)程中加速度和角速度的變化，可以利用其輸出進(jìn)行步態(tài)識(shí)別。為了得到精確地零速區(qū)間，本文設(shè)計(jì)了一種多條件檢測(cè)算法，通過(guò)同時(shí)利用加速度計(jì)輸出的比力信息和陀螺儀的輸出角速度信息進(jìn)行零速檢測(cè)。整個(gè)檢測(cè)算法包括三個(gè)條件：加速度幅值、加速度滑動(dòng)方差[9]和角速度幅值，分別記為C1、C2和C3。各條件實(shí)現(xiàn)方式如下：

① 加速度幅值檢測(cè)法（Acceleration Magnitude Detector，AMD）。在零速階段，人腳的合加速度值在重力加速度附近變化。通過(guò)判斷k時(shí)刻三軸加速度計(jì)的輸出矢量和是否在設(shè)定的閾值范圍內(nèi)，即可得到該時(shí)刻的檢測(cè)結(jié)果。

② 加速度滑動(dòng)方差檢測(cè)法（Acceleration Moving Variance Detector，AMVD）。零速階段和擺動(dòng)階段的加速度波動(dòng)幅度以及劇烈程度差別很大，因此可以通過(guò)設(shè)置一定大小的滑動(dòng)窗口，求取該窗口范圍內(nèi)數(shù)據(jù)點(diǎn)的方差來(lái)表示數(shù)據(jù)的波動(dòng)，進(jìn)而判斷是否為零速階段。

③ 角速度幅值檢測(cè)法（Angular Rate Magnitude Detector，ARMD）。與合加速度幅值檢測(cè)方法類似。在零速階段，人腳的角速度變化趨于零。若k 時(shí)刻合角速度的模值ω 小于設(shè)定的閾值，即可認(rèn)為該時(shí)刻為零速；反之，該時(shí)刻為非零速。

加速度幅值檢測(cè)法的靜態(tài)檢測(cè)效果較好。但在擺動(dòng)階段合加速度有較大波動(dòng)，容易將擺動(dòng)階段檢測(cè)為零速階段，造成誤檢測(cè)。加速度滑動(dòng)方差檢測(cè)法對(duì)滑動(dòng)窗口大小的選取要求較高，窗口過(guò)大或者過(guò)小都會(huì)造成誤檢測(cè)。角速度幅值檢測(cè)法靜態(tài)檢測(cè)效果比加速度幅值檢測(cè)法要好。但也存在一定程度的誤檢測(cè)。為了提高檢測(cè)的準(zhǔn)確性，采用多條件復(fù)合檢測(cè)方法，即將以上三種方法進(jìn)行“與”運(yùn)算：

圖3分別為單一條件零速檢測(cè)和多條件零速檢測(cè)的實(shí)驗(yàn)結(jié)果。可以看出，單一條件檢測(cè)存在明顯誤檢測(cè)，而多條件復(fù)合檢測(cè)綜合了比力和加速度兩種信息，檢測(cè)的準(zhǔn)確性大大提高。所以，本文設(shè)計(jì)的多條件零速檢測(cè)算法可以有效地檢測(cè)出零速階段。

圖3 單一條件與多條件零速檢測(cè)結(jié)果Fig.3 ZVD results under single-condition and multi-condition

3 零速修正

在零速階段人腳的速度為零，因此可以將零速作為外參考速度，建立Kalman濾波方程。選取誤差量X=[δφnδvnδpnεb?b]T為狀態(tài)量，其中，δφn為姿態(tài)誤差角，δvn為速度誤差，δpn為位置誤差，εb為陀螺常值漂移，?b為加速度計(jì)常值漂移。以經(jīng)典捷聯(lián)慣導(dǎo)誤差方程為狀態(tài)方程[10-11]，量測(cè)量取零速階段解算的速度誤差δv。Kalman濾波器可以表示為：

由于只有在零速階段，外參考速度才為零，所以在擺動(dòng)階段只進(jìn)行Kalman濾波一步預(yù)測(cè)，當(dāng)檢測(cè)到零速時(shí)，再進(jìn)行完整的Kalman濾波更新（一步預(yù)測(cè)和量測(cè)更新）。

4 實(shí)驗(yàn)驗(yàn)證

4.1 系統(tǒng)組成

實(shí)驗(yàn)采用Sensonor公司Stim系列MEMS IMU，其內(nèi)部集成了經(jīng)過(guò)工廠校準(zhǔn)的三個(gè)加速度計(jì)和三個(gè)陀螺儀，IMU尺寸為38.6 mm×44.8 mm×21.5 mm。圖4為系統(tǒng)實(shí)物圖。

圖4 系統(tǒng)實(shí)物圖Fig.4 Picture of the system

4.2 步行實(shí)驗(yàn)

為了驗(yàn)證算法的正確性和有效性，設(shè)計(jì)了直線往返、方形和八字形三種路線。直線往返路線總路程約170 m，方形路線總路程約448 m，八字形路線總路程約604 m。每次步行實(shí)驗(yàn)起始點(diǎn)和終止點(diǎn)為同一位置。實(shí)驗(yàn)過(guò)程中，將MEMS-IMU固定在左腳的腳面位置,與筆記本電腦經(jīng)過(guò)RS-422串口連接，實(shí)時(shí)錄取和顯示導(dǎo)航解算結(jié)果，并用Matlab繪制行走軌跡。圖5、圖6和圖7分別為直線往返、方形路線和八字形路線的導(dǎo)航結(jié)果，圓圈為導(dǎo)航起始點(diǎn)，方框?yàn)閷?dǎo)航終點(diǎn)。

圖5 直線往返行走軌跡Fig.5 Trajectory of walking along straight line

圖6 方形路線行走軌跡Fig.6 Trajectory of walking along square route

圖7 八字形路線行走軌跡Fig.7 Trajectory of walking along figure-eight-shaped route

由圖5～7可以看出，通過(guò)引入零速修正技術(shù)，對(duì)系統(tǒng)姿態(tài)誤差、速度誤差和位置誤差進(jìn)行估計(jì)，并將估計(jì)結(jié)果反饋，可以得到較好的導(dǎo)航結(jié)果。表1為各次行走的實(shí)驗(yàn)結(jié)果，定位誤差均在行程的2%以內(nèi)。

表1 行走實(shí)驗(yàn)結(jié)果Tab.1 Results of walking experiments

5 結(jié) 論

本文設(shè)計(jì)了基于MEMS-IMU的個(gè)人導(dǎo)航算法。通過(guò)將IMU固定在腳部，綜合利用加速度計(jì)和陀螺儀的輸出信息進(jìn)行零速檢測(cè)。在此基礎(chǔ)上，以零速階段的速度作為量測(cè)建立了Kalman濾波方程，對(duì)系統(tǒng)姿態(tài)誤差、速度誤差和位置誤差進(jìn)行估計(jì)（零速修正），有效地修正了MEMS傳感器零漂帶來(lái)的累積誤差，提高了導(dǎo)航定位精度。除了正常行走，人在現(xiàn)實(shí)生活中有多種運(yùn)動(dòng)方式，如跑步、跳躍、爬行等。本文提出的算法還不能適應(yīng)所有的運(yùn)動(dòng)形式，可以考慮與超聲波、壓力傳感器、磁力計(jì)等組合，實(shí)現(xiàn)不同條件下的導(dǎo)航定位。

（References）：

[1] 張曉東. 基于MEMS慣性器件的個(gè)人導(dǎo)航系統(tǒng)研究[D].北京: 北京工業(yè)大學(xué)碩士學(xué)位論文, 2015. Zhang Xiao-dong. Study on personal navigation system based on MEMS inertial devices[D]. Beijing: Beijing University of Technology, 2015.

[2] 陳偉. 基于GPS和自包含傳感器的行人室內(nèi)外無(wú)縫定位算法研究[D]. 合肥: 中國(guó)科學(xué)技術(shù)大學(xué), 2010. Chen Wei. Research on GPS/self-contained sensors based seamless outdoor/indoor pedestrian positioning algorithm [D]. Hefei: PhD Thesis of University of Science and Technology of China, 2010.

[3] Alvarez J C, Alvarez D, Lopez A, et al. Pedestrian navigation based on a waist-worn inertial sensor[J]. Sensors, 2012, 12: 10536-10549.

[4] Feliz R, Zalama E. Pedestrian tracking using inertial sensors[J]. Journal of Physical Agents, 2009(3): 35-43.

[5] Fourati H. Heterogeneous data fusion algorithm for pedestrian navigation via foot-mounted inertial measurement unit and complementary filter[J]. IEEE Transaction on Instrumentation and Measurement, 2015. 221-229.

[6] 宋敏, 申閆春. 室內(nèi)定位航位推測(cè)算法的研究與實(shí)[J].計(jì)算機(jī)工程, 2013, 39(7): 293-301. Song Min, Shen Yan-chun. Research and realization of dead reckoning algorithm for indoor localization[J]. Computer Engineering, 2013, 39(7): 293-301.

[7] Davidson P, Takala J. Algorithm for pedestrian navigation combining IMU measurements and gait models[J]. Gyroscopy and Navigation, 2013, 4(2): 79-84.

[8] Bebek O, Suster M A, Rajgopal, et al. Personal navigation via shoe mounted inertial measurement units [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. 2010: 1052-1058.

[9] 張金亮, 秦永元, 梅春波. 基于 MEMS慣性技術(shù)的鞋式個(gè)人導(dǎo)航系統(tǒng)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2011, 19(3), 253-256. Zhang Jin-liang, Qin Yong-yuan, Mei Chun-bo. Shoemounted personal navigation system based on MEMS inertial technology[J]. Journal of Chinese Inertial Technology, 2011, 19(3): 253-256.

[10] Xu Yuan, Chen Xi-yuan, Li Qing-hua, et al. Indoor pedestrian navigation based on double-IMU framework[J]. Journal of Chinese Inertial Technology, 2015, 23(6): 714-717.

[11] 張新喜, 張嶸, 郭美鳳, 等. 足綁式行人導(dǎo)航偏航角誤差自觀測(cè)算法(英文)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2015, 23(4): 457-466. Zhang Xin-xi, Zhang Rong, Guo Mei-feng, et al. Yaw error self-observation algorithm for pedestrian navigation via foot-mounted inertial navigation system[J]. Journal of Chinese Inertial Technology, 2015, 23(4): 457-466.

Pedestrian navigation algorithm based on MEMS-IMU

WANG Li-bing1, YANG Song-pu2, LUO Wei2, PI Yan-yan2
(1. Troop 63961 of PLA, Beijing 100012, China; 2. Tianjin Navigation Instrument Research Institute, Tianjin 300131, China)

The pedestrian navigation system (PNS) based on foot-mounted MEMS-IMU is studied. According to the gait model, the foot has to be stationary periodically in between steps during walking. To detect the stance phase, a zero velocity detection algorithm is proposed by using the vectorial sum of accelerometer output, the sliding variance, and the vectorial sum of gyro’s angular rate as measurements. As the position error of SINS diverges with time, the zero velocity updating (ZUPT) technique is implemented, and the attitude error, the velocity error and the position error estimated by the designed Kalman filter are fedback to correct the accumulated errors of SINS. Experiment results show that the designed amendment navigation algorithm can efficiently reduce the drift error of the MEMS inertial sensors, and the PNS position accuracy can reach 2% of travel distance under various walking path situations.

pedestrian navigation system; MEMS; zero velocity updating; zero velocity detection

U666.1

：A

2016-06-26；

：2016-07-29

國(guó)防基礎(chǔ)科研重點(diǎn)項(xiàng)目（A0320132002）

王立兵（1982—），男，博士，工程師，從事測(cè)繪導(dǎo)航技術(shù)研究。E-mail: chunqiwang@126.com

1005-6734(2016)04-0460-04

10.13695/j.cnki.12-1222/o3.2016.04.008