CN115774455B - 复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法 - Google Patents

Abstract

本发明公布了一种复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，涉及无人智能集群协同控制方法，是一种基于分布式模型预测控制的，可在复杂障碍环境下避免死锁且保证可行性与运动安全性的分布式无人集群协同运动轨迹规划方法，包括：初始化所有对象的预设轨迹；对象之间交流各自的预设轨迹信息以及与死锁状态相关信息；获取所有对象的状态信息；构建安全走廊；通过更新动态优先级获取避让系数；构建分布式无人集群协同运动路径规划的模型函数并进行求解，实现避免死锁的分布式无人集群协同运动路径规划。本发明能够进行复杂集群系统协同运动轨迹规划，规避死锁现象并确保分布式无人集群协同运动的安全性。

Description

复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法

技术领域

本发明涉及无人智能集群协同控制方法，具体涉及一种复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，是一种基于分布式模型预测控制的，可在复杂障碍环境下避免死锁且保证可行性与运动安全性的分布式无人集群协同运动轨迹规划方法。

背景技术

无人集群的分布式协同运动路径/轨迹规划和控制是一个复杂的系统性技术问题，其关键技术主要有信息交互机制、多机协同环境感知技术、协同决策以及集群协同运动规划技术等。而集群协同运动规划是其中的核心部分，其内容大体可以分为分布式运动轨迹规划与避障算法这两大部分。

分布式运动轨迹规划算法的种类繁多，但现有的方法都无法处理因分布式无人集群缺乏中心协调器而产生的死锁问题。死锁问题是指多个对象之间相互阻挡从而使各方都无法到达目的地，这一问题在复杂障碍物以及密集集群情况下更容易发生。现有技术尚缺乏能够避免死锁且保证可行性的分布式无人集群协同运动轨迹规划方法。本发明将采用分布式模型预测控制方法，通过构建含有位置约束以及动力学约束的凸优化问题并进行求解，可以在考虑到控制对象本身的复杂运动学模型的同时保证对象之间的相互避障。

目前较为常用的解决死锁的算法主要有：右旋原则法（right-hand rule）、优先级规划（priority planning）和绕道点法（detourpoint）。这些都属于启发式算法，对于解决死锁问题有一定的价值，但是也有较为明显的缺点，即无法保证个体的运动安全性以及死锁解决的有效性。值得注意的是，以上问题在复杂障碍物环境下更容易出现。因此需要设计一种可以在复杂障碍环境下，实现高效运行且能保证运动安全性的死锁解决算法。

与此同时，考虑到现实中无人集群的各种应用场景中通常包含一定密度的障碍物，因此必须考虑无人集群在复杂障碍环境下的安全运行，这就涉及精确避障的需求。目前常用的避障算法主要有人工势场法（artificialpotentialfeild）以及安全走廊法（safecorridor）。针对复杂动力学模型，人工势场法无法保证安全性，且在复杂障碍物环境下容易使控制对象陷入到死锁状态。安全走廊法通过引入硬约束可以确保安全性，但同时会导致所构建的优化问题不可解。此外现有的安全走廊构建方法，无法兼顾空间利用效率以及计算复杂性。因此需要设计一种可以在保障可行性的前提下，兼顾空间效率与计算复杂度的避障算法。

发明内容

本发明的目的是提供一种可在复杂障碍环境下避免死锁且保证运动安全性的分布式无人集群协同运动轨迹规划方法，用以解决具有复杂动力学的集群系统协同运动轨迹规划问题，规避死锁现象并确保分布式无人集群协同运动的安全性。

本发明基于分布式模型预测控制，针对分布式无人集群运动轨迹规划，通过定义变量、约束以及目标来构建优化问题，最后通过求解优化问题实现无人集群的分布式运动轨迹规划与控制。本发明中的无人集群系统包括无人机系统、无人车系统和无人艇系统等。

