CN109682385A - 一种基于orb特征的即时定位与地图构建的方法 - Google Patents

Abstract

本发明公开了一种基于ORB特征的即时定位与地图构建的方法，所述方法包括：利用改进的RANSAC算法删除误匹配，利用高效的透视n点定位和迭代最近点求解相机的位姿，使得在丢帧的情况下获得精确的位姿，恢复定位；在进行闭环检测时，利用改进的RANSAC算法结合相似变换检测闭环，并且进行闭环的矫正来消除累积误差。本方法获得了更精确的定位和更好的鲁棒性。

Description

一种基于ORB特征的即时定位与地图构建的方法

技术领域

本发明涉及计算机视觉、图像处理、以及定位导航领域，尤其涉及一种基于ORB^[1]特征的即时定位与地图构建的方法。

背景技术

移动机器人在未知环境下的自定位和环境模型的建立问题是密切相关的。定位的实现离不开环境模型，而环境模型的准确度又依赖于定位的精度。机器人在未知的环境中没有参照物，只能依靠自身配备的并不是非常准确的传感器来获取外界信息，这时，实现精确的定位是非常困难的。已有地图实现定位和已知位置创建地图都是容易实现的，但是，没有地图的定位和没有定位的地图创建却无从下手。在现在已有的研究中对这类问题的解决方法可分为两类：一类利用自身携带的多种传感器，包括：里程仪、陀螺仪、以及加速度传感器等，通过自身携带的多种传感信息的融合来减小定位误差。使用多传感器的融合算法多为基于卡尔曼滤波的方法，因为这类方法没有参考外部的信息，累计误差会比较大。另一类方法在依靠自身携带的传感器(陀螺仪，加速度传感器等)估计自身运动的同时，使用外部传感器，如：激光测距仪、视觉传感器等来感知环境，对获得的信息进行分析并提取环境特征保存，在下一步通过对环境特征的比较对自身位置进行校正。但这种方法依赖于环境特征。

SLAM(即时定位与地图构建)用来解决移动机器人在未知环境下定位与建图的问题。SLAM根据传感器的类型，可以分为激光SLAM和视觉SLAM。激光SLAM是利用激光雷达来实现的，视觉SLAM利用视觉传感器如单目^[2]，双目^[3]，RGB-D(深度)相机^[4]。激光SLAM起步比视觉SLAM早，现在技术趋于成熟，精度高。2D激光可以建立环境的二维地图^[5]-[7]，3D激光可以根据采集点云建立3D地图^[8]-[10]。但是激光雷达的成本非常高，视觉传感器的成本相对低很多，应用场景比较丰富，视觉SLAM可以建立稀疏地图，半稠密地图和稠密地图。

早期的视觉SLAM利用SIFT特征，如^[11]-[13]，定位的精度比较高，但是时间代价很大，实现不了SLAM的实时性。T.Bailey^[14]等人利用扩展卡尔曼滤波的方式来计算位姿，但是累计误差不可避免。G.Klein^[15]等人利用图优化的方式来优化相机的位姿，把相机的位姿记录下来，最后做一个整体的优化，如闭环检测。R.Mur-Artal^[16]等人利用ORB特征，使用bundle adjustment(BA-捆集调整)优化位姿。目前，图优化和ORB特征是视觉SLAM的主要研究方向。

视觉SLAM可应用于AR(增强现实)、VR(虚拟现实)、机器人、以及无人机等多个领域，近年来越来越备受关注。

目前SLAM的研究主要存在的问题有：由于丢帧引起的定位精度尤其是室外的定位精度降低，转角时误差较大，闭环检测和优化程度不够，算法时间代价太大不能实现实时性等。

参考文献

[1]E.Rublee,V.Rabaud,K.Konolige,and G.Bradski,“ORB:An efficientalternative to SIFT or SURF,”in Proc.IEEE Int.Conf.Comput.Vision,Barcelona,Spain,Nov.2011,pp.2564–2571.

[2]王彭林,石守东,洪小伟.基于单目视觉和里程计的SLAM算法研究[J].计算机仿真,2008(10):178-181.

[3]吴功伟,周文晖,顾伟康.基于视差空间的双目视觉里程计[J].传感技术学报,2007(06):234-238.

