一种融合深度语义和时空一致性约束的动态场景视觉SLAM方法、系统及设备

本发明涉及动态场景视觉同时定位与建图，具体涉及一种融合深度语义和时空一致性约束的动态场景视觉slam方法、系统及设备。

背景技术：

1、同时定位与建图（simultaneous localization and mapping，slam）是搭载特定传感器的主体在缺乏环境先验信息的情况下，通过运动过程中的传感器观测估计自身位姿，同时构建环境模型的技术，是自主移动机器人、无人机、ar/vr等领域的关键基础。视觉slam基于相机传感器实现，具备成本低、体积小、环境信息丰富等优势，成为当前slam技术的主流研究方向。

2、目前主流的视觉slam算法均基于静态环境假设设计，而实际室内外场景中普遍存在移动行人、车辆、可移动物体等动态干扰因素，会导致特征匹配出现偏差，进而引发位姿估计误差累积、定位漂移、建图失真等问题，严重限制了视觉slam在真实动态场景中的应用。

3、现有动态场景视觉slam方案主要分为基于几何约束的传统动态剔除方法与基于深度学习的语义辅助动态剔除方法两类。

4、基于几何约束的方案中，经典orb-slam3算法采用随机样本一致性方法剔除误匹配点，以降低动态物体对特征匹配的干扰，但在高动态场景下无法有效区分动态特征点与误匹配点，定位精度提升十分有限；另有方案通过三角剖分构建特征点拓扑关系、融合点线特征结合场景结构化约束筛选静态特征点，但普遍存在计算复杂度高、实时性差、依赖场景结构化特性、泛化能力弱等缺陷，难以适配复杂多变的真实动态场景。

5、基于深度学习语义辅助的方案通过语义分割或目标检测提供动态物体先验信息，例如部分方案采用segnet网络分割动态行人，或结合实例分割网络与多视图几何筛选动态特征点，但此类方案普遍存在多处缺陷：一是语义分割网络结构复杂、计算量大，严重影响算法运行速度，难以满足实时定位需求；二是语义掩码边缘分割精度低，易出现过分割或漏分割问题，导致静态特征点被误删或动态特征点漏检；三是仅能检测预定义类别的动态物体，无法识别被移动的家具等非预设潜在动态物体；四是后续几何约束多依赖人工预设的固定阈值，在不同动态场景下适配性差、鲁棒性不足，无法同时保证动态场景下的定位精度与运行效率。

技术实现思路

1、为了克服上述现有技术存在的缺陷，本发明的目的在于提供一种融合深度语义和时空一致性约束的动态场景视觉slam方法、系统及设备，在经典orb-slam3算法的基础上，新增语义线程与跟踪线程的双线程并行处理，添加动态特征剔除模块，通过深度信息修正语义掩码边缘，结合多帧记忆的时间连续性约束修正单帧语义误差，采用自适应阈值的空间一致性检验剔除潜在动态特征点，实现动态场景下的高精度鲁棒定位，同时兼顾算法实时性。

2、为了达到上述目的，本发明采取的技术方案为：

3、一种融合深度语义和时空一致性约束的动态场景视觉slam方法，具体包括以下步骤：

4、步骤一、采集rgb图像和深度图像进行双线程并行处理在跟踪线程中，对输入的rgb图像进行预处理，完成图像去噪、orb特征提取与帧间匹配；

5、在语义线程中，采用轻量级语义分割网络bisenet v2对rgb图像进行像素级语义分割，得到动态物体初始语义掩码，结合深度图像的深度梯度信息，对初始语义掩码的边缘进行局部膨胀修正，得到修正后的语义掩码，并将修正后的掩码图像传递至跟踪线程，完成动静态特征点的初始标记；

6、步骤二、在跟踪线程中，基于修正后的语义掩码，构建特征点动态标记的多帧历史缓存队列，根据时间连续性采用加权策略计算加权动态历史占比，修正单帧语义分割的漏检与误判，完成动态特征点粗筛，得到初始静态特征点集；

7、步骤三、基于初始静态特征点集计算当前帧的相机初始位姿，融合光流跟踪误差和重投影误差生成空间一致性融合误差，采用k-means聚类算法自适应生成划分阈值，剔除剩余潜在动态特征点，得到纯净静态特征点集；

8、步骤四、基于纯净静态特征点集进行相机位姿优化、局部建图与回环检测，输出动态场景下的相机定位轨迹与环境稠密点云地图。

9、所述步骤一，语义线程的具体实现方法如下：

10、采用轻量级语义分割网络bisenet v2对rgb图像进行分割，选择室内场景中出现频率高于设定阈值的真实动态类作为分割目标，输出动态区域像素为255、静态区域像素为0的初始语义掩码；

11、对深度图像进行中值滤波预处理以消除椒盐噪声；采用sobel算子计算滤波后深度图像的深度梯度幅值，量化像素点的深度突变程度；

12、提取初始语义掩码的边缘像素，构建非重叠子窗口，计算窗口内边缘像素的深度梯度均值；设置梯度阈值，对深度突变不显著的窗口进行局部膨胀修正，重复校验直至满足梯度阈值或达到最大膨胀次数，生成修正后的语义掩码。

13、所述中值滤波以每个像素为中心构建3×3邻域，用邻域内深度值的中值替代中心像素的原始深度值；

14、所述非重叠子窗口的尺寸为30×30像素；

15、所述真实动态类包括人、猫、狗。

16、所述步骤二的具体方法为：

