基于智能手机的PDR 定位航向估计算法研究

周 刚 李善文

(江西核工业测绘院集团有限公司 江西南昌330038)

1 引言

室内的导航定位[1]是当前全球的研究热点，目前全球定位导航系统[2]如GPS、北斗[3]等发展成熟，能满足人们生产、生活的需求。但室外定位无法延伸至室内，近年来国内对室内定位技术研究不断深入，近年来出现许多室内定位技术[4]，如WiFi、蓝牙、UWB、ZigBee、惯导等。随着智能手机的普及和MEMS 技术的快速发展，惯性传感器已成为现在主流智能手机的标准配置。基于MEMS 传感器技术[5]的行人航迹推算[6]（Pedestrian Dead Reckoning，PDR）算法在智能手机端定位研究越来越广泛。相比于惯导定位，手机端的MEMS 惯性传感器性能低，无法采用传统的惯性导航原理即采用加速度积分计算距离，陀螺仪角加速度积分计算姿态角。所以在PDR 定位中是利用惯性传感器数据进行步频检测-步长估计和航向估计算法来实现位置迭代更新，航向估计是基于智能手机PDR 定位的难点，当航向估计无误差时，定位轨迹将不再发散，很好的拟合在既定路径上。一般采用陀螺仪和磁力计来估计行人的行走时的方向，但磁力计受地球以及周边磁场的影响较大，陀螺仪虽不受外部环境影响，但误差会随时间而累积。随着多源融合技术的广泛应用，目前航向估计算法都是融合MEMS 传感器的加速度计、陀螺仪、磁力计等九轴传感器数据来对航向进行估计，本文展开基于智能手机的PDR 定位方式研究，着重对航向估计算法进行实验分析与论证。

2 基于智能手机的PDR 定位

基于智能手机的PDR 定位是利用手机内置的MEMS 惯性传感器来对行人的轨迹进行推算，其主要步骤包括步频检测、步长估计和航向估计。

2.1 步频检测

目前针对PDR 定位的步频检测方法[7]主要有峰值检测算法和零速更新算法，零速更新算法针对足绑式MEMS 传感器的步频检测更为有效。由于智能手机在使用者的使用过程中会有不同的携带方式，加速度计三轴方向会实时调整，通过检测合加速度的变化，利用峰值检测算法更为精确。

a合(t)为加速度计在t 时刻三轴的总加速度，ax、ay、az分别表示加速度计在t 时刻X 轴、Y 轴、Z 轴的加速度分量。为滤除重力和其他因素干扰，采用滑动均值滤波器对数据进行平滑处理，平滑结果如图1所示，设定峰值检测的各项阈值条件对步数进行检测：

1）设定波峰阈值amax，即在一个检测周期内a合(t)≥amax才会一个有效波峰。

2）设定波峰之间的时间阈值Δ(t)，即从第二个有效波峰起，连续检测相邻波峰的时刻差是否满足时间阈值要求。

3）设定峰谷差阈值Δa，由于手机携带一般置于上半身，在数据波形上来看，一个波峰会伴随着小波峰，设置峰谷差使检测更为精准。

图1 滑动均值滤波器对合加速度值平滑结果

2.2 步长估计

人行走时的步长具有随机性，每个人由于身高、体重、年龄等不同，步长呈现多样性。在实际应用中查表法并不适用，根据人行走时加速度计与步长的非线性关系，可以得出步长估计模型：

根据一个检测周期内合加速度的最大值Amax、最小值Amin与步长参数ε 的关系，可以估算出其行走的步长L，其中步长参数ε 可以通过数据训练而实时调整。

2.3 航向估计

姿态解算方法一般分为两类，一是从频域滤波、消除噪声，不考虑信号噪声的统计特性，如PI 互补滤波[8]；另一种是使用状态空间法在时域内设计滤波器，如卡尔曼滤波[9]，梯度下降法[10]，都实现了加速度计、陀螺仪和磁力计输出信息利用的最大化，下面简要分析三种算法的实现原理。

2.3.1 PI 互补滤波法

导航坐标系下加速度计数据ab=[axayaz]T和mb=[mxmymz]T磁力计数据通过旋转矩阵到地理坐标系时分别与地理坐标系下的an=[0 0 1]T和mn=[mxmymz]T不相等，即：

将加速度计数据和磁力计数据误差分别进行向量积得到误差，再进行累加后得到总误差[exeyez]T。

对陀螺仪输出的数据[gxgygz]T进行校正，设定好PI 参数：

通过修正后的角速度与四元数对采样时间T 进行积分得到新的四元数，再反解欧拉角得到航向角。

2.3.2 梯度下降法

加速度计与磁力计融合能解算出机体实时运动下的姿态，但都存在误差，先将加速度计和磁力计的误差进行整合，构建误差函数，得到梯度下降法中的雅克比矩阵：

再进行三种传感器数据融合：

2.3.3 卡尔曼滤波法

