基于运动姿态识别的行人惯性导航算法*

张会清， 许潇民

(北京工业大学 信息学部，北京 100124)

0 引 言

导航是引导人或物，从指定路线的一点运动到另一点的方法[1]。由于卫星信号到达地面时信号较弱、不能穿透建筑物[2]，室内环境需要采取新的技术方法实现导航定位。按照是否需要载体设备与相关室内环境中的设备配合，目前，室内定位导航技术可以分为自主式导航和非自主式导航[3]。惯性导航技术是一种自主式导航技术，相对其他室内定位技术具有不需要布设接收装置、不受环境变化影响等优点。1996年,Levi R W和Judd T提出了行人航迹推算(pedestrian dead reckoning,PDR)的概念[4]。很多研究机构将微机电系统 (micro-electro-mechanical system，MEMS)惯性传感器佩戴于人体的一个或多个不同部位，以准确采集行人运动时的惯性数据。由于走动时人体运动情况较复杂，惯性导航算法过程中存在累积误差，使得行人长距离导航时精确性逐渐降低，限制了其应用。

本文提出了一种基于运动姿态识别的行人惯性导航算法，将惯性传感器佩戴于行人脚部，采用零速修正方法和姿态识别方法将行人运动轨迹以单步长划分，分步解算单步位移，统计误差较小的前期步长数据并建立步频—步长模型，根据行人步频和方向的变化实时校正单步长结果，通过行人的运动姿态变化判断匹配相应的步长估计算法，得到精确的行人运动轨迹。最终通过实验验证了改进的行人惯性导航算法的精确性。

1 数据处理与步频检测

1.1 误差分析和数据处理

MEMS惯性传感器静止时输出的数据噪声包括静态漂移噪声和动态漂移噪声[5]。系统对MEMS惯性传感器进行初始校准，将模块保持静止状态，保存一段时间内的测量数据，取其均值作为零偏数据。使用离散傅里叶变换的快速算法对校准后的数据进行频域分析，获得信号的频率结构以及各谐波和相位信息。频谱分析表明：行走时的步频为1～3 Hz，故保留低频分量，频率高于5 Hz的数据判定为干扰，使用低通数字滤波器对纠正零偏后的传感器数据进行滤波处理，经过校准后的静止输出数据趋近于真实的零值，滤除了大部分的静态漂移噪声。

1.2 零速修正算法设计

零速修正是一种操作简便且高效的误差补偿技术，可以有效地为惯性导航系统降低累积误差。行走时、脚部接触地面的过程中，惯性传感器相对于地面有一段短暂的零速时刻，可以认为此时行人处于瞬时静止状态。本文采用三条件判断法，融合了加速度方差检测、加速度幅值检测和角速度幅值检测3种检测方法。公式如下

(1)

(2)

(3)

(4)

(5)

(6)

将单个条件结果进行“与”(&)运算，求出脚着地，即“零速度”的时刻

c=c1&c2&c3

(7)

式中c为逻辑“与”运算的结果。当c取值为1时，说明此刻是“零速度”时刻；当取值为0时，为行人运动的“非零”速度时刻。

零速修正结果如图1，可以看出，通过适当的阈值设置，零速修正可以较好地判断出行人的零速时刻。但零速修正算法存在一些缺陷，当行人行走速度较慢时，可能会出现区域性的连续零速度点，影响结果的准确性。

图1 零速修正结果

1.3 姿态识别检测

针对零速修正中出现连续零速度点影响判断精度的情况，本文引入了脚部姿态识别的方法。行走时，脚面绕水平方向与行进方向垂直的轴向(俯仰角)具有周期性的动作，可以检测行人脚部姿态角变化判断行人的运动状态。

(8)

(9)

公式适用于在静止状态计算姿态角，而运动时的姿态角需要通过陀螺仪数据解算得出。一个动坐标系相对参考坐标系的方位可以完全由动坐标系依次绕3个不同的轴转动的3个角度来确定。如将载体坐标系作为动坐标系，将导航坐标系作为参考坐标系，则姿态角即为一组欧拉角，按一定的转动顺序可以得到导航坐标系到载体坐标系的转换公式[6]

(10)

式中 左侧为本次更新终的欧拉角，对应横滚角γ、俯仰角θ和偏转角ψ；右侧为上个周期测算出的角度，以及三轴陀螺仪在周期t内测得的角速度。因此，求解该微分方程即可解算出当前的欧拉角

(11)

式中φ为需要解算的欧拉角；k为当前数据点；t为惯性传感器的采样周期，取为0.01 s；et为陀螺仪的零偏值。计算出陀螺仪三个轴的角度变化，以加速度计算出的初始姿态角为基准，即可得出佩戴在行人脚部的设备姿态角数据。

