机器人 2023, Vol. 45 Issue (1): 16-27  
0
引用本文
王梦瑶, 宋薇. 动态场景下基于自适应语义分割的RGB-D SLAM算法[J]. 机器人, 2023, 45(1): 16-27.  
WANG Mengyao, SONG Wei. An RGB-D SLAM Algorithm Based on Adaptive Semantic Segmentation in Dynamic Environment[J]. ROBOT, 2023, 45(1): 16-27.  

动态场景下基于自适应语义分割的RGB-D SLAM算法
王梦瑶 , 宋薇     
上海大学, 上海 200444
摘要:目前较为成熟的视觉SLAM算法在应用于动态场景时, 往往会因动态对象干扰而导致系统所估计的位姿误差急剧增大甚至算法失效。为解决上述问题, 本文提出一种适用于室内动态场景的视觉SLAM算法, 根据当前帧中特征点的运动等级信息自适应判断当前帧是否需要进行语义分割, 进而实现语义信息的跨帧检测; 根据语义分割网络提供的先验信息以及该对象在先前场景中的运动状态, 为每个特征点分配运动等级, 将其归类为静态点、可移静态点或动态点。选取合适的特征点进行位姿的初估计, 再根据加权静态约束的结果对位姿进行二次优化。最后为验证本文算法的有效性, 在TUM RGB-D动态场景数据集上进行实验, 并与ORB-SLAM2算法及其他处理动态场景的SLAM算法进行对比, 结果表明本文算法在大部分数据集上表现良好, 相较改进前的ORB-SLAM算法, 本文算法在室内动态场景中的定位精度可提升90.57%。
关键词动态场景    跨帧检测    语义信息    运动等级    静态约束    
中图分类号:TP242.6            文献标志码:A            文章编号:1002-0446(2023)-01-0016-12
An RGB-D SLAM Algorithm Based on Adaptive Semantic Segmentation in Dynamic Environment
WANG Mengyao , SONG Wei     
Shanghai University, Shanghai 200444, China
Abstract: When the existing visual SLAM (simultaneous localization and mapping) algorithms are applied to dynamic environments, the pose error estimated by the system often increases sharply or even the algorithm fails due to the interference of dynamic objects. In order to solve the above problems, a visual SLAM system is proposed in this paper for indoor dynamic environments. By adaptively judging whether the current frame needs semantic segmentation according to the motion level information of the feature points in the current frame, the cross-frame detection of semantic information is realized. According to the prior information provided by the semantic segmentation network and the motion state of the object in the previous scene, each feature point is assigned a motion level and is classified as static point, movable static point or dynamic point. Some appropriate feature points (static points) are selected for initial pose estimation, and then secondary optimization of the pose is performed according to the results of weighted static constraints. In order to verify the effectiveness of the proposed algorithm, experiments are carried out on the TUM RGB-D dynamic scene dataset, and compared with ORB-SLAM2 and other SLAM algorithms for dynamic environments. The results show that the proposed algorithm performs well on most datasets, and the positioning accuracy in indoor dynamic environments can be improved by 90.57% compared with the ORBSLAM algorithm without the improvement.
Keywords: dynamic environment    cross-frame detection    semantic information    motion level    static constraint    

1 引言(Introduction)

以视觉传感器为核心的同步定位与地图构建(SLAM)算法通常被称为视觉SLAM。由于图像本身包含颜色、纹理等丰富的场景信息,且相机的成本相对于激光等其他传感器较低,近年来视觉SLAM算法备受关注。目前较为成熟的视觉SLAM算法有特征点法[1-4]、半直接法[5]、直接法[6]等,但这些算法中都隐含了静态场景的强假设,导致其仅适用于处理全静态或仅包含少量、较小移动对象的场景,在较大程度上限制了其应用范围。

ORB-SLAM[1-3]等传统的视觉SLAM算法是通过随机抽样一致性(RANSAC)算法剔除外点,但仅能处理外点数目较少的情况。当动态对象面积较大或数量较多时,RANSAC算法会误判动态特征点为静态特征点,这使得仅有的少数静态特征点被当作外点剔除,而应当剔除的外点却被用于位姿估计,导致位姿计算结果完全偏离真值。

