【智能系统】基于图优化的移动机器人视觉SLAM

第13卷第2期 智能系统学报 Vol.13 No.2 2018年4月 CAAI Transactions on Intelligent Systems Apr.2018 D0:10.11992/tis.201612004 网络出版地址：http:/kns.cnki.net/kcms/detail/23.1538.TP.20170702.0425.012.html 基于图优化的移动机器人视觉SLAM 张毅，沙建松 (重庆邮电大学智能系统及机器人实验室.重庆400065) 摘要：针对目前移动机器人视觉SLAM((simultaneous localization and mapping)研究中存在的实时性差、精确度不 高、无法稠密化建图等问题，提出了一种基于RGB-D数据的实时SLAM算法。在本算法前端处理中，采用了鲁棒性 与实时性更好的ORB特征检测。利用RANSAC算法对可能存在的误匹配点进行别除完成初始匹配，对所得内点进 行PNP求解，用于机器人相邻位姿的增量估计。在后端优化中，设计了一种遵循图优化思想的非线性优化方法对移 动机器人位姿进行优化。同时结合闭环检测机制，提出了一种点云优化算法，用于抑制系统的累积误差，进一步提升 位姿与点云的精确性。实验验证了本文所提方法能够迅速、准确地重构出稠密化的三维环境模型。 关键词：RGB-D;移动机器人；图优化：同时定位与地图构建：位姿与点云优化 中图分类号：TP24 文献标志码：A文章编号：1673-4785(2018)02-0290-06 中文引用格式：张毅，沙建松.基于图优化的移动机器人视觉SL4M.智能系统学报，2018,13(2)少：290-295， 英文引用格式：ZHANG Yi,SHA Jiansong.Visual--SLAM for mobile robot based on graph optimizationJ.CAAI transactions on intelligent systems,2018,13(2):290-295. Visual-SLAM for mobile robot based on graph optimization ZHANG Yi,SHA Jiansong (Laboratory of Intelligent System and Robotics,Chongqing University of Posts and Telecommunications,Chongqing 400065,China) Abstract:In the present research on the visual SLAM(Simultaneous Localization and Mapping)of mobile robot,some defects as inferior real-timeness,low accuracy and hard to densified mapping exist,therefore,the paper proposed a real- time SLAM algorithm based on RGB-D data.In the front-end processing of the algorithm,the ORB feature detection with better robustness and real-timeness was adopted.RANSAC algorithm was utilized to get rid of the possible mis- match points and complete the initial match.For the obtained inner point,PNP solution was carried out for using as the increment estimate of the adjacent pose of robot.In the rear-end optimization,a nonlinear optimization method obeying image optimization thought was used for optimizing the pose of a moving robot.In addition,in combination with the closed-loop detection mechanism,a point cloud optimization algorithm was proposed to suppress the cumulative error of the system and further improve the accuracy of pose and point cloud.The experimental results show that the proposed method can reconstruct the dense 3D environment model quickly and accurately. Keywords:RGB-D;mobile robot;graph optimization,SLAM;pose and point cloud optimization 同时定位与地图创建(SLAM)技术已经成为目SLAM算法中亟待解决的一个问题。而新型RGB- 前机器人研究领域的热点，随着SLAM问题研究的 D摄像机Kinect,具有丰富3维空间信息与颜色 不断深人，对机器人位姿监测和环境地图创建的精 纹理信息，为上述问题的解决提供了的可能。文献 确度、实时性也都有了更高的要求。因此，如何在 [2]提出的基于RGB-D相机的室内SLAM方法可实 精确度与实时性之间寻求一种平衡是目前众多 现对室内环境的稠密化建图，但论文中的SIFT特 收稿日期：2016-12-05.网络出版日期：2017-07-02 征提取算法较为费时，会占用较多的运算资源，因 基金项目：重庆市科学技术委员会项目(CSTC2015 jcyjBX0066), 通信作者：沙建松.E-mail:277829172@qq.com. 此其实时性较差。ORB-SLAM采用了基于单目摄

DOI: 10.11992/tis.201612004 网络出版地址: http://kns.cnki.net/kcms/detail/23.1538.TP.20170702.0425.012.html 基于图优化的移动机器人视觉 SLAM 张毅，沙建松 （重庆邮电大学 智能系统及机器人实验室，重庆 400065） 摘 要：针对目前移动机器人视觉 SLAM(simultaneous localization and mapping) 研究中存在的实时性差、精确度不 高、无法稠密化建图等问题，提出了一种基于 RGB-D 数据的实时 SLAM 算法。在本算法前端处理中，采用了鲁棒性 与实时性更好的 ORB 特征检测。利用 RANSAC 算法对可能存在的误匹配点进行剔除完成初始匹配，对所得内点进 行 PNP 求解，用于机器人相邻位姿的增量估计。在后端优化中，设计了一种遵循图优化思想的非线性优化方法对移 动机器人位姿进行优化。同时结合闭环检测机制，提出了一种点云优化算法，用于抑制系统的累积误差，进一步提升 位姿与点云的精确性。实验验证了本文所提方法能够迅速、准确地重构出稠密化的三维环境模型。 关键词：RGB-D；移动机器人；图优化；同时定位与地图构建；位姿与点云优化 中图分类号：TP24 文献标志码：A 文章编号：1673−4785(2018)02−0290−06 中文引用格式：张毅, 沙建松. 基于图优化的移动机器人视觉 SLAM[J]. 智能系统学报, 2018, 13(2): 290–295. 英文引用格式：ZHANG Yi, SHA Jiansong. Visual-SLAM for mobile robot based on graph optimization[J]. CAAI transactions on intelligent systems, 2018, 13(2): 290–295. Visual-SLAM for mobile robot based on graph optimization ZHANG Yi，SHA Jiansong (Laboratory of Intelligent System and Robotics, Chongqing University of Posts and Telecommunications, Chongqing 400065, China) Abstract: In the present research on the visual SLAM (Simultaneous Localization and Mapping)of mobile robot, some defects as inferior real-timeness, low accuracy and hard to densified mapping exist, therefore, the paper proposed a real￾time SLAM algorithm based on RGB-D data. In the front-end processing of the algorithm, the ORB feature detection with better robustness and real-timeness was adopted. RANSAC algorithm was utilized to get rid of the possible mis￾match points and complete the initial match. For the obtained inner point, PNP solution was carried out for using as the increment estimate of the adjacent pose of robot. In the rear-end optimization, a nonlinear optimization method obeying image optimization thought was used for optimizing the pose of a moving robot. In addition, in combination with the closed-loop detection mechanism, a point cloud optimization algorithm was proposed to suppress the cumulative error of the system and further improve the accuracy of pose and point cloud. The experimental results show that the proposed method can reconstruct the dense 3D environment model quickly and accurately. Keywords: RGB-D; mobile robot; graph optimization; SLAM; pose and point cloud optimization 同时定位与地图创建 (SLAM) 技术已经成为目 前机器人研究领域的热点，随着 SLAM 问题研究的 不断深入，对机器人位姿监测和环境地图创建的精 确度、实时性也都有了更高的要求。因此，如何在 精确度与实时性之间寻求一种平衡是目前众多 SLAM 算法中亟待解决的一个问题。而新型 RGB￾D 摄像机 Kinect[1] ，具有丰富 3 维空间信息与颜色 纹理信息，为上述问题的解决提供了的可能。文献 [2]提出的基于 RGB-D 相机的室内 SLAM 方法可实 现对室内环境的稠密化建图，但论文中的 SIFT 特 征提取算法较为费时，会占用较多的运算资源，因 此其实时性较差。ORB-SLAM[3]采用了基于单目摄 收稿日期：2016−12−05. 网络出版日期：2017−07−02. 基金项目：重庆市科学技术委员会项目 (CSTC2015jcyjBX0066). 通信作者：沙建松. E-mail：277829172@qq.com. 第 13 卷第 2 期 智 能 系 统 学 报 Vol.13 No.2 2018 年 4 月 CAAI Transactions on Intelligent Systems Apr. 2018

第2期 张毅，等：基于图优化的移动机器人视觉SLAM ·291· 像机的SLAM方案，以高效的ORB特征贯穿于系 SLAM问题建模分析，遵循图优化思想，设计了一 统中的特征处理、匹配、回环检测、重定位等方面， 种非线性优化方法，实现对机器人位姿的优化。并 更侧重于系统处理的实时性，但因为单目相机的限 与闭环检测机制相结合，提出了一种基于局部闭 制，ORB-SLAM算法在稠密化建图方面还有所欠 环与全局闭环约束的点云优化算法，同时完成对位 缺。针对目前移动机器人视觉SLAM研究中存在的 姿与稠密化点云的优化，可有效地抑制系统的累积 实时性差、精确度不高、无法稠密化建图等问题.本 误差，进一步提升整个系统的精确性。最后通过实 文提出了一种基于Kinect深度摄像机的移动机器 验，验证了本文SLAM方法的可行性和有效性。 人三维视觉SLAM算法，以实现对环境进行实时 的、精确的稠密化建图。在本算法前端处理中，通 1系统结构 过分析多种特征点检测算法的优劣，采用了鲁棒性 与实时性更好的ORB特征检测。利用RANSAC 本文提出的室内环境下移动机器人三维视觉 算法对可能存在的误匹配点进行剔除完成初始匹 SLAM算法的总体框架如图1所示，主要分为前端 配，以PNP(perspective--N-point)l问题求解的方式实 的关键帧筛选、帧间配准、闭环检测和后端的图优 现了图像的快速配准。在后端优化中，通过对 化、点云融合、点云优化几个部分。 RGB图像 构建数据帧 关键帧序列 帧间配准 图优化 筛选 点云融合 深度图像 闭环检测 点云优化 点云图 图1基于图优化的视觉SLAM系统框架 Fig.1 Visual-SLAM system framework based on graph optimization 2特征点的检测与配准 tectorExtractor接口设置特征描述子提取器。 3)匹配：通过各个角点的描述子来判断它们在 2.1特征点的检测 两张图像中的对应关系。这里采用的方法为快速最 特征点的检测与匹配是视觉SLAM问题中“图 近邻算法FLANN。 像前端”的部分，也是视觉SLAM中占用计算资源 4)去外点：利用RANSAC算法去除错误匹配 最多，最影响系统实时性的部分。基于FAST特征 的外点，保留正确的内点。 检测改进的ORB特征具有旋转不变和尺度不变 S)位姿求解：调用Opencv的PnPsolver()函数， 特性的二进制特征，它是一种高效的具有良好针对 求解相机的位姿运动(R,)。 视图不变的特征，比SIFT特征检测快两个数量级， 同时特征点的质量较高、鲁棒性好。考虑到SLAM 3位姿优化 问题对鲁棒性、实时性的要求，本文采用了ORB特 针对传统的利用滤波方法估计位姿存在线性化 征提取算法。 误差、更新效率低下等问题，本文依据SLAM研究 2.2特征点的配准 中的图优化思想，设计了一种基于非线性优化的 图像产生的本质是光线通过相机透镜投影到图 位姿优化方法，同时实现局部位姿优化和全局位姿 像传感器上，这个过程是3-D场景到2-D平面的投 优化。 影，因此场景与它的成像之间存在重要的联系，相 3.1模型的建立 同场景的不同成像之间也有联系。而通过这些联 SLAM的经典数学模型通常分为3部分：状态 系，可以实现特征点的配准。 变量、运动方程、观测方程。 特征点配准算法可以分为以下5个步骤： X=[m…xwM2…yw] (1) l)提取特征点：Opencv提供了通用接口 xk=f(Xk-1,Ug)+ug (2) cv:FeatureDetector,.这里将其设定为ORB特征。 =g(xk,y）+ (3) 2)提取描述子：对于检测出的角点，用一些数 本文主要以Kinect作为主要的传感器输入，通 学上的特征对其进行描述，这里通过cv:FeatureDe 过对视觉信号的处理，构建视觉里程计，用于位姿

