基于改进RBPF算法的移动机器人SLAM方法研究

陈国军,陈 巍

(南京工程学院工业中心，江苏 南京 211167)

0 引言

自主导航是移动机器人研究的重点。为了实现自主导航，机器人需要理解运动场景的结构和方向。与此同时，定位和地图构建(simultaneous location and map construction,SLAM)已经成为自主导航研究中的重要内容。机器人创建地图，更新结构和定位自己，实现导航。基于粒子滤波的SLAM算法，例如FastSLAM算法和GMapping算法[1]，利用了粒子滤波非参数特性，能够处理多模态分布，从而估计更接近正确值的状态解。由于粒子滤波器SLAM的结果不一致、内存消耗大，以及马尔可夫假设的误差积累，使得粒子滤波器SLAM的研究遇到很多问题[2]。近年来，许多SLAM算法转向使用位姿图SLAM(oriented fast and rotated brief SLAM,ORBSLAM)算法。位姿图SLAM算法包括两个主要部分，前端用于从数据关联中跟踪机器人轨迹以创建姿态图，后端用于优化姿态图。但是位姿图SLAM也存在很多问题。不良的位姿图结构会破坏优化过程，使得结果陷入局部极小值[3]。本文针对非参数方法趋向于避免陷入局部位姿图极小值的问题，对基于粒子滤波的方法进行了改进，利用修正梯度细化(corrective gradient refinement,CGR)算法对传统的Rao-Backwellized粒子滤波SLAM(Rao-Backwellized particle filtering SLAM,RBPF SLAM)算法进行了扩充。使用栅格地图结构和激光雷达传感器作为提出的广义SLAM算法，并将其扩展为位姿图SLAM前端图构建的基础。

1 RBPF SLAM算法

在RBPF方法中，将状态空间划分为姿态状态和地图状态。粒子滤波(particle filtering,PF)近似机器人的姿态置信度分布，而每个粒子包含表示环境模型的映射[4]。RBPF SLAM算法是使用PF来估计给定里程计测量序列U0:t-1和环境观测Z0:t的机器人轨迹分布p(xt,m|Z0:t,U0:t-1)以及环境地图m。采用该轨迹估计来估计地图和轨迹上的期望分布：

p(xt,m|Z0:t,U0:t-1)=p(m|xt,Z0:t)p(xt|Z0:t,U0:t-1)

(1)

PF工作类似于蒙特卡洛定位，除了一个给定的地图，每个粒子包含一个单独的地图。其思想是将机器人轨迹估计与地图估计过程分开[5]。该分解允许首先估计机器人轨迹，然后计算给定轨迹地图：

p(x1:t,m|Z1:t,U1:t-1)=p(m|x1:t,Z1:t)p(x1:t|Z1:t,U1:t-1)

(2)

由于x1:t和z1:t是已知的(以已知姿态映射)，因此可以有效地进行后验过映射p(m|x1:t,Z1:t)的解析计算。后部p(x1:t|Z1:t,U1:t-1)的估计与定位问题相似，因为仅需要估计机器人轨迹。这是用PF进行增量处理观察和测距读数[6]。整个过程可以概括如下(对于每个粒子i)。

②重要度加权：根据最近的观察、姿态估计和与该粒子相关联的映射，为每个粒子分配单独的重要度权重ω(i)。

(3)

③重采样：具有低重要性权重的粒子ω通常被高重量的样品所取代。此步骤是必要的，因为仅使用有限数量的粒子来近似连续分布。此外，重采样允许在真实分布不同于提议的情况下应用PF。

(4)

基于扫描过程所传递的数据，计算粒子分布的高斯近似Ψ。

(5)

参数μ(i)和∑(i)分别是平均值和方差，分别通过每个粒子i进行确定。这些参数估计如下：

(6)

(7)

归一化因子：

(8)

通过使用式(7)计算生成有效粒子，然后权重可以近似为xj，是接近相应扫描所指出的最大概率的点样本)：

(9)

重新分布的粒子将保持在状态表示中，而另一些则将被去除，这样可以替换样本，并且使得最终的根据状态轨迹重新进行粒子分布。通过重新采样，更新标准决定执行重采样步骤，通过估计有效粒子数neff的方法是估计粒子方差。

