基于PROSAC算法与ORB-SLAM2的RGB-D相机室内视觉定位研究*

曹 蜜,胡凌燕,熊彭文,彭 杰,曾 雷

(南昌大学信息工程学院,南昌 330031)

同时定位和建图(Simultaneous Localization and Mapping,SLAM)是指机器人在构建当前环境地图时,同时定位自身位置和姿态的过程[1]。SLAM技术在过去30年中取得了惊人的进步,其实时性问题是一直是移动机器人导航控制研究过程中未解决的问题,它是当前的研究热点,也是移动机器人在未知环境中真正实现自主导航的关键问题[2]。

近年来,随着图像处理技术的进步,以及视觉传感器(摄像机)较激光传感器的价格低廉的优势,基于视觉信息完成定位建图的视觉SLAM(Visual SLAM,VSLAM)逐渐成为当前移动机器人领域研究的热点[3]。

视觉SLAM系统使用摄像头作为主传感器来估计机器人的位姿和构建环境地图[4]。根据利用图像信息的不同,视觉SLAM算法可分为基于直接法的SLAM方法和基于特征的SLAM方法[5]。基于直接法的SLAM方法中,Forster C等人[6]提出了SVO:Fast semi-direct monocular visual odometry,SVO算法可以在嵌入式系统中实时运行,但位姿估计存在累计误差,并且丢失后不太容易进行重定位;为了构建稠密的三维环境地图,Engel J等人[7]提出了 LSD-SLAM(large-scale direct SLAM)算法,与文献[6]提出的直接的视觉里程计方法相比,该方法在提高相机估计位姿准确性的同时,能够创建大规模的三维环境地图,但无法完成稠密的建图,且单目真实尺度未知,可能出现地图大小与真实地图大小偏差很大;Kinect-fusion[8]是第一个基于Kinect的能在GPU上实时构建稠密三维地图的算法,但实时性差,精度低;RGBD-SLAMv2[9]结合彩色图像和深度图像,由g2o实现最优相机位姿估计,并提出了基于熵的关键帧提取及闭环检方法,大大降低了轨迹误差,但集成度高,计算量大,运用场景有限。基于特征的SLAM方法由滤波算法逐渐发展为关键帧算法[5],Strasdat等人[10]证明基于关键帧的方法比基于滤波的方法精度更高。基于关键帧方法的单目视觉中,最具代表性的是parallel tracking and mapping(PTAM)[11],将跟踪和映射分成单目SLAM中的两个平行线,但是场景小,跟踪容易丢失;ORB-SLAM[12]基于PTAM,提出了一个单目视觉SLAM系统,ORB-SLAM2[13]在此基础上扩展为双目、RGB-D相机,二者均将整个SLAM过程分为定位、创建地图、闭环检测三个线程,是一种基于稀疏特征的轻量级SLAM定位方案,但无法实现机器人导航;且由于特征追踪存在误差,误差不断累积会导致后面帧的位姿离实际位姿越来越远,最终影响系统整体的精度。

RANSAC(Progressive Sample Consensus,随机抽样一致性)[14]算法是一种鲁棒的方法。在一组包含噪声的具有对应关系的数据中,该算法通过随机抽取样本数据,计算模型参数。该算法可以避免错误信息,是一种功能强大的模型参数估计方法,它广泛用于各个领域,如消除图像特征不匹配和自动估计单应性等。但由于RANSAC数据处理具有随机性的特点,算法的效率较低。PROSAC(PROgressive SAmple Consensus,顺序抽样一致性)[15]算法是一种基于RANSAC改进的算法,该算法首先对数据进行相似性排序,选择相似性排名靠前的对应关系作为子集,从子集抽取样本数据进行模型参数估计。与RANSAC算相比,是一种半随机的参数估计方式。在图像匹配误差剔除过程中,求解单应性矩阵更加高效。

基于以上研究现状,我们开展以Kinect 2.0作为图像输入,基于ORB-SLAM2的RANSAC误差剔除改进算法的研究,同时为机器人导航未来工作建立稠密地图奠定基础。本文采用PROSAC算法对图像匹配误差进行剔除以及求解单应性矩阵H获得相机估计位姿,以实现更加高效准确的实时相机定位跟踪。再结合ORB-SLAM2,通过关键帧跟踪,实时建图,回环检测三线程数据处理,得到准确的定位。图像误差剔除匹配实验结果表明,PROSAC算法可以明显提高运算效率,整个匹配过程相对于RANSAC算法效率提高一倍。将该算法结合ORB-SLAM2进行定位实验结果表明,该算法能够获得与传统ORB-SLAM2算法几乎相同的定位精度,两者的绝对轨迹误差均方根误差仅相差0.000 4 m,但该算法的效率明显提高,保证了整个定位系统实时性。

