基于粒子滤波和地图匹配的融合室内定位

周 瑞，鲁 翔，卢 帅，李志强，桑 楠

(电子科技大学信息与软件工程学院 成都 610054)

室内位置信息在许多应用[1-3]中扮演着重要的角色。虽然卫星定位系统等技术已经可以满足大多数户外定位的需求，但不适用于室内环境。由于WiFi设备成本低廉、部署广泛、实用性强，基于WiFi的定位方案已在科研、工业以及日常生活等领域得到认可。WiFi信号指纹匹配是无线定位的常用算法。其原理为相近位置点采集到的WiFi指纹(室内环境中无线接收器的信号强度)也相似，可以通过训练数学模型建立位置和指纹的对应关系。但室内环境的复杂性和干扰的不确定性会导致WiFi指纹的定位结果并不理想。如今手机等手持移动设备基本配备了标准惯性传感器，而该传感器为推算出行走航位(PDR)提供了可能[4-6]。PDR从设备中的传感器获取数据，完全脱离外部信息，短时间内可以达到较好的定位效果。但是，大多数便携式移动设备内置的惯性传感器工艺有限，且精度不高。同时人在移动的过程中随机性太大，导致PDR的误差会随时间累积，定位结果越来越差，最终失效。

研究人员提出将这两种方法融合以获得更高的精度和稳定性。文献[7]采用极大似然算法融合WiFi指纹和PDR结果，在获取到初始位置和移动方向后，不断使用WiFi定位的位置矫正PDR轨迹，从而减少长时间产生的累计误差。文献[8]提出根据惯性传感器数据和WiFi信号模式的变化确定室内的常见标志，如电梯、楼梯、转弯等，然后使用这些室内标志进一步矫正基于WiFi和PDR的卡尔曼滤波融合定位后的结果。文献[9]采用序贯蒙特卡罗卡尔曼滤波来融合WiFi定位和PDR，WiFi定位采用最速下降随机起始算法，方向估计采用粒子滤波算法，位置和速度估计则采用卡尔曼滤波算法。融合之后定位精度得到显著提高，但仍会出现轨迹穿墙等不合理现象。文献[10]采用扩展卡尔曼滤波融合WiFi定位和PDR，使用粒子滤波集成室内地图信息。文献[11]根据人们行走通常走准直线的现象，利用走廊和墙修正限制人的行走轨迹。文献[12]通过墙的位置将室内环境抽象为多个矩形，设定某段时间内粒子分布在某个矩形而不是整个室内环境。文献[13]提出多楼层场景中的地图匹配，将房间简单抽象为一定大小的矩形，而楼梯则抽象为带角度的斜矩形。这些矩形只部分连通，其边界可以约束人的行动轨迹，并用粒子滤波去模拟。文献[14]使用粒子滤波算法估计出目标的粗略位置，结合误差分析、马尔科夫模型、迭代及重采样等过程，地图信息用来进行粒子穿墙检测。文献[15]利用互补扩展卡尔曼滤波融合PDR和WiFi，使用地图进行粒子穿墙或穿越障碍物检测。

本文在前面研究的基础上，采用粒子滤波融合WiFi和PDR的定位结果，并结合地图信息进一步提升定位效果。实验结果表明：经过粒子滤波融合后，定位系统的精度和稳定性都显著提高；地图匹配后，定位精度和合理性得到进一步提高。主要贡献包括：采用SVM分类(support vector classifier, SVC)和SVM回归(support vector regression, SVR)相结合的两级WiFi定位；改进PDR中的计步和步长算法；采用粒子滤波融合WiFi定位和PDR；在粒子滤波中通过室内地图进行粒子穿墙检测；对粒子滤波融合后的轨迹再次进行穿墙矫正。

1 系统结构

图1 粒子滤波融合WiFi定位、PDR和地图匹配

本文采用粒子滤波算法融合WiFi定位和PDR，将基于支持向量机(support vector machines, SVM)的WiFi指纹定位获得的用户位置作为观测数据，通过PDR对用户的步数、步长和方向的估计用于对用户的行为进行建模。该融合系统的结构如图1所示。在粒子滤波中融合室内地图进行粒子穿墙检测，并对融合后的定位结果再次进行穿墙检测，最后得到粒子滤波融合的定位结果。

