密集环境下

编队飞行的稳健高效的轨迹规划

摘要

编队飞行对于空中机器人群在各种应用中具有巨大的潜力。 然而，现有方法缺乏在密集环境下实现完全自主的大规模编队飞行的能力。 为了弥补这一差距，我们提出了一个完整的编队飞行系统，该系统可以有效地将现实世界的约束融入空中编队导航中。 本文提出了一种基于可微图的度量来量化地层之间的整体相似性误差。 该度量对于旋转、平移和缩放是不变的，为编队协调提供了更多的自由度。 我们设计了一个分布式轨迹优化框架，考虑了编队相似性、避障和动态可行性。 优化是解耦的，使得大规模编队飞行在计算上可行。 为了提高高度受限场景中编队导航的弹性，我们提出了一种群体重组方法，通过生成局部导航目标来自适应调整编队参数和任务分配。 这项工作提出了一种称为全局重映射局部重规划的新型群体协议策略和编队级路径规划器来协调全局规划和局部轨迹优化。 为了验证所提出的方法，我们在适应性、可预测性、弹性、复原力和效率方面与其他前沿工作设计了全面的基准和模拟。 最后，该方法与带有机载计算机和传感器的手掌大小的集群平台集成，通过在密集的室外环境中实现最大规模的编队飞行，展示了其效率和鲁棒性。

我简介

编队飞行已成为自主集群实现协调空中机动的基本能力。 在杂乱的荒野和复杂的城市地区，编队导航在搜索救援[1]、协作测绘[2]、包裹投递[3]方面具有广泛的潜力，等等。 然而，将现实世界的约束有效地融入空中编队仍然是一个尚未解决的问题。 本文旨在通过提出完整的编队飞行系统，使空中集群能够在密集环境中保持合作编队行为。

受鸟群、鱼群等自然群体系统的启发，理想的编队飞行系统应具备在密集环境中灵活适应和变形的能力。 通过努力将集群维持在“临界状态”，集群编队可以动态平衡维持编队与躲避障碍之间的冲突。

图1： 户外密集环境下的大规模编队飞行。 (a) 群体机器人准备飞过树林的那一刻的快照。 (b) 执行轨迹的 Rviz 图。 网格地图是使用离线日志数据合并的。 请观看我们附加的视频，了解有关实验的更多信息，网址为 https://www.youtube.com/watch?v=uEMyvPxYqmA。

虽然大量的研究工作集中在编队导航上，但很少有研究能够在障碍物丰富的区域实现稳健的编队飞行。 三个核心挑战限制了实际编队应用：（a）编队维持和避障之间的固有冲突是不可避免的且难以缓解。 (b) 预定阵型缺乏对受限环境的弹性适应性。 (c) 集群系统无法从未知障碍物或所需编队形状突然变化引起的无序状态中快速恢复。

基于上述挑战，我们得出的结论是，理想的编队飞行系统应具有在避障的同时保持编队的能力，根据受限环境调整集群编队分布，并在突发事件后快速重组编队的能力。 这些特征被总结为PAPER标准：

• Portability：空中机器人群应由具有可扩展系统和分布式结构的轻型平台组成。 可扩展的系统意味着每个机器人的主要组件（例如估计、决策、规划和控制模块）都是相同的。 分布式架构本质上对单个硬件故障具有鲁棒性。 这些是大规模编队飞行的基础。

• 适应性：当面对障碍物时，机器人应该局部调整其轨迹，以避免碰撞，从而对整体编队性能的损害最小。 这种能力减轻了编队维持和避障之间的冲突。

• P可预测性：反应式局部反馈方法目光短浅，无法提前考虑约束条件。 机器人应该优化预测范围内的运动，以便编队能够平稳地响应其附近未来的环境变化，这对于密集区域来说是必要的。

• 弹性：在狭窄的走廊或洞等受限环境中，固定地层形状的可行且安全的轨迹可能不存在。 因此，群体机器人需要具备弹性、灵活的变形能力，通过调整编队分布（如形态规模或任务分配），同时保持编队的充分机动性。

• 弹性：编队飞行可能会遇到许多由于未知障碍物或所需编队形状突然变化而导致的不利情况。 导航系统应能够对整个编队进行弹性重组和引导，使飞行能够及时从混乱状态中恢复过来。

完整的编队飞行系统应满足上述PAPER标准，并确保每个标准的条件与其他标准兼容。

我们之前的工作[4]仅部分满足PAPER标准的前三项。 我们将编队飞行作为耦合协作轨迹优化问题来解决，主要适用于小规模编队场景。 然而，使用基于图的相似性度量来解决地层的协作约束计算量很大，导致每次优化迭代期间的开销增加。 此外，将动态机器人间关系集成到耦合轨迹优化问题中会极大地影响优化过程的效率，使其不太适合更大的编队或更复杂的场景。

在本文中，我们提出了一个完整的编队飞行系统，满足所有PAPER标准。 为了解决[4]中的挑战，我们引入了一种解耦的编队优化方法来显着提高计算效率。 该方法由两个部分组成。 首先，预先计算最佳编队位置序列，避免优化过程中重复的度量计算。 其次，采用固定时间间隔采样的方法，将动态的机器人间关系转化为静态约束，大大降低了优化问题的复杂度。 这些改进使我们的方法适用于大规模群体。 此外，以往的方法缺乏重新组织编队的能力，在不利条件下，特别是在初始位置或任务分配不合适的情况下，可能会导致编队飞行无序。 为了解决这个问题，我们提出了一种群体重组方法，该方法可以通过响应外部约束优化编队参数和任务分配来弹性调整编队分布。 随后，我们开发了一种名为“global-remap-local-replan”的群体协议策略，该策略能够快速实施群体重组结果，以在群体代理之间达成共识。 此外，还设计了一种将集群编队视为一个实体的编队级全局寻路方法，旨在引导集群摆脱障碍僵局。 最后，我们将估计、测绘、决策、规划和控制模块集成到带有机载计算机和传感器的手掌大小的集群平台[5]中，从而实现在密集环境中的大规模编队飞行。 详细贡献如下。

1. 我们引入了一个最佳编队位置序列，该序列使用基于可微图的度量[4]预先计算。 该序列代表了相似性误差最低的最佳位置，减少了优化过程中重复计算的需要。

2. 我们设计了一个解耦的时空轨迹优化框架，可以有效处理动态机器人间关系、避障和动态可行性。 与我们之前的工作[4]相比，我们对大规模群体实现了更高的计算效率。

3. 我们提出了一种群体重组方法来实现群体分布的弹性变形，同时解决了最佳编队对齐和任务分配问题（简称ALAS）。 该方法提高了群体形成针对受限环境的弹性。 它减轻了对适当编队排列和任务分配的依赖。

4. 我们设计了一种全局重映射局部重规划策略（简称GRLR），该策略利用了集中式地层参数重映射和分散式局部轨迹重规划的优点。 通过这种策略，分布式异步集群能够快速从无序状态中恢复并快速返回编队飞行。

5. 我们将所有这些模块集成到分层编队飞行系统中。 进行了广泛的基准测试和模拟来验证我们方法的PAPER标准。 一系列真实世界的实验旨在证明所提出的分布式自主编队飞行系统的出色性能。

II 相关作品

II-A 分布式群体轨迹规划

分布式集群的轨迹规划有大量的工作。 Van Den Berg 等人[6,7,8]利用并推广了速度障碍（VO）的概念，以实现多个机器人的相互避碰。 然而，基于 VO 的方法无法保证所得轨迹的平滑性，这显着损害了实际机器人系统的可用性。

为了产生高质量的无碰撞轨迹，分布式多旋翼群文献中广泛引入了基于优化的方法[9,10,11]。 Zhou等人[12]将Voronoi单元细分纳入后退地平线QP方案中，以防止机器人在规划时发生碰撞。 在[13]中，Chen等人利用SCP通过逐步收紧碰撞约束来解决非凸空间中的多智能体规划问题。 Baca 等人[14]将 MPC 与冲突解决策略相结合，以确保室外集群操作的相互避免碰撞。 然而，上述基于优化的方法的计算量很大，这可能会妨碍规划器在高密度场景中的适用性。

最近，Zhou等人[5]提出了一种利用时空轨迹优化的分布式自主四旋翼集群系统，该系统在密集环境中仅需几毫秒即可产生无碰撞运动。 我们的分布式编队轨迹优化基于这项工作。

II-B 自由空间编队飞行

人们提出了多种技术来实现多机器人导航信息，包括虚拟结构[15]、领导者-跟随者[16]、导航功能[17 ]、反应行为[18]、基于共识的局部控制法则[19]以及基于重心坐标的控制[20]. 然而，大多数现有方法仅考虑无障碍情况。

Weinstein 等人[21]提出了一种 VIO-swarm 系统，该系统可以执行机载所有模块，并且可以在自由空间中执行编队飞行而不会发生机器人间碰撞。 Parker等人[22]提出了一种分布式编队控制方法，放宽了公共参考系的依赖性。

随着群体规模的增加，研究人员开始注意到仅通过轨迹规划很难维持队形，特别是当机器人之间存在死锁时。 Turpin等人[23]考虑并发分配和无碰撞轨迹生成问题。 Turpin针对这个问题给出了集中式和分散式的解决方案，允许大规模的航班编队。 Morgan等人[24]还使用模型预测控制在给定所需的队形形状时同时解决任务分配和轨迹生成问题。 除了考虑任务分配之外，Agarwal 和 Akella[25] 还考虑编队对准问题，以优化规模和位置等编队参数。 该方法降低了编组成本，加快了收敛速度。 然而，这些方法忽略了受限环境的影响，在这种环境中，地层应该弹性变形才能导航。

II-C 受限环境下的编队飞行

在存在各种障碍和限制的受限环境中，编队飞行可能是一项具有挑战性的任务，需要不断调整以维持集群结构。 直接的解决方案是通过使用多层势场[26]来设计将编队飞行和防撞相结合的复合控制律，这很容易出现死锁。 更好的解决方案是允许编队形状变形，同时保持整体蜂群结构。 Han等人[27]提出了一种基于复值图拉普拉斯算子的编队控制器，该控制器可以在穿过走廊等群体机动过程中调节编队形状的缩放。 在[28]中，赵提出了一种领导者-跟随者控制律，使得编队能够响应环境变化而进行仿射变换。 基于方位的本地控制器[29, 30]表现出编队飞行的平移、缩放和旋转不变性。 然而，这些方法依赖于领导者或预定义的轨迹，并与复杂的障碍或突然的潜在碰撞作斗争。

与局部反馈方法相比，基于预测优化的方法主动规划群体机器人的未来运动，在编队飞行和避障之间取得平衡。 Alonso-Mora 等人[31]通过优化地重新安排所需的编队并为每架无人机规划局部轨迹来控制群体机器人。 然而，由于分布式规划器中没有车辆间协调，因此在局部规划期间不进行编队维护。 彭等人[32]提出了一种通过对编队形状进行仿射变换并将其作为B样条优化时的软约束来提高飞行安全性的方法。 然而，这种方法需要同时优化所有机器人的轨迹，并且不能应用于大规模群体。 为了解决编队保存问题，Parys等人[33]提出了一种分布式模型预测编队控制器。 该框架对群体施加相对位置约束，并协调代理在障碍物违反位置约束时被动突破。 总体而言，这些方法为群体机器人中的轨迹规划提供了独特的解决方案，但在处理不同场景和机器人系统规模时，它们各自都有局限性。

为了解决这些缺点，我们在轨迹优化中使用可微分度量来制定总体编队要求。 这使得我们能够充分利用群体的协作能力，有效避免僵局，并预见避障。 此外，我们采用分布式、解耦的优化方法来保证动态实时性能。 这种方法可以应用于大规模群体，同时仍然保持有效的轨迹规划。

