融合反光柱的2D 激光SLAM 和高精度定位系统

时间：2024-05-04

周凯月，张建伟

（四川大学视觉合成图形图像技术重点学科实验室，成都 610041）

0 引言

现在，机器人已经广泛应用在工生产和家居生活等环境中，例如工业生产中的AGV 和帮助家庭洁净的扫地机等。机器人在未知环境中自主导航，首先需要依靠自身携带的传感器构建环境的地图。目前，研究人员提出了许多基于激光传感器的2D SLAM 建图系统，如 GMapping[1]、Hector SLAM[2]和 Cartographer[3]等。基于已经重建好的地图，定位系统在环境中的位姿（此时只定位，不建图）称为纯定位。在过去的几十年中，研究人员提出了许多基于滤波的纯定位方法，如AMCL[4]、NDT-MCL[5]和 Kalman Filter[6]等。在这些系统中，采用KLD 方法的AMCL 被广泛使用。上述基于激光的2D SLAM 建图和定位系统，在特征丰富的场景中，可以稳定地工作，但是，在几何特征较少或者玻璃等特殊环境中，往往不能取得让人满意的效果。

为了提高2D 激光SLAM 系统的鲁棒性和精度，我们提出了一个新的融合反光柱的2D 激光SLAM 和纯定位。我们的系统建立在2D 激光SLAM 系统Cartographer[3]之上，我们采用TSDF 地图表示重建的2D 空间[7]。在建图时，我们在获取的激光点中，根据反光柱的反射率分离反光柱上的点，并且将反光柱作为位姿图中的节点，在位姿图中和姿态节点以及激光关键帧节点一起优化。在定位时，不同于传统的基于滤波的方法，我们基于系统生成的TSDF 栅格图，采用联合优化激光匹配误差和反光柱匹配误差的方式做纯定位。对于系统输入的每一帧激光帧，我们首先根据反光柱的反射率分离激光帧中的反光柱点，并且将反光柱点聚类得到每个反光柱的中心点坐标，然后，我们采用最近邻算法将聚类得到的反光柱点和地图中的反光柱节点做关联，最后通过联合优化反光柱的匹配误差和激光点在地图中的匹配误差计算激光帧的位姿。实验表明，建图时，添加反光柱可以大幅度增强系统在几何特征少或者玻璃等场景中定位的稳定性和精度；同时，在系统做纯定位时，融合反光柱信息可以大幅度提高系统定位的精度。

1 相关研究

通常SLAM 系统用来构建未知环境的地图，如果机器人工作在已经建好图的环境中，这时只需要纯定位模块，纯定位模块作用是根据事先重建好的地图，定位机器人在环境中的位姿和姿态。激光SLAM 系统通常通过将当前帧和地图进行配准[2-3，7]，当前帧和地图配准又称为Scan-to-Map；或者将当前帧和上一帧配准的方式（Scan-to-Scan）计算激光帧的位姿[8]。目前，现有的2D 激光建图系统通常采用概率地图表示构建的环境[1-3]。Gmapping[1]采用粒子滤波的方式构建环境的地图，粒子滤波系统根据其他传感器（如里程计、IMU 等）的观测产生可能的粒子状态，并且通过匹配获取的激光点和2D 地图评价预测的粒子状态，而从减小系统的定位和建图误差。Hector SLAM[3]和Cartographer[2]通过Scan-to-Map 的方式，通过优化激光点云在地图匹配误差计算激光帧位姿。为了增大定位优化的收敛范围，Hector SLAM 构建多分辨率地图，并且采用多层金字塔逐级优化。Cartographer[2]在建图时构建子图，并且采用分支定界的方式计算激光帧和子图之间的闭环，并且采用位子图优化减小系统的累计误差。PL-ICP[8]是ICP 算法的一个变种，PL-ICP 算法首先在两个激光帧找匹配点，然后通过优化匹配点的点到线距离计算位姿。

目前，栅格图被广泛应用在2D 机器人定位和导航中，在栅格图中，系统重建的环境由固定大小的方格表示，每个方格存储的值表示方格是障碍物的可能性。当前，基于栅格图和AMCL 算法的定位系统是最常见的2D 定位系统，我们构建SDF 地图，并且采用由SDF地图生成的栅格图做纯定位，近年来SDF 地图在基于RGB-D 的实时重建系统中被广泛采用[15-16]。不同于传统纯定位系统中基于概率格子地图生成的栅格图，通过SDF 地图生成的栅格图，每个格子点保存的是格子到障碍物的距离。

通常，基于栅格图和AMCL 的纯定位系统，重复定位精度在2～5cm，Rowekamper 等人[14]提出的全局定位基于AMCL，局部到点定位基于PL-ICP 的算法，局部到点精度依靠PL-ICP 算法可以达到5mm。我们融合反光柱的定位系统，在玻璃等场景下，也可以达到5mm的定位效果。接下来，在第二节中，我们首先介绍融合反光柱的2D 激光SLAM 系统，然后再第三节中介绍融合反光柱的纯定位系统。

2 融合反光柱的2D SLAM系统

我们的SLAM 和纯定位系统建立在Cartographer基础上，Cartographer 实现了 SDF 建图方法[17]，我们采用Cartographer SDF 地图表示形式。我们融合反光柱的激光SLAM 系统框图如图1 所示。

图1 融合反光柱的激光SLAM系统框图

