以粒子滤波为基础的多信息融合室内定位方法设计详解

0 引言

目前，国内外已有不少对室内定位的研究。FOXLIN E 等人提出基于惯性测量单元（Inertial Measurement Unit，IMU）的传统导航机制，通过对加速度进行两次积分得到行走距离，对陀螺仪积分得到航向变化值，结果得到较为精确的定位结果，然而其高精度性能的基础是价格昂贵的IMU，在行人定位中并不能普及；LEVI R W和 JUDD T 提出了行人航迹推算（Pedestrian Dead Reckoning，PDR）算法来实现行人定位，该算法利用加速度信号探测用户行走时的跨步，采用有关模型估计步长，利用从陀螺仪获得的方向信息推算出行人的速度、位置和距离等信息，其算法简单，比传统惯性导航机制的定位精度更高。然而，随着时间的增加，传感器的误差不断积累，使得到达一定距离后PDR的定位精度急剧下降；谢宏伟 等人设计了一款基于稳定性增强粒子滤波磁场匹配算法的手机室内定位系统，该系统实现了在未知初始方向和位置的情况下的定位，并达到1～2 m的定位精度。但该算法实现复杂，计算速度慢。

基于上述原因，本文提出一种基于粒子滤波的多信息融合室内定位方法，通过融合PDR和地磁信息，实时动态补偿PDR的定位误差，实现高精度、低成本、低复杂度的PDR/地磁融合室内定位。

1 融合定位算法

PDR算法利用加速度计、陀螺仪和磁力计等惯性器件得到行人行走时的原始数据，原始数据经扩展卡尔曼滤波融合得到行人的步频、步长以及方向信息。由于惯性器件误差的积累导致定位误差不断增大，所以本文选用地磁来辅助PDR实现室内定位，原理框图如图1所示。

地磁定位包含3个方面：一是导航区域地磁数据库的建立；二是载体上磁力计的实时测量；三是地磁匹配。首先把载体经过区域中某些点的地磁场特征量绘制成基准图保存下来，当载体穿越这些区域时，将地磁的相似程度作为基准进行匹配，本文选取粒子滤波算法来完成相似度的匹配。

2 PDR定位原理

PDR技术基本思想是通过载体携带的加速度传感器及方向传感器获得单位时间内载体的位移及方向，从而可以根据上一时刻载体的位置和方向推算出载体在当前时刻的位置和方向。若已知初始位置坐标为（x0，y0），则第k步的位置坐标为：

2.1 步态检测

跨步探测方法有零点探测、峰值探测、自相关、步相探测、零速修正等，具体采取哪种方法与加速度安装位置有关 。本文以正常持手机方式采集数据，设计一种基于加速度信号的滑动窗口+峰值探测的步频探测算法。

步频探测算法的结果如图2所示，行走过程中的步态基本都被标注出来，步态判别准确率接近100%。

2.2 步长估计

PDR算法中步长估计方法主要有两种：一是基于惯性导航原理对加速度二次积分；二是利用人员行进特性对其特定动作下的行进距离进行建模 。基于步长模型的方法大多使用低成本惯性器件实现步长估计。

本文PDR算法用建模的方法来确定行人的步长。假设在行走过程中加速度的变化与步长成比例，在一个周期内，第k步的步长lk可以通过下式得到：

其中，app表示加速度峰值与谷值的差，β是比例因子，定位之前将得到的训练数据经最小二乘法拟合得到。

为了检测步长估计算法的性能，进行8次实验来验证该算法。每次测试者行走的总长度是20 m，表1是8组数据的解算距离及误差，表明该步长估计算法的精度在短距离内可以达到1 m。

2.3 方向估计

对于行人行走时方向角测量，普遍采用低成本陀螺仪、加速度计和磁力计等实现。陀螺仪的零点存在温度漂移，加速度计会受到载体震动的影响，磁力计会被外部磁场干扰。因此，为了得到可靠、准确的方向角，必须利用滤波器融合不同传感器的数据、滤除不必要的干扰 。本文选取扩展卡尔曼滤波器实现融合算法。

图3是由扩展卡尔曼滤波器得到的行人方向角，由于误差的存在，由扩展卡尔曼滤波得到的方向角与参考方向还是存在一定的偏差，最大误差接近30°。

3 地磁定位

