一种优化的零速检测行人导航算法

孟祥宾，潘献飞，胡小平

（国防科学技术大学机电工程与自动化学院，长沙410073）

一种优化的零速检测行人导航算法

孟祥宾，潘献飞，胡小平

（国防科学技术大学机电工程与自动化学院，长沙410073）

针对军事上对单兵定位和民用领域为行人提供位置服务的需求，提出了足部固定MIMU的行人导航算法。利用人体行走过程中脚部与地面相接触的静止时间段，采用零速更新（Zero Velocity Update，ZVU）对INS继续对准或标定，而零速检测又是实现零速修正的基础。研究对比了在足部固定MIMU进行零速检测的常用方法，针对常用算法阈值自适应性差，无法检测步速变化转弯等一些异常情况对行人零速检测产生误判等问题，提出优化的零速判断算法，使得加速度计信号和陀螺仪信号综合应用到零速检测。最后，利用改进算法设计行人室内平地正常行走（大约5km/h）包含步速变化和转弯过程的实验。实验结果表明，优化的零速检测算法相比之前的常用算法，能更准确地检测零速并进行修正，对导航轨迹和导航误差有更好的修正效果。

行人导航；MIMU；零速检测；检测算法

0　引言

行人导航（Pedestrian Navigation，PN）是指用户通过负载一定的传感器设备完成导航和实时定位的功能，同时可以选择携带无线通信设备将导航信息传输给监控人员［1］，是未来导航领域最具挑战性的应用之一。新兴的行人导航技术能够为复杂环境下实施抢险救援、楼宇消防、反恐作战的人员提供准确的导航信息，又可以为普通市民在大型超市等室内场所提供准确的位置服务，避免人员走失等事故。

微惯性测量单元（Micro Inertial Measurement Unit，MIMU）具有体积小、价格低、功耗低及易携带等优点，是自主式行人导航的理想器件。实际应用中，MIMU可以负载在人体的腰部、足部、手腕等部位［2］。结合人体运动特点，MIMU负载在人体足部，利用人正常行走过程中，足部接触地面的零速作为观测量来修正系统误差，即零速修正的方法是一种比较常见的行人导航方法。我们可以很好地利用零速时间段来对行人的位置进行修正。因此，准确地检测出零速时刻成为零速修正的基础。

因为MIMU能够为用户提供加速度和角速度信息，很多学者都借助这两种信息的统计特征，利用信号的周期性来实现步态检测。Jimenez在步态识别中利用了角速度信息，取得了较好的检测效果［3］。西北工业大学的秦永元老师采用对加速度信息求取滑动方差，用固定阈值来对方差值依次比较，进而分辨出行人步态的摆动和着地静止阶段［4］，但常用算法多是采用加速度计输出信号，实验多限于简单的直线，步速稳定的情况。给出零速判断大致结果。还有学者利用其他传感装置辅助检测，比如压电传感器装置［5］、肌电图等。精度虽然比利用传统的MIMU提供信息要高，但要付出更大的代价和成本。

本文仅利用足部固定MIMU导航系统，分析零速检测的三种方法：加速度移动方差检测、加速度幅值检测和角速率能量检测方法。最后提出一种可以综合三种算法，将加速度计和陀螺仪输出信号一起用于零速检测的算法。解决步速变化、转弯、足部异常运动造成角速率变大引起的零速误判问题，最大化地利用惯性传感器的输出信号，结果表明，新算法能够更精确地实现零速检测，能够进一步地提高行人导航算法的精度和实用性。

1　行人导航算法总体框架

1.1算法编排

图1　导航编排框图Fig.1 The blocks of navigation mechanism

1.2扩展卡尔曼滤波器设计

扩展卡尔曼滤波器是实现零速修正的方式，在此考虑建立以系统误差为状态的卡尔曼滤波模型［6］。

系统的状态量取：

式中，δφ、δv、δr分别为惯导器件的姿态误差角向量、导航系下速度误差向量和位置误差向量。Δb、Δf、ηb、ηf分别为加速度计的零偏误差和刻度因子误差以及陀螺的零偏误差和刻度因子误差。状态模型有：