图1 PROSAC算法与ORB_SLAM2系统框架

1 系统结构框架

ORB-SLAM2包括特征跟踪、局部建图、回环检测以及在回环检测后的全局BA[16]优化这3+1线程,可实现地图重用、回环检测以及重新定位等功能。在本文中,我们将PROSAC算法取代原ORB-SLAM2中的RANSAC算法,实现特征匹配误差剔除,以求解单应性矩阵H获得相机估计位姿,应用于ORB-SLAM2的特征跟踪线程。PROSAC算法与ORB-SLAM2系统结合框架图,如图1所示。

该系统框架具体实施过程如下:

①在跟踪线程,首先使用词袋模型[17]对前后帧图像的特征点进行预分类,实现特征点快速匹配。针对特征匹配点对,采用PROSAC算法进行更加高效准确的误匹配剔出处理,以计算单应性矩阵H并估计相机位姿。然后跟踪局部地图,通过投影在当前帧与局部地图之间查找对应关系以对当前帧的位姿进行优化。最后判断是否需要插入新的关键帧。

②在局部建图线程,将新的关键帧作为新节点加入共视图后,更新共视图、边以及关键帧生长树,并插入到地图中。然后剔除不符合约束测试的地图点,以保证只保留高质量的地图点,防止外点影响建图。接下来检查关键帧中ORB特征点的正向景深、视差、反投影误差和尺度一致性,无误后创建一个新的地图点。接下来对关键帧和地图点执行局部BA优化,最后剔除冗余关键帧以降低BA复杂度。

③在闭环检测线程,计算加入回环检测线程中的回环候选关键帧与当前关键帧的相似度,若判断确实有回环,则融合重复的地图点,并在共视图中插入新的边以连接闭环。最后执行全局的位姿和地图点优化,并更新关键帧位姿和地图点坐标。

2 图像误匹配剔除算PROSAC算法

本文提出改进的PROSAC算法剔出误差匹配,求解参数模型的方法。该方法基于“置信度更高的点是内点的概率更高”这一假设[15],在选取样本点时置信度高(质量好)的数据将被优先选择。PROSAC算法在尽可能短的时间内,找到所有初步对应集合中的内点,并且尽可能保证所有内点被找到。RANSAC算法是随机抽取样本,没有考虑到样本之间的差异性,PROSAC算法则先将样本质量好坏进行降序排列,再从具有较高质量的样本子集中抽取样本,求解模型参数,PROSAC算法的鲁棒性和计算效率比RANSAC更高[18]。

该方法引入质量函数q(uj),以及数据点是内点的概率P{uj},有以下关系:

当i

P{ui}≥P{uj}

(1)

根据特征匹配点欧氏距离的比值β,对特征点匹配质量进行估计,将β作为质量函数q(uj)的定量表示,β为:

β=D1/D2

(2)

式中:D表示ORB特征点描述子的欧式距离,D1表示欧式距离最小值,D2表示次小值,比值β越小,匹配质量越好,匹配正确的概率越高。

PROSAC算法需要解决以下两个问题:

①假设生成集大小的选择

准则增长函数n=g(t)定义了集合Un中的质量排名前n的对应关系,这n个对应关系将被进行t次采样。定义增长函数为:

g(t)=min{n:T′n≥t}

(3)

式中:T′n表示Tn次采样中,样本点来源于Un的次数;n表示采样次数大于t时的最小n。

②采样停止标准

当集合Un中内点的数量In满足以下两个标准:非随机性——In是非正确模型的内点的概率低于ψ(一般设为5%);极大性——在集合Un中,有超过In个内点,且经过K次抽样仍未被找到的概率低于η0(一般设为5%)。采样将会停止。

随机类内点集合元素有以下二项分布:

(4)

式中:β是与一个错误模型相对应的点不在采样点集合中的概率,并且该采样点集合包含一个类外点。

(5)

在Un中发现一个非随机内点须满足:

(6)

极大性定义了需要多少次采样才能保证解的置信度,定义PIn表示从Un中采样得到的m个点都来自内点的概率,εn表示质量排名前n的数据中内点比例大小,有:

(7)

(8)

η表示经过K次采样后,集合Un中丢失一组内点的概率,有以下关系:

η=(1-PIn)k

(9)

Kn*(η0)≥log(η0)/log(1-PIn*)

(10)

式中:n*表示使得Kn*(n0)采样终止的长度,η0=5%。

PROSAC算法对质量q进行降序排序,质量排名较前的匹配点对用于求取单应性矩阵H。对质量因子进行排序时,虽然增加了计算,但在迭代过程中大大减小了迭代次数,从而在整个求解模型参数过程中,减少了运算量,提高了算法效率。PROSAC算法流程图如图2所示。

图2 PROSAC算法流程图

3 实验及其结果分析

3.1 特征点匹配对比实验

为了验证PROSAC算法的高效性,本文对PROSAC算法与RANSAC算法进行对比实验。本文将6张图片分为三组,三组特征点对数量梯度分别为:特征多、特征较多与特征少,进行误匹配剔除时间对比实验,并对结果进行了分析比较。实验平台在英特尔四核i5 CPU,4G RAM和256G SDD的LENOVO YOGA710笔记本上进行实验数据处理。实验中我们以0.7倍最大欧式距离比值作为阈值进行粗匹配,设置最大迭代次数为2000次。

如表1比较了PROSAC算法与RANSAC算法在进行特征点匹配实验时的各项性能指标。当内点数量比外点数量多时,在特征点数量较少的情况下,使用PROSAC算法剔除外点求解模型参数,在效率上仍要优于RANSAC算法。

表1 PROSAC算法与RANSAC算法各项指标比较

由表1中两种算法之间误匹配剔除时间的差异可知,PROSAC算法虽然在质量排序计算上增加了耗时,但在特征匹配及误匹配剔除求解单应性矩阵H整个过程中总耗时下降。当特征点数量较多时,计算机使用PROSAC算法可以以20帧/s的速度进行图像匹配处理,而RANSAC算法只能处理大概11帧,速度提高了近1倍。当ORB-SLAM系统不断采集图像数据,不断进行图像误匹配剔除求解模型参数最优解计算,PROSAC算法将会大大提高ORB-SLAM线程处理数据的速度。

由于三组情况下的误匹配剔除效果相似,本文选取特征点较多的一组图片进行展示。我们使用ORB特征进行特征提取,ORB特征原始匹配如图3(a)所示,存在较多错误匹配;PROSAC算法处理后如图3(b),图像剔除了错误匹配,效果良好。

图3 特征点匹配结果

3.2 定位实验

PROSAC算法结合ORB-SLAM2后的系统定位精度与传统方法有几乎相同的效果。我们基于 RGBD-benchmark的两个数据集和真实环境的定位实验进行验证,并对本文方法与传统方法的轨迹进行分析比较。在实验中,当我们将本文方法中的PROSAC算法迭代次数设置为100次,传统方法中的RANSAC算法迭代次数设置为300次不变时,发现定位效果几乎一致。

3.2.1 基于RGBD-benchmark数据集

RBGD-benchmark所有数据集序列由高精度运动捕获系统提供,其轨迹可认为是真实轨迹。实验平台在英特尔四核i5 CPU,4G RAM和256G SDD的、笔记本上进行实验数据处理。由于ORB-SLAM2在纹理特征不明显的环境中,易出现特征跟踪丢失无法形成闭环的情况,本文以rgbd_dataset_freiburg1-desk(以下简称Desk)和rgbd_dataset_freiburg2_pioneer_slam3(以下简称Slam3)作为实验数据集,分析对比本文方法与传统方法在形成闭环与无闭环情况下的轨迹精度,其中Desk数据集长度为9.263 m,Slam3数据集长度为18.135 m。并采用绝对轨迹误差的均方根误差来评价估计的轨迹偏差。绝对轨迹均方根误差计算公式:

(11)

图4是本文方法与传统方法针对Desk与Slam3两个数据集,在有回环与无回环情况下的绝对轨迹误差对比实验结果。图4(a)、4(b)表示在ORB-SLAM2在特征跟踪过程中,一直保持在SLAM模式下,形成闭环,与真实轨迹相比较,可以看出均表现出了良好的定位跟踪效果。图4(c)、4(d)表示ORB-SLAM2在定位过程中,由于光照和环境特征不明显等原因,出现特征跟踪丢失的情况,没有形成闭环。但在SLAM模式阶段,本文方法与传统方法均保持了较好的定位效果。