从图2的行人俯仰角数据可以看出：行走时俯仰角具有周期性的变化，其极大值和极小值分别为在踏向地面时和离开地面时的姿态角，由此可以判断行人的步频间隔。优点是可以根据行人运动姿态判断步数，判定结果更加直观、准确，缺点是姿态识别判断条件有限，可能出现多极值的情况，影响判断精度。

图2 姿态识别检测结果

本文采取零速修正和姿态识别结合算法，首先使用零速修正技术检测行人的静止状态，若某静止时刻超过正常步频范围，则在此时刻进行姿态识别检测，区分连续零速度点，实验中3种方法的检测正确率如表1所示，可以看出，零速修正和姿态识别的结合算法提高了计步精度。

表1 3种算法准确率对比

2 改进的行人惯性导航算法设计

获得了精确的步频后，可以通过惯性导航算法计算出单步的位移。传统的捷联惯性导航算法原理是通过对加速度进行二次积分，得到速度和位移数据。

2.1 单步长惯性导航算法

(12)

再由，可得到物体的位移变化量，最后得出运动物体在n系下的位置为

(13)

通过以上惯性导航算法可以计算出每步步频对应的单步长，如图3所示。为利于统计单步长的对应时间以及后续计算，图中“步频”的单位为秒(s)。

图3 惯性导航算法计算步幅

经过统计结果可以看出，基本捷联惯性导航算法计算出的步长与真实值的偏移在30 %以内，误差会随着时间累积，导致位移结果出现较大偏差，影响导航精度。

2.2 步长估计算法

统计数据表明，人的步频和步幅之间具有对应关系，行人步频一定时，其步幅基本固定，而且行人在直线行走时的步幅和步频相对稳定。同时在惯性导航算法从行人静止状态开始计算的一段时间内，其加速度积分导致的累积误差较小，惯性导航算法结果准确性较高，故本系统使用行人前期运动的单步长数据建立行人的步频—步长模型。实验进行了多组不同步速下的实验并建模，统计得出的步频—步长模型如表2所示。

表2 步频—步长模型示例

实验表明:在行人以惯常的步长和步频行走时，通过预先设置的参数可以保证测量的步长精度较高，但当行人转向或者步频突变时，实际步长与步频-步长模型中的参考结果会有一些偏差。

基本捷联惯性导航算法的优势是加速度计和陀螺仪可以适应行人的运动状态突变，在行人步频或航向变化时依然能输出较为精确的结果，缺点是随着行走距离的增加，基本惯性导航计算结果存在漂移和累积误差；使用步频—步长模型的优势是在行人直线行走和步频变化较小时，其精确度很高，可以有效避免长距离行走之后的累积误差，然而其在行人在转弯或发生步频变化时导致步长改变时偏差较大。

结合两种算法的特点，本文采用的改进算法在行人以惯常运动姿态的行走时使用步频—步长模型中的单步长数据；在行人运动姿态发生明显变化时，使用基本惯性导航算法计算行人位移。根据行人的运动姿态变化实时匹配相应的位移算法。

2.3 航向角获取与处理

智能手机电子罗盘的航向角误差主要来自手机内部电路运行时产生的磁场以及手机周围环境磁场产生的磁干扰dt[7]。由于干扰磁场的值变化平缓，不会瞬间突变，因此采用滑动均值滤波算法对电子罗盘输出数据进行校正。滑动均值滤波器公式为

(14)

式中k为当前数据点；xj为智能手机电子罗盘的输出原始数据，j取当前数据点以及当前数据点最近的m组数据；yk为滤波输出结果；m为一次进行滤波处理的数据个数，也称滑动滤波器阶数，本文根据行人的步频特征和实验数据，m取值100。

算法在行人步频和方向平稳时，使用单步中前后两静止时刻航向角的均值作为行人两步之间位移的航向角数据，并去除行人脚在空中时的航向角，可以有效避免电子罗盘由于手机的俯仰角变化而产生的航向角漂移，输出较为精确的方向数据。

3 实验验证

3.1 导航算法

系统使用了集成蓝牙HC—06模块和MPU6050惯性传感器，集成了加速度计、角速度计和磁力计，根据行人的运动特点采用鞋绑式MPU6050惯性传感器和手机相结合的数据采样方式。实验中模块平稳固定于行人脚面，行人手持手机正对行走方向。

基于步长的惯性导航技术主要包括4个关键技术：步频检测、步长估算、方向检测和位置估算。定位算法流程如图4所示，具体步骤如下：

