2024年4月6日发(作者：索晟睿)
基于手机陀螺仪航向修正算法
郭英;姬现磊;刘清华;李冠泽;徐莹
【摘 要】将低价位手机(魅蓝NOTE3)中的陀螺仪作为研究对象,为了减少由于陀螺
仪漂移带来的航向误差,鉴于改进的启发式消除算法(iHDE)在航向修正的优越性,对
基于行人导航算法框架IEZ的iHDE算法做出改进.由于人们携带手机的方式不同
于脚踏式惯性单元IMU,IEZ算法框架已不再适用,故将iHDE应用在基于步态分析
和航向估计的行人航迹推算定位算法框架(SHS-PDR)中,并把航向与主方向的偏差
作为唯一的状态量输入到卡尔曼滤波器中,得到航向的最优估计,即简化了原始
iHDE算法.最后,设计了两类不同难度的行走实验,即常规路线实验与挑战性路线实
验,且实验中定位误差分别为总行进距离的0.2％与0.9％,定位精度与未经航向修正
的算法相比分别提升了88.9％与43.8％.实验证明,应用在基于手机传感器的SHS-
PDR算法框架下的iHDE算法依然保持其良好的航向修正能力.%This paper
takes the gyroscope in the mobile phone (MeiLan NOTE3) as the research
object and aims to reduce the heading error caused by the drift of the
gyroscope. According to the superiority of the improved heuristic drift
elimination (iHDE) algorithm in course corrections, an improved iHDE
algorithm based on the pedestrian navigation algorithm framework IEZ is
proposed. Since the IEZ algorithm framework is not applicable due to the
fact that the way people carrying the mobile phone is different from that
of the foot inertial unit IMU, the iHDE algorithm is applied to the
pedestrian dead reckoning estimation algorithm (SHS-PDR) based on the
gait analysis and heading estimation. Meanwhile, to obtain the optimal
heading estimation, the deviation between the heading and the main
direction is inputted into the Kalman filter as the only state parameter,
which can simplify the original iHDE algorithm. Finally, two kinds of
walking experiments (routine route experiment and challenging route
experiment) are designed and carried out, which show that the positioning
errors are 0.2％ and 0.9％ of the total experimental distance respectively,
and the positioning accuracies are improved by 88.9％ and 43.8％
respectively compared with those without heading correction. The
experiment results verify that the iHDE algorithm applied in the SHS-PDR
algorithm framework based on the mobile sensor still maintains good
heading correction capability.
【期刊名称】《中国惯性技术学报》
【年(卷),期】2017(025)006
【总页数】6页(P719-724)
【关键词】手机陀螺仪;四元数;改进的启发式消除算法;行人航迹推算
【作 者】郭英;姬现磊;刘清华;李冠泽;徐莹
【作者单位】山东科技大学 测绘科学与工程学院,青岛 266590;山东科技大学 测绘
科学与工程学院,青岛 266590;中国测绘科学研究院,北京 100830;山东科技大学 测
绘科学与工程学院,青岛 266590;山东科技大学 测绘科学与工程学院,青岛 266590;
中国测绘科学研究院,北京 100830;山东科技大学 测绘科学与工程学院,青岛
266590
【正文语种】中 文
【中图分类】U666.1
随着智能手机技术和微机电系统（Micro Electro Mechanical System, MEMS）
技术的高速发展，MEMS传感器已成为手机的必备器件，并且智能手机已普及千
家万户，这些都为基于手机惯性传感器定位技术的应用提供了巨大的优势[1]。基
于步态检测和航行估计（Step and Heading, SHS）的航迹推算系统
（PedestrianDead Reckoning, PDR），通过分析、处理加速度数据和陀螺仪数
据，获取每一步的步长和航向信息，由起始位置逐步推算出实时定位信息[4,7,11]。
消除由陀螺仪的漂移误差导致的航向误差一直是阻碍室内定位精度提升的难点
[9,12]，为了减少航向误差，学者们提出了几种航向修正的方法：Borenstein等
人[3]提出了基于脚踏式惯性测试单元（Inertial Measure Unit, IMU）的启发式偏
移减少算法（Heuristic Drift Elimination Reduction, HDR）和启发式漂移消除
算法（Heuristic Drift Elimination, HDE），该算法建立一个反馈机制，通过反馈
信息对陀螺仪数据进行修正，进而达到对航向角修正的目的；楼喜中等人[6]在
HDE的基础上，将航向角作为直接修改的对象，消除了部分陀螺仪积分误差；赵
辉等人[8]基于HDE算法提出了基于卡尔曼滤波和经验参数公式的算法；Ju等人
[2]提出了高级启发式漂移消除算法（Advanced Heuristic Drift Elimination,
AHDE），该算法把行走分为了三种状态，在非直线行走时AHDE不启用，在主
导方向行走和直线但非主导方向行走时分别建立扩展卡尔曼滤波（Extended
Kalman Filter, EKF）进行航向误差修正；Jimenez等人[5]提出了改进的启发式漂
移消除算法（improved Heuristic Drift Elimination, iHDE），直接对航向角误
差进行估计，将方位角与主导方向的偏差作为观测量，利用EKF对航向进行修正。
HDE和HDR算法适合简单的行走路线，在非主导直线行走30~60 s后，HDE算
法的效用使室内定位算法恶化，定位精度下降严重[2,10]。虽然AHDE和iHDE算
法能适应一般的复杂路线，但都是基于 INS (Inertial Navigation
System)+EKF+ZUPT (Zero velocity UPdate）框架（IEZ）下使用该算法，尤其
是iHDE算法被建议使用在IEZ框架下[2]。以上四种算法的提出都是基于脚踏式
IMU进行研究的，对于用户来说，测量装置置于脚下会对用户日常行走带来不舒
适感，基于手机MEMS传感器定位技术有着巨大的优势条件。本文对iHDE算法
进行了改进，将其应用在手机MEMS陀螺仪和基于SHS的PDR算法系统中，使
算法更加简单实用。
1 iHDE算法框架改进
在人造建筑中大多数走廊都是直的，并且彼此平行或正交，把建筑物墙壁和走廊的
方向称为建筑物的“主导方向”，超过99%的人造建筑有四个或八个主导方向[3]。
启发式消除算法就是依托在这种环境下的航向修正算法，如果计算出的航向角与最
接近的主导航向相匹配，则实施算法进行航向矫正。
iHDE是由 Jimenez等人[5]提出的改进的 HDE算法。相对比HDE算法，iHDE
不仅在室内简易环境下效果突出，在一定的复杂环境下也表现了较好的效果，
Jimenez建议iHDE算法用于IEZ算法框架中。IEZ算法是指基于脚踏式IMU的
PDR算法，这种算法根据IMU置于鞋底的数据特性，加入了零速更新算法
（ZUPT）来抑制误差的积累。另外，该框架融入了零角速度更新（Zero Angular
Rate Update, ZARU），当脚部运动时，对角速度进行零角速度矫正。最终应用
包含15个元素误差状态向量的EKF对行人的位置和姿态做出估计，该向量中包含
加速度误差和陀螺仪误差（分别为、 、姿态误差()、位置误差()以及速度误差()。
本文研究对象为智能手机，手机姿态多样化（手持、放口袋、打电话等状态），
ZUPT算法和ZARU算法已不再适用，即IEZ框架在基于手机的行人定位研究中已
不再适用。本文iHDE算法应用在SHS-PDR算法框架下，应用以航向与主航向的
偏差为状态量的卡尔曼滤波器（Kalman Filter，KF）对航向角做出估计，简化了
iHDE算法。改进后的iHDE算法框图如图1所示。该算法基于 SHS-PDR算法系
统，通过三轴加速度数据和三轴陀螺仪数据获取基本的步长信息和航向信息，再对
航向信息进行分析及修正，最终由步长和修正后的航向信息获得精确位置信息。
图1 系统框架Fig.1 System architecture
2 改进算法实现
2.1 航向角解算
第1步：选用四元数法来解算航向信息、初始化四元数。
常用的姿态描述方法有欧拉角法、方向余弦法和四元数法。由于相对比其他两个方
法，四元数方法具有计算简单，易于操作，且能避免出现欧拉角所产生的“奇点”
问题的优点，故本文选择四元数法来解算姿态信息。
初始姿态信息设置：航向角为90°，俯仰角和横滚角设置都为0°。初始化四元数
(,,,)公式如下：
式中： 分别为初始的航向角、俯仰角和横滚角。第2步：利用二阶龙格库塔法实
时更新四元数。解算公式：
式中：h为数据采样间隔；q(t)、q(t+h)分别是t和t+h时刻的四元数；为
式中：分别为x、y和z轴陀螺仪数据。
根据上述公式即可获得实时的四元数更新。
第3步：四元数归一化。
式中：、、、是四元数更新所得的值。
第4步：由四元数计算旋转矩阵并计算航向角。
式中为由载体坐标系b至导航坐标系n的旋转矩阵。
虽然选用简便且精度高的航向角解算方法能获取具有一定精度的航向信息，但陀螺
仪的输出随着时间而产生随机漂移误差，这严重制约着航向角精度的提升[12]。
2.2 对航向角进行修正
本文对航向角信息设置了两次判别分析，旨在准确地对行走状态分类，避免出现不
良校正情况。另外，应用 KF算法，状态量只有航向与主导航向的差值，极大地简
化了iHDE算法。每当行人沿主导方向行走时，iHDE算法就会生效，对航向信息
进行修正，从而抑制了漂移误差，进而提高了定位的精度。具体算法步骤如下。
第1步：设定主导航向。
主方向可以设定为4个或8个主导方向，如图2所示，本文研究以8个主导方向
为研究基础，主导方向的间隔date值为45°。
图2 主导方向Fig.2 Dominant direction
第2步：辨别行走状态。
当时，判定为沿直线行走，否则为曲线行走。当为沿直线行走时，进行下一项判断。
选择最接近的主导方向，计算行走航向与主导方向的差值：
即得行走航向与主导方向的差值。
当时，判定行走为沿主导方向行走，否则为非主导直线行走。当行走为沿主导方向
直线行走时，对航向信息进行下一步修正工作。D和的阈值设定是在阅读文献后
经过大量实验确定的[2,5,8]。
第3步：航向角标准差求解。
其中，为 0.1 rad[5]。
第4步：卡尔曼滤波处理。
将行走航向与最近的主导方向的差值 δ θ（i）作为状态量输入卡尔曼滤波算法中，
其预测方程为
计算误差方差阵：
观测方程为：
计算卡尔曼增益：
计算i时刻最优估值：
更新i时刻下X(i|i)的方差阵：
其中：为系统在i时刻的状态；）为i时刻的观测值；为状态方程噪声，方差为 Q；
为观测方程噪声，方差为R； 为卡尔曼增益；故由最优估计偏差后可得航向角信
息：
式中， 、分别为修正前后的航向角。
2.3 航迹推算
经iHDE算法修正过的航向角信息与加速度计分析所得的步长信息相结合，获得精
确的定位信息。所谓航迹推算，即由上一位置的坐标结合当前一步的航向角信息与
步长信息求解当前位置的坐标，公式如下：
式中，x、y为位置坐标，S为步长。
3 实验分析
3.1 数据采集
为验证航向修正算法的有效性，共设计了5组实验，实验以魅蓝NOTE3手机为研
究对象。魅蓝NOTE3是2016年4月份生产的手机，刚发售时的价格因机身内存
不同设有799元和999元两个价位，本实验采用的是价位为799元的手机，该手
机中有陀螺仪、加速度计、磁力计等基本传感器。本文以陈春阳[13]设计的安卓软
件“导航项目”为工具来获取传感器数据，该软件简单实用，满足实验的需求。实
验数据采集频率为60 Hz，在数据采集实验中实验者手持手机，并平放于身前（如
图3所示），以正常步速按设计的路线行走。
根据行走路线的复杂程度，可分为常规路线实验和挑战性路线实验。
常规路线实验是行走路线以主导方向行走为主，实验场地为山东科技大学测绘学院
办公楼3楼的回型走廊，行走的矩形路线长62 m、宽42 m。该实验包括矩形一
圈行走实验和矩形两圈行走实验。
图3 数据采集Fig.3 Data acquisition
为进一步验证iHDE算法的实用性和稳定性，另外又进行了3组挑战性实验（非主
导方向行走占大比例的路线）：1）折线行走实验：该路线先沿主导方向行走10
m，然后转动30°行走8 m的距离，最后沿主导方向行走 10 m；2）闭合三角形
行走实验：该路线为先沿主导方向行走6.93 m，然后转动30°行走8 m的距离，
最后沿主导方向行走4 m回到起点位置；3）带环形行走实验：该路线为先沿主导
方向行走30 m，然后沿半径为25 m的花坛行走一圈，最后沿主导方向行走30
m回到起点位置。
本文将5组实验数据分别运用原始SHS-PDR算法、基于SHS-PDR算法系统的
HDE算法以及iHDE算法处理，不同方法在同一图中绘出路线图可得到直观的对
比，并进行了精度分析。
3.2 常规路线实验分析
图4为矩形一圈行走中航向角变化图，图5为实验效果图，表1是对常规路线实
验的精度分析。由图4~5和表1分析可知：
图4 航向角变化图Fig.4 Change of heading angle
1）陀螺仪的随机漂移误差导致未修正的航向角变化存在波动，这将对定位精度有
一定的影响；经iHDE算法修正后的航向角变化更平滑，体现了 iHDE算法可以通
过对航向角修正来实现消除陀螺仪漂移误差的影响（见图4）。
图5 常规实验路线图Fig.5 Regular route experiment roadmap
表1 常规实验精度分析Tab.1 Accuracy analysis of regular route experiment
实验类型 TTD/m SHS-PDR HDE iHDE闭合差/m TTD误差 闭合差/m TTD
误差 闭合差/m TTD误差一圈矩形实验 208.000 5.114 2.5% 1.030 0.5%
0.668 0.3%两圈矩形实验 416.000 4.446 1.1% 1.269 0.3% 0.605 0.1%平
均精度 1.8% 0.4% 0.2%
2）未经航向修正的SHSPDR算法行走路线有非常严重的航向偏移现象，虽然
HDE算法和iHDE算法最终的总行进距离（Total Traveled Distance，TTD）误
差都很低且相差不大，但行走过程中HDE算法存在明显的航向偏移，出现了行走
路线从建筑物中穿过的现象。iHDE算法平均TTD误差为0.2%，与未经航向修正
的算法相比定位精度提升了 88.9%，证明了iHDE算法在常规路线中具有高精度、
高稳定性的优点。
3.3 挑战性实验分析
由表2和图6分析可知：
1）HDE算法由于在非主导直线行走和曲线行走情况下错误地校正，使行走路线出
现严重的偏移，定位精度急剧下降。在复杂的行走路线中，HDE算法稳定性低，
航向修正功能失效。
图6 挑战性实验路线图Fig.6 Challenging route experiment roadmap
表2 挑战性实验精度分析Tab.2 Accuracy analysis of challenging route
experiment实验类型 TTD/m SHS-PDR HDE iHDE闭合差/m TTD误差 闭
合差/m TTD误差 闭合差/m TTD误差折线行走实验 28.000 0.382 1.4%
1.996 7.1% 0.161 0.6%三角形实验 18.930 0.259 1.4% 2.303 12.2%
0.235 1.2%带有环形实验 217.080 3.975 1.8% 2.888 1.3% 2.097 1.0%平
均精度 1.6% 6.9% 0.9%
2）本文设计的iHDE算法进行了更准确的行走状态分析，避免了错误的校正，平
均TTD误差为0.9%，与未经航向修正的算法相比定位精度提升了 43.8%。故
iHDE算法仍可以达到对路线整体定位精度提升的效果，说明iHDE算法稳定性更
高，可以适用于具有一定复杂环境路线中。
4 结 论
本文以低价位手机为实验对象，选用四元数法解算航向信息、二阶龙格库塔法进行
四元数更新，然后把经过四元数解算的航向信息与HDE算法、iHDE算法得到的
航向信息分别与步长信息相结合，实现位置解算。通过5组实验和3种不同方法
的定位精度分析可得以下结论：
1）iHDE算法消除了由于漂移引起的误差，大幅度提升了定位精度。综合5组实
验，iHDE算法在相对复杂化的情况下，TTD误差范围为0.2%~0.9%，与未经航
向修正的算法相比定位精度提升范围为 43.8%~88.9%，与 HDE相比具有更高的
精度和稳定性，且适合更复杂的情况。
2）本文实验表明，iHDE算法可用于 SHS-PDR系统中，并且该算法在低价位的
智能手机上得到了较好的应用，有利于为更广大的手机用户提供更可靠的行人定位
服务。
本文已初步把iHDE算法应用到基于手机传感器的 SHS-PDR算法框架中，在日后
的研究中，可以考虑加入磁力计来对航向进一步修正，将iHDE算法和磁力计修正
算法相融合，从而得到更精确的航向信息。
参考文献（
References）：
[1] 刘宇, 周帆, 李云梅, 等. 基于人体多方位运动的三维自主导航定位算法[J]. 中国
惯性技术学报, 2016, 24(4): Y, Zhou F, Li Y M, et al. Three-
dimensional autonomous navigation algorithm based on multi-azimuth
movement pattern[J]. Journal of Chinese Inertial Technology, 2016, 24(4):
449-453.
[2] Ju H J, Min S L, Chan G P, et al. Advanced heuristic drift elimination for
indoor pedestrian navigation[C]//International Conference on Indoor
Positioning and Indoor Navigation. IEEE, 2015: 729-732.
[3] Borenstein J, Ojeda L. Heuristic drift elimination for personnel tracking
systems[J]. Journal of Navigation,2010, 63(4): 591-606.
[4] Hsu Y L, Chen Y J, Shih S W. A particle filter approach for pedestrian
dead reckoning using wearable sensors[C]//International Conference on
Innovative Mobile and Internet Services in Ubiquitous Computing. IEEE
Computer Society, 2016: 26-32.
[5] Jimenez A R, Seco F, Zampella F, et al. Improved heuristic drift
elimination (iHDE) for pedestrian navigation in complex
buildings[C]//International Conference on Indoor Positioning and Indoor
Navigation. IEEE, 2011: 1-8.
[6] 楼喜中, 周乐宇, 叶敏展, 等. 基于角度的 HDE算法在室内行人航迹定位中的研
究[J]. 传感技术学报,2015, 28(4): X, Zhou L, Ye M, et al.
Researching on indoor pedestrian trajectory based on angle HDE
algorithm[J].Chinese Journal of Sensors and Actuators, 2015, 28(4):598-602.
[7] Su L M, Ju H, Song J W, et al. Kinematic model-based pedestrian dead
reckoning for heading correction and lower body motion tracking[J].
Sensors, 2015, 15(11):28129-53.
[8] 赵辉, 李擎, 李超. 基于主方向的行人自主定位航向修正算法[J]. 电子技术应用,
2016, 42(11): H, Li Q, Li C. A heading correction algorithm
based on the main direction for pedestrian navigation[J]. Application of
Electronic Technique, 2016, 42(11): 108-111.
[9] 曾庆军, 刘慧婷, 张明. 基于 HDR的陀螺仪随机漂移分析与处理[J]. 江苏大学学
报(自然科学版), 2016,37(3): Q J, Liu H T, Zhang M. Analysis
and processing on random drift of gyroscope based on HDR[J]. Journal of
Jiangsu University (Natural Science Edition), 2016, 37(3):332-336.
[10] Diez L E, Bahillo A, Bataineh S, et al. Enhancing improved heuristic drift
elimination for step-and-heading based pedestrian dead-reckoning
systems[C]//International Conference of the IEEE Engineering in Medicine
&Biology Society. 2016: 4415.
[11] Harle R. A survey of indoor inertial positioning systems for
pedestrians[J]. IEEE Communications Surveys &Tutorials, 2013, 15(3): 1281-
1293.
[12] 田晓春, 陈家斌, 韩勇强, 等. 一种优化的小波阈值去噪方法在行人导航系统中
的应用[J]. 中国惯性技术学报, 2015, 23(4): X C, Chen J B, Han Y
Q, et al. Application of optimized wavelet threshold de-nosing method in
pedestrian navigation system[J]. Journal of Chinese Inertial Technology,
2015, 23(4): 442-445.
[13] 陈春阳. 基于运动传感器的手机室内定位方法研究[D].青岛: 山东科技大学,
C Y. Study on mobile phone sensor based indoor positioning
method[D]. Qingdao: Shandong University of Science and Technology,
2015.
2024年4月6日发(作者：索晟睿)
基于手机陀螺仪航向修正算法
郭英;姬现磊;刘清华;李冠泽;徐莹
【摘 要】将低价位手机(魅蓝NOTE3)中的陀螺仪作为研究对象,为了减少由于陀螺
仪漂移带来的航向误差,鉴于改进的启发式消除算法(iHDE)在航向修正的优越性,对
基于行人导航算法框架IEZ的iHDE算法做出改进.由于人们携带手机的方式不同
于脚踏式惯性单元IMU,IEZ算法框架已不再适用,故将iHDE应用在基于步态分析
和航向估计的行人航迹推算定位算法框架(SHS-PDR)中,并把航向与主方向的偏差
作为唯一的状态量输入到卡尔曼滤波器中,得到航向的最优估计,即简化了原始
iHDE算法.最后,设计了两类不同难度的行走实验,即常规路线实验与挑战性路线实
验,且实验中定位误差分别为总行进距离的0.2％与0.9％,定位精度与未经航向修正
的算法相比分别提升了88.9％与43.8％.实验证明,应用在基于手机传感器的SHS-
PDR算法框架下的iHDE算法依然保持其良好的航向修正能力.%This paper
takes the gyroscope in the mobile phone (MeiLan NOTE3) as the research
object and aims to reduce the heading error caused by the drift of the
gyroscope. According to the superiority of the improved heuristic drift
elimination (iHDE) algorithm in course corrections, an improved iHDE
algorithm based on the pedestrian navigation algorithm framework IEZ is
proposed. Since the IEZ algorithm framework is not applicable due to the
fact that the way people carrying the mobile phone is different from that
of the foot inertial unit IMU, the iHDE algorithm is applied to the
pedestrian dead reckoning estimation algorithm (SHS-PDR) based on the
gait analysis and heading estimation. Meanwhile, to obtain the optimal
heading estimation, the deviation between the heading and the main
direction is inputted into the Kalman filter as the only state parameter,
which can simplify the original iHDE algorithm. Finally, two kinds of
walking experiments (routine route experiment and challenging route
experiment) are designed and carried out, which show that the positioning
errors are 0.2％ and 0.9％ of the total experimental distance respectively,
and the positioning accuracies are improved by 88.9％ and 43.8％
respectively compared with those without heading correction. The
experiment results verify that the iHDE algorithm applied in the SHS-PDR
algorithm framework based on the mobile sensor still maintains good
heading correction capability.
【期刊名称】《中国惯性技术学报》
【年(卷),期】2017(025)006
【总页数】6页(P719-724)
【关键词】手机陀螺仪;四元数;改进的启发式消除算法;行人航迹推算
【作 者】郭英;姬现磊;刘清华;李冠泽;徐莹
【作者单位】山东科技大学 测绘科学与工程学院,青岛 266590;山东科技大学 测绘
科学与工程学院,青岛 266590;中国测绘科学研究院,北京 100830;山东科技大学 测
绘科学与工程学院,青岛 266590;山东科技大学 测绘科学与工程学院,青岛 266590;
中国测绘科学研究院,北京 100830;山东科技大学 测绘科学与工程学院,青岛
266590
【正文语种】中 文
【中图分类】U666.1
随着智能手机技术和微机电系统（Micro Electro Mechanical System, MEMS）
技术的高速发展，MEMS传感器已成为手机的必备器件，并且智能手机已普及千
家万户，这些都为基于手机惯性传感器定位技术的应用提供了巨大的优势[1]。基
于步态检测和航行估计（Step and Heading, SHS）的航迹推算系统
（PedestrianDead Reckoning, PDR），通过分析、处理加速度数据和陀螺仪数
据，获取每一步的步长和航向信息，由起始位置逐步推算出实时定位信息[4,7,11]。
消除由陀螺仪的漂移误差导致的航向误差一直是阻碍室内定位精度提升的难点
[9,12]，为了减少航向误差，学者们提出了几种航向修正的方法：Borenstein等
人[3]提出了基于脚踏式惯性测试单元（Inertial Measure Unit, IMU）的启发式偏
移减少算法（Heuristic Drift Elimination Reduction, HDR）和启发式漂移消除
算法（Heuristic Drift Elimination, HDE），该算法建立一个反馈机制，通过反馈
信息对陀螺仪数据进行修正，进而达到对航向角修正的目的；楼喜中等人[6]在
HDE的基础上，将航向角作为直接修改的对象，消除了部分陀螺仪积分误差；赵
辉等人[8]基于HDE算法提出了基于卡尔曼滤波和经验参数公式的算法；Ju等人
[2]提出了高级启发式漂移消除算法（Advanced Heuristic Drift Elimination,
AHDE），该算法把行走分为了三种状态，在非直线行走时AHDE不启用，在主
导方向行走和直线但非主导方向行走时分别建立扩展卡尔曼滤波（Extended
Kalman Filter, EKF）进行航向误差修正；Jimenez等人[5]提出了改进的启发式漂
移消除算法（improved Heuristic Drift Elimination, iHDE），直接对航向角误
差进行估计，将方位角与主导方向的偏差作为观测量，利用EKF对航向进行修正。
HDE和HDR算法适合简单的行走路线，在非主导直线行走30~60 s后，HDE算
法的效用使室内定位算法恶化，定位精度下降严重[2,10]。虽然AHDE和iHDE算
法能适应一般的复杂路线，但都是基于 INS (Inertial Navigation
System)+EKF+ZUPT (Zero velocity UPdate）框架（IEZ）下使用该算法，尤其
是iHDE算法被建议使用在IEZ框架下[2]。以上四种算法的提出都是基于脚踏式
IMU进行研究的，对于用户来说，测量装置置于脚下会对用户日常行走带来不舒
适感，基于手机MEMS传感器定位技术有着巨大的优势条件。本文对iHDE算法
进行了改进，将其应用在手机MEMS陀螺仪和基于SHS的PDR算法系统中，使
算法更加简单实用。
1 iHDE算法框架改进
在人造建筑中大多数走廊都是直的，并且彼此平行或正交，把建筑物墙壁和走廊的
方向称为建筑物的“主导方向”，超过99%的人造建筑有四个或八个主导方向[3]。
启发式消除算法就是依托在这种环境下的航向修正算法，如果计算出的航向角与最
接近的主导航向相匹配，则实施算法进行航向矫正。
iHDE是由 Jimenez等人[5]提出的改进的 HDE算法。相对比HDE算法，iHDE
不仅在室内简易环境下效果突出，在一定的复杂环境下也表现了较好的效果，
Jimenez建议iHDE算法用于IEZ算法框架中。IEZ算法是指基于脚踏式IMU的
PDR算法，这种算法根据IMU置于鞋底的数据特性，加入了零速更新算法
（ZUPT）来抑制误差的积累。另外，该框架融入了零角速度更新（Zero Angular
Rate Update, ZARU），当脚部运动时，对角速度进行零角速度矫正。最终应用
包含15个元素误差状态向量的EKF对行人的位置和姿态做出估计，该向量中包含
加速度误差和陀螺仪误差（分别为、 、姿态误差()、位置误差()以及速度误差()。
本文研究对象为智能手机，手机姿态多样化（手持、放口袋、打电话等状态），
ZUPT算法和ZARU算法已不再适用，即IEZ框架在基于手机的行人定位研究中已
不再适用。本文iHDE算法应用在SHS-PDR算法框架下，应用以航向与主航向的
偏差为状态量的卡尔曼滤波器（Kalman Filter，KF）对航向角做出估计，简化了
iHDE算法。改进后的iHDE算法框图如图1所示。该算法基于 SHS-PDR算法系
统，通过三轴加速度数据和三轴陀螺仪数据获取基本的步长信息和航向信息，再对
航向信息进行分析及修正，最终由步长和修正后的航向信息获得精确位置信息。
图1 系统框架Fig.1 System architecture
2 改进算法实现
2.1 航向角解算
第1步：选用四元数法来解算航向信息、初始化四元数。
常用的姿态描述方法有欧拉角法、方向余弦法和四元数法。由于相对比其他两个方
法，四元数方法具有计算简单，易于操作，且能避免出现欧拉角所产生的“奇点”
问题的优点，故本文选择四元数法来解算姿态信息。
初始姿态信息设置：航向角为90°，俯仰角和横滚角设置都为0°。初始化四元数
(,,,)公式如下：
式中： 分别为初始的航向角、俯仰角和横滚角。第2步：利用二阶龙格库塔法实
时更新四元数。解算公式：
式中：h为数据采样间隔；q(t)、q(t+h)分别是t和t+h时刻的四元数；为
式中：分别为x、y和z轴陀螺仪数据。
根据上述公式即可获得实时的四元数更新。
第3步：四元数归一化。
式中：、、、是四元数更新所得的值。
第4步：由四元数计算旋转矩阵并计算航向角。
式中为由载体坐标系b至导航坐标系n的旋转矩阵。
虽然选用简便且精度高的航向角解算方法能获取具有一定精度的航向信息，但陀螺
仪的输出随着时间而产生随机漂移误差，这严重制约着航向角精度的提升[12]。
2.2 对航向角进行修正
本文对航向角信息设置了两次判别分析，旨在准确地对行走状态分类，避免出现不
良校正情况。另外，应用 KF算法，状态量只有航向与主导航向的差值，极大地简
化了iHDE算法。每当行人沿主导方向行走时，iHDE算法就会生效，对航向信息
进行修正，从而抑制了漂移误差，进而提高了定位的精度。具体算法步骤如下。
第1步：设定主导航向。
主方向可以设定为4个或8个主导方向，如图2所示，本文研究以8个主导方向
为研究基础，主导方向的间隔date值为45°。
图2 主导方向Fig.2 Dominant direction
第2步：辨别行走状态。
当时，判定为沿直线行走，否则为曲线行走。当为沿直线行走时，进行下一项判断。
选择最接近的主导方向，计算行走航向与主导方向的差值：
即得行走航向与主导方向的差值。
当时，判定行走为沿主导方向行走，否则为非主导直线行走。当行走为沿主导方向
直线行走时，对航向信息进行下一步修正工作。D和的阈值设定是在阅读文献后
经过大量实验确定的[2,5,8]。
第3步：航向角标准差求解。
其中，为 0.1 rad[5]。
第4步：卡尔曼滤波处理。
将行走航向与最近的主导方向的差值 δ θ（i）作为状态量输入卡尔曼滤波算法中，
其预测方程为
计算误差方差阵：
观测方程为：
计算卡尔曼增益：
计算i时刻最优估值：
更新i时刻下X(i|i)的方差阵：
其中：为系统在i时刻的状态；）为i时刻的观测值；为状态方程噪声，方差为 Q；
为观测方程噪声，方差为R； 为卡尔曼增益；故由最优估计偏差后可得航向角信
息：
式中， 、分别为修正前后的航向角。
2.3 航迹推算
经iHDE算法修正过的航向角信息与加速度计分析所得的步长信息相结合，获得精
确的定位信息。所谓航迹推算，即由上一位置的坐标结合当前一步的航向角信息与
步长信息求解当前位置的坐标，公式如下：
式中，x、y为位置坐标，S为步长。
3 实验分析
3.1 数据采集
为验证航向修正算法的有效性，共设计了5组实验，实验以魅蓝NOTE3手机为研
究对象。魅蓝NOTE3是2016年4月份生产的手机，刚发售时的价格因机身内存
不同设有799元和999元两个价位，本实验采用的是价位为799元的手机，该手
机中有陀螺仪、加速度计、磁力计等基本传感器。本文以陈春阳[13]设计的安卓软
件“导航项目”为工具来获取传感器数据，该软件简单实用，满足实验的需求。实
验数据采集频率为60 Hz，在数据采集实验中实验者手持手机，并平放于身前（如
图3所示），以正常步速按设计的路线行走。
根据行走路线的复杂程度，可分为常规路线实验和挑战性路线实验。
常规路线实验是行走路线以主导方向行走为主，实验场地为山东科技大学测绘学院
办公楼3楼的回型走廊，行走的矩形路线长62 m、宽42 m。该实验包括矩形一
圈行走实验和矩形两圈行走实验。
图3 数据采集Fig.3 Data acquisition
为进一步验证iHDE算法的实用性和稳定性，另外又进行了3组挑战性实验（非主
导方向行走占大比例的路线）：1）折线行走实验：该路线先沿主导方向行走10
m，然后转动30°行走8 m的距离，最后沿主导方向行走 10 m；2）闭合三角形
行走实验：该路线为先沿主导方向行走6.93 m，然后转动30°行走8 m的距离，
最后沿主导方向行走4 m回到起点位置；3）带环形行走实验：该路线为先沿主导
方向行走30 m，然后沿半径为25 m的花坛行走一圈，最后沿主导方向行走30
m回到起点位置。
本文将5组实验数据分别运用原始SHS-PDR算法、基于SHS-PDR算法系统的
HDE算法以及iHDE算法处理，不同方法在同一图中绘出路线图可得到直观的对
比，并进行了精度分析。
3.2 常规路线实验分析
图4为矩形一圈行走中航向角变化图，图5为实验效果图，表1是对常规路线实
验的精度分析。由图4~5和表1分析可知：
图4 航向角变化图Fig.4 Change of heading angle
1）陀螺仪的随机漂移误差导致未修正的航向角变化存在波动，这将对定位精度有
一定的影响；经iHDE算法修正后的航向角变化更平滑，体现了 iHDE算法可以通
过对航向角修正来实现消除陀螺仪漂移误差的影响（见图4）。
图5 常规实验路线图Fig.5 Regular route experiment roadmap
表1 常规实验精度分析Tab.1 Accuracy analysis of regular route experiment
实验类型 TTD/m SHS-PDR HDE iHDE闭合差/m TTD误差 闭合差/m TTD
误差 闭合差/m TTD误差一圈矩形实验 208.000 5.114 2.5% 1.030 0.5%
0.668 0.3%两圈矩形实验 416.000 4.446 1.1% 1.269 0.3% 0.605 0.1%平
均精度 1.8% 0.4% 0.2%
2）未经航向修正的SHSPDR算法行走路线有非常严重的航向偏移现象，虽然
HDE算法和iHDE算法最终的总行进距离（Total Traveled Distance，TTD）误
差都很低且相差不大，但行走过程中HDE算法存在明显的航向偏移，出现了行走
路线从建筑物中穿过的现象。iHDE算法平均TTD误差为0.2%，与未经航向修正
的算法相比定位精度提升了 88.9%，证明了iHDE算法在常规路线中具有高精度、
高稳定性的优点。
3.3 挑战性实验分析
由表2和图6分析可知：
1）HDE算法由于在非主导直线行走和曲线行走情况下错误地校正，使行走路线出
现严重的偏移，定位精度急剧下降。在复杂的行走路线中，HDE算法稳定性低，
航向修正功能失效。
图6 挑战性实验路线图Fig.6 Challenging route experiment roadmap
表2 挑战性实验精度分析Tab.2 Accuracy analysis of challenging route
experiment实验类型 TTD/m SHS-PDR HDE iHDE闭合差/m TTD误差 闭
合差/m TTD误差 闭合差/m TTD误差折线行走实验 28.000 0.382 1.4%
1.996 7.1% 0.161 0.6%三角形实验 18.930 0.259 1.4% 2.303 12.2%
0.235 1.2%带有环形实验 217.080 3.975 1.8% 2.888 1.3% 2.097 1.0%平
均精度 1.6% 6.9% 0.9%
2）本文设计的iHDE算法进行了更准确的行走状态分析，避免了错误的校正，平
均TTD误差为0.9%，与未经航向修正的算法相比定位精度提升了 43.8%。故
iHDE算法仍可以达到对路线整体定位精度提升的效果，说明iHDE算法稳定性更
高，可以适用于具有一定复杂环境路线中。
4 结 论
本文以低价位手机为实验对象，选用四元数法解算航向信息、二阶龙格库塔法进行
四元数更新，然后把经过四元数解算的航向信息与HDE算法、iHDE算法得到的
航向信息分别与步长信息相结合，实现位置解算。通过5组实验和3种不同方法
的定位精度分析可得以下结论：
1）iHDE算法消除了由于漂移引起的误差，大幅度提升了定位精度。综合5组实
验，iHDE算法在相对复杂化的情况下，TTD误差范围为0.2%~0.9%，与未经航
向修正的算法相比定位精度提升范围为 43.8%~88.9%，与 HDE相比具有更高的
精度和稳定性，且适合更复杂的情况。
2）本文实验表明，iHDE算法可用于 SHS-PDR系统中，并且该算法在低价位的
智能手机上得到了较好的应用，有利于为更广大的手机用户提供更可靠的行人定位
服务。
本文已初步把iHDE算法应用到基于手机传感器的 SHS-PDR算法框架中，在日后
的研究中，可以考虑加入磁力计来对航向进一步修正，将iHDE算法和磁力计修正
算法相融合，从而得到更精确的航向信息。
参考文献（
References）：
[1] 刘宇, 周帆, 李云梅, 等. 基于人体多方位运动的三维自主导航定位算法[J]. 中国
惯性技术学报, 2016, 24(4): Y, Zhou F, Li Y M, et al. Three-
dimensional autonomous navigation algorithm based on multi-azimuth
movement pattern[J]. Journal of Chinese Inertial Technology, 2016, 24(4):
449-453.
[2] Ju H J, Min S L, Chan G P, et al. Advanced heuristic drift elimination for
indoor pedestrian navigation[C]//International Conference on Indoor
Positioning and Indoor Navigation. IEEE, 2015: 729-732.
[3] Borenstein J, Ojeda L. Heuristic drift elimination for personnel tracking
systems[J]. Journal of Navigation,2010, 63(4): 591-606.
[4] Hsu Y L, Chen Y J, Shih S W. A particle filter approach for pedestrian
dead reckoning using wearable sensors[C]//International Conference on
Innovative Mobile and Internet Services in Ubiquitous Computing. IEEE
Computer Society, 2016: 26-32.
[5] Jimenez A R, Seco F, Zampella F, et al. Improved heuristic drift
elimination (iHDE) for pedestrian navigation in complex
buildings[C]//International Conference on Indoor Positioning and Indoor
Navigation. IEEE, 2011: 1-8.
[6] 楼喜中, 周乐宇, 叶敏展, 等. 基于角度的 HDE算法在室内行人航迹定位中的研
究[J]. 传感技术学报,2015, 28(4): X, Zhou L, Ye M, et al.
Researching on indoor pedestrian trajectory based on angle HDE
algorithm[J].Chinese Journal of Sensors and Actuators, 2015, 28(4):598-602.
[7] Su L M, Ju H, Song J W, et al. Kinematic model-based pedestrian dead
reckoning for heading correction and lower body motion tracking[J].
Sensors, 2015, 15(11):28129-53.
[8] 赵辉, 李擎, 李超. 基于主方向的行人自主定位航向修正算法[J]. 电子技术应用,
2016, 42(11): H, Li Q, Li C. A heading correction algorithm
based on the main direction for pedestrian navigation[J]. Application of
Electronic Technique, 2016, 42(11): 108-111.
[9] 曾庆军, 刘慧婷, 张明. 基于 HDR的陀螺仪随机漂移分析与处理[J]. 江苏大学学
报(自然科学版), 2016,37(3): Q J, Liu H T, Zhang M. Analysis
and processing on random drift of gyroscope based on HDR[J]. Journal of
Jiangsu University (Natural Science Edition), 2016, 37(3):332-336.
[10] Diez L E, Bahillo A, Bataineh S, et al. Enhancing improved heuristic drift
elimination for step-and-heading based pedestrian dead-reckoning
systems[C]//International Conference of the IEEE Engineering in Medicine
&Biology Society. 2016: 4415.
[11] Harle R. A survey of indoor inertial positioning systems for
pedestrians[J]. IEEE Communications Surveys &Tutorials, 2013, 15(3): 1281-
1293.
[12] 田晓春, 陈家斌, 韩勇强, 等. 一种优化的小波阈值去噪方法在行人导航系统中
的应用[J]. 中国惯性技术学报, 2015, 23(4): X C, Chen J B, Han Y
Q, et al. Application of optimized wavelet threshold de-nosing method in
pedestrian navigation system[J]. Journal of Chinese Inertial Technology,
2015, 23(4): 442-445.
[13] 陈春阳. 基于运动传感器的手机室内定位方法研究[D].青岛: 山东科技大学,
C Y. Study on mobile phone sensor based indoor positioning
method[D]. Qingdao: Shandong University of Science and Technology,
2015.