雷达系列论文翻译（十二）：Cartographer系列（三）

Real-Time Loop Closure in 2D LIDAR SLAM

摘要

便携式激光测距仪，又称激光雷达，同时定位和建图（SLAM）是一种有效的获取竣工平面图的方法。实时生成和可视化楼层平面图有助于操作员评估捕获数据的质量和覆盖范围。构建一个便携式捕获平台需要在有限计算资源的情况下进行。本文介绍了背包建图平台中的实现方法，实现了5cm分辨率的实时建图和闭环矫正。为了实现实时的闭环矫正，我们使用分支和边界方法计算扫描到子图的匹配作为约束。我们提供了实验结果，并与其他知名的方法进行了比较，结果表明，在质量方面，我们的做法足够与现有技术相竞争。

引言

竣工平面图在各种应用中都很有用。为建筑管理任务收集这些数据而进行的人工调查通常结合计算机辅助设计（CAD）和激光卷尺测量。这些方法是缓慢的，而且，通过将人类对建筑的先入为主的观念用于收集直线的集合，并不总是准确地描述空间的真实本质。利用SLAM可以快速、准确地对大小复杂的建筑物进行测量，而人工测量需要几个数量级以上的时间。

在这一领域应用SLAM并不是一个新的想法，也不是本文的重点。相反，本文的贡献是提出了一种新的方法来减少从激光测距数据计算闭环约束的计算量。这项技术使我们能够对数万平方米的非常大的楼层进行建图，同时为操作员提供实时的完整优化的结果。

系统概述

谷歌的 cartographer 为室内建图提供了一个实时解决方案，它是一个配备传感器的背包，可以生成一个$r=5cm$分辨率的二维栅格地图。系统的操作人员可以在穿过一栋建筑时看到正在创建的地图。激光扫描被插入到最佳估计位置的子图中，该位置在短时间内被认为是足够精确的。扫描匹配针对当前的子图，因此它只依赖于当前的扫描，并且在世界坐标系中位姿估计的误差逐渐累积。

为了在硬件要求不高的情况下获得良好的性能，我们的SLAM方法不使用粒子滤波器。为了应对误差的积累，我们定期进行位姿优化。当子图建立完成，即不再向其中插入新的扫描时，他被用来进行闭环矫正所需要的扫描匹配。所有完成的子图和扫描都会被自动地用作闭环矫正。如果根据当前的位姿估计，它们足够接近，扫描匹配器会尝试在子图中找到扫描。如果在当前估计位姿周围的搜索窗口中找到足够好的匹配，它将作为一个闭环约束被添加到优化问题中。通过每隔几秒钟完成一次优化，操作员的经验是，当重新访问某个位置时，闭环会被立即矫正。这导致了软实时约束，即闭环检测的扫描匹配必须比添加新扫描更快，否则会表现出明显的滞后。我们通过使用分支定界方法和每个已经完成的地图的预先计算的栅格来实现这一点。

局部2D SLAM

我们的系统结合了单独的局部和全局方法来实现2D SLAM。这两种方法都优化了雷达观测（之后被称为扫描）的位姿$\zeta =({\zeta }_x,{\zeta }_y,{\zeta }_{\theta })$，包括平移$({\zeta }_x,{\zeta }_y)$和旋转${\zeta }_{\theta }$。在一个不稳定的平台上，比如我们的背包，IMU被用来估计重力的方向，以便将水平安装的激光雷达的扫描投射到2D世界。

在我们的局部方法中，每个连续的扫描都与子图$M$进行匹配，通过使用非线性优化方法，使扫描与子图对齐；这个过程进一步被称为扫描匹配。扫描匹配会随着时间积累误差，然后通过我们的全局方法消除这些误差。

A.扫描

子图的构造是重复对齐扫描和子图坐标系的迭代过程。扫描的原点为$0\in R^2$，我们现在将扫描点云的集合写做 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。子图坐标系中扫描坐标系的位姿$\zeta$被表示为变换$T_{\zeta }$，它将扫描点云从扫描坐标系刚性变换到子图坐标系，定义为
$$T_{\zeta }p=\left(\begin{array}{cc}cos{\zeta }_{\theta }& -sin{\zeta }_{\theta }\\ sin{\zeta }_{\theta }& cos{\zeta }_{\theta }\end{array}\right)p+\left(\begin{array}{c}{\zeta }_x\\ {\zeta }_y\end{array}\right)\text{\hspace{1em}(1)}$$

B.子图

