, ,

(浙江工业大学 信息工程学院，浙江 杭州 310023)

基于RGB-D相机的视觉里程计实现

仇翔,王强,俞立

(浙江工业大学 信息工程学院，浙江 杭州 310023)

针对移动机器人在未知环境中的自身定位问题，提出了一种基于RGB-D相机的移动机器人运动轨迹估计方法.首先，提取当前图像的ORB特征并与关键帧进行特征匹配；然后，采用结合特征匹配质量和深度信息的PROSAC算法对帧间运动进行迭代估计；最后，提取关键帧并利用g2o求解器进行局部优化，得到关键帧位姿的最优估计，进而得到机器人的运动轨迹.实验结果表明：与RANSAC+ICP算法相比，该方法能有效提高移动机器人的定位精度.

视觉里程计；ORB；PROSAC；关键帧

视觉里程计(Visual odometry, VO)[1]作为视觉SLAM[2]的前端，一直被广泛的研究.它是通过搭载在移动机器人上的相机获取的一系列连续视觉信息来推算出机器人的相对运动.和传统轮式里程计(Wheel odometry)相比，视觉里程计不存在车轮打滑而造成的误差，适用于非平面运动环境和非轮式移动机器人[3]，并且能够获取更多的环境信息来用于三维重建[4]、伺服控制[5]和目标跟踪[6]等，具有广泛的应用前景.

随着微软和华硕相继推出成本低廉且能够同时采集色彩和深度信息的相机(RGB-D相机)，利用该类型相机实现的VO得到广泛的应用.基于RGB-D相机的VO研究方法一般分为两类：一类是基于稀疏特征点的方法，另一类是稠密方法.基于稀疏特征点的方法是根据SIFT，SURF，ORB等特征点的匹配来建立帧间联系，Henry等[7]利用随机采样一致性(RANSAC)算法[8]剔除误匹配并把计算得到的相对位姿结果作为迭代最近点算法(Iterative closest point, ICP)[9]的迭代初值，提高了帧间运动估计的精确度.但是由于由空间三维点组成的点云的迭代计算的数据量大，需要较高的硬件成本.Dryanovski等[10]考虑了像素点深度的不确定性并建立高斯混合模型，采用帧到模型的特征点云配准方法，并利用卡尔曼滤波对模型进行动态更新，该方法的实时性高，能够在室内小范围实现闭环而不需要额外的SLAM后端技术，但无法应用于大范围环境.另外，从深度信息中提取的平面信息具有很好的特性，Pathak等[11]提出了一种基于平面的ICP配准算法，高翔等[12]在提取的平面上进行特征点匹配，很大程度上减少了误匹配的发生，但此方法应用环境较为局限.Silva等[13]利用光流法来追踪当前帧的特征点，相比于在两帧上分别提取特征点后进行匹配的方法计算速度更快，但在运动速度较快时方法容易失效.稠密方法是基于图像一致性假设，相比于稀疏特征点方法，它能够充分利用图像的像素信息，最早由Steinbruecker等[14]提出的，由于数据量大，需要利用GPU加速技术实现[15].

综上所述，目前已有的视觉里程计方法大多精度不够或者对硬件要求高.笔者提出了一种基于RGB-D相机的视觉里程计实现方法,利用融合特征匹配质量和深度信息的改进随机采样一致性(PROSAC)算法[16]快速估计帧间运动，采用g2o(General graph optimization)[17]求解器对关键帧进行局部优化，提高了定位精度，并且由于计算复杂度不高，对硬件的要求较低，最后通过实验验证该方法的有效性.

1 问题描述

1.1 相机模型

对于一般的相机，可以采用针孔相机模型，如图1所示，任意空间点P=(x,y,z,1)T的坐标可以由它在图像上投影点p=(u,v)T和该点的深度d=d(p)通过逆向投影函数π-1得到

(1)

式中：fx,fy为相机在x,y两个轴上的焦距；cx,cy为相机的光圈中心；s为深度图的缩放因子，这些参数可以事先通过标定得到.

图1 针孔相机模型Fig.1 The model of pinhole camera

同理，通过正向投影得到空间点的像素坐标为

(2)

1.2 帧间相机运动模型

当相机运动时，所采集到的图像也会发生相应变化，根据相同特征点的关联性，可以计算出前后两帧间的相机相对位姿，并表示为T4×4∈SE(3)，即

(3)

式中：R∈SO(3)为旋转矩阵；t为平移向量.

