自主无人机集群在真实的未知复杂环境中有着广泛的应用前景，例如野外侦查、山林救灾等。尽管目前已诞生一些优秀的集群方案，然而在未知混动态环境中，挑战依旧严峻。在此类环境中，除了山体、树木等静态障碍之外，还同时存在着能够相互通信的集群无人机和无法通信的未知无人机，环境的复杂性和不确定性显著增加。因此，未知混动态环境对无人机轨迹规划提出了更严苛的要求，主要集中在动态避碰和高效规划2个方面。首先，无人机之间的动态避碰关系可以依据有无通信划分为合作避碰与非合作避碰2类。在未知混动态环境中，集群无人机应同时考虑这2类动态避碰，以确保无人机安全。然而，现有的无人机集群方案往往侧重于其中之一。其次，鉴于未知无人机的运动轨迹无法准确预测，集群无人机必须在检测到碰撞风险后尽可能快地实时重规划安全可行的轨迹，以快速机动避免威胁。

现有的动态避碰方案可以分为集中式^[1-3]和分布式^[4-6]2类。由于分布式方案具有更高的灵活性和鲁棒性^[7]，现已有许多研究采用该方案实现了空旷环境下的动态避碰。其中，基于速度信息的一阶避碰算法通过在速度空间上规划避碰速度实现了移动物体间的动态避碰，如速度障碍法(velocity obstacles, VO)^[8]和最优相互避碰法(optimal reciprocal collision avoidance, ORCA)^[9]等。将一阶避碰算法与模型预测控制相结合^[10]，生成的轨迹具有更好的连续性。但是，这些方法不适用于处理非结构化的静态障碍。近年来，为实现未知混动态环境下的集群飞行，一些基于优化的方案通过机间通信实现了动态避碰^[11-13]。文献[14]结合高效冲突搜索算法设计了一种多机实时规划方法，并对一定范围内的无人机轨迹进行联合优化。文献[15]则在ORCA算法的基础上考虑了非完整约束模型和多机死锁问题。文献[16]则提出了一种非线性时变粒子群算法和一种自适应差分进化技术以实现在线规划，并通过分布式方法实现了多机协同。然而，上述方案主要利用完美通信进行合作避碰，非合作避碰问题没能得到有效的解决，这妨碍了无人机集群在实际未知环境中的运用。

轨迹规划的效率至关重要，它决定了无人机能否及时机动避开障碍。传统局部规划方案将轨迹生成问题分为2部分，即路径搜索和轨迹优化^[17-18]。分别提高2部分的运算效率以减少规划用时已成为现有研究的热点。在路径搜索算法中，动力学搜索方法是一种通过离散控制量并生成局部运动轨迹即运动本原来搜索安全路径的方法。相比于几何搜索方法，该方法能够保证路径的动态可行性，因而备受关注^[19-20]。许多文献针对其效率问题进行了一系列研究，典型工作包括定义等价运动本原^[21]或添加视场约束^[22]等以进行剪枝；自适应调整运动本原的持续时间^[23]或同时生成2种不同持续时间的运动本原^[24]；设计良好的启发式函数^[25-26]等。然而大量的计算浪费在了均匀离散策略生成的无效样本上。在轨迹优化阶段，基于梯度的优化方法是局部轨迹规划的主流，但提供梯度信息的欧氏距离场(Euclidean signed distance field, ESDF)耗费了规划70% 的时间^[27]。为此，各种增量式构建ESDF的方法被提出^[28-29]。尽管它们是有效的，但对于在线规划系统来说仍不够理想。文献[30]提出了一种人工梯度法，利用实时搜索到的无碰撞路径来人工构建梯度，直接避免了ESDF的构建。但其依赖的无碰撞路径没能考虑到同伦性约束^[31]，即其无法保证最终轨迹的最优性。

综上所述，在集群避碰方面，现有优异的自主集群方案主要聚焦于合作避碰，但在实际场景中，考虑到通讯故障或未知无人机的出现，无人机必须同时应对合作避碰与非合作避碰的双重挑战；而在轨迹规划方面，动力学路径搜索方法采用的均匀离散策略难以兼顾求解效率与轨迹性能，同时轨迹优化依赖的欧氏距离场耗费大量计算资源。因此，设计一种能够保证安全且快速完成规划的动力学方案具有重要意义。