几个连续的扫描用于构建子图。这些子图采用概率栅格的形式：$r_Z\times r_Z\to [p_{min}，p_{max}]$，它在一个给定的分辨率下将一个离散的栅格点映射为一个值。这些值可以看作是一个栅格点被占据的概率，对于每个网格点，我们定义对应的pixel用来包含靠近这个栅格点的所有点。

这里的pixel实在是不知道怎么翻译，但是说的就是以栅格点为中心，指定分辨率为边长的一个正方形区域

每当扫描要插入概率栅格时，都会计算一组被击中的栅格点和一组未击中的栅格点。对于每一次击中，我们将最近的栅格点插入到击中集合中。对于每一次未击中，我们插入与扫描原点和每个扫描点之间的射线经过的每个pixel对应的栅格点，不包括已经在击中集合中的栅格点。如果每个以前未观察到的栅格点位于其中一个集合中，则为其分配一个击中概率或者未击中概率。如果栅格点$x$已经被观察过，我们会将击中和未击中的可能性更新为
$$odds(p)=\frac{p}{1-p}\text{\hspace{1em}(2)}$$
$$M_{new}=clamp(odd{s}^{-1}(odds(M_{old}(x))\cdot odds(p_{hit})))\text{\hspace{1em}(3)}$$
对于未击中的更新方式类似。

这里的odds也可以理解为一种概率，它与概率p具有正相关的变换关系

C.Ceres扫描匹配

在将扫描插入子图之前，使用基于Ceres的扫描匹配器相对对扫描相对于当前局部子图的位姿$\zeta$进行优化。扫描匹配器负责找到一个扫描对应的位姿，使子图中扫描点云的概率最大化。我们把它看作一个非线性的最小二乘问题
$$\underset{\zeta }{\mathrm{argmin}}\sum _{k=1}^K(1-M_{smooth}(T_{\zeta h_k}){)}^2\text{\hspace{1em}(CS)}$$

其中，$T_{\zeta }$根据扫描的位姿将$h_k$从扫描坐标系变换到子图坐标系。函数$M_{smooth}:R^2\to R$是局部子图中概率值的光滑形式。我们使用双三次插值。因此，区间$[0,1]$之外的值可能出现，但被认为是无害的。

这种光滑函数的数学优化结果通常比网格的分辨率更精确。由于这是一个局部优化问题，需要良好的初始估计。一个能够测量角速度的IMU可以用来估计扫描匹配之间位姿的旋转分量$\theta$。在没有IMU的情况下，可以使用更高频率的扫描匹配或pixel精度的扫描匹配方法，尽管计算量会更大。

闭环矫正

由于扫描只与包含少量最近扫描的子图匹配，因此上述方法缓慢地累积误差。对于几十次连续扫描，累积误差通常很小。

较大的空间通过创建许多小的子图来处理。我们的方法使用稀疏位姿调整来优化所有扫描和子图的位姿。插入扫描的相对位姿存储在内存中，用于进行闭环矫正的优化。除了这些相对位姿之外，一旦子图不再改变,由一个扫描和这个子图组成的所有其他相对位姿都在闭环矫正时候考虑。在后台运行一个扫描匹配器，如果发现一个良好的匹配，则将相应的相对位姿加入到优化问题中。

A.优化问题

闭环矫正优化，像扫描匹配一样，也是一个非线性最小二乘问题，可以很容易地添加残差来考虑额外的数据。每隔几秒钟，我们使用Ceres计算一次下述问题的解
$$\underset{{\Xi }^m,{\Xi }^s}{\mathrm{argmin}}\frac{1}{2}\sum _{ij}\rho (E^2({\zeta }_i^m,{\zeta }_j^s;{\Sigma }_{ij},{\zeta }_{ij}))\text{\hspace{1em}(SPA)}$$
其中子图的位姿 Ξ m = { ζ i m } i = 1 , . . . , m \Xi^m = \{\zeta_i^m\}_{i=1,...,m} Ξm={ζim​}i=1,...,m​和扫描位姿 Ξ s = { ζ j s } j = 1 , . . . , n \Xi^s = \{\zeta_j^s\}_{j=1,...,n} Ξs={ζjs​}j=1,...,n​在给定一些约束条件下进行了优化。这些约束采用相对位姿${\zeta }_{ij}$和相关协方差矩阵${\Sigma }_{ij}$的形式。对于一对子图$i$和扫描$j$，位姿${\zeta }_{ij}$描述了在子图坐标系中扫描匹配到的位置。协方差矩阵${\Sigma }_{ij}$可以根据论文Real-time correlative scan matching中的方法进行评估，或者局部使用Ceres的协方差估计特征。此类约束的残差$E$由以下公式计算：
$$E^2({\zeta }_i^m,{\zeta }_j^s;{\Sigma }_{ij},{\zeta }_{ij})=e({\zeta }_i^m,{\zeta }_j^s,{\zeta }_{ij}{)}^T{\Sigma }_{ij}^{-1}e({\zeta }_i^m,{\zeta }_j^s,{\zeta }_{ij})\text{\hspace{1em}(4)}$$

