LIDAR_Camera长走廊优化

论文名：Graph Structure-Based Simultaneous Localization and Mapping Using a Hybrid Method of 2D Laser Scan and Monocular Camera Image in Environments with Laser Scan Ambiguity

摘要

定位是机器人导航的基本问题，允许机器人自主执行任务。然而，在具有激光扫描模糊的环境中，例如长走廊，利用激光扫描仪的传统SLAM（同时定位和映射）算法可能无法鲁棒地估计机器人姿态。为了解决这个问题，我们提出了一种基于混合方法的新型定位方法，该方法在基于图形结构的SLAM的框架中结合了2D激光扫描仪和单目相机。通过混合方法获取图像特征点的3D坐标，假设墙壁垂直于地面并且垂直平坦。然而，这种假设可以减轻，因为随后的特征匹配过程拒绝倾斜或非平坦壁上的异常值。通过利用混合方法生成的约束的图优化，估计最终的机器人姿势。为了验证所提方法的有效性，在具有长走廊的室内环境中进行了真实的实验。将实验结果与传统的GMapping方法进行了比较。结果表明，可以在具有激光扫描模糊性的环境中实时定位机器人，并且该方法的性能优于传统方法。

1.简介

本文提出了一种基于图形结构（graph structure-based）的SLAM，它使用2D激光扫描仪和单目相机来解决上述问题。从相机获取图像特征数据，并且从激光扫描仪获取深度信息。通过使用融合所获得的传感器数据的图形结构来补偿每个传感器的缺点。这种融合激光扫描仪和相机的混合方法使我们能够在激光扫描模糊的环境中估计机器人姿态，其中仅使用激光扫描仪时位姿估计是困难的。这样的环境是长的走廊或非常大的空间，其中激光扫描仪只能检测周围的一侧，因为从机器人到墙壁的范围大于激光扫描仪的最大可检测范围。混合方法可以克服每个传感器的缺点。通过使用这些方法，可以在具有激光扫描模糊的环境中准确地估计机器人的位姿。

本文的其余部分安排如下。 第2节描述了室内机器人使用来自多传感器的基于图形结构的SLAM来最小化机器人姿态误差的整体方法。 为了验证所提方法的优越性，我们描述了实验环境和整个系统以及在第3节中使用我们的方法获得的实验结果。在第4节中，讨论了结论和未来的工作。

2. Graph Structure-Based SLAM Using Multi-Sensors

本节详细介绍了我们的方法。 首先，简要介绍了GMapping [4]，基于网格的SLAM和Rao-Blackwellized粒子滤波器以及基于图形的SLAM的制定。 然后，我们详细解释如何融合从单目相机中提取的特征数据和来自激光扫描仪的深度数据，以进行机器人定位。 最后，我们描述了位姿图生成和优化的方法。

2.1 Grid-Based SLAM with Rao¨CBlackwellized Particle Filters

GMapping是使用Rao-Blackwellized粒子滤波器的基于网格的SLAM。通过在GMapping中使用自适应重新采样来解决粒子耗尽，即基于粒子滤波器的SLAM的长期问题。通过采用称为改进的提议分布的方法，GMapping降低了机器人姿势的不确定性。该方法基于最近的传感器测量产生提议分布，假设激光扫描比测距更准确。这是用于估计机器人姿势的概率方法，并且可以通过使用激光扫描仪的深度数据来估计机器人在2D平面上的姿势。当连续激光扫描可以匹配而没有模糊时，可以使用GMapping算法。但是，如果在具有激光扫描模糊的环境中利用GMapping算法，则可能发生机器人位姿错误。因此有必要应对这种情况，并在下一小节中描述用于多传感器融合的基于图形的SLAM。

2.2 基于图的SLAM建模(Modeling of Graph-Based SLAM)

在本小节中，我们描述了基于图的SLAM公式和基于图的SLAM的解决方案[27]。 一般的基于图的SLAM可以使用条件概率编写如下：
通常，状态预测是非线性的，因此，为了简化问题，可以使用泰勒级数将预测近似为一阶，如下所示：

2.3 单目相机和激光扫描仪的混合方法!!!!!

在该子部分中，描述了用于融合相机的特征数据和激光扫描仪的深度信息的方法。 使用混合方法预测3D机器人姿势，并且该信息用作基于图形的SLAM的约束。 整体算法和概念分别如图1和图2所示。 在执行激光扫描仪和单目相机的混合方法之前，必须进行固有校准以确定相机的固有参数。 还需要知道相机和激光扫描仪之间的相对姿势，以融合来自它们的数据。 因此，通过外部校准获得两个传感器之间的相对姿势信息。

