CN115930964A - 一种动态环境下的同时定位与三维地图构建方法 - Google Patents

Abstract

本发明公开了一种动态环境下的同时定位与三维地图构建方法，包括以下步骤：S1、根据语义信息对从标记为动态的对象中提取的特征点进行过滤；S2、将其余特征点与前一帧匹配，使用匹配的特征点对计算稳定的基础矩阵；S3、通过几何约束过滤出所有真实的动态特征点对，保留静态特征点对。相比于传统方法根据语义信息直接剔除先验动态特征点，本发明通过采用动态特征过滤策略，结合语义信息和几何约束关系共同检测处于运动状态的目标物体，滤除当前帧各运动目标的特征点，能够有效提高在动态环境下的定位精度和稳定性，从而使机器人充分的感知外界周围环境，安全运行。

Description

一种动态环境下的同时定位与三维地图构建方法

技术领域

本发明涉及视觉SLAM领域，尤其是涉及一种动态环境下的同时定位与三维地图构建方法。

背景技术

机器人自20世纪诞生以来，发展迅速，已应用于各行各业，如工业、农业、军事、生活服务等。其智能化水平越来越高，机器人也越来越接近人类生活。无论是已经被成规模运用的仓储物流小车(AGV)，家用的扫地机器人，还是当今工业界研究热点的无人驾驶，它们取得的成就与发展潜力引人瞩目。而对于移动机器人而言，一个很重要的问题便是如何从环境中获取位置信息，换言之，怎样得到未知环境中绘制出高精度地图，从而实现之后的路径规划等算法。这就是机器人四大核心问题中的感知问题，感知外界环境是机器人在人类社会复杂环境中稳定移动的一个很重要的前提，趋利避害，接近目标，避开障碍物。所以机器人导航系统应该使机器人充分的感知外界周围环境，才能够安全运行。

机器人要想融入到人类生活当中，首先应该能够在人类环境中稳定的移动。人类环境是复杂的，有很多服务于人类的静止的设施，也有些实物是处于不断的运动之中，不仅包括人类个体的运动，还包括人类使用的交通工具的移动以及存在于人类社会中的动物的运动等。这些都是智能机器人在人类环境中安全稳定运行所面临的挑战。

机器人同时定位与三维地图构建技术(SLAM)自上世纪90年代诞生以来便成为机器人导航领域的一个核心问题，它依靠移动机器人上装置的摄像头对环境连续摄像，从获取的连续图像对信息中得到周围环境的三维简化地图，其目标是在估计机器人的位姿参数的同时构建环境的地图。传统的SLAM方法主要依赖于稳定性较好的距离传感器如激光雷达。然而激光雷达获得的距离数据非常稀疏，这就造成SLAM构建得到的环境地图仅包含极少量的路标点，这个地图仅能被用来提高机器人的定位精度，而无法用于路径规划等机器人导航的其它领域。此外激光雷达高昂的价格、较大的体积重量以及耗电量限制了其在某些领域的应用。相较于激光等传感器，视觉不仅能获得环境的距离、深度信息，还能获得对物体的颜色，并结合相关算法达到对物体的认知，其获得信息量几何性的超越激光雷达等传感器。

如今，已经有许多很成熟的视觉SLAM方案，如著名的ORB-SLAM2系统(一个基于特征点的实时单目SLAM系统)。但是这些SLAM方案绝大多数都是基于一个静态场景假设，即环境是静态的，纹理，光照都比较好的条件下。但现实往往是处于一个动态的环境当中，存在大量物体或人的干扰，这就为SLAM问题带来了困难。传统SLAM算法仅仅解决了静态环境下的定位和建图问题，然而当环境中出现大量运动物体时，机器人的定位将会产生较大误差，甚至丢失跟踪。

发明内容

本发明的目的在于解决提高动态环境下移动机器人的定位和建图的精度的问题，提供一种动态环境下的同时定位与三维地图构建方法。

为实现上述目的，本发明采用以下技术方案：

一种动态环境下的同时定位与三维地图构建方法，包括以下步骤：

S1、根据语义信息对从标记为动态的对象中提取的特征点进行过滤；

S2、将其余特征点与前一帧匹配，使用匹配的特征点对计算稳定的基础矩阵；

S3、通过几何约束过滤出所有真实的动态特征点对，保留静态特征点对。

在一些实施例中，步骤S1包括：利用目标检测算法对从标记为动态的对象中提取的特征点进行筛选。

在一些实施例中，所述利用目标检测算法对从标记为动态的对象中提取的特征进行筛选具体为：利用YOLO v5生成的语义检测框对提取的每个ORB特征进行过滤。