图2： 系统架构图。 为了便于理解，我们按照I节中提到的PAPER标准对编队飞行系统的各个模块进行划分。 将PAPER标准集成到集群飞行系统中的主要挑战是确保每个标准的条件与其他标准兼容。 因此我们建立了分层编队飞行系统。 我们使用不同的颜色来代表系统中的不同级别。 而且，通过网络广播的信息来自同一颜色模块。

三系统概述

本文旨在通过协调群体机器人的运动以形成所需的编队形状来优化现实环境中群体机器人的自主性。 为了实现这一目标，我们采用分布式集群空中机器人系统，并提出编队飞行的时空轨迹优化。 为了增强系统的鲁棒性，我们还通过结合自适应群体重组方法和有效的群体协议策略来解决群体无序的情况。

III-A 群体空中机器人系统

集群空中机器人系统由手掌大小的四旋翼平台[5]和深度立体相机用于图像和深度传感，如图2所示。 状态估计、环境感知、决策、轨迹规划、飞行控制等软件模块在机载计算机上实时运行。 这种轻量级且可扩展的平台适合密集的环境。

我们使用视觉惯性里程计 (VIO)[34] 来估计每个机器人相对于其起始帧的位姿，并且在我们之前的工作中仅使用匿名方位测量来恢复与起始帧相关的变换[35]。 为简单起见，在我们的实验中，通过要求机器人从预定义位置起飞来了解这些转换。 为了校正群体机器人之间的定位漂移，我们使用带有机载超宽带（UWB）的漂移校正方法[5]。

分布式系统架构使得每个机器人能够充分利用其计算资源来处理更多信息，缓解网络通信压力。 机器人仅共享重要信息，例如用于高保真无线通信的轨迹，并且没有地面站发送控制输入。

III-B 分布式局部编队轨迹优化

局部编队轨迹优化是分布式且异步的，这使得每个机器人仅根据局部信息生成其轨迹，并且不需要相同的起始时间戳或相同的轨迹持续时间。 每个机器人通过计算编队相似误差来评估编队状态，并生成其最佳编队位置序列（Sec.IV)以维持所需的编队形状。 然后，后续的轨迹优化模块生成用于实时导航的时空编队轨迹（Sec.V)。 上述过程在后退的视野内周期性地循环。 分布式局部编队轨迹优化能够在复杂环境下导航时始终保持整体编队。

III-C 群体重组和协议方法

仅依靠分布式局部轨迹优化，在遇到狭窄走廊或编队形态瞬间转变时，可能会导致编队飞行质量较差。 因此，我们提出群体重组（Sec.VI)和协议方法（Sec.VII)来快速达成群体共识。 受鸟类群体飞行行为的启发，我们设计了一种全局重映射局部重规划（简称GRLR）策略，该策略仅集中重新映射群体形成的关键参数，并通过分布式重新规划局部轨迹使群体形成快速收敛。 首先，当群体编队的稳定状态被破坏或即将被破坏时，群体机器人指定一个领导者（本文中为无人机0）。 通过编队对齐和任务分配优化（简称ALAS）和编队级全局寻路方法分别计算集群编队的关键参数。 从群体重组和一致的角度来看，通过优化编队排列缓解了群体编队与障碍物之间的冲突，通过优化任务分配大大加快了编队收敛速度。 GRLR策略简单但非常有效，结合了分布式方法的效率和集中式方法的最优性。

IV 群体形成的自适应描述

IV-A 基于图的编队定义

在本文中，$N$机器人的群体编队通过无向图$𝒢=(𝒱,ℰ)$建模，其中$𝒱:=\{1,2,\mathrm{\dots },N\}$是顶点集，$ℰ\subset 𝒱\times 𝒱$ 是边的集合。 在图中 $𝒢$ 中，顶点 $i$ 表示具有位置向量 ${𝐩}_i=[x_i,y_i,z_i]\in {ℝ}^3$ 的 $i^{th}$ 机器人。 连接顶点$i\in 𝒱$和顶点$j\in 𝒱$的边$e_{ij}\in ℰ$意味着机器人$i$和$j$可以测量几何形状彼此之间的距离。 在我们的工作中，每个机器人都可以获得所有机器人$\{{𝐩}_1,\mathrm{\dots },{𝐩}_i,\mathrm{\dots },{𝐩}_N\}$的位置，因此图$𝒢$是完整的。 然后我们确定形成图$𝒢$的邻接矩阵$𝐀\in {ℝ}^{N\times N}$和度矩阵$𝐃\in {ℝ}^{N\times N}$：

$$A_{ij}=w_{ij}={\Vert {𝐩}_i-{𝐩}_j\Vert }^2,$$ (1)

(2)

其中非负边权重$w_{ij}$是$i^{th}$和$j^{th}$机器人之间的平方距离，$\parallel \cdot \parallel$表示欧几里德范数。 因此，对应的拉普拉斯矩阵为

$$𝐋=𝐃-𝐀.$$ (3)

利用上述矩阵，图$𝒢$的对称归一化拉普拉斯矩阵定义为

$$\widehat{𝐋}={𝐃}^{-1/2}{\mathrm{𝐋𝐃}}^{-1/2}=𝐈-{𝐃}^{-1/2}{\mathrm{𝐀𝐃}}^{-1/2},$$ (4)

其中 $𝐈\in {ℝ}^{N\times N}$ 是单位矩阵。 $\widehat{𝐋}$ 包含对缩放、平移和旋转不变的信息。

最后，我们使用图论来描述各种所需的地层形状，例如正方形、六边形和金字塔。 通过指定位置 ${𝐩}_i^d=[x_i^d,y_i^d,z_i^d]\in {ℝ}^3,i=1,\mathrm{\dots },N$，计算 ${\widehat{𝐋}}_{des}$ 很简单。 需要注意的是，只要提供了相对位置，所需的地层形状就与坐标系无关。

IV-B 可微分阵相似性误差度量

图3： 使用 2D 队形说明最佳队形位置序列。 (a) 表面显示了当一架无人机在平面上移动而另外三架无人机保持静止时相似性度量的轮廓。 最小值表明形成所需形状的最佳形成位置。 (b) 最佳编队位置的序列对应于时间戳。

为了评估与所需队形的偏差，我们提出了一个可微的队形相似性误差度量：

$f_s$ $=f_s({𝐩}_1,\mathrm{\dots },{𝐩}_i,\mathrm{\dots },{𝐩}_N)=f_s(𝐀,𝐃)=f_s(\widehat{𝐋},{\widehat{𝐋}}_{des})$ (5)

$={\Vert \widehat{𝐋}-{\widehat{𝐋}}_{des}\Vert }_F^2=tr\{{(\widehat{𝐋}-{\widehat{𝐋}}_{des})}^T(\widehat{𝐋}-{\widehat{𝐋}}_{des})\},$

其中$tr\{\cdot \}$表示矩阵的迹，$\widehat{𝐋}$是当前群体编队的对称归一化拉普拉斯算子，${\widehat{𝐋}}_{des}$是所需编队的对应部分。 弗罗贝尼乌斯范数 $\parallel \cdot {\parallel }_F$ 用于我们的距离度量。 作为图表示矩阵，$\widehat{𝐋}$包含有关图结构[36]的信息。 这允许 $f_s$ 仅考虑地层的几何形状，而不受缩放、平移或旋转的影响。 另外，$f_s$是无量纲值，仅反映地层形状相似度的误差。

特别是在分布式框架下，每个机器人只能改变自己的位置，以减少整体编队相似度误差。 因此，(5)中机器人$i$的唯一变量是${𝐩}_i$，而$f_s({𝐩}_1,\mathrm{\dots },{𝐩}_i,\mathrm{\dots },{𝐩}_N)$可以简化为$f_s({𝐩}_i)$.

我们的度量对于每个机器人的位置在分析上是可微的。 对于机器人$i$，我们使用其$N$相邻边$\{e_{i1},e_{i2},\mathrm{\dots },,e_{iN}\}$的权重来形成权重向量${𝐰}_i={[w_{i1},w_{i2},\mathrm{\dots },,w_{iN}]}^T$。根据链式法则，$f_s$相对于${𝐩}_i$的梯度可以写为

$$\frac{\partial f_s}{\partial {𝐩}_i}=\frac{\partial f_s}{\partial {𝐰}_i^T}\frac{\partial {𝐰}_i}{\partial {𝐩}_i}.$$ (6)

根据我们的指标(5)，$f_s$相对于每个权重$w_{ij}$的梯度可以计算如下

${\displaystyle \frac{\partial f_s}{\partial w_{ij}}}$ $=tr\{{({\displaystyle \frac{\partial f_s}{\partial \widehat{𝐋}}})}^T({\displaystyle \frac{\partial \widehat{𝐋}}{\partial w_{ij}}})\},$ (7)

${\displaystyle \frac{\partial f_s}{\partial \widehat{𝐋}}}$ $={\displaystyle \frac{\partial {\Vert \widehat{𝐋}-{\widehat{𝐋}}_{des}\Vert }_F^2}{\partial \widehat{𝐋}}}=2(\widehat{𝐋}-{\widehat{𝐋}}_{des}),$

${\displaystyle \frac{\partial \widehat{𝐋}}{\partial w_{ij}}}$ $=-{\displaystyle \frac{\partial ({𝐃}^{-1/2}{\mathrm{𝐀𝐃}}^{-1/2})}{\partial w_{ij}}}.$

那么梯度$\partial f_s/\partial {𝐰}_i$可以写成

$$\partial f_s/\partial {𝐰}_i={[\partial f_s/\partial w_{i1},\partial f_s/\partial w_{i2},\mathrm{\dots },\partial f_s/\partial w_{iN}]}^T.$$ (8)

至于$\partial {𝐰}_i/\partial {𝐩}_i$，由于权重函数(1)是可微的二次形式，因此可以很容易地导出雅可比行列式。

IV-C 最佳编队位置顺序

在我们之前的工作[4]中，我们将$f_s$直接纳入轨迹优化中，使编队飞行成为耦合轨迹优化问题。 虽然该方法适用于小规模编队飞行，但随​​着机器人数量 $N$ 的增加，计算效率会降低。 考虑耦合轨迹优化的简化方程

$$\underset{{𝐩}_{i,0},\mathrm{\dots },{𝐩}_{i,M_c}}{\min }\sum _{j=0}^{M_c}{f}_s({𝐩}_{i,j})+J_{other},$$ (9)

为了方便起见，其中${𝐩}_{i,j}$表示(19)中$i^{th}$机器人轨迹的$j^{th}$样本点。 $J_{other}$代表所有其他成本函数，$M_c$是具有相应时间戳的样本点的数量。 计算$f_s$的主要目的是提供梯度信息以最小化地层相似误差。 然而，由于图$𝒢$是一个完全图，计算$f_s$的复杂度为$O(N^2)$。 因此，耦合轨迹优化(9)在每次迭代中也表现出$O(N^2)$的高复杂性，限制了其对大规模群体操作的适用性。

为了解决这个问题，我们必须找到一种计算复杂度较低的等效方法来替换(9)中$f_s$的函数。 我们引入了机器人$i$在时间戳$j$的最佳编队位置${𝐩}_{i,j}^{\ast }$的概念，即最小化编队相似度的位置错误$f_s$。 图3(a)以二维地层为例说明了这一概念。 从图中可以看出，存在一个使队形相似误差最小的最佳队形位置${𝐩}_{i,j}^{\ast }$，其偏导数为$\partial f_s/\partial {𝐩}_{i,j}^{\ast }=0$。 在未来一段时间内，用时间戳序列$\{0,\mathrm{\dots },j,\mathrm{\dots },M_c\}$，用最优编队位置序列${𝐩}_i^{\ast }=\{{𝐩}_{i,0}^{\ast },\mathrm{\cdots },{𝐩}_{i,j}^{\ast },\mathrm{\dots },{𝐩}_{i,M_c}^{\ast }\}$表示机器人$i$的预期位置>，如图3(b)所示。 通过预先计算${𝐩}_i^{\ast }$，我们可以利用其二次距离来代替（11）中$f_s$提供的梯度信息，从而降低计算量，如下所示

