Real-Time Loop Closure in 2D LIDAR SLAM 翻译和总结（一）

Hess W , Kohler D , Rapp H , et al. Real-Time Loop Closure in 2D LIDAR SLAM[C]// 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016.
此篇论文是谷歌发表，主要讲了cartographer激光SLAM的实时回环方法，以及大致框架。

摘要

便携式激光测距仪，尤其是激光雷达LIDAR，以及SLAM技术，是一种有效获取建筑平面图的工具和方法。实时生产和可视化建筑平面图可以帮助人们实时评估数据的质量和覆盖范围。基于有限的计算资源构建这样的便携式数据采集和处理平台是非常有必要的。作者提出一种背包建图平台，可以以5cm的分辨率实现实时建图和回环。为了实现实时的回环，文中使用了一种称为分支定界的方法计算scan-to-submap的匹配，并作为约束条件。文中与其他方法进行了实验对比，在性能方面，该方法相对现有方法还是具有较大优势。

1. 引言

建筑平面图在大多数场合都非常有用。调查显示一般采集建筑平面图都是基于当初的建筑设计时的CAD图纸和激光尺测量来进行。这些方法不仅慢，而且因为直线的采集通过人的主观先验确定，这些不总是能够精确描述空间的真实特征。使用SLAM，使得快速精确测量建筑尺寸和复杂度成为可能，而且这相对于手工测量可以降低好几个数量级的时间消耗。
在此领域使用SLAM并不是非常新的想法，论文的重点也不在于此。论文的主要贡献是提出了一种基于激光雷达数据减小计算回环的时间复杂度的新方法。这种方法可以用来对非常大的建筑进行建图，可达上万平方米，同时可以实时的优化整个结果。

2.研究现状

scan-to-scan匹配一般被很频繁的用在激光SLAM中进行相对位姿估计，比如[1]-[4]。就其本身而言，然而，scan-to-scan匹配很快就会产生累计误差。
scan-to-map匹配可以帮助限制误差的积累。文献[5]中使用高斯-牛顿在一个线性插值地图中寻找局部最优匹配。在缺乏比较好的初始先验的情况下，通过高帧率的激光雷达数据来作为先验，局部优化的scan-to-map方法是有效且鲁棒的。在不稳定平台上，基于IMU将激光束投影到水平面上实现重力的估计。
像素级的扫描匹配方法，比如[1]中，可以进一步减小局部误差积累。尽管计算消耗更大，这种方法也可以有效用来进行回环检测。[4]中作者尝试通过在激光点云中提取特征来匹配，从而减小计算量。其他用于回环检测的方法还有直方图匹配[6]，扫描点云中的特征检测，以及机器学习[7]。
两种常用来进一步减小残余局部误差积累的方法分布是粒子滤波和图优化SLAM[2], [8]。
粒子滤波必须在每一个粒子中维护一个全局系统状态的表示。对于基于网格的SLAM，这很快会随着地图的变大而增大资源的消耗，测试用例是用3km的轨迹采集22000$m^2$的的场地。[9]中使用更小维度的特征表示，其不要求每一个粒子对应一个grid map，可以进一步减小资源消耗。当需要存在一个持续更新的grid map时，[10]中建议计算submaps，只有在必要的时候才更小地图，这样final map就是所有子图栅格化的表示。
基于图优化的方法整个过程构建一系列节点，用来表示位姿和特征。图中的边来自观测构成的约束。好多不同的方法可以用来基于所有的约束来最小化整体误差，比如[11], [12]。[13]中描述了一种用于户外的SLAM系统，其基于图优化SLAM，采用局部scan-to-sacn匹配，基于子图特征的直方图实现局部重叠地图的匹配。

3.系统概述

谷歌的cartographer提供了一种使用背包设备进行实时室内建图的解决方案，可以生成分辨率为5cm的2D栅格图。在建筑中行走的过程中，可以看到系统是如何逐渐进行建图的。假设在短期内，基于当前激光雷达扫描匹配估计的最优位姿是足够准确的，然后基于此位姿将当前激光扫描帧插入到一个submap中。扫描匹配是相对一个最近的submap进行，因此其只依赖于最近的几帧扫描，所以在世界坐标系中的位姿估计误差会逐渐累积。
为了在一般的硬件上实现比较好的性能，cartographer没有采用粒子滤波。为了处理世界坐标系下的误差累积，间隔性地运行一次位姿优化（pose optimization ）。当一个submap构建完成时，不会再有新的laser scan被插入其中，只会在回环优化中用来进行扫描匹配。所有已经构建完成的submaps以及scans都会自动用来进行回环检测和优化。如果他们基于当前的位姿估计距离足够近，则基于一个扫描匹配会尝试在此submap中去找对应的最匹配的scan。如果在当前估计的位姿附近的搜索窗口里找到了一个比较好的匹配结果，则其会被作为一个回环约束（loop closing constraint）加入到图优化问题中。通过每隔几秒完成一次优化，采集人员的体验就是当到达一个重复位置时，这个回路会立刻闭合。这会导致一个浅实时约束，回环的扫描匹配需要比新的scan插入的更快，否则这种现象感官上就会明显滞后。cartographer通过一个分支定界方法和对每个已完成的submap进行网格预计算来实现快速回环。

