一种多机器人自主协同定位与地图构建方法及系统

本发明涉及多移动机器人导航，具体为一种多机器人自主协同定位与地图构建方法及系统。

背景技术：

1、近年来，移动机器人的地图构建技术在勘探、救援、环境监测等领域得以广泛应用。随着机器人技术的不断发展和进步，单个机器人已经难以适应当前，日益增长的应用需求。多移动机器人作为一个分布式智能系统，已经成为机器人领域研究的热点之一。相对于单个移动机器人，它具有抗干扰性和容错性等优势，即使部分受到故障或干扰，整个系统仍然可以保持一定的功能，具有重要的应用价值。

2、自主探索和协同地图构建技术使多机器人系统得以广泛应用的关键。自主探索技术帮助机器人主动感知周围环境，通过机间协作完成未知场景的安全探索。协同地图构建则允许多机器人共同准确的环境状态描述。为此结合两种技术并构建多移动机器人协同定位与地图构建系统与方法，是机器人智能化的重要保证。多机器人协同同步定位与地图构建期望使用搭载相同或不同类型传感器(相机，激光雷达，imu等)的多个运动主体(机器人，无人车，无人机等)通过信息交互，估计出各智能体位姿的变化，同时融合构建出未知环境的三维地图。机器人要实现自主运动，首先就需要具有能够感知和理解周围环境的能力。这就需要机器人能够与人一样，当来到一个陌生环境时，能够自主的探索未知环境的同时构建出环境地图，并且能够在地图中获得精准的位置信息。随着机器人应用场景的广泛化和其任务要求的复杂化，因此，需要多移动机器人能够在没有任何先验信息的未知环境中探索，通过自身携带的传感器获取周围环境信息，并且每个机器人在进行独立任务的同时，还可以与其他机器人进行信息交互，相互协调，同时并行的求解问题。

3、随着机器人的智能化水平提高与任务的复杂程度提高，设计一种强自主高实时的轻量级多机器人自主协同定位与地图构建方法仍然是技术难题。这需要使多机器人能够协同探索未知复杂环境，并且可以将各机器人构建的局部地图进行融合，构建环境全局地图。

技术实现思路

1、针对现有技术中存在的问题，本发明提供一种多机器人自主协同定位与地图构建方法，将多移动机器人协同探索算法与多移动机器人协同定位与地图构建算法相结合，能够有效提高多移动机器人在复杂环境下的协同定位精度和地图构建效率。

2、本发明是通过以下技术方案来实现：

3、一种多机器人自主协同定位与地图构建方法，包括以下步骤：

4、步骤1，根据各机器人的栅格地图构建共享栅格地图，各机器人采用自主探索算法对栅格地图进行自主探索，得到目标场景的前沿点集，对各机器人的前沿点集进行聚类，将聚类的中心作为目标点；

5、步骤2，根据导航代价和探索代价确定各目标点与各机器人位置之间的收益，根据收益最大的分配原则为各移动机器人分配目标点；

6、步骤3，各机器人沿最优路径向目标点移动进行空间探索，并获取环境图像；

7、步骤4，采用动静点检测算法剔除各环境图像中的动态特征点，根据静态特征点构建各机器人探索的局部地图；

8、步骤5，将各机器人探索过程中的关键帧图像存储在关键帧库中，将当前的关键帧图像与关键帧库中的关键帧图像进行匹配，当匹配的关键帧图像与当前的关键帧图像隶属于不同的局部地图，获取当前关键帧图像图与匹配关节帧图像的重合区域，根据重合区域对两个局部地图进行融合，得到融合的地图。

9、优选的，步骤1所述自主探索的方法如下：

10、各移动机器人分别使用全局随机树与局部随机树进行全局前沿点检测与局部前沿点检测，全局随机树以各机器人起始位置为根节点不断向外扩张，局部随机树以各机器人当前位置为根节点向外扩张，获取已探索区域与未探索区域的边界，直至达到目标环境的边界得到前沿点集。

11、优选的，在随机树生长过程中如果遇到障碍物，继续保持向该方向的生长策略。