1)数据采集和数据处理：采集佩戴在行人脚部的惯性传感器数据以及行人手持智能手机中的电子罗盘数据并存储。将原始数据进行零偏校准和滤波处理，得到相对准确的实验数据；

2)步频检测：通过零速修正技术和脚部姿态识别技术确定行人行走时的零速时刻，并计算单步步频；

3)步长估计：通过惯性导航算法解算出行人的单步步长，根据行人前期运动的惯性导航算法结果建立步长—步频模型。根据行人运动姿态识别分别匹配相应的步长估计算法；

4)通过基于滑动均值滤波的手机电子罗盘获取行人航向角数据，使用结合步频检测、步长估计的行人惯性导航算法计算得出行人的运动轨迹。

图4 系统流程

3.2 实验验证

为了验证本文算法在实际行人运动中的效果，分别进行了行人的直线行走实验和矩形行走实验。表3中的“基本惯性导航算法”指不区分单步长的基本捷联惯性导航算法的位移解算结果；“改进算法”指加入了零速修正—姿态设别结合算法和基于步频—步长模型的步长估计算法的改进行人惯性导航算法解算结果。

图5为行人自东向西行走35 m时，以坐标零点为起点，分别使用基本惯性导航算法和改进算法计算出行人运动轨迹。从表3的结果对比可以看出：基本惯性导航算法得到的行人位移信息在前期较为准确，其后期的位置计算有误差扩大的趋势，在35 m的直线行进中直线误差超过4 m，垂直于行进方向的纵向误差最大超过0.9 m。而改进算法在行人35 m的直线运动中，其直线误差在1 m以内，纵向误差保持在0.5 m以内，准确性更高。

图5 基本惯导算法和改进算法直线行走轨迹

方向实际结果基本惯导解算改进算法东向/m35．0030．1734．35北向/m 0．100．920．36

图6、图7为行人绕建筑物进行矩形行走示意图，行走轨迹的东西向距离为26.5 m，南北向距离为8.5 m，分别使用基本惯性导航算法和改进算法计算出行人的运动轨迹。从表4的结果对比可以看出：传统的惯性导航算法得到的运动轨迹在前期较准确，但随着时间的推移误差逐渐累积和发散、导致定位失败，最终终点在(-1.9,9.3)m，误差很大。而改进算法充分考虑了行人运动的特点，根据行人步频和航向的变化通过步频—步长模型实时校准单步长，并通过滑动均值滤实时校准航向角，最终输出的导航结果误差为(0.19,-0.23)m，运动中东西向和南北向的位移误差始终保持在2 m以内，保持了较高的稳定性和准确性。

图6 基本惯性导航算法完整行走轨迹

图7 改进算法完整行走轨迹

实际结果基本惯导解算改进算法起点/m(0，0)(0，0)(0，0)终点/m(0，0)(-1．9，9．3) (0．19，-0．23)

4 结 论

提出了一种基于运动姿态识别的行人惯性导航算法，使用MEMS惯性传感器和智能手机采集行人运动时的惯性数据，通过零速修正和姿态识别获得行人的脚部姿态数据，获得了精确的零速时刻和单步步频数据，根据行人前期运动的惯性导航计算结果建立步长—步频模型，并使用基于滑动均值滤波的航向角算法校准行人的方向信息，根据行人当前的运动姿态情况分别使用相应的步长估计算法，保证了行人单步的位移保持较高的精度，最终得到精确的行人位移信息。由于惯性导航算法在室内行人导航中无法对误差进行校准，未来可以考虑引入GPS定位技术或无线定位技术，在使用惯性导航算法时每隔一段时间对行人的当前位置进行识别和校准，可以更好地校正惯性导航算法的累积误差，建立应用范围更广的行人导航系统。

参考文献:

[1] 李作虎.卫星导航系统性能监测及评估方法研究[D].郑州：解放军信息工程大学,2012.

[2] 汪少初.基于PDR的定位与跟踪技术研究及系统设计[D].天津：天津大学,2014.

[3] Patrick E,Helen D.IMU:Inertial sensing of vertical CoM movement[J].Journal of Biomechanics,2009,42 (10):1578-1581.

[4] Levi R W,Judd T.Dead reckoning navigational system using accelerometer to measure foot impacts:US,US 5583776[P].1996—06—08.

[5] 孙 伟,初 婧,丁 伟,等.基于IMU旋转的MEMS器件误差调制技术研究[J].电子测量与仪器学报,2015(2):240-246.

[6] Jiménez A R,Seco F,Zampella F.PDR with a foot-mounted IMU and ramp detection[J].Sensors,2011,11(10):9393-9410.

[7] 代 刚.MEMS-IMU误差分析补偿与实验研究[D].北京：清华大学,2011.