基于 2D 激光雷达和实时回环优化的 SLAM 算法

基于 2D 激光雷达和实时回环优化的 SLAM 算法

原文： https://www.ixueshu.com/document/771d692c7d3c0c40318947a18e7f9386.html

此论文在 SLAM 的背景下，使用了一些机器学习的方法（最小二乘、损失函数等），很有学习的价值。(精译版；翻译不易，转载请注明出处；翻译过程加入了个人理解，如有差错，敬请指正)

摘要: 在构建竣工平面图的过程中，往往会用到便携式激光测距仪(LIDAR)和同步式定位与测绘(SLAM)。操作员在进行构图的过程中，如果能看到实时生成的平面图，这将非常有助于他评估最新捕获数据的质量和覆盖范围。另外，要构建便携式的数据捕获平台，还需要实现在有限的计算资源下完成构图操作。我们提出的方法，已经应用于了我们自己的背包式构图平台，实现了实时构图和回环优化，分辨率为 5cm。(注： 回环优化的目的是为了通过找到重复探测的空间然后构建约束，从而减小累积误差 )为了实现实时回环优化，我们使用了分支界限法作为约束条件，用于计算 scan-to-submap 匹配。在最后，我们提供了实验结果，并与其他众所周知的方法进行了比较。结果表明，在构图质量上，我们的方法与过往已有相比是有优势的。 (scan-to-submap 是本论文中的关键概念，不予主观翻译)

一、背景介绍

 竣工平面图广泛应用于各个领域。在建筑管理相关工作中，手动构图通常使用激光卷尺进行测量，并用 CAD 作图。显然，这种方法是很低效的，同时，基于主观直觉和简单直线的拼凑形成的构图，往往不能精确描绘实际的空间结构。使用 SLAM，可以快速而准确地测量建筑物的大小和复杂程度，而手动测量耗费的时间则会多几个数量级。

二、相关研究

 在基于激光雷达的 SLAM 算法中，scan-to-scan 匹配常用于计算相对位姿变化，例如 [1]-[4]。然而，由于算法本身特性的问题，scan-to-scan 匹配会迅速积累错误。 (注：相对位姿变化的含义是，在经过某个时间段后，激光雷达相对于之前的位置和角度的变化)
 Scan-to-map 匹配有助于抑制这种错误的积累。其中一种方法，它使用 Gauss-Newton 法在线性插值的地图上寻找局部最优值，参见 [5]。在这种情况下，通过使用足够高分辨率的激光雷达对姿态进行良好的初始估计，局部优化的scan-to-map 匹配是有效的，并具备鲁棒性。在路面不平整的情况下，可以使用惯性测量单元(IMU)估计重力方向，从而将激光雷达的扫描平面投射到水平面上。
 Pixel-accurate-scan 匹配方法，进一步减少了局部误差积累，参见 [1]。 尽管这种方法比较消耗计算资源，但它也适用于回环检测。 一些方法侧重于匹配从激光扫描中提取的特征，从而增加了计算资源的消耗，参见 [4]。另外还有一些回环检测方法，包括：基于直方图的匹配(参见 [6])、激光扫描中的特征检测、以及机器学习(机器学习 [7])。
 粒子滤波和 graph-based SLAM 算法，常常被用于解决残余局部误差的积累问题。
 粒子滤波必须在每个粒子中维护一个完整的系统状态。对于基于网格的 SLAM 来说，随着地图的变大，粒子滤波很快就会变成计算密集型的算法；例如，我们的一个测试用例是在 3km 的轨道型空间上测绘 22000 $m^2$ 的地图。不为每个粒子提供网格图，从而降低维度特征，可以用来减少计算资源的消耗，参见 [9]。当需要更新网格图时，[10] 建议计算 submap，只有在必要时才更新，这样最终的地图就是所有 submap 的栅格化。(submap 是本论文中的关键概念，不予主观翻译)
 Graph-based 方法是基于一组表示位姿和特征的数据节点。观察结果产生的约束构成了 Graph 中的边。可以使用各种优化方法来最小化所有约束带来的误差，参见 [11]、[12]。[13] 中描述了一个户外 SLAM 系统，该系统使用了 Graph-based 方法、局部 scan-to-scan 方法、以及基于submap 特征直方图的局部重叠匹配方法。

