移动机器人改进激光SLAM算法研究

陈 丹，吴 欣

西安理工大学 自动化与信息工程学院，西安710048

智能化机器人在面临复杂不确定地形环境时如何确定自身位置，并提供优质的地图具有重要的理论研究价值和实际应用意义[1]。同步定位与地图构建（simultaneous localization and mapping，SLAM）[2-3]利用机器人携带的传感器在未知环境下观测目标，根据机器人与目标之间的相对位置和里程信息来估计机器人与目标点的全局坐标。SLAM 问题一直是机器人研究中的一个基础且重要的开放性研究热点。2017年，Wongsuwan等人[4]提出纠正梯度细化方法对传统RBPF-SLAM进行改进，该改进方法可以应用于多种类型的机器人传感器上。2018年，Jo等人[5]提出了一种新的映射表示方法以及由基本映射和单独映射组成的分解方案，在保证地图精度的基础上节约环境地图的存储空间。2020年，杨爽等[6]提出一种通过搭建地标数据库和位姿推导模型，解决了移动机器人在SLAM过程中定位失真问题。

求解SLAM 问题的方法大致可分为基于概率估计的方法和基于非概率估计的方法两大类。基于概率估计的方法主要有扩展卡尔曼滤波（extended Kalman filter，EKF）、FastSLAM与粒子滤波（particle filter，PF）等。粒子滤波的优点在于解决如状态递归估计或高维非线性、非高斯时变系统的概率推理等复杂问题。粒子滤波作为一种有效解决SLAM 问题的方法仍然存在粒子退化问题[7]。后来，Murphy 等人[8]提出采用RBPF（Rao-Blackwellised particle filter）求解SLAM问题。2007年，Grisetti 等人[9]应用了基于有效样本容量的选择性重采样策略，在一定程度上限制了重采样次数，但对缓解粒子耗尽的现象并没有做出有效的改进。2016年，田梦楚等人[10]提出智能优化算法，采用优胜劣汰机制，提高了精度，但该算法对小权重粒子吸引较小，易造成粒子退化现象。2018年，刘云涛[11]采用基于蝴蝶优化的粒子滤波算法处理重采样之后的粒子，该算法避免陷入局部最优且改善粒子耗散现象，但在粒子间吸引时任意两粒子间交互计算，实时性较差。2019年，吴正越等人[12]提出了基于最小方差重采样方法（minimum sampling variance，MSV）的一种优化SLAM算法，在动态环境下取得了更好的建图和定位效果，但是算法复杂度增加且只能达到有限精度。2020年，王彦等人[13]针对重采样过程导致粒子多样性降低问题，提出一种自适应优化组合重采样方法，但改进算法只能在一定程度上提高建图精度，对于构建尺寸较大的复杂环境地图还有待提升。

迭代最近点（iterative closest point，ICP）算法是一种点云匹配算法，是基于激光雷达扫描相邻帧，寻找激光雷达两个扫描点之间的转换关系。当扫描点数增大时，ICP 会承受过高的计算成本。为了提高ICP 算法的效率和准确性，人们提出了许多改进方法。2016年，Lee等人[14]提出了一种在线学习的方法来进行三维对象配准，实现了更好的鲁棒性和稳定收敛性，但是迭代过程中可能不能收敛到全局最优解。2018 年，Guan 等人[15]利用体素格对点云数据进行重采样，采用寻找对应点的方法来提高点云配准效率。2019 年，Wu 等人[16]通过引入最大相关准则作为相似性度量，提出一种新的鲁棒尺度ICP算法。这些算法虽然提高了点云配准效率，但也丢失了部分配准精度，使得对点云初始位姿的选取更加严格。

因此本文着重提供良好的点云初始态，从采样点的选取和匹配策略方面对激光雷达快速点云配准算法进行了改进，同时针对传统重采样过程中粒子多样性丧失问题，提出了一种改进传统粒子滤波重采样方法，最后基于ROS系统进行真实较复杂场景下的地图构建。

1 RBPF-SLAM的算法改进

1.1 改进ICP算法

ICP 算法配准的目标是通过一个三维变换矩阵将源点云集p={pi} 转换为目标点云集q={qj}，且尽可能使两个点云坐标在同一位置重合。两点云位置配准误差为：

