语义地图中语义物体免疫遮挡定位算法*

左建朋,蒋 林,1b,李云飞,邵慧超,刘 奇

(1.武汉科技大学 a.冶金装备及其控制教育部重点实验室;b.机器人与智能系统研究院,武汉 430081;2.立得空间信息技术股份有限公司,武汉 430073)

0 引言

自主定位是智能移动机器人能够在实际场景中执行任务的一个基本前提。对于室内的二维环境主要使用的是自适应蒙特卡洛定位(adaptive monte carlo localization,AMCL),是目前常用的激光定位算法[1],但在相似度较高或者环境前后变化较大的场景中,机器人会出现定位变慢、甚至出现定位失效的问题,语义SLAM能很好的解决这个问题。蒋林等[2]通过深度学习方法提取物体语义进行语义地图构建,结合AMCL算法进行粒子滤波重定位研究,通过激光和视觉特征的融合克服了原粒子滤波方法仅利用环境结构信息进行匹配的不足,解决了一些相似场景AMCL定位慢和定位失效的问题,有效的提高了原AMCL定位算法的鲁棒性。

语义地图是语义SLAM中不可缺少的一部分,李秀智等[3]使用卷积神经网络进行语义识别及标记,构建了较为初始的2D语义地图。聂文康等[4]融合激光SLAM算法和深度学习的方法,通过坐标转换映射将语义信息与环境栅格地图进行融合,并结合贝叶斯估计的方法对栅格点物体语义存在与否进行增量式估计,得到包含语义信息的二维栅格地图。XIANG等[5]提出了一种基于DA-RNN网络与Kinect Fusion映射技术[6]相结合来构建三维语义地图。MCCORMAC等[7]通过组合CNNs和SOTA SLAM系统ElasticFusion构建了一个有效的语义3D地图。赵哲[8]通过将RGB-D场景重建技术和语义分割技术相结合,来构建高精度全局场景的语义地图。高质量的语义地图能使机器人的语义定位更精准、更快。

在语义定位的研究中,GAWEL等[9]将关键帧组合成一组三维图来求解相机在全局地图中的位置。TSINTOTAS等[10]通过对输入的图片流进行动态分割并聚类来进行回环检测。ZHANG等[11]利用YOLOv3[12]目标检测算法获取环境中关键目标的语义信息及其相对位置关系来进行回环判断。蒋林等[13]提出了基于墙角信息的语义预定位结合AMCL算法,改进了粒子滤波的重定位算法,克服了原本粒子滤波方法仅利用环境结构信息进行匹配的不足,有效的提高了原定位算法的鲁棒性。当在语义定位的过程中存在障碍物遮挡语义物体时,针对语义定位失败,无法获得正确的机器人位姿,如何免疫障碍物的遮挡是急需解决的问题。黄山等[14]将高动态的障碍物滤除并把有价值的半静态障碍物并入地图中,实现机器人在动态环境中的长时间稳定定位。DS-SLAM[15]采用语义分割结合光流法的方法来降低动态物体对视觉SLAM的影响。WANG等[16]通过彩色图像中的光流来检测动态物体。张威[17]通过融入动态物体检测模块来提高ORB-SLAM2系统在动态场景下的适应能力,减少动态特征点对视觉里程计的干扰。

上述这些方法大多需要较高的硬件配置来运行较复杂的计算,对基于二维语义地图的室内重定位研究并不能取得较好的效果。本文提出了一种在语义定位中免疫障碍物遮挡语义物体的方法来提高二维语义地图中语义物体被遮挡的免疫性能,首先通过语义链来确定被遮挡语义物体类别和机器人所处的区域,然后使用未被遮挡的语义物体进行精确的语义定位,最后再同步结合AMCL算法与环境地图匹配进行精确定位。并借助于搭建的机器人平台,对所提方法进行对比实验验证。

1 免疫障碍物遮挡语义物体的定位算法原理

智能机器人在工作的过程中,所处的环境并非一成不变的,但先验语义地图是固定的。当语义物体被遮挡时,语义定位将无法实现或存在多个语义定位位姿,导致机器人在相似环境下粒子无法快速收敛,定位效率大大下降。本文提出一种免疫障碍物遮挡语义物体的定位算法,算法整体流程如图1所示。

图1 免疫遮挡定位算法流程图

