CN112418288A - 一种基于gms和运动检测的动态视觉slam方法 - Google Patents

Abstract

本发明属于视觉空间定位技术领域，公开了一种基于GMS和运动检测的动态视觉SLAM方法，包括：结合GMS和滑动窗口对SLAM***进行初始化，得到初始地图；结合GMS实现SLAM***的跟踪定位；进行回环检测、全局优化；针对RGB‑D图像，构建静态点云地图；数据处理完成后输出位姿轨迹；针对RGB‑D图像，输出静态点云地图。本发明解决了现有技术中动态环境中的SLAM追踪定位效果较差的问题。本发明可以很好的剔除动态特征的影响并融合在SLAM***的各个功能模块中，解决动态场景中视觉SLAM定位和建图的问题，实时性好且定位精度较高。

Description

一种基于GMS和运动检测的动态视觉SLAM方法

技术领域

本发明涉及视觉空间定位技术领域，尤其涉及一种基于GMS和运动检测的动态视觉SLAM方法。

背景技术

即时定位与地图构建(SLAM)技术是智能移动机器人实现上述功能的核心技术。SLAM技术利用激光传感器或者视觉传感器获取的数据流，以增量的方式为当前环境动态构建地图模型，并在地图构建的过程中进行定位。

但是，特征点法和直接法都不能解决场景中常见的动态对象所带来的问题。当前大部分的视觉SLAM方法也依然都是基于观测环境是静态的假设，环境中的运动物体会形成很多错误的数据关联或者使之前跟踪到的地图点因为遮挡而丢失，对相机姿态的估计造成严重影响，导致整个视觉SLAM***定位错误，影响机器人后续的一系列任务。因此研究如何提升视觉SLAM在动态环境中的定位精度和建图效果具有重大意义。

近些年来，也有不少学者开始研究动态场景下的视觉SLAM问题。例如，基于前景背景初始化的方法主要优点是能够追踪暂时停止的动态物体，但是需要预先定义与背景或对象相关的信息，并且当环境中包含许多移动对象时，处理效果较差。基于几何约束的方法数学理论清晰，计算成本低，但是很难区分运动物体造成的残差和错误匹配造成的残差，并且需要预先知道相机的初始位姿。光流方法可处理的信息丰富，但基于亮度恒定性假设，对光照条件变化敏感。基于深度学习的方法整体效果最好，但参数调整复杂，且实时性无法得到保障。

发明内容

本发明通过提供一种基于GMS和运动检测的动态视觉SLAM方法，解决了现有技术中动态环境中的SLAM追踪定位效果较差的问题。

本发明提供一种基于GMS和运动检测的动态视觉SLAM方法，包括以下步骤：

步骤1、结合GMS和滑动窗口对SLAM***进行初始化，得到初始地图；

步骤2、结合GMS实现SLAM***的跟踪定位；

步骤3、进行回环检测、全局优化；针对RGB-D图像，构建静态点云地图；

步骤4、数据处理完成后输出位姿轨迹；针对RGB-D图像，输出静态点云地图。

优选的，所述步骤1中，单目图像对应的初始化操作包括：利用滑动窗口进行初始化关键帧的选取；采用GMS进行特征点的匹配；进行初始帧相机位姿的求解；通过特征三角化得到初始地图。

优选的，所述利用滑动窗口进行初始化关键帧选取的具体实现方式为：读取第一帧图像，对图像进行特征点提取并计算特征点数目，若特征点数目大于第一阈值，则确认为第一个初始化关键帧F₁；否则忽略该帧图像，对后续的图像进行特征提取和阈值验证直至符合阈值要求得到第一个初始化关键帧；在所述第一个初始化关键帧F₁的基础上建立滑动窗口，所述滑动窗口的最小窗口尺寸为n，最大窗口尺寸为m；读取下一个图像帧作为当前帧，如果当前帧的特征点数目大于所述第一阈值，则将当前帧添加至滑动窗口中，否则舍弃；重复上述操作，处理关键帧得到第n个初始化关键帧F_n，添加至窗口中达到最小尺寸n，之后继续处理添加关键帧最终得到第m个关键帧F_m，此时窗口达到最大尺寸m。