$$f_s({𝐩}_{i.j})\Rightarrow {\Vert {𝐩}_{i,j}-{𝐩}_{i.j}^{\ast }\Vert }^2.$$ (10)

由于$f_s$的最优解和二次距离成本是等价的，因此轨迹以最小的队形相似误差接近位置，保持期望的队形。 因此，我们可以通过两步程序有效地解决耦合轨迹优化问题

$\text{①}{𝐩}_i^{\ast }=\arg \min {\displaystyle \sum _{j=0}^{M_c}}{f}_s({𝐩}_{i.j}),$ (11)

$\stackrel{{𝐩}_i^{\ast }}{\Rightarrow }$ $\text{②}\underset{{𝐩}_{i,0},\mathrm{\dots },{𝐩}_{i,M_c}}{\min }{\Vert {𝐩}_{i.j}-{𝐩}_{i.j}^{\ast }\Vert }^2+J_{other}.$

这样一来，之前每次轨迹优化过程中所需的$f_s$的计算就被二次距离的计算所取代，简化了优化问题。 这显着减少了计算需求并实现了大规模群体形成。

公式(11)表示Sec.V中的轨迹优化是针对离散点进行的。 不均匀的离散点可能会导致轨迹不佳和性能次优。 因此，确保这些点的均匀分布对于保持优化过程的有效性至关重要。 在工程实践中，由于图$𝒢$是由一系列离散化时间戳构建而成，如图3(b)所示，因此每个${𝐩}_{i,j}^{\ast }$都是独立的。

为了保证轨迹更加平滑，我们引入了均匀最优编队位置序列${\widehat{𝐩}}_i^{\ast }$，该序列是通过考虑编队相似误差$J_s$和均匀分布成本$J_u$生成的

$${\widehat{𝐩}}_i^{\ast }=\arg \min {\lambda }_s{J}_s+{\lambda }_u{J}_u,$$ (12)

$J_s$ $={\displaystyle \sum _{j=0}^{M_c}}{f}_s({\widehat{𝐩}}_{i,j}^{\ast }),$ (13)

$J_u$ $=𝔼({𝐔}^2)-𝔼{(𝐔)}^2={\displaystyle \frac{{\Vert 𝐔\Vert }_2^2}{M_c}}-{\displaystyle \frac{{\Vert 𝐔\Vert }_1^2}{{(M_c)}^2}},$

其中 ${\lambda }_s$ 和 ${\lambda }_u$ 是相对权重。 $𝔼(\cdot )$ 是数学期望，平方距离向量 $𝐔\in {ℝ}^{M_c}$ 是

$$𝐔=({\Vert {\widehat{𝐩}}_{i,1}^{\ast }-{\widehat{𝐩}}_{i,0}^{\ast }\Vert }_2^2,\mathrm{\cdots },{\Vert {\widehat{𝐩}}_{i,M_c}^{\ast }-{\widehat{𝐩}}_{i,M_c-1}^{\ast }\Vert }_2^2).$$ (14)

我们使用拟牛顿法[37]来解决这个无约束优化问题(12)并为后续的轨迹优化生成均匀的${\widehat{𝐩}}_i^{\ast }$(18)。 通过这样做，Sec.V中这些离散点产生的轨迹可以更加平滑，并避免突然的空间变化。

V 编队飞行时空轨迹优化

V-A 轨迹表示

多旋翼飞行器的微分平坦度[38]有利于轨迹生成，无需积分微分方程。 此外，多旋翼飞行器的运动规划可以在低维平滑轨迹上进行。 在本文中，我们采用一种名为 MINCO [39] 的最先进的轨迹表示来实现三维环境中群体空中机器人的最小控制努力时空轨迹规划。 MINCO 通过使用线性复杂度映射 $ℳ$ 解耦空间和时间参数，对平坦输出 $M$ 片段轨迹 $p(t)$ 进行时空变形

$$p(t)={ℳ}_{𝐪,𝐓}(t),\forall t\in [t_0,t_M],$$ (15)

其中$𝐪={({𝐪}_1,\mathrm{\cdots },{𝐪}_{M-1})}^T\in {ℝ}^{3\times (M-1)}$是每对连接片段之间的相邻中间点，$𝐓={(T_1,\mathrm{\cdots },T_M)}^T\in {ℝ}_{>0}^M$是每个片段的持续时间。

$m$维$M$分段轨迹$p(t)$由分段多项式表示。 而$i^{th}$块$p_i(t)$被定义为一个多次多项式（本文中的$Q=5$)

$$p_i(t)={𝐜}_i^T𝜷(t),\forall t\in [0,T_i],$$ (16)

其中${𝐜}_i\in {ℝ}^{(Q+1)\times m}$是系数矩阵，$𝜷(t)={[t^0,t^1,\mathrm{\cdots },t^Q]}^T$是自然基。

对于 s-积分器（本文中的 $s=3$)链动力学系统，$M$ 段 $2s-1$ 度轨迹 $p(t)$ 定义为恒定的边界和最小的控制努力$\{𝐪,𝐓\}$。 此外，MINCO 还使用线性时间和空间参数映射 $𝐜=ℳ(𝐪,𝐓)$ 将 $\{𝐪,𝐓\}$ 转换为 $\{𝐜,𝐓\}$，其中 $𝐜={({𝐜}_1^T,\mathrm{\cdots },{𝐜}_M^T)}^T$ 是多项式系数。

V-B 问题表述

在第 IV 节中确定所需的队形形状后，我们期望群体机器人有一组轨迹，这些轨迹是平滑的、无碰撞的并且保持队形。 在实践中，在未知的密集环境中使用有限视野的传感器和机载计算机导航群体机器人需要一个专注于本地信息的高效实时规划器。 此外，集中式优化受到群体规模的限制。 因此，我们选择分布式局部轨迹优化进行编队飞行如下

$\underset{𝐪,𝐓}{\min }$ ${\displaystyle {\int }_{t_0}^{t_M}}{\Vert p^{(s)}(t)\Vert }^2𝑑t+\rho \cdot T_{\mathrm{\Sigma }},$ (17a)

$s.t.$ $p(t)={ℳ}_{𝐪,𝐓}(t),\forall t\in [t_0,t_M],$ (17b)

${𝐩}^{[s-1]}(0)={\overline{𝐩}}_0,$ (17c)

${𝐩}^{[s-1]}(t_M)={\overline{𝐩}}_f,$ (17d)

$\mathscr{H}(p(t),\mathrm{\dots },p^{(s)}(t))⪯\mathrm{𝟎},\forall t\in [t_0,t_M].$ (17e)

我们定义平稳性和攻击性的成本 (17a)，以实现平稳高效的飞行。 $\rho$为时间正则化参数，$T_{\mathrm{\Sigma }}={\sum }_{i=1}^M{T}_i$。 机器人$p(t)$(17b)的状态由优化变量$\{𝐪,𝐓\}$参数化。 ${𝐩}^{[s-1]}(t)={(p{(t)}^T,\dot{p}{(t)}^T,\mathrm{\dots },p^{(s-1)}{(t)}^T)}^T\in {ℝ}^{ms}$表示具有$s$积分器的链动态系统的高阶导数。 边界条件包括初始状态${\overline{𝐩}}_0\in {ℝ}^{ms}$(17c)和终止状态${\overline{𝐩}}_f\in {ℝ}^{ms}$(17d)。 连续时间约束$\mathscr{H}$ (17e)包括群体形成相似性、动态可行性、避障和群体相互回避。

V-C 约束转录

为了实时求解连续约束优化问题(17)，我们使用MINCO优化变量(15)来消除各种等式约束(17b)-(17d)。 采用惩罚函数方法[40]来处理不等式约束(17e)。 然后，通过样本点的有限和来评估每个积分。 最后将连续约束优化问题转化为离散无约束优化问题

$\underset{𝐪,𝐓}{\min }{\displaystyle \sum _x}{\lambda }_{\star }{\tilde{J}}_{\star }(𝐪,𝐓,\delta ),$ (18)

其中${\tilde{J}}_{\star }$是成本函数或惩罚的各种项，${\lambda }_{\star }$是相对权重。 下标 $\star =\{f,e,t,o,r,d\}$ ($f$) 群体形成相似性，($e$) 表示控制努力，($t$) 总时间，($o$)避障，($r$)群体相互回避，($d$)动态可行性。 $\delta$是采样时间间隔。

在我们之前的工作[4]中，我们使用固定数量的采样点${\widehat{𝐩}}_{i,j}=p_i((j/{\kappa }_i)\cdot T_i)$来变换优化问题，其中$p_i(t)$是$i^{th}$ 块轨迹，${\kappa }_i$ 是该块上的固定样本数。 然而，考虑到优化过程中总时间$T_{\mathrm{\Sigma }}$的变化，固定数量的采样点${\widehat{𝐩}}_{i,j}$很难在整个轨迹上均匀分布。 因此，我们对整个轨迹采取固定时间间隔的采样点，以保证罚函数采样变换的准确性

${\widetilde{𝐩}}_j(t)=p_i(j\delta -{\displaystyle \sum _{l=1}^{i-1}}{T}_l),$ (19)

$j\in \{0,\mathrm{\cdots },\kappa \},\kappa =\lfloor {\displaystyle \frac{T_{\mathrm{\Sigma }}}{\delta }}\rfloor ,$

其中 $\kappa$ 是样本编号，$T_l$ 是任何 的先前时间。

对于群体机器人的轨迹规划，固定时间间隔采样点${\widetilde{𝐩}}_j(t)$可以简化优化问题。 与${\widehat{𝐩}}_{i,j}$相比，${\widetilde{𝐩}}_j(t)$对应的时间戳是固定的，因此优化过程中其他机器人在该时间戳的状态也是恒定的。 因此，根据优化前的广播轨迹计算其他机器人w.r.t ${\widetilde{𝐩}}_j(t)$的状态是可行的。 那么我们可以提前求解均匀编队位置序列优化(12)，并用${\widehat{𝐩}}_i^{\ast }$代替轨迹优化中的编队相似性度量$f_s$(17a) $i^{th}$ 机器人。 这种解耦编队轨迹优化带来了更高的计算效率，使我们的方法适用于大规模群体机器人。

尽管优化问题在采样数 $\kappa$ 变化时不可微，但成本函数仍然是连续的。 持续时间$𝐓$。 在本文中，我们使用[37]中提出的拟牛顿方法来解决非光滑离散无约束优化问题(18)。

V-D 成本函数和梯度

给定固定的采样时间间隔$\delta$，我们可以通过有限的采样点${\widetilde{𝐩}}_j(t)$之和来评估整个轨迹的成本函数和梯度。 $j^{th}$ 采样点的各种通用惩罚的成本为

$${𝒫}_{\star }(𝐜,𝐓,j\delta )={𝒫}_{\star }({\widetilde{𝐩}}_j(t)),$$ (20)

那么(18)中的成本函数${\tilde{J}}_{\star }$计算如下

${\tilde{J}}_{\star }(𝐪,𝐓,\delta )$ $=J_{\star }(𝐜,𝐓,\delta ),$