[4]李卫成,汪地,宗殿栋,&姜海龙.(2016).基于rgbd的机器人室内slam与路径规划系统[J].工业控制计算机,29(3),77-78.

[5]陈炜楠,朱蕾,张宏,林旭滨,&管贻生.(2018).稀疏视觉slam对平面激光雷达传感的稠密化模拟[J].机器人,40(3),273-281.

[6]石杏喜,&赵春霞.(2010).基于概率的移动机器人slam算法框架[J].计算机工程,36(2),31-32.

[7]武二永,项志宇,沈敏一,&刘济林.(2007).大规模环境下基于激光雷达的机器人slam算法[J].浙江大学学报(工学版),41(12),1982-1986.

[8]张航,陈彬,薛含章,朱正秋,&王戎骁.(2017).基于无人机和lidar的三维场景建模研究[J].系统仿真学报(9),1914-1920.

[9]黄鹤,王柳,姜斌,&罗德安.(2016).3d slam激光影像背包测绘机器人精度验证[J].测绘通报(12),68-73.

[10]J.Zhang and S.Singh,“LOAM:Lidar odometry and mapping in real-time,”in Robotics:Sci.and Syst.Conf.,July 2014.

[11]代云祥,陈星晖,&李迅.(2017).一种基于sift算法的单目视觉里程计设计[J].软件导刊,16(2),97-100.

[12]朱代先,&王晓华.(2011).基于改进sift算法的双目视觉slam研究.计算机工程与应用[J],47(14),170-173.

[13]王彭林,石守东,&洪小伟.(2008).基于sift算法的移动机器人同时定位与地图创建[J].宁波大学学报(理工版),21(1),68-71.

[14]T.Bailey,J.Nieto,J.Guivant,M.Stevens and E.Nebot,“Consistency ofthe EKF-SLAM algorithm”,In Proceedings of the 2006 IEEE/RSJInt.Conf.Intell.Robots Syst.,2006,Beijing,China,pp.3562-3568.

[15]G.Klein and D.Murray,“Parallel tracking and mapping for small ARworkspaces,”in Proc.IEEE ACM Int.Symp.Mixed Augmented Reality,Nara,Japan,Nov.2007,pp.225–234.

[16]R.Mur-Artal,J.M.M.Montiel,and J.D.Tardos,“ORB-SLAM:A versatileand accurate monocular SLAM system,”IEEE Trans.

发明内容

本发明提供了一种基于ORB特征的即时定位与地图构建的方法，本发明利用ORB特征进行特征点匹配，通过改进的RANSAC(随机采样一致)来删除图像的误匹配，并且利用BA来进行位姿优化，从而获得更精确的定位和更好的鲁棒性，详见下文描述：

一种基于ORB特征的即时定位与地图构建的方法，所述方法包括：

利用改进的RANSAC算法删除误匹配，利用高效的透视n点定位和迭代最近点求解相机的位姿，使得在丢帧的情况下获得精确的位姿，恢复定位；

在进行闭环检测时，利用改进的RANSAC算法结合相似变换检测闭环，并且进行闭环的矫正来消除累积误差。

进一步地，所述改进的RANSAC算法具体为：

首先对匹配的图像做个预处理，删除汉明距离大于阈值的匹配点；

然后在剩下的匹配点中随机选取样本，用估计模型对所有点进行计算，如果内点数超过最好模型的内点数，则更新最好模型，并保存内点；用最小二乘在新的阈值下估计新的模型，并再次保存内点；

然后在内点中随机选取样本，进行迭代，直到最好的模型出现或者达到最高迭代次数，最后对最终模型通过捆集调整方式进行位姿的优化。

其中，所述用估计模型对所有点进行计算，如果内点数超过最好模型的内点数，则更新最好模型，并保存内点的步骤具体为：

其中，|model_new|是新估计的模型，|model_best|是当前最好的模型，|inliers_new|是新的模型下内点数目，|inliers_best|是目前最好的模型下内点数。

进一步地，所述用最小二乘在新的阈值下估计新的模型，并保存内点的步骤具体为：

model_sql＝sql(|inliers_best|*m)

