基于目标检测的视觉SLAM改进方法

王晓超 王春林* 袁成祥

1(杭州电子科技大学自动化(人工智能)学院 浙江 杭州 310018) 2(浙江工商大学计算机与信息工程学院 浙江 杭州 310018)

0 引 言

同时定位与地图构建(SLAM)技术是机器人通过传感器对未知环境构建地图的同时实现自定位的过程。大多数视觉SLAM系统是将环境假设为静态场景,而在实际场景中往往会有像人类这样的活动对象,这些动态特征被提取后会严重影响相机的位姿估计,造成轨迹漂移严重,甚至导致系统崩溃。另外,这些SLAM系统主要构建的是几何信息地图,缺少对物体具体语义层次的理解,不能提供带语义信息的地图,制约了移动机器人交互能力和导航能力。

近年来,深度学习在语义信息获取方面的进展及应用为解决这些问题提供了一个可行的方向。深度学习在图像分类[1]、识别、图像分割[2]等几大领域的表现都远远高于传统人工设计的算法。深度学习与SLAM结合可以使机器人从几何和语义两个层次对场景进行抽象理解,获得高层次的感知,提高机器人对周围环境的理解。Yu等[3]提出了DS-SLAM系统,该系统是在ORB-SLAM2[4]的跟踪线程中加入语义分割SegNet网络[5],去除每一帧图像中的动态特征,从而降低位姿估计的误差。Bescos等[6]提出DynaSLAM系统,采用实例分割网络MASK-RCNN[7]对当前帧中的动态物体进行分割,并根据前20个关键帧对去除的地方进行背景修复。由于MASK-RCNN复杂的网络结构,DynaSLAM系统实时性较差。Sünderhauf等[8]同样以ORB-SLAM2系统为基础,在系统中加入SSD目标检测算法[9]对构建的三维点云地图物体识别和分割,最终构建了带有语义信息的三维点云语义地图。Mccormac等[10]提出SemanticFusion系统,该系统使用卷积神经网络(CNN)进行语义分割,再与ElasticFusion系统结合,构建了稠密的三维语义地图。

本文针对机器人视觉SLAM系统中存在的对动态物体难以处理和无法构建带有语义信息地图的问题,以ORB-SLAM2为基础,采用基于深度学习的目标检测算法YOLOv4对系统进行改进,减少系统位姿估计误差,提高系统的鲁棒性,并添加一个点云语义地图构建的线程,构建稠密的三维语义地图,以提高视觉SLAM系统的感知能力。

1 系统构成

本文提出的基于目标检测的视觉SLAM系统改进方法的框架如图1所示。本文采用YOLOv4对传统ORB-SLAM2系统进行部分改进。在跟踪线程中,采用ORB(Oriented FAST and Rotated BRIEF)算法提取出图像帧中的特征点,然后判断这些特征点是否在由YOLOv4定位出的动态物体上,如果是,则去除这些特征点,避免这些动态特征点对系统的干扰,以提高系统位姿估计的准确率。在建图线程中,构建3D点云地图,再对点云地图采用基于图结构的超体素聚类算法生成初步语义地图,与YOLOv4提供的语义标签构建最后的语义地图。

图1 基于YOLOv4改进的SLAM系统框架

2 基于YOLOv4的视觉SLAM改进方法

2.1 YOLOv4

YOLOv4[11]在以残差块结构的Darknet-53为骨干网络的YOLOv3基础上作出了全面的提升,在骨干网络Darknet-53上加入CSPNet(Cross Stage Partial Network)网络结构,减少计算量的同时提高了推理速度和准确性。另外,骨干网络还加入了SPP(Spatial Pyramid Pooling)模块,可以提升模型的感受野,分离更重要的上下文信息。与此同时,还采用PANet(Path Aggregation Network)改进骨干网络结构,加强了特征金字塔的结构,缩短了高低层特征融合的路径。

