融合地图信息与WiFi地标的室内粒子滤波定位算法

张 斌李志远魏 玮赵建立

(1.山东科技大学 计算机科学与工程学院，山东 青岛 266590；2.青岛海尔(胶州)空调器有限公司，山东 青岛 266300；3.青岛海尔工业智能研究院有限公司，山东 青岛 266000)

随着数字信息时代的到来，基于位置的服务(location based service，LBS)变得日益重要，在火灾救援、医疗服务、停车定位等方面都拥有广阔的应用前景。全球定位系统(global positioning systems,GPS)基本可以满足行人在室外的定位，但是基于卫星进行导航定位的GPS系统难以覆盖复杂的室内区域，因此室内定位成为了新的研究热点。目前典型的室内定位技术主要包括基于无线网络的指纹定位[1-2]，基于惯性导航系统(inertial navigation system,INS)的行人航位推算(pedestrain dead rechoning, PDR)算法[3]、三角三边定位[4]，基于红外的定位[5]以及基于射频(radio frequency,RF)[6]的定位等。

目前，智能手机传感器功能的完善与普及使得利用INS解决室内定位问题成为可能。利用INS定位的PDR系统由于传感器数据的不准确，会导致最终定位误差的累积。因此，如何抑制传感器数据误差累积是目前急需解决的问题。Li等[7]提出一种自适应噪声的扩展卡尔曼滤波算法，将WiFi定位结果与PDR结果进行融合以提高定位精度；Shang等[8]提出一种基于距离约束的聚类算法，通过识别环境中存在的Landmark和回溯粒子对行人位置进行校正；Qian等[9]则提出一种基于网格矢量图辅助粒子加权的粒子滤波算法来修正步长和航向估计的偏差；Widyawan等[10]将一种新型的回溯粒子滤波器(backtracking particle filter,BPF)与不同层次的建筑平面图细节相结合，以改善PDR性能；Yu等[11]利用无迹卡尔曼滤波算法(unscented Kalman filter,UKF)，将WiFi定位与行人航迹推算相结合，获得了更高的定位精度;Lee 等[12]提出了一种虚拟链路(virtual link,VL)算法构建地图数据库，利用扩展卡尔曼滤波器(extended Kalman filter，EKF)估计行人位置和惯性测量单元(IMU)传感器的误差。Xi等[13]则提出利用WiFi信号峰值检测地标以更新PDR定位坐标从而纠正行人位置的Lap算法(landmark-aided PDR,Lap)。以上方案仍然存在设施部署成本昂贵、过于依赖WiFi定位结果和大量无线访问节点(access point,AP)、计算复杂度过高等问题，在实际的定位过程中应当考虑到缺少现有指纹库或者AP的情况。

针对以上问题，本研究提出融合地图信息与WiFi地标的粒子滤波定位算法WL+PF(WiFi landmark+particle filter)算法，该算法无需专人进行细粒度的指纹采集，利用地图信息作为先验知识，将无效粒子权重赋值为0，并通过房间内WiFi信息峰值检测[13]地标信息替代PDR的定位坐标，实时消除PDR累积误差。同时，利用地图中所表达出的拐点信息、WiFi峰值检测出的地标信息更新粒子权重，输出粒子位置，增强粒子使用效率，以提高最终的定位精度。相比较其他常规滤波，本研究提出的算法没有直接将WiFi定位结果与PDR定位结果相融合，因此在一定程度上规避了因WiFi信号受干扰而导致的定位跳跃性带来的融合结果误差；在PDR定位结果加入有限的地标信息的基础之上，粒子滤波可以通过大量的随机样本获得最优的状态结果，同时避免由于有限地标的加入而出现PDR定位结果跳跃的情况，进一步提升了最终的定位精度。实验结果证明，基于地图信息的粒子滤波与使用WiFi Landmark相结合的算法，不需要耗费额外人力资源，也可以定期纠正传感器信息漂移，有效改善PDR算法的误差累积问题。