2 粒子滤波

统计信号处理中的滤波问题广泛存在于导航、跟踪、自控和信息融合等领域，其基本目标是运用概率方法从充满噪声的观测数据中估计动态系统的状态。动态系统的状态方程和观测方程可以表示为：

式中，f(⋅)是状态模型；h(⋅)是观测模型；xk代表系统状态；zk代表观测值；vk和wk分别代表系统噪声和观测噪声。滤波的任务就是递推地在每次获得观测数据zk后，估算出系统状态xk的概率分布p(xk|z1：k)。

粒子滤波[16]通过寻找一组状态空间中的样本(称为粒子)，对概率密度函数进行近似，获得状态的最小方差估计。假定k-1时刻系统状态的概率密度为，依据一定原则选取n个粒子为粒子状态，为粒子权重，在k时刻获得观测数据并经过时间预测和观测更新后，n个粒子的概率密度可近似为系统状态的概率密度，即：

式中，δ是Kronecker delta函数。粒子滤波不要求随机变量满足高斯分布，已在许多领域得到成功应用，包括定位问题。

3 基于粒子滤波的WiFi-PDR定位

WiFi定位会出现连续定位结果跳跃的现象，因此必须考虑定位的时序相关性。采用粒子滤波融合WiFi定位和PDR能有效解决时序性问题，进而减少定位误差并提高稳定性。在融合定位过程中，观测数据是基于SVM的WiFi指纹定位结果，系统状态是估计用户位置，而PDR用于分析用户运动状态并建立行为模型。假设z1,z2,… ,zk是截止到时刻k时连续的观测数据，即基于SVM的WiFi指纹定位估计的用户位置，xk代表时刻k的系统状态即用户在时刻k的位置，粒子滤波就是要估计出时刻k在z1,z2,… ,zk的前提下xk的概率分布p(xk|z1：k)。

WiFi定位采用SVC和SVR相结合的两级定位方案。两级定位的思想是先确定目标的大概区域，然后在区域里估计出精确的坐标位置。而定位之前，需要将整个定位区域按照楼层平面结构划分为若干子区域。如一个房间或房间外的一段走廊都是一个小的子区域等，然后分别训练区域分类模型和子区域坐标回归模型。具体为在各子区域内采集信号指纹，通过SVC训练，建立区域的分类模型，从而能够根据信号指纹确定所属的子区域。接着通过SVR训练，建立位置坐标和信号指纹之间的非线性依赖关系模型，在目标子区域可以根据信号指纹获得目标点的精确位置。WiFi指纹定位获得的位置坐标(x,y)作为后续粒子滤波中的观测值，和PDR进行融合。

PDR通过加速度计和磁力计来获得用户的的行走步数、步长和方向，用于在粒子滤波中对用户的行为进行建模。由于人在行走过程中速度不断变化，加速度曲线会出现上下偏移现象。采用简单移动平均算法对加速度数据进行平滑，平滑后的数据曲线分为静止、波峰和波谷3种状态，最后根据状态转换方法识别行走周期。改进PDR算法可根据加速度曲线的实时变化动态设定状态转换参数，从而使行走周期识别更加准确。识别出行走周期后，根据行走位移可得到基础步长[4]。由于人在行走中的步长变化不大，可近似统一为固定步长，并通过卡尔曼滤波估计出更加精准的步长。方向由加速度计和磁力计确定。首先通过加速度向量和重力加速度计算出手机俯仰角和翻滚角，然后将基于手机坐标系的磁场强度向量转换成基于大地坐标系的磁场强度向量，使用大地坐标系中x和y方向的磁场强度确定行人方向。

3.1 粒子初始化

3.2 运动模型和系统状态方程

令k- 1 时刻的位置为 (xk-1,yk-1)，k- 1 到k时刻之间持续时长记为Δtk，该时间段内位移大小记为lk，运动方向与y轴之间的夹角为θ。则人在k时刻的位置坐标可表示为：

假设vx,k和vy,k为在人在Δtk时段内在x和y方向的速度。由于在正常情况下，人行走的速度变化不大，可以认为k时刻速度和k- 1时刻的速度相等，即：

