融合点线特征的视觉惯性同时定位及建图

姚建均, 李英朝, 吴杨, 唐瑞卓, 于新达, 闫宇坤

(1.哈尔滨工程大学 机电工程学院,黑龙江 哈尔滨 150001; 2.维沃移动通信有限公司,广东 东莞 523850)

得益于计算机视觉理论的发展和传感器设备小型化,基于同时定位及建图 (simultaneous localization and mapping, SLAM)的移动机器人发展迅速,使得移动机器人可以在GPS信号较弱的室内进行环境感知,并实现实时的定位和建图[1]。

自SLAM系统提出以来,众多学者都致力于提升其精度和鲁棒性。王立鹏等[2]使用深度学习增强SLAM的定位精度和语义理解能力,伍明等[3]结合目标状态估计与机器人可观性控制增强移动机器人定位精度,但会增加系统的计算负担。惯性测量单元(international mathematical union, IMU)可以辅助相机进行定位并感知绝对尺度[4],形成了视觉惯性同时定位及建图(visual-inertial simultaneous localization and mapping, VI-SLAM)框架[5-6]。Qin等[7]提出了一种基于图优化紧耦合地融合单目相机和IMU信息的VINS-Mono框架,前端采用卡纳德 (Lucas-Kanade,LK)光流跟踪点特征,后端执行4自由度的位姿图优化以增强全局一致性。VINS-Mono框架已经成为众多研究的基础[8-10],但在光照变化和低纹理场景中,特征点提取的质量和重复性严重下降,致使整个SLAM系统精度降低,甚至产生跟踪丢失的问题。

因此,基于点线特征的SLAM受到关注,Wang等[11]利用梯度直方图,提出了统计像素梯度平均向量和标准方差的描述子,Sol等[12]提出了5种不同路标直线参数化的办法,Gomez-Ojeda等[13]提出了基于半直接法的点线特征视觉里程计PL-SVO,Ojeda等[14]在PL-SVO的基础上,加入了局部建图和回环检测线程。但目前线特征提取的实时性、精度和鲁棒性仍然较差。

本文基于点线特征的VI-SLAM系统,引入并改进线特征提取算法,构建点线特征筛选框架,优化特征匹配结果。将所得的线特征提取匹配算法应用于VINS-Mono框架中,得到VI-BLSD-Bow系统。在进行相机-IMU联合标定及词袋训练后,使用智能小车对强光照室内及低纹理走廊环境进行实验验证。

1 线特征提取及匹配算法

线段检测器(line segment detector,LSD)线特征提取算法已经广泛应用于图像处理及SLAM系统中[15-16],但其精度和鲁棒性还有待提升[17]。本文引入LSD线特征及线带描述符(line band discriptor,LBD)描述子,分析LSD算法隐藏参数对线特征提取的影响,提高线特征的提取精度,并设计点线特征匹配及筛选策略,进而得出可应用于SLAM系统的线特征提取算法。

1.1 线特征描述子提取

传统的线特征匹配由于遮挡导致端点位置不明确,且旋转运动下缺乏鲁棒的几何约束。Zhang等[18]提出了LBD线段描述子,引入全局和局部的高斯权重系数,产生更高的匹配精度。

LBD算法首先构建图片高斯金字塔,并对每一层高斯金字塔进行EDLine线特征提取,将不同维度下的直线段一一对应,放入同一个线段向量中,减少在不同尺度下线段匹配的问题。然后在不同尺度下同一线段处建立线段支持区域LSR,并将其分割为条数为m,像素宽度为w的多组条带。定义2个条带方向,分别为直线方向向量dL以及顺时针垂直于dL的垂直分量d⊥。然后建立梯度分布场,在线段中点建立坐标原点,以dL和d⊥作为坐标轴,将线段支持域内像素梯度分别投影到坐标系中:

g′=(gT·d⊥,gT·dL)T(g′d⊥,g′dL)

(1)

式中:g是像素梯度向量;g′是线段支持域内局部像素梯度坐标;d为距离。

为降低距离线段较远的像素梯度对描述子的影响,在LSR第i行设置全局高斯函数fg为:

(2)

式中di表示第i个条带距离坐标原点的距离,且δg=0.5(mw-1)。

为减小条带间的边界效应,再通过在Bj条带上下2个条带之间设置局部高斯函数fl：

(3)

式中d′k为第k行距离条带Bj中心的距离,且δl=w。

对Bj与相邻条带Bj-1、Bj+1之间局部像素梯度进行求和为:

(4)

式中λ=fg(k)fl(k)为综合高斯系数。

最后,将每一行的像素梯度和联立在一起,得到线特征描述矩阵Bj为:

(5)

组合所有条带的特征向量Dj,获得LBD算法的特征描述子:

(6)

1.2 隐藏参数对线特征提取的影响

LSD算法隐藏参数包括:高斯金字塔尺度S,高斯核函数标准差SS,梯度量化值Q,主方向角度差阈值Ta,NFA检测阈值E,外接矩形密度阈值Td,伪排序箱数I以及长度抑制因子F。目前针对LSD算法的优化工作集中于LSD算法与其他算法的融合,针对隐藏参数对LSD算法影响的研究较少。

表1是传统LSD算法的默认参数,在此参数设计下,线段检测的精度以及鲁棒性较低,因此需要对默认参数进行改良。本文主要针对高斯金字塔尺度S、梯度量化值Q、外接矩形密度阈值Td以及长度抑制因子F参数进行优化,分析其对LSD算法检测精度和提取速度的影响,得出适用于SLAM系统的LSD算法检测方案。

高斯金字塔尺度S是高斯降采样下的缩放系数,用于解决图像中的混叠和锯齿问题。设置NFA检测阈值E=1.0,能在带噪声条件下控制每幅图像只存在一个误检。图1是针对Euroc数据集调整高斯金字塔尺度时,线特征提取时间和精度的变化情况。

图1可知,将高斯金字塔尺度缩小到0.4～0.6 (图像金字塔缩小至25%～36%),能减小一半的检测时间。同时,缩小尺度因子虽然减少了使用的像素数量,但对系统的精度影响不大。因此设置优化的LSD算法中S参数为0.5。

在LSD算法计算梯度大小和方向的过程中,平滑区域内的小梯度值点对直线的形成没有帮助。因此可以通过设置梯度阈值ρ,对梯度值进行筛选。梯度阈值ρ为:

ρ=Q/sinτ

(7)

式中τ为角度值。

作为筛选线特征区域的指标,增大Q将加快筛选速度,但是检测到的线特征会减少。且当Q>10时,线段的形成将出现大量错误。为加快线特征提取速度,同时考虑误差筛选,设置Q参数为2.8。

在LSD算法计算梯度大小和方向之后,需要采用伪排序的方式对梯度进行排序,因为梯度值越大,越有可能是边缘。设置箱数为1 024,即将梯度值大小(0～255)划分为1 024个等级,选取梯度最大的点作为种子点,进行区域生长。

在线段支持域形成的过程中,选择好种子节点(梯度值最大的点)后,需要在一定区域内搜寻同性点。主方向角度差阈值将误差容忍范围内的像素点标记为内点,并生成这些内点的外接矩形。如图2所示,若采用主方向角阈值筛选同性点的方式,可能会将小于这个阈值但不同的2个边缘划分到一个矩形当中,可以通过选定矩形内同性点占像素点的密度大小,对矩形进行分割。即当外接矩形区域内部同性点密度很高时,规定外接矩阵密度阈值Td,将大于阈值的外接矩形进行截断,直至满足要求。

图2 相邻边缘划分为一个矩形

密度阈值Td保证了在一个外接矩形中,同性点矩形来自一个边缘,且矩形不被过度分割。因此,在其他参数固定的情况下,调节密度阈值在0.3～0.9变化,研究密度阈值对线段提取的影响,图3所示为Euroc数据集实验结果。

图3 密度阈值对线段提取时间和精度的影响

由图3可知,减小密度阈值,能加快线段支持区域的外接矩形分割的速度,且对系统精度影响较小。因此设置Td参数为0.6。

图4(a)是优化高斯金字塔尺度S、梯度量化值Q、外接矩形密度阈值Td参数后的线特征提取结果。传统LSD算法提取一幅720×480的图片需要47.343 ms,共检测到933条线段。优化后的LSD算法提取时间只需19.626 ms,提升了2.41倍。原因为参数S减小后,高斯金字塔利用的像素点减少,检测时间减少。同时,Td参数的减小将加速线段支持区域外接矩形分割,检测到的线段量减少到336条。

图4 改良参数对LSD算法提取线段的影响

此外,图4(b)中仍然存在细小线段,对SLAM位姿估计没有帮助。所以应使用线段抑制策略,剔除线段长度小于一定阈值的结果。

