CN113837277A - 一种基于视觉点线特征优化的多源融合slam系统 - Google Patents

Abstract

本发明公开了一种基于视觉点线特征优化的多源融合SLAM系统。首先提出一种改进尺度空间的视觉线特征提取方法，并利用基于最小二范数的约束匹配策略对前后帧间同一线特征进行约束匹配，为视觉里程计前端提供更为丰富的特征信息。其次将多帧激光点云投影到视觉坐标系中，实现激光点云和视觉特征深度关联，并利用视觉初始位姿估计结果辅助优化激光雷达扫描匹配精度。最后采用基于贝叶斯网络的因子图法搭建激光‑视觉‑惯性里程计系统，并引入GNSS因子和回环因子对激光‑视觉‑惯性里程计进行全局约束。通过实验对比可知，本算法在实时性、定位精度和建图效果方面均优于同类算法，可以在EuROC数据集中实现定位和建图精度均较优的实时位姿估计。

Description

一种基于视觉点线特征优化的多源融合SLAM系统

技术领域

本发明属于多传感器即时定位与建图方案领域，具体涉及一种基于视觉点线特征优化的多源融合SLAM系统。

背景技术

基于SLAM(Simultaneous Localization and Mapping，即时定位与建图)的多源融合定位技术是移动载体高精度定位领域的关键技术之一。根据传感器不同，可将其分为激光SLAM和视觉SLAM两类，由于激光雷达扫描角度受限和视觉里程计受光照变化影响显著等单一传感器的固有缺陷，近年来，激光雷达-视觉-惯性里程计(LiDAR-Visual-InertialOdometry，LVIO)因其多传感器异质互补的优势已成为SLAM的研究热点。

现有的LVIO多源融合策略可以从前端和后端两个角度进行阐述，首先是前端融合策略。LVIO常采用激光雷达为单目视觉提供深度信息，改善视觉特征的尺度模糊性；反之，激光雷达利用视觉的特征提取来支持扫描匹配。因此，视觉特征的数量和质量深度影响融合系统的位姿精度。现有融合系统中视觉里程计以提取点特征为主，在弱纹理环境中难以提取丰富的有效特征，导致激光雷达扫描匹配失效。因此，需添加对环境纹理和光度变化更为鲁棒的线特征对激光雷达进行额外特征约束。基于点线特征的的视觉SLAM已有相关研究，如PL-VIO、PL-VINS等，但并未广泛应用至LVIO系统。

从后端融合策略的角度，可根据优化算法不同将LVIO分为基于滤波法和因子图法两类。滤波法虽然易于实现多源融合参数估计，但存在增减传感器需频繁重构的原理性缺陷。而因子图法作为新兴方法，因其即插即用的特性可有效提升单一传感器失效时SLAM系统的鲁棒性，适用于处理LVIO此类异质异构非周期数据融合问题。此外，由于LVIO处于局部坐标系中，存在累计误差等固有缺陷，需采用GNSS(Global Navigation SatelliteSystem，全球卫星导航系统)全局定位信息对其进行校正。而采用扩展卡尔曼滤波的方法将GPS定位信息与激光雷达位姿估计值进行松耦合，但存在单次线性化误差大的弊端。而基于因子图法的融合GNSS因子的LVIO框架，可通过滑窗多次迭代优化位姿估计值，但因子图中单一关键帧的量测信息过多导致信息冗余，计算成本较高。

因此，需提出一种基于视觉点线特征优化的多源融合SLAM系统，利用多传感器异质互补的优势提升融合系统在复杂环境中的精度和普适性。

发明内容

为解决上述问题，本发明公开了一种基于视觉点线特征优化的多源融合SLAM系统，在实时性、定位精度和建图效果方面均优于同类算法，可以在EuROC数据集中实现定位和建图精度均较优的实时位姿估计。

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

一种基于视觉点线特征优化的多源融合SLAM系统，具体方法如下：

(1)改进尺度空间的线特征提取