考虑到实际系统中存在由状态建模带来的误差和行走过程中的速度变化，假设σc代表系统状态的噪声，σv代表行走速度的变化，根据式(4)和式(5)可以得到系统状态方程为：

3.3 粒子权重更新和随机重采样

当在k时刻获得一个新的观测值(新的WiFi指纹定位结果)，并依据该观测值对所有粒子的权重进行更新。权重更新后重新筛选粒子，具体为舍弃权重较小的粒子，只保留权重较大的粒子。新的粒子权重值分布采用高斯分布，观测值为原点，各粒子与观测值之间的欧式距离为输入，标准差为1.1，即：

式中，为粒子与观测值之间的距离：

然后将新的权重归一化：

粒子筛选采用随机重采样方式。根据更新后各粒子的权重，依次排列形成[0,1]的粒子权重区间，其中各粒子的权重值决定了其区间长度。然后生成n个[0,1]的随机数，找出其在权重区间的对应位置，将所有对应位置的粒子筛选出来形成新的粒子集。

3.4 系统状态估计

在粒子状态和粒子权重更新后，系统当前状态更新为：

式中，(xk,yk)即为行人当前的位置。

4 室内地图匹配

采用粒子滤波融合WiFi定位和PDR解决了定位的时序性问题，但仍存在定位轨迹穿墙问题，需要结合室内地图匹配来解决。

4.1 室内地图建模

室内地图建模采用矢量表示法与图层表示法相结合的方式。基于矢量表示法，室内地图模型包括：点、线、面。点表示室内某个位置，用地图中的x和y坐标表示。线表示室内的墙和门，包括起点、终点和意义。面则表示房间、走廊、楼梯等空间区域，包括构成该面的所有线和该面的意义。采用图层表示法的设计原则，点、线和面分布在不同的图层中。

4.2 粒子穿墙问题

粒子滤波算法存在粒子转移不当等问题，例如粒子转移到不可到达的区域或穿墙到另一个室内区域。利用室内地图的位置信息约束粒子转移，从而解决粒子的不恰当转移。粒子i的状态空间为，地理属性为为粒子所在的室内区域，为粒子周围的墙，为粒子可穿过的门，为粒子可到达的室内区域。然后在重新计算粒子权重和归一化粒子权重之间加入粒子可用性评估：

1) 如果粒子转移的目的地属于不可到达区域，如没有入口可进入的室内或在定位空间之外的区域，则置该粒子的可达值ωr为0，否则为1：

2) 如果粒子转移到不可直接到达区域，即源区域与目的区域没有门相连，则置该粒子的直达值ωc为0，否则为1：

这样，粒子i便有了3个权重值：由式(7)计算得到是否进入不可达区域的权重、是否进入不可直达区域的权重和距离权重。将这3个权重值相乘得到新的粒子权重：

根据这个新的权重值进行归一化和重采样。

4.3 轨迹穿墙问题

解决粒子穿墙问题后，轨迹穿墙问题明显改善，但还不能杜绝。观察发现这些穿墙通常发生在门附近的墙上。因此，如果滤波融合后估计的轨迹穿墙到另一个区域，则将穿墙点重定位到门的位置。这样直接矫正穿墙会导致后续的定位延迟过大。为弥补这种延迟，采用回退N步的方式进行穿墙矫正。如图2所示，P为当前滤波融合系统估计的轨迹，Pk为k时刻的定位结果，D1和D2为两个门。在k时刻检测到轨迹线Pk-1Pk与墙W相交，而最近的门是D2，则回退N步(此例中N= 2 )，调整k-N时刻的定位结果到门D2上，表示为Qk-N，调整之后新的轨迹为Q，其上Qk-N+1到Qk之间的轨迹点Qi的位置坐标为：

穿墙矫正后，定位点Pk转移到了kQ，滤波融合系统中的粒子也做出相应转移，以kQ为初始点对粒子坐标值重新进行初始化采样，而速度不变。

图2 轨迹穿墙问题

5 实验及结果分析

