CN113888603A - 基于光流跟踪和特征匹配的回环检测及视觉slam方法 - Google Patents

Abstract

本发明涉及一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，属于机器人定位与导航技术领域。本发明设计了基于光流跟踪和特征匹配的视觉SLAM算法框架，该框架分别对视觉里程计、回环检测等模块进行了优化设计。在视觉里程计模块，将LK光流法引入到基于点线特征的数据关联中，设计了基于光流跟踪和点线特征匹配的视觉里程计算法，提高了算法的快速性；在回环检测模块，将图像信息的全局特征算子和局部特征算子相结合，设计了基于点线特征的回环检测算法，提高了算法的准确性。最后开展了算法的实现工作和实验对比研究，该实验在室内EuRoC数据集上进行，实验结果表明本文算法相较于ORB‑SLAM算法具有更高的定位精度和鲁棒性能。

Description

基于光流跟踪和特征匹配的回环检测及视觉SLAM方法

技术领域

本发明属于机器人定位与导航技术领域，具体涉及一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法。

背景技术

同步定位与地图构建(Simultaneous Localization and Mapping，SLAM)作为机器人在未知环境下实现自主作业的关键技术，日益成为机器人领域的研究热点。同时，基于视觉传感器的SLAM技术凭借其低成本、轻量化以及具有丰富的感知能力等优点得到了广大学者的青睐。

光流跟踪描述了图像像素点随着时间在图像之间的运动过程，即：对于前一帧中的像素点，确定出在后一帧即将出现的位置。光流跟踪匹配点特征的过程就是跟踪像素点的过程，相较于描述子匹配，光流跟踪匹配省去了提取点特征、计算描述子等操作，降低了计算量，并且不会出现类似于描述子匹配中存在的误匹配问题。但是，光流跟踪法的缺点在于容易受光照影响，弱纹理场景效果不好，并且当相机发生大尺度移动时，无法很好地跟踪。

回环检测技术为SLAM问题带来更多的约束关系，从而得到更准确的位姿估计。在视觉SLAM中，其本质是一种比较图像间相似性的算法，然而，当场景中的物体具有高度相似性时，基于点特征的回环检测方法容易产生感知混淆的问题，从而做出错误的回环判断。

一直以来，点特征是视觉SLAM中应用最为广泛的图像特征，ORB-SLAM正是一款基于点特征设计的开源算法，它具有定位精度高、逻辑结构清晰、以及代码易读性强等诸多优点。然而，ORB-SLAM对点特征依赖较大，导致其在弱纹理场景、光线较暗场景以及相机运动速度过快等情况下定位性能不佳。

发明内容

要解决的技术问题

为了解决现有回环检测方法在相似度高的场景中检测精度不高的问题以及现有SLAM方法在弱纹理场景、光线较暗场景、相机运动速度过快等情况下定位性能不佳的问题，考虑到结构化的场景中除了点特征外，还存在着丰富的线特征，两者能起到信息互补的作用，因此，本发明在ORB-SLAM算法框架下引入线特征，设计了基于光流跟踪和特征匹配的视觉SLAM算法，提高了ORB-SLAM算法的定位精度和鲁棒性能。

技术方案

一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，其特征在于步骤如下：

步骤1：双目相机采集图像，对图像做预处理；

步骤2：在相邻帧之间，点特征采用LK光流法跟踪匹配，线特征采用描述子的方式匹配；

步骤3：基于相邻帧之间点线特征的重投影误差构建代价函数F₁，对当前帧的位姿进行优化调整；

步骤4：判断当前帧是否为关键帧，如果是，则执行步骤5；如果不是，则设定前一关键帧为当前帧的参考关键帧，然后跳转至步骤9；

步骤5：提取当前帧的ORB点特征，并计算BRIEF描述子；

步骤6：在局部关键帧之间，点线特征均采用描述子的方式匹配；

步骤7：基于局部关键帧之间点线特征的重投影误差构建代价函数F₂，再次优化调整位姿；

步骤8：回环检测，调整全局位姿；

步骤9：输出全部图像帧的位姿。

本发明进一步的技术方案：所述步骤1中的图像预处理方法为：首先将图像转换为灰度图像，然后利用自适应直方图均衡算法对图像做增强处理。

本发明进一步的技术方案：所述步骤2中，包括如下步骤：

