汪兆永，何炳蔚

(福州大学 机械工程及自动化学院，福建 福州 350108)

0 引言

近年来，移动机器人技术迅速发展，在未知环境中实现自主导航是移动机器人的一项重要任务，自主导航也一直是机器人研究的热点问题。特别是在未知环境下的导航问题，机器人在快速自定位和获取周围环境信息方面仍然存在很大困难。学者们针对SLAM 领域展开了大量的研究，提出了许多研究方法。比较成熟的SLAM方法有基于激光传感器、声呐传感器等获取环境信息［1］，但是得到的信息量较小，所需探测环境时间较长，对环境条件要求较高，不适用于复杂的环境。而视觉传感器系统具有信息量大，性价比高等优点，随着计算机图像处理技术能力的提高，学者们开展了大量基于视觉传感器的SLAM 相关研究，也表明了视觉传感器具有很好的发展前景和使用价值。基于视觉传感器的自定位研究主要有单目视觉、双目立体视觉系统、全景视觉和单目全景混合视觉系统［2］，存在的主要问题是连续性不足、系统和处理算法较为复杂。

2010 年微软公司开发的Kinect 传感器，结合了普通摄像头和激光传感器，功能强大，具有较高的性价比，同时可获得环境颜色信息和深度值。其中，获取的环境深度值具有连续性、信息量丰富、受光线影响小等特点。因此，被广泛运用于机器人视觉研究领域［3］。在机器人导航研究中，若单独采用普通视觉系统则存在获取环境深度信息算法复杂的缺陷，而单独采用激光传感器则在垂直方向上的视角限制大，且无法感知环境颜色信息，如果能够很好的把普通摄像头和激光传感器结合起来运用于机器人导航过程中，提高获取信息的效率和实时性，对于移动机器人研究领域具有重大的意义。但是，目前Kinect 研究尚处于起步阶段，它的缺点是获得的深度数据一定程度上受到噪声的影响，且获取深度信息的环境距离也存在限制，应用在机器人自主导航中还存在许多困难。

本文主要开展基于Kinect 传感器的室内机器人自定位研究，首先用Kinect 传感器采集环境的颜色和深度信息，然后采用SIFT 匹配方法获取匹配特征点和RANSAC方法去除误匹配，最后通过ICP 算法计算，不断更新机器人运动参数，从而更新机器人位姿。比较采用其他视觉传感器进行机器人自定位的方法，本文方法仅用Kinect 传感器便可同时获得环境颜色和深度信息，引入的RANSAC方法可去除不可靠的匹配点，算法简单，设备简易且机器人自定位精度较高。

1 KINECT 简介

Kinect(深度测量传感器)传感器是由红外线发射器(IR projector)和红外摄像头(IR camera)组合而成的3D结构光深度测量传感器［4］，可用于测量空间三维点数据。Kinect 既含有普通RGB 摄像头，又含有深度摄像头，如图1。作为一个深度测量传感器，Kinect 具有三个输出:IR 图像、RGB 图像、深度信息。

图1 Kinect 传感器

1)Kinect 原理介绍

IR 摄像头接收和解码IR 发射器在空间投影的IR 三维点信息，生成IR 图像。RGB 图像是由一个标准的分辨率为640 ×480 摄像头拍摄的图像。摄像头的水平视角为57°，垂直视角为43°，获取图像频率是30 fps。通过标定获取摄像头的内参数，从而将RGB 图像和IR 图像特征点集的像素值和深度值(x，y，d)转化为对应的空间三维数据(x，y，z)。

Kinect 传感器主要的作用是输出场景对应的深度信息，返回“深度倒数”值d，而不是直接返回实际深度值z。由红外线发射器和红外摄像头的组合可获取深度图像素值和深度信息，如式(1)。

式中:(x，y)为深度图像像素坐标，(u，v)为IR 图像像素坐标，(u0，v0)为IR 摄像头焦点坐标。

2)Kinect 标定技术

本文采用张正友的方法［5］和matlab 标定工具箱［6］标定Kinect 普通RGB 摄像头的内参数。标定模板为平面黑白棋盘格，棋盘格方格大小为40 mm ×40 mm，图2 为用Kinect 普通RGB 摄像头在5 个不同角度拍摄的棋盘图像对。标定的结果RGB 摄像头内参数为:

不考虑摄像头畸变。

图2 用于标定普通摄像头的图片

2 SIFT 特征介绍

SIFT 特征匹配算法［7－8］能够提取稳定的特征，是指该特征能对旋转、尺度、仿射、视角、光照等图像变化因素保持一定的不变性。一般SIFT 匹配包括四个步骤:1)特征点检测:Lowe［7］在图像二维平面空间和DoG(Difference－of－Gaussian)尺度空间中同时检测局部极值以作为特征点，DoG 算子定义为两个不同尺度的高斯核的差分，DoG 算子如式(3)所示。

