一种避免死锁的分布式无人集群协同运动路径规划方法

摘要

本发明公布了一种避免死锁的分布式无人集群协同运动路径规划方法，分布式无人集群中的N个对象在同一维度空间内运动；对象在初始时刻，从初始位置运动到各自的目标位置，且在运动过程中相互之间不发生碰撞；其特征是，针对分布式无人集群运动路径规划，通过定义约束、目标以及变量，构建分布式无人集群协同运动路径规划的优化问题及模型函数，通过加入终端约束定义无限视界，其次使用含预警带的缓冲维诺单元构建避障约束，并通过在目标函数中引入与预警带相关的罚函数，解决具有复杂动力学的集群系统协同运动路径规划问题，规避死锁问题并保证可行性。

权利要求

1.一种避免死锁的分布式无人集群协同运动路径规划方法，分布式无人集群中的N个对象在同一维度空间内运动；对象在初始时刻t0，从初始位置p0运动到各自的目标位置ptarget，且在运动过程中相互之间不发生碰撞；其特征是，针对分布式无人集群运动路径规划，通过定义约束、目标以及变量，构建分布式无人集群协同运动路径规划的优化问题及模型函数，通过加入终端约束定义无限视界，其次使用含预警带的缓冲维诺单元构建避障约束，并在目标函数中引入与预警带相关的罚函数；

避免死锁的分布式无人集群协同运动路径规划方法包括如下步骤：

1)初始化分布式无人集群中的预设路径；

将分布式无人集群中的任意一个对象i的预设路径记为 p0为初始位置；t0为初始时间；视界k＝1，2，...，K其中K为视界长度；

2)i和其他对象之间交流各自的预设路径信息；

3)通过实时定位系统获取状态信息，包括位置信息以及速度信息；

ij ij ij

4)获取避让系数ρ ＝ρ0exp(sinθ)；θ 为对象i关于对象j的避让角度参数；ρ0是常数

5)根据获得的状态信息以及避让系数，通过加入终端约束定义无限视界，使用含预警带的缓冲维诺单元构建避障约束，并在目标函数中引入与预警带相关的罚函数，构建分布式无人集群协同运动路径规划的优化问题及模型函数；并进行求解，得到输入信息 和位置信息 即实现避免死锁的分布式无人集群协同运动路径规划；具体过程如下：

51)构建分布式无人集群协同运动路径规划优化问题的模型函数，采用分布式模型预测控制方法实现运动路径规划；包括：分布式无人集群中任意一个对象i从初始位置 运动到目标运动位置为 每一个+

对象在每经过时间间隔h，取视界长度K∈Z构建并求解以下凸优化问题进行重规划；

构建的分布式无人集群协同运动优化路径规划优化问题的模型函数表示为：ij

e ＜ε

其中， 为视界k处的控制输入； 为需要规划得到的视界k处的状态量；

表示视界k处的位置序列； 示视界k处的速度序列；C为分布式无人集群协同运动路径规划优化模型的目标函数；amax为最大加速度；vmax为最大速度；

52)定义优化变量，包括控制输入 状态量 以及预警带变量ij

e ；其中自定义预警带变量；

i

规划变量中的初始位置规定 并且规定初始速度为 其中p (t)与为对象的在t时刻的实际位置以及在 时刻的实际速度，由测量获得；

53)定义约束，包括：动力学约束、避障约束、预警带约束、输入约束、速度约束和终端约束；其中自定义避障约束、预警带约束和终端约束；

动力学约束，表示为：

其中：

其中A1＝Id， A2＝hId， A3＝Id，Id表示维度为d的单位矩阵；还有：避障约束为通过含预警带的缓冲维诺单元对空间划分获得的线性约束，表示如下：ij

其中e ∈R为预警带变量；参数 与 定义如下：上一时刻即t‑h时在视界k处的规划结果，也称为预设路径；首次规划时取 其中p0为初始位置；拓展最小距离为 其中，rmin为两个对象之间的最小距离；

预警带约束表示为：

ij

e ＜∈

其中，ε∈R表示缓冲带的宽度；

输入约束为：

速度约束为：

终端约束为：

构建的分布式无人集群协同运动路径规划优化模型的目标函数C表示为式(2)：ij

