基于手机陀螺仪航向修正算法

郭 英，姬现磊,2，刘清华，李冠泽,2，徐 莹

（1. 山东科技大学 测绘科学与工程学院，青岛 266590；2. 中国测绘科学研究院，北京100830）

随着智能手机技术和微机电系统（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°。初始化四元数(,,,)公式如下：

式中：h为数据采样间隔；q(t)、q(t+h)分别是t和t+h时刻的四元数；为

根据上述公式即可获得实时的四元数更新。

第3步：四元数归一化。

虽然选用简便且精度高的航向角解算方法能获取具有一定精度的航向信息，但陀螺仪的输出随着时间而产生随机漂移误差，这严重制约着航向角精度的提升[12]。

2.2 对航向角进行修正

本文对航向角信息设置了两次判别分析，旨在准确地对行走状态分类，避免出现不良校正情况。另外，应用 KF算法，状态量只有航向与主导航向的差值，极大地简化了iHDE算法。每当行人沿主导方向行走时，iHDE算法就会生效，对航向信息进行修正，从而抑制了漂移误差，进而提高了定位的精度。具体算法步骤如下。

第1步：设定主导航向。

主方向可以设定为4个或8个主导方向，如图2所示，本文研究以8个主导方向为研究基础，主导方向的间隔date值为45°。

图2 主导方向Fig.2 Dominant direction

第2步：辨别行走状态。

选择最接近的主导方向，计算行走航向与主导方向的差值：

即得行走航向与主导方向的差值。

第3步：航向角标准差求解。

第4步：卡尔曼滤波处理。

将行走航向与最近的主导方向的差值 δ θ（i）作为状态量输入卡尔曼滤波算法中，其预测方程为

计算误差方差阵：

观测方程为：

计算卡尔曼增益：

计算i时刻最优估值：

更新i时刻下X(i|i)的方差阵：

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

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

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算法和磁力计修正算法相融合，从而得到更精确的航向信息。

（

）：

[1] 刘宇, 周帆, 李云梅, 等. 基于人体多方位运动的三维自主导航定位算法[J]. 中国惯性技术学报, 2016, 24(4):449-453.Liu 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): 598-602.Lou 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): 108-111.Zhao 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): 332-336.Zeng 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): 442-445.Tian 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].青岛: 山东科技大学, 2015.Chen C Y. Study on mobile phone sensor based indoor positioning method[D]. Qingdao: Shandong University of Science and Technology, 2015.