基于虚拟地标的行人惯性SLAM算法

曹志国，熊 智，丁一鸣，李婉玲，王钲淳

(南京航空航天大学自动化学院, 南京 211106)

0 引言

随着社会的飞速发展，日常生活以及国防等领域对行人导航系统的需求日益增加。在室内环境下，卫星信号由于遮挡和多路径效应不可用，无法满足室内定位的精度需求。因此，迫切需要研究一种不依赖全球卫星导航系统(Global Navigation Satellite System, GNSS)的室内行人导航技术。随着微机电系统(Micro-Electro-Mechanical System，MEMS)技术的发展[1]，MEMS惯性测量单元(Inertial Measurement Unit，IMU)凭借体积小、功耗低、质量小、便于携带等优点，使得以MEMS-IMU为基础的室内行人导航方法得到了广泛的关注和研究[2-4]。尽管如此，基于低成本IMU的行人惯性导航系统航向角发散[5-6]这一关键问题尚未完全解决。

为了抑制行人惯性导航系统的航向角发散，国内外学者已经提出了许多解决方法，其中具有代表性的算法主要有三种[7]，即启发式漂移补偿方法、基于预设特征的方法和基于同时定位与建图(Simultaneous Localization and Mapping，SLAM)的方法。启发式漂移补偿方法通常假定行人在室内沿着平行建筑物内墙的方向行走[8-10]，此类方法的缺点在于需要先验地图信息，而先验地图信息往往难以获得。基于地标的方法通过将行人运动过程中采集到的WiFi或磁信号与事先建立的指纹库进行匹配，辅助行人导航系统定位[11-12]，而这类指纹库需要提前部署设备，且需要进行维护。SLAM技术是指装载单一或多种传感器的运动主体，在没有环境先验信息的条件下，在运动过程中构建环境地图的同时估计自身的实时运动[13]。近年来，随着计算机硬件处理能力的快速提升，SLAM技术发展迅速，目前已经有多种不同框架的SLAM方法[14-15]。其中，P.Robertson等[16]将二维平面划分成紧密拼接的六边形栅格，在估计行人位置的同时对未知环境构建了六边形网格地图，实现了仅依靠惯性传感器探索未知建筑物。该方法虽然可以在一定程度上减弱航向角的漂移，但通常计算量较大，难以满足实时定位的需求。

针对以上不足，本文提出了一种新的基于虚拟地标的行人惯性SLAM算法，通过从轨迹中提取并匹配虚拟地标点，求解最小误差位置，并以此为条件补全虚拟地标点间的地图，即建筑中可行走的通路，从而替换传统SLAM方法逐步建立的稠密地图，以降低传统SLAM算法建图的迭代次数和未回环的无效优化，提高了全局位置和地图估计精度，消除了纯惯性行人定位系统的累积误差，实现了纯惯性行人定位系统长时间稳定可靠导航。

1 算法总体框架

如图1所示，本文提出的基于虚拟地标的行人惯性SLAM算法分为两部分，分别为基于虚拟地标的误差补偿方法和基于惯性概率地图的SLAM方法。

图1 基于虚拟地标的惯性SLAM算法框架Fig.1 Inertial SLAM algorithm based on virtual landmark

本文提出的基于虚拟地标的误差补偿方法，以基于惯性概率地图的SLAM方法得出的位置估计为输入，通过从估计轨迹中提取并匹配虚拟地标点，求解最小误差位置，作为建图的依据。传统的基于惯性概率地图的SLAM方法，在每一步更新位置的同时对地图进行更新，导致计算量大且收敛速度较慢。因此，本文提出的算法对此进行了简化，将虚拟地标点视为特征点，只在特征点处更新地图，而其他位置的地图则根据前述的最小误差位置进行补全，在保证精度的同时降低了计算量。

2 基于虚拟地标的误差补偿方法

2.1 虚拟地标的提取

不同于传统意义上需要提前布置设备的地标，虚拟地标从行人惯性里程计输出的航迹中提取。本文研究只针对单层建筑中的行人惯性导航，在单层建筑中，直角转角点是最显著的特征，因此利用惯性传感器的信号辨识直角转角点，即本文所述的虚拟地标点。如图2所示，蓝色线即为行人航向角的估计值，红色线圈出的即为直角转角过程，可以看出，转弯动作开始前的稳定航向角值与完成转弯动作后的稳定航向角值的差值在90°左右，但转角的动作通常不在一步之内完成，且由于传感器的测量误差，导致转角的航向变化值会有波动。

图2 航向角变化曲线图Fig.2 The yaw angle date during walking

