CN112802196B - 基于点线特征融合的双目惯性同时定位与地图构建方法 - Google Patents

Abstract

本发明公开了一种基于点线特征融合的双目惯性同时定位与地图构建方法，在线特征提取后加入长度抑制、近线合并和断线拼接方法。在基于优化的滑动窗口中有效融合了点、线和IMU数据信息，解决了弱纹理环境下点特征视觉SLAM算法精度不高且不稳定的问题，实现了更高精度的位姿估计，利用长度抑制、近线合并和断线拼接策略，在保证快速提取的同时，优化了传统算法线段提取质量不高的问题，降低了系统线特征的误匹配率，更能适应室内弱纹理和无纹理场景。

Description

基于点线特征融合的双目惯性同时定位与地图构建方法

技术领域

本发明涉及一种同时同步定位与地图构建方法，具体涉及一种基于点线特征融合的双目惯性同时定位与地图构建方法，属于机器人控制技术领域。

背景技术

同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)被认为是实现移动机器人自主作业的核心技术，已被广泛应用于无人机、无人车和虚拟现实等领域。对于室内环境，由于建筑物会对GPS信号进行遮蔽，无人机定位多采用SLAM技术，且为了克服单一传感器自身精度的不足，常采用多传感器融合策略，如视觉惯性就是一种有效的融合方法，且相机和惯性测量单元(Inertial Measurement Unit，IMU)均具有重量轻、成本低的特点，符合室内无人机轻量化和低功耗的需求。

基于特征点的SLAM系统在遭遇场景纹理缺失，如走廊、楼道等弱纹理或无纹理环境的时候，特征点提取效果较差，甚至可能追踪不到足够多的特征点来估计相机运动，从而导致运动估计失败。与点特征相比，室内人造场景具有丰富的边缘和线性结构，线特征尤为明显，线特征具有较好的光照变化不变性和旋转不变性，可以提供更多的几何结构信息，因此线特征可以对点特征进行补充，提高基于单一特征点SLAM系统的鲁棒性和准确性。

传统方法大多采用LSD(Line Segment Detector)算法作为线特征检测的工具，但是LSD算法的设计初衷是用来表征场景的结构特征，提取速度较慢，且无参数调整的LSD算法在面对复杂背景或有噪声的图像时，易检测出过多的短线段特征，不仅浪费了线段检测、描述和匹配的计算资源，且易产生较多离群值，导致定位精度明显下降。此外，LSD算法通常存在线段重复检测和过分割问题，即存在过多邻近相似线段和长线段易被分割为多个短线段的情况，使后续的线段匹配任务变得复杂，从而增加了SLAM系统的不确定性。

由于上述原因，本发明人对现有的同时定位与地图构建方法进行了深入研究，提出了一种基于点线特征融合的双目惯性同时定位与地图构建方法。

发明内容

为了克服上述问题，本发明人进行了锐意研究，提出了一种基于点线特征融合的双目惯性同时定位与地图构建方法，包括以下步骤：

S1、前端视觉惯性里程计；

S2、后端优化；

S3、三维环境建图。

步骤S1中，在所述前端视觉惯性里程计中，采用双目相机采集相机前方左、右目图片，作为视觉惯性同时定位与地图构建系统的检测分析输入图像；

所述前端视觉惯性里程计基于点线特征进行,包括步骤S11、点特征的提取和线特征的提取。

进一步地，步骤S1中，线特征提取后，具有步骤：

S12、对线特征长度进行约束；

通过对线特征长度进行约束，去除短的线段，只保留满足约束条件的长线段。

更进一步地，所述约束可以表示为：

其中，l_min表示最小线特征长度阈值，W_I和H_I分别为图像宽度和高度，

表示向上取整，η为比例系数

优选地，步骤S1中，线特征提取后，具有步骤：

S13、近线合并及断线拼接；

