基于深度聚类的移动服务机器人动态环境定位与建图方法

本发明属于智能机器人自主导航领域，特别是一种基于深度聚类的移动服务机器人动态环境定位与建图方法。

背景技术：

1、移动服务机器人的同时定位与地图构建(simultaneous localization andmapping,slam)是实现机器人自主运动的关键技术。

2、目前大多数slam方法是在静态环境下进行的,比如mur-artal等人提出的orb-slam是一个基于特征点的slam系统，该系统拥有跟踪、建图和回环检测三个线程。2017年作者将原来的单目系统扩展到双目和rgb-d，称为orb-slam2，orb-slam系列算法是拥有较高精度与鲁棒性的slam系统之一，但仍然无法应对动态环境的干扰。然而真实环境往往是动态的，如果机器人运行中忽略了环境中的动态对象，很容易导致定位与建图产生较大误差。同时动态环境中的运动物体会降低slam系统的稳定性，甚至导致系统无法正常运行。而少数经过特殊设计，例如加入神经网络后可以适应动态环境的视觉slam系统，往往受限于算法本身计算复杂度与系统计算性能，实时性较差，难以满足实际场景中的需求。因此想要实现机器人在实际场景中的定位与建图，克服环境中动态物体的影响尤为重要。

3、cn113516664a，一种基于语义分割动态点的视觉slam方法，该专利采用mask r-cnn分割网络和多视图几何约束算法剔除动态特征点，提高系统在动态环境中的定位精度和鲁棒性，但mask r-cnn属于实例分割网络，其分割速度很慢，多视图几何法也非常耗时。

4、cn112435262a，一种基于语义分割网络和多视图几何的动态环境信息检测方法，该专利采用轻量级语义分割网络fchardnet和多视图几何约束算法剔除动态特征点，提高系统鲁棒性。但是，该方法中采用的语义分割网络分割处理速度仍然较慢，导致系统不满足实时性要求。

5、cn113066129a，一种基于动态环境下的目标检测的视觉定位与建图系统,借助了目标检测网络的目标检测框来筛选特征点，其速度较快，但是由于目标检测框只是对区域进行一个粗略的分割，因此也容易剔除掉静态特征点，导致系统的精度不如其他引入了语义分割与实例分割的系统。

6、cn115187614a，一种基于stdc语义分割网络的实时同时定位与建图方法，属于智能机器人自主导航领域。包括以下步骤：s1，利用rgb-d深度相机获取环境信息；s2，对输入图像信息进行预处理，利用orb算法提取特征点；s3，利用stdc语义分割网络获取图像语义信息，并利用语义信息剔除动态特征点；s4，将剩余特征点利用orb-slam3算法进行定位和导航。该方法中仅使用语义信息来确定动态特征点，容易导致误剔除。例如，如果该方法将人物划分为高动态目标，而实际上视野中人物处于静止状态，该方法仍然会其身上特征点剔除，但本发明利用几何信息克服了这个问题。同时该方法是通过语义信息生成掩膜的方法剔除特征点，本发明则不需要根据语义信息生成图像掩膜，直接对特征点进行聚类分析，实时性更好。

技术实现思路

1、本发明旨在解决以上现有技术的问题。提出了一种基于深度聚类的移动服务机器人动态环境定位与建图方法。本发明的技术方案如下：

2、一种基于深度聚类的移动服务机器人动态环境定位与建图方法，其包括以下步骤：

3、s1，利用rgb-d深度相机直接拍摄获得rgb图像信息和图像深度信息；

4、s2，对输入图像信息进行预处理，在提取特征点时对每个特征点赋予深度信息。先利用orb算法对rgb图像提取特征点，随后根据rgb-d相机模型借助深度图像对提取出来的特征点投影深度信息；

5、s3，利用yolo目标检测网络获取图像语义信息，并利用语义信息与几何信息获取动态物体所在动态区域；

6、s4，根据动态区域内特征点的深度信息对区域内进行聚类分析，以区分需剔除的动态特征点并剔除；

7、s5，将剩余特征点利用orb-slam2算法进行定位和导航，其中，所述orb-slam2是一个基于特征点的实时slam算法，该算法包括跟踪线程、局部建图线程和回环检测线程。

8、进一步的，所述步骤s2对输入图像信息进行预处理，在提取特征点时对每个特征点赋予深度信息；具体步骤为：

9、1、首先利用fast算法在图像中找到角点即关键点；

10、2、角点响应排序，即计算harris角点响应值，然后对检测到的角点按响应值排序，以便筛选出最显著的角点；

11、3、关键点方向分配：计算每个关键点的方向，使得描述子具有旋转不变性；

12、4、生成旋转不变brief描述子：使用方向修正的brief算法，从关键点周围的像素值生成旋转不变的描述子；

13、5、关键点和描述子匹配：将两个不同图像中的orb描述子进行匹配，找到相似的特征点；

14、6、根据相机模型为特征点分配深度值。

15、进一步的，在提取并筛选fast关键点后，利用图像块的灰度质心为该特征点添加旋转描述，定义图像块a的矩为：

16、

17、

18、式中：i(x,y)为图像像素点(x,y)处的灰度值，mx、my分别表示水平方向的矩和竖直方向的矩；

19、在这之后为处于像素点(x,y)处的特征点定义方向，并计算brief描述子，通过在特征点p(x,y)周围随机选取n个点对，比较点对内的灰度值：

20、

21、式中，a、b分别为点对a、b处的灰度值大小。由此可得brief描述子如下：

22、