因此，本文提出了一种自适应滑动窗口方法，以辨识行人航迹中的直角转角点。根据行人在转弯过程中航向角变化值持续较大的特点，本文先通过设置转弯状态阈值判断行人是否处于转弯状态，具体计算方法如式(1)所示

(1)

其中，Sk为表征行人k时刻是否处于转弯状态的状态量，为1时表示行人处于转弯状态，反之则不处于；Δφk表示k时刻航向角的变化量；φ1表示转弯状态阈值，根据多次实验发现，选取φ1=10°较为合适。滑动窗口的长度为转弯状态量连续为1的区间长度，由行人进入转弯状态到转弯状态结束的时间决定。对滑动窗口内的所有航向角变化量进行累加，即可得到完整转弯的航向角变化，如式(2)所示

(2)

其中，φsum即为航向角的完整变化量，若φsum落在[80°,100°]的阈值区间，则认为滑动窗口内为直角转角过程，且将被判定为直角转角过程中的滑动窗口内的航向角变化序列记录下来

Lposi=(xk,yk)，k取当Δφk最大时

(3)

如式(3)所示，变化值最大的时刻所在的位置点识别为虚拟地标的位置Lposi。

2.2 虚拟地标的匹配

由于行人惯性里程计输出的位置存在累积误差，基于行人惯性里程计输出的位置提取出的虚拟地标位置同样存在累积误差，即同一真实转角点对应的虚拟地标点位置不同。因此,需要对上述的虚拟地标点进行匹配和筛选。

为了进行虚拟地标的匹配，本文认为通过行人惯性里程计识别出的虚拟路标点的位置是对建筑物中真实转角点位置的观测，服从以真实转角点位置为期望、行人惯性里程计位置误差为方差的二维高斯分布[7]，如图3所示，其中红色点为真值点，黑色圆圈表示二维高斯分布的范围。

图3 虚拟地标匹配示意图Fig.3 Virtual landmark matching schematic diagram

因此，当2个不同的虚拟路标点间的直线距离满足二维高斯分布的方差要求时，则认为2个虚拟路标点对应同一真实路标点，此过程称为虚拟路标点的匹配。

本文采用的匹配方法依赖阈值的选取，因此为了尽可能避免匹配失败和误匹配的情况出现，除了多次实验选取合适的阈值外，本文还针对短距离内出现多个转角的复杂情况，设置了虚拟路标点的步数区间阈值Lsth1。因为行人惯性里程计的误差具有随时间缓慢发散的特点，短距离内的匹配对定位精度的提升有限，所以当2个虚拟路标点对应的步数编号小于步数区间阈值时，则只存储先识别出的虚拟路标点，如图4所示，对虚拟地标的筛选可以在降低匹配难度的同时减小误差补偿的计算量。

图4 虚拟地标筛选算法Fig.4 The algorithm of choosing virtual landmark

2.3 基于虚拟地标的误差补偿方法

光束平差法是现有视觉SLAM系统求解三维空间坐标和相机位姿普遍采用的方法。虽然本文使用的传感器为惯性传感器，但同样需要对位置误差进行处理，因此，本文将光束平差法的思想引入基于虚拟地标的误差补偿中。

本文首先对误差进行了数学建模。本文提出的误差补偿方法以基于行人航位推算算法的行人惯性里程计的输出为输入，因此采用步长航向模型，同时为了简化计算，将位置误差近似成高斯分布，可以得到如下的运动学模型

(4)

其中，k表示步数的编号;Lk表示第k步的步长;φk表示第k步的航向;ex,k和ey,k为步长和航向估计不准确引起的位置误差。为了进一步简化位置误差的数学模型，令ux,k=Lkcosφk，uy,k=Lksinφk，则位置误差数学模型简化为

(5)

当虚拟地标匹配成功时，位置误差的数学模型为

(6)

根据上述数学模型，可对行人惯性里程计输出的步长和航向信息建立线性方程组

e=AM-U

(7)

其中

其中，假设(x0,y0)和(x3,y3)为匹配的2个虚拟地标点，则第7、8行为虚拟地标匹配时所增加的行，其余行以此类推，为了使得误差最小，即

⟹Mop=(ATA)-1ATU

(8)

其中，Mop为使得全局位置误差最小的位置矢量，也即是误差补偿后的位置矢量。

由上述算法的特性可知，在虚拟地标匹配计算后，虚拟地标位置点之前的路径都会得到修正，即可用修正后的位置求得修正后的航向角，如式(9)所示

φ′=

(9)

φn=φ′+Δφo

(10)

3 基于虚拟地标的惯性SLAM方法