三、系统概述

 谷歌的 Cartographer 提供了一个实时的室内测绘解决方案，它采用了一个配备传感器的背包，可以生成分辨率为 5cm 的 2D 网格地图。该系统的使用人员可以在穿过建筑物时看到实时创建的地图。激光雷达的扫描数据会被插入到 submap 中的最佳估计位置，在足够短的某一时间段内来看，结果是准确的。扫描匹配只针对最近的一个 submap 进行的，换句话说 submap 只依赖于最近的扫描，而位姿估计的误差会累积在全局的数据帧(frame)中。
 为了在硬件要求不高的情况下获得良好的性能，我们的 SLAM 方法没有使用粒子滤波。为了解决积累误差，我们会定期运行一次全局的位姿优化。当一个 submap 完成时，也就是不再插入新的扫描时，它就会被用来进行回环优化的扫描匹配。所有扫描数据和完成的 submap 都会被自动用于回环优化。根据当前的位姿进行对比计算，如果它们足够靠近，扫描匹配器就会尝试在 submap 中进行搜索。如果在搜索窗口(当前估计的位姿周围)中找了到足够好的匹配，则将其作为回环约束(loop closing constraint)用于位姿优化。实际的操作经验证明，通过每隔几秒钟完成一次回环优化，当某个位置被重新访问时，回环会立刻完成。这对软件实时性提出了严格的要求：回环优化的扫描匹配必须比新扫描的添加速度更快，否则将明显落后。为了实现该实时性，我们采用了分支边界法，并为每个完成的 submap 创建了若干个预计算网格。

四、局部优化

 我们的系统分别包含了局部和全局的 2D SLAM 方法。两种方法都被用来优化位姿数据，位姿数据 $\xi =({\xi }_x,{\xi }_y,{\xi }_{\theta })$ 代表激光雷达采样数据的平移 (x,y) 和旋转 ${\xi }_{\theta }$ ，激光雷达采样数据后面会称之为扫描(scans)。在一个不稳定的平台上，比如我们的背包，需要一个 IMU 用来估计重力的方向，从而把激光雷达的扫描数据投射到水平的 2D 空间。
 在我们的局部优化方法中，我们设定连续的若干个扫描会对应全局中的一小块区域，而这一小块区域被称为 submap M，然后我们使用了非线性优化(最小二乘法)将扫描与 submap 对齐(align)；这个过程后面称之为扫描匹配(scan matching)。扫描匹配会随着时间积累误差，我们会用全局的方法来消除这些误差，参见第五节的内容。

A. 扫描(scans)

 Submap 的构造是对扫描和 submap 的坐标数据帧(后面统称为帧 frames)进行多次对齐(align)的迭代过程。设定扫描的原点是 $0\in {\mathbb{R}}^2$，我们把所有的扫描数据点表示为 H = { h k } k = 1 , . . . , K , h k ∈ R 2 H=\{h_k\}_{k=1,...,K},h_k \in \Reals^2 H={ hk​}k=1,...,K​,hk​∈R2。扫描帧的位姿 $\xi$，在 submap 帧中表示为转换 $T_{\xi }$ ， 其的含义是：把扫描帧中的扫描点通过旋转和平移转换为 submap 的帧数据。计算过程如下：

$T_{\xi }h=\underset{R_{\xi }}{\underbrace{\left(\begin{array}{cc}cos{\xi }_{\theta }& -sin{\xi }_{\theta }\\ sin{\xi }_{\theta }& cos{\xi }_{\theta }\end{array}\right)}}h+\underset{t_{\xi }}{\underbrace{\left(\begin{array}{c}{\xi }_x\\ {\xi }_y\end{array}\right)}}$. (1)

B. Submap

 Submap 由几个连续的扫描构建而成。submap 采用了概率网格的形式 $M:r\mathbb{Z}\times r\mathbb{Z}\to [p_{min},p_{max}]$，从给定分辨率(例如 5cm)的离散网格点映射到值(如图 F1)。 这些值代表相应网格点存在障碍物的概率。对于每个网格点都对应一个像素点，相应的像素代表离它最近的所有点（每个一个像素是尺寸为 $r\times r$ 的方格，它代表对应 $r\times r$ 范围内的所有点 ，注意：扫描数据点是实数，概率网格的尺寸是整数）

 每次将扫描插入概率网格时，我们都会计算一个命中的网格点的集合（代表障碍物），以及另外一个和它不相交的未命中的网格点的集合（代表无障碍空间）。对于每一次命中，我们会把离命中点最近的网格点（像素点）加入到命中集合(hit set)，同时，我们会把原点与扫描点之间的所有像素，加入到未命中网格点集合（如图 F2 所示）。对于每一个以前未被观测到的网格点，如果它在其中一个集合里，就会被赋予一个概率 $p_{hit}$ 或 $p_{miss}$ 。如果是之前已经观察到网格点 $x$，我们将命中或未命中的概率更新为：