在重定位的过程时,首先通过Kinectv2获取彩色图和深度图,利用SSD目标检测进行语义识别,当检测到障碍物时就与建图时保存的关键帧进行相似度匹配,确定相似关键帧,把当前帧包含的语义信息与关键帧语义链进行加权做差来确定被遮挡物体的类别,将机器人所识别到的语义信息类别与语义地图的语义链进行比较来确定机器人所处的区域,通过未被遮挡的高置信度语义物体的深度信息来进行语义定位,最后结合激光定位的方法实现更精确、更快速的定位效果。

1.1 加载地图

加载使用Gmapping算法构建的二维栅格地图和基于目标检测算法构建的二维语义地图。如图2所示,图2a为栅格地图,图2b为语义地图。

(a) 栅格地图 (b) 语义地图图2 二维语义地图

为了方便语义信息的匹配,对语义地图进行了预处理。预处理方法:由于每个区域一定包含墙角语义信息,把墙角语义信息作为基层语义信息,提取墙角周围的语义信息形成语义链,提取范围为r(m),计算公式为式(1),其中θs为相机的水平视场角,d为语义信息提取的最大视距,本文使用Kinectv2相机的最佳观测距离。

(1)

如图2b所示,以r为半径搜索各墙角的语义信息,为不嫌赘余只对图1b中包含较多语义信息的墙角进行搜索。xi和yi表示语义物体对应的像素坐标(i=0,1,2,3,4分别对应墙角、椅子、门、垃圾桶、柜子),各类物体对应的数字表示半径范围内各个物体的数量。语义链如表1所示。

表1 语义链

1.2 关键帧的选择

在构建语义地图时,对满足关键帧选择规则的帧作为关键帧储存下来[18]。本文关键帧的选择规则:

(1)当前帧和上一关键帧之间必须有一定的序列间隔;

(2)当前帧与之前关键帧相互的共视区域低于一定范围;

(3)关键帧有足够的语义信息和特征,保证特征的丰富性。

如图3所示关键帧,包含了墙角、门和柜子等语义信息,且和上一帧关键共视区域较低,有一定的序列间隔,故选为关键帧保存。

图3 关键帧

1.3 被遮挡物体的类别判断

当机器人识别到障碍物时,先对当前帧进行基于词袋模型的相似度匹配,选出相似度评分最高的关键帧作为相似帧,然后将当前帧与相似帧的语义链对比来确定被遮挡物体的类别。

1.3.1 相似度匹配

由于相似度匹配直接用两张图像特征相加减误差很大。本文选择基于词袋模型的方法,用图像中的特征来描述一幅图像。主要步骤为:确定特征对应BoW中的单词,确定当前帧中单词的使用情况,将当前帧转换成了一个向量描述;比较不同的向量来确定不同帧之间的相似程度。对于特征的提取本文采用ORB特征提取;本文创建了一个包含多种场景信息的字典,规模为K=10,d=5。从包含障碍物的关键帧中用ORB提取特征,在字典树中逐层查找,找到特征对应的单词wx,以向量的形式描述,例如一帧图像有“人”、“车”、“猫”等单词,可记为:

Fi=1·w1+1·w2+0·w3

(2)

式中:Fi为第i时刻的一帧图像,w1、w2、w3分别对应单词“人”、“车”、“猫”。

当某个单词在一副图像中出现的频率较高或者频率较低,它所占的权重也不同,因此选择TF-IDF的加权方式(TF部分的思想是,某单词在关键帧中经常出现,它的区分度就高;IDF的思想是:某单词在字典中出现的频率越低,分类图像时区分度越高)。在建立字典时计算IDF:统计某个子叶节点wi中的特征数量相对于所有特征数量的比例作为IDF部分。表达式为:

(3)

式中:n为所有特征的数量,ni为wi的特征数量。

TF部分是指某个特征在某帧图像中出现的频率,表达式为:

(4)

式中:mi为图像Fi中单词w2出现的次数,m为一共出现的次数。

单词wi的权重为:

ηi=TFi×IDFi

(5)

而对于某一关键帧Fi,它的特征点可对应到许多单词wi,构成的BoW如下:

Fi={(w1,η1),(w2,η2),…,(wn,ηn)}⟹vi

(6)

通过词袋用向量vi表示关键帧Fi,相似度计算表达式为:

(7)

1.3.2 语义链匹配

通过目标检测提取关键帧中的语义信息,生成关键帧语义链如表2所示,如图4b所示被遮挡的部分为墙角,基于词袋模型匹配到的相似帧为关键帧1,被遮挡帧包含了柜子和门,关键帧1也包含了门和柜子,表示当前基于词袋模型相似度匹配的相似帧准确。

表2 关键帧语义链

在确定相似帧匹配正确后,将相似帧和当前帧包含的语义链信息转换为向量的形式,给每个物体赋不同的权重(corner=1、door=10、trash=100、chair=1000、cabinet=10 000),进行语义链的对比,确定被遮挡的语义物体类别,式(8)和(9)所示。

A=1·door+0·chair+1·corner+0·trash+1·cabinet

(8)

B=1·door+0·chair+0·corner+0·trash+1·cabinet

(9)

T=A-B

(10)

式中:A为相似帧,B为当前帧。按照式(10)T的值来确定被遮挡的物体类别,当T=1时,被遮挡的物体类别为墙角;当T=10时,被遮挡的物体类别为门;当T=100时,被遮挡的物体类别为垃圾桶;当T=1000时,被遮挡的物体类别为椅子;当T=10 000时,被遮挡的物体类别为柜子。

1.4 语义定位

确认被遮挡语义物体的类别后,给该语义物体的语义信息赋低置信度;通过目标检测算法直接识别到的语义信息赋高置信度,使用高置信度的语义信息进行语义定位。如图4b所示当墙角被遮挡时,使用门和柜子进行语义定位,求解两圆的交点即为机器人的位置如图4a所示。其中C1为查表1获得门的中心坐标,R1为通过深度相机获取的机器人到门的距离,C2为柜子的中心坐标,R2为深度相机获取的机器人到柜子的距离值。其半径的几何关系式如式(11)和式(12)所示。

(11)

(12)

(13)

(14)

pose=C1+an‖±hn⊥

(15)

(16)

式中:定义一个从C1-C2单位方向向量n‖,n⊥为n‖的单位法向量。

求出机器人的可能位置pose1和pose2后,通过确定该点是否在栅格地图中的空闲区域来判断该点是否为机器人的实际位姿(xt,yt),该点在空闲位置,则该点为机器人的实际位姿;反之则排除。对于机器人的偏航角θt,则通过机器人与语义物体的夹角θ1和语义物体与地图正x方向的夹角θ2来求解,如图5a所示,如式(17)和式(18)所示。

(a) 栅格地图坐标系下航向角(b) 机器人坐标系下θ表示图5 航向角求解

θt=θ2-θ1

(17)

θ1=θ-90

(18)

式中:θ为图5b映射的机器人坐标系下语义物体中心与正x轴的夹角。

到此语义定位完成,将语义定位的机器人位姿(xt,yt,θt)结合AMCL激光定位算法对粒子集更新进行精定位。

2 实验结果及分析

本文所用的移动机器人实验平台为实验室自主学习和开发的一款移动机器人,在Ubuntu16.04系统下配有ROS机器人系统,安装Caffe深度学习框架用于深度学习网络训练测试;选用性能稳定的SICK lms111单线激光雷达传感器和kinectv2深度相机。借助开发的移动机器人平台,对本文提出的基于语义地图的免疫障碍物遮挡语义物体的重定位算法进行实验验证。图6a为实验环境;图6b为构建的栅格地图,其中2和3为相似环境。

(a) 实验环境图 (b) 实验环境栅格地图图6 实验环境

2.1 语义定位对比实验

当存在动态障碍物遮挡语义物体时,运行文献[13]的语义定位算法时,机器人视角图如图7a所示,用反光柱模拟障碍物遮挡垃圾桶。在图6b所示3区域原地旋转进行定位,通过SSD目标检测只能识别到墙角和柜子,查找对应的语义链表1,发现4个区域均包含了墙角和柜子,无法确定机器人处于哪一个区域环境下,故只能通过机器人与墙角的相对距离和与柜子的相对距离来确定机器人的位姿,通过求解的位姿是否在地图空闲区域内来确定机器人的实际位姿,结果如图7b所示;机器人的位姿可能在1、3、4三个区域,无法通过该语义定位算法来获取机器人所在的区域和真实的位姿。