3.1 惯性概率地图

由于虚拟地标点的稀疏性，前述方法只在虚拟地标这些离散点有效，而无法作用于行人行走的全过程。为了解决该问题，本文引进了基于惯性概率地图的方法。

基于惯性概率地图的方法最早由德国宇航中心的P.Robertson等[16]提出，该方法首先将行人惯性导航问题建模成动态贝叶斯网络，如图5所示。

图5 行人惯性导航动态贝叶斯网络图Fig.5 Pedestrian inertial navigation dynamic Bayesian network

其中，P为位置变量；U为步长矢量；Z为IMU的量测；Map为地图变量。

根据动态贝叶斯网络模型，行人惯性导航问题可以建模成SLAM问题，即将惯性传感器测量到的数据视为观测，估计以该观测为条件的其他状态量的联合概率分布。针对SLAM问题已有多种较为成熟的框架，作为其中一种基于粒子滤波器的SLAM框架，FastSLAM将地图看成由许多不同地标组成，并假设在已知机器人运动路径的前提下，2个不同地标的估计条件独立。因此，行人惯性导航的概率估计可分解为

(11)

根据式(11)，位置和地图的联合条件分布可以分解成构图问题和定位问题。针对构图问题，可将二维地图栅格化，可以使用三角形、矩形或六边形作为栅格形状，而在60°的离散角度分割下可认为行人各个方向行走的概率是独立的，所以将栅格形状选为六边形。针对定位问题，利用动态贝叶斯网络的特点，可分解为

p({PU}k|{PU}0:k-1)·

(12)

(13)

3.2 基于虚拟地标的惯性SLAM算法

由3.1节的假设可知，地图的估计仅与位置的时间序列相关，又因为将地图划分成六边形栅格，对于地图的估计可以表示为地图中每个六边形栅格的估计的乘积，即

(14)

其中，h(h=0,1,...,NH-1)表示六边形栅格的编号。传统的FootSLAM算法中,地图与位置序列实时相关，而本文使用基于虚拟地标的增量稀疏地图代替实时稠密地图，仅在转角点处更新地图，两转角点间的地图可根据2.3节中求得的最小误差矢量进行补齐。而3.1节定位问题的推导只用了六边形栅格各条边概率满足狄利克雷分布的假设，本文依然采用六边形栅格，故3.1节的推导仍然成立。算法流程如图6所示。

图6 基于虚拟地标的惯性SLAM算法流程图Fig.6 Inertial SLAM algorithm based on virtual landmark

传统的FastSLAM算法通过粒子滤波器实现，由于本文算法是基于FastSLAM算法框架的改进，因此本文算法也使用了粒子滤波器。本文算法的输入为行人惯性里程计每一步输出的步长和航向变化量，首先进行建议分布采样，通过对航向变化量加入高斯白噪声实现，由此生成一定数量的粒子；然后进行粒子权重更新，根据当前粒子的步长矢量与栅格地图的穿边关系更新粒子的权重；更新权重后进行位置估计，即对所有粒子按照概率分布求得期望；求得期望之后，要判断粒子方差是否超出重采样阈值，若超出则认为权重小的粒子偏离估计值，因此舍弃权重较小的粒子，复制权重大的粒子进行补充，以保持粒子的总数目不变。同时，将前述求得的期望位置作为已知轨迹输入到基于虚拟地标的误差补偿中，若此时检测出虚拟地标，且与之前行走过程中存储的虚拟地标匹配，则进行全局误差估计，并更新惯性概率地图，否则不对惯性概率地图进行更新。这样在不损失地图对可通行区域的表达的前提下，简化了地图的构建过程，同时也减少了非必要的粒子更新计算。

4 实验

为了验证基于虚拟地标的惯性SLAM算法的有效性，在南京航空航天大学自动化学院的5楼进行了室内实验。如图7所示，该路线全长约1214m，包括3个房间的行人路径，起始位置和结束位置是同一点。

图7 实验设计路线图Fig.7 The experiment environment and designed path

本文使用的惯性里程计[17]经过行人航位推算算法处理后输出的每一步的航向和步长数据为本文算法的输入，其位置误差约为5%，航向角漂移约为5.4(°)/min，由于行人惯性里程计初始处理过程不是本文要讨论的内容，故不再进行详细阐述。

(a)虚拟地标提取

(b)虚拟地标筛选图8 基于行人惯性里程计原始轨迹的虚拟地标提取Fig.8 Virtual landmark extraction based on original trajectory of pedestrian odometer