故本文提出了一种分布式的动力学规划方案，由改进的动力学路径搜索和基于拓扑引导梯度的轨迹优化2部分组成，旨在为野外未知混动态环境中的无人机快速重规划安全可行的轨迹。主要工作有：1)通过将ORCA与动力学路径搜索相结合，提出一种改进的动力学路径搜索方法，用于寻找一条安全可行的参考路径。该方法一方面利用由改进的ORCA得到的最优避碰控制量引导控制量非均匀离散，提高搜索效率，另一方面依据速度避碰的思想检测运动本原，实现动力学路径搜索的动态避碰能力。2)为提高轨迹优化效率，在人工梯度法的基础上^[30]提出一种适配动力学规划方案的静态障碍梯度构建方法，充分利用参考路径快速构建梯度，使得优化器能够在几毫秒以内生成一条在参考路径同伦类内的最优轨迹。符号说明：若无特殊说明，下文中所述的向量皆为列向量，所述的点既指点也指点的坐标。

1.   预备知识

1.1   最优相互避碰算法

令$A$是无人机，$B$是其附近的动态障碍，$\{ {{\boldsymbol{p}}_A},\;{{\boldsymbol{v}}_A},\;{r_A}\}$和$\{ {{\boldsymbol{p}}_B},\;{{\boldsymbol{v}}_B},\;{r_B}\}$分别表示它们的位置、速度和半径，定义$A$关于$B$的速度障碍${\boldsymbol{O}}(\tau )_{A|B}^{{\text{VO}}}$为将导致$A$在$\tau$时间内与$B$碰撞的相对速度的集合，如 图1所示。令$D({\boldsymbol{p}},r)$表示以${\boldsymbol{p}}$为中心以$r$为半径的开区域，速度障碍${\boldsymbol{O}}(\tau )_{A|B}^{{\text{VO}}}$可以表示为

图  1  动态避碰示意

Fig.  1  Dynamic collision avoidance

下载: 全尺寸图片

式中${{\boldsymbol{V}}_{AB}} = {{\boldsymbol{V}}_A} - {{\boldsymbol{V}}_B}$表示$A$关于$B$的相对速度。若实际相对速度${{\boldsymbol{v}}_{AB}} = {{\boldsymbol{v}}_A} - {{\boldsymbol{v}}_B}$在速度障碍内，假设速度能够瞬间改变，可以改变$A$的速度使得新的相对速度在速度障碍以外来避开动态障碍。最优相互避碰集即是满足条件的$A$的可选速度的集合，其是指能够保证$A$在$\tau$时间内不与$B$碰撞并且尽可能包含更大可行速度区的避碰速度集。令${{\boldsymbol{v}}_{AB\Delta }}$为实际相对速度${{\boldsymbol{v}}_{AB}}$到${\boldsymbol{O}}(\tau )_{A|B}^{{\text{VO}}}$边界上最近一点的向量，令${{\boldsymbol{n}}_{AB\Delta }}$为该向量的方向向量，则$A$关于$B$的最优相互避碰集${\boldsymbol{O}}(\tau )_{A|B}^{{\text{ORCA}}}$为

