突破性算法：让无人机集群在狭窄空间内穿针引线

导读

在建筑救援、森林搜索等任务中，无人机集群经常会遇到狭窄空间限制和动态障碍物变化等挑战。这些挑战会导致集群内部冲突，或在执行任务时因避让动态障碍物而导致系统混乱。实际应用场景和任务的严格特征往往使得全局搜索难以优化，而局部避障容易陷入僵局。为解决上述问题，本文提出了一种受全局时空路径启发的分层在线协同规划框架。

为了在化解整体冲突的同时降低求解复杂度，系统首先进行全局粗略搜索以获得初始时空指导路径。为了在不影响系统内部和谐的情况下克服动态障碍物，系统执行基于初始路径启发的在线冲突趋势聚类协同优化，以实现避障同时提高可扩展性。

本文在包含狭窄管道和动态障碍物的仿真环境中进行了实验测试，并与目前主流的集中式和分布式方法进行了对比。结果表明，本文提出的方法能在约束丰富的动态环境中生成更平滑、更安全的轨迹，并具有更高的成功率。©️【深蓝AI】编译

1. 研究背景和相关工作

无人机集群具有高效覆盖大面积区域和协同执行多样化任务的能力。它们已被应用于搜索与救援、地图绘制、森林火灾监测、农业以及其他多种任务中。然而在实际应用中，无人机集群经常会遇到空间受限和动态环境的挑战，特别是在塌陷建筑物或工厂内进行搜救任务时。这给集群安全高效规划轨迹带来了重大挑战。

▲图1：无人机飞行场景©️【深蓝AI】编译

首先，可通行空间的受限增加了无人机之间发生冲突的可能性。例如，当穿过狭窄孔洞时，无人机必须依次通过以避免碰撞。此外，当在类似管道或走廊等环境结构中导航时，横向机动能力受到限制，这增加了相互碰撞的风险。

其次，无人机的飞行轨迹需要满足多种标准，包括平滑性、动力学可行性以及无人机动力学特性等。因此，轨迹规划相关的优化约束变得更加复杂，这使得问题更难求解。

最后，环境的动态性以及无人机定位的固有漂移，突出了无人机实时重规划的必要性。这种能力对于实现动态避障和提高无人机的容错能力至关重要。

现有的多智能体路径规划(MAPF)算法，主要针对障碍物较少的场景，可以分为集中式和分布式两种方法。

在集中式方法中，Sharon等人提出了基于冲突的搜索(CBS)算法，而Barer等人则在CBS基础上提出了诸如GCBS、BCBS和ECBS等次优方法，以提高搜索速度。这些算法依赖网格地图来在离散时间间隔内计算多个智能体的安全无碰撞路径。然而，基于网格的方法得到的路径未能考虑系统的动力学、运动学和平滑性约束，因此不适合在实际应用中直接执行。为解决这个问题，作者之前的工作通过在CBS算法的低层搜索中集成3D混合A*算法进行了改进。这种方法利用无人机运动学和动力学约束，促进了直接可执行的多重轨迹的生成。然而，这种方法在搜索时表现出效率低下，特别是在受约束的环境中。此外，在可扩展性和动态避障性能方面还需要改进。

对于分布式方法，相关文献提出了基于速度障碍的轻量级方法，这些方法在速度空间中寻找满足轨迹约束的可行解。然而，这种方法中的智能体使用统一的避障策略，每个智能体独立做出规划决策而没有中央协调。这一特征使得该方法容易在受限空间内的大规模群体规划任务中陷入死锁。Zhou等人提出了一种完全自主的分布式EGO-Swarm算法，采用优先级方法来解决无人机之间的冲突，并通过求解具有高可扩展性的非线性优化问题来设计轨迹。然而，由于在优化过程中使用软约束，这种方法不能保证智能体的安全。此外，在狭窄管道等环境中，仅依靠个体本地化信息进行规划可能导致无人机飞行策略频繁改变，如紧急避障和寻找可通行的交叉口。这可能产生曲率过大的飞行轨迹，从而对从事视觉相关任务的无人机产生不利影响。

