室内环境约束的行人航向粒子滤波修正方法

刘 盼，张 榜，黄 超，杨卫军，徐正蓺

(1.中国科学院 上海高等研究院，上海 201210； 2.中国科学院大学，北京 100049； 3.上海科技大学 信息科学与技术学院，上海 201210)(*通信作者电子邮箱xuzy@sari.ac.cn)

0 引言

随着互联网、物联网、智能移动设备等相关技术的发展和应用，基于位置的服务(Location Based Service, LBS)以其强大的实用性和广阔的发展空间逐渐成为了学术领域和工程领域共同关注的热点。

在室内环境中，LBS技术已经开始在很多场景中发挥着重要的作用，比如机场或车站等大型建筑中的实时定位与导航、大型商场中基于位置的商品广告推送、展览馆中参观路线的引导以及基于位置点的讲解等。这些服务和功能都是对如今现代化建筑规模不断扩大、功能不断全面化的适应性产物，也是提升人们生活质量和改善生活体验的刚性需求，而它们得以实现的基础之一就是室内定位技术[1]。面对日益增长的室内定位需求，相关领域的科研人员正在不断地尝试利用新的方法或者改进现有方法来提高室内定位的精度。

步行者航位推算(Pedestrian Dead Reckoning, PDR)[2-4]是一种基于惯性传感器元件的室内行人定位算法，该算法通过惯性传感器采集到的行人运动数据(主要包括三轴加速度计、三轴陀螺仪数据等)、行人运动时的环境数据(主要包括三轴磁力计、激光测距数据等)来计算行人步伐长度和行进航向，并结合起始位置的设定来得到行人当前的位置坐标，最终可以通过搭建的智能显示平台等呈现出室内行人的运动轨迹。在步行者航位推算算法中，步伐检测、步长估计、航向估计是三个关键问题，通过对这三个方面的优化，可以有效地提高室内定位的精度。在步伐检测[5]方面，一般是通过某个时间段内由加速度计得到的加速度或者由陀螺仪得到的角速度变化为零来认定当前脚部处于静止状态，这一方法中使用到的惯性数据采集元件是佩戴在脚部的；对于步长估计[6]，一般是通过经验公式、步长模型等方法来进行，有时候还会结合卡尔曼滤波等算法来消除部分噪声；而为了减小航向误差，零速度更新(Zero Velocity Update, ZUPT)[7]或者零角速度更新(Zero Angle Rate Update, ZARU)的方法常被使用。由于应用惯性系统的定位方式往往缺乏真实可信的航向观测指标，以上方法虽然有一定的效果，但是累积误差仍然是一个十分明显的问题。

为了减小惯性定位的累积误差，研究人员提出了借助额外的信号发生设备来进行定位校准的方法。这里采用的额外设备信号经常被作为建立指纹库的根据之一，或者直接通过信号强度得到距离进而融合到其他算法中。英国的Ubisense系统[8]采用的超宽带脉冲技术，其定位精度达到了6～10 cm。刘小康等[9]利用ZigBee 无线传感器网络的信号强弱建立了指纹数据库，将定位平均误差限制在了1.5 m以内。还有利用无线网络Wi-Fi技术[10]结合区域聚类算法来实现更加精确的室内定位的。此外，诸如蓝牙[11]、射频识别等需要额外架设信号设备的室内定位技术手段都得到了一定程度的研究和利用。不过这种凭借额外设备辅助的定位方法，无论是在设备自身成本还是在设备铺设维护等方面的成本都带来了更重的负担，同时，不确定的室内环境信号干扰往往对这些方法的应用和准确度的提高造成较大困难。

此外，研究人员尝试了基于地磁场匹配的航向校准方法来提高定位精度[12]，这种方法根据室内地磁场的分布情况建立数据库，能够在定位过程中于一定程度上校准航向，同时容易得到行人行走的真北方向，不过仍旧难以避免在复杂室内环境中信号干扰的问题。近年，利用粒子滤波融合支持向量机和卡尔曼滤波的方式[13]也开始被尝试，这种方式通过对传统卡尔曼滤波PDR算法的改进和对数据进行预处理，在一定程度上提高了室内定位精度；但仍然明显地存在着行人估计位置穿墙等不理想的情况。还有研究人员利用地图匹配结合区域生长算法[14]，根据输入的地图图片信息，对每一步的落点位置进行修正，这种方法也在一定程度上提高了定位精度，并且有效地避免了穿墙情景，最终将整体误差控制在1%左右；但是该算法只将地图信息应用在了去除穿墙粒子的层面，并且最终误差仍旧偏大。

