Trajectory planning method based on improved sequential convex optimization algorithm for multiple UAVs
-
摘要: 随着人工智能技术的迅速发展，利用多架无人机执行飞行任务成为研究的热点之一。本文研究多架无人机到达同一目标点的航迹规划问题，首先建立最优控制问题，采用符号距离函数将非凸避障约束进行凸化；接着考虑了无人机离散序列间的避障约束，保证在连续时间内多无人机的飞行安全；最后提出了自适应信赖域序列凸优化算法，能够保证在每次序列迭代中都能找到近似凸子问题的解，快速求解得到多无人机最优航迹。该算法适用于威胁环境下多无人机协同到达指定的目标点，具有较快的收敛速度。Abstract: The rapid development of artificial intelligence technology in recent years has made the execution of collaborative tasks using multiple unmanned aerial vehicles (UAVs) a prominent research topic. In this study, the path planning problem for a multi-UAV cooperative arriving at a target point in a complex environment is studied. First, an optimal control problem is formulated. The non-convex obstacle avoidance constraints are addressed using the sign distance function. Then, the obstacle avoidance constraints between the discrete sequences of UAVs are considered to ensure continuous flight safety for multiple UAVs. Finally, a convex optimization algorithm for adaptive feasible region sequences is proposed, which ensures that the solution of the approximate convex problem can be found in each sequence iteration and the optimal trajectory of multiple UAVs can be quickly determined. The algorithm is suitable for a multi-UAV cooperative arriving at specific target points in an obstacle environment and has a fast convergence speed.
-
无人机技术广泛地应用在各领域，如测绘或监测区域、电力巡检、搜索和营救灾区失踪人员、投放物资、防止恐怖袭击[1-4] 等。单无人机受自身性能、负载能力等约束，难以满足多变环境与复杂的任务要求，多无人机可实现多机配合、协同任务的效果，受到广泛关注，其中航迹规划是多无人机核心技术之一[5-6] 。在复杂环境下，多无人机可能在飞行过程中面临山峰密集、雷达探测、气候不利等情况，针对以上情况，本文研究多无人机协同航迹规划算法，具有重要的工程意义。
在多无人机航迹规划算法研究方面，文献[7] 设计了动态窗口法以解决具有障碍物和感知约束的动态突防任务规划问题；文献[8] 研究了基于稀疏A*算法的规划方法，可满足多无人机时间协同与多无人机飞行航迹协同的要求；文献[9] 基于改进的粒子群算法，采用分段航迹规划策略，通过引入协同变量，设计了多无人机可以多方位同时到达的协同攻击策略；文献[10] 提出了一种降维矩阵博弈求解算法，用于在无人机的攻防场景中快速求解大规模矩阵博弈。以上方法取得了较好的精度，但通常计算成本高、时间长，不利于多无人机在复杂环境中执行任务。
随着凸优化算法的发展，凸优化技术已应用到航天、无人机、无人驾驶等军民领域。文献[11] 利用模型预测静态凸规划算法实现了飞行器再入滑翔轨迹规划；文献[12] 将带落角约束的比例导引和凸优化进行结合，实现火箭垂直软着陆制导；文献[13] 研究了凸优化方法在导弹轨迹优化与制导控制中的应用。文献[14] 提出了匈牙利算法的集群任务分组概念，利用凸优化算法实现多机编队低空突防航迹规划。
上述研究展现了凸优化在求解规划问题中快速、可靠的优势，但是在多无人机航迹规划中，如何降低问题凸化的保守性仍是有待深入研究的方向之一，此外，无人机在离散航迹点之间飞行安全也是提高算法实用性所需要考虑的重要因素。
基于上述分析，本文针对多无人机飞行任务，提出了一种基于改进凸优化算法的多机避障航迹规划方法。首先采用无人机离散运动学方程，建立多无人机非凸优化模型，然后将非凸优化模型进行凸化处理，利用自适应信赖域序列凸优化算法对原问题进行迭代求解。最后进行了静态威胁和突发威胁环境下的仿真验证，并且与伪谱法进行对比实验。
本文的创新之处在于以下3点：1）考虑无人机运动学、障碍物、目标约束建立最优控制问题，利用符号距离函数将非凸避障约束进行凸化，降低规划问题变换的保守性，使得凸化后的问题更逼近于原问题；2）考虑了无人机离散序列间的避障约束，保证在连续时间内多无人机的飞行安全；3）提出了基于罚函数的自适应信赖域序列凸优化算法，通过在每次迭代凸子优化问题中改变信赖域的大小，使得原问题的收敛性得到了保证。
1. 多无人机协同航迹规划非凸模型
针对多无人机协同到达指定目标点的任务，建立多无人机航迹规划模型，考虑到雷达探测、气候影响等威胁因素，建立的航迹规划模型主要包括：无人机运动方程、目标点与起点的状态约束、威胁避障约束等。
1.1 多无人机运动方程描述
本文选取位置与速度作为无人机的状态量，建立无人机离散运动学方程：
$$ p_i^{k + 1} = p_i^k + v_i^k\Delta t{\text{ }} + \frac{{\Delta {t^2}}}{2}a_i^k $$ (1) $$ v_i^{k + 1} = v_i^k + a_i^k\Delta t $$ (2) 式中：
$p_i^{k + 1} \in {{\bf{R}}^3}$ 和$v_i^{k + 1} \in {\bf{R}}$ 分别表示无人机在$k + 1$ 时刻的位置与速度，$i \in \left\{ {1,2, \cdot \cdot \cdot ,N} \right\}$ 表示无人机的架数，$k \in \left\{ {1,2, \cdot \cdot \cdot ,K} \right\}$ 表示当前离散时刻，$a_i^k$ 表示无人机在$k$ 时刻的加速度，$\Delta t$ 表示时间步长。利用式(1)、(2)，位置和速度可以表示为加速度的仿射函数。因此，定义无人机在每个时间步长的加速度
${ \boldsymbol {\chi }}$ 为待优化参数：$$ { \boldsymbol {\chi }} \triangleq \left[ {a_i^k} \right]\left| {_{i = 1, \cdot \cdot \cdot ,N;k = 1,2, \cdot \cdot \cdot K}} \right. \in {{\bf{R}}^{3NK}} $$ (3) 第
$i$ 架无人机在$k$ 时刻的速度和位置可表示为$$ p_i^k=p_i^1+\left(k-1\right)\Delta tv_i^1+\frac{\Delta t^2}{2}\left[\sum\limits_{t=1}^ka_i^t\left(2\left(k-t\right)-1\right)\right] $$ (4) $$ v_i^k = v_i^1 + \Delta t\left[ {\sum\limits_{t = 1}^{k - 1} {a_i^t} } \right] $$ (5) 式中：
$p_i^1$ 和$ v_i^1 $ 分别表示第一个离散点的无人机位置与速度，$v_i^k$ 表示无人机第$k$ 个离散时刻的无人机速度。1.2 多无人机航迹规划的目标函数建立
本文建立多无人机航迹规划的目标函数[15] 公式为
$$ f = \sum\limits_{k = 1}^T {\sum\limits_{i = 1}^N {\left\| {a_i^k} \right\|_{}^2} } $$ 根据式(3)，可将目标函数表示为
$$ f = {{ \boldsymbol {\chi }}^{\text{T}}}{ \boldsymbol {\chi }} $$ 其中，二次函数可以更好地满足仿射等式和不等式约束，也可使得多无人机航迹规划产生平滑的曲线，并且利用现有的二次规划求解器可以容易地解决优化问题。
1.3 多无人机协同任务的约束条件建立
为了执行多无人机协同到达指定目标点的任务，考虑无人机初始状态（位置、速度）约束、目标状态约束以及控制量的约束，式(4)、(5)已经考虑了初始状态的位置与速度，因此，还需对初始状态的加速度、目标状态的位置、速度、加速度进行约束。对于每个无人机在三维环境中还需引入12个约束，则一共得到
${\text{12}}NK$ 个约束。$$ \begin{gathered}a_i^k=a_i^0;k=0; \\ p_i^k=p_i^{t_{\mathrm{f}}};v_i^k=v_i^{t_{\mathrm{f}}};a_i^k=a_i^{t_{\mathrm{f}}};k=K \\ \end{gathered} $$ 式中
$ t\mathrm{_f} $ 表示为无人机到达目标点终端时间。值得注意的是为了保证无人机能够协同到达目标点，在终端约束中，给每架无人机设置了相同的飞行终端时间
$ t\mathrm{_f} $ 和离散点$k$ ，$k \in \left\{ {1,2, \cdot \cdot \cdot ,K} \right\}$ 。在优化问题中加入终端位置的等式约束、速度约束以及加速度约束，即$ p\mathit{_i^{{K}}}=p_i^{t_{\mathrm{f}}};v\mathit{_i^{{K}}}=v_i^{t_{\mathrm{f}}};a\mathit{_i^{{K}}}=a_i^{t\mathrm{_f}} $ 。通过这种方式，保证了在最后一个离散点$K$ 处（即最终时刻）每架无人机的位置、速度和加速度是相同的，从而达到协同的效果。1.4 多无人机飞行状态约束及避障约束
无人机在一定的空间范围内飞行，首先建立无人机位置约束条件为
$$ {p_{\min ,h}} \leqslant p_i^k \leqslant {p_{\max ,h}},h \in \left\{ {x,y,{\textit{z}}} \right\},\forall i,k $$ 同时，考虑到无人机飞行能力的约束，建立速度与加速度的约束，共
$6NK$ 个不等式约束，公式表示为$$ {v_{\min ,h}} \leqslant v_i^k \leqslant {v_{\max ,h}},h \in \left\{ {x,y,{\textit{z}}} \right\},\forall i,k $$ $$ {a_{\min ,h}} \leqslant a_i^k \leqslant {a_{\max ,h}},h \in \left\{ {x,y,{\textit{z}}} \right\},\forall i,k $$ 此外，为了满足无人机的物理约束，必须考虑加速度导数
$\varepsilon $ ，即加加速度，该约束表明加速度是连续的，保证了无人机的平稳飞行。$$ {\varepsilon _{{\text{min}},h}} \leqslant \varepsilon _i^k \leqslant {\varepsilon _{\max ,h}},h \in \left\{ {x,y,{\textit{z}}} \right\},\forall i,k $$ 多无人机执行协同飞行任务时，面临着多种威胁环境，且威胁存在不确定性的情况，需要对无人机进行避障约束，使其在威胁环境内安全飞行。本文将威胁条件膨胀成球体，则无人机飞行航迹必须在一定的安全距离飞行，威胁约束可表示为
$$ \left\| {p_i^k - {p_m}} \right\| \geqslant R $$ 设有
$M$ 个威胁存在，$m \in \left\{ {1,2, \cdot \cdot \cdot ,M} \right\}$ ，${p_m}$ 表示第$m$ 个威胁的圆心位置，$R$ 表示躲避威胁的安全距离。由于本文研究的是多无人机协同航迹规划问题，因此还需避免多无人机之间相互碰撞。设无人机之间的安全距离为$d$ ，则无人机之间的约束可表示为$$ \left\| {p_i^k - p_j^k} \right\| \geqslant d,\forall i,j,i \ne j,\forall k \\$$ (6) 1.5 无人机优化模型
综上，建立多无人机协同航迹规划的最优控制模型为
$ \mathrm{P}_1 $ 。$$ \begin{gathered} \mathop {\min }\limits_{ \boldsymbol {\chi }} {\text{ }}f \\ {\text{s}}{\text{.t}}{\text{. }}{{ \boldsymbol {A}}_{{\text{eq}}}}{ \boldsymbol {\chi }} = {{ \boldsymbol {b}}_{{\text{eq}}}} \\ {\text{ }}{{ \boldsymbol {A}}_{{\text{in}}}}{ \boldsymbol {\chi }} = {{ \boldsymbol {b}}_{{\text{in}}}} \\ \end{gathered} $$ 式中：
$ {{ \boldsymbol {A}}_{{\text{eq}}}} $ 表示为等式约束的矩阵，$ \boldsymbol{b}_{\text{eq}} $ 表示为所满足的等式条件矩阵，$ {{ \boldsymbol {A}}_{{\text{in}}}} $ 表示为不等式约束的组合矩阵，$ {{ \boldsymbol {b}}_{{\text{in}}}} $ 表示为不等式约束所满足的不等式条件组合矩阵。本文的式(4)、(5)的状态量可由控制量仿射函数表征，因此可将其带入其他约束条件显式表示。2. 多无人机航迹规划问题凸处理
2.1 避障约束凸处理
在无人机航迹优化过程中需快速达到收敛状态，本文基于最小平移方法[16] ，采用计算无人机位置与威胁圆心位置的标记距离进行避障凸化。首先将无人机膨胀成一个圆，然后把第
$ i $ 架无人机离散时刻的位置集合记作$ \boldsymbol{\mathit{S}}_i $ ，将威胁的位置集合记作为$ \boldsymbol{\mathit{S}}\mathrm{_{\mathit{m}}} $ 。为使无人机在每个离散时间处满足威胁约束，则$ \boldsymbol{\mathit{S}}_i\cap\boldsymbol{\mathit{S}}_{\mathrm{\mathit{m}}}=\varnothing $ ，最后计算2个集合的闵可夫斯基距离记作${{ \boldsymbol {d}}_i}$ 。若无人机与障碍物不发生碰撞时${{ \boldsymbol {d}}_i} > 0$ ，记${ \boldsymbol {d}}_i^ + $ ，如图1(a)所示，若无人机与障碍物发生碰撞时${{ \boldsymbol {d}}_i} < 0$ ，记${ \boldsymbol {d}}_i^ - $ ，如图1(b)所示。$p_i^{\text{c}}$ 表示无人机距离障碍物最近的点，$p_{m}^{\text{c}}$ 表示障碍物距离无人机最近的点。
图 1 闵可夫斯基距离Fig. 1 Minkowski distance
下载:
全尺寸图片
根据图1可知，对于第
$ i $ 架无人机在整个飞行任务中的闵可夫斯基距离为${{ \boldsymbol {d}}_i} = { \boldsymbol {d}}_i^ + + { \boldsymbol {d}}_i^ - $ 。当无人机集合与威胁集合不相交时，闵可夫斯基距离为正，当无人机集合与威胁集合相交时，闵可夫斯基距离为负，可表示为$$ \begin{gathered} { \boldsymbol {d}}_i^ + = {\text{dist}}\left( {{{ S}_i},{{ S}_m}} \right) = \inf \left\{ {\left\| { \boldsymbol {d}} \right\|\left| {\left( {{ \boldsymbol {d}} + {{ S}_i}} \right) \cap {{ S}_m}} \right. \ne \varnothing } \right\} \\ { \boldsymbol {d}}_i^ - = {\text{dist}}\left( {{{ S}_i},{{ S}_m}} \right) = \inf \left\{ {\left\| { \boldsymbol {d}} \right\|\left| {\left( {{ \boldsymbol {d}} + {{ S}_i}} \right) \cap {{ S}_m}} \right. \ne \varnothing } \right\} \\ \end{gathered} $$ 简单来说，当无人机与威胁不接触时，通过平移最小量
${ \boldsymbol {d}}_i^ + $ ，使得两者接触；当无人机与障碍物接触时，通过平移最小量${ \boldsymbol {d}}_i^ - $ ，两者不接触。本文采用基于罚函数序列凸优化算法，该算法是依赖迭代上一次的解求解近似凸子问题，则设已知无人机上次迭代基准航迹对应的离散点的位置为$ \overline {p_i^k} $ ，然后将闵可夫斯基距离${{ \boldsymbol {d}}_i}$ 线性化处理可得$$ \begin{gathered} {\boldsymbol{d}}_i^k = \overline {{{\boldsymbol{d}}}_i^k} + {{\boldsymbol{d}}}_i^k{{ \boldsymbol {J}}_i}\left( {{ \boldsymbol {p}}_i^k - \overline {{ \boldsymbol {p}}_i^k} } \right) \\ \left({{\boldsymbol{d}}}_i^k\right)^{\text{T}} = \frac{{\overline {{{\boldsymbol{d}}}_i^k} }}{{{{\left\| {{{\boldsymbol{d}}}_i^k} \right\|}_2}}} \\ \end{gathered} $$ (7) 式中：
$\left({{\boldsymbol{d}}}_i^k\right)^{\text{T}}$ 表示无人机到障碍物的法向量距离；$ {{ \boldsymbol {J}}_i} $ 是雅可比矩阵，由于无人机模拟成球体，则雅可比矩阵是单位矩阵。2.2 各无人机之间规避约束凸化
由于多无人机之间规避约束是非凸约束，需要将其转换成凸形式以满足凸优化模型。本文基于上次迭代计算得到的无人机位置
$ \overline{p_i^k}、\overline{p_j^k} $ ，对式(6)进行线性化处理：$$ \begin{gathered}\left|p_i^k-p_j^k\right|=\left|\overline{p_i^k}-\overline{p_j^k}\right|+\mathrm{sign}(\overline{p_i^k}-\overline{p_j^k})(\Delta p_i^k-\Delta p_j^k)\geqslant d \\ \mathrm{sign}(x)=\left\{\begin{gathered}1,\ x > 0 \\ 0,\ x=0 \\ -1,\ x < 0\text{ } \\ \end{gathered}\right. \\ \Delta p_i^k=p_i^k-\overline{p_i^k},\Delta p_j^k=p_j^k-\overline{p_j^k} \\ \end{gathered} $$ 凸化处理后的结果如图2所示，多无人机间避障凸化约束在线性化处理之后，在相对应的离散时刻每架无人机都会在安全距离
$d$ 外，即$d$ 区域随着离散时刻无人机对应的位置变化而变化，并且每个无人机离散时刻位置的连线与$d$ 区域一直保持垂直。
图 2 无人机间凸化Fig. 2 Convexification between two UAVs下载:
全尺寸图片
2.3 各离散点间约束避障凸化
根据上述处理，多无人机满足了彼此在离散点处避免碰撞，但可能在离散点之间会发生碰撞，因此还需引入离散点之间的规避约束[17] 。根据上文已知第
$ i $ 架无人机在离散时刻$ k $ 与$ k+1 $ 的位置分别为$ {p}_{i}^{k}、{p}_{i}^{k+1} $ ，并且本文将无人机膨胀成一个球体，则从离散时刻$k$ 到$k + 1$ 这一步长内扫过的体积为无人机在这一步长内的飞行航迹，将此航迹看成一个多面体，则必须保证在作战环境下的威胁区域与该多面体不发生碰撞。根据2.1节的避障方式，本文假设多面体存在与威胁区域离最近的点$p_i^d$ ，因此，$ {p}_{i}^{k}、{p}_{i}^{k+1}、{p}_{i}^{d} $ 可构成一个凸包，可表示为$$ \begin{gathered} p_i^d\left( { \boldsymbol {\chi }} \right) = \alpha p_i^k + \left( {1 - \alpha } \right)p_i^{k + 1} \\ \alpha = \frac{{\left\| {p_i^{k + 1} - p_i^d} \right\|}}{{\left\| {p_i^k - p_i^d} \right\| + \left\| {p_i^{k + 1} - p_i^d} \right\|}} \\ \end{gathered} $$ 式中：
$ \alpha $ 表示$ {p}_{i}^{k}、{p}_{i}^{k+1} $ 距离权重因子，用于计算$ p_i^d\left( { \boldsymbol {\chi }} \right) $ 到这2个端点之间的线性插值位置；$ p_i^d\left( { \boldsymbol {\chi }} \right) $ 由${ \boldsymbol {\chi }}$ 参数化得到，与式(7)相似，在离散时刻$k$ 和$k + 1$ 处，对航迹进行线性化处理，可表示为$$ \begin{array}{c}\boldsymbol{d}_i=\overline{\boldsymbol{d}_i}+\alpha\overline{\boldsymbol{d}_i}^{\mathrm{T}}\boldsymbol{J}_i^k\left(p_i^k-\overline{p_i^k}\right)+\left(1-\alpha\right)\overline{\boldsymbol{d}_i}^{\text{T}}\boldsymbol{J}_i^{k+1}\left(p_i^{k+1}-\overline{p_i^{k+1}}\right) \\ \boldsymbol{J}_i^k=\dfrac{\boldsymbol{d}}{\boldsymbol{d}\boldsymbol{\chi}^k}p_i^k，\boldsymbol{J}_i^{k+1}=\dfrac{\boldsymbol{d}}{\boldsymbol{d}\boldsymbol{\chi}^{k+1}}p_i^{k+1}\end{array} $$ 式中：
${{{\boldsymbol{d}}}_i}$ 表示第i架无人机点$ p_i^d\left( { \boldsymbol {\chi }} \right) $ 到障碍物$p_{m}^{\text{c}}$ 的闵可夫斯基距离，${ \boldsymbol {J}}_i^k$ 表示第i架无人机在离散时刻k的雅可比矩阵，${ \boldsymbol {J}}_i^{k + 1}$ 同理。无人机离散点之间进行凸化处理之后结果如图3所示。
图 3 离散序列凸化Fig. 3 Convexification between discrete sequences下载:
全尺寸图片
3. 改进序列二次凸优化算法
本节针对第2节所建立的凸问题，设计改进的序列凸优化算法，提出自适应改变信赖域的策略，提高算法的收敛速度。
3.1 基于罚函数的凸优化
第2节所建立的优化模型的目标函数和不等式约束是凸的，等式约束是仿射的，是一个凸优化问题，但是本文的无人机飞行环境复杂，考虑了众多约束，由于第2节近似凸处理，因此构建的优化问题保守性高，利用序列凸优化(sequential convex programming, SCP)求解最优值时，近似凸子问题可能会出现无解的问题[18]，即二次规划求解器返回的结果是空数据，这将使得算法终止迭代，出现找不到最优值的情况。本文采用罚函数法解决此问题，将不等式约束与等式约束引入到目标函数中，该方法是从信赖域出发，并保持在信赖域内进行寻优，若优化的迭代值无限接近信赖域附近，惩罚因子
$ \mu $ 将趋于无穷大，迫使迭代值返回至内部，这使得每次迭代的最优值都为可行解，因此构建出凸子优化问题模型P2为$$ \begin{gathered} \mathop {\min }\limits_{ \boldsymbol {\chi }} {\text{ }}f + \mu \left\| {{{ \boldsymbol {A}}_{{\text{eq}}}}{ \boldsymbol {\chi }} - {{ \boldsymbol {b}}_{{\text{eq}}}}} \right\| + \mu \left\| {{{ \boldsymbol {A}}_{{\text{in}}}}{ \boldsymbol {\chi }} - {{ \boldsymbol {b}}_{{\text{in}}}}} \right\|_1^ + \\ {\text{ s}}{\text{.t}}.{\text{ }}\left\| {{ \boldsymbol {\chi }}_i^{k + 1} - { \boldsymbol {\chi }}_i^k} \right\| \leqslant r,i \in \left\{ {1,2, \cdot \cdot \cdot ,N} \right\} \\ \end{gathered} $$ 式中：
${\left\| g \right\|^ + } = \max \left( {0,g} \right)$ ，$r$ 表示信赖域半径。3.2 基于罚函数的二次序列凸优化
本文采用SCP算法，它可以解决非线性、非凸、复杂约束的问题， SCP本质是将非凸约束进行凸化处理，从而将非凸问题变成近似凸子问题，然后再将凸子问题迭代求解，每次迭代都是依赖上一次迭代的结果进行求解，最后通过逐次逼近得到接近原问题的解[19-20] 。但近似凸子问题可能在迭代过程中出现陷入死循环，导致优化问题无法收敛，因此本文提出自适应信赖域的SCP算法，在每次迭代过程改变信赖域的大小，从而提高算法的收敛速度。改进SCP的算法流程如图4所示。
图 4 改进SCP的算法流程Fig. 4 Flowchart of improved SCP algorithm下载:
全尺寸图片
1)确定无人机架数、初始位置、目标位置；确定信赖域缩放系数
${\beta _{\text{s}}},{\beta _{\text{f}}}$ 且${\beta _{\text{s}}} > 1$ ，$0 < {\beta _{\text{f}}} < 1$ ；确定罚函数权重缩放系数$\gamma > {\text{1}}$ ；初始化可行区域参数${r_{\text{0}}}$ 。初始化不等式约束的罚函数权重$w_{{\text{in}}}^{}$ ，等式约束的罚函数权重$w_{{\text{eq}}}^{}$ 。设置迭代次数为$n$ 。2)由于序列凸优化不依赖初始化航迹，则第一次迭代时可以不考虑避障约束，第一个解作为参考航迹进行下次的迭代(
$n = 1$ )。3)根据凸子优化问题，计算下一次的优化参数，由式(4)可知对应的迭代航迹结果
${ \boldsymbol {\chi }}_{}^n$ ($n > 1$ )，判断计算连续2代的航迹结果的相对误差是否在可行区域内，进而改变可行区域参数${r_{n + 1}}$ 、不等式约束的罚函数权重$w_{{\text{in}}}^{n + 1}$ 以及等式约束的罚函数权重$w_{{\text{eq}}}^{n + 1}$ 。4)如果误差在给定信赖域内接受并更新可行区域值，进而判断基于罚函数的解是否在信赖域内，如果不满足硬惩罚状态约束的情况下，增加权重，使得求解器在下一次迭代中能够满足约束，反之，如果这个误差大于可行区域，则线性近似太粗糙，拒绝
${{ \boldsymbol {\chi }}^{n + 1}}$ ，缩小信赖域，再判断硬约束是否满足。5)将得到n+1代的信赖域值、不等式约束权重、等式约束权重值代入凸子优化问题进行下一代迭代求解。
6)当连续迭代得到相同的解或罚函数权重大于给定的最大罚函数权重时，算法结束。
4. 仿真结果与分析
4.1 静态威胁环境下仿真分析
本节针对所提算法进行仿真模拟，验证其有效性。仿真硬件环境为CPU i7-9700k，内存20 GB，求解器为quadprog。假定在20 m×20 m×20 m三维环境中执行飞行任务，环境中存在气象阻力、山峰、雷达检测区域。设置共有5架无人机参与任务，离散点个数为28，最大飞行速度为4 m/s，最低飞行速度为0.5 m/s，最大加速度为3 m/s2，最小加速度为−3 m/s2，加加速度变化范围为−5~6 m/s3，无人机之间的安全距离
$ d $ 取 1 m，另外，不等式约束罚函数权重${w_{{\text{in}}}}$ 为100，等式约束罚函数权重${w_{{\text{eq}}}} = {w_{{\text{in}}}} \times k$ 。5架无人机和12个威胁的位置信息分别如表1和表2所示。表 1 多无人机状态信息Table 1 Status information of multiple UAVsm 初始位置 目标位置 无人机1 (1.5,1.5,15) (20,20,5) 无人机2 (0,1.5,15) (20,20,5) 无人机3 (1.5,0,15) (20,20,5) 无人机4 (0,3,15) (20,20,5) 无人机5 (3,0,15) (20,20,5) 表 2 飞行环境威胁信息Table 2 Flight environmental threat informationm 位置信息 $ \left(x,y,{\textit{z}},r\right) $ 位置信息 $ \left(x,y,{\textit{z}},r\right) $ 威胁1 (5,5,17,2.5) 威胁7 (8,17,8,3) 威胁2 (4,11,12,2.5) 威胁8 (10,8,12,1.8) 威胁3 (15,7,10,2) 威胁9 (11,13,13,2.3) 威胁4 (10,2,10,2) 威胁10 (18,13,10,2.2) 威胁5 (8,8,0,6.8) 威胁11 (13,14,9,1.8) 威胁6 (5,6,10,1.5) 威胁12 (16,16,0,4) 由于初步迭代不考虑避障约束情况下，运行时间较快，在第2步迭代时，算法自动修改为考虑避障约束的凸子优化问题，由于为算法收敛设置的信赖域影响，每代运行的时间变长。选择较大的信赖域值时，算法可能会在几步之内收敛，这将导致局部最优值成为全局最优值，降低算法的准确率；选择较小的信赖域，可能出现大量的迭代，从而导致较长的运行时间。因此，本文经过实验验证，选择了初始信赖域为0.1，算法收敛，找到最优值。
根据上述的设置，利用本文改进算法对5架无人机进行威胁环境下航迹规划，结果如图5所示，图5(b)、 (c)分别为航迹规划结果的侧视图和俯视图，可看出该算法能够成功完成协同到达指定目标点的任务，并且可以在执行任务过程中成功规避威胁区域。
图 5 基于改进凸优化的多无人机航迹规划Fig. 5 UAV trajectories based on improved convex optimization method下载:
全尺寸图片
为了验证本文所提算法的性能，与伪谱法[21] 进行比较。在相同飞行任务、相同的威胁环境下，伪谱法首先建立无人机的动力学模型，加入约束条件，将其转化为非线性约束，然后，根据路径规划的性能指标，建立了多无人机航迹规划最优控制模型，最后使用GPOPS-II[22] 工具包实现求解最优控制问题，结果如图6所示。表3对2种算法的收敛值与运行时间进行了比较，由此可得出本文所提出的算法求解的目标函数值更小，运行时间更快，收敛速度更快。
图 6 伪谱法多无人机航迹规划Fig. 6 UAV trajectories based on pseudo-spectral method下载:
全尺寸图片
表 3 算法对比Table 3 Comparison of algorithms算法 收敛值/(m/s2) 运行时间/s 伪谱法 15.52 37.41 改进序列二次凸优化 12.18 8.18 由图5、6可知，伪谱法与本文改进的序列凸优化算法都能规避威胁，并且安全完成飞行任务。基于此，本文针对无人机个数对两者算法运行时间的影响进行比较，结果如图7所示。在无人机单机航迹规划时，两者算法效果接近，但随着无人机数量的增加，无人机航迹规划维度也随之逐渐增大，伪谱法远比本文方法的耗时长。这是由于伪谱法采用非线性规划求解一般的参数问题从而导致其耗时更长。因此，通过仿真对比实验，本文改进的凸优化算法可以在短时间内完成多无人机的航迹规划。
图 7 对比结果Fig. 7 Comparison results下载:
全尺寸图片
4.2 突发威胁环境下的仿真分析
为验证本文所提算法在突发威胁环境下的有效性，本节分别在二维和三维飞行空间加入突发威胁，进行多无人机协同航迹规划仿真实验。
4.2.1 二维环境中突发威胁时多无人机航迹规划
设置共有3架无人机参与任务，无人机在飞行的过程中出现突发威胁，具体威胁信息见表4，其中4个威胁信息已知，2个威胁在飞行过程中出现。选取突发威胁前的离散点为10，突发威胁后的离散点为7，无人机最大飞行速度16 m/s，最低飞行速度4 m/s，最大加速度允许在0.7 m/s2，最小加速度允许在−0.5 m/s2，加加速度变化为−0.1~0.1 m/s3，无人机之间的安全距离
$ d $ 取8 m，此外，不等式约束罚函数权重、等式约束罚函数权重与4.1节保持一致。3架无人机的起始位置分别为(0 m, 0 m)、(0 m, 200 m)、(2 000 m, 0 m)。目标位置均为(875 m, 875 m)。表 4 二维威胁位置信息Table 4 2D threat location informationm 位置信息 $ \left(x,y,r\right) $ 威胁1 (200,180,60) 威胁2 (200,600,60) 威胁3 (500,200,80) 威胁4 (500,80,80) 突发威胁1 (400,400,80) 突发威胁2 (600,600,60) 首先根据已知的4个威胁信息（图8中蓝色区域所示），利用本文航迹规划方法，规划得到3架无人机的飞行航迹，如图8中虚线所示，利用本文算法能够快速规划出无人机的航迹。3架无人机按照规划好的航迹飞行，飞行中途，环境中突然出现新的威胁，如图9中红色区域所示。此时，根据新的威胁信息，重新构建多无人机协同规划问题，利用所提算法重新规划无人机飞行航迹，规划结果如图9中实线所示，规划时间3.89 s。
图 8 二维环境下多无人机航迹规划Fig. 8 Multiple UAVs trajectory planning in 2D environment下载:
全尺寸图片
图 9 二维突发威胁环境下多无人机航迹重规划Fig. 9 Multiple UAVs trajectory replanning in 2D environment with unexpected threats下载:
全尺寸图片
二维突发威胁环境下多无人机的状态约束结果如图10所示。
图 10 二维环境下多无人机的状态约束Fig. 10 State constraints of UAVs in 2D environment下载:
全尺寸图片
图10(a)为速度约束，根据图中信息可知，无人机在第6个离散点时，出现突发威胁，重新规划无人机的飞行航迹，速度变化程度大，但让满足速度约束。如图10(b)、(c)所示，其加速度约束与加加速度约束都在其约束范围内，保证了无人机在连续时间内平稳飞行。
4.2.2 三维环境中突发威胁时多无人机航迹规划
在三维环境中同样设置3架无人机参与任务，飞行环境中的威胁信息如表5所示，其中5个威胁信息是已知的，1个威胁在飞行过程中出现。无人机飞行速度、加速度等实验参数与4.1节中的相同。首先，通过基于改进的凸优化算法，对3架无人机进行初始航迹规划，如图11所示中虚线所示。3架无人机根据初始航迹结果飞行，飞行至中途时，环境中突然出现新的威胁，如图12中黄区域所示。此时，重新构建多无人机协同规划问题，加入突发规避威胁约束，然后利用所提算法对无人机航迹结果进行求解。
表 5 三维威胁位置信息Table 5 3D threat location informationm 位置信息 $ \left(x,y,{\textit{z}},r\right) $ 威胁1 (5,5,12,2) 威胁2 (8,8,0,6.8) 威胁3 (8,17,8,3) 威胁4 (10,4,10,2) 威胁5 (16,16,0,4) 突发威胁1 (12,12,10,3) 
图 11 三维环境下多无人机协同航迹规划Fig. 11 Multiple UAVs trajectory planning in 3D environment下载:
全尺寸图片
三维重规划结果如图12所示。虚线为出现突发威胁前无人机航迹结果，实线为重规划结果。可见利用所提方法可以根据新的威胁环境，快速重新规划出飞行航迹，完成对突发威胁的规避飞行。由于在重规划时，规划路径短，选取的离散点个数少，收敛速度快，保证无人机在平稳飞行。
图 12 三维环境下多无人机协同航迹重规划Fig. 12 Multiple UAVs trajectory replanning in 3D environment下载:
全尺寸图片
此外，对速度约束、加速度约束等进行分析，三维突发威胁环境下无人机状态约束结果如图13所示。由图13(b)可知，无人机在未出现突发威胁前平稳飞行，当3架无人机飞行至第10个离散点时，突发威胁出现，这时无人机飞行波动最大，随之，无人机速度迅速减小，进行二次重规划航迹。在第10个离散点之后，无人机再次加速飞行，实现对突发威胁的规避飞行。
图 13 三维环境下多无人机的状态约束Fig. 13 State constrains of multiple UAVs in 3D environment下载:
全尺寸图片
通过上述突发威胁情形下的仿真实验，验证了本文所提改进的序列凸优化算法的实时性和准确性，该算法能够满足高效、安全、快速的多无人机协同航迹规划需求，提升了多无人机在复杂三维环境中的自主飞行安全性。
5. 结束语
为了提高多无人机任务执行的智能化水平，本文提出自适应信赖域的罚函数序列二次凸优化算法，实现多无人机协同航迹规划。首先建立多无人机非凸优化模型；其次将非凸约束进行凸化处理，引入了符号距离函数凸化避障方式；接着考虑了多无人机离散点之间碰撞问题，不仅满足多无人机在离散点处的避免碰撞，而且提高多无人机在连续时间段的安全性；最后基于SCP算法提出自适应信赖域，提高该算法收敛速度，降低了SCP算法的保守性。未来将着力于实物实验以及如何提高非凸转换的效率。
-
图 1 闵可夫斯基距离
Fig. 1 Minkowski distance
下载:
全尺寸图片
图 2 无人机间凸化
Fig. 2 Convexification between two UAVs
下载:
全尺寸图片
图 3 离散序列凸化
Fig. 3 Convexification between discrete sequences
下载:
全尺寸图片
图 4 改进SCP的算法流程
Fig. 4 Flowchart of improved SCP algorithm
下载:
全尺寸图片
图 5 基于改进凸优化的多无人机航迹规划
Fig. 5 UAV trajectories based on improved convex optimization method
下载:
全尺寸图片
图 6 伪谱法多无人机航迹规划
Fig. 6 UAV trajectories based on pseudo-spectral method
下载:
全尺寸图片
图 7 对比结果
Fig. 7 Comparison results
下载:
全尺寸图片
图 8 二维环境下多无人机航迹规划
Fig. 8 Multiple UAVs trajectory planning in 2D environment
下载:
全尺寸图片
图 9 二维突发威胁环境下多无人机航迹重规划
Fig. 9 Multiple UAVs trajectory replanning in 2D environment with unexpected threats
下载:
全尺寸图片
图 10 二维环境下多无人机的状态约束
Fig. 10 State constraints of UAVs in 2D environment
下载:
全尺寸图片
图 11 三维环境下多无人机协同航迹规划
Fig. 11 Multiple UAVs trajectory planning in 3D environment
下载:
全尺寸图片
图 12 三维环境下多无人机协同航迹重规划
Fig. 12 Multiple UAVs trajectory replanning in 3D environment
下载:
全尺寸图片
图 13 三维环境下多无人机的状态约束
Fig. 13 State constrains of multiple UAVs in 3D environment
下载:
全尺寸图片
表 1 多无人机状态信息
Table 1 Status information of multiple UAVs
m 初始位置 目标位置 无人机1 (1.5,1.5,15) (20,20,5) 无人机2 (0,1.5,15) (20,20,5) 无人机3 (1.5,0,15) (20,20,5) 无人机4 (0,3,15) (20,20,5) 无人机5 (3,0,15) (20,20,5) 表 2 飞行环境威胁信息
Table 2 Flight environmental threat information
m 位置信息 $ \left(x,y,{\textit{z}},r\right) $ 位置信息 $ \left(x,y,{\textit{z}},r\right) $ 威胁1 (5,5,17,2.5) 威胁7 (8,17,8,3) 威胁2 (4,11,12,2.5) 威胁8 (10,8,12,1.8) 威胁3 (15,7,10,2) 威胁9 (11,13,13,2.3) 威胁4 (10,2,10,2) 威胁10 (18,13,10,2.2) 威胁5 (8,8,0,6.8) 威胁11 (13,14,9,1.8) 威胁6 (5,6,10,1.5) 威胁12 (16,16,0,4) 表 3 算法对比
Table 3 Comparison of algorithms
算法 收敛值/(m/s2) 运行时间/s 伪谱法 15.52 37.41 改进序列二次凸优化 12.18 8.18 表 4 二维威胁位置信息
Table 4 2D threat location information
m 位置信息 $ \left(x,y,r\right) $ 威胁1 (200,180,60) 威胁2 (200,600,60) 威胁3 (500,200,80) 威胁4 (500,80,80) 突发威胁1 (400,400,80) 突发威胁2 (600,600,60) 表 5 三维威胁位置信息
Table 5 3D threat location information
m 位置信息 $ \left(x,y,{\textit{z}},r\right) $ 威胁1 (5,5,12,2) 威胁2 (8,8,0,6.8) 威胁3 (8,17,8,3) 威胁4 (10,4,10,2) 威胁5 (16,16,0,4) 突发威胁1 (12,12,10,3) -
[1] 符小卫, 徐哲, 朱金冬, 等. 基于PER-MATD3的多无人机攻防对抗机动决策[J]. 航空学报, 2023, 44(7): 196−209. FU Xiaowei, XU Zhe, ZHU Jindong, et al. Maneuvering decision-making of multi-UAV attack-defence confrontation based on PER-MATD3[J]. Acta aeronautica et astronautica sinica, 2023, 44(7): 196−209. [2] 张磊, 李姜, 侯进永, 等. 基于改进强化学习的多无人机协同对抗算法研究[J]. 兵器装备工程学报, 2023, 44(5): 230−238. doi: 10.11809/bqzbgcxb2023.05.033 ZHANG Lei, LI Jiang, HOU Jinyong, et al. Research on multi-UAV cooperative confrontation algorithm based on improved reinforcement learning[J]. Journal of ordnance equipment engineering, 2023, 44(5): 230−238. doi: 10.11809/bqzbgcxb2023.05.033 [3] 武晓龙, 吴涛涛, 张震. 高端战争中低成本无人机集群作战研究[J]. 战术导弹技术, 2023(5): 157−163. WU Xiaolong, WU Taotao, ZHANG Zhen. Low-cost UAVs operational network in high-end warfare[J]. Tactical missile technology, 2023(5): 157−163. [4] 李望西, 王勇, 吴文超, 等. 面向空空作战的有人/无人机协同作战模式研究[J]. 无人系统技术, 2023, 6(4): 76−84. LI Wangxi, WANG Yong, WU Wenchao, et al. Research on manned/unmanned air vehicle cooperative combat model for air-to-air combat[J]. Unmanned systems technology, 2023, 6(4): 76−84. [5] 俞宬, 陈谋, 雍可南. 基于改进RRT*算法的无人机往返航迹规划[J]. 中国科学: 技术科学, 2023, 53(11): 1911−1921. doi: 10.1360/SST-2021-0431 YU Cheng, CHEN Mou, YONG Kenan. Round-trip path planning for unmanned aerial vehicle based on improved RRT* algorithm[J]. Scientia sinica (technologica), 2023, 53(11): 1911−1921. doi: 10.1360/SST-2021-0431 [6] DU Yuwen. Multi-UAV search and rescue with enhanced A* algorithm path planning in 3D environment[J]. International journal of aerospace engineering, 2023, 2023(7): 8614117. [7] FU Jinyu, SUN Guanghui, YAO Weiran, et al. On trajectory homotopy to explore and penetrate dynamically of multi-UAV[J]. IEEE transactions on intelligent transportation systems, 2022, 23(12): 24008−24019. doi: 10.1109/TITS.2022.3195521 [8] 张堃, 刘泽坤, 华帅, 等. 基于T/S-SAS的多无人机四维协同攻击航线生成[J]. 兵工学报, 2023, 44(6): 1576−1587. ZHANG Kun, LIU Zekun, HUA Shuai, et al. Generation of multi-UAV four-dimensional cooperative attack route based on T/S-SAS[J]. Acta armamentarii, 2023, 44(6): 1576−1587. [9] 赵飞虎, 李哲, 王宁, 等. 面向战场的多无人机协同打击航迹规划[J]. 电光与控制, 2023, 30(9): 9−14,91. doi: 10.3969/j.issn.1671-637X.2023.09.002 ZHAO Feihu, LI Zhe, WANG Ning, et al. Path planning for multi-UAV cooperative strike in battlefield environments[J]. Electronics optics & control, 2023, 30(9): 9−14,91. doi: 10.3969/j.issn.1671-637X.2023.09.002 [10] LI Shouyi, CHEN Mou, WANG Yuhui, et al. A fast algorithm to solve large-scale matrix games based on dimensionality reduction and its application in multiple unmanned combat air vehicles attack-defense decision-making[J]. Information sciences, 2022, 594: 305−321. doi: 10.1016/j.ins.2022.02.025 [11] 刘哲, 贾生伟, 张鸣, 等. 再入轨迹多约束模型预测静态凸规划方法[J]. 宇航学报, 2022, 43(12): 1638−1651. doi: 10.3873/j.issn.1000-1328.2022.12.006 LIU Zhe, JIA Shengwei, ZHANG Ming, et al. Reentry trajectory optimization based on constrained model predictive static convex programming[J]. Journal of astronautics, 2022, 43(12): 1638−1651. doi: 10.3873/j.issn.1000-1328.2022.12.006 [12] 安泽, 熊芬芬, 梁卓楠. 基于偏置比例导引与凸优化的火箭垂直着陆制导[J]. 航空学报, 2020, 41(5): 247−260. doi: 10.7527/S1000-6893.2019.23606 AN Ze, XIONG Fenfen, LIANG Zhuonan. Landing-phase guidance of rocket using bias proportional guidance and convex optimization[J]. Acta aeronautica et astronautica sinica, 2020, 41(5): 247−260. doi: 10.7527/S1000-6893.2019.23606 [13] 姜欢. 凸优化方法在导弹轨迹优化与制导控制中的应用[D]. 北京: 北京理工大学, 2017. JIANG Huan. Application convex optimization theory in trajectory optimization, guidance and control of missile[D]. Beijing: Beijing Institute of Technology, 2017. [14] 刘玉杰, 李樾, 韩维, 等. 基于改进凸优化算法的多机编队突防航迹规划[J]. 系统工程与电子技术, 2023, 45(9): 2819−2830. LIU Yujie, LI Yue, HAN Wei, et al. Trajectory planning for penetration of multi-aircraft for mation based on improved convex optimization algorithm[J]. Systems engineering and electronics, 2023, 45(9): 2819−2830. [15] AUGUGLIARO F, SCHOELLIG A P, D’ANDREA R. Generation of collision-free trajectories for a quadrocopter fleet: a sequential convex programming approach[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vilamoura-Algarve: IEEE, 2012: 1917−1922. [16] 徐广通, 王祝, 曹严, 等. 动态优先级解耦的无人机集群轨迹分布式序列凸规划[J]. 航空学报, 2022, 43(2): 420−431. doi: 10.7527/j.issn.1000-6893.2022.2.hkxb202202034 XU Guangtong, WANG Zhu, CAO Yan, et al. Dynamic-priority-decoupled UAV swarm trajectory planning using distributed sequential convex programming[J]. Acta aeronautica et astronautica sinica, 2022, 43(2): 420−431. doi: 10.7527/j.issn.1000-6893.2022.2.hkxb202202034 [17] VIRGILI-LLOP J, ZAGARIS C, ZAPPULLA R, et al. Convex optimization for proximity maneuvering of a spacecraft with a robotic manipulator[C]//27th AAS/AIAA Space Flight Mechanics Meeting. San Antonio: AAS, 2017: 1059−1078. [18] 王祝, 刘莉, 龙腾, 等. 基于罚函数序列凸规划的多无人机轨迹规划[J]. 航空学报, 2016, 37(10): 3149−3158. doi: 10.7527/S1000-6893.2016.0064 WANG Zhu, LIU Li, LONG Teng, et al. Trajectory planning for multi-UAVs using penalty sequential convex pro-gramming[J]. Acta aeronautica et astronautica sinica, 2016, 37(10): 3149−3158. doi: 10.7527/S1000-6893.2016.0064 [19] ZHAO Feng, YU Jianglong, HUA Yongzhao, et al. Decoupled SCP-based trajectory planning in the complex environment for multiple fixed-wing UAV systems[C]//2022 International Conference on Unmanned Aircraft Systems. Dubrovnik: IEEE, 2022: 670−675. [20] MA Yangyang, PAN Binfeng, YAN Rui. Feasible sequential convex programming with inexact restoration for multistage ascent trajectory optimization[J]. IEEE transactions on aerospace and electronic systems, 2023, 59(2): 1217−1230. [21] 李征, 陈建伟, 彭博. 基于伪谱法的无人机集群飞行路径规划[J]. 空天防御, 2021, 4(1): 52−59. doi: 10.3969/j.issn.2096-4641.2021.01.009 LI Zheng, CHEN Jianwei, PENG Bo. UAV cluster path planning based on pseudo-spectral method[J]. Air & space defense, 2021, 4(1): 52−59. doi: 10.3969/j.issn.2096-4641.2021.01.009 [22] PATTERSON M A, RAO A V. GPOPS-II: a Matlab software for solving multiple-phase optimal control problems using hp-Adaptive Gaussian quadrature collocation methods and sparse nonlinear programming[J]. ACM transactions on mathematical software, 2014, 41(1): 1−37.
