CN113156977B - 基于时间优化的多机器人复杂交错轨迹规划方法及系统 - Google Patents

Abstract

本发明公开了基于时间优化的多机器人复杂交错轨迹规划方法及系统，包括：获取每个机器人的若干条已知运动路径；基于所有机器人的若干条已知运动路径，构建包含所有机器人的高维空间；建立路程‑时间二维空间中的障碍物的约束条件、速度约束条件和加速度约束条件；构建时间优化目标函数；目标函数是指所有机器人达到目标位置的最大耗时最短；针对高维空间、约束条件和目标函数，进行求解，得到最优值；所述最优值即机器人沿着路径的移动距离对时间的函数曲线；根据所述函数曲线，对每个机器人进行轨迹引导。本发明简洁高效，尤其适用于多个移动机器人在狭小环境和密集目的地场景中的协调控制。

Description

基于时间优化的多机器人复杂交错轨迹规划方法及系统

技术领域

本发明涉及轨迹规划技术领域，特别是涉及基于时间优化的多机器人复杂交错轨迹规划方法及系统。

背景技术

本部分的陈述仅仅是提到了与本发明相关的背景技术，并不必然构成现有技术。

在多移动机器人协同作业过程中，多机器人沿规划的轨迹独立并行地向各自目标位置运动。在运动过程中存在以下几个问题：

1、冲突问题：当多个机器人在同一时刻到达同一位置时，多者之间发生冲撞，易导致设备损坏，影响任务的正常执行。

2、时间最优：所有机器人到达指定位置的总耗时应尽可能短，以提高多机器人系统运行效率。

3、到达目标位置的顺序问题：针对目标位置密集排列的任务场景，某机器人就位之后会阻断其它机器人的运动轨迹，导致其它机器人无法到达指定位置。

4、运动学约束：机器人运动需要满足速度范围限制和加速度范围限制，同时速度不能突变。

在多机器人运动轨迹复杂交错的情况下，以上问题尤为突出。

针对以上问题，现有技术主要包括优化类方法、时间-速度方法、速度障碍物法等。优化类方法构建约束和目标函数，通过求解优化问题解决以上问题。在优化类方法中，将多机器人冲突构建为非凸约束，采用泰勒展开、Bernstein多项式凸包属性等方法将非凸约束进行凸化和线性化，降低维度。时间-速度方法通过调整机器人到达冲突点的时间，采用加速或暂停等待的方式调解多机器人冲突。速度障碍物法通过速度半平面划分，为机器人实时选择可行速度以防止冲突。由于多机器人冲突约束的非凸性，优化类方法需要解决一个非凸非线性优化问题，求解难度大，耗时长；时间-速度方法难以获得全局最优或次优结果；速度障碍物法实时计算，速度快，但是缺乏全局轨迹的规划。

发明内容

为了解决现有技术的不足，本发明提供了基于时间优化的多机器人复杂交错轨迹规划方法及系统；

第一方面，本发明提供了基于时间优化的多机器人复杂交错轨迹规划方法；

基于时间优化的多机器人复杂交错轨迹规划方法，包括：

获取每个机器人的若干条已知运动路径；基于所有机器人的若干条已知运动路径，构建包含所有机器人的高维空间；

建立路程-时间二维空间中的障碍物的约束条件、速度约束条件和加速度约束条件；

构建时间优化目标函数；目标函数是指所有机器人达到目标位置的最大耗时最短；

针对高维空间、约束条件和目标函数，进行求解，得到最优值；所述最优值即机器人沿着路径的移动距离对时间的函数曲线；

根据所述函数曲线，对每个机器人进行轨迹引导。

第二方面，本发明提供了基于时间优化的多机器人复杂交错轨迹规划系统；

基于时间优化的多机器人复杂交错轨迹规划系统，包括：

获取模块，其被配置为：获取每个机器人的若干条已知运动路径；基于所有机器人的若干条已知运动路径，构建包含所有机器人的高维空间；

约束条件建立模块，其被配置为：建立路程-时间二维空间中的障碍物的约束条件、速度约束条件和加速度约束条件；