式中${{\boldsymbol{V}}_A}$表示$A$的可选新速度，${\boldsymbol{V'}}_{AB}^{}$为${\boldsymbol{O}}(\tau )_{A|B}^{{\text{VO}}}$边界上使得$A$不与$B$碰撞的相对速度。

当空间中存在多个动态障碍，$A$的群体最优相互避碰集${\boldsymbol{O}}(\tau )_A^{{\text{ORCA}}}$为$A$关于所有动态障碍的最优相互避碰集的交集：

在${\boldsymbol{O}}(\tau )_A^{{\text{ORCA}}}$中选择一个最接近期望速度${\boldsymbol{v}}_A^{{\text{des}}}$(通常为指向目标的速度)并满足速度约束的新速度，即为$A$最优避碰速度${\boldsymbol{v}}_A^{{\text{opt}}}$，即求解如下的凸优化问题：

1.2   问题描述

本文采用分布式系统架构，将集群规划问题解耦为单机的轨迹规划问题。单个无人机建模为二阶线性动力学系统：

式中：${\boldsymbol{x}}(t){\text{ = [}}\;{\boldsymbol{p}}{(t)^{\text{T}}},{\boldsymbol{v}}{(t)^{\text{T}}}{{\text{]}}^{\text{T}}} \in {\mathbb{R}^6}$表示动力学系统状态，${\boldsymbol{p}}(t)$、${\boldsymbol{v}}(t)$为无人机的位置和速度，${\boldsymbol{u}}(t) = {\boldsymbol{a}}(t) \in {\mathbb{R}^3}$表示系统控制输入，${\boldsymbol{a}}(t)$为无人机的加速度。

由于四旋翼无人机具有微分平坦特性，其轨迹可以用分段多项式${\varPhi }(t)$来表示^[32]：

式中：$M$和$N$分别为分段多项式轨迹的阶数与段数，${t_0}$和${t_N}$分别为该轨迹的起始与结束时刻，${{\varPhi }_n}(t - {t_{n - 1}})$为该轨迹中的第$n(n = 1,2, \cdots ,N)$段，${a_{nm}}$为该段中各项系数。为使生成的轨迹是最优的，通常令轨迹的控制代价与时间代价总和最小。其中控制代价定义为控制输入${\boldsymbol{u}}(t)$的${\text{L}}2$范数的平方，即

由此，最优轨迹生成一般可以描述为线性二次能量−时间最小化问题^[33]。

线性二次能量−时间最小化问题　给定起始状态${{\boldsymbol{x}}_0} \in {{\boldsymbol{\chi }}_{{\text{free}}}}$、目标状态${{\boldsymbol{x}}_{\text{d}}} \in {{\boldsymbol{\chi }}_{{\text{free}}}}$和起始时刻${t_0}$，设计多项式轨迹${\varPhi }(t)$并确定轨迹持续时长T，使得轨迹的控制与时间的综合代价最小，即求解具有状态约束和输入约束的优化问题：

式中：$\omega$是时间代价权重，用以权衡控制代价与时间代价的相对重要程度；${{\boldsymbol{\chi }}_{{\text{free}}}} = {{\boldsymbol{P}}_{{\text{free}}}} \times {{\boldsymbol{V}}_{{\text{free}}}}$表示与障碍无碰撞且符合动力学约束的自由区域；${\boldsymbol{U}} \subset {\mathbb{R}^3}$表示线性系统的控制约束。

近年来，基于梯度的优化方法是求解上述问题的主流。通过将问题转化为无约束优化问题，该方法既提高了计算效率又增强了数值稳定性^[34]。然而，由于传统梯度优化方法成功率较低^[35]，经典规划方案通常包含路径搜索和轨迹优化2个步骤。遵循这一思路，如图2所示，本文轨迹规划的具体方案为：1)动力学路径搜索，将轨迹规划问题转化为图搜索问题，快速获取参考路径。该路径不能直接用于飞行，但其在最优路径的同伦类内，可用于指导轨迹优化。2)轨迹优化，以动力学路径搜索获取的参考路径为基础，拟合并优化出具有更高性能的最优轨迹。

图  2  轨迹规划方案

Fig.  2  Trajectory planning scheme

下载: 全尺寸图片

2.   动力学路径搜索

本节提出了一种能够同时处理静态障碍与非合作动态障碍的动力学路径搜索方法，利用非均匀离散得到的控制量生成运动本原，可快速构建出连通图并搜索出一条安全可行的参考路径，以用于后续轨迹优化。该方法首先改进ORCA以计算最优避碰速度，并进而获取动态避碰最优控制量，以此引出最优控制非均匀离散；接着通过限制运动本原的平均速度在最优相互避碰集内，保证连通图上路径的安全性。

2.1   动态避碰最优控制量

动态避碰最优控制量为控制量离散提供了关键指引，本节将详细阐述其获取方法，整体流程如图3所示。

图  3  最优控制量获取流程

Fig.  3  Process of obtaining optimal control quantity

下载: 全尺寸图片

ORCA法能够根据障碍信息实时求解速度空间中的凸优化问题，获取动态避碰最优速度，以实现多个移动物体之间的动态避碰。然而，受控制约束影响，当前速度可能无法在一步时间内达到该最优速度。因此，有必要在计算最优速度前考虑加速度约束。

