基于约束优化的多智能体协同编队与避障

褚晶，李佩文，岳颀; Chu Jing; Li Peiwen; Yue Qi

2025年6月15日 22:54 星期日

网刊加载中。。。

使用Chrome浏览器效果最佳，继续浏览，你可能不会看到最佳的展示效果，

确定继续浏览么?

复制成功，请在其他浏览器进行阅读

目录contents

摘要

在多智能体系统的研究与实践中，编队路径规划尤为关键，不仅保障智能体的安全运行，也能提高系统执行任务的效率。本文提出了一种适应动态复杂环境的多智能体编队路径规划方法，确保系统在动态避障和编队形状保持方面的实时响应能力。开发了两种核心算法：一种致力于局部运动规划，另一种聚焦于全局路径规划。局部运动规划算法迭代应用半定规划和二次规划，求解得到智能体周围的无障碍凸区域，并通过连续凸优化技术优化编队参数。这一方法有效解决了多智能体编队在动态障碍物避让方面的问题，确保在遵守环境约束的同时实现编队的稳定保持。全局路径规划阶段进一步采用了这一思想，对自由空间中的无障碍凸区域进行采样，结合约束优化来计算起始编队和目标编队之间的过渡编队，并利用图搜索算法找到通往目标编队的最优路径。本文利用MATLAB搭建一个动态障碍物与静态障碍物并存的复杂仓储环境，验证了方法的有效性，并与虚拟结构法和概率路线图（Probabilistic roadmap，PRM）进行对比，展示了其在效率和准确性方面的优越性。

关键词

协同编队; 动态环境; 避障; 约束优化; 路径规划

随着自动化系统的迅猛发展，多智能体系统在编队协作方面展现出了显著优势。这类系统通过智能体的协同合作，能够更加高效和准确地完成复杂任务。以自动化仓储环境为例，当多个智能体需要协同搬运物品时，编队的稳定性对任务执行的效率和安全性具有决定性影响。在多智能体编队控制的研究和应用中，路径规划技术尤其关键。高效的编队路径规划策略不仅需要确保每个智能体的安全性和任务执行效率，还必须考虑到整体编队的协同作用和优化。特别是在动态和复杂的环境中，路径规划的难度大幅增加，智能体需要能够实时响应环境的变化，包括动态避障、保持编队形状和最优路径求解等。因此，开发一种能够高效处理这些挑战的多智能体编队路径规划方法变得尤为重要。

现有研究通常采用“全局路径规划+局部运动规划”的框架解决动态障碍物存在时的编队路径规划问题。

全局路径规划的主要目标是找到一条从初始构形到目标构形的无碰撞全局路径^［

1］。现有方法中，Kushleyev等^{［参考文献 2

百度学术}2］提出使用混合整数二次优化问题进行全局规划；Saha^{［参考文献 3

百度学术}3］依赖离散化线性时序逻辑进行全局路径规划；毛文平等^{［参考文献 4

百度学术}4］提出了一种自适应机制改进蚁群算法，以优化移动机器人在二维栅格地图下的全局路径规划。尽管这些方法为全局路径规划提供了理论保证，但在智能体数量增多时，它们的性能表现并不理想，且受限于特定形状的编队定义。另一种方法是通过随机采样来规划多智能体编队的路径^{［参考文献 5‑7}5‑7］，例如Barfoot和Clark，及Krontiris等^{［参考文献 8‑9}8‑9］使用的概率路线图（Probabilistic roadmap， PRM）方法。虽然这些方法能够定义安全的路径构形，但它们通常忽视了多边形编队的形状。另外，虽然A^*算法、快速随机搜索树（Rapidly‑exploring random trees ，RRT^*）和势场法等传统全局路径规划算法^{［参考文献 10‑12}10‑12］可以规划全局最优路径，但它们往往以整个空间为对象进行计算，计算量大^{［参考文献 13

百度学术}13］，尤其当存在动态障碍物时仍然针对整个空间进行规划，进一步增加了计算量^{［参考文献 14

百度学术}14］。

此外，局部运动规划是保证多智能体编队在动态环境中运动的关键。以往研究中，提出了多种方法，如Conner等和Kallem等^［

15‑16］使用的三角化方法，以及Ayanian等^{［参考文献 17

百度学术}17］将三角化与路径规划功能结合用于多智能体运动规划，虽对单智能体运动规划有效，但在处理动态环境中的多智能体编队运动时则受限。传统方法如Lyapunov函数、模型预测控制、群体运动和领导者‑跟随者控制等^{［参考文献 18‑21}18‑21］，虽在静态或预测性较强的环境中表现良好，但在动态环境中（如存在移动障碍物的情况）可能难以适应^{［参考文献 22

百度学术}22］。这些局限性表明，在复杂动态环境中进行编队运动规划时^{［参考文献 23

百度学术}23］，需要更灵活和适应性强的方法来确保编队的有效性和安全性。

针对这些不足，本研究利用二次规划、半定规划、非线性规划等高效的优化技术，实现了多智能体编队的全局路径规划和局部运动规划。

相较于PRM或纯采样策略在整个空间进行采样^［

24‑25］，本文提出的全局路径规划方法仅基于对无障碍自由空间的采样，并利用多边形表示自由空间中不同的凸区域。首先在起始位置和目标位置基于半定规划和二次规划的迭代使用生成相应的无障碍凸多边形区域，并通过约束优化技术计算出该凸区域内满足要求的编队构形。然后通过采样确定从起始区域到目标区域的一系列无障碍凸多边形区域，在此基础上，基于图论方法构建出连接初始和目标编队构形的过渡自由凸区域。最后结合A^*算法在这些区域内规划编队的最优路径，确保了路径规划的效率和适应性。此外，本文介绍了一种局部运动规划方法，该方法基于半定规划和二次规划的迭代使用，求得智能体周围的无障碍凸区域，并在该区域内基于非线性规划来优化编队的参数，如尺寸和位姿，以确保在复杂动态环境中，多智能体在保持编队结构的同时，能够有效地避免碰撞。该方法的实施步骤详见图1，其优势在于能够适应动态障碍物与静态障碍并存的复杂环境，同时保持编队的稳定性。

图1 方法概述

Fig.1 Method overview

1 问题定义与假设

1.1　问题描述

在自动化和机器人技术领域，高效规划多智能体系统在充满静态与动态障碍物的复杂环境中的路径，是一个重大挑战。在这种环境下，多智能体编队不仅需要保持其结构完整性，还要有效避开障碍物，确保任务安全、有效地完成。本文提出的路径规划策略集中于解决地面智能体编队在复杂环境中的运动规划问题，尤其是在动态和静态障碍物共存的情况下。通过结合局部运动规划和全局路径规划的方法，并运用约束优化技术，专注于编队构形的控制和路径优化，未深入探讨动力学模型，目的在于多智能体编队在动态复杂环境下的控制和路径规划问题。

1.2　假设条件

为了构建实用和全面的路径规划模型，提出以下假设条件。

（1）环境特征：假定环境中存在静态和动态障碍物，且所有障碍物的形状均为多边形结构。

（2）动态障碍物的特性：对于动态障碍物，其移动轨迹和速度已知。每个动态障碍物的形状、初始位置和方向也被视为已知信息。

（3）路径规划的边界条件：智能体的起点和终点位置是预先设定的，同时，智能体的初始构形也是已知的，包括其位姿和编队的尺寸。

1.3　优化目标

