基于目标检测与深度信息关联的RGB-D SLAM算法

张开平,黄宜庆,吴 桐

(1.安徽工程大学电气工程学院,安徽,芜湖 241000;2.安徽省电气传动与控制重点实验室,安徽 芜湖 241000;3.高端装备先进感知与智能控制教育部重点实验室,安徽 芜湖 241000)

实时定位与建图(Simultaneous Localization and Mapping, SLAM),是指机器人在没有先验信息的情况下,搭载特定的传感器,通过一系列运动来获取自身的位姿和构建周围环境地图的过程.视觉SLAM就是指将摄像头作为自身的主要传感器来采集数据,从而实现定位与建图功能的方法.目前较为成熟的开源视觉SLAM方案主要有MonoSLAM(Real-Time Single Camera)[1],SVO[2],ORB-SLAM2[3],ORB-SLAM3[4]等.

传统的SLAM系统在面对例如动态场景和欠纹理的极端环境时,系统的精确性和鲁棒性会受到极大的挑战.此类SLAM系统在工作时,只允许场景中出现少量的动态物体,当动态物体占据场景的大部分面积时,就会影响跟踪过程的数据关联,从而导致相机运动估计轨迹漂移等问题.为了提高SLAM在动态场景下的定位精度和鲁棒性,研究者们开始尝试在SLAM系统中去除这些由动态物体导致的影响.此类工作主要分为三类:1)基于多视图几何来实现动态点的剔除.Cheng等人提出了一种稀疏运动去除(SMR)模型[5],利用贝叶斯框架检测每一帧的动态和静态区域并去除动态区域,在检测中考虑了相邻帧的相似性和差异性来减少检测的不稳定性.Derome等人[6]通过当前图像和预测图像之间的残差获得光流并计算变换矩阵,将当前帧变换为前一帧图像来检测运动物体.但是仅仅依靠几何和光流方法会导致算法的鲁棒性不足.2)通过深度学习对输入帧进行处理,以获得图像的语义信息以剔除动态信息.Jing等人[7]基于ORB-SLAM3,提出使用SparseInst网络[8]对图像帧进行语义分割来获取语义标签和动态对象掩膜,随后使用光流和极线约束结合动态掩膜进一步确定动态特征信息,最后设计独立的点云线程来构建语义全局3D稠密点云地图.此类方法相比于仅使用几何方法具有更好的精确性,但是依赖于深度学习网络的质量,结构简单的网络不能够准确识别图像中的物体,复杂的网络会大大降低SLAM系统的运行速度并增加系统的运算量.3)结合多视图几何信息与深度学习来检测动态物体.WEI等人[9]设计了一种自适应语义分割算法对ORB-SLAM2的跟踪线程进行了改进,并使用全局BA算法优化相机位姿. DRSO-SLAM[10]计算特征点的光流场来跟踪动态物体光流,并将语义分割网络得到语义先验信息作为掩膜信息来计算基础矩阵,最后通过极线几何方法去除动态点.DP-SLAM[11]提出了一种实时运动概率传播方法,结合极线几何和语义分割去除动态特征信息.Ayman等人提出了一种深度注意力模块SLAM(DMA-SLAM)算法[12],通过与深度相关的自适应阈值和影响因子来考虑几何和语义模块中使用的深度影响.Zhang等人[13]提出了一种不受噪声影响的运动边界框算法,该算法可以检测不同姿态下的人和物体的运动状态,避免了在筛选动态特征信息时的误剔除.目前大多数动态环境SLAM算法都是采用此类方法,但是当动态物体占据场景中的很大比例时,直接去除动态物体上的全部信息,会导致用于跟踪的特征信息过少,造成轨迹丢失,对地图构建造成影响.

为了解决在动态场景下的定位与建图问题,本文构造了一种基于图像深度信息与语义先验信息相关联的RGB-D SLAM算法,对于语义分割网络的实时性问题,利用YOLOv5[13]目标检测轻量级网络对输入图像进行目标检测得到动态物体框.对于防止误剔除动态特征信息的问题,给出一种深度信息随机采样一致性(Depth-RANSAC)算法来剔除动态物体框内的动态特征点,保留框内的静态特征点,提高对于动态特征信息检测的精确性.

1 本文算法结构

本文算法在ORB-SLAM2的RGB-D模式基础上进行改进,总框架如图1所示,该框架是在ORB_SLAM2的基础上加入了目标检测线程和D-RANSAC特征点剔除算法.嵌入此线程和算法的目的是过滤被ORB-SLAM2提取的特征点集,这些点集中包括了动态特征点和静态特征点.如果所有特征信息都被用来估计相机运动轨迹的话,则会造成估计轨迹的误差,所以只需要静态特征点来计算相机位姿.本文算法的目标检测线程是基于YOLOv5网络实现的,以获取场景中的动态语义信息,D-RANSAC算法进一步筛选动态特征点,防止过多特征点被剔除而导致跟踪失败.

图1 本文算法框架图Figure 1 The framework diagram of this algorithm

