基于足部安装MIMU的行人导航系统设计

,

(1.北京理工大学 自动化学院，北京100081; 2.北京自动化控制设备研究所，北京 100074)

0 引言

行人导航作为导航学科的一门新兴分支，近年来在国内外得到了广泛的关注和研究，尤其是当前基于人员位置服务的市场化应用需求，更拓展了行人导航系统研究的广度和热度。行人导航系统可广泛应用于消防抢险、智慧城市、单兵作战等领域。

与汽车、飞机等载体不同，行人带负载能力有限；此外，针对无卫星信号环境中的人员定位及目标找寻，还需要行人导航系统具有较强的自主定位能力。随着半导体设计方法及工艺的提升，目前市场上MEMS惯性传感器的精度可以达到商业级或战术级水平，因此由MEMS传感器构成的惯性测量单元能够满足行人在无卫星或者卫星失效环境下的自主定位需求[8，15]。基于此，本文采用足部安装惯性测量单元的方案设计行人导航系统，完成了传感器选型并设计了行人导航鞋样机，并在此基础上开展了Kalman滤波架构下的零速修正算法和航向修正算法设计，实现了对系统的误差估计。

1 导航鞋样机设计

根据行人导航系统的适用对象和实际应用环境，导航鞋样机设计主要从鞋的选型、惯性测量单元的安装等方面进行考虑。

1.1 传感器选型

行人导航系统选用的传感器应具有质量小、尺寸小、功耗低、动态范围大、便于穿戴等特性，这样既不会影响行人的正常运动，又能实现长时间导航。当前关于行人导航系统研究的文献中应用较为广泛的惯性测量单元有荷兰Xsens公司生产的MTi/MTx系列产品[1]、美国Analog Devices公司生产的ADIS系列产品[2]、美国MicroStrain公司生产的3DM-GX系列产品[3]及美国InterSense公司生产的InertiaCube系列产品[4-5]等[11]。这些惯性测量单元都在一个结构中集成了可以测量3个正交轴向上载体加速度与角速率的加速度计和陀螺仪，同时还在结构内部集成了微处理器用于信号的处理及传输。这些惯性测量单元只通过一个复合接口就能够实现供电以及与其他外设通信的功能。除惯性传感器外，有的惯性测量单元还集成了磁传感器、气压计、温度计等传感器，以满足不同场合的应用需求。综合考虑，本文选用了Xsens公司生产的MTi系列大量程惯性测量单元，其主要性能指标如表1所示。

1.2 导航鞋选型

在导航鞋的选型方面，考虑到MTi惯性测量单元内部集成了磁传感器，需首先选择不含铁质饰品的鞋子，以避免铁磁性干扰对磁传感器输出造成的影响；其次，实际使用时需要将惯性测量单元与鞋体固连，所以制鞋的材料应以硬质面料为主，例如皮革或仿皮质材料。本文惯性测量单元安装在鞋后跟处，所以在选鞋时还应以高帮鞋为主。

表1 MTi惯性测量单元传感器性能指标

为了便于数据传输，在设计惯性测量单元的安装方式时选用了数据线接口插槽竖直向上的方案。为了防止行走过程中惯性测量单元与地面接触，惯性测量单元安装在鞋上后，其底端与鞋底之间应至少有20mm的距离；同时结合传感器尺寸，本文选择导航鞋时鞋帮的高度应不小于85mm。综合考虑以上因素，选用了一款由皮革面料制作的长筒军靴用于导航鞋设计[13-14]。

1.3 传感器安装

完成导航鞋的选型后，需考虑如何将惯性测量单元安装在鞋体上。在惯性测量单元安装方面，一些学者采用鞋带或者其他材料捆绑的方式来固定惯性测量单元，这样的安装方式虽然便于拆卸，但是行人行走过程中惯性测量单元容易晃动，会引入干扰加速度和角速度，从而影响传感器对步态运动参数的准确测量；还有学者通过在鞋底开槽的方式将惯性测量单元固定在鞋中，这种方法虽然能够有效地测量行人的步态参数，但是传感器的安装、拆卸过程相对繁琐。为了实现惯性测量单元安装简单、拆卸方便，且能够稳固地与导航鞋固连的目标，本文设计了一块惯性测量单元安装板，其外形图以及与鞋体固联后的效果如图1所示。

图1 传感器安装板示意图Fig.1 Pinboard of inertial sensors

