Cartographer论文精读-Real-Time Loop Closure in 2D LIDAR SLAM

Real-Time Loop Closure in 2D LIDAR SLAM
这篇论文是16年5月发表的，虽然时间比较久，但是很经典， 对于了解整个cartographer代码有很大帮助。第一次看这个论文的时候觉得很抽象，所以学习的过程 也是一个不断填坑的过程。如果有理解不到位的地方，欢迎评论区留言哦~

摘要：

摘要中 提到LIDAR传感器和SLAM方法是 获取 平面 竣工图的有效方法。提取出四个重点就是：
①构建便携式平台-在有限的计算资源下运行；
②构建分辨率为5cm的栅格地图，并进行实时构图和 闭合回路；
③为了达到实时效果，采用分枝定界进行加速。
④实验表明，本文方法具有竞争力。

Introduction：

建议参看cartographer官网流程图：
https://google-cartographer.readthedocs.io/en/latest/

传感器输入
①激光雷达数据， 经过网格/体素滤波，就是2D和3D的区别，（size是体素的长度，fixed size是指 这一次的体素滤波是边长固定的）-再经过自适应体素滤波（是因为需要根据配置文件设置options）这样两次滤波的目的是较少一部分的数据量。

②里程计和IMU数据，这个IMU数据，经过imu tracker（计算重力加速度的方向-比如在论文中的使用的背包进行测量-需要求解roll和pitch） 之后和里程计的数据进行融合， 再结合local slam中的scan matching 一起来实现位姿的插值（pose extrapolator保留一段时间的位姿用来计算线速度和角速度，然后用计算得到的线速度和角速度来位姿插值-去除运动的畸变）

local部分（也就是前端）
③接收处理后的输入数据，对激光数据进行scan matching ，scan matching通过匹配lidar数据得到一个位姿，将当前帧激光插入到子图当中。如果传感器没有运动，是静止状态，那这一帧数据需要去除）这样一帧一帧的激光数据处理，我们不断更新得到子图。

global SLAM（后端）
这部分主要是一个回环检测，后端优化，在这其中提到了加速的方法采用的分枝定界法。

tips：
前端Local部分就是构建一系列的栅格子图，然后新的激光scan通过CSM的方法插入子图的最佳位置；后端-构建回环来消除子图之间匹配的误差，其实就是一个优化问题。

Related work**

在这部分，这要提到几种常用的扫描匹配的方法，以及他们的优缺点
①针对scan to scan ，激光帧之间的匹配，常用的方法是ICP PL-ICP，方法，但是这两种方法容易陷入局部极值，如果针对大场景的时候，计算的相对位姿 误差累计会很明显。

②而本文采用的是scan to map matching可以限制减小误差的积累，采用CSM的方法（相关匹配-其实也是一个暴力搜索，但是它构建的是似然场模型，不会陷入极值，这个CSM可以理解为在较粗糙的层面下进行的，之后需要再加一个梯度优化进行精细的搜索，如果采用G-N的话，寻找线性插值映射上的局部最优解。需要一个良好的初值，可以采用起始位置用高频率的激光数据，或者3D环境，初始位姿用IMU给出）—这个CSM也可以理解为似然场模型-对图像进行高斯平滑-对障碍物膨胀（离障碍物越远，得分越低）-得分与障碍物有关，是连续的，不需要期望值，每一个地图点都有得分，查表-得分高为障碍物。同时适合结构化与非结构化环境。（结合后面提到的栅格地图占用的概率以及hit miss概率）

③Pixel-accurate scan matching（像素精确匹配）可以减少误差的积累，但是代价是庞大的计算量；

ALL以上说的是几种激光数据的扫描匹配方法，接下来说常用的两种处理剩余局部误差累积的方法：滤波方法与图优化；

①就粒子滤波来说，对于基于网格的SLAM，随着地图变得越来越大，这很快就会成为资源密集型的；滤波器-粒子滤波-Gmapping-粒子衰减严重-价值不大-当环境增大，前面出现问题的概率增大，只估计当前的位姿，不对之前的位姿修正。在实际应用中，可以把粒子数目降低一些来降低内存。但是针对定位滤波器更有优势，只关注定位结果，但是建图更依赖图优化

②基于图优化的方法，就是通过构建节点与边之间的约束。找到一个最优配置。
之前的一系列介绍，都在引出Google cartographer

SYSTEM OVERVIEW