(a) 机器人视角图 (b) 语义定位位姿图7 遮挡垃圾桶文献[13]语义定位结果

在该场景下,运行本文算法,首先通过SSD目标检测识别到障碍物如图8a所示,其次通过与之前保存的关键帧进行基于词袋模型的相似度匹配,当前帧与关键帧3相似度匹配得分最高,初步判断相似帧为关键帧3如图8b所示,将当前帧和关键帧包含的语义链信息进行检验,当前帧包含了柜子和墙角,相似帧也包含了柜子和墙角,故确定相似匹配正确;再次将关键帧和当前帧的语义链进行赋值加权,依据式(8)～式(10)计算T=100,因此确定被障碍物遮挡的语义物体为垃圾桶,如图8c所示;然后通过查找语义链表1来查找包含墙角、柜子和垃圾桶语义信息的区域,确定机器人处在3区域;最后在进行精确的位姿确定,由于垃圾桶被遮挡了,其深度信息并不准确,把垃圾桶的定位置信度置为0,通过墙角和柜子进行语义定位如图8d所示,语义定位结果如图8e所示。

(a) 识别到障碍物 (b) 相似帧(c) 当前帧语义信息

(d) 位姿求解 (e) 语义定位位姿图8 遮挡垃圾桶本文语义定位结果

在2区域对椅子进行遮挡,由于椅子的体积较大反光柱无法完全遮挡椅子,把椅子移走,将反光柱放在环境中椅子的位置上,模拟椅子被障碍物遮挡,机器人视角如图9a所示;文献[13]的语义定位结果为图9b所示。

(a) 机器人视角图 (b) 语义定位位姿图9 遮挡椅子文献[13]语义定位结果

当运行本文算法时,首先识别到障碍物如图10a所示,其次匹配相似帧为关键帧2如图10b所示;再次将关键帧和当前被遮挡帧的语义链进行赋值加权,依据式(8)～式(10)计算求解T=1000,因此确定被遮挡部分的语义物体为椅子如图10c所示;然后在语义链表1中查找包含柜子、墙角和椅子语义信息的区域,确定机器人在2区域;最后使用墙角和椅子到机器人的相对距离进行位姿求解如图10d所示,其语义定位的正确位姿如图10e所示。

(a) 识别到障碍物 (b) 相似帧 (c) 当前帧语义信息

(d) 位姿求解 (e) 语义定位位姿图10 遮挡椅子本文语义定位算法结果

综上两组对比实验,本文算法相对文献[13]能够免疫动态障碍物遮挡语义物体的影响,能得到正确的语义定位位姿,本文语义定位算法拥有更高的鲁棒性,为后续机器人再同步结合AMCL算法与环境地图匹配进行精定位提供了有效的先验位姿信息。

2.2 粒子收敛性能对比实验

在3区域用反光柱模拟障碍物遮挡墙角,进行融合语义定位的AMCL重定位算法实验。当没有检测到语义信息时,机器人在3区域内原地旋转进行定位。开始时如图11a均匀分布粒子,控制机器人开始运动,随着机器人获得的传感器数据与地图进行匹配,粒子收敛如图11b和图11c所示,粒子逐渐向两个相似区域2和3收敛。由于环境较相似,真实位姿下得到的激光数据在多个假设位姿下与地图匹配较均衡,导致假位姿粒子无法消失,随着机器人的不断运动粒子依然无法收敛。

(a) 均匀分布粒子 (b) 粒子迭代 (c) 粒子迭代 (d) 粒子收敛图11 本文算法粒子收敛过程

当图11c之后通过SSD目标检测识别到障碍物如图12a后,进行相似度匹配确定相似帧为关键帧3如图12b,当前帧包含了垃圾桶和柜子语义信息,关键帧中也包含,故确定相似帧为3;将关键帧和当前帧的语义链进行赋值加权,依据式(8)～式(10)计算得T=1,由此确定被遮挡部分的物体类别为墙角如图12c所示;查找语义链表1确定机器人处在3区域;在3区域使用柜子和垃圾桶进行语义定位如图12d所示,求出满足条件的交点坐标,在栅格地图中语义定位结果如图12e所示。把当前语义定位的位姿发送给AMCL,重新融合当前粒子集计算权重并更新系数,进行粒子二次权重的更新,将语义定位的位姿附近粒子权重增加,多次重采样后粒子逐渐集中在真实位姿附近,机器人最终定位成功如图11d所示。