优选的，所述步骤1中，双目图像、RGB-D图像对应的初始化操作包括：利用滑动窗口进行初始化关键帧的选取；采用GMS进行特征点的匹配；通过特征三角化得到初始地图。

优选的，所述利用滑动窗口进行初始化关键帧选取的具体实现方式为：读取第一帧图像，对图像进行特征点提取并计算特征点数目，若特征点数目大于第二阈值，则确认为第一个初始化关键帧F₁；否则忽略该帧图像，对后续的图像进行特征提取和阈值验证直至符合阈值要求得到关键帧；在所述第一个初始化关键帧F₁的基础上建立滑动窗口，所述滑动窗口的最小窗口尺寸为n；读取下一个图像帧作为当前帧，如果当前帧的特征点数目大于所述第二阈值，则将当前帧添加至滑动窗口中，否则舍弃；重复上述操作，处理关键帧得到第n个初始化关键帧F_n，添加至窗口中达到最小尺寸n，之后继续处理添加关键帧最终得到第m个关键帧F_m，此时窗口达到最大尺寸m。

优选的，所述步骤2包括：

步骤2.1、单目、双目或RGB-D类型的任一种图像信息进入SLAM***中进行追踪；

步骤2.2、进行局部地图的追踪，完成追踪数据和局部地图数据的融合追踪，生成判别关键帧；

其中，所述步骤2.1具体包括以下子步骤：

步骤2.1.1、SLAM***进入恒速跟踪模型，利用前一帧的位姿变换作为当前帧的初始位姿，并将参考帧中的地图点投影至当前帧，完成3D-2D数据关联；

步骤2.1.2、进行3D-2D匹配检验得到最终地图点与特征点的匹配数目，如果匹配数目大于等于第三阈值，则直接跳转至步骤2.2；否则进入步骤2.1.3，进行参考帧追踪；

步骤2.1.3、SLAM***进入参考帧跟踪模型，利用前一帧的位姿变换作为当前帧的初始位姿，通过当前帧和参考关键帧的特征匹配，将参考关键帧中特征点对应的地图点关联到当前帧的匹配特征点上，采用GMS进行特征点的匹配，得到剔除动态特征点对，形成静态3D-2D的数据关联，并且使用BoW词袋法加速匹配，最后通过BA优化最小化重投影误差求解位姿；进行3D-2D匹配检验得到最终地图点与特征点的匹配数目，如果匹配数目大于等于第三阈值，则直接跳转至步骤2.2，否则进入步骤2.1.4，进行重定位；

步骤2.1.4、SLAM***进入重定位模型，通过BoW计算与当前帧具有共视关系的候选关键帧，使用GMS格网运动将当前帧与候选关键帧进行特征匹配，当匹配特征点大于第四阈值时，结合PNP和RANSAC算法估计当前帧的位姿，使用BA优化算法获得当前帧在局部地图上对应的地图点；如果当前帧的地图点数恢复大于第五阈值，则重定位成功。

优选的，所述步骤2.2具体包括以下子步骤：

步骤2.2.1、SLAM***进入局部地图跟踪模型，利用当前帧的初步位姿，建立局部地图进行跟踪；

步骤2.2.2、完成局部地图跟踪后，对当前帧进行判别，创建关键帧，对当前关键帧和参考关键帧进行GMS特征匹配得到当前帧的静态特征，进行三角化创建地图点。

优选的，所述步骤3中的所述构建静态点云地图包括：对特征进行判断筛选；集合深度图完成3D点的融合。

优选的，所述步骤3中，所述对特征进行判断筛选的具体实现方式为：

对关键帧进行运动检测，将参考关键帧和当前关键帧输入到训练完成的YOLO v3网络中分割出参考帧潜在运动区域S_ref和当前帧潜在运动区域S_cur，p_r为参考关键帧中的特征点，p_c为投影至当前关键帧后的像素点，且该投影点p_c对应的深度图中深度信息为Z，依据相机模型计算出p_c对应的投影深度值Z_proj，得到深度差值：