假设${{\boldsymbol{v}}_A}$为无人机当前速度，${a_{\max }}$和${v_{\max }}$为最大加速度与最大速度，$\delta$为步长。如图4所示，在速度空间中，定义允许速度区域(浅灰色)是以原点${{\boldsymbol{V}}_o}$为中心，半径为${v_{\max }}$的球域${{\mathcal{O}}_v}$，定义可达速度区域(深灰色)是以${{\boldsymbol{v}}_A}$为圆心，半径为$\delta {a_{\max }}$的球域${{\mathcal{O}}_a}$。它们的数学描述为

图  4  允许速度区与可达速度区

Fig.  4  Allowable speed zone and reachable speed zone

下载: 全尺寸图片

2个球域的交集即为无人机$A$在一步时间$\delta$内能够达到的速度域${{\mathcal{O}}_A}$：

此外，在构建凸优化的目标函数时，本节考虑了2部分内容：一是惩罚速度的变化，二是惩罚最优速度与期望速度的偏差。则二次代价函数为

式中${\lambda _1}$和${\lambda _2}$为2项的系数。由此，求解动态避碰最优速度的凸优化问题可以重新描述为

对于本文所采用的二阶系统，系统的控制输入为加速度，因此需要将最优速度转化为最优控制量。得益于在描述速度约束${{\boldsymbol{V}}_A} \in {{\boldsymbol{O}}_A}$时考虑了加速度限制，最优控制量与最优速度间的映射关系为

2.2   最优控制非均匀离散

动力学路径搜索通过离散化控制量以生成多条运动本原来探索空间，最终获得到达目标点的参考路径。其常用的离散化策略为均匀离散策略，即在控制空间沿着每个轴${U_s} = {[ - {u_{\max }},\;{u_{\max }}]_s}$以${u_\Delta }$为间隔均匀采样^[36]，离散后的控制集${{\boldsymbol{U}}_C}$如图5(a)所示。给定无人机的当前状态${{\boldsymbol{s}}_n} = {[{\boldsymbol{p}}_n^{\text{T}},\;{\boldsymbol{v}}_n^{\text{T}}]^{\text{T}}}$，${{\boldsymbol{U}}_C}$中每个控制量${{\boldsymbol{u}}_c}$将为系统生成一个时长为$\delta$的运动本原，如图5(b)所示。

图  5  均匀离散策略

Fig.  5  Uniform discrete strategy

下载: 全尺寸图片

对控制量的均匀离散会产生许多无效样本，故本节提出一种最优控制非均匀离散策略，利用前节所述的非合作避碰最优控制量来指导控制量非均匀离散。具体来说，以当前状态${{\boldsymbol{s}}_n}$的最优控制量坐标为中心，以${u_\Delta }$为最小采样间隔，在控制空间中沿着每一轴由密到疏非均匀地采样，离散后的控制集${{\boldsymbol{U}}_{nC}} = \left\{ {{{\boldsymbol{u}}_{n1}},\;{{\boldsymbol{u}}_{n2}},\; \cdots ,\;{{\boldsymbol{u}}_{nC}}} \right\} \subset {\boldsymbol{U}}$如图6(a)所示，相应的运动本原如图6(b)所示。这种离散策略将增加获取有效样本的概率，提高计算效率。

图  6  最优控制非均匀离散策略

Fig.  6  Non-uniform discrete strategy with optimal control variable

下载: 全尺寸图片

不同状态下生成的运动本原将状态空间离散并构建了一个连通图，上述连续空间中的线性二次能量−时间最小化问题可以转化为连通图上的最短路径搜索问题^[24]以快速求解。

最短路径搜索问题　给定一个由运动本原构成的连通图，在图上寻找一条由$N$个运动本原构成的路径，并要求该路径的控制与时间代价最小，即求解问题：

式中：${{\boldsymbol{s}}_n}$、${{\boldsymbol{u}}_n}$和${{\boldsymbol{v}}_n}$分别为所选择路径上第$n + 1$个运动本原的起始状态、控制输入和平均速度，${\boldsymbol{O}}(\tau )_{A,n}^{{\text{ORCA}}}$为无人机$A$在${{\boldsymbol{s}}_n}$状态下的群体最优相互避碰集。

以上问题可使用算法1来有效解决。从初始节点开始，采用上述方法获取控制集以作为系统控制输入。接着，通过前向积分生成运动本原和新节点。随后，再对新节点重复上述操作，直至某一节点到达目标点。至此，即可得到一条动态可行且安全的参考路径。