图4 不同数据集绝对轨迹误差对比

针对以上数据集实验结果,我们分别计算了两组数据集在不同方法下的均方根误差、中值以及位姿对比对数,如表2所示。

表2 基于数据集各项指标比较

如表2所示,对于定位形成闭环的Desk1数据集,传统方法与本文方法的均方根误差仅相差0.000 552 m,中值仅相差0.000 524 m,而位姿对比对数均达500对以上,二者仅相差1对;对于定位未形成闭环的Slam3数据集,传统方法与本文方法的均方根误差仅相差0.000 372 m,中值仅相差0.001 892 m,而位姿对比对数均达1 500对以上,二者仅相差9对。

图4和表2的实验结果表明,采用RANSAC算法的传统ORB-SLAM2系统与采用PROSAC算法降低迭代次数后的本文方法的系统,在定位效果及精度上,均保持了较好的定位效果,且差别很小。

3.2.2 基于真实环境的实时定位

基于实验室真实环境,我们再次验证了采用本文方法系统的定位精度仍能保持良好的实时定位效果。本实验以Kinect2.0和移动机器人Summit为数据采集平台,在英特尔四核i5 CPU,4G RAM和256G SDD的笔记本上进行实验数据处理,图像数据以ROS(机器人操作系统)主题的形式发布进行实验。为尽可能保证图像质量,我们对Kinect2.0传感器进行了标定,同时在明亮且光照均匀实验室内进行试验[19-20]。

将Kinect2.0安放在移动机器人Summit上,控制机器人围绕长3.5 m,宽2 m的标准矩形行走一圈。机器人先以0.1 m/s的线速度沿直线运动2.5 m,再以0.26 rad/s(15 °/s)的角速度,0.5 m/s的线速度沿着半径为0.5 m圆弧转弯,行走1/4圆后继续沿直线以0.1 m/s的速度运动1.5 m,如此往复,行走一圈。

图5(a)为实验平台,移动机器人Summit搭载相机Kinect2.0获取数据,并用笔记本进行实验数据处理;图5(b)代表ORB-SLAM2定位与地图构建过程,其中蓝色代表相机在运动过程中存储的关键帧,绿色帧表示当前关键帧位置,绿色线条代表相机运动轨迹,黑色和红色的点代表储存的地图点。

本文方法与传统方法的轨迹对比如图6所示,轨迹绝对误差对比如图7所示。

图5 实验平台及实验过程

图6 移动机器人定位轨迹对比

由图7可知,实验采集的2 680帧数据中,有四个阶段出现绝对误差峰值,这是由于相机快速旋转带来场景图像变换,使得图像特征匹配时误匹配较多,导致绝对误差较大。在误差较小时刻,相机在做平稳的直线运动,匹配效果较好。基于PROSAC的本文方法与基于RANSAC的传统方法虽存在差别,但差别较小,跟踪效果良好。分别对两种方法的绝对轨迹误差均方根误差进行计算,本文方法值为0.023 9 m,传统方法值为0.024 3 m,二者仅相差0.000 4 m。机器人在实时场景运动过程中,使用本文方法定位轨迹与使用传统方法二者轨迹的误差区别很小,几乎没有区别。但PROSAC在误匹配剔除方面效果明显提高,实时性比较好。

图7 绝对误差对比

4 结论

本文提出用PROSAC算法取代ORB-SLAM2中的RANSAC算法的室内视觉定位方法。PROSAC算法通过将样本质量好坏进行降序排列,再从具有较高质量的样本子集中抽取样本,最后对误匹配数据进行剔除求解模型参数,可以明显提高运算效率。

实验结果表明,相对于RANSAC算法在误匹配剔除计算模型参数上效率提高一倍,将该算法结合ORB-SLAM2进行定位,该算法能够获得与传统ORB-SLAM2算法几乎相同的定位精度,两者的轨迹误差均方根误差仅相差0.000 4 m。用PROSAC算法取代ORB-SLAM2中的RANSAC算法可以在不降低整个定位准确度的情况下,提高明显提高算法效率,保证实际定位过程的实时性和流畅性。

由于ORB-SLAM2是一种基于稀疏特征的轻量级定位方案,无法利用其稀疏地图进行机器人导航[21]。未来我们将利用Kinect2.0,基于PROSAC算法与ORB-SLAM2,进一步进行稠密地图构建,实现机器人导航[22]。