Cartographer三部曲（三）：Real-Time Loop Closure in 2D LIDAR SLAM

Real-Time Loop Closure in 2D LIDAR SLAM

主要贡献：提出一种实时运行的激光slam，精度能达到5cm，前端采用CSM后端采用SPA，并采用分枝定界算法添加回环检测功能。采用构建submap的形式使得系统能够在大型环境中运行。

前端

scan

每当获取到一帧扫描数据后快速的与子地图进行匹配得到相对的坐标变换关系
$$T_{\xi }p=\underset{R_{\xi }}{\underbrace{\left(\begin{array}{cc}\cos {\xi }_{\theta }& -\sin {\xi }_{\theta }\\ \sin {\xi }_{\theta }& \cos {\xi }_{\theta }\end{array}\right)}}p+\underset{t_{\xi }}{\underbrace{\left(\begin{array}{c}{\xi }_x\\ {\xi }_y\end{array}\right)}}$$

submaps

子地图的表达 连续的几帧激光数据构成子地图（submap）。子地图采用概率栅格地图表达，首先将连续空间按照设定的精度栅格化，每一个栅格中都含有一个值代表该栅格被占据的概率。

插入子地图 计算击中和错过的栅格点，对这些点的概率按照如下公式进行更新。
$$\begin{array}{rl}\mathrm{odds}(p)& =\frac{p}{1-p}\\ M_{\text{new }}(x)& =\mathrm{clamp}\left({\text{ odds }}^{-1}\left(\mathrm{odds}\left(M_{\text{old }}(x)\right)\cdot \mathrm{odds}\left(p_{\text{hit }}\right)\right)\right)\end{array}$$

Ceres scan matching

调用Ceres库进行帧间匹配的优化，其中误差函数为：

$$\underset{\xi }{\mathrm{argmin}}\sum _{k=1}^K{\left(1-M_{\mathrm{s}\mathrm{m}\mathrm{o}\mathrm{o}\mathrm{t}\mathrm{h}}\left(T_{\xi }{h}_k\right)\right)}^2$$
//TODO 文中并未对此过多的解释，需要在源码中进一步解读

由于优化方法得到的是局部极值，因此初值的确定便显得尤为重要，在论文中采用IMU的值作为初值可以提高精度。

回环

回环是整个论文的核心创新点，思路在于采用SPA对所有的子地图和scan数据进行优化，并不断检测当前帧和一个窗口内的地图的匹配程度若匹配程度高则达成回环条件将结果立即加入优化中进行回环。

优化问题

优化采用SPA方法，在第二篇论文中有详细的介绍。

分枝定界算法

在一个窗口内寻找地图和scan的最佳匹配是非常耗时的，因此采用分支定界算法对此进行加速从而达到了实时计算的效果。

节点选择

分支定界算法将搜索区域采用树的方式表达，对树的搜索采用深度优先的准则，即在所有节点中每次都访问得分最高的节点。

算法流程

根据节点的选择可以得到如下的算法流程：

其中关键在于

• 将父节点分割成多个子节点
• 计算父节点的得分从而保证找到最优值

分支规则

将每个区域平分为四块，不断的重复这个过程知道达到要求精度为止。

计算得分上界

若是叶节点只需要直接计算得分即可，若非叶节点则需要满足父节点得分不小于所有任意子节点的得分，这样才能保证找到全局最优解。

为了简化计算，我们求父节点得分可以求在父节点代表的区域内每个激光得分的最大值之和。

$$\begin{array}{rl}\mathrm{score}(c)& =\sum _{k=1}^K\underset{j\in {\overline{\mathcal{W}}}_c}{\max }{M}_{\text{nearest }}\left(T_{{\xi }_j}{h}_k\right)\\ & \ge \sum _{k=1}^K\underset{j\in {\mathcal{W}}_c}{\max }{M}_{\text{nearest }}\left(T_{{\xi }_j}{h}_k\right)\\ & \ge \underset{j\in {\overline{\mathcal{W}}}_c}{\max }\sum _{k=1}^K{M}_{\text{nearest }}\left(T_{{\xi }_j}{h}_k\right)\end{array}$$

同时为了加快速度会预先计算好预匹配地图，即计算栅格地图中每个点被测量为占用的概率，后续计算仅需要查表即可得到概率。