算法1　动力学路径搜索

输入　${{\boldsymbol{s}}_0}$。

输出　${{\boldsymbol{s}}_n}$。

1) ${\boldsymbol{R}} \leftarrow {{\boldsymbol{s}}_0},{\boldsymbol{C}} \leftarrow \varnothing$

2) while ${\boldsymbol{R}} \ne \varnothing$ do

3) ${{\boldsymbol{s}}_n} \leftarrow {\boldsymbol{R}}.{\text{pop()}},{\boldsymbol{C}}.{\text{push(}}{{\boldsymbol{s}}_n}{\text{);}}$

4) if ${{\boldsymbol{s}}_n} = {{\boldsymbol{x}}_{\text{d}}}$ then

5) return ${{\boldsymbol{s}}_n}$;

6) ${\boldsymbol{a}}_n^{{\text{opt}}} \leftarrow$EnhancedORCA(${{\boldsymbol{s}}_n}$)

7) ${{\boldsymbol{U}}_{nC}} \leftarrow$Nonunidiscertization(${\boldsymbol{a}}_n^{{\text{opt}}}$)

8) for all ${{\boldsymbol{u}}_{nc}} \in {{\boldsymbol{U}}_{nC}}$do

9) ${{\boldsymbol{x}}_{nc}}(t) \leftarrow {\boldsymbol{F}}(t){{\boldsymbol{s}}_n} + {\boldsymbol{G}}(t){{\boldsymbol{u}}_{nc}}$;

10) if ${{\boldsymbol{x}}_{nc}}(t) \subset {{\boldsymbol{\chi }}^{{\text{free}}}}$then

11) if ${{\boldsymbol{v}}_{nc}} \in {\boldsymbol{O}}(\tau )_{A,n}^{{\text{ORCA}}}$then

12) ${{\boldsymbol{s}}_i} \leftarrow {{\boldsymbol{x}}_{nc}}(\delta )$;

13) ${\boldsymbol{P}}({\boldsymbol{s}}) \leftarrow {\boldsymbol{P}}({\boldsymbol{s}}) \cup \{ {{\boldsymbol{s}}_i}\}$;

14) for all ${{\boldsymbol{s}}_j}$in${\boldsymbol{P}}({\boldsymbol{s}})$do

15) ${\boldsymbol{R}}.{\text{add}}({{\boldsymbol{s}}_j})$

以下详细说明算法1中伪代码的含义。

1) 初始化(第1～3行)：构造集合并将初始节点加入，之后循环从集合中取出代价最小的节点。

2) 到达检测(第4～5行)：检查取出的节点是否到达目标点，若到达即可通过回溯获取路径。

3) 离散化(第6～7行)：采用本文的最优控制非均匀离散策略获得离散控制量。

4) 扩展新节点(第8～12行)：利用控制量前向积分生成运动本原，并进行安全检查。接着，在安全的运动本原末端建立新节点。

5) 添加新节点(第13～15行)：对多个新节点进行剪枝操作后加入到集合中。

3.   轨迹优化

本节利用动力学路径搜索获得的参考路径拟合出初始轨迹，并为其构建梯度信息，接着通过无约束优化平滑轨迹并调整轨迹与障碍的间距以增强鲁棒性，获取一条高性能的最优轨迹。首先提出一种拓扑引导梯度法，用于快速计算轨迹与静态障碍的梯度，为轨迹远离障碍提供方向引导。然后根据避障约束和其他约束构建出无约束优化问题，并给出其他各项惩罚函数，最后通过梯度下降法进行求解。

3.1   拓扑引导梯度法

轨迹参数化方法多种多样，常用的有贝塞尔曲线和${\text{B}}$样条曲线等，不同的方法具有不同的优势。本节使用了一种新的时空联合轨迹参数化方法 (minimum control, MINCO)^[37]，能够有效调整轨迹的时间与空间分布。根据路径搜索获取的参考路径，利用${\text{MINCO}}$在三维空间中拟合出一条$M$阶$N$段的初始轨迹${\varPhi }(t)$，第$n$段轨迹多项式定义为