当系统工作时,由RGB-D相机采集到的图像会同时传入跟踪线程和目标检测线程,首先在跟踪线程中对输入图像提取FAST关键点[15]并计算BRIEF描述子[16],同时目标检测线程处理传入的图像,生成场景中动态或潜在动态的物体框信息,并将结果传入跟踪线程.在跟踪线程中,对提取的特征信息与目标检测线程传入的语义先验信息进行拟合,并与场景深度信息相关联,通过D-RANSAC算法剔除属于动态物体上的特征点.过滤后的特征信息被用来进行位姿初始化和重定位,此后的流程类似于ORB-SLAM2.

目标检测算法的目标是识别出场景中的各类物体以及获得这些物体的位置信息.目前目标检测算法可以分为two-stage和one-stage两类.YOLO(You Only Look Once)[17]是Joseph Redmon和Ali Farhadi等人于2015年提出的基于单个神经网络的目标检测系统.YOLOv5是Glenn Jocher在2021年发布的一阶段目标检测算法,该算法在YOLOv4的基础上做出了一系列改进,其速度和精度都取得了一定进步.

2 深度信息随机采样一致性(D-RANSAC)算法

ORB-SLAM2是基于ORB特征点来实现定位与建图的SLAM系统,其提取的特征点质量关系到整个SLAM系统的鲁棒性与精确性.动态特征信息会使系统在计算相机运动轨迹时产生误差漂移.在DMS-SLAM[18]中,将YOLO目标检测网络获得的动态框内的所有特征点全部去除,来实现动态信息的筛除,但是当人占场景的三分之二以上的时候,这种方法会剔除场景中的大多数特征点,从而导致SLAM系统跟踪所需要的特征点数量不足,出现跟踪失败、系统的鲁棒性下降、相机估计轨迹漂移等问题.

本文给出一种结合语义先验信息和深度信息的动态点剔除方法,这种方法可以准确剔除动态框内属于人身上的特征点,保留框内的静态特征点.在目标检测线程中会获得当前场景中物体的bounding box.根据深度图像可以区分处于不同深度场景的不同物体的轮廓,如图2所示.借助这些信息,可以利用D-RANSAC算法来筛选落在人身上的特征点,从而保留那些属于动态框内的静态特征点.此算法是基于RANSAC(Random Sample Consensus)算法[19]改进而来,RANSAC是可以从一个包含有“外点”的点集中,通过迭代的方式来估计唯一数学模型的算法.本文D-RANSAC算法的使用有一些前提,首先是人需要占据bounding box的三分之二以上,其次落在人身上的特征点的深度值差异小于阈值,最后是人与周围背景要有明显深度差异.

图2 深度图像Figure 2 Depth image

深度随机采样一致性算法流程如下:首先将动态框内的特征点push一个集合Pdynamic,然后进入循环,当前迭代次数n小于我们初始化的迭代次数N时,在Pdynamic中随机选取两个特征点,获得这两个点在其深度图像上的深度信息d1,d2,计算Dmod,如式(1)所示.

(1)

随后遍历Pdynamic中剩余的点,计算每个点与Dmod的差值,当差值小于阈值Th时,将此点加入内点集,更新内点集的数量s和迭代次数N,迭代完成时,输出最终的内点集合.传统RANSAC算法需要提前规定好算法所需的迭代次数,如果迭代次数过大,则会导致计算资源的浪费,若设计的迭代次数过小,则不能获得比较精确的内点集.本文在D-RANSAC算法中加入了动态更新迭代次数算法,具体如下:

在Pdynamic中随机选择k个点来计算数学模型,则内点在全部点中的百分比P:

(2)

则k个点中至少有一个点为外点的概率为1-Pk.在N次迭代的条件下,每一次选择的点集中都存在离散点的概率为(1-Pk)n,用Ptrue来表示从点集中随机选择的点在N次迭代中都是内点的概率,则:

Ptrue=1-(1-Pk)N

(3)

那么迭代次数N为

(4)

每个点为内点的概率P是先验值,Ptrue是通过RANSAC计算得出正确数学模型的概率.RANSAC算法开始时P的准确值是未知的,在初始化时把P设为一个足够大的值,在每次更新深度模型和内点集的时候,利用当前内点集合计算出P,由P更新迭代次数N,由此可以根据由当前计算出的深度模型所获得的内点集合,动态更新RANSAC迭代次数,达到更精确的输出结果和更快的计算速度.

3 实验分析

为了进行对比,本节在特征信息提取、SLAM定位评估两个方面对算法进行评估,并与ORB-SLAM2、YOLO-SLAM等两种算法在TUM数据集上进行对比试验.ORB-SLAM2使用RANSAC算法来实现对于动态点的剔除,以获得相对准确的相机姿态.YOLO-SLAM是本文在ORB-SLAM2的基础上,仅加入YOLOv5目标检测网络并将动态框内的所有特征信息去除来实现动态信息的筛选的SLAM系统.

3.1 特征信息提取

本小节对本文中提出的深度信息随机采样一致性算法进行实验.实验平台是CPU为Intel i7-8700 ,GPU为GeForce GTX2080Super,内存为64GB的计算机.

