引入控制量变化率约束的双层协同轨迹规划器

温 岳，冯肖雪，谢 天，龚柏春，吴云华

（1.北京理工大学自动化学院，北京 100081；2.南京航空航天大学航天学院，南京 210016）

0 引言

随着战场模式不断发展，单亚音速飞行器目标攻击已逐渐不能满足战场复杂态势需要［1］，多亚音速飞行器协同攻击逐渐成为战场主要作战方式［2］。而多飞行器协同轨迹规划是实现多飞行器协同攻击的关键技术之一。多飞行器协同轨迹规划需要充分考虑环境障碍、飞行器性能约束、任务目标等条件，为了更好地完成任务通常需要飞行器实现编队飞行。因此对于多飞行器编队飞行轨迹规划的研究具有重要意义。

多飞行器协同轨迹规划问题通常建模为非线性动力学模型约束、状态量和控制量约束等的最优控制问题。求解其最优控制问题的方法一般分为间接法和直接法［3］。间接法根据Pontryagin 极小值原理得到最优控制问题的一阶必要条件，将其转化为Hamilton边值问题求解。但是由于亚音速飞行器动力学方程的强非线性和强耦合性，使用间接法求解存在转化困难、推导复杂、难以求解等问题。而直接法通过对变量进行离散化，将最优控制问题转化为非线性规划（NLP）问题，再采用许多成熟的非线性规划算法进行快速求解。直接法中的伪谱法由于其快速的收敛性和较高的计算精度，近年来逐渐成为轨迹规划问题的一种常用方法。伪谱法将状态量和控制量同时进行离散，通常再结合序列二次规划（SQP）求解得到高精度轨迹。根据插值基函数以及配点类型的不同，目前发展比较完备的伪谱法主要包括Gauss 伪谱法［4］（GPM）、Legendre 伪谱法［5］（LPM）、Radau 伪谱法［6］（RPM）和Chebyshev 伪谱法［7］（CPM）。文献［8］改进了Gauss 伪谱法，定义两种新的断点，将密集的高斯点定义到指定区域附近，使所有位置均可满足复杂约束的要求。文献［9］综合考虑了无人作战飞机气动力特性、大气环境特性等构建了无人作战飞机四维攻击轨迹规划理论框架模型，采用RPM 进行转化，利用SNOPT 软件包进行求解。文献［10］考虑无人机集群飞行过程中的约束条件，建立了集群飞行路径规划的数学模型，采用Rauda伪谱法进行求解，分别进行了无人机群集结任务仿真和编队飞行任务仿真。

尽管基于伪谱法的轨迹规划有诸多研究成果，但目前伪谱法仍然存在一些问题：1）大多文献均未考虑控制量的变化率约束［11-13］，而控制量的变化率过大可能使制导系统无法进行跟踪，造成实际规划的路径结果不可行。为了避免过度剧烈的控制量变化，控制量变化率的限制是非常重要的，这可以确保控制系统在操作过程中更加平稳和可靠，同时也保证了系统的安全性和高效性。目前常用的处理形式是引入控制变量的导数作为虚拟控制量，将实际控制量转化为状态变量［14-16］。这种处理方式形式简单，只需增加与控制变量相同数量的状态方程即可达到控制量变化率约束的要求。但是这种处理方法增加了待优化变量的个数，进而增加了求解的困难度。同时增加的控制变量微分方程为线性微分方程，可能会产生奇异弧引起虚拟控制变量振荡甚至发散。为此文献［17］引入有限差分法将控制量变化率约束转化为相邻离散点处控制量的线性约束，避免了虚拟控制量振荡问题。但是有限差分法的精度受到区间分辨率的限制，对于Legendre 伪谱法中使用的Legendre-Gauss-Lobatto配点（LGL配点）两端紧密，中间稀疏的分布，可能会造成较大误差。2）多导飞行器协同轨迹规划问题约束更加复杂，使用伪谱法求解时较差的初值可能造成计算时间长、迭代次数增加甚至结果无法收敛。文献［18-20］采用群智能算法进行初始值猜测，作为伪谱法求解的初始参考解，以提升解的最优性和收敛速度。但是文献［18］采用群智能算法时并未考虑动力学方程，只给出了在位置状态量初值，无法求解出全部的状态量及控制量初值。文献［19-20］的方法在使用群智能算法求解初值时需要进一步求解动力学方程，因此会额外增加大量计算量。