(4)

令e=0，根据3个匹配好的特征点对，可以求解出Tk-1,k的四个可能值，然后利用第四特征点对来消除歧义，得到帧间相对运动Tk-1,k.

根据帧间运动模型计算得到的Tk-1,k的依次递推，可以得到移动机器人当前位姿，进而解决机器人的定位问题.

图2 帧间运动模型Fig.2 The motion model of inter-frame

2 视觉里程计算法流程

为了实现移动机器人的精确定位，设计了视觉里程计系统，如图3所示.

图3 视觉里程计系统框架Fig.3 The system framework of VO

视觉里程计系统的流程：1)提取当前帧图像ORB特征点，并与当前关键帧进行特征匹配；2)根据相机模型，将二维特征像素点坐标投影成空间三维点坐标，并采用结合特征匹配质量和深度信息的PROSAC算法对帧间运动进行迭代估计；3)提取关键帧，建立图优化模型，并采用g2o求解器进行局部优化，得到关键帧的最优估计.具体实现如下.

2.1ORB特征提取与匹配

ORB特征是在FAST角点检测算法和BRIEF特征描述算法基础上改进而来的，相比于SIFT和SURF，其实时性更高.ORB特征提取可以分为以下两步进行：

θ=atan2(m01,m10)

(5)

然后，构建具有旋转不变性的rBRIEF(rotated BRIEF)描述子.BRIEF描述子自身不具有方向信息，为其添加特征点的主方向θ来解决旋转不变性问题.BRIEF描述子是以二进制比特串的方式存储的，对于任意n个二进制准则特征集，定义了一个2n矩阵为

(6)

那么具有旋转不变性的BRIEF描述子为

gn(p,θ)=fn(p)|(xi,yi)∈Sθ

(7)

ORB特征是否匹配是根据rBRIEF描述子的Hamming距离来判断的，根据快速最近邻算法FLANN检测得到最近邻点和次近邻点，令特征点与最近邻点的Hamming距离为d1，与次近邻点的Hamming距离为d2，当距离比率γ=d1/d2小于给定阈值时，匹配成功，否则失败.且γ越小，匹配程度越高.

2.2 帧间相机运动估计

在前后两帧中提取特征点后，利用匹配好的特征点对，根据帧间运动模型求解出相对位姿变换.但是由于特征点对存在误匹配情形，需要排除这些误匹配的影响，来得到最优的帧间运动估计.

笔者采用基于PROSAC算法的P3P模型来估计帧间运动.PROSAC算法是随机采样一致性算法的改进，两者的区别在于RANSAC算法对采样点等概率采样，而PROSAC算法根据采样点与模型的匹配程度进行排序，匹配质量高的优先采样，因此算法的实时性和稳定性更高.

考虑到RGB-D相机的深度信息具有一定的误差，且误差值随着采样点深度值d增加而加大，近处的特征匹配点更加可靠.因此，笔者提出了一种融合深度信息d和特征点对匹配程度γ的排序算法，设特征匹配点可信度函数为

q=1/(αγ+βd)

(8)

式中：α，β为比例因子.

基于特征匹配点可信度的PROSAC算法步骤：

1)根据q从高到低对特征匹配点进行排序.

2)设置初值，包括内点误差阈值、内点数量阈值和最大迭代次数.

3)根据抽样次数t和假定样本集合增长函数n=g(t)来确定样本集M.

4)从M中选取4个特征匹配点，利用帧间运动模型计算得到模型参数，利用其余所有特征匹配点进行模型验证，得到满足该模型的内点集.

5)判断是否满足循环终止条件，若否，则循环执行3)～5)，反之循环结束.循环终止条件：迭代次数达到最大迭代次数；最大内点集中内点数量大于内点数量阈值；k次采样后的内点数与k-1次抽样后得到的内点数没有增加.

得到最大内点集后，利用该集合中所有特征匹配点，重新利用非线性最小二乘法计算帧间相机运动的最优估计.

2.3 关键帧选取与局部优化

由于帧间运动估计不可避免存在一定的误差，为了减轻累积误差对移动机器人定位精度的影响，将采用基于关键帧提取和局部优化的算法.

图4给出了该算法的基本表示，nt表示当前帧，Ki表示关键帧，每个当前帧都是和之前最近的关键帧进行帧间匹配得到自身位姿状态，而不是相邻帧.关键帧提取方法一般根据当前帧与前一关键帧匹配时的内点数和运动大小来选定，当内点数小于设定阈值或者运动大于设定阈值时，则认为机器人位姿变化较大，而把当前帧设为关键帧.

