足部安装MEMS-IMU个人导航系统

王立兵，杨松普，罗 巍，皮燕燕

（1. 中国人民解放军63961部队，北京 100012；2. 天津航海仪器研究所，天津 300131）

足部安装MEMS-IMU个人导航系统

王立兵1，杨松普2，罗 巍2，皮燕燕2

（1. 中国人民解放军63961部队，北京 100012；2. 天津航海仪器研究所，天津 300131）

为了实现室内外环境下个人自主导航，研究了足部安装的MEMS-IMU个人导航系统。根据人行走时足部具有周期性零速的特征，以加速度计输出矢量和、滑动方差和陀螺仪输出的角速度矢量和为检测量，设计了一种多条件零速检测算法，有效地提高了零速检测的准确性。针对MEMS惯性传感器零漂大、精度低的问题，导航定位算法以传统的捷联解算算法为基础，进行了适应性改进。引入零速修正（ZUPT）技术，设计了以速度信息作为伪量测的Kalman滤波器。在零速阶段对系统速度，姿态，位置误差进行估计，将估计结果反馈以修正导航解算的累积误差。实验结果表明，基于上述导航修正算法可以有效地消除MEMS惯性传感器零漂引起的累积误差，使得多组多种行走路径下系统的定位误差均小于行程的2%。

个人导航系统；MEMS；零速修正；零速检测

个人导航系统（Pedestrian Navigation System，PNS）是指实时确定行人行进方向、速度、位置等信息的系统[1]。一般采用各种传感器模块测量行人运动参数，经过导航解算实时提供可视的导航信息。在消防救援，矿井勘探，单兵作战等领域，能够已知人员的位置信息，对于保证人员安全，提高救援速度，增强作战能力等具有重要意义。

传统的卫星导航具有全方位实时定位功能，但在卫星信号不可用时，无法定位，因此只适用于室外无遮挡的环境。MEMS惯性器件的快速发展及其成本低，体积小，重量轻，功耗低的特点，使得基于MEMS-IMU的个人导航系统成为研究热点。文献[2] 研究利用两轴数字罗盘和三轴加速度计测量行人的速度和航向，采用航位推算（PDR）算法进行定位；文献[3-5]研究了将MEMS IMU固定在脚部，采用经典捷联惯导解算方案进行定位，利用人行走时的步态特征，系统检测到脚部为零速时，即进行零速修正以校正位置误差；文献[6]介绍了一种将IMU固定在腰部，采用PDR算法，并利用人行走时的动态模型作为虚拟速度传感器来抑制定位误差；文献[7]根据典型计步器原理及步长估算获取位置信息，利用磁力计得到航向角进行航位推算（PDR），室内定位精度在5%以内。

图1为本文个人导航系统的功能组成。以传统的捷联惯性导航解算算法为基础，经过适应性改进完成导航定位解算。然后根据人行走时的步态特征设计了零速检测（ZVD）算法，检测行走过程中的零速区间。最后，以速度信息为伪量测建立Kalman滤波器，进行零速修正提高系统定位精度。

图1 系统功能框图Fig.1 Block chart of system function

1 导航定位算法

以初始位置作为坐标原点，建立当地水平坐标系（n系）。利用加速度计输出计算初始水平姿态角，计算公式为

选取当地水平坐标系为导航坐标系，b系为MEMS-IMU坐标系。惯性导航微分方程如下所示：

2 零速检测与零速区间确定

如图2所示，人行走时的步态可以描述为：脚跟触地（Heel strike）→脚跟和脚尖都触地（Foot flat）→站立（Mid-stance）→脚跟离地（Push off）→脚尖离地（Toe off）的循环往复过程[7]。在站立阶段，脚部与地面完全接触，处于静止状态，即在站立阶段，脚部速度为零。因此可以将零速作为卡尔曼滤波器的量测信息。

图2 人行走的步态周期Fig.2 Human gait cycle

如何有效、准确地检测出零速阶段是零速修正的前提和基础。固定在脚部的MEMS-IMU能够敏感行走过程中加速度和角速度的变化，可以利用其输出进行步态识别。为了得到精确地零速区间，本文设计了一种多条件检测算法，通过同时利用加速度计输出的比力信息和陀螺仪的输出角速度信息进行零速检测。整个检测算法包括三个条件：加速度幅值、加速度滑动方差[9]和角速度幅值，分别记为C1、C2和C3。各条件实现方式如下：

① 加速度幅值检测法（Acceleration Magnitude Detector，AMD）。在零速阶段，人脚的合加速度值在重力加速度附近变化。通过判断k时刻三轴加速度计的输出矢量和是否在设定的阈值范围内，即可得到该时刻的检测结果。