$=\delta {\displaystyle \sum _{j=0}^{\kappa }}{\overline{\omega }}_j{𝒫}_{\star }(𝐜,𝐓,j\delta )+$ (21)

${\displaystyle \frac{1}{2}}(T_{\mathrm{\Sigma }}-\kappa \delta )\left[{𝒫}_{\star }(𝐜,𝐓,\kappa \delta )+{𝒫}_{\star }(𝐜,𝐓,T_{\mathrm{\Sigma }})\right],$

其中$({\overline{\omega }}_0,{\overline{\omega }}_1,\mathrm{\cdots },{\overline{\omega }}_{\kappa -1},{\overline{\omega }}_{\kappa })=(1/2,1,\mathrm{\cdots },1,1/2)$是遵循梯形规则[41]的正交系数。 并且MINCO允许任何二阶连续成本函数${\tilde{J}}_{\star }(𝐪,𝐓)$用$J_{\star }(𝐜,𝐓)$表示。 因此，可以分别从$\partial J_{\star }/\partial 𝐜$和$\partial J_{\star }/\partial 𝐓$高效地获得$\partial {\tilde{J}}_{\star }/\partial 𝐪$和$\partial {\tilde{J}}_{\star }/\partial 𝐓$，这有利于优化的构造和求解问题。 在(19)中，采样时间$t=j\delta -{\sum }_{l=1}^{i-1}{T}_l$与之前的时间$T_l$相关，因此$J_{\star }$相对于${𝐜}_i$ 和 $T_l$ 计算为

$$\frac{\partial J_{\star }}{\partial {𝐜}_i}=\frac{\partial J_{\star }}{\partial {𝒫}_{\star }}\frac{\partial {𝒫}_{\star }}{\partial {\widetilde{𝐩}}_j(t)}\frac{\partial {\widetilde{𝐩}}_j(t)}{\partial {𝐜}_i},$$ (22)

$$\frac{\partial J_{\star }}{\partial T_l}=\frac{\partial J_{\star }}{\partial {𝒫}_{\star }}\frac{\partial {𝒫}_{\star }}{\partial {\widetilde{𝐩}}_j(t)}\frac{\partial {\widetilde{𝐩}}_j(t)}{\partial t}\frac{\partial t}{\partial T_l},$$ (23)

(24)

其中$\partial J_{\star }/\partial {𝒫}_{\star }$的计算很简单，下面给出了用于各种通用目的的${𝒫}_{\star }({\widetilde{𝐩}}_j(t))$的详细信息。

V-D1 群体形成相似度的成本 ${𝒫}_f$

在IV-C节中，我们通过构造无约束优化问题来计算每个采样点的统一最优编队位置序列${\widehat{𝐩}}_i^{\ast }$，将编队相似误差度量与轨迹优化解耦。 这一改进避免了编队相似性度量$f_s$的多次计算。 然后，我们使用二次形式来计算群体形成相似度的成本

$${𝒫}_f({\widetilde{𝐩}}_j(t))=\max {\{\parallel {\widetilde{𝐩}}_j(t)-{\widehat{𝐩}}_{i,j}^{\ast }{\parallel }^2,0\}}^3.$$ (25)

V-D2 控制努力$J_e$

轨迹的$s^{th}$（本文中的$s=3$)控制输入及其梯度写为

$$J_e=\sum _{i=1}^M{\int }_0^{T_i}{\Vert p_i^{(s)}(t)\Vert }^2𝑑t,$$ (26)

$$\frac{\partial J_e}{\partial {𝐜}_i}=2\left({\int }_0^{T_i}{𝜷}^{(s)}(t){𝜷}^{(s)}{(t)}^T𝑑t\right){𝐜}_i,$$ (27)

$$\frac{\partial J_e}{\partial T_i}={𝐜}_i^T{𝜷}^{(s)}(T_i){𝜷}^{(s)}{(T_i)}^T{𝐜}_i.$$ (28)

V-D3 总时间$J_t$

为了保证轨迹的攻击性，我们最小化总时间$J_t={\sum }_{i=1}^M{T}_i$。 梯度由$\partial J_t/\partial 𝐜=\mathrm{𝟎}$和$\partial J_t/\partial 𝐓=\mathrm{𝟏}$给出。

V-D4 避障成本${𝒫}_o$

受[42]的启发，避障惩罚$J_o$是使用欧几里德符号距离场（ESDF）计算的。 我们对距离障碍物太近的采样点进行惩罚

$${𝒫}_o({\widetilde{𝐩}}_j(t))=\max {\{{\psi }_o({\widetilde{𝐩}}_j(t)),0\}}^3,$$ (29)

$${\psi }_o({\widetilde{𝐩}}_j(t))=d_o-d_o({\widetilde{𝐩}}_j(t)),$$ (30)

其中$d_o$是根据实际情况设置的安全阈值，$d_o({\widetilde{𝐩}}_j(t))$是${\widetilde{𝐩}}_j(t)$与其周围最近的障碍物之间的距离。 ${𝒫}_o$相对于${\widetilde{𝐩}}_j(t)$的梯度是

$$\frac{\partial {𝒫}_o}{\partial {\widetilde{𝐩}}_j(t)}=-\nabla d^T,$$ (31)

其中$\nabla d$是${\widetilde{𝐩}}_j(t)$中ESDF的梯度。

V-D5 群体相互回避的成本 ${𝒫}_r$

当太接近固定时间戳$t=j\delta$处的轨迹$p_{\varphi }(t),\varphi \in \mathrm{\Phi }$时，我们对${\widetilde{𝐩}}_j(t)$进行惩罚，其中$\mathrm{\Phi }$代表蜂群。 与我们之前的工作[4]相比，其他具有固定时间戳的机器人$p_{\varphi }(j\delta )$的状态在优化过程中是恒定的，并且不会产生关于$𝐓$的梯度> 成本函数$J_r$。 因此优化问题和梯度被简化。

群体相互回避的成本定义为

$${𝒫}_r({\widetilde{𝐩}}_j(t))=\sum _{\mathrm{\Phi }}\max {\{{\psi }_r({\widetilde{𝐩}}_j(t),p_{\varphi }(j\delta )),0\}}^3,$$ (32)

$${\psi }_r({\widetilde{𝐩}}_j(t),p_{\varphi }(j\delta ))=d_r^2-{\Vert {\widetilde{𝐩}}_j(t)-p_{\varphi }(j\delta )\Vert }^2,$$ (33)

其中$d_r$是每个机器人之间的安全间隙。 ${𝒫}_r$相对于${\widetilde{𝐩}}_j(t)$的梯度是

$$\frac{\partial {𝒫}_r}{\partial {\widetilde{𝐩}}_j(t)}=-2{({\widetilde{𝐩}}_j(t)-p_{\varphi }(j\delta ))}^T.$$ (34)

V-D6 动态可行性成本${𝒫}_d$

我们限制速度和加速度的最大值以保证机器人能够执行轨迹。

${𝒫}_d({\widetilde{𝐩}}_j(t))$ $={𝒫}_{d,v}({\widetilde{𝐩}}_j(t))+{𝒫}_{d,a}({\widetilde{𝐩}}_j(t)),$ (35)

${𝒫}_{d,v}({\widetilde{𝐩}}_j(t))$ $=\max {\{\parallel {\dot{\widetilde{𝐩}}}_j(t){\parallel }^2-v_m^2,0\}}^3,$

${𝒫}_{d,a}({\widetilde{𝐩}}_j(t))$ $=\max {\{\parallel {\ddot{\widetilde{𝐩}}}_j(t){\parallel }^2-a_m^2,0\}}^3,$

其中 $v_m$ 和 $a_m$ 是最大速度和加速度。

V-E 轨迹优化求解质量的探讨

所提出的轨迹优化过程(17)旨在解决具有挑战性的多阶段线性二次最小时间（LQMT）问题，该问题本质上是非凸和非线性的。 此外，结合 ESDF 进行避障引入了进一步的非凸约束。 因此，用拟牛顿法保证全局最优解并不总是可能的。 为了解决有关局部最小值和不可行解决方案的问题，我们采取了优先考虑安全性和动态可行性的措施，同时保持高性能编队飞行。

首先，我们利用混合A*搜索算法[42]生成无碰撞且动态可行的初始轨迹，确保有效的最终解轨迹。 在优化过程中，我们更加重视避障和动态约束，以优先考虑安全性和可行性。 此外，我们还对轨迹进行碰撞检查以提高安全性。 此外，我们的分布式群体优化框架有效地减轻了局部最小值对整体编队性能的影响。 通过实施这些措施，我们的方法可靠地实现了稳健的编队飞行，同时保持了计算效率。

VI 群体重组方法

在编队导航过程中，集群可能会遇到空间高度受限、任务分配不当、突然编队切换命令等多种不利条件。 为了从这些情况中恢复过来，我们提出了一种群体重组方法。 该方法旨在生成满足所需编队分布并尊重每个机器人当前状态的高质量局部目标。 有了这些局部目标，即使在狭窄的走廊或洞等高度受限的环境中，群体也可以快速改变所需的形状。

与高频分布式编队轨迹优化不同，集群重组方法仅在编队飞行稳定状态被破坏或即将被破坏时以低频运行。 该方法首先计算队形约束意识，然后求解最优队形AL分配和任务AS分配问题（ALAS）。 前者的感知分布式地量化了每个机器人的编队维持和避障之间的冲突，而后者则集中解决了ALAS问题。

VI-A 队形约束意识

首先，我们需要导出权重来指示环境对机器人的约束程度。 这些权重称为编队约束意识，应根据当前编队状态和障碍物信息来确定。

受我们之前的工作[43]的启发，我们希望根据不同梯度信息之间的关系来描述冲突程度。 首先，我们检索 ESDF 距离 $d_o(p)$ 和相应的障碍物梯度 $\nabla d(p)$。 同时，我们计算地层相似项的当前梯度$\nabla f_s(p)$。 其次，我们计算渐变$\nabla d(p)$和$\nabla f_s(p)$之间的角度的余弦$\beta$

$$\beta =\frac{\nabla d(p)\nabla f_s(p)}{\Vert \nabla d(p)\Vert \Vert \nabla f_s(p)\Vert }.$$ (36)

然后我们利用sigmoid函数将角度的余弦映射为冲突系数$\eta$

$$\eta (\beta )=\frac{1}{1+e^{(\alpha \beta +\gamma )}},$$ (37)

其中 $\alpha$ 调节随着余弦值 $\beta$ 增加，这种意识上升的速度，$\gamma$ 调节这种基于角度的意识的死区和激活区。 当$\nabla d(p)$和$\nabla f_s(p)$的方向相反时，冲突系数$\eta$最大，这表明冲突最严重。 当两个梯度方向相同时，$\eta$达到最小值，这意味着没有冲突。

编队约束感知还应考虑梯度大小和当前最近障碍物距离的影响。 因此，我们将$i^{th}$机器人的队形约束意识$g_i$设计为

$$g_i=\lambda \cdot \eta (\beta )\cdot \frac{\Vert \mathrm{▽}{J}_f(p)\Vert }{d_o(p)}.$$ (38)

我们将计算应用于群体中的每个机器人，从而得到整个群体的编队约束意识向量$𝐠=\{g_1,\mathrm{\dots },g_N\}$。 为了区分最受约束的，我们使用 softmax 函数来放大意识向量 $𝐠$ 的方差并对向量进行归一化

$$𝐰=\text{softmax}(𝐠).$$ (39)

(38)中的$\lambda$用于调整$𝐰$中元素的方差。 $𝐰$是描述群体中机器人的编队-障碍冲突程度的最终意识向量。

