一起自学SLAM算法：8.2 Cartographer算法_cartographer算法原理

连载文章，长期更新，欢迎关注：

---

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统

        8.1 Gmapping算法

        8.2 Cartographer算法

        8.3 LOAM算法

第9章-视觉SLAM系统

第10章-其他SLAM系统

第11章-自主导航中的数学基础

第12章-典型自主导航系统

第13章-机器人SLAM导航综合实战

Gmapping代码实现相对简洁，非常适合初学者入门学习。但是Gmapping属于基于滤波方法的SLAM系统，明显的缺点是无法构建大规模的地图，这一点已经在第7章中讨论过了。而基于优化的方法则可以构建大规模的地图，基于优化的方法实现的激光SLAM算法也有很多，比如Hector、Karto、Cartographer等。而Cartographer是其中获好评最多的算法，Cartographer是这几种算法中提出时间最新、开发团队来自著名的谷歌公司、代码的工程稳定性较高、少有的建图和重定位兼具的算法。所以，下面将从原理分析、源码解读和安装与运行这3个方面展开讲解Cartographer算法。

8.2.1 Cartographer原理分析

其实基于优化方法的激光SLAM已经不是一个新研究领域了，谷歌的Cartographer算法主要是在提高建图精度和提高后端优化效率方面做了创新。当然Cartographer算法在工程应用上的创新也很有价值，Cartographer算法最初是为谷歌的背包设计的建图算法，谷歌的背包是一个搭载了水平单线激光雷达、垂直单线激光雷达和IMU的装置，用户只要背上背包行走就能将环境地图扫描出来。由于背包是背在人身上的，最开始Cartographer算法是只支持激光雷达和IMU建图的，后来为了适应移动机器人的需求，将轮式里程计、GPS、环境已知信标也加入到算法，也就是说Cartographer算法是一个支持多激光雷达、IMU、轮式里程计、GPS、环境已知信标的传感器融合建图算法。下面将结合Cartographer算法的核心论文[3]对Cartographer的原理展开分析。

基于优化方法的SLAM系统通常采用前端局部建图、闭环检测和后端全局优化这种经典框架，如图8-8所示。

图8-8  基于优化的SLAM经典框架

1.局部建图

局部建图就是利用传感器扫描数据构建局部地图的过程，在第7章中已经介绍过，机器人位姿点、观测数据和地图之间通过约束量建立联系。如果在机器人位姿准确的情况下，可以把观测到的路标直接添加进地图。由于从机器人运动预测模型得到的机器人位姿存在误差，所以需要先用观测数据对这个预测位姿做进一步更新，以更新后的机器人位姿为基准来将对应的观测加入地图。用观测数据对这个预测位姿做进一步更新，主要有下面几种方法：

• Scan-to-scan matching
• Scan-to-map matching
• Pixel-accurate scan matching

最简单的更新方法，就是Scan-to-scan matching方法。由于机器人相邻两个位姿对应的雷达扫描轮廓存在较大的关联性，在预测位姿附近范围内将当前帧雷达数据与前一帧雷达数据进行匹配，以匹配位姿为机器人位姿的更新量。但是，单帧雷达数据包含的信息太少了，直接拿相邻两帧帧雷达数据做匹配更新会引入较大误差，并且雷达数据更新很快，这将导致机器人位姿的误差快速累积。

而Scan-to-map matching方法则不同，其采用当前帧雷达数据与已构建出的地图做匹配。由于已构建出的地图信息量相对丰富稳定，所以并不会导致机器人位姿的误差累积过快的问题，如图8-9所示。Cartographer的局部建图就是采用这种方法，也称为局部优化。

而Pixel-accurate scan matching方法，其匹配窗口内的搜索粒度更精细，这样能得到精度更高的位姿，缺点是计算代价太大，后面将讲到的Cartographer闭环检测采用的就是这种方法。当然，关于闭环检测也有很多别的方法，比如extracted features matching方法、histogram-based matching方法、machine learning方法等，感兴趣的读者可以查阅相关资料。

图8-9  scan-to-scan与scan-to-map对比

在介绍Cartographer局部建图的具体过程之前，需要先了解一下Cartographer地图的组成形式。Cartographer采用局部子图（submap）来组织整个地图，其中若干个激光雷达扫描帧（scan）构成一个submap，然后所有的submap构成全局地图（submaps），如图8-10所示。

图8-10  Cartographer地图结构