,
2. 重庆邮电大学 光电工程学院, 重庆 400065
,
2. College of Electroning Engineering College, Chongqing University of Posts and Telecommunications, Chongqing 400065, China
近年来，移动机器人同时定位与地图构建已经成为机器人领域研究的热点[1]。过去，国内外学者对移动机器人二维SLAM进行了大量研究,然而在二维SLAM中丢失了很多有价值的形状和几何信息。三维SLAM能够充分利用环境三维属性，精确描述出局部环境形状以及物体的几何外形，因而移动机器人三维SLAM对于全面实现移动机器人精确导航具有重要意义。在三维环境建模中最常用的方式是利用范围传感器获得的深度点云来表示环境模型[2]。然而，这种方法大多利用精度高、价格昂贵的三维激光扫描器。Kinect 具有廉价、方便的特点[3]。近年来，将Kinect应用于移动机器人三维SLAM已成为机器人领域研究的新热点，并取得了大量研究成果。文献[4]提出一种室内环境下的三维环境建模方法，基于GPU编程的SIFT算法提高了系统的运算效率，且能实现三维环境的建立，但效果欠佳且无法进行位姿估计。Henry[5]等人提出了一个基于Kinect的交互式的三维模型重建系统，但是该系统依靠关键帧的选取，计算量大、时间复杂度高，难以用于移动机器人三维地图在线创建。针对上述问题，本文提出了一种室内环境下移动机器人三维视觉SLAM方法。针对点云配准过程存在的匹配误差较大、效率低下的问题，提出一种分层式点云配准策略。同时引入基于体积融合的匹配算法完成精确配准，有效的去除了拼接结果中的重复点，采用了g20优化方法对机器人位姿进行优化，实验验证了本文方法的可行性和有效性。
1 系统结构本文提出的室内环境下移动机器人三维视觉SLAM系统的总体框架如图 1所示，主要分为特征点检测与匹配、Kinect标定与点云生成、分层式点云配准3部分。
 |
| 图 1 室内环境下移动机器人三维视觉SLAM总体框架图Fig. 1 Total frame of three dimensional visual SLAM for mobile robot in indoor environment |
经典的特征点检测算法包括Harris[6]、SIFT[7]、SURF[8]算法等。经综合考虑为获得鲁棒性、稳定性更好的特征点检测结果，本文采用具有旋转和尺度不变性且速度较快的SURF算法。SURF特征点提取过程主要包括特征点检测、特征点主方向选取、生成特征描述子。判断基于描述的特征点是否匹配，可通过计算欧式距离来实现，设匹配图像的特征点集合分别为 X和Y。 欧式距离可以描述为
欧式距离越小表示特征点相似程度越高，本文在匹配过程中采用了最大紧邻向量匹配方法，即通过比较特征点集合中的每一个特征点与特征点集合中各特征点的欧式距离，D1和D2分别代表最近和次近的欧式距离，若D1≤αD2(α为设定的最近次近距离比值，本文设定α=0.60)，则认为两个特征点匹配，反之丢弃该点。
3 标定与点云生成为获取Kinect的彩色与深度相机的内参数矩阵以及彩色与深度相机之间的刚体变换，减小深度图像与红外图像之间存在的漂移[9]，需要对Kinect进行标定，本文采用文献[10]标定方法对Kinect进行标定。在完成Kinect对准校正后，可根据图像中任意一点对应的深度值得到该点三维坐标，进而生成三维彩色点云数据。给定任意深度图像像素点(xd,yd)，投射到三维空间点p(x,y,z),其坐标计算如下：
本文采用RANSAC算法进行特征点的粗匹配，以使2组点集的公共区域能够大致重合，为下一步精确匹配做准备。算法设定一个容许误差将所有的匹配点对分为内点和外点。本文实验中设定的阈值d=3.00 舍去误差大于d的匹配点对(外点)。为了获得摄像机位姿的变化，本文采用了最小二乘逼近的方法[11]。利用算法所得内点进行最小二乘算法下机器人相邻位姿的估计。摄像机第i时刻的位姿 Pi 相对于第j时刻的摄像机位姿 Pj的转换关系为Pj=PiTij 其中 Tij 为刚性变换矩阵：
传统的ICP算法的基本原理利用最小二乘的优化思想通过计算使函数(7)中的R和T最小化。
本文采用了点到切平面的最近搜索算法。另外，在初值的选取上本文采用了阈值法，利用欧氏距离阈值和方向向量阈值，提高了初值的匹配准确度。欧氏距离阈值法可剔除噪声点，提高初值选择成功率。设有点云S和Q，对S中的一个点 si ，搜索Q中与 si 欧氏距离最近的3个点，分别为 q1 、 q2 和 q3 。若满足 si－qi≤T ，则为正确的匹配点，反之为误匹配点予以剔除。式中T为距离超出阈值，经过欧氏距离阈值可基本剔除点云数据的噪声点。然后结合方向向量夹角判断是否为对应点，提高初值选择的正确率。若 si 、 qi 为一对正确的匹配点，在满足式(7)的基础上还应当满足下式：
1) 在S中选择初始点集 si0 。
2) 使用欧氏距离阈值法在目标点集Q中求出与 si0 最近的点集qi0 、 si0 和 qi0 构成对应点集。
3)应用方向向量阈值法选定点集 si0 和 qi0 ，运算初值点集 si1 和 qi1 。
4) 应用SVD法求得点集 si1和 qi1 之间的旋转矩阵R和平移矢量T。
5) 根据式 si2=R1si1+T1 ，计算数据点集 si1 经过一次坐标变换后所得到的数据点集 si2 ，然后重复步骤3)～5)，直到满足式(9)：
传统ICP算法存在的一个问题是直接融合两帧点云会造成融合后的点云中出现冗余点。因此，本文引入了基于体积空间融合的匹配算法。算法主要思想是在对点云进行融合时，建立包含全部帧的一个立方体栅格来表示三维空间，以一定的分辨率来划分立方体为空间的像素点。每一个点记录了到物体模型表面的距离如图 2所示，使用正负来表示表面的被遮挡面和可见面，而过零点就代表是表面上的点。然后依次对每一帧数据进行处理，当有新的数据加入时由式(10)和式(11)进行处理，通过累加将各帧数据融合起来，最后空间体中的零值像素即为最后的曲面上的点。
 |