其中，sql是用最小二乘法估计位姿的函数，m为倍数。

其中，所述在内点中随机选取样本，进行迭代，直到最好的模型出现或者达到最高迭代次数具体为：

model_best＝iterate(inliers_in，model_best，counts)

其中，iterate是迭代函数，inliers_in是在inliers_best中随机选出的内点，model_best是限制条件，表示最佳模型出现则停止迭代，counts表示迭代次数的上限。

本发明提供的技术方案的有益效果是：

1、本方法利用ORB特征进行特征点匹配，通过改进的RANSAC来删除图像的误匹配，并且利用BA来进行位姿优化，从而获得更精确的定位和更好的鲁棒性；

2、本方法无需GPU加速，在CPU上就可以实现实时性；本方法比目前流行的SLAM所采用的算法有着更好的鲁棒性；

3、本方法比目前流行的SLAM所采用的算法精度更高。

附图说明

图1为一种基于ORB特征的即时定位与地图构建的方法的流程图；

图2为使用本发明删除误匹配前后的对比图；

其中，(a)为未筛选无匹配图；(b)为用本发明筛选误匹配后的示意图。

图3为TUM数据集的轨迹对比图；

其中，实线是实际轨迹图，虚线是本发明估计的轨迹图，点线是RM方法估计的轨迹图。

(a)为序列fr1_desk主视图；(b)为序列fr1_desk俯视图；(c)为序列fr2_xyz主视图；(d)为序列fr2_xyz俯视图；(e)为序列fr3_office主视图；(f)为序列fr3_office俯视图。

图4为KITTI数据集的轨迹对比图；

其中，实线是实际轨迹图，虚线是本发明估计的轨迹图，点线是RM方法估计的轨迹图。

(a)为序列05；(b)为序列07。

图5为EuRoC数据集的估计轨迹图；

其中，虚线是实际轨迹图，实线是本发明估计的轨迹图。

(a)为序列V1_01_easy的示意图；(b)为序列V1_02_medium的示意图；(c)为序列V2_02_medium的示意图；(d)为序列MH_03_medium的示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面对本发明实施方式作进一步地详细描述。

由于基于SIFT特征的方法时间消耗很大，不能实现SLAM的实时性。基于滤波器的方法会造成很大的累积误差，本发明实施例旨在开发一种基于ORB特征的即时定位与地图构建的技术，ORB特征具有视点不变性和光照不变性。该方法利用一种改进的RANSAC算法删除误匹配，利用EPnP(一种高效的透视n点定位)和ICP(迭代最近点)求解相机的位姿，使得在丢帧的情况下可以获得精确的位姿，恢复定位。在进行闭环检测时，利用该方法求取Sim3(相似变换)检测闭环并且进行闭环的矫正来消除累积误差。该方法无需GPU进行加速，在CPU上就可以实现实时性，该方法既可以满足工程应用对定位精度及鲁棒性的要求，也能满足其对实时性的要求，可以广泛用于各种基于位置的服务中。

实施例1

本发明实施例提供了一种基于ORB特征的即时定位与地图构建的方法，该ORB特征具有视点不变性和光照不变性，参见图1，该方法包括以下步骤：

101：利用改进的RANSAC算法删除误匹配，利用EPnP和ICP求解相机的位姿，使得在丢帧的情况下可以获得精确的位姿，恢复定位；

102：在进行闭环检测时，利用改进的RANSAC算法结合Sim3检测闭环并且进行闭环的矫正来消除累积误差。

本方法分为前端、后端、建图和回环检测。前端用来特征匹配和计算位姿，后端进行位姿优化，建图是建立稀疏的点云图，回环检测用来消除累积误差。程序分为三个线程：tracking，local mapping和loop closing。

综上所述，本方法利用ORB特征进行特征点匹配，通过改进的RANSAC来删除图像的误匹配，并且利用BA来进行位姿优化，从而获得更精确的定位和更好的鲁棒性。

实施例2

下面结合具体的计算公式、实例、以及图1对实施例1中的方案进行进一步地介绍，详见下文描述：

一、Tracking(跟踪)