通过对相近线特征进行合并，对分割的线特征进行拼接，从而减少线特征的数量，提高所提取线特征的质量。

进一步地，通过以下步骤对线特征进行近线合并及断线拼接：

S131、对线特征进行排序；

S132、对线特征进行筛选；

S133、对线特征进行合并拼接；

S134、完成对所有线特征进行合并拼接。

在步骤S131中，按线特征长度对提取出的线特征进行排序，获得线特征序列{L₁，L₂，...，L_n}，L₁外的线特征表示为剩余线特征集合：

L＝{L₂，L₃，...，L_n}。

在步骤S132中，所述筛选包括角度筛选，所述角度筛选可以表示为：

其中，L_α表示经过角度筛选得到候选线段组，L_m表示剩余线特征集合中不同的线特征；

θ_min为衡量线特征角度相近程度的角度筛选阈值；

μ为自适应比例系数。

优选地，所述筛选还包括水平距离筛选，所述水平距离筛选可以表示为：

其中，线段L_i和L_j为近线合并改进措施下需要合并的两种线段，其首尾端点分别为

和

d_min为衡量线段特征水平距离接近程度的距离筛选阈值；

所述筛选还包括垂直距离筛选，所述垂直距离筛选可以表示为：

其中，线段L_u和L_v为断线拼接改进措施下需要拼接的两种线段，其首尾端点分别为

和

d′_min为衡量线段特征垂直距离接近程度的距离筛选阈值。

根据本发明，在步骤S133中，将L₁加入经过筛选的候选线段集中，组成新的线段特征集合{L₁,L_γ1,L_γ2}，从中分别选择首尾端点中偏离最远的两端点作为新线特征的首尾端点，合成新的线特征L_M，重新计算其角度为θ_M，

若满足θ_M＜θ_min，合并成立，以合并线特征L_M替换线段集合{L₁,L_γ1,L_γ2}；

若不满足θ_M＜θ_min，放弃合并，此时合并前后角度相差过大，合并结果偏离原线段。

本发明所具有的有益效果包括：

(1)根据本发明提供的基于点线特征融合的双目惯性同时定位与地图构建方法，在基于优化的滑动窗口中有效融合了点、线和IMU数据信息，解决了弱纹理环境下点特征视觉SLAM算法精度不高且不稳定的问题，实现了更高精度的位姿估计。

(2)根据本发明提供的基于点线特征融合的双目惯性同时定位与地图构建方法，利用长度抑制、近线合并和断线拼接策略，在保证快速提取的同时，优化了传统算法线段提取质量不高的问题，降低了系统线特征的误匹配率，在保证快速性的同时，提取线段质量更高，更能适应室内弱纹理和无纹理场景。

(3)根据本发明提供的基于点线特征融合的双目惯性同时定位与地图构建方法，该方法能够在弱纹理和无纹理的室内结构化场景下，快速且准确地提取更多有效的线特征，并与点特征和IMU预积分数据进行多源信息融合，得到全局一致的相机位姿数据。

附图说明

图1示出根据本发明一种优选实施方式的基于点线特征融合的双目惯性同时定位与地图构建方法示意图；

图2示出根据本发明一种优选实施方式的基于点线特征融合的双目惯性同时定位与地图构建方法中近线合并示意图；

图3示出根据本发明一种优选实施方式的基于点线特征融合的双目惯性同时定位与地图构建方法中断线拼接结构示意图；

图4示出对比例2中线特征提取效果图；

图5示出对比例3中线特征提取效果图；

图6示出实施例1中线特征提取效果图。

具体实施方式

下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

本发明提供的基于点线特征融合的双目惯性同时定位与地图构建方法，如图1所示，包括以下步骤：

S1、前端视觉惯性里程计；

S2、后端优化；

S3、三维环境建图。

步骤S1中，在所述前端视觉惯性里程计中，采用双目相机采集相机前方左、右目图片，作为视觉惯性同时定位与地图构建系统的检测分析输入图像。