4.local 2D SLAM

系统将独立的local方法和global方法整合到一起形成2D SLAM系统。两种过程都是基于激光雷达的观测信息优化包含2个$(x,y)$位移和1个旋转$\theta$的pose $\xi =({\xi }_x,{\xi }_y,{\xi }_{\theta })$，优化完位姿的激光雷达观测信息进一步会作为激光扫描建图结果（scans）在一个不稳定的平台上，比如作者们使用的背包平台，使用和激光雷达固定在一个水平面上的IMU将优化完成的scans投影到2D水平世界坐标系中。
在local方法中，每一帧连续的扫描都会与一个子图（称为submap M）使用非线性优化进行匹配，从而对齐当前scan与submap，此过程进一步也被称为扫描匹配（scan matching）。Scan mathching随着时间累积的误差会在global方法中进行消除，这在下一部分进行叙述。

4.1 Scans

Submap的构建过程是重复对齐scan和submap坐标系的迭代过程，并进一步插入作为submap里的扫描帧。单扫描帧的原点是（0， 0），令激光扫描帧里的激光点集合为 H = { h k } k = 1 , . . . K , h k ∈ R 2 H=\{h_k\}_{k=1,...K}, h_k \in R^2 H={hk​}k=1,...K​,hk​∈R2。在submap里的扫描帧的pose为$\xi$可以表示为变换$T_{\xi }$，其可以将扫描帧里的扫描点从scan坐标系（激光雷达坐标系）转换到submap坐标系。

4.2 Submaps

几个连续的scans构建成一个submap。这些submaps以概率栅格的形式表示$M$：$r\ast r$–>$[p_{min},p_{max}]$，此栅格地图基于给定分辨率$r$的离散化网格点进行构建， 比如5cm。这些概率值可以被认为是一个栅格点是否有障碍的概率。对每一个栅格点，定义关联的$pixel$，其包含所有最靠近该栅格点的所有点此处的最靠近的范围如何定义？下图是什么意思？pixel表示4领域栅格吗。

每当一个scan被插入概率栅格中时，都会计算一组hits栅格点和另一组不相交的misses栅格点集。对于每个hit, 将距离最近的栅格点也插入到hit集合中。对于每个miss, 将其每个pixel关联的栅格点插入其中，该栅格点与scan原点和每个scan点之间的射线相交（激光点射线会穿过这些miss栅格点），这里不包括已经插入的hit集合。如果之前未被观测过的栅格属于hit或者misses集合，则会被分配一个概率$p_{hit}$
或者$p_{miss}$。如果栅格$x$已经被观测过，则按照下面公式更新其对应的概率值：
$$odds(p)=p/(1-p)\text{\hspace{1em}(2)}$$
$$M_{new}(x)=clamp(odd{s}^{-1}(odds(M_{old(x)})\ast odds(p_{hit})))\text{\hspace{1em}(3)}$$
对于misses的更新同上，clamp表示区间限定函数，将新的概率值限定在$[p_{min},p_{max}]之间$。
上述对栅格进行多次测量概率融合的公式在代码里的描述为:

inline float Odds(float probability) {
  return probability / (1.f - probability);
}
inline float ProbabilityFromOdds(const float odds) {
  return odds / (odds + 1.f);
}
constexpr float kMinProbability = 0.1f;
constexpr float kMaxProbability = 1.f - kMinProbability;

此处对上述代码进行具体分析：
Odds表示可能性，odds表示可能性，大于1则表示hit, 小于1则表示miss，可将一个0-1的概率值转换为可能性描述。有以下几个特点:

• p>0.5并越大，返回值大于1并越大
• p<0.5并越小，返回值小于1并越小
• 大于1的数字连乘，会越来越大，并永远大于1, 表示hit的可能性越来越大
• 小于1的数字连乘，会越来越小，并永远小于1, 表示miss的可能性越来越大

ProbabilityFromOdds表示概率，大小在0-1之间，同公式里的$odd{s}^{-1}$，0-0.5表示miss,
0.5-1表示hit，可将一个可能性值转换为0-1之间的概率值。有以下几个特点:

• value>1并越大, 则返回值大于0.5并越大
• value<1并越小，则返回值小于0.5并越小
• 此处可将多次测量的融合结果转换为0-1的概率描述

clamp函数进行概率区间限定，将ProbabilityFromOdds的转换结果限制在(0.1, 0.9)之间。

4.3 Ceres scan matching

