CN112509044A - 一种基于点线特征融合的双目视觉slam方法 - Google Patents

Abstract

本发明涉及一种基于点线特征融合的双目视觉同步定位与地图绘制SLAM方法，属于移动机器人自主定位导航技术应用领域。本发明首先在ORB‑SLAM2系统的双目结构基础上进行扩展，采用相同的主体机构，加入线特征信息，在特征跟踪线程中，对输入的图像帧进行图像预处理；其次，特征点匹配时，利用PROSAC算法剔除误匹配点；此外，在特征的融合过程中引入加权思想，对点线特征权重进行合理分配；最后通过点、线特征权重划分的重投影误差得到相机的位姿。本发明提出的方法在精度、鲁棒性和实时性等方面都取得了不错的效果，能够在不同的环境下得到满意的定位精度，线特征的引入也解决了视觉SLAM系统在低纹理场景无法正常工作的情况。

Description

一种基于点线特征融合的双目视觉SLAM方法

技术领域

本发明属于移动机器人自主定位导航技术应用领域，涉及一种基于点线特征融合的双目视觉SLAM方法。

背景技术

目前主流的视觉SLAM方法分为光流法、直接法和特征点法。光流法基于灰度不变假设，把特征点法中的描述子和匹配换成了光流跟踪，利用光流跟踪获取数据关联，并进行位姿计算。直接法是假设同一个空间点的像素灰度，在不同视角的图像中是固定不变的，通过最小化光度误差来估计位姿，计算量较大。而特征点法是最流行的方法之一，它是先提取图像的特征、计算其描述子，再进行特征匹配，最后根据匹配得到的数据关联信息进行相机的位姿计算。大多数视觉SLAM方法使用图像中的角点作为特征进行定位跟踪，然而往往由于点特征不足或者分布不均而导致定位精度低、系统鲁棒性和实时性差，甚至系统都无法正常工作。而与点特征相比，线特征承载更多的结构信息，且人造场景中具有丰富的边缘和线性形状的规则结构，线特征尤为明显。针对这个问题，文献“Ruben Gomez-Ojeda等人，Robust Stereo Visual Odometry through a Probabilistic Combination of Pointsand Line Segments.IEEE International Conference on Robotics and Automation(ICRA),2016”提出利用点线特征融合的方式弥补一些场景点特征的不足或分布不均匀，但没有对线特征提取进行额外处理，需要付出高昂的计算代价，影响系统实时性。文献“谢晓佳等人，Robust Visual SLAM with Point and Line Features.2017”改进了空间线的表示方法，首次采用正交点表示作为最小化参数来模拟线特征以及视觉SLAM中的点特征，并分析得出了关于线参数的重投影误差的雅可比矩阵，改进了SLAM的解决方案，但是该方法直接将线特征匹配的误差引入系统，不利于SLAM系统的准确性。

综上所述，目前在视觉SLAM技术领域存在的问题是：1)视觉SLAM中，过于依赖场景特征，大部分视觉SLAM采用的为点特征，当场景中纹理信息缺失时，点特征的数量往往较少，难以追踪足够多的有效特征点，系统定位精度差。2)线段特征虽然易于提取，但是线特征的直接引入，没有进行额外处理，计算复杂度高，影响系统实时性。3)移动机器人面临的环境复杂多变，导致视觉SLAM系统的鲁棒性差。因此，本发明在实际应用中有非常重要的应用价值。

发明内容

有鉴于此，本发明的目的在于提供一种基于点线特征融合的双目视觉SLAM方法，该方法对低纹理场景点特征不足或分布不均匀导致系统定位精度和鲁棒性差；线特征和线匹配误差的直接引入，没有额外处理，导致系统计算复杂度高、准确性低，不利于系统实时性等问题，引入梯度密度滤波器加速线特征提取并提高线匹配的准确度。特征匹配时，采用PROSAC算法剔除误匹配点对，提高定位精度。此外，引入点线特征的加权融合方式，从而对点线特征的权重进行合理分配，提高双目SLAM中融合点、线特征后的系统鲁棒性和定位精度。

为达到上述目的，本发明提供如下技术方案：

一种基于点线特征融合的双目视觉同步定位与地图绘制SLAM方法，该方法包括以下步骤：

S1：引入线特征信息，并对输入的图像帧进行图像预处理；

S2：进行点线特征的检测、描述和双目匹配，并利用PROSAC方法剔除误匹配；

S3：估计相机的位姿；

S4：优化局部地图中的点线和位姿，同时负责维护点线的增删和关键帧的插入和剔除，并根据重投影误差，使用光束平差法BA进行局部地图优化；

S5：回环检测。

可选地，步骤S1具体为：

利用梯度密度滤波器对图像帧进行滤波处理，梯度密度滤波器的计算公式如下：