双目相机相较于RGBD相机测量范围更大，抗光照变化能力更强，与单目相机相比具有尺度恢复能力，鲁棒性更好。

进一步地，所述前端视觉惯性里程计基于点线特征进行,包括步骤S11、点特征和线特征的提取。

根据本发明，步骤S11中，在图像中并行检测、跟踪点特征及线特征。

在本发明中，对图像中点特征的检测、跟踪方法不做特别限定，本领域技术人员可根据经验自由选择，例如按照Shi J.Good features to track[C]//1994ProceedingsofIEEE conference on computer vision and pattern recognition.IEEE,1994:593-600中的方法提取角点作为特征点，利用KLT光流法实现特征点跟踪，并基于反向光流(例如Baker S,Matthews I.Lucas-kanade 20years on:A unifying framework[J].International journal of computervision,2004,56(3):221-255)跟踪剔除差异较大的点。

进一步地，在点特征跟踪中，采用前后帧和左右帧两种跟踪模式：以前帧为基础，跟踪当前帧；以当前左帧为基础，跟踪当前右帧。

在本发明中，线特征与左侧相机图像的前后帧进行匹配，具体地，对左侧每帧图像进行线特征的提取，对于帧间匹配，在本发明中，使用LBD描述符来匹配前一帧中检测到的线段，从而实现线特征匹配。

具体使用LBD描述符的方式采用Zhang L,Koch R.An efficient and robustline segmentmatching approach based on LBD descriptor and pairwise geometricconsistency[J].Journal of Visual Communication and Image Representation,2013,24(7):794-805中介绍的方法进行，在本发明中不做赘述。

进一步地，所述线特征提取包括如下子步骤：

S101、图像灰度化：对于传感器输入的彩色图根据图像各个通道的采样值进行加权平均转化为灰度图。

S102、降噪：使用高斯滤波器滤除噪声并平滑图像，以防止由噪声引起的错误检测。

S103、计算图像中每个像素的梯度值和方向：利用边缘差分算子计算水平方向的差分g_x(x，y)和垂直方向的差分g_y(x，y)，得到梯度值g(x，y)和方向θ，可以表示为：

进一步地，所述边缘差分算子可以是Rober、Prewitt或Sobel，在本发明中不做特别限制。

S104、非极大值抑制。

对图像进行梯度计算后，仅仅基于梯度值提取的边缘仍然很模糊，采用非极大值抑制将局部最大值之外的所有梯度值抑制为0。

所述非极大值抑制是一种计算机视觉任务中广泛使用的算法，在本发明中，对具体的非极大值抑制方法不做特别限定，本领域技术人员可根据实际需求进行设定。

S105、双阈值检测。

应用双阀值策略，滤除噪声或颜色变化引起的小梯度值而保留大梯度值。

所述双阀值策略是指通过一个高阀值和一个低阀值来区分边缘像素：如果边缘像素点梯度值大于高阀值，则被认为是强边缘点；如果边缘梯度值小于高阀值，大于低阀值，则被认为是弱边缘点；小于低阀值的点则被抑制掉。

S106、滞后边界跟踪。

搜索所有连通的弱边缘点，如果一条连通的弱边缘的任何一个点和强边缘点连通，则保留这条弱边缘，否则抑制这条弱边缘。

S107、获取线段特征参数。

通过检测到的线段首尾两端点坐标，通过下式获取线段的长度l_i和线段与水平方向的夹角θ_i：

其中，i表示不同的线段，k表示线段序号，线特征l首尾端点坐标分别为A(x_A，y_A)和B(x_B，y_B)。

发明人发现，若采用经过上述步骤提取的线特征，直接进行线特征匹配，会检测出过多的短线段特征，使得匹配浪费巨大的计算资源，计算时间长，严重影响了视觉惯性里程计的实时性，而且增加了线段误匹配的概率，导致定位精度明显下降。

