基于双MIMU的行人双足轨迹约束算法

傅温慧 伍萍辉 曾成 花中秋

摘要 基于MIMU的行人导航系统受惯性器件固有属性的影响，导航误差会随着时间累积而发散，为提高行人惯性导航系统的精度，提出了一种基于双MIMU的行人导航算法。将2套MIMU分别固联在行人的左右脚上，先对单足进行零速检测和零速修正，再根据行人双足的运动特征，设计了一种行人双足轨迹间距离约束的算法。利用行人正常行走时左脚轨迹和右脚轨迹间距不超过某个最大值，构造不等式约束的卡尔曼滤波器，对零速修正后的行人位置进行二次校正。实验结果表明，采用行人双足轨迹间最大距离约束算法，位置误差比约束前平均降低了39.14%。

关 键 词 惯性导航;MIMU;零速修正;不等式约束;卡尔曼滤波

中图分类号 TN966     文献标志码 A

Abstract The MIMU-based pedestrian navigation system is affected by the inherent properties of the inertial device， and the navigation error will accumulate over time. To improve the accuracy of the pedestrian inertial navigation system， a pedestrian navigation algorithm based on dual MIMU is proposed. The two MIMUs are fixed on the left and right feet of the pedestrian respectively. Firstly， the zero-velocity detection and zero-velocity updates are performed on the single foot. Then， according to the motion characteristics of the pedestrian's feet， an algorithm for the distance constraint between the pedestrian and the foot is designed. When the pedestrian is walking normally， the distance between the left foot track and the right foot track does not exceed a certain maximum value， and the Kalman filter with inequality constraint is constructed to perform secondary correction on the pedestrian position after zero-velocity updates. The experimental results show that the position error is reduced by 39.14% on average before the constraint by using the maximum distance constraint algorithm between pedestrian bipeds.

Key words inertial navigation;  MIMU;  zero-velocity update;  inequality constraints;  Kalman filter

0 引言

隨着科技的发展，全球定位系统（Global Positioning System，GPS）在信号良好的室外环境可以达到很高的定位精度，被广泛应用于众多领域。但是对于内部结构复杂的室内环境，GPS不能达到令人满意的定位效果。所以需要一种可以提供室内精确位置的定位技术来弥补这一不足。目前，国内外已经研究出多种室内定位技术，其中基于微机电系统（Micro-Electro-Mechanical System，MEMS）的惯性导航便是其中的一种。

基于MEMS的微惯性测量单元（Micro Inertial Measurement Unit，MIMU）具有隐蔽性强、抗干扰性强、体积小、耗能低、环境适应性强等优点[1-3]，可以用作行人室内导航的核心器件。但是基于MEMS惯性传感器的导航方案存在误差随时间累积而发散[4-7]的缺点，定位精度会随误差的累积迅速降低。因此需要利用MIMU得到的惯性信息和解算的姿态信息等对误差进行补偿。零速修正技术[8]是一种简单有效的误差补偿技术，利用惯性器件的零速作为观测量来修正系统误差。在行人惯性导航中MIMU多采用足绑式，将脚接触地面时作为零速来对载体的其他信息进行修正。由于天向陀螺的可观测性较低[3]，零速修正技术无法补偿行人在导航过程中的航向漂移[9-10]，容易造成航向发散。

针对这些问题国内外学者进行了多种方案的尝试，Prateek等[8]提出了一种双MIMU行人惯性导航方案，将2套MIMU分别绑在左右脚上，利用行人步行中两脚之间的距离对解算结果进行约束，起到了很好的效果。卡内基梅隆大学对双足方案进行了深入研究，实验达到了很高的定位精度。哈尔滨工程大学的李佳璇等[1]提出了一种单足双MIMU的行人导航方案，将2套MIMU固定在同一只脚上，两套MIMU之间的距离是不变的，在解算中利用MIMU之间的固定距离对结果进行约束，起到了提高精度的作用。为提高行人导航的位置精度，抑制行人长时间运动中的航向漂移，本文采用双MIMU的行人导航方案，提出了一种将行人双足轨迹间距离作为约束条件，利用不等式卡尔曼滤波校正位置误差的算法，并进行了约束前后的实验对比，验证了该算法的有效性。

