0 引言

    行人导航系统(PNS)主要用于跟踪人员的实时位置信息。基于微机电惯性测量单元(MEMS IMU)的行人导航定位系统由于其不受环境约束、使用灵活和鲁棒性好等优点，在应急救灾、反恐安全以及日常生活等方面均具有较高的应用价值^[1]。

    行人惯性导航系统通常采用陀螺仪来估计航向，由于陀螺仪存在漂移误差，航向修正问题一直是国内外研究的热点。如南京航空航天大学曾庆化等人提出了一种蜂窝网格粒子滤波算法^[2]；中国科学院光电研究院公续平等人提出采用视觉陀螺仪的方法^[3]；英国诺丁汉大学ABDULRAHIM K等人提出利用建筑物的结构信息来修正行人航向信息的方法^[4]；美国密歇根大学BORESTEIN J等人提出启发式漂移消除算法，利用行人走直线时航向角的变化量对陀螺仪输出进行校正^[5]。但以上方法的修正效果不够理想，算法复杂且实时性较差。

    为了解决行人惯性导航中航向角发散的问题，在不引入外部信息的情况下，本文提出了一种基于主方向的航向修正算法，相比于启发式漂移消除算法中利用二进制积分控制器对陀螺仪的角速率进行修正的方法，本文直接对航向角误差进行估计，并将差值作为观测量，利用卡尔曼滤波器对航向进行修正，算法更加简练实用。

1 计步检测与步长估计

1.1 计步检测

    采用行人航位推算(PDR)的方法进行位置解算，首先要进行准确的计步检测，即判断行人是否跨步，然后再进行步长估计。计步检测主要利用加速度计信号，为了提高检测的鲁棒性，将三轴加速度信号作如下处理：

    常用的计步检测方法有：峰值检测、平区检测法和过零检测法^[6]。简便起见，本文采用峰值检测法来进行计步检测。为提高检测的准确率，特别加入以下两个约束条件：

    (1)加速度峰值必须大于阈值，避免因身体抖动带来的虚检测；

    (2)两连续峰值之间的时间间隔必须大于设定阈值，去除一步中多峰值的情况。

    峰值探测效果图如图1所示，图中圆圈表示峰值点，一个圆圈代表一步。

从图1可以看出，共有35个圆圈，表明行走了35步，与实际行走步数吻合，说明该计步探测算法具有较高准确率。

1.2 步长估计

    常用的步长估计模型主要分为：线性估计模型和非线性估计模型。

    线性步长估计模型如下所示：

    本文认为步长估计应采用非线性模型进行估计更为准确，并且非线性模型只有一个训练参数，更易于进行实时的步长估计，所以在进行单步距离计算时采用非线性模型。

2 航向估计与修正

2.1 航向估计

    基于PDR的行人导航系统的导航精度主要由步长估计精度和航向估计精度决定。考虑到算法精度，本文采用四元数法来解算航向角。

    在初始时刻，定义3个姿态角为0，利用式（4）初始化四元数，其中Ψ₀、θ₀、γ₀分别为初始的航向角、俯仰角和横滚角，公式左边为初始四元数。

   当陀螺仪数据更新后，利用二阶龙格库塔算法求解四元数微分方程，进行四元数的更新。四元数微分方程如式(5)所示，其中ω_x、ω_y、ω_z为3个轴的角速率，Q为t时刻的四元数。

    通过以上步骤的解算，便可以利用陀螺仪的角速率输出来得到行人的航向角，但陀螺仪的输出会随着时间而产生漂移，从而影响航向角的解算精度。为了抑制由于陀螺漂移而产生的航向角发散现象，需要采用相关手段对航向角进行修正。