其中，C为目标函数；Qk∈R是视界k处的权值常数； 为对象的目标位置；ρ ＝ρ0expij ij

(sinθ )是对象i关于对象j的避让系数，其中ρ0∈R是常数，θ 是对象i关于对象j的避让角度参数；

54)求解上述集群运动路径规划问题，得到的结果包括输入信息 和位置信息i

55)将求解结果 作为t时刻的输入u (t)输入到下层运动控制系统中，从而实现运动控制，并达到运动避障的目的；

6)根据优化问题的得到的位置信息结果 获取预设路径 其中，h为设置的时间间隔；通过上述步骤，即实现避免死锁的分布式无人集群协同运动路径规划。

2.如权利要求1所述避免死锁的分布式无人集群协同运动路径规划方法，其特征是，分布式无人集群中的个体对象包括无人机、无人车和无人艇多种形式。

3.如权利要求1所述避免死锁的分布式无人集群协同运动路径规划方法，其特征是，分布式无人集群中的个体对象同一维度d＝{2，3}维空间内运动。

4.如权利要求1所述避免死锁的分布式无人集群协同运动路径规划方法，其特征是，针对分布式无人集群中任意一个编号为i的对象，其从初始位置运动到目标运动位置的过程表示为：

其中 为状态信息的导数； 和 为运动学矩阵：i i i n

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

||u(t)||2≤amaxi

||v||2≤vmax.

其中，amax为最大加速度，vmax为最大速度；|| ||2表示向量的模。任意两个对象i与j之间避障需要满足以下条件：

i j

||p‑p||2≥rmin其中rmin是两个对象i、j之间的最小间距，小于这一间距时二者将会发生碰撞。

5.如权利要求1所述避免死锁的分布式无人集群协同运动路径规划方法，其特征是，实时定位系统具体是采用Vicon、Optitrack或GPS。

6.如权利要求1所述避免死锁的分布式无人集群协同运动路径规划方法，其特征是，下层运动控制系统具体是采用crazyfiles。

说明书

一种避免死锁的分布式无人集群协同运动路径规划方法

技术领域

[0001] 本发明涉及无人智能集群协同控制方法，具体涉及一种基于分布式模型预测控制，规避死锁且保证可行性的分布式无人集群协同运动路径规划方法。

背景技术

[0002] 分布式无人集群协同控制是一个复杂的系统的技术问题，集群中的个体可以是无人机、无人车和无人艇等多种形式，其需要解决的关键技术主要有信息交互机制、多机协同
环境感知技术、协同决策以及集群协同运动路径规划技术等。而集群协同运动路径规划是
其中的核心部分。

[0003] 针对集群协同运动路径规划算法，目前相对成熟的集群分布式协同运动路径规划算法主要有：争端解决法(conflict‑resolution)、人工势场法（potitientialflieds）、分
布式模型预测控制(distributed model predictive control)和几何法（geometric）。争
端解决法与人工势场法是当前常用的两种分布式协同运动路径规划算法，更多情况下仅起
有限的相互规避作用，无法保证集群间避障，几何法则无法考虑到被控对象的运动模型，仅
适用于少数应用场景如单纯的速度运动控制系统.。分布式模型预测控制方法中通过构建
含有位置约束以及动力学约束的凸优化问题，可以保证相互避障并考虑到被控对象本身的
复杂运动学模型。但现有的方法都无法处理因分布式无人集群缺乏中心协调器所带来的死
锁问题。死锁问题是指多个对象之间相互阻挡从而使各方都无法到达目的地，这一问题在
密集集群情况下更容易发生。现有技术尚缺乏能够避免死锁且保证可行性的分布式无人集
群协同运动路径规划方法。

发明内容

[0004] 本发明的目的是提供一种保证可行性和避免死锁的分布式无人集群协同运动路径规划方法，用以解决具有复杂动力学的集群系统协同运动路径规划问题，规避死锁问题
并保证可行性。

[0005] 本发明基于分布式模型预测控制，针对分布式无人集群运动路径规划，通过定义约束、和目标以及变量，构建分布式无人集群协同运动路径规划的优化问题及模型函数，具
体地，首先通过加入终端约束引入无限视界的概念，由此实现模型预测控制中优化问题解
的可行性，从而保证了运动路径规划算法的可行性，其次使用含预警带的缓冲维诺单元
（warningbufferedVoronoi cell）构建避障约束，并通过在目标函数中引入与预警带相关
的罚函数，由此保障运动路径规划算法的可行性，彻底规避死锁问题。

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