$odds(p)=\frac{p}{1-p}$. (2)

$M_{new}(x)=clamp(odd{s}^{-1}(odds(M_{old}(x))\cdot odds(p_{hit})))$. (3)

上述公式中， $clamp()$ 可以将数值限制在 $[p_{min},p_{max}]$ 区间，未命中的公式和上述公式一样。（注意：$odds$ 函数把概率值转换为胜率值， $odd{s}^{-1}$ 是 $odds$ 的反函数）

C. 基于 Ceres 的扫描匹配

 把一个扫描插入一个 submap 的过程，就是计算它们的最优相对位姿 $\xi$ 的过程，也是局部优化的过程，称之为扫描匹配。计算 $\xi$ 使用的是基于 Ceres 的扫描匹配器（Ceres Solver 是谷歌出品的一款用于非线性优化的库），参见 [14]。扫描匹配器负责找到一个扫描位姿，该位姿使 submap 中扫描点的概率最大化。我们把它转换成为了一个非线性最小二乘问题，公式如下：

$\underset{\xi }{\mathrm{argmin}}{\sum }_{k=1}^K(1-M_{smooth}(T_{\xi }{h}_k){)}^2$ (CS)

上述公式中，使用 $T_{\xi }$ 把扫描数据帧 $h_k$ 转换为 submap 数据帧。另外，函数 $M_{smooth}:{\mathbb{R}}^2\to \mathbb{R}$ 是局部 submap 概率值的平滑版本。我们使用双三次插值。因此，区间 [0,1] 之外的值可能出现，但会被认为是无害的。（注意：$\underset{x}{\mathrm{argmin}}f(x)$ 的含义是：使 $f(x)$ 具有最小值 M 的 $x$ 的值的集合）
 这种光滑函数的数学优化结果通常比网格的分辨率精度更高。由于是局部优化，因此对初始状态的估计值越准确越好。一个测量角速度的 IMU 可以用来估计两次扫描匹配间相对位姿的旋转分量 $\theta$ 。在没有 IMU 的情况下，可以使用更高频率的扫描匹配或 pixel-accurate 的扫描匹配方法，但是这需要更大的计算量。

五、全局优化(回环优化)

 由于新的扫描只会与包含最近几个扫描的 submap 匹配，因此上面描述的方法积累误差的过程很慢，同时只有几十次连续扫描，累积误差很小。
 面对较大的空间时，通过创建多个 submap 来处理。我们的方法中，使用了稀疏位姿调整（SPA），对全部扫描和 submap 的位姿进行整体优化，参见 [2]。把扫描插入 submap 时使用的相对位姿，会被存储在内存中，以便在回环优化中使用。除了这些相对姿态之外，一旦 submap 不再变化，所有其他 (scan，submap) 数据对都将被用于回环优化。扫描匹配器在后台运行，如果获得良好的匹配（也就是回环约束），则会将相应的相对位姿添加到优化(SPA)计算中。

A. 如何优化

 与扫描匹配一样，回环优化也被表述为一个非线性最小二乘问题，这样一来，实时计算的过程，可以简单地使用添加残差（实际观察值与估计值/拟合值之间的差）的方式实现，从而处理不断增加的新数据。每隔几秒钟，我们都会使用 Ceres [14] 计算出一个解：

$\underset{{\Xi }^m,{\Xi }^s}{\mathrm{argmin}}\frac{1}{2}{\sum }_{ij}\rho (E^2({\xi }_i^m,{\xi }_j^s;{\Sigma }_{ij},{\xi }_{ij}))$ (SPA)