具体地，构建无人集群的分布式运动轨迹规划与控制优化模型，首先通过加入终端约束，确保优化模型问题解的可行性，从而保证了运动轨迹规划算法的安全性；其次使用含预警带的缓冲维诺单元（warningbuffered Voronoi cell）构建控制对象之间的避障约束；并使用在线生成的安全走廊构建控制对象与环境障碍物之间的避障约束；最后将动态优先级机制与右旋原则相结合构造了一种新的死锁解决方法，从而实现高效的无人集群协同运动轨迹规划。

本发明提供的技术方案如下：

一种复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，该方法包括如下的对象信息与任务需求：分布式无人集群中的个体（对象）可以为无人机、无人车和无人艇等多种形式；分布式无人集群中存在在同一维度空间内运动的 N个对象； N个对象在同一维空间内运动，每个对象可以自行确定自己的运动控制输入，并通过无线通讯与其他对象交换各自的信息；从初始时刻开始，各对象需要从各自的初始位置运动到各自的目标位置，并保证在运动过程中相互之间及与障碍物之间不发生碰撞。

该方法包括如下步骤：

1）初始化无人集群中所有对象的预设轨迹；

以分布式无人集群中的第 i个对象为例，假设为其初始位置，为初始时刻，t为当前运行时刻；在每一时刻每个对象都有由 K个预测点组成的预测视界，用以对此时刻往后的 K步轨迹进行预测，视界序号，其中 K为视界长度；则对象i的初始预设轨迹记为，。

2）集群中每一个对象 i和其他对象之间交流各自的预设轨迹信息 以及死锁状态相关信息；

3）通过实时定位系统获取所有对象的状态信息，包括位置信息、速度信息；

4）对预设轨迹进行扩展，并划分未若干段，获取每一段轨迹中对象 i和所有障碍物之间的分割超平面的参数向量，其中 o表示障碍物，k代表视界序号，同一段轨迹中的所有视界处对应的参数向量是相同的，即对应同一个分割超平面；依次获取每一段轨迹对应的分割超平面参数向量，并在此基础上构建安全走廊。

安全走廊构造过程分为三步：规划参考路径、扩展预设轨迹的分段化、计算分割超平面。

4.1）构造参考路径

利用现有的路径规划算法（例如ABIT*算法）构造出一条连接预设轨迹终端位置与目标位置的参考路径 （假设其索引下标为 m=1,2,…, K，代表点集合中的第 m个元素），然后定义一个引导点，其中是参考路径中的元素索引下标的最大值、使得由参考路径与预设轨迹终端位置构成的凸包不与任何障碍物干涉。在此基础上可以定义扩展预设轨迹（extended predeterminedtrajectory, EPT）如下：

其中,, … ,为预设轨迹；扩展点。

4.2）扩展预设轨迹的分段化；

将扩展预设轨迹划分为段，记作，，划分方法如下：

①初始化；

②将位置点按逆序从到依次添加到集合中，直到构成的凸包与障碍物产生干涉；

③初始化，其中是中的最后一个点；

④往复执行①-③的操作，直到预设轨迹的第一个点被加入了最后一段。

4.3）计算分割超平面

对于第 n段扩展预设轨迹，采用如下的优化方法确定最佳分割超平面：

其中和是用于确定超平面的中间系数，，代表障碍物上的点，是裕度变量，代表到超平面的距离。上述优化问题可以转化为一个二次规划问题：

其中，。

通过求解该二次规划问题，可以获得第 n段扩展预设轨迹分割超平面的参数向量 ， ，由此定义分割超平面。分割超平面将当前对象与障碍物分隔开来，只要对象在自己所处的一侧规划运动便不会与障碍物发生碰撞。那么，由所有这些分割超平面构成的一系列凸多面体即构成了一个安全走廊。

5）通过更新动态优先级获取避让系数；