综上所述，本文针对传统惯性定位算法中航向累积误差的问题，提出了一种室内环境约束的粒子滤波算法来进行行人航向修正。在算法整体设计上，首先利用PDR框架下的传统卡尔曼滤波方法得到室内行人步长和原始航向，并将后者作为粒子滤波航向修正的滤波对象，同时融合室内环境约束以完成航向校准。区别于以往地图匹配中单纯剔除穿墙粒子或仅在拐弯处建立信标的方法，改进的算法通过粒子动态选择地图数据得到的航向与墙体夹角来构建粒子自修正机制，通过粒子动态选择地图数据得到的粒子与墙体距离构建粒子赋权机制，以更加充分地利用室内环境信息。此外，该算法将航向作为唯一状态量，相比于传统粒子滤波过程中多状态量同时求解的特点，单一状态更便于数据分析和处理，降低了计算复杂度。

实验部分通过佩戴多惯性测量单元(Multiple Inertial Measurement Unit, MIMU)[15]传感器在办公楼内分多组多人次进行了不同行走距离的实验。实验结果表明,所提算法可以减小航向误差，提高定位精度，并且在行人行走距离增加时具有更好的稳定性。

1 基于卡尔曼滤波的PDR室内定位

航位推算算法是主流的室内定位方法之一，但由于传感器误差的累积，往往会形成较大的航向和定位累积误差。通过引入卡尔曼滤波算法可以在一定程度上使此种误差减小，以提高航向和定位的精度。而由于缺乏真实可信的行人方向观测数据，基于卡尔曼滤波的PDR室内定位算法得到的航向仍需校准。下面将简要给出PDR原理和卡尔曼滤波流程，并将其解算结果作为粒子滤波对象，以针对航向进一步修正。

1.1 航位推算算法

航位推算算法一般是利用惯性传感器采集三轴加速度数据、三轴角速度数据，对其进行一次积分、二次积分、零速度检测、步伐判定等一系列的处理后得到步长估计和航向估计，再结合设定的初始位置坐标点，逐步或者逐点绘制出室内行人的运动轨迹。文中涉及到的滤波算法均是在航位推算框架下完成的。

在二维坐标系中航位推算算法如图1所示。其中：p0、pN代表室内行人起始位置以及终点位置的坐标点，pk、pk+1代表行走过程中在k落点时刻和k+1落点时刻的坐标，所谓落点时刻即是当步伐检测算法判定行人脚部贴合地面的稳定时刻。另外，lk、θk代表k落点时刻行人的步长估计和航向估计。

图1 航位推算算法示意图Fig. 1 Schematic diagram of dead reckoning algorithm

在得到k落点时刻的步长估计和航向估计之后，结合k落点时刻的位置坐标，就可以进一步地通过式(1)得到k+1落点时刻的位置坐标：

(1)

其中：(Xk,Yk)和(Xk+1,Yk+1)分别表示k落点时刻和k+1落点时刻行人在二维坐标系中的位置坐标；θk+1和Δθ分别表示k+1落点时刻航向以及k落点时刻所产生的航向变化量。

值得注意的是，由于PDR算法本身对于步长和航向的估计是通过单纯积分的方式得到的，这并未对其中惯性元件产生的噪声加以抑制，因此会存在较大的累积误差。为了减小累积误差，就需要结合适当的滤波算法。

1.2 基于卡尔曼滤波的航位推算

卡尔曼滤波算法是在室内定位研究中主流的噪声处理方法之一，该算法通过构建状态模型并结合观测数据对每一时刻的状态量进行噪声滤除，能够在一定程度上减小PDR算法中存在的累积误差。本文提出的算法中用到的步长以及原始航向就是通过传统的卡尔曼滤波算法结合PDR得到的。

状态模型为：

Tk|k-1=ATk-1|k-1+uk

(2)