相机模型 雷达单目坐标系转换
让我们假设墙壁与地面完全垂直并且垂直平坦。 然后，图像平面上相同u^C上的所有点将具有相同的深度值。 使用具有相同u^C的重叠激光扫描线上的点来确定图像上的特征点的深度值。 由于重叠激光扫描线上的点是离散的，因此可以通过来自相邻重叠扫描点的深度值的线性插值来获取相应的深度值。 让我们将线性插值的深度值表示为~z。 通过用~z替换等式（10）和（11）中的z^C，可以如下获得相机坐标系中的增强3D特征点：（视觉对着墙，问题很大）

也可以使用从相机到机器人的外在校准来获取该变换。 由于可以使用这些相对姿势确定来自地面的激光扫描的高度，因此可以从图像平面提取地面区段。 剩下的部分可以被认为是墙壁部分，并且假设墙壁是垂直的而不是倾斜的。 之后可以减轻这种假设，因为可以通过特征匹配过程去除异常值。 然后运行特征提取算法，并且在该算法中仅利用2D图像中的墙壁上的特征点。 图3显示了使用所提出的算法的示例。 结果表明，利用激光扫描仪的深度数据可以准确地分离墙面和地面，同时也可以估算到墙面的距离，从而可以估算出从图片中提取的特征点的三维坐标。

2.4 从多传感器获取的位姿数据

在机器人上安装多传感器后，使用相应的传感器估算机器人的位姿，然后，混合算法融合各自的结果，如图4所示。使用协方差值和每个传感器的测量值用于生成图结构的约束。通过使用所生成的约束信息组织图形结构，通过图优化来获得最终校正的机器人姿势。测距仪生成测距信息，该信息用于生成位姿约束。使用来自2D激光扫描仪的深度数据制作2D网格图。然后使用GMapping从2D网格图和ICP匹配生成2D机器人姿势约束[4]。在通过相机图像的SURF（加速鲁棒特征）[29]算法提取特征点之后，通过增加来自2D激光扫描仪的深度信息来获得每个特征点的3D坐标。然后从特征点的3D坐标生成3D机器人位姿约束。在前面的小节中详细描述了使用单目相机和激光扫描仪获得机器人姿势的混合方法。

从多传感器生成位姿图结构的示例如图5所示。每个节点表示机器人的姿势（x_i），连接节点的边是约束，其由测量（z_i,j）组成及其协方差（Λ_i,j）。绿色实线表示使用来自odometry的航位推算创建的约束。红色虚线表示使用GMapping的激光扫描仪产生的约束[4]。在使用GMapping时，使用2D激光扫描仪从扫描匹配方法获得2D网格图。然后可以使用Rao-Blackwellized粒子滤波器获得约束。由于GMapping是基于粒子滤波器的SLAM算法，因此它仅提供相对于原点的当前姿势估计。因此，不能使用GMapping生成图结构中的连续节点之间的约束，并且可以仅生成原点和机器人之间的约束。蓝色虚线表示使用混合方法从相机的特征点和激光扫描仪的深度数据生成的约束。仅当检测到回环时，混合方法才在相应节点之间生成约束。因此，可以不通过混合方法为连续节点生成约束。来自每个传感器的姿势预测和协方差值被用作图中的约束，以使用图优化技术来校正最终的机器人姿态估计。

2.5 位姿图优化

本小节通过从激光扫描仪的深度数据和来自摄像机的特征点生成节点和约束来解释图形结构构造的方法。该方法中使用的基本SLAM算法基于3D SLAM算法[13]，并且仅使用单目相机和激光扫描仪来估计特征点的距离信息。整个算法的流程图如图6所示。当输入来自单目相机的数据时，从2D图像中提取特征点。墙上提取的特征点用于视觉测距和回环检测。使用前一小节中描述的混合方法获得图像上的特征点的3D坐标。使用3D-RANSAC [26]使用配对图像上匹配的特征点的3D坐标确定匹配图像之间的3D姿势（六个DoF（自由度））。节点之间的初始连接使用里程计信息。在每个节点处获得的2D图像特征点存储在特征管理器中并用于构造节点之间的约束。特征管理器使用新生成的节点的特征数据找到具有公共图像特征的先前节点。这个过程称为回环检测[13]。通过使用特征点施加的节点之间的约束被用作用于形成表示机器人的整个轨迹的图结构的信息。然后将位姿图SLAM应用于通过上述方法形成的图形结构，以获得最终的校正姿势。基于图的SLAM问题通过使用稀疏线性代数技术实时优化图模型来解决[2]。