CN114565675A - 一种在视觉slam前端去除动态特征点的方法 - Google Patents

Abstract

本发明公开了一种在视觉SLAM前端去除动态特征点的方法，属于计算机视觉技术领域。该方法主要是通过将YOLOv5目标检测算法与传统视觉SLAM前端相结合，通过深度相机采集环境图像信息，对采集到的RGB图像进行ORB特征提取的同时，对其进行目标检测，对检测出的物体分为动态物体和静态物体两类，筛选并剔除仅存在于动态物体框内的特征点。之后，计算相邻两帧之间匹配对的场景流模值，建立高斯混合模型，进一步分离场景中动态物体和静态物体，剔除剩余的动态特征点。剩余的静态特征点用于特征匹配并估计相机位姿。利用本方法在ORBSLAM2前端进行测试，大大提升了其在动态环境中的定位精度，且能够满足实时性要求。

Description

一种在视觉SLAM前端去除动态特征点的方法

技术领域

本发明涉及一种在视觉SLAM前端去除动态特征点的方法，属于计算机视觉与深度学习技术领域。

背景技术

同时定位与建图技术(Simultaneous Localization And Mapping,SLAM)是指机器人在没有任何先验知识的情况下，凭借自身携带的环境感知传感器完成对周围未知环境地图的构建。使用视觉传感器完成地图构建的技术称为视觉SLAM，因视觉传感器价格低，采集环境信息丰富，可实现自主定位于导航功能，使得其在相关领域备受关注，成为目前SLAM技术领域的研究热点。因此也涌现出一批诸如ORB-SLAM，RGBD-SLAM，OPENVSLAM等优秀的开源视觉SLAM算法。

传统的视觉SLAM算法大都基于静态环境假设，在实际环境中，往往因为环境中存在不断运动的物体如人或其他动物的走动，车辆的移动等，使得机器人位姿估计出现较大偏差，从而导致定位失败。针对上述传统视觉SLAM算法在实际动态环境中存在的不足，本发明提出一种面向动态环境，在视觉SLAM前端去除动态特征点的方法。使用去除动态特征点之后剩余特征点进行位姿估计，可以大大提高视觉SLAM系统在动态环境中的定位精度，以及它的鲁棒性和稳定性。

发明内容

本发明所要解决的技术问题是，提供一种在视觉SLAM前端去除动态特征点的方法，此方法应用在视觉SLAM系统中，可有效去除动态物体对位姿估计的影响，提高视觉SLAM在动态环境中的精确性和稳定性。

为解决上述技术问题，本发明采用的技术方案为：

一种在视觉SLAM前端去除动态特征点的方法，包括如下步骤：

Step1：使用深度相机获取彩色和深度图像，对采集到的RGB图像均匀化提取ORB特征点，并使用目标检测算法获取边界框检测结果；

Step2：将边界框检测结果分类，分为动态物体和静态物体并对其建立集合，筛选出仅存在于动态物体检测框内的特征点并剔除；

Step3：计算剩余特征点相邻帧间场景流，根据场景流计算结果筛选出剩余动态特征点并剔除；

Step4：利用剩下的静态特征点进行特征匹配，得到最佳匹配特征点并进行相机位姿估计，得到准确的相机运动结果，从而实现准确的面向动态场景下的视觉SLAM。

Step1中，均匀化提取ORB特征点的方法如下：首先构建图像金字塔模型，即对原始的RGB图像用高斯卷积核进行高斯模糊处理，然后对其进行降采样，依次将原始的RGB图像分成若干个层，在各个层上进行FAST角点的提取，然后计算图像金字塔每层需要提取的特征点数量N_α，公式如下：

式(1)中，N代表设置的各层的总特征点数量，α代表当前层数，s代表图像金字塔各层的图像的缩放因子，l代表图像金字塔总层数，其次对构建的图像金字塔每一层都均匀的划分出30×30像素的格子，单独对每个格子提取FAST角点，如果有格子提取不到FAST角点则降低FAST阈值，从而保证一些纹理较弱的区域也可以提取到FAST角点；最后对于提取到的大量FAST角点，使用基于四叉树的方法，均匀的选取N_α个FAST角点。