在检测尺度空间极值时，某个像素需要跟同一尺度周围领域的8 个像素以及相邻尺度对应位置的领域9 ×2 个像素(总共26 个像素)进行比较，以确保在尺度空间和2 维图像空间都能检测到局部极值。2)特征点描述，即建立特征向量:每个关键点由2 ×2 共4 个种子点构成，每个种子点包含8 个方向向量信息。为了增强匹配稳定性，Lowe［7］对每个关键点采用4 ×4 共16 个种子点来描述，这样对于每个关键点就可以产生128 个数据，即最后每个关键点建立128 维的SIFT 特征向量。3)进行特征匹配以获取候选匹配点:当两幅图像的SIFT 特征向量生成后，下一步采用关键点特征向量间的欧式距离来作为两幅图像中关键点的相似性判定度量。取两幅图像欧式距离最近的前两对关键点，在这两对关键点中，如果最近的距离除以次近的距离少于某个比例阈值，则接受这一对匹配点。4)消除误匹配:本文采用RANSAC 算法［9］去除外点，RANSAC 算法是通过多次随机抽取一定的样本对参数进行估计，先得出初始参数模型，然后根据估计参数将所有数据进行分类，一部分数据在设定误差范围内，则称为内点，反之称为外点，经过多次迭代计算出最优模型参数。

本文在采用SIFT 匹配的同时，引入RANSAC 去除误匹配，提高匹配点可靠性。若不采用RANSAC 方法消除误匹配，也可以完成SIFT 特征点匹配，但是当存在较多不可靠匹配点时，对于运动参数计算存在很大影响，从而影响机器人自定位精度，RANSAC 方法相对于其他方法仅需两幅图像颜色信息便可消除误匹配，且能稳定并准确地去除误匹配，提高机器人运动参数的计算精度。

3 迭代就近点法(ICP 算法)

计算机视觉研究者Besl 和Mckay［10］介绍了一种高层次的基于自由形态曲面的配准方法，也称为迭代就近点法ICP(Iterative Closest Point)。ICP 算法可以由两幅不同RGB 图像或者深度图像的对应点云数据计算两者之间的变换矩阵(R，T)。迭代就近点法ICP 算法经过十几年的发展，不断地得到了完善和补充。本文采用奇异值分解算法(SVD)来求解变换矩阵(R，T)，因为采用SVD 算法计算相对简单［11］，本节将阐述采用的ICP 算法，如何通过SVD 算法获得特征点对的最优匹配参数。

1)匹配点搜索

采用上节提到的SIFT 特征匹配算法分别提取两幅图像的特征点，通过计算两图像间特征点欧几里德距离来确定特征点是否为对应的匹配点，经过SIFT 匹配后得到一组匹配点对。用Kinect 传感器可以获取对应匹配点对的深度信息，将匹配点对的坐标归一化后为x=［u，v，1］T，然后由已标定的RGB 摄像头内参数对归一化坐标进行转换，获得匹配点对在RGB 摄像头坐标系下三维坐标x=［x，y，z］T，坐标轴方向取为:x 轴竖直向上，y 轴水平向右，z 轴向前，计算如式(4)。

式中 (fx，fy)为标定的RGB 摄像头焦距，(u0，v0)为光轴中心的像素坐标，本文计算不考虑摄像头畸变的影响。

2)运动参数计算

目前大部分运动参数计算都是采用最小二乘法，本文采用奇异值分解算法(SVD)来求解变换矩阵(R，T)，算法相对简单。运动参数是由3 ×3 旋转矩阵R 和1 ×3 平移向量T 构成，可以由式(5)求解:

式中:x1表示第一幅图像SIFT 特征匹配点集在RGB 摄像头坐标系下的三维坐标，x2表示第二幅图像中与x1对应的SIFT 特征点集三维坐标，k 表示两幅图像经过SIFT 匹配、RANSAC 去除误匹配后剩余的匹配点对数。

则可以由公式(7)求得优化解旋转矩阵R 和平移向量T:

已经有很多种方法计算R，T 参数，上述方法比较简单且相对稳定。求解出R，T 后，可计算x2经过R，T 转换后的坐标)，然后与x1比对重建误差，当计算得出的与x1的距离小于阈值，则计算结束，R，T 符合要求，否则去除误差大于阈值的对应点后，返回式(4)重新计算。通过这个方法可以计算出两组三维数据点之间机器人的运动参数。

4 自定位方法的实现

上节中描述了SIFT 匹配，RANSAC 去除误匹配，以及ICP 算法。本节将提出如何结合前述的方法解决机器人自定位问题。本方法将由3 部分构成:关键点检测，去除外点，计算运动参数。

1)关键点检测

获取两幅图像的SIFT 匹配点对及其对应的深度信息，具体步骤是先用SIFT 方法在图像Image1 和图像Image2 中分别提取SIFT 特征点集Data1、Data2，然后用Kinect 传感器提取两幅图像SIFT 特征点集对应的深度信息depth1、depth2，最后对两幅图像采用SIFT 进行匹配，获得初始匹配点对。

2)外点去除

由于SIFT 提取的误匹配点对于后续参数的计算影响很大，故采用RANSAC 方法进一步去除初始匹配点对中的误匹配点，从而提高参数的计算精度。