此外，为实现动态避障，已有文献通过将动态障碍物的预测轨迹投影到静态障碍物地图上构建时空概率图。随后基于这些时空概率图进行轨迹规划。然而，动态障碍物的运动本质上具有不确定性。为解决这个问题，相关研究将随机避碰建模为机会约束优化问题，提出了基于有界体积扩张的避碰方法。然而，这些方法要么过于保守，要么由于概率建模中的不确定性而增加算法的复杂性。

2. 系统概述

本文做出如下假设：地图是先验信息，其中 $\mathcal{O}$ 表示静态障碍物占据的空间， $\mathcal{F}$ 表示自由空间。考虑有 $\mathcal{N_u}$ 个无人机和 $\mathcal{N_{dyn}}$ 个动态障碍物，每个无人机都能获得其他无人机和动态障碍物的实时位置信息。本文的目标是在自由空间中为无人机集群找到满足轨迹约束的无碰撞飞行轨迹。

本文结合了集中式和分布式路径规划框架的优势。如图2所示，整个系统框架主要包含两个模块：全局规划和局部规划。

▲图2：系统框架©️【深蓝AI】编译

全局规划使用基于网格的地图作为先验知识，系统在简化建模且不考虑动态障碍物的情况下，通过ECBS算法为每个无人机搜索从起点到终点的无碰撞路径。这条路径作为初步的全局参考路径，用于指导后续的局部规划。这使得无人机能够避免陷入死锁或被困在死胡同中。

局部规划系统进行精确的实时重规划，以准确避开静态和动态障碍物，确保无人机轨迹无碰撞。局部规划过程包括以下几个步骤：

(1) 基于全局参考轨迹和当前无人机位置，为未来一段时间生成局部初始路径。这个初始路径由多个离散的三维位置点组成。

(2) 基于障碍物分布和初始路径，为每个无人机生成安全走廊。

(3) 基于无人机之间安全走廊的冲突趋势，将集群迭代分类为多个组。

(4) 当从预测模块获得未来动态障碍物轨迹预测结果后，通过构建包含约束的凸优化问题，对同一组内的无人机进行协同实时重规划。

通过这种分层框架，系统能够在保证安全性的同时，实现高效的路径规划。全局规划提供整体导航指导，而局部规划则确保实时的避障能力和轨迹可行性。这种结构充分利用了集中式方法的全局优化能力和分布式方法的实时性与灵活性。

3. 解决方案

3.1 全局冲突解决

受到集中式规划方法在无死锁方面优势的启发，该框架首先执行一个粗略的全局冲突解决过程来获得全局参考路径。在作者之前的工作中，尝试将无人机的运动学模型集成到ECBS算法的底层搜索中。实验表明，这种方法能生成更平滑的轨迹，但代价是增加了搜索时间。在本文中，由于全局参考路径仅作为方向性参考，因此将使用不包含运动学模型的ECBS搜索算法，并进一步简化其物理和碰撞模型以缩短搜索时间。

参考路径在网格地图上的搜索包括两个层次：高层搜索和低层搜索。在高层搜索中，检查无人机之间路径的冲突，并对发生冲突的无人机施加特定的冲突约束。在低层搜索中，在遵守冲突约束的情况下为每个无人机生成新的路径。

算法在高层搜索和低层搜索之间迭代循环，直到生成的多条路径无冲突为止。作者的简化策略包括：（1）在高层搜索中减少无人机间的碰撞检测半径；（2）低层搜索中将邻居节点从7个扩展到15个，允许无人机对角线运动。

通过上述算法，获得多条离散路径后，使用最小二乘法对这些路径进行多项式拟合，得到每个无人机的连续全局参考路径。本文使用ceres库来解决最小二乘问题，将第i个无人机的参考路径表示为 $\mathcal{R_{i}}(t)$ 。

3.2 基于冲突趋势的局部聚类

局部重规划在全局粗略参考路径的启发下进行，以确保生成安全和平滑的可执行轨迹。重规划包括局部初始路径生成、安全走廊创建和冲突趋势聚类等模块。

1. 局部初始路径生成

当在时间 $T_{replan}$ 进行重规划时，第 $i$ 个无人机的初始路径定义为 $T_{replan}$ ，包含多个离散点：

$\Gamma_i=\left\{\mathcal{P}_j^i\in\mathbb{R}^3\mid j=1,2,\cdots,\mathcal{N}_{initPath}\right\}$