本文利用MS COCO数据集来训练YOLOv4的网络模型,数据集里包含人、茶杯、键盘、显示器、鼠标、玩具熊等80个类别。由于网络结构的优势,YOLOv4可以在MS COCO数据集上AP(Average Precision)为43.5%的同时可以达到65帧/s,是目前最新的快速而高效的目标检测器。

2.2 跟踪线程

2.2.1特征提取与特征匹配

ORB[12]是一种快速特征点提取和特征描述的算法,由关键点和描述子两部分组成,ORB特征提取主要分为以下两个步骤:(1) 方向FAST特征点检测;(2) BRIEF特征描述。

FAST是一种角点,主要是比较局部像素灰度与周围像素点是否变化比较明显,如果该像素点与周围的像素点差别较大,那么它可能是角点。为解决传统的FAST角点不具有尺度和方向性的问题,ORB在原来的基础上添加了尺度和旋转的描述。为解决尺度不变性,采用构建图像金字塔方式,在每一层金字塔上检测角点。而特征的旋转则是用灰度质心法来实现。

所谓灰度质心法指的是将图像的灰度值作为权重的中心。例如在一小的图像块A中,定义图像块的矩为:

(1)

式中:I(x,y)为点(x,y)处的灰度值。得到图像的质心为:

(2)

那么特征点与质心的夹角定义为FAST特征点的方向:

θ=arctan(m01,m10)

(3)

通过以上方法,FAST角点便有了尺度和旋转的描述,提升了特征点的鲁棒性,为后续特征匹配提供了方便。

ORB的特征描述则是选择BRIEF算法,该算法采用随机选取点的方式,选择特征点周围S×S大小的像素块,随机选取n对像素点,定义:

(4)

式中:p(x)是点x处的灰度值;p(y)是点y处的灰度值。则特征点p的描述子定义为:

(5)

特征点提取后,接下来就是特征匹配。特征匹配是SLAM系统中的重要部分,为保证位姿估计的精度,特征点间必须正确地、有效地相互匹配。以上的特征描述子均是二进制描述子,为后续计算特征相似度减少了计算压力。这里比较特征相似度采用计算汉明距离的方式,汉明距离是指两字符串之间对应位置字符不同的总数量。当两特征描述子的汉明距离低于设定的阈值时,便认为这两个特征是同一个点。

2.2.2YOLOv4去除动态特征

将当前图像帧输入YOLOv4网络模型,经过目标检测算法定位出动态物体的位置,本文将人类视为动态物体类别,将人类用矩形框框出来,并将框内的特征点视为动态特征点,全部去除,如图2所示。当动态特征剔除后,进行特征匹配时,避免了很多在人身上的特征点的匹配,以提高后续位姿估计的精度,如图3所示。

(a) 传统特征提取 (b) YOLOv4改进后特征提取图2 特征提取结果

(a) 传统ORB-SLAM2特征匹配结果

(b) YOLOv4改进后特征匹配结果图3 特征匹配结果

2.2.3位姿估计

将特征匹配好的2D点从其对应深度图像中获取该点的深度,转换成3D点。采用ICP(Iterative Closest Point)[13]算法对配对好的3D点进行位姿估计。假设有两组匹配好的3D点:

(6)

式中:P为当前帧特征点的集合;P′为前一帧特征点的集合。寻找一个欧氏变换R、t使得:

(7)

定义一个误差项ei为:

(8)

然后,构建最小二乘问题,求使误差平方和达到极小值时的R和t:

(9)

(10)

(11)

将式(10)代入式(9)中,可得:

(12)

式(12)的第一项只与旋转矩阵R相关,第二项既有R也有t,将式(11)代入第一项可得:

(13)

旋转矩阵R*具体计算方法是首先定义矩阵H:

(14)

H是一个3×3的矩阵,对H进行SVD分解:

H=UΣVT

(15)