$$e({\zeta }_i^m,{\zeta }_j^s,{\zeta }_{ij})={\zeta }_{ij}-\left(\begin{array}{c}{R}_{{\zeta }_i^m}^{-1}(t_{{\zeta }_i^m}-t_{{\zeta }_j^s})\\ {\zeta }_{i;\theta }^m-{\zeta }_{j;\theta }\end{array}\right)\text{\hspace{1em}(5)}$$
损失函数$\rho$，例如huber损失函数，用于减少当扫描匹配向优化问题添加不正确的约束时可能出现在（SPA）中的异常值的影响。

B.分枝定界扫描匹配

我们对最佳的pixel精度的匹配感兴趣
$${\zeta }^{\ast }=\underset{\zeta \in W}{\mathrm{argmax}}\sum _{k=1}^K{M}_{nearest}(T_{\zeta }{h}_k)\text{\hspace{1em}(BBS)}$$
其中，$W$是搜索窗口，$M_{nearest}$通过四舍五入将所有属于$R^2$的值对应到最近的栅格点，即将栅格点的值扩展到相应的pixel。使用（CS）公式中的方法可以进一步提高匹配质量。

通过仔细地选择步长提高了效率，我们选择了角步长${\delta }_{\theta }$，使得最大范围$d_{max}$内的扫描点移动不超过一个pixel的宽度$r$。利用余弦定律，我们导出
$$d_{max}=\underset{k=1,...,K}{\max }||h_k||\text{\hspace{1em}(6)}$$
$${\delta }_{\theta }=arccos(1-\frac{r^2}{2d_{max}^2})\text{\hspace{1em}(7)}$$

我们计算了覆盖给定线性和角度搜索窗口大小的整数步数，例如$W_x=W_y=7m$以及$W_{\theta }=30^o$◦，
$$w_x=\lceil \frac{W_x}{r}\rceil ,w_y=\lceil \frac{W_y}{r}\rceil ,w_{\theta }=\lceil \frac{W_{\theta }}{{\delta }_{\theta }}\rceil \text{\hspace{1em}(8)}$$
这导致一个有限的集合$W$围绕一个位于其中心的估计${\zeta }_0$形成一个搜索窗口，
W ˉ = { − w x , . . . , w x } × { − w y , . . . , w y } × { − w θ , . . . , w θ } (9) \bar{W} = \{-w_x,...,w_x\} \times \{-w_y,...,w_y\} \times \{-w_\theta,...,w_\theta\} \tag{9} Wˉ={−wx​,...,wx​}×{−wy​,...,wy​}×{−wθ​,...,wθ​}(9)
W = { ζ 0 + ( r j x , r j y , δ θ j θ ) : ( j x , j y , j θ ) ∈ W ˉ } (10) W = \{\zeta_0 +(rj_x,rj_y,\delta_\theta j_\theta):(j_x,j_y,j_\theta) \in \bar{W}\} \tag{10} W={ζ0​+(rjx​,rjy​,δθ​jθ​):(jx​,jy​,jθ​)∈Wˉ}(10)
一个简单的求${\zeta }^{\ast }$的算法可以很容易地表达出来，参见算法1，但是对于我们所考虑的搜索窗口大小来说，它太慢了。

首先，我们使用分支定界方法在较大的搜索窗口上高效地计算${\zeta }^{\ast }$。通用的方法见算法2。

其主要思想是将可能解的子集表示为树中的节点，其中根节点表示所有可能的解，也就是我们问题中的$W$。每个节点的子节点形成其父节点的一部分，因此它们一起表示相同的可能解的集合。叶节点为单不可再分的；每一个叶节点都给出了一个可行的解决方案。注意算法是准确的。它提供了与原始方法相同的解决方案，只要内部节点的$score(c)$是其元素得分的上界。在这种情况下，只要一个节点是有界的，它的子树中就不存在比已知解更好的解。

