学校编码：10384 分类号 密级 学号：31520121153008 UDC
硕 士 学 位 论 文
基于 6-DOF 姿态估计的室内移动二维与三
维构图
Indoor Mobile 2D and 3D Mapping Based on 6-DOF Pose
Estimation
戚传江
指导教师姓名：温程璐 助理教授
专 业 名 称：计算机技术
论文提交日期：2015 年 月
论文答辩时间：2015 年 月
学位授予日期：2015 年 月
答辩委员会主席：
评 阅 人：
2015 年 月
厦门大学博硕士论文摘要库
厦门大学学位论文原创性声明
本人呈交的学位论文是本人在导师指导下,独立完成的研究成果。
本人在论文写作中参考其他个人或集体已经发表的研究成果，均在文
中以适当方式明确标明，并符合法律规范和《厦门大学研究生学术活
动规范（试行）》。
另外，该学位论文为（ ）课题（组）
的研究成果，获得（ ）课题（组）经费或实验室的
资助，在（ ）实验室完成。（请在以上括号内填写
课题或课题组负责人或实验室名称，未有此项声明内容的，可以不作
特别声明。）
声明人（签名）：
年 月 日
厦门大学博硕士论文摘要库
厦门大学学位论文著作权使用声明
本人同意厦门大学根据《中华人民共和国学位条例暂行实施办法》
等规定保留和使用此学位论文，并向主管部门或其指定机构送交学位
论文（包括纸质版和电子版），允许学位论文进入厦门大学图书馆及
其数据库被查阅、借阅。本人同意厦门大学将学位论文加入全国博士、
硕士学位论文共建单位数据库进行检索，将学位论文的标题和摘要汇
编出版，采用影印、缩印或者其它方式合理复制学位论文。
本学位论文属于：
（ ）1.经厦门大学保密委员会审查核定的保密学位论文，
于 年 月 日解密，解密后适用上述授权。
（ ）2.不保密，适用上述授权。
（请在以上相应括号内打―√‖或填上相应内容。保密学位论文应
是已经厦门大学保密委员会审定过的学位论文，未经厦门大学保密委
员会审定的学位论文均为公开学位论文。此声明栏不填写的，默认为
公开学位论文，均适用上述授权。）
声明人（签名）：
年 月 日
厦门大学博硕士论文摘要库
I
摘 要
移动机器人的同时定位与地图构建是机器人学的研究热点，具有重要的研究 价值和广泛的应用前景。精确的定位与地图是移动机器人自我控制与自主导航的 基础，是移动机器人在未知环境中顺利开展工作的关键问题，在高危环境营救、 矿井环境探测、智能服务型机器人、太空及海底探索等领域有着广泛的应用价值。 本文以移动机器人在未知的室内真实环境中同时定位与地图构建为研究目 标，以 Pioneer3-AT 四轮移动机器人为移动平台，通过搭载 LMS 100 激光雷达、 MTi-10 惯性测量单元（IMU）及 Kinect 摄像机等传感器，构建了一个基于 6-DOF 姿态估计、二维栅格地图和三维点云地图同时构建的室内构图系统。该系统由 “2D SLAM”模块、“6-DOF 姿态估计”模块和“三维地图构建”模块共三个模 块有机组成，其中，“2D SLAM”模块与“6-DOF 姿态估计”模块可以单独分离 构成 6-DOF 姿态估计系统。 “2D SLAM”模块利用二维激光雷达高精度、高分辨率的性能，采用 EKF-SLAM 解决方案，构建精确的二维栅格地图和估计移动机器人二维平面内 的 3-DOF 姿态信息；“6-DOF 姿态估计”模块采用信息融合技术，融合惯性测量 单元（IMU）和“2D SLAM”中的 3-DOF 姿态信息，实现移动机器人的 6-DOF 姿态估计；“三维地图构建”模块将 6-DOF 姿态信息引入配准算法，对 ICP 的初 始值进行优化，构建信息丰富的三维点云地图，通过粒子滤波器进行闭环检测并 以 g2 o 图优化器完成全局最优化，最终生成全局一致的三维点云地图。 本文构建的二维和三维同时构图系统在室内真实环境中进行测试，并通过与 当前其他解决方案进行实验对比，测试结果表明本文系统在未知室内环境中能够 高效、可靠的进行 6-DOF 姿态估计、构建二维栅格地图和三维点云地图。而且 该系统还具有良好的扩展性。 关键词： SLAM，6-DOF 姿态估计，惯性测量单元（IMU）， 三维地图构建厦门大学博硕士论文摘要库
II
III
Abstract
The problem of simultaneous localization and mapping (SLAM) for the mobile robot has been one of the most actively research focus in the robotics and has very important research value and wide application prospects. Accurate localization and map is the basis of self-control and autonomous navigation for mobile robot and the key problem for mobile robots to do Successful working in unknown environments, which has wide application value in many fields such as the rescue in high-risk environment, mine environmental detection, intelligent service robots, exploration for outer space and seabed.
For the research of SLAM for mobile robot in the unknown indoor real environment, this paper presents a novel indoor mobile mapping system that provides a 6 degree of freedom (6-DOF) pose estimation and builds a 2D grid-based map and 3D point cloud map at the same time. This system has a four-wheeled Pioneer3-AT mobile robot as the basic mobile platform and is equipped with LMS 100 laser scanner, MTi-10 inertial measurement unit (IMU) and Kinect camera. This system consists of three integral modules: "2D SLAM" module, "6-DOF pose estimation" module and "3D mapping" module, in which "2D SLAM" module and "6-DOF pose estimation" module can been separated to form a 6-DOF pose positioning system.
"2D SLAM" module uses EKF-SLAM solution, makes full use of high accuracy and high resolution of modern laser scanner to build an accurate 2D grid-based map and estimate 3-DOF pose information of mobile robot in a 2D plane. "6-DOF pose estimation" module uses information fusion technology to fuse the IMU information and the 3-DOF pose information from "2D SLAM" module, in order to achieve 6-DOF pose localization of mobile robot. In the "3D mapping" module, 6-DOF pose information has been used to optimize the initial value of the ICP registration algorithm, which aim is to build an information-rich 3D point cloud map. In addition, this paper uses particle filter algorithm to detect closed loop and complete global
IV
optimization with g2o graph optimizer. Finally, this module results into a 3D point cloud map with global consistency.
Our indoor mobile mapping system is tested in real indoor environment. Compared with other similar solutions, the test results show that the proposed system can be more efficient and higher robust in 6-DOF pose estimation and the building of 2D grid-based map and 3D point cloud map Simultaneously in unknown indoor environment. Furthermore, our system also has good scalability.
Key Words：SLAM, 6-DOF pose estimation, Inertial Measurement Unit (IMU), 3D
mapping
V
目 录
摘 要 ... I
Abstract ... III
第一章 绪论 ... 1
1.1 研究背景及意义 ... 1 1.2 研究现状 ... 2 1.2.1 同时定位与地图构建的研究概况 ... 2 1.2.2 室内 SLAM 研究现状 ... 7 1.3 本文的主要研究工作及其创新点 ... 10 1.4 本文的结构安排 ... 12第二章 基于二维激光雷达的 2D SLAM 系统
...
13
2.1 引言 ... 13 2.2 地图的描述方式 ... 14 2.3 常用的 SLAM 系统模型 ... 16 2.3.1 SLAM 基本原理 ... 16 2.3.2 EKF-SLAM 系统 ... 18 2.3.3 RBPF-SLAM 系统 ... 20 2.4 2D SLAM 系统 ... 21 2.5 本章小结 ... 22第三章 基于数据融合的 6-DOF 姿态估计
...
23
3.1 引言 ... 23 3.2 基于 EKF 的 6-DOF 姿态估计系统 ... 24 3.2.1 系统状态方程 ... 24 3.2.2 观测模型的优化和协方差矩阵的求解 ... 26 3.2.3 基于 EKF 的信息融合 ... 29 3.3 本章小结 ... 29厦门大学博硕士论文摘要库
VI
第四章 基于 6-DOF 姿态估计的三维地图构建 ... 31
4.1 引言 ... 31 4.2 三维地图构建 ... 32 4.2.1 点云配准算法 ... 32 4.2.2 RGB-D SLAM 系统 ... 34 4.2.3 三维地图构建方案 ... 35 4.3 闭环检测与全局最优化解决方案 ... 36 4.4 本章小结 ... 37第五章 实验结果及分析 ... 39
5.1 系统结构 ... 39 5.1.1 系统硬件结构 ... 40 5.1.2 系统软件结构 ... 43 5.2 测试实验与结果分析 ... 44 5.2.1 室内二维构图比较实验 ... 44 5.2.2 配准算法的对比实验 ... 48 5.2.3 6-DOF 姿态估计对三维地图构建的影响 ... 51第六章 总结与展望 ... 57
参考文献 ... 59
附录 攻读硕士期间发表的论文 ... 69
致 谢 ... 71
厦门大学博硕士论文摘要库
VII
Contents
Abstract in Chinese ... I
Abstract in English ... III
Chapter 1 Introduction ... 1
1.1 Research Background and Signification ... 1
1.2 Research Review ... 2
1.2.1 Research Summary of Simultaneous Localization and Mapping ... 2
1.2.2 Research Summary of Indoor SLAM ... 7
1.3 Main Work and Innovation of This Paper ... 10
1.4 Structure of This Paper ... 12
Chapter 2 2D SLAM System Based on 2D Laser Scanner
...
13
2.1 Introduction ... 13
2.2 Map Representation ... 14
2.3 Common SLAM System Model ... 16
2.3.1 Basic Principle of SLAM ... 16
2.3.2 EKF-SLAM System ... 18
2.3.3 RBPF-SLAM System ... 20
2.4 2D SLAM System ... 21
2.5 Conclusions ... 22
Chapter 3 6-DOF Pose Estimation Based on Data Fusion
...
23
3.1 Introduction ... 23
3.2 6-DOF Pose Estimation Based on EKF ... 24
3.2.1 System State Equation ... 24
3.2.2 Optimization of Observation Model and Solving of Covariance Matrix .... 26
3.2.3 Data Fusion Based on EKF ... 29
3.3 Conclusions ... 29
VIII
Chapter 4 3D Mapping Based on 6-DOF Pose Estimation ... 31
4.1 Introduction ... 31
4.2 3D Mapping ... 32
4.2.1 Registration Algorithn of Point Cloud... 32
4.2.2 RGB-D SLAM System ... 34
4.2.3 3D Mapping Solution ... 35
4.3 Loop Closure Detection and Global Optimization Solution ... 36
4.4 Conclusions ... 37
Chapter 5 Experimental Results and Analysis ... 39
5.1 System Structure ... 39
5.1.1 The Structure of System Hardware ... 40
5.1.2 The Structure of System Software ... 43
5.2 Tested Experimental and Result Analysis ... 44
5.2.1 Comparative Experimental of Indoor 2D Mapping ... 44
5.2.2 Comparative Experimental of Registration Algorithm ... 48
5.2.3 The Effect of 6-DOF Pose Estimation on 3D Mapping ... 51
Chapter 6 Summary and Future Work ... 57
References ... 59
Appendix My Published Papers ... 69
Acknowledgements ... 71
第一章 绪论 1
第一章 绪论
1.1 研究背景及意义
自上世纪 60 年代以来，随着计算机技术、人工智能、多传感器信息融合等 专业技术的不断发展，机器人领域取得了巨大的科研成果。与传统的工业机器人 相比，移动机器人具有更大的优势，体现了更多的自主性、智能性和自适应性, 在工业、军事、科学研究等领域具有十分广泛的应用价值。 移动机器人在高危环境营救、矿井环境探测、服务型机器人、太空及海底探 索等领域有着不可替代的作用。这些未知的危险环境要求移动机器人不仅要构建 出环境地图，使人类和机器人能够认知当前环境，而且要解决自身的定位问题， 便于机器人完成下一步决策与路径规划。因此，可靠的自主导航能力是移动机器 人完成作业的前提，是移动机器人实现自主移动的基础。真正的自主导航要求移 动机器人在未知环境不断探索，逐步构建环境地图，并且进行精确的自身定位， 以及执行相应的规划任务。 同时定位与地图构建是提高移动机器人未知环境下工作能力所迫切需要解 决的关键问题。加快移动机器人自身定位和环境构图方面的研究，不仅可以提高 机器人学的技术水平，还增强了移动机器人在未知环境的自主性、智能性和自适 应性，使其更好地应用在各个行业。因此，开展自身定位和环境构图领域的研究 具有非常重要的科学前景和经济效益。 本文就移动机器人在未知室内环境中自身定位和地图构建问题展开研究。以 Pioneer3-AT 移动机器人为移动平台，利用二维激光雷达解决二维 SLAM 问题， 构建二维栅格地图，并估计移动机器人在平面内 3-DOF 姿态信息；融合 3-DOF 姿态和惯性测量单元（IMU）信息，获得移动机器人 6-DOF 姿态信息；利用 RGB-D 摄像机获取环境的局部三维地图，结合上述姿态信息，采用扫描匹配和特征点匹 配等匹配方法，最终生成全局三维地图，并在真实室内环境中对整个系统进行测 试实验与结果分析。厦门大学博硕士论文摘要库
基于 6-DOF 姿态估计的室内移动二维与三维构图 2 本章首先简要介绍移动机器人自身定位与环境构图的研究背景及意义，随后 阐述同时定位与地图构建的研究概况并详细综述室内地图构建领域的研究现状， 最后简要阐述本文的主要工作及其创新点，并说明本文的结构安排。
1.2 研究现状
1.2.1 同时定位与地图构建的研究概况 自主导航问题是移动机器人研究中需要解决的关键问题。当前，自主导航领 域的三个基本问题由 Leonard 和 Durrant-Whyte[10]提出：（1）“我在哪儿？（Wheream I?）”；（2）“我要去哪儿？（Where am I going?）”；（3）“怎么到达那里？（How should I going there?）”。其中，第一个问题是移动机器人的定位问题；第二个问 题是指移动机器人的目标；第三个问题是移动机器人的路径规划问题。精确的环 境地图是解决以上三个问题的关键所在。 路径规划/运动控制 自身定位 地图构建 探测 综合探测 SLAM 主动 定位 图 1.1 SLAM 与机器人学各个领域的关系 移动机器人的地图构建需要自身的精确定位，而自身定位依赖准确的环境地 图。因此，移动机器人的自身定位和地图构建问题被描述为“鸡和蛋”的问题， 针对此问题，研究人员提出了同时定位与地图构建的可能性。移动机器人构建环 境地图，并同时运用这个地图进行机器人定位，称作同时定位与地图构建 (Simultaneous Localization and Mapping ， SLAM) 或 并 发 定 位 和 地 图 构 建 （ Concurrent Localization and Mapping, CLM ）。 SLAM 问 题 是由 Smith 和 Cheeseman[11]等人在 1988 年提出来，是一种同时解决定位问题和地图构建问题
第一章 绪论 3 的概率方法。经过 20 多年的研究发展，SLAM 问题取得极大的突破与发展，这 主要得益于 SLAM 算法的多样化、传感器的性能提高以及多传感器的融合技术。 可以预见，在不远的将来，移动机器人在特定的环境中帮助人类出色地完成探险、 探测和营救等任务。 移动机器人的地图构建问题与定位问题密切相关，主要分为在已知姿态下的 地图构建问题和在未知姿态下的地图构建问题两大类。在已知姿态下的地图构建 十分简单，只需要将各姿态的局部地图根据移动机器人运动信息，整合到同一个 世界坐标，即可构建出整个环境的全局地图。而在未知姿态下的地图构建十分复 杂。此时，在未知环境中，移动机器人从某一未知位置开始探索，在前行过程中 不断利用自身传感器获取周围环境信息，构建环境地图，同时利用已构建的地图 完成自身定位，同时回答“现在的世界是什么样？”和“我在哪里”这两个问题。 这就是移动机器人的同时定位与地图构建问题（Simultaneous Localization and Mapping）。
图 1.2 SLAM 结构图[7]
目前，SLAM 已成为未知环境下移动机器人定位与导航的主要研究方向，也 是移动机器人领域中的研究热点。当前 SLAM 问题常用的解决方法包括以下几 类：
（1）最大似然估计算法（Maximum Likelihood Estimation, MLE）[12]；
（2）扩展卡尔曼滤波器（Extended Kalman Filter, EKF）[13][14]；
（3）基于期望最大化算法（Expectation Maximization, EM）[16]；
Degree papers are in the “Xiamen University Electronic Theses and Dissertations Database”.
Fulltexts are available in the following ways:
1. If your library is a CALIS member libraries, please log on
http://etd.calis.edu.cn/ and submit requests online, or consult the interlibrary loan department in your library.
2. For users of non-CALIS member libraries, please mail to etd@xmu.edu.cn
for delivery details.