上述公式中，我们在给定的一些回环约束(loop closing constraint)条件下，优化全局中的 submap 位姿 Ξ m = { ξ i m } i = 1 , . . . , m \Xi^m = \{\xi_i^m\}_{i=1,...,m} Ξm={ ξim​}i=1,...,m​，以及扫描位姿 Ξ s = { ξ j s } j = 1 , . . . , n \Xi^s = \{\xi_j^s\}_{j=1,...,n} Ξs={ ξjs​}j=1,...,n​ 。这些约束条件采使用相对位姿 ${\xi }_{ij}$ 和协方差矩阵 ${\Sigma }_{ij}$ 来表示。对于一对数据： submap $i$ 和 scan $j$，相对位姿 ${\xi }_{ij}$ 的意思是：扫描在 submap 的坐标帧中匹配到的位置。协方差矩阵 ${\Sigma }_{ij}$ 是求值所得，例如，可以使用 [15] 中的方法，或者参照公式(CS)，使用 Ceres [14] 中的协方差估计特征。 对于这样的约束条件，残差 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})$, (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)$. (5)

上述公式中，${\xi }_{i;\theta }^m-{\xi }_{j;\theta }^s$ 是旋转角度分量， $R_{{\xi }_i^m}^{-1}(t_{{\xi }_i^m}-t_{{\xi }_j^s})$ 是坐标平移分量。
 当扫描匹配的的结果（相对位姿和协方差矩阵等）不准确时， 可能导致 (SPA) 产出异常值。使用损失函数 $\rho$（例如 Huber 损失），可以降低异常值的影响。实验表明，异常可能发生在局部对称的环境中，例如办公隔间等。针对异常值的替代方法参见 [16]。

B. 基于分枝界限法的扫描匹配

 我们希望得到符合预期并精确到像素的位姿 ${\xi }^{\ast }$ ：

${\xi }^{\ast }=\underset{\xi \in W}{\mathrm{argmin}}{\sum }_{k=1}^K{M}_{nearest}(T_{\xi }{h}_k)$, (BBS)

上述公式中，$W$ 是搜索窗口， $M_{nearest}$ 是 $M$ 首先将参数四舍五入到最近的网格点，也就是将网格点的值扩展到相应的像素 。可以使用公式 (CS)进一步提高匹配质量。
 认真选择扫描步长，可以提高效率。我们选择角步长为 ${\delta }_{\theta }$ ，从而保证扫描数据中的最大射程 $d_{max}$ 每次移动移动不超过 $r$ （一个像素的宽度）。利用余弦定理，我们可以得到：

$d_{max}=\underset{k=1,...,K}{\max }\Vert h_k\Vert$, (6)

${\delta }_{\theta }=arccos(1-\frac{r^2}{2d_{max}^2})$. (7)

 如果给定搜索窗口大小(平移和旋转两个维度)，我们可以计算出需要的搜索比对次数（是一个整数值），如，$W_x=W_y=7m$ , $W_{\theta }=30°$：

$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$. (8)

 这将生成一个有限集合 $W$ 组成的搜索窗口，窗口的中心点是估计值 ${\xi }_0$：

W ‾ = { − w x , . . . , w x } × { − w y , . . . , w y } × { − w θ , . . . w θ } \overline{W} = \{-w_x,...,w_x\} \times \{-w_y,...,w_y\} \times \{-w_\theta,...w_\theta\} W={ −wx​,...,wx​}×{ −wy​,...,wy​}×{ −wθ​,...wθ​}, (9)

W = { ξ 0 + ( r j x , r j y , δ θ j θ ) : ( j x , j y , j θ ) ∈ W ‾ } W = \{\xi_0 + (rj_x,rj_y,\delta_{\theta}j_{\theta}):(j_x, j_y, j_\theta) \isin \overline{W}\} W={ ξ0​+(rjx​,rjy​,δθ​jθ​):(jx​,jy​,jθ​)∈W}. (10)

综上所述，简单可以得到一个查找 ${\xi }^{\ast }$ 的朴素算法，参见 Algorithm 1。但是我们需要考虑搜索窗口极大的情况，这个算法速度太慢了。

 在搜索窗口更大的情况下，我们使用分支界限法，可以大幅度提高计算 ${\xi }^{\ast }$ 的效率。Algorithm 2 展示了分支界限法的通用模型。这种方法最初在 mixed integer linear program 中被提及，参见 [17]。关于这个主题的文献非常多，在 [18] 中对它们有一个简短的概述。

 其主要思想是，将可能性的子集表示为树中的节点，其中根节点表示所有可能的解，在我们的例子中是W。每个节点的子节点形成其父节点的一个分区，因此它们合在一起表示了和父节点相同的一组可能性。叶节点是单节点；每一个都代表一个可能的解。关于算法的准确性：只要内部节点 $c$ 的得分($score(c)$)是其所有子节点得分中的最大值，通过它就能得到与朴素方法相同的解。在这种情况下，只要当一个节点是有界限的，这个子树中就不存在比已知的最好的解决方案更好的解决方案。
 为了得到一个具体的算法，我们必须确定如何去：节点选择、进行分枝(Branch)、计算极大边界(Upper bound)。