其中Tk=[θk,ωk,vk,pk]为状态量，θk、ωk、vk、pk分别为航向、姿态角、速度和位置；A为状态转移矩阵；uk为系统噪声。

测量方程为：

Gk=HTk|k-1+rk

(3)

其中：Gk=[vk,ωk]为观测向量；H为测量矩阵；rk为测量噪声。在检测到脚部静止[16]时，进行滤波更新，具体的卡尔曼算法实现流程如图2所示。 通过前后两次零速度时刻的位置坐标p1、p2可以计算当前步长为lk=‖pk-pk-1‖，此步长将作为后续粒子滤波算法的输入。

图2 卡尔曼滤波算法示意图Fig. 2 Schematic diagram of Kalman filtering algorithm

通过实验验证，基于卡尔曼滤波算法得到的行人步长估计比较准确。同时从图2中可以直观地看出，该算法缺乏真实可信的航向观测数据，这导致该算法航向估计仍存在较大的累积误差。针对这个问题，本文提出室内环境约束的粒子滤波算法，将航向作为状态量进一步滤波修正，以达到更为理想的定位效果。

2 室内环境约束的粒子滤波航向修正方法

在第1章得到原始航向和步长数据的基础上，本章应用室内环境约束的粒子滤波算法对行人航向进行校准，其中仍然将单步作为航位推算的基本单元。在粒子滤波过程中，利用地图信息融合进行了两点改进：一是在粒子初始化和粒子补偿阶段，利用原始航向与自主匹配的参照墙体进行粒子补偿判断并进行补偿，以此构建粒子自修正机制；二是在粒子权重分配阶段，依照粒子形成的步伐落点与自主选择的墙体距离进行权重计算，以此构建依距离赋权机制。最终使位置估计和航向估计更加贴合实际情况。

2.1 粒子初始化

传统的粒子滤波算法应用于室内定位时，在粒子初始化阶段往往需要构建多维度状态量，除航向以外，还包括姿态、二维位置、加速度等，而本算法在粒子初始化阶段仅对卡尔曼滤波得到的原始航向进行粒子采样，采样后的粒子由式(4)表示：

F=(f1,f2，…，fN)

(4)

其中：F表示粒子集合;N表示本次采样产生的粒子总数。

对于完成采样的集合F，其航向粒子服从式(5)表示的正态分布：

(5)

需要注意的是，标准差σ会根据粒子位置情况进行动态调整，即当全部粒子位置表现为穿墙时，适当增大σ利于提供更多符合修正条件的粒子。该操作也可以视为一种简化的重采样过程。

根据算法的采样方式，粒子分为两种情况，即大于或者小于原始航向。然而，当粒子过大、过小或粒子分布过于均衡时，可能会出现过拟合或者欠拟合的情况，即有过多粒子出现过度补偿或不被补偿的现象。因此，根据实验情况，在采样阶段确定一个恰当的标准差是必要的。

图3 航向采样示意图Fig. 3 Schematic diagram of sampling heading

2.2 粒子航向修正

对航向完成合理的采样后就要进行粒子修正，在该阶段，首先将地图信息抽象成用线段表示的结构体，具体做法是根据室内环境以及行人室内行走特点，对通路尺寸和相对位置进行实地测量和标注，以两点一线方式结合角度在平面内绘制出墙体轮廓，并以二维坐标数据的形式加以保存以便于后续计算。

结构体如式(6)和式(7)所示：

map=(s1,s2,s3,s4...sn)

(6)

si=(p1i,p2i,λi)

(7)

其中：n表示墙体数量；si表示第i个墙体，且由起点p1i、终点p2i、墙体方向λi构成。

基于已有的室内环境结构体，提出通过动态地关联室内环境数据与待校准样本，即依据航向采样产生的粒子与其相邻墙体形成夹角的情况，自适应地对粒子航向进行补偿。

其中，在每一个粒子选择参照墙体时，会根据结合步长推算的当前落点位置时刻坐标对平面所有墙体与该坐标的相对距离进行排序。完成距离排序后会出现以下三种情况：

1)有唯一的最小距离值。

2)有不唯一的最小距离值，且多个相等的最小距离来自平行的墙体。

3)有不唯一的最小距离值，且多个相等的最小距离来自不平行的墙体。