在一些实施例中，步骤S2包括：遍历参考帧中的所有地图点，分别向当前帧进行投影，在当前帧中找到一个描述子距离最相近的特征点作为其匹配点。

在一些实施例中，步骤S3包括：若当特征点对在动态语义边界框内，将D>λ1θ的特征点对识别为真实的动态特征点对并剔除；若当特征对在动态语义边界框外，将D>λ2θ的特征点对识别为真实的动态特征点对并剔除；其中e₁和e₂为两个不同的阈值，θ是预设阈值。

在一些实施例中，所述λ1小于λ2。

在一些实施例中，所述预设阈值θ由以下公式表示：

其中，ORB特征点总数表示为N，D_i是第i个特征到其极线的距离。

本发明还提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时能实现上述的方法。

本发明还提供一种机器人，其上存储有计算机程序，该计算机程序被处理器执行时能实现上述的方法。

本发明具有如下有益效果：

本发明通过采用动态特征过滤策略，结合语义信息和几何约束关系共同检测处于运动状态的目标物体，滤除当前帧各运动目标的特征点，相比于传统方法根据语义信息直接剔除先验动态特征点，能够有效提高在动态环境下的定位精度和稳定性，从而使机器人充分的感知外界周围环境，安全运行；

进一步地，相比于传统方法采用的语义分割网络，本发明实施例选择更轻量级的目标检测算法，能够有效的对场景物体进行标记，有效地解决在动态环境下非静态物体定位与地图构建的干扰问题，提高系统的鲁棒性。

附图说明

图1是本发明实施例中的动态环境下的同时定位与三维地图构建方法的流程图；

图2是本发明实施例中的基于语义信息融合的动态场景下SLAM方法的流程图；

图3是本发明实施例中的极线约束示意图；

图4是本发明实施例中的动态特征过滤策略流程图；

图5是本发明实验例中标记动态特征点的示意图；

图6是本发明实验例中保留静态特征点对的示意图。

具体实施方式

以下对本发明的实施方式做详细说明。应该强调的是，下述说明仅仅是示例性的，而不是为了限制本发明的范围及其应用。

在实际应用中，智能机器人不可能完全工作在静态环境中，所以在动态环境下SLAM算法尤为重要。此外，传统SLAM算法仅仅为机器人提供了环境的几何信息，即机器人只能获取环境中障碍物的位置信息，利用这些信息移动机器人可以规划出导航路径，但是无法利用环境信息执行更加高级的导航任务。总结起来，SLAM技术要解决的问题包括两点：一是提高动态环境下移动机器人的定位和建图的精度；二是获取周围环境中更高层次的语义信息。

目前现有技术有以下的缺点：

1.目前只有少量的研究涉及动态环境下的SLAM方法，如浙江大学章国锋教授课题组的RDSLAM方法在借鉴PTAM方法的基础上引入了基于时序先验的自适应RANSAC(随机抽样一致算法)方法来滤除动态物体上的误匹配点。该方法适用于室内桌面小规模环境下场景缓慢改变的情况。CD SLAM是一种基于关键帧的SLAM方法，文中提出了HoC(高阶组件)过滤非静态特征点，从而保持长时间相对可靠的SLAM。上述方法均基于相同的原理：将图像中的运动物体部分视为离群值，在估计姿态的过程中将其排除在外，仅依赖于场景中的静态部分。这就造成目前此类方法估计的准确度依赖于场景静态部分特征点所占的比例，如果场景中动态物体过多过密将会严重影响位姿估计的可靠性，进而会影响地图构建的准确度

2.另一种方案是结合深度学习进行特征点判别。近年来，深度学习技术席卷了人工智能研究领域。深度学习在视觉相关的图像识别、分割等领域体现了出色的性能。因此近年来也出现了将深度学习引入机器人导航领域的呼声。一些经典的语义分割算法如YOLACT++、Polarmask、Mask r-cnn等在与SLAM结合方面被寄予厚望。如Bescos等人设计的DynaSLAM(一种建立在ORB-SLAM2上的视觉SLAM系统)将Mask r-cnn引入ORB-SLAM(一种基于ORB描述子的特征点法视觉SLAM)，通过实例分割获得的语义信息对特征点进行标注，过滤非静态物体对SLAM系统的干扰。其缺点在于：1)现有方案大多利用深度学习mask，将具有先验动态性质的物体上的所有特征点一律去除，这将导致即使该目标是静态时，系统也会误将静止物体上的特征点去除，造成相机位姿的估计误差，使得构建得到的静态场景地图失去这些物体的信息；2)现有方案使用语义分割对非静态特征点进行剔除，但是语义分割网络运算成本高、运行速度低，与SLAM结合后难以实现实时性。另一方面语义分割会出现分割不干净、延伸冗余等情况出现；3)现有的采用语义和极线约束过滤动态特征点的方案，用于极线约束的基础矩存在较大误差，影响了算法性能。