1）节点选择： 我们的算法默认选择使用深度优先搜索(DFS)，因为这是目前最好的选择：该方法之所以效率更高，因为树的很大一部分会被修剪掉。这取决于两件事：一个良好的极大边界和一个良好的当前解。后一部分借助于 DFS，它可以快速计算大量叶节点。由于我们不希望添加糟糕的匹配作为回环约束，所以我们还引入了一个阈值，我们对低于这个阈值的最优解不感兴趣。由于在实践中阈值通常不会被超过，节点选择和寻找初始的启发式解就显得没那么重要了。对于在 DFS 期间访问子节点的顺序，我们计算每个子节点得分的上界，首先访问拥有最大的上界的子节点，因为它是最优解的可能性最大。参见 Algorithm 3。

2）分枝的原则（树的结构）： 树中的每个节点都是这样一个整数元组：$c=(c_x,c_y,c_{\theta },c_h)\in {\mathbb{Z}}^4$。 在树的第 $c_h$ 层的每一个节点，最多可以包含 $2^{c_h}\times 2^{c_h}$ 个平移值，但只包含一个特定的旋转值 ：

W ‾ ‾ c = ( { ( 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 θ } ) \overline{\overline{W}}_c = \bigg( \bigg\{ (j_x,j_y) \isin \Z^2 : \begin{alignedat}{2} c_x \le j_x < c_x + 2^{c_h} \\ c_y \le j_y < c_y + 2^{c_h} \end{alignedat}\bigg\} \times \{c_\theta\} \bigg) Wc​=({ (jx​,jy​)∈Z2:cx​≤jx​<cx​+2ch​cy​≤jy​<cy​+2ch​​}×{ cθ​}), (11)

${\overline{W}}_c={\overline{\overline{W}}}_c\cap \overline{W}$. (12)

叶节点在树中对应 $c_h=0$ 层，对应着完整的解空间： $W\ni {\xi }_c={\xi }_0+(r{c}_x,r{c}_y,{\delta }_{\theta }{c}_{\theta })$ 。
 在我们的 Algorithm 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 } . \overline{W}_{0,x} = \{-w_x + 2^{h_0}j_x : j_x \isin \Z, 0 \le 2^{h_0}j_x \le 2w_x\}, \\ \overline{W}_{0,y} = \{-w_y + 2^{h_0}j_y : j_y \isin \Z, 0 \le 2^{h_0}j_y \le 2w_y\}, \\ \overline{W}_{0,\theta} = \{j_\theta \isin \Z : -w_\theta \le j_\theta \le w_\theta\}, \\ C_0 = \overline{W}_{0,x} \times \overline{W}_{0,y} \times \overline{W}_{0,\theta} \times \{h_0\}. W0,x​={ −wx​+2h0​jx​:jx​∈Z,0≤2h0​jx​≤2wx​},W0,y​={ −wy​+2h0​jy​:jy​∈Z,0≤2h0​jy​≤2wy​},W0,θ​={ jθ​∈Z:−wθ​≤jθ​≤wθ​},C0​=W0,x​×W0,y​×W0,θ​×{ h0​}. (13)

 在 $c_h>1$ 的给定节点 $c$ 上，我们最多可以分支得到 4 个高度为 $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 } C_c = \bigg( (\{ c_x,c_x + 2^{c_h - 1} \} \times \{ c_y,c_y + 2^{c_h - 1} \} \times c_\theta)\cap \overline{W} \bigg) \times \{c_h - 1\} Cc​=(({ cx​,cx​+2ch​−1}×{ cy​,cy​+2ch​−1}×cθ​)∩W)×{ ch​−1} (14)

3）计算极大边界：分枝界限法的剩下的部分就是如何有效地（保证计算工作量和边界值的质量）计算内部节点的上界。我们使用了：