23、式中：n表示该特征点第n位描述子。

24、进一步的，进行角点响应排序，即计算harris角点响应值，然后对检测到的角点按响应值排序；针对得到的每一个fast角点，先获取其在x和y方向上的梯度矩阵g，再通过下式计算harris响应值r：

25、r＝det(g)-k*(trace(g))2

26、其中，det(g)是矩阵g的行列式，trace(g)是矩阵g的迹，k则是一个经验参数。根据计算得到的harris响应值，对响应值进行降序排序。具有较大harris响应值的特征点将被保留。

27、进一步的，所述根据相机模型为特征点分配深度值，具体包括：每一个rgb图像像素点(x,y)对应的深度值depth就是深度图像中点(x,y)处的灰度值，于是图像中提取出来的特征点的空间位置可以利用笛卡尔坐标系表示：

28、p(x,y,depth)

29、进一步的，所述步骤s3，利用yolo目标检测网络获取图像语义信息，并利用语义信息与几何信息获取动态物体所在动态区域，具体包括：

30、选取yolov5目标检测网络，用于检测图像中的目标物体；

31、所述的动态区域确认方法由语义信息和几何信息联合确认，首先通过语义信息确认静态区域；随后通过语义信息寻找可能的动态物体，只针对可能的动态物体计算前后帧物体框区域内与静态区域的相对位置差异来对物体的动态特性做判断；检测框内物体的位移大小利用下式计算：

32、

33、式中：pd(u,v)是检测框内物体的像素坐标；d代表动态，u,v是像素坐标系的坐标；m，m-1是连续的图像帧；

34、前后帧的静态物体背景的特征点位移如下：

35、

36、式中，ns表示该图像静态背景中特征点的数量；s代表静态，i是第i个点，si表示第i个静态点，表示三维空间坐标系下静态特征点；z是特征点在深度图中对应的深度信息；k则是相机内参；和表示包含相机平移与旋转量的四维矩阵，表示相机位姿变换矩阵的李代数。

37、最后通过下面公式判断该物体框是否属于动态区域，其中ρ表示决策系数，满足该公式，则该检测框内就属于动态区域；

38、

39、进一步的，所述s4，根据动态区域内的深度信息对区域内进行聚类分析，以剔除动态的特征点并剔除，具体包括：

40、利用k-means聚类方法对深度信息进行聚类，其思想在于使同一个类内的点距离最小：

41、

42、式中，d代表两点之间的深度距离，仅考虑深度depth，不考虑x和y；xi表示聚类中的第i点，c(xi)是含有点xi的聚类的中心点。

43、进一步的，所述根据深度聚类剔除动态特征点具体为：首先假设检测框中动态物体与静态背景之间的深度值有差异，该差异足以使将物体与背景区分开来，随后假设在动态区域内，动态物体所占面积比例较大，动态物体特征点的数量比较多；于是在去除离群点后借助k-means深度聚类方法分类后得到的分类结果中，数量最多，平均深度值最小的那一类就是动态特征点所在聚类；

44、在进行动态特征点剔除时，若得到的动态特征点聚类结果正好满足数量最多，平均深度值最小的条件，则只去除这一类内的特征点，若不满足这一条件，则去除动态区域内所有特征点。

45、进一步的，所述步骤s5将剩余的特征点通过orb-slam2算法实现定位与导航，具体步骤如下：

46、跟踪线程：在局部地图中搜索特征点并进行匹配，通过光束平差ba算法来最小化重投影误差，从而确定每一帧相机的位置与姿态；

47、局部建图线程：利用局部ba算法对相机的位置和特征点云进行优化；

48、回环检测线程：寻找回环并通过优化位姿图来消除累积的漂移误差；在完成位姿图优化之后，会启动全局ba算法线程，以计算整个系统的最优结构和运动结果。

49、本发明的优点及有益效果如下：

50、本发明针对融合了语义分割、实例分割网络的slam系统分割效率低，而融合目标检测网络的slam系统精度又低的问题，提出一种基于深度聚类的移动服务机器人动态环境定位与建图方法。本发明一方面采用yolo目标检测网络来为系统提高语义信息，提高了语义信息获取的精确度和速度；由于直接利用语义信息准确度不高，使用多视图几何方法速度又慢，本文使用语义信息结合对极约束的方法确认动态区域，准确性高且速度较快。另一方面，本文借助深度聚类的方法对动态区域内特征点进行细分，使得在使用目标检测网络的情况下能够达到类似语义分割的效果，而且特征点聚类方法速度较快，目标检测网络加特征点聚类方法的耗时远低于使用语义分割或实例分割网络的耗时。

51、最后通过在公开数据集tum上测试，实验表明，基于深度聚类的移动服务机器人动态环境定位与建图方法具有较好的定位精度和鲁棒性，且在目标检测与深度聚类过程中耗时较低，满足实时性要求，取得了优越的性能。本发明在进行特征点分类剔除时，其余方法往往是针对图像生成掩膜后在依据掩膜图像对特征点进行剔除，而本方法因为先前对特征点赋予了深度信息，可以直接对特征点进行分类，无需生成掩膜。在进行图像语义分割或实例分割并生成掩膜时，系统需要消耗非常多的资源，处理一帧图像往往要几十上百毫秒，而单纯聚类处理图像中带深度信息的特征点所需耗时甚至不足一毫秒。即便加上目标检测网络的耗时，系统的实时性也远好于其余使用语义分割网络或实例分割网络的方法。同时系统精度又高于仅使用目标检测的方法。