发明人经过锐意研究，发现对于点线特征融合的同时定位与地图构建系统，线特征提供约束以提高位姿估计的精度，因此线段检测算法只需要检测出场景中明显的线段特征，而不需要用线特征精细地描述场景。

进一步地，发明人还发现，长线段特征更稳定，且更容易在多帧图像中被重复检测到，数量越多，对定位精度的贡献越

在本发明中，优选地，在步骤S107之后，任选地，还具有步骤：

S12、对线特征长度进行约束。

通过对线特征长度进行约束，去除短的线段，只保留满足约束条件的长线段，所述约束可以表示为：

其中，l_min表示最小线特征长度阈值，W_I和H_I分别为图像宽度和高度，

表示向上取整，η为比例系数。

优选地，η取值为0.03～0.2，更优选为0.08；例如左侧相机图像宽度为640像素，高度为480像素，η＝0.08时，则L_min＝0.08×480＝38.4，即剔除长度小于38.4像素的线段，保留长度大于38.4像素的线段进行匹配。

发明人还发现，上述步骤提取的线特征，经常将一段较长的线段分割成几个较短的线段，此外在一些薄区域的边界边缘处会多次检测，导致检测到的线段可能离得比较近，存在大量相似线特征，这些被检测到的线段的存在往往使后续的线段匹配任务变得复杂，从而增加了系统精度的不确定性。

优选地，在步骤S107之后，还具有步骤

S13、近线合并及断线拼接。

通过对相近线特征进行合并，对分割的线特征进行拼接，从而减少线特征的数量，提高所提取线特征的质量，如图2、3所示。优选地，通过如下步骤对线特征进行近线合并及断线拼接：

S131、对线特征进行排序；

S132、对线特征进行筛选；

S133、对线特征进行合并拼接；

S134、完成对所有线特征进行合并拼接。

在步骤S131中，按线特征长度对提取出的线特征进行排序，优选按照从长到短的顺序进行降序排列，获得线特征序列{L₁，L₂，...，L_n}，其中L₁表示线特征长度最长的线特征。

由于较长的线段往往来自具有连续强梯度的图像区域，所以从最长的线特征L₁开始进行更加可靠。

进一步地，将L₁外的线特征表示为剩余线特征集合：

L＝{L₂，L₃，...，L_n} (4)

其中，n为线特征总数。

在步骤S132中，所述筛选包括角度筛选，所述角度筛选可以表示为：

其中，L_α表示经过角度筛选得到候选线段组，L_m表示剩余线特征集合中不同的线特征；

θ_min为衡量线特征角度相近程度的角度筛选阈值；

μ为自适应比例系数，与线特征长度l_i成反比，l_i越小，系数μ越大，即线特征长度越短，可合并性越大。

优选地，所述筛选包括水平距离筛选，所述水平距离筛选可以表示为：

其中，线段L_i和L_j为近线合并改进措施下需要合并的两种线段，其首尾端点分别为

和

d_min为衡量线段特征水平距离接近程度的距离筛选阈值。

优选地，所述筛选包括垂直距离筛选，所述垂直距离筛选可以表示为：

其中，线段L_u和L_v为断线拼接改进措施下需要拼接的两种线段，其首尾端点分别为

和

d′_min为衡量线段特征垂直距离接近程度的距离筛选阈值。

在本发明中，通过筛选，将将角度和空间上都与L₁很接近的线段分为一组，得到最终候选线段集，进一步地，以水平距离和垂直距离筛选取代欧几里得距离筛选，由于仅涉及绝对值的加减运算，极大地提高了筛选效率，减少计算代价。

在步骤S133中，将L₁加入经过筛选的候选线段集中，组成新的线段特征集合{L₁,L_γ1,L_γ2}，从中分别选择首尾端点中偏离最远的两端点作为新线特征的首尾端点，合成新的线特征L_M，重新计算其角度为θ_M，