如图8所示，蓝色线为在上述实验路线下重复5圈的行人惯性里程计原始轨迹，红色圆圈所在位置为本文算法识别出的虚拟地标点。从图8中可以看出，本文的虚拟地标识别方法准确识别出了5圈轨迹中的共计69个直角转角点，本文算法对识别出的虚拟地标点进行了进一步的筛选，结果如图8(b)所示。

从图8中可以看出，通过2.2节所述对较小范围内出现的多个转角点进行筛选，图8(a)红色圈中的4个地标点只保留图8(b)红色圈中的1个。

(a)算法效果对比图

(b)惯性栅格概率地图图9 两种算法实验结果Fig.9 Experiment results of two algorithms

根据筛选出的30个虚拟地标点，采用基于虚拟地标的误差补偿方法计算得到图9(a)中的绿色曲线，即为修正后的5圈轨迹。在实验中发现，基于虚拟地标的误差补偿方法只在虚拟地标点处进行全局修正，导致行人行走于离散虚拟地标点间时误差仍随时间发散，而基于虚拟地标的SLAM算法则可以解决这一问题。图9(a)中的红色曲线为基于虚拟地标的SLAM算法计算得到的轨迹，可以看出，基于虚拟地标的SLAM算法得到的曲线更加平滑，且误差更小。图9(b)为基于虚拟地标的惯性SLAM算法得到的六边形地图，六边形的边颜色表示该边的穿边概率，可以看出，六边形栅格概率高的边组成的通路与真实环境一致。

为了定量比较两种算法的效果，本文需要选择相应的位置点计算轨迹误差。由于图9(a)中紫色圈中的两点(0,0)和(61.8,32.8)是实验路径中距离最长的两点，且这两点没有用于补偿算法的计算，因此本文选择这两点作为计算的基准点。

利用本文的虚拟地标提取方法，对行人惯性里程计原始轨迹和两种算法处理过的轨迹进行处理，得到表1所示每条轨迹每圈中两点的对应地标点位置坐标。

表1 用于误差评估的虚拟地标点位置

计算每圈的位置误差，如图10所示。从图10中可以看出，行人惯性里程计原始轨迹的位置误差随时间发散，而两种算法修正后的轨迹位置误差稳定在一定范围内，其中，基于虚拟地标的SLAM算法的位置误差要优于仅基于虚拟地标的算法。

图10 位置误差变化曲线Fig.10 The curve of position error with the number of rounds

图10所示为所有误差评估点的平均误差变化，而在行人定位算法中也常用终点位置误差来评估精度，若仅考虑终点位置误差，三者分别为55.20m、4.08m和1.61m，相对误差分别为4.6%、0.3%和0.1%，可以看出，基于虚拟地标的SLAM算法仍然是精度最高的。

基于虚拟地标的惯性SLAM算法中采用了基于蒙特卡罗方法的粒子滤波器，具有一定的随机性，故本文对上述行人惯性里程计的数据进行了100次的重复处理，并计算了这100次处理的5圈位置误差的平均值，如图11所示。其中，基于虚拟地标的惯性SLAM算法平均位置误差始终小于10m。而由图10可知，仅基于虚拟地标的误差补偿算法的平均位置误差为11.1m，因此基于虚拟地标的惯性SLAM算法的平均位置误差始终优于仅基于虚拟地标的误差补偿算法，且其小于9m的概率为97%。

图11 基于虚拟地标的惯性SLAM算法位置误差分布Fig.11 Position error distribution of inertial SLAM algorithm based on virtual landmark

5 结论

针对当前仅使用MIMU的行人导航系统航向角存在较大累积误差的问题，提出了基于虚拟地标的惯性SLAM算法。本文算法可以在行人行走过程中，利用行人惯性里程计输出的航向和步长信息识别出虚拟地标点，即直角转角点，当行人重复经过同一直角转角点时，即可对轨迹进行修正，同时对航向角的漂移误差进行补偿。在此基础上，引入SLAM的思想，在估计位置的同时建立六边形栅格地图，进一步提高了算法的估计精度和实用性。最后，通过实验验证了本文算法在2027.04m2的单层建筑物中空间位置误差小于10m。

本文提出的基于虚拟地标点的惯性SLAM算法尚具有以下局限性：1)虚拟地标点仅限于直角转角点，算法只适用于处理二维平面的运动；2)基于虚拟地标的误差补偿方法构造的最小二乘方程计算复杂度随时间递增。在后续的研究中，将进一步丰富虚拟地标点的种类，将算法推广到三维空间，且对最小二乘方程的计算进行优化，降低其计算复杂度，同时将基于虚拟地标点的惯性SLAM算法应用于行人实时导航中。