一个鲁棒的基于图优化的精确雷达建图

摘要

工业环境中的移动机器人需要准确的地图以执行导航和服务任务。 本文介绍了一个SLAM框架，该框架由不同的优化组件组成，旨在构建非常精确的地图。 我们在前端提供高级水平的为数据关联提供先验的特征点提取器。 由于感知混叠，2D激光表示可能导致错误的闭环检测，这可以通过后端中的现有图优化技术可靠地解决。最后的后处理步骤使用包含激光测距仪特定特征的先进传感器模型生成高度精确的地图。 所获得的框架在从典型工业环境获得的数据集上进行评估。 给定遍历环境的地面实况测量值来估计最终的地图精度。

简介

图1

移动机器人在工业环境中的自主操作需要平台相对于全局参考系的精确位置和方向。设置诸如视觉标记之类的人造地标可以容易地解决该问题，然而，这需要修改环境。相比之下，使用板载传感器构建的先验地图使机器人能够独立于标记的存在进行导航。提供遍历区域的路径是已知的，可以容易地构建地图。对于大多数情况，这些路径是难以获得的且对于室内环境而言难以获得高精度的路径。如果地图和路径都是未知的，则机器人必须同时保持关于其位置的估计以及可通过的环境，这被称为同时定位和建图（SLAM）问题。本文提出了一个SLAM框架，可以使用激光测距仪构建高度精确的地图，如图1所示。这包括一个通过局部运动和闭环提供机器人姿态空间关系的前端。这些信息被传递到后端，该后端维持姿势关系并估计机器人行进的路径。在给定估计的轨迹之后执行关节位姿和地图优化。所提出的框架利用g2o [1]，一种通用的广泛用于机器人和计算机视觉的图优化后端。特别是基于优化的SLAM问题方法正在积极研究中。因此，存在大量前期的工作，这些工作在摘录中概述。Olson等人介绍了一种使用梯度下降进行图优化的有效方法[2]。 Grisetti等，提出了一个基于树的实现，创造了TORO，它明确地将图优化与地图的大小联系起来而不是轨迹的长度[3]。在[4]中，Kretzschmar和Stachniss建议通过信息理论进一步降低姿势图的复杂性。Blanco等人提出了混合度量拓扑SLAM的通用接口，构建了基于占用网格的子图的图形，非常适合于映射大规模环境[5]。基于度量的SLAM系统，例如RBPF [6]，通过位置识别方法补充，例如，FAB-MAP [7]。 iSAM [8]给出了一种基于SLAM的在线应用优化解决方案，这种方法的关键思想是，只需要更新因子图中的一小部分变量以进行增量操作。 Kaess等人。进一步扩展了他们的算法，使用贝叶斯树实现增量变量重新排序和重新线性化[9]。在[9]中，证明了iSAM在实际数据集上优于其他最先进的方法，如HOG-MAN [10]和Spa2D [11]。然而，我们的框架更侧重于构建一个准确的地图，该地图不一定必须在线执行，因为数据集是为此目的而进行批量处理的。
上面提到的基于优化的SLAM方法在准确性方面提供了实质性结果，因为它们试图根据相关约束找到给定图表示的最优解。然而，假设所有回环检测都是正确的，这对前端中的数据关联提出了很高的要求。在视觉SLAM系统中常见的几何一致性检查，例如[12]中描述的，可以最小化错误检测的数量，但通常不能避免它们。这是因为一些地方由于感知混叠而难以区分，特别是对于存在许多重复结构的2D激光扫描，例如在工业环境中。除此之外，由于更悲观的评估，还需要昂贵的数据关联技术，这些技术也可能拒绝真正的回环。另一方面的虚假约束可能会导致图优化中的分歧，这些不能通过诸如[13]中所示的伪Huber误差函数的鲁棒方法来充分减轻。在[14]中，Sünderhauf和Protzel提出了一种强大的优化方法，使后端能够通过切换回环约束来修改姿势图。这允许前端中非常简单的数据关联技术将更多数量的回环候选和相关的置信度传递给后端，后者能够禁用错误的约束。 由于该技术被包含在所提出的框架中，因此在本文的其余部分中给出了更详细的描述。
预计从鲁棒的后端将获得拓扑一致的姿势图(pose graph)。 然后根据该位姿图估计最终地图。 在[15]中，正如Ruhnke等人所证明的那样，有利的是对激光束的物理进行建模，以便最大化地图的局部精度。 这具体地解决了光束的范围和入射角。 通过联合优化光束测量的特性，局部环境和原点传感器姿态，可以获得高精度的地图表示[15]。 本文通过仅使用2D激光测距仪和车轮里程计的移动机器人绘制工业环境，展示了该想法的好处。
本文的其余部分安排如下。 第2节和第3节描述了框架的前端和后端及其组件。 我们在第4节中提出实验结果，然后在第5节结束。