其中G_ij为像素点(i,j)的梯度值，G_s为强度阈值；ρ_ij为以像素点(i,j)为中心的n×n区域内的梯度密度，ρ为像素区域梯度密度阈值，D_ij为线段检测的有效区域标识。

可选地，步骤S2具体为：

线特征的提取采用LSD算法，用于检测图像中局部的轮廓，并引入线段之间的角度差异，通过距离、角度信息筛选出割裂线段进行合并；

在点特征匹配时，采用PROSAC算法剔除误匹配；PROSAC算法在图像的匹配中剔除误匹配点，在图像的匹配过程中针对每一对特征点都会建立欧式距离的比值β，计算公式如下：

式中，d_min为最小欧式距离，d_min2为次小欧式距离，并且利用质量因子γ来衡量其匹配质量，计算公式如下：

并将该质量因子引入特征点分级中。

可选地，步骤S3具体为：

通过点、线特征权重划分的重投影误差得到相机的位姿估计，

点、线特征权重划分由公式：

M_p＞M:w_p＝0.8,w_l＝0.2

决定，其中w_p和w_l为点、线特征权重，M_p表示点匹配成功的个数，M为点特征的个数。

可选地，步骤S4具体为：

应用光束平差法BA进行局部地图优化的过程中，不管是采用高斯牛顿还是LM算法，都需反复地在当前估计值附近对代价函数进行线性展开，即计算重投影误差，最终求点线重投影误差关于状态变量的雅可比矩阵，其点、线重投影误差关于状态变量的雅可比矩阵计算公式分别如下：

其中f_x，f_y为相机内参，R_cw为旋转矩阵；

为线特征重投影误差对投影直线l'的雅可比矩阵，

为投影直线l'对普吕克坐标L_c的雅可比矩阵，

为普吕克坐标L_c对位姿δ_ξ的雅可比矩阵。

可选地，步骤S5具体为：

根据字典树进行回环检测，计算回环帧与当前帧的SE3变换，设检测到回环时，两帧之间的SE3变换计算过程为：

1)对当前帧和闭环帧进行特征匹配，利用构建好的视觉词典将线特征划分到字典树，进行线特征的匹配；

2)得到点、线的匹配对后，通过3D-2D的方式求解位姿，结合PROSAC算法剔除其中的错误数据关联；

3)对于点特征的匹配对，通过EPnP求解；对于线特征的匹配对，则通过两帧双目图像中三焦点张量关系求解。

本发明的有益效果在于：1)本发明改善了基于特征点法的视觉SLAM系统定位精度低、鲁棒性和实时性差、计算复杂度高等难题，能够在不同的复杂环境下达到满意的定位跟踪效果；2)本发明提出的图像预处理流程能够很好的解决视觉SLAM系统相机位姿估计准确度低的现象，使系统处理图像特征的速度更加快速，特征匹配更加准确。首次将点线特征的权重分配方式引入到双目视觉SLAM中，提高了双目SLAM中融合点、线特征后的系统鲁棒性和定位精度。仿真结果表明，本发明与ORB-SLAM2、LSD-SLAM、PL-SLAM等算法相比具有更高的性能。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1是本发明的基于点线特征融合的双目视觉SLAM算法流程图；

图2是本发明的图像预处理流程图；

图3是本发明提供的PROSAC算法流程图；

图4是本发明提供的基于PROSAC算法剔除误匹配点对的对比图，(a)是基于PROSAC算法剔除前误匹配点对图，(b)是基于PROSAC算法剔除后误匹配点对图；

图5是本发明采用的双目视觉SLAM方法与PL-SLAM方法的轨迹与真实轨迹之间的均方根误差(RMSE)对比示意图，(a)是本发明采用的双目视觉SLAM方法的轨迹与真实轨迹之间的均方根误差(RMSE)图，(b)是PL-SLAM方法的轨迹与真实轨迹之间的均方根误差(RMSE)图；

图6是本发明采用双目视觉SLAM方法与PL-SLAM方法的轨迹与真实轨迹之间的对比示意图，(a)是本发明采用的双目视觉SLAM方法的轨迹和PL-SLAM方法的轨迹与真实轨迹示意图，(b)是本发明采用的双目视觉SLAM方法和PL-SLAM方法在x，y，z轴上的轨迹与真实轨迹示意图；

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

请参阅图1～图6，为一种基于点线特征融合的双目视觉SLAM方法。

图1是本发明实施例提供的基于点线特征融合的双目视觉SLAM算法流程图，如图所示，本发明实施例提供的基于点线特征融合的双目视觉SLAM算法包括：

特征跟踪线程中，引入线特征信息，并对输入的图像帧进行图像预处理，其图像预处理流程如图2所示。利用梯度密度滤波器对图像帧进行滤波处理，加速线特征提取并提高线匹配的准确度。其梯度密度滤波器的计算公式如下：

