1、摘 要I硕士学位论文RGB-D 数据融合的 SLAM 方法及实现学位申请人：指导教师： 学科名称：控制科学与工程年 月西安交通大学硕士学位论文摘 要随着计算机视觉理论和移动机器人学的发展，Simultaneous Localization And Mapping (SLAM)技术在机器人导航、虚拟现实、人机交互、远程操控和环境重建等多个领域得到了广泛应用。以 Microsoft Kinect 为代表的 RGB-D 摄像机作为一种新型传感器，在采集彩色图像的同时能获得与之相对应的深度图像信息。丰富的传感器数据，使其近年来越来越多地被用于 SLAM 问题的研究中。相较于传统单一数据来源的SLAM
2、系统，本文设计并实现了一个 RGB-D 数据融合的鲁棒 SLAM 方法，通过彩色和深度数据融合的相对位姿估计算法以及图结构模型的 SBA 优化算法实现摄像机位姿的鲁棒精确估计并完成三维环境的实时稠密重建。本文主要围绕以下三个部分展开：1） RGB-D 摄像机标定。本文阐述了 Kinect 摄像机的成像原理以及其对应的立体摄像机成像模型，针对模型采用棋盘格标定算法实现对 Kinect 摄像机中彩色摄像头和红外摄像头的联合标定。2） RGB-D 数据融合的相对位姿估计算法实现。本文通过分析传统 SLAM 系统中图像特征的 3D-3D 点集匹配方法和稠密点云匹配方法的原理和优缺点，提出精度更高的彩色
3、和深度数据融合的相对位姿估计算法，给出了算法的原理和建模方法原理，并利用添加鲁棒函数的方法进一步增强算法对不同环境的适应性。3） 基于图结构的优化。本文将摄像机位姿估计问题转化为图结构模型的建立和优化问题，利用结点约束添加机制、闭环检测机制构建图模型，通过图结构的联合优化实现对摄像机绝对位姿的全局一致性鲁棒估计。本文设计并实现了一个 RGB-D 数据融合的鲁棒 SLAM 方法，通过对方法进行的位姿估计精度实验，验证了方法中各主要关键部分的性能和作用。本文还在公开数据集上与其他先进 SLAM 系统进行了对比实验，通过实验证明本方法具有更加良好的精确性、可靠性以及场景适应性，获得了令人满意的结果。
4、关 键 词：RGB-D 摄像机；SLAM；数据融合；位姿估计。论文类型：应用研究ABSTRACTIIIABSTRACTSimultaneous Localisation and Mapping (SLAM) is important to many relevant applications in robotics and computer vision such as navigation, virtual reality, man-machine interaction, telepresence and reconstruction. As a new sensor, the RGB-D
5、 camera represented by Kinect could capture RGB images augmented with depth data at each pixel with a high data rate. For the reason of abundant sensor data, RGB-D cameras have been become more and more popular in the areas of SLAM. Compared with traditional SLAM system based on single data sources,
6、 we designed a robust fusion method for RGB-D SLAM. Through the RGB-D data fusion pose estimation algorithm and pose graph based optiminzing, our method could estimate the camera pose robustly and reconstruct the 3D environment accurately in real time. This paper contains three main parts:1. RGB-D c
7、amera calibration. This paper described a stereo camera model for the Kinect, and according to the model we utilized checkrboard to jointly calibrate the RGB camera and IR camera in the Kinect.2. The relative pose estimation algorithm based on RGB-D data fusion. According to analysing the feature ma
8、tching algorithm and point cloud ICP algorithm, we proposed a more accurate and robust algorithm that fusing the color and deep data to estimate the relative pose of camera. With the help of the robust function, the environmental adaptability of the fusion algorithm has been further enhanced.3. Pose
9、 graph based optiminzing.For the sake of estimating the most accurate camera poses, our method used pose graph to describe the poses estimation problem. The pose and constraint adding mechanism and loop detection were utilized to build the pose graph, and the optimizing of pose graph was used to obt
10、aining the pose estimation with a well global consistency.This paper designed and implemented a robust fusion method for RGB-D SLAM. Through the accuracy of pose estimation experiments, the necessity and function of each part in the method was verified. This paper also compared the fusion method wit
11、h two state-of-the-art SLAM systems on the public dataset. The comparison experiments demonstrated that our method could robustly and accurately estimate the camera trajectory and reconstruct the 3D environment for the RGB-D SLAM system, even in the environment with poor 3D geometry or the area with
12、 sparse texture.KEY WORDS: RGB-D camera; SLAM; Data fusion; Pose estimation TYPE OF THESIS: Applied Research西安交通大学硕士学位论文IV目 录1 绪论 .11.1 引言 .11.2 目前相关领域的研究情况 .11.3 主要研究内容与组织结构 .22 RGB-D SLAM 系统设计 .42.1 系统总体设计方案 .42.2 系统硬件组成部分 .52.2.1 微软 Kinect 摄像机 .52.3 系统软件组成部分 .62.3.1 系统的软件开发平台 ROS.62.3.2 开放自然交互接口
13、OpenNI.72.3.3 点云库 PCL .92.4 主要算法模块 .102.4.1 RGB-D 摄像机标定算法模块 .102.4.2 RGB-D 数据融合的位姿估计算法模型 .102.4.3 基于图结构的优化算法模型 .122.5 本章小结 .123 RGB-D 摄像机标定 .133.1 Kinect 原理介绍 .133.1.1 Kinect 数据特点 .143.1.2 深度图像获取原理 .153.2 摄像机成像模型 .193.2.1 参考坐标系 .193.2.2 针孔模型 .213.2.3 立体摄像机模型 .233.3 摄像机标定原理 .243.4 Kinect 摄像机标定实验及结果分析
14、 .273.4.1 Kinect 摄像机联合标定实验 .273.4.2 改进后的 Kinect 摄像机联合标定实验 .303.5 本章小结 .334 RGB-D 数据融合的位姿估计方法 .344.1 基于图像特征的 3D-3D 点集匹配算法 .344.1.1 算法原理 .34目 录Error! No text of specified style in document.V4.1.2 算法分析 .364.2 稠密点云 ICP 匹配算法 .364.2.1 算法原理 .364.2.2 算法分析 .384.3 RGB-D 数据融合的相对位姿估计算法 .384.3.1 融合算法的主要思想 .384.3
15、.2 融合算法的数学模型 .394.3.3 添加鲁棒函数的融合算法数学模型 .424.3.4 融合算法的工作流程 .444.4 本章小结 .455 基于图结构模型的优化 .465.1 RGB-D SLAM 图结构模型 .475.2 图结构前端建图系统 .485.2.1 结点和约束添加机制 .485.2.2 闭环检测机制 .505.2.3 丢帧寻回机制 .535.3 图结构后端优化系统 .535.4 本章小结 .556 实验设计及结果分析 .566.1 实验设计 .566.2 实验结果分析 .576.2.1 位姿估计精度实验 .576.2.2 与其他 RGB-D SLAM 系统的对比实验 .61
16、6.3 本章小结 .717 结论与展望 .727.1 结论 .727.2 展望 .72致 谢 .74参考文献 .75攻读学位期间取得的研究成果 .77Error! No text of specified style in document.Error! No text of specified style in document.11 绪论1.1 引言在计算机视觉、计算机图形学和机器人学等诸多领域中，移动机器人的实时定位和地图绘制问题 1（Simultaneous Localisation And Mapping）一直是虚拟现实、人机交互、远程操控和环境重建等诸多应用的重点研究内容。上述应用
17、期望通过 SLAM 技术及时、准确地获取三维环境的重建模型以及移动平台在三维环境中的位置与姿态信息。对于大多数 SLAM 系统，其主要工作包含：利用传感器信息（图像信息或激光信息）通过一定的匹配算法进行摄像机在不同时刻下的相对位姿估计；利用相对位姿关系以及闭环检测机制通过一定的优化方法获得具有全局一致性的绝对位姿估计结果；最终实现对三维环境的重建工作。其中获得不同时刻下摄像机的相对位姿以及估计具有全局一致性的绝对位姿是整个 SLAM 系统的核心任务，也是本文的主要工作。近年来，以 Kinect2为代表的 RGB-D 摄像机自问世以来便获得了 SLAM 研究领域的高度关注。相较于传统摄像机，RG
18、B-D 摄像机在获取彩色图像的同时能够通过投射的结构光，获得相应的深度图像信息，因此即便面对缺乏纹理或者光照不一致的环境，RGB-D 摄像机依然能获得有效的传感器数据。丰富的传感器信息允许 SLAM领域的研究人员尝试用全新的方法进行摄像机位姿的估计和优化，从而获得更鲁棒更精确的位姿估计结果。本文正是在这样的背景下，使用 Kinect 摄像机，来研究通过 RGB-D 数据融合的相对位姿估计方法和图结构模型优化方法实现摄像机位姿的鲁棒精确估计，完成三维环境的实时稠密重建，解决 SLAM 问题。1.2 目前相关领域的研究情况随着机器人学和计算机视觉的发展，单目摄像机，双目摄像机甚至激光等传感器都被应
19、用于 SLAM 问题的研究中，其中最重要的研究内容就是估计不同时刻下摄像机的相对位姿情况。如何求解连续数据序列间的相对位姿关系主要取决于数据的类型。这些传统的 SLAM 系统 3456仅仅使用单一的图像信息或者点云数据估计不同时刻间的相对位姿。对于激光数据，iterated closet point(ICP)789是主要的求解算法。ICP算法匹配两帧点云数据中的每个点，求取二者之间严格的旋转平移关系，具有良好的精度，但是由于激光设备过于昂贵难以广泛应用。在纯视觉的 SLAM 系统中，图像的稀疏特征点匹配 1011是求解不同时刻下摄像机相对位姿关系的主要方法。双目立体摄像机 12在提取图像特征点
20、后可以从左右两视图中获得特征点的深度信息，利用 2D-3D匹配算法匹配特征点来获得摄像机不同时刻下的相对位姿关系，并利用 RANSAC 机制提高估计的鲁棒性，但是其深度求解的运算代价过大，且由于匹配点集仅为一部分西安交通大学硕士学位论文2表观特征点，算法没有 ICP 算法精确。使用单目摄像机 13。进行位姿估计主要通过提取连续图像中的特征点，然后对这些特征点进行追踪，提取场景信息，最终利用 2D-2D 特征匹配算法计算出摄像机自身运动的位姿。然而单目摄像机位姿估计存在着范围规模不明确、系统静止时发生漂移无法正确估计位姿等问题。近年来，使用以 Microsoft Kinect 摄像机为代表的 R
21、GB-D 摄像机进行位姿估计逐渐成为一个主流方式。RGB-D 摄像机在获取彩色图像的同时还能实时获得与之相对应的深度信息。由于传感器数据的丰富，基于 RGB-D 摄像机的位姿估计方法有别于传统 SLAM 系统中的方法。在 RGB-D 摄像机中，彩图和深度图是逐像素严格对准的，因此在没有任何运算代价的前提下可以直接获得彩色图像中任意像素的深度值，基于这个条件 Felix Endres14的 RGBDSLAM 系统，利用 2D 图像的特征匹配，获得不同时刻下特征点集的 3D-3D 匹配关系。通过这些匹配关系，利用 RANSAC 机制获得不同时刻间摄像机的相对位姿关系。这种方法速度快且位姿估计结果比
22、较稳定，然而由于位姿求解过程中使用的是特征点的匹配方法，而特征点的稀疏性导致位姿估计结果不够精确，此外该系统无法在缺乏纹理和光照不恒定的环境中使用。相对于激光传感器，RGB-D 摄像机利用非常低廉的代价同样可以通过深度信息获得 3D 点云，因此Newcombe15的 Kinectfusion 系统 16使用 ICP 匹配方法获得摄像机相对位姿估计结果。其结果相较于基于表观 3D-3D 特征点集匹配更加精确，然而由于算法本身的原因，ICP 匹配在缺乏良好几何结构的环境中（如地面，墙面）或没有良好初值的情况下通常无法得到正确的结果，甚至会发生严重的错误，直接影响位姿估计的精度。Henry17的 R
23、GB-D Mapping 项目与本文类似，通过有效结合深度和彩色信息提高位姿估计精度，但这个系统未对离群值进行专门处理，导致其会直接影响最终的位姿估计精度，并且该系统在对位姿联合优化时并未考虑可能存在的观测错误情况，此外该系统中用于优化的求解器 TORO1819仅基于 2.5D 重投影误差的方法，结果精度难以保证。从整个 SLAM 系统的角度来看，Kinectfusion 系统是个开环的系统，没有任何闭环检测 202122的功能，无法克服累积误差对位姿估计结果的影响；而 RGBDSLAM和 RGBD Mapping 的闭环检测工作都需要和很多抽样出的彩图进行特征匹配，如果抽样频率太低会漏掉潜在
24、的闭环，而如果抽样频率过高会急剧增加运算负担。针对上述问题，本文 RGB-D 数据融合的 SLAM 方法都有相应的改进和完善措施，并通过在公开数据集 23上的测试，证明本方法具有精度高、鲁棒性强且不受应用场景限制等特点。1.3 主要研究内容与组织结构本文的主要工作是设计并实现了一个 RGB-D 数据融合的鲁棒 SLAM 方法，在这个方法中，首先完成了对 RGB-D 摄像机的标定；随后利用标定后的 RGB-D 摄像机采集同步的彩色图像数据和深度图像数据，提出精度更高的 RGB-D 数据融合相对位姿估计算法；进而在图结构框架下利用 Sparse Bundle Adjustment（SBA） 242
25、5算法通过Error! No text of specified style in document.Error! No text of specified style in document.3g2o26工具包实现对摄像机位姿的联合优化；最后通过 PCL2728点云库对三维场景进行稠密重建。文章在公开数据集上对各个主要模块进行测试实验，根据实验结果分析各个模块的功能以及实现情况，最后将本文方法与其他先进的 SLAM 系统进行对比试验，验证本文方法的精确性、鲁棒性和实用性。下面是本文的章节安排。第一章阐明了文章的研究背景，本章调研目前 SLAM 问题尤其是基于 RGB-D 摄像机的 SLAM
26、问题的研究情况，并对各个主要方法的优缺点进行分析，结合自身的应用需求，初步确定了 RGB-D 数据融合的鲁棒 SLAM 研究方法。第二章是 RGB-D 数据融合 SLAM 研究方法的整体设计，本章给出了方法的整体设计方案，简要介绍了系统中的主要硬件：RGB-D 摄像机；介绍了系统涉及到的各个软件部分：ROS 29机器人操作平台、OpenNI 30开放自然交互接口以及 PCL 点云库；并对方法中主要算法：RGB-D 摄像机标定算法、RGB-D 数据融合的位姿估计算法、基于图结构模型的优化算法进行了概述。第三章是 RGB-D 摄像机的标定实现，该部分重点介绍了本文使用的 RGB-D 摄像机Kine
27、ct 的成像原理、成像模型，推导了 Kinect 摄像机的标定原理，并完成对Kinect 摄像机中彩色摄像头和红外摄像头的联合标定工作。第四章是 RGB-D 数据融合的相对位姿估计算法的设计与实现，该部分通过分析图像特征匹配方法和稠密点云匹配方法的原理和优缺点，提出精度更高的数据融合的相对位姿估计算法，给出了算法的原理和建模方法，并利用添加鲁棒函数的方法进一步增强算法对不同环境的适应性。第五章是基于图结构的优化，本章将摄像机位姿估计问题转化为图结构模型的建立和优化问题，针对结点约束添加机制、闭环检测机制以及图模型联合优化原理进行介绍，通过对图结构的联合优化实现对摄像机绝对位姿的全局一致性鲁棒估
28、计。第六章是实验部分，该部分包括了实验的设计方案、位姿估计精度实验、与其他SLAM 系统的对比实验以及相应的实验结果分析。第七章是对全文所做工作进行了总结，并对后续的工作做了初步的展望。西安交通大学硕士学位论文42 RGB-D SLAM 系统设计基于 RGB-D 数据融合的 SLAM 方法通过 RGB-D 摄像机同步获取彩色图像以及对应的深度图像，融合彩图信息和深度信息鲁棒而精确的估计不同时刻间摄像机的相对位姿关系，进而通过图结构模型的全局优化获得具有全局一致性关系的摄像机绝对位姿估计结果，并利用开源的 PCL 库对三维场景进行实时稠密重建，完成 SLAM 工作。相对于单一的基于彩色图像和基于深度点云的 SLAM 系统，本系统的摄像机位姿估计以及三维重建结果具有更高的精度及鲁棒性。根据系统的工作原理，本文设计了基于 RGB-D 数据融合的 SLAM 系统的总体方案。2.1 系统总体设计方案基于 RGB-D 数据融合的 SLAM 系统的总体设计方案如图 2-1 所示，系统由Kinect 摄像机、软件系统以及算法模块等几部分组成。其中软件系统包括 ROS 机器人操作平台、OpenNI 开放自然交互接口以及 PCL 点云库，主要算法模块包括：RGB-D 摄像机标定算法、RGB-D 数据融合的位姿估计算法、基于图结构模型的优化算法。RGB-D 摄像机负责采集彩色图像数据以及深度图像数