对于第1)种和第2)种情况，可以依据两直线夹角的性质任选一条线段作为参照墙体。对于第3)种情况，可以认为此时行人处于转弯处或者某些粒子散落到墙体内：对于前者，此时不予补偿，待行人转弯过后再重复以上墙体选择过程；对于后者，直接将墙体内粒子舍弃。

(8)

所有的粒子完成航向补偿后具有这样的特点，即不论粒子偏向行人直线行走整体方向的哪一侧，补偿行为总是通过增大或者减小航向粒子数值使其向行人行走方向收缩，以此来达到校准原始航向的目的。但是，对于每一个粒子应在校准过程中占有多大的比重，还需要通过权重分配算法进一步确定。

2.3 粒子权重分配

在传统的粒子滤波中引入室内环境约束的一大优势就是可以根据环境条件设置合理的伪观测数据，而该数据又是粒子滤波过程中确定权重分配的必要前提。在本文算法中，依据行人室内行走实际情况对通路中线进行坐标数据转换，标定每一个粒子位置到中线的距离，以此作为伪观测数据。

因为在粒子航向修正阶段已经将落入墙体的粒子进行剔除，所以本阶段权重分配只针对落在通路内的粒子。在此，提出通过关联室内环境数据与粒子落点构建依距离赋权机制，即将某一落点时刻所有粒子到中线的距离进行排序，按照距离大小确定粒子权重。

在上述过程中的墙体选取方式与上一阶段粒子补偿时的墙体选取方式相同，因此不再赘述。但需要注意的是，在粒子赋权阶段会将形成一条通路的两个墙体都选取出来，以便后续计算。

至此，粒子开始进行具体的权重计算，如图4所示，粒子权重由通路中线向选取的两侧墙体逐渐递减，其关系满足式(9):

wi∝1/di

(9)

其中:wi为第i个航向粒子的绝对权重;对于第i个航向粒子,di为估计位置到通路中线的距离。

式(10)确定了粒子归一化权重值:

(10)

其中：Wi为归一化权重；N为参与权重分配的所有粒子总数。

值得注意的是，为了提高计算效率，实际权重分配是由每个粒子到其相邻墙体的距离决定的，如式(11)所示：

(11)

其中ci表示通路中线到墙体的距离。

图4 权重分配示意图Fig. 4 Schematic diagram of weight allocation

这样设计是因为，一方面当粒子距离墙面越远，距离通路中线就越近，其权重也就越大；另一方面，式(11)可以直接从已有的室内环境结构体中获取数据，从而避免增加不必要的计算代价。

2.4 确定行人位置

完成粒子采样、粒子航向修正、粒子权重分配的过程后，进一步地，可以利用式(12)得到校准后的航向：

(12)

然后结合步长估计就可以利用式(1)中(Xk+1，Yk+1)的计算方法得到行人当前位置坐标。

综上所述，以下给出本文算法的伪代码。

输入 室内环境测量数据、通过卡尔曼滤波得到的步长估计lk以及原始航向θk(k=1,2,…,M)；

输出 校准后的航向θcor以及位置坐标(X，Y)。

1)

对于θk∈[θ1,θM]执行以下命令

//零速度检测算法得到行人前行过程中M个航向角

2)

//粒子采样过程

3)

F=normrnd(θk,σ,1,N)

//采样粒子数为N，正态分布期望为θk，标准差为σ

4)

//粒子航向补偿过程

5)

构建室内环境结构体map=(s1,s2,…,sn)

6)

选择参照墙体si用其完成第8)～9)行代码计算

7)

fori= 1:N

8)

9)

10)

11)

end

//在补偿过程之后粒子数量可能低于初始的采样数，

//但为了方便叙述，仍用N表示

12)

//粒子权重分配

13)

选择通路墙体si、sj用其完成第14)～15)行代码计算

14)

di=第i个粒子距离墙面的距离

15)

//计算归一化的权重

16)

end

//粒子滤波对航向校准的过程完毕

17)

//得到优化后的第k步的航向

18)

//计算位置坐标

19)

以上伪代码对提出算法的整体逻辑进行了概括。进一步地，可以通过分析其中一些关键步骤，讨论算法在计算代价上的改进。