步骤2.1：采用LK光流法，由当前帧的左目图像跟踪前一帧的左目图像，进行点特征的跟踪匹配；

步骤2.2：筛选并补充ORB点特征；

步骤2.3：采用LSD线特征提取算法以及LBD线特征描述算法，在当前帧的左、右目图像之间提取线特征并计算线特征描述子；

步骤2.4：相邻帧的左目图像之间通过LBD描述子匹配的方式进行线特征匹配。

本发明进一步的技术方案：所述步骤3中，包括如下步骤：

步骤3.1：根据相邻帧之间点线特征的数据关联关系，由光束平差法构建基于点线特征重投影误差的代价函数F₁；

步骤3.2：采用高斯牛顿法求解F₁，对当前帧的位姿进行优化调整。

本发明进一步的技术方案：所述步骤4中，判断当前帧是否为关键帧具体为：

关键帧同时满足了以下条件：

1)与上一个关键帧之间至少含有20个图像帧；

2)至少成功追踪到了50个点特征和15条线特征；

3)与上一个关键帧之间的共视点、线特征信息少于75％。

本发明进一步的技术方案：所述步骤6中，包括了如下步骤：

步骤6.1：当前帧的左、右目图像之间的点特征通过BRIEF描述子匹配的方式，进行双目立体匹配；

步骤6.2：局部关键帧的左目图像之间的点、线特征均分别通过BRIEF、LBD描述子匹配的方式，进行双目立体匹配。

本发明进一步的技术方案：所述步骤7中，包括了如下步骤：

步骤7.1：根据局部关键帧之间点线特征的数据关联关系，由光束平差法构建基于点线特征重投影误差的代价函数F₂；

步骤7.2：采用高斯牛顿法求解F₂，对局部关键帧的位姿进行优化调整。

本发明进一步的技术方案：所述步骤8中，包括了如下步骤：

步骤8.1：利用全局特征算子对图像进行结构校验，选取回环候选帧；

步骤8.2：利用局部特征算子比较图像之间的相似性，验证回环候选帧是否是真回环。

有益效果

本发明提出的一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，设计了基于光流跟踪和特征匹配的视觉SLAM算法框架，该框架分别对视觉里程计、回环检测等模块进行了优化设计。在视觉里程计模块，将LK光流法引入到基于点线特征的数据关联中，设计了基于光流跟踪和点线特征匹配的视觉里程计算法，提高了算法的快速性；在回环检测模块，将图像信息的全局特征算子和局部特征算子相结合，设计了基于点线特征的回环检测算法，提高了算法的准确性。最后开展了算法的实现工作和实验对比研究，该实验在室内EuRoC数据集上进行，实验结果表明本发明算法相较于ORB-SLAM算法具有更高的定位精度和鲁棒性能。

本发明方法相比于现有技术，具有以下技术特征：1.相邻帧之间点线特征分别采用光流跟踪匹配以及描述子匹配的方式建立数据关联，构建代价函数F₁；2.局部关键帧之间点线特征均采用描述子匹配的方式建立数据关联，构建代价函数F₂；3.基于点线特征进行回环检测，调整全局位姿。本发明由于采用了上述技术特征，在相似度高的场景中，提高了回环检测的检测精度；在弱纹理场景、光线较暗场景、相机运动速度过快等情况下提高了定位精度以及鲁棒性能。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明实施例的流程图；

图2为线特征的重投影误差示意图；

图3为回环检测算法流程图；

图4为ORB全局特征算子提取示意图；

图5为融合点线特征的视觉词典；

图6为回环检测算法P-R曲线图：(a)City Centre实验结果；(b)New College实验结果；

图7为在MH_01_easy图像序列上的实验结果：(a)APE对比图；(b)误差数据对比图；

图8为在V1_01_easy图像序列上的实验结果：(a)APE对比图；(b)误差数据对比图；

图9为在MH_03_difficult图像序列上的实验结果：(a)APE对比图；(b)误差数据对比图；

图10为在MH_04_difficult图像序列上的实验结果：(a)APE对比图；(b)误差数据对比图；

图11为在V2_01_easy图像序列上的实验结果：(a)APE对比图；(b)误差数据对比图；

图12为在V2_03_difficult图像序列上的实验结果：(a)APE对比图；(b)误差数据对比图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