目标函数构建模块，其被配置为：构建时间优化目标函数；目标函数是指所有机器人达到目标位置的最大耗时最短；

求解模块，其被配置为：针对高维空间、约束条件和目标函数，进行求解，得到最优值；所述最优值即机器人沿着路径的移动距离对时间的函数曲线；

轨迹规划模块，其被配置为：根据所述函数曲线，对每个机器人进行轨迹引导。

第三方面，本发明还提供了一种电子设备，包括：一个或多个处理器、一个或多个存储器、以及一个或多个计算机程序；其中，处理器与存储器连接，上述一个或多个计算机程序被存储在存储器中，当电子设备运行时，该处理器执行该存储器存储的一个或多个计算机程序，以使电子设备执行上述第一方面所述的方法。

第四方面，本发明还提供了一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时，完成第一方面所述的方法。

与现有技术相比，本发明的有益效果是：

本发明构建包含所有机器人的全局运动参数的高维空间，建立路程-时间障碍物约束和速度加速度约束，构建时间优化目标函数，通过优化搜索求解，得到机器人沿路径的移动距离对时间的函数曲线(S-t函数曲线)，即规划机器人于某时刻到达某位置，得到各个机器人的轨迹。每个机器人按照各自的S-t函数行动，可以保证在运动学约束下、无冲突和碰撞地到达各自目标位置，并且所有机器人完成运动的总耗时最少。本发明通过将多机器人运动过程映射到路程-时间二维空间内，构建多约束优化模型，能够同时解决多机器人沿各自轨迹并发运动过程中的多个问题：冲突问题、到达目标位置的顺序问题，并使运动过程整体最优。本发明简洁高效，尤其适用于多个移动机器人在狭小环境和密集目的地场景中的协调控制。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1为第一个实施例的目标函数求解流程图；

图2为第一个实施例的计算所有粒子的适应度的流程图。

具体实施方式

应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

实施例一

本实施例提供了基于时间优化的多机器人复杂交错轨迹规划方法；

基于时间优化的多机器人复杂交错轨迹规划方法，包括：

S101：获取每个机器人的若干条已知运动路径；基于所有机器人的若干条已知运动路径，构建包含所有机器人的高维空间；

S102：建立路程-时间二维空间中的障碍物的约束条件、速度约束条件和加速度约束条件；

S103：构建时间优化目标函数；目标函数是指所有机器人达到目标位置的最大耗时最短；

S104：针对高维空间、约束条件和目标函数，进行求解，得到最优值；所述最优值即机器人沿着路径的移动距离对时间的函数曲线；

S105：根据所述函数曲线，对每个机器人进行轨迹引导。

进一步地，所述S101：获取每个机器人的若干条已知运动路径；基于所有机器人的若干条已知运动路径，构建包含所有机器人的高维空间；具体包括：

在每个机器人已知的每条运动路径上选取三个路径中间点，以使每条运动路径被等分为四段；被选取的三个路径中间点与起点和终点一起构成五个路径关键点；其中，起点对应的运动时刻为0，速度也为0；终点对应的运动速度为0；

除起点以外的四个路径关键点的时刻参数分别为t₂,t₃,t₄,t₅；

除起点和终点以外的三个路径中间点的机器人速度分别为v₂,v₃,v₄；

每个机器人沿着已知的路径运动的方式，采用7维参数描述(t₂,t₃,t₄,t₅,v₂,v₃,v₄)；

根据三次曲线方式，构建包含所有机器人运动参数的高维空间

示例性的，S-t三次曲线的标准形式为：

S＝a₀+a₁t+a₂t²+a₃t³ (1-2)

其中，a₀、a₁、a₂、a₃为三次曲线方程的各次项系数，S为机器人沿路径移动的路程，t为时间。a₀、a₁、a₂、a₃通过三次曲线起止点时刻t₀、t₁的移动距离S₀和S₁，以及机器人在起止点的速度v₀和v₁计算确定。令t_f＝t₁-t₀，S＝S₁-S₀。