在多智能体编队路径规划问题的解决过程中，一个关键目标是确保智能体在面对复杂环境时能保持无碰撞的状态，同时识别并通过安全区域 $Ρ'$ ，以便为整个编队规划出一条无障碍的路径。此外，为了精确控制编队的结构——尺寸、位置及方向，本研究引入了同构变换的概念来定义多智能体编队系统。这一定义涉及3个主要的约束参数： $s' \in R^{+}$ 表示编队形状的尺寸，可以通过控制编队形状的顶点凸包的大小来实现； $t \in R^{2}$ 表示编队质心的位置，通过控制编队形状的顶点凸包的中心位置来实现；以及由单位四元数 $q \in S_{S O} (2)$ 表示编队质心的旋转方向（共轭用 $\bar{q}$ 表示），可以通过旋转编队形状的顶点凸包来实现。利用这3个约束参数来描述一个编队构形 $z = [t, s', q] \in R^{2} \times R^{+} \times S_{S O} (2)$ 。

这种控制策略要求对智能体之间的相对位姿进行调整，以确保在复杂环境中维持所期望的编队形态。为达到这一目标，确定以下主要的优化目标。

（1）高效避障：在环境中有效处理静态和动态障碍物，确保智能体在避免碰撞的同时，沿最优路径移动。

（2）编队保持：在整个移动过程中，算法应在绕过障碍物的同时保持编队构形。

（3）动态适应性：算法应具有高度的适应性，能够根据环境变化和障碍物的动态特性实时调整路径。

1.4　问题分析

基于上述假设和目标，本文提出一种综合路径规划策略。这包括局部运动规划，以应对动态环境中的即时避障需求，以及全局路径规划，以确保从起始点到目标点的整体路径最优化。这种策略将结合现有的路径规划算法（A^*算法）和非线性优化技术，以处理复杂环境中的路径规划挑战。

所提出的方法将在MATLAB模拟环境中进行验证，证明其在实际应用中的实用性和有效性。通过这种方法，本研究旨在为多智能体编队在复杂环境的路径规划问题中提供一个安全、高效的运动规划解决方案。

2 动态复杂环境中编队路径规划问题的建模

本文提出的多智能体协同编队运动规划方法通过将整个规划问题建模为约束优化问题：包括路径优化、避障和编队保持等相关约束，以有效应对环境的复杂性，实现安全的路径规划。

2.1　环境模型

在复杂环境中对静态和动态障碍物进行精确建模，以便于多智能体编队的有效运动规划。这一建模过程涉及详细定义环境的几何和空间特性，以及障碍物的具体参数。

首先，定义x轴方向、y轴方向的环境边界为［l_b，u_b］：上界u_b，下界l_b。接着，在全局地图下定义了一组静态障碍物 $O \subset R^{2}$ ，如墙壁或货架。这些静态障碍物通过其质心的位姿（x，y，θ）来定义，位姿包括二维空间中的坐标位置。除此之外，障碍物的包围盒尺寸 $(l, w)$ （包括长度和宽度）也被明确规定，从而得到每个障碍物的顶点表示。这些顶点表示不仅有助于更准确地刻画障碍物的形状和尺寸，也为智能体提供了避障的关键信息。

对于动态障碍物 $O (t) \subset R^{2}$ ，如其他智能体或多智能体编队，考虑其位姿 $k = [x (t), y (t), θ (t)]$ 随时间变化的情况，并通过速度 $v = [v_{x (t)}, y_{y (t)}]$ 来描述其运动特性。此外，为动态障碍物模型施加位置约束： $[m i n (k + v * t), m a x (k + v * t)]$ ，确保它们不会超出环境的上下界限［l_b，u_b］。

通过这样的建模方法，能够在复杂环境中为多智能体编队提供精确的空间和障碍物信息，从而促进更有效和安全的运动规划。

2.2　无障碍凸区域空间模型

本文提出了一种基于凸优化理论的方法来建立多智能体编队的无障碍空间模型，这个模型的主要目的是确定安全通过的区域 $Ρ'$ ，从而帮助智能体规划无碰撞路径。

通过交替使用二次规划和半定规划方法，将避障问题进行凸化。这一过程依赖于静态和动态障碍物模型的顶点映射，以计算环境中分离超平面集及其内部的最大面积内切椭圆。在该超平面集内部的编队能够有效避开所有障碍物。

基于二次规划求得的分离超平面集是一种能够将可通行区域与障碍物区域分隔开的线集合。在这个过程中，定义变量 $w$ 为超平面的法向量， $b'$ 为常数项。二次规划的目标是最小化 $‖w‖$ ，以找到最佳的分离超平面。同时，引入约束条件确保所有的障碍物点全部位于超平面的一侧

w^{T} O_{i} + b' \geq 1

（1）

式中 $O_{i}$ 为障碍物顶点集。式（1）能确保只有无障碍物区域被分到可通行区域，而障碍物区域被分割到不可通行区域。

得到分离超平面集后，构建半定规划问题来确定最大无障碍凸区域。定义一个椭圆 $E' (C, d) = \{x = C \bar{x} + d | ‖\bar{x}‖ \leq 1\}$ ，其中， $E'$ 表示一个矩阵形式的椭圆， $C (\bar{x})$ 表示C对输入向量x的线性变换，d为偏置项，表示椭圆中心在空间中的位置。半定规划目标是最大化椭圆的面积，并同时满足以下约束条件：椭圆必须完全包含在分离超平面的一侧。通过调整矩阵C和偏置项d，可以生成不同大小和形状的椭圆，以适应因动态障碍物移动导致的环境变化。

提供如图2所示的两个示例来展示该方法的应用。在一个简单的二维环境中设置了矩形障碍物，在另一个复杂的二维环境中设置了多个不规则障碍物。通过遍历每个障碍物顶点并利用二次规划生成与其相切的超平面，定义了一组线性约束的相交点，即无障碍凸多边形。接着，使用半定规划在多边形内寻找最大椭圆，进而定义出新的无障碍凸多边形区域。

图2 二维环境凸区域生成

Fig.2 Generation of convex regions in a 2D environment

在多智能体编队任务中用超平面的并集描述无障碍凸区域 $Ρ'$ 可为后续协同编队提供无障碍环境，以确保智能体可以安全地移动并避免碰撞。

2.3　编队模型

编队模型的建立旨在实现多智能体编队任务中的无碰撞移动和编队形状的控制。具体而言，模型以一个由多个智能体构成的正方形编队结构f为基础，如图3所示。在这个结构中，使用一个集合 $\{f_{1}^{i}, f_{2}^{i}, \dots, f_{n}^{i}\}$ 来描述智能体相对于编队质心的顶点，同时引入一个距离参数 $d_{0}^{i}$ 表示编队中任意两个智能体之间的距离，确保智能体在编队中保持此固定距离，从而实现编队的稳定性。

图3 编队定义

Fig.3 Formation definitions

为了简化问题，假设每个智能体的运动学相同，并用 $\{p_{1}^{i}, p_{2}^{i}, \dots, p_{n}^{i}\}$ 来表示它们的位置。编队的外部轮廓由智能体在编队中的位置构成的凸包来表示，这个顶点凸包包围了所有智能体，可通过将智能体的编队位置与相对顶点结合来构建编队模型。

编队优化变量可由x=（t，s'，q）决定，将智能体的位置和编队的顶点利用上述变量表示为

\begin{array}{l} p_{j}^{i} = t + s' r o t (q, p_{j}^{i}) \forall j \in [1, n] \\ f_{j}^{i} = t + s' r o t (q, f_{j}^{i}) \forall j \in [1, n_{i}] \end{array}

（2）

式中：“rot”表示旋转操作，用于改变方向和角度，且大小和位置不会发生改变。 $S_{S O} (2)$ 中的旋转可定义为

{[\begin{matrix} 0 & r o t (q, x) \end{matrix}]}^{T} = q \times {[\begin{matrix} 0 & x \end{matrix}]}^{T} \times \bar{q}

（3）

对于编队 $f$ 和构形 $z$ ，可表示为外部顶点的集合

V (z, f) = [v_{1}^{f}, v_{2}^{f}, \dots, v_{n}^{f}]

（4）

该构形空间z表示了智能体在运动过程中的位置和姿态的集合，通过控制编队构形参数使得多智能体能够达到一致，实现多智能体系统中所有个体的位姿状态趋于一致确保编队保持。

3 局部运动规划器

局部规划是本研究的关键组成部分，采用了无障碍自由空间凸区域的概念，特别强调了对动态障碍物的规避策略。在全局路径规划器确定了目标位置g、目标姿态q和尺寸 $\bar{s}'$ 之后，局部运动规划算法运用非线性优化技术，在已确定的无障碍凸区域内对编队参数进行精细调整。这一过程不仅确保了智能体编队在避免碰撞的同时保持其形状，而且还实现了有效的局部路径规划，从而计算出在动态环境中多智能体编队的安全路径。

3.1　无障碍凸区域

在多智能体编队运动中，可能遇到突发动态障碍物，增加碰撞风险。为了保证运动过程中的安全性，本节将研究在保持编队几何形状的同时实现动态避障的问题。采用基于凸优化的方法，结合二次规划和半定规划，计算分隔障碍物的超平面集和最大椭圆，从而动态更新智能体位置的约束，确保无碰撞状态。这一方法能有效应对动静态障碍物，保障智能体在复杂环境中的安全运动。图4清晰展示了算法的流程，从障碍物识别到避障，提高了编队任务的安全性和效率。

图4 算法流程图

Fig.4 Algorithm flowchart

3.1.1　分离超平面的确定

由起点位置给定一个初始椭圆 $E' (C, d) = \{x = C \bar{x} + d | ‖\bar{x}‖ \leq 1\}$ 。利用线性约束代替超平面集合： $Ρ = (x | A x \leq b)$ ，用于限定一组超平面的性质和位置。当寻求最大的无碰撞空间时，目标是找到最大的内接椭圆。椭圆的定义赋予了一种相对简单的方式来最大化椭圆面积，因为椭圆面积与 $C$ 的行列式的对数成正比，而行列式的对数 $l n (d e t C)$ 是一个凹函数。因此，可以有效地进行最大化。目标函数可表示为

m a x l n (d e t C)

（5）

可以通过寻找与障碍物边界相交的直线来获得分离超平面，同时确保该平面与内接椭圆相切。椭圆区域的确定及其与障碍物分离的超平面需要满足以下约束条件

a_{i}^{T} v_{k} \geq b_{i}, v_{k} \in O_{i} i = 1,2, \dots, N

（6（a））

\underset{‖\bar{x}‖ \leq 1}{s u p} a_{i}^{T} (C \bar{x} + d) \leq b_{i} \forall i = \{1,2, \dots, N\}

（6（b））

式中： $a_{i}$ 为矩阵 $A$ 的行元素， $b_{i}$ 为向量 $b$ 的元素， $O_{i}$ 为环境中动态障碍及静态障碍（ $i = 1,2, \dots, N$ ）中的顶点集合（由障碍物坐标信息，通过MATLAB Convhull函数计算障碍物凸包获取顶点集合）。约束条件1（式6（a））可使障碍物 $O_{i}$ 的所有顶点被分隔到由 $a_{i}^{T} x = b_{i}$ 定义的超平面的一侧。条件2（式6（b））约束确保椭圆 $x = C \bar{x} + d$ 处于该平面另外一侧。将避碰问题转化为对障碍物集合 $O_{i}$ 添加约束条件，确保椭圆体与障碍物完全分离。

3.1.2　距椭圆最近点的确定

本小节将分离超平面的确定问题简化为求解单一的最小距离的优化问题。设椭圆为 $Ε' (C, d)$ ， $v_{i 1}, v_{i 2}, \dots, v_{i m}$ 为凸障碍 $O_{i}$ 的顶点集合。为了找到距离原点最近的障碍物上的点，利用 $C v_{i k} + d$ 映射来找到 $O_{i}$ 上距离椭圆最近的点 $x^{*}$ 。将距离障碍物最近点的查找问题转化为一个二次规划问题。优化目标是确定椭圆 $Ε'$ 内部与障碍物不相交的一点。

a r g m i n {‖\bar{x}‖}^{2}

（7）

点 $\bar{x}$ 即为距离原点最近的点，它是 $v_{i j}$ 的凸集合。

\begin{matrix} [v_{i 1}, v_{i 2}, \dots, v_{i m}] η = \bar{x} \\ \sum_{j = 1}^{m} η_{i} = 1 η_{i} \geq 0 \end{matrix}

（8）

优化生成 ${\bar{x}}^{*}$ ，通过映射 $x^{*} = C {\bar{x}}^{*} + d$ 求得障碍物 $O_{i}$ 上距离椭圆最近的点 $x^{*}$ 。

3.1.3　求解椭圆切线

如2.2小节中所述，通过求解 $Ε'$ 的逆来找到椭圆的切线： $E' = \{x | {(x - d)}^{T} C^{- 1} C^{- T} (x - d) \leq 1\}$ ，计算椭圆的闸函数在 $x^{*}$ 处的梯度找到垂直于椭圆表面的向量

a_{i} = \frac{d}{d x} {[{(x - d)}^{T} C^{- 1} C^{T} (x - d)]}_{x^{*}} = 2 C^{- 1} C^{- T} (x^{*} - d)

（9）

由于 $x^{*}$ 在超平面上，通过上述求得的 $a_{i}$ ，继而找到 $b_{i}$ 为

b_{i} = a_{i}^{T} x^{*}

（10）

3.1.4　求解凸多边形中最大椭圆

由3.1.1椭圆定义及椭圆面积的性质，将求解多边形 $Ρ = \{x | A x \leq b\}$ 中内接椭圆问题表述为求解以下凸优化问题：

\begin{array}{l} \underset{C, d}{m a x} l n (d e t C) \\ s . t . \underset{‖\bar{x}‖ \leq 1}{s u p (a_{i}^{T} C \bar{x}) + a_{i}^{T} d} \leq b_{i} \forall i = [1,2, \dots, N] C ≽ 0 \end{array}

（11）

由文献［

26］知，可将问题转化为不包含

\bar{x}

的形式

\begin{array}{l} \underset{C, d}{m a x} l n (d e t C) \\ s . t . ‖a_{i}^{T} C‖ + a_{i}^{T} \leq b_{i} \forall i = [1,2, \dots, N] C ≽ 0 \end{array}

（12）

利用MATLAB Mosek求解器中Ben‑Tal and Nemirovski所提出的半定规划及二次约束规划对上述约束优化问题进行求解，计算出椭圆，并在每次迭代中交替使用这两种凸优化算法，超平面集和内接椭圆都将被更新，以此扩大内切椭圆的面积以求得最大的无障碍空间，从而扩大移动智能体的安全通行范围。最后利用A^*算法在无障碍空间内求得全局最优路径。

通过分析一组简单实验来验证该计算方法，如图5所示。在环境中设置多个不规则障碍物（灰色），利用起点（绿色）信息在起点周围生成一个无障碍的凸区域。通过多次迭代最大化移动智能体的无障碍运动规划：左边图像表示生成分离障碍物的超平面，利用生成的超平面集来表示无障碍凸多边形结构（红色），右边图像表示生成多边形的内接椭圆（蓝色）。其中最终求得的凸多边形结构即为无障碍凸区域 $P'$ 。凸区域的生成可以避免由于智能体移动而造成的碰撞危险，同时降低了现有路径问题解决过程中可能遇到的复杂环境情况。

图5 二维障碍物环境中无碰撞凸区域的迭代生成示例

Fig.5 Iterative generation example of collision‑free convex regions in two‑dimensional obstacle environment

该方法基于数学优化将碰撞问题转化为优化问题，不会受限于环境中障碍密集、作业区间狭窄以及动态障碍存在的情形，并结合A^*算法在全局环境中寻找最优路径，提高智能体编队的避障能力和路径规划效率。

3.2　编队控制

本节在得到无障碍凸区域的前提下，提出了一个用于计算目标编队的算法。为了有效实施此算法，首先需要明确地定义多智能体编队问题的关键要素，这包括确定智能体的数量、它们的初始状态、目标编队的具体要求，以及在编队运动过程中需遵守的各项约束等。这些定义为编队算法的实施提供了必要的前提条件和框架，确保了算法能够针对具体的编队需求进行有效的规划和调整。

3.2.1　编队的顶点凸包

本算法首先关注计算多智能体目标编队的位置凸包 $C'$ 。位置凸包是指由智能体在空间中的位置形成的凸多边形，这个凸多边形是最小的凸形状，覆盖了所有智能体的位置。在编队中，这个位置凸包有助于确定整个编队的形状和位置。通过计算位置凸包，能够为编队的整体移动提供必要的信息，可通过MATLAB中的Convhull函数实现，输入数据为每个智能体的位置坐标。

3.2.2　最优编队

根据2.3节编队模型的建立可知编队构形 $z$ 的表示，通过对每个智能体 $i$ 进行非线性优化，优化过程的主要目标是最小化编队构形参数的误差，使实际编队的尺寸、位置和方向尽可能接近期望值。为了实现这一目标，构建一个目标函数。这个目标函数包括两个方面的优化项：

（1）各智能体相对于期望目标编队 $g$ 的质心位置参数 $t$ 的偏差加权和最小，以确保编队的质心位置接近期望值；

（2）期望编队形状的偏差以及编队期望方向的参数偏差最小，以确保编队的大小和方向接近期望值。

从而优化问题的目标函数如下

\underset{x = (t, s', q)}{a r g m i n} {‖t - g‖}^{2} + ω_{s} {‖s' - \bar{s}'‖}^{2} + ω_{q} {‖q - \bar{q}‖}^{2} + c_{f}

（13）

通过调整每个智能体的位置和方向，目标函数的最小化过程可以实现多智能体编队的控制目标。当编队偏离期望值时，编队形状的尺寸和位姿将被最小化，从而使编队尽可能接近期望构形。

3.2.3　避碰避障

为了解决多智能体编队系统的障碍物避碰避障问题，根据3.1节相关内容求解了无障碍凸区域的最优控制问题。椭圆被构建以最大程度地适应超平面，且它的大小受到超平面的约束。对于每个超平面，确定其内部的最大椭圆，这进一步生成了一个最大的无障碍空间。所以，最大椭圆的生成伴随着最大的无障碍超平面集的生成，求得的最大超平面集即为障碍环境中最大的凸多边形可行区域，将该可行凸多边形描述成约束形式 $P = \{x | A x \leq b\}$ ，作为最优控制输入施加于协同编队控制优化问题中以求解满足避障要求的控制率。

上述编队避障控制率可表示为一个具有约束的非凸问题，以一种动态更新的不等式约束来约束智能体远离障碍物。由式（2）编队的顶点和智能体的位置表示及可行凸多边形 $Ρ$ 的求解，约束条件可表述为

A (t + s' r o t (q, f_{0, j}^{f})) \leq b \forall j \in \{1,2, \dots, n_{f}\}

（14）

约束条件确保编队凸包 $C'$ 的所有顶点都在凸多边形 $P$ 内，智能体不会与任何障碍物碰撞。

在多智能体编队中，编队中的每个智能体都需要遵守一定的约束条件：任意相邻智能体之间保持期望的相对距离，以确保它们之间不会发生碰撞，约束可以表示为

\underset{k \to \infty}{l i m} (p_{j} (k) - p_{i} (k)) = d_{f} j \in n_{i}

（15）

式中： $n_{i}$ 为第 $i$ 个智能体相邻的智能体集合， $d_{f}$ 为两相邻智能体间的最小期望距离。约束条件确保编队中智能体之间不会发生碰撞。

要跟踪一个智能体的旋转移动，可以将其初始方向表示为单位四元数，然后根据智能体的旋转运动更新四元数。通过对单位四元数进行更新，可以跟踪智能体的位置和方向变化，式（16）对四元数进行约束使其具有单位长度。

{‖q‖}^{2} = 1

（16）

将上述约束条件集成到编队模型中，提供多智能体编队的规划运动。并利用MOSEK非线性来求解由式（13~16）构成的非凸优化问题。

3.2.4　智能体被分配到编队中的位置

根据目标编队 $f^{*}$ 以及式（2）中定义的多智能体编队位置集合 $\{p_{1}^{*}, p_{2}^{*}, \dots, p_{n}^{*}\}$ ，将智能体的代价函数定义为其位置到编队顶点位置的距离平方

U_{i} = {‖p_{i} - V_{i}‖}^{2}

（17）

式中： $U_{i}$ 为智能体 $i$ 的代价函数， $V_{i}$ 为分配给智能体 $i$ 的目标位置， $p_{i}$ 为智能体 $i$ 的当前位置。

定义总目标函数，它是智能体之间距离平方和的最小化，可以表示为

m i n {\sum ‖p_{i} - p_{i}^{*}‖}^{2}

（18）

总目标函数是智能体以最小编队误差完成编队，因此，以智能体位置到编队顶点位置的距离定义每个智能体的代价函数。智能体被分配到目标位置，并通过迭代优化，不断更新智能体的位置，使它们逐渐接近其目标位置，将每个智能体 $i$ 分配到目标位置 $p_{i}^{*}$ ，同时保持编队之间的安全距离，从而实现编队任务。

4 全局路径规划器

全局路径规划的目的是为多智能体编队计算从初始构形空间到目标构形空间的可行路径。本研究使用了采样法和受约束的非线性优化相结合的策略。

4.1　图论

在全局路径规划中，运用图 $G = \{V, E\}$ 表示和管理多智能体的路径和编队之间的关系。智能体的编队和位置被表示为图的节点 $V$ 。这些节点代表了多智能体可能处于的不同状态或构形。因此，图中的每个节点都对应着一个可行编队的构形 $z$ 。边 $E$ 连接了不同的节点，表示多智能体从一个构形过渡到另一个构形的可能性。如果存在一个凸多边形 $P$ ，凸多边形在图中表示了两个编队之间的连接关系，其中包含了这两个编队构形。这些边的存在意味着智能体可以在这两个编队之间变换，同时保持碰撞避免。

利用多边形 $Ρ'$ 的列表，对于每个多边形 $P \in Ρ'$ ，保留一个构形列表 $L_{P}$ ，其中包含智能体编队完全位于多边形内的构形。首先初始化节点列表和多边形列表，分别包括初始构形 $z_{s}$ （质心为 $s$ ）和目标构形 $z_{g}$ （质心为 $g$ ）相关信息。多边形列表被初始化为凸区域 $P_{s}$ 和 $P_{g}$ ，它们分别包含初始构形和最终构形。然后，基于采样法在工作空间中随机采样生成随机点 $p$ ，每个采样点对应于一个节点，边则连接了这些节点。它提供了从初始构形到目标构形的可能路径。如果这些点在障碍物或多边形内，采取剔除操作，否则将执行以下步骤。

（1）从随机点 $p$ 开始，生成一个凸多边形 $P_{p}$ 。

（2）对于与 $P_{p}$ 相交的每个多边形 $P \in Ρ'$ ，计算构形 $z$ 和编队 $f$ ，使得编队的顶点完全包含在两个多边形的交集内 $V (z, f) \subset P_{p} ⋂ Ρ'$ ，且最小化其质心到目标点 $g$ 的平方距离。构形 $z$ 和编队 $f$ 通过第3.2节的非线性优化方法计算。对于多边形 $P$ ，用formation $(P)$ 来表示这个函数。如果存在有效的构形，则会将其添加到节点列表中。

（3）如果一个有效的构形 $z$ 被添加到节点列表中，则为所有构形 $z_{i} \in L_{P_{p}}$ 添加一条边 $(z, z_{i}, P_{p})$ 来连接新节点和与之相交的多边形内的节点，这保证了智能体可以通过相关的凸多边形在这两个编队之间移动。

随着采样的进行，图会逐渐增长，包括新的节点和边。一旦图建立完成，多智能体需要在图中搜索找到一条最佳路径，以在构形空间之间切换，而不与障碍物碰撞。在图中执行图搜索A^*算法，对于两种构形 $z_{1}$ 和 $z_{2}$ 之间的每条边 $E$ ，用 $z_{1}$ 和 $z_{2}$ 的质心之间的欧式距离 $d (z_{1}, z_{2})$ 来定义其代价，以找到最优路径。

算法1 全局运动规划

（1） Given：障碍 $O$ ，起始构形 $z_{s}$ （质心 $s$ ）和目标构形 $z_{g}$ （质心 $g$ ）

（2）返回：从 $s$ 到 $g$ 的可行编队构形的路径 $L$

（3）初始化图 $G \{V, E\}$ ： $V = \emptyset; E = \emptyset$

（4）初始化空多边形列表 $Ρ' = \emptyset$

（5）将初始构形添加到节点列表 $V \leftarrow z_{s}$ 中

（6）从 $s$ 和 $g$ 中生成 $P_{s}$ ， $P_{g}$ （添加到多边形列表中）

（7） $Ρ' \leftarrow P_{s}, Ρ' \leftarrow P_{g}$ （计算目标区域中的有效构形）

（8） $z_{g}$ = formation $(P_{g})$

（9）将目标构形添加到节点列表 $V \leftarrow z_{g}$ （为两个多边形创建有效构形列表）

（10） $L_{P_{s}} = \{z_{s}\}, L_{P_{g}} = \{z_{g}\}$ （检查开始和目标是否可以连接）

（11） if $\exists z$ =formation $(P_{s} ⋂ P_{g})$ then

（12） $V \leftarrow \{z\}$

（13） $E \leftarrow \{z_{s}, z, P_{s}\}$

（14） $E \leftarrow \{z_{g}, z, P_{g}\}$

（15） end if

（执行以下搜索循环，直到整个空间被探索找到第1个可行路径）

（16） while 循环未结束 do

（17）基于采样生成随机样本点 $p$

（18）从 $p$ 生成多边形 $P_{p}$

（19） if $\exists z$ = formation $(P_{p})$ then

（20） $L_{P_{p}} = \emptyset$

（21） for $P \in Ρ'$ do

（22） if $z_{1}$ =formation $(P ⋂ P_{p})$ then

（23） for $z_{i} \in L_{P}$ do

（24） $E \{z_{1}, z_{i}, P\}$

（25） end for

（26） for $z_{i} \in L_{P_{p}}$

（27） $E \leftarrow \{z_{1}, z_{i}, P_{p}\}$

（28） end for

（29） $V \leftarrow z_{1}$

（30） $L_{P_{p}} \leftarrow \{z_{1}\}; L_{P} \leftarrow \{z_{1}\}$

（31） end if

（32） end for

（33） $Ρ' \leftarrow P_{p}$

（34） end if

（35） return $L$ =shortestPath $(G)$

（36） end while

算法2 $z$ =formation $(P)$

Input：凸多边形 $P$

Output：一个有效的构形 $z$ ，使 $V (z, f) \subset P$ 或 $\emptyset$

（1） if $P = \emptyset$ then

（2） return $\emptyset$

（3） else

（4） return 第3.2节的凸多边形 $P$ 的非线性优化

\underset{x = (t, s', q)}{a r g m i n} {‖t - g‖}^{2} + ω_{s} {‖s' - \bar{s}'‖}^{2} + ω_{q} {‖q - \bar{q}‖}^{2} + c_{f}

A (t + s' r o t (q, f_{0, j}^{f})) \leq b \forall j \in \{1,2, \dots, n_{f}\}

（

V (z, f) \subset P

）

\underset{k \to \infty}{l i m} (p_{j} (k) - p_{i} (k)) = d_{f} j \in n_{i}

（5） end if

4.2　结合局部规划

可由序列 $L = (z_{s}, z_{s + 1}, \dots, z_{g})$ 定义一全局路径，序列中的每个构形z为多智能体提供了一个中间设定点，多智能体遵循中间设置点的序列，对于每个中间设置点，局部规划器根据环境信息实时更新与障碍物分隔的超平面集，该超平面集即为生成的无障碍凸多边形P，并控制编队顶点在该集合内保持不变。如果该凸多边形包含了多智能体的当前位置并指向中间设置点，则将智能体引导到相关的构形空间。路径规划流程如算法3所示。

算法3 最优路径L= shortestPath $(G)$

Input：多智能体编队的图模型 $G$

Output：一系列有效构形 $L = (z_{s}, z_{s + 1}, \dots, z_{g})$ 和凸多边形 $\{P_{s}, P_{s + 1}, \dots, P_{g}\}$ ，多智能体编队可以从起点运动至目标，或 $\emptyset$ 。

（1） if $z_{s}$ 和 $z_{g}$ 在 $G$ 中没有连接

（2） return $\emptyset$

（3） else

（4） return 在 $G$ 中使用A^*方法搜索结果，其中遍历一条边 $E = \{z_{1}, z_{2}, P\}$ 的代价由 $d (z_{1}, z_{2})$ 定义

（5） end if

5 仿真实验

本节将在MATLAB环境下搭建一个仿真环境，以验证所提算法的有效性。该实验将重点关注障碍物避障、编队形状控制和路径规划等关键方面。

为了全面评估所提算法的性能，将其与基于传统虚拟结构的编队控制方法及PRM算法结合使用的多智能体运动规划进行对比。这种对比将有助于评估新算法在多智能体协同运动规划中的表现，并验证其在复杂环境中的适应性和优越性

使用MATLAB创建仿真环境，模拟一个典型的仓储场景。考虑到规划过程中仓库内部布局可能包含货架、障碍物以及其他设施，路径规划需要充分考虑这些因素，以确保智能体能够安全且高效地到达目标位置。

在全局坐标系下，模拟一个包含动态障碍和静态障碍的复杂仓储环境（图6）。黑色区域表示静态墙壁和货架，蓝色区域代表移动的多智能体编队，灰色区域表示行走的员工，而黄色区域表示可移动的单个智能体。静态货架被布置在地图中的不同位置，给定静态货架的尺寸为 $6 m \times 2$ m；动态障碍物以一定速度和方向移动，其中单智能体尺寸为 $1 m \times 0.5$ m，初始位姿坐标（7 m，9 m）；行走员工的尺寸为 $1.5 m \times 1.5$ m，初始位姿坐标（15 m，9 m）；多智能体编队尺寸为 $2 m \times 2$ m，初始位姿为（7 m，15 m）。

图6 仓储障碍环境

Fig.6 Storage obstacle environment

通过这个仿真实验，可以观察并分析所提算法在实际应用场景中的表现，特别是在处理动态和静态障碍物时的效率和准确性，以及编队保持和路径规划的能力。

5.1　静态仓储环境下多智能体编队

在本研究的静态仓储环境模拟中，考虑4个智能体协同编队构形，并对其在环境中从起点（1.5 m，1.5 m）到终点（22 m，21 m）的最优路径进行规划。根据第2.2节中关于编队构形z的定义，假设这个编队采用正方形的形状，且每个智能体的尺寸为0.6 m×0.4 m。利用质心位姿及其尺寸来定义一个智能体，编队初始构形的空间信息与编队形状如表1及图7所示。

表1 智能体初始信息

Table 1 Initial information of agents

智能体	质心初始位置/m	智能体质心间相对距离/m
1	（0.5，1.5）	1.35
2	（1.5，0.6）	1.35
3	（2.5，1.5）	1.35
4	（1.5，2.4）	1.35

图7 编队形状

Fig.7 Formation shape

本文所提出的运动规划算法首先应用了MOSEK求解器，旨在求解出仓储环境下的最大无障碍凸区域P'优化问题。如图5所示的二维障碍环境示例中，采用超平面分隔方法进行障碍物避障，有效地将可行空间与障碍物区域分离。

本研究融合了局部规划器和全局规划器来控制编队在保持形状的前提下进行多智能体运动规划。从障碍物模型中提取的顶点信息用来表示障碍物的外轮廓，为局部运动规划提供了关键数据。局部规划算法结合环境中凸障碍物的顶点信息、周围边界以及起点的位置约束，通过迭代应用半定规划和二次规划的凸优化技术，在智能体附近的空间内生成最大的无障碍凸区域，这些区域由分隔超平面构成，表示为红色线段。使用超平面将可行空间和障碍物区分开来，超平面集合构成无障碍凸区域P。全局路径规划器基于这些多边形结构来表示自由空间中的凸多边形区域，并利用多边形的交集来保证编队在环境中无碰撞运动。此外，结合局部规划器通过受约束的优化来优化多智能体编队的参数，计算出一组目标编队的形状，并通过非线性优化来确保编队参数在该无障碍区域内保持稳定，从而实现在复杂环境中的安全运动。

首先由起点和终点信息，生成满足初始构形节点和目标构形节点的无障碍凸多边形区域，如图8所示。

图8 起点及终点位置处可进行编队优化设计的无障碍凸区域

Fig.8 Obstacle free convex areas of starting and ending positions optimized for formation design

针对一个具有静态障碍物的仓储环境，设计一种全局路径规划器来规划4个智能体的编队运动。基于图论 $G = \{V, E\}$ 对环境进行采样生成随机点，直到最后求得的无障碍凸多边形可表示两个编队构形之间的连接关系，确保编队运动过程中的平滑过渡，并且通过优化编队参数以形成一个安全、无碰撞的编队保持构型。在图9中，起始和最终的编队构形均用黄色表示、所有的采样点以绿色显示、多边形以红色突出，可行的路径由实心的绿色线条表示。优化后的编队构型同样以绿色标识，与采样点形成统一的视觉语言，同时区分起始和最终的编队构形。通常，在样本数量很少的情况下，就可以找到一个可行的路径。

图9 无障碍协同编队构形

Fig.9 Obstacle‑free formation configuration

最后，将全局路径规划算法与A^*算法结合，规划编队质心从起点到终点的运动路径，如图10所示，从而找到多智能体编队的最优路径。图10展示了编队质心的最优路径和整个编队的最优路径，其路径图显示出平滑的移动轨迹，没有出现不稳定的行为，证明了本文方法在实现编队保持的同时，也优化了智能体之间的位置分布，确保了编队的实际形状和分布与预期形状一致。

图10 路径规划图

Fig.10 Path planning

为了验证所提方法的有效性，本研究将其与传统的虚拟结构编队方法和PRM路径规划算法进行了对比。这两种传统方法在多智能体协同运动中均发挥关键作用：前者通过虚拟结构指导智能体形成有序编队，后者则通过概率性路径规划来应对环境的复杂性。设置智能体之间的距离为0.75 m，与虚拟点的距离为0.53 m。传统方法的路径规划结果如图11所示，所提方法与传统方法的对比效果详见表2。

图11 基于虚拟结构法的编队路径规划图

Fig.11 Formation path planning convex using virtual structure method

表2 静态仓储环境方法对比

Table 2 Comparison of methods in a static storage environment

变量	传统虚拟结构及 PRM方法^［ 27］	本文方法
路径节点数	26	4
与障碍物碰撞次数	2	0
路径拐点数	3	2
计算时长/s	62	50

在静态仓储环境下的实验结果显示，本文方法在构建从起点到终点的无障碍凸区域方面表现出高效的避碰能力，与A^*算法结合使用时能够生成更少的路径节点和拐点，展现出路径的平滑性。该方法在搜索过程中有效地剪枝，选择最优路径节点，从而实现了高效的路径规划。

相比之下，基于传统PRM方法的编队可能会发生与障碍物相碰的情况。这是因为PRM算法的规划并未考虑编队整体的避碰约束，且需要在全局仓储地图下进行大量采样和构建图的过程，通过在全局自由空间中采样点并连接有效路径，由于节点数量受到采样密度的影响，导致编队路径节点相对较多，从而增加了运算量和规划时间。实验数据表明，在相同的任务环境下，PRM方法的路径规划时间长于本文提出的方法。

在PRM算法基础上增加编队约束，对障碍物进行膨胀处理，增加编队尺寸约束，可以实现有效避碰，但仍无法完全避免PRM算法在采样时多智能体与障碍物碰撞的问题。而本文方法结合局部运动规划与全局路径规划，超平面分割无障碍空间的同时优化编队参数，无需对A^*路径规划算法及障碍物进行额外处理，便能够实现无障碍编队保持运动规划，为编队系统的高效和安全运行提供了可行的解决方案。

智能体编队之间相对距离的误差值变化曲线如图12，确保智能体运动过程中保持编队队形不变。由图12可知，本文提出的约束优化方法实现了编队中智能体之间相对距离稳定在期望值附近，使得编队达到了稳定状态。与此相反，在基于虚拟结构法的编队控制方法中，智能体之间的相对距离误差较大且波动起伏程度较大。

图12 速度及智能体间相对距离误差值

Fig.12 Error values for velocity, relative distance between agents

相对距离误差是智能体之间的实际相对距离与期望相对距离之间的误差值。相对距离误差保持在非常小的范围内，这表明智能体能够维持期望的相对位置。编队的结构得到了有效的维护，没有出现明显的偏差，从而有效实现了编队目标。

5.2　动态仓储环境下多智能体编队

引入动态障碍物验证复杂仓储环境下算法的有效性。设置动态障碍移动时长为2 s，时间间隔为0.5 s。障碍物移动规律及动态位姿变化如表3和4所示。

表3 动态障碍物移动规律

Table 3 Dynamics of moving obstacles

时间

单智能体/

（m·s）^-1

行走员工/

（m·s）^-1

多智能体编队/（m·s）^-1

前1.5 s

（3，-0.5）

（-0.75，-0.5）

（1.75，0.5）

后0.5 s

（-1.5，-1.5）

（-1，-0.5）

（10，-0.5）

表4 动态障碍物位姿变化

Table 4 Changes in the positions and orientations of dynamic obstacles

移动时长	单智能体位姿/m	行走员工位姿/m	多智能体编队位姿/m
动态移动0.5 s	（8.5，8.75）	（14.625，8.75）	（7.875，15.25）
动态移动1 s	（10，8.5）	（14，25.85）	（8.75，15.5）
动态移动1.5 s	（11.5，8.25）	（13.875，8.25）	（9.625，15.75）
动态移动2 s	（10.75，7.5）	（13.375，8）	（14.625，15.5）

利用无障碍凸优化算法，在动态环境下针对随动态障碍物移动的每一步，计算多智能体在起点和终点位置处可实现编队构形优化的无障碍凸区域，如图13所示。

图13 动态环境下起点及终点位置处可进行编队构形设计的无障碍凸区域

Fig.13 Obstacle free convex areas of starting and ending positions optimized for formation design in dynamic environment

当障碍物以一定速度移动时，算法可以找到动态环境下安全无碰撞凸区域，每一步的生成过程如图14所示（左侧为随障碍物移动生成的无障碍凸多边形区域，右侧为满足编队过渡的无障碍编队构形区域）。对比静态环境下无障碍凸区域图形（图9）。由图14观察到，局部规划器可根据障碍物移动实时位姿信息，分离超平面不断动态更新直至到达终点得到最终超平面集，同时优化编队参数，结合全局路径规划器规划出多智能体编队的无障碍构形空间。最后，通过将全局运动规划算法与A^*算法结合，规划了如图15所示编队质心的最优路径。

图14 动态环境协同编队构形

Fig.14 Cooperative formation in dynamic environment

图15 动态障碍物每一步移动下的路径规划

Fig.15 Path planning for each step of dynamic obstacle movement

图16展示了本文所提出的基于约束优化的编队路径规划图。图16通过优化编队参数，实现了编队构形的有效维持。可以看出多个智能体在动态环境中，按照特定的编队构形运动，展示了复杂的路径和实时的编队移动。此规划图不仅展示了复杂动态环境下路径选择的智能性，还突出了编队保持的优化过程，反映了全局路径规划和编队控制技术的应用。

图16 编队路径规划

Fig.16 Formation path planning

动态障碍物环境下，基于虚拟结构法及PRM路径规划算法的多智能体运动规划路径如图17所示，与本文方法的数据对比见表5。

图17 动态环境基于虚拟结构法路径规划图

Fig.17 Formation path planning diagram in dynamic environment using virtual structure method

表5 动态仓储环境方法对比

Table 5 Comparison of methods in a dynamic storage environment

变量	传统虚拟结构及 PRM方法^［ 27］	本文方法
路径节点数	26	7
与障碍物碰撞次数	8	0
路径拐点数	5	5
计算时长/s	62	60

动态环境下两种方法的多智能体编队之间相对距离的误差值变化曲线如图18所示。

图18 动态环境下编队速度及相对距离误差值

Fig.18 Error values for formation velocity, and relative distance in dynamic environment

基于二次规划和半定规划的凸优化算法的碰撞避免策略可以在静态环境及复杂动态环境下确保智能体成功避免与障碍物的碰撞，确保了编队的安全运行。相较之下，利用传统的PRM路径避障方法在动态环境下计算最优路径时，其运动过程中与障碍物碰撞的风险较大。

在具有约束条件的多智能体系统编队控制问题中，每个智能体之间的相对位置保持不变，使得智能体能够在沿期望轨迹到达目的地的过程中保持编队队形不变。为了评估编队控制的性能，通过测量编队智能体之间的相对距离误差来衡量，控制每个智能体与邻近智能体保持规定距离，以维持编队的整体布局，避免与其他智能体发生碰撞。

实验结果显示，控制算法成功地将多智能体编队成期望的形状。智能体之间的相互协作和控制策略确保了编队形状的稳定性和准确性。这表明，在多智能体编队控制中，基于约束优化算法的碰撞避免策略在面对静态和动态环境时能够有效确保编队的安全运行。

5.3　大尺度环境下的多智能体编队

为进一步验证本文提出算法的实用性和广泛适用性，模拟一个规模为 $100 m \times 100$ m的大型环境，其中布置了众多静态障碍物。这个扩展实验旨在测试算法在更广阔且障碍密集的环境下的避障能力和编队控制效率。

定义编队的起始位置为（1.5 m，1.5 m）和目标位置为（98 m，98 m）。应用所提出的编队避障算法，规划编队从起点到终点的最优路径，同时确保编队在避开障碍物的过程中保持稳定的构形，利用本文算法求解大规模环境下多智能体起点及终点无障碍凸区域如图19所示。

图19 大尺度环境下起点及终点位置处可进行编队构形优化设计的无障碍凸区域

Fig.19 Accessible convex regions for optimizing formation configuration at the starting and ending positions in large-scale environment

连接编队起点处无障碍凸多边形区域与终点处无障碍凸多边形区域的各个过渡无障碍凸多边形区域如图20（a）所示，无障碍凸多边形区域中规划得到的编队构形路径规划如图20（b）所示，编队中各智能体的路径如图20（c）所示。在此过程中，路径的计算时长为300 s，这显示了算法在大尺度环境下进行路径规划的效率。

图20 大尺度静态环境下协同编队构形及路径规划

Fig.20 Cooperative formation and path planning in large‑scale static environment

在大尺度环境中引入14个未知动态障碍物，用于测试并验证所提算法的有效性和适应性。如图21所示，这些动态障碍物以蓝色标记加以区分，它们在2 s的时间内以0.5 s为间隔，用不同的速度在环境中移动，反映了现实世界中可能遇到的各种移动障碍物的行为模式。在这一复杂环境下，移动障碍物的数量和活动范围增加了编队路径规划的难度，对算法的动态避障能力提出了更高的要求。

图21 大规模动态环境下起点及终点位置处可进行编队构形优化设计的无障碍凸区域

Fig.21 Accessible convex regions for optimizing formation configuration at the starting and ending positions in large‑scale dynamic environment

随着动态障碍物的移动，算法在每一步生成起点及终点位置处可进行编队构形优化设计的无障碍凸区域如图21所示。

当障碍物以一定速度移动时，算法能够有效地在大尺度动态环境下找到每一步的安全无碰撞凸区域。如图22所示，左侧展示了随着障碍物移动而生成的无障碍凸多边形区域，而右侧显示了在这些凸区域中满足编队过渡需求的编队构形区域。这种方法不仅确保了编队的连贯性，还有效避免了障碍物带来的碰撞风险，展现了算法在应对大尺度复杂动态环境下的高效性和适应性。

图22 大规模动态环境协同编队构形

Fig.22 Cooperative formation in a large‑scale dynamic environment

图23展示了在大尺度动态环境中，考虑动态障碍物移动的情况下，多智能体编队的全局最优路径。这一全局路径规划反映了算法在处理大尺度动态环境下的路径规划能力，同时确保了整个运动规划过程的安全性。该过程计算所需的时长为450 s，表明算法在处理复杂动态大尺度的场景时具有一定的效率和适应性，能够在合理的时间内完成路径规划任务。

图23 动态障碍物每一步移动下的编队路径规划

Fig.23 Formation path planning under each step of dynamic obstacle movement

实验结果表明，即使在存在多个动态障碍物的大尺度环境中，本文所提出的基于约束优化的方法也能有效规划出无碰撞的路径。

综上所述，本研究通过在不同规模和复杂度的环境中进行一系列实验，充分验证了所提算法的有效性和适用性。在静态和动态障碍物的复杂环境中，算法成功实现了编队避开障碍物，同时保持编队结构的稳定。在大尺度环境的扩展实验中，算法同样展现出了卓越的性能，证明了其在更广阔且障碍密集的环境下的适用性。这些实验结果表明，本文提出的约束优化方法在多智能体编队路径规划领域，不仅适用于小规模复杂环境，也能够有效应对大尺度复杂场景。

6 结论

本研究针对复杂环境，提出了一种基于约束优化的多智能体协同编队方法。首先交替使用二次规划和半定规划两种算法成功确定了环境中的无碰撞可达空间，得到后续多智能体协同编队规划的输入。这一方法的显著特点是仅依赖于对编队的顶点凸包及编队参数进行约束优化，便可实现多智能体间的有效编队保持。此外，本方法巧妙地结合了基于采样的方法和非线性优化，有效降低了采样问题的维度，增强了该方法在复杂环境中的实用性和可行性。该方法在复杂环境中展现了出色的通用性，包括在动态环境中的实时局部运动规划和静态环境中的路径规划。

本研究还进行了与传统虚拟结构和PRM路径规划算法结合的编队规划的对比实验，以及拓展了对大尺度环境下方法的测试，以适应复杂的实际应用场景。实验结果表明，相比于传统方法，本文所提出的基于约束优化的方法在编队性能、路径避碰效率以及无障碍空间利用等方面表现优异。

综上所述，本文所提出的基于约束优化的多智能体协同编队方法，为多智能体编队在实际的复杂环境应用提供了一种高效且实用的解决方案，展现了其在此领域的强大潜力和应用前景。

参考文献

DESARAJU V R， HOW J P. Decentralized path planning for multi-agent teams with complex constraints［J］. Autonomous Robots， 2012， 32（19）： 385-403. [百度学术]

Kushleyev A， Mellinger D， Powers C， et al.Towards a swarm of agile micro quadrotors［J］.Autonomous Robots， 2013， 35（4）： 287-300. [百度学术]

Saha I.Automated composition of motion primitives for multi-robot systems from safe LTL specifications［C］//Proceedings of 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.［S.l.］：IEEE， 2014. [百度学术]

毛文平，李帅永，谢现乐，等. 基于自适应机制改进蚁群算法的移动机器人全局路径规划［J］.控制与决策，2023，38（9）： 2520-2528. [百度学术]

Mao Wenping， Li Shuaiyong， Xie Xianle， et al. Global path planning for mobile robots based on adaptive mechanism improved ant colony algorithm［J］. Control and Decision Making， 2023，38（9）： 2520-2528. [百度学术]

Kumar S， Sikander A. A modified probabilistic roadmap algorithm for efficient mobile robot path planning［J］. Engineering Optimization， 2023， 55（9）： 1616-1634. [百度学术]

JANSON L， ICHTER B， PAVONE M. Deterministic sampling-based motion planning： Optimality， complexity， and performance［J］. The International Journal of Robotics Research， 2018， 37（1）： 46-61. [百度学术]

BOGIATZIS P， RYCHERT C A， HARMON N. Multiple graph realizations method： Improving the accuracy and the efficiency of the shortest path method through random sampling［J］. Geophysical Journal International， 2021， 227（1）： 669-679. [百度学术]

Barfoot T D， Clark C M. Motion planning for formations of mobile robots［J］. Robotics & Autonomous Systems， 2004， 46（2）： 65-78. [百度学术]

Krontiris A， Louis S， Bekris K E. Multi-level formation roadmaps for collision-free dynamic shape changes with non-holonomic teams［C］//Proceedings of IEEE International Conference on Robotics & Automation. ［S.l.］： IEEE， 2012： 1570-1575. [百度学术]

Ju C， Luo Q， Yan X. Path planning using an improved A-star algorithm［C］//Proceedings of 2020 11th International Conference on Prognostics and System Health Management. Jinan， China：［s.n.］， 2020： 23-26. [百度学术]

Nguyen T H， Nguyen X T， Pham D A， et al.A new approach for mobile robot path planning based on RRT algorithm［J］. Modern Physics Letters B， 2023， 37（18）： 2340027. [百度学术]

Ji Y， Ni L， Zhao C， et al. TriPField： A 3D potential field model and its applications to local path planning of autonomous vehicles［J］. IEEE Transactions on Intelligent Transportation Systems， 2023，24（3）： 3541-3554. [百度学术]

王涛，黎玉康，刘文学.无人车辆路径规划算法发展现状［J］.舰船电子工程，2022，42（5）： 15-22. [百度学术]

WANG Tao， LI Yukang， LIU Wenxue. Current status of unmanned vehicle path planning algorithms［J］. Ship Electronic Engineering， 2022， 42（5）： 15-22. [百度学术]

CHARALAMBOPOULOS N， XIDIAS E， NEARCHOU A. Efficient ship weather routing using probabilistic roadmaps［J］. Ocean Engineering， 2023， 273（56）： 114031. [百度学术]

Conner D C， Rizzi A A， Choset H. Composition of local potential functions for global robot control and navigation［C］//Proceedings of IEEE/RSJ International Conference on Intelligent Robots & Systems.［S.l.］： IEEE， 2003. [百度学术]

KALLEM V， KOMOROSKI A T， KUMAR V. Sequential composition for navigating a nonholonomic cart in the presence of obstacles［J］. IEEE Transactions on Robotics， 2011， 27（6）： 1152‑1159. [百度学术]

Ayanian N， Kallem V， Kumar V. Synthesis of feedback controllers for multiple aerial robots with geometric constraints［C］//Proceedings of 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.［S.l.］： IEEE， 2011： 3126-3131. [百度学术]

Song G， Liu X， Gao R. Model predictive control based consensus scheme of discrete-time multi-agent systems with communication delay［J］. International Journal of System Control and Information Processing， 2021，3（4）： 307-318. [百度学术]

Huang C， Xu T， Wu X. Leader‑follower formation control of magnetically actuated millirobots for automatic navigation［J］. IEEE/ASME Transactions on Mechatronics， 2024， 29（2）： 1272-1282. [百度学术]

Ma Z， Liu Z， Huang P. Discrete-time practical robotic control for human-robot interaction with state constraint and sensor less force estimation［J］. ISA Transactions， 2022， 129（61）： 659-674. [百度学术]

Cheng P D C， Indri M， Possieri C， et al. Path planning in formation and collision avoidance for multi-agent systems［J］. Nonlinear Analysis： Hybrid Systems， 2023， 47： 101293. [百度学术]

吴金霆，王琢，张子超，等.基于领航-跟随法和人工势场法的巡检机器人编队［J］.国外电子测量技术，2024，43（1）： 182-188. [百度学术]

WU Jinting， WANG Zhuo， ZHANG Zichao， et al. Inspection robot formation based on navigation following method and artificial potential field method［J］. Foreign Electronic Measurement Technology， 2024，43 （1）： 182-188. [百度学术]

WU L， HUANG X， CUI J， et al. Modified adaptive ant colony optimization algorithm and its application for solving path planning of mobile robot［J］. Expert Systems with Applications， 2023， 215： 119410. [百度学术]

封澳，杨锦宇，谢玉阳，等.基于改进PRM算法的机器人路径规划［J］.计算机技术与发展，2024，34（2）：127-133. [百度学术]

FENG Ao， YANG Jinyu， XIE Yuyang， et al. Robot path planning based on improved PRM algorithm［J］. Computer Technology and Development， 2024， 34（2）： 127-133. [百度学术]

曹园山，成月，郑鹏，等.基于多约束的改进RRT^*算法三维全局路径规划研究［J］.舰船科学技术，2024，46（8）： 14-18. [百度学术]

CAO Yuanshan， CHENG Yue， ZHENG Peng， et al. Research on improved RRT^* algorithm for 3D global path planning based on multiple constraints［J］. Ship Science and Technology， 2024，46（8）： 14-18. [百度学术]

WINKLER A， HAVOUTIS I，BAZEILLE S， et al.Path planning with force-based foothold adaptation and virtual model control for torque controlled quadruped robots［C］//Proceedings of IEEE International Conference on Robotic & Automation. ［S.l.］： IEEE，2014. [百度学术]

ABUJABAL N， FAREH R， SINAN S， et al. A comprehensive review of the latest path planning developments for multi-robot formation systems［J］. Robotica， 2023，41（7）： 2079-2104. [百度学术]

请使用 Firefox、Chrome、IE10、IE11、360极速模式、搜狗极速模式、QQ极速模式等浏览器，其他浏览器不建议使用!

基于约束优化的多智能体协同编队与避障 PDF

摘要

关键词

1 问题定义与假设

1.1 问题描述

1.2 假设条件

1.3 优化目标

1.4 问题分析

2 动态复杂环境中编队路径规划问题的建模

2.1 环境模型

2.2 无障碍凸区域空间模型

2.3 编队模型