求解误差函数式（1）的最小值，获得下一次位姿变换估计值：

式中，“⊕”表示旋转平移操作符，Sref为旋转平移参考面，k为匹配算法迭代次数，索引i和j分别代表点云p和q中的点，R和T分别为旋转和平移矩阵。ICP算法原理简单、直观，但算法的效率和全局最优收敛取决于初始迭代值。

主成分分析（principal component analysis，PCA）是一种有效的简化检测数据集方法，可以减少数据集维数，反映数据集对方差贡献的最大特征。因此，相似度大的两片点云，通过PCA 算法只要把其参考坐标系调整到一致，即可达到粗配准目的，粗配准为点到线ICP点云精配准提供了良好的点云初始变换。

传统ICP 配准算法采用两点间欧氏距离最近点作为配准点，易受到噪声的干扰。因此，本文在对应点匹配中，将最近的点度量从两点之间的欧氏距离改变为到两点连线的垂直距离，对点云采用PCA 粗配准后再进行点到线ICP算法精确配准，以提高匹配精度和收敛速度，实现移动机器人的精确定位。点到线ICP解决曲面匹配问题，可以表示如下：给出一个参考Sref和一组点云{pi}，最小化点pi旋转平移到其在表面Sref上投影的距离：

式中ni为曲面在投影点处的法线，Qk是对点云粗配准后得到的初始变换值。

基于点到线的改进ICP配准算法步骤：

（1）设置距离误差阈值δ，改进算法采用点到线的距离作为误差；

（2）将当前帧的点云数据根据初始位姿投影到参考坐标系中；

（3）对于点云中的每一个点pi，在参考帧中找到与它最近的两个点和；

（4）计算配准误差，误差计算采用下式：

（5）当误差小于误差阈值δ，停止配准迭代，否则返回步骤（2）。

1.2 改进重采样策略

优化的重采样算法对于粒子滤波来说既可保持样本的有效性和多样性，还可提高滤波器的性能。传统重采样方法完全丢弃了权值较小的粒子，多次迭代后削弱了粒子的多样性，导致收敛精度不足。针对该问题，本文提出一种改进重采样方法，将所有粒子按权值进行分类，根据预先设定的粒子权值阈值wt，在大权重和小权重粒子集合内分别进行采样，最后对新的粒子集进行权重归一化，进入下一步滤波过程。与传统重采样方法相比，改进重采样着重于在大权重集合内进行采样，大量复制粒子，同时在小权重集合内采用随机重采样方法获取粒子，在保证高权重粒子数的同时低权重粒子的选取机会增大，增加粒子多样性，进而提高机器人状态的估计精度。

改进重采样算法步骤如下：

（1）设定重采样门限阈值Nth，当有效粒子数Neff=（其中表示归一化权值），小于阈值Nth时，进行重采样步骤（2），否则进行下一时刻的滤波过程。

（2）对权重大于阈值wt的粒子进行重采样，将高权值粒子进行复制生成M个粒子，复制次数为粒子总数N与粒子权重的乘积取整floor(N×w)，高权值粒子集记为，其中r=1,2,…,M表示粒子个数。

（3）对权重小于阈值wt的粒子，进行随机重采样生成低权值粒子集记为，其中j=1,2,…,N-M表示粒子个数。

（4）分别对步骤（2）和步骤（3）产生的两部分粒子进行排序，得到新的粒子集。

（5）对新的粒子集进行权重归一化，完成后面的滤波过程。

1.3 改进RBPF-SLAM算法

RBPF-SLAM算法[17]将SLAM问题分解为机器人位姿估计和基于估计得到的位姿进行地图构建两部分进行描述。核心思想是利用外部传感器信息z1:t=z1,z2,…,zt-1和里程计信息u1:t=u1,u2,…,ut-1估计环境地图m和机器人轨迹x1:t=x1,x2,…,xt的联合后验概率密度p(x1:t,m|z1:t,u1:t-1)，该后验概率为：

式中，p=p(x1:t|z1:t,u1:t-1)表示利用激光雷达观测信息和里程计信息估计机器人位姿；p(m|x1:t,z1:t)表示通过当前机器人姿态更新地图m。

本文对传统RBPF-SLAM 算法从扫描匹配和粒子重采样两方面进行改进，改进后的RBPF-SLAM算法步骤如下：