ΔZ＝Z_proj-Z

计算p_r和p_c与对应空间点p_w之间的夹角α，当α和ΔZ满足预设条件，则判断该像素点p_c为动态点，从当前帧中筛除该像素点；将当前关键帧中的所有点按照以上方法进行处理最终得到静态特征点集合。

优选的，所述步骤3中，所述集合深度图完成3D点的融合的具体实现方式为：

将每一个关键帧构造的单帧静态点云地图记为C_k，该关键帧对应时刻相机相对于世界坐标系的位姿记为T_k，将所有关键帧的点云转换到世界坐标系中，构建全局静态点云地图W，表示为：

其中，N表示关键帧集合的总数。

本发明中提供的一个或多个技术方案，至少具有如下技术效果或优点：

在发明中，首先，结合GMS和滑动窗口对SLAM***进行初始化，得到初始地图；然后，结合GMS实现SLAM***的跟踪定位；之后，进行回环检测、全局优化；针对RGB-D图像，构建静态点云地图；最后，数据处理完成后输出位姿轨迹；针对RGB-D图像，输出静态点云地图。即本发明提供的视觉SLAM方法结合了GMS(网格运动统计)和运动检测，相对于已经存在传统的处理方法，本发明可以很好的剔除动态特征的影响并融合在SLAM***的各个功能模块中，解决动态场景中视觉SLAM定位和建图的问题，实时性好且定位精度较高。此外，相对于传统的基于特征的SLAM***形成的地图多为稀疏地图，本发明基于RGB-D数据类型添加静态地图线程，形成的稠密地图相对于稀疏地图能够体现更多的环境特征也更能够体现环境的细节信息。

附图说明

图1为本发明实施例提供的一种基于GMS和运动检测的动态视觉SLAM方法的整体流程图。

具体实施方式

为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

本实施例提供了一种基于GMS和运动检测的动态视觉SLAM方法，包括以下步骤：

步骤1、结合GMS和滑动窗口对SLAM***进行初始化，得到初始地图；

步骤2、结合GMS实现SLAM***的跟踪定位；

步骤3、进行回环检测、全局优化；针对RGB-D图像，构建静态点云地图；

步骤4、数据处理完成后输出位姿轨迹；针对RGB-D图像，输出静态点云地图

下面对本发明做进一步的说明。

本实施例提供了一种基于GMS和运动检测的动态视觉SLAM方法，参见图1，具体包括以下步骤：

步骤1：结合GMS和滑动窗口对SLAM***进行初始化，得到初始地图。

即使用GMS和滑动窗口结合对SLAM***进行初始化操作，剔除运行场景中动态特征影响。

针对单目、双目和RGB-D不同数据类型完成相应的初始化操作，单目操作为步骤1.1，双目和RGB-D类型初始化直接跳转至步骤1.2。

步骤1.1：单目初始化主要分为四个阶段，第一阶段利用滑动窗口进行初始帧的选取；第二阶段采用GMS进行特征点的匹配；第三阶段进行初始帧相机位姿的求解；第四阶段特征三角化得到初始地图。

步骤1.1.1：利用滑动窗口进行初始化关键帧选取，首先读取第一帧图像，对图像进行特征点提取，并计算特征点数目，如果特征点数目大于第一阈值(例如100)，则确认为第一个初始化关键帧F₁，否则，读取下一帧图像直至得到符合条件的初始化关键帧；在第一个初始化关键帧F₁的基础上建立滑动窗口，滑动窗口的最小窗口尺寸为n，最大窗口尺寸为m。设置滑动窗口得到有一定时空间隔的初始关键帧F₁与关键帧F_n或F_m的目的是通过特征匹配并采用GMS稳健算法匹配剔除动态特征得到较为纯净的特征点，相比于在相邻关键帧上进行GMS处理动态误匹配点能够得到更加彻底的剔除效果。然后对之后的图像帧进行处理，读取下一个图像帧作为当前帧，如果当前帧的特征点数目大于第一阈值，则该当前帧就被添加进入滑动窗口中，否则舍弃；重复上述操作，处理关键帧得到第n个初始化关键帧F_n，添加至窗口中达到最小尺寸n，之后继续处理添加关键帧最终得到第m个关键帧F_m，此时窗口达到最大尺寸m。

