全面解析谷歌Cartographer算法_概要

目前国内大多数AGV厂家使用的是2D激光SLAM算法，但是前段时间交流的过程中，发现他们几乎没有人整明白谷歌的这套开源代码，这也是合情合理，毕竟已经是自顾不暇了（对于只参加过社招的我来说，已经到达了巅峰）。所以我写博客的原因主要有两点：1.现在各个AGV公司也都有自己的SLAM算法，如何让算法更具鲁棒性是当前的主要目标，所以希望能在这里和大家有更多思维上的碰撞；2.这套代码的优化方法，从原来的EKF，演进到现在的非线性优化，已经是激光SLAM中state-of-the-art的算法了，且上次更新还停留在去年对于landmark所做的调整,版本也比较稳定。

谷歌这套算法包含cartographer、cartographer_ros两部分，cartographer_ros可以理解为算法的一层外壳，调用cartographer的函数来实现，并传输submap信息，在ROS上可视化。所以大可不必在意cartographer_ros,从算法层面，它唯一做的就是对点云数据做了初始化：1.将只有距离信息的点云数据转换到世界坐标系的3D坐标；2.将点云坐标从雷达坐标系转换到机器人坐标系，同时包含每个点云及当前帧点云的时间信息。那么从算法上来看，它的特点就是以构建子图的方式构图，及scantomap的匹配方式。开始构图的时候新建一条轨迹，从一个连续的多帧传感器数据中抽取若干关键帧，也就是我们的Node，一条轨迹有一串Node组成，并且每个Node相对于它所在的submap的相对坐标变换要已知，每个子图相对于世界坐标系的位姿要估计出来，最后将不同的submap及其Node的位姿变换矩阵连在一起做全局优化。

 

我将大致分为以下几点进行解析：

1.数据初始化

单激光雷达、双/多激光雷达时的数据初始化过程，同时雷达间数据读取频率的注意事项：不当会导致之后传感器时间同步时将正确的点云信息过滤掉。

https://blog.csdn.net/qq_25646217/article/details/108042904

2.多传感器融合及点云运动畸变去除

IMU、里程计的解算，与估计出来的pose所解算出的线速度进行融合，构成位姿外插计，对点云进行运动畸变滤除。（谷歌这套代码是用于自动驾驶的，对于目前行动速度不快的AGV来说有点大材小用了）

3.时间同步

一开始我们用的是15Hz的激光雷达，远远赶不上算法处理一帧点云的速度，后来不得以对点云时间做了修改，原则上讲，只影响点云的运动畸变滤除，不过由于是低速AGV，影响几乎没有。如果是多激光雷达，那么时间同步是必须重视的。

4.概率地图的绘制及更新策略

概率直接查表，提高了计算效率，及概率更新方式，我们可以设置更新的快慢，即激光集中的点为黑色，直观来看是中间穿行区域由灰变白的速度。

5.scan_match匹配算法

real_time_correlative scan_match通过暴力枚举，生成数组经过旋转的点云，再挨个遍历x,y方向上的偏移量，将各种偏移量情况下的点云进行匹配，每个点云的概率值即为得分。最后将得到的最优的位姿作为初始位姿丢到ceres的优化库进行优化。

6.分支定界和fast_correlative scan_match算法

这二者是一起使用的，由于搜索窗口比real_time大数十倍以上，故必须使用分支定界，并进行剪枝。

7.栅格地图的绘制过程

通过旋转画笔坐标系绘制各个submap，最终画出全图。

 

目前能想到的就这些啦，待更~~     也欢迎大家提出想要了解的点。

这段时间听到的让我印象很深刻的话：任何事情说以后做就是不做。共勉之，尽早结束这部分也算是对一年的付出有个交代了。