此线程通过局部地图之间的特征匹配得到相机的位姿估计，用纯运动的BA最小化重投影误差。

视觉传感器采集图像信息，获取图像的ORB特征，进行特征点匹配，进行初始化，开始追踪。位姿追踪有三种方式：一是根据匀速运动的模型获取位姿，假设相机是匀速运动的。二是追踪关键帧，根据关键帧之间的特征匹配来获取位姿。三是从重定位中恢复位姿，这种情况在追踪失败的时候执行，获取局部地图与当前帧的匹配，最小化重投影误差优化位姿。如果很长时间没有新的关键帧插入，局部地图会空闲，这时要加入新的关键帧。

二、Local Mapping(局部地图)

检查队列的关键帧数，更新地图点和关键帧的关联。剔除地图中新添加的质量不好的地图点，剔除判断如下：

1)将地图点投影到当前帧上，如果投影到该图像之外，就将其舍弃；

2)计算视线方向向量v与地图点云平均视线方向向量n的夹角，舍弃n·v<cos(60°)的点云；

3)计算地图点到相机中心的距离d，认为[dmin,dmax]是尺度不变的区域，若d不在这个区域，就将其舍弃；

4)计算图像的尺度因子，为d/dmin；

5)将地图点的特征描述子与还未匹配上的特征描述子进行比较，根据步骤4)计算得到的尺度因子，找到重投影误差最小的匹配。

这样，相机位姿就能通过匹配所有地图点，最终被优化。如果地图点过于少的话，要生成新的地图点补充局部地图，和共视程度(说明：由之前的帧间匹配，得到当前帧的地图点，通过遍历这些地图点，得到也能观测到这些地图点的关键帧，观测到的地图点越多，共视程度越高)比较高的关键帧通过三角测量(求特征点的空间位置的一种方法)恢复出一些地图点。进行地图点的融合(融合相同的地图点)，检查当前帧与相邻帧重复的地图点，如果90％以上的地图点被至少三个关键帧观测到，则认为该关键帧为冗余关键帧，剔除冗余的关键帧。

三、Loop Closing(闭环)

该线程用来检测回环，通过图优化矫正累计误差。该线程会在检测到闭环之后激发全局的BA。主要分为两步：回环的检测与确认、修正和优化位姿。

回环检测使用词袋向量的方案，初输入一个新的关键帧后，将它和这个词袋向量进行对比，对比的对象是在共视图中相邻的点。只对比新增的关键点和新增的关键帧相邻的点，得到一个最小值S。

这个最小值S在其他所有的关键帧中，同时会和新增的关键帧进行词袋向量的对比；如果这个值比刚才计算的最小值更小，那这些关键帧点就不会考虑到闭环检测中。

当检测到闭环之后，计算相似矩阵的变换获取回环的累积误差，然后对整幅图进行一个全局性的最优化，消除该累积误差，完成闭环优化。闭环优化过程如下：

1)根据共视关系更新当前帧与其他关键帧之间的连接；

2)通过位姿传播，得到Sim3优化后，修正与当前帧相连的关键帧的位姿以及它们的地图点；

3)检查当前帧的地图点与闭环匹配帧的地图点是否存在冲突，对冲突的地图点进行替换或填补；

4)将通过闭环相连的关键帧的局部地图点投影到这些关键帧中，进行地图点的检查与替换；

5)更新当前关键帧之间的共视相连关系，得到因闭环时地图点融合而新得到的连接关系；

6)进行Essential Grap优化；

7)添加当前帧与闭环匹配帧之间的边；

8)进行全局BA优化。

上述描述是说明整个方法的流程和三个线程主要的任务，以下介绍本方法的详细操作，详见下文描述：

在tracking线程的位姿跟踪中，如果出现丢帧跟踪失败的情况，会用到本发明实施例提出的改进的RANSAC算法结合EPnP和ICP求解位姿；在loopclosing线程的闭环检测中，会用到本发明实施例提出的改进的RANSAC算法结合Sim3来检测闭环。

其中，根据改进后的RANSAC算法结合EPnP和ICP求解位姿、以及结合Sim3来检测闭环的操作流程，采用本领域技术人员通常的处理流程，本发明实施例对此不做赘述。