为了得到一个具体的算法，我们必须确定节点选择、分支和上界计算的方法。

1. 节点选择：
   在没有更好的选择的情况下，我们的算法使用深度优先搜索（DFS）作为默认选择：算法的效率取决于被修剪的树的很大一部分。这取决于两件事：一个好的上界和一个好的当前的解。后一部分由DFS提供帮助，DFS可以快速评估多个叶子节点。由于我们不想添加弱匹配作为闭环约束，我们还引入了一个分数阈值，低于该阈值的解我们不感兴趣。由于在实践中，该阈值通常不会被经常超过，这降低了节点选择或寻找初始启发式解的重要性。关于在DFS期间访问子节点的顺序，我们计算每个子节点得分的上界，首先访问具有最大上界的最有保证的子节点。该方法是算法3。
   

利用栈的先进后出的结构实现的深度优先搜索算法

2. 分枝规则：树中的每个节点由一个整数元组$c=(c_x,c_y,c_{\theta },c_h)\in Z^4$来描述。在高度$c_h$处的节点组合成$2^{c_h}\times 2^{c_h}$个可能的平移，但代表一个特定的旋转：
   W ˉ ˉ = ( { ( j x , j y ) ∈ Z 2 : c x ≤ j x < c x + 2 c h c y ≤ j y < c y + 2 c h } × { c θ } ) (11) \bar{\bar{W}} = \begin{pmatrix} \{ (j_x,j_y) \in Z^2 : \begin{matrix} c_x \leq j_x < c_x + 2^{c_h} \\ c_y \leq j_y < c_y + 2^{c_h} \end{matrix} \} \times \{c_\theta\}\end{pmatrix} \tag{11} Wˉˉ=({(jx​,jy​)∈Z2:cx​≤jx​<cx​+2ch​cy​≤jy​<cy​+2ch​​}×{cθ​}​)(11)
   $$\overline{W_c}=\overline{\stackrel{ˉ}{W_c}}\cap \bar{W}\text{\hspace{1em}(12)}$$
   叶节点的高度$c_h=0$，对应的可行解$W\ni {\zeta }_c={\zeta }_0+(r{c}_x,r{c}_y,{\delta }_{\theta }{c}_{\theta })$。

   在我们的算法3的公式中，包含所有可行解的根节点没有显式出现，而是以可以覆盖搜索窗口的固定高度$h_0$分支到一组初始节点$C_0$中
   W ˉ 0 , x = { − w x + 2 h 0 j x : j x ∈ Z , 0 ≤ 2 h 0 j x ≤ 2 w x } W ˉ 0 , y = { − w y + 2 h 0 j y : j y ∈ Z , 0 ≤ 2 h 0 j y ≤ 2 w y } W ˉ 0 , θ = { j θ ∈ Z , − w θ ≤ j θ ≤ w θ } C 0 = W ˉ 0 , x × W ˉ 0 , y × W ˉ 0 , θ × { h 0 } (13) \bar{W}_{0,x} = \{-w_x + 2^{h_0}j_x:j_x \in Z,0 \leq 2^{h_0}j_x \leq 2w_x\} \\ \bar{W}_{0,y} = \{-w_y + 2^{h_0}j_y:j_y \in Z,0 \leq 2^{h_0}j_y \leq 2w_y\} \\ \bar{W}_{0,\theta} = \{j_\theta \in Z,-w_\theta \leq j_\theta \leq w_\theta\} \\ \\ C_0 = \bar{W}_{0,x} \times\bar{W}_{0,y} \times \bar{W}_{0,\theta} \times\{h_0\} \tag{13} Wˉ0,x​={−wx​+2h0​jx​:jx​∈Z,0≤2h0​jx​≤2wx​}Wˉ0,y​={−wy​+2h0​jy​:jy​∈Z,0≤2h0​jy​≤2wy​}Wˉ0,θ​={jθ​∈Z,−wθ​≤jθ​≤wθ​}C0​=Wˉ0,x​×Wˉ0,y​×Wˉ0,θ​×{h0​}(13)
   在$c_h>1$的一个给定节点$c$上，我们最多可以分出四个高度为$c_h-1$的子节点。
   C c = ( ( { c x , c x + 2 c h − 1 } × { c y , c y + 2 c h − 1 } × c θ ) ∩ W ˉ ) × { c h − 1 } (14) C_c = ((\{c_x,c_x + 2^{c_h-1}\} \times \{c_y,c_y + 2^{c_h-1}\} \times c_\theta) \cap \bar{W}) \times \{c_h -1\} \tag{14} Cc​=(({cx​,cx​+2ch​−1}×{cy​,cy​+2ch​−1}×cθ​)∩Wˉ)×{ch​−1}(14)

   这里的核心思想就是对一个大的步长在x和y方向进行对半拆分，而$\theta$不变