将MTi惯性测量单元用铜螺丝固定在安装板上即完成了导航鞋样机的设计，效果如图2所示。本文设计的导航鞋样机具有接触稳固、安装简单、拆卸方便等优点。

图2 导航鞋样机Fig.2 Prototype of navi-shoe

2 零速修正算法

2.1 基于多条件约束的零速检测算法

零速检测是零速修正算法的先决条件和关键环节。本文设计了一种综合加速度矢量和阈值、滑动窗口方差、加表峰值检测+时间阈值、角速度阈值等多种指标的多条件约束零速检测法，可以较好地兼顾零速检测灵敏性与准确性[9]。几种零速检测指标的计算方法如下。

1)加速度矢量和阈值。该指标通过计算3个正交轴向上加速度计输出的矢量和，将其与设定的零速阈值区间相比较，判断是否处于零速状态。

(1)

(2)

其中，Accnorm(k)表示k时刻加速度矢量和，ai(k)表示k时刻i轴向上加速度计输出，i=x,y,z。TA_min、TA_max分别是阈值区间的左边界和右边界，当加速度模值在阈值区间内时，判定为零速状态。

2)滑动窗口方差。设置一个滑动窗口，动态计算加速度计信号的方差值，判断加速度计信号离散度并以此作为零速状态的判断依据之一。

(3)

(4)

3)加表峰值检测+时间阈值。足部触地瞬间会产生加速度计输出峰值，通过检测这一峰值可以辅助确定触地后的时间区间Δt为零速区间。

(5)

(6)

其中，aPeak(k)表示k时刻检测到加速最大峰值点，ay(k)表示竖直轴向加速度，Tay表示加速度阈值， (k+Δt1,k+Δt2)表示判断到加速度峰值后设定的行人处于静止的时间区间。

4)角速度阈值。足部处于零速区间时横向陀螺测量的足部摆动应处于比较小的状态，可以利用横向陀螺输出作为零速检测的约束条件。

(7)

其中，ωy(k)表示k时刻横向陀螺输出的绝对值，Tω为角速率阈值，当横向陀螺输出角速率绝对值低于阈值时，认为运动脚静止。

单一的判据容易出现错判和误判，本文通过合理选择阈值，将式(1)～式(4)中k时刻的判断结果进行与运算，即

ZUPT(k)=C1(k)&C2(k)&C3(k)&C4(k)

在保证零速判断灵敏度的前提下提高检测的正确性。通过试验验证了不同步速条件下上述零速检测算法的检测效果，结果如图3和表2所示[10]。

2.2 零速修正算法

本文设计的行人导航系统本质为组合系统，故可通过Kalman滤波技术进行数据融合，估计行人导航系统的误差并利用误差的估计值对导航参数进行校正。离散化的Kalman方程可表示为：

(8)

实验类型速度/(km/h)步数零速区间实际值计算值误差/%慢速行走3781447272034514874740常速行走4481346767045813467670快速行走6011165858057911859590

其中，δxk为k时刻的系统状态，Φk为状态转移矩阵，wk为过程噪声；δzk为k时刻误差观测量，H为测量矩阵，vk为测量噪声。

本文行人导航系统的误差方程可表示为：

(9)

结合式(9)中导航系统误差方程及惯性传感器误差方程设计Kalman滤波器，系统状态向量取

(10)

对于足部安装自包含传感器的行人导航系统来说，零速修正是数据融合算法的基础，本文Kalman滤波器状态更新的触发条件就是检测到步态中的零速区间。行人步态中的零速区间可认为是虚拟的零速速度传感器，其测量值能够作为步态静止相位中的速度观测值。采用Kalman滤波进行误差状态估计时，可根据式(11)将行人导航系统中惯性测量单元解算的步态速度值与步态静止相位中输出的虚拟零速作差，并将计算的差值作为速度误差观测量输入到Kalman滤波器。通过利用速度误差与姿态误差、位置误差以及传感器误差之间的耦合关系可以估计出行人导航系统的状态误差参数[6]。

(11)

将计算得到的速度误差δV输入到Kalman滤波器进行误差估计。零速修正过程中测量矩阵H的表达式为

(12)

3 航向角修正算法