改变长度抑制因子F,图5是线段提取时间和系统精度变化情况。图5(a)是长度抑制因子对算法提取速度的影响,易知长度抑制策略并不影响LSD算法提取线段的时间。图5(b)显示长度抑制因子在0.04～0.2能改善系统精度,在0.2～0.6对系统精度影响不大,大于0.6后,减小的线特征影响系统的整体精度。由此可知,长度抑制因子越大,系统检测到的线段越少,即越少的线段约束被添加进后端优化中,系统精度将有所降低。因此设置长度抑制因子参数为0.1。

图5 长度抑制因子对线段提取时间和精度的影响

长度抑制因子能调节进入后端优化的线条数量,图6是改变长度抑制因子对线段提取条数的影响。图6(a)中,在长度抑制因子为0.08时,线段检测数量减少为82条,基本不存在细小线段。长度抑制因子大于0.2后,参与后端优化的线特征减少,系统精度下降。图6(b)中,长度抑制因子大于0.7后,线段只能检测到1条直线,相当于只加入点特征进行优化,精度下降明显。

图6 不同长度抑制因子提取线段条数

综上,减小高斯金字塔尺度S、减小外接矩阵密度阈值Td和增大梯度量化值Q能加快LSD算法,而长度抑制因子F能筛选进入后端优化的线段数量。针对不同参数进行实验,表2是4组参数下LSD算法的检测结果,其中参数1条件下系统无法正常运行。综合考虑LSD算法提取速度和系统精度,最终选定S=0.5,Q=2.8,Td=0.6,F=0.1作为LSD线特征提取算法的参数。

表2 不同参数对LSD算法的影响

1.3 点线特征匹配与筛选

提取点线特征后,需要借助描述子进行匹配。误匹配将影响位姿解算,降低系统鲁棒性,因此匹配关系的筛选就尤为重要。本文级联交叉验证、KNN-Matching算法以及RANSAC (随机样本一致性方法)3种算法[19-21]进行点线误匹配的剔除。

在点特征的提取与匹配方面,本文提取加速分段试验特征(features from accelerated segment test,FAST)角点,匹配二值鲁棒独立初等特征(binary robust independent elementary features,BRIEF)描述子。图7(a)是提取点特征,并根据汉明距离进行暴力匹配的结果,匹配关系为422对。

图7 点特征匹配及筛选结果

图7(b)是将匹配结果的最小距离与最大距离比值大于0.6的点剔除的结果,匹配关系为323对。图7(c)进一步计算重投影误差,并使用RANSAC过滤的结果,匹配关系为292对。结果显示,交叉验证、距离筛选、RANSAC迭代的组合匹配策略可以很好的降低关键帧之间特征点的误匹配数量。

线特征匹配采用LBD二进制描述子,在加入全局和局部高斯权重系数后,添加线段局部外观约束和几何约束,加速特征的匹配。

计算LBD描述子后,首先计算二进制描述子之间的汉明距离,图8(a)是根据汉明距离暴力匹配的结果,出现了较多误匹配,匹配关系为65对。为减少误匹配数目,首先采用KNN将最优匹配距离与次优匹配距离比值小于2/3的匹配剔除,再对优化后的匹配采用交叉验证。图8(b)是优化后特征线匹配结果,匹配关系为55对。

图8 线特征匹配及筛选结果

2 VI-BLSD-Bow系统框架

基于VINS-Mono框架,并引入本文设计的点线特征提取和匹配算法,图9为本文搭建的VI-BLSD-Bow系统原理。首先进行点线特征提取和筛选,进行单目相机和IMU联合初始化建立初始地图;然后在后端构建包含先验、点线重投影误差以及IMU误差的滑动窗口因子图优化框架;再基于汉明距离匹配进行时间和空间一致性的检测,确定回环候选帧;最后进行4自由度位姿图优化,得到全局一致的轨迹和地图。SLAM系统的前端采用普吕克坐标参数化线特征的旋转和重投影变换,在后端采用普吕克坐标的正交形式进行参数优化。

图9 VI-BLSD-Bow系统原理图

本文搭建的VI-BLSD-Bow系统内部每个线程具体的功能为:

1) 点线特征提取及初始化输入单目图像和IMU测量信息,构建移动机器人运动模型,计算IMU预积分,提取本文设计的点线特征并跟踪,进行初步的位姿解算和误匹配的剔除,为后端优化提供良好初值。

2) 单目联合IMU初始化。

基于IMU预积分计算重投影残差和偏置残差,通过优化的方式恢复重力方向及绝对尺度,进行单目相机和IMU联合初始化,更新对应的移动机器人位姿,确认联合初始化完成程度。

