>
返回
论文阅读笔记

[论文阅读]Real-Time Loop Closure in 2D LIDAR SLAM

Real-Time Loop Closure in 2D LIDAR SLAM 主要概括了 Cartographer 的原理，因此阅读这篇论文对理解 Cartographer 的算法原理很有帮助。

1.简介

本文对于 SLAM 的贡献主要在于提出了一种高效的算法能够降低利用激光雷达数据进行回环检测的算力需求。这种算法可以在上万平方米的区域中进行实时建图和优化。

2.相关工作

论文中主要介绍了前后帧激光数据扫描以及后处理去除误差的相关工作，主要分为：

  • 观测数据匹配，主要包括以下三类方法：
    • Scan-to-Scan: 使用前后帧激光扫描进行匹配求解相对位移，这种相对简单且直观，但定位误差会迅速累计，效果不好
    • Scan-to-Map：将激光数据与局部地图做匹配，使用高斯牛顿等方法可以有效控制累计误差
    • Pixel-accurate scan matching：可以进一步减少局部误差累计，计算量相对而言更高

进一步消除累计误差的方法有两种：

  • 粒子滤波：需要在每个粒子中维护整个地图系统的装啊提，因此消耗资料会迅速增加
  • 基于图的方法：图优化的方法主要是建立在一系列用于表示位姿和特征的节点，图的边则是由观测误差生成

3.系统总览

Cartographer 提供了一种室内实时建图方案可以生成分辨率为 5cm 的栅格地图。每帧激光扫描数据会以当前估计的最佳位置插入到一个局部地图 (submap) 中，并且假定这个局部地图在短时间内是足够准确的。由于激光扫描只用于插入局部地图，因此比较对象只有最近的激光扫描，因此其相对与世界坐标系的误差会累积。

为了降低 SLAM 的硬件需求，Cartographer 没有使用粒子滤波，因此会存在累计误差。为了消除累计误差，Cartographer 定期采取一次位姿优化。此外，每当一个局部地图建立完成后，其都会被用于进行回环检测。回环检测的过程为：检测局部地图的位姿，如果位姿较近则会调用激光扫描匹配(scan matcher)，如果在该位姿附近的一个窗口附近找到足够数量的匹配关系，则会添加一个回环约束进入优化问题中。而通过几秒一次进行优化问题的求解，回环检测的更新几乎可以算是实时完成。

4.局部 2D SLAM

Cartographer 的局部 SLAM 中估计每一帧激光扫描数据对应的位姿 $\xi = (\xi_x, \xi_y, \xi_{\theta})$，包括平移分量 $(x,y)$ 和转向 $\xi_{\theta}$。在不稳定的平台上还会使用 IMU 来估计重力的指向来将水平放置的激光扫描数据投影到 2D 世界平面上。下面分别从扫描、局部地图、扫描匹配来介绍这个过程。

  • 扫描 (Scan)

扫描是构建局部地图的基本单位。局部地图的构建就是重复的将扫描（也就是帧）匹配至局部地图的迭代过程。由于是 2D SLAM，作以下约定：扫描的原点在 $\mathbf{0}\in \mathbb{R}^2$、激光扫描点的位置信息（在 lidar 坐标系下）为 $H = {h_k}_{k=1,...,L},h_k\in\mathbb{R}^2$，扫描帧中的激光点在局部地图的位姿 $\xi$ 用 $T_\xi$ 来表示，因此将激光点在扫描扫描坐标系下的坐标转换为局部地图坐标系下的坐标的过程为：

$$
T_\xi p = \underbrace{\begin{bmatrix}
  \cos{\xi_\theta} & -\sin{\xi_\theta} \\
  \sin{\xi_\theta} & \cos{\xi_\theta}
\end{bmatrix}p}_{R_\xi} + \underbrace{\begin{bmatrix}
  \xi_x \\ \xi_y
\end{bmatrix}}_{t_{\xi}}
$$
  • 局部地图 (Submap)

局部地图由一部分帧组成，并构建一个二维概率栅格，将一定分辨率 $r$ 的离散栅格点映射到一个概率值，即：$M: r\mathbb{Z}\times r\mathbb{Z} \rightarrow [p_{\min}, p_{\max}]$ 。其中概率值表示该栅格点为一个障碍物的概率，对于每一个栅格点，用一个对应的像素来表示，其包含所有离该栅格点最近的点。

每当一帧扫描数据插入概率栅格地图中，首先需要计算两个集合：激光点击中的栅格集合 $hits$ 和经过的栅格集合 $misses$。对于每一束激光束做以下处理：每一个激光点击中的位置，将其最近的栅格插入击中集合中，而对其经过（从激光束原点到击中位置）的栅格将将其经过的栅格（除了已经在当次击中集合的）放入经过集合中，如下图所示。