其中G_ij为像素点(i,j)的梯度值，G_s为强度阈值；ρ_ij为以像素点(i,j)为中心的n×n区域内的梯度密度，ρ为像素区域梯度密度阈值。为了判别梯度密度过高的像素区域，以进行滤波处理，减少图像中特征稠密区域对于特征匹配的影响。将梯度密度ρ_ij大于ρ的像素区域视为特征稠密区域，线段检测时视为无效区域，不进行处理，即D_ij为线段检测有效区域标识。

接着进行点线特征的检测和描述以及双目匹配。线特征的提取采用LSD算法，LSD线段检测器能在线性时间内直接检测图像中局部的轮廓，无需参数调节，并引入线段之间的角度差异，通过距离、角度等信息筛选出割裂线段进行合并。在点特征匹配时，采用PROSAC算法剔除误匹配，其PROSAC算法流程图如图3所示。PROSAC算法在图像的匹配中可以有效地剔除误匹配点，在图像的匹配过程中针对每一对特征点都会建立欧式距离的比值β，计算公式如下：

式中，d_min为最小欧式距离，d_min2为次小欧式距离。比值越小，则说明距离越小，特征点匹配的质量也就越好。利用质量因子γ来衡量其匹配质量，计算公式如下：

将该质量因子引入特征点分级中，使得特征点匹配质量提高。

利用点、线特征权重划分的重投影误差得到相机的位姿估计。点、线特征权重划分由以下公式得到：

M_p＞M:w_p＝0.8,w_l＝0.2

其中w_p和w_l为点、线特征权重，M_p表示点匹配成功的个数，M为点特征的个数。

局部建图线程里，在求解BA的过程中，不管是采用高斯牛顿还是LM算法，都需反复地要在当前估计值附近对代价函数进行线性展开，即计算重投影误差最终是求点线重投影误差关于状态变量的雅可比矩阵，其点、线重投影误差关于状态变量的雅可比矩阵计算公式分别如下：

其中f_x，f_y为相机内参，R_cw为旋转矩阵；

为线特征重投影误差对投影直线l'的雅可比矩阵，

为投影直线l'对普吕克坐标L_c的雅可比矩阵，

为普吕克坐标L_c对位姿δ_ξ的雅可比矩阵。

在回环检测线程中，根据字典树进行回环检测，计算回环帧与当前帧的SE(3)变换。当检测到回环时，两帧之间的SE(3)变换计算过程为：

1)对当前帧和闭环帧进行特征匹配，仿照ORB-SLAM2的做法，利用构建好的视觉词典将线特征划分到字典树的某一层，进行线特征的匹配；

2)得到点、线的匹配对后，通过3D-2D的方式求解位姿，结合PROSAC算法剔除其中的错误数据关联；

3)对于点特征的匹配对，通过EPnP求解，对于线特征的匹配对，则通过两帧双目图像中三焦点张量关系求解。

本发明使用KITTI和TUM数据集中的数据对提出的基于点线特征融合的双目视觉SLAM方法进行了实验，KITTI数据集通过自主驾驶车进行采集，而TUM数据集由MicrosoftKinect RGB-D相机采集室内办公室场景得到，由深度图和RGB图组成，是各种SLAM/VO算法实验中常用的数据集。

图4表示的是本发明引入的PROSAC算法剔除误匹配前后结果对比图，(a)是基于PROSAC算法剔除前误匹配点对图，(b)是基于PROSAC算法剔除后误匹配点对图；从图中可以看到在特征点匹配的时候出现了很多的误匹配，叉号标记的就是一些误匹配对。经过PROSAC算法剔除误匹配后，匹配效果得到了提升，使相机的位姿估计更加准确，从而达到提高定位精度的目的。

图5是本发明与PL-SLAM方法的轨迹与真实轨迹之间的均方根误差(RMSE)，(a)是本发明采用的双目视觉SLAM方法的轨迹与真实轨迹之间的均方根误差(RMSE)图，(b)是PL-SLAM方法的轨迹与真实轨迹之间的均方根误差(RMSE)图；采用了KITTI数据集的01序列。可以看到本发明所采用的方法精度优于PL-SLAM方法的精度，并且本发明的方法在初始以及各个阶段都能进行良好的跟踪定位，由此验证了本发明的方法具有更好的鲁棒性。

图6是本发明采用双目视觉SLAM方法与PL-SLAM方法的轨迹与真实轨迹之间的对比示意图，(a)是本发明采用的双目视觉SLAM方法的轨迹和PL-SLAM方法的轨迹与真实轨迹示意图，(b)是本发明采用的双目视觉SLAM方法和PL-SLAM方法在x，y，z轴上的轨迹与真实轨迹示意图；采用了KITTI数据集的00序列，从图中可以清晰的看到本发明的轨迹更接近于真实轨迹，且鲁棒性更好。尽管在某些地方出现了一定的偏差，但不影响跟踪定位的整体效果。