地磁定位的关键是地磁匹配算法，地磁匹配是指利用地磁图进行导航定位的过程。首先把载体经过区域中某些点的地磁场特征量绘制成地磁匹配参考图或称基准图保存在载体计算机中，当载体穿越这些区域时，将地磁的相似程度作为基准进行匹配，认为最佳匹配点就是行人的位置 。

3.1 地磁基准数据库的建立

克里金插值法是一种最优、线性和无偏的估计方法，是应用最广泛的空间内插方法之一，能最大程度地利用所给的信息，具有很高的插值精度 。图4是经克里金插值后的室内地磁基准图。

3.2 粒子滤波

粒子滤波是一种使用有限数量粒子来表示状态的后验分布的非参数化滤波算法。在粒子滤波算法中，状态的后验分布采样点被称为粒子，每个粒子都表示对当前状态的假设，通过粒子的权重来衡量该粒子所表示状态的可能性大小，如果一个区域内的粒子非常密集，表示实际状态在该区域的概率非常大 。

粒子滤波主要由状态方程、观测方程以及重采样等部分组成。本文的状态方程如式（3）所示：

式中，n表示观测值z的维度，本文用磁场向量的模值作为观测值，所以n=1；V是协方差；obss表示由地磁匹配得到的当前位置对应的地磁值；z表示当前位置地磁的观测值。

PDR算法可以计算行人的步长，然而，由于惯性器件的累积误差使得步长的误差较大。本文通过设定步长初始值并由粒子权重不断修正的方式来得到动态步长。正常人行走的步长范围为0.5 m~0.7 m，假设步长的初始值l0=0.6 m，则步长计算如式（5）所示：

3.3 重采样

在粒子滤波中存在粒子退化的问题，粒子权值的方差随着时间增大，状态空间中的有效粒子数较少，随着无效采样粒子数目的增加，使得大量的计算浪费在对估计后验滤波概率分布几乎不起作用的粒子上，使得估计性能下降，粒子群逐渐发散 。

重采样方法可以有效解决粒子退化问题 。当有效粒子数小于某一阈值时，便对粒子进行重采样。重采样的思想就是舍弃权重小的粒子，将权重大的粒子复制几次得到新粒子，用这些新粒子代替舍弃掉的粒子，至于每个粒子复制几次完全由粒子的权重决定。

4 实验结果及分析

为了验证本文提出的基于粒子滤波的多信息融合室内定位算法的可行性，在室内环境进行了实验。实验场地为大学图书馆四楼413实验室。为了提高磁场定位的精度，采用克里金插值算法对采集到的地磁数据进行0.5 m间隔的空间插值。设定初始位置已知，实验者手持手机在实验室沿指定路径行走一周。在行走的过程中，手机以20 Hz的频率采集并保存加速度计、陀螺仪以及磁力计的原始数据，最终由MATLAB软件仿真出结果。

本文对两种定位算法的结果进行对比：第一种是基于扩展卡尔曼滤波的纯PDR定位算法，第二种是基于粒子滤波的多信息融合室内定位算法。图5是由两种算法得到的MATLAB仿真图，由图5看出基于粒子滤波的融合室内定位算法的性能明显优于PDR算法。由于惯性器件的精度不高，随着时间的增加由扩展卡尔曼滤波器得到的方向角误差不断增大，导致由PDR得到的行人轨迹严重偏离实际轨迹。由于粒子滤波算法能够不断地修正步长和方向角，这就解决了PDR中误差不断积累的问题。所以，随着行走距离的增加，基于粒子滤波的融合室内定位算法的精度能够保持稳定。

本文比较两条仿真轨迹与参考轨迹的坐标数据，得到两种算法的误差，如表2所示。

由表2可知，本文提出的算法与PDR算法相比，平均误差减少了70.5%，均方误差减少了70.8%。实验结果表明，本文提出的基于粒子滤波的多信息融合室内定位算法可以解决定位误差累积的问题，有效地提高了定位精度。

5 结论

在室内定位技术中，针对传统PDR短距离精度高、长距离累积误差严重和地磁匹配容易受外界干扰磁场的影响等问题，本文提出一种基于粒子滤波的多信息融合室内定位算法，该算法通过动态补偿步长和方向角误差有效地抑制了定位结果的发散。实验结果表明，该算法的精度能达到1 m~2 m，验证了该算法的有效性和可行性。