若满足θ_M＜θ_min，合并成立，以合并线特征L_M替换线段集合{L₁,L_γ1,L_γ2}；

若不满足θ_M＜θ_min，放弃合并，此时合并前后角度相差过大，合并结果偏离原线段。

在步骤S134中，去除L₁，将其余的线特征重新进行排序，重复步骤S131～S133，完成所有线特征的合并和拼接。

进一步地，在本发明中，对步骤S12和步骤S13的先后顺序不做特别限定，可以先进行步骤S12再进行步骤S13，也可以先进行步骤S13再进行步骤S12。

在步骤S2中，为了保证系统的实时性，在后端优化中使用了滑动窗口模型，k时刻包含点、线、IMU的滑动窗口中的全状态向量χ可以表示为：

其中，x_k表示IMU在世界坐标系下第k帧中的位置

方向

速度

以及IMU加速度计偏置b_a和陀螺仪偏置b_g的集合；

表示点特征滑动窗口中的状态向量，下标0，1，…，n_k表示滑动窗口中的关键帧数；

λ表示空间中三维点的逆深度，下标0，1，…，n_p表示观察到的特征点；

lg表示三维直线的四参数化正交表示，下标0，1，…，n_l表示特征线的数量；

进一步地，通过最小化目标函数来优化滑动窗口中的所有状态变量：

其中，

表示x_k与x_k-1帧间IMU的测量残差，B是滑动窗口中所有IMU预积分测量值的集合；

表示点特征的重投影残差；

表示线特征的重投影残差；

F和L分别为相机观测到的点、线特征集合；

和

分别表示IMU预积分噪声项的协方差矩阵、点特征噪声项的协方差矩阵和线特征噪声项的协方差矩阵；

r_p表示先验信息误差，J_p为其雅克比矩阵；

ρ是用于抑制异常值的鲁棒核函数，优选地，ρ为Huber核函数。

进一步地，通过Ceres求解器，分别加入待优化的参数项和残差项，使用DogLeg算法求解完成后端优化，此过程具体方法与常规SLAM相同，在本发明中不在赘述。

传统的SLAM算法多基于特征点的视觉，系统每帧仅采用几百个点的像素信息去表示一帧图片，导致建图稀疏，所建地图无法有效表示系统周围环境的信息。

在步骤S3中，通过点特征和线特征共同表示一帧图片，在提升系统精度的同时也可以更好地还原场景的结构信息，构建更高层次的环境几何结构地图。

进一步地，传统的SLAM算法由于点云地图占有内存过大，很难进行稠密地图的表达，在本申请中，通过线特征构建稠密地图，优选建立八叉树地图。

进一步地，在八叉树地图中，很多路径规划方法要求给出到障碍物的距离，在本发明中，采用TSDF(Truncated Signed Distance Functions)截断的有符号距离函数进行表面重建，进而可以生成稠密地图。

所述TSDF是一种成熟的三维场景重建方法，例如Kinect Fusion,Dense VisualSLAM for RGB-D Cameras中使用的方法，在本发明中不做赘述。

进一步优选地，采用Oleynikova H,Taylor Z,Fehr M,et al.Voxblox:Incremental 3d euclidean signed distance fields for on-boardmav planning[C]//2017Ieee/rsj International Conference on Intelligent Robots and Systems(iros).IEEE,2017:1366-1373中的方法，利用TSDF增量构建ESDF Map欧式有符号距离函数的方法，利用Grouped Raycast方法，Voxblox可以加速TSDF的融合过程，直接从TSDF构建ESDF地图，从而完成本发明。

实施例

实施例1

采用公开数据集进行仿真实验，其中，数据集为EuRoC数据集[31]中四个场景提取难度最大的子序列MH_04_difficult、MH_05_difficult、V1_03_difficult、V2_03_difficult。

在步骤S1中，对点特征和线特征进行检测跟踪，在点特征检测跟踪中，提取角点作为特征点，利用KLT光流法实现特征点跟踪，并基于反向光流跟踪剔除差异较大的点。