(a) 识别到障碍物 (b) 相似帧 (c) 当前帧语义信息

(d) 位姿求解 (e) 语义定位位姿图12 遮挡墙角本文语义定位结果

为了进一步说明本文定位算法进行重定位能有效的提高重定位的鲁棒性。在随意遮挡墙角、垃圾桶和椅子等语义物体的条件下进行30组重定位实验,实验结果如表3所示。由于实验场景较相似,原始的AMCL算法仅利用激光雷达观测模型与栅格地图匹配概率来更新粒子的权重,粒子无法准确收敛,重定位成功率仅为40%;使用文献[13]算法加载语义地图进行重定位时,由于存在障碍物遮挡语义物体,进行语义定位可能获得多个先验位姿,粒子可能聚集在错误的区域,导致机器人重定位失败,其重定位成功率为60%;使用本文算法加载语义地图进行重定位,当检测到障碍物时,首先通过相似度匹配和语义链查找来确定被遮挡的语义物体的类别,然后确定机器人所处的区域,粒子能够聚集到正确的房间中,粒子能够很快的收敛,最后机器人定位成功,其重定位成功率为90%,失败的案例是发生了误识别,把窗子识别成了门,从而导致定位失败。以上数据表明本文的免疫遮挡定位算法能够提高机器人重定位的鲁棒性。

表3 实验结果对比

为了验证本文算法机器人重定位速度快慢,在3区域不遮挡垃圾桶和遮挡垃圾桶进行实验。分别使用本文算法、文献[13]方法和原AMCL进行重定位,在3种定位方法都能够成功地情况下进行比较,记录每次重采样后的粒子的均权重。图13和图14分别为无障碍物遮挡和有障碍物遮挡垃圾桶的粒子均权重变化图。

图13 无遮挡时粒子收敛对比图 图14 墙角被遮挡粒子收敛对比图

从图13可以看出,垃圾桶未被遮挡的情况下本文定位算法和文献[13]算法由于加载了语义地图进行语义定位,粒子在迭代16次后均权重趋于稳定,此时粒子收敛成功;而原始AMCL由于2区域和3区域较相似,粒子均权重一直不能上升,直到第30次迭代,粒子均权重开始上升,第50次开始区域稳定,粒子成功收敛。当垃圾桶被遮挡时,由于本文算法能通过语义链判断出被遮挡的语义物体的类别,进而确定机器人所处的区域,依据其他高置信度的语义信息进行语义定位。

从图14可以看出,粒子在第18次迭代后均权重趋于稳定,此后粒子收敛成功;文献[13]算法虽然也加载了语义地图,但由于语义物体垃圾桶被遮挡,语义定位结果如图7b所示,存在多个位置的语义预定位结果,而且所处环境较相似,粒子均权重无法快速上升,直到第29次迭代后粒子均权重迅速上升,最后在第40次迭代后趋于稳定,机器人定位成功;原AMCL算法在本文相似环境下,开始粒子均权重一直无法快速上升,直到迭代50次后粒子均权重趋于稳定,机器人定位成功。

该实验说明,在相同条件下,融合视觉语义定位能够有效提高定位的速度,相比原始AMCL算法粒子收敛速度提高了60%;当语义环境中存在动态障碍物遮挡关键语义信息时,依靠语义链能够免疫障碍物的影响,本文算法的定位速率相对文献[13]提高了38%,说明了本文算法能够使机器人在更加复杂的环境下完成语义定位,进一步提高机器人重定位的速度。

3 结束语

为了解决存在障碍物遮挡语义物体而导致语义定位算法失效或产生多个定位位姿的问题,本文提出了一种免疫障碍物遮挡的语义定位算法,首先利用基于词袋模型的相似度匹配和通过语义向量链加权做差确定被遮挡的语义物体的类别,其次通过查找对应的地图语义链确定机器人处在哪个区域,然后使用其它未被遮挡的高置信度语义信息来进行语义定位,最后结合AMCL定位算法来进行精确的重定位,让机器人能在复杂相似环境下更快速实现重定位。通过移动机器人在现实场景下的实验,证明了本文算法能够免疫障碍物遮挡语义信息进行语义定位,提高了机器人定位的速度和鲁棒性。