3)计算外参数

去除匹配外点后，剩余两组为正确的对应匹配点集，然后由匹配点集的像素坐标和深度信息转为摄像头坐标系下匹配点集的三维坐标。假定Kinect 第一个方位拍摄图像的特征点集三维坐标为x1，其对应第二个方位拍摄图像的特征点集三维坐标为x2，由x1、x2采用ICP 算法计算两个方位下机器人的运动参数R、T。设定机器人初始方位为坐标原点，机器人移动到第三个方位时，以第二个方位Kinect 拍摄图像的特征点三维坐标作为新的x1，以第三个方位的拍摄图像的特征点三维坐标作为新的x2，以新的x1、x2计算机器人第二个方位和第三方位的相对运动参数R、T，依次迭代就可以对机器人进行自定位。此方法的流程图如图3。

图3 提出方法的流程图

5 实验

实验采用的是AS－R 移动机器人，在其上安装一个Kinect 传感器，如图4。Kinect 传感器的普通摄像头内参数已经事先标定好，用Kinect 传感器在机器人两个方位下拍摄两幅RGB 图像，分别提取并保存两幅图像SIFT特征点及对应的深度信息。对两幅图像采用SIFT 匹配如图5 所示，初始共有181 对匹配特征点，其中包含很多错误匹配，然后经过RANSAC 算法估计筛选出正确匹配特征点对数101 对，如图6 所示。实验设定匹配点重建后两点距离误差阈值为dis=30 mm，输入机器人里程计往前直线运行距离d=250 mm。剔除重建后与x2的距离大于阈值dis 的x2点，并重新计算运动参数R、T。采用本文方法的实验计算结果:机器人两方位相对旋转矩阵:

平移向量T=[7.074 5 27.090 7 253.890 3 ]T，重建误差小于阈值，则计算结束。通过计算得出机器人往前运行距离为dc=255.429 5 mm，与机器人自带里程计比较相差Δd=5.429 5 mm，考虑到机器人自身运行过程中轮子打滑、Kinect 传感器获取深度信息时本身精度及噪声影响等误差的存在，所以实验结果存在一定误差。实验结果验证了本文求机器人运动参数R、T 方法的可行性。

图4 安装有Kinect 的AS-R 机器人

图5 用SIFT 对两个方位下拍摄的图像进行初始匹配

图6 采用RANSAC 方法去除误匹配后最终匹配效果

6 结语

本文采用Kinect 传感器对室内机器人进行定位研究，因为Kinect 传感器同时具备普通RGB 摄像头和深度传感器的优点，所以本文的方法较仅采用普通视觉系统的方法性价比高，设备及算法简单，计算精度高，因此具有推广价值。

本方法的不足是Kinect 传感器提取深度信息受噪声影响，这会影响计算运动参数的精度，同时Kinect 传感器深度测量的距离有限，因此很多方面仍需要进一步研究。

［1］Kim Ho Duck，Seo San wook，Jan Inhun.SLAM of Mobile Robot in the Indoor Environment with Digital Magnetic Compass and Ultrasonic Sensors［C］.International Conference on Control Automation and Systems in COEX，Seoul，Korea，2007.

［2］He Bingwei，Chen Zhipeng.Determination of the Common View Field in Hybrid Vision System and 3D Reconstruction Method.Robot，vol.33(5)，pp.614-620，September 2011.

［3］Takeda Y，Aoyama N，Tanaami T，et al.Study on the Indoor SLAM Using Kinect［M］.Advanced Methods，Techniques，and Applications in Modeling and Simulation.Springer Japan，2012:217-225.

［4］K.Konolige，P.Mihelich.Technical description of Kinect calibration.http://www.ros.org/wiki/kinect_calibration/technical，2011.1，3.

［5］Zhengyou Zhang.A flexible new technique for camera calibration.IEEE Transactions on Pattern Analysis and Machine Intelligence，vol.22(11)，pp.1330-1334，2002.

［6］J.Y.Bouguet.Camera calibration toolbox.http://www.vision.caltech.edu/bouguetj/calib_doc/，2010.

［7］Lowe D G.Object recognition from local scale-invariant features.IEEE International Conference on Computer Vision，Piscataway，NJ，USA，pp.130-132，1999.

［8］Yuquan Wang，Guihua Xia，Qidan Zhu，et al.Modified Scale Invariant Feature Transform in Omnidirectional Image.Proceedings 2009 IEEE International Conference on Mechatronics and Automation (ICMA)，pp.2632-2636，2009.

［9］Wang Wei，Hong Jun，Tang Yiping.Image matching for geomorphic measurement based on sift and RANSAC methods［C］.International Conference on Computer Science and Software Engineering.Wuhan，Hubei，IEEE，2008，317-320.

［10］Paul J.Besl，Neil D.McKay.A method for registration of 3-D shapes［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，1992.14(2):239-256.

［11］Eggert，D.W.，Lorusso，A.，Fisher，R.B..Estimating 3-D rigid body transfor-mations:a comparison of four major algorithms.Machine Vison and Applications 9，272-290 (1997).