基于智能手机MARG 传感器的行人导航算法

田增山，张 媛

(重庆邮电大学移动通信重点实验室，重庆 400065)

0 引言

目前比较完善的导航技术如全球定位系统(global positioning system，GPS)，由于微波易被建筑物、金属遮盖物等吸收，在室内场合则无法使用，基于多微机电(micro－electro－mechanic system，MEMS)惯性传感器的导航系统便能很好地解决这一问题。惯性系统是一种自主导航系统，能提供位置、航向和姿态角数据，所产生的导航信息连续性好而且数据更新率高、短期精度和稳定性好［1］。近年来，随着基于MEMS技术的磁力计，陀螺仪和加速度计(magnetic，angular rate and gravity，MARG)传感器在智能手机上的广泛应用，行人惯性导航应用成为国内外研究的热点。特别是在无法接收到GPS信号的环境下，对于手持式移动设备中的行人导航需求在过去的几年里已大大增加。

在行人惯性导航系统中，最常用的方法就是行人航迹推算［2］(pedestrian dead reckoning，PDR)算法，该算法避免了对加速度观测值积分，而基于行人步行的运动生理学特性，利用行走时加速度波形的周期性特征和统计值与行走速度相关的特点，直接估计行走步长，航向则主要是将陀螺仪通过积分得到航向变化值或通过磁罗盘直接得到［3］。该算法的技术难点在于如何在复杂且没有任何外界参考的环境下获得一个很好的航向。

利用陀螺角速率对姿态角度更新的算法主要有欧拉角法和四元数法。欧拉角算法通过求解欧拉角微分方程直接计算航向角、俯仰角和横滚角。由于欧拉角固有的缺陷，总是存在一对奇异点，当俯仰角接近90°时，方程会出现退化现象，使得航向角和横滚角无法唯一确定。四元数法避免了欧拉角的奇异问题，只需求解4个未知量的线性微分方程组，算法简单，易于操作，在实际应用中非常广泛。由于陀螺仪测量角度存在累计误差，使得无法使用陀螺仪对角度进行长时间测量，必须与加速度计和磁力计组合使用才能获得稳定可靠的姿态角，这种数据融合的方法主要有互补滤波、卡尔曼滤波和梯度下降算法等。文献［4］采用了基于四元数的梯度下降算法，将测得的绝对角度与角速度进行数据融合，实现了三自由度的姿态测量，该算法简单且易于实现，但是测量精度不高。文献［5］采用了基于四元数的扩展卡尔曼滤波(extended Kalman filter，EKF)算法，但系统状态量较多，计算量大，不利于实时处理，不易在手机平台上实现。另外，还存在的主要问题是，当载体产生大的线性加速度，或者磁力计受到周围环境(如铁制品)等产生的磁场干扰时，会导致加速度计和磁力计输出的测量值解算的姿态角误差增大，从而降低了系统测量精度。

为了能在智能手机上实现行人惯性导航应用，本文提出一种易于实现且精度较高的行人导航算法，针对姿态测量精度问题，设计了一种EKF，利用自适应的方法来调整测量噪声的协方差矩阵，从而提高系统精度。

1 PDR算法

本文采用的PDR算法是一种基于已知用户的初始位置的相对定位方法。智能手机实验平台通常包含多个MEMS传感器，其中，最为主要的是三轴加速度计、三轴磁力计和三轴陀螺仪。本文主要利用加速度计数据，探测用户行走时的步态和总步数，并作为位置更新点;同时，利用经验模型，估计行人的步长，并作为点到点的位移大小。此外，将MARG传感器数据通过EKF进行融合，以获得航向信息，从而最终推算行人的位置信息，系统的总体架构如图1所示。

图1 行人导航系统架构Fig.1 Pedestrian navigation system architecture

2 步态检测和步长估计

根据行人步行的运动生理学特性，行走时加速度计三轴模值输出波形成周期性变化，因此，可利用该周期性特征来检测步态。三轴加速度模值为

(1)式中:Acc_norm为加速度模值;ax，ay和az分别为加速度计三轴输出的数据。