Step1中，目标检测算法包括以下内容：当获取到深度相机采集到的RGB图像后，使用YOLOv5网络模型对输入的RBG图像进行目标检测，在YOLOv5网络模型中新增Focus结构对RBG图像进行切片操作，输入一个3×608×608像素的RBG图像，经过切片操作后，变成12×304×304像素的特征图，最后使用32个卷积核进行一次卷积，变成32×304×304像素的特征图；YOLOv5网络模型在Backbone和Neck中使用了两种不同的CSP；在Backbone中，使用带有残差结构的CSP1_X，在Neck中使用CSP2_X，将YOLOv5网络模型的主干网络的输出分成了两个分支，后将其concat，使YOLOv5网络对输入图像的特征(包括边界，纹理等)的融合能力得到加强，使其输出带有边界框检测结果的边界框位置以及顶点坐标，以便于后续对提取的特征点进行分类。

在Backbone中，使用带有残差结构的CSP1_X，因为Backbone网络较深，残差结构的加入使得层和层之间进行反向传播时，梯度值得到增强，有效防止网络加深时所引起的梯度消失，得到的特征粒度更细；在Neck中使用CSP2_X，相对于单纯的CBL将主干网络的输出分成了两个分支，后将其concat，使网络对特征的融合能力得到加强，保留了更丰富的特征信息。将YOLOv5模型移植到嵌入式设备，使其输出带有检测结果的边界框位置以及顶点坐标，以便于后续对提取的特征点进行分类。

Step2中，剔除动态特征点的方法如下：

将边界框检测结果按照动态物体和静态物体分类，假设输入YOLOv5网络模型的第N帧RGB图像的动态物体像素点集合为：

静态物体像素点集合为

其中

为第N帧图像中的动态物体检测框，即边界框内的第i个像素点，

为第N帧图像中的静态物体检测框内的第j个像素点；第N帧图像中提取到的特征点(即使用特征提取方法之后提取到的具有明显特征的像素点)集合为

其中

为第N帧图像中的第k个特征点；若

且

则判断该特征点为动态特征点并从集合

中删除。

Step3中，剩余特征点相邻帧间场景流的计算方法如下：

对相邻两帧(普通帧)图像，根据Step1方法提取出特征点，并进行特征匹配，记录匹配成功的特征点并标记该特征点在前后两帧图像中的位置；

根据匹配的前后两帧RGB图像中特征点位置，在depth图中找到同一特征点的深度值，记当前帧特征点三维坐标为(x_c，y_c，z_c)，与之匹配的前一帧图像特征点三维坐标为(x_p，y_p，z_p)，则特征点在当前帧与上一帧的三维位置变化即场景流为：

Step3中，根据场景流计算结果筛选出剩余动态特征点并剔除的方法如下：

依据上述计算得到的匹配成功的特征点对场景流计算其模值c和角度θ，利用高斯混合模型的方法得到各特征点的高斯值G_p，将高斯值G_p大于一定阈值G_o的特征点记为动态特征点，计入集合D_f＝{p|G_p＞G₀}，p代表G_p＞G_o的特征点，G_o计算公式如下：

式(3)中：n为特征点个数；H_k为使用加权平均法对每一个特征点构造的评价指标，计算公式为：

式(4)中：c_k为第k个特征点对应的场景流模值，

θ_k为第k个特征点对应的场景流角度，

根据上述内容，可得特征点p的状态为：

即对于特征点p，若其高斯值G_p＞G_o，则判断其为动态特征点，计入集合D_f＝{p|G_p＞G₀}并剔除，反之则为静态特征点计入集合S_f＝{p|G_p＜G₀}。

Step4中，特征匹配与位姿估计的方法如下：基于上述提取到的静态特征点，对相邻关键帧间特征点做特征匹配；在特征匹配中，使用RANSAC算法对错误数据进行过滤；RANSAC算法中评估的模型就是帧间的位姿估计，在位姿估计求解时，通过不断地将错误匹配设置为局外点，配合位姿求解算法，得到最优的位姿估计。

