Page 61 - 《软件学报》2020年第9期

2682 Journal of Software 软件学报 Vol.31, No.9, September 2020

旋转和平移的位姿估计.在位姿融合中,因为 IMU 和里程计的采集速率比激光雷达传感器高很多,因而在一定时
间段内可能没有激光扫描匹配估计的位姿数据,这时需要通过 IMU 和里程计数据进行推断;同时,因为这两个传
感器存在相对激光扫描较大的累积误差,所以在有估计位姿的时候对两者进行一些更新和初始化消除累积误
差;同时,在估计位姿的旋转量时,可以在没有 IMU 数据时利用之前的位姿或者里程计估算出的速度进行模拟,
在一定程度上可以保持位姿数据的稳定性.本方法将所有数据进行缓存,放在双端队列中,便于新数据的添加和
旧数据的删除.算法不断轮询处理来自扫描匹配器优化的位姿、IMU 数据以及里程计数据.在获取最后估计位
姿以及处理 IMU 和里程计两种传感器数据期间,位姿的变化由 3 个跟踪器表示,基于 IMU 数据,对这 3 个跟踪
器进行更新.最后获取融合完成的位姿,提供给扫描匹配器作为初值.
对各数据源的数据进行了严格的时间对齐后,能够为后续的非线性优化提供一个较为准确的估计初值.同
时,在使用该位姿融合方法后,需要在算法外部配合激光雷达的传感器发布速率,同时添加一个发布局部位姿优
化的模块,将局部优化的位姿类似于传感器数据一样作为一个外部数据源.
1.4 基于回环检测的全局优化
由于扫描仅与时间相近的子图匹配,其包含扫描数量少,局部扫描匹配方法会逐渐累积误差.在短时间的连
续扫描中,累积的误差很小,在局部便可进行优化;而长期建图累积的误差,则需要通过检测回环通过稀疏位姿
调整 [15] 完成全局优化来消除.
在算法中,被插入扫描的相对位姿存储在内存中,用于在回环闭合时进行优化.扫描匹配器一直在后端运
行,如果找到一个高于一定阈值的匹配,就将相应的相对位姿添加到优化约束中.与扫描匹配一样,全局优化也
被转化为一个非线性最小二乘问题,这样,在有其他数据时能够较容易地添加残差,残差即约束与真实值之差.
约束被添加后,每隔一段时间,通过优化计算全局位姿最优解.
在回环检测中,最优的匹配是精确到像素的匹配.原始算法以一定的角度步长遍历搜索窗口内的每个像素,
但在目前的搜索量级下,运行速度非常慢.
Cartographer 中采用基于 DFS(depth first search)的分支定界法进行回环的检测,这种方法首次在混合整数
线性规划中提出,在 Land 和 Clausen 的论文中 [16,17] 有相关概述.该方法的主要思想是:将可能性子集表示为一棵
树中的节点,其中:根节点表示所有可能的解,每个节点的子节点是其父节点的分区,一起表示同一组的可能性;
叶节点表示单独个体,每个表示一个可行解.只要内部节点 c 的得分是其所有子节点的得分上限,这样的算法得
到的解与原始算法相同.在这种情况下,每当节点有界时,在该子树中不存在比目前已知解更好的解.算法获得
目标解的效率取决于搜索树被修剪的部分是否占大多数,而这又依赖于一个好的上界以及一个好的当前解,
DFS 可以快速评估大量叶节点获得较好的当前解来降低计算量.同时,因为不希望将较差的匹配作为闭环的约
束,在匹配中引入一个分数阈值,低于这个阈值则不将其作为解.引入分数阈值后,在一定程度上减少了对节点
选择以及寻找启发式解的依赖.DFS 算法中,访问一个节点的子节点顺序是先计算每个子节点的上界,然后访问
具有最大上界也就是最有可能的子节点.

2 基于扫地机器人 Player 平台的 SLAM 系统实现

本节中,在扫地机器人 Player 平台 [18,19] 上实现了基于增强 Cartographer 算法的 SLAM 系统.整个系统分为
系统配置、Player 传感器数据交互、接口通信、数据管理及分发、数据处理以及增强的 Cartographer 算法这 6
个模块.

2.1 基于增强Cartographer算法的SLAM系统设计
2.1.1 基于 Player 平台的 SLAM 整体设计
Cartographer 中原有的接口是基于 ROS [20] 的,ROS 可以提供硬件抽象、设备驱动程序、库、可视化器、消
息传递以及包管理等功能.但因为 ROS 集成各种功能,它显得过于庞大而冗余,无法部署在成本较低的嵌入式设
备上.而本文中所使用的 Player 平台是一个轻量级的中间件,具有小而快的优点,同时最大化实用性和适用性,能