像机的 SLAM 方案，以高效的 ORB特征贯穿于系 统中的特征处理、匹配、回环检测、重定位等方面， 更侧重于系统处理的实时性，但因为单目相机的限 制，ORB-SLAM 算法在稠密化建图方面还有所欠 缺。针对目前移动机器人视觉 SLAM研究中存在的 实时性差、精确度不高、无法稠密化建图等问题，本 文提出了一种基于 Kinect 深度摄像机的移动机器 人三维视觉 SLAM 算法，以实现对环境进行实时 的、精确的稠密化建图。在本算法前端处理中，通 过分析多种特征点检测算法的优劣，采用了鲁棒性 与实时性更好的 ORB 特征检测。利用 RANSAC 算法[4]对可能存在的误匹配点进行剔除完成初始匹 配，以 PNP(perspective-N-point)[5]问题求解的方式实 现了图像的快速配准。在后端优化中，通过对 SLAM 问题建模分析，遵循图优化思想，设计了一 种非线性优化方法，实现对机器人位姿的优化。并 与闭环检测[6]机制相结合，提出了一种基于局部闭 环与全局闭环约束的点云优化算法，同时完成对位 姿与稠密化点云的优化，可有效地抑制系统的累积 误差，进一步提升整个系统的精确性。最后通过实 验，验证了本文 SLAM 方法的可行性和有效性。 1 系统结构 本文提出的室内环境下移动机器人三维视觉 SLAM 算法的总体框架如图 1 所示，主要分为前端 的关键帧筛选、帧间配准、闭环检测和后端的图优 化、点云融合、点云优化几个部分。 ׻RGBప ׻Ꮢప⌝ Ჰᐦ᪜ᢚፓ ڟ䩚ፓᎻ݃ ょ䔵 ፓ䬠䙹۲ పфࡂ 䬙⣛ᷬ≷ ◥ν㲹ऴ ◥νфࡂ◥ νప 图 1 基于图优化的视觉 SLAM 系统框架 Fig. 1 Visual-SLAM system framework based on graph optimization 2 特征点的检测与配准 2.1 特征点的检测 特征点的检测与匹配是视觉 SLAM 问题中“图 像前端”的部分，也是视觉 SLAM 中占用计算资源 最多，最影响系统实时性的部分。基于 FAST 特征 检测改进的 ORB 特征[7]具有旋转不变和尺度不变 特性的二进制特征，它是一种高效的具有良好针对 视图不变的特征，比 SIFT 特征检测快两个数量级， 同时特征点的质量较高、鲁棒性好。考虑到 SLAM 问题对鲁棒性、实时性的要求，本文采用了 ORB 特 征提取算法。 2.2 特征点的配准 图像产生的本质是光线通过相机透镜投影到图 像传感器上，这个过程是 3-D 场景到 2-D 平面的投 影，因此场景与它的成像之间存在重要的联系，相 同场景的不同成像之间也有联系。而通过这些联 系，可以实现特征点的配准。 特征点配准算法可以分为以下 5 个步骤： 1) 提取特征点：Openc v 提供了通用接口 cv::FeatureDetector，这里将其设定为 ORB 特征。 2) 提取描述子：对于检测出的角点，用一些数 学上的特征对其进行描述，这里通过 cv::FeatureDe￾tectorExtractor 接口设置特征描述子提取器。 3) 匹配：通过各个角点的描述子来判断它们在 两张图像中的对应关系。这里采用的方法为快速最 近邻算法 FLANN。 4) 去外点：利用 RANSAC 算法去除错误匹配 的外点，保留正确的内点。 5) 位姿求解：调用 Opencv 的 PnPsolver() 函数， 求解相机的位姿运动 (R,t)。 3 位姿优化 针对传统的利用滤波方法估计位姿存在线性化 误差、更新效率低下等问题，本文依据 SLAM 研究 中的图优化[8]思想，设计了一种基于非线性优化的 位姿优化方法，同时实现局部位姿优化和全局位姿 优化。 3.1 模型的建立 SLAM 的经典数学模型通常分为 3 部分：状态 变量、运动方程、观测方程。 X = [x1 x2 ··· xN y1 y2 ··· yM] (1) xk = f(xk−1,uk)+uk (2) zk, j = g(xk , yj)+nk,j (3) 本文主要以 Kinect 作为主要的传感器输入，通 过对视觉信号的处理，构建视觉里程计，用于位姿 第 2 期 张毅，等：基于图优化的移动机器人视觉 SLAM ·291·