（1）点云匹配。当机器人在移动过程中获得来自激光雷达的扫描数据时，采用本文改进点云配准算法，对机器人位姿进行精确估计。

（2）抽样。对上一代第i个粒子估计得到当前时刻粒子。选取激光雷达匹配结果作为优化提议分布，如式（6）所示：

（3）更新权重。根据权重计算公式（7）计算出每个粒子的权值。

（4）重采样。当Neff

（5）更新地图。根据第i个粒子表示的移动机器人位姿和当前观测情况，对第i个粒子构建的地图进行更新。

2 地图构建算法性能仿真与分析

本文基于激光雷达点云配准算法和粒子滤波重采样算法两方面对RBPF-SLAM 算法进行了改进。针对这两种改进策略，分别从算法配准精度、估计性能以及算法运行时间进行了性能仿真分析，同时将本文所改进重采样粒子滤波算法与传统重采样粒子滤波以及文献[12]所提MSV算法进行了性能对比。

2.1 改进点云配准算法性能

用激光雷达数据集对原始ICP算法和本文改进ICP算法进行点云匹配仿真实验，原点云数据与目标点云数据的配准结果如图1 所示，其中蓝色点云代表源点云，红色点云代表目标点云。图1（a）为原始点云分布，图1（b）为传统ICP配准后点云分布，图1（c）为本文改进ICP算法配准后点云分布。由图1 可知，图（c）中红色覆盖面积大于图（b），因此本文改进配准算法的点云配准准确度更高。在机器人定位过程中，优良的配准算法可以进一步提高激光雷达点云的配准精度，从而实现移动机器人的精确定位。

图1 传统ICP和改进ICP算法配准后点云分布Fig.1 Point cloud distribution after registration of traditional ICP and improved ICP algorithms

此外，本文对传统ICP 算法和改进ICP 算法的运行时间进行了比较，对于相同点云数据集，传统ICP 算法进行点云配准运行时间需要12.537 s。而本文不但改进了精配准算法，同时增加了粗配准，改进精配准算法与原始ICP配准算法相比，算法运行时间大大减小。因此加上粗配准时间依旧比原始ICP 精配准算法运行时间短，总共时间为8.063 s，比传统ICP 算法直接配准时间节省了4.474 s，由此说明改进算法实时性更好。

2.2 改进重采样算法性能

为了验证本文基于改进重采样方法的粒子滤波状态估计性能，选取典型的单变量非平稳模型进行仿真分析。该模型具有典型的非线性特征，且后验分布具有双峰特性，其状态方程和观测方程分别为：

其中，k为状态时间，x(k)为状态向量，z(k)为观测向量，v(k-1)和n(k)是零均值高斯随机变量，其方差分别为Q(k-1)和R(k)。

对目标位置估计误差性能采用均方根误差（RMSE）进行评价：

其中，x(k)和(k)为k时刻的滤波状态真实值和估计值，N是蒙特卡罗模拟次数。

本文对改进重采样粒子滤波、MSV算法[12]和传统重采样粒子滤波三种算法进行了性能对比。仿真中Q(k-1)和R(k)分别取10 和1，状态初始值x(0)=0.1,N取100次[18]，粒子数设定为100。图2 是三种粒子滤波算法对状态估计值的仿真对比，图3是三种粒子滤波算法状态估计均方根误差。由图2、图3可知，本文提出的改进重采样粒子滤波状态估计值与真实值接近，均方根误差均小于其他两种算法，说明本文所提算法提高了粒子的多样性和估计精度。

图2 三种重采样算法滤波状态估计Fig.2 Three resampling algorithms for filtering state estimate

图3 三种重采样算法滤波均方根误差Fig.3 Three resampling algorithms for RMSE

图4为原始重采样算法、MSV算法和本文改进重采样粒子滤波三种算法滤波后粒子分布情况。由图4 可知改进重采样粒子滤波算法拟合精度高，大部分粒子分布紧密靠近真值，只存在个别粒子样本发散，可以有效提高地图估计精度。

图4 三种重采样算法滤波后粒子分布Fig.4 Particle distribution after filtering by three resampling algorithms

