基于手机传感器和互补滤波的行人航向解算

郭 英，孙玉曦,2，姬现磊，冯茗杨，刘清华

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

随着新型移动设备(如智能手机、平板电脑、运动手环)及基于位置的服务(location based service，LBS)的迅速发展，行人航迹推算(pedestrian dead reckoning，PDR)作为室内定位的重要技术，逐渐成为有关专家和部门研究与应用的热点。PDR中的姿态信息主要通过由陀螺仪、加速度计组成的微惯性测量单元(miniature inertial measurement unit，MIMU)测量获得，但是由于MIMU的漂移及运动噪声的影响，若不加以处理，这些误差会随着时间的增加而积累，导致导航精度降低或完全无法导航[1]。

如何较好地降低各种误差的影响，专家、学者进行了一系列研究。将陀螺仪和加速度传感器信号进行融合，从而修正陀螺仪的偏差，该方法能较好地减少水平面倾角的漂移[2]。文献[3]给出了基于四元数法的互补滤波器，并通过调节互补滤波截止频率，降低了姿态解算的运算量，提高了效率和精度。

以上几种方法虽然都能对行人航向角进行一定的修正，但都没有考虑外界环境的影响。由于地磁场复杂多变，且易受外界环境的干扰，使得导航定位发生偏差。文献[4]提出了一种基于准静止磁场检测的行人导航算法框架，该方法能在磁干扰环境中有效地修正航向漂移误差，但其流程复杂，难以理解与实现。

如何在外界磁干扰环境及传感器噪声较大的情况下较好地推算航向角，已经成为研究者们亟待解决的问题。基于此，本文在分析互补滤波原理的基础上，加入PI调节器，并以陀螺仪输出的角速度数据为依据，实时调节PI调节器的误差补偿系数，提出一种可以自适应补偿系数的改进型互补滤波算法，以提高滤波效果和航向角融合精度。

1 传感器姿态解算

目前，利用陀螺仪进行姿态解算，用于描述载体坐标系与导航坐标系转换关系的方法有欧拉角法、方向余弦法和四元数法等。其中，欧拉角法在求解姿态时存在奇点，不能用于全姿态的解算[5]；方向余弦可用于全姿态的解算但计算量大，不能满足实时性要求[6]；四元数法算法简单，易于操作，只需要求解4个参数，计算量小，可以在实现过程中修正漂移，应用比较广泛[7]。

基于四元数的AHRS姿态解算算法，根据采用数据的不同，分为三轴陀螺仪数据解算、六轴数据融合解算及九轴数据融合解算。

2 基于改进互补滤波算法的航向角融合

2.1 互补滤波

陀螺仪动态响应特性良好，但计算姿态时，长时间工作会产生累积误差，使得解算出来的姿态出现漂移。加速度计测量姿态没有累积误差，但动态响应较差。它们在频域上具有互补性，可利用互补滤波原理对陀螺仪测得的姿态角进行高通滤波，对加速度计测得的姿态角进行低通滤波，得到在高频段和低频段都较好的信号，提高测量精度和系统的动态性能[7]。

2.2 改进型互补滤波

虽然互补滤波能同时消除低频和高频的干扰，实现姿态数据的融合，但是由于互补滤波的低通阻带衰减较慢，且噪声较大时误差大，滤波效果不理想[8]。因此，本文在此基础上，增加PI调节器，并以陀螺仪输出的角速度数据为依据，计算适当的阈值，实时调整补偿系数kp，构成改进型互补滤波器，提高姿态数据融合精度，从而解算出准确的航向角。

以航向角φ为例，首先，将通过磁力计和加速度计解算的航向角φdc和上一时刻的最终航向角φ作差

φerr=φdc-φ

(1)

然后，将φerr作为PI调节器的输入，进行修正误差

(2)

补偿系数kp可以通过式(3)得到，其中，ωmax为手机陀螺仪的最大量程，ωres为手机陀螺仪的分辨率，ki取0.000 6。

(3)

最终，融合后航向角的计算公式为

(4)

将其与下一时刻加速度和磁力计解算的航向角作差值运算，以形成负反馈。改进型互补滤波算法如图1所示。

3 航向角推算流程

3.1 九轴数据融合解算流程

融合九轴数据的姿态解算算法是在数据层进行融合的，其将加速度计数据向量叉乘误差与磁力计数据向量叉乘误差相加，作为PI调节的输入，得到补偿值来修正陀螺仪的角速度输出；然后通过解算四元数微分方程，求得更新后的四元数，进而根据姿态矩阵的四元数表达式解算实时姿态角，将解算的航向角作为行人航向角进行PDR轨迹跟踪。

由于该算法把加速度计、陀螺仪和磁力计原始数据融合在一起进行四元数解算，当磁力计突然受到强磁干扰时，其自身的误差会扩散到横滚角和俯仰角，从而影响整个姿态角的解算精度，使得解算出的航向角出现较大的误差。