·292· 智能系统学报 第13卷 估计。所以这里只考虑状态变量和观测方程，构建 3.2图的建立与优化 出目标函数： 上述SLAM问题全部抽象成节点与边，节点代 (xy,2=zkj-g(k,y） (4) 表了优化变量，包括pose节点和point节点。边代 E(X)= e(,y,z)'2'e(xy2） (5) 表了误差项约束条件，包括pose-point约束和pose 化EC pose约束。pose-point约束依赖于相机观测产生的 X=arg min E(X) (6) 约束，如图3(a)所示。pose-pose约束则依赖于系统 式中：x表示第k时刻的机器人位姿，也称为图优化 中闭环检测产生的约束，如图3b)、(c)所示，可有效 的一个pose节点；y,表示第k时刻机器人所能观测 地抑制系统的累积误差，从而得到信息一致的 到的路标，也称为图优化的一个point节点；，表示 地图。 pose节点与point节点的约束；特别地，在闭环检测 优化过程中，代表pose节点与pose节点的约束； (xy,)为由两顶点组成的边所产生的向量误差 函数，代表两顶点满足约束条件z,的程度，当它的 值为0时，表示两顶点完全满足匹配约束。 目标函数的物理意义：这里假设了误差函数e() (a)相机观测约束下的位姿优化 符合高斯分布(O,2),则SLAM问题的贝叶斯概率描 述为，求解适合的(x,y),使系统最有可能产生当前的 观测数据Z,即最大似然(MLE): (x,y)'=argmax p(ax.y) (7) I.V 最终所求的(，y,)的二次型（式(6）)，实质 上是在求解最大似然。即在假设噪声是高斯分布之 后，将整个SLAM问题等效为一个最小二乘问题。 进一步，利用李代数的扰动模型，求得目标函数关 b)局部闭环约束下的位姿优化 于pose和point的导数J和J,可对此最小二乘问题 求解。以此建立最终的求解方程： 6 e(x+6,y+6,)≈e(x,y)+J6x+J,6, (8) H=∑JFJ, (9) b=∑JFg'e, (10) 环 H6=-b (11) 式中：J代表相机微小运动后，像素的变化；J,代表 (©)全局闭环约束下的位姿优化 空间点微小运动后，像素的变化。式(11)代表了考 图3图的建立与优化 虑所有的变量与边的Full SLAM问题的求解。结 Fig.3 Establishment and optimization of graph 合SLAM问题的稀疏性，选用合适的梯度下降方 法，用求得的6用于状态变量X的更新，通过多次 4闭环检测下的点云优化 迭代，最终求得满足目标方程的状态变量X的收敛 在基于位姿增量估计的SLAM建图中，会产生 值。可借助通用图优化工具g2o来解决上述非线 误差的累积，则在长时间的大范围SLAM建图中， 性最小二乘问题，求解过程如图2所示。 就需要闭环检测对位姿和生成的点云进行优化。 构建图 确定目标 求雅克比 求增量方程 RGBD-SLAM算法是在局部回环检测后，只使用局 G=/VE 函数E(X) 矩阵J H&=-b 部回环优化后的位姿作为三维地图点云的重建。 状态更新 优化后的 ORB-SLAM算法则是在运动跟踪过程中，时刻检测 Xx=X十6 位姿 并维护关键帧之间的点云共视关系，同时实现对点 图2优化求解流程 云优化。但这种方法只适用于规模有限的稀疏点 Fig.2 Process of optimizing solution 云，并不适用于稠密化点云的优化。基于此，提出

估计。所以这里只考虑状态变量和观测方程，构建 出目标函数： e(xk , yj ,zk, j)=zk, j −g(xk , yj) (4) E(X) = ∑ (k, j)∈C e(xk , yj ,zk, j) TΩk j −1 e(xk , yj ,zk, j) (5) X ∗ = argmin X E(X) (6) xk yj zk, j zk, j e(xk , yj ,zk j) zk, j 式中： 表示第 k 时刻的机器人位姿，也称为图优化 的一个 pose 节点； 表示第 k 时刻机器人所能观测 到的路标，也称为图优化的一个 point 节点； 表示 pose 节点与 point 节点的约束；特别地，在闭环检测 优化过程中， 代表 pose 节点与 pose 节点的约束； 为由两顶点组成的边所产生的向量误差 函数，代表两顶点满足约束条件 的程度，当它的 值为 0 时，表示两顶点完全满足匹配约束。 (0,Ω) (x, y) 目标函数的物理意义：这里假设了误差函数 e(·) 符合高斯分布 ，则 SLAM 问题的贝叶斯概率描 述为，求解适合的 ，使系统最有可能产生当前的 观测数据 Z，即最大似然 (MLE)： (x, y) ∗ = argmax x,y p(z|x, y) (7) e(xk , yj ,zk j) Jx Jy 最终所求的 的二次型 (式 (6))，实质 上是在求解最大似然。即在假设噪声是高斯分布之 后，将整个 SLAM 问题等效为一个最小二乘问题。 进一步，利用李代数[9]的扰动模型，求得目标函数关 于 pose 和 point 的导数 和 ，可对此最小二乘问题 求解。以此建立最终的求解方程： e(x+δx , y+δy) ≈ e(x, y)+ Jxδx + Jyδy (8) H = ∑ i Ji TΩ −1 Ji (9) b = ∑ i Ji TΩ −1 ei (10) Hδx = −b (11) Jx Jy δx 式中： 代表相机微小运动后，像素的变化； 代表 空间点微小运动后，像素的变化。式 (11) 代表了考 虑所有的变量与边的 Full SLAM 问题的求解。结 合 SLAM 问题的稀疏性，选用合适的梯度下降方 法 [10] ，用求得的 用于状态变量 X 的更新，通过多次 迭代，最终求得满足目标方程的状态变量 X 的收敛 值。可借助通用图优化工具 g2o[11]来解决上述非线 性最小二乘问题，求解过程如图 2 所示。 a/cۇٴⅮ䯱 ⴕ䭡J ⟢ᔭᰠ᫜ XK+1=XK喎δ * ⶚჆Ⱊᴳ ܩ᪜E(X) Ⅾ්䛻᫥⼷ Hδ=−b ⮰ऺࡂф ѹ༫ Ჰᐦప G={V,E} 图 2 优化求解流程 Fig. 2 Process of optimizing solution 3.2 图的建立与优化 上述 SLAM 问题全部抽象成节点与边，节点代 表了优化变量，包括 pose 节点和 point 节点。边代 表了误差项约束条件，包括 pose-point 约束和 pose￾pose 约束。pose-point 约束依赖于相机观测产生的 约束，如图 3(a) 所示。pose-pose 约束则依赖于系统 中闭环检测产生的约束，如图 3(b)、(c) 所示，可有效 地抑制系统的累积误差，从而得到信息一致的 地图。 (a) Ⱔᱦ㻮≷㏒᲋̷⮰ѹ༫фࡂ (b) ᅬ䘔䬙⣛㏒᲋̷⮰ѹ༫фࡂ 2 8 3 4 5 6 9 7 1 10 䬙⣛ 䬙⣛ 1 2 3 4 5 6 7 (c) ڔᅬ䬙⣛㏒᲋̷⮰ѹ༫фࡂ 图 3 图的建立与优化 Fig. 3 Establishment and optimization of graph 4 闭环检测下的点云优化 在基于位姿增量估计的 SLAM 建图中，会产生 误差的累积，则在长时间的大范围 SLAM 建图中， 就需要闭环检测对位姿和生成的点云进行优化。 RGBD-SLAM 算法是在局部回环检测后，只使用局 部回环优化后的位姿作为三维地图点云的重建。 ORB-SLAM 算法则是在运动跟踪过程中，时刻检测 并维护关键帧之间的点云共视关系，同时实现对点 云优化。但这种方法只适用于规模有限的稀疏点 云，并不适用于稠密化点云的优化。基于此，提出 ·292· 智 能 系 统 学 报 第 13 卷

第2期 张毅，等：基于图优化的移动机器人视觉SLAM ·293· 了一种基于局部闭环与全局闭环约束的点云优化算 法，一共产生了929个匹配特征点，但误匹配点过 法，同时完成对位姿与稠密化点云的优化。算法具 多。因此，通过人工制定规则的筛选，去除了部分 体步骤如下： 误匹配，点，此时的匹配个数为250。为进一步提升 l)封装构造数据帧Frame,根据帧间的关联程 匹配精读，通过RANSAC算法对图4(b)进行进一 度，筛选出关键帧序列。 步的匹配筛选。最终，产生了97个经筛选后的匹配 2)执行帧间配准。完成相机位姿初始估计。 点，实验中整个匹配所耗时间为0.0035s。 3)在相机观测约束下，直接应用式(6)，构建如 图3(a)所示的图优化。求得优化后的相机的位姿 变化(R,),将原始点云进行坐标系变换，实现点云的 拼接融合。 4)执行闭环检测。通过动态计算闭环帧筛选 阈值，得到匹配质量较高的闭环帧候选序列。 (a)特征匹配算法后 5)闭环检测结束后，遍历闭环帧候选序列，在 当前帧与闭环候选帧之间，进行Si3(相似变换矩 阵)求解与优化。当某一候选帧通过了Sim3求解 与优化，则认为闭环检测成功，跳出对闭环顿候选 序列的遍历。求得对应相似变换矩阵为 s-RcSOR3CRIcR (12) b)人工规则筛选后 6)位姿优化的传播。当前帧坐标系与世界坐 标系之间的变换在5)中已经确定并优化。通过一 级相连关系的传播，可以确定这些相连的关键帧坐 标系与世界坐标系之间的相似变换矩阵。 7)点云优化。利用6)中的相似变换矩阵S,将 原始点云的世界坐标系转换到当前闭环帧处的坐标 (C)RANSAC算法匹配 系，进行点云的拼接融合，实现点云的优化。 图4特征提取与配准实验结果 5实验结果及分析 Fig.4 The experimental results of feature extraction and matching 5.1实验平台 5.2.2回环检测约束 为了验证算法的有效性，并保证本文实验的一 通过回环检测，可以增加帧间的约束，进一步 致可对比性，故采用了知名的TUM标准数据集四 提升系统的鲁棒性，进而提升整个SLAM的轨迹与 并选用了基于Kinect视觉传感器采集的室内基准 地图的质量。在本文算法中，通过筛选关键帧之后 数据包。该基准数据包中包含Kinect产生的彩色 进行回环检测，并以PNP求解的结果作为对边的估 图像和深度图像序列，以及机器人的真实运动位 计，增加到图优化中，如图5所示。 姿。本文实验数据处理平台为一台ntel双核2.4GHz 主频的PC机，运行Ubuntu14.04操作系统。程序 均设定以30帧/秒的速度同时读取数据包中的RGB 数据和深度数据。 5.2室内环境下的三维视觉SLAM 5.2.1特征提取与特征点配准 对两帧图像进行ORB特征提取后，在利用特 征匹配算法(FLANN)获得相邻帧间对应关系的基 础上，利用RANSAC算法对点云进行匹配，匹配结 图5回环检测产生的约束 果如图4(c)所示。在图4(a)中，通过特征匹配算 Fig.5 Loop closure of constraint