步骤1.1.2：利用GMS匹配算法消除动态对象上的误匹配点对。在步骤1.1.1中对初始帧F₁和F_n提取的特征进行特征匹配得到匹配点集，将图像划分为G个格网(一般取20*20)。对第一个初始化关键帧F₁的每一个特征点X_i，该特征点对应的匹配点落在F_n匹配数目较多的格网中，且以该特征点以及匹配点网格为中心扩大区域建立一个3*3的网格邻域，记录网格邻域中对应的匹配点集为{a^k,b^k}，其中，k＝{0,1,2...9}，依据对应点集计算特征匹配的综合得分S_i，参考公式如下：

其中，K＝9，

是邻域{a^k,b^k}中的匹配点集，统计领域中匹配点的数目。利用综合得分S_i和阈值t比较来判断特征匹配的准确性。阈值t设为：

其中，α设为0.6，n为k(即为9)个邻域格网中每个格网的平均特征点数目。当得分S_i>t，则判断该特征点为正确匹配点，否则为错误匹配点。依据上述原理经初始关键帧之间的匹配点对进行删选，得到静态匹配点集合{p_c,p_r}。

步骤1.1.3：相机位姿求解。利用上一步骤中获取的静态匹配点集合{p_c,p_r}，分别计算本次运动的单应矩阵H_cr和基础矩阵F_cr，依据匹配点计算得到归一化平面的坐标{x_c,x_r}，对于单应矩阵H建立直接线性变换矩阵：

x_c＝H_crx_r

基于RANSAC使用归一化4点法算法求解出单应矩阵H。对于基础矩阵F，依据对极几何约束建立方程：

基于RANSAC，使用归一化8点算法求解基础矩阵。其中RANSAC算法在一定程度上可以剔除外点，同时针对单应矩阵H和基础矩阵F计算矩阵得分R_H：

S_H和S_F分别是单应矩阵和基础矩阵的模型评分，如果阈值R_H>0.45，使用H矩阵通过SVD分解计算相机位姿，否则使用F矩阵计算本质矩阵E，然后通过SVD分解本质矩阵E计算位姿。最后对位姿进行检查，求出最优解，如果初始化位姿不符合初始化要求，就取后面一帧和第一个初始化关键帧F₁作为初始帧重复以上的操作，直至滑动窗口最大尺寸m处的一帧F_m初始化仍然不成功，则将滑动窗口整体后移，将滑动窗口的第一帧作为第一个初始化关键帧F₁，继续以上的操作，直至得到初始位姿。

步骤1.1.4：特征三角化获取初始地图。依据归一化平面的坐标对{x_c,x_r}，存在的几何关系为：

其中，z_c,z_r为对应的相机坐标系下的Z轴坐标(即深度信息)，T_cw和T_rw分别为世界坐标系到当前关键帧和参考关键帧的位姿变换，P_w为对应的3D点坐标。对以上公式叉乘对应的归一化平面的点可得：

整理可得：

此时，对公式进行SVD求解最终得到最终的3D点坐标P_w，对{p_c,p_r}的匹配点对完成以上的三角化的操作后最终得到初始地图，完成初始化操作。

步骤1.2：双目/RGB-D初始化相对比较简单，也可分为三个阶段，第一阶段利用滑动窗口选取初始帧；第二阶段完成GMS特征筛选；第三阶段完成特征三角化得到初始地图。

