Cartographer论文翻译(回环)

2023-05-16

摘要

便携式激光测距仪（也被称为激光雷达）和同步定位与建图（SLAM）技术是获取竣工平面图的有效方法。实时生成和可视化平面图有助于操作员评估捕获数据的质量和覆盖范围。构建一个便携式捕获平台需要在有限的计算资源下进行操作。我们介绍了我们的背包测试平台中使用的方法，该平台实现了5厘米分辨率的实时测绘和闭环检测。为了实现实时闭环检测，我们使用分支定界方法来计算scan-to-submap的匹配作为约束。我们提供了实验结果，并与其他众所周知的方法进行了比较，结果表明，就质量而言，我们的方法与现有技术具有竞争力。

1 介绍

2 相关工作

3 系统概述

谷歌的Cartographer以配备了传感器的背包的形式为室内地图绘制提供了实时解决方案，该背包可以生成5厘米分辨率的二维占据栅格地图。系统操作员在穿过建筑物时可以看到正在创建的地图。激光的scans被插入到最佳估计位置的submap中，该submap被假设在短时间内足够精确。Scan matching是针对最近的子地图进行的，因此它只取决于最近的scans和世界坐标系中姿态的估计累积误差。

为了在适度的硬件要求下实现良好的性能，我们的SLAM方法不使用粒子滤波器。为了应对误差的累积，我们定期进行位姿优化。当一个submap完成后，即不再有新的scan插入时，它将参与回环检测的scan mathing。所有完成的submaps和scans都会自动进行闭环检测。如果基于当前姿态估计，它们足够接近，则scan matcher会尝试在submaps中找scan。如果在当前估计姿态周围的搜索窗口中找到足够好的匹配，则将其作为回环检测约束添加到优化问题中。通过每隔几秒钟完成一次优化，操作员的经验是，当重新访问某个位置的时候回环会立即闭合。这导致了软实时约束，即回环检测匹配必须比添加新scan更快地进行，否则会明显落后。我们通过使用分支定界方法和several precomputed grids per finisher submap。

4 2D SLAM

我们的系统结合了单独的局部和全局方法来实现2D SLAM。这两种方法同时优化Lidar观测数据的姿态 $\xi =(\xi _{x}, \xi _{y}, \xi _{\theta })$ ，该姿态由平移 $(\xi _{x}, \xi _{y})$ 和旋转 $\xi _{\theta }$ 组成。这些值可以被认为是网格点被遮挡的概率。在一个不稳定的平台上，比如我们的背包，IMU用于估计重力的方向以将scans从水平安装的激光雷达投影到2D世界中。

在我们的局部方法中，使用非线性优化将每个连续scan与世界的一小块（称为submap M）进行匹配，该非线性优化将scan与submap对齐；这个过程被进一步称为scan匹配。Scan匹配会随着时间的推移误差累积，在稍后我们的全局方法中会消除这些误差，如第五节所述。

A Scan

Submap的构造是重复对齐scan和submap坐标系的迭代过程，也称为帧。在scan原点为 $0\epsilon R^{2}$ 的情况下，我们现在将有关扫描点的信息写为 ${h_{k}},_{k=1, ..., K}, h_{k}\epsilon R^{2}$ 。Submap中scan的姿态ξ表示为变换为 $T_{\xi }$ ，它将scan points从scan坐标系刚性地变换到submap坐标系，定义为：

B Submaps

使用几次连续的scan来构建submap。Submap采用概率grids $M: rZ * rZ->[p_{min}, p_{max}]$ ，which map from discreate grid points at a given 分辨率r，例如5cm，to values。这些值可以被认为是grid point被遮挡的概率。对于每个grid point，我们定义相应的pixel是由最靠近该grid point的所有点组成。

无论何时将scan插入probability grid，a set of grid points for hits and a disjoint set for misses are computed。对于每一次命中，我们都会将最近的网格点插入命中集。对于每个未命中，我们将与scan原点和每个scan点之间的一条射线相交的每个pixel插入相应的grid point，不包括已经在命中集中的grid point。如果每个以前未观测到的grid point位于其中一个集合中，则为其分配概率 $p_{hit}$ 或 $p_{miss}$ 。如果已经观察到grid point x，我们将命中和未命中的几率更新为一下：

对于未命中是等效的。

C Ceres scan matching

在将scan插入submap之前，使用基于ceres的【14】的scan matcher相对于当前局部submap优化scan姿态ξ。scan matcher负责找到一个scan姿态，使得scan points在submap中的概率最高。我们认为这是一个非线性最小二乘问题：

其中 $T_{\xi }$ 根据scan姿态将 $h_{k}$ 从scan坐标系变换到submap坐标系。函数 $M_{smooth:R^{2}->R}$ 是局部submap中概率值的平滑版本。我们使用bicubic插值。因此，可能会出现区间[0，1]之外的值，但这些值被认为是无害的。

这种平滑函数的数学优化通常比网格的分辨率提供更好的精度。由于这是一个局部优化，因此需要良好的初始估计。能够测量角速度的IMU可以用来估计姿态的旋转分量θ。在没有IMU的情况下，可以使用更高频率的scan匹配或pixel-accurate的scan匹配方法，尽管计算更密集。