| 图 2 基于体积空间的点云融合Fig. 2 Point cloud integration based on space |
在位姿转换计算中通常会存在误差累计，为此采用了g20优化方法，误差函数F(x)定义如下：
本文实验平台分为数据采集端和处理端两部分，其中数据采集端(图 3(b)所示)主要由Pioneer3-DX机器人、Kinect和笔记本电脑组成，Kinect图像分辨率为640×480，最高帧频为30帧/s，水平视场角为52°。数据处理端处理器为一台Intel 双核2.0 GHz 主频的PC机,运行Ubuntu 12.04 Linux 操作系统。为保证较高的鲁棒性，实验中机器人移动速度v为0.1 m/s、转弯角速度w为0.52 rad/s(30°/s)对每个关键帧完成特征点提取、误匹配剔除和地图更新的时间约为300 ms。
 |
| 图 3 实验装置与实验场景Fig. 3 Experimental apparatus and experimental scene |
 |
| 图 4 环境三维彩色点云Fig. 4 Environment 3D color point cloud |
在利用特征匹配算法获得相邻帧间对应关系的基础上，利用RANSAC算法对点云进行粗匹配，粗匹配结果如图 5所示,由图可以看出，经过RANSAC算法进行粗匹配后能够将匹配过程中出现的错误匹配剔除，提高特征匹配的正确率，实验中完成粗匹配所耗时间为0.004 25 s。
 |
| 图 5 粗匹配实验结果Fig. 5 The experimental results of coarse matching |
机器人以0.1 m/s的线速度在室内环境下创建地图，实验过程中最大迭代次数为50，阈值设定为5 cm，内点数量阈值 N=13； 台式机处理采集到的图像帧，当特征点对少于50组时，将舍弃该帧数据，不进行初始配准。
图 6(a) 为采用文献[4]方法得到的三维环境重构结果，所构建的三维地图存在8冗余即8点较多、环境中物体轮廓模糊等问题。图 6(b) 为采用本文方法构建的三维环境重构结果，获得的三维地图较好的解决了上述问题，物体轮廓清晰，冗余点减少。
 |
| 图 6 室内环境三维重构结果Fig. 6 3D reconstruction result of the indoor environment |
为了获得相机位姿的变化，本文采用最小二乘逼近的方法对机器人相邻位姿进行估计，借助 g20 优化方法对估计位姿进行优化。为了使运动轨迹更为直观，实验给出了机器人运动轨迹在 X/Y 平面上的投影如图 7(a)所示，以及机器人在三维空间中的运动轨迹如图 7(b)所示。可以看出本文方法能较好的实现机器人的运动轨迹跟踪。
 |
| 图 7 机器人运动轨迹跟踪Fig. 7 Robot trajectory tracking |
本文提出的分层式点云配准策略，较好地解决了点云配准过程中匹配误差较大、效率低下的问题。在三维地图生成部分，引入了基于体积空间的多帧融合方法来减少冗余点。实验结果表明本文所提方法的有效性。下一步计划对系统进行两方面的改进：1)对创建的环境三维地图的全局优化。2)研究采用并行技术，进一步提高系统执行效率，缩短系统的运行时间，使系统具有更好的实时性。
| [1] | RYDE J, HU H S. 3D mapping with multi-resolution occupied voxel lists[J]. Autonomous Robots, 2010, 28(2):169-185. |
| [2] | KOPPILA H S, ANAND A. Semantic labeling of 3D point clouds for indoor scenes[C]//25th Annual Conference on Neural Information Processing Systems. New York, USA:Curran Associates Inc., 2011. |
| [3] | HERRERA C D, KANNALA J, HEIKKILA J. Joint depth and color camera calibration with distortion correction[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2012, 34(10):2058-2064. |
| [4] | 杨扬,曹其新,朱笑笑.面向机器人手眼协调抓取的3 维建模方法[J].机器人,2013,35(2): 151-155.YANG Yang,CAO Qixin,ZHU Xiaoxiao. A 3D modeling method for robot’s hand-eye coordinated grasping[J]. Robot,2013,35(2):151-155. |
| [5] | HENRY P, KRAININ M, HERBST E, et al. RGB-D mapping: Using Kinect-style depth cameras for dense 3D modeling of indoor environments[J]. International Journal of Robotics Research, 2012, 31(5): 647-663. |
| [6] | HARRIS C, STEPHENS M. A combined corner and edge detector[C]//Proc. Alvey Vis. Conf. ,1988: 147-151. |
| [7] | LOWE D G. Object recognition from local scale-invariant features [C]//International Conference on Computer Vision. Kerkyra,Corfu,Greece,1999:1150-1157. |
| [8] | BAY H, ESS A, TUYTELAARS T, et al. Speeded-up robust features (SURF) [J].Comput. Vis. Image Understanding, 2008,110:346-359. |
| [9] | KUMMERLE R, GRISETTI G, STRASDAT H, et al. G2o: A general framework for graph optimization [C]//IEEE International Conference on Robotics and Automation. Piscataway: IEEE,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 Electronics 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. |