了一种基于局部闭环与全局闭环约束的点云优化算 法，同时完成对位姿与稠密化点云的优化。算法具 体步骤如下： 1) 封装构造数据帧 Frame，根据帧间的关联程 度，筛选出关键帧序列。 2) 执行帧间配准。完成相机位姿初始估计。 (R,t) 3) 在相机观测约束下，直接应用式 (6)，构建如 图 3(a) 所示的图优化。求得优化后的相机的位姿 变化 ，将原始点云进行坐标系变换，实现点云的 拼接融合。 4) 执行闭环检测。通过动态计算闭环帧筛选 阈值，得到匹配质量较高的闭环帧候选序列。 5) 闭环检测结束后，遍历闭环帧候选序列，在 当前帧与闭环候选帧之间，进行 Sim3(相似变换矩 阵) 求解与优化。当某一候选帧通过了 Sim3 求解 与优化，则认为闭环检测成功，跳出对闭环帧候选 序列的遍历。求得对应相似变换矩阵为 S = [ sR t 0 1 ] ,R ∈ SO(3),s ∈ R,t ∈ R 3 (12) 6) 位姿优化的传播。当前帧坐标系与世界坐 标系之间的变换在 5) 中已经确定并优化。通过一 级相连关系的传播，可以确定这些相连的关键帧坐 标系与世界坐标系之间的相似变换矩阵。 7) 点云优化。利用 6) 中的相似变换矩阵 S ，将 原始点云的世界坐标系转换到当前闭环帧处的坐标 系，进行点云的拼接融合，实现点云的优化。 5 实验结果及分析 5.1 实验平台 为了验证算法的有效性，并保证本文实验的一 致可对比性，故采用了知名的 TUM 标准数据集[12] ， 并选用了基于 Kinect 视觉传感器采集的室内基准 数据包。该基准数据包中包含 Kinect 产生的彩色 图像和深度图像序列，以及机器人的真实运动位 姿。本文实验数据处理平台为一台 Intel 双核 2.4 GHz 主频的 PC 机，运行 Ubuntu 14.04 操作系统。程序 均设定以 30 帧/秒的速度同时读取数据包中的 RGB 数据和深度数据。 5.2 室内环境下的三维视觉 SLAM 5.2.1 特征提取与特征点配准 对两帧图像进行 ORB 特征提取后，在利用特 征匹配算法 (FLANN) 获得相邻帧间对应关系的基 础上，利用 RANSAC 算法对点云进行匹配，匹配结 果如图 4(c) 所示。在图 4(a) 中，通过特征匹配算 法，一共产生了 929 个匹配特征点，但误匹配点过 多。因此，通过人工制定规则的筛选，去除了部分 误匹配点，此时的匹配个数为 250。为进一步提升 匹配精读，通过 RANSAC 算法对图 4(b) 进行进一 步的匹配筛选。最终，产生了 97 个经筛选后的匹配 点，实验中整个匹配所耗时间为 0.003 5 s。 (a) ➥ᒭࡥ䙹ッ∁ऺ (b) 人工规则筛选后 (c) RANSAC ッ∁ࡥ䙹 图 4 特征提取与配准实验结果 Fig. 4 The experimental results of feature extraction and matching 5.2.2 回环检测约束 通过回环检测，可以增加帧间的约束，进一步 提升系统的鲁棒性，进而提升整个 SLAM 的轨迹与 地图的质量。在本文算法中，通过筛选关键帧之后 进行回环检测，并以 PNP 求解的结果作为对边的估 计，增加到图优化中，如图 5 所示。 图 5 回环检测产生的约束 Fig. 5 Loop closure of constraint 第 2 期 张毅，等：基于图优化的移动机器人视觉 SLAM ·293·