$$\begin{gathered} score(c)={\sum }_{k=1}^K\underset{j\in {\overline{\overline{W}}}_c}{\max }{M}_{nearest}(T_{{\xi }_j}{h}_k) \\ \ge {\sum }_{k=1}^K\underset{j\in {\overline{W}}_c}{\max }{M}_{nearest}(T_{{\xi }_j}{h}_k) \\ \ge \underset{j\in {\overline{W}}_c}{\max }{\sum }_{k=1}^K{M}_{nearest}(T_{{\xi }_j}{h}_k) \end{gathered}$$ (15)

 为了能更高效地计算最大值，我们使用了预计算网格 $M_{precomp}^{c_h}$。预先为树的每一层 $c_h$ 计算一个网格。这样可以使我们计算得分(score)所用的计算量，与扫描点的数量呈线性关系。请注意，为了做到这一点，我们还计算了 ${\overline{\overline{W}}}_c$ 的最大值，在搜索空间的边界附近 ${\overline{\overline{W}}}_c$ 可能比 ${\overline{W}}_c$ 大。

$score(c)={\sum }_{k=1}^K{M}_{precomp}^{c_h}(T_{{\xi }_c}{h}_k)$, (16)

$M_{precomp}^h(x,y)=\underset{\begin{array}{r}{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 })}$. (17)

上述公式中， ${\xi }_c$ 和之前的叶节点一样。注意 $M_{precomp}^h$ 的网格结构与 $M_{nearest}$ 相同，但是在每个网格（像素）中存储的是从那里开始的 $2^h\times 2^h$ 个的像素的最大值。图 F3 给出了这种预先计算的网格的一个示例。

 我们在概率网格不再接收任何更新之后，才构建预计算网格，并开始与之匹配。这样降低构建预计算网格的计算量。
 对于每个预计算网格，我们计算从每个像素开始的行(宽为 $2^h$ 个像素)的最大值。使用这个中间结果，然后构造下一个预计算网格。
 如果按添加值的顺序删除值，则在 $O(1)$ 实现复杂度下，更改值集合的最大值可以保持为最新。 连续最大值保持在双端队列中，该双端队列可以递归定义为包含当前集合中所有值的最大值，然后是第一次出现最大值后所有值的连续最大值列表。 对于空值集合，此列表为空。 使用这种方法，构建预计算网格的时间复杂度是 $O(n)$，其中 $n$ 是每个预计算网格中的像素数。
 另一种计算上界的方法是降低概率网格的分辨率，依次将分辨率减半，参见 [1]。由于我们的方法中消耗的额外内存在承受范围内，因此我们不推荐使用低分辨率的概率网格，与公式 (15) 相比，这会产生更差的边界，从而导致最终构图效果变差。

六、试验结果

 在本节中，我们将展示我们的 SLAM 算法的一些实验结果，这些结果是利用在线算法从记录的传感器数据中计算出来的，传感器数据来自背包，在线算法和背包中的交互算法是相同的。首先，我们使用 Cartographer 背包收集了位于慕尼黑的德国博物馆的数据来，并展示了结果。其次，通过使用了吸尘器机器人的传感器收集的数据，我们证明了我们的算法在廉价硬件上运行良好。最后，我们使用 Radish 据集，并展示了结果，并与已发表的结果进行比较，参见 [19]。

A. 实地实验：慕尼黑博物馆

 使用德国博物馆收集的数据 1913s 的传感器数据或 2253m(根据计算结果)，我们计算得到了如图 F4 所示的地图。我们使用了 Intel Xeon E5-1650（主频 3.2 GHz） 的工作站上，我们的 SLAM 算法使用了 1018 s CPU 时间，最多使用了 2.2 GB 内存，同时最多 4 个后台线程进行回环扫描匹配。它在 360s 的挂钟时间（主观时间）后结束，这意味着它实现了 5.3 倍的实时性能。
 为回环优化生成的图由 11456 个节点和 35300 条边组成。每次向图中添加几个节点时，都会运行(SPA)。平均来说，每次优化运算大约需要迭代 3 次，大约在 0.3 秒内完成。