对于基于足部安装惯性传感器的行人导航系统来说，零速修正是一种十分有效的误差修正方法。它利用行人步态中周期性存在零速区间的特点对系统误差进行修正，能有效抑制横滚俯仰误差，但由于航向误差不可观测，ZUPT无法估计航向误差漂移[7]，而在行人导航系统中航向误差会导致计算的行人航迹位置信息存在较大的误差[12]。

针对行人导航系统中航向角误差修正问题，本文利用陀螺仪计算航向角具有短时精度高、磁传感器在不受外界干扰时计算的航向角精度高的特点，设计了零速区间内有效磁航向角判定函数，实现了对航向角的准确提取。同时，采用平滑滤波技术对零速区间内所有的有效航向角求最优值。这种最优航向角计算方法在一定程度上抑制了外界干扰导致的航向误差，优化了局部航向角的计算值，为滤波器提供较为精确的外部航向观测量。

设行人步态中零速区间的时间长度为ΔT，微惯性测量单元数据输出周期为Ts，则零速区间内包含的零速点数目为

n=ΔT/Ts

(13)

当检测到行人零速区间后，根据式(14)中航向角计算公式并结合零速区间内磁传感器的输出便能计算出每一个零速时刻点对应的航向角

(14)

其中，Φmag(ti)为零速区间内ti时刻由磁传感器计算的航向角，Bx(ti)和By(ti)分别为磁传感器输出的水平磁分量，D为当地磁偏角。

在实际应用中会存在磁干扰的情况，从而导致磁传感器计算的航向角存在误差，因此在实际应用中需考虑磁干扰的问题。为了消除环境中突然出现的磁干扰，本文设计了干扰磁航向角识别算法用于识别外界干扰导致的错误航向角。在行人导航系统中，利用陀螺输出计算的航向角具有短时精度高、不受外界干扰的特点，但随着时间的增长航向角误差不断累积；而磁传感器计算的航向角在不受外界干扰时精度较高。由于行人步态周期较短，短时间内陀螺计算的航向角可以作为参考信息，因此对于判定零速区间内ti时刻磁传感器计算的航向角是否受外界干扰可通过捷联解算的航向角进行辅助判断。

定义ti时刻捷联解算的航向角为ΦSINS(ti)，进行磁干扰判断后得到的磁航向角有效输出为ΦMAG(ti),则有效磁航向角的判定条件为

ΦSINS(ti)-Φmag(ti)

T1≤ΦSINS(ti)-Φmag(ti)

(15)

ΦSINS(ti)-Φmag(ti)≥T2

式中，k1、k2分别表示捷联解算航向角和磁计算航向角的加权系数；T1、T2表示捷联解算的航向角与磁传感器计算的航向角差值的判断阈值。

将零速区间内计算的所有有效航向角进行平滑滤波，得到零速区间内优化的航向角，计算公式如下

(16)

其中，φmag(ti)表示经过平滑滤波后ti时刻的航向角优化值，可作为外部观测量进行航向角误差计算。航向角误差表达式如下

δφ=φSINS-φmag

(17)

将计算的航向角误差代入Kalman滤波器估计行人导航系统航向误差，并对捷联惯导系统中陀螺仪计算的航向参数进行修正，消除陀螺漂移引起的累积误差。航向角修正过程中测量矩阵H的表达式为

(18)

4 实验测试及分析

考虑到行人在坡度较缓的环境中运动时也需要提供高度参考信息的情况，本文开展了行人上下坡测试实验，并采用文中设计的导航算法计算了行人在缓坡环境中的导航参数。如图4所示，实验环境为一坡型闭合区域，行人运动路线总长度为51.7m，坡顶与坡底之间的高程差为0.6m，行人在该实验区域内行走一圈，解算得到的路径曲线及高度变化曲线分别如图5、图6所示。

图4 上下坡测试实验Fig.4 Pedestrian positioning experiment in slope environment

图5 上下坡航迹曲线Fig.5 Calculated trajectory when going up/down the slope

从图5中可以看出，导航解算的行人航迹曲线与实验环境中规划的行走路线一致，行人先从水平路段起始点出发，行走一段距离后进入上坡路段，然后经过坡顶平台后开始下坡，最后回到起始点。行人运动轨迹终点处位置误差为0.17m，占行人行走总路程的0.33%。从终点处位置解算精度来看，在没有高度约束的条件下，本文设计的行人导航算法也能精确地计算行人在缓坡路段行走过程中的位置信息。