1 行人航迹推算

(1)

1.1 步行检测

行人在行走的过程中，加速度信息主要由水平、垂直、侧向三个分量组成，其分别对应手机坐标系的y、z、x轴。检测步行的常用方法是波峰检测算法，由于垂直方向的加速度信息会有周期性的变化，利用加速度的差异，检测一步之内满足阈值限制的峰值和谷值，从而获得行人的步数。

1.2 步长估计

步长估计方法一般分为两种：一种是根据行人的身高体重等状态设置一个常数值作为行人的步长，另一种是根据行走特征建立自适应步长计算公式。后者虽然比较复杂，但相对于前者，精度会有一定的提升。本研究采用文献[15]的算法，利用线性回归模型来获得步长计算公式中自适应K值：

(2)

其中，vstep为每一步中三维轴vstepx、vstepy、vstepz上平均速度的大小,即：

(3)

最终获得步长

(4)

其中，Amax与Amin分别表示一步之内的最大、最小加速度值。

1.3 方向角估计

方向角的准确测量对最终的定位结果有着较大的影响。由于本研究采用电子罗盘对方向数据进行收集，用户的移动方式和环境磁场变化都会在一定程度上导致电子罗盘采集的航向数据产生噪声，因此，需对方向角加以限定，以在一定程度上抑制噪声，这对于提高最终的定位精度至关重要。本研究首先通过阈值检测行人的运动状态，当检测到行人转弯时，若下一时刻状态为直行状态，则使用数据库中存储的走廊方向信息对此时的电子罗盘采集的方向角进行匹配从而校准航向信息，并在后续的直行过程中一直使用，直到检测到下一个转弯状态再重新匹配新的走廊方向信息。

具体的，通过下式检测行人运动状态：

||(θmi)|-|(θmi-1)||>Th1∪||θmi|-|θmi+1||

(5)

其中，θm表示方向角的测量值。用下面两个条件对方向角进行限定，确定行人转弯行为：①当前时刻设为i时刻的方向角θmi与上一时刻的方向角θmi-1求差值并大于给定的阈值Th1；②i时刻的θmi与i+1时刻的方向角θmi+1的差值小于阈值Th2。

在直行状态下，可利用走廊方向对行人的行走方向进行修正，具体公式见参考文献[16]。之后继续利用式(5)检测行人运动状态，直到出现下一次直行状态，并对方向角进行修正。

2 AP有限情况下的Lap算法存在问题分析

PDR定位算法在行人行走的过程中会产生累积误差，因此需要加入地标来定期矫正行人的位置。文献[13]使用检测到的WiFi信号峰值点来确定直行状态下的地标点，将当前时刻PDR算法得到的定位点重置为真实的坐标后，继续计算后续的行人位置，但未考虑AP有限情况下算法所存在的局限性。

PDR定位结果如图1所示，由于传感器收集数据存在噪声等其他因素的影响，导致PDR算法最终定位结果严重偏移真实轨迹；图2中分别显示了加入较少AP的前提下使用地标获得的定位结果，以及在此基础上添加方向角修正的定位结果。在AP有限的情况下，由于检测到的地标较少，后续的定位结果依然存在漂移现象，且由于步长估计产生的误差等因素，会出现当前时刻更新到地标点的定位点与上一时刻利用PDR算法计算出的定位结果之间距离过长、定位点跳跃的情况。对方向角进行修正之后，行人轨迹的漂移情况虽然得到了一定的缓解，但是依旧存在定位点跳跃、拐点处轨迹不符合实际路径的情况。因此，本研究将文献[17]引入到Lap算法中，得到第3节的融合地图信息与WiFi地标的粒子滤波定位算法。

3 融合地图信息与WiFi地标的粒子滤波定位算法

