第10卷第4期 智能系统学报 Vol.10 No.4 2015年8月 CAAI Transactions on Intelligent Systems Aug.2015 D0:10.3969/j.issn.1673-4785.201504003 网s络出版地址：http:/www.cnki.net/kcms/detail/23.1538.TP.20150630.1541.001.html 室内环境下移动机器人三维视觉SLAM 张毅，陈起，罗元2 (1.重庆邮电大学自动化学院，重庆400065：2.重庆邮电大学光电工程学院，重庆400065) 摘要：在移动机器人三维视觉SLAM中，针对点云配准过程存在的匹配误差较大、效率低下的问题，提出一种分层 式点云配准策略。利用RANSAC算法对可能存在的误匹配点进行剔除完成初始配准，对所得内点进行最小二乘算 法下机器人相邻位姿的估计，并采用改进的CP算法完成精确配准，最后采用g0优化方法对移动机器人位姿进行 优化。针对点云拼接结果中存在重复点过多以及空洞的问题，引入基于体积空间融合的匹配算法对空洞进行填补。 实验验证了本文所提方法能够准确重构出环境三维模型和精确估计出移动机器人的运动轨迹，并有效去除拼接结 果中的重复点，填补了空洞。 关键词：移动机器人；Kinect相机：同时定位与地图构建；点云配准：轨迹跟踪：机器人导航；鲁棒性 中图分类号：TP24文献标志码：A文章编号：1673-4785(2015)04-0615-05 中文引用格式：张毅，陈起，罗元.室内环境下移动机器人三维视觉SLAM[J].智能系统学报，2015,10(4)：615-619. 英文引用格式：ZHANG Yi,CHEN Qi,,LUO Yuan.Three dimensional visual SLAM for mobile robot in indoor environment[J]. CAAI Transactions on Intelligent Systems,2015,10(4):615-619. Three dimensional visual SLAM for mobile robots in indoor environments ZHANG Yi',CHEN Qi',LUO Yuan? (1.Automation College,Chongqing University of Posts and Telecommunications,Chongqing 400065,China;2.College of Electroning Engineering College,Chongqing University of Posts and Telecommunications,Chongqing 400065,China) Abstract:In this paper,it aiming at the problems of large matching error and inefficiency in the process of point cloud registration in mobile robots 3D visual SLAM,a hierarchical point cloud registration strategy is proposed.It completes the initial registration by eliminating possible false matching points by RANSAC algorithm and then esti- mates the robot adjacent pose using the least square algorithm of interior points.Meanwhile,accurate registration is completed by the improved ICP algorithm and finally the mobile robot pose is optimized by the g0 optimization method.Addressing the problem of too much repeated points and hole existing in point cloud splicing results,a matching algorithm based on volume space fusion is introduced to fill the hole.Experimental results obtained dem- onstrated that the proposed method can accurately reconstruct the 3D model and estimate the trajectory of the mobile robot,and can effectively remove the repeated points and fill the hole. Keywords:mobile robot;Kinect camera;SLAM;point cloud registration;trajectory tracking;robot navigation;robustness 近年来，移动机器人同时定位与地图构建已经 导航具有重要意义。在三维环境建模中最常用的方 成为机器人领域研究的热点。过去，国内外学者 式是利用范围传感器获得的深度点云来表示环境模 对移动机器人二维SLAM进行了大量研究，然而在 型。然而，这种方法大多利用精度高、价格昂贵 二维SLAM中丢失了很多有价值的形状和几何信 的三维激光扫描器。Kinect具有廉价、方便的特 息。三维SLAM能够充分利用环境三维属性，精确 点[]。近年来，将Kinect应用于移动机器人三维 描述出局部环境形状以及物体的几何外形，因而移 SLAM已成为机器人领域研究的新热点，并取得了 动机器人三维SLAM对于全面实现移动机器人精确 大量研究成果。文献[4]提出一种室内环境下的三 维环境建模方法，基于GPU编程的SFT算法提高 收稿日期：2015-04-07.网络出版日期：2015-06-30. 了系统的运算效率，且能实现三维环境的建立，但效 基金项目：国家自然科学基金资助项目(51075420) 通信作者：陈起.E-mail:chengisansan(@163.com. 果欠佳且无法进行位姿估计。Henry)等人提出了第 １０ 卷第 ４ 期 智 能 系 统 学 报 Ｖｏｌ．１０ №．４ ２０１５ 年 ８ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ａｕｇ． ２０１５ ＤＯＩ：１０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３⁃４７８５．２０１５０４００３ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ＴＰ．２０１５０６３０．１５４１．００１．ｈｔｍｌ 室内环境下移动机器人三维视觉 ＳＬＡＭ 张毅１ ，陈起１ ，罗元２ （１．重庆邮电大学 自动化学院，重庆 ４０００６５；２．重庆邮电大学 光电工程学院，重庆 ４０００６５） 摘 要：在移动机器人三维视觉 ＳＬＡＭ 中，针对点云配准过程存在的匹配误差较大、效率低下的问题，提出一种分层 式点云配准策略。 利用 ＲＡＮＳＡＣ 算法对可能存在的误匹配点进行剔除完成初始配准，对所得内点进行最小二乘算 法下机器人相邻位姿的估计，并采用改进的 ＩＣＰ 算法完成精确配准，最后采用 ｇ ２ ０ 优化方法对移动机器人位姿进行 优化。 针对点云拼接结果中存在重复点过多以及空洞的问题，引入基于体积空间融合的匹配算法对空洞进行填补。 实验验证了本文所提方法能够准确重构出环境三维模型和精确估计出移动机器人的运动轨迹，并有效去除拼接结 果中的重复点，填补了空洞。 关键词：移动机器人；Ｋｉｎｅｃｔ 相机；同时定位与地图构建；点云配准；轨迹跟踪；机器人导航；鲁棒性 中图分类号： ＴＰ２４ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１５）０４⁃０６１５⁃０５ 中文引用格式：张毅，陈起，罗元． 室内环境下移动机器人三维视觉 ＳＬＡＭ［Ｊ］． 智能系统学报， ２０１５， １０（４）： ６１５⁃６１９． 英文引用格式：ＺＨＡＮＧ Ｙｉ， ＣＨＥＮ Ｑｉ， ＬＵＯ Ｙｕａｎ． Ｔｈｒｅｅ ｄｉｍｅｎｓｉｏｎａｌ ｖｉｓｕａｌ ＳＬＡＭ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔ ｉｎ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔ［ Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１５， １０（４）： ６１５⁃６１９． Ｔｈｒｅｅ ｄｉｍｅｎｓｉｏｎａｌ ｖｉｓｕａｌ ＳＬＡＭ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ｉｎ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔｓ ＺＨＡＮＧ Ｙｉ １ ，ＣＨＥＮ Ｑｉ １ ，ＬＵＯ Ｙｕａｎ ２ （１．Ａｕｔｏｍａｔｉｏｎ Ｃｏｌｌｅｇｅ， Ｃｈｏｎｇｑｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｐｏｓｔｓ ａｎｄ Ｔｅｌｅｃｏｍｍｕｎｉｃａｔｉｏｎｓ， Ｃｈｏｎｇｑｉｎｇ ４０００６５， Ｃｈｉｎａ； ２．Ｃｏｌｌｅｇｅ ｏｆ Ｅｌｅｃｔｒｏｎｉｎｇ Ｅｎｇｉｎｅｅｒｉｎｇ Ｃｏｌｌｅｇｅ， Ｃｈｏｎｇｑｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｐｏｓｔｓ ａｎｄ Ｔｅｌｅｃｏｍｍｕｎｉｃａｔｉｏｎｓ， Ｃｈｏｎｇｑｉｎｇ ４０００６５， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｉｎ ｔｈｉｓ ｐａｐｅｒ， ｉｔ ａｉｍｉｎｇ ａｔ ｔｈｅ ｐｒｏｂｌｅｍｓ ｏｆ ｌａｒｇｅ ｍａｔｃｈｉｎｇ ｅｒｒｏｒ ａｎｄ ｉｎｅｆｆｉｃｉｅｎｃｙ ｉｎ ｔｈｅ ｐｒｏｃｅｓｓ ｏｆ ｐｏｉｎｔ ｃｌｏｕｄ ｒｅｇｉｓｔｒａｔｉｏｎ ｉｎ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ３Ｄ ｖｉｓｕａｌ ＳＬＡＭ， ａ ｈｉｅｒａｒｃｈｉｃａｌ ｐｏｉｎｔ ｃｌｏｕｄ ｒｅｇｉｓｔｒａｔｉｏｎ ｓｔｒａｔｅｇｙ ｉｓ ｐｒｏｐｏｓｅｄ． Ｉｔ ｃｏｍｐｌｅｔｅｓ ｔｈｅ ｉｎｉｔｉａｌ ｒｅｇｉｓｔｒａｔｉｏｎ ｂｙ ｅｌｉｍｉｎａｔｉｎｇ ｐｏｓｓｉｂｌｅ ｆａｌｓｅ ｍａｔｃｈｉｎｇ ｐｏｉｎｔｓ ｂｙ ＲＡＮＳＡＣ ａｌｇｏｒｉｔｈｍ ａｎｄ ｔｈｅｎ ｅｓｔｉ⁃ ｍａｔｅｓ ｔｈｅ ｒｏｂｏｔ ａｄｊａｃｅｎｔ ｐｏｓｅ ｕｓｉｎｇ ｔｈｅ ｌｅａｓｔ ｓｑｕａｒｅ ａｌｇｏｒｉｔｈｍ ｏｆ ｉｎｔｅｒｉｏｒ ｐｏｉｎｔｓ． Ｍｅａｎｗｈｉｌｅ， ａｃｃｕｒａｔｅ ｒｅｇｉｓｔｒａｔｉｏｎ ｉｓ ｃｏｍｐｌｅｔｅｄ ｂｙ ｔｈｅ ｉｍｐｒｏｖｅｄ ＩＣＰ ａｌｇｏｒｉｔｈｍ ａｎｄ ｆｉｎａｌｌｙ ｔｈｅ ｍｏｂｉｌｅ ｒｏｂｏｔ ｐｏｓｅ ｉｓ ｏｐｔｉｍｉｚｅｄ ｂｙ ｔｈｅ ｇ ２ ０ ｏｐｔｉｍｉｚａｔｉｏｎ ｍｅｔｈｏｄ． Ａｄｄｒｅｓｓｉｎｇ ｔｈｅ ｐｒｏｂｌｅｍ ｏｆ ｔｏｏ ｍｕｃｈ ｒｅｐｅａｔｅｄ ｐｏｉｎｔｓ ａｎｄ ｈｏｌｅ ｅｘｉｓｔｉｎｇ ｉｎ ｐｏｉｎｔ ｃｌｏｕｄ ｓｐｌｉｃｉｎｇ ｒｅｓｕｌｔｓ， ａ ｍａｔｃｈｉｎｇ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｖｏｌｕｍｅ ｓｐａｃｅ ｆｕｓｉｏｎ ｉｓ ｉｎｔｒｏｄｕｃｅｄ ｔｏ ｆｉｌｌ ｔｈｅ ｈｏｌｅ． Ｅｘｐｅｒｉｍｅｎｔａｌ ｒｅｓｕｌｔｓ ｏｂｔａｉｎｅｄ ｄｅｍ⁃ ｏｎｓｔｒａｔｅｄ ｔｈａｔ ｔｈｅ ｐｒｏｐｏｓｅｄ ｍｅｔｈｏｄ ｃａｎ ａｃｃｕｒａｔｅｌｙ ｒｅｃｏｎｓｔｒｕｃｔ ｔｈｅ ３Ｄ ｍｏｄｅｌ ａｎｄ ｅｓｔｉｍａｔｅ ｔｈｅ ｔｒａｊｅｃｔｏｒｙ ｏｆ ｔｈｅ ｍｏｂｉｌｅ ｒｏｂｏｔ， ａｎｄ ｃａｎ ｅｆｆｅｃｔｉｖｅｌｙ ｒｅｍｏｖｅ ｔｈｅ ｒｅｐｅａｔｅｄ ｐｏｉｎｔｓ ａｎｄ ｆｉｌｌ ｔｈｅ ｈｏｌｅ． Ｋｅｙｗｏｒｄｓ：ｍｏｂｉｌｅ ｒｏｂｏｔ； Ｋｉｎｅｃｔ ｃａｍｅｒａ； ＳＬＡＭ； ｐｏｉｎｔ ｃｌｏｕｄ ｒｅｇｉｓｔｒａｔｉｏｎ； ｔｒａｊｅｃｔｏｒｙ ｔｒａｃｋｉｎｇ； ｒｏｂｏｔ ｎａｖｉｇａｔｉｏｎ； ｒｏｂｕｓｔｎｅｓｓ 收稿日期：２０１５⁃０４⁃０７． 网络出版日期：２０１５⁃０６⁃３０． 基金项目：国家自然科学基金资助项目（５１０７５４２０）． 通信作者：陈起． Ｅ⁃ｍａｉｌ：ｃｈｅｎｑｉｓａｎｓａｎ＠ １６３．ｃｏｍ． 近年来，移动机器人同时定位与地图构建已经 成为机器人领域研究的热点［１］ 。 过去，国内外学者 对移动机器人二维 ＳＬＡＭ 进行了大量研究，然而在 二维 ＳＬＡＭ 中丢失了很多有价值的形状和几何信 息。 三维 ＳＬＡＭ 能够充分利用环境三维属性，精确 描述出局部环境形状以及物体的几何外形，因而移 动机器人三维 ＳＬＡＭ 对于全面实现移动机器人精确 导航具有重要意义。 在三维环境建模中最常用的方 式是利用范围传感器获得的深度点云来表示环境模 型［２］ 。 然而，这种方法大多利用精度高、价格昂贵 的三维激光扫描器。 Ｋｉｎｅｃｔ 具有廉价、方便的特 点［３］ 。 近年来，将 Ｋｉｎｅｃｔ 应用于移动机器人三维 ＳＬＡＭ 已成为机器人领域研究的新热点，并取得了 大量研究成果。 文献［４］提出一种室内环境下的三 维环境建模方法，基于 ＧＰＵ 编程的 ＳＩＦＴ 算法提高 了系统的运算效率，且能实现三维环境的建立，但效 果欠佳且无法进行位姿估计。 Ｈｅｎｒｙ ［５］ 等人提出了