3. 计算上界：分支定界法的剩余部分是无论是在计算量还是在定界的质量方面都高效的计算内部节点上界的一种方法。我们使用
   $$\begin{gathered} score(c)=\sum _{k=1}^K\underset{j\in \overline{\stackrel{ˉ}{W_c}}}{\max }{M}_{nearest}(T_{{\zeta }_j}{h}_k) \\ \ge \sum _{k=1}^K\underset{j\in \overline{W_c}}{\max }{M}_{nearest}(T_{{\zeta }_j}{h}_k) \\ \ge \underset{j\in \overline{W_c}}{\max }\sum _{k=1}^K{M}_{nearest}(T_{{\zeta }_j}{h}_k)\text{\hspace{1em}(15)} \end{gathered}$$

   这个公式是定界的核心理论公式，直观的来讲，对于一个节点包含的所有的可能解来说，将当前扫描中的每个点经过其中的最优解投影后计算得到的地图分数和一定是小于等于将扫描中的每个点经过所有可能解进行投影，之后对每个投影点在不同解下得到的地图分数取最大值最后相加得到的分数。

   为了能够有效地计算最大值，我们使用了预计算的栅格地图$M$。预先计算每个可能的高度$c_h$对应的栅格地图，让我们计算分数的复杂度与扫描中的点数线性相关。
   $$score(c)=\sum _{k=1}^K{M}_{precomp}^{c_h}(T_{{\zeta }_c},h_k)\text{\hspace{1em}(16)}$$
   $$M_{precomp}^h(x,y)=\underset{\begin{array}{c}{x}^{{}^{\prime }}\in [x,x+r(2^h-1)]\\ y^{{}^{\prime }}\in [y,y+r(2^h-1)]\end{array}}{\max }{M}_{nearest}(x^{{}^{\prime }},y^{{}^{\prime }})\text{\hspace{1em}(17)}$$
   请注意，$M_{precomp}^h$与$M_{nearest}$有着相同的pixel结构，但在每个pixel中储存了从pixel开始的$2^h\times 2^h$区域内的最大值。下图给出了这种预计算网格的示例。
   

   注意这个预计算的网格实现了上述的对与扫描在每一个可能解下的投影取最大值的方法，也是能够进行定界的核心

   为了降低构建预计算栅格地图的计算量，我们将等待概率栅格地图不再接收更新。之后我们计算一组预计算栅格地图，并开始对其进行匹配。

   对于每个预计算的栅格地图，我们计算从每个pixel开始的$2^h$pxel宽的行的最大值。利用这个中间结果，然后构造下一个预计算栅格地图。

   如果值是按添加顺序删除的，则值集合的最大值进行更新的复杂度为$O(1)$。连续最大值保存在一个队列中，该队列可以被递归的定义并且包含当前集合中所有值的最大值，然后是这个最大值第一次出现之后所有值的连续最大值列表。对于空值集合，此列表为空。使用这种方法，可以在$O(n)$的复杂度内计算预计算栅格地图，其中n是每个预计算栅格地图中的pixel数。

   这个具体的实现看论文不是非常的清晰，需要看源码来理解

   另一种计算上界的方法是计算低分辨率概率栅格地图，依次将分辨率减半。由于我们的方法额外的内存消耗是可以接受的，我们更喜欢使用它。使用较低的分辨率概率网格，这会导致比（15）更坏的界限，从而对性能产生负面影响。

   这里的产生更坏的界限应该是由于使用低分辨率地图是对几个pixel的值进行融合而不是取最大值

结论

本文提出并实验验证了一个二维SLAM系统，该系统结合扫描到子图的匹配、闭环检测和图优化，利用基于栅格地图的局部SLAM方法生成子图轨迹。在背景中，使用pixel精度的扫描匹配将所有扫描匹配到附近的子图，以创建闭环约束。在后台线程中对子图和扫描位姿的约束图进行周期性优化。最后的地图是由GPU加速的已完成的子图和当前子图的组合，操作员可以看到最终地图的最新预览。我们证明了我们的算法可以在普通硬件上实时运行。