第29卷第1期中国惯性技术学报V ol.29 No.1 2021年02月Journal of Chinese Inertial Technology Feb. 2021 文章编号：1005-6734(2021)01-0008-08 doi.10.13695/jki.12-1222/o3.2021.01.002
郭 英，刘寒烁，叶 瑾，盛方园
（山东科技大学 测绘与空间信息学院，青岛266590）
摘要：基于手机陀螺仪的航向推算误差是影响行人航位推算定位的重要误差源之一。为了修正陀螺仪引起的航向漂移、抖动等误差，借助高级启发式漂移消除法(AHDE)在航向修正上的特点，将基于足绑式IMU的AHDE算法改进并应用于智能手机的行人航向推算中。首先利用互补滤波算法初步估算航向，通过时域内航向及其变化幅度判断行人是否直行或者沿主导方向直行，并将其与行人航向的差值作为输入量推算可靠的行人航向，从而简化原AHDE算法并降低修正延迟；同时，针对直线路径下航向抖动过大引起的误判和偏差，利用航向平滑模型改正。另外，为进一步适应室内的复杂路线，通过分析行人停顿时加速度数据变化特点，提出了停顿点的识别方法；通过对比手机不同姿态下传感器数据的特点，建立动态转换矩阵，将贴耳打电话时的手机传感器数据转换至平放状态，进而采用上述算法推算行人航向。综合实验表明所提出的方法相较于互补滤波算法在相对闭合差上精度平均提升60%以上。 关 键 词：手机陀螺仪；行人航位推算；高级启发式漂移消除法；互补滤波；航向修正
中图分类号：U666.1 文献标志码：A
Pedestrian heading correction method based on AHDE and mobile
phone gyroscope
GUO Ying, LIU Hanshuo, YE Jin, SHENG Fangyuan
(College of Geodesy and Geomatics, Shandong University of Science and Technology, Qingdao 266590, China)
Abstract: The heading error based on mobile phone gyroscope is one of the important error sources that affect PDR positioning. In order to correct the errors of heading drift and jittering caused by gyroscope, the advanced heuristic drift elimination method (AHDE) based on the foot mounted IMU is improved and applied to the pedestrians dead reckoning of the smartphone with the help of the AHDE in the heading correction. Firstly, complementary filtering algorithm is used to calculate the original heading, and the heading and its variation range in the time domain are used to judge whether the pedestrian is going straight or along the dominant direction, and the difference between
the heading and the pedestrian is used as the input to calculate the reliable pedestrian heading, so as to simplify the original AHDE algorithm and reduce the correction delay. Taking into account the misjudgment and deviation caused by heading jitter, the heading smoothing model is established. Moreover, to further adapt to the complex indoor route, the recognition method of the paused point is determined by analyzing the characteristics of acceleration data changes during pedestrian pauses. By comparing the characteristics of the sensor data of the mobile phone in different postures, a dynamic conversion matrix is established to convert the mobile phone sensor data from calling to handing, and then the above algorithm is used to calculate the pedestrian heading. Comprehensive experiments show that the proposed method improves the accuracy of relative closure error by more than 60% on average compared with the complementary filtering algorithm.
Key words: mobile phone gyroscope; pedestrians dead reckoning(PDR); advanced heuristic drift elimination(AHDE); complementary filter; heading correction
收稿日期：2020-09-06；修回日期：2021-01-11
基金项目：山东省重点研发计划项目（2018GGX106003）
作者简介：郭英（1973—），女，副教授，硕士生导师，研究方向为室内定位方向。E-mail：********
******
第1期郭英等：基于AHDE和手机陀螺仪的行人航向修正方法9
近年来，位置服务成为了人们日常生活中必不可少的服务，全球卫星导航系统(Global Navigation Satellite System, GNSS)在位置服务中占据着重要的位置，然而导航卫星信号的可用性在室内不能得到保证，GNSS在室内环境中的性能严重下降。因此，建立一个准确、可靠、实时的室内定位系统，满足公众的室内定位需求具有重要意义[1]。
随着微机电技术(Micro-Electro-Mechanical Systems, MEMS)的不断发展，惯性测量单元(Inertial Measurement Units, IMU)的体积越来越小，价格越来越便宜，并且随着智能手机的普及和改进，IMU被普遍应用于智能手机中，使基于智能手机的行人导航定位成为室内定位领域的一个重要新分支[2]。
然而普通的IMU均存在一定程度的累积误差影响，为了在误差影响下提高行人航向估算精度，国内外学者提出了一些航向修正方法：Borenstein等人[3]提出了基于陀螺仪航向估计的启发式漂移消除算法（Heuristic Drift Elimination, HDE），该方法利用室内环境大多为矩形，走廊一般为直线型的特性，预先设定好四个主导方向，当检测到行人靠近主导方向行走时，航向偏差会由HDE算法来修正；Jimenez等人[4]提出了改进的启发式漂移消除算法（improved Heuristic Drift Elimination, iHDE），该方法在HDE算法原有的主导方向上再加入四个，每个主导方向相隔45 °，相较于HDE算法，iHDE算法 在非理想的室内环境中的表现要优于HDE算法[5]。为了进一步提高航向估算精度，一些基于固定式IMU的航向修正算法被提出。Muhammad等人[6]提出了骨盆旋转-零速率更新旋转检测算法（Pelvic Rotation-ZUPT Turn Detection, PZTD），该算法基于腰部穿戴式惯性传感设备，检测行人是否直行和处于主导方向，对行人沿主导方向直行和未在主导方向直行两种情况进行航向修正，不仅对主导方向的航向修正也提高了非主导方向上的航向精度。王鹏宇等人[7]基于iHDE算法，使用腰绑式惯性传感器，对航向建立缓冲区来抑制iHED算法在特定场景下的过度修正问题。Ju等人[12]提出了基于足绑式IMU的高级启发式漂移消除算法（Advanced Heuristic Drift Elimination, AHDE），该算法利用行人连续六步的位置数据判断是否直线行走或者沿主导方向直线行走。相比iHDE算法，AHDE加入了非主导方向直行的航向修正。在实际中，手机相较于固定式IMU更加灵活，一些基于固定式IMU的航向修正算法在手机端难以实现；室内环境复杂，非主导方向直行的情况较多，iHDE算法难以发挥优势；室内直行的路线长短不一，AHDE算法使用连续多步数据检测是否直行会直接影响计算效率和修正效果。
基于此，本文首先分析基于智能手机框架的互补滤波航向推算原理，然后根据一秒时域内航向变化幅度判断行人是否直行，针对直线行走和主导方向下直线行走建立航向修正算法，并且对行人停顿和姿态转换进行识别与处理，最后实验验证该方法在常规和复杂环境中的有效性。
1 基于互补滤波的航向推算基本原理
描述姿态常用的方法有欧拉角法、四元数法、罗德里格斯参数、方向余弦法等。四元数法相较于欧拉角法计算量大但计算精度更高[9]。而相较于罗德里格斯参数和方向余弦法，四元数法计算量小、简单高效，并且可以全姿态求解。而且四元数法避免了产生的奇点的问题[14]。因此这里使用四元数法来描述姿态，根据式(1)将初始欧拉角转换为四元数。
⎪
⎪
⎪
⎪
⎩
⎪
⎪
⎪
⎪
⎨
⎧
-
=
+
=
-
=
+
=
2
sin
2
sin
2
cos
2
cos
2
cos
2
sin
2
cos
2
sin
2
cos
2
sin
2
cos
2
sin
2
cos
2
sin
摄像机机芯2
sin
2
sin
2
cos
2
cos
里德穆勒
2
cos
2
cos
2
cos
2
sin
2
sin
2
sin
3
2
1
γ
θ
ψ
γ
θ
ψ
γ
θ
ψ
γ
θ
ψ
γ
θ
ψ
γ
θ
ψ
γ
θ
ψ
γ
θ
ψ
q
q
q
q
(1)
式中，
ψ、
θ和
γ分别为航向角、俯仰角以及横滚角。
为了减弱陀螺仪漂移误差，提升航向推算精度，这里使用互补滤波融合陀螺仪和加速度计数据[10]。即
通过四元数计算得到载体坐标系下的重力矢量b
v
g，
b b n
v n
g C g
=⋅，其中[]T
0,0,
n
g g
=，n g为重力场在导航坐标系下的投影，单位为2
m/s，b
n
C为导航坐标系转换为载体坐标系的旋转矩阵；加速度计数据计算得到
载体本身的重力矢量b
a
g；实际上b b
v a
g g
≠，也就是陀螺仪数据在一定程度上存在偏差，将上述两矢量叉乘得到偏差矢量e：
b b
v a
e g g
=⊗(2) 根据矢量偏差e及控制参数修正陀螺仪偏差：
kp e ki e
ωω
=+⋅+⋅∑(3) 式中，kp为比例控制参数，ki为积分控制参数，ω为三轴陀螺仪数据。根据二阶龙格库塔法使用修正后的陀螺仪数据更新的四元数，并转换为姿态角。
2 基于高级启发式漂移消除算法（AHDE）航向修
正方法
由于航向漂移误差会随着行走距离的增加不断增
10 中国惯性技术学报 第29卷
大，且行人在走路的过程中以直线行走居多，故有必要加入一定的航向修正算法。本文利用AHDE 算法中将行人行走状态分为非直线、直线、沿主导方向直线行走的思想建立航向修正算法。 2.1 AHDE 算法原理及分析
AHDE 算法是由Ju 等人[12]提出的高级HDE 算法，该算法通过判别行人行走的方式将行人行走分为三种模式，即非直线行走、直线行走和沿着主导方向直线行走，改进了iHDE 算法缺少非主导方向直线行走时修正航向的问题[11]。在非直线行走时AHDE 算法不生效，误差与基于足绑式惯性单元PDR 算
法框架（INS-EKF-ZUPT, IEZ ）一致[8]；在直线行走时，将当前行进航向与五步前的航向作差，使用该差值作为扩展卡尔曼滤波（Extended Kalman Filter, EKF ）的测量值估计航向；在沿着主导方向直线行走时，将当前航向与主导航向的差值作为EKF 的测量值估计航向。
相比较于HDE 算法，AHDE 算法不仅能在简单的主导方向上起到明显效果，而且能在复杂的路线上起到一定作用。而AHDE 算法应用于IEZ 框架中，在手机端不适用；AHDE 算法在判定行走状态时用到连续六步位置数据，由于步数较大，航向修正将会延迟[6]
；同时，AHDE 算法使用五步的位置数据估算直线行走的航向也会引起不必要的误差。而且，AHDE 算法未考虑到主导方向偏离时过度修正问题。 2.2 航向修正算法改进
考虑到手机陀螺仪自身误差，这里以互补滤波推算得到的航向为输入值，按照时域分析航向角的变化情况判断是否直行；对直线行走的航向加入直线修正估算行人航向，而非使用第一步的航向值作为直线行走的航向；沿主导方向直线行走时加入直线和主导方向修正算法估算行人航向，将直线修正后的航向与主导方向的差值作为输入量估计行人航向；对非直线行走航向加入航向抖动检测及消弱算法，
避免手机抖动过大引起的误判；最后对停顿点和姿态转换进行识别与处理，提高行人在复杂室内环境中的航向精度。 2.2.1 行走模式识别
传统的AHDE 算法中，确定直线行走所需要的步数相当大，导致检测滞后，引起不必要的延迟和误差。室内环境下直行路线长短不一，可能轨迹的末端是非主导方向，或者直行步数达不到检测所需步数，这种情况下航向修正效果可能更差。为了避免这种情况，通过式(4)计算航向变化幅度，通过记录航向和1 s 内航向变化幅度确定是否直行。
()||/i m
i n i n i
m δφφ+==-∑
(4)
式中，m 为1 s 窗口大小（在这里m =50），i φ为历元i 下的航向角，i δ为历元i 下的航向变化幅度。一些学者分析行人在走路的过程中盆腔最大旋转的幅度在4 °到9 °之间。在行人前进的过程中手持的手机会随之产生一定程度的抖动，如果按照最大旋转幅度来考虑行人在行走过程中的抖动情况，可以认为一步的航向变化在9 °内。
为了能更加准确的判别行人的行走模式，这里通过实验来验证行人行走时的航向变化幅度。实验地点为山东科技大学J6楼，实验中使用的智能手机为
OPPO R9s ，该智能手机的惯性传感器为博世公司的BMI160，该传感器的部分参数见表1。
表1 BMI 160传感器技术参数
Tab.1 Specifications of the BMI 160
加速度计
陀螺仪
量程（FS ）±2 g
±2000 °/s
非线性 0.5% FS
0.1% FS
零偏稳定性
- 15 °/h
在实验中，我们招募了5名志愿者覆盖了不同的年龄身高以及性别，其中包括男性A ，175 cm/70 kg/50岁；男性B ，180 cm/90 kg/29岁；女性C ，
174 cm/63 kg/26岁；男性D ，167 cm/56 kg/16岁；女性E ，163 cm/58 kg/50岁。志愿者被要求在40 m-100 m 的直线中行走，传感器采样频率为50 Hz ，实验中将手机平放在胸前，保持导航模式行走。为了直观展示航向变化幅度的分布情况，该部分数据以箱型图的形式展示，如图1所示，其中A1和B1为志愿者A 和B 非匀速行走数据。在箱型图中，中心标记为数据中值，箱的上下端分别表示第25%和75%位的数据，“+”表示异常值。
图1 行人航向变化幅度箱型图
Fig.1 The box-plot of change of pedestrians heading
从图1和表2可以看出，匀速行走下的航向变化幅度平均集中在3 °以下，即便是最大异常值仍在4 °以内；非匀速行走的数据是为了模拟直线行走过程中
第1期 郭英等：基于AHDE 和手机陀螺仪的行人航向修正方法 11
手机不平稳的情况，平均最大异常值在4.5 °左右，在非匀速A1实验中超出了4.5 °，虽然在实验处理中将非连续性超出阈值的异常数据认作噪音去除掉，但也说明过度的航向抖动可能会引起误判，为减弱这一情况的影响，后续的航向修正会针对误判情况进行修正。根据盆腔旋转幅度以及实际直线行走的结果可以得出合适的判断直行的阈值D Th 。
表2 行人航向变化幅度数据表（单位：（°））
Tab.2 Table of pedestrian course change amplitude data （Unit ：（°））
实验组 上边缘 下边缘 中线 最大异常值匀速
A 2.9 0.3 1.4 3.7
B 3.1 0.2 1.4 3.4
C 3.1 0.3 1.5 3.8
D 3.0 0.5 1.6 3.7
E 3.1 0.2 1.3 3.9
非匀速
A1 4.1 0.5 1.9 4.6 B1 3.4 0.5 1.7 4.5
2.2.2 直线行走航向修正
传统的直线航向修正是在判断行人连续几步的航向变化不大的情况下，将后续的几步航向角与判断为直线行走起的航向保持一致，直至脱离直线行走。传统方法判定为直线行走时行人的航向是保持不变的（即1k k φφ-=，直到脱离直线行走），这与实际并不相符（行人直线行走时航向会小幅度波动）。
为了降低直线行走过程中航向偏差，以及考虑到阈值判断时产生的误判情况，公式(5)将当前航向与直线方向的偏差'k φ作为输入量对直线行走模式下的航向偏差分等级修正，其修正示意图见图2。
图2 直线行走下航向修正对比图
Fig.2 Comparison chart of heading correction when walking
straight
其中小角度偏差被完全减弱，大角度偏差保留一定原有特性，若'k φ超出直线行走的正常航向变化范围，结束直线修正。这样不仅修正直线行走航向偏差
而且降低误判造成的过度修正影响。
4'round 52
k r φπ
φπ⎛⎫=⨯ ⎪⎝⎭ (5)
式中，r φ为修正后偏差值，'k φ为历元k 下航向与直线方向的偏差，round 为取整函数。 2.2.3 主导方向航向修正
主导方向是事先建立好的方向，如图3所示，当行人沿着主导方向直线行走时，航向被主导方向修正算法修正。
图3 主导方向示意图
Fig.3 The chart of dominant direction
图3将主导方向定为8个，行人直线行走时的航向在主导方向附近时会被识别算法检测，式(6)[6]为主导方向判别公式。
mod(round , 2)22.5i φε⎛⎫
=
⎪⎝⎭
(6) 式中，round 为取整函数，mod 为取余函数，i φ为历元i 下的航向。ε为1则认为不在主导方向上行走；当ε为0则认为行人沿主导方向行走，此时的航向与主导方向差值在12 °内。为了提高修正精度并降低过度修正误差，公式(7)以当前航向与主导方向的偏差量
i c 为输入量，降低6 °以上的偏差量的修正幅度，减弱因行人行走模式误判导致的过度修正；将6 °以内偏差量的航向看作完全沿主导方向行走。
航向偏差值i b 为：
()sin 4round 12i i i i i c b c c c ⎛
⎫
=-⋅⋅ ⎪⎝⎭
(7)
当前航向与主导航向偏差量i c 为：
22.5round 22.5i i i c φφ⎛⎫
=-⋅ ⎪⎝⎭
细胞核染(8)
通过以上公式进行两圈矩形100 m 行走测试，如图4所示。互补滤波计算得到的航向偏差会随着时间不断变大，图中行走状态为1则行人转弯；为0则行人直行，可以看出行走模式能够被正确判断，航向偏差能够被检测并且被修正算法补偿。
12 中国惯性技术学报 第29卷
图4 主导方向直线行走航向修正对比图
Fig.4 Heading correction comparison chart for straight walk
along the dominant directions
2.2.4 航向抖动识别与减弱
废棉考虑到在修正航向角的过程中可能因各种影响造成航向抖动导致直线行走未被正确识别，通过加权平均模型建立航向抖动减弱公式(9)，该公式使用非直线行走下连续三步的航向数据来判断航向抖动情况，即连续三步航向相互作差，将抖动情况分为三类并分别修正，其修正示意图见图5。
()
()()()11211211212",,,"',,,""',,, or ,,',other k k k k k k k
A A A Th
B
C Th A Th A A Th
D
E A Th A Th A Th A Th A Th A Th ηηθηθηηθηηφφφφφφφ---⎧<⎪
⎪+>><⎪⎪
=+<><⎨⎪
<<>⎪⎪
⎪⎩(9)
在这里，'φ和''φ分别为未修正航向和计算完成后的航向，1A 、2A 、A 分别为1'-'k k φφ+、21'-'k k φφ++、
()()121'-''-'k k k k φφφφ++++。B ，C ，D ，E 为系数，
Th θ和Th η为判断值，由直线判别时的角度变化幅度确定。
图5 航向抖动优化示意图
Fig.5 The chart of heading jitter optimization
行人步频使用传统的峰值检测法计算，步幅模型为非线性模型中的Weinberg 方法[13]，最后结合步长和航向信息推算行人航迹。
2.2.5 停顿点识别与处理
为了解决行人在行走的过程中遇到障碍物，或者因其他原因在原地停顿而产生位置误差的问题，在推算行人航向的同时加入了停顿点检测。
由于行人在某一位置停顿时加速度计数据趋于稳定，因此选择建立加速度数据方差识别行人的停顿位置。
图6(a)展示了手机在平放状态下行人停顿和行走
时的加速度计数据，停顿状态下明显稳定于行走状态，从图6(b)可以看出根据其3轴加速度计计算的窗口方差，在停顿区域的方差值十分稳定，相互配合可以比较准确地确定出停顿点的位置。
镜片镀膜机
(a) 加速度计数据
(b) 加速度方差
(a) Accelerometer data (b) Acceleration variance
图6 停顿检测示意图
Fig.6 The chart of pause detection
为此通过研究建立了行人停顿时的检测识别方法，如下式：
1 , ,
0 other ax ay az V Th V Th V Th C <<<⎧⎪=⎨
⎪⎩
(10) 其中，
ax V ，ay V ，az V 分别为加速度计三轴的方差，Th 为阈值，C 为1时处于停顿状态，C 为0时处于行
走状态。
由于舍去停顿区间分段建立行走路线，需要匹配停顿区间前后的航向，选取停顿前短时间内未发生大幅度变化的四元数的均值，使用该四元数来推算下一段路线的行人航向。
2.2.6 手机姿态转换识别与处理电子蜂毒采集器
图7 不同手机位置下加速度数据对比图
Fig.7 The chart of acceleration data in different mobile phone
position
手机平放查阅及打电话模式之间的转换为手机
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议