2. 建图前端

本节描述了建图框架的前端。 它提供了关于连续运动机器人姿势的初始估计。 除此之外，前端负责识别先前观察到的位置，从而将回环候选传递给后端。 这是通过从激光范围扫描(range scan)中提取与先前扫描获得的特征相匹配的特征来完成的。(通过匹配特征)

A. 估计初始化

机器人连续轨迹间的姿态相对变换由里程计测量值(车轮)给出。 由于车轮滑动，这些估计可能会迅速发生变化。 我们使用激光扫描匹配来校正从里程计获得的位置估计。 应用基于点到线度量的ICP算法[16]。

B. 特征提取

我们使用快速激光兴趣区域变换(Fast Laser Interest Region Transform，FLIRT)[17]，灵感来自于先前工作中的计算机视觉中尺度不变点特征检测器。 FLIRT将激光的范围数据(range data)视为被映射成多尺度表示的一维曲线。 因此，我们获得了由变化高斯核来平滑的所有尺度的曲线。 对平滑核进行归一化以使其对采样密度不变。 检测每个尺度的局部极值。 这种数学推导在[18]中给出。 最后，我们获得梯度高于某个阈值的一组兴趣点作为局部最大值。 这些点通常指的是2D扫描中的角点。(不太懂)
为了将被检测的兴趣点与先前观测的兴趣点相互关联，需要进一步给兴趣点分配描述子。类似于计算机视觉中的常见特征检测器Tipaldi等提出的用特征描述符捕获感兴趣点的局部环境[17]，其能够识别后续扫描中观察到的特征。诸如LRF的范围测量传感器给出关于由传感器的视场（FOV）覆盖的自由空间和占用空间的明确信息。因此，将其用描述子是有利的。在每个兴趣点周围建立了一个极坐标的计算方法。这与占用栅格建图类似，但是我们仅在兴趣点周围生成高分辨率网格，而不是描述激光扫描覆盖的整个区域。由于FLIRT兴趣点具有高度的独特性，并且不假设特定的几何图元，因此它们非常适合于许多非特征性的环境类型（例如长走廊）。关于特征检测器和描述符的更多细节可以在[17]中找到。

C. 回环检测

SLAM系统最重要的需求是能够检测和合并已经访问过的地方的信息，这些信息被称为回环。 通过回环能够减少累积姿势不确定性随时间增加。 现在已经进行了针对2D范围扫描中的回环检测的大量研究，并且可以将其分为栅格地图(gird map)和基于特征的方法。
基于栅格地图方法通常不能随着地图尺寸的增加而很好地扩展，因为它们编码了大量的自由空间。 对于基于Rao-Blackwellized粒子滤波器（例如[6]）的方法尤其如此，因为每个粒子保持整个轨迹的单独图。 另一方面基于特征的方法通常假设在诸如角点的环境中的特定结构，这妨碍了它们在无结构环境中的使用。 然而，作为本文特别关注的工业环境通常包括由诸如架子或机器之类的物体引起的精细结构。 通过从每次激光扫描中提取的FLIRT兴趣点很好地考虑这些结构。 这些与使用对称χ²度量和最近邻策略的与先前扫描的特征进行匹配。(文章假设了环境存在角点)一组对应的特征点被传递给RANSAC进行几何一致性检查。每对描述子距离小于一定阈值的兴趣点被推定为潜在的匹配。从对应集中随机抽取最少的两对，然后如下进行RANSAC迭代。计算推定匹配的空间距离。如果这些距离的偏移低于阈值，则通过估计这两个点之间的变换来生成假设。此变换应用于所有观察到的特征，以便将这些特征投影到参考扫描的坐标系中。因此，为每个观察到的特征点分配参考扫描的最接近的对应特征点。将所有对的平方距离相加，其用作两次扫描的匹配分数。只有距离低于阈值的那些对被添加到内部集合中。在基于RANSAC的数据关联中，前述的过程重复进行。如果剩余的一组内点高于阈值t_min，则生成回环约束。t_min的值高度依赖于优化后端的稳健性，特别是它处理数据关联错误的能力。(这一段翻译的特别乱，下附原文，这一段大概在将RANSAC的作用)