为了验证本发明所提出的基于光流跟踪和特征匹配的回环检测及视觉SLAM方法的定位性能，在室内EuRoC数据集上进行了与ORB-SLAM算法的对比实验。实验所用计算机的配置为：CPU为A4-5000，主频为1.50GHz，内存为8GB，系统为Ubuntu16.04。

图1为本发明实施例的流程图。如图1所示，本发明所提出的一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，包括以下步骤：

步骤1：读取双目图像，对图像进行预处理；

从EuRoC图像序列中读取双目图像，其中，图像预处理方法为：首先将图像转换为灰度图像，然后利用自适应直方图均衡算法对图像做增强处理。

步骤2：在相邻帧之间，点特征采用LK光流法跟踪匹配，线特征采用描述子的方式匹配，包括如下步骤：

步骤2.1：采用LK光流法，由当前帧的左目图像跟踪前一帧的左目图像，进行点特征的跟踪匹配；

步骤2.2：筛选并补充ORB点特征；

步骤2.3：采用LSD线特征提取算法以及LBD线特征描述算法，在当前帧的左、右目图像之间提取线特征并计算线特征描述子；

步骤2.4：相邻帧的左目图像之间通过LBD描述子匹配的方式进行线特征匹配。

步骤3：基于相邻帧之间点线特征的重投影误差构建代价函数F₁，对当前帧的位姿进行优化调整，包括如下步骤：

步骤3.1：根据相邻帧之间点线特征的数据关联关系，由光束平差法构建基于点线特征重投影误差的代价函数F₁；

点特征与位姿的约束关系如下：

P_c＝[P_x P_y P_z]^T＝R_cwP_w+t_cw (1)

其中，P_c是相机坐标系下的空间点特征，P_x，P_y，P_z分别是空间点在X，Y，Z轴上的坐标。R_cw是从世界坐标对齐到相机坐标的旋转矩阵，t_cw是平移向量，P_w是世界坐标系下的空间点特征。

其中，Φ表示针孔相机的投影模型，T_cw表示相机位姿，f_x，f_y是焦距，即相机的焦点到成像平面的距离，c_x，c_y是像素与成像平面之间原点的平移。

其中，Φ表示针孔相机的投影模型。

空间点特征的重投影误差定义如下：

其中，(x_i,j,y_i,j)表示第j个点特征在第i帧图像上的像素坐标。

图2为线特征的重投影误差示意图，其中，

是第i帧图像所观测到的第k条线特征在世界坐标系下的两个3D端点，

是将s_w,k,e_w,k重投影到第i帧图像上的两个2D端点，

是s_w,k,e_w,k在第i帧图像中对应的两个端点，

分别是s_i,k,e_i,k对应的齐次坐标点，d_s,d_e分别表示两个端点

各自到重投影线段

的距离。

π是由两个齐次坐标点^hs_i,k,^he_i,k以及相机的光心c_i所确定的平面，用l_i,k表示平面π的单位法向量：

空间线特征的重投影误差定义如下：

另外，^hΦ为Φ的齐次坐标形式，即：

在假设观测误差为高斯分布的情况下，可以构建综合了点线特征的重投影误差的代价函数F₁：

其中，

分别表示点特征、线特征重投影误差的信息矩阵，H_P,H_l分别表示点特征、线特征的Huber鲁棒核函数，ρ_l,I_l分别表示点特征集、线特征集。

步骤3.2：采用高斯牛顿法求解F₁，对当前帧的位姿进行优化调整。

最小化代价函数F₁的过程就是在求解待估计状态量的过程。为了利用非线性优化方法优化代价函数，需要先计算出误差函数关于状态变量的雅克比矩阵。

当相机发生微小位姿变化ξ∈SE(3)时(ξ是变换矩阵T_iw所对应的李代数)，记：g_P是P_w,j在相机坐标系下对应的坐标，g_s,g_e分别是s_w,k,e_w,k在相机坐标系下对应的坐标：

g_P＝error^ξP_w,j＝R_iwP_w,j+t_iw (7)

g_s＝error^ξs_w,k＝R_iws_w,k+t_iw (8)

g_e＝error^ξe_w,k＝R_iwe_w,k+t_iw (9)

点特征重投影误差

关于相机微小位姿变化ξ∈SE(3)的雅克比矩阵

如下所示：

其中，

点特征重投影误差

关于空间点坐标P_w的雅克比矩阵

如下所示：

线特征重投影误差

关于相机微小位姿变化ξ∈SE(3)的雅克比矩阵