步骤1.2.1：该步骤和步骤1.1.1类似，读取第一帧图像，对图像进行特征点提取并计算特征点数目，若特征点数目大于第二阈值(例如500)，则确认为第一初始化关键帧F₁，否则读取下一帧图像直至得到符合条件的初始化关键帧；在第一初始化关键帧F₁的基础上建立滑动窗口，最小窗口尺寸为n，最大窗口尺寸为m；对之后的图像帧进行处理，读取下一个图像帧作为当前帧，如果当前帧的帧特征数目大于第二阈值，则该当前帧就被添加进入滑动窗口中，否则舍弃；重复上述操作，处理关键帧得到第n个初始化关键帧F_n，添加至窗口中达到最小尺寸n，之后继续处理添加关键帧最终得到第m个关键帧F_m，此时窗口达到最大尺寸m。

步骤1.2.2：GMS筛选匹配点对，该步骤与步骤1.1.2一致最终得到原始帧F₁和F_n之间的匹配点对进行删选得到静态匹配点集合{p_c,p_r}。

步骤1.2.3：该阶段完成特征三角化。对于RGB-D类型比较简单，在F_n上利用正确的匹配点结合深度图恢复深度信息，创建静态初始地图。对于双目来说，F_n的正确匹配点进行双目三角化，对于双目左右帧之间的任一静态匹配点p_L(u_L,v_L)和p_R(u_R,v_R)，它们对应的3D点P，利用双目之间的几何关系如下：

其中，z代表P的深度信息，b代表双目的基线，f为相机的焦距，d为两帧图之间的视差。采用该方法进行处理最终可以获得初始地图信息。

步骤2：结合GMS实现SLAM***的跟踪定位。

当***初始化成功之后，进入跟踪线程，初始化关键帧F₁作为参考帧和局部地图中的第一帧关键帧，当接受到新的图像帧时，使用参考帧跟踪模型或者恒速跟踪模型估计相机位姿，然后更新局部关键帧和局部地图点，将局部地图点重投影至当前帧，建立图优化模型，进一步优化位姿。步骤2主要分为两个部分，首先为追踪和局部地图的创建，之后为***的回环检测和全局优化。

步骤2.1：该步骤中的追踪和建图实现沿袭了ORB-SLAM的基本思路。图像信息进入***中采用参考帧跟踪模型、恒速跟踪模型不同模式进行追踪。

步骤2.1.1：SLAM***首先进入恒速跟踪模型，利用前一帧的位姿变换作为当前帧的初始位姿ξ，并将参考帧中的地图点投影至当前帧完3D-2D数据关联，关系如下：

其中，李代数ξ^表示相机位姿，u_i为观测点像素坐标，s_i为尺度信息，K为内参矩阵，P_i为空间点3维坐标，ξ^*为需要优化的相机位姿。之后进行BA优化最小化重投影误差优化位姿。

步骤2.1.2：采用传统的ORB-SLAM***中的3D-2D匹配点对检验方法进行过滤剔除得到最终地图点与特征点的匹配数目matches Map，如果匹配数目大于等于第三阈值，例如匹配数目>＝10，则直接跳转至步骤2.2，否则进入步骤2.1.3，进行参考帧追踪。

步骤2.1.3：SLAM***进入参考帧追踪模块，同样以上一帧的位姿变换作为当前帧初始位姿，通过当前帧和参考关键帧的特征匹配，然后将参考关键帧中特征点对应的地图点关联到当前帧的匹配特征点上，之后采用GMS进行特征点的匹配，得到剔除动态特征点对，形成静态3D-2D的数据关联，并且使用BoW词袋法加速匹配，最后通过BA优化最小化重投影误差求解位姿。参考步骤2.1.2，进行3D-2D匹配检验，如果matches Map数目大于等于第三阈值，则直接跳转至步骤2.2，否则进入步骤2.1.4，进行重定位。

步骤2.1.4：SLAM***进入重定位模型。当场景中运动物体较多，静态特征点不足，或者相机移动过快时，姿态跟踪丢失，进入重定位模块。与ORB-SLAM2类似，本文通过BoW计算与当前帧具有共视关系的候选关键帧。然后使用GMS格网运动将当前帧与候选关键帧进行特征匹配。当匹配特征点大于第四阈值(例如30)时，结合PNP和RANSAC算法估计当前帧的位姿，使用BA优化算法获得当前帧在局部地图上对应的地图点。如果当前帧的地图点数恢复大于第五阈值(例如50)，则重定位成功，之后进入步骤2.2。