本发明选用精度较高且无需调节显式参数的LSD算法作为线特征提取算法。根据底层参数优化策略，本发明提出一种改进尺度空间的LSD算法，并提出一种最小几何约束法实现线特征约束匹配。

给定一个N层高斯金字塔作为LSD线特征的尺度空间，定义每层中图像尺度缩放比为s，缩放图像s倍后进行一次降采样，得到所有像素梯度值，并根据同性点密度进行像素梯度矩形合并，得到一个类似矩形的线段l。矩形内的同性点密度d可表示为：

其中，k为矩形内像素点总数，D为同位点密度阈值。不同s和D取值影响在数据集中算法运行精度和实时性，本发明选用s＝0.5，D＝0.5。

(2)基于最小二范数的线特征约束匹配

考虑到载体移动过程中同一线特征对的角度与平移变化，为保证前后帧间提取的线特征匹配总误差最小，可构建一个最小化的稀疏矩阵模型。

不同于特征提取时同一帧内不同线特征的邻线合并，基于最小二范数的约束匹配的对象是连续两帧间角度与距离改变的同一线特征对。假设两连续帧间匹配所得的所有线特征对:

F_ij＝{(l_i,l_j)|j∈[1,n]} [2]

其中，l_i和l_j分别为前帧和后帧内提取的任一线特征，n为后帧内的线特征总数。

根据帧间线特征变化情况，可设参数矩阵为e_ij＝[θ_ij,μ_ij,ρ_ij,d_ij]^T，θ_ij为前后帧间同一线特征夹角，d_ij为平移距离，μ_ij和ρ_ij分别为前后帧间线特征投影比和长度比。利用参数矩阵可构建后帧对于l_i的线性约束矩阵A_i＝[e_i1,...e_ij,e_in]。设l_i的匹配判别向量目标向量为m_i＝[m_i1,...m_ij,...m_in]^T，各分量的值由特征匹配结果决定，匹配为1，不匹配为0。若∑m_in＝1，则满足线性约束A_im_i＝t，由此，可以将线特征匹配问题优化为一个基于最小二范数的约束匹配方程：

其中，λ为权重系数，t＝[0,1,1,0]^T为约束目标向量。

(3)激光雷达辅助特征深度关联

为改善单目相机的尺度模糊性，可采用激光雷达辅助深度关联。由于激光雷达分辨率远低于相机，仅使用单帧稀疏点云深度关联将导致大量视觉特征深度缺失。因此，本发明采用叠加多帧稀疏点云的策略获得点云深度值，从而与视觉特征建立深度关联。

设一个视觉坐标系{V}内的特征点f₁ ^V，一组激光雷达坐标系{L}内的深度点

将

投影到以f₁ ^V为球心的单位球面{V_g}上，得到投影点

其中，

和

分别为{L}到{V_g}的旋转矩阵和外参矩阵。以f₁ ^V为根节点建立KD树搜索球体上三个最近的深度点d₁,d₂,d₃，连接f₁ ^V与相机中心O交d₁,d₂,d₃于O_d，则f₁ ^V的特征深度为f₁ ^VO_d。

为解决叠加多帧稀疏点云后导致深度值与特征点云的非单射情况，本发明设定了深度剔除阈值ζ＝1.8m，一个视觉特征对应的所有深度点中，大于阈值的深度点将被剔除。

(4)基于因子图的激光-视觉-惯性里程计-GNSS优化框架

构建世界坐标系内的全局状态向量为：

其中，x_i表示IMU预积分后提供的位置p_i，旋转四元数q_i，速度v_i和IMU偏置b_i；λ_p表示空间内视觉点特征的逆深度，ο_l表示视觉线特征的正交化坐标，

和

分别为激光雷达特征点到与之匹配的边缘线和特征面的距离。由此，可以构造一个最大后验估计问题，即利用高斯-牛顿法最小化所有代价函数，从而对滑动窗口内的状态向量进行非线性优化：

其中，{r_p,J_p}为先验信息，

和p_i分别为IMU残差和协方差矩阵，

和

分别为视觉点、线特征的重投影误差，p_c为视觉噪声的协方差矩阵。ρ为Huber损失函数，具体取值为：