针对上述问题，本文提出了一种双层亚音速飞行器协同轨迹规划器。为了保证轨迹的可行性，基于Lagrange 插值函数计算控制量的导数作为约束项，避免引入新的状态方程造成计算复杂度增加。使用顶层单飞行器规划器在不考虑多飞行器协同约束的条件下使用较少的配点快速得到每个飞行器的粗略轨迹结果，将求解结果进行拉格朗日插值作为底层多飞行器规划器的初值，以解决伪谱法初值敏感问题并提高优化效率。在底层多飞行器规划器使用更多的配点以确保伪谱法求解的精度，在考虑多飞行器协同约束条件下进行伪谱法求解，实现多飞行器编队飞行协同规划，提高算法效率。最后通过仿真实验验证了算法的有效性。

1 编队轨迹规划问题建模

1.1 亚音速飞行器动力学模型

亚音速飞行器模型涉及自身姿态、空气阻力、发动机推力等多个因素，并且是一个强耦合的复杂非线性系统。而亚音速飞行器动力学特性并非本文研究重点，因此为了简化研究，将亚音速飞行器看作一个无延迟的理想模型，并将其看作一个质点。亚音速飞行器三维动力学模型如下［21］

式中：第i个亚音速飞行器状态记为Xi=[xi，yi，zi，Vi，γi，ψi]T；N表示飞行器数量；[xi，yi，zi]T表示飞行器在三维空间下的坐标；Vi表示飞行器的速度大小；γi和ψi分别代表飞行器的航迹角和航向角；m代表飞行器的质量；g代表重力加速度取常值9.8 m/s2；ayi和azi分别代表水平横向加速度垂直纵向加速度，即为飞行器的控制量记为Ui=［azi，ay］iT；Di代表飞行阻力，计算公式如下

式中：ρ代表空气密度，其计算方式为ρ=ρ0exp (-zi／β)，ρ0=1.225 kg/m3，β=7 100 m，zi为飞行器高度；S代表飞行器参考面积；Cd代表阻力系数。

1.2 约束条件

1）状态量和控制量约束

对于状态量的约束主要包含飞行中的过程约束和终端约束，过程约束为飞行器状态量的阈值，可表示为

式 中：xmin，xmax，ymin，ymax，zmin，zmax，Vmin，Vmax，γmin，γmax，ψmin，ψmax分别代表各状态量的边界值。

第i个飞行器终端约束为在初始时刻t0，i和末端时刻tf，i的状态量，即

式中：X（t0，）i表示飞行器在初始时刻的状态向量，X0，i表示每个飞行器的起始状态值约束向量。X（tf，）i表示飞行器在终端时刻的状态向量，Xf，i表示第i个飞行器的终端时刻的状态值约束向量。

控制量约束为受限于飞行器的机动性，表示为

式中：aymax和azmax为大于0 的常量，决定了飞行器的机动性。

同时考虑轨迹可行性增加控制量变化率约束

式中：uymax和uzmax为大于0的常量。

2）威胁约束

在亚音速飞行器攻击目标过程中可能遇到敌方火力威胁或由于天气原因带来的不可行区域，在飞行器飞行过程中需要尽可能避开这些威胁区域以确保攻击任务的成功。为了简化处理，将其空间区域分为可行区域与禁飞区，并将禁飞区建模为无限高的圆柱体。假设M个圆柱体中心坐标为（xjF，yjF），半径为Rj，j=1，2，…，M，则威胁约束表示为

式中：（xi，y）i为第i个飞行器轨迹的水平面坐标，RF为飞行器到威胁区域边界的最小安全距离。

3）编队约束

针对多飞行器协同规划问题，若要求到达目标点时刻一致，则飞行器协同时间约束为

为了描述飞行器编队队形，设定相邻飞行器之间的横向距离和纵向距离之间保持在一定的范围内，队形约束表示为［9］

式中：xij，min，yij，min，zij，min，xij，max，yij，max，zij，max为相邻飞行器之间的距离阈值。

1.3 目标函数