B. 实地试验：Neato’s Revo LDS

 来自 Neato Robotics 的吸尘器机器人使用的激光测距传感器(LDS) 称之为 Revo LDS，该传感器的售价地域 30 美元，参见 [20]。我们使用手推车推着吸尘器来获取数据，同时通过它的 debug 连接以大约 2hz 的频率获取扫描数据。图 F5 显示了最终的 5cm 分辨率的平面图。为了评估平面图的质量，我们选取了 5 条直线，分别对比了它们的激光卷尺测量值与我们计算结果中的测量值。结果如 TABLE I 所示，所有数值均以米为单位。这些值大致按照行两端一个像素的预期大小顺序排列。

C. 和 Radish 数据集的对比结果

 我们使用 [21] 中建议的基准测量方法将我们的方法与其他方法进行了比较，该方法将相对姿态变化中的错误与手动管理的地面真值关系进行了比较。Table II 显示了我们的 SLAM 算法计算的结果。为了进行比较，我们引用了来自 [21] 的图映射(GM)结果。此外，我们在 Table III 中引用了最近发表的来自 [9] 的结果。所有的误差都以米和度为单位，包括绝对误差和平方误差，以及它们的标准差。
每个公共数据集都使用一个独特的传感器配置进行收集，这与我们的制图背包不同。因此，需要调整各种算法参数来产生合理的结果。根据我们的经验，调优地图绘制器只需要将算法与传感器配置匹配，而不需要与特定的环境匹配。

 因为每个公共数据集都有一个专有的传感器配置，所以我们不能确定我们有没有将参数匹配到特定的位置。唯一例外的是 Freiburg 医院的数据集，它有两个独立的关系文件。分别使用这两个文件进行测试，我们都得到了不错的结果。
 所有数据集之间最显著的差异是激光扫描的频率和质量，以及测程的可用性和质量。
 尽管公共数据集中使用的传感器硬件相对过时，但我们的 SLAM 的性能始终在预期之内，甚至在MIT CSAIL 的情况下，我们的性能要比 Graph Mapping 差得多。对于 Intel 的数据集，我们的表现优于 Grap Mapping，但相对于 Graph FLIRT 较差。对于 MIT Killian Court，我们在所有指标上都优于 Grap Mapping。在所有其他情况下，在大多数情况下(但不是所有情况下)，我们的算法都比Graph Mapping 和 Graph FLIRT 更出色。
 由于我们在 submap 和扫描之间添加了回环约束，因此数据集不包含它们的 Ground Truth（标准答案）。很难与其它基于 scan-to-scan 的方法进行比较。表 Table IV 显示了为每个测试用例中匹配到的回环约束的数量及准确度。当我们计算(SPA)时，所谓正确的约束，也就是误差不超过 20cm 或 1° 的约束。虽然我们的 scan-to-submap 匹配过程产生了错误的约束，并用于了(SPA)计算，但它还是能够在所有测试用例中提供足够数量的回环约束。我们在(SPA)中使用 Huber 损失函数是使(SPA)对异常回环具有鲁棒性。在 Freiburg 医院的例子中，如果选择低分辨率和较低的回环检测分数阈值，会产生相对较高的误报率。提高回环检测的分值阈值可以提高检测精度，但是，根据 Ground Truth，在某些维度上这也会降低最终结果的质量。我们认为 Ground Truth 仍然是最终地图质量的更好基准。

 我们的 SLAM 中的参数并没有针对 CPU 性能进行调整 。表 Table V 中提供的测量数据是程序执行的挂钟时间，该测试运行于 Intel Xeon E5-1650（主频 3.2 GHz）工作站。作为比较，我们同时提供了传感器数据的持续时间。

七、结论

 本文提出并实验验证了一个将 scan-to-submap 匹配与回环检测和图优化相结合的二维 SLAM 系统。使用我们局部的、基于网格的 SLAM 方法可以用来创建独立的 dubmap 轨迹。后台中，会使用像素精度的扫描匹配来创建回环约束，并将所有的扫描都被匹配到就近的 submap。后台中，会周期性地优化 submap 与扫描位姿的约束图。使用了 GPU 加速，可以让系统操作员实时看到最新的预览图，预览图由完成的 submap 和当前 submap 组合而成。我们证明了在适当的硬件上实时运行我们的算法是可能的。

参考文档