接下来进行栅格概率值的更新：对应之前没有观测过的栅格点直接分配一个击中/经过对应的概率 $p_{hit}$ 或 $p_{miss}$。如果栅格之前有观测过的话则根据下列式子进行更新：

$$
\begin{aligned}
{\rm odds}(p) &= \frac{p}{1-p} \\
M_{\rm new}(x) &= {\rm clamp}({\rm odds}^{-1}({\rm odds}(M_{\rm old}(x)){\rm odds}(p_{\rm hits})))
\end{aligned}
$$
  • 扫描匹配 (Scan matching)

在将扫描数据插入局部地图之前，首先需要用基于 Ceres 的优化器来将该扫描数据对应的位姿进行优化。该优化器负责找到一个位姿使得该帧数据所有扫描点在局部地图的可能性最大。具体要优化的方程如下：

$$
\arg\min_{\xi} \sum_{k=1}^K (1-M_{\rm smooth}(T_\xi h_k))^2
$$

其中 $T_{\xi}$ 将 $h_k$ 根据估计的位姿从扫描坐标系转换到局部地图坐标系。函数 $M_{\rm smooth}:\mathbb{R}^2\rightarrow\mathbb{R}$ 将信息矩阵转换为连续概率值。使用这种连续函数的数学优化通常比根据栅格的分辨率能得到更好的结果。由于这是一个局部优化问题，因此需要一个较好的初值。这里可以使用 IMU 来测量角速度从而进行位姿中旋转部分 $\theta$ 的估计。在没有 IMU 的场合则使用更好频率的扫描匹配，这样位姿之间相隔的时间更短，因此丢失的可能性也会降低。

5.回环检测

由于每次扫描匹配只与最近几次扫描数据匹配，因此上述的方法会逐渐累计误差。Cartographer 中使用稀疏位姿调整（Sparse Pose Adjustment） 来对所有局部地图中的所有扫描数据的位姿进行优化。内存中存储了扫描数据之间的相对位置来进行回环优化，除了这些相对位姿以外，一旦一个局部地图完成（不再变化之后），其他包含一个扫描和一个局部地图的关系对都被作为一个约束添加进回环检测中。为此，在后台会持续进行扫描匹配，一旦一个上述关系（扫描和任意一个不再变化的局部地图匹配）发现之后，相应的相对位姿会添加进优化问题中。

  • 优化问题

回环优化和扫描匹配一样可以表示为非线性最小二乘问题，这样可以很容易添加额外数据作为残差进问题中。每隔几秒，Cartographer 会调用 Ceres 对下列问题求解：

其中，优化的变量是在给定部分约束条件下世界坐标系下局部地图的位姿集合 ${\xi_i^m}_{i=1,...,m}$ 以及扫描的位姿 ${\xi_j^s}_{i=j,...,n}$。约束条件中包含相对位姿 $\xi_{ij}$ 以及对应的协方差矩阵 $\Sigma_{ij}$，对于一对局部地图 $i$ 以及扫描数据 $j$ 而言，$\xi_{ij}$ 描述了扫描数据在局部地图坐标系下的插入位姿，协方差矩阵 $\Sigma_{ij}$ 可以通过 Real-time correlative scan matching 或者直接调用 Ceres 进行估算，有了相对位姿以及协方差矩阵之后，残差 E 的形式为：

$$
\begin{aligned}
E^2(\xi_i^m,\xi_j^2,\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}) \\
e(\xi_i^m,\xi_j^s,\xi_{ij}) &= \xi_{ij} - \begin{bmatrix}
R_{\xi_i^m}^{-1}(t_{\xi_i^m}-t_{\xi_j^n}) \\
\xi_{i;\theta}^m - \xi_{j;\theta}^s
\end{bmatrix}
\end{aligned}
$$

为了降低 SPA 中可能出现的噪点数据对优化问题的影响，可以采用损失函数 $\rho$ 例如 Huber loss。噪点数据有可能出现在结构对称的环境里，例如办公室隔间等。

  • 分支定界扫描匹配

回环检测中，需要找到以下最优匹配：

$$
\xi^\star = \arg\max_{\xi\in W}\sum_{k=1}^K(M_{\rm nearest}(T_\xi h_k))
$$

其中，$W$ 是搜索窗口，$M_{\rm nearest}$ 是 $M$ 扩展至 $\mathbb{R}^2$ 得到值。具体扩展的方式：首先圆整输入位置到最近的栅格点，然后使用对应的像素的概率值作为结果。