·294· 智能系统学报 第13卷 5.2.3三维环境重构效果对比 本算法实时性也远优于RGBD-SLAM,与ORB 图6(a)为采用了ORB-SLAM方法得到的三维 SLAM的实时性相当。RGBD-SLAM处理一帧数 环境重构结果，所构建的三维地图为稀疏点云。图 据平均用时约为350ms,每秒钟只能处理3帧数 6(b)为采用RGBD-SLAM方法得到的三维环境重 据，因此不适于实时SLAM。而本算法前端处理一 构结果，物体轮廓可以辨别，但冗余点相对较多。 帧数据平均约为31ms,后端经过位姿优化与点云 图6(c)为采用本文的SLAM方法得到的三维环境 优化，以约5帧/秒的频率更新点云地图。可以满 重构结果，物体轮廓清晰可辨，冗余点明显减少。 足SLAM系统对实时性要求。 (a)ORB-SLAM重构结果 (b)RGBD-SLAM重构结果 (©)本文方法重构结果 图6室内环境三维重构结果 Fig.6 3D reconstruction result of the indoor environment 5.2.4机器人运动轨迹跟踪 的投影，如图7所示。表1则记录了不同算法在不 为了获得机器人位姿的变化，本文以3D-2D的 同数据集下，由计算所得的机器人在三维空间中的 特征点约束，采用PNP求解的方法对机器人相邻位 运动轨迹的真实位姿之间的均方根误差(RMSE)。 姿进行估计，并借助图优化方法对估计位姿进行优 通过均方根误差的对比，可有效地评估算法对机器 化。为了使运动轨迹更为直观，实验给出了本文算 人位姿跟踪的误差大小。可以看出本文方法能较好 法在不同数据集下，机器人运动轨迹在XY平面上 地实现机器人的运动轨迹跟踪。 1.0 真实位姿 1.0 真实位姿 估计位姿 一估计位姿 0.5 两种位姿的差异 0.5 两种位姿的差异 0.0 00 -0.5 -1.0 -1.0 5 -1.0-0.50.00.51.01.5 10 -0.5-0.00.51.01.52.0 m r/m (a)FR1/room (b)FR1/desk 0.2 真实位姿 1.0 真实位姿 0.0 估计位姿 0.9 估计位姿 0.2 一两种位姿的差异 0.8 两种位姿的差异 -0.4 0.6 0.6 -0.8 0.5 -1.0 -12 0.3 -0.50.00.51.01.52.0 1.1 1.2 1.3 1.4 m x/m (a)FR1/floor (b)FR1/xyz 图7真实位姿与估计位姿对比图 Fig.7 Comparison between ground truth and estimated trajectories