如图1所示，本发明实施例的动态环境下的同时定位与三维地图构建方法包括以下步骤：

S1、根据语义信息对从标记为动态的对象中提取的特征点进行过滤；

S2、将其余特征点与前一帧匹配，使用匹配的特征点对计算稳定的基础矩阵；

S3、通过几何约束过滤出所有真实的动态特征点对，保留静态特征点对。

本发明实施例要解决的问题是用已有的动态环境下的SLAM算法存在的三个问题：

利用深度学习网络，将具有先验动态性质的物体上的所有特征点一律去除，这将导致即使该目标是静态时，系统也会误将静止物体上的特征点去除，造成相机位姿的估计误差，使得构建得到的静态场景地图失去这些物体的信息。

现有方案使用语义分割对非静态特征点进行剔除，但是语义分割网络运算成本高、运行速度低，与SLAM结合后大大降低系统实时性。另一方面语义分割会出现分割不干净、延伸冗余等情况出现

现有的采用语义和极线约束过滤动态特征点的方案，用于极线约束的基础矩阵存在较大误差，影响了算法性能。

本发明实施例提出了一种新的动态ORB特征过滤方法，称为基于语义信息融合的动态场景下SLAM方法。

本实施例方法的流程图如图2所示。首先，利用YOLO v5(一种目标检测模型)生成的语义检测框对提取的每个ORB特征进行过滤。然后，将其余特征点与前一帧匹配，使用匹配的特征点对计算稳定的基础矩阵。最后，通过几何约束过滤出所有真实的动态特征点对，具体步骤如下：

1.语义检测

本发明实施例采用YOLO v5检测标记为动态的对象。YOLO v5可以在嵌入式平台上实时实现。该方法在PASCAL VOC(视觉对象类)数据集上训练，总共可以检测20个类。在室内环境中，人很可能是动态对象，因此在本发明实施例中，人被标记为动态对象。除YOLO v5外还可以使用其他的高性能物体检测算法也可以实现动态特征点的识别和剔除。

过滤掉从标记为动态的对象中提取的所有特征是提高SLAM系统准确性的一种简单方法。然而，这种方法存在两个问题。首先，在真实的室内环境中，人不是唯一独立的动态对象。例如，由人移动的椅子也是个动态对象。其次，YOLO v5只能提供一个检测框，其中可能包含许多静态特性。当检测框占据当前帧的一半以上时，其余特征很少，这可能会导致SLAM系统中的跟踪线程失败。

总之，仅对从标记为动态的对象中提取的所有特征使用语义信息过滤不足以提高定位精度，并且在某些极端情况下会导致跟踪失败。

本发明实施例从标记为动态的对象中提取的特征点，首先根据其语义信息进行筛选。然后，根据剩余特征计算稳定的基础矩阵。最后，通过极线约束过滤掉所有真实的动态特征点对。

将特征与前一帧进行特征匹配：遍历参考帧中的所有地图点，分别向当前帧进行投影，在当前帧中找到一个描述子距离最相近的特征点作为其匹配点

2.几何约束法

鉴于语义约束的局限性，本发明实施例使用几何约束方法进一步过滤出动态特征。极线约束即几何约束是摄影几何的一部分，它独立于两幅图像之间的外部环境而存在，并且仅取决于相机的内部参数和相对位姿。两幅相邻图像之间的极线约束关系如图3所示。

参考图3，I₁和I₂是从同一场景捕获的两幅图像。P是一个空间点，其在I₁和I₂上的投影分别为x₁和x₂。O₁和O₂是两个摄像头的光学中心，它们的连接分别在e₁和e₂处与I₁和I₂相交。l₁和l₂是I₁和I₂的极线，可通过与I₁、I₂和PO₁O₂图像平面相交获得；x₂和p指如果空间点P(未知)在I1上的像点为x1.(已知)，那么世界点P在I2上的像点必然在极线l2.上；例如若空间点在p的位置，那么它在I2上的像点就是x2，也是在极线l2上；这就是极线约束。

如果I₁和I₂之间的空间几何变换记为(R,t)，那么x₁和x₂可以表示为