其中 $\mathcal{N}_{initPath}$ 是离散点的数量。对于 $\Gamma_{i}$ 中的第一个点 $\mathcal{P}_1^i$ ，它代表这次重规划过程的起点，对应无人机在时间 $T_{replan}$ 的位置。生成算法如下：

2.局部安全走廊生

成为防止与静态障碍物发生碰撞，无人机轨迹将被限制在安全走廊内。局部安全走廊被构建为轴对齐包围盒(AABB)，这便于在后续无人机聚类模块中进行简化的冲突检测。每个安全走廊包含六个元素，即全局地图坐标系中矩形安全走廊一对顶点的三维坐标。

▲图3：2D和3D下的安全走廊©️【深蓝AI】编译

3. 安全走廊聚类

在无人机的安全走廊相交的情况下，可能发生碰撞。我们的方法通过聚类无人机来解决这个问题，将那些有碰撞风险的无人机归类进行协同优化，从而确保安全。

聚类算法的流程如下：

▲图4：安全走廊聚类©️【深蓝AI】编译

初始时，我们确定安全走廊之间是否存在任何碰撞。碰撞检测使用AABB方法进行。如图3所示，只有当两个安全走廊在x、y和z坐标轴上的投影线都相交时，才认为它们发生相交。具有相交安全走廊的无人机被整合到一个组中。

然而，在特定情况下，高密度的无人机可能加剧安全走廊之间的冲突，导致一个组中包含大量无人机。这种现象会增加协同优化任务的复杂性。因此，下一步是对包含超过上限数量无人机的组进行重新聚类。重新聚类采用K-means++算法。

3.3 局部协同轨迹规划

在基于冲突趋势进行聚类后，将得到多个组。随后，在这些组内进行实时协同轨迹规划，以获得安全、平滑且动力学可行的集群轨迹。

1. 轨迹描述

贝塞尔曲线将被限制在由一组控制点形成的凸包内，这便于通过将控制点限制在安全走廊内来实现避障。在每次重规划中，第i个无人机的局部轨迹将使用分段贝塞尔曲线描述。此外，贝塞尔曲线的k阶导数仍然满足贝塞尔曲线的性质，当k=1或k=2时，分别可以得到轨迹的速度或加速度曲线。

2. 静态障碍物避障约束

考虑到无人机在垂直方向的下洗气流效应，将无人机的碰撞模型定义为椭球区域，这也是无人机和障碍物之间的安全区域。在这个过程中，作者利用曲线的凸包性质，将约束进一步进行转换。

3. 交互避障约束

将根据之前建立无人机之间避碰的相对安全走廊，为确保无人机之间不发生碰撞，两个无人机直接最短距离应大于安全间距。

4. 动态障碍物避障约束

为实现动态障碍物的避障，将采用匀速模型来预测动态障碍物的未来路径。随后，使用最小二乘法将这些路径拟合为贝塞尔曲线。然后基于无人机与动态障碍物之间的相对安全走廊构建避碰约束，其中动态障碍物建模为半径为的球体。

5. 动力学限制

考虑到无人机的动力学限制，将无人机在各轴的速度和加速度必须限制在最大速度和加速度的边界内。

6. 目标函数

在局部优化中，目标函数被表述为两个部分的加权和：轨迹平滑度和局部起终点状态约束。此外，决策变量是贝塞尔曲线的控制点。通过优化上述目标函数，可以保证轨迹的连续性。

7. 轨迹优化

上述建立的优化约束和目标函数具有凸特性，允许将其表述为二次规划问题。利用既有的凸优化求解器（如CPLEX）可以高效地获得优化解，得到满足约束同时最小化成本的轨迹。

这种优化方法不仅确保了轨迹的安全性和可行性，还保证了轨迹的平滑性和动力学特性，使其适合实际的无人机飞行任务。

4. 实验

实验验证在配备Intel 8336C@ 2.3GHz CPU的环境下进行仿真对比。系统采用Octomap库来表示3D占用地图，使用CPLEX QP求解器进行轨迹优化。贝塞尔曲线的阶数设定为n = 5。

4.1 空旷环境试验

三种算法在空旷环境中使用8架无人机进行圆形路径位置交换测试。下表展示了离线规划时间toff(s)、在线规划时间ton(s)、总飞行距离fdis(m)、平均速度Vel(m/s)和平均加速度Acc(m/s²)的统计数据。

