复杂场景下移动机器人的SLAM与路径规划问题

丛佩超，吕昆峰，周加超

(广西科技大学机械与交通工程学院，广西柳州 545006))

1 引言

随着传感器技术的日益提升，室内场景下的移动式机器人集成了地图构建、导航定位、路径规划等诸多功能，其应用范围日益扩大[1，2]。

在复杂的室内环境中，移动式机器人实现高精度导航的前提是构建优质的先验地图。因此，提高同时定位和地图(Simultaneous Localization And Mapping，SLAM)构建精度一直是机器人研究领域中的一个开放性研究热点[3，4]。2016年，Sanjiv Singh等人[5]提出了一种实时和低漂移激光雷达建图方法，实现了机器人位姿运动估计的低漂移和低计算复杂度。2018年，Zhongze Liu等人[6]提出了一种实时6维度激光建图定位方法，有效地降低了激光测深位姿累积误差，并具有良好的建图性能及有助于机器人完成路径规划。2020年，FUYU NIE等人[7]提出了一种改进的激光回环检测和修正系统通过对存在高扫描匹配误差的子图进行分割，减少了地图内部的误差。

提高室内复杂场景下的机器人导航精度，需要降低实时建图与定位的累积误差。其方法有多种，文献[8]采用GPS卫星定位导航技术，可在地图上获得机器人的移动轨迹并观察到机器人所在的位置，但是容易受到数据网络波动影响，甚至搜星困难，导致机器人导航定位误差过大。文献[9]提出一种无源RFID(Radio Frequency Identification，射频识别)的室内移动机器人定位方法，可提高机器人定位精度，但是该方法需要提前在室内布置好射频标签(Tag)等设备。文献[10]基于深度相机采集的RGB图像信息，估算机器人当前位姿，但是存在不同帧间的匹配问题，且构建的地图有较多的点云重叠，与实际环境有较大的偏差，严重降低移动机器人的导航定位精度。

针对上述问题，本文通过Gazebo仿真平台，构建搭载Delta3A型号激光雷达的虚拟样机，引入Gmapping(基于Rao-Blackwellized粒子滤波器算法)、Hector SLAM(基于Gauss-Newton迭代法)及Cartographer三种主流算法的建图机理，优化Delta3A激光传感器的参数配置，降低三种建图算法所获二值优质地图与原图的匹配偏差，并在机器人操作系统[11]中融合A-star和DWA算法，分别进行全局和局部路径规划的导航分析。本文在虚拟样机仿真分析的基础上，基于Spark-T移动式机器人，进行了相应的优质地图构建与导航定位问题分析，进一步验证了所用算法的实效性。

2 实时建图与定位算法分析

SLAM是指根据移动式机器人在一个未知环境下的移动，在已知机器人输入控制值U1：t和传感器观测值Z1：t的条件下，估计概率分布P(m|X1：t，Z1：t，U1：t)来分别表示机器人定位和地图构建，即P(xt，m|Z1：t，U1：t，x1)。

2.1 HectorSLAM算法

HectorSLAM结合基于鲁棒性扫描匹配的2DSLAM系统[12]，利用激光雷达的高更新率和低距离测量噪声特点，对机器人位姿进行实时估计。该算法是基于Gauss-Newton迭代法[13]，解决非线性最小二乘问题，即通过观察自变量和因变量数值，求解非线性目标函数的系数参数，使得函数模型与观测量相近似，Hector SLAM将似然概率达到最大似然估计，通过最小二乘的方式来表达E=argmax(xt，m|Z1：t，U1：t，x1)的最大似然问题

(1)

式中T=Tx，Ty，Tθ：机器人的位姿；Si(T)：把激光点i用T转换之后的坐标；M(x)：得到坐标x的地图占用概率；[1-M(Si(T))]2：势场，其中Si表示得分。