1 行人双足运动轨迹分析

行人在正常行走时，左右脚交替前进，左脚轨迹与右脚轨迹近似平行，两条轨迹之间的距离应小于某个最大值。例如在行人导航解算中，右脚位置在水平面上的投影到左脚轨迹的垂直距离不会大于这个最大值。如图1a）中虚线为双足轨迹在水平面上的投影，黑点为落脚点的位置，为方便研究左右脚运动轨迹的关系，将轨迹近似为折线，如图1a）中实线段所示。其中一步如图1b）所示，AB为左脚轨迹，A点为前一个落脚点位置，B点为当前落脚点位置;CD为估计的右脚轨迹，C点为右脚前一落脚点的位置，E点为当前时刻右脚位置在水平面上的投影。E点到直线AB的距离为d，即双足轨迹距离，在行人正常行走过程中双足轨迹距离d不超过某个最大值λ。在解算时就可以利用这一约束关系对导航估计状态进行二次修正，抑制系统的航向漂移，提高位置精度。

2 双足轨迹间最大距离约束算法

卡尔曼滤波是惯性导航解算中常用的一种状态估计方法[10]，利用前一时刻的状态估计值和现在时刻的状态观测值来估计当前时刻的状态。简单来说，卡尔曼滤波是一种最优化自回归数据处理算法，具有很高的运算效率。本文根据双足轨迹间的距离关系，构造约束方程，利用不等式约束的卡尔曼滤波[4]的方法来实现双足轨迹间最大距离约束算法。

2.1 经典离散型卡尔曼滤波

2.2 不等式约束卡尔曼滤波

在实际应用中，状态变量会受到现实环境、实验方法等条件的限制，这些限制条件无法由系统模型表示，如果将其忽略就可能造成最优解不满足实际条件，可信度降低。若将这些限制条件用约束方程表示，例如某些限制条件和状态变量之间的关系可以用不等式表示，再结合卡尔曼滤波使得最优解满足约束方程，这就是不等式约束卡尔曼滤波。

不等式约束卡尔曼滤波最优解可以表示为

2.3 双足轨迹间最大距离约束算法设计

在进行导航解算时，左右脚轨迹之间存在距离约束，可以将这种关系用不等式表示，再用不等式对卡尔曼滤波器进行约束，使最優解满足约束条件，通过这种方法得到的估计状态更加符合实际。

在导航解算中卡尔曼滤波器的状态量包括3维位置状态向量s，3维速度状态向量[v]，3维姿态角状态向量[θ]。令[xji=[sjivjiθji]T]为惯性导航系统在k时刻9维状态估计向量，其中j∈{L，R}，L表示左脚，R表示右脚。双足之间的距离为

当人在正常行走时[dik]总是不大于双足轨迹间的最大距离，即存在[dik≤λ]。可将导航估计状态约束到合理的子空间，得到精度更高的状态估计值，即满足不等式约束[4]：

当双足轨迹间的距离超出最大值，即[dik>λ]时则使用不等式约束的卡尔曼滤波对导航状态重新估计。定义超出双足轨迹最大距离后运动脚的位置状态观测量为[pik]，根据双足轨迹间的最大距离约束原理（图1），将垂线上到垂足距离为[λ]的点作为位置观测量;将导航解算位置作为一步预测量，重新估计导航状态。

3 实验验证和结果分析

通过行人双足足绑MIMU正常行走的实验来验证双足轨迹间最大距离约束算法的可行性，根据实验人员的步行特点，轨迹间最大距离取[λ] = 0. 3 m。为了更方便地判断导航精度，本文采用的是封闭行走路线，通过最终位置与初始位置的距离差值来判断导航定位的精度。

实验人员以初始位置为原点做近似匀速运动，最终回到原点，初始航向角为0°，采样频率设为100 Hz。实验开始后先在初始位置静止2 min左右，通过静止时的加速度信息得到初始横滚角和俯仰角，通过角速度信息得到陀螺仪的零偏信息。然后开始正常步行，在行走过程中要确保MIMU与脚之间不出现滑动和脱落。最后回到初始位置，静止1 min以方便提取运动过程的数据。

