基于视觉传感器的半直接SLAM算法*

彭 虎, 董 翔, 李 腾, 樊 渊

(安徽大学 电气工程与自动化学院,安徽 合肥 230601)

0 引 言

当RGB-D相机发布后,许多研究开始专注于研究以RGB-D相机作为传感器的视觉同时定位和地图创建(vision simultneous localization and mapping,VSLAM)系统[1]。目前VSLAM方法主要分为两种:特征点法和直接法[2]。在基于特征点法的VSLAM中,Mur-Artal R等人[3]使用图形优化技术开发了ORB-SLAM2,系统围绕着ORB特征点进行优化计算,同时提升了效率和精度;但它计算量大,而且在特征点缺失的区域,建图效果不理想。相比于特征点法,直接法对特征点缺失区域,可以保证追踪的稳定性和实时性。Schöps T等人[4]通过对像素点对之间的基线立体配准提出了BAD SLAM,它是针对像素进行的,在CPU上实现了半稠密点云地图的构建;但和其他直接法一样，其对相机内参和曝光比较敏感,而且在相机快速运动时容易丢失。

针对以上问题,本文提出了一种快速的半直接SLAM(fast semi-direct SLAM,FSD-SLAM)算法。利用基于子图的特征增强模块和改进的相机位姿估计方法,使得系统能够在无结构和特征缺失等特殊场景中实现精确、稳定的相机运动估计。再使用增量式动态协方差缩放算法优化相机位姿,提高了优化的效率和实时性,并将优化后的相机位姿提供给地图构建程序,获得最终的三维重建地图。

1 FSD-SLAM系统框架

1.1 初始化与特征增强

FSD-SLAM系统框架如图1所示。当视频帧输入时,计算相邻两帧特征并进行匹配。当匹配到足够多数量的特征时,求解与特征关联的像素点深度和像素点的三维坐标,完成初始化。

图1 系统的总体框架

提取针对运动模糊和无纹理环境的关键点对于VSLAM系统起着重要作用。基于子图的特征增强模块可以对FAST关键点[5]进行特征增强,根据机器人测距的可靠性,通过对三维特征点的增强生成子图,方便系统稳定地提取图像特征点。系统完成初始化后,将视频帧添加到一个固定长度的视频时间轴Qt队列中,子图的特征增强模块从Qt头部提取RGB输入帧,并将处理之后具有特征点增强的帧更新回队列当中,得到具有FAST特征点积累的RGB帧。图2显示了这个模块在不充分的特征环境中如何工作。第i个子图中特征R的增强三维点li表示如下

(1)

图2 在不充足特征的环境下通过增强三维特征点实现子地图的概念

1.2 相机位姿估计

在特征点法中的重投影误差基础上加权融合了匹配特征点对的光度误差,通过优化求解这个误差方程来计算相机位姿。基于特征点法的相机运动估计的方法如图3所示。

图3 特征点法相机位姿估计示意

在利用匹配特征点对重投影误差的相机位姿估计方法中,假设三维空间点P11在连续相邻两帧图像上的二维像素点坐标分别为p11和p13,如果相机位姿估计准确,则真实位置p12和p13为同一个点；如果位姿估计不正确,投影点p13与p12之间必然会存在着一个重投影误差e。不失一般性,考虑任意一个空间点P1i,e定义如下

(2)

对于全部的特征点,总的重投影误差方程定义如下

(3)

式中p1i为三维空间坐标点P1i在后一帧上对应的二维像素坐标点；T,K以及Z2i分别为估计的相机位姿、相机内参以及P1i对应相机坐标系下的Z轴坐标；N为特征匹配总数。

另外,通过光度误差来求解相机的运动估计示意图如图4所示。

图4 光度误差示意

假定在前一个相机帧中的任意一个像素点p21对应世界坐标系下的空间点为P21,利用相机旋转和平移到下一帧图像后对应的像素位置为p22。如果相机位姿估计的不正确,则两者会存在一个光度误差。根据以上描述,某个像素的光度误差可以定义为

(4)

所有对应像素对的误差函数之和定义为

(5)

式中p2i为前面一个相机帧中的二维像素坐标点；P2i为像素点p2i对应的三维空间点坐标；T,K以及Z2i分别为估计的相机位姿、相机内参以及P2i对应相机坐标系下的Z轴坐标；I1()和I2()为提取前面一个相机帧和后面一个相机帧中对应像素的光度信息；M为像素点总数。

将相互匹配的特征点对的重投影误差和对应像素的光度误差进行加权融合,可以求出相机位姿的估计值如下所示

(6)

式中λ为设定的权值系数,它是用来线性地调整相互匹配特征点对的重投影误差和其对应光度误差在相机位姿优化中所起的作用。其中,权重λ取经验值0.5。

1.3 全局优化

随着输入图像的增多,需要去优化闭环检测的信息以及不同时刻相机位姿估计的误差。闭环检测采用基于视觉词袋[6]的快速检测方法。具有代表性全局优化算法有SC(switchable constraints)算法[7]。SC算法需要同时求出开关变量、位姿和闭环的联合优化,所以计算量大而且实时不强。本文提出来的算法是对SC算法的改进。不失一般性考虑两个节点k和l之间的边,可以将误差分为两个块,第一个块考虑除kl之外的所有边,第二个块仅考虑边kl。可以将目标函数重新定义为