为了进一步分析改进算法的实时性，对传统重采样、MSV 算法和本文改进重采样粒子滤波三种算法在相同初始粒子数情况下进行多次仿真实验，统计算法运行时间均值，如表1 所示。可以看出，在相同粒子个数下，改进算法的运行时间低于传统算法以及MSV算法，验证了本文所改进重采样粒子滤波算法在地图构建具有一定的实时性。

表1 三种算法运行时间对比Table 1 Comparison of running time of three algorithms

3 Turtlebot机器人SLAM实验

在实际情况下，机器人的工作环境比仿真环境更加复杂，环境特征更加丰富，存在行人等不确定因素。为了验证改进算法在这些情况下的有效性，分别采用传统重采样算法、MSV 算法以及本文改进重采样与配准算法，基于Turtlebot机器人对西安理工大学教五楼室内和室外走廊环境分别进行了SLAM 实验。构建地图时所需的粒子个数如表2所示，每个环境重复10次实验。实验场景1是室外走廊，实验场景2为室内，由于室内障碍物较多，环境复杂，构建地图所需粒子数多于室外所需粒子数。

表2 构建地图粒子数Table 2 Number of particles to construct a map

图5（a）和（b）分别为进行地图构建的室外走廊和室内真实环境。

图5 地图构建真实场景Fig.5 Map building real scene

采用三种RBPF-SLAM算法构建的网格图如图6和图7 所示。由图6 和图7 红色圈标记地方可以看出，本文所提改进算法构建的地图符合真实场景，原始算法构建的地图在拐角处存在偏移和重影问题，而改进算法构建的地图更清晰、更整齐。对比图6（a）、（b）和（c），图（a）中原始算法没有构建出障碍物完整的形状，图（b）中MSV 算法对障碍物的构建不够完整，地图中灭火器和电梯口位置构建不清晰。而图（c）中本文改进重采样与点云配准算法构建的地图信息更加精确地反映环境信息，线条较为清晰，表示障碍物对应所在位置的单元栅格状态更为确定，同时对于电梯口等拐角位置信息的构建更加完整。

图6 实验室外走廊环境下构建地图效果图Fig.6 Effect of map constructed in experimental outdoor corridor environment

图7 中间部分箭头所标注的红色圈为室内摆放的障碍物，可以看出室内靠墙两边的书桌位置以及中间矩形障碍物的形状被清晰地构建出来。图7（c）所构建的障碍物效果也明显优于采用原始构图算法获得的图7（a）和基于MSV的构图算法获得的图7（b）。对于障碍物的大小、形状和位置构建效果提升也很明显，可以建立正确且一致性良好的环境地图，算法的定位精度得到了一定程度上的改善。构建地图的差异主要是因为原始SLAM算法在粒子滤波重采样过程中，注重复制大权值粒子和频繁的重采样，导致粒子多样性丧失、粒子退化，从而导致机器人位姿与估计位姿不重合，地图构建过程中出现地图倾斜与墙体重影。而MSV算法采用计算最小采样方差的方式，使得重采样前后粒子分布保持一致，差异达到最小化，缓解了样本贫化问题，但是计算复杂度增加，粒子分布比较集中，缺乏多样性，只能达到有限精度。本文所提改进算法克服了原始算法和MSV算法的缺点，并且由表2 还可以看出，改进算法用于地图构建，用更少的粒子数，构建出更加精确的地图，减小了算法计算复杂度，进一步验证了改进算法构建环境地图的有效性。

图7 实验室环境下构建地图效果图Fig.7 Effect of map rendering in laboratory environment

4 总结

本文提出了一种基于激光雷达的改进RBPF 算法。采用改进ICP算法完成激光扫描点集配准后，通过引入改进重采样的粒子滤波算法，减少了粒子退化，在保留好粒子并创造新的有用粒子基础上增加粒子多样性，提高了移动机器人的定位性能和构建地图的准确性。将其与传统SLAM算法进行了性能仿真对比，改进算法的估计精度优于传统SLAM算法，能更准确地表达真实粒子的状态。最后利用Turtlebot 机器人在真实场景下对改进算法地图构建进行实验。结果表明改进SLAM算法比原始SLAM算法构建地图准确性更高，能清晰地表达障碍物的形状及其位置信息，更能清晰准确体现所构建环境地图的细节及边缘信息。