s₁x₁＝kp₁

s₂x₂＝kp₂＝k(Rp₁+t)     (1)

其中p₁、p₂是I₁和I₂相机坐标系下P的坐标，s₁、s₂是其深度值，k是相机的参数矩阵，R,T分别代表了旋转矩阵和平移向量。旋转矩阵和平移向量描述了一个刚体在三维空间中的运动过程。

如果归一化坐标

那么我们可以得到以下方程式：

将上述方程乘以

然后会得到：

其中T代表矩阵的转置；

其中[t]_x是向量t的反对称矩阵。将方程(1)代入方程(3)，我们得到以下方程：

其中F是基础矩阵，其中T代表矩阵的转置。上述等式说明了匹配特征对的正确关系。换句话说，如果一个特征点与另一个特征点正确匹配，则它必须位于其图像平面的极线上。然而，在实际场景中，由于噪声和摄像机标定参数误差的影响，计算出的基础矩阵往往存在一定的误差，往往使得特征点无法精确地落在计算出的极线上，即因为噪声、动态环境造成的错误的匹配点的影响，可能会造成计算得到的基础矩阵存在较大误差，所以得到的不是稳定的基础矩阵。因此，我们可以使用每个特征点到极线的距离来衡量匹配点对的可靠性。

如果x₁和x₂的齐次坐标分别为x₁＝[u₁,v₁,1]，x₂＝[u₂,v₂，1]，则l₁上的对极线可通过以下等式计算：

其中，X、Y和Z表示极线方程向量。然后，可通过以下公式获得从点x₂到极线的距离：

其中D表示x₂到极线l₂的距离。在真实场景中，通常有两种情况会导致此距离过大。第一种是特征点不匹配，第二种是从动态物体中提取的特征点。对距离D使用约束可以移除不匹配的点对和动态特征。如果D＞θ，则该点对被视为一个动态特征，其中θ是预设阈值，可根据方程(7)计算

其中，ORB特征点总数表示为N，D_i是第i个特征到其极线的距离。

3.动态特征过滤策略

仅使用语义信息无法完全过滤动态特征，而仅使用几何约束方法也存在一些问题。具体而言，当仅使用某种动态特征来计算基础矩阵时，计算的基础矩阵将包含较大的误差。

本发明实施例结合语义和几何约束来识别场景的动态特征：

首先，利用YOLO v5生成的语义检测框对提取的ORB特征进行过滤。然后，将其余特征点与前一帧进行匹配，并使用动态对象检测框过滤的匹配特征对计算稳定的基础矩阵。最后，通过几何约束过滤出所有真实的动态特征点对。详细的动态特征过滤策略流程图如图4所示，若当特征点对i在动态语义边界框，即YOLO v5生成的语义检测框内，将D>λ1θ的特征点对识别为真实的动态特征点对并剔除，保留静态特征点对；若当特征对i在动态语义边界框外，将D>λ2θ的特征点对识别为真实的动态特征点对并剔除，保留静态特征点对；其中，D表示特征与极线之间的距离，θ由方程(7)计算得出。可以看出，设置了两个不同的阈值λ1和λ2来过滤算法中的动态特征，因为标记为动态对象的检测框中的特征很可能是真正的动态特征。因此，检测框区域中的特征被更严格地过滤，这意味着λ1小于λ2。

本发明实施例主要包括一种基于语义信息融合的动态场景SLAM方法，提升在动态环境下的定位精度。本发明实施例相较现有技术的改进有：

相比于传统方法根据语义信息直接剔除先验动态特征点，本发明实施例通过采用动态特征过滤策略，结合语义信息和极线约束关系共同检测处于运动状态的目标物体，滤除当前帧各运动目标的特征点，估计相机位姿。

在研究过程中，首先须选择一种深度学习网络对非静态特征进行剔除。对于图像深度学习子模块，相比于传统方法采用的语义分割网络，本发明实施例选择更轻量级的目标检测算法，能够有效的对场景物体进行标记，有效地解决在动态环境下非静态物体定位与地图构建的干扰问题，提高系统的鲁棒性。

本实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时能实现上述的方法。

本实施例还提供一种机器人，其上存储有计算机程序，该计算机程序被处理器执行时能实现上述的方法。