其中，本发明实施例提出的一种改进的RANSAC算法原理如下：

在模型中估计参数时，假设第k次获得最佳的模型。设p是从数据集中随机选取的点均是内点的概率，w是每次从数据集中选取一个内点的概率，如下式所示：

w＝局内点的数目/数据集的数目

通常情况下，本发明实施例事先并不知道w的值，但是可以给出一些鲁棒的值。假设估计模型需要选定n个点，w_n是所有n个点均为局内点的概率；(1-w_n)是n个点中至少有一个点为局外点的概率。(1-w_n)^k表示算法永远都不会选择到n个点均为局内点的概率，它和1-p相同。因此，

1-p＝(1-w_n)^k (1)

本发明实施例对上式的两边取对数，得出：

本发明实施例做了一个改进，首先对匹配的图像做个预处理，

其中，thr是汉明距离的阈值，distance是匹配点的汉明距离，该处理删除汉明距离大于阈值的匹配点。

然后在剩下的匹配点中随机选取样本，估计模型，用最小二乘在新的阈值下估计新的模型，并保存内点，然后在内点中随机选取样本，进行迭代，直到最好的模型出现或者达到最高迭代次数，最后对模型做一个BA优化，算法流程如下：

1)随机选取n个样本估计模型；

model＝estimate(samples_n)

其中，estimate是位姿估计函数，n的值为4，samples为匹配的特征点。

2)用估计模型对所有点进行计算，如果内点数超过最好模型的内点数，则更新最好模型，并保存内点；

其中，|model_new|是新估计的模型，|model_best|是当前最好的模型，|inliers_new|是新的模型下内点数目，|inliers_best|是目前最好的模型下内点数。

3)用最小二乘法以新的阈值(以前阈值的m倍)去估计新的模型，并保存内点；

model_sql＝sql(|inliers_best|*m)

其中，sql是用最小二乘法估计位姿的函数。

4)在内点中随机选取n个样本，迭代估计模型，误差减少则更新模型，直到最好的模型出现或者达到最高迭代次数为止。

model_best＝iterate(inliers_in，model_best，counts)

其中，iterate是迭代函数，inliers_in是在inliers_best中随机选出的内点，model_best是限制条件，表示最佳模型出现则停止迭代，counts表示迭代次数的上限。

5)得到最优模型后，对最好的模型做一个BA优化。

model_best＝optimize(model_best)

其中，optimize是BA优化函数。

综上所述，本方法不用GPU进行加速，在CPU上就可以实现实时性，该方法既可以满足工程应用对定位精度及鲁棒性的要求，也能满足其对实时性的要求，可以广泛用于各种基于位置的服务中。

实施例3

下面结合具体的实验数据、图2-图5、以及表1和表2对实施例1和2中的方案进行可行性验证，详见下文描述：

为验证本方法的效果，本发明实施例在多种情况下进行实验，利用室内的手持RGB-D摄像机采集数据集TUM(慕尼黑工业大学)，室外高速公路和城市交通的数据集KITTI(卡尔斯鲁厄理工学院和丰田技术学院)以及微型飞行器(MAV)采集的数据集EuRoC进行对比试验。参数设置如下：p的值为0.99，最大总的迭代次数为300，(说明：迭代限制分两个，内层是假设的第k次，最外层是整个系统总的迭代次数，最大设为300，达到300的话，认为跟踪失败，需要重启)随机选取的样本数为4，每次从数据集中选取一个内点的概率w为0.5，判断最大误差的阈值为5.991σ²，其中，σ＝0.3。迭代时随机选取的样本数为：min(14,I/2),其中I是上一轮的内点数，阈值改变的倍数为0.7。

定性角度，图2显示了fr1_desk的两帧图片的匹配，(a)是没有经过删除误匹配的结果，(b)是用本方法删除误匹配后的结果。方框内可以看出，本方法大大减少了误匹配的点，使得匹配朝着同样的方向。

图3显示了本方法与文献[15]所提出的算法(在这里简称为RM方法)的定位效果对比。本方法画出TUM数据集的fr1_desk，fr2_xyz，和fr3_office的实际轨迹(实线)，ORB的估计轨迹(点线)以及本发明的估计轨迹(虚线)的俯视图和主视图，图3中进行了局部放大，可以看出，本发明更加接近实际的轨迹。