如下所示：

其中，

在上述过程中，[]_∧表示进行反对称矩阵的转换，具体转换如下式：

线特征重投影误差

关于空间线特征的两个端点s_w,k,e_w,k的雅克比矩阵分别为

和

如下所示：

计算出雅克比矩阵后，采用高斯牛顿法求解优化问题。

步骤4：判断当前帧是否为关键帧，如果是，则执行步骤5；如果不是，则设定前一关键帧为当前帧的参考关键帧，然后跳转至步骤9；

关键帧选择是指剔除冗余的图像帧，保留具有代表性的图像帧。本发明所选取的关键帧同时满足了以下条件：

1)与上一个关键帧之间至少含有20个图像帧；

2)至少成功追踪到了50个点特征和15条线特征；

3)与上一个关键帧之间的共视点、线特征信息少于75％。

根据上述关键帧的选取条件，如果当前帧不是关键帧，那么将上一个关键帧设定为当前帧的参考关键帧，并计算两帧之间的相对位姿量，将其记录下来。若当前帧位姿为T_C∈SE(3)，参考关键帧位姿为T_R∈SE(3)，则相对位姿量为：

T_Rel＝T_C(T_R)^-1 (16)

步骤5：提取当前帧的ORB点特征，并计算BRIEF描述子；

对当前帧图像建立图像金字塔，分别在金字塔图像上进行影像分块，利用ORB点特征提取算法以及BRIEF点特征描述算法在每个分块里进行点特征的提取与描述。

步骤6：在局部关键帧之间，点线特征均采用描述子的方式匹配，包括如下步骤：

步骤6.1：当前帧的左、右目图像之间的点特征通过BRIEF描述子匹配的方式，进行双目立体匹配；

步骤6.2：局部关键帧的左目图像之间的点、线特征均分别通过BRIEF、LBD描述子匹配的方式，进行双目立体匹配。

此处的局部关键帧指的是由当前帧和之前的10个关键帧共同组成的11个关键帧。

步骤7：基于局部关键帧之间点线特征的重投影误差构建代价函数F₂，再次优化调整位姿；

步骤7.1：根据局部关键帧之间点线特征的数据关联关系，由光束平差法构建基于点线特征重投影误差的代价函数F₂；

其中，

分别是点特征、线特征重投影误差的信息矩阵，H_P,H_l分别是点特征、线特征的Huber鲁棒核函数，ρ_l,I_l,κ_l分别表示点特征集、线特征集以及局部关键帧。

步骤7.2：采用高斯牛顿法求解F₂，对局部关键帧的位姿进行优化调整。

在步骤3中，根据相邻帧之间的点、线特征的数据关联关系，构建出了代价函数F₁，从而优化求解当前帧的位姿；在步骤7中，是根据局部关键帧之间的点、线特征的数据关联关系，再次构建了新的代价函数F₂，并采用高斯牛顿法对局部关键帧的位姿进行了优化调整。

每一个非关键帧都有相对应的参考关键帧，并且两者之间的相对位姿量为T_Rel。在步骤7.2中，对局部关键帧位姿进行了优化调整，因而，当参考关键帧位姿T_R被优化调整为T_R′时，根据相对位姿量T_Rel对非关键帧的位姿也进行调整，即：T_C被调整为T_C′，其中，T_C′＝T_RelT_R′。通过设定参考关键帧的方式，对非关键帧的位姿进行了再次优化调整。

步骤8：回环检测，调整全局位姿；

图3为回环检测算法流程图。如图3所示，回环检测算法主要通过结构校验模块和词袋模块实现，其中，结构校验模块利用全局特征算子对图像进行结构校验，选取回环候选帧；词袋模块利用局部特征算子(视觉单词)比较图像之间的相似性，验证回环候选帧是否是真回环。

步骤8.1：利用全局特征算子对图像进行结构校验，选取回环候选帧；

图4为ORB全局特征算子提取示意图。如图4所示，首先对图像进行灰度化、均衡化、归一化处理，并将处理之后的图像的中心位置作为ORB点特征的位置；然后将归一化的图像作为ORB点特征的邻域区域，把计算的ORB局部特征描述子作为整幅图像的全局描述子。

步骤8.2：利用局部特征算子比较图像之间的相似性，验证回环候选帧是否是真回环。