实验结果表明：本文方法在集中式搜索过程中采用简化的碰撞模型，相比集中式方法有效减少了搜索时间。同时，集中式方法飞行距离较长，这是因为在集中式全局优化过程中，为满足全局轨迹的平滑性和连续性要求，无人机需要执行大量转弯或绕行。此外，在全局参考轨迹启发下的局部规划使我们能够实现更高的平均速度，但这也导致与其他两种方法相比略高的平均加速度。

4.2 复杂约束环境试验

为评估算法在狭窄、多约束环境中的性能，在包含狭窄管道的不同尺寸地图上部署了40架无人机。无人机需要完成两种任务：

● 简单任务：与地图对称另一侧的无人机进行位置交换，无人机间冲突概率较低

● 复杂任务：需要对角线飞行到地图的对面，这将产生更多无人机间的冲突

在这种场景下，安全走廊的最大范围定义为6r×6r×6r，其中r表示无人机半径，系数6是一个经验值。这个阈值下形成的安全走廊空间可以容纳4~6架无人机，确保它们有足够的交互空间。此外，最大离线规划时间设置为1500秒。实验结果如图5和表2所示。

实验结果表明，在搜索时间方面，通过简化碰撞模型和约束，作者的方法在离线阶段相比[9]中集中式方法有更快的搜索时间。在在线优化中，Ego-Swarm显示出更快的优化时间，这是因为它使用非线性优化并将平滑、连续性和避障构造为软约束，从而提供更快的优化速度。相比之下，作者的方法采用带硬约束的凸优化，虽然较慢但确保所有约束都得到满足。与非线性优化相比，避免了局部最小值问题且不需要初始解。

在成功率方面，[9]中的集中式方法在约束丰富的多约束环境中，全局轨迹的集中式优化成功率为0%，相比之下，本文方法在局部优化时只优化未来的局部轨迹。这种方法使用较小的变量和较低的自由度，便于优化。此外，分布式方法Ego-Swarm的成功率在65%到80%之间。这是因为UAV完全依赖局部环境信息，在某些情况下无法有效定位长狭窄管道中的入口和出口，此外，Ego-Swarm在优化中将避障约束构造为软约束，这不能保证UAV的绝对安全。

在飞行时间和速度方面，与EGO-Swarm相比，本文方法通过跟随全局路径实现了更快的飞行速度和更短的飞行时间。EGO-Swarm由于是分布式的，在飞行过程中经常会出现不必要的绕行，原因是需要避开其他UAV或飞出走廊内部，这会增加飞行距离并降低速度。

4.3 动态场景测试

与集中式方法相比，本文方法能够适应动态环境。在这一节中，仅对提出的框架在动态环境中进行实验验证，原因是目前最新的多智能体动态避障算法没有开源，而开源算法MADER[10]需要提供准确的动态障碍物轨迹，这在现实中是不可行的。

动态障碍物避障测试在30m×30m×3m的狭窄走廊动态场景中进行。作者在环境中心位置了8个半径为0.3m的球形动态障碍物，这些障碍物的速度分别设置为0.5m/s、1m/s和2m/s。UAV的运动模式设置为围绕固定线段或圆形进行往返运动，分别使用8架和16架UAV进行测试，目标是从地图一端对角线穿越到另一端。

试验表明，算法的离线规划时间在有无动态障碍物的场景中差异很小，这是因为离线规划阶段并不考虑动态障碍物的影响。随着动态障碍物速度接近UAV的速度上限，成功率会降低。相比无动态障碍物的场景，避开动态障碍物的必要性导致UAV的行程距离和平均加速度略有增加。

4.4 消融试验

安全走廊的扩展长度和聚类模块是影响在线规划时间的两个主要因素。在这一小节中，对聚类模块进行了消融实验，使用三种不同的安全走廊扩展长度阈值：短、中、长，来评估它们对规划时间的影响。

实验结果表明，在没有聚类模块的情况下，24架UAV在固定起终点的情况下发生10次碰撞，在随机起终点的情况下发生11次碰撞，这是由于规划时间过长导致重规划频率不足，当检测到动态障碍物时，系统无法及时重新规划，最终导致碰撞。有聚类模块的情况下，得益于及时的重规划，没有发生任何碰撞，这证明了聚类模块在动态环境中的有效性。