[发明专利]多机器人协同轨迹规划方法、装置、电子设备和存储介质

权利要求书

1.一种多机器人协同轨迹规划方法，其特征在于，包括：

将机器人划分为轨迹规划组和不变轨迹组；

根据所述轨迹规划组中各个待规划机器人在当前时刻的位置、速度以及目标时刻的位置、速度，分别构建各个待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程；

基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，以供各个待规划机器人沿所述轨迹协同运动；

其中，求解任一待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数时，所述任一待规划机器人需与所述不变轨迹组中的各个已规划机器人满足避碰安全距离约束，并在求解完毕后将所述任一待规划机器人移动到所述不变轨迹组中，直到轨迹规划组为空时，全部待规划机器人完成轨迹规划；

任一待规划机器人对应的包含待优化参数的时空贝塞尔曲线轨迹方程为：

其中，为时空贝塞尔曲线轨迹方程，自变量的取值范围为 ，伯恩斯坦基函数为：

其中，包含四个控制点， =0、1、2、3；

表示的三维空间位置， 表示 的时刻；表示时空贝塞尔曲线的初始位置和时刻，表示时空贝塞尔曲线的目标位置和时刻；中间控制点和的位置分别表示为：

其中， 表示时空贝塞尔曲线的初始速度，表示时空贝塞尔曲线的目标速度；所述待优化参数为和；

所述基于粒子群算法以及多机器人需满足的物理约束，依次求解各个待规划机器人对应的时空贝塞尔曲线轨迹方程中的待优化参数，得到各个待规划机器人的同步时间轨迹，具体包括：

初始化步骤：随机初始化粒子群算法中各个粒子的位置和速度；其中，各个粒子的位置对应和 的一组取值；

优化步骤：更新各个粒子的位置和速度，基于各个粒子的位置求解得到所述任一待规划机器人的多个时空贝塞尔曲线轨迹方程，离散化各个轨迹方程，并基于所述物理约束计算各个粒子对应的目标函数值，从而确定整个种群中当前最优解对应粒子的位置；

迭代步骤：重复执行所述优化步骤，不断优化各个粒子的位置，直至达到终止条件；基于整个种群中最优解对应粒子的位置，确定所述任一待规划机器人对应的最优时空贝塞尔曲线轨迹方程，即得到所述任一待规划机器人的最优轨迹；

其中，求解各个待规划机器人的同步时间轨迹时，根据如下关系：

其中，为时间，当满足 时，时间函数恒为增函数，因此基于一组离散化 值计算出各个待规划机器人对应的离散化 值，再代入时空贝塞尔曲线轨迹方程，可满足各个机器人轨迹离散点的时刻对应相等，即求得各个待规划机器人的同步时间轨迹。

2.根据权利要求1所述的多机器人协同轨迹规划方法，其特征在于，任一粒子对应的目标函数值基于如下公式计算：

其中， 为所述任一粒子对应的目标函数值，为 对应的时空贝塞尔曲线轨迹的路程，为预设数值，其中,， 为判别系数，包含的积分项表示沿路径点的线积分，实际利用各个相邻离散点的距离总和近似该项。

3.根据权利要求2所述的多机器人协同轨迹规划方法，其特征在于，所述判别系数基于如下步骤确定：

基于所述任一粒子的位置，对所述任一待规划机器人的时空贝塞尔曲线轨迹方程进行离散化；

基于离散化的时空贝塞尔曲线轨迹，计算所述任一待规划机器人在每个离散点处的速度、加速度、曲率、与障碍物表面间的距离、运动轨迹高度以及与不变轨迹组中任一机器人间的距离；

基于所述任一待规划机器人在每个离散点处的速度、加速度、曲率、与障碍物表面间的距离、运动轨迹高度以及与不变轨迹组中任一机器人间的距离，以及所述物理约束，确定所述判别系数。

4.根据权利要求1至3任一项所述的多机器人协同轨迹规划方法，其特征在于，所述物理约束包括最大速度约束、最大加速度约束、最大曲率约束、避障安全距离约束、地形安全高度约束和避碰安全距离约束。