图4： 编队排列和任务分配的图示。 (a) 在解决 ALAS 问题之前，初始编队目标与机器人的过渡距离较大且分配无序，可能导致死锁。 (b) 在机器人处于相同位置的情况下，解决 ALAS 后，编队目标具有较低的距离成本和更好的分配。

图5： 狭窄走廊中加权编队对准问题的图示。 当六边形阵型进入走廊时，原有的阵型分布（粉色点）就无法再维持。 上下机器人（红点）受到障碍物的严重挤压，因此具有最大的约束意识。 在解决了意识加权编队对齐问题后，群体获得了与受约束机器人最匹配的新的期望编队分布（蓝点）。

VI-B 编队排列和任务分配优化

用于受限场景中的编队飞行，例如在狭窄的走廊中，由于障碍物不妨碍编队要求，约束意识较低的无约束机器人拥有更大的空间与其他机器人自由协调。 相反，具有较大意识的受限机器人总是陷入编队维持与避障之间的冲突。 因此，在生成编队重组的局部目标时，细化无约束机器人的位置以与受约束机器人相匹配更为合理。 在这项工作中，我们在调整编队分布时使用VI-A节中的$𝐰$来对机器人进行称重，并在受约束的机器人上放置更多的权重。

在本节中，我们仅详细阐述局部目标生成的 ALAS 问题。 之后，global-remap-local-replan策略使用生成的局部目标来重新组织编队，这在第VII-A节中有详细介绍。

让${\mathrm{𝐥𝐠}}_i={[l{g}_{ix},l{g}_{iy},l{g}_{iz}]}^T,i=1,\mathrm{\dots },N$代表机器人的当前位置。 所需的地层形状模板由$N$位置${𝐪}_j={[q_{jx},q_{jy},q_{jz}]}^T,j=1,\mathrm{\dots },N$给出。 那么，对齐的编队位置${𝐪}_j^{\prime }$可以写为

$${𝐪}_j^{\prime }=s\cdot {𝐪}_j+𝐝,$$ (40)

其中$s\in ℝ$是缩放因子，$𝐝\in {ℝ}^3$表示平移因子。 在这项工作中，编队对齐由缩放因子和平移因子决定。

ALAS由编队对齐和任务分配组成，如图4所示。 前者旨在基于加权欧几里德距离成本找到所需编队的最佳对齐方式。 后者解决了将智能体与局部目标相匹配的最优分配。

任务分配问题被表述为

$$\underset{\sigma }{\min }\sum _i^n{\Vert {\mathrm{𝐥𝐠}}_i-(s^{\ast }\cdot {𝐪}_{\sigma (i)}+{𝐝}^{\ast })\Vert }^2,$$ (41)

其中$\sigma \in S_N$是形成任务的分配图，$S_N$是从集合$\{1,\mathrm{\dots },N\}$到其自身的所有排列的对称群。 问题 (41) 解决了最小化当前机器人与对齐的局部目标之间的总体过渡距离的分配。

队形对准问题可表述为

$$\underset{s,𝐝}{\min }\sum _i^n{w}_i\cdot {\Vert {\mathrm{𝐥𝐠}}_i-(s\cdot {𝐪}_{{\sigma }^{\ast }(i)}+𝐝)\Vert }^2,$$ (42)

其中${\sigma }^{\ast }$代表最佳分配，$w_i$是来自Sec.VI-A的认知权重。 问题(42)根据约束意识加权的距离成本生成最适合当前机器人位置的标准队形。 图5说明了当蜂群穿越走廊时，队列如何调整编队分布。

问题 (41) 和 (42) 是耦合的。 整个 ALAS 问题具有三个决策变量：缩放因子 $s$、平移 $𝐝$ 和赋值 $\sigma$。 ALAS 的目标是找到一组最佳决策变量，使 (42) 和 (41) 最小化。 请注意，公式中没有乘以认知权重$w_i$ (41)。 因为在分配优化中，我们只关心总欧氏距离成本，与任何智能体的约束程度无关。

然而，对于仅使用缩放因子$s$和平移$𝐝$的编队对齐，[25]证明相应的赋值$\sigma$可以是以解耦的方式进行优化，而不是迭代地交替两个优化阶段。 在[25]中，最优分配解${\sigma }^{\ast }$对于编队比例因子$s$和平移$𝐝$的变化是不变的。 并且 (41) 的解可以通过使用新的伪成本 ${\kappa }_{ij}$ 求解以下整数规划来直接优化

$\underset{\sigma =(x_{ij})}{\min }{\displaystyle \sum _{i=1}^n}{\displaystyle \sum _{j=1}^n}{\kappa }_{ij}{x}_{ij},$ (43)

$\text{where}{\kappa }_{ij}=-{\mathrm{𝐥𝐠}}_i^T{𝐪}_j.$

公式 (LABEL:equ:task_cost) 独立于比例参数 $s$ 和平移 $𝐝$。 因此，(LABEL:equ:task_cost) 可以在编队对齐阶段之前首先求解。 然后我们使用优化的分配 ${\sigma }^{\ast }$ 确定最佳对齐方式。 具有意识权重的编队对齐问题仍然是凸的，并且在附录-A中给出了(41)的封闭形式解。

给定 ALAS 的解，$i^{th}$ 机器人生成的本地目标 ${\mathrm{𝐥𝐠}}_i^{\prime }$ 的位置计算如下

$${\mathrm{𝐥𝐠}}_i^{\prime }=s^{\ast }\cdot {𝐪}_{{\sigma }^{\ast }(i)}+{𝐝}^{\ast }.$$ (44)

经过 ALAS 优化后，生成的局部目标的分布符合所需的编队形状，并尊重每个机器人的编队与障碍物冲突。

七群体协议法

VII-A 全局重映射局部重规划策略

图6： GRLR 策略说明。 (a) 局部重新规划策略在规划范围内产生初始局部目标。 由于冲突参考轨迹，群体机器人预计会与当前编队行为陷入僵局。 因此，全局重映射策略调用ALAS方法来重新调整形状并重新分配初始局部目标的任务。 (b) 解决细化的局部目标后，全局重映射策略生成无冲突的参考轨迹。 这样，群体编队就会与新的编队行为快速收敛。

1:Global reference trajectory ${𝒯}_{ref}$,   Local trajectory ${𝒫}_i$,   Initial local goals ${\mathrm{𝐥𝐠}}_i$,   Global goals ${𝐆}_i$,   Assignments $\sigma$,   Scale $s$,   Translation $𝐝$ ;

2:Initialize: $CallGlobalRemap\leftarrow False$,

3:for each robot i do

4: $g_i\leftarrow$ ConstrainedAwareness(${𝒫}_i$); $▷$ detailed in (38)

5: if $g_i>g_d$ then $▷$ $g_d$ is threshold of $g_i$

6: $CallGlobalRemap\leftarrow True$;

7: $𝐰\leftarrow \text{softmax}(𝐠)$;

8: end if

9:end for

10:if SimilarityError($\cdot$)$>e_{sim,d}$ then $▷$ detailed in (5)

11: $CallGlobalRemap\leftarrow True$;

12: $𝐰\leftarrow \text{softmax}(𝐈)$;

13:end if

14:if $CallGlobalRemap$ then

15: ${\sigma }^{\ast }\leftarrow$ Assignment($\sigma ,s,𝐝$);$▷$ detailed in (41)

16: $s^{\ast },{𝐝}^{\ast }\leftarrow$Alignment($𝐰,{\sigma }^{\ast }$);$▷$ detailed in (42)

17: ${\mathrm{𝐥𝐠}}_i^{\ast }\leftarrow$RemapLocalGoals(${\sigma }^{\ast },s^{\ast },{𝐝}^{\ast }$);

18: ${𝐆}_i^{\ast }\leftarrow$RemapGlobalGoals(${𝐆}_i,{\sigma }^{\ast }$);

19: ${𝒯}_{ref}^{\ast }\leftarrow$GlobalTrajectoryReplan(${\mathrm{𝐥𝐠}}_i^{\ast },{𝐆}_i^{\ast }$);

20: Return ${𝒯}_{ref}^{\ast }$;

21:end if

算法1 全局重映射策略

集群系统无法快速从未知障碍物或所需编队形状突然变化引起的无序状态中恢复。 为了应对这一挑战，我们提出了一种利用全局重映射局部重映射（GRLR）策略来生成轨迹的新方法。 这种方法使我们能够有效地驾驭复杂的环境，同时将集群保持在所需编队的“临界状态”，从而有效地平衡协调性和适应性。

对于分布式框架，通信有助于机器人获取其他机器人的信息并产生更好的协调行为。 特别是在编队变换的情况下，协同决策可以使集群编队快速收敛。 然而，有时会因通信延迟而出现循环依赖，难以保证决策的一致性。 因此，我们为蜂群编队系统设计了G全局-R地图-L局部-R计划（GRLR）策略，该策略只集中重新映射编队的关键参数，并通过分布式重新规划局部轨迹来保持编队协调。

GRLR策略包括单个机器人的局部重新规划和编队级系统的全局重新映射。 local-replan是一种后退地平线增量规划策略[44]，它允许每个机器人在其有限的感知范围内规划一条轨迹。 局部目标是在规划范围${\mathrm{\Psi }}_p$内的全局参考轨迹上选择的，如图6（a）所示。 global-remap是一种高效的中心化策略，仅通过求解ALAS方法重新映射局部目标并细化全局参考轨迹，如图6（b）所示。 GRLR策略非常适合分布式异步系统，即使存在网络延迟，Swarm系统也不会出现死锁。

所提出的全局重映射策略的主要工作流程在算法1中描述。 在生成新的编队行为之前，全局重映射策略会检查是否存在任何紧急事件（第 1-12 行），例如群体编队的稳定状态正在被摧毁（第 9 行）或即将被摧毁（第 4 行） 。 与以固定频率触发的局部重新计划策略不同，全局重新映射策略由紧急事件启动（第 13 行）。 然后调用ALAS方法来求解最优分配${\sigma }^{\ast }$和对齐$s^{\ast },{𝐝}^{\ast }$（第14-15行）。 全局重映射策略重新映射局部目标${\mathrm{𝐥𝐠}}_i^{\ast }$和全局目标${𝐆}_i^{\ast }$，并为每个机器人生成新的全局轨迹${𝒯}_{ref}^{\ast }$（第16-19行）。 最后，机器人通过执行局部重新规划策略形成新的队形。

在这项工作中，我们利用这种半分布式 GRLR 策略，通过在 1 Hz 频率下重新规划局部轨迹并检查突发事件以在 20 Hz 频率下触发全局重映射策略，使集群编队能够适应未知障碍物或所需编队形状的突然变化。 。

VII-B 编队级全局寻路

算法2 编队级全局寻路

1:Tree $𝒯$,  State $𝐳$,  Path cost $c$,  Path $𝐏$;

2:Initialize: ${𝒯}_a\leftarrow \mathrm{\varnothing }\cup \{{\text{z}}_{start}\}$,  ${𝒯}_b\leftarrow \mathrm{\varnothing }\cup \{{\text{z}}_{goal}\}$,   $c_{best}\leftarrow \mathrm{\infty }$,  $FoundSolution\leftarrow False$;

3:for $i=1$ to $N$ do

4: ${\text{z}}_{random}\leftarrow$ Sample(${\text{z}}_{start},{\text{z}}_{goal},c_{best}$);

5: if not $FoundSolution$ then

6: ${\text{z}}_{new}\leftarrow$ GreedyExtendTree(${𝒯}_a,{\text{z}}_{random}$);

7: ${\text{z}}_{conn}\leftarrow$ NearestVertice(${\text{z}}_{new},{𝒯}_b$);

8: $c_{new}\leftarrow$ Connect(${\text{z}}_{new},{\text{z}}_{conn},{𝒯}_a,{𝒯}_b$);