在代码第3)行粒子采样时，仅涉及到原始航向θk一个维度，相比于传统粒子滤波算法中构建多维度状态量的过程，将矩阵运算变为数值运算，其对于运算代价的减少是显而易见的。此外，在代码第6)、13)、15)行的计算过程中实现了对室内环境结构体的复用，尤其是在第15)行中通过转换伪观测数据在权重分配中的应用方式，可以有效地提升计算效率。

3 实验结果与分析

为了验证本文提出的室内环境约束的行人航向粒子滤波修正方法的有效性，测试人员在研究院1号办公楼三层进行了多组不同距离的重复实验。实验过程如图5所示，图中测试人员右脚佩戴MIMU集成的惯性传感器模块，佩戴位置位于脚尖。该模块采样频率为125 Hz，可以同时完成4组时钟对齐的数据采集。图5中测试人员右手为手持式激光测距仪，用来测试实际行走距离以及进行定位精度标定，其测量精度为0.01 m。

图5 实验过程示意图Fig. 5 Schematic diagram of experimental process

在数据处理过程中，使用的重力加速度为9.788 m/s2，使用的加速度数据和陀螺仪数据为MIMU输出数据的均值，采样粒子数为3 000。本文算法最终轨迹效果以及数据对比结果通过Matlab软件仿真来实现。

通过实地测试分析，本文中对两种室内定位算法的结果进行了比较：一种是本文提出的室内环境约束的行人航向粒子滤波修正算法，另一种是传统的卡尔曼滤波室内定位算法。两种算法均可在行人行走估计轨迹与实际轨迹的对比中直观地看到解算效果。如图6所示，实线轨迹为实际行人室内行走路线，行走距离为246 m，虚线轨迹为传统卡尔曼滤波算法得到的行人行走路线效果，双划线轨迹为本文提出算法得到的行人行走路线效果。

由图6可知，对比实际行走轨迹，传统卡尔曼滤波算法的解算会出现较大的位置和航向偏差，并发生明显的穿墙行走现象；经过计算，该算法终点位置绝对误差为0.57 m，相对误差为0.23%，在终点处航向累计误差为7.6°。而本文提出算法所得到的估计轨迹更加贴合实际轨迹，并且有效地避免了穿墙行走现象；经过计算，本文算法终点位置的绝对误差为0.25 m，相对误差为0.10%，在终点处航向累计误差为0.39°。因此，在该次实验中，本文算法相比传统卡尔曼滤波定位算法降低了0.32 m的绝对位置累积误差，降低了7.21°的航向累积误差。本次实验结果可以在一定程度上说明本文算法的有效性。

为了进一步验证本文提出算法的有效性和稳定性，测试人员设置了5段不同的行走距离，分别为61 m、108 m、215 m、323 m、435 m，每段行走距离进行独立的三组重复实验。实验计算两种算法得到终点位置绝对误差以及航向累计误差。其中，将三组重复实验得到的终点位置绝对误差的均值作为对应行走距离的位置绝对误差，将三组重复实验得到的航向累计误差的均值作为对应行走距离的航向累计误差，以此减小实验结果的偶然性。

图6 不同算法下行人轨迹对比Fig. 6 Pedestrian trajectory comparison of different algorithms

5段不同行走距离重复实验的位置绝对误差和累计航向误差统计如表1所示。

从表1中可以得知，对于传统卡尔曼滤波算法来说，不论是位置误差还是航向误差，在整体上都是呈现随着行人行走距离增大而不断增大的趋势。但在一些个别实验中，这两种误差会出现小于较短距离条件下误差的情况，这是由于传统卡尔曼滤波算法本身在航向漂移方面具有不稳定的性质造成的。所以在实际实验中应用多组数据统计求均值的方法是很具有必要的，能够提高实验结果的可信度。

而对于本文算法来说，从数据观察的角度并没有发现明显的发散趋势；同时，可以通过数据对比发现不同距离下的绝对位置误差和航向误差并没有过大的差距。这就说明了本文算法能够有效地减小航向漂移的不稳定性，进一步地，与传统卡尔曼滤波算法得到的误差数值进行逐个对比也可以说明本文算法能够在一定程度上减小两种误差。不过，为了满足对比实验对严谨性的要求，解算结果仍然对三组重复实验取均值。