这部分提到了一个软实时约束， 可以理解为非真正的实时约束，他的效率是跟不上。也可以理解为lazy decision-晚一点做决策，降低回环时候的错误率。-针对大环境我们在线跑cartographer的时候会发现他的约束格网是跟不上我们的行进速度的，我们更关注后处理的轨迹优化，所以会把一些参数设的更大，实时只是出一个初始pose，离线跑bag的时候，会发现数据跑完之后还会有一个相当长时间的optimization优化。同时这里还提到了一个预计算网格。

**

LOCAL 2D SLAM（局部）

**
A:SCAN:
子图的构成，就是一帧一帧激光数据扫描匹配 对齐的 迭代过程，每一帧的激光数据转换到子图中，其实是旋转加一个平移。

B:SUBMAPS:

几个连续的激光scan，构成一个子图，这些子图是栅格地图M， 其中离散的栅格地图可以理解为分辨率 本文采用的是5cm的分辨率 ， 那么2d的格子图，大小是5cm*5cm，每一个格子都有一个自己的概率值，（pmin pmax）（在概率最小到最大的空间中），当一个栅格的概率值小于pmin时，表示这个点没有障碍物，在两者概率之间表示不确定，大于pmax表示该点是障碍物。一般位置环境设置为0.5，然后不断进行更新。

上面这个公式（2），其实是一个简化-采用贝叶斯对地图进行估计。clamp是一个区间限定函数。

这个部分还涉及一个cell,可以理解为多个栅格地图组成一个cell,cell要比栅格大。

预计算网格：
当激光打在栅格地图上的时候，被击中的方格，进行hit+1（就是激光点打在建筑物上的概率）；hit的那个点到激光原点之间的格子进行miss+1。对于之前没有hit或者miss的点，我们为其分配一个概率phit pmiss.每当有一帧激光打在网格上时，会重新计算hit与miss的值。

C Ceres scan matching

在将每一帧scan插入submap之前，使用 基于ceres的scan matching对位姿进行优化。 这个scan matching负责找到一个扫描位姿，使submap中扫描点scan的概率最大化。这个问题转化为一个非线性最小二乘问题。

**

CLOSING LOOP 回环检测

回环检测目的简单来说，就是我们通过在大场景中创建小子图来抑制误差的累计，短时间内，连续的激光数据组成的子图误差累计的很小，但是不同子图之间要想消除误差，需要进行回环检测来优化位姿。本质上可以理解为将位姿之间的差值转换为最小二乘来求解。这里面提到一个SPA稀疏姿态调整公式，可以参考一个论文具体了解
**

A. Optimization problem（优化问题）

其中上式中，表示子图的一系列位姿、scan的位姿（这样的位姿是建立在世界坐标系下的，两者之间的约束，用协方差来表示） ，第四个参数参数是两者之间的相对位姿。e是计算他们之间的残差。

原文中提到一个损失函数：鲁棒性回归的损失函数。

（公式摘自链接：https://blog.****.net/hero_cjn/article/details/81068311?ops_request_misc=&request_id=&biz_id=102&utm_source=distribute.pc_search_result.none-task-blog-SOBAIDUWEB-0）

这个a表示为残差，可以表示为观测值与预测值之间的差值，如果这个残差很小的话，进行平方操作；如果残差很大的话，直接加入到系统当中，会把整个系统引导到错误的方向，所以进行线性化的操作，这样就避免为系统引入较大的残差项。 在实际应用中，主要是针对对称环境中，减小错误匹配带来的问题。 比如走廊中的白墙，特征点很少，很容易错误匹配。

Branch-and-bound scan matching（分枝定界加速问题）

左侧可以理解为信度和，是将当前帧scan插入到submap中，遍历所有位姿得到的最高分，这个值越高，表示越匹配，我们的目的就是要找到这个分值最高的数值。这样导致的计算量是巨大的，所以在实际中用的是分支上界法

CSM-枚举位姿，在局部势场中，存在很多局部极值，采用枚举，可以减少错误匹配，同时采用加速策略。构造似然场（其实是高斯模糊的过程）、根据机器人在各个方向的运动速度（dx dy dtheta-比如在1m*1m空间内，角度为30°）即在指定的搜索空间内，进行搜索，计算每一个位姿的得分（选出最优位姿）、根据位姿匹配的得分，计算位姿匹配的方差。