在线特征检测跟踪中，通过下述步骤对线特征进行提取：

S101、图像灰度化：对于传感器输入的彩色图根据图像各个通道的采样值进行加权平均转化为灰度图；

S102、降噪：使用高斯滤波器滤除噪声并平滑图像，以防止由噪声引起的错误检测；

S103、计算图像中每个像素的梯度值和方向：利用边缘差分算子计算水平方向的差分g_x(x，y)和垂直方向的差分g_y(x，y)，得到梯度值g(x，y)和方向θ，表示为：

边缘差分算子为Rober；

S104、非极大值抑制：对图像进行梯度计算后，仅仅基于梯度值提取的边缘仍然很模糊，采用非极大值抑制将局部最大值之外的所有梯度值抑制为0；

S105、双阈值检测：应用双阀值策略，滤除噪声或颜色变化引起的小梯度值而保留大梯度值；

S106、滞后边界跟踪：搜索所有连通的弱边缘点，如果一条连通的弱边缘的任何一个点和强边缘点连通，则保留这条弱边缘，否则抑制这条弱边缘。

S107、获取线段特征参数：通过检测到的线段首尾两端点坐标，通过下式获取线段的长度l_i和线段与水平方向的夹角θ_i：

在步骤S107之后，还具有步骤：

S12、对线特征长度进行约束。

通过对线特征长度进行约束，通过去除短的线段，只保留长线段，所述约束表示为：

其中，l_min表示最小线特征长度阈值，W_I和H_I分别为图像宽度和高度，

表示向上取整，η为0.08。

在步骤S12之后，还具有步骤S13、近线合并及断线拼接，包括：

S131、对线特征进行排序；

S132、对线特征进行筛选；

S133、对线特征进行合并拼接；

S134、完成对所有线特征进行合并拼接。

在步骤S131中，按线特征长度从长到短的顺序对提取出的线特征进行降序排列，获得线特征序列{L₁，L₂，...，L_n}，其中L₁表示线特征长度最长的线特征。

将L1外的线特征表示为剩余线特征集合：

L＝{L₂，L₃，...，L_n} (4)

其中，n为线特征总数。

在步骤S132中，所述筛选包括角度筛选、水平距离筛选和垂直距离筛选，所述角度筛选表示为：

所述水平距离筛选可以表示为：

所述垂直距离筛选可以表示为：

在步骤S133中，将L₁加入经过筛选的候选线段集中，组成新的线段特征集合{L₁,L_γ1,L_γ2}，从中分别选择首尾端点中偏离最远的两端点作为新线特征的首尾端点，合成新的线特征L_M，重新计算其角度为θ_M，

若满足θ_M＜θ_min，合并成立，以合并线特征L_M替换线段集合{L₁,L_γ1,L_γ2}；

若不满足θ_M＜θ_min，放弃合并，此时合并前后角度相差过大，合并结果偏离原线段。

在步骤S134中，去除L₁，将其余的线特征重新进行排序，重复步骤S131～S133，完成所有线特征的合并和拼接。

进一步地，在前端视觉惯性里程计中，将提取的线特征进行参数化，具体地，旋转坐标系下直线L_w的普吕克坐标可以表示为：

其中，M_w，k，N_w，k表示观察到的第k条线特征的两个三维端点,这两端点的齐次坐标为

和

v是线特征的方向向量；n是过该直线且通过原点的平面的法向量，其垂直于v；w₁和w₂表示齐次因子。

进一步地，将直线L_w转换到相机坐标系，记为L_c，则相机坐标系下直线L_c的普吕克坐标表示为：

其中，(t_cw)^表示平移向量的反对称阵，R_cw表示旋转矩阵，t_cw表示平移向量。

L_c投影到图像平面得到l_c，可表示为：

l_c＝K_ln_c