9: if then

10: $c_{best}\leftarrow c_{new}$;

11: $FoundSolution\leftarrow True$;

12: end if

13: else

14: ${\text{z}}_{new}\leftarrow$ ExtendTree(${𝒯}_a,{\text{z}}_{random}$);

15: ${𝒵}_{near}\leftarrow$ NearVertex(${\text{z}}_{new},{𝒯}_a$);

16: ${𝒯}_a\leftarrow$ Rewire(${\text{z}}_{new}$,${𝒵}_{near}$);

17: ${\text{z}}_{conn}\leftarrow$ NearestVertice(${\text{z}}_{new},{𝒯}_b$);

18: $c_{new}\leftarrow$ Connect(${\text{z}}_{new},{\text{z}}_{conn},{𝒯}_a,{𝒯}_b$);

19: if then

20: $c_{best}\leftarrow c_{new}$;

21: end if

22: end if

23: SwapTrees(${𝒯}_a,{𝒯}_b$);

24:end for

25:$𝐏\leftarrow$ RetrievePath(${𝒯}_a,{𝒯}_b$);

26:Return $𝐏$;

我们提出了一种编队级全局寻路方法。 给定起始和目标配置，规划器会生成一条将它们与无碰撞中间编队连接起来的可行路径。 采用双向 RRT 方法来解决此寻路问题。

许多导航任务都期望编队能够以所需的规模进行机动。 在实际应用中，编队规模过大会降低车辆的通信质量，而编队规模过小则会增加车辆碰撞的风险。 与[45]中的方法仅对地层中心位置$𝐩\in {ℝ}^3$进行采样不同，我们的方法将地层尺度$s$添加到采样空间中，并使编队配置$𝐳=\{𝐩,s\}\in {ℝ}^3\times {ℝ}^{+}$。 这样，可以通过最小化配置空间中路径的$L_2$-范数距离$𝐳$来实现维持所需尺度的目标，即最小化沿路径的尺度变化。 >。

在密集环境中导航需要机器人保持队形，同时让障碍物穿过队形。 [46]中的方法对中心位置$𝐩$进行采样，然后通过优化无障碍凸区域的编队布置来求解比例因子$s$ 。 然而，这种方法不允许任何障碍物与地层的凸包相交，因此浪费了许多解。 相比之下，我们的方法直接对编队配置$𝐳$的整个状态进行采样，以充分探索解空间。 对于我们的 RRT 算法的每个边缘，都会对每个机器人而不是编队的整个凸包进行碰撞检查，以允许障碍物通过。

我们的双向 RRT 规划器的主要工作流程在 Algorithm.2 中描述，其中两棵树 ${𝒯}_a$ 和 ${𝒯}_b$ 从初始状态 ${\text{z}}_{start}$ 和目标状态 ${\text{z}}_{goal}$ 分别。 在找到第一个解决方案之前，双向规划器以 RRT-Connect[47] 方式扩展树（第 5-7 行）。 在GreedyExtendTree()和Connect()中，采用贪婪启发式[47]积极探索环境并进行树连接尝试。 找到可行解后，即Connect()返回有限路径成本$c_{new}$后，函数Sample()计算一个知情采样集新成本 $c_{new}$ 如 [48] 中所示。 然后，在每个循环中执行标准双向 RRT*[49] 过程来更新树（第 13-17 行）。 由于路径成本是配置空间$𝐳$中的$L_2$-范数距离，因此通知采样[48]和双向RRT*[49] 可以保证路径解的渐近最优性。

当全局环境信息可用时，部署该编队级路径规划器来渲染全局路径。 然后使用 MINCO[39] 生成连接全局路径的路点的全局轨迹，并采用 Sec.V 中的框架来优化局部运动。

八基准

在基准测试中，重要的是评估当前编队${ℱ}^c$相对于飞行期间所需编队${ℱ}^d$的扭曲程度。 受 [22] 的启发，我们求解了以下非线性优化问题，以找到使 ${ℱ}^c$ 与 ${ℱ}^d$ 一致的最佳相似性变换（$Sim(3)$ 变换）。 然后在归一化地层尺度上计算平均地层距离度${\overline{e}}_{dist}$

$${\overline{e}}_{dist}=\frac{1}{s_o\cdot L}{\int }_{ℒ}\underset{𝐑,𝐭,s}{\min }\sum _{i=1}^n{\Vert {𝐩}_i^d-(s𝐑{𝐩}_i^{𝐜}+𝐭)\Vert }^{2}dl,$$ (45)

其中${𝐩}_i^d$和${𝐩}_i^c$分别表示$i^{th}$机器人在编队${ℱ}^d$和${ℱ}^c$中的位置。 $Sim(3)$变换由旋转$𝐑\in SO(3)$、平移$𝐭\in {ℝ}^3$和缩放$s\in {ℝ}_{+}$组成。 其中，$s_o$为初始编队规模，$L$为编队轨迹长度$ℒ$。优化 (45) 中的变换并将其应用于编队，可以消除缩放和旋转的影响，以便通过测量所需编队的位置误差来公平地评估所有编队。 较大的 ${\overline{e}}_{dist}$ 表示与所需形态 ${ℱ}^d$ 的变形较大。 此外，我们还计算了平均队形相似度${\overline{e}}_{sim}$

$${\overline{e}}_{sim}=\frac{1}{s_o\cdot L}{\int }_{ℒ}{\Vert \widehat{𝐋}-{\widehat{𝐋}}_{des}\Vert }_F^2𝑑l,$$ (46)

其中 $\widehat{𝐋}$ 和 ${\widehat{𝐋}}_{des}$ 详见 (5)，形成相似度误差 ${\Vert \widehat{𝐋}-{\widehat{𝐋}}_{des}\Vert }_F^2$ 在第 IV 节中提出。 我们在表 I 中显示了以下基准、模拟和实际实验中使用的重要参数。 所有基准测试均在配备 Intel i7-12700 CPU 的台式机上运行。

表一： 该方法的地层参数

Parameter	Symbol	Value
Similarity error threshold	$e_{sim,d}$	0.05
Constraint awareness threshold	$g_d$	$2/N$
Parameter for regulation in (37)	$\alpha$	5
Parameter for regulation in (37)	$\lambda$	25
Parameter for regulation in (37)	$\gamma$	-1
Sampling time interval ($s$)	$\delta$	0.5
Planing Horizon ($m$)	${\mathrm{\Psi }}_p$	7.5
Max velocity ($m/s$)	$v_m$	1.0
Max acceleration ($m/s^2$)	$a_m$	6.0
Weight for control effort	${\lambda }_e$	10000.0
Weight for total time	${\lambda }_t$	80.0
Weight for swarm reciprocal avoidance	${\lambda }_r$	10000.0
Weight for obstacle avoidance	${\lambda }_o$	10000.0
Weight for swarm formation similarity	${\lambda }_f$	10000.0
Weight for dynamic feasibility	${\lambda }_d$	10000.0

图7： 随机森林地图和基准的形成类型。 (a) 随机森林图。 (b) 正六边形形状。 (c) 形状不规则。 (d) 三棱柱形状。 (e) 八面体形状。

表二： 编队定义方法的性能比较

	Formation type	Regular hexagon		Irregular shape		Triangular prism		Octahedron
Scenario		${\overline{e}}_{dist}(\%)$	${\overline{e}}_{sim}(\%)$	${\overline{e}}_{dist}(\%)$	${\overline{e}}_{sim}(\%)$	${\overline{e}}_{dist}(\%)$	${\overline{e}}_{sim}(\%)$	${\overline{e}}_{dist}(\%)$	${\overline{e}}_{sim}$(%)
Same to same	Position	39.120	0.384	35.649	0.384	34.506	0.374	21.534	0.341
	Displacement	16.231	0.172	15.023	0.159	14.952	0.125	11.645	0.153
	Distance	15.489	0.164	14.295	0.131	14.009	0.118	10.285	0.113
	Ours	15.443	0.161	14.287	0.128	14.012	0.119	10.281	0.112
Rotation change	Position	57.456	0.945	51.298	0.732	49.821	0.612	34.124	0.542
	Displacement	39.456	0.412	31.012	0.439	29.546	0.345	23.125	0.353
	Distance	27.513	0.312	22.312	0.234	21.031	0.201	14.173	0.159
	Ours	19.234	0.218	17.032	0.171	15.013	0.151	12.146	0.138
Scale change	Position	59.654	1.098	58.416	0.784	53.246	0.741	37.845	0.555
	Displacement	42.516	0.629	40.021	0.624	39.412	0.398	29.845	0.395
	Distance	59.542	1.030	59.105	0.799	54.126	0.632	38.451	0.578
	Ours	18.332	0.192	18.196	0.185	16.023	0.179	12.264	0.164
Scale & rotation change	Position	62.584	1.304	60.124	0.796	56.213	0.832	41.856	0.635
	Displacement	45.627	0.755	40.194	0.631	40.168	0.423	31.288	0.504
	Distance	62.154	1.250	61.059	0.804	54.317	0.684	42.138	0.684
	Ours	20.231	0.243	18.345	0.204	16.851	0.183	12.357	0.175

表三： 编队导航方法的性能比较

	Formation type	Regular hexagon
Scenario		$successrate(\%)$	$length(m)$	${\overline{e}}_{dist}(\%)$	${\overline{e}}_{sim}(\%)$
Sparse	VRB[26]	75	22.978	57.962	0.984
	Spatial-only	100	21.923	15.023	0.152
	Spatial-temporal	100	21.756	11.240	0.138
Medium	VRB[26]	25	-	-	-
	Spatial-only	100	22.130	14.927	0.158
	Spatial-temporal	100	21.932	13.274	0.153
Dense	VRB[26]	0	-	-	-
	Spatial-only	100	22.283	17.630	0.185
	Spatial-temporal	100	22.133	15.443	0.161

VIII-A 基于图的地层定义的适应性

为了证明第IV节中基于图的地层定义的适应性，我们与[50]中总结的主流地层定义方法进行了多次基准测试，这些方法是基于基于受控变量的方法，即基于位置的[51]、基于距离的[52]和基于位移的方法[53]。

我们在我们的框架中实现这些方法，并通过替换(12)中的原始成本$J_s$来适应密集环境，以生成统一的最佳编队位置序列${\widehat{𝐩}}_i^{\ast }$ 对于每个机器人$i$。 对于基于位置的方法，我们将 Drone_0 设置为领导者，并为所有其他机器人预定义绝对相对位置以指定所需的编队。 所以它的成本是$J_{s,1}=0$。 基于距离的方法优化了期望代理间距离的误差

$$J_{s,2}=\sum _{j\in N}{\left(\Vert {𝐩}_i-{𝐩}_j\Vert -\Vert {𝐩}_i^d-{𝐩}_j^d\Vert \right)}^2,$$ (47)

其中 $N$ 是机器人的数量，${𝐩}_i^d$ 是 $i^{th}$ 机器人所需的位置向量。 基于位移的方法优化了所需相对位移的误差

$$J_{s,3}=\sum _{j\in N}{\Vert ({𝐩}_i-{𝐩}_j)-({𝐩}_i^d-{𝐩}_j^d)\Vert }^2.$$ (48)

然后，我们在$40\times 15m$大小的高密度环境中模拟四种不同的几何形态类型，并随机生成障碍物，如图7（a）所示。 考虑不规则和规则几何形状的2D和3D地层，即正六边形、不规则几何、三棱柱和八面体的地层类型，如图7(b)-(e)所示。 为了充分比较这些方法的适应性，我们设计了四种不同的场景，考虑地层形状的缩放和旋转变化。 编队的初始位置和最终位置可能在规模和旋转方面有所不同。 然后将场景相应分类为“相同到相同”、“旋转变化”、“比例变化”和“比例和旋转变化”。 我们针对每种场景和编队类型对每种方法进行 20 次测试。 表II总结了${\overline{e}}_{dist}$(45)和${\overline{e}}_{sim}$(46)的相应结果>。