近些年,国内外有较多学者对上述问题进行研究。Wang等[7]在光流信息的基础上,基于最大组属于静态环境的假设进行稀疏点追踪聚类,完成对动态对象的划分。Sun等[8]根据自运动补偿图像差异的方法来大致检测运动对象的运动,再使用粒子滤波器和最大后验(MAP)估计器来精确确定前景,剔除动态干扰,但此方法要求场景中必须包含平面,且静态背景在图像中的占比要大于动态对象。林志林等[9]将场景流表示为运动矢量,通过虚拟地图点和高斯混合模型进行运动物体检测。Chen[10]提出一种双目视觉SLAM系统,通过改进的半全局匹配算法获得视差图,结合超像素分割的结果最终确定运动对象的边界。文[11]将提取的特征点投影至深度图,在深度图上进行聚类操作,再通过两次极线约束以及运动模型完成动态点的剔除,但是在静态背景与动态对象的深度差较小时,容易产生错误的聚类结果。文[12]提出点特征和线特征结合的方法,根据点特征计算初始位姿并赋予线特征静态权重,进一步剔除动态线特征,此算法中使用的线特征虽然稳定性较好,但不易提取,且该算法仅包含里程计部分,并非一个完整的SLAM系统。魏彤等[13]提出一种适用于视觉导盲场景的双目SLAM系统,其采用改进的半全局块匹配算法结合极线约束对动态对象进行区域分割,再通过种子点生长算法对轮廓进行优化,此算法虽然适用于动态场景,但受其算法限制,只适用于场景中静态点数量占优的情况。Bojko等[14]使用提前生成的动态对象掩模对网络进行训练,将此网络与ORB-SLAM2和LDSO算法相结合,提升了原算法在动态场景中的定位精度。Bescos等[15]使用语义分割网络和生成式对抗模型对场景中的动态对象进行分割并对分割后的动态对象部位进行修复,最终生成了静态点云地图。Bao等[16]针对工地场景中有多台大型建筑机器的情况,将语义信息与目标层几何约束相结合快速检测场景的静态部分,提高了在真实工地场景中相机运动跟踪的鲁棒性。

随着深度学习技术和硬件水平的快速发展,越来越多的研究将目标检测、语义分割、实例分割等深度学习网络应用于SLAM系统,以提高机器人对周围环境的认知能力。Bescos等[17]提出DynaSLAM算法,利用Mask R-CNN[18]分割出“人”所在区域,通过多视图几何法检测非先验动态对象,并构建静态背景地图,可生成较好的位姿估计结果。Yu等[19]提出DS-SLAM算法,使用SegNet网络[20]进行语义分割,结合语义信息和光流信息进行运动一致性检测,提高了位姿估计精度并生成包含语义信息的八叉树地图。但上述DynaSLAM和DS-SLAM系统对每一帧图像均进行语义分割处理,耗时较长,即使SegNet网络的分割速度稍高于Mask R-CNN法,但仍远远不能达到实时性的要求。Xiao[21]为解决目标检测网络SSD[22]查全率低的问题,基于运动对象的加速度趋于0的假设,提出了一种漏检补偿算法,根据前后帧动态点的位移大小决定是否要将其剔除。文[23]提出的Detect-SLAM算法仅在关键帧进行目标检测,在一定程度上提高了运行效率。SaD-SLAM算法[24]使用Mask R-CNN网络获得语义信息,借助极线约束完成真实动态点的确定,在一定程度上提升了SLAM系统的位姿估计精度,但是该算法在使用前必须先获取场景数据集,提前做好掩模获取工作,因此不能用于需要即时定位的场景。Zhang等[25]利用实例分割网络和稠密光流追踪法,完成室外动态场景下的位姿计算及对象追踪,但该方法仅适用于室外运动的刚性对象,如汽车等。

本文针对室内动态场景,提出一种基于语义分割的视觉SLAM算法,通过跨帧检测的形式,将语义先验信息向后传递,再与加权静态约束结合,完成特征点真实运动状态的分类。真实运动状态的判断,一方面可以减少动态特征点与静态特征点的误判;另一方面,针对语义分割出的运动物体,并非直接剔除,而是判断其当前运动状态,如当前静止则可看作是静态特征点,从而增加参与位姿计算的特征点个数,提高位姿估计的精度。同时,跨帧检测方法可降低深度学习网络对SLAM系统实时性的影响,保证系统在提高位姿精度的同时,仍满足一定的实时性要求。

2 系统框架(System framework)

本文的系统框架基于ORB-SLAM2算法构建,如图 1所示,共包含4个并行线程:追踪线程、语义分割线程、局部地图线程以及回环检测线程,重点对其中的追踪线程和语义分割线程进行了改进。系统的输入为一组连续的RGB-D图像,将其传入追踪线程后,先进行ORB特征点提取,根据先前场景中的运动等级信息和运动等级分配算法确定当前帧中每个特征点的运动等级,将特征点分为静态特征点、可移静态点和动态特征点集合,然后根据静态特征点集初步计算相机位姿。

图 1 系统框架图 Fig.1 System framework

若系统判断当前帧需要增加语义信息来保证帧中特征点能被正确分类,则将当前帧的RGB图像传入语义分割线程进行处理,得到当前场景中包含的潜在运动对象的类别以及位置信息。追踪线程根据此信息以及特征点的运动等级信息对特征点的真实状态进行判断,避免发生动态特征点与静态特征点的误判情况。最后将追踪线程初步优化后的位姿、特征点、关键帧等信息传入局部建图线程以及回环检测线程,利用局部光束平差法(BA)、全局BA等优化方法对位姿、地图点再次优化,最终获得精度较高的位姿估计结果。

3 基于语义分割的特征点运动等级划分(Semantic segmentation-based motion level division of feature points)

近年来,随着深度学习技术的快速发展,提出了SegNet、Mask R-CNN、YOLO v1-v5等多种语义分割、实例分割或目标检测网络。其中,目标检测速度相对较快,但会同时选中目标和部分背景,因此需要再进行额外处理;而语义分割和实例分割可以直接获得较为准确的轮廓信息,其中实例分割可进一步区分同一类别的不同个体,但耗时也更多。