5.1）通过死锁检测机制判断对象 i是否发生了死锁现象，若是，则更新死锁状态指示量为真（True）。基于死锁状态指示量，更新对象 i的死锁程度指示量；

5.2）对进行更新的方法为：

其中是一个自定义参数，是的数值上界； h为每两步之间的时间间隔；为预警带变量；为对象的缓冲带宽度。

       5.3）确定对象 i的动态优先级并进行更新。

动态优先级分三级：最低为1，中等为2，最高为3。拥有更高优先级的对象行为会更“自私”，会无视其他对象的排斥力、优先考虑自身的最优轨迹并执行，以脱离死锁状态。

最开始所有对象的优先级均为2，如果到达目标点，则优先级置为1，其他情况下优先级根据相应原则进行更新。

的具体更新方法为：

①判断是否存在时刻t使得：若是，则使；若否，则进入②；

②判断是否满足 ：若是，则使并进入③；若否，则不进行操作；

③判断是否且且 ：若是，则使；若否，则不进行操作；

④完成①-③后，判断是否且：若是，则使；若否，则不进行操作。

其中是对象 i关于对象 j的死锁上界指示量。

5.4）根据更新获取避让系数。代表第 j个对象对对象 i的排斥力，其更新原则如下：

其中是xOy平面上到的向量的投影与到的向量的投影之间的夹角变量，称为避让角度参数，用于使对象 i进行右旋偏转、进而脱离与对象 j发生的死锁现象，其更新流程如图4所示；为惩罚值调整函数，如为为当前时刻t对应的惩罚值调整函数，用于在一定范围内调整惩罚值，其中 ，初始时刻该函数值为；,和均为系数，满足。

6）根据从定位系统获得的状态信息以及通过动态优先级机制更新的避让系数，使用含预警带的缓冲维诺单元构建对象之间的避障约束、使用在线生成的安全走廊构建对象与环境障碍物之间的避障约束，并在目标函数中引入与预警带相关的惩罚函数，构建分布式无人集群协同运动路径规划的模型函数及优化问题，并进行求解，得到输入加速度信息和规划轨迹序列，即实现了避免死锁的分布式无人集群协同运动路径规划。

具体过程如下：

6.1）构建分布式无人集群协同运动轨迹规划的模型函数和优化问题，采用分布式模型预测控制方法实现运动轨迹规划，包括：

集群中的每一个对象 i从初始位置运动到目标运动位置为，其具有二阶系统的动力学；

集群中的每一个对象在每经过时间间隔 h，取视界长度，构建并求解以下凸优化问题进行重规划；构建的分布式无人集群协同运动轨迹规划模型函数表示为：

S.T:

式（1）

其中，为视界 k处的运动控制输入（即运动加速度）；为通过规划得到的视界 k处的状态量； ；表示视界 k处的位置；表示视界 k处的速度；为目标函数；

6.2）定义优化变量，包括运动控制输入、状态变量以及自定义的预警带变量。变量中的初始位置规定，并且规定初始速度为，其中与为对象在 t时刻的实际位置以及在时刻的实际速度，由定位系统测量获得。

6.3）定义约束，包括：动力学约束、输入约束、速度约束、相互间避障约束、对象与障碍物间避障约束、预警带约束和终端约束，其中自定义避障约束。

①动力学约束：

其中：

其中表示维度为 d的单位矩阵。

②输入约束和速度约束：

其中是已知的正定矩阵，分别表示最大速度和最大加速度。

③对象之间的避障约束：

通过含预警带的缓冲维诺单元对空间划分获得以下线性约束：

其中为自定义的预警带变量，为自定义的缓冲带最大宽度；参数与定义如下：

是上一时刻即时在视界 k处的规划结果，也称为预设轨迹；首次规划时取，其中为初始位置；拓展最小距离 ；

预警带约束表示为：

④对象与环境障碍物之间的避障约束：

其中与是用于确定安全走廊边界，并有安全走廊边界。

⑤终端约束为：