本发明实施例通过融合语义信息，即根据语义信息对从标记为动态的对象中提取的特征点进行过滤、将其余特征点与前一帧匹配、使用匹配的特征点对计算稳定的基础矩阵，对极线约束进行了修改。利用YOLO(一种目标检测模型)生成的语义检测框对提取的ORB(一种特征提取算法)特征进行过滤，然后使用匹配的特征点对计算稳定的基础矩阵，最后通过几何约束过滤出所有真实的动态特征点对。本发明实施例利用语义检测、语义信息对极线约束的改进：基于标记为动态对象语义检测框外的特征计算基础矩阵、对动态特征点过滤策略：针对动态对象检测框内外的特征点采取不同的过滤阈值，解决了动态环境下机器人定位产生较大误差、机器人无法利用环境信息执行更加高级的导航任务的问题。

能够产生如下的效果：

1.提高动态场景下位姿估计精度。

本发明实施例基于语义SLAM的动态环境相机位姿估计方法，针对动态场景，提出了基于语义信息和极线约束的位姿估计改进方法，结合语义信息计算更加精确的基础矩阵，通过极线约束关系检测处于运动状态的目标物体，以更精确的方式滤除运动目标的特征点，克服了场景内运动物体的对相机位姿估计产生的影响，提高了动态场景下相机位姿估计精度。

2.提升位姿估计运行速度

本发明实施例采用轻量级神经网络YOLO v5，现有方案大多使用语义分割对非静态特征点进行剔除，但是语义分割网络运算成本低、运行速度高，与SLAM结合后大大降低系统实时性。而采用YOLO v5检测标记为动态的对象，可以在嵌入式平台上实时实现。大大提升了动态环境下SLAM系统的运行速度。

3.提高了建图的稳定性和鲁棒性

通过动态特征检测模块，精确地剔除了动态特征点，提高了后续建图的稳定性和鲁棒性，增强了地图对于环境的表达能力。

实验例

参考图5，利用YOLO v5检测并标记动态的对象；因为在室内环境中，人很可能是动态对象，因此在本方法中，人被标记为动态对象。本例中，我们将识别到的人由动态对象检测框框住，然后根据动态检测框外的特征点计算稳定的基础矩阵；参考图6，再通过几何约束过滤出所有真实的动态特征点对，保留静态特征点对。实验结果如图6所示，人物身上的动态特征点均被剔除。具体的，通过利用YOLO v5生成的语义检测框对提取的每个ORB特征进行过滤。然后，将其余特征点与前一帧匹配，使用匹配的特征点对计算稳定的基础矩阵。最后，通过几何约束过滤出所有真实的动态特征点对。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上内容是结合具体/优选的实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，其还可以对这些已描述的实施方式做出若干替代或变型，而这些替代或变型方式都应当视为属于本发明的保护范围。在本说明书的描述中，参考术语“一种实施例”、“一些实施例”、“优选实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。尽管已经详细描述了本发明的实施例及其优点，但应当理解，在不脱离专利申请的保护范围的情况下，可以在本文中进行各种改变、替换和变更。

Claims (9)

1.一种动态环境下的同时定位与三维地图构建方法，其特征在于，包括以下步骤：

S1、根据语义信息对从标记为动态的对象中提取的特征点进行过滤；

S2、将其余特征点与前一帧匹配，使用匹配的特征点对计算稳定的基础矩阵；

S3、通过几何约束过滤出所有真实的动态特征点对，保留静态特征点对。

2.如权利要求1所述的方法，其特征在于，步骤S1包括：利用目标检测算法对从标记为动态的对象中提取的特征点进行筛选。

3.如权利要求2所述的方法，其特征在于，所述利用目标检测算法对从标记为动态的对象中提取的特征进行筛选具体为：利用YOLO v5生成的语义检测框对提取的每个ORB特征进行过滤。

4.如权利要求1所述的方法，其特征在于，步骤S2包括：遍历参考帧中的所有地图点，分别向当前帧进行投影，在当前帧中找到一个描述子距离最相近的特征点作为其匹配点。

5.如权利要求3所述的方法，其特征在于，步骤S3包括：若当特征点对在动态语义边界框内，将D>λ1θ的特征点对识别为真实的动态特征点对并剔除；若当特征对在动态语义边界框外，将D>λ2θ的特征点对识别为真实的动态特征点对并剔除；其中e₁和e₂为两个不同的阈值，θ是预设阈值。

6.如权利要求5所述的方法，其特征在于，所述λ1小于λ2。

7.如权利要求5所述的方法，其特征在于，所述预设阈值θ由以下公式表示：

其中，ORB特征点总数表示为N，D_i是第i个特征到其极线的距离。

8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时能实现权利要求1-7任一项所述的方法。

9.一种机器人，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时能实现权利要求1-7任一项所述的方法。

Images