公式中各传感器代价函数的具体含义如下所述。

①视觉线特征因子

将空间中观测得到的一条线特征l(p₁,p₂)投影到归一化平面得到

则线特征的重投影误差可用观测线特征的两端点到投影线特征的点线距离表示，即：

②激光雷达因子

激光雷达辅助单目视觉深度关联后，VIO将为激光雷达提供视觉初始位姿估计值，用以校正激光雷达点云的运动畸变，提高扫描匹配精度。本发明涉及的激光雷达相邻关键帧间扫描匹配的误差可由特征点到与之匹配的边缘线和特征面的距离表示：

其中，

表示边缘特征点，

和

为与特征点匹配的边缘线的端点坐标。

表示平面特征点，与之匹配的特征面可由

和

三点表示。

③GNSS因子及回环因子

当载体运动至GNSS信号良好区域，可添加GNSS因子与局部传感器共同优化。设两帧GNSS观测值时间间隔为△t，给定ENU坐标系中的GNSS测量值

LVIO在△t内在的位置增量

则GNSS因子可用以下观测残差表示：

基于轻量化原则，本发明仅当GNSS测量值协方差小于LVIO位姿估计值时，才将其视为置信因子加入因子图优化中。回环因子同理，仅当检测到运动轨迹回环时才在因子图中加入回环因子，以提升位姿估计精度和计算效率。

本发明的有益效果是：

本发明提出了一种基于视觉点线特征优化的多源SLAM算法：首先，提出了一种改进尺度空间的线特征提取和基于最小二范数的约束匹配策略，为视觉里程计前端提供更为丰富的特征信息。其次，将多帧激光点云投影到视觉坐标系中实现特征深度关联，并利用视觉初始位姿估计结果辅助优化激光雷达扫描匹配精度。最后，采用基于贝叶斯网络的因子图法搭建LVIO融合系统，并引入GNSS因子和回环因子对LVIO进行全局约束。通过实验对比可知，在EuROC数据集中，本算法在实时性、定位精度和建图效果方面均优于同类算法。

附图说明

图1是本系统建图效果图；

图2是特征深度关联原理图；

图3是本系统与其他算法运行时间对比图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图所示，本发明所述的一种基于视觉点线特征优化的多源融合SLAM系统，具体方法如下：

(1)改进尺度空间的线特征提取

本发明选用精度较高且无需调节显式参数的LSD算法作为线特征提取算法。根据底层参数优化策略，本发明提出一种改进尺度空间的LSD算法，并提出一种最小几何约束法实现线特征约束匹配。

给定一个N层高斯金字塔作为LSD线特征的尺度空间，定义每层中图像尺度缩放比为s，缩放图像s倍后进行一次降采样，得到所有像素梯度值，并根据同性点密度进行像素梯度矩形合并，得到一个类似矩形的线段l。矩形内的同性点密度d可表示为：

其中，k为矩形内像素点总数，D为同位点密度阈值。不同s和D取值影响在数据集中算法运行精度和实时性，本发明选用s＝0.5，D＝0.5。

(2)基于最小二范数的线特征约束匹配

考虑到载体移动过程中同一线特征对的角度与平移变化，为保证前后帧间提取的线特征匹配总误差最小，可构建一个最小化的稀疏矩阵模型。

不同于特征提取时同一帧内不同线特征的邻线合并，基于最小二范数的约束匹配的对象是连续两帧间角度与距离改变的同一线特征对。假设两连续帧间匹配所得的所有线特征对:

F_ij＝{(l_i,l_j)|j∈[1,n]} [12]

其中,l_i和l_j分别为前帧和后帧内提取的任一线特征，n为后帧内的线特征总数。