关键帧是经过筛选之后的、代表其附近帧的特殊帧，可以减少信息冗余度。

此关键帧内的特征点包含了其附近帧超过75％的特征点。附近帧为关键帧附近的帧。

动态物体包括人、猫、狗。

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

(1)本发明将YOLOv5目标检测算法融入到视觉SLAM前端，检测场景中动态目标并剔除动态特征点，相对于现有主流的使用像素级语义分割方法如DS-SLAM、Dyna-SLAM，速度方面得到了大幅提升；

(2)在提取动态特征点方面，将目标检测得到的动态特征点与静态特征点取差集，然后再利用计算特征点场景流的方式进一步提取特征点。此方法不仅能够精确地提取到场景中的动态特征点还能够更好的保留住较多的高质量静态特征点用于位姿估计；

(3)相对于传统视觉SLAM，本发明中所述的方法更适用于实际的场景，在动态场景下定位精度得到大幅提升。

本发明提供了一种将基于深度学习的目标检测算法用于视觉SLAM前端去除动态特征点的方法，属于计算机视觉技术领域。该方法主要是通过将YOLOv5目标检测算法与传统视觉SLAM前端相结合，通过深度相机采集环境图像信息，对采集到的RGB图像进行ORB特征提取的同时，对其进行目标检测，对检测出的物体分为动态物体和静态物体两类，筛选并剔除仅存在于动态物体框内的特征点。之后，计算相邻两帧之间匹配对的场景流模值，建立高斯混合模型，进一步分离场景中动态物体和静态物体，剔除剩余的动态特征点。剩余的静态特征点用于特征匹配并估计相机位姿。利用本方法在ORBSLAM2前端进行测试，大大提升了其在动态环境中的定位精度，且能够满足实时性要求。

附图说明

图1为本发明在视觉SLAM前端去除动态特征点方法的流程图；

图2为本发明剔除动态特征点实例图；

图3为fr3_walking_xyz序列下本发明实例定位效果与ORB-SLAM2的定位效果对比图，其中，图(a)为本发明定位效果图，图(b)为ORB-SLAM2的定位效果图；

图4为fr1_xyz序列下本发明实例定位效果与ORB-SLAM2的定位效果对比图，其中，图(c)为本发明定位效果图，图(d)为ORB-SLAM2的定位效果图。

具体实施方式

下面结合附图对本发明作更进一步的说明。

如图1所示，一种在视觉SLAM前端去除动态特征点的方法，

Step1：使用RGB-D相机采集图像，对采集到的RGB图像均匀化地提取ORB特征点并同时使用目标检测算法获取边界框检测结果，具体过程如下：

构建图像金字塔，保证ORB特征点尺度不变性。计算金字塔每层需要提取的特征点数量N_α，公式如下：

其中N代表设置的总特征点数量，α代表当前层数，s代表缩放因子，l代表金字塔总层数。对构建的图像金字塔每一层都均匀的划分出30*30pixels的小格子，单独对每个格子提取FAST角点，如果有格子提取不到角点则降低FAST阈值，这样可以保证一些纹理较弱的区域也可以提取到FAST角点。根据Oriented FAST计算出的角度，选取最优的256个点对，并把这256个点对的坐标旋转之后，再取灰度，增加BRIEF描述子的旋转不变性。最后对于提取到的大量FAST角点，使用基于四叉树的方法，均匀的选取N_α个角点。

当获取到深度相机采集到的图像数据后，使用YOLOv5网络模型对输入的RBG图像进行目标检测。在YOLOv5中新增了Focus结构对图片进行切片操作，输入一个3×608×608的图片，经过切片操作后，变成12×304×304的特征图，最后使用32个卷积核进行一次卷积，变成32×304×304的特征图。YOLOv5在Backbone和Neck中使用了两种不同的CSP；在Backbone中，使用带有残差结构的CSP1_X，因为Backbone网络较深，残差结构的加入使得层和层之间进行反向传播时，梯度值得到增强，有效防止网络加深时所引起的梯度消失，得到的特征粒度更细；在Neck中使用CSP2_X，相对于单纯的CBL将主干网络的输出分成了两个分支，后将其concat，使网络对特征的融合能力得到加强，保留了更丰富的特征信息。将YOLOv5模型移植到嵌入式设备，使其输出带有检测结果的边界框位置以及顶点坐标，以便于后续对提取的特征点进行分类。