[0007] 一种避免死锁的分布式无人集群协同运动路径规划方法，分布式无人集群中的个体对象包括无人机、无人车和无人艇多种形式；分布式无人集群存在在同一维度空间内运
动的N个对象；N个对象在同一 空间内运动，在初始时刻 ，从初始位置 运动到
各自的目标位置 ，并在运动过程中相互之间不发生碰撞；针对分布式无人集群运动
路径规划，通过定义约束、和目标以及变量，构建分布式无人集群协同运动路径规划的优化
问题及模型函数，具体地，首先通过加入终端约束引入无限视界，实现模型预测控制中优化
问题解的可行性，从而保证了运动路径规划算法的可行性，其次使用含预警带的缓冲维诺
单元构建避障约束，并通过在目标函数中引入与预警带相关的罚函数避免死锁；分布式无
人集群协同运动路径规划方法包括如下步骤：

[0008] 1）初始化分布式无人集群中的预设路径，以对象i为例，其预设路径记为， 为初始位置，i为分布式无人集群中的对象； 为初始时间；视界
，其中K为视界长度；

[0009] 2）分布式无人集群中任意一个对象i和其他对象之间交流各自的预设路径信息；

[0010] 3）通过实时定位系统获取状态信息，包括位置信息以及速度信息；

[0011] 4）获取避让系数 ； 为对象i关于对象j的避让角度参数； 是常数；

[0012] 5）根据获得的状态信息以及避让系数，通过加入终端约束定义无限视界，使用含预警带的缓冲维诺单元构建避障约束，并在目标函数中引入与预警带相关的罚函数，构建
分布式无人集群协同运动路径规划的优化问题及模型函数；并进行求解，得到输入信息
和信息 ，即实现避免死锁的分布式无人集群协同运动路径规划；具体过程如下：

[0013] 51）构建分布式无人集群协同运动路径规划优化问题（模型函数），采用分布式模型预测控制方法实现运动路径规划；包括：

[0014] 分布式无人集群中任意一个对象i从初始位置 运动到目标运动位置为 ，其具有二阶系统的动力学；

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

[0016]

[0017]

[0018]                                                    式（1）

[0019] 其中， 为视界k处的控制输入； 为需要规划得到的视界k处的状态量；； 表示视界k处的位置序列； 表示视界k
处的速度序列；C为分布式无人集群协同运动路径规划优化模型的目标函数；

[0020] 52）定义优化变量，包括控制输入 、状态量 以及预警带变量 ；其中自定义预警带变量；

[0021] 规划变量中的初始位置规定 ，并且规定初始速度为 ，其中与 为对象的在t时刻的实际位置以及在 时刻的实际速度速度，由测量获
得；

[0022] 53）定义约束，包括：动力学约束、避障约束、预警带约束、输入约束、速度约束和终端约束；其中自定义避障约束、预警带约束和终端约束；

[0023] 动力学约束，表示为：

[0024]

[0025] 其中：

[0026]

[0027] 其 , , ， 表示维度为d的单位矩阵；还有：

[0028]

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

[0030]

[0031] 其中 为预警带变量；参数 与 定义如下：

[0032]

[0033] 是上一时刻即 时在视界k处的规划结果，也称为预设路径；首次规划时取 ，其中 为初始位置；拓展最小距离 ；
其中， 为两个对象之间的最小距离；

[0034] 预警带约束表示为：

[0035]

[0036] 其中， 表示缓冲带的宽度，由人为设定。

[0037] 输入约束为：

[0038]

[0039] 速度约束为：

[0040]

[0041] 终端约束为：

[0042]

[0043] 构建的分布式无人集群协同运动路径规划优化模型的目标函数 表示为式（2）：

[0044]   式（2）

[0045] 其中，C为目标函数； 是视界k处的权值常数； 为对象的目标位置；是对象i关于对象j的避让系数，其中 是常数， 是对象i关于对象
j的避让角度角度参数；

[0046] 54）求解上述集群运动路径规划问题，得到的结果包括输入信息 和位置信息；

