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

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

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

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

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

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

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

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

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

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

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

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

以初始位置作為坐標原點，建立當?shù)厮阶鴺讼担╪系）。利用加速度計輸出計算初始水平姿態(tài)角，計算公式為

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

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

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

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

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

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

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

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

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

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

圖3 單一條件與多條件零速檢測結(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īng)典捷聯(lián)慣導(dǎo)誤差方程為狀態(tài)方程[10-11]，量測量取零速階段解算的速度誤差δv。Kalman濾波器可以表示為：

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

4 實驗驗證

4.1 系統(tǒng)組成

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

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

4.2 步行實驗

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

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

表1 行走實驗結(jié)果Tab.1 Results of walking experiments

5 結(jié) 論

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

（References）：

[1] 張曉東. 基于MEMS慣性器件的個人導(dǎo)航系統(tǒng)研究[D].北京: 北京工業(yè)大學碩士學位論文, 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)外無縫定位算法研究[D]. 合肥: 中國科學技術(shù)大學, 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)定位航位推測算法的研究與實[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ù)的鞋式個人導(dǎo)航系統(tǒng)[J]. 中國慣性技術(shù)學報, 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)航偏航角誤差自觀測算法(英文)[J]. 中國慣性技術(shù)學報, 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

國防基礎(chǔ)科研重點項目（A0320132002）

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

1005-6734(2016)04-0460-04

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