一种多机器人协同建图系统的制作方法

本发明涉及同时定位与建图，具体是涉及一种多机器人协同建图系统。

背景技术：

1、同时定位与建图(simultaneous localization and mapping，slam)是一项关键的技术，通过机器人自身搭载的传感器，能够获取周围环境的信息并估计自身的位置和方向，从而实现地图的构建，这一技术为机器人在未知环境中实现自主定位和导航提供了重要支持，目前，单一机器人slam技术已经取得了相当的鲁棒性，而且在多个领域得到了广泛的应用。

2、然而，在应对大规模环境的地图构建时，单一机器人slam技术面临一些挑战，单机slam效率低，估计时间增长，估计精度下降，往往难以达到良好的效果。

3、综上所述，亟需一种多机器人协同建图系统。

技术实现思路

1、鉴于上述的分析，本发明提出了一种多机器人协同建图系统，用以解决现有技术中建图的效率低，建图时间消耗长的技术问题。

2、为了实现上述目的，本发明提供一种多机器人协同建图系统，

3、包括多个移动平台和中央控制中心；

4、每个移动平台均搭载激光雷达、激光里程计、计算主机；

5、所述激光雷达用于获取周围环境的点云数据；

6、所述激光里程计用于获取移动平台的位姿数据；

7、所述计算主机对所述点云数据和位姿数据进行处理得到关键帧点云和关键位姿图，并通过无线网卡将所述关键帧点云和关键位姿图发送至中央控制中心；

8、所述中央控制中心基于所述关键帧点云和所述关键位姿图进行回环检测找到回环候选对，基于回环检测结果、所述关键帧点云和所述关键帧点云进行位姿图聚合及优化得到待建图区域的全局点云地图。

9、进一步地，所述中央控制中心下达同步授时指令给每个移动平台的所述计算主机，所述对每个移动平台上的所有设备进行同步授时服务；

10、所有设备同步之后，所述中央控制中心下达操控各个移动平台的运动指令至计算主机，所述计算主机操控各个移动平台在所述待建图区域内进行移动，各个移动平台的移动路径覆盖整个待建图区域，运动完成后各移动平台返回起点位置；

11、各所述计算主机实时发送所述关键帧点云和关键位姿图发送至中央控制中心。

12、进一步地，所述激光雷达通过所述交换机与所述计算主机连接；

13、所述无线网卡和所述激光里程计均通过usb接口与所述计算主机连接；

14、所述无线网卡用于所述计算主机与中央控制中心的通讯；

15、所述中央控制中心包括大算力计算主机和存储设备；

16、所述大算力计算主机用于所述回环检测和生成所述全局点云地图；

17、所述存储设备用于存储所述关键点云帧数据、关键位姿图数据、回环检测结果及全局点云地图。

18、进一步地，每个计算主机拥有两路网口，一路用于所在移动平台内部网络数据传输，另一路用于和所述中央控制中心的数据传输。

19、进一步地，所述计算主机对所述点云数据和位姿数据进行处理得到关键帧点云和关键位姿图，包括：

20、所述计算主机对所述点云数据进行预处理，使用匀速运动假设对所述点云数据去除运动畸变，并应用pcl点云库中自适应体素化滤波器进行体素降采样去除噪点，得到预处理后的点云帧；

21、每个移动平台运行搭载的雷达里程计，雷达里程计使用点云帧-局部地图匹配，估计所述移动平台的位姿，构建所述移动平台的当前局部位姿图；

22、将预处理后的激光雷达点云帧作为输入，将所述点云帧与所述当前局部位姿图使用gicp算法进行匹配，输出点云帧扫描时刻移动平台的位姿；

23、然后将该位姿加入到所述当前局部位姿图中，进行增量式构建局部位姿图；

24、所述计算主机从局部位姿图中挑选关键位姿节点，构建关键位姿图。

25、进一步地，所述关键位姿图中的每个节点对应于一个所述雷达里程计估计的位姿；

26、连接两个节点的边为这对节点之间的相对位姿变换；

27、当所述移动平台超出运动阈值之后的位姿被添加为新的关键位姿节点；

28、每个所述关键位姿节点对应的所述点云帧被挑选为关键帧点云。

29、进一步地，所述中央控制中心基于所述关键帧点云和所述关键位姿图进行回环检测找到回环候选对，包括：

30、所述大算力计算主机将最新从各个计算主机接收到的所述关键位姿图中的关键位姿节点作为当前节点，计算所述当前节点与其他节点的欧式距离；

31、将与当前节点的欧式距离小于阈值dmax的节点组成的节点对，作为回环候选对；

32、其中，其他节点为每个移动平台生成的所述关键位姿图中的所有历史关键位姿节点。

33、进一步地，所述大算力计算主机利用所述回环候选对的两个节点对应的两个关键帧点云，使用两阶段由粗至精进行回环确认得到所述两个节点之间的精确相对位姿变换，包括：

34、所述大算力计算主机基于所述计算队列中的回环候选对的两个关键帧点云，使用teaser++算法提取局部特征描述子，对两个关键帧点云的特征进行匹配，建立两个关键帧点云的对应关系，通过最小误差函数估计两个关键帧点云之间的相对位姿变换；

35、基于经过teaser++算法粗略估计后的所述两个关键帧点云之间的相对位姿变换使用gicp算法通过迭代的方式最小化两个关键帧点云之间的距离，gicp算法迭代过程逐步调优找到两个关键帧点云之间的最佳相对位姿变换，得到回环候选对的精确的相对位姿变换。

36、进一步地，所述大算力主机进行关键位姿图聚合及优化获得多移动平台轨迹和全局点云地图，包括：

37、获取各回环候选对对应的两个关键位姿图或一个关键位姿图；

38、对于各回环候选对，将两个关键位姿图或一个关键位姿图中的两个候选节点用边连接，所述边代表候选对的精确相对位姿变换，实现多个关键位姿图的聚合，得到完整位姿图；

39、使用gnc算法对所述所述完整位姿图优化，通过多次迭代调整所述完整位姿图中各个位姿节点的位置，去除噪声和异常值，得到多移动平台移动轨迹和全局点云地图。

40、进一步地，所述欧式距离阈值dmax为2-5米或30°-40°。

41、与现有技术相比，本发明至少可实现如下有益效果之一：

42、1、低算力要求和内存占用的优势：通过在移动平台独立运行激光里程计，按一定距离和角度阈值挑选出稀疏的关键帧点云和位姿节点并构建稀疏位姿图，减轻了位于基站的计算负荷和内存占用；

43、2、鲁棒性和容错性的优势：根据环境特征丰富度对位姿图中的回环候选对进行筛选和优先级排序，剔除了退化程度较高的回环候选对，优先计算可观测性较好的回环候选对；通过渐进式非凸(gnc)算法执行鲁棒的位姿图优化，进一步增强了系统的鲁棒性和容错性；

44、3、在大范围环境下提供更稳定和更高效的定位和建图。

45、综上，本发明旨在解决现有技术中的高算力和内存需求与鲁棒性容错性问题，从而提供了一种多机器人协同建图系统。

46、本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。