(10)

(11)

2 改进RBPF SLAM算法

所提出的算法遵循连接图路由算法(contact graph routing，CGR)局部化结构，并通过Rao-Backwellized粒子滤波器将其扩展到SLAM应用[8]。该方法是将CGR算法合并到RBPF中，以便改进轨迹p(x1:t|z1:t,u1:t-1)。通过四个CGR算法步骤(预测、精炼、接受、更新和重采样)[9]，其算法可以总结如下。

①预测步骤：遵循CGR预测步骤，得到初始状态建议分布集q0作为结果。

③验收测试步骤：在产生最终分布集q的CGR算法，精确验收测试步骤。

④更新和重采样步骤：根据新的RBPF权重推导，通过广义细化步骤的改进建议来更新权重。粒子集重采样准则改变后，使用选择性重采样来防止粒子退化。

(12)

在实现过程中，为了降低复杂度、避免浮点误差，忽略了归一化常数，优选对数似然版本。D是对应距离dk的集合，将非归一化的观测对数似然定义为[11]：

(13)

传统的Beam端点模型的实现需要对整个地图的似然分数缓存进行预计算，这主要是为了进行定位。然而，这在SLAM情况下是不合适的。这是因为在计算观测似然分数之前没有关于地图的信息。此外，通过使用匹配，不能保证每个观测光束zk都能匹配其对应关系。因此，当未找到其对时，通过惩罚似然得分，产生了似然得分的最终形式。

(14)

式中：umull为一个恒定的惩罚距离，因为没有找到对应关系-空似然得分。

3 试验研究

3.1 仿真试验

试验测试结果如图1所示。

图1 试验测试结果

本文在机器人操作系统(robot operating system,ROS)上实现了该算法，并进行试验评估。先使用2个基准数据集进行仿真测试，由激光传感器采集。数据集是在具有动态对象的无约束户外环境中收集的。本文算法至少需要20个粒子来创建这个图，第一个基准数据集(Intel试验室数据集)是由一个差分驱动器配备LMS激光雷达，仿真场景大小近似为28 m×28 m。本文算法需要5个粒子来正确地创建一个具有5厘米栅格分辨率的闭环映射，如图1(a)所示。在第二个数据集(Campus数据集)上进行测试，配备SICK LMS激光雷达，仿真场景大小近似为250 m×250 m，如图1(b)所示。试验表明，本文算法利用在实际的基准数据集中创建地图，同时在所需的粒子方面保持效率。

3.2 室内场景试验

本文使用从安装有RPLIDAR A1激光传感器的开源ROS移动机器人平台收集自己的试验数据集，在计算机上运行 Liunx(Ubuntu 16.04)的ROS系统，完成同时定位与地图构建。为了测试改进RBPF SLAM算法提供的校正姿态的质量，需要确定适当的最小阈值分数以接受或拒绝姿态。由于得分根据传感器和地图上下文而不同，因此需要调优以获得适当的地图，将扫描匹配得分定义为：

(15)

式中：σsm为扫描匹配标准偏差。

本文将长度为30 m的试验楼走廊设置为场景，试验结果如图2所示。

其中，图2(a)、图2(b)和图2(c)通过根据上面的公式改变分数应用RBPF SLAM算法得到的结果，图2(d)是根据本文的算法给出的结果。从结果来看，通过接受性测试与当前地图最匹配的观察可能性来概率地评估姿态，本文提出的方法能给出更少失真的地图，使移动机器人定位精度得到了有效的提高，地图构建的准确性也得到了有效的提升。试验结果表明，该方法具有良好的系统鲁棒性。

图2 试验结果

4 结束语

本文在RBRF SLAM算法的基础上，通过将RBPF算法与CGR算法相结合，提出了基于CGR算法的改进RBPF SLAM方法。该方法有效地减少了所需粒子的数量，降低了在滤波预测阶段机器人姿态估计的不确定性。本文提出的算法是在激光传感器上实现的，在移动机器人试验室环境下的试验和结果分析。在LIDAR SLAM基准数据集上的测试表明，通过试验分析，基于粒子滤波的RBPF SLAM算法能够实时在线生成高精度栅格地图，与实际地图更加一致。