一种多机器人自主探索方法及系统

本发明涉及一种多机器人自主探索方法及系统，属于移动机器人自主探索。

背景技术：

1、移动机器人技术作为一种可替代人工的技术，有着极多优点，移动机器人可代替人进行重复机械劳动，也可代替人执行危险任务。随着移动机器人诸多相关技术的发展、潜在的巨大效益以及其为人员带来的安全性，移动机器人相关技术不断向多机系统发展，且这一趋势愈来愈高，其应用范围愈来愈大。

2、激光即时定位与地图构建(simultaneous localization and mapping，slam)方法是目前比较普遍应用的同步地图构建与定位方法，它可以较为精确地、大范围地构造地图，且能包含较多细节；对于大规模环境，狭长通道多的场景，单个机器人slam效率终究是有限的，因此研究多机器人系统中的多机协同slam方法对于提高地图探索效率是非常有必要的。

3、移动机器人自主探索是为了让机器人在未知有界环境中对未知区域主动地进行快速、完整、准确的观测，以构建可进行导航与规划的完整的环境地图，其解决方案是不断地寻找未知区域边界附近的一系列观测点，并控制机器人对其进行访问，使未知区域最终被探索完毕。单机器人进行自主探索的效率较为有限，这就需要开发多机器人自主探索系统以及相关算法。

技术实现思路

1、本发明所要解决的技术问题是：提供一种多机器人自主探索方法及系统，实现多机器人系统协同探索未知空间，可以实现较为快速准确的探索，且各机器人探索轨迹比较平滑。

2、本发明为解决上述技术问题采用以下技术方案：

3、一种多机器人自主探索方法，包括如下步骤：

4、步骤1，已知各机器人的初始位置，对各机器人进行初始化；对于每个机器人，向前方行进六米，对机器人初始位置周围环境进行初步构建；

5、步骤2，判断当前迭代次数是否达到最大迭代次数或是否接收到停止指令，若达到最大迭代次数或接收到停止指令，则迭代结束；否则继续探索过程；

6、步骤3，利用多机slam算法获得各机器人激光点云信息与定位信息，并进行tsdf地图构建，用于局部探索和全局探索；

7、步骤4，对于每个机器人，根据机器人轨迹进行全局图的扩展，在机器人周围随机采样全局图节点并进行筛选扩展；

8、步骤5，对机器人周围进行局部图节点均匀采样以构建局部图并进行障碍物的筛选，输出局部图及其状态；

9、步骤6，判断步骤5输出的局部图状态是否成功，若是则对局部图所有的叶子节点计算体素增益，将体素增益大于第一预设阈值的叶子节点记为局部图边界节点，输出局部图及其状态；否则转入步骤9，进行全局探索；

10、步骤7，判断步骤6输出的局部图状态是否成功，若是则对局部图边界节点进行旅行商问题求解得到局部图边界节点的最短访问顺序，并输出局部图状态；否则转入步骤9，进行全局探索；

11、步骤8，判断步骤7输出的局部图状态是否成功，若是则根据所述最短访问顺序拼接路径并进行路径优化，将路径中的局部图边界节点作为全局图的节点，将路径中相邻两个局部图边界节点连成的路径作为全局图的边，输出局部探索的最终路径；否则转入步骤9，进行全局探索；

12、步骤9，计算全局图中的所有节点的探索增益以及体素增益，从而得到全局图边界节点，同时获取其它机器人的全局图进行融合；

13、步骤10，对于每个机器人，利用astar算法计算机器人到达与其最近的全局图边界节点的最短路径；

14、步骤11，迭代次数加一，对各机器人局部探索得到的最终路径或全局探索得到的最短路径进行优化，得到跟踪路径，将跟踪路径返回给各机器人进行跟踪，返回步骤2。

15、作为本发明方法的一种优选方案，所述步骤11中，在优化之前，通过多路径冲突检测算法检测各机器人探索得到的最终路径或最短路径是否存在冲突，即预先设置各机器人的优先级，将各机器人探索得到的最终路径或最短路径均分割为短路径，通过各机器人膨胀来检测其中一个机器人分割得到的短路径与另一个机器人分割得到的段路径之间是否存在重叠，若存在重叠，则根据优先级选择级别更高的机器人对应的短路径。

16、作为本发明方法的一种优选方案，所述步骤1中，利用多机slam算法获得各机器人激光点云信息与定位信息，并进行tsdf地图构建，具体过程如下：