5.2.3 三维环境重构效果对比 图 6(a) 为采用了 ORB-SLAM 方法得到的三维 环境重构结果，所构建的三维地图为稀疏点云。图 6(b) 为采用 RGBD-SLAM 方法得到的三维环境重 构结果，物体轮廓可以辨别，但冗余点相对较多。 图 6(c) 为采用本文的 SLAM 方法得到的三维环境 重构结果，物体轮廓清晰可辨，冗余点明显减少。 本算法实时性也远优于 RGBD-SLAM，与 ORB￾SLAM 的实时性相当。RGBD-SLAM 处理一帧数 据平均用时约为 350 ms，每秒钟只能处理 3 帧数 据，因此不适于实时 SLAM。而本算法前端处理一 帧数据平均约为 31 ms，后端经过位姿优化与点云 优化，以约 5 帧/秒的频率更新点云地图。可以满 足 SLAM 系统对实时性要求。 (a) ORB-SLAM 䛹Ჰ㏿᳈ (b) RGBD-SLAM 䛹Ჰ㏿᳈ (c) ᱘᪳᫥∁䛹Ჰ㏿᳈ 图 6 室内环境三维重构结果 Fig. 6 3D reconstruction result of the indoor environment 5.2.4 机器人运动轨迹跟踪 为了获得机器人位姿的变化，本文以 3D-2D 的 特征点约束，采用 PNP 求解的方法对机器人相邻位 姿进行估计，并借助图优化方法对估计位姿进行优 化。为了使运动轨迹更为直观，实验给出了本文算 法在不同数据集下，机器人运动轨迹在 X/Y 平面上 的投影，如图 7 所示。表 1 则记录了不同算法在不 同数据集下，由计算所得的机器人在三维空间中的 运动轨迹的真实位姿之间的均方根误差 (RMSE)。 通过均方根误差的对比，可有效地评估算法对机器 人位姿跟踪的误差大小。可以看出本文方法能较好 地实现机器人的运动轨迹跟踪。 1.0 0.5 0.0 −0.5 −1.0 −1.5 y/m x/m −1.5 −1.0 −0.5 0.0 0.5 1.0 1.5 真实位姿 两种位姿的差异 估计位姿 (a) FR1/room 1.0 0.5 0.0 −0.5 −1.0 −1.5 y/m x/m −1.0 −0.5 −0.0 0.5 1.0 1.5 2.0 (b) FR1/desk 0.2 0.0 −0.2 −0.4 −0.6 −0.8 −1.0 −1.2 −1.4 y/m x/m −1.0 −0.5 0.0 0.5 1.0 1.5 2.0 (a) FR1/floor 1.0 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 y/m x/m 1.0 1.1 1.2 1.3 1.4 1.5 (b) FR1/xyz 真实位姿 估计位姿 两种位姿的差异 真实位姿 估计位姿 两种位姿的差异 真实位姿 估计位姿 两种位姿的差异 图 7 真实位姿与估计位姿对比图 Fig. 7 Comparison between ground truth and estimated trajectories ·294· 智 能 系 统 学 报 第 13 卷