各项系数计算方法如下所示：

其中，a₀、a₁、a₂、a₃为三次曲线方程的各次项系数，三次曲线起止点时刻为t₀、t₁，对应的移动距离为S₀和S₁，机器人在三次曲线起止点的速度为v₀和v₁。

示例性的，本发明构建包含所有机器人的S-t函数曲线关键参数的高维空间。在机器人已知运动路径上选取3个路径中间点，与起点和终点构成5个路径关键点。其中起点对应的运动时刻为0，速度为0；终点对应的运动速度为0。因此，采用除起点外的4个路径关键节点对应的时刻参数(t₂,t₃,t₄,t₅)，以及除起点和终点外的3个路径中间点处机器人的速度(v₂,v₃,v₄)，按照四段三次曲线方式构建整个运动轨迹的S-t曲线。因此每个机器人沿已知路径的运动方式采用7维参数描述(t₂,t₃,t₄,t₅,v₂,v₃,v₄)。对于包含N个机器人的系统，构建7×N维空间。

进一步地，所述S102：建立路程-时间二维空间中的障碍物的约束条件、速度约束条件和加速度约束条件；其中，路程-时间二维空间中的障碍物的约束条件，具体是指：

当前机器人在移动的过程中，其运动在路程-时间二维空间内的映射，不与路程-时间二维空间中的障碍物发生碰撞；

在三维物理空间中，该约束表现为当前机器人在运动的过程中不会碰到其它机器人。

示例性的，对于每一个机器人，计算其与其它机器人的路径交点S_c，即其它机器人将于何处阻断该机器人的路径；设置机器人长宽外形参数ε，利用其它机器人的移动距离对时间S-t曲线，计算其它机器人的首部开始干扰轨迹的时刻t_c1，并计算机器人尾部离开轨迹的时刻，得到干扰持续时间Δt_c。

通过以上计算，对每个机器人维护一个路程-时间障碍物图，路程-时间障碍物图中障碍物为矩形，其中心位置为

宽为Δt_c，高为ε。

在算法运行过程中实时计算障碍物的位置和尺寸并动态更新。

在路程-时间障碍物图中，当机器人的S-t曲线穿过障碍物区时，表示该机器人将在穿过障碍物的时刻和位置发生碰撞，即违反了路程-时间障碍物约束。

进一步地，所述速度约束条件，是指：

当前机器人在运动过程中的速度应时刻在速度限制以内，且不能发生突变。

示例性的，所述速度约束条件，用公式表达为：

其中，v_n是第n个机器人的运动速度，v_max是机器人最大运动速度，t为运动时间，T_end是运动过程结束的时刻。

进一步地，所述加速度约束条件，是指：

当前机器人在运动过程中的加速度应时刻在加速度限制以内。

示例性的，所述加速度约束条件，用公式表达为：

a_n∈[-a_max,a_max],a_max＞0；(2-3)

其中，a_n是第n个机器人的运动加速度，a_max是机器人最大运动加速度。

示例性的，机器人的速度曲线为S-t曲线的一阶导数，加速度曲线为二阶导数。分别计算速度曲线和加速度曲线的最大值和最小值，若某最值超过了速度和加速度范围，则该段S-t曲线违反了速度和加速度约束。

进一步地，所述S103：构建时间优化目标函数J；目标函数是指所有机器人达到目标位置的最大耗时最短；具体包括：

minJ＝T_max+α₁P_coll+α₂P_cons (3)

其中，T_max表示所有机器人到达目标位置的最大耗时，P_coll表示路程-时间障碍物约束惩罚，P_cons表示速度和加速度约束惩罚，α₁、α₂表示约束惩罚系数。T_max的计算方法为：取N个机器人中最晚到达目标位置的机器人所用的时间，即所有机器人的t₅参数的最大值。

P_cons的计算方法为：根据S-t曲线计算每一段的速度最值和加速度最值，超过速度限制和加速度限制的部分按照式(4)进行惩罚。

其中，ρ为比例系数，V_max和V_min是机器人运动速度的上下限，a_max和a_min是机器人加速度的上下限。k为曲线段数，取值为1到4。n为机器人序号，取值范围为1到N。