基于图像的局部特征算子设计基于点线特征的词袋模型，并根据词袋模型计算图像间的相似性，主要包括以下部分：视觉词典、计算权重和计算相似性。

(1)视觉词典

图5为融合点线特征的视觉词典，其生成过程如下：

第一步：提取KITTI数据集中所有的点特征、线特征，并分别计算二进制的BRIEF描述子、LBD描述子；

第二步：为BRIEF描述子、LBD描述子添加额外标志位区分点线特征；

第三步：使用K-means++算法对所有描述子聚类，生成一个深度为5、分支为8的词典树，该词典树便是融合了点线特征的视觉词典。

(2)计算权重

生成词典之后，采用TF-IDF模型计算单词的权重值。对于单词w_i，TF值和IDF值由以下公式计算：

其中，n表示图像中所有单词的数量，n_i表示单词w_i在图像中出现的次数，N表示词典中所有单词的数量，N_i表示单词w_i在词典中出现的次数。

单词w_i的权重η_i等于TF值与IDF值的乘积：

η_i＝TF_i×IDF_i (20)

加入权重之后，对于某幅图像A，用词典向量进行表示：

通过视觉词典，用单个向量v_A描述了一幅图像A，向量v_A是一个稀疏向量，它的非零部分表示了图像A中含有哪些单词，而这些单词的权值为TF-IDF的值。

(3)计算相似性

由于不同的图像可以使用不同的词典向量表示，因此，通过它们词典向量之间的距离来计算图像间的相似性。如(22)式所示，本发明采用L₁范数计算两个词典向量之间的距离。

本发明对图像点特征、线特征的相似性得分分别设置了μ和1-μ的权重，μ可以根据经验设置，在室内环境下μ可设置小一点。如(23)式所示，s(v₁,v₂)便是基于图像点线特征计算出的相似性得分，其范围为[0～1]。

s(v₁,v₂)＝μs(v₁,v₂)_p+(1-μ)s(v₁,v₂)_l (23)

实验选用了KITTI数据集，进行基于ORB点特征和LSD线特征的综合特征训练，构建了基于点线特征的视觉词典，KITTI中的11组图像序列包含了城市、乡村、公路以及动态和静态多种场景类型，确保了训练出的视觉词典的可靠性。为了评价本节设计的基于点线特征的回环检测算法的性能，分别在City Centre和New College数据集上进行了与ORB-SLAM中的回环检测算法(DBow)的对比实验。其中，DBow是一个基于点特征的回环检测算法。

表1数据集介绍

City Centre和New College是目前使用最广泛的用来评价回环检测算法的数据集，如表1所示，分别包含了2474和2146幅双目图像，左目图像用于建立图像数据库，右目图像作为查询图像。实验中，相似度系数μ的取值范围是[0～1]，通过设置不同的相似度阈值系数，绘制出对应的P-R曲线。

图6为回环检测算法P-R曲线图，由图6可知，在City Centre场景上，当准确率为100％时，本发明算法的召回率为64％，相较于DBow算法，提升了23％；在New College场景上，当准确率为100％时，本发明算法的召回率为72％，相较于DBow算法，提升了35％。相较于New College场景，DBow算法在City Centre场景中的召回率更高，原因在于：CityCentre场景中有着丰富的点特征，而New College场景以线特征为主，点特征较少。同时，无论在相似度低的场景(City Centre)，还是在相似高的场景(New College)，本节设计的回环检测算法相较于ORB-SLAM中的DBow算法均有着更高的回环检测性能。

步骤9：输出全部图像帧的位姿。

循环步骤1至步骤9对EuRoC图像序列进行处理，计算每一帧图像对应的位姿，并将位姿结果记录在txt文档中。

接下来，根据记录在txt文档中的位姿结果，对本算法的定位性能进行评价分析。

在定位精度分析的过程中，采用绝对位姿误差(Absolute Pose Error，APE)作为评价指标对本算法的定位精度进行评价。APE是通过计算估计位姿与真实位姿的距离得到的，i时刻，真实位姿为T_i∈SE(3)，估计位姿为T_i′∈SE(3)，则APE为：APE_i＝T_i(T_i′)^-1。

(1)强、弱纹理场景的实验对比

MH_01_easy序列是在纹理较强的场景中采集的，场景中包含了丰富的点、线特征，V1_01_easy是在纹理较弱的场景中采集的，场景中点特征较少。图7、图8分别是本发明算法和ORB-SLAM算法在MH_01_easy、V1_01_easy图像序列上的实验结果。图(a)是两种算法的APE对比图，图(b)是两种算法的误差数据对比图。