在将一个扫描scan插入到一个submap中之前，使用基于Ceres的扫描匹配器（scan mathcher）优化求解该扫描帧相对当前局部local submap的位姿$\xi$。扫描匹配器负责找到submap中能够最大化当前扫描点概率的位姿。作者将此此过程构建为如下所示的非线性最小二乘问题：
$$argmin\sum _{k=1}^K(1-M_{smooth}(T_{\xi }{h}_k){)}^2\text{\hspace{1em}(CS)}$$
上式中$T_{\xi }$基于扫描帧的相对位姿将扫描帧点云$h_k$从激光雷达扫描坐标系转换到submap坐标系， K表示当前扫描帧的所有扫描点数量。函数$M_{smooth}：R^2->R$是局部submap概率值的平滑函数，此处使用双三次插值算法（即对转换完成的submap下的位姿附近的点进行概率查找，然后基于此函数进行概率平滑得到submap中的匹配概率值），其输出在[0, 1]之外的值会被剔除。
基于此平滑函数的数学优化一般可以的得到优于栅格分辨率的位姿精度。因为这是一个局部优化，需要要求有一个比较好的初始估计值。IMU可以用来估计扫描匹配时位姿的旋转角分量$\theta$，并作为初始估计值。当没有IMU时，可以通过提高扫描匹配的频率，或者进行像素级的扫描匹配（比如ICP，ICL等）方法来进行位姿估计，但是会增加计算消耗。

5.回环

因为扫描帧scans只相对于包含几个最近scans的一个submap进行匹配，所以上述的local方法总会缓慢的产生误差积累。对于几十个连续的scans来说，这种累积误差非常小。
当处理更大的空间时，会产生很多小的submap。进一步，作者提出全局方法，基于$SparsePoseAdjustment$对所有submaps和scans的位姿进行优化。scans被插入到submap中的相对位姿会被存下来，在回环优化的时候使用。除此之外，一旦submap不再更新，则所有的（one scan, one submap）对都会被考虑进回环中。一个帧扫描器scan matcher会一直在后端运行，如果发现了一个匹配度很高的候选帧，则对应的相对位姿（对应的匹配到的候选帧的相对位姿作为当前帧的位姿观测加入？）会被加入到优化问题中。

5.1 优化问题

回环优化，与scan matching也类似，为了将额外的其他数据考虑进去，也构建了一个运行增加残差进去的非线性最小二乘问题。每过几秒或者几个节点，则使用$Ceres$基于下式计算一个结果：
$$argmin({\Xi }^m,{\Xi }^s)=\frac{1}{2}{\Sigma }_{ij}\rho (E^2({\xi }_i^m,{\xi }_j^s;{\Sigma }_{ij},{\xi }_{ij}))\text{\hspace{1em}(SPA)}$$
上式中世界坐标系下的submap位姿 Ξ m = { ξ i m } i = 1 , . . . , m \Xi^m=\{{\xi_i^m}\}_{i=1, ..., m} Ξm={ξim​}i=1,...,m​，scan位姿 Ξ s = { ξ j s } i = 1 , . . . , n \Xi^s=\{{\xi_j^s}\}_{i=1, ..., n} Ξs={ξjs​}i=1,...,n​，都会基于添加的约束constraints进行优化。 这些约束由submap与scan之间的相对位姿${\xi }_{ij}$和关联的协方差${\Sigma }_{ij}$组成（在local方法构建submap的过程中产生，还有一些在global方法回环检测部分产生）。对于一对儿submap $i$ 和 scan $j$， 位姿${\xi }_{ij}$描述了在该submap坐标系下这个scan被匹配到的最优位置（前端Ceres Scan Matching得到）。协方差${\Sigma }_{ij}$可以根据文献[15]中所述方法进行计算，也可以在local方法中基于公式（CS）使用Ceres的协方差估计特点进行计算。对于这样一个约束，残差$E$可以通过下式计算：
$$E^2({\xi }_i^m,{\xi }_j^s;{\Sigma }_{ij},{\xi }_{ij}))=e({\xi }_i^m,{\xi }_j^s;,{\xi }_{ij}{)}^T{\Sigma }_{ij}^{-1}e({\xi }_i^m,{\xi }_j^s;,{\xi }_{ij})\text{\hspace{1em}(4)}$$
$$e({\xi }_i^m,{\xi }_j^s;,{\xi }_{ij})={\xi }_{ij}-\left(\begin{array}{c}{R}_{{\xi }_i^m}^{-1}(t_{{\xi }_i^m}-t_{{\xi }_j^s})\\ {\xi }_{i;\theta }^m-{\xi }_{j:\theta }^s\end{array}\right)\text{\hspace{1em}(5)}$$
损失函数$\rho$目的时用来减少外点的影响，比如使用Huber loss，当扫描匹配在优化问题里加入了不正确的约束时，容易在SPA里出现这种较大的外点误差。比如，这种情况会比较容易发生在对称环境中，比如办公室隔间，其他处理外点的方法可参见[16]。
接下篇（Real-Time Loop Closure in 2D LIDAR SLAM 翻译和总结（二））