步骤2.2：完成帧间追踪后需要进行局部地图的追踪完成追踪数据和局部地图数据的融合追踪，之后判别关键帧的生成。

步骤2.2.1：在局部地图跟踪模型中，则是利用当前帧的初步位姿，建立局部地图进行跟踪。首先将观察到当前帧地图点的关键帧集合作为一级相连关键帧，和一级相连关键帧共视区域较高的帧以及父帧和子帧共同组成局部关键帧。然后将局部关键帧中的地图点作为局部地图点，把局部地图点中能被当前帧观测到的地图点重投影至当前帧，建立观测边更多的图优化模型，进一步优化当前帧位姿。

步骤2.2.2：关键帧的创建。完成局部地图跟踪后，会对当前帧进行判别，是否将当前帧封装为关键帧，判别采用的预设条件为：当前帧跟踪到的特征点数目和参考帧总特征点数的比值小于一定的阈值，局部建图线程中正在等待处理的关键帧数量不超过3个，且距离上次***关键帧最少间隔8帧；若不满足以上预设条件，则返回追踪线程进行下一帧图像的追踪，否则，创建关键帧，同时对当前关键帧和参考关键帧进行GMS特征匹配得到当前帧的静态特征进行三角化创建地图点，每一帧关键帧构造的单帧静态点云地图为C_k。

步骤3：进行回环检测、全局优化；针对RGB-D图像，构建静态点云地图。

步骤3.1：回环检测、全局优化。对于新的关键帧，仍然使用BoW计算所有关键帧的共视关系，并将具有共视关系但没有直接相连的关键帧作为候选关键帧。本文使用GMS算法对候选关键帧和当前关键帧进行特征匹配，并结合PNP和RANSAC算法来求解和优化位姿(单目相机位姿为7自由度，双目相机和RGB-D相机位姿为6自由度)。在确定闭环关键帧后，利用位姿图模型优化局部地图中所有关键帧的位姿。最后，使用BA算法对整个局部地图的所有关键帧和地图点进行优化。

步骤3.2：静态地图构建。步骤针对的是RGB-D数据集，分为两个阶段，首先对特征进行判断筛选，第二步集合深度图完成3D点的融合。

步骤3.2.1：对关键帧进行运动检测，将参考关键帧KF_ref和当前关键帧KF_cur输入到训练完成的YOLO v3网络中分割出参考关键帧潜在运动区域S_ref和当前关键帧S_cur，p_r为参考关键帧中的特征点，p_c为投影至当前帧后的像素点且该投影点对应深度图的深度信息为Z，依据相机模型可以计算出对应的投影深度值Z_proj，得到深度差值：

ΔZ＝Z_proj-Z

同时计算p_r和p_c与对应空间点p_w之间的夹角α，当α<＝30°且ΔZ＞T_z，其中T_z是深度阈值，一般设置为1，符合上式条件则判断该点p_c为动态点，从当前帧中筛除该点。将KF_cur中所有点按照以上方法进行处理最终得到静态特征点集合。

步骤3.2.2：每一帧关键帧构造的单帧静态点云地图为C_k，相对于世界坐标系的位姿为T_k，将所有帧的点云转换到世界坐标系中，构建全局静态点云地图W，即：

其中，N关键帧集合的总数，即代表全局地图的关键帧数目，最终拼接完成静态点云地图。

步骤4：数据处理完成后输出位姿轨迹；针对RGB-D图像，输出静态点云地图。

本发明实施例提供的一种基于GMS和运动检测的动态视觉SLAM方法至少包括如下技术效果：

(1)本发明利用GMS和运动检测相结合的方法能够可以很好地剔除动态特征的影响并融合在SLAM***的各个功能模块中，能够解决动态场景中视觉SLAM定位和建图的问题，实时性好且定位精度较高。

(2)相对于传统的基于特征的SLAM***形成的地图多为稀疏地图，本发明添加了基于RGB-D类型的稠密地图，形成的稠密地图可以相对于稀疏地图体现更多的环境特征也更能够体现环境的细节信息。