17、为每个orb特征点构建长度为5的动态标记历史缓存队列，记录当前帧与前4帧的动静态标记结果，标记为1代表该特征点在该帧为动态，标记为0代表静态；

18、采用指数加权策略设计权重向量，帧间隔越小权重越高，对权重进行归一化处理后，计算每个特征点的加权动态历史占比；

19、设置动态占比阈值，当加权动态历史占比高于阈值时判定为动态点，低于阈值时判定为静态点，完成动态特征点粗筛；

20、每帧处理完成后更新缓存队列，移除最旧帧标记，加入下一帧初始标记。

21、所述指数加权策略的衰减系数为0.8；

22、所述动态占比阈值为0.5。

23、所述步骤三的具体方法为：

24、采用金字塔lk光流法实现特征点的帧间跟踪，计算每个特征点的光流跟踪误差；

25、基于初始位姿计算特征点对应的3d地图点的重投影误差；

26、设置误差平衡系数，融合光流跟踪误差与重投影误差生成空间一致性融合误差；

27、将特征点按先验标记划分为动态点集合与静态点集合，对两个集合的融合误差分别进行k-means二类聚类，生成自适应划分阈值；

28、基于阈值筛选被误标记的静态点，剔除误差大于阈值的潜在动态点，得到纯净静态特征点集。

29、所述误差平衡系数为0.5；

30、所述光流跟踪误差为特征点真实坐标与光流跟踪坐标的欧氏距离；

31、所述重投影误差为光流观测坐标与将3d地图点投影到当前帧所得预测坐标的偏差。

32、所述步骤四中采用绝对轨迹误差ate与相对位姿误差rpe作为算法性能的评估指标，所述ate和rpe均通过均方根误差rmse、平均值mean和标准差s.d.进行统计度量；

33、绝对轨迹误差用于衡量估计轨迹与真实轨迹的全局偏差，其均方根误差计算公式为：

34、

35、其中，为轨迹的总帧数，为第帧的真实位姿矩阵，为第帧的估计位姿矩阵，ate越小，表示该slam算法的估计轨迹与真实轨迹越接近，其全局定位精度越高；

36、相对位姿误差用于衡量相邻帧之间的局部位姿偏差，分为平移相对误差与旋转相对误差，其均方根误差计算公式为：

37、

38、

39、其中，为用于计算相对误差的相邻帧对数，、为第组相邻帧的真实相对平移向量和估计相对平移向量，、为第组相邻帧的真实相对旋转矩阵和估计相对旋转矩阵，、分别为平移和旋转相对位姿误差的均方根误差；rpe各分量数值越小，表示该slam算法估计的相对位姿与真实相对位姿越接近，局部位姿估计越稳定。

40、一种融合深度语义和时空一致性约束的动态场景视觉slam系统，包括：

41、数据预处理模块，用于在跟踪线程中对rgb图像进行去噪预处理、orb特征提取与帧间匹配；

42、语义分割与掩码修正模块，用于在语义线程中对rgb图像进行像素级语义分割，结合深度梯度信息完成掩码边缘修正，输出动静态特征点初始标记；

43、时间连续性约束模块，用于构建多帧动态标记缓存队列，通过加权动态历史占比修正单帧漏检与误判，完成动态特征点粗筛；

44、空间一致性检验模块，用于融合光流跟踪误差与重投影误差，通过聚类自适应划分动静态特征点，输出纯净静态特征点集；

45、定位建图模块，用于基于纯净静态特征点进行相机位姿优化、局部建图与回环检测，输出定位轨迹与稠密点云地图。

46、一种融合深度语义和时空一致性约束的动态场景视觉slam设备，包括：

47、图像采集器，用于采集动态场景的rgb图像与深度图像；

48、处理器，被配置为执行计算机程序时实现权利要求1至8中任一项所述的方法；

49、存储器，用于存储所述计算机程序；

50、显示器，用于显示相机定位轨迹与环境稠密建图结果；

51、以及一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1至8中任一项所述的方法。

52、与现有技术相比，本发明有益效果为：

53、1、本发明针对语义掩码边缘分割模糊的问题，提出基于深度梯度的窗口局部膨胀修正方法，利用动态物体与背景的深度突变特性，自适应修正掩码边缘，避免过分割与漏分割，提升动静态特征点初始标记的准确性。

54、2、本发明针对单帧语义分割易出现偶然漏检误判的问题，设计多帧记忆增强的时间连续性约束，通过指数加权的动态历史占比，利用运动的时间连续性修正单帧误差，降低遮挡、运动模糊对动态检测的干扰。

55、3、本发明针对潜在动态物体检测不足与固定阈值鲁棒性差的问题，提出自适应阈值的空间一致性检验，融合光流跟踪误差与重投影误差表征特征点动静态特性，通过k-means聚类实现阈值自适应划分，无需人工预设阈值，可有效检测语义未定义的潜在动态物体，同时避免误删有效静态特征点。

56、4、本发明采用轻量级bisenet v2网络与双线程并行架构，在保证高精度的同时兼顾实时性，单帧跟踪耗时仅38.3ms；在tum高动态数据集上，绝对轨迹误差最高降幅97.38%，相对位姿误差最高降幅95.16%，定位精度与鲁棒性显著优于现有主流动态slam算法。

57、综上，本发明在orb-slam3的基础架构上，通过深度语义掩码修正、时间连续性约束与自适应空间一致性检验的动态特征剔除模块，有效解决了动态场景下视觉slam定位漂移、建图失真的问题，同时兼顾算法实时性，可广泛应用于室内服务机器人、工业巡检机器人等真实动态场景的自主定位与导航。