② 加速度滑动方差检测法（Acceleration Moving Variance Detector，AMVD）。零速阶段和摆动阶段的加速度波动幅度以及剧烈程度差别很大，因此可以通过设置一定大小的滑动窗口，求取该窗口范围内数据点的方差来表示数据的波动，进而判断是否为零速阶段。

③ 角速度幅值检测法（Angular Rate Magnitude Detector，ARMD）。与合加速度幅值检测方法类似。在零速阶段，人脚的角速度变化趋于零。若k 时刻合角速度的模值ω 小于设定的阈值，即可认为该时刻为零速；反之，该时刻为非零速。

加速度幅值检测法的静态检测效果较好。但在摆动阶段合加速度有较大波动，容易将摆动阶段检测为零速阶段，造成误检测。加速度滑动方差检测法对滑动窗口大小的选取要求较高，窗口过大或者过小都会造成误检测。角速度幅值检测法静态检测效果比加速度幅值检测法要好。但也存在一定程度的误检测。为了提高检测的准确性，采用多条件复合检测方法，即将以上三种方法进行“与”运算：

图3分别为单一条件零速检测和多条件零速检测的实验结果。可以看出，单一条件检测存在明显误检测，而多条件复合检测综合了比力和加速度两种信息，检测的准确性大大提高。所以，本文设计的多条件零速检测算法可以有效地检测出零速阶段。

图3 单一条件与多条件零速检测结果Fig.3 ZVD results under single-condition and multi-condition

3 零速修正

在零速阶段人脚的速度为零，因此可以将零速作为外参考速度，建立Kalman滤波方程。选取误差量X=[δφnδvnδpnεb∇b]T为状态量，其中，δφn为姿态误差角，δvn为速度误差，δpn为位置误差，εb为陀螺常值漂移，∇b为加速度计常值漂移。以经典捷联惯导误差方程为状态方程[10-11]，量测量取零速阶段解算的速度误差δv。Kalman滤波器可以表示为：

由于只有在零速阶段，外参考速度才为零，所以在摆动阶段只进行Kalman滤波一步预测，当检测到零速时，再进行完整的Kalman滤波更新（一步预测和量测更新）。

4 实验验证

4.1 系统组成

实验采用Sensonor公司Stim系列MEMS IMU，其内部集成了经过工厂校准的三个加速度计和三个陀螺仪，IMU尺寸为38.6 mm×44.8 mm×21.5 mm。图4为系统实物图。

图4 系统实物图Fig.4 Picture of the system

4.2 步行实验

为了验证算法的正确性和有效性，设计了直线往返、方形和八字形三种路线。直线往返路线总路程约170 m，方形路线总路程约448 m，八字形路线总路程约604 m。每次步行实验起始点和终止点为同一位置。实验过程中，将MEMS-IMU固定在左脚的脚面位置,与笔记本电脑经过RS-422串口连接，实时录取和显示导航解算结果，并用Matlab绘制行走轨迹。图5、图6和图7分别为直线往返、方形路线和八字形路线的导航结果，圆圈为导航起始点，方框为导航终点。

图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可以看出，通过引入零速修正技术，对系统姿态误差、速度误差和位置误差进行估计，并将估计结果反馈，可以得到较好的导航结果。表1为各次行走的实验结果，定位误差均在行程的2%以内。

表1 行走实验结果Tab.1 Results of walking experiments

5 结 论

本文设计了基于MEMS-IMU的个人导航算法。通过将IMU固定在脚部，综合利用加速度计和陀螺仪的输出信息进行零速检测。在此基础上，以零速阶段的速度作为量测建立了Kalman滤波方程，对系统姿态误差、速度误差和位置误差进行估计（零速修正），有效地修正了MEMS传感器零漂带来的累积误差，提高了导航定位精度。除了正常行走，人在现实生活中有多种运动方式，如跑步、跳跃、爬行等。本文提出的算法还不能适应所有的运动形式，可以考虑与超声波、压力传感器、磁力计等组合，实现不同条件下的导航定位。

（References）：

[1] 张晓东. 基于MEMS惯性器件的个人导航系统研究[D].北京: 北京工业大学硕士学位论文, 2015. Zhang Xiao-dong. Study on personal navigation system based on MEMS inertial devices[D]. Beijing: Beijing University of Technology, 2015.

[2] 陈伟. 基于GPS和自包含传感器的行人室内外无缝定位算法研究[D]. 合肥: 中国科学技术大学, 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] 宋敏, 申闫春. 室内定位航位推测算法的研究与实[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惯性技术的鞋式个人导航系统[J]. 中国惯性技术学报, 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] 张新喜, 张嵘, 郭美凤, 等. 足绑式行人导航偏航角误差自观测算法(英文)[J]. 中国惯性技术学报, 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

国防基础科研重点项目（A0320132002）

王立兵（1982—），男，博士，工程师，从事测绘导航技术研究。E-mail: chunqiwang@126.com

1005-6734(2016)04-0460-04

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