(3)本发明在初始化过程中加入滑动窗口机制，建立不含动态对象特征点的静态初始地图，相当于现有技术，由于相邻关键帧之间时间间隔和运动幅度较小，动态特征点很难识别，采用滑动窗口可以将有一定时间间隔的关键帧进行匹配能对动态点进行更好的判别剔除，得到更加彻底的静态特征点，因此提高了初始地图点的质量。

(4)本发明在初始化过程中添加了GMS，由于GMS可以在匹配的特征点对的基础上判别出动态特征点，因此可以更好地剔除运动信息。

(5)本发明利用YOLO v3网络可以分割出潜在运动区域并利用投影点深度以及旋转角度来判别潜在运动区域是都为动态区域，若为动态区域则剔除对应区域的特征点最终得到静态特征点，更好的剔除运动信息。

最后所应说明的是，以上具体实施方式仅用以说明本发明的技术方案而非限制，尽管参照实例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种基于GMS和运动检测的动态视觉SLAM方法，其特征在于，包括以下步骤：

步骤1、结合GMS和滑动窗口对SLAM***进行初始化，得到初始地图；

步骤2、结合GMS实现SLAM***的跟踪定位；

步骤3、进行回环检测、全局优化；针对RGB-D图像，构建静态点云地图；

步骤4、数据处理完成后输出位姿轨迹；针对RGB-D图像，输出静态点云地图。

2.根据权利要求1所述的基于GMS和运动检测的动态视觉SLAM方法，其特征在于，所述步骤1中，单目图像对应的初始化操作包括：利用滑动窗口进行初始化关键帧的选取；采用GMS进行特征点的匹配；进行初始帧相机位姿的求解；通过特征三角化得到初始地图。

3.根据权利要求2所述的基于GMS和运动检测的动态视觉SLAM方法，其特征在于，所述利用滑动窗口进行初始化关键帧选取的具体实现方式为：读取第一帧图像，对图像进行特征点提取并计算特征点数目，若特征点数目大于第一阈值，则确认为第一个初始化关键帧F₁；否则忽略该帧图像，对后续的图像进行特征提取和阈值验证直至符合阈值要求得到第一个初始化关键帧；在所述第一个初始化关键帧F₁的基础上建立滑动窗口，所述滑动窗口的最小窗口尺寸为n，最大窗口尺寸为m；读取下一个图像帧作为当前帧，如果当前帧的特征点数目大于所述第一阈值，则将当前帧添加至滑动窗口中，否则舍弃；重复上述操作，处理关键帧得到第n个初始化关键帧F_n，添加至窗口中达到最小尺寸n，之后继续处理添加关键帧最终得到第m个关键帧F_m，此时窗口达到最大尺寸m。

4.根据权利要求1所述的基于GMS和运动检测的动态视觉SLAM方法，其特征在于，所述步骤1中，双目图像、RGB-D图像对应的初始化操作包括：利用滑动窗口进行初始化关键帧的选取；采用GMS进行特征点的匹配；通过特征三角化得到初始地图。

5.根据权利要求4所述的基于GMS和运动检测的动态视觉SLAM方法，其特征在于，所述利用滑动窗口进行初始化关键帧选取的具体实现方式为：读取第一帧图像，对图像进行特征点提取并计算特征点数目，若特征点数目大于第二阈值，则确认为第一个初始化关键帧F₁；否则忽略该帧图像，对后续的图像进行特征提取和阈值验证直至符合阈值要求得到关键帧；在所述第一个初始化关键帧F₁的基础上建立滑动窗口，所述滑动窗口的最小窗口尺寸为n；读取下一个图像帧作为当前帧，如果当前帧的特征点数目大于所述第二阈值，则将当前帧添加至滑动窗口中，否则舍弃；重复上述操作，处理关键帧得到第n个初始化关键帧F_n，添加至窗口中达到最小尺寸n，之后继续处理添加关键帧最终得到第m个关键帧F_m，此时窗口达到最大尺寸m。