Step2：将目标检测结果分类，分为动态物体和静态物体并对其建立集合，筛选出仅存在于动态物体检测框内的特征点并剔除，具体过程如下：

根据coco数据集标注方式，将目标检测结果中的people，cat，dog这三类结果归为动态物体，其余结果作为静态物体。假设输入的第N帧图像的动态物体像素点集合为：

静态物体像素点集合为

其中

为第N帧图像中的动态物体检测框内的第i个像素点，

为第N帧图像中的精态物体检测框内的第j个像素点。第N帧图像中提取到的特征点集合为

其中

为第N帧图像中的第k个特征点。若

且

则判断该特征点为动态特征点并从集合

中删除。

Step3：计算剩余特征点相邻帧间场景流，根据场景流计算结果帅选出剩余动态特征点并剔除，具体过程如下：

对相邻两帧图像，根据Step1方法提取出特征点，并进行特征匹配，记录匹配成功的特征点并标记该特征点在前后两帧图像中的位置。

根据匹配的前后两帧RGB图像中特征点位置，在depth图中找到同一特征点的深度值，记当前帧特征点三维坐标为(x_c，y_c，z_c)，与之匹配的前一帧特征点三维坐标为(x_p，y_p，z_p)，则特征点在当前帧与上一帧的三维位置变化即场景流为：

依据上述计算得到的特征点对场景流计算其模值c和角度θ，利用高斯混合模型的方法得到各特征点的高斯值G_p，将高斯值大于一定阈值的特征点记为动态特征点，计入集合D_f＝{p|G_p＞G₀}，其中G₀表示阈值，计算公式如下：

式中：n为特征点个数；H_k为使用加权平均法对每一个特征点构造的评价指标，计算公式为：

式中：c_k为第k个特征点对应的场景流模值，θ_k为第k个特征点对应的场景流角度。

根据上述内容，可得特征点p的状态为：

即对于特征点p，若其高斯值G_p＞G₀，则判断其为动态特征点，计入集合D_f＝{p|G_p＞G₀}并剔除，反之则为静态特征点计入集合S_f＝{p|G_p＜G₀}。

Step4：利用剩下的静态特征点进行特征匹配，得到最佳匹配特征点并进行相机位姿估计，得到准确的相机运动结果，从而实现准确的面向动态场景下的视觉SLAM，具体过程如下：

基于上述提取到的静态特征点，对相邻关键帧间特征点做特征匹配。在特征匹配中，存在一定的相似的特征点，极易产生误匹配，为了很好的去除误匹配，使用RANSAC算法对错误数据进行过滤。RANSAC算法中需要评估的模型就是帧间的位姿估计。在位姿估计求解时，通过不断地将错误匹配设置为局外点，配合位姿求解算法，得到最优的位姿估计，从而得到准确的相机运动结果，实现准确的面向动态场景下的视觉SLAM。

基于上述实施步骤，以TUM数据集作为具体实施例(如附图2所示)给予说明：

Step1：当获取到RGB-D相机采集到的RGB图像后，对其均匀化的提取ORB特征点并使用YOLOv5目标检测算法检测图中动态目标。视觉SLAM系统中加载的YOLOv5目标检测模型是基于ms coco数据集训练，该目标检测器可以快速准确的检测到80个类别，并且可以实时获取到高质量的检测结果。在实施例中将people这一类别设为动态目标，其他为静态目标，剔除动态目标框内特征点如附图2所示。

