多机器人编队路径规划方法、装置、电子设备及存储介质

本发明涉及机器人，尤其涉及一种多机器人编队路径规划方法、装置、电子设备及存储介质。

背景技术：

1、随着机器人技术的发展，机器人在航拍、植保、地质监测、通信中继等各个领域得到了广泛的应用。通过对多无人机进行编队导航，采用多机器人编队协同配合的方式开展任务。

2、针对多机器人编队导航的技术，主要包括基于控制律、局部优化和预测规划三类方法；其中，基于局部控制律的方案无法在预测范围内进行规划，使得无法保证密集障碍环境下的编队导航任务完成；基于局部优化的方法通过对每个机器人施加相对位置约束来维持队形，但队形会在障碍场景中被动局部屈服于障碍物，缺乏全局更优轨迹的引导；基于预测规划的方法在编队通过密集障碍环境时，队形保持质量差，无法形成严格编队。

3、综上所述，如何在密集障碍环境下实现多无人机编队的路径规划是亟需解决的问题。

技术实现思路

1、本发明提供一种多机器人编队路径规划方法、装置、电子设备及存储介质，用以解决如何在密集障碍环境下实现多无人机编队的路径规划是亟需解决的问题。

2、本发明提供一种多机器人编队路径规划方法，包括：

3、基于目标区域的密集障碍环境，对多个机器人编队进行全局路径规划，得到所述多个机器人编队在所述目标区域的全局路径；所述目标区域的密集障碍环境采用二维栅格占据地图表示，所述全局路径包括各所述机器人在每个时刻的待跟踪路径点；

4、基于各所述机器人在每个时刻的待跟踪路径点，对各所述机器人的局部轨迹进行优化，得到所述多个机器人编队的优化轨迹。

5、根据本发明提供的一种多机器人编队路径规划方法，所述基于各所述机器人在每个时刻的待跟踪路径点，对各所述机器人的局部轨迹进行优化，得到所述多个机器人编队的优化轨迹，包括：

6、针对每个机器人，基于所述机器人在每个时刻的待跟踪路径点，构建无约束优化目标；所述无约束优化目标用于对所述机器人的局部轨迹进行优化；

7、基于所述无约束优化目标，确定所述多个机器人编队的优化轨迹。

8、根据本发明提供的一种多机器人编队路径规划方法，所述基于所述机器人在每个时刻的待跟踪路径点，构建无约束优化目标，包括：

9、基于所述机器人在每个时刻的待跟踪路径点的姿态，确定所述机器人在每个时刻的速度、加速度和所述机器人的优化变量；所述优化变量包括姿态序列和时间间隔序列，所述姿态序列是基于各所述姿态确定的，所述时间间隔序列是基于相邻时刻之间的运动时间差组成的；

10、基于所述优化变量、所述速度、所述加速度和障碍物的位置，分别确定总时间代价函数、编队约束代价函数、编队路径跟随约束代价函数、避障代价函数、速度约束代价函数和加速度约束代价函数；

11、基于所述总时间代价函数、所述编队约束代价函数、所述编队路径跟随约束代价函数、所述避障代价函数、所述速度约束代价函数和所述加速度约束代价函数，构建所述无约束优化目标。

12、根据本发明提供的一种多机器人编队路径规划方法，所述基于所述总时间代价函数、所述编队约束代价函数、所述编队路径跟随约束代价函数、所述避障代价函数、所述速度约束代价函数和所述加速度约束代价函数，构建所述无约束优化目标，包括：

13、基于所述总时间代价函数、所述编队约束代价函数、所述编队路径跟随约束代价函数、所述避障代价函数、所述速度约束代价函数和所述加速度约束代价函数，采用公式(1)构建所述无约束优化目标；其中，公式(1)表示为：

14、