其中，K_l表示直线的投影矩阵。

在步骤S2中，在后端优化中使用了滑动窗口模型，k时刻包含点、线、IMU的滑动窗口中的全状态向量χ可以表示为：

通过最小化目标函数来优化滑动窗口中的所有状态变量：

通过Ceres求解器，分别加入待优化的参数项和残差项，使用DogLeg算法求解完成后端优化。

在步骤S3中，通过点特征和线特征共同表示一帧图片，通过线特征构建八叉树地图，采用TSDF截断的有符号距离函数进行表面重建，利用TSDF增量构建ESDF Map欧式有符号距离函数，利用Grouped Raycast方法加速TSDF的融合过程，直接从TSDF构建ESDF地图。

对比例1

重复实施例1，区别在于，采用Von Gioi R G,Jakubowicz J,Morel J M,etal.LSD:aline segment detector[J].Image Processing On Line,2012,2:35-55中的LSD算法进行线特征提取。

对比例2

重复实施例1，区别在于，采用Lee J H,Lee S,Zhang G,et al.Outdoor placerecognition in urban environments using straight lines[C]//2014IEEEInternational Conference on Robotics and Automation(ICRA).IEEE,2014:5550-5557中的FLD算法进行线特征提取。

对比例3

采用与实施例1相同的数据集，采用VINSFusion状态估计器进行仿真实验，VINSFusion是港科大开源的基于优化的多传感器状态估计器，支持双目视觉惯导SLAM方案，具体设置方式可参见论文Qin T,Cao S,Pan J,et al.A general optimization-basedframework for global pose estimation with multiplesensors[J].arXivpreprint arXiv:1901.03642,2019。

对比例4

采用与实施例1相同的数据集，采用PL-VIO视觉惯性里程计进行仿真实验，PL-VIO是贺一家博士开源的使用点和线特征的紧耦合单目视觉惯性里程计，在VINS-Mono的基础上增加LSD线特征提取器，并在后端使用Ceres求解器加以优化。

实验例1

统计实施例1、对比例1、对比例2中每帧的耗时和线特征提取数量，结果如表一所示。

表一

EuRoC数据集下Machine Hall提取效果如图4～6所示，其中图4为对比例1提取效果，图5为对比例2提取效果，图6为实施例1提取效果，从表一和图4～6中可以看出，对比例1提取线段数量最多，对比例2次之，实施例1中最少，对比例1和对比例2有大量短线段特征存在，增加线段检测和匹配的计算成本，且一些长线段被分割成许多短线段以及邻近线段的存在，使后续的线段匹配任务变得复杂，从而增加了系统精度的不确定性。

实施例1大大减少了短线段特征的数量，保留对系统定位精度影响较大的长线段特征，使用近线合并和断线拼接方法，再次改良了提取线段的质量，有效提升了系统精度。

实验例2

统计实施例1、对比例3、对比例4中运动估计误差，采用绝对轨迹误差评估算法的准确性，即计算估计位姿与真实位姿之间欧氏距离的均方根误差(root mean squareerror，RMSE)和最大值。在与真实轨迹作对比时使用EVO(evaluation ofodometry andSLAM)工具进行数据对齐和误差计算，其中均方根误差为同时考虑平移和旋转两部分得到的最终误差。

其结果如表二所示。

表二

从表二可以看出，实施例1因线特征提取质量高，定位精度优于对比文件3、对比文件4，除了MH_01_easy和MH_02_easy两个简单序列下的精度略输于PL-VIO，其余场景均为最优，在场景纹理缺失的MH_04_difficult和相机快速运动的V2_03_difficult序列下，算法均表现出优异的稳定性，没有出现对比文件4在V1_02_medium中特征丢失的情形(如表中“×”所示)。

同时，实施例1采用双目相机也有效避免了单目相机尺度不确定的缺陷，在提升系统精度的同时，鲁棒性更优。由表二可以看出，实施例1与仅使用点特征的对比例3比较，平均定位误差下降了68％；与使用单目点线特征的对比例4比较，平均定位误差下降了30％。