图4是KITTI数据集的05和07序列(都有闭环)，可以看出，本方法优于RM方法，尤其在转角的部分比较明显。图5是EuRoC数据集的轨迹，可以看出，本方法估计的轨迹很接近实际的轨迹。证明了本方法具有更强的鲁棒性。

表1 TUM数据集估计轨迹的均方根误差(单位：米)

序列	fr1_desk	fr2_xyz	fr3_office	fr3_nst
					RM方法	0.016	0.004	0.010	0.018
本发明方法	0.014	0.003	0.009	0.011

表2 KITTI数据集估计轨迹的均方根误差(单位：米)

序列	05	07	08
				RM方法	0.94	0.58	4.8
本发明方法	0.89	0.52	4.8

从定量角度，表1为TUM数据集两种估计的轨迹均方根误差的对比，可以看出，本方法的定位更加精确，尤其是fr1_desk和fr3_nst，比RM方法的均方根误差分别降低了12.5％and 38.9％。

表2为KITTI数据集两种估计的轨迹均方根误差的对比，可以看出在05和07序列，本方法的均方根误差比RM方法小，分别精确了5厘米和6厘米，序列08和RM方法达到了相同的定位误差。证明了本方法具有很强的容错性。

在实际应用中，需对本方法涉及到的相关参数进行设置。在本实验中，汉明距离的阈值thr设为100，p的取值范围为(0.95-0.99)，本实验选择0.99，最大总的迭代次数设为300，迭代时随机选取的样本数为：min(14,I/2),其中I是上一轮的内点数，阈值改变的倍数为0.7。实验结果证明，在该参数设置下，本方法在定位精度、实时性、鲁棒性及容错性方面均取得非常好的效果。

本领域技术人员可以理解附图只是一个优选实施例的示意图，上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于ORB特征的即时定位与地图构建的方法，其特征在于，所述方法包括：

利用改进的RANSAC算法删除误匹配，利用高效的透视n点定位和迭代最近点求解相机的位姿，使得在丢帧的情况下获得精确的位姿，恢复定位；

在进行闭环检测时，利用改进的RANSAC算法结合相似变换检测闭环，并且进行闭环的矫正来消除累积误差。

2.根据权利要求1所述的一种基于ORB特征的即时定位与地图构建的方法，其特征在于，所述改进的RANSAC算法具体为：

首先对匹配的图像做个预处理，删除汉明距离大于阈值的匹配点；

然后在剩下的匹配点中随机选取样本，用估计模型对所有点进行计算，如果内点数超过最好模型的内点数，则更新最好模型，并保存内点；用最小二乘在新的阈值下估计新的模型，并再次保存内点；

然后在内点中随机选取样本，进行迭代，直到最好的模型出现或者达到最高迭代次数，最后对最终模型通过捆集调整方式进行位姿的优化。

3.根据权利要求2所述的一种基于ORB特征的即时定位与地图构建的方法，其特征在于，所述用估计模型对所有点进行计算，如果内点数超过最好模型的内点数，则更新最好模型，并保存内点的步骤具体为：

其中，|model_new|是新估计的模型，|model_best|是当前最好的模型，|inliers_new|是新的模型下内点数目，|inliers_best|是目前最好的模型下内点数。

4.根据权利要求3所述的一种基于ORB特征的即时定位与地图构建的方法，其特征在于，所述用最小二乘在新的阈值下估计新的模型，并保存内点的步骤具体为：

model_spl＝spl(|inliers_best|*m)

其中，sql是用最小二乘法估计位姿的函数，m为倍数。

5.根据权利要求1所述的一种基于ORB特征的即时定位与地图构建的方法，其特征在于，所述在内点中随机选取样本，进行迭代，直到最好的模型出现或者达到最高迭代次数具体为：

model_best＝iterate(inliers_in，model_best，counts)

其中，iterate是迭代函数，inliers_in是在inliers_best中随机选出的内点，model_best是限制条件，表示最佳模型出现则停止迭代，counts表示迭代次数的上限。