Step2：对于未去除掉的动态特征点，如附图2中方框内的特征点，通过剩余特征点对场景流计算其模值c和角度θ，利用高斯混合模型的方法得到各特征点的高斯值G_p，将高斯值大于一定阈值的特征点记为动态特征点并剔除。

Step3：基于上述提取到的静态特征点，对相邻关键帧间特征点做特征匹配。在特征匹配中，存在一定的相似的特征点，极易产生误匹配，为了很好的去除误匹配，使用RANSAC算法对错误数据进行过滤。RANSAC算法中需要评估的模型就是帧间的位姿估计。在位姿估计求解时，通过不断地将错误匹配设置为局外点，配合位姿求解算法，得到最优的位姿估计，从而得到准确的相机运动结果，实现准确的面向动态场景下的视觉SLAM。

在本实施例中，使用绝对轨迹误差(Absolute Trajectory Error，ATE)作为视觉SLAM定位精度的评价指标。绝对轨迹误差指的是估计得到的相机轨迹与真实轨迹值之间的均方根误差值，将数据集的真值序列记为G_1，G_2…G_n∈SE(3)，算法对相机轨迹预测值序列记为P_1，P_2…P_n∈SE(3)，序列长度记为n，计算公式为：

将本发明方法与现有的主流视觉SLAM算法ORB-SLAM2在TUM fr3_walking_xyz和fr3_xyz数据集序列下做对比，如图3和图4所示。其中fr3_walking_xyz是RGB-D相机录制的动态场景序列，fr3_xyz是RGB-D录制的静态场景序列。对比结果如附图3和附图4所示，表1为量化的对比结果。通过量化的对比结果表明，改进后的算法在动态环境中具有很好的适应性，相较于ORB_SLAM2算法定位精度有了明显的大幅度提升，而在静态环境中，改进后的算法与ORB_SLAM2表现几乎没有差别。

表1绝对轨迹误差对比结果

应当理解，为了精简本公开并帮助理解各个发明方面中的一个或多个，在上面对本发明的示例性实施例的描述中，本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多特征。更确切地说，如权利要求书所反映的那样，发明方面在于少于前面公开的实施例的所有特征。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本发明的单独实施例。

尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。此外，应当注意，本说明书中使用的语言主要是为了可读性和教导的目的而选择的，而不是为了解释或者限定本发明的主题而选择的。因此，在不偏离所附权利要求书的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的，本发明的范围由所附权利要求书限定。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (8)

1.一种在视觉SLAM前端去除动态特征点的方法，其特征在于，包括如下步骤：

Step1：使用深度相机获取彩色和深度图像，对采集到的RGB图像均匀化提取ORB特征点，并使用目标检测算法获取边界框检测结果；

Step2：将边界框检测结果分类，分为动态物体和静态物体并对其建立集合，筛选出仅存在于动态物体检测框内的特征点并剔除；

Step3：计算剩余特征点相邻帧间场景流，根据场景流计算结果筛选出剩余动态特征点并剔除；

Step4：利用剩下的静态特征点进行特征匹配，得到最佳匹配特征点并进行相机位姿估计，得到准确的相机运动结果，从而实现准确的面向动态场景下的视觉SLAM。

2.根据权利要求1所述的一种在视觉SLAM前端去除动态特征点的方法，其特征在于，Step1中，均匀化提取ORB特征点的方法如下：首先构建图像金字塔模型，即对原始的RGB图像用高斯卷积核进行高斯模糊处理，然后对其进行降采样，依次将原始的RGB图像分成若干个层，在各个层上进行FAST角点的提取，然后计算图像金字塔每层需要提取的特征点数量N_α，公式如下：

式(1)中，N代表设置的各层的总特征点数量，α代表当前层数，s代表图像金字塔各层的图像的缩放因子，l代表图像金字塔总层数，其次对构建的图像金字塔每一层都均匀的划分出30×30像素的格子，单独对每个格子提取FAST角点，如果有格子提取不到FAST角点则降低FAST阈值，从而保证一些纹理较弱的区域也可以提取到FAST角点；最后对于提取到的大量FAST角点，使用基于四叉树的方法，均匀的选取N_α个FAST角点。