12、优选的，步骤1中采用均值漂移聚类对前沿点集进行聚类，将聚类的中心作为机器人需要探索的目标点。

13、优选的，步骤2中根据机器人的当前位置和目标点位置之间的欧式距离确定导航代价；

14、根据当前目标点所在期望探索的未知区域的面积确定探索收益。

15、优选的，步骤2中所述收益的计算方法如下：

16、r(xfp)＝λh(xfp，xr)i(xfp)-n(xfp)

17、其中，n为导航代价，i为探索代价，λ指探索代价的权重，h(xfp，xr)指迟滞收益函数，xfp为目标点，xr为机器人位置。

18、优选的，步骤3中各机器人沿最优路径向目标点移动进行空间探索的过程中，并根据步骤1和步骤2的方法实时获取新的目标点，并根据收益更新机器人的目标点，机器人沿最优路径向更新后的目标点移动。

19、优选的，步骤4中构建局部地图的方法如下：

20、获取相邻帧图像序列的单位变换矩阵h，再将当前帧图像通过单位变换矩阵h反变换后，与前一帧图像进行光流计算检测动态区域，并对落入动态区域的特征点进行剔除，根据静态特征点对相邻帧进行特征匹配估计机器人自身位姿，根据静态特征点和机器人位姿构建各机器人的三维点云局部地图并进行回环检测。

21、优选的，步骤5中所述局部地图融合的方法如下：

22、对当前关键帧图像通过k均值聚类生成词袋向量，根据词袋向量与关键帧库中的关键帧图像进行匹配，若该匹配关键帧图像与当前关键帧图像属于不同局部地图时，则通过场景辨识判断是否为地图间重叠区域，如果存在重叠区域，则对两个局部地图进行融合。

23、一种多机器人自主协同定位与地图构建方法的系统，包括，

24、目标点模块，根据各机器人的栅格地图构建共享栅格地图，各机器人采用自主探索算法对栅格地图进行自主探索，得到目标场景的前沿点集，对各机器人的前沿点集进行聚类，将聚类的中心作为目标点；

25、目标点分配模块，根据导航代价和探索代价确定各目标点与各机器人位置之间的收益，根据收益最大的分配原则为各移动机器人分配目标点；

26、图像采集模块，各机器人沿最优路径向目标点移动进行空间探索，并获取环境图像；

27、局部地图构建模块，采用动静点检测算法剔除各环境图像中的动态特征点，根据静态特征点构建各机器人探索的局部地图；

28、地图融合模块，将各机器人探索过程中的关键帧图像存储在关键帧库中，将当前的关键帧图像与关键帧库中的关键帧图像进行匹配，当匹配的关键帧图像与当前的关键帧图像隶属于不同的局部地图，获取当前关键帧图像图与匹配关节帧图像的重合区域，根据重合区域对两个局部地图进行融合，得到融合的地图。

29、与现有技术相比，本发明具有以下有益的技术效果：

30、本发明提供的一种多机器人自主协同定位与地图构建方法

31、本发明公开了一种多机器人自主协同定位与地图构建方法，实现了未知环境的多机器人自主协同探索与点云地图构建，主要包括自主探索与协同建图两个部分。自主探索部分基于激光雷达构建栅格地图，在服务端共享并融合栅格地图。随后采用基于co-rrt(crossed the obstacles rapidly-exploring random trees)的多移动机器人协同自主探索算法来对未知环境进行协同探索，使用全局随机树和局部随机树进行前沿点检测，对获取的前沿点集进行均值漂移聚类处理，通过导航代价和探索代价两个因素分配导航目标。最后，通过路径规划控制各机器人沿最优路径到达目标点；协同建图部分则在探索过程中基于视觉采集图像序列。机器人通过动静点算法检测动态特征点并剔除，以构建复杂环境下各局部地图，然后通过关键帧匹配，当两个关键帧存在相同的环境，则对两个机器人的局部地图进行融合，完成复杂环境下的多机器人自主协同定位与地图构建任务。该方法具有轻量级、准确、高效、自主等特点，可以在不受人工干预的情况下，自主完成未知环境下的大范围高精度点云地图构建。