式中，ωin为导航系相对惯性系的旋转角速度，ωen为导航系相对地球的旋转角速度，η为陀螺输出误差，Δ为加速度计输出误差。

选取北东地（N，E，D）坐标系为导航系，系统方程为：

式中，F为系统转移矩阵，W为系统噪声。F可通过对式（2）线性化获得。观测方程有：

利用建立的系统模型进行卡尔曼滤波，对误差状态量进行估计，实现零速修正。

可见，零速检测和修正是实现精确行人导航的基础，本文将重点研究几种常用算法的优缺点同时设计改进算法。

1.3常用零速检测算法

常用的零速检测算法思路是：取定一个适当的滑动窗口，随着时间的推移遍历整个加速度数据，并对每一次窗口内的加速度序列求取方差，利用已经设定好的阈值对结果进行筛选。若大于阈值，则认为此刻脚步处于摆动，否则认为处于静止零速状态［3］。目前，大多数学者根据人体行走的步态特征，设置多个阈值用来检测不同步速下的零速状态，构建出适应多步态的零速检测算法。

但是，常用的零速检测算法存在一定的局限性。第一，加速度阈值单一，对不同行走状态自适应性程度不高；第二，行人在正常行走时，可能出现的角速率值突变、步速加快等干扰，而加速度方差曲线规律性变化消失，不能很好地给出零速判断结果。

图2给出了一组传统算法解算的零速检测结果，在检测结果的后半段，由于足部的运动状态出现加速变化，使得用于求取加速度计平均方差用的零速检测方法出现误判，事先设定的加速度固定阈值也无法满足这种步态变化对零速检测带来的影响。为了更好地进行零速检测就需要改进阈值设置办法，设置更多的限制约束条件来处理一些正常行走状态可能出现的加速、转弯等使零速检测出现误判情况的运动状态。自适应阈值、陀螺仪输出数据能够体现这些变化带来的影响，基于这样一个事实，我们对常用零速检测算法进行优化。

综合考虑，我们将加速度方差、幅值、阈值设置以及角速度信号一起作为零速检测的判断标准，设计优化的零速检测算法。

图2　常用算法得出的一组零速检测结果Fig.2 A result of stance phase detection of traditional algorithm

2　优化的零速检测算法

2.1优化零速检测算法基本思路

本文优化零速检测算法的思路是：根据行人步态特征的先验条件和传感器输出信号，在常用加速度移动方差检测方法基础上，引入加速度幅值检测和角速率能量检测两种方法进行综合判断，增加了零速判断的限制约束条件，由实际经验以及相关文献［7］可知，加速度移动方差检测对于正常步速直线行走的零速判断效果较好。加速度幅值检测对于行人大步、步速稍快条件下零速检测辅助效果较好。角速率能量检测主要是针对陀螺信号处理，对于行走时可能出现的足部左右晃动异常引起的角速率值突变以及转弯过程零速检测效果明显。这样，将加速度计输出信息和陀螺仪输出信息综合用于零速检测，使我们能够更准确地判断出零速时刻，进行零速修正，降低由于微惯性测量单元精度低带来的导航误差漂移影响。

优化零速检测算法可以自适应地实现对加速行走（大于5km/h）、角速率值突变和转弯等一些步态的零速检测。算法流程图如图3所示。

2.2优化算法执行步骤及理论框架

步骤1：根据原始加速度计、陀螺仪数据特征初步判断行人在常速行走时出现加速行走、迈大步和角速率值突变等运动状态，根据结果选择最适合的零速判断标准。

步骤2：优化算法采用自适应的算法设置阈值，与以往传统固定阈值相比，算法阈值自适应变化，可以从前1～10步行走数据设置阈值。以行人第6步开始为例，加速度方差算法为：Var1，Var2，…，Var6分别为这6步的最小方差，令阈值γv=max（VarN-5，VarN-4，…，VarN），相比于单一阈值，改进算法的阈值设置可以根据步态特征自适应变化。加速度峰值、角速率能量检测器阈值γm和γω设置参照此方法。

