一种动态环境下点线融合机器人SLAM方法及系统

本发明涉及机器人基于视觉的定位与导航，具体为一种基于目标检测网络的动态环境下点线融合机器人slam方法及系统。

背景技术：

1、在人工智能及大数据科技高速发展的今天，移动机器人正以智能化的形式进入人类的生活工作中。在未知环境中确定自身位置与姿态是移动机器人进行环境感知和动态决策的重要前提。通过对目前主流定位方法slam系统进行不断研究和改进，使得智能机器人更好的应用于制造、物流等复杂的工作环境。

2、slam技术目前主要是利用相机、惯导和激光雷达等传感器对环境进行感知。基于激光雷达的slam技术较为成熟，并且已经在无人驾驶和人工智能机器人等方面实现了商业化的应用。但由于其多线束规格的产品成本较高并且不具备颜色和纹理信息，这很大程度上提高了移动机器人的研究成本和信息处理难度。而视觉slam使用的相机传感器具有造价便宜、图像中纹理信息丰富等优点，使得移动机器人更加适用于在各个应用领域的研究和推广。目前视觉slam在真实环境中仍面临着一些实际性挑战。例如，在室外高亮环境下图像易出现无纹理信息和移动速度过快易出现图像模糊等现象，造成图像特征追踪丢失等问题，进而影响移动机器人的位姿轨迹估计准确度。

3、在目前的视觉slam定位算法研究中，针对视觉中的图像特征信息求解问题，大多数的视觉算法都是以假设图像场景中的物体是静态作为基本前提，并未考虑实际环境中存在的移动对象，例如车辆和行人。由于场景中的移动物体会对图像造成信息遮挡等现象，导致图像在特征点匹配中发生误匹配，进而对载体的位姿状态估计造成偏差。

技术实现思路

1、发明目的：为解决在动态场景下现有视觉slam定位算法存在检测精度和鲁棒性低等问题，本发明提出了一种基于目标检测网络的针对动态环境下点线融合机器人slam方法与系统，将目标检测网络、几何法与点线融合slam系统结合，使slam系统前端提取特征点的同时能够有效剔除动态特征，然后利用保留的静态特征进行图像帧间位姿状态求解，进而有效提升移动机器人基于特征信息的位姿估计准确度。

2、技术方案：一种动态环境下点线融合机器人slam方法，包括以下步骤：

3、步骤1：采集动态环境下的图像帧序列；

4、步骤2：对图像帧序列进行点特征提取，构建点特征对集合；对图像帧序列进行线特征提取，构建线特征对集合；

5、步骤3：对图像帧序列进行动态检测，得到图像中的动态区域，将对组成动态区域的点特征从点特征对集合中进行剔除；

6、步骤4：利用对极几何特性，检查点特征对集合中剩余的点特征是否为动态点特征，对动态点特征进行剔除；

7、步骤5：采用prosac算法对点特征对集合中剩余的静态点特征进行滤除操作，得到可用的点特征对；

8、步骤6：通过最小化图像光度误差，利用稀疏图像对齐算法实现对机器人位姿的初步估计；

9、步骤7：基于可用的点特征对和线特征对通过最小化重投影误差优化机器人位姿；

10、步骤8：通过局部建图进行点云处理，获得稀疏点云地图；

11、步骤9：利用回环检测优化机器人位姿。

12、进一步的，步骤2中，所述对图像帧序列进行点特征提取，构建点特征对集合，具体操作包括：

13、采用surf算法，获取图像帧序列的surf点特征；

14、对surf点特征进行高斯滤波；

15、对于任意一经过高斯滤波后的surf点特征，以自身为中心点，在周围取s×s的窗口，在该选定的窗口中选取n对点特征，对于选取的每个点特征按照式(1)进行计算：

16、

17、式中，p(x)表示点特征x＝(u1,v1)的像素值，p(y)表示点特征y＝(u2,v2)的像素值。

18、按照式(2)，得到每个点特征的二进制编码：

19、

20、取相邻两帧图像的点特征，基于每个点特征的二进制编码，计算得到两个点特征之间的最短与次短汉明距离；当两个点特征之间的汉明距离的比例小于比例阈值时，且两个点特征之间的汉明距离小于距离阈值时，两个点特征为匹配的点特征对，以此构建得到点特征对集合。

21、进一步的，步骤2中，所述对图像帧序列进行线特征提取，构建线特征对集合，具体操作包括：

22、对图像帧序列中的图像进行图像缩放；

23、采用lsd算法对缩放后的图像中的每个像素点的右下四个像素进行梯度计算，得到每个像素点的梯度幅值，取梯度幅值不小于梯度幅值阈值的像素点构建得到线段区域；

24、通过线段区域内像素点的梯度方向，确定构建的线段区域的梯度方向；

25、随机选取位于线段区域周围的一个孤立像素点，计算该孤立像素点的梯度方向，和该孤立像素点的梯度方向与线段区域的梯度方向之间的方向容忍值；

26、仅当该方向容忍值小于方向容忍值阈值时，将该孤立像素点纳入线段区域内，以此更新线段区域；