图4 关键帧提取与局部优化Fig.4 Key frame extraction and local optimization

在系统运行过程中，若检测到新的关键帧，则将该帧与关键帧序列中后m个关键帧进行匹配，匹配是否成功取决于两帧间的特征匹配点数是否大于设定阈值.若大于则可以建立新的约束，实现局部的位姿优化，提高系统的定位精度.将该问题转换为图优化模型，建立模型的误差函数为

(9)

式中：K为所选取的关键帧位姿状态集合；Ki,Kj∈K；Ti,j为Ki和Kj间状态转移矩阵；Ωi,j为信息矩阵；e(Ki,Kj,Ti,j)为由Ki和Kj组成的边所产生的误差.

g2o是通用的图优化求解器，其中包括Gauss-Newton，Levenberg-Marqudart(LM)等非线性最小二乘求解算法.对于笔者所建立的图优化模型，将关键帧的位姿设置为顶点，关键帧之间的约束设置为边，在g2o求解器中采用LM算法最小化f(K)可以得到关键帧位姿的局部最优估计.

3 实 验

3.1 实验环境

实验采用的是华硕的XtionPRO LIVE深度相机，图像分辨率为640×480，深度量程为0.8～3.5 m,采样频率为30 Hz.计算机配置为AMD A6-3400M处理器，主频为1.40 GHz，内存4.00 GB，操作系统为64 位Ubuntu14.04.使用的移动机器人为Kuka公司的Youbot移动机器人，如图5所示，另外它上面搭载了激光传感器，可以利用激光定位得到更高精度的定位结果作为真实轨迹.

图5 实验机器人Fig.5 The experimental robot

3.2 帧间匹配内点数对比

基于稀疏特征点的方法，帧间匹配内点数是反映计算准确性的重要指标.在相同的ORB特征提取和FLANN匹配的参数设置下，对比了分别利用RANSAC+ICP算法和笔者算法对采集到的数据处理得到的内点数，如图6所示.其中笔者方法中匹配程度函数两个系数分别设置为α=1，β=0.2.从图6可知：满足笔者算法得到的帧间运动模型的特征匹配点的数量更多.

图6 帧间匹配内点数Fig.6 The number of inliers between inter-frame matching

3.3 直线运动实验

本实验是在Youbot移动机器人平台上进行的，其直线运动的速度为0.2 m/s，运动距离为2.8 m.分别利用RANSAC+ICP算法[9]和笔者算法估计移动机器人运动轨迹，如图7所示.实验结果表明：在直线运动中，笔者算法平均定位误差为1.03%，相比于RANSAC+ICP算法的3.57%误差更小.

图7 直线运动轨迹Fig.7 The trajectory of linear motion

3.4 圆周运动实验

本实验移动机器人进行圆周运动，其线速度为0.2 m/s，角速度为0.2 rad/s，运动半径为1 m，运动距离为6.28 m.实验结果如图8所示，RANSAC+ICP算法平均的定位误差为8.30%，而笔者算法的平均定位误差为2.68%.

图8 圆周运动轨迹Fig.8 The trajectory of circular motion

3.5 实验分析

实验表明：与RANSAC+ICP算法相比，笔者算法的定位精度更高，通过比较和分析，原因有两点：1) 通过对特征匹配点进行可信度排序，优先采样可信度高的特征匹配点，用更少的迭代次数获得最佳的帧间运动估计；2) 通过提取关键帧并对关键帧进行局部的优化，可以尽量减轻累积误差对定位精度的影响.

4 结 论

利用RGB-D相机获得的图像和深度信息，提出了一种移动机器人视觉里程计实现方法.根据ORB特征点建立帧间联系，采用融合匹配质量和深度信息的PROSAC算法求解P3P模型并得到帧间运动估计，结合关键帧提取和g2o求解器进行局部状态优化，实现了移动机器人较高精度的自主定位.实验证明：该算法具有可行性和有效性.考虑到系统运行过程中仍存在误差累积，下一步工作将会考虑全局优化问题，将该方法应用到视觉SLAM中，构建具有全局一致性的运动轨迹和地图模型.

[1] SCARAMUZZA D, FRAUNDORFER F. Visual odometry: partⅠ: the first 30 yearccs and fundamentals[J]. IEEE robotics & automation magazine,2011,18(4):80-92.