为速度最值超过速度限制的大小，

为加速度最值超过加速度限制的大小。二者根据式(5)和式(6)计算得到：

其中，

是第k段曲线的速度最大值和最小值，

是第k段曲线的加速度最大值和最小值，V_max和V_min是机器人运动速度的上下限，a_max和a_min是机器人加速度的上下限。

P_coll的计算方法为：以设定的颗粒度δ对路程-时间二维空间中的曲线S-t进行离散化，遍历离散点，查找是否存在落于障碍区的离散点。

碰撞惩罚，采用式(7)计算：

其中，

表示第n个机器人的S-t曲线上落于障碍区的采样点数量，γ是比例系数。n为机器人序号，取值范围为1到N；α₁、α₂通过调试获得。进一步地，所述S104：针对高维空间、约束条件和目标函数，进行求解，得到最优值；所述最优值即机器人沿着路径的移动距离对时间的函数曲线；具体包括：

S1041：将已知路径P_i等分4段，其中，i＝1,2,3…N；

选取5个关键路径节点p_k,k＝1,2,3,4,5。这些路径节点包含起点p₁、终点p₅和三个路径中间点p₂,p₃,p₄；

S1042：计算对应各段曲线长度的最短用时

x∈[1,2,3,4]；

S1043：计算两两路径间的交叉点，交叉点位置表示可能在此处发生冲突；

S1044：初始化M个粒子；

S1045：计算各粒子内各机器人的S-t曲线参数；

S1046：计算各粒子的适应度值；

S1047：计算粒子最佳适应度和种群最佳适应度，使用粒子群速度更新和粒子群位置更新方法，更新粒子；

S1048：判断粒子各维参数是否满足设定要求；若某维参数不满足，修改该维参数；若满足，则进入下一步；

S1049：返回S1045循环执行，直到满足停止条件；

S10410：迭代停止后的种群最佳粒子即为优化结果。

进一步地，所述S105：根据所述函数曲线，对机器人进行轨迹引导；具体包括：

根据所述函数曲线，引导机器人在某个时刻到达某个位置。

本发明采用经典的粒子群优化算法作为优化器，对高维空间进行优化搜索。每个粒子的维度为7×N，包含所有机器人的S-t函数曲线关键参数。通过式(1)计算每个粒子的适应度，通过求解优化问题，得到最优粒子，即所有机器人的最优7维变量描述，组成各个机器人沿已知路径运动的S-t函数。机器人按照该函数行进，可以保证在运动学约束下、无冲突碰撞地到达各自目标位置，并且所有机器人的总耗时最少。

在优化计算过程中，由于搜索空间维度较高，经典粒子群优化方法难以快速发现满足约束的最优解。因此本发明依据待解决问题的特征，对搜索空间进行了限制，刨除明显违反物理特性的部分搜索空间。具体操作如下：

1、依据三次曲线特性，整段S-t曲线上，v∈[0,V_max]，a∈[a_min,a_max]，机器人到达各路径关键节点的时刻t₁～t₅满足以下关系。

其中，x∈{2,3,4,5}，r∈[0，1]是随机数，λ是时刻波动范围，二者用于增加粒子分布的多样性和随机性。

为机器人按照三次曲线运动模式、在第x段路径运动时的、满足速度和加速度限制的最短用时。其计算方法如下。第一段曲线路径长度为S₁。

1)当

时，

为下式的最小正实根

其中，V_max是机器人运动速度的上限，a_max是机器人加速度的上限。t_f为待求用时。2)当

时，

其中，V_max是机器人运动速度的上限，a_max是机器人加速度的上限。

第二段曲线路径长度为S₂。

其中，V_max是机器人运动速度的上限，t_f为待求用时。第三段曲线路径长度为S₃。

其中，V_max是机器人运动速度的上限，t_f为待求用时。第四段曲线路径长度为S₄。第四段运动可以通过逆运动原理转化为与第一段同类型的运动。因此采用与第一段曲线相同的