27、对最新的线段区域进行矩形近似计算，得到lsd线特征；

28、对lsd线特征计算lbd描述子并进行匹配，得到线特征对，以此构建得到线特征对集合。

29、进一步的，步骤3中，采用轻量级目标检测网络yolo-efficientnetv2对图像帧序列进行动态检测。

30、进一步的，步骤4中，所述利用对极几何特性，检查点特征对集合中剩余的点特征是否为动态点特征，具体操作包括：

31、从点特征对集中随机选出四对点特征对，求解得到基础矩阵f；计算点特征对集中其余点特征到基础矩阵f所对应极线的距离d，仅当距离d小于距离阈值时，判定该点特征为内点，记录此时被判定为内点的点特征数量；重复执行上述步骤n次后，取被判定为内点的点特征数量最多的一次，将该次求得的基础矩阵f作为最终的基础矩阵f；

32、基于最终的基础矩阵f，对点特征对集中所有的点特征，计算到最终的基础矩阵f所对应极线的距离，判定距离超过距离阈值的点特征为动态点特征。

33、进一步的，所述步骤5，具体包括：

34、按照式(7)，计算得到各点特征对的匹配质量：

35、

36、式中，γ为质量因子，β为每一对点特征对建立的欧式距离的比值，表示为：

37、

38、式中，dmin、dmin2分别为每一对点特征对建立的最小和次小欧式距离；

39、根据匹配质量对点特征对集合中的点特征对进行筛选，得到可用的点特征对。

40、进一步的，所述步骤6具体包括：

41、构建重投影误差模型，具体包括：

42、定义第i帧观测到的第m个点特征pm和第n条线特征ln的重投影误差分别表示为：

43、

44、

45、式中，k表示点的投影矩阵，kl表示线的投影矩阵，p'm和l'n分别表示与点pm匹配的点和与ln匹配的线段，表示当前位姿下的直线变换矩阵；

46、该重投影误差模型表示为：

47、

48、式中，ωp及ωl分别为点线的协方差矩阵；hp和hl分别代表点线的huber核函数；为待优化的变量；

49、huber核函数，表示为：

50、

51、式中，δ表示误差阈值；

52、采用列文伯格-马夸尔特算法对式(11)进行求解，得到第i帧的旋转矩阵和平移向量

53、通过第i-2帧和第i-1帧的旋转矩阵，得到第i帧的旋转矩阵，记为

54、

55、式中，表示第i-1帧的旋转矩阵，表示第i-2帧的旋转矩阵；根据式(14)，将旋转矩阵与旋转矩阵进行融合，得到第i帧的旋转矩阵

56、

57、得到第i帧机器人的位姿为位姿包括旋转矩阵和平移向量

58、本发明公开了一种动态环境下点线融合机器人slam系统，包括：

59、特征提取匹配模块，用于对图像帧序列进行点特征提取，构建点特征对集合；对图像帧序列进行线特征提取，构建线特征对集合；

60、点特征预处理模块，用于对图像帧序列进行动态检测，得到图像中的动态区域，将对组成动态区域的点特征从点特征对集合中进行剔除；

61、点特征对处理模块，用于采用prosac算法对点特征对集合中剩余的点特征进行滤除操作，得到可用的点特征对；

62、位姿初步估计模块，用于通过最小化图像光度误差，利用稀疏图像对齐算法实现对机器人位姿的初步估计；

63、位姿优化估计模块，用于基于可用的点特征对和线特征对，通过最小化重投影误差优化机器人位姿；

64、建图模块，用于通过局部建图进行点云处理，获得稀疏点云地图；

65、回环检测模块，用于利用回环检测优化机器人的位姿。

66、进一步的，所述点特征预处理模块中，还包括移动一致性检测模块；

67、所述移动一致性检测模块，用于利用对极几何特性，检查点特征对集合中的点特征是否为动态点特征，仅对动态点特征进行剔除。

68、有益效果：本发明与现有技术相比，具有以下优点：

69、(1)本发明通过点线特征和目标检测结合去除动态信息的方式，提高了视觉slam系统在动态环境下的运行的精度和效率；

70、(2)本发明采用s-orb算法进行点特征提取匹配，s-orb算法结合了surf算法鲁棒性好与orb算法速度快的优点，对slam系统中前端特征匹配的准确率与效率进行了优化；

71、(3)本发明采用点线特征结合的方式进行特征提取，使系统在弱纹理环境中传统点特征方法失效的情况下拥有较高的运行鲁棒性；

72、(4)本发明的目标检测模块采用轻量级目标检测网络yolo-efficientnetv2，该模型具有体积小与检测速度快的优点，有助于在本就复杂的动态slam系统上缓解运算量大的压力；

73、(5)本发明方法相比于其他的基于深度学习的面向动态环境下的slam方法，如基于语义分割的dynaslam等系统，其速率更快，能有效提升系统的实时性；

74、(6)本发明将经过滤除后的特征集合经过对极几何约束，可进一步减少动态特征对于位姿估计的影响。