张 盟,王志亮,刘汉忠
(1. 南京工程学院机械工程学院, 江苏 南京 211167;2. 南京工程学院自动化学院, 江苏 南京 211167)
同步定位与地图构建(simultaneous localization and mapping,SLAM)是移动机器人在未知环境下实现自主定位导航的核心技术[1].视觉同步定位与地图构建(visual SLAM,VSLAM)因采用视觉传感器比激光、雷达等具有一定价格优势,成为SLAM研究领域的热点方向之一.
文献[2]构建了PTAM(parallel tracking and mapping)系统,实现了跟踪与建图过程的并行化.文献[3]在此基础上构建了ORB-SLAM2(Oriented FAST and Rotated BRIEF SLAM2)系统.VSLAM前端系统在图像匹配时需满足实时性与准确性的要求,但受到噪声干扰,会在匹配中出现特征点的误匹配,影响后续姿态估计等工作[4].随机抽样一致(random sample consensus,RANSAC)算法[5]通过随机抽取样本数据,计算模型参数,能快速找到最优估计[6],因此,ORB-SLAM2系统在特征匹配时采用RANSAC算法剔除误匹配点.RANSAC算法因其随机性而增大迭代次数,导致算法运行低效,因此,本文在图像匹配过程中引入一种粗剔除与精剔除相融合的误匹配剔除方法,相较于传统的RANSAC方法,提高了图像特征点匹配的准确度.利用本文算法替换ORB-SLAM2系统的RANSAC算法,选用双目相机,在实验室环境下对改进后的ORB-SLAM2算法进行试验,获得相机轨迹,进而证明本文算法在VSLAM中的可行性.
ORB算法运用FAST算法来寻找特征点,FAST算法检测过程为:
1) 从图像中选取像素p,假设它的亮度为Ip;
2) 设值一个合适的阈值T(如Ip的20%);
3) 以像素p为中心,选出半径为3的圆上的16个像素点,如图1所示;
4) 若选取的圆上有连续的N个点的亮度超过设定的阈值,则认为p是特征点.
图1 FAST算法构建圆形区域示意图
通过构建图像金字塔,在金字塔的每一层检测角点,实现尺度不变性.特征的旋转通过灰度质心法实现[7],其具体步骤为:
1) 在一个小的图像块B中,定义图像块的矩为:
(1)
2) 通过矩找到图像块的质心:
(2)
3) 确定特征点的方向为:
θ=arctan(m01,m10)
(3)
ORB算法使用的是BRIEF描述子,属于二进制描述算法,即用0和1描述特征.定义S×S大小的图像领域P的二值化τ为:
(4)
式中,p(x)为图像邻域P在点x的灰度值.
则n个测试点对比形成描述子的计算为:
(5)
式中,n可为128、256或512等.
上述描述子不具有旋转不变性,ORB算法利用角点方向使描述子包含方向信息.定义一个2×n的矩阵:
(6)
对A使用特征点方向θ得到对应旋转矩阵:
Aθ=RθA
(7)
得到具备旋转不变性的描述子:
gn(p,θ)=fn(p)|(xi,yi)∈Aθ
(8)
获取两幅图的所有特征点描述子后,采用暴力匹配进行特征匹配,即对图中每一个特征点的描述子与另一幅图中所有的描述子的汉明距离[8]进行计算并排序,描述子距离表示两个特征点间的相似度,暴力匹配选取距离最近的一个特征点作为匹配点.
利用最小距离阈值法计算图像间所有匹配点对的距离,对第i个匹配对中特征向量的距离di、初始匹配集中匹配点对的最小距离dmin设定合适的阈值α[9]:
di<αdmin
(9)
若阈值设置过大,会遗漏误匹配点,增加精剔除难度;若阈值设置过小,则匹配点对剩余数量不足,且其中可能还存在误匹配点对,影响后续根据点对估计相机运动的工作.因此,通过控制变量法找到合适的阈值,再对初始匹配集P进行粗略的误匹配剔除,获得匹配集P1[10].
对匹配集P1中的匹配点对,以特征点p为中心,选取半径为3的圆上的5个像素点(1、3、5、9、13号像素点),计算它们与特征点像素值的差值Ii=|g(p)-g(xi)|,i=1,2,…,5,构造特征点p的邻接矩阵R:
(10)
对R进行奇异值分解,可获得特征值:
λ1(p)≥λ2(p)≥…≥λ5(p)
(11)
试验平台采用Ubuntu16.04系统、CPU Intel-i5、8 GB内存,利用OpenCV2.4.10开源库.为对比ORB算法在采用RANSAC算法与本文算法后误匹配剔除的效果,采集实验室真实环境图片进行对比试验,采集图像像素大小为640×480.试验结果如图2所示.图2中的圆点代表图像之间进行特征匹配的特征点,图像之间的直线表示匹配后特征的对应关系.
(a) ORB特征点直接匹配
(b) 粗剔除
(c) RANSAC剔除误匹配
(d) 本文算法剔除误匹配
从图2(a)中可以看出,基于ORB特征的直接匹配,会因各种原因产生错误的匹配.图2(b)所示为采用最小阈值距离法进行粗剔除的结果,当阈值设置为10时,剩余匹配对数为297,其中仍存在一定的误匹配;当阈值设置为8时,匹配对数只有233对,虽然一定程度上降低了误匹配的数量,但剩余匹配对数偏少.本文阈值选取9,从RANSAC算法与本文算法进行剔除后的结果看(图2(c)、图2(d)),误匹配的对数均大大减少,两种匹配算法的误匹配剔除时间及匹配正确率如表1所示.试验结果说明本文算法在运算速度上与RANSAC算法相近,但匹配精度提高了5.96%,符合VSLAM 系统实时性与匹配准确性的要求.
表1 两种算法对相同图片的匹配时间和正确率
图3为本文ORB-SLAM系统框架,主要包括跟踪定位、局部地图构建和回环检测三个主线程[11].ORB-SLAM2系统的跟踪线程利用视觉传感器获取图像,从图像中提取ORB特征,利用RANSAC算法剔除误匹配,获得更精确的匹配点,跟踪局部地图并判别新的关键帧[12].本文在该系统基础上,在提取ORB特征及匹配后,用本文的误匹配剔除算法替代原RANSAC算法进行试验.
试验控制部分包括工控机与电机驱动控制器,工控机装有Ubuntu16.04系统.视觉传感器选用小觅双目相机,上位机配置为CPU Intel-i5、8 GB内存,接受移动机器人的运动信息与图像信息.试验以基于ROS的改进ORB-SLAM2系统作为试验框架.从试验结果来看,ORB-SLAM2算法初始化效果较好,能实时运行.如图4所示,相机跟踪运行稳定,无丢帧或者失帧的情况,能建立实验室环境特征点地图(见图5),证明本文算法在VSLAM系统中具有良好的可行性.
图3 本文ORB-SLAM系统框架
(a) 运行序列帧1
(b) 运行序列帧2
(c) 运行序列帧3
(d) 运行序列帧4
图5 关键帧轨迹与特征点地图
基于ORB特征进行位姿估计时,快速、有效的误匹配剔除过程十分重要.针对原有ORB-SLAM2系统采用RANSAC算法进行误匹配剔除存在迭代次数不稳定、花费时间长等问题,本文提出一种基于ORB特征的误匹配剔除算法,将剔除过程分为粗剔除和精剔除两个阶段,粗剔除采取设定阈值为9的最小距离阈值法,精剔除通过构建特征点的差值矩阵来重新描述特征点.试验结果表明,在匹配时间相近的情况下,本文算法相较于RANSAC算法在匹配正确率上有了较大的提升,能有效排除误匹配的干扰.对改进后的ORB-SLAM2算法进行实际运行,获得相机轨迹,验证了本文算法在VSLAM中具有良好的可行性.