(7)

式(7)表明,SLAM问题的解包括节点状态向量X*和开关变量S*。其中,sij为开关变量,Ξij为开关先验,xi为时间i处的机器人姿态,zij为通过测距法或传感器测量获得的两个姿态之间的转换,而f(xi,xj)为预测xi和xj之间的测量值函数。里程表和传感器测量值的协方差为Σ和Λ。

令e=X*,S*,整理式(7)得

(8)

式中g(·)为不包含边kl以外所有的误差,h(·)为包含边kl的误差。一旦优化器收敛,则对整个函数中所有变量的偏导数为零。所以对式(8)中的开关变量skl求偏导有

(9)

联合式(8)和式(9),化简求解h(·)并对其求导得到

(10)

则易知0≤h≤Φ。此时需要考虑所有节点k和l之间的边,对于所有的开关变量,都满足约束0≤h≤Φ。于是有

(11)

由式(11)可以求得

(12)

至此就找到了最优的开关变量s的取值区间。大大缩小了原SC算法给出的开关变量取值区间,减少计算量的同时,增加了全局优化的准确性。

2 实验结果与分析

2.1 TUM数据集上定量的评估结果

为了验证本文系统中特征增强模块的性能,对TUM[8]数据集中fr3_lon-g_office序列的每一幅图像,分别用F-SD-SLAM和SURF、FAST以及SIF-T三种流行的特征提取算法进行特征提取,计算每一幅图像的特征提取效率,对整个序列分别取Min、Max、Mean和RMSE,结果如图5。由实验结果可以看出,FSD-SLAM可以在短时间里提取更多的特征点,具有更好的特征提取效率。

图5 与其他方案相比的特征提取效率

对于本文系统优化算法准确性的评估,以优化后的绝对轨迹误差(absolute tracking of error,ATE)和相对姿态误差(relative pose error,RPE)[9]为评价标准,选取了TUM数据集中的freiburg1_room序列测试优化的工作效果,并与具有代表性的RGB-D SLAM[10]优化的效果做了对比。测试时轨迹经过优化后的ATE与RPE结果如图6所示。由实验结果可以看出,在相同的视频序列freiburg1_room下FSD-SLAM算法相比于RGB-D SLAM算法优化后得到的ATE和RPE要小的多。

图6 优化后估计轨迹和地面实况轨迹之间的ATE和相机位姿的RPE

定量的评价此时优化后的ATE,结果如表1所示。

表1 Freiburg1_room序列测试优化后的绝对轨迹误差 m

定量评价优化后的RPE,分为平移误差和旋转误差,结果分别如表2和表3所示。由表1和表2的结果分析可知,FSD-SLAM优化后的定位精度达到0.472 %,RGB-D SLAM优化后的定位精度为0.341 %,FSD-SLAM优化后的定位精度比RGB-D SLAM优化后的定位精度提高了38个百分点。

表2 Freiburg1_room序列测试优化后的相对位姿平移误差m

表3 Freiburg1_room序列测试优化后的相对位姿旋转误差(°)

进一步验证本文系统在视觉特征稀缺以及低纹理环境下的鲁棒性。其中评估的标准是将系统运行20次,系统跟丢的次数。表4的实验结果表明,ORB-SLAM2系统在fr3_nostructure_notexture_far、fr3_nostructure_notexture_near、fr3_structure_notexture_far和fr3_structure_notexture_near这些图像序列中都跟丢了,而FSD-SLAM能很好地跟踪这些序列。其在大多数无纹理和无结构的环境中,具有很少的跟丢次数。

表4 FSD-SLAM与ORB-SLAM2鲁棒性比较

2.2 室内环境中定性的评估结果

真实环境中的实验平台为turtlebot2,配备的传感器为KinectV2。从启动机器人到结束工作,对应的持续运动时长为48.90 s,以及3.986 m的地面实况轨迹长度,0.432 m/s的平均平移速度和29.882 rad/s平均角速度。测试比较了本文方法和几个当前具有代表性的SLAM方法在相机位姿估计上的ATE和RPE,这几个SLAM方法分别为RGB-D SLAM、HOOFR SLAM[11]、MIS-SLAM[12]、BAD SLAM[4]和PL-SLAM[13],结果如表5所示。

表5 在室内环境下FSD-SLAM与其他方法相比较的ATE与RPE m

图7显示了系统得到的初始点云图和全局稠密点云图。

图7 系统生成的初始点云图和全局稠密点云图

由表5的实验结果可以看出,本文方法优于了其他实时的SLAM方法,实现了更低的错误率。同时由图7可知,运用本文系统能够重建室内环境的稠密全局点云地图。

3 结 论

本文提出的FSD-SLAM方法在初始化与特征增强部分,使用了基于子图的特征增强模块,有效地解决了弱纹理环境下特征的提取问题；在相机位姿估计部分,加权融合了图像特征点匹配对的重投影信息和特征对应像素对的光度信息,并结合增量式动态协方差缩放算法对估计的相机位姿进行全局优化,使得系统在无结构和特征缺失等特殊的场景中仍然能实现精确的相机运动估计和环境地图重建。TUM数据集和实际室内场景中的测试实验均表明：FSD-SLAM保持较高的定位精度,并且在无结构和特征缺失等特殊环境下具有较好的鲁棒性。