6.4）构建的分布式无人集群协同运动轨迹规划优化模型的目标函数表示为：

式（2）

其中是视界 K与引导点之间的距离和连续速度之间的平衡项，是用于死锁解决的惩罚项，二者的定义如下：

其中是视界 k处的权值常数；为对象的目标位置；是对象 i关于对象 j的避让系数，通过动态优先级机制获取并在每一步时更新；

6.5）求解上述集群运动轨迹规划问题（式（1）），得到的结果包括输入信息和规划位置序列信息；

6.6）将求解结果作为 t时刻对象 i的加速度输入输入到下层运动控制系统（如用于控制crazyflies无人机运动飞行的底层控制系统）中，从而实现一步运动控制，并达到运动避障的目的；

7）根据优化问题得到的规划位置序列，将其作为下一步 t+h时刻的预设轨迹，随后返回步骤2），进入下一轮主循环；

直至所有对象到达目标位置，则运行结束。

通过上述步骤1）-7），即实现避免死锁的分布式无人集群协同运动轨迹规划。

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

本发明提出一种可以在复杂障碍环境下避免死锁且保证安全性的无人集群协同运动轨迹规划方法。通过使用在线安全走廊构建方法，生成控制对象与环境障碍物之间的避障约束，实现了运动过程的完全避障。并通过引入动态优先级机制并在目标函数中引入与预警带相关的惩罚函数来规避死锁问题。这一方法可以使无人集群在复杂障碍环境中进行运动轨迹规划时规避死锁问题且保证安全性，同时可以实现优化模型算法求解的高效性、满足较苛刻的实时性要求，特别适用于动态和复杂障碍环境中的具有复杂动力学要求和高机动性要求的密集无人集群的运动轨迹规划。可以应用到如多机器人协同运输、多无人机协同侦察以及多无人艇水面探测等情形。

附图说明

图1是本发明具体实施例采用的平台模块的结构框图，包括感知层、规划层和执行层。

图2是本发明方法的实施流程框图。

图3是本发明具体实施例基于无人集群系统optitrack和crazyflies实现本发明方法的程序流程框图。

图4是获取避让角度参数的示意图；

其中， xOy为人为规定的参考平面，为对象 i在视界 K处的预设位置，为对象 j在视界 K处的预设位置，是对象 i关于对象 j的避让角度。

图5是的更新过程的流程框图；

其中是对象 i和对象 j的死锁上界指示量。

具体实施方式

下面结合附图，通过实施方式进一步描述本发明，但不以任何方式限制本发明的范围。

本发明提供一种可在复杂障碍环境下避免死锁且保证可行性的分布式无人集群协同运动轨迹规划方法，能够解决具有复杂动力学的集群系统协同运动轨迹规划问题，规避死锁问题并保证可行性。同时可以保证算法求解的高效性、满足较苛刻的实时性要求。

图1所示为具体实施时实现本发明方法采用的实验平台的结构，本发明方法的整体实施流程如图2所示，具体每个对象的对应程序执行流程如图3所示。其中对象 i关于对象 j的避让角度参数的获取方法如图4所示，对象 i的动态优先级的更新过程如图5所示。

以下是规避死锁且保证可行性的分布式无人集群协同运动轨迹规划中每一个对象的具体运行过程：

1）初始化无人集群中所有对象的预设轨迹；

以对象 i为例，其初始的预设轨迹记为，为初始位置， i为分布式无人集群中的对象；为初始时间；视界序号，其中 K为视界长度；之后进入主循环；

2）集群中每一个对象 i和其他对象之间交流各自的预设轨迹信息；

3）通过实时定位系统(如Vicon、Optitrack或GPS等)获取所有对象的状态信息，包括位置信息以及速度信息；

4）通过构建安全走廊，获取对应的分割超平面参数；

5）根据动态优先级机制更新获取避让系数；