[0047] 55）将求解结果 作为t时刻的输入 输入到下层运动控制系统中，从而实现运动控制，并达到运动避障的目的；

[0048] 6）根据优化问题的得到的位置信息结果 ，获取预设路径 ；其中，h为设置的时间间隔；

[0049] 即：将位置信息结果 设置为预设路径 ；其中，k指第k个视界处；h为设置的时间间隔；t为时刻；i指第i个对象。

[0050] 通过上述步骤，即实现避免死锁的分布式无人集群协同运动路径规划。

[0051] 本发明具体实施采用crazyfiles无人机系统实现本发明方法的流程。具体实施时，采用在python语言中通过凸优化函数包CVXPY以及与之配套的求解器MOSEK进行求解上
述优化模型，可以在较短时间内获得准确的全局最优解，并且具有极高的可靠性。

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

[0053] 本发明提出一种保证可行性且避免死锁的无人集群协同运动路径规划方法。基于分布式模型预测控制，通过加入终端约束引入无限视界的概念，由此实现了优化问题解的
可行性，从而保证了运动路径规划的可行性。其次通过引入含有预警带的缓冲维诺单元
（warningbuffered Voronoi cell）划分不同对象的运动空间，并通过在目标函数中引入与
预警带相关的罚函数，规避死锁问题。这一方法可以使无人集群在运动路径规划的保证可
行性以及规避死锁，特别适用于动态环境、具有复杂动力学要求、高机动性情况下的密集集
群环境的运动路径规划。可以应用到如多导弹协同打击、多机器人协同运输、多无人机协同
侦察等情形。

附图说明

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

[0055] 图2是本发明方法的流程框图。

[0056] 图3是本发明具体实施例基于optitrack和crazyflies实现本发明方法的流程框图。

[0057] 图4是获取避让角度参数 的示意图。

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

具体实施方式

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

[0060] 本发明提供一种保证可行性和避免死锁的分布式无人集群协同运动路径规划方法，能够解决具有复杂动力学的集群系统协同运动路径规划问题，规避死锁问题并保证可
行性。

[0061] 图1所示为具体实施时实现本发明方法采用的实验平台的结构，本发明方法的整体流程如图2所示。以下是规避死锁且保证可行性的分布式无人集群协同运动路径规划中
每一个对象的具体过程：

[0062] 1）初始化分布式无人集群中的预设路径，以对象i为例，初始化的预设路径记为， 为预设路径的初始位置，i为分布式无人集群中的对象；为初始时刻；k为视
界序号；之后进入主循环；

[0063] 2）分布式无人集群中任意一个对象 和其他对象之间交流各自的预设路径信息；

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

[0065] 4）获取避让系数 ； 为对象i关于对象j的避让角度参数；是常数；

[0066] 5）在获得状态信息以及避让系数之后，利用CVXPY构建优化问题，并在CVXPY中使用MOSEK求解器进行求解，获得的结果包括输入信息 和位置信息 ；将获得的结果
中的输入信息 设为控制输入；

[0067] 6）然后利用以上优化问题的结果中的位置信息 ，获取新的预设路径 ；

[0068] 7）最后输入控制量 到下层运动控制系统（crazyfiles）。通过控制输入实现分布式无人集群中的对象的运动过程。

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

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

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

[0072]

[0073] 其中 为状态信息的导数； 和 为运动学矩阵：

[0074]

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

[0076]

[0077] 其中， 为最大加速度， 为最大速度； 表示向量的模。任意两个对象i与j之间避障需要满足以下条件：

[0078]

[0079] 其中 是两个对象i、j之间的最小间距，小于这一间距时二者将会发生碰撞。

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

[0081]

[0082]

[0083]

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

[0085] 带有避障要求的运动路径规划问题，在转化为优化问题之后是一个典型的非凸优化问题或着说非线性规划问题，其存在求解速度慢，且不存在全局最优解的特点。因此如何
将其转化为一个凸优化问题，是模型预测控制中的核心问题，因此需要对以上凸优化问题
的构建进行细致的描述。

[0086] 在下文中，视界 ，其中K为视界长度。

[0087] 优化变量包括控制输入 、状态量 以及预警带变量，其中，本发明定义了预警带变量。