计算方法。

2、第二、三、四个路径中间点处机器人的速度(v₂,v₃,v₄)通过各点两侧S-t曲线段的平均速度计算得到。计算方法为：

其中，x∈[2,3,4]，

为第x个轨迹关键点左侧曲线段的平均速度，

为第x个轨迹关键点右侧曲线段的平均速度。r∈[0,1]为随机数，用于增加粒子分布的多样性和随机性；V_max是机器人运动速度的上限。

本发明使用以上结论对演化过程中的粒子各维度的取值范围进行限制，有效降低搜索难度，提高了优化器发现满足约束的最优解的速度。

为解决到达目标位置的顺序问题，即某个机器人在其目标位置处干扰其它机器人的行进路线问题，本发明将该种干扰等效为一个持续时间为无穷久的干扰，对应于路程-时间障碍物图中的一个宽度范围[t_c1,+∞)的障碍区。被干扰的机器人只有提前通过干扰点，否则将会穿过障碍区，产生冲突惩罚。

如图1所示，整个发明的算法流程如下：

第一步：将已知路径P_i,(i＝1,2,3…N)等分4段，选取5个关键路径节点p_k,(k＝1,2,3,4,5)。这些路径节点包含起点p₁、终点p₅和三个路径中间点p₂,p₃,p₄。

第二步：通过式(9)(10)(11)计算对应各段曲线长度的最短用时

x∈[1,2,3,4]。

第三步：计算两两路径间的交叉点，交叉点位置表示可能在此处发生冲突。

第四步：初始化M个粒子。粒子各维参数由式(8)和式(12)计算。

第五步：利用式(2-1)计算各粒子内各机器人的S-t曲线参数。

第六步：按照式(4)(5)(6)(7)计算各粒子的适应度值。

第七步：计算粒子最佳适应度和种群最佳适应度，使用经典粒子群速度-位置更新方法，更新粒子。

第八步：判断粒子各维参数是否满足式(8)和式(12)。若某维参数不满足，按照式(8)和式(12)修改该维参数。

第九步：返回第五步循环执行，直到满足停止条件。

第十步：迭代停止后的种群最佳粒子即为优化结果。

如图2所示，计算所有粒子的适应度的方法包括：

S1：计算所有机器人到达目标位置的最大耗时；

S2：计算各段三次曲线的速度最值和加速度最值；

S3：计算加速度和速度违反约束惩罚；

S4：根据交叉点位置和双方的速度，计算速度障碍物；

S5：计算碰撞惩罚；

S6：计算适应度。

实施例二

本实施例提供了基于时间优化的多机器人复杂交错轨迹规划系统；

基于时间优化的多机器人复杂交错轨迹规划系统，包括：

获取模块，其被配置为：获取每个机器人的若干条已知运动路径；基于所有机器人的若干条已知运动路径，构建包含所有机器人的高维空间；

约束条件建立模块，其被配置为：建立路程-时间二维空间中的障碍物的约束条件、速度约束条件和加速度约束条件；

目标函数构建模块，其被配置为：构建时间优化目标函数；目标函数是指所有机器人达到目标位置的最大耗时最短；

求解模块，其被配置为：针对高维空间、约束条件和目标函数，进行求解，得到最优值；所述最优值即机器人沿着路径的移动距离对时间的函数曲线；

轨迹规划模块，其被配置为：根据所述函数曲线，对每个机器人进行轨迹引导。

此处需要说明的是，上述获取模块、约束条件建立模块、目标函数构建模块、求解模块和轨迹规划模块对应于实施例一中的步骤S101至S105，上述模块与对应的步骤所实现的示例和应用场景相同，但不限于上述实施例一所公开的内容。需要说明的是，上述模块作为系统的一部分可以在诸如一组计算机可执行指令的计算机系统中执行。

上述实施例中对各个实施例的描述各有侧重，某个实施例中没有详述的部分可以参见其他实施例的相关描述。