通过小心选择步长可以提高效率。这里选择使用一个角度 $\delta_{\theta}$ 作为步长，使得最远距离 $d_{\rm max}$的激光点不会移动超过 $r$，即一个像素的宽度。可以通过余弦法则计算出该步长：

$$
\begin{aligned}
d_{\rm max} &= \max_{k=1,...,K}||h_k|| \\
\delta_{\theta} &= \arccos{(1 - \frac{r^2}{2d^2_{\rm max}})}
\end{aligned}
$$

对于角度的搜索分辨率 Cartographer 采用上述等式计算得出的 $\delta_{\theta}$，而对于线性搜索分辨率则事先指定 $r$，因此搜索窗口的尺寸可以根据分辨率以及总长度（$W_x = W_y = 7{\rm m}, W_{\theta}=30\degree$）下式计算得出：

$$
\begin{aligned}
w_x &= \frac{W_x}{r} \\
w_y &= \frac{W_y}{r} \\
w_\theta &= \frac{W_\theta}{\delta_\theta}
\end{aligned}
$$

因此每次搜索范围通过以下有限集定义得出：

$$
\begin{aligned}
\bar{\mathcal{W}} &= \{-w_x,...,w_x\}\times \{-w_y,...,w_y\}\times\{-w_\theta, ..., w_\theta\} \\
\mathcal{W} &= \{\xi_0 + (rj_x,rj_y,\delta_\theta j_\theta):(j_x,j_y,j_\theta)\in\bar{\mathcal{W}}\}
\end{aligned}
$$

关于怎么搜索最优位姿，论文中给出了两个方法，首先第一种是暴力递归搜索空间中所有解并选择分数最优的，如下所示：

这种方法在搜索窗口较大时速度很慢，因此 Cartographer 中实际使用的是一种分支定界算法，算法原理具体在 An automatic method of solving discrete programming problems 有介绍，基本算法如下所示：

算法的主要思想是将包含不同数量的解的子集表示为了树中的不同节点，而根节点则表示了所有的可能解（在本问题上即上文定义的 $\mathcal{W}$）。每个节点的子节点则表示该节点包含的可能解的不同部分，并且所有子节点包含的解和该节点本身包含的解是一致的。树中每个根节点都表示一个特定的解。同时，当每个内部节点的分数 $score(c)$ 是它对应元素的分数上界，算法本身结果是确定的，即得出的解和 Algorithm 1 的得到的解是一样的。在这种情况下，只要一个节点是有界的，在该子树上就不存在并当前最优解更优的解。

论文中为了根据问题完善这种算法，主要介绍了以下三个方面的方法：节点选择、分支以及计算上界。

  • 节点选择

算法中使用了深度优先搜索（DFS）作为默认选项来进行节点选择。算法的效率依赖树的最大深度，即以来两个参数：一个好的上界（有助于最大程度进行路径的筛选）以及一个好的当前解。后者由于 DFS 可以快速评估叶节点所以有所改善。并且由于作者不希望将不那么好的匹配也加入约束中，因此在筛选出的最优解中还设置了一个阈值，如果匹配度低于该阈值则不将其加入回环约束中。在实际中这个阈值不常被超过，因此降低了节点选择的重要性。在 DFS 过程遍历叶节点的顺序上，它们计算每个子节点的上界，并从中选择最高上界的子节点进行优先遍历，如果算法 3 所示。

  • 分支规则

每一个树中的节点由一个整数 tuple 进行表示 $c = (c_x, c_y, c_\theta, c_h)\in\mathbb{Z}^4$，高度为 $c_h$ 的节点包含最多 $2^{c_h}\times 2^{c_h}$ 可能的平移情况（转向固定），如下所示：

$$
\bar{\bar{\mathcal{W}}}_c = (\{(j_x,j_y)\in\mathbb{Z}^2: \begin{aligned}
c_x\leq j_x \leq c_x + 2^{c_h} \\
c_y\leq j_y \leq c_y + 2^{c_h}
\end{aligned}\} \times \{c_\theta\}),
\bar{\mathcal{W}}_c = \bar{\bar{\mathcal{W}}}_c \cap \bar{\mathcal{W}}
$$

其中叶节点的高度 $c_h = 0$ 因此只关联一个特定解 $\xi_c = \xi_0 + (rc_x,rc_y,\delta_\theta c_\theta) \in \mathcal{W}$

正如算法 3 所示，包含所有解的根节点并不是显式出现，而是分支为一系列固定高度为 $h_0$初始节点 $\mathcal{C}_0$ 的集合，该集合包含了如下的搜索窗口：