本文从深度学习网络的性能及SLAM系统实时性的角度考虑,最终选用Light-Weight RefineNet[26]网络完成语义分割的任务,对网络进行微调并利用PASCAL VOC2012数据集对该网络进行训练,该数据集共有包含背景在内的21种类别,涵盖了室内常见的物体,如人、桌子、椅子、杯子、显示器等。对象的运动状态由静到动,取值范围为0~10,0表示该物体的移动等级最低,可以认为是完全静止,比如建筑物等;10则表示该物体的移动等级最大,比如人、动物等。

为了平衡语义分割网络的精度和速度,本文在系统中单独开设线程来完成语义分割工作,并提出跨帧检测算法来进一步降低语义分割网络对SLAM系统实时性的影响。算法根据特征点运动等级初始化的结果,结合可初始化的特征点个数与总数的占比自适应选择图像帧进行语义分割处理。若采用间隔固定帧数的方法,则当间隔较大时,可能会产生某些帧中的部分特征点运动等级无法初始化的问题;若间隔较小,则语义分割次数较多,无法起到保证系统实时性的目的。

3.1 运动等级初始化

当SLAM系统处于初始化阶段时,仅根据第1帧图像无法判断当前场景中的对象是否处于运动状态,因此在初始化阶段,特征点的运动等级取决于语义分割线程获得的掩模。运动等级的定义如下:

$ \begin{align} {\rm{Class}} & = \{{\rm{person}}, {\rm{chair}}, {\rm{table}}, {\rm{bottle}}, \cdots \} \end{align} $ (1)
$ \begin{align} L_{t} & =\{L_{t}^{0}, L_{t}^{1}, \cdots, L_{t}^{i} , \cdots, L_{t}^{20} \} \end{align} $ (2)
$ \begin{align} L_{0} (p_{j})& =L_{t}^{i}, \quad {\rm{if\;KeyPoint}}_{j}^{0} \in {\rm{Class}}_{i} \end{align} $ (3)

其中,Class集合为PASCAL VOC2012数据集划分的除背景以外的物体的20种类别;$ L_t $与Class中的元素个数相同,表示各类别的先验运动等级,取值范围为0~10,具体取值如表 1所示;式(3)用来初始化特征点的运动等级,设初始化阶段第0帧中第$ j $个特征点$ {\rm{KeyPoint}}_{j}^{0} $所属对象类别为$ {\rm{Class}}_{i} $,则该特征点的初始化运动等级$ L_{0} (p_{j}) $等于所属对象的先验运动等级。

表 1 运动等级初始化 Tab. 1 Motion level initialization

表 1中*表示该类别在室内不会出现,因此直接将其等级设为0,其他则是根据对应类别对象在日常生活中的状态进行定义。本文将运动等级小于等于4的特征点定义为静态点,大于4且小于等于7的特征点定义为可移静态点,大于7且小于等于10的特征点定义为动态点。这样,在初始化阶段,利用语义分割结果便可得到各特征点的运动等级。

3.2 运动等级传递策略

为保证系统实时性,本文并非对每一帧图像都进行语义分割,而是由跨帧检测算法自适应地选择部分图像帧进行处理。对于未采取分割操作的图像帧,本文提出了一种融合上一帧、对应关键帧、以及局部地图点等信息的运动等级传递策略来判定当前帧中特征点的运动等级。

在运动等级传递之前,需要先完成特征点匹配。采用基于ORB-SLAM2算法的特征点匹配方法,通过恒速运动模型和词袋模型加速特征点的匹配。之后的运动等级传递过程如图 2所示。

图 2 运动等级传递策略 Fig.2 Motion level delivery strategy

根据特征点在上一帧或在局部地图中是否有匹配点,可将分配方法主要分为2种:

(1) 如果在上一帧或局部地图中找到匹配点,如图 2中标号①②所示的匹配过程,则可以直接将该点的运动等级赋值为匹配点的运动等级;

(2) 对于没有找到匹配点的特征点(如当前帧I中的蓝色点所示),则需根据当前帧RGB图及对应深度图的信息在当前帧的特征点中寻找它的“相似点”(如当前帧II中的实线椭圆所示),并将该点的运动等级赋值为对应“相似点”的运动等级。

如果该过程中也没有找到“相似点”(如帧中紫色点所示),则需调用最近一次进行语义分割处理的图像帧。如果在该帧找到了匹配点,则该点的运动等级等于其匹配点的运动等级;否则,认为该点为新的特征点(如当前帧IV中的蓝色点所示),将其运动等级初始化为5,以便后续正确分类。

该过程的数学模型描述如下:如果第$ m $帧的第$ j $个特征点$ p_{j}^{m} $在上一帧或局部地图中找到了匹配点,则用与其匹配的上一帧特征点$ p_{i}^{m-1} $或地图点$ P_{k} $对应的运动等级来更新该特征点的运动等级。该过程可表示为式(4),其中“$ \sim $”表示匹配关系。

$ \begin{align} \begin{cases} L(p_{j}^{m})=L(P_{k}), & {\rm{if\;}} p_{j}^{m} \sim P_{k} \\ L(p_{j}^{m})=L(p_{i}^{m-1}), & {\rm{if\;}} p_{j}^{m} \sim p_{i}^{m-1} \end{cases} \end{align} $ (4)