6）在获得状态信息以及避让系数之后，利用CVXOPT构建优化并求解优化问题，获得的结果包括输入信息和位置信息；将获得的结果中的输入信息作为该时刻的控制输入；

7）利用以上优化问题的结果中的位置信息，获取新的预设轨迹；

8）将控制量输入给下层运动控制系统。通过控制输入实现分布式无人集群中的对象的运动。

在分布式无人集群协同控制技术领域，最为常见的集群运动轨迹规划问题描述如下：

假设集群存在 N个对象。需要这 N个对象在同一维空间内运动，在初始时刻，，从初始位置运动到各自的目标位置，并在运动过程中相互之间不发生碰撞。

针对其中任意一个编号为 i的对象，其需要从初始位置运动到目标运动位置，其具有二阶系统的动力学，尽可能地逼近实际情况：

其中：

其中表示维度为 d的单位矩阵。

表示系统状态，,分别表示对象的位置、速度和控制输入，并且需要满足以下输入约束以及速度约束：

其中是已知的正定矩阵，分别表示最大速度和最大加速度。

任意两个对象 i与 j之间的避障需要满足以下条件：

其中预警带约束为：

对象与障碍物之间的避障需要满足以下条件：

针对以上集群运动轨迹规划问题，采用分布式模型预测控制方法实现运动轨迹规划。分布式模型预测控制需要使每一个对象在每经过时间间隔 h，取视界长度，构建并求解以下优化问题进行重规划。

S.T:

其中，，表示这一重规划时刻的时间。将求解结果可以作为系统在 t时刻的输入输入到下层运动控制系统中，从而实现运动控制，并达到运动避障的目的。

图3给出了采用crazyflies无人机系统实现本发明方法的流程。

本发明具体实施时，在python语言中通过凸优化函数包CVXOPT对上述优化模型进行求解，可以在较短时间内获得准确的全局最优解，并且具有极高的可靠性。

本发明基于预设轨迹构建安全走廊对空间进行分割，可证明本次优化过程中所使用的预设轨迹满足本次凸优化中的所有限制，以此保证本次凸优化的可行域非空，实现了不同时刻多个连续凸优化问题（即序列凸优化）的递归可行性；同时使用在线生成的安全走廊构建对象与环境障碍物之间的避障约束，实现了运动过程中对障碍物的完全避障，保证了本方法在复杂障碍环境下的避障可行性与高效性。因此，本发明方法保证了避障可行性。

本发明还通过在目标函数的惩罚项中加入预警带变量，使得多个对象在相互阻挡过程中可以产生可控的相互作用力，之后通过动态优先级机制联合右旋原则获取避让系数，用以控制这一相互作用力的大小，从而打破多对象间因均衡的作用力而产生的死锁状态。与此同时，动态优先级机制与右旋原则的结合可以保证复杂障碍物环境下的死锁解决的可行性和有效性。因此，本发明方法保证了死锁解决的可行性和有效性。

具体实施表明，本发明方法可以使无人集群在复杂障碍环境中进行运动轨迹规划时避免死锁且保证运动安全性，同时可以实现算法求解的高效性、满足较苛刻的实时性要求，特别适用于复杂障碍环境中的具有复杂动力学要求和高机动性要求的密集无人集群的运动轨迹规划。可以应用到如多机器人协同运输、多无人机协同侦察以及多无人艇水面探测等情形。

需要注意的是，公布实施例的目的在于帮助进一步理解本发明，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的范围内，各种替换和修改都是可能的。因此，本发明不应局限于实施例所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。

Claims (10)

1.一种复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，所述分布式无人集群中有同一维度空间内运动的N个对象；每个对象自行确定运动控制输入，并通过无线通讯与其他对象交换各自的信息；从初始时刻开始，各对象分别从初始位置运动到目标位置，且在运动过程中各对象相互之间及与障碍物之间不发生碰撞；所述分布式无人集群轨迹规划方法包括如下步骤：