在本发明的描述中，需要说明的是，术语“上”、“下”、“内”、“外”、“前”、“后”等指示的方位或位置关系为基于本发明工作状态下的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”、“第四”仅用于描述目的，而不能理解为指示或暗示相对重要性。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”“相连”“连接”应作广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体的连接普通；可以是机械连接，也可以是电连接；可以是直接连接，也可以通过中间媒介间接连接，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。

Claims (5)

1.一种基于点线特征融合的双目惯性同时定位与地图构建方法，包括以下步骤：

S1、前端视觉惯性里程计；

S2、后端优化；

S3、三维环境建图；

步骤S1中，在所述前端视觉惯性里程计中，采用双目相机采集相机前方左、右目图片，作为视觉惯性同时定位与地图构建系统的检测分析输入图像；

所述前端视觉惯性里程计基于点线特征进行,包括步骤S11、点特征的提取和线特征的提取；

在线特征提取后，还具有步骤：

S13、近线合并及断线拼接；

通过对相近线特征进行合并，对分割的线特征进行拼接，从而减少线特征的数量，提高所提取线特征的质量，通过以下步骤对线特征进行近线合并及断线拼接：

S131、对线特征进行排序；

S132、对线特征进行筛选；

S133、对线特征进行合并拼接；

S134、完成对所有线特征进行合并拼接；

在步骤S132中，所述筛选包括角度筛选，所述角度筛选表示为：

其中，L_α表示经过角度筛选得到候选线段组，L_m表示剩余线特征集合中不同的线特征；

θ_min为衡量线特征角度相近程度的角度筛选阈值；

μ为自适应比例系数；

所述筛选包括水平距离筛选，所述水平距离筛选表示为：

其中，线段L_i和L_j为近线合并改进措施下需要合并的两种线段，其首尾端点分别为

和

d_min为衡量线段特征水平距离接近程度的距离筛选阈值；

所述筛选还包括垂直距离筛选，所述垂直距离筛选表示为：

其中，线段L_u和L_v为断线拼接改进措施下需要拼接的两种线段，其首尾端点分别为

和

d′_min为衡量线段特征垂直距离接近程度的距离筛选阈值。

2.根据权利要求1所述的基于点线特征融合的双目惯性同时定位与地图构建方法，其特征在于，

步骤S1中，在线特征提取后，还具有步骤：

S12、对线特征长度进行约束；

通过对线特征长度进行约束，去除短的线段，只保留满足约束条件的长线段。

3.根据权利要求2所述的基于点线特征融合的双目惯性同时定位与地图构建方法，其特征在于，

所述约束表示为：

其中，l_min表示最小线特征长度阈值，W_I和H_I分别为图像宽度和高度，

表示向上取整，η为比例系数。

4.根据权利要求1所述的基于点线特征融合的双目惯性同时定位与地图构建方法，其特征在于，

在步骤S131中，按线特征长度对提取出的线特征进行排序，获得线特征序列{L₁，L₂，...，L_n}，L₁外的线特征表示为剩余线特征集合：

L＝{L₂,L₃,...,L_n}。

5.根据权利要求1所述的基于点线特征融合的双目惯性同时定位与地图构建方法，其特征在于，

在步骤S133中，将L₁加入经过筛选的候选线段集中，组成新的线段特征集合{L₁,L_γ1,L_γ2}，从中分别选择首尾端点中偏离最远的两端点作为新线特征的首尾端点，合成新的线特征L_M，重新计算其角度为θ_M，

若满足θ_M＜θ_min，合并成立，以合并线特征L_M替换线段集合{L₁,L_γ1,L_γ2}；

若不满足θ_M＜θ_min，放弃合并，此时合并前后角度相差过大，合并结果偏离原线段。

Images