与我们基于图形的编队定义不同，在其他方法中，在飞行过程中不允许改变编队的规模和旋转。 如表II所示，在相同地层类型和相同场景下，数据表明我们的方法取得了有希望的结果，${\overline{e}}_{sim}$和${\overline{e}}_{dist}$几乎是最低的。 此外，当场景变得更加复杂时，我们的方法显示出最低的错误增长率。 在同一场景下，随着地层类型从2D到3D中心对称结构的变化，所有方法的畸变程度均减小，这说明地层的维持也与地层本身的结构稳定性有关。 此外，基于距离的方法对旋转具有不变性，并且在“旋转变化”场景中实现了相对可接受的性能。 然而，它无法处理尺寸变化的情况。 同样，其他方法对旋转或缩放敏感，导致此类场景中的性能显着下降。 一般来说，我们基于图的地层定义方法实现了缩放和旋转不变性。 这种不变性提高了编队飞行的适应性，在复杂场景下优于主流方法。

图8： 不正确初始条件下使用和不使用 ALAS 时编队飞行的比较。 (a) 在没有 ALAS 的情况下，执行的轨迹（橙线）是蜿蜒的，并且由于全局轨迹（蓝线）交叉，地层形状（红线）缓慢收敛。 (b) 就 ALAS 而言，集群重组使编队飞行过程变得有序。

图9： 有和没有 ALAS 的受限环境中编队飞行的比较。 浅蓝色区域代表一堵有洞的墙，群体机器人必须缩小才能通过。 (a) 在没有 ALAS 的情况下，地层形状（红线）发生变形，强行穿过该区域。 (b) 在 ALAS 的情况下，编队形状主动变小以通过该区域。 (c) ALAS 编队飞行会降低每个机器人的约束意识（红色虚线）。 这一改进保持了地层形状的相似性误差较低（红线）。

VIII-B 时空轨迹优化的可预测性

为了证明第V节中编队轨迹优化的可预测性，我们将我们的工作与虚拟刚体（VRB）方法[26]进行比较，这是一种SOTA编队控制框架，利用势场避开障碍物。 此外，我们还比较了仅空间优化和时空优化之间的性能，以说明时域对于编队飞行的重要性。 我们模拟七架无人机在正六边形中从一侧飞行到另一侧，速度限制为 $0.5m/s$。 杂乱区域的大小为$30\times 15m$，测试了三种障碍物密度进行比较。 参数经过微调以获得每种方法的最佳性能。

结果如表III所示，表明VRB方法[26]在处理中等和密集障碍物时的成功率并不理想。 这主要是由于多个相互作用的势场产生的短期避障，往往会导致走廊附近出现局部极小值，导致机器人被困而失败。 然而，优化方法考虑了编队未来的运动，因此可以平衡编队维持和避障，但不会破坏编队形状。 因此，优化方法可以获得更好的性能并保持成功率。

我们还可以得出结论，在考虑时间优化时，时空方法更加省力、稳健和灵活。 仅空间方法无法在时域中调整轨迹，从而导致轨迹发生过度的空间变形。 因此，仅空间方法中的轨迹长度和编队误差${\overline{e}}_{sim}$和${\overline{e}}_{dist}$较大。

VIII-C 群体重组方法的弹性

为了验证VI节中的蜂群重组方法，我们设计了两个基准来说明任务分配的必要性和编队排列的适应性。

首先设计了不正确初始条件下带ALAS和不带ALAS的正六边形编队飞行对比，验证编队任务分配的必要性，如图8所示。 蓝线代表每个机器人的全局轨迹及其在编队中分配的任务。 在图8(a)中，由于任务分配不当，全局轨迹部分交叉，导致轨迹优化冲突。 所以橙色线所示的执行轨迹看起来非常混乱，而红色线所示的队形形状收敛缓慢。 图8(b)中，通过考虑ALAS，上述问题得到有效解决。 经过一次ALAS计算后，集群机器人重新分配编队任务，并迅速达成集群共识。 然后，蜂群编队平稳地收敛到所需的形状，并以节能的方式导航到目的地。 本次测试的结果验证了任务分配的必要性。

然后，我们比较了通过受限孔时有和没有 ALAS 的编队飞行，以显示 ALAS 的适应性。 图9(a)和图9(b)的结果表明，在没有ALAS的情况下通过走廊时，地层形状可能会发生变形。 另外，ALAS 的情况可以自适应地调整编队形状以适应受限环境。 从图9(c)的定量分析结果来看，使用ALAS的情况可以快速调整编队规模，使群体达成共识，从而使每个机器人的编队相似误差和约束意识下降迅速。 然而，在没有ALAS的情况下，直到群体编队离开洞口之前，都会保持较高的相似性误差和约束意识，这意味着群体编队始终处于环境的限制之内，从而导致编队形状无法收敛。 该基准证明了队形对准的适应性。

表四： 全局寻路方法对比

	Alonso-Mora’s method[46]	Ours
Sampling time $(s)$	2.0	2.0
Desired scale $(m)$	3.0	3.0
Path length $(m)$	50.77	24.10
Min scale along the path $(m)$	1.88	2.98

图10： 在同一约束地图中阿隆索莫拉方法与我们的编队级寻路方法的比较。 (a) 该方法对自由空间中的凸区域（紫色多面体）进行采样，如果交点在地层中可穿过，则将它们连接起来。 由于凸区域必须在安全空间中生成，因此该方法过于保守，无法生成较小尺度的较长路径。 (b) 我们的方法直接对 3D 尺度编队配置的整个状态进行采样，以充分探索解空间，使编队能够穿过微小的障碍物。

图11： 之前的工作和建议的工作之间的时间消耗比较[4]。 我们比较了 15 个机器人场景中两种方法的性能。 详细比较结果如表V所示。

VIII-D 群体协议方法的弹性

为了突出我们的群体协议方法的弹性，我们在约束地图中比较了 Alonso-Mora 的全局规划方法[46]和我们的编队级寻路方法，如图 10。 这张地图由几个方块和一些微小的障碍物组成，群体机器人需要在其中找到允许编队安全通过的路径。 我们对规划器进行了 20 次测试，表 IV 显示了平均结果数据。 结果表明，Alonso-Mora 的方法[46]在处理这种情况时效果并不理想。 方法[46]对地层规模变化没有惩罚，可以选择导致地层规模突变的廊道路线。 此外，它无法处理微小的障碍物，因此会屈服于较小尺度的较长路径，如图10(a)所示。 与阿隆索-莫拉的方法不同，我们的编队级寻路方法直接在增强的3D尺度空间中采样，可以更好地保持所需的编队尺度。 如图10(b)所示，我们的方法生成了更短的路径，并且只牺牲了少量的地层尺度。 因此，我们的方法可以处理带有块和微小障碍物的地图，并为群体形成找到安全的指导，这更适合密集的环境。

VIII-E 解耦编队优化的效率

表五： 15架无人机场景性能对比

	Previous method[4]	Proposed method
Time consumption $(ms)$	141.7	38.2
success rate $(\%)$	95.0	100.0
length $(m)$	47.387	45.282
${\overline{e}}_{dist}$ $(\%)$	12.439	11.724
${\overline{e}}_{sim}$ $(\%)$	0.147	0.139

我们将我们提出的解耦编队优化与之前的耦合编队优化[4]进行比较，后者在优化过程中多次直接调用编队相似距离度量(5)。 为了确保公平比较，我们在此基准测试中排除了 ALAS 问题。 两种方法的结果如图11所示。 之前的方法[4]仅支持小规模的群体形成，因为计算时间呈指数增长。 由于采用了解耦的方法，我们提出的方法对于42个机器人的集群的时间消耗不超过150ms，可以支持大规模集群的实时应用。

我们从 15 架无人机场景中选择实验数据，如表 V 所示。我们的方法不仅比以前的方法显着缩短了计算时间，而且在编队维护、成功率和轨迹长度方面表现出了更好的性能。 这验证了我们方法的有效解耦，从而实现了全面的性能改进。

IX 现实世界的实验和模拟

IX-A 现实世界的实验

我们的方法与III节中所述的自主分布式空中集群系统集成。 群体通过广播网络共享一些信息，例如轨迹，这是所有机器人之间的唯一连接。 如图12所示，我们使用带有本地传感器和机载计算机的手掌大小的四旋翼平台[5]。 估计、感知、规划、控制等软件模块都在船上实时运行。 实际实验中群体机器人的最大数量为 16 个。 设计了三个不同的真实世界实验来充分验证所提出的编队飞行系统的特性。

图 12： 手掌大小的群体空中机器人的插图。

图 13： 正四面体结构穿过走廊的合成快照。 虫群旋转并压缩队形，从右向左飞过狭窄的空间。 蓝线显示地层形状的轮廓。

图 14： 3-D 地层转化实验插图。 蓝色圆圈代表分配给上部位置的四旋翼飞行器，黄色圆圈代表分配给下部位置的四旋翼飞行器。 白线代表地层形状的轮廓。 (a) 编队飞行从不收敛的初始位置和不正确的初始任务分配开始。 因此群体机器人调用GRLR策略来重新组织编队参数。 (b) 3 秒后，群体机器人收敛到所需的立方体形状。 然后Swarm收到编队变换命令并快速调用GRLR策略。 (c) 3 秒后，群体机器人收敛到所需的双箭头形状。 (e) 执行的轨迹（浅蓝色线）表明群体形成在大多数情况下是收敛的。 (d) 更准确的数值分析表明，调用 GRLR 策略后，相似性误差迅速减小。

在第一个实验中，如图13所示，四架四旋翼飞行器组成三维正四面体，成功地安全通过了狭窄的走廊。 在飞行过程中，集群会根据环境变化自适应地旋转和压缩编队形状。 该测试证明，缩放和旋转不变性为受限空间中的编队飞行提供了更大的灵活性。