1）初始化分布式无人集群中所有对象的预设轨迹；

以分布式无人集群中的第i个对象为例，假设为其初始位置，为初始时刻，t为当前运行时刻；在每一时刻每个对象都有由K个预测点组成的预测视界，用以对此时刻往后的K步轨迹进行预测，视界序号，其中K为视界长度；则对象i的初始预设轨迹记为，；

2）集群中每一个对象i与其他对象之间交流各自的预设轨迹信息以及死锁状态相关信息；

3）通过实时定位系统获取所有对象的状态信息，包括位置信息、速度信息；

4）对预设轨迹进行扩展并划分为多段，获取每一段扩展预设轨迹中对象i和所有障碍物之间的分割超平面的参数向量，得到分割超平面，并构成一个安全走廊；包括：构造参考路径、扩展预设轨迹的分段化、计算分割超平面；

4.1）构造参考路径；包括：

构造出一条连接预设轨迹的终端位置与目标位置的参考路径 ；设参考路径索引下标为m=1,2,…,K，代表点集合中的第m个元素；

定义一个引导点，其中是参考路径中的元素索引下标的最大值，使得由参考路径与预设轨迹终端位置构成的凸包不与任何障碍物干涉；

定义扩展预设轨迹为：

为扩展预设轨迹；其中，；

4.2）扩展预设轨迹进行分段；

将扩展预设轨迹划分为段，记作，；划分方法如下：

初始化；

将位置点按逆序从到依次添加到集合中，直到构成的凸包与障碍物产生干涉；

初始化，其中是中的最后一个点；

循环执行①-③的操作，直到预设轨迹的第一个点被加入最后一段；

4.3）计算分割超平面；包括：

对于第n段扩展预设轨迹，采用如下的优化方法确定最佳分割超平面：

其中，和是用于确定超平面的系数，，代表障碍物上的点，是裕度变量，代表到超平面的距离；将上述优化目标转化为：

其中，；通过求解获得分割超平面的参数向量 ， ；

分割超平面将当前对象与障碍物分隔开，使得对象在自己所处的一侧规划运动时不与障碍物发生碰撞；由所有分割超平面构成一个安全走廊；

5）通过更新动态优先级获取避让系数；

5.1)通过死锁检测机制判断对象i是否发生死锁现象，若是，则更新对象i的状态指示量为真；基于状态指示量更新对象i的死锁程度；t为当前运行时刻；

5.2)对进行更新的方法为：

其中，为自定义参数，是的数值上界；h为每两步之间的时间间隔；为对象的缓冲带宽度；

5.3)确定对象i的动态优先级并进行更新；动态优先级高的对象优先考虑自身的最优轨迹并执行，以脱离死锁状态；

所有对象的初始动态优先级均取值为2；

如果到达目标点，则动态优先级置为1；其他情况下动态优先级进行更新；

5.4)通过更新对象i的动态优先级获取避让系数；代表第j个对象对对象i的排斥力；避让系数表示为：

其中，为避让角度参数，是xOy平面上到的向量的投影与到的向量的投影之间的夹角变量，用于使对象i进行右旋偏转、进而脱离与对象j发生的死锁现象；为惩罚值调整函数；,和均为系数，满足；

6）根据从定位系统获得的状态信息以及获取的避让系数，使用含预警带的缓冲维诺单元构建对象之间的避障约束、使用在线生成的安全走廊构建对象与环境障碍物之间的避障约束，并在目标函数中引入与预警带相关的罚函数，构建分布式无人集群协同运动路径规划的模型函数并进行求解，得到运动控制输入信息和规划轨迹序列，即实现避免死锁的分布式无人集群协同运动路径规划；过程如下：

6.1）构建分布式无人集群协同运动轨迹规划的模型函数，采用分布式模型预测控制方法实现运动轨迹规划，包括：