为方便描述，用${t_r}$代替$t - {t_{n - 1}}$表示全局时间$t$在某段轨迹上的相对时间，${{\boldsymbol{c}}_n} \in {\mathbb{R}^{(M + 1) \times 3}}$为第$n$段轨迹的系数，${\boldsymbol{\beta }}({t_r}) =$${[1,\;{t_r},\; \cdots \;,\;t_r^M]^{\text{T}}}$为自然基。

在每一段拟合轨迹上均匀取$K$个约束点，检查这些约束点是否在静态障碍附近。与文献[30]不同的是，对于处在静态障碍内(或附近)的第$n$段上第$k(k \in [1,K])$个约束点${{\boldsymbol{p}}_{nk}}$，直接在参考路径上检索出与其最近的点并从${{\boldsymbol{p}}_{nk}}$向该点引一条射线，记录射线在障碍表面的交点和方向向量，称为引导点${{\boldsymbol{p}}_{nk,w}}$和引导向量${{\boldsymbol{v}}_{nk,w}}$。如图7所示，其中$w$表示轨迹优化迭代过程中该约束点附近的不同静态障碍的序号。

图  7  拓扑引导梯度

Fig.  7  Topology guided gradient

下载: 全尺寸图片

由此，约束点${{\boldsymbol{p}}_{nk}}$与第$w$个障碍的障碍距离${d_{nk,w}}$可以表示为

给定${L_{\mathrm{o}}}$为静态避障安全距离，为使得约束点在安全距离以外，设计约束点关于第$w$个静态障碍的避障约束为

当障碍距离小于安全距离时，构建避障梯度

在优化迭代时，该梯度信息将引导轨迹朝着障碍距离增大的方向移动。通过参考路径引导约束点远离静态障碍，保证优化后的轨迹在参考路径的同伦类内，进而保证轨迹最优性不受破坏。

3.2   无约束优化

将上述避障约束与其他约束转化为约束惩罚添加到目标函数中，以将优化问题转化为无约束优化问题，并利用梯度信息调整轨迹与各种障碍的距离以及平滑性等。

无约束优化问题 令${T_n} = {t_n} - {t_{n - 1}}$为多项式轨迹第$n$段的时长，通过设计多项式系数${\boldsymbol{C}} = [{\boldsymbol{c}}_1^{\text{T}},\;{\boldsymbol{c}}_2^{\text{T}},\; \cdots , \;{\boldsymbol{c}}_N^{\text{T}}]^{\text{T}} \in {\mathbb{R}^{N(M + 1) \times 3}}$和时长$\boldsymbol{T}=[T_1,\; T_2,\; \cdots,\; T_N]^{\text{T}}$，使得

式中：$\lambda$为各项权重，权衡不同项之间的相对重要程度；${J_e}$表示常用的目标函数，下标$e = \{ {\text{u}},\;{\text{t}},\;{\text{d}}\}$表示控制$({\text{u}})$、时间$({\text{t}})$以及动态可行性$({\text{d}})$，具体描述可参见文献[38]，此处不再赘述；${J_x}$表示惩罚函数，下标$x = \{ {\text{o,}}\;{\text{s,}}\;{\text{w}}\}$表示静态避障、合作避碰以及非合作避碰，这几种惩罚函数可以统一由约束点处惩罚的加权和形式描述：

式中：${j_{xnk}}$为约束点${{\boldsymbol{p}}_{nk}}$处的惩罚；${\rho _k}$是系数，当$k$为1和$K$时取$1/2$，否则为1。

约束点${{\boldsymbol{p}}_{nk}}$处各项惩罚与梯度的具体描述如下。

1)静态避障惩罚：为了使得轨迹远离障碍物，所设计的惩罚函数需要满足在轨迹靠近障碍物时惩罚迅速增大，在远离障碍物的地方惩罚趋于0。根据上一节中所述的静态避障约束，设计静态避障惩罚${j_{{\text{o}}nk}}$为

静态避障惩罚与障碍距离成负相关，当障碍距离小于安全距离${L_0}$时，惩罚增大，优化器为减小惩罚将使轨迹远离静态障碍。

2)合作避碰惩罚：合作避碰的无人机之间通过异步通信交换轨迹信息，当无人机$h$的轨迹${{\varPhi }_h}(t)$与其他合作无人机在相同全局时间下的轨迹间距大于给定的安全距离${L_{\text{s}}}$时，认为无人机对其他合作障碍是安全的。定义2架合作无人机在约束点${{\boldsymbol{p}}_{nk}}$所对应时刻的避碰约束为