实验场景为电子科技大学第三教学楼二楼，大小为64 m×14 m，采用校园网WiFi环境，对定位区域中的200个参考点进行了WiFi指纹采样，设备是基于Android的三星Galaxy Note。实验为穿过多个房间和走廊的行走，手机屏幕向上并手持于胸前。测试主要采集3类数据：WiFi信号指纹、加速度和磁场强度，共采集到90条WiFi信号指纹、5 974条加速度数据和5 911条磁场强度数据，用时120 s。使用同样实验数据，将基于SVM的WiFi指纹定位、改进的PDR、基于粒子滤波的WiFi+PDR融合定位和基于粒子滤波的WiFi+PDR+Map融合定位4种方法进行对比。

5.1 定位误差分析

本文采用平均定位误差和定位误差累计概率分布作为定位性能的度量标准。定位误差指的是实际位置和估计位置之间的欧几里得距离；平均定位误差是所有测试位置的定位误差的均值；定位误差累计概率分布则为定位误差小于特定值的累计概率。

4种方法的定位误差如表1所示。由于无线信号的不稳定性及周围环境的干扰，WiFi定位的平均误差为3.77 m；PDR短期定位效果好，但会出现误差累积的现象，实验中平均误差为2.87 m；而经过粒子滤波融合后，WiFi定位和PDR相互补偿从而使得平均误差降到2.75 m，相比WiFi定位和PDR提高了27.1%和4.2%。地图匹配后，通过将粒子和轨迹约束到合理区域内，平均误差达到1.81 m，相比粒子滤波的融合结果又提高了34.2%。图3为4种方法的定位误差累计概率分布。可以看出基于粒子滤波的融合定位系统明显优于单独的定位系统，地图匹配能进一步明显地提高定位精度。在定位初期，PDR效果良好，甚至优于WiFi和PDR的融合结果，但随着PDR误差的累积，融合系统逐渐显现出优势。

图3 定位精度对比

表1 定位误差对比

5.2 定位轨迹分析

图4为粒子滤波融合WiFi和PDR的定位结果，图5为粒子滤波融合WiFi、PDR和地图匹配的定位结果。实验人员从图最右侧位置开始，经过走廊和4个房间，最终在左下角房间结束，接近匀速行走。两图中房间内的编号代表用于两级WiFi定位的子区域编号，带小方框的实线是真实轨迹，基于SVM的WiFi定位绘制的轨迹点和真实轨迹相差较大。由于WiFi定位是单点定位，且没有上下文关联信息，故定位轨迹难以形成连续结果，位置跳变现象经常出现。PDR绘制的轨迹和真实轨迹的重合度比WiFi结果要好，但后期的误差累积导致时间越长定位误差越大。图4中基于粒子滤波的WiFi+PDR融合定位算法得到的行走轨迹虽然和真实轨迹不尽重合，但由于融合算法中WiFi定位修正了PDR的误差累积，而PDR修正了WiFi定位结果不连续的现象，使得WiFi+PDR融合算法明显优于WiFi定位和PDR。基于粒子滤波的WiFi+PDR+Map融合算法绘制出的轨迹，由于运用室内地图矫正了粒子穿墙和轨迹穿墙现象，使得定位结果更加合理与准确，是4种方法中最接近真实轨迹的定位曲线，误差波动较小，表现优异。由于在轨迹穿墙处采用回退N步的方式进行穿墙矫正，基于粒子滤波的WiFi+PDR+Map融合算法得到的定位轨迹，包含若干在穿墙处的断点。

图4 WiFi+PDR估计的行走轨迹

图5 WiFi+PDR+Map估计的行走轨迹

6 结束语

基于WiFi指纹和基于手持移动设备传感器的PDR是室内定位常用的方案。但是WiFi指纹的定位精度和稳定性都不高，而单一PDR方法会出现误差线性累计问题，不能应用于实际复杂的定位场景。本文融合这两个定位结果并结合地图信息，充分发挥各自的技术特点，避免了WiFi定位和PDR单一定位系统的不足，使其在稳定性、适应性和精确性等方面都得到提高。