[2] DURRANTWHYTE H, BAILEY T. Simultaneous localization and mapping: partⅠ[J]. IEEE robotics & automation magazine,2006,13(2):99-110.

[3] 曹美会，鲜斌，张旭，等.基于视觉的四旋翼无人机自主定位与控制系统[J].信息与控制,2015,44(2):190-196.

[4] 管秋，金俊杰，张剑华，等.基于最优RANSAC算法的非增加式多视图三维重建[J].浙江工业大学学报,2015,43(5):473-478.

[5] 孙建辉，刘坤.基于序列图像特征识别实时反馈伺服跟踪研究[J].浙江工业大学学报，2015，43(2):185-189.

[6] 杨东勇，冯形松.采用形态学算子的运动目标检测跟踪方法研究及实现[J].浙江工业大学学报,2010,38(2):149-154.

[7] HENRY P, KRAININ M, HERBST E, et al. RGB-D mapping: using depth cameras for dense 3D modeling of indoor environments[J]. International journal of robotics research,2014,31(5):647-663.

[8] FISCHLER M A, BOLLES R C. Random sample consensus: a paradigm for model fitting with application to image analysis and automated cartography[J]. Communications of the association for computing machinery,1981,24(6):381-395.

[9] BESL P J, MCKAY N D. Method for registration of 3-D shapes[J]. IEEE transactions on pattern analysis & machine intelligence,1992,14(2):239-256.

[10] DRYANOVSKI I, VALENTI R G, XIAO J Z. Fast visual odometry and mapping from RGB-D data[C]//2013 IEEE International Conference on Robotics and Automation. Piscataway: IEEE,2013:2305-2310.

[11] PATHAK K, BIRK A, VASKEVICIUS N, et al. Fast registration based on noisy planes with unknown correspondences for 3-D mapping[J]. IEEE transactions on robotics,2010,26(3):424-441.

[12] GAO X, ZHANG T. Robust RGB-D simultaneous localization and mapping using planar point features[J]. Robotics & autonomous systems,2015(72):1-14.

[13] SILVA B M F, GONCALVES L M G. A fast visual odometry and mapping system for RGB-D cameras[C]//Joint Conference on Robotics: SBR-LARS Robotics Symposium and Robocontrol. Piscataway: IEEE,2014:55-60.

[14] STEINBRUCKER F, STURM J, CREMERS D. Real-time visual odometry from dense RGB-D images[C]//IEEE International Conference on Computer Vision Workshops. Piscataway: IEEE,2011:719-722.

[15] WHELAN T, JOHANNSSON H, KAESS M, et al. Robust real-time visual odometry for dense RGB-D mapping[C]//IEEE International Conference on Robotics & Automation. Piscataway: IEEE,2013:5724-5731.

[16] CHUM O, MATAS J. Matching with PROSAC-progressive sample consensus[C]//IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Piscataway: IEEE,2005:220-226.

[17] KUMMERLE R, GRISETTI G, STRASDAT H, et al. G2o: a general framework for graph optimization[C]//IEEE International Conference on Robotics & Automation. Piscataway: IEEE,2011:3607-3613.

RealizationofvisualodometrybasedonRGB-Dcamera

QIU Xiang, WANG Qiang, YU Li

(College of Information Engineering, Zhejiang University of Technology, Hangzhou 310023, China)

Aiming at the problem of localization of mobile robot in unknown environment, a method to estimate the trajectory of robot based on RGB-D camera was proposed. Firstly, ORB(oriented FAST and rotated BRIEF) features were extracted in current frame and matched with key frame. Secondly, combined the quality of feature matching with depth information, the PROSAC(Progressive sample consensus) algorithm was adopted to iteratively estimate inter-frame motion. Finally, key frame was detected and g2o(general graph optimization) solver was used to make local optimization to get the best pose estimation of key frame, and then the trajectory of robot was gained. The result shows that this method can effectively improve the positioning accuracy of mobile robot compared with RANSAC+ICP algorithm.

visual odometry; ORB; PROSAC; key frame

2017-02-22

浙江省自然科学基金重点项目(LZ15F030003)

仇 翔(1980—)，男，浙江湖州人，讲师，研究方向为先进控制理论、机器人控制技术，E-mail: qiuxiang@zjut.edu.cn.

TP24

A

1006-4303(2017)06-0634-05

(责任编辑：陈石平)