步骤3：行人大部分时间都是沿直线行走，首先采用加速度滑动方差方法进行判断，利用MIMU采集到的原始加速度数据，构建一个滑动窗口，让窗口随着加速度信息的采集依次对数据进行遍历，求取加速度移动方差：

当满足式（5）时，初步判断为零速静止状态，执行步骤2；不满足式（5）则判断此刻为非零速。

步骤4：将加速度幅值零速检测方法应用于辅助加速度移动方差检测，用于改进行走过程中大步或者步速稍快状态下零速检测的误判。该算法基本思路是：检测量要使得零速检测算法能对多种步态比力矢量是否接近于g进行判断，如果是则得出结论：IMU是静止状态，行人处于零速。如果加速度幅值满足以下条件：

若满足式（6）条件，执行步骤3进一步判断，不满足条件判定行人此刻非零速。

步骤5：行人正常行走时可能出现足部左右异常摆动，利用陀螺仪信号的能量值进一步检测MIMU是否处于静止状态［8］，这样就能改进在足部抖动、转弯过程中的一些零速误判，利用角速率能量来检验行人是否处于静止状态需满足以下条件：

优化算法将加速度计和陀螺仪输出信号同时应用于零速检测，三种方法相互辅助，具有一定的互补性，提高行人导航零速检测的精度。

图3　优化零速检测算法流程图Fig.3 Flow chart of modified ZUPT mechanism

3　实验验证

3.1硬件介绍

实验使用的MIMU为荷兰Xsens公司的MTi集成传感器。它内部包含有三轴加速度计和三轴陀螺以及三轴磁强计，可以通过RS232接口或USB接口直接与计算机相连。如图4所示，该系统以其体积小、质量小、低成本和较好的可操作性使得其在工程中得到了广泛应用。

图4　实验所用MIMUFig.4 The MIMU used in the experiment

MTi性能标称参数：实际由静态测试得到的陀螺三个轴零偏稳定性参数分别为30.7（°）/h，26.8（°）/h，40.5（°）/h。

器件用于实验时，为减小脚部的抖动干扰以及行进时可能带来的较大加速度冲击对实验数据的影响，在器件与脚面之间加入减震模块，用于缓解干扰。

3.2零速检测算法验证

实验测试路线为实验楼的一层大厅，路径为从起始点行走矩形框，行走过程中会有步速的不同变化和静止时段以及转弯过程，行人行走4圈，最后回到起始点；试验过程中将试验设备固定在试验者左脚后鞋跟，从鞋后引出USB线连接电脑采集数据，安装示意图如图5所示。首先，测试者在实验场内进行15min的系统静止预热，完成系统的初始对准。MTi输出频率设为100Hz。

图5　MIMU安装示意图Fig.5 The installation instruction of MIMU

图6选取了一组行人沿设定轨迹、步态行走下两种零速检测算法处理后的对比结果。从图6中可以看出常用加速度方差零速检测在方差呈现规律变化时，零速判断结果与方差曲线对应较好，如85s～87s的零速判断。为验证算法，实验过程中我们设计了迈大步、步速加快和转弯过程等特殊时刻，常用算法零速结果出现误判，如83s～84s的大步行走使得加速度方差值突变，88s～90s的足部开始动作前异常抖动造成的不规律变化。在优化算法中，为了更好地区分对比，我们用“1”和“0”来代表零速决策的真假，“1”为零速时刻。对比步态不规律时刻零速判断结果，与实际实验零速结果对应可知改进的零速检测算法能有效克服误判，检测精度更高。

图6　零速检测结果对比Fig.6 The comparison of two stance phase detection methods

3.3楼内实测零速修正导航轨迹结果

为了进一步验证优化零速检测算法对行人导航结果精度的提升，利用惯导解算方法得出行人运动轨迹作对比分析。