为了更加直观地说明本文算法相比于传统卡尔曼滤波算法在减小终点位置绝对误差和累计航向误差方面的有效性和稳定性，还需要结合多组重复实验的结果完成进一步的数据分析。

表1 不同距离重复实验误差统计Tab. 1 Error statistics of repeated experiments at different distances

传统卡尔曼滤波定位算法和本文所提算法在不同距离情况下终点位置绝对误差的对比如图7所示，该图数据是通过对所有重复实验完成均值处理所得。由图7可知，传统卡尔曼滤波定位算法在不同行走距离情况下终点位置绝对误差始终大于1.50 m，并且，随着行走距离的增加，绝对误差呈现出一种发散的趋势，不断增大；在行走距离超过400 m时，该算法绝对误差达到了5.50 m，此时的相对误差为1.3%左右。而本文所提算法在不同行走距离情况下的终点位置绝对误差相对稳定，并且始终不超过1.50 m；在行走距离超过400 m时，该算法绝对误差为0.87 m，此时的相对误差仅为0.2%左右。通过两种算法对比可知，无论是在特定行走距离方面，还是在随着行走距离增大时的误差趋势方面，本文所提算法始终优于传统的卡尔曼滤波算法，并且在行走距离大于400 m时，基于本文算法的位置精度提高了一个量级，表明了本文算法的有效性和稳定性。

图7 不同算法终点位置绝对误差对比Fig. 7 Comparison of absolute error at end position of different algorithms

在航向校准方面，传统的卡尔曼滤波算法只将航向作为状态量之一，实际解算时还要完成包括位置、加速度、姿态角等多个参量共同的状态转移，该过程相对繁琐且航向在长距离行走情景下依然会存在较大的偏移。而本文算法只对单一的航向参量进行粒子采样和航向补偿，并通过权重分配使得每一步落点都更靠近真实位置，计算过程相对简洁。同时我们注意到，行人在行走时的单步真实航向只通过惯性传感器本身很难进行数值标定，但在行走一段直线距离时，根据起始点位置计算的航向是相对准确且具有实际意义的。出于以上考虑，本文提出的航向算法在直线行走阶段的解算是更加贴近该直线轨迹下的真实航向的。两种算法航向误差对比如图8所示，图中的航向解算就是以直走廊为参考得出的。

图8 不同算法航向误差对比Fig. 8 Comparison of heading error of different algorithms

由图8可知，传统卡尔曼滤波算法得到的航向误差在行走距离为61 m时就超过了3°，并且随着行走距离的增加，航向误差不断累积，在行走距离超过400 m时达到了15°以上。整体上，传统卡尔曼滤波算法得到的航向误差呈现发散趋势，同时短距离时的较大误差使得该算法难以在实际情境下直接应用。而本文提出的算法在实际测试过程中始终将航向误差稳定在了0.9°以内，相比于传统卡尔曼滤波算法，航向精度有了明显的提高，验证了本文算法的有效性和稳定性。

4 结语

针对在传统卡尔曼滤波室内定位算法中航向累积误差的问题，本文提出了室内环境约束的行人航向粒子滤波修正方法，在卡尔曼滤波的基础上，结合室内环境信息，利用粒子滤波算法对航向进行修正，得到了更加精确的行人位置。本文主要贡献如下:1)在粒子动态选取参照墙体的基础上，构建粒子航向自修正机制，使补偿后的粒子更趋近于行人行走方向。2)构建关联通路伪测量数据的依距离赋权机制。通过确立合理的粒子赋权规则，在有效避免过拟合的基础上提升了整体航向的精度。3)精简了粒子滤波模型，将航向作为唯一的状态量，在提高精度的同时方便了数据分析和处理。经过实际场景分距离多组重复实验验证，本文算法在减小行人定位绝对误差和航向累积误差方面的有效性及稳定性都明显优于传统卡尔曼滤波算法。但本文算法在底层数据降噪处理以及场景泛化方面存在着一定的局限性。未来工作中，一方面会基于已有成果改进粒子状态转移过程以进一步提高算法场景适用度；另一方面会融合多部位、多特征底层数据进行关联研究，并将多数据关联应用于已有算法预处理、状态转移和权重更新阶段，以此进一步提高基于多数据融合的行人航向精度。