3) 后端优化。

接受前端所有帧间的位姿信息,制定关键帧筛选策略,设计点线特征及相机位姿间的几何约束,建立局部地图的共视关系,构建包含点线特征和IMU测量信息的因子图,优化点线误差和IMU观测误差,进行边缘化处理。

4) 回环检测及全局位姿优化。

通过词袋向量和关键帧数据库计算图像相似度,确定回环候选帧,进行四自由度位姿优化,剔除误匹配后通过紧耦合的联合优化 (bundle adjustment, BA)对齐相机位姿与回环位姿,进而对所有关键帧进行全局位姿优化,消除累积误差和轨迹漂移。

线特征的端点在匹配过程中,容易受到遮挡和光照的影响,不确定性较大,不能直接通过线段端点初始化线特征。图10是空间直线的确定原理,分别通过匹配线段的2个端点与相机光心三点确定平面,再将这2个平面相交获得空间直线。相机坐标系原点和空间确定的平面为:

图10 空间直线的确定

πx(x-x0)+πy(y-y0)+πz(z-z0)=0

(8)

(9)

式中:sc1和ec1为左右端点在相机归一化平面下的坐标;ci为相机坐标系原点。

平面π1和平面π2相交所确定的空间直线,普吕克坐标表达为:

(10)

(11)

3 视觉惯性SLAM系统实验

本文利用搭载单目相机和IMU的AGV小车进行实验分析,使用的相机型号为Realsense D435i (不使用深度信息),IMU型号为BMI055,上位机型号为Jetson Nano。首先对单目相机和IMU进行联合标定,减少相机畸变和IMU零偏影响。然后提取实验环境图像信息,针对低纹理环境重新训练词袋模型。最后进行室内和走廊场景实验,针对强光照和低纹理场景,对比VI-BLSD-Bow系统和VINS-Mono系统的定位精度。

3.1 实验结果定性分析

图11为在强光照条件进行的室内实验,在白墙等点特征稀少处,VINS-Mono方案会出现严重的轨迹漂移,甚至跟踪丢失且重定位失败。但在此场景下,线特征丰富且易于提取,能弥补点特征缺少带来的影响。分析可知,引入本文设计的线特征提取算法能有效改善系统在复杂环境下,自主跟踪和定位的能力,提高系统的鲁棒性。

图11 不同算法在室内强光照且点特征缺少条件下的表现

图12是低纹理走廊实验,走廊特征分布松散,光照不足,特征点较为稀少,但强约束的线特征分布均匀。与室内场景类似,在AGV小车转弯处点特征不足,VINS-Mono系统容易产生轨迹漂移并且跟踪丢失。线特征能迅速补充定位特征,有效提高了系统的稳定性和鲁棒性。

图12 不同算法在点特征缺少走廊条件下的表现

3.2 实验结果定量分析

在实验中,为了比较不同算法的实际精度,采用绝对轨迹误差 (absolute trajectory error, ATE)进行比较。ATE比较待估计轨迹与真实轨迹的偏差,绝对轨迹误差及绝对平移误差分别为:

(12)

(13)

式中:Aall表示同时考虑平移和旋转的误差;Atrans表示仅考虑平移部分的误差;下角标truth表示轨迹真值;下角标test表示待估计轨迹。

此外,在进行轨迹误差分析时还可以只考虑旋转部分的绝对轨迹误差。针对不同算法在室内和走廊场景下采集得到的轨迹信息,表3和表4是分别计算得到的绝对轨迹误差。

表3 室内场景下的不同算法误差对比

表4 走廊场景下的不同算法误差对比

由实验结果可知,无论是否采用回环,本文设计的点线特征提取和匹配策略都可以明显提升VINS-Mono系统的定位精度。相较于VINS-Mono系统,VI-BLSD-Bow系统在室内环境下定位精度提高了24.2%,走廊环境下精度提高了8%。

4 结论

1) 修改LSD算法隐藏参数,引入长度抑制因子,可以大幅提高线特征提取速度及精度,并筛选进入后端优化的线段数量,提升SLAM系统运行效率。

2) 指定合适的点线特征筛选框架,可以大幅降低点线特征的误检问题,进而提升SLAM算法的运行速度和精度。

3) 在点特征缺失条件下,引入线特征可以有效提高SLAM定位系统精度和鲁棒性,在SLAM系统遇到大量低纹理墙面时,基于点线特征的系统定位精度远高于只基于点特征的系统。