通过Lagrange插值法[14]解得∇M(Si(T))，构建离散的栅格地图，计算高斯-牛顿方程，解得机器人位姿。Hector SLAM对初值敏感，求解∇M(Si(T))采用插值法，在理论上存在不连续的因素，难以处理闭环问题。

2.2 RBPF-SLAM算法

Gmapping基于Rao-Blackwellized粒子滤波器(Rao-Blackwellized Particle Filter，RBPF)，在Fast-SLAM的基础上进行二次优化，但仍然存在粒子耗散问题[15]。该算法将SLAM问题表示为求解机器人位姿以及基于位姿构建地图两部分内容。其主要思想是：结合传感器观测量Z1：t=z1，…，zt-1和里程计信息U1：t=u1，…，ut-1来求解机器人位姿X1：t=x1，…，xt和地图信息mi=m1，m2，…。联合概率密度可表示为

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

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

(2)

RBPF利用粒子滤波器求解时，保留高权重的粒子并舍弃低权重粒子，重采样时存在一定的随机性，采样次数增加时，粒子的多样性会耗散掉，即存在粒子耗散问题。引入自适应阈值Neff表示当前的求解值和真实分布的差异性

(3)

当Neff过大时，则差异性过小，不进行重采样，当Neff过小时，则差异性过大，进行重采样，从而起到减少重采样次数，缓解粒子耗散的作用。

2.3 Cartographer算法

Cartographer是基于图优化的算法框架，满足大场景建图。采用的是实时扫描与建图匹配方法，同时进行回环检测[16]，通过分支定界优化算法提高回环检测的效率。

Cartographer算法分为局部和全局进行实时建图与定位。构建局部地图是通过将激光点云图的激光点H={hk}k=1，…，K转换后匹配到子地图中，在匹配前转换：

(4)

式中ξ=(ξx，ξy，ξθ)：激光点云图中点束的姿态；Tξ：表示{hk}k=1，…，K在子地图中的位置。

通过子地图的累加构建全局地图，在累加过程中进行回环检测优化，其方法同样采用求解最小二乘问题

(5)

将子地图的位姿Ξm和传感器的位姿Ξs匹配，则其残差E为

(6)

Cartographer构建全局地图时，对子地图进行优化，即残差E达到最小，其过程如图1所示。

图1 Cartographer算法处理流程图

Cartograher相比Hector SLAM，采用了回环检测，对建立的子地图进行实时优化和匹配，在机器人快速旋转和俯仰等位置下，仍有效地降低地图的偏移问题，构图精度高。相比Gmapping，Cartograph算法构建更广阔的地图时，对计算量要求较低，求解最小二乘值和优化的速度也较快，建图效率高。

3 建图与导航仿真

上一章理论性地分析了三种SLAM算法，为验证其可行性和空间复杂度，本章通过搭建载有Delta3A激光雷达的虚拟机器人，在Gazebo仿真平台中控制可变参数，将三种算法建图效果可视化，并对机器人进行全局路径规划和局部路径规划，测试其导航精度。

3.1 同时定位与地图构建仿真

3.1.1 激光雷达参数配置

通过launch文件和xacro文件优化Delta3A激光雷达的参数配置，相关优化过程如下：

0 0 0 0 0 0

5.5

-3

3

优化完成后，Delta3A激光雷达传感器的性能得到极大地提升，其最大测量范围约为16米，采样率为8000次/秒，优化后的传感器可有效降低前述三种算法在建图时因激光点云信息丢失而造成的误差。

3.1.2 建图与分析

在Gazebo仿真平台中搭建室内环境，并投影成二值地图，如图2所示。

图2 模拟实验中使用的室内环境

为评估三种SLAM算法所获地图质量，对二值地图与其构建地图间的误差进行分析。为此，计算二值地图与算法地图的最佳拟合对齐(如图3所示)。具体工作过程如下：每个算法获得的地图进行二值化，该地图仅包含场景边界和障碍物，使用Generalized Dual Bootstrap-ICP图像配准算法，将二值化地图与各自的地面对齐。由于室内模型的二值地图和三种SLAM算法所获地图都是对齐的，然后使用Matlab软件中的Knnsearch函数计算地图间的误差，这种误差测量提供了一个标准化的距离测量值，如表1所示。