根据亚音速飞行器执行任务的不同，多飞行器轨迹规划可以选择不同的性能指标。当确定飞行器起始位置和目标位置后，可以选择飞行时间最短、飞行路径最短、能量消耗最少等。不失一般性，本文选择飞行器路径长度和最短作为性能指标

1.4 最优控制问题

因此多飞行器轨迹规划问题可以建模为如下Bolza形式目标函数的经典最优控制问题

式中：Φ表示终端性能值；l表示系统动态性能值。

最优控制问题的目标是确定控制量x（t）和状态量u（t），最小化式（11）目标函数，同时满足以下动力学方程约束

式中：f（x（t），u（t），t）为形如式（1）的动力学方程。

飞行器的终端约束、时间协同约束等等式约束统一表示为边界约束

飞行器飞行中的过程约束、威胁约束、队形约束等不等式约束统一表示为路径约束，即

2 双层亚音速飞行器协同轨迹规划器

2.1 引入控制量变化率约束的Legendre伪谱法

Legendre 伪谱法的基本原理是将最优控制问题在一系列LGL 配点处离散化，然后通过拉格朗日插值多项式逼近连续控制和状态，将其转化为非线性规划问题（NLP）。

因为轨迹优化问题是在时间区间［t0，t］f上定义的，但是LGL 点落在［-1，1］范围内，所以自变量应该映射到一般区间τ∈［-1，1］，进行下式转换

配点τk（k=1，2，…，n）是K阶LGL 点，是多项式(1 -τ2)(τ)的根，即n-1阶Legendre多项式的导数和点-1，1。

使用Lagrange插值多项式近似状态变量x（τ）和控制变量u（τ）

对式（16）中状态变量的近似表达式求导得到

每个拉格朗日多项式在LGL 点处的导数可以定义为如下微分矩阵

因此将动力学微分方程转换为

控制量变化率约束对于轨迹可行性具有重要作用。通常为了满足控制量变化率约束，需要引入新的状态方程和虚拟控制量，即

式中：βzi和βyi表示控制量的变化率即引入的虚拟控制量。

将式（20）与式（1）共同作为亚音速飞行器动力学方程约束以进行求解，这种方法会增加问题的求解难度和计算复杂度。

为了保证飞行路径的可行性并保证优化效率，本文改进控制量变化率约束。如式（16）伪谱法使用Lagrange 插值多项式近似控制变量，因此对控制量的近似表达求导可以快速得到控制量的导数。由控制量近似式计算得到控制量变化率为

因此控制量变化率约束表示为

式中：umax为控制量变化率的阈值。

通过引入控制量变化率不等式约束式（22），避免了传统方法中引入新的状态方程和虚拟控制量所带来的计算复杂度增加问题，并在满足控制量变化率约束的条件下实现了优化过程。由于在确定配点数后微分矩阵Dki为常量矩阵，因此式（22）将控制量变化率的约束转化为控制量离散形式的线性约束，对控制量的一阶偏导数为0，在求解过程中无需反复计算偏导数，减小计算量。

边界约束转换为

路径约束转换为

因此目标函数转换为

2.2 双层协同轨迹规划器

双层协同轨迹规划器分为顶层单飞行器快速规划器与底层多飞行器协同规划器。顶层单飞行器快速规划器在不考虑多飞行器协同约束的条件下使用较少的配点快速得到每个飞行器的粗略轨迹结果，将求解结果进行拉格朗日插值作为底层多飞行器规划器的初值，底层多飞行器协同规划器在考虑多飞行器协同约束条件下使用更多的配点进行伪谱法求解。具体流程如图1所示。

2.2.1 顶层单飞行器快速规划器

顶层单飞行器快速规划器为每个飞行器分别单独求解规划轨迹，顶层规划器不考虑编队约束，仅考虑飞行器动力学方程（1）、状态量和控制量约束式（3）～（6）以及威胁约束式（7）。

单飞行器快速规划器采用少量的LGL 配点，设置迭代次数较少，可以有效地减少计算量，提高计算效率。由于单飞行器约束项较少，因此采用线性插值方法进行伪谱法求解初始化，其主要特点是快速求解，可以快速得到一条基本满足约束条件的路径，但是由于配点数较少，规划的航路可能可行性较差，造成航路点精度差、除配点外的其它航路点不满足威胁要求、控制量不满足阈值要求等问题［8］。但是顶层单飞行器快速规划器是以较少的计算量快速得到每一个飞行器的粗略轨迹，为底层多飞行器协同规划器求解提供初值参考，优化底层多飞行器协同规划器的计算效率。飞行器之间的编队协同通过底层多飞行器协同规划器进一步优化求解。

