基于主方向的行人自主定位航向修正算法*

赵 辉，李 擎，李 超

（北京信息科技大学 高动态导航技术北京市重点实验室，北京 100101）

基于主方向的行人自主定位航向修正算法*

赵 辉，李 擎，李 超

（北京信息科技大学 高动态导航技术北京市重点实验室，北京 100101）

无信标环境下的行人导航问题是目前导航领域的难题和研究热点，考虑到系统的便携性和实用性，行人自主导航系统多采用惯性器件进行定位解算。针对当前的行人惯性导航系统航向角发散问题，在启发式漂移消除算法（HDE）的基础上，提出一种基于主方向的航向修正算法，根据室内的行走方向大多分为8个主方向的事实，当检测到行人轨迹为直线时，将当前的航向角与主方向角的差值作为观测量进行卡尔曼滤波，对航向角进行修正，并利用腰部PDR的方案进行了单圈和两圈矩形轨迹实验。实验结果表明，该算法在航向修正方面具有一定有效性，且重复性好，定位误差为总路程的1%～2%。

室内行人导航；陀螺漂移；航向修正；主方向；卡尔曼滤波器

0 引言

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

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

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

1 计步检测与步长估计

1.1 计步检测

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

其中，a3D为三轴合成加速度，ax、ay、az分别为三轴加速度信号。采用滑动平均滤波对合成加速度进行处理，以消除其中的噪声信息。

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

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

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

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

图1 峰值探测

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

1.2 步长估计

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

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

式中，SL为估计步长，SF为步频，SV为每一步的加速度方差，a、b、c为回归参数。

非线性步长估计模型如下：

式中，Amax、Amin分别为一步中加速度的最大值和最小值，k为经验常数，通过实际测试确定。

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

2 航向估计与修正

2.1 航向估计

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

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

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

姿态转换矩阵得到后，利用式(7)求出航向角。其中Ψ为航向角，Tij(n)表示姿态矩阵的第 i行、第 j列的元素。

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

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

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

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

图2 主方向划分

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

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

式中，m表示行走路线状态，1代表直线，0代表转弯；Ψk表示当前步的航向角，Ψk-1和 Ψk-2分别表示前一步和前两步的航向角，Ψth表示偏差阈值。通常情况下，设定角度值10°作为阈值。

当判断行人走直线时，计算当前航向角与当前主方向差值，将差值作为观测量进行卡尔曼滤波。

设置9维误差状态向量，如下所示：

式中δφn=[δθδγδΨ]，为3个姿态角误差向量；δvn=[δvxδvyδvz]为三轴速度误差向量；εn=[εxεyεz]为三轴角速度误差向量。

建立的误差状态方程如下所示：

式中 Φk，k-1是状态转移矩阵，Γk-1和 Wk-1分别是噪声强度矩阵和噪声矩阵。

针对航向角误差的观测方程如下所示：

其中，Vyaw为航向角观测噪声矩阵。

3 实验与结果

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

图3 单矩形轨迹行走效果图

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

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

图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.

A heading correction algorithm based on the main direction for pedestrian navigation

Zhao Hui，Li Qing，Li Chao
(Beijing Key Laboratory of High Dynamic Navigation Technology，Beijing Information Science and Technology University，Beijing 100101，China)

Pedestrian navigation in a non beacon environment is a difficult problem and a hot research topic in the field of navigation.Considering the portability and practicality of the system,the pedestrian self-navigation system uses the inertial device for positioning.Aiming at the heading divergence problem of the current pedestrian inertial navigation system,a heading correction algorithm based on the main direction under the foundation of Heuristic Drift Elimination is proposed.It is a fact that most of the corridors and the paths in the man-made buildings are perpendicular or parallel to each other.The heading direction is divided into eight directions.The difference between the current heading and the main direction is as the observation value when the pedestrian is walking along a straight line.Then the filter is used to correct the heading angels.Single rectangle and double rectangle tests are done.The results show that the algorithm can correct the heading direction efficiently and the location error is about 1%to 2%of total travel distance.

indoor pedestrian navigation；gyroscope drift；heading correction；main direction；Kalman filter

TP391.9

A

10.16157/j.issn.0258-7998.2016.11.029

赵辉，李擎，李超.基于主方向的行人自主定位航向修正算法[J].电子技术应用，2016，42(11)：108-111.

英文引用格式：Zhao Hui，Li Qing，Li Chao.A heading correction algorithm based on the main direction for pedestrian navigation[J]. Application of Electronic Technique，2016，42(11)：108-111.

2016-03-29)

赵辉（1992-），男，硕士研究生，主要研究方向：行人自主导航与定位。

李擎（1964-），通信作者，女，博士，教授，主要研究方向：高动态导航与控制，E-mail：liqing@bistu.edu.cn。

李超（1990-），男，博士，主要研究方向：室内定位技术、组合导航算法。

国家自然科学基金(61471046)；北京市教委市属高校创新能力提升计划项目(TJSHG201510772017）