图3 二值地图(红色)与算法地图(黑色)拟合对齐

表1 二值地图中各算法的误差估计值

根据图2和表1，分析三种算法在Gazebo仿真平台中的仿真结果。在相同的环境中，Cartographer算法生成的二值地图与原地图的重叠率最高，且误差估计值比另外两种算法小，具有较优的建图效果。此外，Gmapping算法构建的地图与原地图重叠率相比Cartographer低，且其误差估计值在5.3670，而Hector SLAM的建图结果呈现出最高的误差值，其主要原因是当机器人处于较大的速度时容易发生显著的地图漂移。同时，Cartographer是一种基于图优化的算法，其在生成一个子地图后，会进行一次局部的闭环检测，故在室内复杂大场景下所获地图的质量比Hector SLAM和Gmapping算法更高。

3.2 路径规划仿真

移动式机器人通过Cartographer算法获得高精度的室内地图后，可以此为基础进行优良的路径规划。移动式机器人自主导航功能取决于全局和局部路径规划算法。两种规划算法主要基于传感器数据、里程计数据、地图服务器等信息，具体的导航框架如图4所示。

图4 导航框架

3.2.1 全局路径规划

全局路径规划可通过地图服务器提供的全局代价地图，并融合A-star算法[17]来实现全局导航。A-star算法使用启发式函数，在起点到终点的多个路径中，选取恰当的行驶路径。其启发式函数，如下

F(n)=G(n)+H(n)

(6)

式中：G(n)表示从初始节点到指定节点n的移动代价，H(n)表示从指定节点n到最终点的估算代价。

在地图中给定机器人导航目标点和方向，机器人会根据实时环境规划出最佳导航路径，当起始点到目标点之间没有障碍物时，其运动轨迹为一条直线，进而实现机器人的全局路径规划，如图5所示。

图5 基于Cartographer地图的全局路径规划

3.2.2 局部路径规划

采用动态窗口法 (Dynamic Windows Approach，DWA[18])可实现移动式机器人的局部路径规划。该算法主要是在机器人控制过程中进行多组速度离散采样，并在系统内部计算单元中先模拟这些速度下机器人的前进轨迹，并将每组轨迹进行评价，选取最优轨迹及速度传送给机器人的速度控制器，以驱动机器人实现避障。

仿真过程：当机器人完成全局路径规划后，在直线行驶路径上摆放障碍物，机器人会相应地做出局部路径规划，不再按照直线轨迹行驶，而产生新的行进路线，实时避开障碍物，完成机器人的局部路径规划，仿真结果如图6所示。

图6 基于Cartographer的局部路径规划

4 实体机器人实验

此外，本文还在真实场景中使用实体机器人进行测试，以便在没有理想模拟工况下研究三种建图算法。因为在模拟环境中有相对较优的条件，如：激光传感器无噪声累积、测量精度不受距离限制等。在真实环境中，本文使用Spark-T机器人(如图7所示)。在复杂环境中进行三次不同算法的建图与定位，并在获得相对优质的地图后，进行其全局与局部路径规划的实验分析。

图7 Spark-T移动机器人

4.1 移动式机器人系统

本文研究的移动式机器人由上位机和下位机构成，上位机负责远程状态监控、数据处理与分析等工作，下位机配备了连接Delta3A激光雷达等传感器的工控机，并在主控PC中预装Ubuntu16.04和ROS Kinetic，其系统架构如图8所示。

图8 软、硬件系统架构图

4.2 室内定位与构建地图实验

为验证三种算法在相同实验环境下的建图效果，使用Spark-T机器人进行了三次不同轨迹和初始位置的建图测试，并在RVIZ可视化界面[19]中实时反馈、记录建图效果。现实复杂环境如图9所示。