根据帧间线特征变化情况，可设参数矩阵为e_ij＝[θ_ij,μ_ij,ρ_ij,d_ij]^T，θ_ij为前后帧间同一线特征夹角，d_ij为平移距离，μ_ij和ρ_ij分别为前后帧间线特征投影比和长度比。利用参数矩阵可构建后帧对于l_i的线性约束矩阵A_i＝[e_i1,...e_ij,e_in]。设l_i的匹配判别向量目标向量为m_i＝[m_i1,...m_ij,...m_in]^T，各分量的值由特征匹配结果决定，匹配为1，不匹配为0。若∑m_in＝1，则满足线性约束A_im_i＝t，由此，可以将线特征匹配问题优化为一个基于最小二范数的约束匹配方程：

其中，λ为权重系数，t＝[0,1,1,0]^T为约束目标向量。

(3)激光雷达辅助特征深度关联

为改善单目相机的尺度模糊性，可采用激光雷达辅助深度关联。由于激光雷达分辨率远低于相机，仅使用单帧稀疏点云深度关联将导致大量视觉特征深度缺失。因此，本发明采用叠加多帧稀疏点云的策略获得点云深度值，从而与视觉特征建立深度关联。

设一个视觉坐标系{V}内的特征点f₁ ^V，一组激光雷达坐标系{L}内的深度点

将

投影到以f₁ ^V为球心的单位球面{V_g}上，得到投影点

其中，

和

分别为{L}到{V_g}的旋转矩阵和外参矩阵。以f₁ ^V为根节点建立KD树搜索球体上三个最近的深度点d₁,d₂,d₃，连接f₁ ^V与相机中心O交d₁,d₂,d₃于O_d，则f₁ ^V的特征深度为f₁ ^VO_d。

为解决叠加多帧稀疏点云后导致深度值与特征点云的非单射情况，本发明设定了深度剔除阈值ζ＝1.8m，一个视觉特征对应的所有深度点中，大于阈值的深度点将被剔除。

(4)基于因子图的激光-视觉-惯性里程计-GNSS优化框架

构建世界坐标系内的全局状态向量为：

其中，x_i表示IMU预积分后提供的位置p_i，旋转四元数q_i，速度v_i和IMU偏置b_i；λ_p表示空间内视觉点特征的逆深度，ο_l表示视觉线特征的正交化坐标，

和

分别为激光雷达特征点到与之匹配的边缘线和特征面的距离。由此，可以构造一个最大后验估计问题，即利用高斯-牛顿法最小化所有代价函数，从而对滑动窗口内的状态向量进行非线性优化：

其中，{r_p,J _p}为先验信息，

和p_i分别为IMU残差和协方差矩阵，

和

分别为视觉点、线特征的重投影误差，p_c为视觉噪声的协方差矩阵。ρ为Huber损失函数，具体取值为：

公式中各传感器代价函数的具体含义如下所述。

①视觉线特征因子

将空间中观测得到的一条线特征l(p₁,p₂)投影到归一化平面得到

则线特征的重投影误差可用观测线特征的两端点到投影线特征的点线距离表示，即：

②激光雷达因子

激光雷达辅助单目视觉深度关联后，VIO将为激光雷达提供视觉初始位姿估计值，用以校正激光雷达点云的运动畸变，提高扫描匹配精度。本发明涉及的激光雷达相邻关键帧间扫描匹配的误差可由特征点到与之匹配的边缘线和特征面的距离表示：

其中，

表示边缘特征点，

和

为与特征点匹配的边缘线的端点坐标。

表示平面特征点，与之匹配的特征面可由

和

三点表示。

③GNSS因子及回环因子

当载体运动至GNSS信号良好区域，可添加GNSS因子与局部传感器共同优化。设两帧GNSS观测值时间间隔为△t，给定ENU坐标系中的GNSS测量值

LVIO在△t内在的位置增量

则GNSS因子可用以下观测残差表示：

基于轻量化原则，本发明仅当GNSS测量值协方差小于LVIO位姿估计值时，才将其视为置信因子加入因子图优化中。回环因子同理，仅当检测到运动轨迹回环时才在因子图中加入回环因子，以提升位姿估计精度和计算效率。

下面根据公共数据集实验对本发明的技术方案的定位精度和建图效果进行对比验证。首先利用EuROC数据集评估算法在室内环境中位姿估计结果，评估策略为通过比较数据集真值与算法输出位姿估计结果，求取平均轨迹误差(Mean Error)值和绝对轨迹误差的均方根误差(APE_RMSE)值。