[0088] 为视界k处的控制输入， 为需要规划出来的视界k处的状态量，其中 表示视界k处的位置序列， 表示视界k处的
速度序列。此外规划变量中的初始位置规定为 ，并且规定初始速度为
，其中 与 为对象的在t时刻的实际位置以及在 时刻的实际
速度速度，由测量获得； 为预警带变量。

[0089] 约束包括：动力学约束、避障约束、预警带约束、输入约束、速度约束和终端约束；本发明定义了其中的避障约束、预警带约束和终端约束。

[0090]

[0091]

[0092] 其中：

[0093]

[0094] 其中 , , ， 表示维度为d的单位矩阵；还有：

[0095]

[0096] 避障约束：

[0097] 由含预警带的缓冲维诺单元对空间划分可以获得以下线性约束：

[0098]

[0099] 其中参数 与 定义如下：

[0100]

[0101] 是上一时刻即 时在视界k处的规划结果，也称为预设路径；首次规划时取 ，其中 为初始位置；拓展最小距离 ；
其中， 为两个对象之间的最小距离；

[0102] 预警带约束表示为：

[0103]

[0104] 其中， 表示缓冲带的宽度，由人为设定。

[0105] 输入约束：

[0106]

[0107] 速度约束：

[0108]

[0109] 终端约束：

[0110]

[0111] 终端约束的存在保证了优化问题解的存在性。

[0112] 构建的分布式无人集群协同运动路径规划优化模型的目标函数C表示为式（3）：

[0113]    式（3）

[0114] 其中，C为目标函数； 是视界k处的权值常数； 为对象的目标位置；是对象i关于对象j的避让系数，其中 是常数， 是对象i关于对象j
的避让角度角度参数，其获取如图4。图4中，xOy为人为规定的参考平面， 为对象i在视界
K处的预设位置， 为对象j在视界K处的预设位置。 为 与 之间的夹角。

[0115] 图3给出本发明具体实施采用crazyfiles无人机系统实现本发明方法的流程。

[0116] 本发明具体实施时，采用在python语言中通过凸优化函数包CVXPY以及与之配套的求解器MOSEK进行求解上述优化模型，可以在较短时间内获得准确的全局最优解，并且具
有极高的可靠性。

[0117] 本发明通过在优化问题模型中加入终端约束，可以将模型预测控制中的视界长度等效为无穷大，进而引入无限视界的概念。然后，基于预设路径使用含预警带的缓冲维诺单
元对空间进行分割，从而可以证明本次优化过程中所使用的预设路径满足本次凸优化中的
所有限制，以此保证本次凸优化的可行域非空，实现了不同时刻多个连续凸优化问题（即序
列凸优化）的递归可行性。本发明使用拓展最小距离 替代最小距离
，构成避障约束（式（11）‑（12）），相比于原有最小距离 只能考虑到采样点处的避障
情况，拓展距离最小 可以考虑到采样点之间时间段内的避障情况，从而进一步保证了
全过程内的避障可行性。因此，本发明方法保证了避障可行性。

[0118] 本发明还通过在避障约束中加入预警带变量 ，使得多个对象在相互阻挡过程中可以产生相互作用力，之后通过设定避让系数 控制这一相互作用力
的大小，产生一种右旋作用力，从而打破多对象间因均衡的作用力而产生的死锁状态。这一
右旋作用力可以通过凸优化问题最优解的KKT（Karush‑Kuhn‑Tucker）条件进行证明，并进
一步阐释 对打破这一力平衡的作用，通过设定 ，打破多对象间因均
衡的作用力而产生的死锁状态。

[0119] 综上描述的具体实施方法以及实施案例，本发明基于分布式模型预测控制，通过加入终端约束引入无限视界的概念，实现了优化问题解的可行性，从而保证了运动路径规
划的可行性。再通过引入含有预警带的缓冲维诺单元（warningbuffered Voronoi cell）划
分不同对象的运动空间，并通过在目标函数中引入与预警带相关的罚函数，规避死锁问题。
具体实施表明，本发明方法可以使无人集群在运动路径规划的保证可行性以及规避死锁，
特别适用于动态环境、具有复杂动力学要求、高机动性情况下的密集集群环境的运动路径
规划。可以应用到如多导弹协同打击、多机器人协同运输、多无人机协同侦察等情形。

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