单纯地在PDR算法中加入地标可以一定地提升定位精度，但是由于地标数量的限制，加入较少的地标依然无法解决后续定位过程中误差累积的问题。因此，在文献[13]的基础之上，本研究融合一种基于地图信息的PF算法，该算法基于已知的地图信息，使用WiFi峰值点识别的地标点，在对观测值修正的同时重新输出粒子位置、更新粒子权重[17]。使用此算法可以避免在AP较少的情况下所获得的地标数目不足导致的定位点跳跃等问题，进一步提升最终定位精度。图3为融合地图信息与WiFi地标的粒子滤波定位算法流程图：

图1 PDR定位结果Fig.1 PDR positioning results

图2 使用地标校准后PDR结果Fig.2 PDR results after calibration using landmarks

图3 融合地图信息与WiFi地标的粒子滤波定位算法流程图Fig.3 Flow chart of particle filter positioning algorithm combining map information and WiFi landmarks

在很多场景中都需要利用一系列的观测值对系统状态向量进行估计，常用的有卡尔曼滤波(Kalman filtering,KF)[18]、粒子滤波(particle filter,PF)[19]等算法，其中卡尔曼滤波是一种通过系统观测值估计系统最优状态量的算法，但并不适用非线性系统。PF利用蒙特卡罗方法解决贝叶斯系统状态估计问题，通过离散的随机样本来逼近实际的概率分布p(x0:k|z1:k)的积分值，通过大量的样本估计一个系统的最优状态值。PF可应用于非高斯、非线性系统，应用范围较卡尔曼滤波算法而言相对广泛。其公式为：

(6)

其中，p(x0)为该动态系统的先验概率分布，k时刻系统状态xk的后验概率分布为p(x0:k|z1:k)，N代表粒子数量，本研究设置为100。x0:k表示系统从0到k时刻的系统状态，z1:k代表系统的观测值，在应用中表示与PDR结果的距离。粒子的权重需要满足归一化条件

(7)

权重更新如下式：

(8)

(9)

下面给出改进算法的伪代码。

算法1 基于地图匹配的改进粒子滤波算法输入:地标点LandMark,拐点数据coner,粒子数目Nx,PDR定位值Sensor,地标点时间戳Landt,传感器时间戳sensort,可行走区域Map /∗其中地标点包括WiFi峰值检测地标点与拐点信息∗/输出:行人位置position1:初始化粒子 P(xi0),P(yi0)2:i∈[1,N],Nx=1003:for k=1;k

图4 实验环境平面图Fig.4 Floor plan of the indoor environment

4 仿真实验

4.1 实验设置

本研究选择的实验场地为山东科技大学计算机科学与工程学院3楼。使用MATLAB2016b进行仿真实验，实验定位终端采用三星Galaxys IV型号手机，对加速度计、电子罗盘、WiFi信号信息以及样本时间戳进行了采集。实验区域为44 m×27.6 m，在该范围内选择了4个AP作为锚点的检测点，分别为TP_WR842N、TP_linkA5210E、JWY和anran_company。具体建筑信息图和实验理想路线如图4所示。图中的圆圈表示行人的起点，三角代表行人终点。图4中的黑色轨迹是实验设置的行人轨迹。实验者从初始点(0,0)出发，将智能手机水平手持于胸前，步伐保持匀速平稳，按照路线图中标识的方向沿走廊行走一圈。

4.2 实验结果

图5显示了传统PF算法的定位结果，可以看出，应用此算法的PDR定位精度有一定的提高，但是由于粒子的轨迹没有受到任何地图信息的约束，导致依然存在行人位置穿墙的问题，且粒子输出的位置波动比较大；行人定位结果依旧存在漂移现象。

本研究提出的算法校正后的行人轨迹如图6所示，此时的粒子位置受到地图信息的约束并且重新校准到地标位置，能更准确地获得行人轨迹。且行人轨迹更符合真实轨迹，粒子输出位置的波动也更加平稳，有效改善了单纯加入地标时出现的定位点跳跃过大的问题。图7为7种定位方式与实际路径在真实地图中的对比定位结果，可以看出，相比较[13]中单独加入地标的算法，以及传统的PDR算法和PF算法，改进后的算法行人轨迹比较平稳，有效地改善了PDR算法误差累计的问题。