数据集测试结果如下表所示：

表1各算法在EuROC数据集内的位姿估计误差

Table.1 Motion estimation errors of each algorithm in EuROC dataset

由上表可知，在EuROC数据集实验中，得益于视觉点线特征优化的前端和因子图优化后端的双重作用，本算法较三种常用的VIO算法(Vins_Mono、PL-VIO和PL-VINS)的平均轨迹误差和绝对轨迹精度明显较优，在室内环境数据集中可得到精确的位姿估计结果。

通过测试结果可知，本发明提出的一种基于视觉点线特征优化的多源融合SLAM系统，在实时性、定位精度和建图效果方面均优于同类算法，可以在EuROC数据集中实现定位和建图精度均较优的实时位姿估计。

需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (5)

1.一种基于视觉点线特征优化的多源融合SLAM系统，其特征在于：具体方法如下：

(1)改进尺度空间的线特征提取

选用LSD算法作为线特征提取算法；根据底层参数优化策略，采用一种改进尺度空间的LSD算法，并提出一种最小几何约束法实现线特征约束匹配；具体操作如下

给定一个N层高斯金字塔作为LSD线特征的尺度空间，定义每层中图像尺度缩放比为s，缩放图像s倍后进行一次降采样，得到所有像素梯度值，并根据同性点密度进行像素梯度矩形合并，得到一个类似矩形的线段l；矩形内的同性点密度d表示为：

其中，k为矩形内像素点总数，D为同位点密度阈值；不同s和D取值影响在数据集中算法运行精度和实时性，此处选用s＝0.5，D＝0.5；

(2)基于最小二范数的线特征约束匹配

考虑到载体移动过程中同一线特征对的角度与平移变化，为保证前后帧间提取的线特征匹配总误差最小，构建一个最小化的稀疏矩阵模型；

不同于特征提取时同一帧内不同线特征的邻线合并，基于最小二范数的约束匹配的对象是连续两帧间角度与距离改变的同一线特征对；假设两连续帧间匹配所得的所有线特征对:

F_ij＝{(l_i,l_j)|j∈[1,n]} [2]

其中，l_i和l_j分别为前帧和后帧内提取的任一线特征，n为后帧内的线特征总数；

根据帧间线特征变化情况，设参数矩阵为e_ij＝[θ_ij,μ_ij,ρ_ij,d_ij]^T，θ_ij为前后帧间同一线特征夹角，d_ij为平移距离，μ_ij和ρ_ij分别为前后帧间线特征投影比和长度比；利用参数矩阵构建后帧对于l_i的线性约束矩阵A_i＝[e_i1,...e_ij,e_in]；设l_i的匹配判别向量目标向量为m_i＝[m_i1,...m_ij,...m_in]^T，各分量的值由特征匹配结果决定，匹配为1，不匹配为0；若∑m_in＝1，则满足线性约束A_im_i＝t，由此，将线特征匹配问题优化为一个基于最小二范数的约束匹配方程：

其中，λ为权重系数，t＝[0,1,1,0]^T为约束目标向量；

(3)激光雷达辅助特征深度关联

为改善单目相机的尺度模糊性，采用激光雷达辅助深度关联；由于激光雷达分辨率远低于相机，仅使用单帧稀疏点云深度关联将导致大量视觉特征深度缺失；因此，采用叠加多帧稀疏点云的策略获得点云深度值，从而与视觉特征建立深度关联；

设一个视觉坐标系{V}内的特征点f₁ ^V，一组激光雷达坐标系{L}内的深度点

将

投影到以f₁ ^V为球心的单位球面{V_g}上，得到投影点

其中，

和

分别为{L}到{V_g}的旋转矩阵和外参矩阵；以f₁ ^V为根节点建立KD树搜索球体上三个最近的深度点d₁,d₂,d₃，连接f₁ ^V与相机中心O交d₁,d₂,d₃于O_d，则f₁ ^V的特征深度为f₁ ^VO_d；