利用数字低通滤波器对加速度模值进行滤波，得到较好的单峰值曲线图，如图2所示。可以准确地检测到峰值点并计算出步数。由于行人将手机拿在手中时会发生一些小的抖动，这时加速度计模值输出波形也会出现峰值，因此，需要设定相应的阈值来消除因抖动而造成的计步误差。步态检测算法采用阈值法，设置的阈值条件为

(2)式中:ΔT为2个相邻峰值之间的时间间隔;g为当地重力加速度;TTh和ATh分别是时间阈值和峰值阈值。

本文根据三轴加速度模值的行走特征，估算行人的步长。步长估计采用经验模型，其公式为

(3)式中:step_length为行人的步长;Amax和Amin分别为每一步加速度模值的最大值和最小值;C为校准系数，由参考轨迹距离的真实值与估计值的比率求得。

(4)式中，dreal和destimated分别为参考轨迹距离的真实值和估计值。该方法估计不同行人的步长，可以通过调整校准系数C得到较为准确的步长估计。由于该步长估计算法仅涉及一个参数，于是实现过程较为简单。

图2 加速度计模值输出Fig.2 Modulus values output of accelerometer

3 航向估算

3.1 陀螺四元数法姿态更新

三轴陀螺仪可得到手机载体坐标系下x，y，z轴角速度，从已知初始姿态开始，通过对基于四元数刚体运动学微分方程进行积分，可以得到手机当前姿态角。该姿态四元数微分方程为

(6)式中:ωi(i=1，2，3)为相应姿态角速度，即载体坐标系x，y，z轴上分量;Ω(ω)为一个4×4的反对称矩阵。

为了便于计算，通常假设在采样时间间隔Ts内角速度为恒值，对微分方程组(6)进行求解，可得到姿态四元数离散时间计算公式:

3.2 扩展卡尔曼滤波器设计

尽管陀螺仪与组合磁力计/加速度计都可得到航向角，但这2种方法解算得到的航向角，会分别因为陀螺仪累积误差和磁力计受外界干扰造成测量精度降低［7］。因此，将陀螺仪与组合磁力计/加速度计融合求解航向角，可以有效地提高航向角的解算精度。本文采用EKF将传感器数据进行融合。

根据陀螺四元数法更新姿态的离散时间模型，将旋转四元数作为状态矢量，可以得到卡尔曼滤波器的状态方程为

(13)式中:by和bz分别为磁场强度在水平方向和垂直方向上归一化后的分量。

系统中的观测方程是非线性的，通过(14)式求得雅可比矩阵F来进行线性化处理。

在静止且无磁干扰状态下，加速度计和磁力计的量测噪声基本不变，但是当载体存在线性加速度时或者周围环境中存在随机磁源干扰时，会导致加速度计和磁力计实际的量测噪声方差与理论值不符。针对这个问题，本文采用以下自适应的方法构造协方差和来修正测量噪声协方差阵R的加权系数:

(15)—(16)式中:‖ak‖和‖mk‖分别为加速度计和磁力计数据模值;ki1和ki2为设定的权重因子;var(ik－N/2:ik+N/2)为滑动窗口大小为N的测量值模值方差;i为a或m，N的大小由实验得到。

4 实验验证及结果分析

本文采用华为智能手机集成的MARG传感器作为惯性测量单元，惯性测量单元包含三轴加速度计传感器(ST LIS3DH)、三轴磁力计传感器(akm8963)和三轴陀螺仪(ST L3G4200D)。华为智能手机采用安卓操作系统，从系统提供的应用程序接口(application program interface，API)可直接获取MARG传感器的原始数据(采样速率为50 Hz)，利用本文设计的行人导航算法对这些原始数据进行处理，解算得到行人的行走轨迹。在实际使用中，由于固定偏差和比例误差的存在，严重影响了数据融合的精度，因此，在实际应用之前.必须对传感器数据进行滤波、误差校准等预处理［8］。

4.1 无磁干扰环境下测试

本文选用的无磁干扰环境测试地点为标准足球场的外围跑道，如图3所示，总长度400 m，行人手持智能手机围绕操场行走一圈，通过本文设计的行人导航算法对MARG传感器数据进行处理得到其行走的轨迹图，如图4所示。误差为1.8 m，满足了误差小于2%的需求。

