基于改进粒子滤波的SLAM算法研究

孙 昊，周 阳，李丽娜

(辽宁大学 物理学院，辽宁 沈阳 110036)

随着机器人技术成熟，移动机器人在家政服务，无人驾驶，监控跟踪等方面发挥出越来越重要的作用。移动机器在固定区域内的自我定位和环境构建成为机器人领域的关键技术。基于粒子滤波〔1-2〕的Slam〔3〕算法证明移动机器根据周围环境自我定位和映射周围环境信息在理论上可行，但存在着两个致命缺陷，一是建图对机器人位姿有较高的要求〔4〕，二是频繁的重采样会导致粒子耗散。GMapping正是针对这两处缺陷，提出了具有针对性的解决方案。改进一：改善粒子采样选择；改进二：选择性重采样。本文通过改进的GMapping构建了室内全局地图。

1 粒子滤波理论描述

1.1 粒子滤波原理

在把粒子滤波原理应用到slam问题中时，一个粒子需要同时保存所有历史时刻的机器人位姿和整个地图信息。粒子滤波是贝叶斯滤波〔5〕在特殊条件下一种特殊方式。

根据条件贝叶斯法则，有：

p(x1：t,m|z1：t，u1：t-1) =p(m|x1：t，z1：t)·

p(x1：t|z1：t，u1：t-1)

(1)

这一分解可看成把SLAM分解为定位与建图两部分，该方法称为粒子滤波方法(RBPF)。

1.2 粒子滤波缺陷

粒子滤波(RBPF)为slam提供了理论的支撑，在实践上遇到两个主要的问题：

(1)通过机器人运动模型和特征点法采样局部关键帧，可以得到非常多的粒子。而这些粒子分布又很分散，与真实分布相差过大。要想提高真实分布附近的粒子数，就一定要整体增加粒子数，而运算过程中涉及到求矩阵逆，粒子数越多计算量指数倍增大。(2)在机器人的行走过程屡次采样会引起粒子耗散。各个粒子信息含有机器人走过路径上大部分的位姿和整个地图，因重采样依赖的权重更多取决于当前观测，而不是某次观测在几分钟前。本文改进的GMapping算法正是能够有效解决这两种问题，为粒子滤波在slam问题上的应用提供了有效的改进办法。

2 粒子滤波优化

2.1 改进一：改善粒子采样选择

大部分粒子不能真实反映位姿信息是因为粒子滤波采用加速度计运动模型作为提议分布。和加速度计运动模型不同，激光雷达观测模型使用滤波器搭配直接法选择局部关键帧。关键帧上采样得到的粒子分布在一个相对集中的范围，如图1所示。

图1 粒子采样分布位置图

具体的方法是先让加速度计给出位姿估计，激光雷达观测模型给出观测，再以该观测为初值进行特征匹配。在特征点匹配过程中，文章在点云匹配过程中同时计算各维护点到点之间的距离和点到线之间的距离〔6〕，只有当两种距离都符合设定的条件，才判定为激光点云匹配成功。当机器人运动回之前经过的位置并且方向和角度和之前很像，这种方法增强了回环的可靠性。由于运用两种方法进行匹配，如果按照传统的最小二乘法计算欧式距离最小值，计算开支非常大,建图和定位实时性会下降。所以特征点之间距离计算本文采用累加曼哈顿距离和切比雪夫距离的线性相加来取代欧式距离，减小了cpu的计算量，提高了运行速度和实时性。扫描匹配之后，本文就找到了L位置的突起区域，接下来需要在L中随机抽样采集N个点。依据这N个点的加速度计估计模型和激光雷达观测模型确定该突起区域对应的高斯分布的均值和方差，如下式所示：

(2)

(3)

(4)

这样就获得了最佳估计，采样得到新的粒子继续从该突起的高斯分布中抽样采集得到。

接着需要对于每个粒子计算一个权重，来确定粒子的可信度大小，供接下来的重采样使用，如公式(5)所示：

(5)

2.2 改进二：选择性重新采样

如果频繁进行采样，不但粒子的丰富性会消失，而且存储器中粒子和机器人位姿也会变得更加单调，所以应限制重采样次数来避免粒子耗散。

一个重要的问题是何时进行重新采样。应该清楚的知道，只有检测到机器人回到之前的位置后并且方向角度和之前相似才会进行回环，回环时计算误差大小，进而为不同的粒子传递不同的权值。准确粒子传递大的权值而误差大粒子传递较小的权值，只有它们的差有质的变化，这时再重新采样才能够起到好的效果。

GMapping不存在回环检测〔7〕，要想知道回环是不是发生，需要引入新的计算方法，本文给出了有效地实现检测的方法。下面通过引入公式计算粒子权重的差距大小。

(6)

Neff值越大，粒子被传递权重之间相减相应就越小。当Neff很小，就说明粒子准确性差距很大，有较少的粒子能够真实反映现实空间，较多的粒子不能反映真实值。当准确表示真实值的粒子较少，这时就需要引入新的粒子。在这个时间点，正需要做重新采样。所谓重新采样，即以wi的概率来接受这个粒子，生成一个随机数，依据所在区间决定wi及重复数次。为减少重采样次数，用一个量Neff来表示当前估计和真实分布的差异性: 如果Neff较小，表明差异性较大，需要进行重采样；如果Neff较大，表明差异性较小，暂停重新采样。如此可以极大的减少重新采样的次数，缓解粒子耗散问题。

图2 随机重采样图

3 GMapping从理论到实践

改进的GMapping流程图如图3所示，采样之后计算高斯分布，再从高斯分布中重采样并计算权重，得到位姿之后进行地图更新，之后不断重复这个过程直到得到全局的地图。

图3 改进的GMapping程序流程图

图4是在实验室中基于改进的GMapping算法构建的地图，对比图5一般粒子滤波器构建的地图，图中标记2的位置可以看出改进的GMapping算法在相同条件下建图连续性更好，图中标记1的位置可以看出改进之后算法的误差更小，更加准确的构建了实验室的环境。对比图6实验室实景可以看出，地图可以准确的表达实验室的真实结构。

图5 一般粒子滤波器构建地图

图6 实验室实景图

4 结语

针对办公室、实验室或工厂等小型静态环境，改进的GMapping算法改善了粒子采样的选择，通过设定严格的激光点云匹配策略，更精确地估计了机器人轨迹和室内环境。为了降低计算量，本文特征点之间距离计算采用曼哈顿距离和切比雪夫距离的线性相加来取代欧式距离。为了限制重采样次数通过设定阈值选择性重新采样，改进的GMapping 虽然好用但只限于实验室或办公环境等小场景，在大场景下进行全局地图构建〔8〕还存在误差累积等问题，未来还需要在大场景下继续优化。