为解决叠加多帧稀疏点云后导致深度值与特征点云的非单射情况，设定深度剔除阈值ζ＝1.8m，一个视觉特征对应的所有深度点中，大于阈值的深度点将被剔除；

(4)基于因子图的激光-视觉-惯性里程计-GNSS优化框架

构建世界坐标系内的全局状态向量为：

其中，x_i表示IMU预积分后提供的位置p_i，旋转四元数q_i，速度v_i和IMU偏置b_i；λ_p表示空间内视觉点特征的逆深度，ο_l表示视觉线特征的正交化坐标，

和

分别为激光雷达特征点到与之匹配的边缘线和特征面的距离；由此，构造一个最大后验估计问题，即利用高斯-牛顿法最小化所有代价函数，从而对滑动窗口内的状态向量进行非线性优化：

其中，{r_p,J_p}为先验信息，

和p_i分别为IMU残差和协方差矩阵，

和

分别为视觉点、线特征的重投影误差，p_c为视觉噪声的协方差矩阵；ρ为Huber损失函数，具体取值为：

公式中各传感器代价函数的具体含义如下所述；

①视觉线特征因子

将空间中观测得到的一条线特征l(p₁,p₂)投影到归一化平面得到

则线特征的重投影误差用观测线特征的两端点到投影线特征的点线距离表示，即：

②激光雷达因子

激光雷达辅助单目视觉深度关联后，VIO将为激光雷达提供视觉初始位姿估计值，用以校正激光雷达点云的运动畸变，提高扫描匹配精度；此处涉及的激光雷达相邻关键帧间扫描匹配的误差由特征点到与之匹配的边缘线和特征面的距离表示：

其中，

表示边缘特征点，

和

为与特征点匹配的边缘线的端点坐标；

表示平面特征点，与之匹配的特征面由

和

三点表示；

③GNSS因子及回环因子

当载体运动至GNSS信号良好区域，添加GNSS因子与局部传感器共同优化；设两帧GNSS观测值时间间隔为△t，给定ENU坐标系中的GNSS测量值

LVIO在△t内在的位置增量

则GNSS因子用以下观测残差表示：

基于轻量化原则，仅当GNSS测量值协方差小于LVIO位姿估计值时，才将其视为置信因子加入因子图优化中；回环因子同理，仅当检测到运动轨迹回环时才在因子图中加入回环因子，以提升位姿估计精度和计算效率。

2.根据权利要求1所述的一种基于视觉点线特征优化的多源融合SLAM系统，其特征在于，步骤(1)所述的改进尺度空间的线特征提取方法，通过底层参数优化策略，推导线特征提取时的同性点密度公式，在改进尺度空间中进行视觉线特征提取。

3.根据权利要求1所述的一种基于视觉点线特征优化的多源融合SLAM系统，其特征在于，步骤(2)所述的基于最小二范数的线特征约束匹配方法，利用前后帧间同一线特征偏移角、偏移距离、投影比和长度比建立构建一个最小化的稀疏矩阵模型，对前后图像帧间同一线特征进行约束匹配。

4.根据权利要求1所述的一种基于视觉点线特征优化的多源融合SLAM系统，其特征在于，步骤(3)所述激光雷达辅助特征深度关联，利用叠加多帧激光点云得到点云深度值，并将其投影在以视觉特征为球心的单位球面，利用相机中心与视觉特征点连线获得深度值，建立激光雷达与视觉特征的深度关联。

5.根据权利要求1所述的一种基于视觉点线特征优化的多源融合SLAM系统，其特征在于，步骤(4)中所述的基于因子图的激光-视觉-惯性里程计-GNSS优化框架，利用先验信息，IMU残差，视觉点线特征重投影误差，激光雷达残差构建非线性优化方程；当GNSS测量值协方差小于激光-视觉-惯性里程计位姿估计值时，将GNSS因子视为置信因子加入因子图优化中；当检测到运动轨迹回环时；将回环因子加入因子图优化中，以提升位姿估计精度和计算效率。

Images