4.2 有磁干扰环境下测试

本文选用的有磁干扰环境为地铁站，长70 m，宽50 m的矩形场地，行人手持智能手机围绕该场地行走一圈。通过磁力计三轴模值数据(见图5)可以看出，该环境下存在着很强的磁干扰。

图3 行走参考轨迹图Fig.3Walking reference trajectory of pedestrians

图4 仿真轨迹复原图Fig.4 Walking simulation trajectory of pedestrians

图5 磁力计模值Fig.5 Modulus values ofmagnetometer

图6是航向估计对比图，由于陀螺仪存在漂移，求得的航向角存在累计误差，但在短时间内精度较高;磁力计由于受到周围环境中如铁制品等产生的磁场干扰，解算得到的航向角出现上下波动的情况;本文提出的EKF算法减少了磁干扰的影响，从而提高了系统整体定位精度。

图7为本文设计的行人导航算法得到的行人轨迹，通过EKF算法得到的轨迹图更接近实际轨迹，误差为3.5 m，也满足了误差小于2%的需求。

5 结束语

图6 航向估计对比图Fig.6 Comparison of heading angles

图7 磁干扰下轨迹对比Fig.7 Comparison of trajectories undermagnetic interference

本文提出了一种基于智能手机MARG传感器的行人导航算法，在智能手机平台上进行测试验证，实验结果表明，在无磁或有磁干扰环境下，本文提出的行人导航算法均可保证准确、可靠、持续的位置信息。该算法除了在华为智能手机上实现，同样适用于其他智能手机，为Wi－Fi/MEMS组合导航提供了重要的理论基础。未来的工作将在以下2个方面进行展开:①最佳的航向估计仍然是研究的重点，它决定着系统的整体精度;② 通过对步长算法的优化，算法将自动适用于不同的行人。

［1］SOEHREN W，HAWKINSON W.Prototype personal navigation system［J］.Aerospace and Electronic Systems Magazine，IEEE，2008，23(4):10－18.

［2］陈伟.基于 GPS和自包含传感器的行人室内外无缝定位算法研究［D］.合肥:中国科技大学，2010.

CHEN Wei.Research on GPS/Self－Contained Sensors Based Seam less Outdoor/Indoor Pedestrian Positioning Algorithm［D］.Hefei:University of Science and Technology of China，2010.

［3］孙作雷，茅旭初，田蔚风，等.基于动作识别和步幅估计的步行者航位推算［J］.上海交通大学学报，2008，42(12):2002－2005，2009.

SUN Zuolei，MAOXuchu，TIANWeifeng，etal.Pedestrian Dead Reckoning Based on Activity Recognition and Stride Assessement［J］.Journal of Shanghai jiaotong university，2008，42(12):2002－2005，2009.

［4］MADGWICK S O H，HARRISON A J L，VAIDYANATHAN R.Estimation of IMU and MARG orientation using a gradient descent algorithm［C］//IEEE International Conference on Rehabilitation Robotics，Zurich:IEEE Press，2011:1－7.

［5］SABATINIA M.Quaternion－based extended Kalman filter for determining orientation by inertial and magnetic sensing［J］.Biomedical Engineering，IEEE Transactions on，2006，53(7):1346－1356.

［6］罗磊，田增山，陈俊亚.EKF定位跟踪算法研究［J］.重庆邮电大学学报:自然科学版，2009，21(1):50－52.

LUO lei，TIAN Zengshan，CHEN Junya.Algorithm of EKF positioning and tracking［J］.Journal of Chongqing University of Posts and Telecommunications:Natural Science Edition，2009，21(1):50－52.

［7］MUNGUIA R，GRAU A.An attitude and heading reference system(AHRS)based in a dual filter［C］//IEEE 16th Conference on Emerging Technologies＆Factory Automation.Toulouse:IEEE Press，2011:1－8.

［8］REN H，KAZANZIDESP.Investigation of attitude tracking using an integrated inertial and magnetic navigation system for hand－held surgical instruments［J］.Mechatronics，IEEE/ASME Transactions on，2012，17(2):210－217.

(编辑:刘 勇)