在这部分，他在scan点集周围选定一个搜索的窗口W,寻找最佳的位置， 要对搜索中的角度，搜索最远距离，步长，以及分辨率进行设定。 其中，这个hk代表点集，分辨率r一般都是从大到小。


算法1：

**

算法2 采用分支上界的方法：（计算上界，保证计算效率和质量）

**
主要思想是将可能性子集表示为树中的节点，其中根节点表示所有可能的解决方案，在我们的例子中是W。

为了得到一个具体的算法，我们必须确定节点选择、分支和上界计算的方法。
1）节点选择：DFS(depth-first search深度优先搜索)方法
与广度优先搜索算法不同，这种搜索算法所遵循的搜索策略是尽可能“深”地搜索一个图。它可以快速评估许多叶节点。

参考链接：https://blog.****.net/m0_37316917/article/details/70880521
深度优先搜索思想：
首先从节点a开始，假设左边的优先级高于右边，先访问相邻的左节点b,，然后再访问相邻的节点d。d已经没有相邻的节点了，那就返回上一级再访问e,然后再访问h , 当h也没有相邻的节点了，退回。之前访问过得所有节点都设置为已经访问了，所以不断往上搜索到没有访问的节点，如此的递归，直到访问完所有的节点。

同时由于我们不想将差的匹配添加为到回环检测中，所以引入了一个分数阈值， 低于该阈值，对最优解不感兴趣。但是在实践中，阈值通常不会被超过。

2）分枝

树中的每个节点都用一个整数元组来描述，可以理解为一个candidate可构建四个子Candidate。就是一个父节点可以分成4个子节点。其中角度参数不变，因为针对栅格地图，激光端点的平移-在四个节点上的平移针对x,y，所以分别加入一个偏移量x_index_offset，y_index_offset。所以角度是不变的。

分枝的过程其实就是一种枚举的过程，只不过在cartographer中是一种隐式的枚举重点是如何定义目标函数-用来剪枝。

1、假设顶层是C0 ，计算所有C0的得分，从大到小进行排序；
2、然后往下一层，从C0到C1，计算这四个C1的得分，依然是从大到小排序；
3、重复第二步，继续往下C2…Cn，直到计算到了最底层的Cn，这个层数depth=max的时候，计算那四个candidate子节点的得分，将得分最高的作为best_score;
4、然后范围倒数第二层里，将刚才得到的best_score与剩下的三个Cdepth-1的进行比较，如果这个bset_score依然是最大的，那么就没有再进入到倒数第二层那剩余的三个节点里了。就达到了一个剪枝的目的。
5、这样不断的返回，知道遍历整个分支数，返回最优的结果就可以了。

深度减一，不断进行分枝

3）计算上界
这一部分的公式，其实就是通过预计算网格的值，构造得分函数score，来进行一个上界的搜索和计算。最大程度的提高效率。



1 它是通过设定分数阈值计算最优得分best_score;
2 它先计算C0的得分。初始化一个栈C，将这些分数由大到小进行排序，存储。
3 如果这个栈不是空的，那就把里面的小c跳出来；
4 如果它的得分是大于best_score，
5 并且它是叶子节点，我们就将这个子节点的值付给best_score
6 如果当前节点不是叶子节点，就是还没到最精细的，最下面的一层，我们就继续进行分支。
7 最后结束 这个过程，返回的是最优得分best_score与MATCH这个函数 这个match函数在代码中后面跟着的一些参数，后面的匹配就依赖于这个match后面带的参数。

结论

①本文提出并实验验证了一个二维SLAM系统，该系统将scan到submap匹配、环路闭合检测和图形优化相结合。
②在后台，使用pixel accurate match来创建循环闭合约束，所有scan都与附近的submap匹配。并且会对约束图进行周期性的优化。
③实验证明在适当的硬件上实时运行的可能性。

好了~ 恭喜你竟然看完了 其实这篇论文如果读得细一点，可以 对整个cartographer流程有更进一步的理解，对于理解代码之间的跳转也很有帮助，但是这篇论文很抽象，还是需要在学习中不断刷新认知高度。希望可以帮到你哦~ 如果有什么错误可以评论区留言~

哈哈，开心每一天 **

上篇有我整理的整个激光SLAM的流程，可以结合着看

* 对了，附上blibli上视频链接~

https://www.bilibili.com/video/BV1nV411Z7W5?from=search&seid=6422162790679147998