式中:Σ为奇异值组成的对角矩阵,对角线元素从大到小排列;U和V为对角矩阵。当H满秩时,R*为:

R*=VUT

(16)

利用式(16)求得R后再求解平移量t:

t=p-R*p′

(17)

2.3 建图线程

本文在ORB-SLAM2建图线程后添加一个构建带有语义信息的点云地图线程。以ORB-SLAM2的点云地图为输入,采用基于超体素聚类(Supervoxel Clustering)的算法对点云地图进行初步分割,再与YOLOv4获得的语义标签融合构建最终的语义地图。

2.3.1基于超体素聚类的物体分割算法

超体(Supervoxel)[14]是一种集合,集合的元素是“体”,其本质是一个个小方块。超体素聚类并不是分割出某个物体,而是对点云过分割(over segmentation),将场景点云化成许多的小方块,根据颜色、法向量方向等进行局部分割。

超体素聚类算法是在空间中选择一定数量的种子点作为超体的初始化。首先将空间分割成半径为Rseed分辨率的体素网格,Rseed的大小要比各像素间的范围Rvoxel大得多,然后将种子作为每个体素网格的中心。如图4所示,其中Rsearch表示放置种子空间距离。

图4 影响超体素聚类的不同半径

从种子点开始通过计算特征空间如空间范围、法向量、颜色等进行扩展,通过式(18)计算两个素体之间的距离:

(18)

式中:Dc为RGB空间中的归一化欧氏距离;Ds为归一化的空间的欧氏距离;Dn为归一化的法向量角度距离;wc、ws、wn分别为颜色、空间、法向量的权重。

超体是使用迭代的方式增长的。首先从种子最近邻点开始,如果计算出体素是距离当前种子最近的点,则将该体素加入当前超体中,接着使用近邻图继续将该体素所有近邻点加入到搜索队列中,然后再处理下个种子,一直迭代到超体边界。这种方式能保证在处理过程中每个像素里中心点的层次水平是相同的。在超体搜索结束后,接着更新每个超体的中心为其组成成分的重心点,这样经过几次迭代,直达超体中心稳定停止。超体聚类效果如图5所示。

图5 超体素聚类后的点云分割地图

2.3.2基于YOLOv4获取语义标签

以上将点云进行超体素聚类属于几何结构的分割,虽有不错的分割效果,但是不能得到物体的语义信息。为获取图像中的语义信息,本文采用YOLOv4进行目标检测获取图像帧的语义标签,目标检测效果如图6(a)所示。

(a) YOLOv4目标检测 (b) 带有语义标签的点云图6 获取语义标签

彩色图片通过YOLOv4将检测到的物体用不同颜色框出,并获取框的左顶点位置(xi,yi)和框宽wi、高hi,则类别ci在彩色图片中的描述为:

Di={(xi,yi),wi,hi,ci}

(19)

在深度图片中定位到对应位置,并将框内的所有深度归为此类并用某颜色标出,则加入深度信息di和标注颜色colori后,类别ci的描述为:

(20)

3 实验与分析

本节中,我们使用开源数据集对本文构建的系统进行评估,将本文系统、传统ORB-SLAM2和基于SegNet改进的DS-SLAM三系统进行比较,分析系统在动态环境中的性能是否提高,并构建带有语义信息的稠密地图。所有实验均在处理器为Intel i5 8400、显卡GTX1060 6 GB显存和内存16 GB的计算机上进行。

3.1 数据集

为评价系统的综合能力,实验采用的数据集是德国慕尼黑工业大学开源的TUM RGB-D数据集[15],该数据集由39个序列组成,这些序列是由Microsoft Kinect传感器以30 Hz速率记录在不同的室内场景中,包含RGB图片、深度图片和地面实况数据。本文实验主要采用TUM RGB-D数据集中的5个序列。其中walking系列的4个序列是两人在桌前桌后行走或者坐在桌前的场景,都属于高动态序列,相机在4种状态下拍摄:(1) 沿x-y-z轴移动;(2) 保持静止;(3) 在翻滚、仰俯和偏航轴上旋转;(4) 直径为一米的半球面移动。freiburg2_xyz序列是相机沿着(x,y,z)方向缓慢移动,确保了数据足够清晰。