图5 粒子滤波定位结果Fig.5 Particle filter positioning results

图6 WL+PF定位结果Fig.6 WL+PF positioning results

图7给出了WiFi定位结果，由图可知，此时收集到的WiFi信号值波动较大，出现了WiFi定位点跳跃严重、大部分定位结果在走廊以外的情况。而文献[11]提出的UKF融合算法、文献[7]提出的ADEKF(adaptive extended kalman filter,自适应扩展卡尔曼滤波)融合算法，都是将WiFi定位值与PDR定位信息相融合以提升定位精度，但是由于WiFi定位结果跳跃过于严重，此时使用滤波算法反而会导致定位结果精度下降。图8误差累计分布图表明，本研究所提出的算法误差分布基本在2 m左右，明显优于其他几种定位算法。图9和图10分别给出了本算法与其他5种算法的定位误差分布图。在图9中可以清晰的看出，由于WiFi信号受到外界因素干扰存在较大的波动性，导致WiFi定位结果跳跃严重，因此采用EKF或者UKF算法将PDR定位结果与WiFi定位结果进行融合，反而导致定位误差增加、定位结果波动性较大；而传统的PF算法由于没有受到地图信息的约束，定位精度没有太大的提升。由图10的定位误差分布图可以看到，PDR定位轨迹伴随时间增长存在累计误差，且定位误差波动较大；本研究提出的算法在一定程度上缓解了PDR的误差累计问题。相比较其他5种定位算法，本研究提出的定位算法误差分布均匀、变化平稳，误差分布在2 m以内，有着较好的定位精度提升效果。

图7 本研究算法与6种定位算法定位结果对比Fig.7 Comparison of the results of this research algorithm with six kinds of positioning algorithms

图8 本研究算法与6种定位算法误差累积分布对比Fig.8 Comparison of the error cumulative distribution of this research algorithm with six kinds of positioning algorithms

为了更进一步分析算法的定位精度，表1对比了其中6种定位算法的平均误差与最大定位误差。与单纯的PDR算法相比，Lap、WiFi+PF、PF的定位结果精度分别提升了39%、79%、8.2%。其中PF算法由于没有受到地图信息的约束，定位精度提升比较小；基于地图信息与地标点的WiFi+PF算法相对于PF算法精度提升了78%，且此时的行人轨迹比较平稳，最大定位误差比PF定位算法降低了70%；Lap算法由于定位结果跳跃，最大定位误差比PF、WiFi+PF、PDR三种算法分别增长了9.7%、74%、18%。

图9 6种定位算法误差分布对比Fig.9 Comparison of error distribution of the six kinds of positioning algorithms

图10 PDR与WL+IMPF的定位误差分布对比Fig.10 Comparison of positioning error distribution between PDR and WL+IMPF

表1 定位误差

从表1中可以看出，基于WiFi定位结果的ADEKF与UKF融合算法，由于采集的WiFi信号值波动较大，导致最终的融合定位精度比原始的PDR算法分别下降了63%和55%，比本研究的WiFi+PF算法分别下降了77%和73%。

综上所述，本研究提出的WiFi+PF算法能够有效提升最终定位精度，在WiFi信号值波动较大的情况下优于常规的滤波融合算法，且在定位过程中有较好的稳定性。

5 结论

研究了PDR室内行人定位算法，提出一种融合室内地图信息与WiFi地标的粒子滤波算法。该算法利用地图信息以及地标点对观测值进行修正的同时，对粒子的位置和权重进行了更新，最后输出行人的最终位置。实验结果表明，本研究提出的室内行人定位算法能有效地抑制传感器的误差累积，增加粒子轨迹输出的稳定性，提升定位精度。