2.2.2 底层多飞行器协同规划器

底层多飞行器协同规划器在单飞行器规划的基础上进行精细化协同规划。底层多飞行器协同规划器除了考虑飞行器的动力学方程式（1）、状态量和控制量约束式（3）～（6）、威胁约束式（7）外，重点考虑多飞行器间编队约束式（8）～（9）。由于底层多飞行器协同规划器将所有飞行器进行集中式求解，求解变量较多，求解规模较大，因此底层多飞行器协同规划器对于初值选取要求更高。通过顶层单飞行器快速规划器得到的求解结果可以为底层多飞行器协同规划器提供一个较好的初值参考，提升NLP的求解效率。

设由单飞行器快速规划器求解分别得到的各飞行器状态量和控制量分别为Xsi，Usi（i=1，2，…，N）。在多飞行器协同规划器增加配点数量以增加轨迹规划的精度，解决单飞行器快速规划器出现的精度差等问题。若多飞行器规划器LGL 配点数为n2，根据单飞行器规划器的求解结果分别对各飞行器状态量和控制量进行Lagrange插值：

将插值后的结果xmi，umi（i=1，2，…，N）作为多飞行器协同规划器的初值进行伪谱法求解，可以更好地解决伪谱法对初值敏感的问题，提高多飞行器协同轨迹规划的效率。在此基础上，结合飞行器之间的编队约束，确保实现共同攻击任务。

2.2.3 算法流程

双层亚音速飞行器协同轨迹规划器算法步骤如下：

①根据单飞行器轨迹规划问题约束式（1）～（7）、目标函数式（10）将问题建模为式（11）～（14）的公式化表述。

②设定顶层单飞行器快速规划器LGL 配点数n1，设定求解迭代次数I1。

③根据飞行器初始时刻状态值X（t0，）i和终端时刻状态值X（tf，）i采用线性插值作为顶层单飞行器快速规划器求解的初值。

④根据式（16）离散每一个飞行器的状态变量和控制变量，通过LPM 将问题转化为式（19）、式（23）～（25）的NLP问题，使用SQP方法求解。

⑤根据多飞行器轨迹规划问题约束式（1）～（9）、目标函数式（10）将问题建模为式（11）～（14）的公式化表述。

⑥设定底层多飞行器协同规划器LGL 配点数n2，设定求解迭代次数I2。

⑦将顶层单飞行器快速规划器求解结果Xsi，Usi根据式（26）进行n2个LGL点的Lagrange插值得到插值结果xmi，umi。

⑧将插值结果xmi，umi作为底层多飞行器协同规划器的初值，通过LPM 将问题转化为式（19）、式（23）～（25）的NLP 问题，使用SQP 方法求解得到最终轨迹结果。

3 仿真结果与分析

针对提出的双层亚音速飞行器协同轨迹规划器，进行单飞行器轨迹规划实验和多飞行器协同轨迹规划实验。

表1为仿真实验亚音速飞行器所需参数［21］。仿真场景设定参数如表2所示。

表1 亚音速飞行器参数Table 1 Parameters of subsonic vehicles

表2 实验场景参数Table 2 Parameters of experimental scene

3.1 单飞行器轨迹规划实验

为了验证控制量变化率约束改进的有效性和可行性，使用表2 中飞行器1 参数进行单亚音速飞行器轨迹规划仿真实验。对于控制量变化率约束的处理方式对比文献［17］采用的有限差分法和文献［16］采用的增加控制量变化率方程方法。所有方法配点数设置为21，迭代次数设置为200。