将加速度计与磁力计共同解算的姿态角作为卡尔曼滤波器中观测值，陀螺仪解算的姿态角作为预测值，测定好三种传感器测量中误差等参数，构建卡尔曼滤波器。假设K 时刻的航向角为φk，状态方程和预测方程为：

其中状态估计方程为：

卡尔曼增益：

更新后的滤波值为：

3 实验设计

（1）为验证三种航向解算算法对航向角解算的精度，设置转台实验如下图2 所示。将智能手机放置在旋转平台上，用水准尺对平台进行水平校正，设置好旋转起点。静置一段时间后，将旋转平台逆时针匀速旋转5 圈重回到起点位置后静置一段时间。从理论上来说，由于平台置于水平，所以智能手机旋转一圈后，姿态角中俯仰角和横滚角不变，航向角有360°的变化。通过对比三种算法对姿态角解算的精度，再对各算法的优缺性进行分析。

图2 转台实验示意图

（2）为对比三种航向估计算法对PDR 定位轨迹的影响，设置PDR 定位实验，实验示意图如下图3所示。在室内合适空间采用测距仪精确测定四个坐标点，分别设置为A（1,1）、B（1,7）、C（8,7）、D（8,1）。将四点围成的图形作为真实轨迹。利用黑色胶带进行标识，胶带宽为5cm，设定起始点为A。从A 点出发沿着轨迹ABCD 进行PDR 定位再回到起点A 完成闭合。为控制单一变量，其中步频检测和步长估计分别采用本文中2.1 节、2.2 节方法。以上两个实验都采用开源的Android 手机惯性传感器数据采集软件进行数据采集。

图3 PDR 定位实验示意图

4 实验数据处理与分析

如将采集的传感器数据，分别用三种航向估计算法进行处理后，三种算法的航向角如图4 所示，三种算法的俯仰角如图5 所示，三种算法的横滚角如图6 所示。

图4 三种算法的航向角解算曲线图

图5 三种算法的俯仰角解算曲线图

图6 三种算法的横滚角解算曲线图

由图4 可知，梯度下降法和卡尔曼滤波法对航向角的解算曲线基本重合，两种算法的解算精度要高于PI 互补滤波法。由图5 可知，卡尔曼滤波法和PI 互补滤波法对俯仰角的解算曲线的误差比梯度下降法要小，由实验设置环境可知，表明卡尔曼滤波法和PI 互补滤波法的抗差能力更强。且三种算法的误差不超过0.04rad，说明三种算法的解算精度高。由图6 可知，卡尔曼滤波法对横滚角的解算曲线误差比PI 互补滤波法和梯度下降法要小，误差约为0.02rad。由实验设置环境可知，表明卡尔曼滤波法比PI 互补滤波法、梯度下降法的抗差能力更强。再在相同条件下，比对三种航向估计算法对PDR 定位轨迹的影响，将定位实验采集的数据处理得到图a、b、c、d四个定位轨迹图：

图7 定位轨迹图

由图a 知，蓝色框线表示参考的真实路径，橙色曲线表示PI 互补滤波法的定位轨迹图，绿色曲线表示梯度下降法的定位轨迹图，红色曲线表示卡尔曼滤波法的定位轨迹图。由对比可知，三种算法在定位初期定位航向误差小，与参考路线的拟合程度高。随着定位时间的累积，航向估计误差逐渐累积，在第一个转向点后，三种算法的PDR 定位轨迹逐渐发散，但卡尔曼滤波法的发散程度最小，与第二段参考曲线的拟合程度最高。表明基于卡尔曼滤波的航向估计算法精度高，且抗差能力强。图b、图c、图d 分别表示三种算法的PDR 定位轨迹，说明了PDR 定位中航向估计误差对定位轨迹的影响大，纯PDR 定位的误差在短时间内小，长时间定位误差会逐渐加大，必须对PDR 定位误差进行抑制和校正。

5 结束语

本文通过对基于智能手机的PDR 定位进行研究，介绍了步频检测和步长估计算法原理和模型，着重介绍了目前九轴传感器数据融合的航向估计算法，并设置对比实验对三种算法对姿态角解算精度进行比较，通过数据处理并进行分析，结果表明基于卡尔曼滤波法对欧拉角中航向角解算精度高，对俯仰角和横滚角的解算误差小，抗差能力强。为验证三种航向估计算法对PDR 定位轨迹的影响，设置了三种算法的PDR 定位实验，对数据进行处理后分析，结果表明了基于卡尔曼滤波的航向估计算法的PDR定位轨迹对参考轨迹的拟合更好，误差小，抗差能力强。通过本文可知，航向估计对PDR 定位的影响最大，在短距离内累积误差较小，定位精度较高；长时间、长距离定位时，PDR 定位精度急剧下降，必须对PDR 的航向误差进行校正，为后续航向校正算法与多定位方式融合算法研究打下坚实基础。