3. 优化后端

由于前端，我们给出了机器人姿势和表达空间约束的关系的初始图。 此图的优化是后端的任务。 本节的其余部分简要介绍位姿图SLAM，解释如何处理错误回环检测并构建最终地图。

A. 位姿图(pose graph) SLAM

提供的移动机器人导航是在2维空间中进行的，其状态向量可以用x =（x，y，φ）T来描述。 位姿图图SLAM仅优化给定轨迹的机器人姿势x_i，省略分别优化地标扫描点。 该图由位姿x_i和位姿x_i和x_j之间边界给出的顶点组成。 并且执行动作u_i =Δ（x，y，φ）以便从状态x_i到x_i+1根据运动模型合并：

B. 鲁棒的优化

Tipaldi等人在[20] 中，在SLAM框架中展示了FLIRT兴趣点的鲁棒性和高重复性。 然而，作者使用TORO [3]作为优化后端，在存在伪回环检测时这种后端不鲁棒。 对于具有多个重复结构的环境而言，这是一个显着的缺点，这些结构难以通过2D激光扫描来区分。 提出了将位姿搜索空间限制在当前估计周围的局部区域的滑动窗口以最小化该问题。 然而，它阻碍了在具有大环路和较差的先验位姿估计的环境中的应用，例如通过车轮里程计获得。
对于SLAM问题的通常基于最小二乘的优化方法保持所提供的位姿图的结构固定，假设所有回环约束是正确的。 Sünderhauf和Protzel提出了可切换约束，通过修改的目标函数来解决优化问题。他们的工作使用表示可切换回环约束的图形模型中的因子。基于此，图的拓扑结构不是固定的，但也可以进行优化。以这种方式可以禁用表示为开关变量s_ij的闭环。回环对优化的贡献可以通过其初始值γ_ij和相应的协方差Ξ_ij来合并。例如，这些估计器可以通过前端的地点识别系统的一定程度的信任来给出。这些开关先验是必要的，以避免优化器关闭所有回环，如[14]所示。为了在优化中包括可切换约束，目标函数（式5）被修改如下：(本文加入了可以切换的回环约束)

C. 地图构建

稀疏表面调整（SSA）是一个联合优化问题，旨在寻找机器人位姿和激光测量值的最佳设置。这种方法由Ruhnke等人在[15]中提出，与计算机视觉中的稀疏束调整（SBA）非常相似。 SBA通过最小化点的重投影误差来优化一组相机位姿和图像序列的3D点。类似地，SSA优化机器人位姿和激光点，然而SBA需要点对点，和数据关联中的点对面对应的SSA相反。进一步假设环境由光滑表面组成，这对于人造结构是非常常见的。改进的传感器模型明确地解释了激光测距仪的距离测量中的不确定性，其主要由于入射角和距离变化的局部表面特征而发生。这是通过将激光扫描描述为一组表面切线来完成的，每个表面切线代表一个激光束。切线被建模为高斯的，其中平均值μ_ik以机器人的激光束k为中心，位姿i和基于周长点的协方差估计∑_ik。表面沿着它们的切线方向平滑地移动，并且沿着面向机器人姿势的法线方向更加刚性。机器人姿态X和表面贴片(?surface patches)M的联合优化问题公式如下[15]：

4. 实验

为了评估所提出的框架，进行了许多实验。 为此，我们通过仓库手动操纵移动机器人捕获激光范围扫描和车轮里程计。 以下部分简要介绍了用于实验的移动平台，并讨论了获得的结果。 首先，证明了强大的优化后端的必要性。 第二部分显示了在典型的工业环境中使用所提出的框架实现的最终建图精度。

A. 平台

使用由德国Metralabs GmbH开发的Scitos G5平台（参见图2）。 它配备了四个摄像头，两个激光测距仪（Hokuyo URG04-LX，SICK S300），一个TOF摄像头（PMD Camcube 3.0）和车轮里程计。 只有前向SICK S300用于地图构建。

B. 贡献鲁棒的后端