2.2 基于主方向的航向修正算法

    陀螺仪的输出误差会随着时间而不断增大，在不使用外部辅助信息（如GNSS、地图）的情况下，通常采用零角速度修正（ZARU）、启发式随机漂移消除法（HDE）^[5]和磁力计进行偏航角的校正。

    本文在HDE算法的基础上，提出了基于主方向的航向修正算法。首先假设行人在室内环境行走时遵循8个主方向的原则，即可选移动方向呈一个“米”字型，如图2所示。

    HDE算法在计算航向角过程中，只利用了陀螺仪数据，但角速率直接积分会产生累积误差，另外系统对用来修正角速率的反馈系数比较敏感，导致系统的鲁棒性较差。而本文直接对航向角进行处理，将解算得到的航向角与当前主方向的差作为观测量进行卡尔曼滤波，从而对航向角进行修正。

    当行人按直线或接近直线的线路行走时，相邻两步的航向角变化是非常小的。行人进行转向时，当前步的航向角相对于前一步的航向角会发生巨大变化，因此可以利用相邻步之间航向角的突变来探测行人的转向活动。为了保证转向探测的正确率，本文采用3个相邻步间的航向变化情况来探测转向运动，公式如下所示：

3 实验与结果

    为验证提出的航向修正算法的有效性，本文利用自研的MIMU导航模块进行室内行人定位实验，将模块固定在腰部，设定行走轨迹为一个矩形，实际的效果图如图3所示。

    图3可以看出，未修正的虚线轨迹的航向角一直在偏移，导致最后形成的是平行四边形的轨迹，而修正后的轨迹虽然与真实轨迹仍存在一定误差，但已较好地接近实际轨迹，并且误差为总行进路程的1%，说明本文的航向修正算法具有较好的修正效果。

    为了进一步验证本文算法的修正效果，另外进行了两圈矩形行走实验，实际效果如图4所示。

    图4中可以看出，未修正的虚线轨迹严重偏离，两圈轨迹的重复性也较差；采用本文算法修正的实线轨迹较好地接近实际行走的矩形轨迹，并且两圈的重复性也较好，最终误差为总行程的2.3%，进一步验证了本文算法在航向上起到了一定的修正效果，提高了行人导航定位的精度。

4 结论

    航向发散问题是行人导航定位的主要误差源之一，为了抑制由于陀螺漂移而带来的航向误差问题，本文在启发式漂移消除算法（HDE）的基础上，提出了基于主方向的航向修正算法，将行人航向划分为8个主方向，当检测到行人在沿类直线轨迹行走时，将行人当前航向与当前主方向的差值作为观测量进行卡尔曼滤波，从而实现对航向的修正。

    实验结果表明，本文算法能够较好地修正行人航向角以及运动轨迹，从而提高行人导航的定位精度。但本文算法不适合在空旷环境中行人随意行走的情况下使用，在以后的工作中，考虑引入绝对位置信息，如地图信息[7]，或结合其他室内定位方法，如WiFi、UWB等，进一步改善行人惯性导航的航向问题，提高导航定位精度。

参考文献

[1] 申崇江，冯成涛，崔莹，等.穿戴式室内行人航位推算系统研究[C].第五届中国卫星导航学术年会论文集-S9 组合导航与导航新方法，2014.

[2] 曾庆化，万骏炜，刘建业，等.基于蜂窝网格粒子滤波的行人导航航向估计方法[J].中国惯性技术学报，2014(5)：576-579.

[3] 公续平，魏东岩，李祥红，等.一种面向智能终端的视觉陀螺仪/PDR/GNSS组合导航方法[C].第六届中国卫星导航学术年会，2015.

[4] ABDULRAHIM K，HIDE C，MOORE T，et al.Aiding low cost inertial navigation with building heading for pedestrian navigation[J].Journal of Navigation，2011，64(2)：219-233.

[5] BORENSTEIN J，OJEDA L，KWANMUANG S.Heuristic reduction of gyro drift for personnel tracking systems[J].Journal of Navigation，2009，62(1)：41-58.

[6] PARK J，KIM Y，LEE J.Waist mounted Pedestrian Dead-Reckoning system[C].Ubiquitous Robots and Ambient Intelligence(URAI)，2012 9th International Conference on.IEEE，2012：335-336.

[7] 胡安冬，王坚，高井祥.一种基于地图匹配辅助行人航位推算的室内定位方法[J].测绘科学技术学报，2014(5)：529-532.