集群中的每一个对象i从初始位置运动到目标运动位置为，具有二阶系统的动力学；集群中的每一个对象在每经过时间间隔h，取视界长度，构建并求解以下凸优化问题进行重规划；构建的分布式无人集群协同运动轨迹规划模型函数表示为：

S.T: 

式（1）

其中，为视界k处的控制输入；为通过规划得到的视界k处的状态量； ；表示视界k处的位置；表示视界k处的速度；为目标函数；；；其中表示维度为d的单位矩阵；是已知的正定矩阵；分别表示最大速度和最大加速度；

6.2）定义优化变量，包括运动控制输入、状态变量以及自定义的预警带变量；

变量中的初始位置设置为，且初始速度为，其中与为对象在t时刻的实际位置以及在时刻的实际速度；

6.3）定义约束，包括：动力学约束、输入约束、速度约束、相互间避障约束、对象与障碍物间避障约束、预警带约束和终端约束；

其中自定义对象相互间避障约束；包括：

通过含预警带的缓冲维诺单元对空间划分获得以下线性约束：

其中为自定义的预警带变量，为自定义的缓冲带最大宽度；参数与定义如下：

是上一时刻即时在视界k处的规划结果，也称为预设轨迹；首次规划时取，其中为初始位置；拓展最小距离 ；

预警带约束表示为：

6.4）构建的分布式无人集群协同运动轨迹规划优化模型的目标函数，表示为：

式（2）

其中，是视界K与引导点之间的距离和连续速度之间的平衡项；是用于死锁解决的惩罚项；二者的定义分别为：

其中，是视界k处的权值常数；为对象的目标位置；是对象i关于对象j的避让系数；

6.5）求解式（1）的分布式无人集群协同运动轨迹规划模型函数，得到的结果包括运动控制输入信息和规划位置序列信息；

进一步可将作为t时刻对象i的运动控制输入输入到无人集群运动控制系统中，实现一步运动控制和运动避障；

7）将得到的规划位置序列作为下一步t+h时刻的预设轨迹，随后返回步骤2），进入下一轮主循环；直至所有对象到达目标位置，则运行结束；

通过上述步骤1）-7），即实现避免死锁的分布式无人集群协同运动轨迹规划。

2.如权利要求1所述复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，其特征是，分布式无人集群中的对象包括无人机、无人车和无人艇；分布式无人集群中的对象在同一维空间内运动。

3.如权利要求1所述复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，其特征是，步骤4.1）构造参考路径中，具体时利用路径规划算法ABIT*算法构造出一条连接预设轨迹终端位置与目标位置的参考路径。

4.如权利要求1所述复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，其特征是，分布式无人集群中对象的动态优先级分为三级。

5.如权利要求1所述复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，其特征是，无人集群运动控制系统包括用于控制Crazyflies无人机运动飞行控制系统。

6.如权利要求1所述复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，其特征是，步骤5.3）中，对象i的动态优先级的更新方法具体是：

①判断是否存在时刻t使得：若是，则使；若否，则进入②；

②判断是否满足 ：若是，则使并进入③；若否，则不进行操作；

③判断是否且且 ：若是，则使；若否，则不进行操作；

④完成①-③后，判断是否且：若是，则使；若否，则不进行操作；

其中是对象i关于对象j的死锁上界指示量。

7.如权利要求1所述复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，其特征是，步骤6.3）中，动力学约束定义为：

其中：

其中表示维度为d的单位矩阵。

8.如权利要求7所述复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，其特征是，步骤6.3）中，输入约束和速度约束定义为：

其中是已知的正定矩阵，分别表示最大速度和最大加速度。

9.如权利要求8所述复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，其特征是，步骤6.3）中，对象与环境障碍物之间的避障约束表示为：

其中与是用于确定安全走廊边界，并有安全走廊边界。

10.如权利要求9所述复杂障碍环境下避免死锁的分布式无人集群轨迹规划方法，其特征是，步骤6.3）中，终端约束表示为：

。

Images