5 回环检测

由于scan仅与包含最近几次scan的submap相匹配，因此上述方法会慢慢累积误差。对于只有几十次连续scan，累积误差很小。

通过创建许多小的submap来处理较大的空间。我们的方法，基于Sparse Pose Adjustment【2】优化所有scan和submap。插入的scan的相对位姿被存储在内存中用来在闭环优化中使用。除了这些相对位姿，all other pairs consisting of a scan and a submap are considered for loop closing once the submap no longer changes。此外Scan mather在后台运行，如果找到好的匹配后，则就把相应的相对位姿添加到优化问题中。

A 优化问题

与scan匹配一样，闭环优化也被公式化为一个非线性最小二乘问题，它可以很容易地添加残差以考虑额外的数据。每隔几秒钟，我们使用Ceres【14】来计算：

其中世界坐标系中的submap的位姿为 $\xi _{i}^{m}$ 和scan位姿为 $\xi _{j}^{s}$ ，在给定一些约束的情况下进行优化。这些约束采用相对位姿 $\xi _{ij}$ 和相关协方差矩阵 $E_{ij}$ 的形式。对于一对submap i和scan j，位姿 $\xi _{ij}$ 描述了scan在submap坐标系中匹配的位置。例如，可以按照【15】中的方法评估协方差矩阵 $E_{ij}$ ，或者locally using the covariance estimation feature of Ceres 【14】with CS。这种约束的残差E由以下公式计算：

损失函数ρ，例如Huber损失函数，用于减少scan匹配在优化问题中添加不正确的约束，（SPA）中可能出现的异常值的影响。例如，这种情况可能发生在局部对称的环境中，例如办公室隔间。异常值的替代方法to outliers include【16】。

B 分支定界scam matching

我们对最优和pixel-accurate的匹配感兴趣。

其中W是搜索窗口，并且 $M_{nearest}$ is M extended to all of R2 by rounding its arguments to the nearest grid point first, that is extending the value of a grid points to the corresponding pixel。使用（CS）可以进一步提高匹配的质量。

通过仔细选择步长可以提高效率。我们选择角度步长 $\delta _{\theta }$ ，以便在最大范围 $d_{max}$ 处的扫描点移动不超过r，即一个像素的宽度。最后利用余弦定律，我们推导出：

我们计算出了一个整数的步数以覆盖了给定的线性和角度搜索窗口，比如 $W_{x}=W_{y}=7,W_{\theta }=30^{\circ}$ ，

这导致有限集合W在位于其以估计位姿 $\xi _{0}$ 为中心形成了一个搜索窗口。

我们可以很容易的想到一个找到 $\xi ^{*}$ 的简单方法，请参阅算法1，但需要注意的是，它速度太慢了。

相反，我们使用分支定界法在更大的搜索窗口上计算 $\xi ^{*}$ 。通用方法见算法2。这种方法最初是在混合整数线性规划的背景下提出的【17】。关于这一主题的文献十分丰富；请参阅【18】以概览。

分支定界法的主要思想是将subsets of possibilities表示为树中的节点，其中根节点表示所有可能的解决方案，在我们的场景下它是W。每个节点的子节点是其父节点的一个分区，因此它们一起表示同一组可能性。叶节点是singletons；每一个都代表一个可行的解决方案。请注意，算法
是准确的。它提供了与算法1相同的解决方案，只要内部节点的分数is an upper bound on the score of its elements。在这种情况下，whenever a node is bounded，a solution better than the best known solution so far does not exist in this subtree。

为了得到一个具体的算法，我们必须决定node selection、branching和computation of upper bounds。

1 节点选择

在没有更好的选择的情况下，我们的算法使用深度优先搜索（DFS）作为默认选择：算法的效率很大一部分依赖于于被修剪的树。这取决于两件事：一个是好的上限，另一个是好的当前解决方案。后一部分得到了DFS的帮助，它可以快速评估许多叶节点。由于我们不想添加较差的匹配作为闭环约束，we also introduce a score threshold below which we are not interested in the optimal solution。由于在实践中不会经常超过阈值，这降低了节点选择或寻找初始启发式解决方案的重要性。关于DFS期间访问子节点的顺序，我们计算每个子节点的分数上限，首先访问具有最大边界的最有希望的子节点。这种方法是算法3。

2 分支规则

树中的每个节点由整数元组描述。

3 计算上界

6 实验结果

7 结论

在本文中，我们提出并实验验证了一种2D SLAM系统，该系统将scan到submap匹配与闭环检测和图优化相结合。单独的submap轨迹是使用我们的局部、基于网格的SLAM方法创建的。在后端，使用像素精确的scan匹配将所有scan匹配到附近的submap，以创建回环约束。由submap和scan位姿构成的约束图在后端中被周期性地优化。操作员将看到最终地图的最新预览，作为完成的子地图和当前子地图的GPU加速组合。我们证明了在普通硬件上实时运行我们的算法是可能的。

8 致谢

9 参考文献

[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.

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

cartographer

论文翻译