为了更好地对比本发明算法和ORB-SLAM算法的定位精度，将图7、图8所示实验结果总结到表2中，分析了两种算法各自在MH_01_easy序列和V1_01_easy序列下实验的均方根定位误差(RMSE)、最大定位误差(MAX)和最小定位误差(MIN)。

表2强、弱纹理场景定位精度对比

由表2可知，在纹理较强、较弱的场景中，本发明算法的定位精度均高于ORB-SLAM。在纹理较强的场景下(MH_01_easy)，本发明算法的最大定位误差约为0.08036m，比ORB-SLAM算法减小了0.0017m，本发明算法的均方根误差约为0.03548m，相较于ORB-SLAM算法提高了5.99％的定位精度。在纹理较弱的场景下(V1_01_easy)，本发明算法的最大定位误差约为0.37278m，比ORB-SLAM算法减小了0.15724m，本发明算法的均方根误差约为0.07139m，相较于ORB-SLAM算法提高了59.44％的定位精度。

实验结果表明，场景纹理的强、弱会对视觉SLAM算法的定位精度产生不同的影响，其中，在弱纹理场景中，ORB-SLAM算法的定位精度要低于在纹理丰富的场景中的定位精度，本发明算法将线特征引入ORB-SLAM中，能够显著提高ORB-SLAM算法在弱纹理场景中的定位精度。

(2)光线明、暗场景的实验对比

MH_03_difficult、MH_04_difficult图像序列分别是在光线明亮和光线较暗的场景中采集的。图9、图10分别是本发明算法和ORB-SLAM算法在MH_03_difficult、MH_04_difficult图像序列上的实验结果。图(a)是两种算法的APE对比图，图(b)是两种算法的误差数据对比图。

为了更好地对比本发明算法和ORB-SLAM算法的定位精度，将图9、图10所示实验结果总结到表3中，分析了两种算法各自在MH_03_difficult序列和MH_04_difficult序列下实验的均方根定位误差(RMSE)、最大定位误差(MAX)和最小定位误差(MIN)。

表3光线明、暗场景定位精度对比

由表3可知，在光线明亮、较暗的场景中，本发明算法的定位精度均高于ORB-SLAM。在光线明亮的场景下(MH_03_difficult)，本发明算法的最大定位误差约为0.09398m，比ORB-SLAM算法减小了0.00515m，本发明算法的均方根误差约为0.03700m，相较于ORB-SLAM算法提高了17.5％的定位精度。在光线较暗的场景下(MH_04_difficult)，本发明算法的最大定位误差约为0.18717m，比ORB-SLAM算法减小了0.00889m，本发明算法的均方根误差约为0.05626m，相较于ORB-SLAM算法提高了26.81％的定位精度。

实验结果表明，场景中光线的明、暗会对视觉SLAM算法的定位精度产生不同的影响，其中，在光线较暗的场景中，ORB-SLAM算法的定位精度要低于在光线明亮的场景中的定位精度，本发明算法将线特征引入ORB-SLAM中，能够显著提高ORB-SLAM算法在光线较暗的场景中的定位精度。

(3)飞行速度快、慢的实验对比

采集V2_01_easy图像序列时，飞行器以0.33m/s的线速度，0.28rad/s的角速度慢速飞行，采集V2_03_difficult图像序列时，飞行器以0.75m/s的线速度，角速度为0.66rad/s的速度快速飞行，由于飞行速度的不同，导致了前者拍摄的图像是清晰的，后者拍摄的图像出现了运动模糊的情况。图11、图12分别是发明算法和ORB-SLAM算法在V2_01_easy、V2_03_difficult图像序列上的实验结果。图(a)是两种算法的APE对比图，图(b)是两种算法的误差数据对比图。

为了更好地对比本发明算法和ORB-SLAM算法的定位精度，将图11、图12所示实验结果总结到表4中，分析了两种算法各自在V2_01_easy序列和V2_03_difficult序列下实验的均方根定位误差(RMSE)、最大定位误差(MAX)和最小定位误差(MIN)。

表4快速、慢速飞行的定位精度对比