图 15： 地层多次变换模拟。 轨迹和机器人的不同颜色对应于不同的时间戳。 我们选择四个特定的时间戳，对应的队形构成“FAST”(http://zju-fast.com/)。

图 16： 在迷宫地图中编队飞行。 编队级全局路径（蓝线）避开墙壁，同时忽略小障碍物。 然后集群编队遵循全局路径并生成局部轨迹以避开所有障碍并保持编队形状。

然后我们设计了一个3D地层形状变换实验来验证我们方法的重组能力，如图14所示。 在图14(a)中，所需的队形是立方体形状，但群体机器人从未收敛的初始位置和不正确的初始任务分配中导航。 一开始，集群机器人快速调用GRLR策略，使编队形状快速收敛到所需的正方形，如图14(b)所示。 然后，集群机器人接收来自站位笔记本电脑的编队变换命令，并汇聚成双箭头形状，如图14(c)所示。 导航过程的俯视图如图14(e)所示。 从浅蓝色的执行轨迹可以看出，群体机器人的飞行行为在时间$[4,10]$和时间$[14,20]$期间趋于一致。 此外，在时间$[0,4]$和时间$[10,14]$期间，群体机器人试图达成群体共识并频繁调整飞行行为以形成所需的编队形状。 从图14(d)可以看到更准确的数值分析。 当编队系统远离收敛状态或满足编队变换命令时，集群机器人通过在虚线对应时刻调用GRLR策略来调整编队排列和任务分配。 根据红线代表的相似度误差，我们可以看到，除了初始时刻的不收敛状态和突然的队形变换之外，集群编队在避开障碍物的同时，能够保持所需的形状。

最后，我们在室外进行了16架无人机编队飞行实验。 据我们所知，这是复杂户外环境下最大规模的全自主编队飞行实验。 如图1所示，16架无人机排成三角形队列，成功穿越障碍物丰富的区域，且未发生碰撞。 该地区有许多行道树、木桩和路灯。 该实验证明了我们提出的方法的鲁棒性和大规模能力。 更多详情请观看实验视频。

图 17： 大规模编队飞行，由远及近。 从执行的轨迹可以看出，集群机器人在避开障碍物的同时仍然保持队形。

IX-B 模拟实验

为了全面展示所提出的编队飞行方法的特点，我们还进行了多次模拟实验来补充现实世界的实验。 所有模拟实验均在配备 Intel i7-12700 CPU 的台式机上实时运行。

首先，我们设计了编队多重变换模拟，以验证在应对密集环境中的紧急变化时群体重组能力，如图15所示。 不同颜色的轨迹代表编队飞行从左到右的时间流。 相同颜色的机器人对应于该时间戳处的编队形状。 队形变换的命令是实时下达的，而不是预先设定的。 群体机器人必须克服所需编队形状的瞬时变化并快速收敛到新的编队状态。 由于所提出的方法在复杂环境中快速变换队形的出色能力，我们最终为我们的实验室生成了缩写“FAST”。

然后，我们建立了一个特殊的迷宫模拟环境，由墙壁和许多小障碍物（如柱子和圆环）组成。 在这个模拟中，我们的目的是验证群体协议的能力。 编队级全局寻路方法首先在全局地图中运行，这是VII-B中提出的集中式方法。 然后，它生成考虑地层形状尺度的全局路径，如图16中的蓝线所示。 蓝色立方体形状代表采样点。 之后，8 个机器人形成立方体队形，遵循全局路径进行导航，并使用第 V 节中的分布式方法生成局部队形轨迹。 此次仿真表明，我们的编队飞行系统能够更好地适应障碍物约束，为编队飞行提供更安全的引导。

最后，为了测试我们的方法在大规模不规则编队中的有效性，我们设计了一个由 30 架无人机组成的双箭编队。 如图17所示，集群成功避开了障碍物，并且在飞行过程中很好地保持了所需的队形。

X 结论和未来工作

本文详细分析了密集环境下实现编队飞行的核心困境，准确总结了解决上述问题的PAPER标准。 然后，我们提出了一种分层编队飞行架构，由基于图的编队定义、分布式编队轨迹优化、群体重组方法和群体协议方法组成。 所提出的完整编队飞行系统满足所有PAPER标准，并在密集环境中保持协作编队飞行方面取得了优异的性能。 我们在适应性、可预测性、弹性、弹性和效率方面设计了全面的基准，以验证我们提出的方法的出色性能。 最后，我们进行了大量的现实世界实验和模拟，证明我们解决了大规模集群密集环境下的自主编队飞行问题。

未来，我们打算通过子图的局部信息传播，进一步提高分布式编队飞行方法的效率。 此外，虽然我们目前的工作需要操作员手动确定地层形状，但优化地层形状的研究是一个有前途的领域。 我们相信它有潜力展示更智能、更协作的群体行为，最终提高任务能力。

-A 对准问题的闭式解

现在我们推导出队形对准问题的封闭式解。 假设在分配阶段之后，我们有了当前的机器人位置 ${\mathrm{𝐥𝐠}}_i$ 和具有最佳匹配的所需队形 ${𝐪}_i$。 我们使用$c_i$来表示(42)中对齐目标的$i^{th}$项。 然后我们有

$c_i$ $=w_i\cdot {\Vert {\mathrm{𝐥𝐠}}_i-(s{𝐪}_i+𝐝)\Vert }^2,$ (49)

$=w_i{({\mathrm{𝐥𝐠}}_i-s{𝐪}_i-𝐝)}^T({\mathrm{𝐥𝐠}}_i-s{𝐪}_k-𝐝),$

$=w_i{\mathrm{𝐥𝐠}}_i^T{\mathrm{𝐥𝐠}}_i+s^2{w}_i{𝐪}_i^T{𝐪}_i-2s{w}_i{\mathrm{𝐥𝐠}}_i^T{𝐪}_i+$

$\mathrm{\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}2}s{w}_i{𝐪}_i^T𝐝-2w_i{\mathrm{𝐥𝐠}}_i^T𝐝+w_i{𝐝}^T𝐝.$

我们使用$F$来表示(42)中的对齐目标。 那么目标$F$可以写成

$F(s,𝐝)$ $={\displaystyle \sum _i^n}{c}_k,$ (50)

$=b_{lg}+s^2{b}_q-2s{b}_{lg,q}$

$+2s{\widehat{𝐪}}^T𝐝-2{\widehat{\mathrm{𝐥𝐠}}}^T𝐝+{𝐝}^T𝐝,$

在哪里

$b_{lg}$ $={\displaystyle \sum _i^n}{w}_i{\mathrm{𝐥𝐠}}_i^T{\mathrm{𝐥𝐠}}_i,$ (51)

$b_q$ $={\displaystyle \sum _i^n}{w}_i{𝐪}_i^T{𝐪}_i,$

$b_{lg,q}$ $={\displaystyle \sum _i^n}{w}_i{\mathrm{𝐥𝐠}}_i^T{𝐪}_i,$

$$\widehat{𝐪}=\sum _i^n{w}_i{𝐪}_i,\widehat{\mathrm{𝐥𝐠}}=\sum _i^n{w}_i{𝐩}_i.$$ (52)

由于意识权重 $w_i$ 是 softmax 函数的输出，因此我们有属性 ${\sum }_i^n{w}_i=1$。 请注意，我们使用此属性来简化 (50) 中的最后一项。 现在我们需要证明 (50) 中的目标 $F$ 是凸的。 缩放参数$s$和平移$𝐝$。 目标的 Hessian 矩阵由下式给出

(53)

Hessian 矩阵的特征值$\lambda$如下

$$\lambda =\{2,\mathrm{\hspace{0.17em}\hspace{0.17em}2},(b_q+1)\pm \sqrt{{(b_q+1)}^2-4b_w}\},$$ (54)

在哪里

$b_w$ $=b_q-{\widehat{𝐪}}^T\widehat{𝐪},$ (55)

$={\displaystyle \sum _i^n}{w}_i{𝐪}_i^T{𝐪}_i-{({\displaystyle \sum _i^n}{w}_i{𝐪}_i)}^T({\displaystyle \sum _i^n}{w}_i{𝐪}_i).$

我们需要证明(55)是非负的，那么(54)中的所有特征值都将是非负的，因此Hessian是半正定的， (50) 中的目标将是凸的。 变量 $s$ 和 $𝐝$。

对于非负权重$w_i$，我们具有来自softmax函数的属性${\sum }_i{w}_i=1$，因此我们构造一个凸函数$h(𝐱)={𝐱}^T𝐱$，并应用加权詹森不等式。 我们得到

$$w_{1}h({𝐪}_1)+\mathrm{\cdots }+w_{n}h({𝐪}_n)\ge h(w_1{𝐪}_1+\mathrm{\cdots }+w_n{𝐪}_n),$$ (56)

$$\Rightarrow \sum _i^n{w}_i{𝐪}_i^T{𝐪}_i\ge {(\sum _i^n{w}_i{𝐪}_i)}^T(\sum _i^n{w}_i{𝐪}_i).$$ (57)

因此 (55) 是非负的，目标 $F$ 是凸的。 然后我们可以通过求解获得该对齐问题的封闭式解

(58)

问题 (42) 的封闭式解由下式给出

$s^{\ast }$ $={\displaystyle \frac{b_{lg,q}-{\widehat{\mathrm{𝐥𝐠}}}^T\widehat{𝐪}}{b_q-{\widehat{𝐪}}^T\widehat{𝐪}}},$ (59)

${𝐝}^{\ast }$ $=\widehat{\mathrm{𝐥𝐠}}-s^{\ast }\widehat{𝐪}.$

Lun Quan received the B.Eng. degree in control science and engineering in 2019 from Zhejiang University, Hangzhou, China, where he is currently working toward a Ph.D. degree in control science and engineering. His research interests include motion planning for multi-robot systems, swarm intelligence, and autonomous navigation for unmanned vehicles.

Longji Yin received the B.Eng. degree in automation from Zhejiang University, Zhejiang, China, in 2019 and the M.Sc. degree in robotics from Johns Hopkins University, Maryland, U.S. in 2021. In 2022, he worked as a research assistant at Zhejiang University, Zhejiang, China. He is currently working toward a Ph.D. degree in mechanical engineering at the University of Hong Kong, Hong Kong. His research interests include motion planning and mapping for autonomous aerial robots.

Tingrui Zhang received the B.Eng. degree in automotive engineering from Beijing Institute of Technology, Beijing, China, in 2022. He is currently pursuing an M.Phil. degree in control engineering at Zhejiang University, Hangzhou, China. His research interests include motion planning for autonomous robots and numerical optimization.

Mingyang Wang received the B.Eng. degree in control science and engineering from Zhejiang University, Hangzhou, China, in 2022. He is currently working toward an M.Phil. degree in control science and engineering from Zhejiang University, Hangzhou, China. His research interests include motion planning and control for aerobatic flight.

Ruilin Wang received the B.Eng. degree in control science and engineering from Zhejiang University, Hangzhou, China, in 2023. He is currently working toward an M.Phil. degree in artificial intelligence at Sun Yat-sen University, Zhuhai, China. His research interests include aerial robots and motion planning.

Sheng Zhong received the B.Eng. degree in control science and engineering from Zhejiang University, China, in 2023. He is currently working toward a Ph.D. degree in control science and engineering at Hunan University, Changsha, China. His research interests include motion estimation based on event cameras.

Xin Zhou received the B.Eng. degree in electrical engineering and automation from China University of Mining and Technology, Xuzhou, China, in 2019. He is currently working toward the Ph.D. degree in control engineering from Zhejiang University, Hangzhou, China. His research interests include motion planning and mapping for aerial swarm robotics.

Yanjun Cao received his Ph.D. degree in computer and software engineering from the University of Montreal, Polytechnique Montreal, Canada, in 2020. He is currently an associate researcher at the Huzhou Institute of Zhejiang University, as a PI in the Center of Swarm Navigation. He leads the Field Intelligent Robotics Engineering (FIRE) group of the Field Autonomous System and Computing Lab (FAST Lab). His research focuses on key challenges in multi-robot systems, such as collaborative localization, autonomous navigation, perception, and communication.

Chao Xu received his Ph.D. in Mechanical Engineering from Lehigh University in 2010. He is currently Associate Dean and Professor at the College of Control Science and Engineering, Zhejiang University. He serves as the inaugural Dean of ZJU Huzhou Institute, as well as plays the role of the Managing Editor for IET Cyber-Systems & Robotics. His research expertise is Flying Robotics, Control-theoretic Learning. Prof. Xu has published over 100 papers in international journals, including Science Robotics (Cover Paper), Nature Machine Intelligence (Cover Paper), etc. Prof. Xu will join the organization committee of the IROS-2025 in Hangzhou.

Fei Gao received the Ph.D. degree in electronic and computer engineering from the Hong Kong University of Science and Technology, Hong Kong, in 2019. He is currently a tenured associate professor at the Department of Control Science and Engineering, Zhejiang University, where he leads the Flying Autonomous Robotics (FAR) group affiliated with the Field Autonomous System and Computing (FAST) Laboratory. His research interests include aerial robots, autonomous navigation, motion planning, optimization, and localization and mapping.