第13卷第5期 智能系统学报 Vol.13 No.5 2018年10月 CAAI Transactions on Intelligent Systems Oct.2018 D0:10.11992/tis.201705018 网络出版地址：http:/kns.cnki.net/kcms/detail/23.1538.tp.20180423.0957.004.html 基于Kinect的改进移动机器人视觉SLAM 蔡军，陈科宇，张毅 (重庆邮电大学重庆市信息无障碍与服务机器人工程技术研究中心，重庆400065) 摘要：针对传统ICP(iterative closest points,迭代最近点算法)存在易陷入局部最优、匹配误差大等问题，提出 了一种新的欧氏距离和角度阈值双重限制方法，并在此基础上构建了基于Kinect的室内移动机器人RGB-D SLAM(simultaneous localization and mapping)系统。首先，使用Kinect获取室内环境的彩色信息和深度信息，通 过图像特征提取与匹配，结合相机内参与像素点深度值，建立三维点云对应关系；然后，利用RANSAC(random sample consensus)算法剔除外点，完成点云的初匹配；采用改进的点云配准算法完成点云的精匹配；最后，在关 键帧选取中引人权重，结合g2o(general graph optimization)算法对机器人位姿进行优化。实验证明该方法的有 效性与可行性，提高了三维点云地图的精度，并估计出了机器人运行轨迹。 关键词：移动机器人；Kinect:同时定位与地图构建；迭代最近点算法；关键帧；随机采样一致性；位姿估计；三维 重建 中图分类号：TP242.6文献标志码：A文章编号：1673-4785(2018)05-0734-07 中文引用格式：蔡军，陈科宇，张毅.基于Kinect的改进移动机器人视觉SLAM[J.智能系统学报，2018,13（⑤）：734-740. 英文引用格式：CAI Jun,CHEN Keyu,ZHANG Yi.Improved V-SLAM for mobile robots based on KinectJ]..CAAI transactions on intelligent systems,2018,13(5):734-740. Improved V-SLAM for mobile robots based on Kinect CAI Jun,CHEN Keyu,ZHANG Yi (Chongqing Information Accessibility and Service Robot Engineering Technology Research Center,Chongqing University of Posts and Telecommunications,Chongqing 400065,China) Abstract:Given that the traditional iterative closest points(ICP)algorithm easily falls into the local optimum and has a large matching error,a novel double restriction method containing Euclidean distance and angle threshold is proposed. To realize this,an indoor mobile robot RGB-D SLAM(simultaneous localization and mapping)using a Kinect camera was developed.First,the Kinect camera was used to get color information and depth information for the indoor environ- ment.Through the image feature extraction and matching procedure,the relationship between two 3D point clouds was established by combining the camera intrinsic parameters and pixel depth values.Then,the initial registration was com- pleted using the random sample consensus(RANSAC)algorithm to remove outliers.Meanwhile,accurate registration was completed using the improved ICP algorithm.Finally,the weight was introduced into the selection of the key frames,and the general graph optimization(g2o)algorithm was used to optimize the pose of the robot.The experiment- al results prove effectiveness and feasibility of the method,and this method improves the accuracy of the 3D point cloud map and estimates the trajectory of the robot. Keywords:mobile robot:Kinect:SLAM:ICP:key-frame:RANSAC:pose estimate:three-dimensional reconstruction 近年来，随着SLAM研究的不断深入，使三上来说，SLAM所有的计算都是对传感器数据的 维重建成为了机器人研究领域的热潮山。从本质 处理，因此不同传感器的基本方程的参数化有很 收稿日期：2017-05-15.网络出版日期：2018-04-24. 大的差别。常见的SLAM传感器有IMU、激光传 基金项目：国家自然科学基金项目(61673079)：重庆市科学技 术委员会资助项目(cstc2015 SjcyjBX0066). 感器。MU通常包含陀螺仪和加速度计。陀螺 通信作者：陈科宇.E-mail:keyul118@163.com. 仪具有良好的动态响应，但会产生累计误差；加
DOI: 10.11992/tis.201705018 网络出版地址: http://kns.cnki.net/kcms/detail/23.1538.tp.20180423.0957.004.html 基于 Kinect 的改进移动机器人视觉 SLAM 蔡军，陈科宇，张毅 （重庆邮电大学 重庆市信息无障碍与服务机器人工程技术研究中心，重庆 400065） 摘 要：针对传统 ICP(iterative closest points，迭代最近点算法) 存在易陷入局部最优、匹配误差大等问题，提出 了一种新的欧氏距离和角度阈值双重限制方法，并在此基础上构建了基于 Kinect 的室内移动机器人 RGB-D SLAM(simultaneous localization and mapping) 系统。首先，使用 Kinect 获取室内环境的彩色信息和深度信息，通 过图像特征提取与匹配，结合相机内参与像素点深度值，建立三维点云对应关系；然后，利用 RANSAC(random sample consensus) 算法剔除外点，完成点云的初匹配；采用改进的点云配准算法完成点云的精匹配；最后，在关 键帧选取中引入权重，结合 g2o(general graph optimization) 算法对机器人位姿进行优化。实验证明该方法的有 效性与可行性，提高了三维点云地图的精度，并估计出了机器人运行轨迹。 关键词：移动机器人；Kinect；同时定位与地图构建；迭代最近点算法；关键帧；随机采样一致性；位姿估计；三维 重建 中图分类号：TP242.6 文献标志码：A 文章编号：1673−4785(2018)05−0734−07 中文引用格式：蔡军, 陈科宇, 张毅. 基于 Kinect 的改进移动机器人视觉 SLAM[J]. 智能系统学报, 2018, 13(5): 734–740. 英文引用格式：CAI Jun, CHEN Keyu, ZHANG Yi. Improved V-SLAM for mobile robots based on Kinect[J]. CAAI transactions on intelligent systems, 2018, 13(5): 734–740. Improved V-SLAM for mobile robots based on Kinect CAI Jun，CHEN Keyu，ZHANG Yi (Chongqing Information Accessibility and Service Robot Engineering Technology Research Center, Chongqing University of Posts and Telecommunications, Chongqing 400065, China) Abstract: Given that the traditional iterative closest points (ICP) algorithm easily falls into the local optimum and has a large matching error, a novel double restriction method containing Euclidean distance and angle threshold is proposed. To realize this, an indoor mobile robot RGB-D SLAM (simultaneous localization and mapping) using a Kinect camera was developed. First, the Kinect camera was used to get color information and depth information for the indoor environment. Through the image feature extraction and matching procedure, the relationship between two 3D point clouds was established by combining the camera intrinsic parameters and pixel depth values. Then, the initial registration was completed using the random sample consensus (RANSAC) algorithm to remove outliers. Meanwhile, accurate registration was completed using the improved ICP algorithm. Finally, the weight was introduced into the selection of the key frames, and the general graph optimization (g2o) algorithm was used to optimize the pose of the robot. The experimental results prove effectiveness and feasibility of the method, and this method improves the accuracy of the 3D point cloud map and estimates the trajectory of the robot. Keywords: mobile robot; Kinect; SLAM; ICP; key-frame; RANSAC; pose estimate; three-dimensional reconstruction 近年来，随着 SLAM 研究的不断深入，使三 维重建成为了机器人研究领域的热潮[1]。从本质 上来说，SLAM 所有的计算都是对传感器数据的 处理，因此不同传感器的基本方程的参数化有很 大的差别。常见的 SLAM 传感器有 IMU、激光传 感器。IMU 通常包含陀螺仪和加速度计。陀螺 仪具有良好的动态响应，但会产生累计误差；加 收稿日期：2017−05−15. 网络出版日期：2018−04−24. 基金项目：国家自然科学基金项目 (61673079)；重庆市科学技 术委员会资助项目 (cstc2015jcyjBX0066). 通信作者：陈科宇. E-mail: keyu1118@163.com. 第 13 卷第 5 期 智 能 系 统 学 报 Vol.13 No.5 2018 年 10 月 CAAI Transactions on Intelligent Systems Oct. 2018
第5期 蔡军，等：基于Kinect的改进移动机器人视觉SLAM ·735· 速度计虽然不会产生累计误差，但易受到机器运 法虽大大减少了算法运算时间，但在精度上却不 行时的振动干扰：激光传感器精度高、探测范围 如传统ICP算法。文献[6]针对服务机器人手眼协 广，但价格昂贵、功耗大；相机主要分为单目相 调抓取提出了一种利用RGB-D传感器对家庭常 机、双目相机和RGB-D四相机，单目相机无法直接 见物体进行3-D建模的方法。该方法仅需环绕物 获取图像深度信息，需要通过移动相机来产生深 体采取数十帧即可重建三维模型，但无法估计相 度；双目相机通过视差计算深度，在推算图像中 机位姿。 物体距离时计算量大，实时性较差：典型的RGB- 针对以上研究现状，本文提出了一种新的室 D相机，如Kinect,,具有价格低廉、信息丰富、更 内移动机器人视觉SLAM方法。通过Kinect相 新速度快、能直接获取图像深度等优点，因此其 机获取室内环境信息，对获取的彩色图像进行 成为了RGB-D SLAM系统的主流视觉传感器，受 特征提取与匹配，结合标定后的Kinect内参与 到广大视觉SLAM研究者的青睐。因此，国内外 像素点深度值，得到对应的空间对应点云，运用 学者对Kinect在室内移动机器人上的应用研究取 RANSAC方法筛选出可靠的内点进行初始位姿 得了一定的研究成果。 估计完成初匹配。在精匹配过程中，初匹配的结 一个完整的RGB-D SLAM系统应该包含传 果作为精匹配的初值，通过添加点对间距离和角 感器数据、视觉里程计、非线性优化、建图与回环 度阈值双重限制，剔除误匹配点对，提高点云配 检测等主要5个部分。视觉里程计的作用是估计 准的精度。在优化端的关键帧选取中引入权重， 相机运动，其中包括特征点检测与提取、相邻帧 通过回环检测消除累积误差，实现三维地图的优 间配准、迭代最近点等。非线性优化是从带有噪 化与更新。 声的传感器数据中优化机器人运行轨迹和在运行 过程中增量式构建环境地图。回环检测是机器 1 基于Kinect的改进移动机器人V- 人通过识别图像间的相似性来判断是否回到之前 SLAM算法基本原理 到过的位置。在视觉里程计部分，通常采用 ICP算法去估计相机的运动。然而传统的ICP算 1.1 系统结构 法搜寻对应点对相当耗时，且容易出现大量误 本文提出的室内移动机器人视觉SLAM系统 匹配点对。针对以上问题，许多学者提出了优秀 框架如图1所示，在进行传感器数据采集之前，需 的改进ICP算法。文献[5]针对多场景三维重建问 对Kinect传感器进行标定以获取相机内部参数， 题，在基于2D局部特征匹配的基础上，提出了一 对获取到的彩色图像进行特征检测与提取，进行 种新颖且高效的3D配准方法。该方法将三维点 初步筛选后，利用RANSAC算法剔除离群点获取 云转换为2D轴承角图像，然后在两幅图像之间 初始位姿估计，再用改进的ICP算法完成点云的 利用SURF特征搜寻匹配的像素对，获取3D点云 精准匹配，在关键帧选取中引入权重，通过回环检测 的对应点，再通过基于SVD的方法求出最优旋转 消除累积误差，实现三维点云地图的优化与更 矩阵，将点云与矩阵对齐来重建3D模型。该方 新，最终获得机器人运行轨迹和室内环境点云地图。 彩色 特征检测 RANSAC 改进的ICP完 位姿 运行 图像 与匹配 去除误匹配 成点云配准 估计 轨迹 Kinect相机 回环 检测 深度 Kinect标定 点云 信息 与点云生成 地图 图1室内环境下基于移动机器人的RGB-D SLAM系统框架图 Fig.1 The frame of RGB-D SLAM system for mobile robot in indoor environment 1.2特征检测与匹配 为视觉里程计。视觉里程计的实现方法主要分为 SLAM包含两个问题，一个是“定位”，一个是 特征法和直接法。特征法运行稳定，对光照、动 “建图”。解决定位问题的关键是在相机获取了机 态物体不敏感，是目前比较成熟的解决方案，因 器人所在的室内环境信息后，如何根据相邻图像 此本文采用特征法。 的信息，粗略估计出相机运动，这一过程也被称 随着在计算机视觉领域的长年探索，研究者
速度计虽然不会产生累计误差，但易受到机器运 行时的振动干扰；激光传感器精度高、探测范围 广，但价格昂贵、功耗大；相机主要分为单目相 机、双目相机和 RGB-D[2]相机，单目相机无法直接 获取图像深度信息，需要通过移动相机来产生深 度；双目相机通过视差计算深度，在推算图像中 物体距离时计算量大，实时性较差；典型的 RGBD 相机，如 Kinect，具有价格低廉、信息丰富、更 新速度快、能直接获取图像深度等优点，因此其 成为了 RGB-D SLAM 系统的主流视觉传感器，受 到广大视觉 SLAM 研究者的青睐。因此，国内外 学者对 Kinect 在室内移动机器人上的应用研究取 得了一定的研究成果。 一个完整的 RGB-D SLAM 系统应该包含传 感器数据、视觉里程计、非线性优化、建图与回环 检测等主要 5 个部分。视觉里程计的作用是估计 相机运动，其中包括特征点检测与提取、相邻帧 间配准、迭代最近点等。非线性优化是从带有噪 声的传感器数据中优化机器人运行轨迹和在运行 过程中增量式构建环境地图。回环检测[3]是机器 人通过识别图像间的相似性来判断是否回到之前 到过的位置。在视觉里程计部分，通常采 用 ICP 算法去估计相机的运动。然而传统的 ICP 算 法 [4]搜寻对应点对相当耗时，且容易出现大量误 匹配点对。针对以上问题，许多学者提出了优秀 的改进 ICP 算法。文献[5]针对多场景三维重建问 题，在基于 2D 局部特征匹配的基础上，提出了一 种新颖且高效的 3D 配准方法。该方法将三维点 云转换为 2D 轴承角图像，然后在两幅图像之间 利用 SURF 特征搜寻匹配的像素对，获取 3D 点云 的对应点，再通过基于 SVD 的方法求出最优旋转 矩阵，将点云与矩阵对齐来重建 3D 模型。该方 法虽大大减少了算法运算时间，但在精度上却不 如传统 ICP 算法。文献[6]针对服务机器人手眼协 调抓取提出了一种利用 RGB-D 传感器对家庭常 见物体进行 3-D 建模的方法。该方法仅需环绕物 体采取数十帧即可重建三维模型，但无法估计相 机位姿。 针对以上研究现状，本文提出了一种新的室 内移动机器人视觉 SLAM 方法。通过 Kinect 相 机获取室内环境信息，对获取的彩色图像进行 特征提取与匹配，结合标定后的 Kinect 内参与 像素点深度值，得到对应的空间对应点云，运用 RANSAC 方法筛选出可靠的内点进行初始位姿 估计完成初匹配。在精匹配过程中，初匹配的结 果作为精匹配的初值，通过添加点对间距离和角 度阈值双重限制，剔除误匹配点对，提高点云配 准的精度。在优化端的关键帧选取中引入权重， 通过回环检测消除累积误差，实现三维地图的优 化与更新。 1 基于 Kinect 的改进移动机器人 VSLAM 算法基本原理 1.1 系统结构 本文提出的室内移动机器人视觉 SLAM 系统 框架如图 1 所示，在进行传感器数据采集之前，需 对 Kinect 传感器进行标定以获取相机内部参数， 对获取到的彩色图像进行特征检测与提取，进行 初步筛选后，利用 RANSAC 算法剔除离群点获取 初始位姿估计，再用改进的 ICP 算法完成点云的 精准匹配，在关键帧选取中引入权重，通过回环检测 消除累积误差，实现三维点云地图的优化与更 新，最终获得机器人运行轨迹和室内环境点云地图。 彩色 图像 Kinect 相机 深度 信息 特征检测 与匹配 Kinect 标定 与点云生成 RANSAC 去除误匹配 改进的 ICP 完 成点云配准 位姿 估计 运行 轨迹 点云 地图 回环 检测 图 1 室内环境下基于移动机器人的 RGB-D SLAM 系统框架图 Fig. 1 The frame of RGB-D SLAM system for mobile robot in indoor environment 1.2 特征检测与匹配 SLAM 包含两个问题，一个是“定位”，一个是 “建图”。解决定位问题的关键是在相机获取了机 器人所在的室内环境信息后，如何根据相邻图像 的信息，粗略估计出相机运动，这一过程也被称 为视觉里程计。视觉里程计的实现方法主要分为 特征法和直接法。特征法运行稳定，对光照、动 态物体不敏感，是目前比较成熟的解决方案，因 此本文采用特征法。 随着在计算机视觉领域的长年探索，研究者 第 5 期 蔡军，等：基于 Kinect 的改进移动机器人视觉 SLAM ·735·
·736· 智能系统学报 第13卷 们提出了许多优秀的图像特征检测方法。如著名 1.4点云配准与位姿估计 的FASTI7、SIFT8以及SURF。FAST特征点检 1.4.1 RANSAC算法完成粗匹配 测计算速度快，但FAST不具有方向性。SURF算 ICP算法依赖良好的初值选取，如果初值选 法采用盒子滤波器，积分图像的运用减少了采用 取不合适，迭代易陷入局部最优，以至不能达到 盒子滤波进行卷积的时间，盒子滤波可以加速卷 全局最优效果。RANSAC是一种随机参数估计 积运算过程，在尺度空间建立上比SFT更迅速。 算法，通过随机抽选样本子集、计算模型参数、设 由于图像特征检测与匹配仅仅是整个SLAM系 定阈值等步骤获取最佳模型参数1。本文采用 统诸多环节中的一个，为了满足实时性，采用具 RANSAC算法对两组相邻RGB图的特征点进行 有旋转和尺度不变性的SURF特征。在提取了图 粗匹配，通过预先设定一个阈值d将全部匹配点 像特征以后，需确定两个点集元素的对应关系。 对区分为内点和外点，剔除大于此阈值的匹配点 最简单的方法是暴力匹配(brute-.force matcher, 对即剔除了外点对粗匹配的影响。将筛选后的内 BFM),然而当特征点数量很大时，暴力匹配法的 点进行最小二乘法下对Kinect相机的初始位姿估 运算量将变得很大，运行过程非常耗时，因此选 计，可将源点集A大致配准到目标点集B,粗匹配 用更适合于匹配点数量极多情况的快速近似最近 后的两片点云分别记为P和Q。相机在第i时刻 (fast library for approximate nearest neighbour, 的位姿P,与第i+1时刻的位姿P:的位姿转换关 FLANN)算法。 系为 利用SURF算法进行特征点检测与匹配的主 P PTi (5) 要过程如下： 1.4.2传统ICP算法 1)对图像关键点进行检测： 传统ICP算法通过解决最小二乘均方差问 2)使用“金字塔”构建尺度空间： 题，求解包含旋转矩阵R和平移矩阵t的刚体变 3)关键点定位； 换T,从而实现相机相邻位姿的运动估计。传统 4)确定每个关键点的主方向： ICP算法步骤如下。 5)生成特征描述子； 1)为源点集P中的每一点P,在目标点集Q中 6)运用FLANN对特征描述子进行匹配。 搜索其最近邻点q,从而构成了两组一一对应的 1.3 Kinect标定与点云生成 配对点云，分别记为P和Q': 彩色图像和深度图像需结合Kinect内参生成 P={p,p2,…,Pal (6) g={q1,92,…,9n} (7) 三维点云。Kinect相机在出厂之后具有固定的内 2)通过最小化均方目标函数来求解刚体变 参，但这些参数会随着温度、湿度、气压等环境因 换T。 素的变化而改变，在使用前有必要进行相机标 定。本文采用文献12]的方法对Kinect进行标 d.=2,-Rn+ (8) 定。在相机完成标定之后，图像中任意一点可结 n r=R t3xI 合该点深度值得到其对应的三维空间点坐标，进 0 1 (9) 而生成三维点云数据。二维图像中的像素点（其 式中：R为旋转矩阵，t为平移矩阵。 中d指深度数据)与其空间点的对应关系为 3)运用刚体变换转换点云。 v=6 +Cy (1) Pat1 =Rm+i Pm+tm+l (10) u=f 式中m表示迭代次数。 +Cx (2) 4)当满足dm-dm+1<ε时停止迭代，否则回到 d=zs (3) 步骤1)中，其中ε为大于零的阈值。 式中：f和f分别表示深度相机在x、y轴上的焦距； 1.4.3改进的ICP算法完成精匹配 c,和c,表示相机的光圈中心；s表示深度图的缩放 通过RANSAC算法进行特征点的粗匹配可 因子，即深度图数据与实际距离的比例，由于深 粗略地重合相邻两帧点云。传统的ICP算法是基 度图中都是以毫米为单位，因此通常为1000。通 于两组点云完全重合这一假设理论，然而在实际 常情况下、f、c、c,被定义为相机的内参矩阵C。 应用中点云P中的一点并不能总在点云Q中找 到其对应点，传统ICP算法存在大量错误匹配点 (4) 对，算法易陷人局部最优，甚至不能收敛。为了 减少配准误差，提高点云配准精度，需要利用改
们提出了许多优秀的图像特征检测方法。如著名 的 FAST[7] 、SIFT[8]以及 SURF[9]。FAST 特征点检 测计算速度快，但 FAST 不具有方向性。SURF 算 法采用盒子滤波器，积分图像的运用减少了采用 盒子滤波进行卷积的时间，盒子滤波可以加速卷 积运算过程，在尺度空间建立上比 SIFT 更迅速。 由于图像特征检测与匹配仅仅是整个 SLAM 系 统诸多环节中的一个，为了满足实时性，采用具 有旋转和尺度不变性的 SURF 特征。在提取了图 像特征以后，需确定两个点集元素的对应关系。 最简单的方法是暴力匹配 (brute-force matcher， BFM)，然而当特征点数量很大时，暴力匹配法的 运算量将变得很大，运行过程非常耗时，因此选 用更适合于匹配点数量极多情况的快速近似最近 邻 [10] (fast library for approximate nearest neighbour， FLANN) 算法。 利用 SURF 算法进行特征点检测与匹配的主 要过程如下： 1) 对图像关键点进行检测； 2) 使用“金字塔”构建尺度空间； 3) 关键点定位； 4) 确定每个关键点的主方向； 5) 生成特征描述子； 6) 运用 FLANN 对特征描述子进行匹配。 1.3 Kinect 标定与点云生成 彩色图像和深度图像需结合 Kinect 内参生成 三维点云。Kinect 相机在出厂之后具有固定的内 参，但这些参数会随着温度、湿度、气压等环境因 素的变化而改变，在使用前有必要进行相机标 定 [11]。本文采用文献[12]的方法对 Kinect 进行标 定。在相机完成标定之后，图像中任意一点可结 合该点深度值得到其对应的三维空间点坐标，进 而生成三维点云数据。二维图像中的像素点 (其 中 d 指深度数据) 与其空间点的对应关系为 v = y · fy z +cy (1) u = x · fx z +cx (2) d = z·s (3) fx fy x y cx cy s fx fy cx cy C 式中： 和 分别表示深度相机在 、 轴上的焦距； 和 表示相机的光圈中心； 表示深度图的缩放 因子，即深度图数据与实际距离的比例，由于深 度图中都是以毫米为单位，因此通常为 1 000。通 常情况下 、 、 、 被定义为相机的内参矩阵 。 C = fx 0 0 0 fy 0 cx cy 1 (4) 1.4 点云配准与位姿估计 1.4.1 RANSAC 算法完成粗匹配 A B P Q i Pi i+1 Pi+1 ICP 算法依赖良好的初值选取，如果初值选 取不合适，迭代易陷入局部最优，以至不能达到 全局最优效果。RANSAC 是一种随机参数估计 算法，通过随机抽选样本子集、计算模型参数、设 定阈值等步骤获取最佳模型参数[13]。本文采用 RANSAC 算法对两组相邻 RGB 图的特征点进行 粗匹配，通过预先设定一个阈值 d 将全部匹配点 对区分为内点和外点，剔除大于此阈值的匹配点 对即剔除了外点对粗匹配的影响。将筛选后的内 点进行最小二乘法下对 Kinect 相机的初始位姿估 计，可将源点集 大致配准到目标点集 ，粗匹配 后的两片点云分别记为 和 。相机在第 时刻 的位姿 与第 时刻的位姿 的位姿转换关 系为 Pi+1 = PiT i+1 i (5) 1.4.2 传统 ICP 算法 传统 ICP 算法通过解决最小二乘均方差问 题，求解包含旋转矩阵 R 和平移矩阵 t 的刚体变 换 T，从而实现相机相邻位姿的运动估计。传统 ICP 算法步骤如下。 P pi Q qj P ′ Q ′ 1) 为源点集 中的每一点 ，在目标点集 中 搜索其最近邻点 ，从而构成了两组一一对应的 配对点云，分别记为 和 ： P ′ = {p1 , p2 ,··· , pn} (6) Q ′ = {q1 ,q2 ,··· ,qn} (7) 2) 通过最小化均方目标函数来求解刚体变 换 T。 dm = 1 n ∑n i=1 qj −(Rpi + t) 2 (8) T = [ R3×3 0 t3×1 1 ] (9) 式中：R 为旋转矩阵，t 为平移矩阵。 3) 运用刚体变换转换点云。 Pm+1 = Rm+1Pm + tm+1 (10) 式中 m 表示迭代次数。 dm −dm+1 < ε ε 4) 当满足 时停止迭代，否则回到 步骤 1) 中，其中 为大于零的阈值。 1.4.3 改进的 ICP 算法完成精匹配 通过 RANSAC 算法进行特征点的粗匹配可 粗略地重合相邻两帧点云。传统的 ICP 算法是基 于两组点云完全重合这一假设理论，然而在实际 应用中点云 P 中的一点并不能总在点云 Q 中找 到其对应点，传统 ICP 算法存在大量错误匹配点 对，算法易陷入局部最优，甚至不能收敛。为了 减少配准误差，提高点云配准精度，需要利用改 ·736· 智 能 系 统 学 报 第 13 卷
第5期 蔡军，等：基于Kinect的改进移动机器人视觉SLAM ·737· 进的ICP算法对点云进行精确配准。在精匹配过 匹配点对近似法向量的夹角；为经欧氏距离去 程中，粗匹配的结果作为精匹配的初值，采用点 噪后的点对与所在同一坐标系原点O的夹角； 对间欧氏距离阈值法和角度阈值法对误匹配点对 w和π为设定的夹角阈值。 进行剔除，以筛选出满足阈值条件的点对，进行 1.44改进后的ICP算法步骤 精确相机位姿估计。所提阈值法原理如图2所示。 1)在点集P中选择初始点集Po 2)在目标点集Q中搜索与点集P距离最近的 点集，使用欧氏距离阈值法剔除噪声点，得到配 对点云P0与Q0 3)应用角度阈值法进一步剔除误匹配点对， 得到精配对点集P和Qao 4)采用奇异值分解(singular value decomposi- tion,SVD)求得点集Pa和Q之间的旋转矩阵R与 平移矩阵t。 5)根据式P2=RP1+t计算点集Pu经一次刚体 变换后得到的数据点集P2。 图2欧氏距离阈值法及角度阈值法原理示意图 6)重复35)，直到满足式(15)： Fig.2 Euclidean distance threshold method and angle (dm-dm+I< threshold method (15) 1)欧氏距离阈值 n 正确的匹配点对，两点对之间的欧氏距离不 1.5三维地图的更新与优化 应过大。本文添加了点对间欧氏距离阈值限制， 1.5.1关键帧选取 去除小于平均点对距离的匹配，且认为同一片点 Kinect相机以一定的帧率采集室内环境信 云下任一点与其邻点的拓扑结构不会随刚体变换 息，即使相机固定不动，程序的内存占用也会越 而变化。因此在粗匹配后，配对点云P中的任一 来越高。在RGB-D SLAM系统中，过多的关键帧 点与其邻点，在另一个配对点云Q中的也应是邻 会增加闭环检测与全局优化的计算耗时，过少的 近点，给出了式(11)和式(12)两个距离阈值限制： 关键帧会导致关键帧间间隙增大，造成帧间配准 p:-q<u (11) 易出现失败的情况。为了满足帧间配准的成功率 和系统实时性要求，引入一种关键帧选取机制。 IlP:-Pineill -llg;-giell (12) 在关键帧选取上，旋转的变化比平移更加敏感， P:-Pineill +lggneill 因此在旋转出现微小变化或平移相对大一段距离 式中：p,与pe分别表示配对点云P中的任意一点 后添加关键帧。当新帧到来时，检测其图像特征 与其邻点；q,与qme分别表示p,与pn在另一配对点 并与前一关键帧进行变换计算，如果变换超过阈 云Q中对应的点；表示粗匹配后匹配点对间的平 值，则添加关键帧，反之，则丢弃。具体选取标 均欧氏距离；6表示距离阈值。经过欧氏距离阈值 准如式(16)所示： 法可剔除大部分噪声点，提高点云初值选取的成 p=[△x△y△l2+d2l(a,B,yl2 (16) 功率。 式中：[△x△y△z为平移向量；(a,B,y)为相对欧拉 2)角度阈值 角；入为平移权重参数；2为欧拉角权重参数。 经过欧氏距离阈值法可剔除大部分点云数据 1.5.2位姿优化 噪声点，再结合角度阈值，能进一步检测点对匹配 在进行机器人同时定位与地图构建过程中， 的正确性，提高点云初值选取的正确率。本文采 两帧之间的位姿变换可通过式（⑤）计算得到，各 用了点到切平面的最近搜索算法，对于给定点P 时刻相对于起始时刻的变换矩阵可通过式(17) 及其对应点q,通过其邻近点集拟合平面，并求出各 获得： 自的近似法向量。提出的角度阈值限制为 T=T×T×…×T- (17) cos0=nxn; 在各个位姿转换过程中均存在误差，如果不 Inlxn (13) 进行优化，随着转换过程的叠加，会产生累计误 sino-sin<T (14) 差。本文采用g20优化算法对位姿转换过程中 式中：n,和n,分别为点p,和q的近似法向量；0为两 产生的累计误差进行优化，误差函数F(x)定义为
进的 ICP 算法对点云进行精确配准。在精匹配过 程中，粗匹配的结果作为精匹配的初值，采用点 对间欧氏距离阈值法和角度阈值法对误匹配点对 进行剔除，以筛选出满足阈值条件的点对，进行 精确相机位姿估计。所提阈值法原理如图 2 所示。 ni nj pi pinei qj qjnei φ θ nj y x O z 图 2 欧氏距离阈值法及角度阈值法原理示意图 Fig. 2 Euclidean distance threshold method and angle threshold method 1) 欧氏距离阈值 P Q 正确的匹配点对，两点对之间的欧氏距离不 应过大。本文添加了点对间欧氏距离阈值限制， 去除小于平均点对距离的匹配，且认为同一片点 云下任一点与其邻点的拓扑结构不会随刚体变换 而变化。因此在粗匹配后，配对点云 中的任一 点与其邻点，在另一个配对点云 中的也应是邻 近点，给出了式 (11) 和式 (12) 两个距离阈值限制： pi −qj ω (13) |sinφ−sinθ| < τ (14) 式中： ni和 nj 分别为点 pi和 qj 的近似法向量； θ 为两 φ ω τ 匹配点对近似法向量的夹角； 为经欧氏距离去 噪后的点对与所在同一坐标系原点 O 的夹角； 和 为设定的夹角阈值。 1.4.4 改进后的 ICP 算法步骤 1) 在点集 P 中选择初始点集 Pi0。 Q Pi0 Pi0 Qi0 2) 在目标点集 中搜索与点集 距离最近的 点集，使用欧氏距离阈值法剔除噪声点，得到配 对点云 与 。 Pi1 Qi1 3) 应用角度阈值法进一步剔除误匹配点对， 得到精配对点集 和 。 Pi1 Qi1 4) 采用奇异值分解 (singular value decomposition，SVD) 求得点集 和 之间的旋转矩阵 R 与 平移矩阵 t。 Pi2 = RPi1 + t Pi1 Pi2 5) 根据式 计算点集 经一次刚体 变换后得到的数据点集 。 6) 重复 3)~5)，直到满足式 (15)： dm −dm+1 < ε dm = 1 n ∑n i=1 ∥Qim − Pim∥ 2 (15) 1.5 三维地图的更新与优化 1.5.1 关键帧选取 φ Kinect 相机以一定的帧率采集室内环境信 息，即使相机固定不动，程序的内存占用也会越 来越高。在 RGB-D SLAM 系统中，过多的关键帧 会增加闭环检测与全局优化的计算耗时，过少的 关键帧会导致关键帧间间隙增大，造成帧间配准 易出现失败的情况。为了满足帧间配准的成功率 和系统实时性要求，引入一种关键帧选取机制。 在关键帧选取上，旋转的变化比平移更加敏感， 因此在旋转出现微小变化或平移相对大一段距离 后添加关键帧。当新帧到来时，检测其图像特征 并与前一关键帧进行变换计算，如果变换超过阈 值 ，则添加关键帧，反之，则丢弃。具体选取标 准如式 (16) 所示： φ = λ1 [ ∆x ∆y ∆z ]T 2 +λ2∥(α, β, γ)∥2 (16) [ ∆x ∆y ∆z ] (α, β, γ) λ1 λ2 式中： 为平移向量； 为相对欧拉 角； 为平移权重参数； 为欧拉角权重参数。 1.5.2 位姿优化 在进行机器人同时定位与地图构建过程中， 两帧之间的位姿变换可通过式 (5) 计算得到，各 时刻相对于起始时刻的变换矩阵可通过式 (17) 获得： T k 0 = T 1 0 ×T 2 1 × ··· ×T k (k−1) (17) F (x) 在各个位姿转换过程中均存在误差，如果不 进行优化，随着转换过程的叠加，会产生累计误 差。本文采用 g2o 优化算法[14]对位姿转换过程中 产生的累计误差进行优化，误差函数 定义为 第 5 期 蔡军，等：基于 Kinect 的改进移动机器人视觉 SLAM ·737·
·738· 智能系统学报 第13卷 F(x)= e'2e》 (18) 机获取到的第二帧数据，两帧数据的视角只在水 (i.j)ec 平方向发生了较小的平移。图5为利用标定后 x'=argminF(x) (19) 的Kinect相机获取到的第一帧彩色信息，结合像 式中：x表示第i时刻的机器人位姿；表示x和 素点深度值生成的彩色点云数据。图6(a)为未 x之间构成的约束；2表示x和x之间的信息矩 经RANSAC算法的匹配点对，两帧数据只在水平 阵；e(c,x)表示x,和x之间的误差函数，误差函 方向上发生了移动，因此只有水平的匹配线才是 数越小，表示：和x满足约束条件的程度越高。 正确的匹配。图6(b)为经RANSAC算法后的匹 2实验结果及分析 配点对，可以看出，剔除误匹配后，基本消除了噪 声对匹配结果的影响，提高了点对匹配正确率。 本文实验平台由3个主要部分组成：第一部 分为一台Intel双核4.0GHz主频的笔记本电脑， 装配以Linux为内核的Ubuntu14.04操作系统：第 二部分为Kinect XBOX360,图像分辨率为 640×480,最高帧率为30帧/秒，水平视场角为 52°；第三部分为Pioneer.3-DX机器人。实验环境 (a)第一帧数据 为一间约80m2的小型实验室，图3(a)为实验真 实场景，图3(b)为实验设备。 (b)第二帧数据 图4室内环境的两帧数据 Fig.4 Two frames in indoor environment (a)实验场景 b)实验设备 图3实验场景及实验设备 Fig.3 Experimental scene and experimental installation 在实验过程中，Pioneer3.-DX机器人搭载笔记 本电脑与Kinect相机，并以0.1m/s的速度在实验 室内移动，Kinect相机通过获取室内环境的彩色 信息与深度信息，利用关键帧的选取机制，对新 图5第一帧数据的三维彩色点云图 来的关键帧进行图像特征检测与提取，结合相机 Fig.5 3-D color point cloud of the first frame data 标定内参与像素点深度值，生成三维点云数据。 采用改进的基于RANSAC的ICP点云配准算法 将两片相邻点云进行配准，获取相机位姿估计。 实验过程中各参数设置为：f=518.0,f=519.0, cx=325.5,c,=253.5,s=1000,d=5.00cm,6=0.05, (a)未经RANSAC算法的特征点对匹配图 w=0.95,t=0.05。不同真实场景采集图像不同， 提取的特征点数量和点对间距离也不同，因此为 不定值。点云分辨率设为0.02，最大迭代次数为 100次，内点数量阈值为N=13,对每个关键帧完 成特征点匹配、误匹配剔除以及地图更新的时间 约为200ms。 (b)经RANSAC算法别除误匹配点对后的匹配图 在实验场景中，图4(a)为相机获取到的第一 图6粗匹配实验结果 帧数据（包含彩色信息和深度信息），图4(b)为相 Fig.6 The experimental results of coarse matching
F (x) = ∑ (i, j)∈C e T ( xi , xj ,zi j) Ωi je ( xi , xj ,zi j) (18) x ∗ = argmin x F (x) (19) xi i zi j xi xj Ωi j xi xj e ( xi , xj ,zi j) xi xj xi xj zi j 式中： 表示第 时刻的机器人位姿； 表示 和 之间构成的约束； 表示 和 之间的信息矩 阵； 表示 和 之间的误差函数，误差函 数越小，表示 和 满足约束条件 的程度越高。 2 实验结果及分析 本文实验平台由 3 个主要部分组成：第一部 分为一台 Intel 双核 4.0 GHz 主频的笔记本电脑， 装配以 Linux 为内核的 Ubuntu 14.04 操作系统；第 二部分 为 Kinect XBOX360 ，图像分辨率 为 640×480，最高帧率为 30 帧/秒，水平视场角为 52°；第三部分为 Pioneer3-DX 机器人。实验环境 为一间约 80 m2 的小型实验室，图 3(a) 为实验真 实场景，图 3(b) 为实验设备。 (a) 实验场景 (b) 实验设备 图 3 实验场景及实验设备 Fig. 3 Experimental scene and experimental installation fx = 518.0 fy = 519.0 cx = 325.5 cy = 253.5 s = 1 000 d = 5.00 δ = 0.05 ω = 0.95 τ = 0.05 u N = 13 200 ms 在实验过程中，Pioneer3-DX 机器人搭载笔记 本电脑与 Kinect 相机，并以 0.1 m/s 的速度在实验 室内移动，Kinect 相机通过获取室内环境的彩色 信息与深度信息，利用关键帧的选取机制，对新 来的关键帧进行图像特征检测与提取，结合相机 标定内参与像素点深度值，生成三维点云数据。 采用改进的基于 RANSAC 的 ICP 点云配准算法 将两片相邻点云进行配准，获取相机位姿估计。 实验过程中各参数设置为： ， ， ， ， ， cm， ， ， 。不同真实场景采集图像不同， 提取的特征点数量和点对间距离也不同，因此 为 不定值。点云分辨率设为 0.02，最大迭代次数为 100 次，内点数量阈值为 ，对每个关键帧完 成特征点匹配、误匹配剔除以及地图更新的时间 约为 。 在实验场景中，图 4(a) 为相机获取到的第一 帧数据 (包含彩色信息和深度信息)，图 4(b) 为相 机获取到的第二帧数据，两帧数据的视角只在水 平方向发生了较小的平移。图 5 为利用标定后 的 Kinect 相机获取到的第一帧彩色信息，结合像 素点深度值生成的彩色点云数据。图 6(a) 为未 经 RANSAC 算法的匹配点对，两帧数据只在水平 方向上发生了移动，因此只有水平的匹配线才是 正确的匹配。图 6(b) 为经 RANSAC 算法后的匹 配点对，可以看出，剔除误匹配后，基本消除了噪 声对匹配结果的影响，提高了点对匹配正确率。 (a) 第一帧数据 (b) 第二帧数据 图 4 室内环境的两帧数据 Fig. 4 Two frames in indoor environment 图 5 第一帧数据的三维彩色点云图 Fig. 5 3-D color point cloud of the first frame data (a) 未经 RANSAC 算法的特征点对匹配图 (b) 经 RANSAC 算法剔除误匹配点对后的匹配图 图 6 粗匹配实验结果 Fig. 6 The experimental results of coarse matching ·738· 智 能 系 统 学 报 第 13 卷
第5期 蔡军，等：基于Kinect的改进移动机器人视觉SLAM ·739· 图7(a)为采用文献[6]的方法进行三维重建的 0.5 0 效果图，所重构出的点云图中的货物架、AGV小 -0.5 车以及墙板等物体存在轮廓不清晰的情况，左边 -1.0 窗帘冗余点较多。图7()为采用本文改进后的 ICP算法进行三维重建的效果图。相比之下，利 426 -2.5 用本文算法得到的点云图精度明显提高，货物 -3.0 架、AGV小车以及墙板等物体轮廓变得清晰，窗 -3.5 帘上的冗余点明显得到减少。图8为采用g20算 -4.0 -1 2 法对地图进行优化与更新，深蓝部分为关键帧， x/m 其顶点表示相机的各个姿态，连接两顶点的边即 图9机器人运行轨迹估计 表示两点之间的位姿变换。局部回环表示当前帧 Fig.9 Robot trajectory estimation 与就近m帧间的匹配，大回环表示当前帧与关键 帧序列中随机n帧的匹配。本文中设置m=5,n= 3结束语 5。图9计算机记录机器人移动的位姿数据后，利 本文提出了一种新的基于Kinect的室内移动 用MATLAB仿真生成的机器人运行轨迹估计。 机器人同时定位与地图构建系统。针对相机位姿 估计过程中，点云配准精度不高，易陷入局部最 优等问题，提出了一种新的欧氏距离与角度阈值 的双重限制法，提高了点云配准精度，减少了配 准误差。结合关键帧选取机制和g20优化算法， 实现了三维地图的优化与更新。在真实室内环境 论廊不清晰 下进行在线实验，实验结果证明了本文算法的有 效性与可行性。进一步改进工作将集中在：1)进 一步研究欧氏距离与角度阈值的选取标准；2)提 (a)文献[6算法三维重建效果 出移动机器人在同一环境下持续运动所产生的关 键帧冗余的情况的解决方案。 参考文献： [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]KERL C.STURM J.CREMERS D.Dense visual SLAM for RGB-D cameras[C]//Proceedings of 2013 IEEE/RSJ (b)本文算法三维重建效果 International Conference on Intelligent Robots and Sys- 图7室内环境三维重建结果 tems.Tokyo,Japan,2014:2100-2106. Fig.7 The experimental results of 3D reconstruction in in- [3]HOU Yi,ZHANG Hong,ZHOU Shilin.Convolutional door environment neural network-based image representation for visual loop closure detection[C]//Proceedings of 2015 IEEE Interna- tional Conference on Information and Automation.Lijiang. 关键帧 局部回环 China,2015:2238-2245 [4]解则晓，徐尚.三维点云数据拼接中ICP及其改进算法 大回环 综述.中国海洋大学学报：自然科学版，2010,40(1)： 99-103 XIE Zexiao,XU Shang.A survey on the ICP algorithm and its variants in registration of 3D point clouds[J].Peri- odical of ocean university of China,2010,40(1):99-103. 图8g20优化图 [5]LIN CC,TAI Y C,LEE JJ,et al.A novel point cloud re- Fig.8 g2o optimization diagram gistration using 2D image features[J].Eurasip journal on
m n m = 5 图 7(a) 为采用文献[6]的方法进行三维重建的 效果图，所重构出的点云图中的货物架、AGV 小 车以及墙板等物体存在轮廓不清晰的情况，左边 窗帘冗余点较多。图 7(b) 为采用本文改进后的 ICP 算法进行三维重建的效果图。相比之下，利 用本文算法得到的点云图精度明显提高，货物 架、AGV 小车以及墙板等物体轮廓变得清晰，窗 帘上的冗余点明显得到减少。图 8 为采用 g2o 算 法对地图进行优化与更新，深蓝部分为关键帧， 其顶点表示相机的各个姿态，连接两顶点的边即 表示两点之间的位姿变换。局部回环表示当前帧 与就近 帧间的匹配，大回环表示当前帧与关键 帧序列中随机 帧的匹配。本文中设置 ，n = 5。图 9 计算机记录机器人移动的位姿数据后，利 用 MATLAB 仿真生成的机器人运行轨迹估计。 (a) 文献 [6] 算法三维重建效果 (b) 本文算法三维重建效果 物体轮廓不清晰 冗余点较多 图 7 室内环境三维重建结果 Fig. 7 The experimental results of 3D reconstruction in indoor environment 大回环 关键帧 局部回环 图 8 g2o 优化图 Fig. 8 g2o optimization diagram −1 0 1 2 3 4 −4.0 −3.5 −3.0 −2.5 −2.0 −1.5 −1.0 −0.5 0 0.5 x/m y/m 图 9 机器人运行轨迹估计 Fig. 9 Robot trajectory estimation 3 结束语 本文提出了一种新的基于 Kinect 的室内移动 机器人同时定位与地图构建系统。针对相机位姿 估计过程中，点云配准精度不高，易陷入局部最 优等问题，提出了一种新的欧氏距离与角度阈值 的双重限制法，提高了点云配准精度，减少了配 准误差。结合关键帧选取机制和 g2o 优化算法， 实现了三维地图的优化与更新。在真实室内环境 下进行在线实验，实验结果证明了本文算法的有 效性与可行性。进一步改进工作将集中在：1) 进 一步研究欧氏距离与角度阈值的选取标准；2) 提 出移动机器人在同一环境下持续运动所产生的关 键帧冗余的情况的解决方案。 参考文献： 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. [1] KERL C, STURM J, CREMERS D. Dense visual SLAM for RGB-D cameras[C]//Proceedings of 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo, Japan, 2014: 2100–2106. [2] HOU Yi, ZHANG Hong, ZHOU Shilin. Convolutional neural network-based image representation for visual loop closure detection[C]//Proceedings of 2015 IEEE International Conference on Information and Automation. Lijiang, China, 2015: 2238–2245. [3] 解则晓, 徐尚. 三维点云数据拼接中 ICP 及其改进算法 综述[J]. 中国海洋大学学报: 自然科学版, 2010, 40(1): 99–103. XIE Zexiao, XU Shang. A survey on the ICP algorithm and its variants in registration of 3D point clouds[J]. Periodical of ocean university of China, 2010, 40(1): 99–103. [4] LIN C C, TAI Y C, LEE J J, et al. A novel point cloud registration using 2D image features[J]. Eurasip journal on [5] 第 5 期 蔡军，等：基于 Kinect 的改进移动机器人视觉 SLAM ·739·
·740· 智能系统学报 第13卷 advances in signal processing,2017,2017:5. [14]KUMMERLE R,GRISETTI G,STRASDAT H,et al. [6]杨扬，曹其新，朱笑笑，等.面向机器人手眼协调抓取的 G20:a general framework for graph optimization[C]//Pro- 3维建模方法[).机器人，2013,35(2：151-155. ceedings of 2011 IEEE International Conference on Ro- YANG Yang,CAO Qixin,ZHU Xiaoxiao,et al.A 3D botics and Automation.Shanghai,China,2011: modeling method for robot's hand-eye coordinated grasp- 3607-3613 ing[J.Robot,.2013,35(2:151-155. [7]ROSTEN E.PORTER R.DRUMMOND T.Faster and bet- 作者简介： ter:a machine learning approach to corner detection[J]. 蔡军，男，1977年生，副教授，主 IEEE transactions on pattern analysis and machine intelli- 要研究方向为机器人技术、信号处理 gence,2010,321):105-119. 模式识别。发表学术论文9篇，主编 和参编教材5部。 [8]LOWE D G.Distinctive image features from scale-invari- ant keypoints[J].International journal of computer vision, 2004,60(2:91-110. [9]BAY H,ESS A,TUYTELAARS T,et al.speeded-up ro- bust features(SURF)[J].Computer vision and image un- 陈科宇，男.1992年生，硕士研究 derstanding,2008,110(3):346-359. 生，主要研究方向为机器人同时定位 与地图构建、机器人三维视觉导航。 [10]MUJA M.LOWE D G.Scalable nearest neighbor al- gorithms for high dimensional data[J].IEEE transactions on pattern analysis and machine intelligence,2014, 36(11):2227-2240. [lI]陈胜勇，刘盛.基于OpenCV的计算机视觉技术实现 张毅.男，1966年生，教授，博士 [M.北京：科学出版社，2008. 生导师，中国人工智能学会理事，主要 [12]DANIEL H C,KANNALA J,HEIKKILA J.Joint depth 研究方向为智能系统与移动机器人、 and color camera calibration with distortion correction[J]. 机器视觉与模式识别、多传感器信息 IEEE transactions on pattern analysis and machine intelli- 融合。主持完成国家级和省部级基金 gence,2012,34(10:2058-2064. 项目10余项，获国家发明专利10余 [13]朱德海，郭浩，苏伟.点云库PCL学习教程M.北京： 项。发表学术论文160余篇，被SCI 北京航空航天大学出版社，2012 EI检索80余篇
advances in signal processing, 2017, 2017: 5. 杨扬, 曹其新, 朱笑笑, 等. 面向机器人手眼协调抓取的 3 维建模方法[J]. 机器人, 2013, 35(2): 151–155. YANG Yang, CAO Qixin, ZHU Xiaoxiao, et al. A 3D modeling method for robot’s hand-eye coordinated grasping[J]. Robot, 2013, 35(2): 151–155. [6] ROSTEN E, PORTER R, DRUMMOND T. Faster and better: a machine learning approach to corner detection[J]. IEEE transactions on pattern analysis and machine intelligence, 2010, 32(1): 105–119. [7] LOWE D G. Distinctive image features from scale-invariant keypoints[J]. International journal of computer vision, 2004, 60(2): 91–110. [8] BAY H, ESS A, TUYTELAARS T, et al. speeded-up robust features (SURF)[J]. Computer vision and image understanding, 2008, 110(3): 346–359. [9] MUJA M, LOWE D G. Scalable nearest neighbor algorithms for high dimensional data[J]. IEEE transactions on pattern analysis and machine intelligence, 2014, 36(11): 2227–2240. [10] 陈胜勇, 刘盛. 基于 OpenCV 的计算机视觉技术实现 [M]. 北京: 科学出版社, 2008. [11] DANIEL H C, KANNALA J, HEIKKILÄ J. Joint depth and color camera calibration with distortion correction[J]. IEEE transactions on pattern analysis and machine intelligence, 2012, 34(10): 2058–2064. [12] 朱德海, 郭浩, 苏伟. 点云库 PCL 学习教程[M]. 北京: 北京航空航天大学出版社, 2012. [13] KUMMERLE R, GRISETTI G, STRASDAT H, et al. G2o: a general framework for graph optimization[C]//Proceedings of 2011 IEEE International Conference on Robotics and Automation. Shanghai, China, 2011: 3607–3613. [14] 作者简介： 蔡军，男，1977 年生，副教授，主 要研究方向为机器人技术、信号处理、 模式识别。发表学术论文 9 篇，主编 和参编教材 5 部。 陈科宇，男，1992 年生，硕士研究 生，主要研究方向为机器人同时定位 与地图构建、机器人三维视觉导航。 张毅，男，1966 年生，教授，博士 生导师，中国人工智能学会理事，主要 研究方向为智能系统与移动机器人、 机器视觉与模式识别、多传感器信息 融合。主持完成国家级和省部级基金 项目 10 余项，获国家发明专利 10 余 项。发表学术论文 160 余篇，被 SCI、 EI 检索 80 余篇。 ·740· 智 能 系 统 学 报 第 13 卷