$$
\begin{aligned}
\bar{\mathcal{W}}_{0,x} &= \{-\mathcal{w}_x + 2^{h_0}j_x: j_x\in\mathbb{Z},0\leq 2^{h_0}j_x \leq 2\mathcal{w}_x\}, \\
\bar{\mathcal{W}}_{0,y} &= \{-\mathcal{w}_y + 2^{h_0}j_y: j_y\in\mathbb{Z},0\leq 2^{h_0}j_y \leq 2\mathcal{w}_y\}, \\
\bar{\mathcal{W}}_{0,\theta} &= \{j_\theta \in \mathbb{Z}: -\mathcal{w}_\theta \leq j_\theta \leq \mathcal{w}_\theta\}, \\
\mathcal{C}_0 &= \bar{\mathcal{W}}_{0,x} \times \bar{\mathcal{W}}_{0,y} \times \bar{\mathcal{W}}_{0,\theta}
\end{aligned}
$$

在任意给定高度 $c_h > 1$ 的节点中，我们将其分支为高度为 $c_h - 1$ 的四个子节点：

$$
\mathcal{C}_c((\{c_x, c_x+2^{c_h-1}\} \times \{c_y, c_y+2^{c_h-1}\} \times c_\theta)\cap\bar{\mathcal{W}}) \times \{c_h - 1\}
$$
  • 计算上界

作者使用下述方法来表示节点的上界：

$$
\begin{aligned}
score(c) &= \sum_{k=1}^K\max_{j\in\bar{\bar{\mathcal{W}}}_c} M_{\rm nearest}(T_{\xi_j}h_k) \\
&\geq \sum_{k=1}^K\max_{j\in\bar{\mathcal{W}}_c} M_{\rm nearest}(T_{\xi_j}h_k) \\
&\geq \max_{j\in\bar{\mathcal{W}}_c} M_{\rm nearest}(T_{\xi_j}h_k)
\end{aligned}
$$

为了提高计算最大值的效率，这里他们使用了预先计算的栅格 $M_{\rm precomp}^{c_h}$。在每一个可能的高度 $c_h$ 预先计算一个栅格，可以让我们在扫描激光点的线性时间内计算分数。需要注意的是，为了完成这个目的，还需要计算 $\bar{\bar{\mathcal{W}}}_c$ 的最大值，因为其有可能在搜索范围的边界上有可能会大于 $\bar{\mathcal{W}}_c$：

$$
\begin{aligned}
score(c) &= \sum_{k=1}^K M_{\rm precomp}^{c_h}(T_{\xi_c}h_k) \\
M_{\rm precomp}^{c_h}(x, y) &= \max_{
    \begin{aligned}
    x'\in [x,x+r(2^h-1)] \\
    y'\in [y,y+r(2^h-1)]
    \end{aligned}
} M_{\rm nearest}(x',y')
\end{aligned}
$$

其中 $\xi_c$ 表示根节点，注意这里 $M_{\rm precomp}^h$ 的像素结构和 $M_{\rm nearest}$ 一样，但是在每一个像素中存储的是以该位置为中心的 $(2^h\times 2^h)$ 个最大值。该栅格的例子如下所示：

为了将构造该栅格的成本降低，Cartographer 会等待知道概率栅格不再更新为止才进行构造，此时会计算一系列高度的预计算栅格由于进行匹配。

对于任何一个预计算栅格，算法会计算以该栅格为起点的宽度为 $2^h$ 的像素点的最大值，并且使用这个结果可以构造下一个预计算栅格。

当一系列值发生增减时其最大值可以在 O(1) 时间内进行更新，只要值的删除的顺序和增加的顺序保持一致（先进先出）。这个算法可以用一个 deque 实现，通过这种算法可以在 O(n) 复杂度内计算预计算该栅格，n 为该预计算栅格的像素个数。

另一种替代的方法是计算较低分辨率的概率栅格，不过 Cartographer 中没有使用所以不进行介绍。

6.实验结果

这里论文展示了 Cartographer 在一些数据集上的表现：

  • Real-World Experiment: Deutsches Museum
    • 传感器数据时长及范围：1913 s, 2253 m
    • 计算匹配：Intel Xeon E5-1650 @ 3.2 GHz
    • 算法结果
      • 消耗资源：1018 s CPU 时间，2.2 GB 内存，4线程，实际上在 360 s 时钟时间内完成（多线程的原因），所以实际完成速度是实时速度的 5.3 倍
      • 构建回环优化图包含 11456 个节点和 35300 条边，每一个解通常使用 3 个循环求得，耗时 0.3 s
      • 结果如图：
  • Real-World Experiment: Neato’s Revo LDS
    • 传感器数据时长及范围：Revo LDS 传感器，2 Hz
    • 算法结果
      • 结果如图：
      • 精确度（和人工作图相比较）：
© 2020 - 2022 XT 的技术博客
Built with Hugo
Theme Stack designed by Jimmy