第2期 张毅，等：基于图优化的移动机器人视觉SLAM ·295· 表1不同算法位姿估计的均方根误差对比 tion for large-scale multi-session graph-based slam[C]//Pro- Table 1 Pose estimation comparison of different algorithms ceedings of 2014 IEEE/RSJ International Conference on In- about RMSE /m telligent Robots and Systems.Chicago,IL,USA:IEEE, 数据来源 ORB-SLAM RGBD-SLAM 本文方法 2014:2661-2666. FR1/room 0.056 0.087 0.051 [7]RUBLEE E,RABAUD V,KONOLIGE K,et al.ORB:an FR1/desk 0.014 0.026 0.016 efficient alternative to SIFT or SURF[C]//Proceedings of 2011 International conference on computer vision.Bar- FR1/floor 0.013 0.023 0.015 celona,Spain:IEEE,2011:2564-2571. FR1/xyz 0.004 0.018 0.009 8]梁明杰，闵华清，罗荣华.基于图优化的同时定位与地图 创建综述U.机器人，2013,35(4)：500-512 6结束语 LIANG Mingjie,MIN Huaqing,LUO Ronghua.Graph- based SLAM:a survey[J].Robot,2013,35(4):500-512. 在本文基于图优化的视觉SLAM算法前端框 [9]FADDEEV L D,RESHETIKHIN N Y,TAKHTAJAN L A. 架中，采用的ORB特征检测、RANSAC算法等方 Quantization of lie groups and lie algebras[Ml//KASHI- 法，较好地解决了帧间配准过程中匹配误差较大、 WARA M.KAWAI T.Algebraic analysis:papers dedic- 效率低下的问题。在三维地图生成部分，以图优化 ated to professor mikio Sato on the occasion of his sixtieth 后的位姿为基础，进行点云构建，并引人闭环检测 birthday.Boston,MA:Academic Press,2014:129-139 的机制，实现了对位姿与稠密化点云的双重优化。 [10]SHAWASH J,SELVIAH D R.Real-time nonlinear para- 实验验证了本文所提方法能够准确、迅速地重构出 meter estimation using the Levenberg-Marquardt al- 稠密化的三维环境模型，并能精确估计出移动机器 gorithm on field programmable gate arrays[J.IEEE trans- actions on industrial electronics,2013,60(1):170-176. 人的运动轨迹。下一步计划对系统进行两方面的改 [11]KUMMERLE R,GRISETTI G,STRASDAT H,et al.G2o: 进：)改进前端的帧间配准方法，考虑采用直接法 a general framework for graph optimization[C]//Proceed- 进行位姿跟踪，完成位姿估计；2)研究环境三维地 ings of 2011 IEEE International Conference on Robotics 图与语义信息的结合，实现更好的人机交互与导航。 and Automation.Shanghai,China:IEEE,2011:3607- 参考文献： 3613. [12]STURM J,MAGNENAT S,ENGELHARD N,et al.To- [1]ZHANG Zhengyou.Microsoft kinect sensor and its wards a benchmark for RGB-D SLAM evaluation[CV/RGB-D effect[J].IEEE multimedia,2012,19(2):4-10. Workshop on Advanced Reasoning with Depth Cameras at [2]ENDRES F,HESS J,STURM J,et al.3-D mapping with an Robotics:Science and Systems Conference.Los Angeles, RGB-D camera[J].IEEE transactions on robotics,2014, United States:RSS,2011:1-2 30(1):177-187. 作者简介： [3]MUR-ARTAL R,MONTIEL J MM,TARDOS J D.ORB- 张毅，男，1966年生，教授，博士 SLAM:a versatile and accurate monocular slam system[J]. 生导师，中国人工智能学会理事，国家 IEEE transactions on robotics,2015,31(5):1147-1163. 信息无障碍研发中心主任，主要研究 [4]RUSU R B,COUSINS S.3D is here:point cloud library 方向为智能系统与移动机器人、机器 (PCL)[C]//Proceedings of 2011 IEEE International Confer- 视觉与模式识别、多传感器信息融 合。主持完成国家级和省部基金项目 ence on Robotics and Automation.Shanghai,China:IEEE. 10余项，发表学术论文100余篇，被 2011:1-4. SCI、EI和ISTP收录30余篇次，出版著作5部，获国家发明 [S]HARTLEY R,ZISSERMAN A.计算机视觉中的多视图几 专利30余项。 何M).韦穗，杨尚骏，章权兵，等，译.合肥：安徽大学出版 社，2002：158-256 沙建松，男，1991年生，硕士研究 HARTLEY R,ZISSERMAN A.Multiple view geometry in 生，主要研究方向为机器人同时定位 与地图创建(SLAM)、基于语义信息的 computer vision[M].WEI Sui,YANG Shangjun,ZHANG 机器人三维视觉导航。 Quanbing,et al,Trans.Hefei:Anhui University Press,2002: 158-256. 6]LABBE M.MICHAUD F.Online global loop closure detec-