由表4可知，在飞行器慢速飞行情况下(V2_01_easy序列)，相机拍摄的图像是清晰的，本发明算法的最大定位误差约为0.16109m，比ORB-SLAM算法减小了0.06047m，本发明算法的均方根误差约为0.05686m，相较于ORB-SLAM算法提高了19.6％的定位精度。在快速飞行的情况下(V2_03_difficult序列)，相机拍摄的图像出现了运动模糊的情况，本发明算法的最大定位误差约为0.44133m，比ORB-SLAM算法减小了1.18591m，本发明算法的均方根误差约为0.16804m，相较于ORB-SLAM算法提高了77.09％的定位精度。

实验结果表明，飞行器飞行速度的快、慢会对视觉SLAM算法的定位精度产生不同的影响，其中，在飞行器飞行速度较快的情况下，ORB-SLAM算法的定位精度要低于在飞行器飞行速度较慢的定位精度，本发明算法将线特征引入ORB-SLAM中，能够显著提高ORB-SLAM算法在飞行器快速飞行的情况下的定位精度。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。

Claims (8)

1.一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，其特征在于步骤如下：

步骤1：双目相机采集图像，对图像做预处理；

步骤2：在相邻帧之间，点特征采用LK光流法跟踪匹配，线特征采用描述子的方式匹配；

步骤3：基于相邻帧之间点线特征的重投影误差构建代价函数F₁，对当前帧的位姿进行优化调整；

步骤4：判断当前帧是否为关键帧，如果是，则执行步骤5；如果不是，则设定前一关键帧为当前帧的参考关键帧，然后跳转至步骤9；

步骤5：提取当前帧的ORB点特征，并计算BRIEF描述子；

步骤6：在局部关键帧之间，点线特征均采用描述子的方式匹配；

步骤7：基于局部关键帧之间点线特征的重投影误差构建代价函数F₂，再次优化调整位姿；

步骤8：回环检测，调整全局位姿；

步骤9：输出全部图像帧的位姿。

2.根据权利要求1所述的一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，其特征在于所述步骤1中的图像预处理方法为：首先将图像转换为灰度图像，然后利用自适应直方图均衡算法对图像做增强处理。

3.根据权利要求1所述的一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，其特征在于所述步骤2中，包括如下步骤：

步骤2.1：采用LK光流法，由当前帧的左目图像跟踪前一帧的左目图像，进行点特征的跟踪匹配；

步骤2.2：筛选并补充ORB点特征；

步骤2.3：采用LSD线特征提取算法以及LBD线特征描述算法，在当前帧的左、右目图像之间提取线特征并计算线特征描述子；

步骤2.4：相邻帧的左目图像之间通过LBD描述子匹配的方式进行线特征匹配。

4.根据权利要求1所述的一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，其特征在于所述步骤3中，包括如下步骤：

步骤3.1：根据相邻帧之间点线特征的数据关联关系，由光束平差法构建基于点线特征重投影误差的代价函数F₁；

步骤3.2：采用高斯牛顿法求解F₁，对当前帧的位姿进行优化调整。

5.根据权利要求1所述的一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，其特征在于所述步骤4中，判断当前帧是否为关键帧具体为：

关键帧同时满足了以下条件：

1)与上一个关键帧之间至少含有20个图像帧；

2)至少成功追踪到了50个点特征和15条线特征；

3)与上一个关键帧之间的共视点、线特征信息少于75％。

6.根据权利要求1所述的一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，其特征在于所述步骤6中，包括了如下步骤：

步骤6.1：当前帧的左、右目图像之间的点特征通过BRIEF描述子匹配的方式，进行双目立体匹配；

步骤6.2：局部关键帧的左目图像之间的点、线特征均分别通过BRIEF、LBD描述子匹配的方式，进行双目立体匹配。

7.根据权利要求1所述的一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，其特征在于所述步骤7中，包括了如下步骤：

步骤7.1：根据局部关键帧之间点线特征的数据关联关系，由光束平差法构建基于点线特征重投影误差的代价函数F₂；

步骤7.2：采用高斯牛顿法求解F₂，对局部关键帧的位姿进行优化调整。

8.根据权利要求1所述的一种基于光流跟踪和特征匹配的回环检测及视觉SLAM方法，其特征在于所述步骤8中，包括了如下步骤：

步骤8.1：利用全局特征算子对图像进行结构校验，选取回环候选帧；

步骤8.2：利用局部特征算子比较图像之间的相似性，验证回环候选帧是否是真回环。

Images