3.2 基于改进互补滤波算法解算流程

将加速度计和磁力计数据解算的航向角和陀螺仪输出的角速度数据进行互补滤波融合，得到融合后的航向角，又将其与手机电子罗盘解算的航向角作差值运算，作为PI调节的输入，形成负反馈。由于本文算法是将电子罗盘解算的航向角和陀螺仪积分后航向角在外部融合，即使有强磁干扰，也不会造成全方位的失控。将融合的φ作为行人的航向角，对行人运动过程进行轨迹跟踪。本文算法流程如图2所示。

4 试验验证和结果分析

4.1 数据采集平台

本文采用mi6智能手机作为行人导航数据采集与存储模块进行测试试验。在mi6智能手机上安装AndroSensor，用来采集行人运动过程中的三轴加速度、三轴陀螺仪及三轴磁力计数据。为了达到较高的测量精度，在采集数据之前要对手机的内置传感器进行校准。加速度计和陀螺仪要进行零偏校准[18]，磁力计要进行椭球校正。

4.2 测试试验

为了验证本文算法对陀螺仪累计误差有较好的修正效果，在室外环境下，设计了多种行进路线衔接的独特路线，并以实际行人行走路线为参考，进行了有、无PI调节互补滤波融合的对比试验；考虑外界磁场环境对试验的影响，又在外界磁干扰强、弱环境下，分别与九轴数据融合算法进行对比，验证了本文算法有较强的抗磁干扰性能。

4.2.1 室外测试试验

本文选择学校的足球场作为室外试验场地，是由于足球场较为空旷，没有高大建筑物，可视为外界磁场干扰较小的环境。在足球场的罚球区设计如图3所示的闭合路线(路线设计包含直线行走，弧线行走，直角转弯及任意角度转弯，涵盖了行人正常行走的各种情况)，测试人员手持智能手机进行校准后，围绕操场行走一个如图3所示的设计路线，然后通过设计的航向解算算法对手机传感器数据进行处理，得到其在室外环境下的行人行走轨迹。

室外测试试验数据统计结果见表1。

表1 室外试验精度分析

注：DTO为路线闭合差与其总路线的比值[11]；AHE为平均航向误差。

由图3和表1分析可知：

(1) 本文算法适用于多种行进路线(直线行走，弧线行走，直角转弯及任意角度转弯)(如图3所示)。

(2) 由于室外环境下外界磁干扰较弱，加入PI调节，行人轨迹闭合差与无PI算法相比，相差不大；而AHE则大幅度减小，减小了1.9°，验证了本文算法能够对行人航向角进行较好的修正(详见表1)。

(3) 本文算法与九轴数据融合算法在室外环境下，行人轨迹闭合差分别为1.2和0.8 m，AHE分别为-1.2°和-1.7°(见表1)，相差不大，验证了其在室外弱磁干扰环境中的适用性。

4.2.2 室内测试试验

本文选择学校J6的室内矩形过道作为室内试验场地，路线长62 m，宽42 m。由于室内环境下，现代建筑物的钢筋混凝土结构及办公室门把手等都会对磁力计数据产生干扰，视为外界磁场干扰较大的环境。测试人员手持智能手机，进行校准以后，围绕J6矩形过道进行测试试验，然后通过设计的航向解算算法对手机传感器数据进行处理，得到其在室内环境下行人行走的轨迹图，如图4所示。

室内测试试验数据统计结果见表2。

表2 室内试验精度分析

由图4和表2分析可知：

(1) 在室内环境下，有PI调节算法，其DTO误差为3.0%，与无PI调节算法相比定位精度提升了68.4%，与九轴融合算法相比定位精度提升了65.9%。

(2) 由于室内环境下外界磁干扰增强，九轴数据融合算法解算的行人轨迹出现明显的偏差，本文算法解算行人轨迹则与参考路线较为吻合(如图4(b)所示)，其AHE减小了1.8°(见表2)，验证了本文算法有较强的抗磁干扰性能，即使在磁干扰的环境下也有较强的适用性。

5 结 语

基于低成本的智能手机，针对传统行人航向解算算法中存在陀螺仪累积误差的问题，本文提出了一种改进的航向角融合算法。该算法根据智能手机传感器自身的特点，对互补滤波算法进行了一定的改进，并根据陀螺仪输出角速度数据，实时调节误差补偿系数进行PI调节，将调节后的手机电子罗盘解算的航向角与陀螺仪数据进行融合，得到融合后更可靠的航向角。试验证明，该算法对于磁干扰较强的环境也有很好的适用性，能较好地过滤掉加速度计的噪声干扰、收敛陀螺仪的累积积分误差，满足使用者的需求，对于低成本的室内行人导航有一定的应用价值。