表1统计了本发明的方法与其它不同算法在TUM数据集下的均方根误差。从表1中的数据可以看到，本发明的方法在几个数据集中都取得了较好的结果，对位姿估计的精度有一定的提升。本发明的方法与LSD-SLAM比较，平均定位误差减小了86.3％，并且在大多数情况下也要优于ORB-SLAM2算法。对于具有动态物体的场景(fr2_desk_person)，本发明提出的方法明显优于LSD-SLAM，定位均方根误差减小了86.9％，使用线特征的优势体现了出来。

表1 TUM数据集下RMSE(单位：m)

本发明提出的一种基于点线特征融合的双目视觉SLAM算法有效解决了在低纹理场景系统无法正常工作的情况，提高了系统定位精度和鲁棒性。首次将点线特征权重引入到双目视觉SLAM系统中，可以有效避免点特征和线特征误差叠加导致的数据关联失效。仿真结果表明，本发明提出的点线特征融合的双目视觉SLAM算法在精度、鲁棒性等方面都取得了不错的效果，具有良好的性能。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (6)

1.一种基于点线特征融合的双目视觉同步定位与地图绘制SLAM方法，其特征在于：该方法包括以下步骤：

S1：引入线特征信息，并对输入的图像帧进行图像预处理；

S2：进行点线特征的检测、描述和双目匹配，并利用PROSAC方法剔除误匹配；

S3：估计相机的位姿；

S4：优化局部地图中的点线和位姿，同时负责维护点线的增删和关键帧的插入和剔除，并根据重投影误差，使用光束平差法BA进行局部地图优化；

S5：回环检测。

2.根据权利要求1所述的一种基于点线特征融合的双目视觉SLAM方法，其特征在于：所述步骤S1具体为：

利用梯度密度滤波器对图像帧进行滤波处理，梯度密度滤波器的计算公式如下：

其中G_ij为像素点(i,j)的梯度值，G_s为强度阈值；ρ_ij为以像素点(i,j)为中心的n×n区域内的梯度密度，ρ为像素区域梯度密度阈值，D_ij为线段检测的有效区域标识。

3.根据权利要求1所述的一种基于点线特征融合的双目视觉SLAM方法，其特征在于：所述步骤S2具体为：

线特征的提取采用LSD算法，用于检测图像中局部的轮廓，并引入线段之间的角度差异，通过距离、角度信息筛选出割裂线段进行合并；

在点特征匹配时，采用PROSAC算法剔除误匹配；PROSAC算法在图像的匹配中剔除误匹配点，在图像的匹配过程中针对每一对特征点都会建立欧式距离的比值β，计算公式如下：

式中，d_min为最小欧式距离，d_min2为次小欧式距离，并且利用质量因子γ来衡量其匹配质量，计算公式如下：

并将该质量因子引入特征点分级中。

4.根据权利要求1所述的一种基于点线特征融合的双目视觉SLAM方法，其特征在于：所述步骤S3具体为：

通过点、线特征权重划分的重投影误差得到相机的位姿估计，

点、线特征权重划分由公式：

M_p＞M:w_p＝0.8,w_l＝0.2

M_p＜M:

决定，其中w_p和w_l为点、线特征权重，M_p表示点匹配成功的个数，M为点特征的个数。

5.根据权利要求1所述的一种基于点线特征融合的双目视觉SLAM方法，其特征在于：所述步骤S4具体为：

应用光束平差法BA进行局部地图优化的过程中，不管是采用高斯牛顿还是LM算法，都需反复地在当前估计值附近对代价函数进行线性展开，即计算重投影误差，最终求点线重投影误差关于状态变量的雅可比矩阵，其点、线重投影误差关于状态变量的雅可比矩阵计算公式分别如下：

其中f_x，f_y为相机内参，R_cw为旋转矩阵；

为线特征重投影误差对投影直线l'的雅可比矩阵，

为投影直线l'对普吕克坐标L_c的雅可比矩阵，

为普吕克坐标L_c对位姿δ_ξ的雅可比矩阵。

6.根据权利要求1所述的一种基于点线特征融合的双目视觉SLAM方法。其特征在于：所述步骤S5具体为：

根据字典树进行回环检测，计算回环帧与当前帧的SE3变换，设检测到回环时，两帧之间的SE3变换计算过程为：

1)对当前帧和闭环帧进行特征匹配，利用构建好的视觉词典将线特征划分到字典树，进行线特征的匹配；

2)得到点、线的匹配对后，通过3D-2D的方式求解位姿，结合PROSAC算法剔除其中的错误数据关联；

3)对于点特征的匹配对，通过EPnP求解；对于线特征的匹配对，则通过两帧双目图像中三焦点张量关系求解。

Images