式中$g = 1,\;2,\; \cdots ,\;G$表示与$h$成合作避碰关系的无人机。合作避碰惩罚${j_{{\text{s}}nk}}$同样可以定义为

当机间距离小于安全距离时，合作避碰惩罚将使无人机远离其他合作无人机，合作避碰梯度为

3)非合作避碰惩罚：在优化阶段进一步采用位置避碰的方式来优化无人机与非合作障碍间的距离，增加鲁棒性。与上一节静态避障的方法相同，构造非合作避碰的引导点${{\boldsymbol{p}}_{nk,b}}$和引导向量${{\boldsymbol{\nu }}_{nk,b}}$，由此定义避碰约束与惩罚：

式中$b = 1,2, \cdots ,B$表示与$h$成非合作避碰关系的无人机，${L_{\text{w}}}$为非合作避碰安全距离。当$E_{nk}^{hb} > 0$时，构建非合作避碰梯度为

鉴于在上述优化求解过程中全面考虑了静态障碍、合作障碍以及非合作障碍，本方案赋予了所有无人机独立规划最优轨迹的能力，充分呈现了分布式特性。

4.   实验对比

完整的自主导航系统架构如图8所示。

图  8  系统架构

Fig.  8  System architecture

下载: 全尺寸图片

集群中每架无人机独立运行本文设计的分布式方案，其中合作无人机之间利用通信模块交换轨迹信息。这种分布式方案使得集群系统的耦合性更小。

4.1   动力学路径搜索实验

为评估本文改进动力学搜索方法的高效性，分别采用本文方法与SBMP(search-based motion planning)^[26]及其后续工作^[36]中的方法进行仿真实验。在${\text{20 m}} \times {\text{20 m}}$的区域中随机部署200个非结构化静态障碍模拟野外未知森林场景，本节在其中进行2组对比实验。在实验1中，单架无人机需要穿越森林到达另一侧的目标点；在实验2中，4架不可通信无人机分布在森林四周，它们需要穿越森林并相互避让，最终交换彼此的位置。

无人机的最大速度与加速度设置为2 m/s和4 m/s²。控制量采样间隔是动力学路径搜索算法中的重要参数，同时影响算法的求解效率与轨迹性能。典型工作^[31]通常将采样间隔设置为1～2 m/s²以尽可能兼顾两者。由于本文假定的场景为野外未知复杂场景，因此选取1.0 m/s²和1.5 m/s²2组参数，讨论在不同参数下2种算法的求解效率和轨迹性能表现。在不同参数和场景下各进行10次实验，平均结果汇总如表1和表2。

根据表1和表2中的数据，与SBMP方法相比，当最小采样间隔为$1.5{\text{ m/}}{{\text{s}}^{\text{2}}}$时，搜索用时分别下降了44%和40%；当最小采样间隔为${\text{1 m/}}{{\text{s}}^{\text{2}}}$时，搜索用时分别下降了75%和66%。同时，在生成路径的性能方面，即飞行用时与飞行距离，2种方法之间并没有明显的差距。这表明本文的方法能够在不影响路径最优性的前提下大幅减少搜索用时，即有效提升算法效率。

4.2   梯度构建实验

本文提出的拓扑引导梯度法是在EGO(esdf-free gradient-based local planner)^[30]的人工梯度法基础上建立的，通过保证最终轨迹与参考路径的同伦性来保证轨迹的最优性不被轨迹优化所破坏。为验证所提出方法的有效性，同样在上节所述的森林场景中进行4架无人机飞行实验。实验采用相同的动力学搜索方法提供参考路径，以及相同的参数化方法和优化方法生成最终的轨迹，2种方案仅构建梯度的方法不同。10次实验结果汇总如表3，飞行轨迹见图9。

图  9  基于不同梯度构建方法的轨迹

Fig.  9  Trajectory based on different methods of constructing gradient

下载: 全尺寸图片