3.根据权利要求1所述的一种在视觉SLAM前端去除动态特征点的方法，其特征在于，Step1中，目标检测算法包括以下内容：当获取到深度相机采集到的RGB图像后，使用YOLOv5网络模型对输入的RBG图像进行目标检测，在YOLOv5网络模型中新增Focus结构对RBG图像进行切片操作，输入一个3×608×608像素的RBG图像，经过切片操作后，变成12×304×304像素的特征图，最后使用32个卷积核进行一次卷积，变成32×304×304像素的特征图；YOLOv5网络模型在Backbone和Neck中使用了两种不同的CSP；在Backbone中，使用带有残差结构的CSP1_X，在Neck中使用CSP2_X，将YOLOv5网络模型的主干网络的输出分成了两个分支，后将其concat，使YOLOv5网络对输入图像的特征的融合能力得到加强，使其输出带有边界框检测结果的边界框位置以及顶点坐标，以便于后续对提取的特征点进行分类。

4.根据权利要求1所述的一种在视觉SLAM前端去除动态特征点的方法，其特征在于，Step2中，剔除动态特征点的方法如下：

将边界框检测结果按照动态物体和静态物体分类，假设输入YOLOv5网络模型的第N帧RGB图像的动态物体像素点集合为：

静态物体像素点集合为

其中

为第N帧图像中的动态物体检测框，即边界框内的第i个像素点，

为第N帧图像中的静态物体检测框内的第j个像素点；第N帧图像中提取到的特征点集合为

其中

为第N帧图像中的第k个特征点；若

且

则判断该特征点为动态特征点并从集合

中删除。

5.根据权利要求1所述的一种在视觉SLAM前端去除动态特征点的方法，其特征在于，Step3中，剩余特征点相邻帧间场景流的计算方法如下：

对相邻两帧图像，根据Step1方法提取出特征点，并进行特征匹配，记录匹配成功的特征点并标记该特征点在前后两帧图像中的位置；

根据匹配的前后两帧RGB图像中特征点位置，在depth图中找到同一特征点的深度值，记当前帧特征点三维坐标为(x_c，y_c，z_c)，与之匹配的前一帧图像特征点三维坐标为(x_p，y_p，z_p)，则特征点在当前帧与上一帧的三维位置变化即场景流为：

6.根据权利要求5所述的一种在视觉SLAM前端去除动态特征点的方法，其特征在于，Step3中，根据场景流计算结果筛选出剩余动态特征点并剔除的方法如下：

依据上述计算得到的匹配成功的特征点对场景流计算其模值c和角度θ，利用高斯混合模型的方法得到各特征点的高斯值G_p，将高斯值G_p大于一定阈值G_o的特征点记为动态特征点，计入集合D_f＝{p|G_p＞G₀}，p代表G_p＞G_o的特征点，G_o计算公式如下：

式(3)中：n为特征点个数；H_k为使用加权平均法对每一个特征点构造的评价指标，计算公式为：

式(4)中：c_k为第k个特征点对应的场景流模值，

θ_k为第k个特征点对应的场景流角度，

根据上述内容，可得特征点p的状态为：

即对于特征点p，若其高斯值G_p＞G_o，则判断其为动态特征点，计入集合D_f＝{p|G_p＞G₀}并剔除，反之则为静态特征点计入集合S_f＝{p|G_p＜G₀}。

7.根据权利要求6所述的一种在视觉SLAM前端去除动态特征点的方法，其特征在于，Step4中，特征匹配与位姿估计的方法如下：基于上述提取到的静态特征点，对相邻关键帧间特征点做特征匹配；在特征匹配中，使用RANSAC算法对错误数据进行过滤；RANSAC算法中评估的模型就是帧间的位姿估计，在位姿估计求解时，通过不断地将错误匹配设置为局外点，配合位姿求解算法，得到最优的位姿估计。

8.根据权利要求4所述的一种在视觉SLAM前端去除动态特征点的方法，其特征在于，动态物体包括人、猫、狗。

Images