徐 兴,刘 琼,黄开坤
动态场景下仓储机器人的视觉定位与建图
徐 兴,刘 琼,黄开坤
(南华大学 机械工程学院,湖南 衡阳 421200)
针对目前大多数室内仓储机器人的视觉定位与建图算法(SLAM)是假设机器人处在静态的环境,但是当场景中出现移动的物体时,机器人自身定位的准确性和稳定性易受到巨大影响,而其他室内定位技术比如超宽带、蓝牙等必须在无线信号覆盖的条件下工作的问题,提出一种面向室内仓储机器人在动态场景下的视觉SLAM算法:在基于旋转不变特征点的定位与建图改进算法(ORB-SLAM2)基础上,用几何对应神经网络(GCNv2)来替换基于图像金字塔的特征点提取算法;添加目标检测(YOLOv4)的语义线程,并且使用光流法来追踪特征点;然后通过运动一致性检测来识别图像中潜在的动态物体;最后剔除动态特征点后进行位姿估计。实验结果表明,在高度动态的室内场景下,提出的算法相对于ORB-SLAM2算法的绝对轨迹误差可减小95.56%~98.21%,能够有效解决ORB-SLAM2在动态场景下定位不准确的问题。
仓储机器人;动态场景;几何对应神经网络(GCNv2);目标检测;基于旋转不变特征点的定位与建图改进算法(ORB-SLAM2)
目前我国对移动机器人的研究正处在快速发展阶段,尤其是在物流行业中,人们需要仓储机器人来调配货物,这就需要机器人具备能够适应陌生环境的能力[1]。因此,对于仓储机器人来说,定位与建图(simultaneous localization and mapping,SLAM)[2]技术可以帮助其在陌生环境中完成自身的定位与建图。SLAM可以分为基于激光雷达的激光SLAM和基于相机的视觉SLAM 2种类型。虽然激光SLAM技术已经成熟,但其成本过高,因此越来越多的研究人员寻求可以降低成本的方法。相比之下,基于相机的视觉SLAM具有价格低廉,易于融合其他传感器和可获取丰富的环境信息等优点,成为相关领域主要研究方向之一[3]。
目前已有许多出色的视觉SLAM算法被研究出来,比如:基于旋转不变特征点(oriented fast and rotated brief,ORB)的定位与建图算法ORB-SLAM[4]、半直接视觉里程计(semi-direct visual odometry,SVO)[5]、大规模直接单目 SLAM(large scale direct monocular SLAM,LSD-SLAM)[6]等等。然而这些算法大多是基于静态环境的假设,如果环境内存在动态物体,动态特征点就会影响相机的位姿估计。因为这些算法会从图像中提取一些具有代表性的像素作为特征点,当相机运动时,根据这些像素出现在图像的不同位置来估计相机的运动轨迹,如果图片中的物体发生运动,图像间的像素匹配可能会出现错误,这就导致算法计算出来的相机位姿不准确,甚至定位失败[7]。近年来,随着目标检测、语义分割等技术的发展,越来越多的研究人员尝试着将这些技术应用到视觉SLAM中。文献[8]在基于旋转不变特征点的定位与建图改进算法(ORB-SLAM2)的基础上提出了动态场景中的跟踪、映射和修复(tracking, mapping and inpainting in dynamic scenes,DynaSLAM)算法,该算法使用多视图几何的方法来判断环境中的动态特征点。还有基于ORB-SLAM2算法的面向动态环境的语义视觉SLAM(semantic visual SLAM towards dynamic environments,DS-SLAM)算法[9],通过加入基于语义分割网络(segmentation network,SegNet)的语义分割部分来避免环境中动态物体对相机姿态估计的影响。但是这些算法存在着占用过大的计算资源以及对物体的边缘部分分类准确度不足的问题。
鉴于现有的视觉SLAM算法存在在动态环境下鲁棒性较差并且相机位姿估计不够准确等问题,本文针对ORB-SLAM2算法进行改进:
1)在特征点提取阶段,替换ORB-SLAM2基于图像金字塔的特征点提取算法,采用几何对应神经网络(geometric correspondence network version 2,GCNv2)[10]来提取图像的特征点。虽然基于旋转不变特征点的定位与建图[11]算法相对于加速稳健特征(speeded up robust features,SURF)[12]和尺度不变特征变换(scale-invariant feature transform,SIFT)[13]算法有更快的计算速度[14],但是仍然存在着特征点误匹配的问题,并且提取的特征点不够稳定,在纹理不足的场景很容易跟丢[15]。在某些ORB-SLAM2无法跟踪的极端场景下,GCNv2神经网络算法有着更好的表现。
2)在ORB-SLAM2算法的基础上,添加了目标检测线程。该线程会使用目标检测(you only look once version 4,YOLOv4)[16]算法对传输过来的图片进行目标检测,并对图片中的物体添加语义信息。最后将结果传入到跟踪线程。
3)本文算法使用光流法[17]跟踪图像中的特征点,对于跟踪到的特征点进行运动一致性检测,根据特征点到极线的距离判断是否为动态特征点。然后结合YOLOv4检测到的物体框,如果物体框内的动态特征点与总特征点的比值大于一定阈值,就将该物体框设置为动态物体框,反之设置为静态物体框。由于YOLOv4的目标检测框过大,会出现动态物体框和静态物体框重叠的情况,如果直接删除动态框内的特征点,会损失大量的静态特征点,反而会影响算法的精度。对于动态物体框内的特征点,只删除不在静态物体框的特征点。这个方法能有效检测出潜在的运动物体,并尽可能地减小动态物体框所占的图像面积,然后在后续的操作中会删除动态区域里的特征点,降低动态特征点对算法精度的影响。
4)通过测试本文算法在慕尼黑工业大学(Technical University of Munich,TUM)提供的数据集下的表现情况,并与ORB-SLAM2算法进行对比,判断本文算法的优化程度。
本文算法是基于ORB-SLAM2,在原本算法的跟踪、局部建图和回环检测这3个主要线程[18]中添加了目标检测线程。整体算法框架如图1所示,图中RGB-D指彩色-深度(Red Green Blue-Depth)数据集。当开始对系统输入图像数据的时候,这些图像会同时传入跟踪线程和目标检测线程进行处理。首先,在跟踪线程中,特征点提取部分使用GCNv2神经网络来提取特征点,随后通过对网络输出进行非极大值抑制和双线性采样来获取特征点和描述子。目标检测线程通过YOLOv4对输入图像进行目标检测,得到每个物体的语义标签。对图像的特征点进行光流跟踪,通过运动一致性检测判断跟踪成功的特征点是否为动态特征点。当一个物体检测框里的动态特征点所占比例达到一定阈值的时候,该物体即是潜在的运动物体,将该物体框设置为动态物体框,反之设置为静态物体框。最后保留处于动态物体框内的静态特征点,删除剩余的动态特征点,用这些优化后的静态特征点来进行相机的位姿估计。
图1 算法框架
在基于特征点法的视觉SLAM中,特征点提取算法的优劣影响着后续对相机的位姿估计以及系统的稳定性。ORB算法对于加速分割测试特征(features from accelerated segment test,FAST)角点的改进以及添加二进制的描述子,很大程度上提升了特征点的准确度和计算速度。虽然ORB算法采用了四叉树对图像中的特征点进行均匀分配,但是仍然会出现特征点重叠、分布不均匀的现象,从而影响系统的稳定性[19]。
为了解决在动态场景下ORB算法提取的特征点分布不均匀以及计算复杂的问题,使用GCNv2神经网络来替换ORB算法特征点提取部分,获得更为准确的关键点和描述子。GCNv2是对GCN的改进,是一种能够计算图像关键点和描述子的深度神经网络,可用来作为实时SLAM的前端。GCNv2采用了和ORB算法相同格式的二进制描述子,该网络通过卷积的方法,由原始灰度图获得低像素、多通道的概率图和特征图。再通过上采样改变概率图的像素大小,并根据变化后的概率图中各点的概率值确定图中的特征点位置。最后以特征点位置为基础,通过特征图中的对应值和一个二值化网络层提取出每个特征点对应的二值化描述子。如图2所示为GCNv2的神经网络结构,图中代表输入图像的分辨率。
图2 GCNv2神经网络结构
ORB提取的特征点分布如图3(a)所示,GCNv2提取的特征点分布如图3(b)所示。对于室内场景,ORB提取的特征点分布并不均匀,并且有些区域的特征点没有检测出来;而GCNv2神经网络提取的特征点分布得更加均匀,并且没有出现特征点重叠的现象,这是因为GCNv2在训练的时候使用了非极大值抑制。当相机快速运动的时候,ORB特征跟踪有时候会出现数据丢失的情况,但是GCNv2提取的特征点能很好地完成跟踪,以此可见,GCNv2提取的特征点稳定性更高。
图3 特征点提取
目标检测是机器人视觉研究领域的主要内容之一,是从图片中分辨出所有感兴趣的目标,并且精准识别出目标类型和位置[20]。目前,目标检测算法主要分为2类,即单阶网络和双阶网络。单阶网络可以从网络中直接提取特征来进行物体种类的预测以及位置的计算,这些算法的优点是通过适当地降低目标识别精度来大幅提升算法的计算速度。双阶网络的第一阶段是生成预定义的候选包围框;第二阶段通过卷积神经网络对包围框内的物体进行分类,该算法识别精度高但计算用时长。
由于本文算法对物体识别精度要求不是很高,为了提升算法的计算速度,采用单阶网络YOLOv4来对图像进行目标检测。如图4所示为使用YOLOv4对室内环境进行目标检测的结果,可以很清楚地看到,YOLOv4算法能够很好地将图片中各个物体检测出来。
图4 目标检测结果
通过YOLOv4目标检测,获得了边界框。由于YOLOv4的目标检测框过大,会出现动态物体框和静态物体框部分重叠的情况。如果直接删除动态框内的关键点,会损失大量的静态特征点,反而影响算法的精度,造成定位失败的后果。所以本文提出的算法将目标检测框分为动态框及静态框,只有当物体处在动态框而不处于静态框内的时候,才会将其删除。
对于潜在的运动物体,比如大部分时间处于静止但又能被人轻易移动的书本、椅子等物体,通过光流法追踪其特征点,并用运动一致性检测来判断其是否为动态物体。
1.4.1 光流法
光流法是一种计算图像中特征点运动的方法。它计算了图像中的每一个特征点的运动方向和速度,从而实现对目标运动的跟踪。光流算法利用图像间的像素位移来计算物体的运动信息,是一种非常有效的物体运动估计方法,在计算机视觉、机器人、目标跟踪等领域有着广泛的应用。目前光流法主要分为2种,即稠密光流和稀疏光流。稠密光流计算了图像中所有像素的光流向量,它的优点是精度高,对于细微的运动表示效果较好。但是,稠密光流的计算量很大,不适用于实时处理。相比之下,稀疏光流只对图像中的一些关键点计算光流,从而减少计算量。为了尽可能减少算法的运行时间,提高实时性,本文采用稀疏光流追踪特征点的位置。假设物体在2帧图像中亮度一样,并且在时刻处于图像(,)处像素的灰度值为(),当到+d时刻时,该点像素运动到(d,d,d),即有
将等式左边进行一阶泰勒展开得
设dd,dd,写成矩阵形式即
由于该等式存在2个未知数,所以还需要添加一个额外的约束来求解、。假设在一个大小为×的窗口中,像素有相同的运动速度,则有
式(5)即为的形式,由此可求得光流()的最小二乘解为
1.4.2 运动一致性检测
通过使用YOLOv4可以有效地检测出输入图像中的动态对象,但是并不能很好地识别潜在的动态对象。为了解决这个问题,采用DS-SLAM提出的基于稀疏光流的动态点检测方法,即运动一致性检测。首先,通过计算光流金字塔,得到当前帧中匹配的特征点,并删除一些太接近图像边缘的匹配特征点及周边像素差异过大的匹配特征点。然后通过匹配的特征点计算出基本矩阵,再利用基本矩阵计算出当前帧中的极线。最后,计算出匹配点到极线的距离。如果距离大于设定的阈值,则将其设定为动态特征点,反之则认为是静态特征点。该方法的具体原理如图5所示。
图5 移动一致性检测
图5中:2个平行四边形1、2表示的是2个不同位置相机的成像平面;1、2是相机的光心;是空间点,对应在2个成像平面的像素坐标是1、2,设其坐标为1=(111),2=(221);点1、2组成的平面与2个相机的成像平面的交线即为极线1、2。极线1可以通过式(7)计算出来,即
式中:为基本矩阵;、、为极线1的方程参数,即极线1在平面的方程为0。对级约束描述了一幅图像的一个点到另一幅图像中的对应极线的映射,映射关系可以通过式(8)描述,即
通过目标检测算法可以得到潜在运动物体的物体框,在计算物体框内动态点占所有特征点的比例时,当比例超过预设阈值,就将该物体框视为动态物体框,并将动态物体框内处于静态物体框之外的特征点全部删除,最后使用剩余的静态特征点进行位姿估计。
本文改进算法的测试平台是台式电脑,硬件配置是Intel i5-11400H,主频2.7 GHz,内存为16 G,显卡为RTX3060,搭载系统为Ubuntu18.04。这些硬件和软件配置足以支持本文的算法实验,并且能够提供充足的计算资源来进行测试和评估。
为了评估本文提出的在室内动态场景下仓储机器人定位的精确性和鲁棒性,采用德国慕尼黑工业大学提供的TUM RGB-D(Red Green Blue Depth)数据集,该数据集是开源数据集,由39组不同室内环境的图像序列组成。采集数据的设备是微软公司的RGB-D Kinect相机,录制频率为 30 Hz,包含彩色图像、深度图像和相机位姿真值3个部分。其中的动态数据集分为2个部分,分别是“sitting”序列和“walking”序列。“sitting”序列平均时长为31 s左右,涵盖了2个人坐在桌子前的场景,包含人物的肢体动作和静态物体的移动。“walking”序列平均时长为30 s左右,涵盖了2个人围着桌子移动的场景,动态场景占据该序列一半以上的时间。除此之外,相机的运动分为以下4种:1)halfsphere(半圆,即相机按照直径为1 m的半圆形轨迹运动);2)rpy(相机按照翻滚—俯仰—偏摆即roll—pitch—yaw的顺序进行翻转运动);3)static(静止,即相机基本保持不动);4)(相机按照轴的顺序移动)。该数据集适合评估在复杂动态室内环境下所提出的算法性能。
本文算法的评价指标为相对位姿误差(relative pose error,RPE)和绝对轨迹误差(absolute trajectory error,ATE)及变化幅度,其中相对姿态误差是指在SLAM系统中,相邻帧间姿态的差异。这是由多种因素导致的,包括传感器误差、计算误差和环境干扰。相对姿态误差会影响SLAM系统的性能。因为如果相邻帧间姿态误差过大,它们就不能正确地匹配,进而影响地图的精确性。相机第帧的相对位姿误差计算公式为:
绝对轨迹误差是指SLAM系统中机器人的实际轨迹与预测轨迹的偏差。它可以用来评估SLAM系统的性能。第帧的绝对轨迹误差的计算公式为:
式中为转换矩阵。
变化幅度的计算公式为
式中:为ORB-SLAM2的运行结果;为本文算法的运行结果。
为了验证本文算法提取特征点的有效性,首先显示ORB-SLAM2特征点的分布以及本文算法的特征点分布,通过观察场景中人身上是否有特征点来判断算法是否成功。并且比较2个算法在静态环境下的绝对轨迹误差和相对轨迹误差。为了实验公平,数据集选择运动幅度较小的sitting序列来进行测试。如图6(a)所示为ORB-SLAM2算法提取的特征点,可以看到,人的身上分布着大量的特征点。如图6(b)所示为改进后的算法,可知有效地剔除了动态物体“人”身上的特征点。
图6 算法提取特征点
如表1所示为GCNv2和ORB-SLAM2在特征点提取方面的对比。可知,在静态环境或者微动态环境下,GCNv2提取的特征点相比于ORB-SLAM2提取的特征点,准确性上都更好一点。
表1 ORB-SLAM2和本文算法的绝对轨迹误差和相对轨迹误差
为了验证本文算法在高度动态场景下的稳定性及准确度,采用walking序列的数据集,测试本文算法及ORB-SLAM2算法的相机绝对轨迹误差,如表2所示,其中Mean和Median分别表示数据的平均值和中位数。
表2 ORB-SLAM2和本文算法的绝对轨迹误差
对表2中数据进行分析,可以得到改进算法的提升程度。在高度动态场景下,移动的物体对传统ORB-SLAM2算法的精度影响很大。本文提出的算法有效地剔除了动态特征点,绝对轨迹误差明显减小,减小的幅度在95.56%~98.21%之间。如图7和图8所示为ORB-SLAM2和本文算法对数据集walking_、walking_halfsphere相机的运行轨迹估计值和运行轨迹真实值在、、轴上的可视化曲线以及RPE误差。
在图7(a)、图7(b)、图8(a)、图8(b)中,黑线代表相机的运行轨迹真值,蓝线代表算法的估计轨迹值,红线代表二者的误差。由可视化曲线可以很明显地看出来,本文算法有效地减小了ORB-SLAM2算法对相机位姿估计的误差。图7(c)、图7(d)、图8(c)、图8(d)描述了随着时间的变化,估计算法轨迹与真实轨迹之间的平移误差变化。图7(c)、图7(d)中间部分误差较小,原因是此处数据集中相机运动相对稳定,场景中动态物体运动幅度较小。可知,本文算法轨迹的平移量比ORB-SLAM2算法的轨迹平移量要小很多。
图7 ORB-SLAM2估计的轨迹以及平移误差
图8 本文算法估计的轨迹以及平移误差
本文提出了仓储机器人在室内动态场景下的视觉SLAM算法,这个算法在ORB-SLAM2算法的基础上,用GCNv2来提取特征点,用YOLOv4来进行目标检测。通过对特征点进行光流跟踪以及移动一致性检测,成功地将场景中的动态特征点剔除,大幅提升了系统在动态场景下的准确性和鲁棒性。实验结果表明,本文算法明显地提升了ORB-SLAM2算法在动态场景下的稳定性与准确性。本文的算法仍有以下不足之处:1)YOLOv4算法可能出现检测出错的情况,后续可以优化网络或者使用精度更好的目标检测算法来解决这个问题。2)本文算法在数据集中得到了较好的验证,但尚未应用在实时场景,后续可对其在实际场景下进行测试。
[1] 葛研. AGV自动导引车在国内的发展前景探讨[J]. 设备管理与维修, 2021, 489(Z1): 157-159. DOI:10.16621/j.cnki.issn1001-0599.2021.02.81.
[2] 吴凡, 宗艳桃, 汤霞清. 视觉SLAM的研究现状与展望[J/OL]. 计算机应用研究, 2019: 1-9[2023-04-19]. https://doi.org/10.19734/j.issn.1001-3695.2019.02.0035.
[3] 任伟建, 高强, 康朝海, 等. 移动机器人同步定位与建图技术综述[J]. 计算机测量与控制, 2022, 30(2): 1-10,37. DOI:10.16526/j.cnki.11-4762/tp.2022.02.001.
[4] MUR-ARTAL R, MONTIEL JMM, TARDÓS D J. Orb-SLAM: A versatile and accurate monocular SLAM system[J]. CoRR, 2015(5): 1147-1163.
[5] Forster C, Pizzoli M, D Scaramuzza. SVO: Fast semi-direct monocular visual odometry[C]// IEEE International Conference on Robotics & Automation. IEEE, 2014.
[6] Engel J, Schps T, Cremers D. LSD-SLAM: Large-scale direct monocular SLAM[C]// European Conference on Computer Vision. Springer, Cham, 2014.
[7] 张乐添,赵冬青, 贾晓雪, 等. 面向动态环境的双目/惯性SLAM系统[J]. 导航定位学报, 2022, 10(6): 144-150. DOI:10.16547/j.cnki.10-1096.20220619.
[8] Berta B, José M F, Javier C, et al. Dynaslam: Tracking, mapping and inpainting in dynamic scenes[J]. IEEE Robot Autom Lett, 2018, 3(4): 4076-4083.
[9] Yu C, Liu Z, Liu X, et al. DS-SLAM: A semantic visual SLAM towards dynamic environments[C]// 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018.
[10] TANG Jiexiong, Ericson L, Folkesson J, et al. Gcnv2: Efficient correspondence prediction for real-time SLAM[J]. IEEE Robot Autom Lett, 2019, 4(4): 3505-3512.
[11] RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB:An efficient alternative to SIFT or SURF[C]// International Conference on Computer Vision. IEEE, 2012: 2564-2571.
[12] BAY H, ESS A, TUYTELAARS T, et al. Speeded-Up robust features(SURF)[J]. Computer Vision & Image Understanding, 2008, 110(3): 346-359.
[13] LOWE D G. Distinctive image features from scale-invariant keypoints[J]. International Journal of Computer Vision, 2004, 60(2): 91-110.
[14] 索春宝, 杨东清, 刘云鹏. 多种角度比较SIFT、SURF、BRISK、ORB、FREAK算法[J]. 北京测绘, 2014(4): 23-26,22. DOI:10.19580/j.cnki.1007-3000.2014.04.006.
[15] 付元辰, 马鸣庸, 丁龙阳. 面向室内视觉定位的点特征提取算法比较[J]. 导航定位学报, 2022, 10(1): 36-40.
[16] Redmon J, Divvala S, Girshick R, et al. You only look once: Unified, real-time object detection[C]// Computer Vision & Pattern Recognition. IEEE, 2016.
[17] BEAUCHEMIN S S, BARRON J L. The computation of optical flow[J]. ACM Computing Surveys (CSUR), 1995, 27(3): 433-466.
[18] Mur-Artal R, Tardós 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.
[19] 成怡, 佟晓宇. 基于改进ORB算法的移动机器人视觉SLAM方法研究[J]. 电子技术应用, 2019 ,45(1): 10-13,18. DOI:10.16157/j.issn.0258-7998.183046.
[20] 尹宏鹏, 陈波, 柴毅, 等. 基于视觉的目标检测与跟踪综述[J]. 自动化学报, 2016, 42(10): 1466-1489. DOI:10.16383/j.aas.2016.c150823.
Vision SLAM of storage robot in dynamic scene
XU Xing, LIU Qiong, HUANG Kaikun
(School of Mechanical Engineering, University of South China, Hengyang, Hunan 421200, China)
Aiming at the problems that the current majority of visual simultaneous localization and mapping (SLAM) algorithms utilized by indoor storage robots are predicated upon the assumption that the robot be operating within a static environment, however, the presence of mobile objects within the visual field can greatly impair the accuracy and stability of the robot's inherent positioning capabilities, while other indoor positioning technologies such as ultra wide band (UWB) and bluetooth must work under the condition of wireless signal coverage, the paper proposed a vision-based SLAM algorithm tailored specifically to indoor storage robots operating within dynamic scenes: the geometric correspondence network version 2 (GCNv2) was leveraged to supplant the traditional oriented fast and rotated brief (ORB) feature point extraction algorithm based on image pyramids based on the ORB-SLAM2 framework; and the semantic threads of target detection technique you only look once version 4 (YOLOv4) were added in tandem with optical flow to track feature points; then the potential dynamic objects were identified in the image through motion consistency detection; finally, after the removal of dynamic feature points, the position and attitude estimation was performed. Experimental result showed that in highly dynamic indoor scenes, the absolute trajectory error of the proposed algorithm, compared to the ORB-SLAM2 algorithm, would be reduced by 95.56%~98.21%, which indicates that the proposed method could effectively solve the problem of inaccurate positioning of ORB-SLAM2 in dynamic scenarios.
storage robot; dynamic scene; geometric correspondence network version 2 (GCNv2); object detection; oriented fast and rotated brief (ORB)-simultaneous localization and mapping (SLAM) 2
徐兴,刘琼,黄开坤. 动态场景下仓储机器人的视觉定位与建图[J]. 导航定位学报, 2023, 11(6): 110-118.(XU Xing, LIU Qiong, HUANG Kaikun. Vision SLAM of storage robot in dynamic scene[J]. Journal of Navigation and Positioning, 2023, 11(6): 110-118.)DOI:10.16547/j.cnki.10-1096.20230614.
P228
A
2095-4999(2023)06-0110-09
2023-03-23
国家重点研发计划项目(2023YFC3010900);南华大学教学改革项目(2019ZD-XJG09)。
徐兴(1996—),男,安徽宣城人,硕士研究生,研究方向为室内定位。
刘琼(1980—),男,湖南衡阳人,博士,副教授,研究方向为机械设计及理论。