所提出的系统，可以通过其他的方式实现。例如以上所描述的系统实施例仅仅是示意性的，例如上述模块的划分，仅仅为一种逻辑功能划分，实际实现时，可以有另外的划分方式，例如多个模块可以结合或者可以集成到另外一个系统，或一些特征可以忽略，或不执行。

实施例三

本实施例还提供了一种电子设备，包括：一个或多个处理器、一个或多个存储器、以及一个或多个计算机程序；其中，处理器与存储器连接，上述一个或多个计算机程序被存储在存储器中，当电子设备运行时，该处理器执行该存储器存储的一个或多个计算机程序，以使电子设备执行上述实施例一所述的方法。

应理解，本实施例中，处理器可以是中央处理单元CPU，处理器还可以是其他通用处理器、数字信号处理器DSP、专用集成电路ASIC，现成可编程门阵列FPGA或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器可以包括只读存储器和随机存取存储器，并向处理器提供指令和数据、存储器的一部分还可以包括非易失性随机存储器。例如，存储器还可以存储设备类型的信息。

在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。

实施例一中的方法可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器、闪存、只读存储器、可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器读取存储器中的信息，结合其硬件完成上述方法的步骤。为避免重复，这里不再详细描述。

本领域普通技术人员可以意识到，结合本实施例描述的各示例的单元及算法步骤，能够以电子硬件或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

实施例四

本实施例还提供了一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时，完成实施例一所述的方法。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.基于时间优化的多机器人复杂交错轨迹规划方法，其特征是，包括：

获取每个机器人的若干条已知运动路径；基于所有机器人的若干条已知运动路径，构建包含所有机器人的高维空间；

建立路程-时间二维空间中的障碍物的约束条件、速度约束条件和加速度约束条件；所述加速度约束条件，是指：当前机器人在运动过程中的加速度应时刻在加速度限制以内；

将某个机器人在目标位置处对其他机器人行进路线的干扰等效为一个持续时间为无穷久的干扰；

构建时间优化目标函数J；目标函数是指所有机器人达到目标位置的最大耗时最短；具体为：

minJ＝T_max+α₁P_coll+α₂P_cons

其中，T_max表示所有机器人到达目标位置的最大耗时，P_coll表示路程-时间障碍物约束惩罚，P_cons表示速度和加速度约束惩罚，α₁、α₂表示约束惩罚系数；

针对高维空间、约束条件和目标函数，进行求解，得到最优值；所述最优值即机器人沿着路径的移动距离对时间的函数曲线；具体包括：

(1)：将已知路径P_i等分4段，其中，i＝1,2,3…N；

选取5个关键路径节点p_k,k＝1,2,3,4,5；这些路径节点包含起点p₁、终点p₅和三个路径中间点p₂,p₃,p₄；

(2)：计算对应各段曲线长度的最短用时

(3)：计算两两路径间的交叉点，交叉点位置表示可能在此处发生冲突；

(4)：初始化M个粒子；

(5)：计算各粒子内各机器人的S-t曲线参数；

(6)：计算各粒子的适应度值；

(7)：计算粒子最佳适应度和种群最佳适应度，使用粒子群速度更新和粒子群位置更新方法，更新粒子；

(8)：判断粒子各维参数是否满足设定要求；若某维参数不满足，修改该维参数；若满足，则进入下一步；

(9)：返回(5)循环执行，直到满足停止条件；

(10)：迭代停止后的种群最佳粒子即为优化结果；

根据所述函数曲线，对每个机器人进行轨迹引导。

2.如权利要求1所述的基于时间优化的多机器人复杂交错轨迹规划方法，其特征是，获取每个机器人的若干条已知运动路径；基于所有机器人的若干条已知运动路径，构建包含所有机器人的高维空间；具体包括：

在每个机器人已知的每条运动路径上选取三个路径中间点，以使每条运动路径被等分为四段；被选取的三个路径中间点与起点和终点一起构成五个路径关键点；其中，起点对应的运动时刻为0，速度也为0；终点对应的运动速度为0；

除起点以外的四个路径关键点的时刻参数分别为t₂,t₃,t₄,t₅；