导航解算行走轨迹如图7所示。图7（a）和图7（b）分别表示常用加速度方差零速检测算法轨迹和改进后零速检测算法轨迹结果。对比两组测试的解算轨迹可以看到，原算法处理后的轨迹尤其是每一圈矩形框之间进行切换时，矩形框的偏移很大，而转弯过程、大步或快速行走的零速检测误判是造成导航轨迹偏差较大的原因之一；而优化后的零速检测算法却可以有效降低位置漂移率，使轨迹处理结果相对规则。从而表明改进后的零速检测算法能对行走过程中步速加快、大步、转弯等过程有较好的零速判断能力，进而能够有效地减少行人室内导航的漂移误差，进一步提高在没有卫星条件下行人导航的精度。

图7　两种算法导航定位结果Fig.7 Positioning trajectories of two methods

4　总结

本文研究了行人导航系统中的零速检测算法，提出了一种优化的零速检测算法，将加速度计信号和陀螺仪信号综合应用于零速检测，充分利用惯性传感器的输出信号特征，对行走过程中可能出现的迈大步、步速变化、转弯过程的零速判别效果较好。并通过实验验证与常用的加速度方差零速检测算法进行对比，结果表明优化的零速检测导航算法具有更高的零速检测精度和导航精度，进一步提高了行人导航算法的可靠性和实用价值。

［1］ 李辰祥.基于MEMS行人惯性导航的零速度修正技术研究［D］.厦门大学，2014. LIChen-xiang.Researchonzero-velocityupdate technology of pedestrian inertial navigation based on MEMS［D］.Xiamen University，2014.

［2］ Groves P D.GNSS与惯性及多传感器组合导航系统原理（第二版）［M］.北京：国防工业出版社，2015. Groves P D.Principles of GNSS，inertial and multisensor intergrated navigation systems（2nd）［M］.Beijing：National Defense Industry Press，2015.

［3］ Jimenez A R，Seco F，Prieto J C，et al.A comparison of pedestrian dead-reckoning algorithms using a low-cost MEMS IMU［C］.Proceeding of the 6thInternational Symposium on Intelligent Signal Processing，Budapest，Hungary：IEEE Press，2009.

［4］ 张金亮，秦永元，梅春波.基于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.

［5］ Harle R，Taherian S，Pias M，et al.Towards real-time profiling of sprints using wearable pressure sensors［J］. Computer Communications，2012，35（6）：650-660.

［6］ Titterton D H，Weston J L.Strapdown inertial navigation technology［M］.Lexington，Massachusetts，USA，2007.

［7］ Skog I，Handel P，Nilsson J O，et al.Zero-velocity detection-an algorithm evaluation［J］.IEEE Transactions on Biomedical，Engineering，2010，57（11）：2657-2666.

［8］ Feliz R，Zalama E，Gómez J.Pedestrian tracking using inertial sensors［J］.Journal of Navigation，2007，60（3）：391-407.

A Novel Pedestrian Navigation Algorithm Based on ZVU

MENG Xiang-bin，PAN Xian-fei，HU Xiao-ping
（College of Mechatronic Engineering and Automation，National University of Defense Technology，Changsha 410073）

In order to meet the needs of the location of a solider in military fields and provide location service for pedestrian in civilian fields.We propose foot pedestrian navigation algorithm based on MIMU.We use zero-speed updates to the alignment of the INS or calibration at the stance period when the foot landing on the ground.The zero speed detection is the basis of zero velocity update.This paper compares the traditional approach in the foot fixed MIMU zero speed detection. As for threshold of conventional algorithms is not adaptive，and can not detect the impact of the problems caused by the change of velocity.We suggest an modified algorithm for zero speed judgment，making the accelerometer signal and the gyroscope signal in maximize application to zero speed detection.Finally，we carry out the experiment of indoor pedestrian walking in normal speed（about 5km/h）.The experimental results show that the new zero speed detection algorithm can detect the zero speed more accurately than the traditional algorithm and correct the navigation error and the navigation error is better than the traditional algorithm.

pedestrian navigation；MIMU；zero velocity update testing（ZUPT）；detection methods

U666.1

A

1674-5558（2016）01-01196

10.3969/j.issn.1674-5558.2016.04.005

2015-09-09

孟祥宾，男，硕士，研究方向为单兵导航算法。