15、其中，表示所述机器人从初始姿态到终止姿态的n个姿态以及相邻姿态之间的时间间隔组成的离散序列，表示上述无约束优化问题的最优解，s1表示所述机器人的初始姿态，sn表示所述机器人的终止姿态，t表示时间，λt表示代价函数的权重，jt表示总时间代价函数，f表示编队，jf表示编队约束代价函数，p表示待跟踪点，jp表示编队路径跟随约束代价函数，o表示障碍物，jo表示避障代价函数，v表示速度，jv表示速度约束代价函数，a表示加速度，ja表示加速度约束代价函数。

16、根据本发明提供的一种多机器人编队路径规划方法，所述基于目标区域的密集障碍环境，对多个机器人编队进行全局路径规划，得到所述多个机器人编队在所述目标区域的全局路径，包括：

17、基于所述目标区域的密集障碍环境，采用全局路径规划算法对所述多个机器人编队进行全局路径规划，得到所述多个机器人编队在所述目标区域的全局路径；所述全局路径规划算法用于对所述多个机器人编队在所述目标区域的路径进行全局规划。

18、根据本发明提供的一种多机器人编队路径规划方法，所述基于所述目标区域的密集障碍环境，采用全局路径规划算法对所述多个机器人编队进行全局路径规划，得到所述多个机器人编队在所述目标区域的全局路径，包括：

19、步骤a：基于所述目标区域的密集障碍环境，初始化所述多个机器人编队的队形、在所述目标区域的起始节点、目标节点、初始评价值和初始移动代价值；所述起始节点和所述目标节点采用编队坐标系在地图坐标系上的二维坐标和航向角表示；

20、步骤b：将所述起始节点放入第一集合；

21、步骤c：在所述第一集合不为空的情况下，将所述第一集合中评价值最小的第一节点放入第二集合；

22、步骤d：在所述评价值最小的第一节点不为所述目标节点，且所述第一节点周围的第二节点不为空的情况下，分别计算所述第一节点对应的第一移动评价值以及所述第二节点的第二移动评价值；

23、步骤e：在所述第一移动评价值小于所述第二移动评价值的情况下，将所述第一节点设为父节点，并将所述第二节点放入所述第一集合中，得到新的第一集合，并重复执行步骤b-步骤e，直至所述新的第一集合中的评价值最小的第一节点周围的第二节点为空；

24、步骤f：在最终得到的第一集合为空的情况下，回溯所有的第二节点，生成所述多个机器人编队在所述目标区域的全局路径。

25、本发明还提供一种多机器人编队路径规划装置，包括：

26、全局路径规划模块，用于基于目标区域的密集障碍环境，对多个机器人编队进行全局路径规划，得到所述多个机器人编队在所述目标区域的全局路径；所述目标区域的密集障碍环境采用二维栅格占据地图表示，所述全局路径包括各所述机器人在每个时刻的待跟踪路径点；

27、局部轨迹优化模块，用于基于各所述机器人在每个时刻的待跟踪路径点，对各所述机器人的局部轨迹进行优化，得到所述多个机器人编队的优化轨迹。

28、本发明还提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任一种所述多机器人编队路径规划方法。

29、本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上述任一种所述多机器人编队路径规划方法。

30、本发明还提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时实现如上述任一种所述多机器人编队路径规划方法。

31、本发明提供的多机器人编队路径规划方法、装置、电子设备及存储介质，基于目标区域的密集障碍环境，对多个机器人编队进行全局路径规划，得到多个机器人编队在目标区域的全局路径；目标区域的密集障碍环境采用二维栅格占据地图表示，全局路径包括各机器人在每个时刻的待跟踪路径点；再基于各机器人在每个时刻的待跟踪路径点，对各机器人的局部轨迹进行优化，得到多个机器人编队的优化轨迹，实现了多个机器人编队在密集障碍环境中的全局路径规划以及局部轨迹优化，使得多个机器人编队中每个机器人在任意离散时刻能够保持编队队形，并基于优化轨迹能够最短的时间到达目标位置，有效提升了多机器人编队在密集障碍环境中编队的效率以及完成导航任务的成功率。