对同一组数据分别采用零速修正技术和双足轨迹间最大距离约束算法进行轨迹推算，以解算的最终位置到初始位置的距离作为判断精度的依据。为消除实验的偶然性，该测试在不同的时间段重复进行4次，多次实验解算得到的结果应无显著差异。

实验人员从初始位置开始围绕大楼正常行走，行走完一圈后停在初始位置，总路程约为300 m。

在不进行双足轨迹间最大距离约束时，行人双足轨迹解算结果如图2所示。从图中可以看出，随着行走距离的增加，左右脚轨迹出现偏移。图3为该轨迹中偏离较为严重的两部分，从图中可以看出左右脚轨迹间的距离超过了3 m，双足最终位置距离超5 m，显然不符合行人正常的生理特征。

对同一组数据用双足轨迹间最大距离约束算法解算，双足轨迹如图4所示。从图中可以看出，双足轨迹近似重合，无明显偏离的部分。图5轨迹与图3轨迹为行人轨迹的同一部分，经过双足轨迹间最大距离约束后，双足轨迹间的距离被限制在0.3 m之内，两条轨迹近似平行状态，偏离情况得到明显改善，符合行人正常生理特征。

为消除实验偶然性，重复做4组实验，约束前最终的位置误差和约束后最终的位置误差如表1所示。约束前4次实验的平均位置误差为2.475 5 m。约束后4次实验的平均位置误差为1.503 2 m。综合以上实验结果，经过双足轨迹间最大距离约束后双足的平均位置误差下降了39.28%，达到了提高行人导航精度的目的。

4 结论

本文对行人正常行走时双足的运动特点进行了分析，在零速修正的基础上加入了双足轨迹间最大距离约束算法对导航位置估计进行二次修正。根据行人双足轨迹特点，构造约束方程，利用不等式约束卡尔曼滤波的方法对约束信息融合到惯性解算中，得到了比零速修正更高的位置精度。实验结果表明，双足轨迹间最大距离约束算法有效利用了行人双足运动特征，进一步限制了惯性导航系统的误差累积，起到了提高定位精度的效果。双足轨迹间最大距离约束算法是利用行人本身的一些运动特点和生理特性，通过信息融合的方法提高导航精度，为行人惯性导航中其他约束校正方法提供了一种思路。

参考文献：

[1]    李佳璇，周广涛，刘晓旭，等. 基于等式约束卡尔曼的双MIMU行人导航方案[J]. 压电与声光，2015，37（2）：237-241.

[2]    张钊，侯书军. 双轴式惯性激振器运转噪音与振动特性关系研究[J]. 河北工业大学学报，2018，47（6）：50-55.

[3]    时伟，王阳. 基于不等式约束卡尔曼滤波的双MIMU导航位置校正方法[J]. 中国惯性技术学报，2017，25（1）：11-16.

[4]    丁承君，赵泽羽. 基于多传感器数据融合的火灾探测系统[J]. 河北工业大学学报，2018，47（5）：17-22.

[5]    DONOVAN G T. Position error correction for an autonomous underwater vehicle inertial navigation system （INS） using a particle filter[J]. IEEE Journal of Oceanic Engineering，2012，37（3）：431-445.

[6]    LIU W R，NING W，LIU G B，et al. Integration navigation method of dual INS[J]. Journal of Chinese Inertial Technology，2014，22（1）：1-4，13.

[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]    PRATEEK G V，GIRISHA R，HARI K V S，et al. Data fusion of dual foot-mounted INS to reduce the systematic heading drift[C]//2013 4th International Conference on Intelligent Systems，Modelling and Simulation. Bangkok，Thailand. IEEE，2013：208-213.

[9]    ZHANG X X，ZHANG R，GUO M F，et al. Online three-axis magnetometer calibration for a pedestrian navigation system using a foot-mounted inertial navigation system[J]. Journal of Tsinghua University，2016，56（2）：211-217.

[10]  周婧，陳庙红，吴豪杰. 基于惯性导航的平面航迹推算的研究[J]. 计算机科学，2017，44（z1）：582-586.

[责任编辑    田    丰]