各方法得到飞行器的三维轨迹曲线如图2 所示，控制量变化曲线如图3 所示。文献［16］增加控制量约束方程可以实现对于控制量变化率约束的要求，控制量的变化率得到约束。但是增加新的约束方程带来了复杂度的增加，因此增加了计算的困难度。以亚音速飞行器约束方程为例，共6 个变量和2个控制变量，增加控制量变化率方程后变为8个状态变量与2 个控制变量，理论上会增加25%的复杂度与计算量。对于伪谱法实际计算，所有的变量都需要SQP 算法迭代计算求解，变量的震荡会导致NLP收敛变慢，由此会造成计算更加耗时，降低算法的可行性。文献［17］引入的有限差分法与本文方法不需要引入虚拟控制变量，避免了计算复杂度的增加，减小计算耗时。三种方法各自重复10次得到算法耗时平均值，文献［16］方法为5.890 2 s，文献［17］方法为4.386 1 s，本文方法为3.716 1 s。

图2 三维轨迹结果Fig.2 3D trajectory results

图3 控制量变化曲线对比Fig.3 Comparison of control variable change curves

根据实验结果可知，增加控制量变化率方程的方法明显增加了计算耗时。而本文方法相较于文献［17］的方法进一步提升了算法效率。对于有限差分法受限于区间分辨率，由于LGL 配点分布并非等间距分布，因此有限差分法与伪谱法结合存在一定的局限性，造成了一定的优化困难。本文算法通过Lagrange多项式的计算得到控制量变化率作为约束条件，契合伪谱法理论推导，对控制量变化率约束的同时保证了算法效率。由实验结果可知，本文方法较文献［17］的有限差分法计算得到的控制量变化曲线更加平滑，同时有更优的计算速度。

3.2 多飞行器编队协同轨迹规划仿真实验

在单飞行器情况下约束较少，优化变量较少，改进的控制量变化率约束方法和无控制量变化率约束的传统伪谱法优化效率大体相当。通过多飞行器协同轨迹规划仿真实验进一步体现本文双层飞行器协同轨迹规划器在求解性能上的提升。多飞行器仿真实验飞行器参数如表1 所示，实验场景环境设定与各飞行器初始状态如表2所示。文献［16］增加控制量变化率方程方法配点数设置为19，迭代次数设置为200；本文所提双层规划器顶层规划器配点数设置为9，迭代次数20次，底层规划器配点数为19，迭代次数200次。

求解结果如图4 和图5 所示。实验结果表明，两种方法均可获得规划结果。在控制量限制阈值为50 的条件下，增加控制量变化率方程后，控制量的变化曲线表明横向加速度在10 s左右时控制量明显超过阈值。这是由于拉格朗日插值法在两端点较稠密，而在中间段较稀疏，插值误差会导致控制量超过阈值。另外，增加控制量变化率方程会增加计算的复杂度，并且缺乏良好的初始值的情况下，导致优化困难。本文方法中顶层规划器可以提供底层规划器所需的优化初始值，从而更好地控制优化过程。其次，控制量变化率的约束可以进一步限制控制量的震荡，避免控制量的过度波动。

图4 三维轨迹结果Fig.4 3D trajectory results

图5 控制量变化曲线对比Fig.5 Comparison of control variable change curves

算法运行时间统计如表3 所示。可以看出，增加控制量变化率方程方法的消耗时间是本文方法的8.61 倍。其主要原因是多飞行器情况下约束条件更复杂，待优化变量显著增加，双层规划器结构结合对控制量变化率约束的改进，避免增加新的约束方程和新的待优化变量，使算法更加高效。顶层规划器得到的结果尽管精度较低，可行性较差，但是其求解迅速，再通过底层规划器的进一步优化求解，保证了求解的精度，同时提高了规划的效率。因此，本文方法可以有效地解决轨迹规划问题中的初值敏感和高维度优化问题，并且具有更好的求解效率和精度。

表3 算法耗时Table 3 Algorithm time consumption

4 结论

本文提出了一种双层亚音速飞行器协同轨迹规划求解器，解决了多飞行器编队轨迹规划问题中的约束复杂和求解困难的问题。该方法首先在传统伪谱法的基础上，通过基于Lagrange 插值多项式的方法计算控制量变化率，从而避免了引入虚拟控制变量和增加状态方程所带来的计算复杂度增加问题。然后，采用双层规划器，有效解决了伪谱法初值敏感问题，提高了算法的求解效率。通过仿真实验的结果表明，相比于增加控制量变化率方程的方法，本文提出的双层亚音速飞行器协同轨迹规划求解器在多飞行器编队飞行的轨迹规划问题中具有更高的优化效率。