17、(1)对每个机器人进行点云预处理，删除机器人周围设定距离内的机器人自身点云，按照固定分辨率下采样点云并进行存储；

18、(2)对每个机器人进行单机位姿计算，包括瞬时帧间位姿估计、关键帧选取以及帧图匹配；

19、(3)订阅各机器人当前存储的点云数据，进行点云下采样，然后将各机器人当前点云、下采样得到的点云以及估计的位姿累加得到当前全局已构建地图点云。

20、作为本发明方法的一种优选方案，所述瞬时帧间位姿估计通过比较当前帧点云以及存储的上一帧点云得到机器人传感器坐标系下的六自由度的变换til；通过最小化目标函数的误差来进行求解获得帧间坐标变换，目标函数如下：

21、

22、其中，表示当前帧点云pi中的第1至第n个点，表示上一帧点云pi-1中的第1至第n个点，ej表示进行坐标变换后第j个点对间的有向距离，表示点的协方差矩阵，表示点的协方差矩阵，右上角t表示矩阵转置。

23、作为本发明方法的一种优选方案，所述关键帧选取通过定义关键帧结构体，包含一帧点云以及机器人当前位姿，当获得当前帧点云后，通过计算得到当前机器人的位姿估计，若当前位姿估计相较于上一关键帧位姿变换超过第二预设阈值，选取当前帧为关键帧，同时添加到关键帧队列。

24、作为本发明方法的一种优选方案，所述帧图匹配通过将当前帧点云与当前局部子地图进行匹配，将当前帧与关键帧队列中的每个关键帧进行遍历匹配，找到上述遍历匹配后距位姿最近的k个关键帧更新局部子地图，帧图匹配通过最小化目标函数误差来求解位姿变换，目标函数如下：

25、

26、其中，til表示机器人传感器坐标系下的六自由度的变换，表示世界坐标系下上一帧计算出的全局坐标变换，tiw表示世界坐标系下当前帧计算出的全局坐标变换，ej表示进行坐标变换后第j个点对间的有向距离，表示点的协方差矩阵，表示局部子地图中帧sij的协方差矩阵，右上角t表示矩阵转置。

27、一种多机器人自主探索系统，所述系统包括：多个机器人平台，每个机器人平台包括具有差速轮式的机器人底盘、3d机械式激光雷达、工控机和随车路由，3d机械式激光雷达、工控机和随车路由均设置在机器人底盘上；工控机带有两个网口，3d机械式激光雷达通过其中一个网口与工控机进行数据传输，随车路由通过另一个网口与工控机进行数据传输，两个网口设置在不同网段，机器人底盘通过can总线与工控机连接进行数据传输；每个机器人平台对应一个自组网路由器，各路由器通过mesh进行自组网；中央控制终端、自组网路由器以及机器人平台的工控机和随车路由组成局域网；

28、所述3d机械式激光雷达用于获取机器人周围激光点云信息；

29、所述工控机用于部署多机器人自主探索算法，接收中央控制终端的控制指令，并控制机器人平台获取点云地图信息、位姿信息、机器人平台探索过程构建的局部与全局无向图以及机器人的历史轨迹；

30、所述随车路由用于与其他机器人平台和中央控制终端进行信息交互；

31、所述自组网路由器用于扩展局域网确保各随车路由与中央控制终端之间通信的有效性；

32、所述中央控制终端用于向各机器人平台的工控机发送控制指令以及路径冲突检测分割后发送给各机器人平台的短路径，同时中央控制终端接收各机器人平台发送的点云地图信息、位姿信息、各机器人平台探索过程构建的局部与全局无向图以及各机器人的历史轨迹信息。

33、本发明采用以上技术方案与现有技术相比，具有以下技术效果：

34、1、本发明构造了一套多机器人自主探索系统，搭建了多个车体部分以及多车通信系统，在实机部署时可以实现较大规模环境下的多机通信交互以及多机协同工作。

35、2、本发明根据多机器人系统，将基于单个slam方法扩展到多机器人系统中，在进行初始位姿标定后实现多机器人协同建图与定位，可实现多机器人系统中各机的准确定位以及协同地图构建。

36、3、本发明开发了一种基于图的多机器人自主探索算法，通过为每个小车构建局部均匀无向图与全局随机无向图来高效的覆盖环境，最终实现多机器人系统协同探索未知空间，可以实现较为快速准确的探索，且各机器人探索轨迹比较平滑。