第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)等人提出了
第 １０ 卷第 ４ 期 智 能 系 统 学 报 Ｖｏｌ．１０ №．４ ２０１５ 年 ８ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ａｕｇ． ２０１５ ＤＯＩ：１０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３⁃４７８５．２０１５０４００３ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ＴＰ．２０１５０６３０．１５４１．００１．ｈｔｍｌ 室内环境下移动机器人三维视觉 ＳＬＡＭ 张毅１ ，陈起１ ，罗元２ （１．重庆邮电大学 自动化学院，重庆 ４０００６５；２．重庆邮电大学 光电工程学院，重庆 ４０００６５） 摘 要：在移动机器人三维视觉 ＳＬＡＭ 中，针对点云配准过程存在的匹配误差较大、效率低下的问题，提出一种分层 式点云配准策略。 利用 ＲＡＮＳＡＣ 算法对可能存在的误匹配点进行剔除完成初始配准，对所得内点进行最小二乘算 法下机器人相邻位姿的估计，并采用改进的 ＩＣＰ 算法完成精确配准，最后采用 ｇ ２ ０ 优化方法对移动机器人位姿进行 优化。 针对点云拼接结果中存在重复点过多以及空洞的问题，引入基于体积空间融合的匹配算法对空洞进行填补。 实验验证了本文所提方法能够准确重构出环境三维模型和精确估计出移动机器人的运动轨迹，并有效去除拼接结 果中的重复点，填补了空洞。 关键词：移动机器人；Ｋｉｎｅｃｔ 相机；同时定位与地图构建；点云配准；轨迹跟踪；机器人导航；鲁棒性 中图分类号： ＴＰ２４ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１５）０４⁃０６１５⁃０５ 中文引用格式：张毅，陈起，罗元． 室内环境下移动机器人三维视觉 ＳＬＡＭ［Ｊ］． 智能系统学报， ２０１５， １０（４）： ６１５⁃６１９． 英文引用格式：ＺＨＡＮＧ Ｙｉ， ＣＨＥＮ Ｑｉ， ＬＵＯ Ｙｕａｎ． Ｔｈｒｅｅ ｄｉｍｅｎｓｉｏｎａｌ ｖｉｓｕａｌ ＳＬＡＭ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔ ｉｎ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔ［ Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１５， １０（４）： ６１５⁃６１９． Ｔｈｒｅｅ ｄｉｍｅｎｓｉｏｎａｌ ｖｉｓｕａｌ ＳＬＡＭ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ｉｎ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔｓ ＺＨＡＮＧ Ｙｉ １ ，ＣＨＥＮ Ｑｉ １ ，ＬＵＯ Ｙｕａｎ ２ （１．Ａｕｔｏｍａｔｉｏｎ Ｃｏｌｌｅｇｅ， Ｃｈｏｎｇｑｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｐｏｓｔｓ ａｎｄ Ｔｅｌｅｃｏｍｍｕｎｉｃａｔｉｏｎｓ， Ｃｈｏｎｇｑｉｎｇ ４０００６５， Ｃｈｉｎａ； ２．Ｃｏｌｌｅｇｅ ｏｆ Ｅｌｅｃｔｒｏｎｉｎｇ Ｅｎｇｉｎｅｅｒｉｎｇ Ｃｏｌｌｅｇｅ， Ｃｈｏｎｇｑｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｐｏｓｔｓ ａｎｄ Ｔｅｌｅｃｏｍｍｕｎｉｃａｔｉｏｎｓ， Ｃｈｏｎｇｑｉｎｇ ４０００６５， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｉｎ ｔｈｉｓ ｐａｐｅｒ， ｉｔ ａｉｍｉｎｇ ａｔ ｔｈｅ ｐｒｏｂｌｅｍｓ ｏｆ ｌａｒｇｅ ｍａｔｃｈｉｎｇ ｅｒｒｏｒ ａｎｄ ｉｎｅｆｆｉｃｉｅｎｃｙ ｉｎ ｔｈｅ ｐｒｏｃｅｓｓ ｏｆ ｐｏｉｎｔ ｃｌｏｕｄ ｒｅｇｉｓｔｒａｔｉｏｎ ｉｎ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ３Ｄ ｖｉｓｕａｌ ＳＬＡＭ， ａ ｈｉｅｒａｒｃｈｉｃａｌ ｐｏｉｎｔ ｃｌｏｕｄ ｒｅｇｉｓｔｒａｔｉｏｎ ｓｔｒａｔｅｇｙ ｉｓ ｐｒｏｐｏｓｅｄ． Ｉｔ ｃｏｍｐｌｅｔｅｓ ｔｈｅ ｉｎｉｔｉａｌ ｒｅｇｉｓｔｒａｔｉｏｎ ｂｙ ｅｌｉｍｉｎａｔｉｎｇ ｐｏｓｓｉｂｌｅ ｆａｌｓｅ ｍａｔｃｈｉｎｇ ｐｏｉｎｔｓ ｂｙ ＲＡＮＳＡＣ ａｌｇｏｒｉｔｈｍ ａｎｄ ｔｈｅｎ ｅｓｔｉ⁃ ｍａｔｅｓ ｔｈｅ ｒｏｂｏｔ ａｄｊａｃｅｎｔ ｐｏｓｅ ｕｓｉｎｇ ｔｈｅ ｌｅａｓｔ ｓｑｕａｒｅ ａｌｇｏｒｉｔｈｍ ｏｆ ｉｎｔｅｒｉｏｒ ｐｏｉｎｔｓ． Ｍｅａｎｗｈｉｌｅ， ａｃｃｕｒａｔｅ ｒｅｇｉｓｔｒａｔｉｏｎ ｉｓ ｃｏｍｐｌｅｔｅｄ ｂｙ ｔｈｅ ｉｍｐｒｏｖｅｄ ＩＣＰ ａｌｇｏｒｉｔｈｍ ａｎｄ ｆｉｎａｌｌｙ ｔｈｅ ｍｏｂｉｌｅ ｒｏｂｏｔ ｐｏｓｅ ｉｓ ｏｐｔｉｍｉｚｅｄ ｂｙ ｔｈｅ ｇ ２ ０ ｏｐｔｉｍｉｚａｔｉｏｎ ｍｅｔｈｏｄ． Ａｄｄｒｅｓｓｉｎｇ ｔｈｅ ｐｒｏｂｌｅｍ ｏｆ ｔｏｏ ｍｕｃｈ ｒｅｐｅａｔｅｄ ｐｏｉｎｔｓ ａｎｄ ｈｏｌｅ ｅｘｉｓｔｉｎｇ ｉｎ ｐｏｉｎｔ ｃｌｏｕｄ ｓｐｌｉｃｉｎｇ ｒｅｓｕｌｔｓ， ａ ｍａｔｃｈｉｎｇ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｖｏｌｕｍｅ ｓｐａｃｅ ｆｕｓｉｏｎ ｉｓ ｉｎｔｒｏｄｕｃｅｄ ｔｏ ｆｉｌｌ ｔｈｅ ｈｏｌｅ． Ｅｘｐｅｒｉｍｅｎｔａｌ ｒｅｓｕｌｔｓ ｏｂｔａｉｎｅｄ ｄｅｍ⁃ ｏｎｓｔｒａｔｅｄ ｔｈａｔ ｔｈｅ ｐｒｏｐｏｓｅｄ ｍｅｔｈｏｄ ｃａｎ ａｃｃｕｒａｔｅｌｙ ｒｅｃｏｎｓｔｒｕｃｔ ｔｈｅ ３Ｄ ｍｏｄｅｌ ａｎｄ ｅｓｔｉｍａｔｅ ｔｈｅ ｔｒａｊｅｃｔｏｒｙ ｏｆ ｔｈｅ ｍｏｂｉｌｅ ｒｏｂｏｔ， ａｎｄ ｃａｎ ｅｆｆｅｃｔｉｖｅｌｙ ｒｅｍｏｖｅ ｔｈｅ ｒｅｐｅａｔｅｄ ｐｏｉｎｔｓ ａｎｄ ｆｉｌｌ ｔｈｅ ｈｏｌｅ． Ｋｅｙｗｏｒｄｓ：ｍｏｂｉｌｅ ｒｏｂｏｔ； Ｋｉｎｅｃｔ ｃａｍｅｒａ； ＳＬＡＭ； ｐｏｉｎｔ ｃｌｏｕｄ ｒｅｇｉｓｔｒａｔｉｏｎ； ｔｒａｊｅｃｔｏｒｙ ｔｒａｃｋｉｎｇ； ｒｏｂｏｔ ｎａｖｉｇａｔｉｏｎ； ｒｏｂｕｓｔｎｅｓｓ 收稿日期：２０１５⁃０４⁃０７． 网络出版日期：２０１５⁃０６⁃３０． 基金项目：国家自然科学基金资助项目（５１０７５４２０）． 通信作者：陈起． Ｅ⁃ｍａｉｌ：ｃｈｅｎｑｉｓａｎｓａｎ＠ １６３．ｃｏｍ． 近年来，移动机器人同时定位与地图构建已经 成为机器人领域研究的热点［１］ 。 过去，国内外学者 对移动机器人二维 ＳＬＡＭ 进行了大量研究，然而在 二维 ＳＬＡＭ 中丢失了很多有价值的形状和几何信 息。 三维 ＳＬＡＭ 能够充分利用环境三维属性，精确 描述出局部环境形状以及物体的几何外形，因而移 动机器人三维 ＳＬＡＭ 对于全面实现移动机器人精确 导航具有重要意义。 在三维环境建模中最常用的方 式是利用范围传感器获得的深度点云来表示环境模 型［２］ 。 然而，这种方法大多利用精度高、价格昂贵 的三维激光扫描器。 Ｋｉｎｅｃｔ 具有廉价、方便的特 点［３］ 。 近年来，将 Ｋｉｎｅｃｔ 应用于移动机器人三维 ＳＬＡＭ 已成为机器人领域研究的新热点，并取得了 大量研究成果。 文献［４］提出一种室内环境下的三 维环境建模方法，基于 ＧＰＵ 编程的 ＳＩＦＴ 算法提高 了系统的运算效率，且能实现三维环境的建立，但效 果欠佳且无法进行位姿估计。 Ｈｅｎｒｙ ［５］ 等人提出了
·616 智能系统学报 第10卷 一个基于Kinect的交互式的三维模型重建系统，但 了g0优化方法对机器人位姿进行优化，实验验证 是该系统依靠关键帧的选取，计算量大、时间复杂度 了本文方法的可行性和有效性。 高，难以用于移动机器人三维地图在线创建。针对 上述问题，本文提出了一种室内环境下移动机器人 1 系统结构 三维视觉SLAM方法。针对点云配准过程存在的匹 本文提出的室内环境下移动机器人三维视觉 配误差较大、效率低下的问题，提出一种分层式点云 SLAM系统的总体框架如图1所示，主要分为特征 配准策略。同时引入基于体积融合的匹配算法完成 点检测与匹配、Kinect标定与点云生成、分层式点云 精确配准，有效的去除了拼接结果中的重复点，采用 配准3部分。 彩色图像 SURT 特征点检 测与匹配 剔除 RANS 无匹 ICP- 位姿 AC算 配点 TSDP 优化 位姿轨迹 深度图像 法完成 及环境三 完成精 生成 3D点 点云初 维地图 确配准 Kinect标定 点云 坐标 始配准 与数据对准 RGB 数据 图1室内环境下移动机器人三维视觉SLAM总体框架图 Fig.1 Total frame of three dimensional visual SLAM for mobile robot in indoor environment 2特征点检测与匹配 (x4,y）,投射到三维空间点p(x,y,z),其坐标计算 如下： 经典的特征点检测算法包括Harrist6]、SFT】 (xu-cd）x depth(xa,ya） (2) SURF)算法等。经综合考虑为获得鲁棒性、稳定性 fa 更好的特征点检测结果，本文采用具有旋转和尺度 不变性且速度较快的SURF算法。SURF特征点提 y=（-Sa)×depth(x,a） (3) fyd 取过程主要包括特征点检测、特征点主方向选取、生 z=depth(xa,ya) (4) 成特征描述子。判断基于描述的特征点是否匹配， 式中：P,=PT为当前像素点深度值，c和c为图 可通过计算欧式距离来实现，设匹配图像的特征点 像光心坐标，f和f为深度相机焦距，其中ff均 集合分别为X和Y。欧式距离可以描述为 可由标定得到。 D(X,Y)=IX-Y‖= (X:-Y)"(1) 4分层式点云配准 Ai=1 欧式距离越小表示特征点相似程度越高，本文 4.1 RANSAC算法粗匹配 在匹配过程中采用了最大紧邻向量匹配方法，即通 本文采用RANSAC算法进行特征点的粗匹配， 过比较特征点集合中的每一个特征点与特征点集合 以使2组点集的公共区域能够大致重合，为下一步 中各特征点的欧式距离，D,和D,分别代表最近和 精确匹配做准备。算法设定一个容许误差将所有的 次近的欧式距离，若D,≤aD,(α为设定的最近次近 匹配点对分为内点和外点。本文实验中设定的阈值 距离比值，本文设定=0.60)，则认为两个特征点 d=3.00舍去误差大于d的匹配点对（外点）。为了 匹配，反之丢弃该点。 获得摄像机位姿的变化，本文采用了最小二乘逼近 的方法[山。利用算法所得内点进行最小二乘算法 3 标定与点云生成 下机器人相邻位姿的估计。摄像机第i时刻的位姿 为获取Kinect的彩色与深度相机的内参数矩 P,相对于第j时刻的摄像机位姿P,的转换关系为 阵以及彩色与深度相机之间的刚体变换，减小深度 P,=PT其中T为刚性变换矩阵： 图像与红外图像之间存在的漂移)，需要对Kinect T= 「R3x33x1 (5) 进行标定，本文采用文献[l0]标定方法对Kinect进 01 行标定。在完成Kinect对准校正后，可根据图像中 式中：R3x3为旋转矩阵；3x1为平移矩阵。对于移动 任意一点对应的深度值得到该点三维坐标，进而生 机器人同步定位与地图构建，可通过下式获得各时 成三维彩色点云数据。给定任意深度图像像素点 刻数据相对于初始时刻的变换矩阵：
一个基于 Ｋｉｎｅｃｔ 的交互式的三维模型重建系统，但 是该系统依靠关键帧的选取，计算量大、时间复杂度 高，难以用于移动机器人三维地图在线创建。 针对 上述问题，本文提出了一种室内环境下移动机器人 三维视觉 ＳＬＡＭ 方法。 针对点云配准过程存在的匹 配误差较大、效率低下的问题，提出一种分层式点云 配准策略。 同时引入基于体积融合的匹配算法完成 精确配准，有效的去除了拼接结果中的重复点，采用 了 ｇ ２ ０ 优化方法对机器人位姿进行优化，实验验证 了本文方法的可行性和有效性。 １ 系统结构 本文提出的室内环境下移动机器人三维视觉 ＳＬＡＭ 系统的总体框架如图 １ 所示，主要分为特征 点检测与匹配、Ｋｉｎｅｃｔ 标定与点云生成、分层式点云 配准 ３ 部分。 图 １ 室内环境下移动机器人三维视觉 ＳＬＡＭ 总体框架图 Ｆｉｇ． １ Ｔｏｔａｌ ｆｒａｍｅ ｏｆ ｔｈｒｅｅ ｄｉｍｅｎｓｉｏｎａｌ ｖｉｓｕａｌ ＳＬＡＭ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔ ｉｎ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔ ２ 特征点检测与匹配 经典的特征点检测算法包括 Ｈａｒｒｉｓ ［６］ 、ＳＩＦＴ ［７］ 、 ＳＵＲＦ ［８］算法等。 经综合考虑为获得鲁棒性、稳定性 更好的特征点检测结果，本文采用具有旋转和尺度 不变性且速度较快的 ＳＵＲＦ 算法。 ＳＵＲＦ 特征点提 取过程主要包括特征点检测、特征点主方向选取、生 成特征描述子。 判断基于描述的特征点是否匹配， 可通过计算欧式距离来实现，设匹配图像的特征点 集合分别为 Ｘ 和 Ｙ。 欧式距离可以描述为 Ｄ（Ｘ，Ｙ） ＝ ‖Ｘ － Ｙ‖ ＝ ∑ ｄ ｉ ＝ １ （Ｘｉ － Ｙｉ） ｗ （１） 欧式距离越小表示特征点相似程度越高，本文 在匹配过程中采用了最大紧邻向量匹配方法，即通 过比较特征点集合中的每一个特征点与特征点集合 中各特征点的欧式距离， Ｄ１ 和 Ｄ２ 分别代表最近和 次近的欧式距离，若 Ｄ１ ≤ αＤ２（α 为设定的最近次近 距离比值，本文设定 α ＝ ０．６０）， 则认为两个特征点 匹配，反之丢弃该点。 ３ 标定与点云生成 为获取 Ｋｉｎｅｃｔ 的彩色与深度相机的内参数矩 阵以及彩色与深度相机之间的刚体变换，减小深度 图像与红外图像之间存在的漂移［９］ ，需要对 Ｋｉｎｅｃｔ 进行标定，本文采用文献［１０］标定方法对 Ｋｉｎｅｃｔ 进 行标定。 在完成 Ｋｉｎｅｃｔ 对准校正后，可根据图像中 任意一点对应的深度值得到该点三维坐标，进而生 成三维彩色点云数据。 给定任意深度图像像素点 （ｘｄ ，ｙｄ ），投射到三维空间点 ｐ（ｘ，ｙ，ｚ），其坐标计算 如下： ｘ ＝ （ｘｄ － ｃｘｄ ） × ｄｅｐｔｈ（ｘｄ ，ｙｄ ） ｆ ｘｄ （２） ｙ ＝ （ｘｄ － ｃｙｄ ） × ｄｅｐｔｈ（ｘｄ ，ｙｄ ） ｆ ｙｄ （３） ｚ ＝ ｄｅｐｔｈ（ｘｄ ，ｙｄ ） （４） 式中： Ｐｊ ＝ ＰｉＴ ｊ ｉ 为当前像素点深度值，ｃｘｄ和 ｃｙｄ为图 像光心坐标，ｆ ｘｄ和 ｆ ｙｄ为深度相机焦距，其中 ｆ ｘｄ 、ｆ ｙｄ均 可由标定得到。 ４ 分层式点云配准 ４．１ ＲＡＮＳＡＣ 算法粗匹配 本文采用 ＲＡＮＳＡＣ 算法进行特征点的粗匹配， 以使 ２ 组点集的公共区域能够大致重合，为下一步 精确匹配做准备。 算法设定一个容许误差将所有的 匹配点对分为内点和外点。 本文实验中设定的阈值 ｄ ＝ ３．００ 舍去误差大于 ｄ 的匹配点对（外点）。 为了 获得摄像机位姿的变化，本文采用了最小二乘逼近 的方法［１１］ 。 利用算法所得内点进行最小二乘算法 下机器人相邻位姿的估计。 摄像机第 ｉ 时刻的位姿 Ｐｉ 相对于第 ｊ 时刻的摄像机位姿 Ｐｊ 的转换关系为 Ｐｊ ＝ＰｉＴ ｊ ｉ 其中 Ｔ ｊ ｉ 为刚性变换矩阵： Ｔ ｊ ｉ ＝ Ｒ３×３ ｔ ３×１ ０ １ é ë ê ê ù û ú ú （５） 式中： Ｒ３×３ 为旋转矩阵； ｔ ３×１ 为平移矩阵。 对于移动 机器人同步定位与地图构建，可通过下式获得各时 刻数据相对于初始时刻的变换矩阵： ·６１６· 智 能 系 统 学 报 第 １０ 卷
第4期 张毅，等：室内环境下移动机器人三维视觉SLAM ·617. T。=T0*T…T-1 (6) 文引入了基于体积空间融合的匹配算法。算法主要 4.2改进的ICP算法精确配准 思想是在对点云进行融合时，建立包含全部帧的一 传统的ICP算法的基本原理利用最小二乘的优 个立方体栅格来表示三维空间，以一定的分辨率来 化思想通过计算使函数(7)中的R和T最小化。 划分立方体为空间的像素点。每一个点记录了到物 N 体模型表面的距离如图2所示，使用正负来表示表 fR,T)=∑IQ.-rS,-TI2 (7) i=1 面的被遮挡面和可见面，而过零点就代表是表面上 式中：N为对应点对的数量，R为3×3的旋转矩阵， 的点。然后依次对每一帧数据进行处理，当有新的 T为三维的平移向量，S,和Q.分别为初始和目标数 数据加入时由式(10)和式(11)进行处理，通过累加 据点集。ICP匹配算法是基于两片点云是完全重合 将各帧数据融合起来，最后空间体中的零值像素即 这一假设，因此，理论上总是能搜索到最近点。但是 为最后的曲面上的点。 在实际应用中，两片点云之间只有部分是重合的，因 单帧 此，在实际应用中ICP匹配算法容易陷入局部最优， 曲面 甚至不能收敛，要对ICP算法的搜索策略进行改进。 空间体 视线 本文采用了点到切平面的最近搜索算法。另 X 外，在初值的选取上本文采用了阈值法，利用欧氏距 近 远 离阈值和方向向量阈值，提高了初值的匹配准确度。 欧氏距离阈值法可剔除噪声点，提高初值选择成功 相机✉ 中 率。设有点云S和Q,对S中的一个点s,搜索Q中 视线 与s:欧氏距离最近的3个点，分别为91、92和93。 到曲面的 X 距离 若满足s,-9:≤T,则为正确的匹配点，反之为误 、过零点 新过 零点 匹配点予以剔除。式中T为距离超出阈值，经过欧 图2基于体积空间的点云融合 氏距离阈值可基本别除点云数据的噪声点。然后结 Fig.2 Point cloud integration based on space 合方向向量夹角判断是否为对应点，提高初值选择 的正确率。若、9：为一对正确的匹配点，在满足 式(7)的基础上还应当满足下式： D1(=(x)D.(）+W(x)D(x -(10) W(x)+W:+(x) ni x ngi arccos×np ≤0 (8) W+1(x)=W(x)+W+(x) (11) Erπ(d)=d2×0.0035 (12) 式中，nan分别为s:和9：所在曲面的法向量，0为 设定的方向向量夹角阈值。经上述分析，本文改进 w(x）= x-x· (13) Err(x) 的ICP算法步骤如下： 式中：i表示栅格距离值，Eπ(d)为传感器误差函 1)在S中选择初始点集s和。 数，x·为单帧曲面上的点到相机中心距离。算法使 2)使用欧氏距离阈值法在目标点集Q中求出 用权重值进行融合，且具有最小二乘优化性质，因而 与sm最近的点集qm、50和q0构成对应点集。 对传感器的噪声起到了一定的抑制作用。 3)应用方向向量阈值法选定点集s0和90，运 4.4位姿优化 算初值点集s1和91。 在位姿转换计算中通常会存在误差累计，为此 4)应用SVD法求得点集s1和91之间的旋转 采用了g0优化方法，误差函数F(x)定义如下： 矩阵R和平移矢量T。 5)根据式s2=R1s1+T,计算数据点集s经过 F(X)=∑e(x步，g)'D,e(xyg)(14) (ij)eC 一次坐标变换后所得到的数据点集s2,然后重复步 X·=argminF(x) (15) 骤3)~5)，直到满足式(9)： 式中：x:表示第i时刻的机器人位姿，也称g0为图 [d4-d+1<e 中的一个点，表示和x两点之间的约束，e(x.. N (9) )为x:和x,组成的边所产生的向量误差函数，代表 ,19肚-5法2 x:和x满足约束条件z:的程度，当它的值为0时，x, 式中：ε表示的是大于零的阈值，判断迭代是否收 和x,完全满足匹配约束。 敛，收敛则迭代结束。 5实验结果及分析 4.3基于体积空间融合的匹配算法 传统ICP算法存在的一个问题是直接融合两帧 5.1实验平台 点云会造成融合后的点云中出现冗余点。因此，本 本文实验平台分为数据采集端和处理端两部
Ｔ ｋ ０ ＝ Ｔ １ ０∗Ｔ ２ １…Ｔ ｋ ｋ－１ （６） ４．２ 改进的 ＩＣＰ 算法精确配准 传统的 ＩＣＰ 算法的基本原理利用最小二乘的优 化思想通过计算使函数（７）中的 Ｒ 和 Ｔ 最小化。 ｆ（Ｒ，Ｔ） ＝ ∑ Ｎ ｉ ＝ １ ‖Ｑｉ － ＲＳｉ － Ｔ‖２ （７） 式中： Ｎ 为对应点对的数量，Ｒ 为 ３×３ 的旋转矩阵， Ｔ 为三维的平移向量， Ｓｉ 和 Ｑｉ 分别为初始和目标数 据点集。 ＩＣＰ 匹配算法是基于两片点云是完全重合 这一假设，因此，理论上总是能搜索到最近点。 但是 在实际应用中，两片点云之间只有部分是重合的，因 此，在实际应用中 ＩＣＰ 匹配算法容易陷入局部最优， 甚至不能收敛，要对 ＩＣＰ 算法的搜索策略进行改进。 本文采用了点到切平面的最近搜索算法。 另 外，在初值的选取上本文采用了阈值法，利用欧氏距 离阈值和方向向量阈值，提高了初值的匹配准确度。 欧氏距离阈值法可剔除噪声点，提高初值选择成功 率。 设有点云 Ｓ 和 Ｑ，对 Ｓ 中的一个点 ｓｉ ，搜索 Ｑ 中 与 ｓｉ 欧氏距离最近的 ３ 个点，分别为 ｑ１ 、 ｑ２ 和 ｑ３ 。 若满足 ｓｉ － ｑｉ ≤ Ｔ ，则为正确的匹配点，反之为误 匹配点予以剔除。 式中 Ｔ 为距离超出阈值，经过欧 氏距离阈值可基本剔除点云数据的噪声点。 然后结 合方向向量夹角判断是否为对应点，提高初值选择 的正确率。 若 ｓｉ 、 ｑｉ 为一对正确的匹配点，在满足 式（７）的基础上还应当满足下式： ａｒｃｃｏｓ ｎｓｉ × ｎｑｉ ｎｓｉ × ｎｑｉ ≤ θ （８） 式中， ｎｓｉ、ｎｑｉ 分别为 ｓｉ 和 ｑｉ 所在曲面的法向量，θ 为 设定的方向向量夹角阈值。 经上述分析，本文改进 的 ＩＣＰ 算法步骤如下： １） 在 Ｓ 中选择初始点集 ｓｉ０ 。 ２） 使用欧氏距离阈值法在目标点集 Ｑ 中求出 与 ｓｉ０ 最近的点集 ｑｉ０ 、 ｓｉ０ 和 ｑｉ０ 构成对应点集。 ３）应用方向向量阈值法选定点集 ｓｉ０ 和 ｑｉ０ ，运 算初值点集 ｓｉ１ 和 ｑｉ１ 。 ４） 应用 ＳＶＤ 法求得点集 ｓｉ１ 和 ｑｉ１ 之间的旋转 矩阵 Ｒ 和平移矢量 Ｔ。 ５） 根据式 ｓｉ２ ＝ Ｒ１ ｓｉ１ ＋ Ｔ１ ，计算数据点集 ｓｉ１ 经过 一次坐标变换后所得到的数据点集 ｓｉ２ ，然后重复步 骤 ３） ～５），直到满足式（９）： ｄｋ － ｄｋ＋１ ＜ ε ｄｋ ＝ １ Ｎ∑ Ｎ ｉ ＝ １ ‖ｑｉｋ － ｓｉｋ‖２ ì î í ï ï ïï （９） 式中： ε 表示的是大于零的阈值，判断迭代是否收 敛，收敛则迭代结束。 ４．３ 基于体积空间融合的匹配算法 传统 ＩＣＰ 算法存在的一个问题是直接融合两帧 点云会造成融合后的点云中出现冗余点。 因此，本 文引入了基于体积空间融合的匹配算法。 算法主要 思想是在对点云进行融合时，建立包含全部帧的一 个立方体栅格来表示三维空间，以一定的分辨率来 划分立方体为空间的像素点。 每一个点记录了到物 体模型表面的距离如图 ２ 所示，使用正负来表示表 面的被遮挡面和可见面，而过零点就代表是表面上 的点。 然后依次对每一帧数据进行处理，当有新的 数据加入时由式（１０）和式（１１）进行处理，通过累加 将各帧数据融合起来，最后空间体中的零值像素即 为最后的曲面上的点。 图 ２ 基于体积空间的点云融合 Ｆｉｇ． ２ Ｐｏｉｎｔ ｃｌｏｕｄ ｉｎｔｅｇｒａｔｉｏｎ ｂａｓｅｄ ｏｎ ｓｐａｃｅ Ｄｉ＋１（ｘ） ＝ Ｗｉ（ｘ）Ｄｉ（ｘ） ＋ Ｗｉ＋１（ｘ）Ｄｉ＋１（ｘ） Ｗｉ（ｘ） ＋ Ｗｉ＋１（ｘ） （１０） Ｗｉ＋１（ｘ） ＝ Ｗｉ（ｘ） ＋ Ｗｉ＋１（ｘ） （１１） Ｅｒｒ（ｄ） ＝ ｄ ２ × ０．００３ ５ （１２） ωｉ（ｘ） ＝ ｘ － ｘ ∗ Ｅｒｒ（ｘ ∗ ） （１３） 式中：ｉ 表示栅格距离值， Ｅｒｒ（ｄ） 为传感器误差函 数， ｘ ∗ 为单帧曲面上的点到相机中心距离。 算法使 用权重值进行融合，且具有最小二乘优化性质，因而 对传感器的噪声起到了一定的抑制作用。 ４．４ 位姿优化 在位姿转换计算中通常会存在误差累计，为此 采用了 ｇ ２ ０ 优化方法，误差函数 Ｆ（ｘ）定义如下： Ｆ（Ｘ） ＝ （ｉ∑，ｊ）∈Ｃ ｅ （ｘｉ，ｙｊ，ｚｉｊ） ＴΩｉｊ ｅ（ｘｉ，ｙｊ，ｚｉｊ） （１４） Ｘ ∗ ＝ ａｒｇｍｉｎ ｘ Ｆ（ｘ） （１５） 式中：ｘｉ表示第 ｉ 时刻的机器人位姿，也称 ｇ ２ ０ 为图 中的一个点，ｚｉｊ表示 ｘｊ和 ｘｉ两点之间的约束，ｅ（ｘｉ ， ｘｊ ， ｚｉｊ）为 ｘｉ和 ｘｊ组成的边所产生的向量误差函数，代表 ｘｉ和 ｘｊ满足约束条件 ｚｉｊ的程度，当它的值为 ０ 时，ｘｉ 和 ｘｊ完全满足匹配约束。 ５ 实验结果及分析 ５．１ 实验平台 本文实验平台分为数据采集端和处理端两部 第 ４ 期 张毅，等：室内环境下移动机器人三维视觉 ＳＬＡＭ ·６１７·
.618 智能系统学报 第10卷 分，其中数据采集端（图3(b)所示）主要由Pio 配剔除，提高特征匹配的正确率，实验中完成粗匹配 neer3-DX机器人、Kinect和笔记本电脑组成，Kinect 所耗时间为0.00425s。 图像分辨率为640×480，最高帧频为30帧/s,水平 视场角为52°。数据处理端处理器为一台Intel双核 2.0GHz主频的PC机，运行Ubuntu12.04 Linux操 作系统。为保证较高的鲁棒性，实验中机器人移动 速度v为0.1m/s、转弯角速度w为0.52rad/s (30°/s)对每个关键帧完成特征点提取、误匹配剔除 (a)未经RANSAC算法匹配 和地图更新的时间约为300ms。 (b)经RANSAC算法匹配 图5粗匹配实验结果 Fig.5 The experimental results of coarse matching (a)实验场景 5.22三维环境重构效果对比 机器人以0.1m/s的线速度在室内环境下创建 地图，实验过程中最大迭代次数为50，阈值设定为 5cm,内点数量阈值N=13;台式机处理采集到的 图像帧，当特征点对少于50组时，将舍弃该帧数据， 不进行初始配准。 物体轮廓模糊， (b)实验装置 冗余点较多 图3实验装置与实验场景 Fig.3 Experimental apparatus and experimental scene 5.2真实环境下的三维视觉SLAM 5.2.1彩色点云生成与位姿估计 以室内真实环境（图3(a)所示）作为实验场 (a)传统方法重构结果 景，在对Kinect进行标定的基础上，由所得内外参 数以及对准校正后的像素点深度值将图像点转换为 彩色点云数据，如图4所示。 物体轮廓清晰 冗余点减少 (b)本文方法重构结果 图4环境三维彩色点云 图6室内环境三维重构结果 Fig.4 Environment 3D color point cloud Fig.6 3D reconstruction result of the indoor environment 在利用特征匹配算法获得相邻帧间对应关系的 图6(a)为采用文献[4]方法得到的三维环境重 基础上，利用RANSAC算法对点云进行粗匹配，粗 构结果，所构建的三维地图存在8冗余即8点较多 匹配结果如图5所示，由图可以看出，经过RANSAC 环境中物体轮廓模糊等问题。图6(b)为采用本文 算法进行粗匹配后能够将匹配过程中出现的错误匹 方法构建的三维环境重构结果，获得的三维地图较好
分，其中数据采集端（ 图 ３ （ ｂ） 所示） 主要由 Ｐｉｏ⁃ ｎｅｅｒ３⁃ＤＸ 机器人、Ｋｉｎｅｃｔ 和笔记本电脑组成，Ｋｉｎｅｃｔ 图像分辨率为 ６４０×４８０，最高帧频为 ３０ 帧／ ｓ，水平 视场角为 ５２°。 数据处理端处理器为一台 Ｉｎｔｅｌ 双核 ２．０ ＧＨｚ 主频的 ＰＣ 机， 运行 Ｕｂｕｎｔｕ １２．０４ Ｌｉｎｕｘ 操 作系统。 为保证较高的鲁棒性，实验中机器人移动 速度 ｖ 为 ０． １ ｍ ／ ｓ、 转弯角速度 ｗ 为 ０． ５２ ｒａｄ ／ ｓ （３０°／ ｓ）对每个关键帧完成特征点提取、误匹配剔除 和地图更新的时间约为 ３００ ｍｓ。 （ａ） 实验场景 （ｂ） 实验装置 图 ３ 实验装置与实验场景 Ｆｉｇ． ３ Ｅｘｐｅｒｉｍｅｎｔａｌ ａｐｐａｒａｔｕｓ ａｎｄ ｅｘｐｅｒｉｍｅｎｔａｌ ｓｃｅｎｅ ５．２ 真实环境下的三维视觉 ＳＬＡＭ ５．２．１ 彩色点云生成与位姿估计 以室内真实环境（图 ３ （ ａ） 所示） 作为实验场 景，在对 Ｋｉｎｅｃｔ 进行标定的基础上，由所得内外参 数以及对准校正后的像素点深度值将图像点转换为 彩色点云数据，如图 ４ 所示。 图 ４ 环境三维彩色点云 Ｆｉｇ． ４ Ｅｎｖｉｒｏｎｍｅｎｔ ３Ｄ ｃｏｌｏｒ ｐｏｉｎｔ ｃｌｏｕｄ 在利用特征匹配算法获得相邻帧间对应关系的 基础上，利用 ＲＡＮＳＡＣ 算法对点云进行粗匹配，粗 匹配结果如图 ５ 所示，由图可以看出，经过 ＲＡＮＳＡＣ 算法进行粗匹配后能够将匹配过程中出现的错误匹 配剔除，提高特征匹配的正确率，实验中完成粗匹配 所耗时间为 ０．００４ ２５ ｓ。 （ａ） 未经 ＲＡＮＳＡＣ 算法匹配 （ｂ） 经 ＲＡＮＳＡＣ 算法匹配 图 ５ 粗匹配实验结果 Ｆｉｇ． ５ Ｔｈｅ ｅｘｐｅｒｉｍｅｎｔａｌ ｒｅｓｕｌｔｓ ｏｆ ｃｏａｒｓｅ ｍａｔｃｈｉｎｇ ５．２．２ 三维环境重构效果对比 机器人以 ０．１ ｍ ／ ｓ 的线速度在室内环境下创建 地图，实验过程中最大迭代次数为 ５０，阈值设定为 ５ ｃｍ，内点数量阈值 Ｎ ＝ １３； 台式机处理采集到的 图像帧，当特征点对少于 ５０ 组时，将舍弃该帧数据， 不进行初始配准。 （ａ） 传统方法重构结果 （ｂ） 本文方法重构结果 图 ６ 室内环境三维重构结果 Ｆｉｇ． ６ ３Ｄ ｒｅｃｏｎｓｔｒｕｃｔｉｏｎ ｒｅｓｕｌｔ ｏｆ ｔｈｅ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔ 图 ６（ａ） 为采用文献［４］方法得到的三维环境重 构结果，所构建的三维地图存在 ８ 冗余即 ８ 点较多、 环境中物体轮廓模糊等问题。 图 ６（ｂ） 为采用本文 方法构建的三维环境重构结果，获得的三维地图较好 ·６１８· 智 能 系 统 学 报 第 １０ 卷
第4期 张毅，等：室内环境下移动机器人三维视觉SLAM .619. 的解决了上述问题，物体轮廓清晰，冗余点减少。 [3]HERRERA C D,KANNALA J,HEIKKILA J.Joint depth 5.2.3机器人运动轨迹跟踪 and color camera calibration with distortion correction[J]. 为了获得相机位姿的变化，本文采用最小二乘 IEEE Transactions on Pattern Analysis and Machine Intelli- gence,2012,34(10):2058-2064. 逼近的方法对机器人相邻位姿进行估计，借助g0 [4]杨扬，曹其新，朱笑笑.面向机器人手眼协调抓取的3维 优化方法对估计位姿进行优化。为了使运动轨迹更 建模方法[J].机器人，2013,35(2)：151-155. 为直观，实验给出了机器人运动轨迹在X/Y平面上 YANG Yang,CAO Qixin,ZHU Xiaoxiao.A 3D modeling 的投影如图7(a)所示，以及机器人在三维空间中的 method for robot's hand-eye coordinated grasping[J].Ro- 运动轨迹如图7(b)所示。可以看出本文方法能较 bot,2013,35(2):151-155. 好的实现机器人的运动轨迹跟踪。 [5]HENRY P,KRAININ M,HERBST E,et al.RGB-D map- ping:Using Kinect-style depth cameras for dense 3D model- ing of indoor environments[J].International Journal of Ro- 2 botics Research,2012,31(5):647-663. 里程计记录的机 [6]HARRIS C,STEPHENS M.A combined corner and edge de- 器人运动轨迹 tector[C]//Proc.Alvey Vis.Conf.,1988:147-151. [7]LOWE D G.Object recognition from local scale-invariant features [C]//International Conference on Computer Vi- sion.Kerkyra,Corfu,Greece,1999:1150-1157. 预测的运动 [8]BAY H,ESS A,TUYTELAARS T,et al.Speeded-up ro- 轨迹 bust features (SURF)[J].Comput.Vis.Image Understand- 3 ing,2008,110:346-359. x/m [9]KUMMERLE R,GRISETTI G,STRASDAT H,et al.G2o: A general framework for graph optimization [C]//IEEE In- (a)运动轨迹在X/Y平面上的投影 ternational Conference on Robotics and Automation.Piscat- away:EEE,2011:3607-3613. 系统跟踪的机 [10]JAN S,MICHAL J,TOMAS P.3D with Kinect [C]// 器人运动轨迹 2011 IEEE International Conference on Computer Vision Workshops.Barcelona:Institute of Electrical and Electron- ics Engineers Inc,2011:1154-1160. [11]TYKKALA T,AUDRAS C.Comport A I.Direct iterative closest point for real-time visual odometry C]//ICCV Workshops.IEEE,Barcelona,Spain,2011:2050-2056. 作者简介： 张毅，男，1966年生，教授，博士生 (b)轨迹跟踪结果 导师，中国人工智能学会理事，国家信 图7机器人运动轨迹跟踪 息无障碍研发中心主任，主要研究方向 Fig.7 Robot trajectory tracking 为智能系统与移动机器人、机器视觉与 模式识别、多传感器信息融合。主持完 6结束语 成国家级和省部基金项目10余项，发 表学术论文160余篇，SCI、EI收录80 本文提出的分层式点云配准策略，较好地解决 余篇，获国家发明专利10余项。 了点云配准过程中匹配误差较大、效率低下的问题。 陈起，男，1991年生，硕士，主要研 在三维地图生成部分，引入了基于体积空间的多帧 究方向为机器人同时定位与地图创建 融合方法来减少冗余点。实验结果表明本文所提方 (SLAM)、机器人三维视觉导航。 法的有效性。下一步计划对系统进行两方面的改 进：1)对创建的环境三维地图的全局优化。2)研究 采用并行技术，进一步提高系统执行效率，缩短系统 的运行时间，使系统具有更好的实时性。 罗元，女，1972年生，教授，博士，美 参考文献： 国OSA,SPIE、中国仪器仪表学会、中国 光学学会高级会员，主要研究方向为人 [1]RYDE J,HU H S.3D mapping with multi-resolution occu- 机交互、基于图像视频处理的人机交 pied voxel lists [J].Autonomous Robots,2010,28(2): 互、基于图像视频处理的测试。主持和 169-185. 参与国家、省部基金项目10余项，发表 [2]KOPPILA H S,ANAND A.Semantic labeling of 3D point 学术论文100余篇，被SCI、EI收录40余篇，获国家发明专 clouds for indoor scenes[C]//25th Annual Conference on 利6项。 Neural Information Processing Systems.New York,USA: [责任编辑：陈峰] Curran Associates Inc.,2011
的解决了上述问题，物体轮廓清晰，冗余点减少。 ５．２．３ 机器人运动轨迹跟踪 为了获得相机位姿的变化，本文采用最小二乘 逼近的方法对机器人相邻位姿进行估计，借助 ｇ ２ ０ 优化方法对估计位姿进行优化。 为了使运动轨迹更 为直观，实验给出了机器人运动轨迹在 Ｘ ／ Ｙ 平面上 的投影如图 ７（ａ）所示，以及机器人在三维空间中的 运动轨迹如图 ７（ ｂ）所示。 可以看出本文方法能较 好的实现机器人的运动轨迹跟踪。 （ａ） 运动轨迹在 Ｘ／ Ｙ 平面上的投影 （ｂ） 轨迹跟踪结果 图 ７ 机器人运动轨迹跟踪 Ｆｉｇ． ７ Ｒｏｂｏｔ ｔｒａｊｅｃｔｏｒｙ ｔｒａｃｋｉｎｇ ６ 结束语 本文提出的分层式点云配准策略，较好地解决 了点云配准过程中匹配误差较大、效率低下的问题。 在三维地图生成部分，引入了基于体积空间的多帧 融合方法来减少冗余点。 实验结果表明本文所提方 法的有效性。 下一步计划对系统进行两方面的改 进：１）对创建的环境三维地图的全局优化。 ２）研究 采用并行技术，进一步提高系统执行效率，缩短系统 的运行时间，使系统具有更好的实时性。 参考文献： ［１］ＲＹＤＥ Ｊ， ＨＵ Ｈ Ｓ． ３Ｄ ｍａｐｐｉｎｇ ｗｉｔｈ ｍｕｌｔｉ⁃ｒｅｓｏｌｕｔｉｏｎ ｏｃｃｕ⁃ ｐｉｅｄ ｖｏｘｅｌ ｌｉｓｔｓ ［ Ｊ］． Ａｕｔｏｎｏｍｏｕｓ Ｒｏｂｏｔｓ， ２０１０， ２８ （ ２）： １６９⁃１８５． ［２］ＫＯＰＰＩＬＡ Ｈ Ｓ， ＡＮＡＮＤ Ａ． Ｓｅｍａｎｔｉｃ ｌａｂｅｌｉｎｇ ｏｆ ３Ｄ ｐｏｉｎｔ ｃｌｏｕｄｓ ｆｏｒ ｉｎｄｏｏｒ ｓｃｅｎｅｓ［Ｃ］ ／ ／ ２５ｔｈ Ａｎｎｕａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｎｅｕｒａｌ Ｉｎｆｏｒｍａｔｉｏｎ Ｐｒｏｃｅｓｓｉｎｇ Ｓｙｓｔｅｍｓ． Ｎｅｗ Ｙｏｒｋ， ＵＳＡ： Ｃｕｒｒａｎ Ａｓｓｏｃｉａｔｅｓ Ｉｎｃ．， ２０１１． ［３］ ＨＥＲＲＥＲＡ Ｃ Ｄ， ＫＡＮＮＡＬＡ Ｊ， ＨＥＩＫＫＩＬＡ Ｊ． Ｊｏｉｎｔ ｄｅｐｔｈ ａｎｄ ｃｏｌｏｒ ｃａｍｅｒａ ｃａｌｉｂｒａｔｉｏｎ ｗｉｔｈ ｄｉｓｔｏｒｔｉｏｎ ｃｏｒｒｅｃｔｉｏｎ［ Ｊ］． ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉ⁃ ｇｅｎｃｅ， ２０１２， ３４（１０）：２０５８⁃２０６４． ［４］杨扬，曹其新，朱笑笑．面向机器人手眼协调抓取的 ３ 维 建模方法［Ｊ］．机器人，２０１３，３５（２）： １５１⁃１５５． ＹＡＮＧ Ｙａｎｇ， ＣＡＯ Ｑｉｘｉｎ， ＺＨＵ Ｘｉａｏｘｉａｏ． Ａ ３Ｄ ｍｏｄｅｌｉｎｇ ｍｅｔｈｏｄ ｆｏｒ ｒｏｂｏｔ’ ｓ ｈａｎｄ⁃ｅｙｅ ｃｏｏｒｄｉｎａｔｅｄ ｇｒａｓｐｉｎｇ［ Ｊ］． Ｒｏ⁃ ｂｏｔ，２０１３，３５（２）：１５１⁃１５５． ［５］ＨＥＮＲＹ Ｐ， ＫＲＡＩＮＩＮ Ｍ， ＨＥＲＢＳＴ Ｅ， ｅｔ ａｌ． ＲＧＢ⁃Ｄ ｍａｐ⁃ ｐｉｎｇ： Ｕｓｉｎｇ Ｋｉｎｅｃｔ⁃ｓｔｙｌｅ ｄｅｐｔｈ ｃａｍｅｒａｓ ｆｏｒ ｄｅｎｓｅ ３Ｄ ｍｏｄｅｌ⁃ ｉｎｇ ｏｆ ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔｓ［ Ｊ］． Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｒｏ⁃ ｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ， ２０１２， ３１（５）： ６４７⁃６６３． ［６］ＨＡＲＲＩＳ Ｃ， ＳＴＥＰＨＥＮＳ Ｍ． Ａ ｃｏｍｂｉｎｅｄ ｃｏｒｎｅｒ ａｎｄ ｅｄｇｅ ｄｅ⁃ ｔｅｃｔｏｒ［Ｃ］ ／ ／ Ｐｒｏｃ． Ａｌｖｅｙ Ｖｉｓ． Ｃｏｎｆ． ，１９８８： １４７⁃１５１． ［７］ ＬＯＷＥ Ｄ Ｇ． Ｏｂｊｅｃｔ ｒｅｃｏｇｎｉｔｉｏｎ ｆｒｏｍ ｌｏｃａｌ ｓｃａｌｅ⁃ｉｎｖａｒｉａｎｔ ｆｅａｔｕｒｅｓ ［ Ｃ］ ／ ／ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉ⁃ ｓｉｏｎ． Ｋｅｒｋｙｒａ，Ｃｏｒｆｕ，Ｇｒｅｅｃｅ，１９９９：１１５０⁃１１５７． ［８］ＢＡＹ Ｈ， ＥＳＳ Ａ， ＴＵＹＴＥＬＡＡＲＳ Ｔ， ｅｔ ａｌ． Ｓｐｅｅｄｅｄ⁃ｕｐ ｒｏ⁃ ｂｕｓｔ ｆｅａｔｕｒｅｓ （ＳＵＲＦ） ［Ｊ］．Ｃｏｍｐｕｔ． Ｖｉｓ． Ｉｍａｇｅ Ｕｎｄｅｒｓｔａｎｄ⁃ ｉｎｇ， ２００８，１１０：３４６⁃３５９． ［９］ＫＵＭＭＥＲＬＥ Ｒ， ＧＲＩＳＥＴＴＩ Ｇ， ＳＴＲＡＳＤＡＴ Ｈ， ｅｔ ａｌ． Ｇ２ｏ： Ａ ｇｅｎｅｒａｌ ｆｒａｍｅｗｏｒｋ ｆｏｒ ｇｒａｐｈ ｏｐｔｉｍｉｚａｔｉｏｎ ［Ｃ］ ／ ／ ＩＥＥＥ Ｉｎ⁃ ｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ． Ｐｉｓｃａｔ⁃ ａｗａｙ： ＩＥＥＥ，２０１１： ３６０７⁃３６１３． ［１０］ ＪＡＮ Ｓ， ＭＩＣＨＡＬ Ｊ， ＴＯＭＡＳ Ｐ． ３Ｄ ｗｉｔｈ Ｋｉｎｅｃｔ ［ Ｃ］ ／ ／ ２０１１ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ Ｗｏｒｋｓｈｏｐｓ． Ｂａｒｃｅｌｏｎａ： Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｅｌｅｃｔｒｉｃａｌ ａｎｄ Ｅｌｅｃｔｒｏｎ⁃ ｉｃｓ Ｅｎｇｉｎｅｅｒｓ Ｉｎｃ，２０１１： １１５４⁃１１６０． ［１１］ＴＹＫＫＡＬＡ Ｔ，ＡＵＤＲＡＳ Ｃ， Ｃｏｍｐｏｒｔ Ａ Ｉ． Ｄｉｒｅｃｔ ｉｔｅｒａｔｉｖｅ ｃｌｏｓｅｓｔ ｐｏｉｎｔ ｆｏｒ ｒｅａｌ⁃ｔｉｍｅ ｖｉｓｕａｌ ｏｄｏｍｅｔｒｙ ［ Ｃ ］ ／ ／ ＩＣＣＶ Ｗｏｒｋｓｈｏｐｓ． ＩＥＥＥ，Ｂａｒｃｅｌｏｎａ， Ｓｐａｉｎ，２０１１：２０５０⁃２０５６． 作者简介： 张毅，男， １９６６ 年生，教授，博士生 导师，中国人工智能学会理事，国家信 息无障碍研发中心主任，主要研究方向 为智能系统与移动机器人、机器视觉与 模式识别、多传感器信息融合。 主持完 成国家级和省部基金项目 １０ 余项，发 表学术论文 １６０ 余篇，ＳＣＩ、ＥＩ 收录 ８０ 余篇，获国家发明专利 １０ 余项。 陈起，男，１９９１ 年生，硕士，主要研 究方向为机器人同时定位与地图创建 （ＳＬＡＭ）、机器人三维视觉导航。 罗元，女，１９７２ 年生，教授，博士，美 国 ＯＳＡ、ＳＰＩＥ、中国仪器仪表学会、中国 光学学会高级会员，主要研究方向为人 机交互、基于图像视频处理的人机交 互、基于图像视频处理的测试。 主持和 参与国家、省部基金项目 １０ 余项，发表 学术论文 １００ 余篇，被 ＳＣＩ、ＥＩ 收录 ４０ 余篇，获国家发明专 利 ６ 项。 ［责任编辑：陈峰］ 第 ４ 期 张毅，等：室内环境下移动机器人三维视觉 ＳＬＡＭ ·６１９·