实验在TUM数据集进行,实验结果如图3所示.

图3 特征信息提取实验结果Figure 3 Experimental results of feature information extraction

如实验结果所示,图3(A)表示ORB-SLAM2工作时,会从动态物体上提取特征点信息,这些信息会导致相机的估计轨迹出现误差.图3(B)表示只使用YOLOv5目标检测网络来获取动态物体并将动态框内的信息全部剔除.当动态物体占据场景的大部分时,会造成SLAM系统跟踪所需的信息过少,从而导致跟踪丢失.图3(C)表示在图3(B)的基础上加入了深度信息随机采样一致性算法后,将动态框内属于人身上的特征点筛除,保留了框内的静态特征点信息,从而不会因为特征信息过少导致跟踪丢失.

3.2 定位精确性检验

本节在TUM数据集上对本文算法进行了检验,并与ORB-SLAM2、YOLO-SLAM在定位评估方面做了定量分析.TUM数据集由在不同场景使用RGB-D相机记录的39个序列组成,包含了测试、手持SLAM、三维物体重建、动态物体等多种针对不同任务的数据集.本节主要测试TUM数据集中的动态场景序列.

本文算法与ORB-SLAM2、YOLO-SLAM算法在高动态序列上的轨迹误差如图4所示.从图4(A)可以看出没有深度信息随机采样一致性算法的YOLO-SLAM2表现得比原始的ORB-SLAM2差,这说明只通过目标检测网络筛除全部的动态信息,会使SLAM系统跟踪所需的特征信息过少,导致跟踪丢失,造成了估计轨迹的误差.在图4的实验结果中,本文算法表现最好,这表明深度随机采样一致性算法有效解决了YOLO-SLAM导致的跟踪信息过少的问题,并在高动态场景中相较于ORB-SLAM2算法具有更好的性能.

本文算法与ORB-SLAM2、YOLO-SLAM的对比结果如表1～3所示.表1是各算法绝对轨迹误差,可以看出本文算法在前四个高动态序列表现最好.其中,在动态场景较为复杂的“fr3/w/rpy”序列上,本文算法相较于ORB-SLAM2,相机估计轨迹在RMSE和STD精度提高了25%和15%,而与YOLO-SLAM相比较提高了15%.在前四个高动态序列中,本文算法相较于ORB-SLAM2在RMSE上平均提升55.5%,在STD上平均提升63.25%,相较于YOLO-SLAM,平均精度分别提高47.25%和38.75%.由此可见,本文算法在高动态环境下的精度均优于对比算法.

表1 绝对轨迹误差对比结果

从表2看出,YOLO-SLAM在“fr3/w/rpy”、“fr3/w/static”和“fr3/w/half”三个动态序列上的精度相较于原始的ORB-SLAM2,取得了较差的结果.原因是YOLO-SLAM将目标检测网络中获得的动态物体框内的特征信息全部去除,造成了跟踪所需的信息过少,导致了跟踪丢失.而本文给出的深度信息随机采样一致性算法去除了动态物体框内真正属于动态物体(在本文中是人这一类别)上的特征信息,保留了静态特征,所在高动态序列RPE平移和旋转部分的对比中,本文算法表现最好.

表2 相对轨迹误差对比结果

从表3可以看出,在低动态序列“fr3/s/xyz”和“fr3/s/static”中,本文算法相较于对比算法,提升并不明显.这是因为ORB-SLAM2在低动态和静态场景下工作的精度足够好,所以在此场景下的本文算法的改进并不明显.

表3 相对轨迹误差旋转部分对比结果

从图5(A)可以看出,ORB-SLAM2在fr3/w/xyz序列中的估计轨迹在y方向的误差达到了1.25m,YOLO-SLAM的估计轨迹与真实轨迹在y方向的误差达到了0.6 m,而本文算法相较于对比算法具有更小的误差,在y方向上只有0.3 m,在x方向只有0.2 m.因此在动态序列中,本文算法比对比算法具有更优的鲁棒性.

图5 估计轨迹与真实轨迹误差图Figure 5 Error diagram of estimated trajectory and real trajectory

4 结 语

本文构造了一种基于ORB-SLAM2的动态环境RGB-D SLAM系统,使用YOLOv5目标检测网络来对输入图像进行动态物体检测,再将语义先验信息与场景深度信息相关联,使用D-RANSAC算法来剔除动态物体框内的动态特征点,保留框内的静态特征点,利用保留下来的稳定静态特征点来进行动态序列下的运动轨迹估计和地图构建.通过实验平台对TUM数据集的检测,结果表明本文算法在高动态环境下的相机姿态和定位精度上有明显提高.本文算法在面对低动态场景以及静态场景时,前者由于目标检测网络将人定义为高动态物体,当人是静止或者是缓慢移动的时候,会将人身上的一些静态特征点剔除,导致跟踪效果下降.如何正确筛除当前正在运动的动态特征点时下一步工作的重点,未来考虑改进本文的D-RANSAC算法,使其适应更复杂的环境,使其在工作时具有更好的鲁棒性.