对于没有找到匹配点的特征点,则需作进一步的处理,即根据距离约束和深度约束的加权得分在当前帧中寻找“相似点”。在以当前特征点$ p_{j}^{m} $为中心、$ r $为半径的圆形区域内搜索特征点并将其放入KeyPoint_R集合中,本文认为与当前特征点距离越近、深度差值越小的特征点与当前特征点越相似。但多数情况下,最小距离和最小深度差不能同时满足,因此将两者进行加权,总得分计算公式为

$ \begin{align} s_{i} =\alpha \frac{1}{\beta_{1}} d_{i, j} +(1-\alpha)\frac{1}{\beta_{2}} | {z_{i, j}} | \end{align} $ (5)

其中,$ s_{i} $表示集合KeyPoint_R中第$ i $个特征点$ p_i^m $加权得分,$ d_{i, j} $表示特征点$ p_i^m $与当前帧第$ j $个特征点$ p_{j}^{m} $之间的欧氏距离,单位为像素,$ |z_{i, j} | $表示$ p_{i}^{m} $$ p_{j}^{m} $之间的深度差的绝对值,单位为m,由于$ d_{i, j} $$ z_{i, j} $单位不同,因此需要用参数$ \beta_1 $$ \beta_2 $将其归一化,$ \alpha $则为距离值和深度差值的权重。

由式(5)可知,KeyPoint_R集合中得分越低的特征点的相似度越高,但为确保2个特征点之间具有足够的相似性,仅当该点得分小于等于阈值$ \tau_1 $时,才认为两者“相似”,否则认为在当前帧中没有特征点与其“相似”,需要与最近一次进行语义分割的帧再次进行匹配,若依旧没有找到匹配点,则认为该特征点为新出现的特征点,将其运动等级初始化为5。

运动等级传递前后的特征点状态如图 3所示,图中标志中心点为特征点位置,绿色标志表示参与位姿计算的特征点,红色标志表示不参与位姿计算的动态特征点。图 3(a)为直接采用ORB-SLAM2算法得到的当前帧特征点,没有经过筛选,所有点都将参与位姿计算;而图 3(b)为经过运动等级传递过程后的特征点分类结果,图中2个人正处于运动状态,由图可知,2个人身上的大部分特征点都被正确归类为动态点,不参与位姿计算过程,因此会在较大程度上提高后续位姿估计的精度。

图 3 运动等级传递示意图 Fig.3 Diagram of motion level delivery
4 基于加权静态约束的位姿优化(Pose optimization based on weighted static constraints) 4.1 位姿估计

运动等级传递过程结束后,可根据特征点的运动等级值将其存入对应的静态点、可移静态点、动态点集合中。利用静态特征点集,采用最小化重投影误差的方法进行位姿估计:

$ \begin{align} (\mathit{\boldsymbol{R}}, \mathit{\boldsymbol{t}})= \mathop{\rm arg\;min}\limits_{\mathit{\boldsymbol{R}}, \; {t\mathit{\boldsymbol{}}}} \sum\limits_{i=1}^n \rho \Big\| \mathit{\boldsymbol{p}}_{i} -\mathit{\boldsymbol{\pi}} (\mathit{\boldsymbol{RP}}_{i} +\mathit{\boldsymbol{t}}) \Big\|_{2}^{2} \end{align} $ (6)

其中,$ (\mathit{\boldsymbol{R}}, \mathit{\boldsymbol{t}}) $表示相机位姿;$ \rho $为Huber核函数,用来避免当2范数以指数增长时误差项太大导致优化方向错误的问题,保证优化过程的正确性[1]$ n $为静态点集中匹配点对的总数;$ \mathit{\boldsymbol{p}}_{i} $为2D特征点坐标;$ \mathit{\boldsymbol{P}}_{i} $表示与特征点匹配的3D空间点坐标;$ \mathit{\boldsymbol{\pi}} (\cdot) $为从相机坐标到像素坐标的投影函数,定义如下:

$ \begin{align} \mathit{\boldsymbol{\pi}} ([X, Y, Z]^{\rm T})=a \begin{bmatrix} f_{x} \dfrac{X}{Z}+c_{x} \\[6pt] f_{y} \dfrac{Y}{Z}+c_{y} \end{bmatrix} \end{align} $ (7)

其中,$ (f_x, f_y) $表示焦距,$ (c_x, c_y) $指主点坐标。

4.2 加权静态约束

若直接根据静态特征点计算位姿,则会存在静态点数量较少或难以发觉某对象已由静态转为动态的问题,从而降低位姿估计的精度。为了有足够数量的特征点能参与后续局部BA优化和全局BA优化,需要进一步确认场景中可移静态点和动态点的真实运动状态,如其在当前场景中处于静止状态,则可看作是静态特征点,从而增加可参与位姿计算的特征点个数。

本文提出加权静态约束的方法获得特征点的真实运动状态,并对位姿进行更新。