表 1 不同算法位姿估计的均方根误差对比 Table 1 Pose estimation comparison of different algorithms about RMSE /m 数据来源 ORB-SLAM RGBD-SLAM 本文方法 FR1/room 0.056 0.087 0.051 FR1/desk 0.014 0.026 0.016 FR1/floor 0.013 0.023 0.015 FR1/xyz 0.004 0.018 0.009 6 结束语 在本文基于图优化的视觉 SLAM 算法前端框 架中，采用的 ORB 特征检测、RANSAC 算法等方 法，较好地解决了帧间配准过程中匹配误差较大、 效率低下的问题。在三维地图生成部分，以图优化 后的位姿为基础，进行点云构建，并引入闭环检测 的机制，实现了对位姿与稠密化点云的双重优化。 实验验证了本文所提方法能够准确、迅速地重构出 稠密化的三维环境模型，并能精确估计出移动机器 人的运动轨迹。下一步计划对系统进行两方面的改 进: 1) 改进前端的帧间配准方法，考虑采用直接法 进行位姿跟踪，完成位姿估计；2) 研究环境三维地 图与语义信息的结合，实现更好的人机交互与导航。 参考文献： ZHANG Zhengyou. Microsoft kinect sensor and its effect[J]. IEEE multimedia, 2012, 19(2): 4–10. [1] ENDRES F, HESS J, STURM J, et al. 3-D mapping with an RGB-D camera[J]. IEEE transactions on robotics, 2014, 30(1): 177–187. [2] MUR-ARTAL R, MONTIEL J M M, TARDÓS J D. ORB￾SLAM: a versatile and accurate monocular slam system[J]. IEEE transactions on robotics, 2015, 31(5): 1147–1163. [3] RUSU R B, COUSINS S. 3D is here: point cloud library (PCL)[C]//Proceedings of 2011 IEEE International Confer￾ence on Robotics and Automation. Shanghai, China: IEEE, 2011: 1–4. [4] HARTLEY R, ZISSERMAN A. 计算机视觉中的多视图几 何[M]. 韦穗, 杨尚骏, 章权兵, 等, 译. 合肥: 安徽大学出版 社, 2002: 158–256. HARTLEY R, ZISSERMAN A. Multiple view geometry in computer vision[M]. WEI Sui, YANG Shangjun, ZHANG Quanbing, et al, Trans. Hefei: Anhui University Press, 2002: 158–256. [5] [6] LABBÉ M, MICHAUD F. Online global loop closure detec￾tion for large-scale multi-session graph-based slam[C]//Pro￾ceedings of 2014 IEEE/RSJ International Conference on In￾telligent Robots and Systems. Chicago, IL, USA: IEEE, 2014: 2661–2666. RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB: an efficient alternative to SIFT or SURF[C]//Proceedings of 2011 International conference on computer vision. Bar￾celona, Spain: IEEE, 2011: 2564–2571. [7] 梁明杰, 闵华清, 罗荣华. 基于图优化的同时定位与地图 创建综述[J]. 机器人, 2013, 35(4): 500–512. LIANG Mingjie, MIN Huaqing, LUO Ronghua. Graph￾based SLAM: a survey[J]. Robot, 2013, 35(4): 500–512. [8] FADDEEV L D, RESHETIKHIN N Y, TAKHTAJAN L A. Quantization of lie groups and lie algebras[M]//KASHI￾WARA M, KAWAI T. Algebraic analysis: papers dedic￾ated to professor mikio Sato on the occasion of his sixtieth birthday. Boston, MA: Academic Press, 2014: 129–139. [9] SHAWASH J, SELVIAH D R. Real-time nonlinear para￾meter estimation using the Levenberg–Marquardt al￾gorithm on field programmable gate arrays[J]. IEEE trans￾actions on industrial electronics, 2013, 60(1): 170–176. [10] KÜMMERLE R, GRISETTI G, STRASDAT H, et al. G2o: a general framework for graph optimization[C]//Proceed￾ings of 2011 IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE, 2011: 3607– 3613. [11] STURM J, MAGNENAT S, ENGELHARD N, et al. To￾wards a benchmark for RGB-D SLAM evaluation[C]//RGB-D Workshop on Advanced Reasoning with Depth Cameras at Robotics: Science and Systems Conference. Los Angeles, United States: RSS, 2011: 1–2. [12] 作者简介： 张毅，男，1966 年生，教授，博士 生导师，中国人工智能学会理事，国家 信息无障碍研发中心主任，主要研究 方向为智能系统与移动机器人、机器 视觉与模式识别、多传感器信息融 合。主持完成国家级和省部基金项目 10 余项，发表学术论文 100 余篇，被 SCI、EI 和 ISTP 收录 30 余篇次，出版著作 5 部，获国家发明 专利 30 余项。 沙建松，男，1991 年生，硕士研究 生，主要研究方向为机器人同时定位 与地图创建 (SLAM)、基于语义信息的 机器人三维视觉导航。 第 2 期 张毅，等：基于图优化的移动机器人视觉 SLAM ·295·