6.根据权利要求1所述的基于GMS和运动检测的动态视觉SLAM方法，其特征在于，所述步骤2包括：

步骤2.1、单目、双目或RGB-D类型的任一种图像信息进入SLAM***中进行追踪；

步骤2.2、进行局部地图的追踪，完成追踪数据和局部地图数据的融合追踪，生成判别关键帧；

其中，所述步骤2.1具体包括以下子步骤：

步骤2.1.1、SLAM***进入恒速跟踪模型，利用前一帧的位姿变换作为当前帧的初始位姿，并将参考帧中的地图点投影至当前帧，完成3D-2D数据关联；

步骤2.1.2、进行3D-2D匹配检验得到最终地图点与特征点的匹配数目，如果匹配数目大于等于第三阈值，则直接跳转至步骤2.2；否则进入步骤2.1.3，进行参考帧追踪；

步骤2.1.3、SLAM***进入参考帧跟踪模型，利用前一帧的位姿变换作为当前帧的初始位姿，通过当前帧和参考关键帧的特征匹配，将参考关键帧中特征点对应的地图点关联到当前帧的匹配特征点上，采用GMS进行特征点的匹配，得到剔除动态特征点对，形成静态3D-2D的数据关联，并且使用BoW词袋法加速匹配，最后通过BA优化最小化重投影误差求解位姿；进行3D-2D匹配检验得到最终地图点与特征点的匹配数目，如果匹配数目大于等于第三阈值，则直接跳转至步骤2.2，否则进入步骤2.1.4，进行重定位；

步骤2.1.4、SLAM***进入重定位模型，通过BoW计算与当前帧具有共视关系的候选关键帧，使用GMS格网运动将当前帧与候选关键帧进行特征匹配，当匹配特征点大于第四阈值时，结合PNP和RANSAC算法估计当前帧的位姿，使用BA优化算法获得当前帧在局部地图上对应的地图点；如果当前帧的地图点数恢复大于第五阈值，则重定位成功。

7.根据权利要求6所述的基于GMS和运动检测的动态视觉SLAM方法，其特征在于，所述步骤2.2具体包括以下子步骤：

步骤2.2.1、SLAM***进入局部地图跟踪模型，利用当前帧的初步位姿，建立局部地图进行跟踪；

步骤2.2.2、完成局部地图跟踪后，对当前帧进行判别，创建关键帧，对当前关键帧和参考关键帧进行GMS特征匹配得到当前帧的静态特征，进行三角化创建地图点。

8.根据权利要求1所述的基于GMS和运动检测的动态视觉SLAM方法，其特征在于，所述步骤3中的所述构建静态点云地图包括：对特征进行判断筛选；集合深度图完成3D点的融合。

9.根据权利要求8所述的基于GMS和运动检测的动态视觉SLAM方法，其特征在于，所述步骤3中，所述对特征进行判断筛选的具体实现方式为：

对关键帧进行运动检测，将参考关键帧和当前关键帧输入到训练完成的YOLO v3网络中分割出参考帧潜在运动区域S_ref和当前帧潜在运动区域S_cur，p_r为参考关键帧中的特征点，p_c为投影至当前关键帧后的像素点，且该投影点p_c对应的深度图中深度信息为Z，依据相机模型计算出p_c对应的投影深度值Z_proj，得到深度差值：

ΔZ＝Z_proj-Z

计算p_r和p_c与对应空间点p_w之间的夹角α，当α和ΔZ满足预设条件，则判断该像素点p_c为动态点，从当前帧中筛除该像素点；将当前关键帧中的所有点按照以上方法进行处理最终得到静态特征点集合。

10.根据权利要求8所述的基于GMS和运动检测的动态视觉SLAM方法，其特征在于，所述步骤3中，所述集合深度图完成3D点的融合的具体实现方式为：

将每一个关键帧构造的单帧静态点云地图记为C_k，该关键帧对应时刻相机相对于世界坐标系的位姿记为T_k，将所有关键帧的点云转换到世界坐标系中，构建全局静态点云地图W，表示为：

其中，N表示关键帧集合的总数。

Images