Real-time correlative scan matching 论文算法分析（读懂Cartographer）

---

---

背景

　　读这篇论文的初衷在于希望能够读懂 Google’s Cartographer（开源激光SLAM算法），这篇文章是 Cartographer 的论文 《Real-Time Loop Closure in 2D LIDAR SLAM》的引用文献之一，也是代码实现里 real time correlative scan matcher 和 fast correlative scan matcher 的算法来源（两者分别用在了前段和后端的算法中），总之是读懂 Cartographer 的关键论文之一，故这里写下我读这篇论文的一些理解。

算法

有一些基础的概念这里是默认读者是能理解的
1. Scan Matching —— 求解两帧雷达数据（scan to scan）或者雷达数据与地图（scan to map）之间的旋转矩阵（位移和旋转）的过程；
2. Maximum likelihood estiamtion (极大似然估计) —— 利用已知样本结果，在使用某个模型的基础上，反推最有可能导致这样的结果的参数。这是一个相对生涩的解释，有兴趣的读者可以自行搜索相关的博客或文章来看，完整的看下来会比较容易理解。

基础模型

　　我们这里用极大似然估计的思路来给 Scan Matching 建模，这里我们采用的是 Cartographer 中的 Scan-to-Map 的模式：

x∗t=argmaxxt{p(zt|xt,mt−1)p(xt|xt−1,ut−1)}(CS)(CS)xt∗=argmaxxt{p(zt|xt,mt−1)p(xt|xt−1,ut−1)}

其中， xtxt 是上一时刻到这一时刻的运动量（或者运动相关的观测量，如里程计数据、IMU数据等）。
那么，这个式子的比较通俗的解释（可能跟式子中的含义并非完全相同）即：

在上一时刻 t−1t−1

算法简化

　　首先，并不是所有的机器人都可以获取运动信息（odom、 imu），也就是模型里面的 ut−1ut−1 并不是在所有应用场景都存在，或者说这个数据的可信度有时候很低，我们并不一定使用这个观测量，在这样的情况下我们省略掉 （CS）式中第二个概率计算式（通常称作运动模型），而专注于第一个概率计算式（通常称作观测模型）。

　　接着，针对这个观测模型：

p(zt|xt,mt−1)(Observation)(Observation)p(zt|xt,mt−1)

上的所有点，这样建模要相对简单的多（后面我们也会说明这样的简化并不是使定位或者简图的效果变差）。

核心

　　好了，经过上面的简化，我们的算法已经相对要简单多了，下面就是这个算法的核心：
　　首先，在我们的直观感受里，ztzt
　　上面的式子将整个一帧观测数据的概率拆分成了当前帧的数据的每一个点的概率，在计算这个概率之前，我们需要对数据帧以及地图做一些处理——栅格化。
　　对 ROS 比较熟悉的读者会知道 ROS 里面有一种常用的 2d 地图表达形式，就是占据栅格图（occupancy grid map），就是利用了栅格化的思路，原理很简单，如下图是一帧scan数据的一小部分，我们将整个观测平面切分成固定宽度的方格，我们可以类比图片，每个方格都是一个像素，每个位置有障碍物的概率对应该像素的灰度值。概率越高值越则该像素越黑，说明该位置很有可能有障碍物；相反概率越低越白，也就意味着该位置很可能是空的；未知区域的灰度为128。

　　到了最后一步，scan to map 的 scan matching 问题。如下图（我不知道用什么工具可以快速画出上面图中的效果，所以只好手绘拍下来，有更好办法的读者希望可以留言告知），我们假设已经有的地图为 Figure.1Figure.1，我们很容易就能判断出当前的机器人位置（较上一时刻前进了３个像素），不过我们需要用算法实现这个判断。

• 现在设 ztzt 集合中所有坐标上概率和，找到那个概率和最高的就是我们要找的当前机器人的位姿。

Cartographer 中的实现思路大概如上，详细可以看Cartographer代码中的 real_time_correlative_scan_matcher 的实现。不过论文中提供了几个思路的比较，如BF, 2D Slices 等，不过重点在于一个分辨率切换的思路—— multi-resolution:

• 首先选择一个相对低的分辨率对 map 和 scan 进行栅格化，每个栅格上保留高分辨率情况下的该区域的最高概率，这样在匹配时就不会错过正确的位姿估计
• 低分辨率找到最佳匹配后再在该位置进行高分辨率的栅格化，并设定相对较小的 search window( translation & rotation)进行计算。

这个思路在保证不错过最佳匹配的同时明显减少了计算量，使得scan matching可以达到real time的效果。

后记

　　Cartographer中有两个scan matcher都涉及到了这个论文里的类似算法，包括前面提高的real time correlative scan matcher, 而真正实现了上面的multi-resolution的思路的是fast correlative scan matcher，有兴趣的读者可以去阅读一下源码，代码量不大，也不难懂。