除起点和终点以外的三个路径中间点的机器人速度分别为v₂,v₃,v₄；

每个机器人沿着已知的路径运动的方式，采用7维参数描述(t₂,t₃,t₄,t₅,v₂,v₃,v₄)；

根据三次曲线方式，构建包含所有机器人运动参数的高维空间

3.如权利要求1所述的基于时间优化的多机器人复杂交错轨迹规划方法，其特征是，建立路程-时间二维空间中的障碍物的约束条件、速度约束条件和加速度约束条件；其中，路程-时间二维空间中的障碍物的约束条件，具体是指：

当前机器人在移动的过程中，其运动在路程-时间二维空间内的映射，不与路程-时间二维空间中的障碍物发生碰撞；

在三维物理空间中，该约束表现为当前机器人在运动的过程中不会碰到其它机器人。

4.如权利要求1所述的基于时间优化的多机器人复杂交错轨迹规划方法，其特征是，所述速度约束条件，是指：

当前机器人在运动过程中的速度应时刻在速度限制以内，且不能发生突变。

5.如权利要求1所述的基于时间优化的多机器人复杂交错轨迹规划方法，其特征是，根据所述函数曲线，对每个机器人进行轨迹引导；具体包括：

根据所述函数曲线，引导机器人在某个时刻到达某个位置。

6.基于时间优化的多机器人复杂交错轨迹规划系统，其特征是，包括：

获取模块，其被配置为：获取每个机器人的若干条已知运动路径；基于所有机器人的若干条已知运动路径，构建包含所有机器人的高维空间；

约束条件建立模块，其被配置为：建立路程-时间二维空间中的障碍物的约束条件、速度约束条件和加速度约束条件；所述加速度约束条件，是指：当前机器人在运动过程中的加速度应时刻在加速度限制以内；

将某个机器人在目标位置处对其他机器人行进路线的干扰等效为一个持续时间为无穷久的干扰；

目标函数构建模块，其被配置为：构建时间优化目标函数；目标函数是指所有机器人达到目标位置的最大耗时最短；具体为：

minJ＝T_max+α₁P_coll+α₂P_cons

其中，T_max表示所有机器人到达目标位置的最大耗时，P_coll表示路程-时间障碍物约束惩罚，P_cons表示速度和加速度约束惩罚，α₁、α₂表示约束惩罚系数；

求解模块，其被配置为：针对高维空间、约束条件和目标函数，进行求解，得到最优值；所述最优值即机器人沿着路径的移动距离对时间的函数曲线；具体包括：

(1)：将已知路径P_i等分4段，其中，i＝1,2,3…N；

选取5个关键路径节点p_k,k＝1,2,3,4,5；这些路径节点包含起点p₁、终点p₅和三个路径中间点p₂,p₃,p₄；

(2)：计算对应各段曲线长度的最短用时

(3)：计算两两路径间的交叉点，交叉点位置表示可能在此处发生冲突；

(4)：初始化M个粒子；

(5)：计算各粒子内各机器人的S-t曲线参数；

(6)：计算各粒子的适应度值；

(7)：计算粒子最佳适应度和种群最佳适应度，使用粒子群速度更新和粒子群位置更新方法，更新粒子；

(8)：判断粒子各维参数是否满足设定要求；若某维参数不满足，修改该维参数；若满足，则进入下一步；

(9)：返回(5)循环执行，直到满足停止条件；

(10)：迭代停止后的种群最佳粒子即为优化结果；

轨迹规划模块，其被配置为：根据所述函数曲线，对每个机器人进行轨迹引导。

7.一种电子设备，其特征是，包括：一个或多个处理器、一个或多个存储器、以及一个或多个计算机程序；其中，处理器与存储器连接，上述一个或多个计算机程序被存储在存储器中，当电子设备运行时，该处理器执行该存储器存储的一个或多个计算机程序，以使电子设备执行上述权利要求1-5任一项所述的方法。

8.一种计算机可读存储介质，其特征是，用于存储计算机指令，所述计算机指令被处理器执行时，完成权利要求1-5任一项所述的方法。

Images