尽管FLIRT兴趣点非常独特，但回环检测错误的数量很高。 这主要是由于2D激光扫描的有限场景表示引起的感知混叠。 几何一致性检查无法在所有情况下阻止这种情况，因为环境中存在大量重复结构，例如货架和盒子。 图3显示了位姿图优化的结果，强调了鲁棒的后端的重要性。 我们使用了Switchable Constraints [14]实现，创造具有线性切换功能。 此外，我们使用了通用图优化后端g2o [1]。 这两种实现都是从openslam2平台获得的。

参考文献相关论文阅读

1.参考文献16，ICP-point-to-line

参考文献：

[1] R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g2o: A general framework for graph optimization,” in ICRA, Shanghai, 2011.
[2] E. Olson, J. Leonard, and S. Teller, “Fast iterative optimization of pose graphs with poor initial estimates,” 2006, pp. 2262–2269.
[3] G. Grisetti, S. Grzonka, C. Stachniss, P. Pfaff, and W. Burgard, “Efficient estimation of accurate maximum likelihood maps in 3d,” San Diego, CA (USA), 2007, pp. 3472–3478.
[4] H. Kretzschmar and C. Stachniss, “Information-theoretic compression of pose graphs for laser-based slam,” vol. 31, pp. 1219–1230, 2012.
[5] J.-L. Blanco, J.-A. Fernandez-Madrigal, and J. Gonzalez, “A new approach for large-scale localization and mapping: Hybrid metrictopological slam,” in Robotics and Automation, 2007 IEEE International Conference on, 2007, pp. 2061 –2067.
[6] 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. Proc. of the 2005 IEEE International Conference on, Apr. 2005, pp. 2432 – 2437.
[7] M. Cummins and P. Newman, “FAB-MAP: Probabilistic Localization and Mapping in the Space of Appearance,” The International Journal of Robotics Research, vol. 27, no. 6, pp. 647–665, 2008.
[8] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental smoothing and mapping,” IEEE Trans. on Robotics (TRO), vol. 24, no. 6, pp. 1365–1378, Dec. 2008.
[9] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping using the Bayes tree,” Intl. J. of Robotics Research (IJRR), vol. 31, pp. 217–236, Feb. 2012.
[10] G. Grisetti, R. Kummerle, C. Stachniss, U. Frese, and C. Hertzberg, “Hierarchical optimization on manifolds for online 2d and 3d mapping,” in Robotics and Automation (ICRA), 2010 IEEE International Conference on, 2010, pp. 273–278.
[11] K. Konolige, G. Grisetti, R. Kümmerle, W. Burgard, B. Limketkai, and R. Vincent, “Efficient sparse pose adjustment for 2d mapping,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), Taipei, Taiwan, October 2010.
[12] D. Galvez-Lopez and J. D. Tardos, “Bags of binary words for fast place recognition in image sequences,” IEEE Transactions on Robotics, vol. 28, no. 5, pp. 1188–1197, October 2012.
[13] N. Sünderhauf, “Robust optimization for simultaneous localization and mapping,” Ph.D. dissertation, 2012.
[14] N. Sünderhauf and P. Protzel, “Towards a robust back-end for pose graph slam,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on, may 2012, pp. 1254 –1261.
[15] M. Ruhnke, R. Kümmerle, G. Grisetti, and W. Burgard, “Highly accurate maximum likelihood laser mapping by jointly optimizing laser points and robot poses,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2011.
[16] A. Censi, “An ICP variant using a point-to-line metric,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), May 2008.
[17] G. Tipaldi and K. Arras, “Flirt - interest regions for 2d range data,” in Robotics and Automation (ICRA), 2010 IEEE International Conference on, 2010, pp. 3616 –3622.
[18] R. Unnikrishnan and M. Hebert, “Multi-scale interest regions from unorganized point clouds,” in Workshop on Search in 3D (S3D), IEEE Conf. on Computer Vision and Pattern Recognition (CVPR), June 2008.
[19] M. W. M. G. Dissanayake, P. Newman, S. Clark, H. F. Durrantwhyte, and M. Csorba, “A solution to the simultaneous localization and map building (slam) problem,” IEEE Transactions on Robotics and Automation, vol. 17, pp. 229–241, 2001.
[20] G. D. Tipaldi, M. Braun, and K. O. Arras, “Flirt: Interest regions for 2d range data with applications to robot navigation,” in Proceedings of the International Symposium on Experimental Robotics (ISER), Dehli, India, 2010.
[21] E. Einhorn, C. Schroter, and H. Gross, “Finding the adequate resolution for grid mapping - cell sizes locally adapting on-the-fly,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on, 2011, pp. 1843–1848.