实验先通过粒子滤波将WiFi指纹和PDR结果进行融合，使得定位系统时序相关，从而降低定位系统中定位结果跳变现象的可能性；然后建立矢量室内地图，将室内地图信息融合到基于粒子滤波的WiFi+PDR定位算法中，采用粒子穿墙约束和轨迹穿墙矫正方法，减少不正常的状态转移，显著提高了最终的定位精度。通过仿真对比实验，基于粒子滤波的融合定位算法通过对地图匹配前后粒子滤波融合系统的定位精度和行走轨迹对比，应用已知的室内地图信息能够有效地解决定位轨迹穿墙问题，使得定位结果更加合理精确。

[1]BAHL P, PADMANABHAN V N. RADAR： an in-building RF-based user location and tracking system[C]//19th Annual Joint Conference of the IEEE Computer and Communications Societies. Tel Aviv, Israel： IEEE, 2000(2)：775-784.

[2]YOUSSEF M A, AGRAWALA A, UDAYA SHANKAR A.WLAN location determination via clustering and probability distributions[C]//1st IEEE International Conference on Pervasive Computing and Communications. Fort Worth, TX：IEEE, 2003： 143-150.

[3]WOODMAN O, HARLE R. Pedestrian localisation for indoor environments[C]//10th International Conference on Ubiquitous Computing. Seoul, South Korea： ACM, 2008.

[4]SCARLETT J. Enhancing the performance of pedometers using a single accelerometer[EB/OL]. [2015-11-20].http：//www.analog.com/en/analog-dialogue/articles/enhancin g-pedometers-using-single-accelerometer.html.

[5]SHIN S H, PARK C G, KIM J W, et al. Adaptive step length estimation algorithm using low-cost MEMS inertial sensors[C]//IEEE Sensors Applications Symposium. San Diego, CA： IEEE, 2007.

[6]PARK S K, SUH Y S. A zero velocity detection algorithm using inertial sensors for pedestrian navigation systems[J].Sensors, 2010, 10(10)： 9163-9178.

[7]CHEN L H, WU E H K, JIN M H, et al. Intelligent fusion of Wi-Fi and inertial sensor-based positioning systems for indoor pedestrian navigation[J]. IEEE Sensors Journal, 2014,14(11)： 4034-4042.

[8]CHEN Z, ZOU H, JIANG H, et al. Fusion of WiFi,smartphone sensors and landmarks using the Kalman filter for indoor localization[J]. Sensors, 2015, 15： 715-732.

[9]LI W W L, ILTIS R A, WIN M Z. A smartphone localization algorithm using RSSI and inertial sensor measurement fusion[C]//IEEE Global Communications Conference.Atlanta, GA： IEEE, 2013.

[10]WANG J, HU A, LIU C, et al. A floor-map-aided WiFi/pseudo-odometry integration algorithm for an indoor positioning system[J]. Sensors, 2015, 15： 7096-7124.

[11]BAO H, WONG W C. An indoor dead-reckoning algorithm with map matching[C]//9th IEEE International Conference on Wireless Communications and Mobile Computing. [S.l.]：IEEE, 2013.

[12]PERTTULA A, LEPPAKOSKI H, KIRKKO-JAAKKOLA M, et al. Distributed indoor positioning system with inertial measurements and map matching[J]. IEEE Transactions on Instrumentation and Measurement, 2014, 63(11)：2682-2695.

[13]ASCHER C, KESSLER C, WEIS R, et al. Multi-floor map matching in indoor environments for mobile platforms[C]//2012 International Conference on Indoor Positioning and Indoor Navigation. [S.l.]： [s.n.], 2012.

[14]ASCHER C, KESSLER C, WANKERL M, et al. Dual IMU indoor navigation with particle filter based map-matching on a smartphone[C]//2010 International Conference on Indoor Positioning and Indoor Navigation.[S.l.]： IEEE, 2010.

[15]LEPPÄKOSKI H, COLLIN J, TAKALA J. Pedestrian navigation based on inertial sensors, indoor map, and WLAN signals[J]. Journal of Signal Processing Systems,2013, 71(3)： 287-296.

[16]程水英, 张剑云. 粒子滤波评述[J]. 宇航学报, 2008,29(4)： 1009-1111.CHENG Shui-ying, ZHANG Jian-yun. Critical particle filter[J]. Journal of Astronautics, 2008, 29(4)： 1009-1111.