此外,TUM RGB-D数据集还提供了用于系统评估的两种方法:(1) 绝对轨迹误差(Absolute Trajectory Error,ATE),代表运动轨迹的全局一致性;(2) 相对位姿误差(Relative Pose Error,RPE),测量平移和旋转漂移。

3.2 实验结果

3.2.1定量结果

为对比本文系统、ORB-SLAM2和DS-SLAM在TUM RGB-D数据集中的4个高动态序列的实验结果,评价指标为均方根误差(RMSE)、平均误差(MEAN)、标准偏差(S.D.)。均方根误差计算估计值与真实值之间的偏差;平均误差描述所有估计误差的平均水平;标准偏差反映系统轨迹估计的离散程度。三种评价指标可以很好地体现SLAM系统的稳定性和可靠性。

从表1-表3的比较结果可以看出,本文系统相比于传统的ORB-SLAM2系统和DS-SLAM系统在三种评价误差中均有不同程度的减少。图7显示了在freiburg3_walking_halfsphere序列中传统ORB-SLAM2、DS-SLAM和本文的SLAM系统的ATE和RPE图。可明显看出本文系统的绝对轨迹误差和相对位姿误差均降低了很多。

表1 绝对路径误差(ATE)对比 单位:m

表2 相对位移误差(RPE)对比 单位:m

表3 相对旋转误差(RPE)对比 单位:°

(a) ORB-SLAM2绝对轨迹误差

(b) DS-SLAM绝对轨迹误差

(c) 本文系统绝对轨迹误差

(d) 相对位姿误差对比图7 绝对轨迹误差和相对位姿误差对比

3.2.2语义地图构建

本节实验是构建freiburg2_xyz序列数据集的稠密语义点云地图,其效果如图8所示。图8(a)是传统SLAM系统所构建的点云地图,虽然可以看出物体的轮廓,但是各物体间分层不够清晰,并且地图中只是三维点,并没有物体的语义信息,这样的地图并不能定位到具体的某物体中。而图8(b)是将带语义标签的点云地图与超体素聚类的点云地图相融合后的结果,可以看出实验桌上物体分层清晰,由YOLOv4算法识别出的物体用不同颜色标出。本文构建的带有语义信息的点云地图可以容易地定位到某物体的位置,提高SLAM系统的交互能力和感知能力。

(a) 传统SLAM系统点云

(b) 本文系统的语义点云地图图8 传统SLAM点云地图和本文语义点云地图比较

3.2.3实时性分析

如表4所示,本文系统对每帧图像平均计算时间为68 ms,虽比DS-SLAM计算较慢些,但是系统的位姿估计精度比DS-SLAM系统高20%～40%。另外,本文系统每秒的计算帧数为15,基本满足SLAM系统的实时性要求。

表4 系统实时性比较

4 结 语

为减少动态环境对SLAM系统的位姿和轨迹估计的影响和无法建立带有语义信息的地图,构建一种基于目标检测的视觉SLAM系统。在ORB-SLAM2的跟踪线程中采用YOLOv4定位出动态物体,从而去除动态特征。与传统的ORB-SLAM2相比,系统的性能有明显的提升。在建图线程中加入构建稠密点云语义地图线程,用超体素聚类对点云地图初步分割,再与YOLOv4获取的标签融合得到最终的语义地图,地图构建效果良好,且本文SLAM系统计算速度为15帧/s,能很好地满足实时性的要求。在接下来的工作中,重点研究将深度学习运用到视觉SLAM的闭环检测线程中,进一步提高系统的准确性。