图9 现实环境

测试结果表明：三种SLAM算法都能成功构建地图，如图10所示。机器人在真实室内复杂环境构建地图的效果比仿真环境下略差，这是由于：激光雷达传感器扫描地图时存在机器人位置和噪声的估计误差。

图10 真实环境下建图效果

尽管现实环境与虚拟环境之间存在差异，但现实环境与仿真环境获得的结果都遵循前面分析算法的特性。Hector SLAM在真实环境中构建的地图与模拟环境地图具有相似特性，较快速度下均产生地图漂移，Gmapping建图效果相比Hector SLAM较优，虽然未发生严重的地图偏移，但是非常依赖里程计信息，其间接地影响了建图精度，对复杂的大场景物体特征细节处理不足。Cartographer在测试结果中表现最优，比其余两种算法更少地受到传感器噪声影响，这是因为Cartographer在室内复杂的大环境下建图采用了先创建子地图，并通过后端回环检测降低了子地图的误差，利用分支界定法提高求解子地图与全局地图匹配问题的效率，最终获得构建地图的最优解。

除了对建图误差分析外，本文还将每种算法对计算机的负载进行了实验分析，以此来验证三种算法的空间复杂度。实体样机配备了英特尔G4560型号CPU、8GBDDR4内存，实验结果如图11、表2所示。

图11 三种SLAM算法的CPU占存随时间变化

表2 三种SLAM算法CPU占存的平均值标准差σ

通过对比可知：在相同实验环境下，将CPU占存作为评价指标时，Gmapping算法对系统负载最高，其次是Cartographer算法和Hector SLAM算法，这与前面对三种算法分析的结论相符合。从上述实验结果可知：无论从建图效果还是空间复杂度来评价三种算法，基于Delta3A激光雷达传感器的Cartographer算法表现最佳。

4.3 路径规划实验分析

4.3.1 复杂环境的全局路径规划

基于Cartographer算法构建的先验地图，进行复杂环境下移动式机器人的全局路径规划实验。将先验地图导入Spark-T机器人系统，在其操作系统中对障碍物会产生安全膨胀边界，该边界可起到保护机器人的作用。当机器人确定前进目标点后，融合A-star算法在其当前位置与目标位置间会由全局路径规划器生成一条最佳路径，机器人根据该路径进行移动，可完成全局导航任务，如图12所示。

图12 基于先验地图的全局路径规划

4.3.2 复杂环境下的局部路径规划

为测试复杂环境地图下的机器人局部路径规划能力，即局部避障。当机器人开始循迹全局路径时，在其移动路线上摆放动态障碍物。当机器人遇到移动障碍物时，局部路径规划器重新规划前进路径，实现机器人在复杂环境下的实时避障动作，完成局部路径规划，在绕过障碍物后继续向目标点运动，如图13所示。

图13 基于先验地图的局部路径规划

通过上述实验分析可知：基于Cartographer算法构建的先验地图，采用A-star算法为全局路径规划算法、DWA算法为局部路径规划算法，可实现移动式机器人在室内复杂环境下的高精度导航定位。

5 结论

本文分别通过计算机仿真和实验测试，分析了三种代表性的二维SLAM算法。仿真和实验结果表明：采用Cartographer算法可有效解决室内复杂场景的实时建图与定位累积误差问题，且该算法具备高时效、低功耗等优势。基于该优质先验地图，融合A-star和DWA路径规划算法，可有效提升移动式机器人的导航精度。

本文采用的Delta3A单线激光雷达具有结构简单、功耗低、成本低等优点，但其局限于平面式扫描；多线激光雷达可多维度检测物体特征，但成本偏高。本文下一步工作将融合惯性测量单元信息，提高单线激光雷达在多维度对物体信息感知的性能，进一步改善复杂场景下室内移动式机器人的SLAM系统。