[1] E. Olson, “M3RSM: Many-to-many multi-resolution scan matching,”
in Proceedings of the IEEE International Conference on Robotics and
Automation (ICRA), June 2015.
[2] K. Konolige, G. Grisetti, R. Kummerle, W. Burgard, B. Limketkai, ¨
and R. Vincent, “Sparse pose adjustment for 2D mapping,” in IROS,
Taipei, Taiwan, 10/2010 2010.
[3] F. Lu and E. Milios, “Globally consistent range scan alignment for
environment mapping,” Autonomous robots, vol. 4, no. 4, pp. 333–349, 1997.
[4] F. Mart´ın, R. Triebel, L. Moreno, and R. Siegwart, “Two different
tools for three-dimensional mapping: DE-based scan matching and
feature-based loop detection,” Robotica, vol. 32, no. 01, pp. 19–41,2014.
[5] S. Kohlbrecher, J. Meyer, O. von Stryk, and U. Klingauf, “A flexible
and scalable SLAM system with full 3D motion estimation,” in
Proc. IEEE International Symposium on Safety, Security and Rescue
Robotics (SSRR). IEEE, November 2011.
[6] M. Himstedt, J. Frost, S. Hellbach, H.-J. Bohme, and E. Maehle, ¨
“Large scale place recognition in 2D LIDAR scans using geometrical
landmark relations,” in Intelligent Robots and Systems (IROS 2014),
2014 IEEE/RSJ International Conference on. IEEE, 2014, pp. 5030–5035.
[7] K. Granstrom, T. B. Sch ¨ on, J. I. Nieto, and F. T. Ramos, “Learning to ¨
close loops from range data,” The International Journal of Robotics
Research, vol. 30, no. 14, pp. 1728–1754, 2011.
[8] G. Grisetti, C. Stachniss, and W. Burgard, “Improving grid-based
SLAM with Rao-Blackwellized particle filters by adaptive proposals
and selective resampling,” in Robotics and Automation, 2005. ICRA 2005.
Proceedings of the 2005 IEEE International Conference on. IEEE, 2005, pp. 2432–2437.
[9] G. D. Tipaldi, M. Braun, and K. O. Arras, “FLIRT: Interest regions for
2D range data with applications to robot navigation,” in Experimental Robotics.
Springer, 2014, pp. 695–710.
[10] J. Strom and E. Olson, “Occupancy grid rasterization in large environments for teams
of robots,” in Intelligent Robots and Systems (IROS),
2011 IEEE/RSJ International Conference on. IEEE, 2011, pp. 4271–4276.
[11] R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, ¨
“g2o: A general framework for graph optimization,” in Robotics and
Automation (ICRA), 2011 IEEE International Conference on. IEEE,
2011, pp. 3607–3613.
[12] L. Carlone, R. Aragues, J. A. Castellanos, and B. Bona, “A fast
and accurate approximation for planar pose graph optimization,” The
International Journal of Robotics Research, pp. 965–987, 2014.
[13] M. Bosse and R. Zlot, “Map matching and data association for largescale
two-dimensional laser scan-based SLAM,” The International
Journal of Robotics Research, vol. 27, no. 6, pp. 667–691, 2008.
[14] S. Agarwal, K. Mierle, and Others, “Ceres solver,” http://ceres-solver.org.
[15] E. B. Olson, “Real-time correlative scan matching,” in Robotics
and Automation, 2009. ICRA’09. IEEE International Conference on.
IEEE, 2009, pp. 4387–4393.
[16] P. Agarwal, G. D. Tipaldi, L. Spinello, C. Stachniss, and W. Burgard,
“Robust map optimization using dynamic covariance scaling,” in
Robotics and Automation (ICRA), 2013 IEEE International Conference on. IEEE, 2013, pp. 62–69.
[17] A. H. Land and A. G. Doig, “An automatic method of solving discrete
programming problems,” Econometrica, vol. 28, no. 3, pp. 497–520,1960.
[18] J. Clausen, “Branch and bound algorithms-principles and examples,”
Department of Computer Science, University of Copenhagen, pp. 1–30, 1999.
[19] A. Howard and N. Roy, “The robotics data set repository (Radish),” 2003. [Online].
Available: http://radish.sourceforge.net/
[20] K. Konolige, J. Augenbraun, N. Donaldson, C. Fiebig, and P. Shah,
“A low-cost laser distance sensor,” in Robotics and Automation, 2008.
ICRA 2008. IEEE International Conference on. IEEE, 2008, pp.3002–3008.
[21] R. Kummerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, ¨
C. Stachniss, and A. Kleiner, “On measuring the accuracy of SLAM
algorithms,” Autonomous Robots, vol. 27, no. 4, pp. 387–407, 2009.