图6所示为行人上下坡行走时高度变化曲线。从图6中可以看出，解算得到的高度变化值与行人实际运动过程中的高度变化趋势一致，但由于没有对导航解算高度误差进行约束，随着行走距离的增加，高度误差不断累积，在终点处达到最大。综合分析图5、图6中的导航参数曲线可以看出，在包含坡路的环境中，随时间累积的高度误差是影响行人导航定位精度的主要因素。而对于缓坡路段，由于行人行走过程中高度变化较小，所以在不增加高度约束的条件下也能比较准确地计算出高度信息供行人参考。

5 结论

零速修正与航向修正是影响行人导航系统定位精度的2个关键因素。本文提出了一种基于多约束条件的零速检测方法，在保证灵敏度的前提下提高系统的检测正确率。此外，本文设计了一种基于陀螺辅助的抗干扰磁航向修正算法，解决因环境磁场干扰导致的航向角误修正问题，实现了零速区间内有效磁航向角的提取。采用三维速度零速修正方式，实现高度通道定位误差的有效阻尼。导航试验表明，在上下坡环境中，本文设计的行人导航系统能够较好地满足行人在三维空间内的导航定位需求，位置误差为0.33%。在研究中发现，目前文献中的各个算法对跑步条件下的零速检测问题都没有令人满意的效果，是未来研究中亟待解决的重点问题。

[1] Liu W, Zhang Y, Yang X, et al. Pedestrian navigation using inertial sensors and altitude error correction[J]. Sensor Review, 2015, 35(1): 68-75.

[2] Lan K C, Shih W Y. Using smart-phones and floor plans for indoor location tracking-withdrawn[J]. IEEE Transactions on Human-Machine Systems, 2014, 44(2): 211-221.

[3] Tian X, Chen J, Han Y, et al. A novel zero velocity interval detection algorithm for self-contained pedestrian navigation system with inertial sensors[J]. Sensors 2016,16(10):1578.

[4] 张金亮, 秦永元, 梅春波. 基于MEMS惯性技术的鞋式个人导航系统[J]. 中国惯性技术学报, 2011, 19(3): 253-256.

[5] Yun X, Calusdian J, Bachmann E R, et al. Estimation of human foot motion during normal walking using inertial and magnetic sensor measurements[J]. IEEE Transactions on Instrumentation and Measurement, 2012, 61(7): 2059-2072.

[6] Bebek Ö, Suster M A, Rajgopal S, et al. Personal navigation via high-resolution gait-corrected inertial measurement units[J]. IEEE Transactions on Instrumentation and Measurement, 2010, 59(11): 3018-3027.

[7] Woodman O. Pedestrian localisation for indoor environments[D]. St Catharine’s College, Digital Technology Group Computer Laboratory University of Cambridge, 2010:68-75.

[8] Abdulrahim K, Hide C, Moore T, et al. Rotating a MEMS inertial measurement unit for a foot-mounted pedestrian navigation[J]. Journal of Computer Science, 2014, 10(12): 2619-2627.

[9] 田晓春，陈家斌，韩勇强, 等. 多条件约束的行人导航零速区间检测算法[J].中国惯性技术学报, 2016,24(1):1-5.

[10] Liu J, Chen R, Pei L, et al. A hybrid smartphone indoor positioning solution for mobile LBS[J]. Sensors, 2012, 12(12): 17208-17233.

[11] 李若涵, 张金艺, 徐德政, 等. 运动分类步频调节的微机电惯性测量单元室内行人航迹推算[J]. 上海大学学报: 自然科学版, 2014, 20(5): 612-623.

[12] Jirawimut R, Ptasinski P, Garaj V, et al. A method for dead reckoning parameter correction in pedestrian navigation system[J]. IEEE Transactions on Instrumentation and Measurement, 2003, 52(1): 209-215.

[13] Ladetto Q, Gabaglio V, Van Seeters J. Pedestrian navigation method and apparatus operative in a dead reckoning mode: U.S. Patent 6826477[P]. 2004-11-30.

[14] Stirling R, Collin J, Fyfe K, et al. An innovative shoe-mounted pedestrian navigation system[C]// Proceedings of European Navigation Conference GNSS. 2003: 110-5.

[15] Fang L, Antsaklis P J, Montestruque L A, et al. Design of a wireless assisted pedestrian dead reckoning system-the NavMote experience[J]. IEEE Transactions on Instrumentation and Measurement, 2005, 54(6): 2342-2358.