动力学路径搜索提供的参考路径是综合考虑了动力学限制与时间代价的结果，而人工梯度法在建立避障梯度时仅从最短避障距离的角度考虑，这将导致采用该梯度信息进行优化得到的最终轨迹可能不在参考路径的同伦类内，即导致轨迹不再是最优的。如图9红框所示，采用人工梯度信息优化后的轨迹发生了突兀的转向，导致轨迹能量代价增加。表3中的数据也说明了这点。由于这种拓扑变化的情况并非普遍存在，且所增加的能量代价在平均计算后显得相对较小，因此2种方法的代价差距在表中并不显著。然而，这并不意味着在避障轨迹规划中，可以忽视拓扑引导梯度法所带来的优势。

4.3   在线规划实验

本节将所提出的方案与目前最新且优异的多机在线规划方案MADER(multiagent and dynamic environments)^[7]、SMFR(swarm of micro flying robots)^[38]和AFDE(autonomous flights in dynamic environments)^[31]进行比较。设计了极具挑战性的实验场景，在${\text{15 m}} \times {\text{15 m}}$的障碍区域中随机部署60个非结构化静态障碍物模拟森林，5架可通信无人机与5架不可通信无人机成圆形围绕在森林外围，它们需要穿越森林并在森林中心相互避碰，最终交换彼此的位置。

由于双目相机精确感知距离通常为4～6 m，因此本文设定轨迹长度为6.5 m。为避免偶然性，每种算法进行20次实验，每次实验的森林都是随机生成。无人机的最大速度与加速度设置为2 m/s和4 m/s²。相关数据取平均值，实验结果汇总见表4。

MADER方案通过为无人机添加关于所有动态障碍的平面约束实现避碰。然而复杂的约束致使算法求解困难，因此求解用时最长。同时，为满足实时性要求，该算法每次仅规划较短的轨迹，从而导致了最高的规划次数。SMFR方案直接采用连接起点与终点的直线作为初始解，通过梯度优化来生成最终轨迹。其使用的${\text{MINCO}}$参数化方法能同时在时间和空间上调整轨迹，因此它能够以最少的时间生成更平滑的轨迹，如图10所示。然而，该算法将使得无人机在森林中心的狭窄区域相遇，狭小的解空间会导致一些无人机频繁重规划轨迹，飞行速度也同时受到影响。如图11(a) 所示，有2架无人机速度最低下降到0.5 m/s。AFDE采用的动力学搜索能够获取良好的参考路径，因此规划次数相对较少。但均匀采样策略导致了较长的计算用时，如表4数据所示。同时，该方案使用的B样条参数化方法难以进行时间优化，因而导致了最大的飞行代价。

图  10  由4种规划方案生成的轨迹

Fig.  10  Trajectory generated by four planning schemes

下载: 全尺寸图片

图  11  不同算法下可通信和不可通信无人机速度曲线

Fig.  11  Communicationand non-communication UAV speed curves using different algorithms

下载: 全尺寸图片

与它们相比，本文的方案具有2种提高规划效率的方法，求解用时仅次于SMFR，能够满足混动态场景的实时性要求。其次，在轨迹性能方面，如图11 所示，采用本文算法的可通信无人机和不可通信无人机都能保持相对较高的速度飞行。此外，通过同时考虑速度和位置避碰，本文方案在动态避碰方面表现最好，能够以最少的规划次数生成高性能的轨迹。

为评估规划方案在不同场景中的表现，本节将图9森林场景中的垂直柱体障碍替换为悬浮在空中随机位置的立方体，从而使得实验场景具有更强的三维特性。在该场景中重复上述实验，飞行轨迹图如图12所示。实验结果表明，本文提出的方案求解时间短、轨迹质量高、能量代价低、规划次数最少，即具有综合最优的性能。

图  12  由4种规划方案生成的轨迹

Fig.  12  Trajectory generated by four planning schemes

下载: 全尺寸图片

通过全面的实验对比，可以发现，本文的算法对于野外未知森林环境具有较好的表现，适用于森林火灾的监测与救援和偏远灾区搜救等实际任务场景。

5.   结束语

本文提出了一种未知混动态环境下多机自主飞行的实时动力学规划方案。该方案改进了动力学路径搜索方法，使其具备有效处理动态障碍的能力，并大幅提高其搜索效率，快速获取一条动态可行且安全的参考路径。此外，还提出了一种拓扑引导梯度法，用于高效构建轨迹优化所需的梯度信息，同时保证轨迹的最优性。所提出的方案经过大量的仿真实验与对比实验验证，结果表明本文的方案具有高效性与最优性。未来将在野外森林中进一步验证并改进本文的方案。