当相机从不同视角观察同一空间点时,见图 4(a)$ C_1 $$ C_2 $分别为参考帧和当前帧的相机中心,$ P $为某一空间点,直线$ l_1 $$ l_2 $为极线,$ p_1 $$ p_2 $为空间点$ P $在2个相平面上的投影点,其理论上应满足:

$ \begin{align} \mathit{\boldsymbol{x}}_{2} \mathit{\boldsymbol{Fx}}_{1}^{\rm T}=0 \end{align} $ (8)
图 4 极线约束示意图 Fig.4 Diagram of epipolar constraint

其中$ \mathit{\boldsymbol{x}}_{1}, \mathit{\boldsymbol{x}}_{2} \in \mathbb{R}^{3} $分别为$ p_1 $$ p_2 $的归一化像素坐标,$ \mathit{\boldsymbol{F}} $表示基础矩阵,其可根据4.1节中的初始位姿通过八点法计算得到。

理想情况下,$ p_2 $位于极线$ l_2 $上,满足式(8)约束。设极平面的法向量为$ \mathit{\boldsymbol{n}} $,则此时可满足:

$ \begin{align} \mathit{\boldsymbol{n}}\times \overrightarrow {C_{2} p_{2}'} =0 \end{align} $ (9)

实际情况下,若空间点发生移动,如图 4(b)中红色点所示,相机由参考帧移动至当前帧位置时,空间点$ P_{M1} $移动至$ P_{M1}' $,此时$ P_{M1}' $在当前帧上的投影位置将偏离对应的极线$ l_2 $,导致不能满足上述约束。但当特征点沿着相机光轴方向运动时,如图 4(c)所示,空间点由$ P_{M2} $移动至$ P_{M2}' $位置,由于移动方向的特殊性,移动后的$ P_{M2}' $位置在当前帧上的投影点$ P_{m4}' $仍落在极线上,满足极线约束。由此可见,仅通过极线约束判断特征点的运动情况具有一定的局限性,因此为准确判断特征点的真实运动状态,本文定义一种加权静态约束:

$ \begin{align} s=\varphi |\cos ( \mathit{\boldsymbol{n}}, \overrightarrow {C_{2} p_{2}'})|+\omega |z(P, P')| \end{align} $ (10)

其中,$ s $表示加权静态约束得分,$ \cos (\mathit{\boldsymbol{n}}, \overrightarrow {C_{2} p_{2} '} $)表示矢量$ \overrightarrow {C_{2} p_{2} '} $与极平面法向量$ {\mathit{\boldsymbol{n}}} $夹角的余弦值,夹角越接近90$ ^{\circ} $,说明投影点$ p_{2} ' $相对于极线的偏离程度越低,则该点为动态特征点的可能性越小;$ z(P, P') $表示相机在参考帧和当前帧所观测的地图点的深度差值;$ \varphi $$ \omega $表示对应的权重,当该余弦值的绝对值大于预先设定的阈值$ \tau_{2} $时,令$ \varphi =1 $$ \omega =0 $,即表示不需要计算深度差便可认为该特征点为动态点,否则令$ \varphi =0 $$ \omega =1 $。最后设置阈值$ \tau_{3} $,若$ s\geqslant \tau_{3} $,则认为该特征点为动态点。

通过加权静态约束及先前运动等级来更新当前帧中特征点的最终运动等级,更新策略如表 2所示。其中$ L $表示某特征点经过运动等级传递后得到的运动等级(运动等级的定义详见表 1),$ L' $表示施加加权静态约束后特征点的最终移动等级,$ s $为式(10)的计算结果,$ \tau_{3} $为判定动态或静态点的阈值。当按运动等级更新策略更新后的$ L' $值大于10时,令$ L'=10 $。运动等级更新的原则是尽量保证动态点的检出率,因此当特征点被判定为动态时,运动等级的修改步长比静态情况下更大。

表 2 运动等级更新策略 Tab. 2 Motion level update strategy

图 5(a)~(f)为TUM数据集fr3/walking_xyz序列中的连续6帧图像,当前场景中右侧的人正在向其右前方移动,左侧的人正坐在转椅上旋转。每帧图像上绿色点表示可参与位姿初优化的特征点,即静态点和可移静态点集合,红色点表示外点,其不参与位姿的优化,构成动态点集合。对图 5(a)对应的帧进行了语义分割,再结合上一帧的移动等级传递,使得2个人身体上的特征点以及椅背部分被正确分类为动态点,图 5(b)~(e)都为运动等级传递以及加权静态约束得到的结果,没有进行语义分割处理,虽然2个人身上或人体边缘背景上仍有少数被错分的特征点,但数量较少,在位姿优化过程中可通过RANSAC(随机抽样一致性)算法剔除。图 5(f)中,虽然场景没有发生很大变化,但由于特征点的随机性,该帧图像上从人体上提取的特征点相较于图 5(e)变化较大,因此存在较多特征点无法找到匹配点,进而无法获得运动等级信息,满足跨帧检测的条件,因此在该帧图像进行了语义分割操作,使人体上的特征点又被归类为动态点。

图 5 特征点运动状态分类结果 Fig.5 Classification result of motion state of keypoints
4.3 位姿二次优化

采用上述加权静态约束方法确定场景中特征点的真实运动状态并更新了运动等级后,需要对位姿进行二次优化以提高位姿估计的精度。优化仍选用最小化重投影误差的方法,初始值设为位姿初步估计值,参与优化的特征点为静态点集以及可移静态点集中的所有特征点,即通过特征点的状态更新利用特征点数目增加后的结果对位姿初值进一步优化,得到更精确的位姿结果。

5 实验与分析(Experiment and analysis)

本文算法采用的运行测试平台为笔记本电脑,运行环境为64位Ubuntu 18.04系统,AMD 4800H CPU,内存大小为16 GB,显卡为GTX 1650。

采用公开数据集TUM RGB-D与原系统ORB-SLAM2以及一些用于处理动态场景的优秀的SLAM系统进行比较,并尽可能采用原文所提供的实验结果。本文实验结果为5次运行结果的平均值。评价指标采用SLAM算法的常用指标:绝对轨迹误差(ATE)以及相对位姿误差(RPE)。

5.1 TUM数据集及实验结果对比

TUM数据集[27]由慕尼黑工业大学的计算机视觉实验室发布,其包含3个序列,其中fr1、fr2为静态场景数据集,而fr3为动态场景数据集。每个序列中包括RGB图像、深度图像以及该序列对应的相机运动轨迹真值。图像由Kinect深度相机采集,分辨率为$ 640\times 480 $,并经过对齐等预处理。

fr3序列包含fr3/walking、fr3/sitting两个动态序列,其中fr3/walking序列的动态程度较高,场景中有2个人在桌前走动,而fr3/sitting序列的动态程度较低,2个人坐在桌前交谈,运动幅度较小。

5.1.1 ORB-SALM2

首先,为验证本文算法的有效性,利用高动态数据集对比原ORB-SLAM2算法和本文算法。

图 6为绝对轨迹误差对比图,(a)~(c)分别表示ORB-SLAM2(上)与本文算法(下)在fr3/walking_halfsphere、fr3/walking_xyz及fr3/walking_xyz_validation序列上的绝对轨迹误差对比图,图中黑色曲线表示相机轨迹真值,蓝色部分表示ORB-SLAM2算法或本文算法的轨迹估计值,红色线段表示估计值和真值之间的绝对误差,因此红色线段越短表示误差越小,系统精度越高。图 7为2种算法在上述3个序列上的3维轨迹误差热力图对比图,图中灰色虚线表示轨迹真值,带有颜色的实线表示系统估计轨迹,颜色越偏向深蓝,表示误差越小,颜色越偏向深红,表示误差越大。图 8为2种算法分别在上述3个序列上的位姿计算结果与真值轨迹之间的相对位姿误差(平移部分)对比图。第1列表示的是ORB-SLAM2算法分别在3个序列上的位姿计算结果与真值之间的相对位姿误差,第2列表示本文算法分别在3个序列上的位姿计算结果与真值之间的相对位姿误差。横轴表示整个数据序列的时间,单位为s;纵轴表示算法在某一时刻的位姿计算结果与位姿真值之间的相对位姿误差,单位为m。由图可知,本文算法相比ORB-SLAM2算法在动态场景下的位姿估计精度方面有较大的提升。

图 6 绝对轨迹误差对比图 Fig.6 ATE comparison
图 7 3维轨迹误差热力图对比 Fig.7 Comparison of 3D trajectory error heat map
图 8 相对位姿误差示意图 Fig.8 Comparison of relative pose error

表 3~5中,第1列表示数据序列的名称,*表示该序列为低动态的,在该序列中,仅有小幅度的身体扭动和手势变化,这种低动态场景中仅包含极少数的外点,因此在ORB-SLAM2算法中仅根据RANSAC算法便可得到处理,并同时保留了更多的静态点参与优化,但在本文算法中,由于特征传递过程可能会导致少量潜在移动对象在静止状态下被错分为移动状态,使得参与位姿优化的特征点数目减少,导致精度稍低于ORB-SLAM2系统。但本文算法相比原始ORB-SLAM2系统在高动态场景中系统位姿估计精度有较大提升,在高动态场景中,位姿的绝对轨迹误差的均方根值可提升95.81%~99.31%,相对位姿误差的平移部分的均方根值提升89.46%~96.23%,旋转部分的均方根值提升85.24%~95.39%。

表 3 本算法与ORB-SLAM2算法的绝对轨迹误差对比(单位:m) Tab. 3 Comparison of ATE between the ORB-SLAM2 algorithm and the proposed algorithm (unit: m)
表 4 本文算法与ORB-SLAM2算法的相对轨迹误差(平移部分)对比(单位:m) Tab. 4 Comparison of relative trajectory error (translation part) between the ORB-SLAM2 algorithm and the proposed algorithm (unit: m)
表 5 本算法与ORB-SLAM2算法的相对位姿误差(旋转部分)对比(单位:°) Tab. 5 Comparison of relative trajectory error (rotation part) between the ORB-SLAM2 algorithm and the proposed algorithm (unit: °)
5.1.2 其他SLAM算法

为进一步验证本文算法的先进性,将本算法与DynaSLAM、DS-SLAM、SaD-SLAM等目前较为优秀的处理动态场景的算法进行对比,对比结果如表 6所示,表中“–”表示该文献中没有此数据集对应的位姿精度分析结果。根据对比结果可以看出,本文算法在大部分数据集上表现良好。但由于ORB-SLAM2算法内包含静态场景的假设,只允许系统中有极少数的误匹配点对存在,因此其在静态数据集上表现较好。而DynaSLAM算法本身是将位于“人”这个类别上的关键点直接剔除,因此在部分数据集上表现较好,但此算法并没有考虑到人的真实运动状态,当人在图像中占大部分面积时,极可能会导致算法失效。

表 6 本文算法与其他处理动态场景的先进SLAM算法的绝对轨迹误差对比(单位:m) Tab. 6 Comparison of ATE between other advanced dynamic SLAM algorithms and the proposed algorithm (unit: m)
5.2 实时性评估

为验证本文算法的实时性,选用每帧的跟踪时间作为系统实时性评估的参数,跟踪时间是指在系统运行期间每帧图像在跟踪线程中的耗时。通过对多次实验的跟踪时间进行均值计算,得到本算法在处理TUM数据集时平均每帧耗时0.0529 s,即平均每秒可跟踪18.9帧,基本达到实时性。

本文系统各部分的耗时情况如表 7所示。其中语义分割部分耗时较为严重,但本文并非对每一帧图像都采用语义分割,而是由跨帧检测算法自适应地选择图像帧进行语义处理,因此较大程度上降低了其对系统实时性的影响。

表 7 本算法跟踪阶段每部分耗时情况(单位:ms) Tab. 7 Time consumption of each part in tracking (unit: ms)

虽然系统可基本实现实时性,但本文采用的跨帧检测方法会导致进行语义分割的图像帧处理速度较慢,使整个序列处理的速度不均匀。本文通过选取轻量级语义分割网络来尽量降低对系统实时性的影响,因此系统在等待语义分割结果时不会出现等待时间较长或者长时间运行速度较慢的现象。

由于场景、相机运动方向和角度、运动对象的不同,特征点提取的随机性等原因,经在TUM数据集上实验,发现每次间隔的帧数并不固定,跨帧情况见表 8,表中数字表示进行语义分割操作的平均间隔帧数。第1列表示数据序列,第2~4列为本文算法在对应序列上的3次运行结果,第5列为3次运行结果的均值。由表中数据可知,低动态场景的语义分割间隔要大于高动态场景,这是因为低动态场景中运动对象的移动幅度较小,通过移动等级传递即可确定绝大部分特征点的状态。在同一动态程度的不同序列中,当相机运动幅度较大时,便需要更多的语义分割信息以确定特征点的状态。

表 8 跨帧检测平均间隔帧数 Tab. 8 Average number of interval frames in inter-frame detection
6 结论(Conclusion)

为提升传统视觉SLAM系统的鲁棒性,本文基于ORB-SLAM2算法提出一种融合语义信息的RGB-D SLAM算法,可在动态场景下保持较好的定位精度和实时性。本算法借助语义先验信息与等级传递过程确定特征点的移动等级,利用静态点完成位姿初估计,然后通过加权静态约束确定特征点的真实运动状态,最后进行二次优化,保证SLAM系统的定位精度。最后在TUM公开数据集上对本文算法以及ORB-SLAM2、DynaSLAM、DS-SLAM等算法进行比较,结果表明在动态场景下,本文算法的位姿估计精度相对于ORB-SLAM2算法有较大的提升,并且相对于其他处理动态场景的先进SLAM算法,也有较高的定位精度和实时性,算法的鲁棒性也得到提升,但仍存在一定问题,例如帧处理速度不一致。

在下一阶段的工作中,将进一步发挥语义信息在SLAM系统中的作用,尝试构建语义八叉树地图来服务于上层应用,如机器人的路径规划、避障等,同时加入IMU(惯性测量单元)等其他传感器,进一步提升SLAM系统的精度和鲁棒性。

参考文献(References)
[1]
Mur-Artal R, Montiel J M M, Tardos J D. ORB-SLAM:A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163. DOI:10.1109/TRO.2015.2463671
[2]
Mur-Artal R, Tardos J D. ORB-SLAM2:An open-source SLAM system for monocular, stereo, and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262. DOI:10.1109/TRO.2017.2705103
[3]
Elvira R, Tardós J D, Montiel J M M. ORBSLAM-Atlas:A robust and accurate multi-map system[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2019:6253-6259.
[4]
Qin T, Li P L, Shen S J. VINS-Mono:A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020. DOI:10.1109/TRO.2018.2853729
[5]
Forster C, Pizzoli M, Scaramuzza D. SVO:Fast semi-direct monocular visual odometry[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2014:15-22.
[6]
Engel J, Koltun V, Cremers D. Direct sparse odometry[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2018, 40(3): 611-625. DOI:10.1109/TPAMI.2017.2658577
[7]
Wang Y B, Huang S D. Towards dense moving object segmentation based robust dense RGB-D SLAM in dynamic scenarios[C]//13th International Conference on Control, Automation, Robotics & Vision. Piscataway, USA:IEEE, 2014:1841-1846.
[8]
Sun Y X, Liu M, Meng M Q H. Improving RGB-D SLAM in dynamic environments:A motion removal approach[J]. Robotics and Autonomous Systems, 2017, 89: 110-122. DOI:10.1016/j.robot.2016.11.012
[9]
林志林, 张国良, 姚二亮, 等. 动态场景下基于运动物体检测的立体视觉里程计[J]. 光学学报, 2017, 37(11): 187-195.
Lin Z L, Zhang G L, Yao E L, et al. Stereo visual odometry based on motion object detection in the dynamic scene[J]. Acta Optica Sinica, 2017, 37(11): 187-195.
[10]
Chen L, Fan L, Xie G D, et al. Moving-object detection from consecutive stereo pairs using slanted plane smoothing[J]. IEEE Transactions on Intelligent Transportation Systems, 2017, 18(11): 3093-3102. DOI:10.1109/TITS.2017.2680538
[11]
Wang R Z, Wan W H, Wang Y K, et al. A new RGB-D SLAM method with moving object detection for dynamic indoor scenes[J]. Remote Sensing, 2019, 11(10). DOI:10.3390/rs11101143
[12]
张慧娟, 方灶军, 杨桂林. 动态环境下基于线特征的RGBD视觉里程计[J]. 机器人, 2019, 41(1): 75-82.
Zhang H J, Fang Z J, Yang G L. RGB-D visual odometry in dynamic environments using line features[J]. Robot, 2019, 41(1): 75-82.
[13]
魏彤, 李绪. 动态环境下基于动态区域剔除的双目视觉SLAM算法[J]. 机器人, 2020, 42(3): 82-91.
Wei T, Li X. Binocular vision SLAM algorithm based on dynamic region elimination in dynamic environment[J]. Robot, 2020, 42(3): 82-91.
[14]
Bojko A, Dupont R, Tamaazousti M, et al. Learning to segment dynamic objects using SLAM outliers[C]//25th International Conference on Pattern Recognition. Piscataway, USA:IEEE, 2021:9780-9787.
[15]
Bescos B, Cadena C, Neira J. Empty cities:A dynamic-objectinvariant space for visual SLAM[J]. IEEE Transactions on Robotics, 2021, 37(2): 433-451. DOI:10.1109/TRO.2020.3031267
[16]
Bao R Q, Komatsu R, Miyagusuku R, et al. Stereo camera visual SLAM with hierarchical masking and motion-state classification at outdoor construction sites containing large dynamic objects[J]. Advanced Robotics, 2021, 35(4): 228-241.
[17]
Bescos B, Facil J M, Civera J, et al. DynaSLAM:Tracking, mapping, and inpainting in dynamic scenes[J]. IEEE Robotics and Automation Letters, 2018, 3(4): 4076-4083. DOI:10.1109/LRA.2018.2860039
[18]
He K M, Zhang X Y, Ren S Q, et al. Deep residual learning for image recognition[C]//IEEE Conference on Computer Vision and Pattern Recognition. Piscataway, USA:IEEE, 2016:770-778.
[19]
Yu C, Liu Z X, Liu X J, et al. DS-SLAM:A semantic visual SLAM towards dynamic environments[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2018:1168-1174.
[20]
Badrinarayanan V, Kendall A, Cipolla R. SegNet:A deep convolutional encoder-decoder architecture for image segmentation[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2017, 39(12): 2481-2495. DOI:10.1109/TPAMI.2016.2644615
[21]
Xiao L H, Wang J G, Qiu X S, et al. Dynamic-SLAM:Semantic monocular visual localization and mapping based on deep learning in dynamic environment[J]. Robotics and Autonomous Systems, 2019, 117: 1-16. DOI:10.1016/j.robot.2019.03.012
[22]
Liu W, Anguelov D, Erhan D, et al. SSD:Single shot multibox detector[C]//European Conference on Computer Vision. Cham, Switzerland:Springer, 2016:21-37.
[23]
Zhong F W, Wang S, Zhang Z Q, et al. Detect-SLAM:Making object detection and SLAM mutually beneficial[C]//IEEE Winter Conference on Applications of Computer Vision. Piscataway, USA:IEEE, 2018:1001-1010.
[24]
Yuan X, Chen S. SaD-SLAM:A visual SLAM based on semantic and depth information[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2020:4930-4935.
[25]
Zhang J, Henein M, Mahony R, et al. VDO-SLAM:A visual dynamic object-aware SLAM system[DB/OL]. (2020-05-22)[2021-08-01]. https://arxiv.org/abs/2005.11052.
[26]
Nekrasov V, Shen C, Reid I. Light-weight RefineNet for realtime semantic segmentation[DB/OL]. (2018-10-08)[2021-08-01]. https://arxiv.org/abs/1810.03272.
[27]
Sturm J, Engelhard N, Endres F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2012:573-580.
[28]
艾青林, 刘刚江, 徐巧宁. 动态环境下基于改进几何与运动约束的机器人RGB-D SLAM算法[J]. 机器人, 2021, 43(2): 167-176.
Ai Q L, Liu G J, Xu Q N. An RGB-D SLAM algorithm for robot based on the improved geometric and motion constraints in dynamic environment[J]. Robot, 2021, 43(2): 167-176.