对于输入的激光帧，我们首先根据激光点的反射强度分离反光柱上的激光点，通常，反光柱上返回的激光点的反射强度比普通障碍物高很多，我们首先将激光点强度大于一定阈值（对于R2000，阈值设为1000）的激光点分离出来，并且插入到待处理的队列Q1中，因为建图系统插入到后端位姿图中的激光帧节点是选择性插入的（当机器人位移、角度或者时间大于一定阈值才插入新的节点），我们根据反光柱的时间戳，和位姿图中激光节点的时间戳，判断待处理的反光柱队列中的反光柱，在激光节点中是否存在前后节点，如果存在前后节点，则根据反光柱的时间戳和前后激光节点时间戳，以及激光节点的位姿插值得到反光柱点的位置，并且将插值得到位置的反光柱点插入到队列Q2中。

在构建地图时，一帧激光帧可能可以观测到多个反光柱，我们假设在摆放反光柱时，反光柱之间的距离大于一定阈值，实验中大于1m。我们将队列Q2中的反光柱点聚类，聚类得到每个反光柱上返回的点，聚类做法如下。假设聚类得到的各反光柱上的点为{Ci}，各反光柱的中心点坐标为{Mi}，我们依次处理Q2中的点，判断当前处理的点和已经聚类点中心之间的距离，如果距离大于一定阈值(1m)，则将新处理的点初始化新的聚类点，如果当前处理的点和其中一个聚类中心Mi距离小于阈值，则将当前处理的点插入到Ci中，并且重新计算类的中心点坐标Ci，如此得到聚类后的各反光柱的中心点坐标点。

聚类得到各反光柱上点Ci以及各反光柱中心点坐标Mi后，因为激光点在障碍物深度不连续的地方可能产生外点，我们采用迭代聚类的方式去除外点，重新计算每个聚类的中心坐标Mi，方法如下：

（1）对于聚类Ci中的点，假设所有点均为内点，我们计算聚类Ci点的中心Mi。

（2）对于所有的内点计算坐标的均值μ和方差Σ。

（4）将高斯方程值小于阈值的点从内点中移除。

（5）对于 2-5 步，重复执行 3-5 次，直到 Σ 中所有值小于10-4，或者Σ 的逆变得不稳定，或者内点的数量小于4。

（6）最后，对于每个聚类Ci，计算点的中心点作为反光柱的最终位置。

聚类得到每个反光柱的中心点Mi后，聚类得到的反光柱在位姿图中已有反光柱，按照距离找匹配，对于找到匹配的反光柱，将当前帧计算得到的反光柱中心作为一个的观测；如果新聚类得到的反光柱在地图中找不到匹配，则根据聚类得到的反光柱中心点，在位姿图中初始化一个新的反光柱节点，位姿图如图2 所示，其中黑色圆圈表示激光帧节点，红色圆圈表示子图节点，五角星表示反光柱节点，黑色方框表示激光帧对于反光柱的观测。

图2 融合反光柱的激光SLAM系统位姿图示意图

3 融合反光柱的纯定位方法

在纯定位模式，我们采用联合优化激光点在地图中匹配误差和反光柱匹配误差计算位姿，我们采用由TSDF 地图生成的栅格图作为纯定位的地图，由SDF 地图生成的栅格图，每个格子点保存了格子到障碍物的距离为F(ci)，我们还同时保存了建图时SDF 融合的权重W(ci)，我们联合优化的目标函数表达式如下：

其中，di为当前帧激光点位置，Rj为激光帧聚类得到的反光柱中心点，P为待优化的激光帧位姿，Rj为根据估计的当前帧位姿计算的反光柱位置，Mj为地图中匹配的反光柱节点位置。

在纯定位模式，对于输入的每帧激光帧，同章节2中的建图流程，首先根据激光点的反射强度提取反光柱点，提取的反光柱点，根据上一帧位姿（或者融合里程计、IMU 等传感器数据）预测的位姿，计算反光柱点在地图中的位置，然后根据计算的位置，在地图中选择最近的反光柱节点作为匹配点。

4 实验

我们首先基于仿真数据测试了融合反光柱的建图系统，使用Robot Operating System（ROS）可视化得到的建图结果如图3 所示。图中不同颜色的圆圈表示的是场景中的反光柱，白色部分为空白区域，灰色部分为有物体的遮挡区域，蓝色线条为机器人运动轨迹。从图中可以看出，我们的建图系统工作良好，系统准确地构建出了反光柱在地图中的位置。

图3 融合反光柱SLAM系统仿真建图结果

我们还测试了融合反光柱的纯定位系统，首先在真实环境中布置反光柱，然后通过融合反光柱的建图系统重建带有反光柱的2D 地图，建图结果如图4 中的（a）所示，因为构建的环境右上角存在玻璃，系统构建的地图右上角SDF 地图杂乱，在此区域的定位效果较差，融入反光柱以后因为有了额外的约束，会显著提升此区域的定位结果。

图4 融合反光柱的真实场景建图结果

图5 重复定位精度对比

基于建图测试的机器人重复定位精度如图4 所示。重复定位首先在建图结果中指定两点，然后让机器人从一点移动到另一点，通过到达指定点的误差来判断重复定位精度，图中是重复测量30 次重复定位机器人到达终点的落点分布，尺度为mm，可以看到，如图5（a）所示，在没有融合反光柱的情况下，机器人的重复定位落点误差在20mm 的范围内，在融合了反光柱以后（图5（b）），重复定位落点误差缩小到了5mm。