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

第 45 卷第 1 期 2024 年 1 月宇 航 学 报Journal of AstronauticsNo.12024JanuaryVol.45引入控制量变化率约束的双层协同轨迹规划器温岳1，冯肖雪1，谢天1，龚柏春2，吴云华2（1.北京理工大学自动化学院，北京 100081；2.南京航空航天大学航天学院，南京 210016）摘要：针对亚音速飞行器编队飞行轨迹规划约束复杂、求解困难的问题，提出了一种引入控制量变化率约束的双层亚音速飞行器协同轨迹规划器。首先，引入控制量变化率约束以保证轨迹可行性，为了避免增加新的状态方程而增加计算量，采用拉格朗日插值多项式计算控制变量变化率，将其作为求解最优控制问题伪谱法的约束条件。然后，使用顶层单飞行器规划器在不考虑多飞行器协同约束的条件下使用较少配点的伪谱法快速求解每个飞行器的粗略轨迹结果，求解结果使用拉格朗日插值作为底层多飞行器规划器的初值。底层多飞行器规划器使用更多的配点以确保求解的精度，在考虑多飞行器协同约束条件下进行伪谱法求解，以解决伪谱法初值敏感问题并提高优化效率。最后仿真结果表明了所提方法的有效性，与引入虚拟控制量而增加状态方程方法相比，此方法计算量更小，求解效率更优，满足协同轨迹规划的要求。关键词：编队飞行；轨迹规划；变化率约束；双层规划器中图分类号：V249 文献标识码：A 文章编号：1000-1328（2024）01-0101-09 DOI：10.3873/j.issn.1000-1328.2024.01.011Two-layer Cooperative Trajectory Planner with Constraint on the Change Rate of Control VariableWEN Yue1，FENG Xiaoxue1，XIE Tian1，GONG Baichun2，WU Yunhua2（1.School of Automation，Beijing Institute of Technology，Beijing 100081，China；2.College of Astronautics，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China）Abstract:Aiming at the problems of complex constraints and difficulty in solving for subsonic vehicles formation flight trajectory planning，a two-layer subsonic vehicles cooperative trajectory planner based on pseudo-spectral method is proposed.Firstly，the constraint on the change rate of the control variable is introduced to ensure the feasibility of the trajectory，to avoid adding new state equations and increasing the computational cost，the Lagrange interpolation polynomial is used to calculate the change rate of the control variable，and it is used as the constraint of the pseudo-spectral method to solving optimal control problem.Then，top-level single-vehicle planner is used to quickly solve the rough trajectory of each vehicle by using the pseudo-spectral method with fewer collocation points without considering the multi-vehicles coordination constraints，and the solution result uses Lagrange interpolation as the initial value of the bottom-level multi-vehicles planner.The bottom-level multi-vehicles planner uses more collocation points to ensure the accuracy of the solution，and solves under the consideration of multi-vehicles coordination constraints，to overcome the initial value sensitivity problem of the pseudo-spectral method and improve the optimization efficiency.Finally，the simulation results show the effectiveness of the proposed method.Compared with the method of introducing virtual control variables and increasing the state equations，the method proposed has less computational cost and better solving efficiency，and meets the requirements of cooperative trajectory planning.Key words:Formation flight；Trajectory planning；Rate of change constraint；Two-layer planner收稿日期：2023-04-26；修回日期：2023-08-25基金项目：国家自然科学基金（62261160575，61991414，61973036）；国防科技173计划技术领域基金（20220601053，20220601030）宇航学报第 45 卷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 插值函数计算控制量的导数作为约束项，避免引入新的状态方程造成计算复杂度增加。使用顶层单飞行器规划器在不考虑多飞行器协同约束的条件下使用较少的配点快速得到每个飞行器的粗略轨迹结果，将求解结果进行拉格朗日插值作为底层多飞行器规划器的初值，以解决伪谱法初值敏感问题并提高优化效率。在底层多飞行器规划器使用更多的配点以确保伪谱法求解的精度，在考虑多飞行器协同约束条件下进行伪谱法求解，实现多飞行器编队飞行协同规划，提高算法效率。最后通过仿真实验验证了算法的有效性。102第 1 期温岳等：引入控制量变化率约束的双层协同轨迹规划器1编队轨迹规划问题建模1.1亚音速飞行器动力学模型亚音速飞行器模型涉及自身姿态、空气阻力、发动机推力等多个因素，并且是一个强耦合的复杂非线性系统。而亚音速飞行器动力学特性并非本文研究重点，因此为了简化研究，将亚音速飞行器看作一个无延迟的理想模型，并将其看作一个质点。亚音速飞行器三维动力学模型如下21xi=Vicosicosiyi=Visinicosizi=VisiniVi=-Dim-gsini i=-azi-gcosiVi=ayiVicosi ，i=1，2，N（1）式中：第i个亚音速飞行器状态记为Xi=xi，yi，zi，Vi，i，iT；N 表示飞行器数量；xi，yi，ziT表示飞行器在三维空间下的坐标；Vi表示飞行器的速度大小；i和i分别代表飞行器的航迹角和航向角；m代表飞行器的质量；g代表重力加速度取常值9.8 m/s2；ayi和azi分别代表水平横向加速度垂直纵向加速度，即为飞行器的控制量记为Ui=azi，ayiT；Di代表飞行阻力，计算公式如下Di=V2iSCd2（2）式中：代表空气密度，其计算方式为=0 exp(zi)，0=1.225 kg/m3，=7 100 m，zi为飞行器高度；S代表飞行器参考面积；Cd代表阻力系数。1.2约束条件1）状态量和控制量约束对于状态量的约束主要包含飞行中的过程约束和终端约束，过程约束为飞行器状态量的阈值，可表示为xmin xi xmaxymin yi ymaxzmin zi zmaxVmin Vi Vmaxmin i maxmin i max，i=1，2，N（3）式中：xmin，xmax，ymin，ymax，zmin，zmax，Vmin，Vmax，min，max，min，max分别代表各状态量的边界值。第i个飞行器终端约束为在初始时刻t0，i和末端时刻tf，i的状态量，即X(t0，i)=X0，iX(tf，i)=Xf，i，i=1，2，N（4）式中：X（t0，i）表示飞行器在初始时刻的状态向量，X0，i表示每个飞行器的起始状态值约束向量。X（tf，i）表示飞行器在终端时刻的状态向量，Xf，i表示第i个飞行器的终端时刻的状态值约束向量。控制量约束为受限于飞行器的机动性，表示为|ayi|aymax|azi|azmax ，i=1，2，N（5）式中：aymax和azmax为大于0的常量，决定了飞行器的机动性。同时考虑轨迹可行性增加控制量变化率约束|ayi|uymax|azi|uzmax ，i=1，2，N（6）式中：uymax和uzmax为大于0的常量。2）威胁约束在亚音速飞行器攻击目标过程中可能遇到敌方火力威胁或由于天气原因带来的不可行区域，在飞行器飞行过程中需要尽可能避开这些威胁区域以确保攻击任务的成功。为了简化处理，将其空间区域分为可行区域与禁飞区，并将禁飞区建模为无限高的圆柱体。假设 M个圆柱体中心坐标为（xjF，yjF），半径为Rj，j=1，2，M，则威胁约束表示为 ()xi-xjF2+()yi-yjF2 Rj+RFi=1，2，.，N，j=1，2，M（7）式中：（xi，yi）为第i个飞行器轨迹的水平面坐标，RF为飞行器到威胁区域边界的最小安全距离。3）编队约束针对多飞行器协同规划问题，若要求到达目标点时刻一致，则飞行器协同时间约束为tf，1=tf，2=tf，i=tf，N（8）为了描述飞行器编队队形，设定相邻飞行器之间的横向距离和纵向距离之间保持在一定的范围内，队形约束表示为9 xij，min xi-xj xij，maxyij，min yi-yj yij，maxzij，min zi-zj zij，max ，i=2，N，j=i-1（9）式中：xij，min，yij，min，zij，min，xij，max，yij，max，zij，max为相邻飞行器之间的距离阈值。103宇航学报第 45 卷1.3目标函数根据亚音速飞行器执行任务的不同，多飞行器轨迹规划可以选择不同的性能指标。当确定飞行器起始位置和目标位置后，可以选择飞行时间最短、飞行路径最短、能量消耗最少等。不失一般性，本文选择飞行器路径长度和最短作为性能指标J=mini=1Nt0tf，iVidt（10）1.4最优控制问题因此多飞行器轨迹规划问题可以建模为如下Bolza形式目标函数的经典最优控制问题J=x(tf)，tf+t0tfl(x(t)，u(t)，t)dt（11）式中：表示终端性能值；l表示系统动态性能值。最优控制问题的目标是确定控制量x（t）和状态量u（t），最小化式（11）目标函数，同时满足以下动力学方程约束x(t)=f(x(t)，u(t)，t)，t t0，tf（12）式中：f（x（t），u（t），t）为形如式（1）的动力学方程。飞行器的终端约束、时间协同约束等等式约束统一表示为边界约束 x(t0)，t0，x(tf)，tf=0（13）飞行器飞行中的过程约束、威胁约束、队形约束等不等式约束统一表示为路径约束，即C x(t)，u(t)，t 0（14）2双层亚音速飞行器协同轨迹规划器2.1引入控制量变化率约束的Legendre伪谱法Legendre伪谱法的基本原理是将最优控制问题在一系列LGL配点处离散化，然后通过拉格朗日插值多项式逼近连续控制和状态，将其转化为非线性规划问题（NLP）。因为轨迹优化问题是在时间区间 t0，tf 上定义的，但是LGL点落在 1，1 范围内，所以自变量应该映射到一般区间 1，1，进行下式转换=2ttf-t0-tf+t0tf-t0（15）配点k（k=1，2，n）是K阶LGL点，是多项式(1-2)Pn-1()的根，即n-1阶Legendre多项式的导数和点-1，1。使用Lagrange插值多项式近似状态变量x（）和控制变量u（）x()X()=i=1nX()iLi()u()U()=i=1nX()iLi()（16）式中：Li()=j=0，j in-ji-j ，i=0，1，n。对式（16）中状态变量的近似表达式求导得到x(i)X(i)=i=1nX()iLi()=i=1nX()iDki（17）每个拉格朗日多项式在 LGL点处的导数可以定义为如下微分矩阵Dki=Li(k)=l=1Nj=1，j i，ln()k-jj=1，j in()i-j（18）因此将动力学微分方程转换为i=1nX(i)Dki-tf-t02f(X(k)，U(k)，k)=0（19）控制量变化率约束对于轨迹可行性具有重要作用。通常为了满足控制量变化率约束，需要引入新的状态方程和虚拟控制量，即azi=ziayi=yi ，i=1，2，N（20）式中：zi和yi表示控制量的变化率即引入的虚拟控制量。将式（20）与式（1）共同作为亚音速飞行器动力学方程约束以进行求解，这种方法会增加问题的求解难度和计算复杂度。为了保证飞行路径的可行性并保证优化效率，本文改进控制量变化率约束。如式（16）伪谱法使用Lagrange插值多项式近似控制变量，因此对控制量的近似表达求导可以快速得到控制量的导数。由控制量近似式计算得到控制量变化率为u (i)U(i)=i=1nU()iLi()=i=1nU()iDki（21）因此控制量变化率约束表示为i=1n1U(i)Dki umax（22）式中：umax为控制量变化率的阈值。通过引入控制量变化率不等式约束式（22），避免了传统方法中引入新的状态方程和虚拟控制量所带来的计算复杂度增加问题，并在满足控制量变化率约束的条件下实现了优化过程。由于在确定104第 1 期温岳等：引入控制量变化率约束的双层协同轨迹规划器配点数后微分矩阵Dki为常量矩阵，因此式（22）将控制量变化率的约束转化为控制量离散形式的线性约束，对控制量的一阶偏导数为0，在求解过程中无需反复计算偏导数，减小计算量。边界约束转换为 x(0)，0，x(f)，f=0（23）路径约束转换为C x(k)，u(k)，k 0 ，k=1，2，n（24）因此目标函数转换为J=x(1)，tf+tf-t02k=1nkg x(k)，u(k)，k（25）式中：k=-11Li()d为积分权重。2.2双层协同轨迹规划器双层协同轨迹规划器分为顶层单飞行器快速规划器与底层多飞行器协同规划器。顶层单飞行器快速规划器在不考虑多飞行器协同约束的条件下使用较少的配点快速得到每个飞行器的粗略轨迹结果，将求解结果进行拉格朗日插值作为底层多飞行器规划器的初值，底层多飞行器协同规划器在考虑多飞行器协同约束条件下使用更多的配点进行伪谱法求解。具体流程如图1所示。2.2.1顶层单飞行器快速规划器顶层单飞行器快速规划器为每个飞行器分别单独求解规划轨迹，顶层规划器不考虑编队约束，仅考虑飞行器动力学方程（1）、状态量和控制量约束式（3）（6）以及威胁约束式（7）。单飞行器快速规划器采用少量的LGL配点，设置迭代次数较少，可以有效地减少计算量，提高计算效率。由于单飞行器约束项较少，因此采用线性单飞行器航迹规划问题建模多飞行器协同航迹规划问题建模动力学方程约束状态量和控制量约束威胁约束编队约束威胁约束.多飞行器协同航迹规划结果多飞行器协同航迹规划伪谱法的初值单飞行器航迹规划问题建模单飞行器航迹规划问题建模.单飞行器快速规划层飞行器i飞行器1动力学方程约束状态量和控制量约束威胁约束动力学方程约束状态量和控制量约束威胁约束动力学方程约束状态量和控制量约束设定配点数n1的LGL配点，设定SQP迭代次数I1LPM问题转化SQP求解飞行器1结果LPM问题转化SQP求解LPM问题转化SQP求解飞行器i结果LPM问题转化SQP求解飞行器N结果设定配点数n1的LGL配点，设定SQP迭代次数I1设定配点数n1的LGL配点，设定SQP迭代次数I1飞行器N设定配点数n2的LGL配点lagrange插值多飞行器协同规划层飞行器1飞行器i飞行器NLGL配点数为n2，设定迭代次数I2图1双层亚音速飞行器协同轨迹规划器流程图Fig.1Flow chart of Two-layer subsonic vehicles Cooperative Trajectory Planner105宇航学报第 45 卷插值方法进行伪谱法求解初始化，其主要特点是快速求解，可以快速得到一条基本满足约束条件的路径，但是由于配点数较少，规划的航路可能可行性较差，造成航路点精度差、除配点外的其它航路点不满足威胁要求、控制量不满足阈值要求等问题8。但是顶层单飞行器快速规划器是以较少的计算量快速得到每一个飞行器的粗略轨迹，为底层多飞行器协同规划器求解提供初值参考，优化底层多飞行器协同规划器的计算效率。飞行器之间的编队协同通过底层多飞行器协同规划器进一步优化求解。2.2.2底层多飞行器协同规划器底层多飞行器协同规划器在单飞行器规划的基础上进行精细化协同规划。底层多飞行器协同规划器除了考虑飞行器的动力学方程式（1）、状态量和控制量约束式（3）（6）、威胁约束式（7）外，重点考虑多飞行器间编队约束式（8）（9）。由于底层多飞行器协同规划器将所有飞行器进行集中式求解，求解变量较多，求解规模较大，因此底层多飞行器协同规划器对于初值选取要求更高。通过顶层单飞行器快速规划器得到的求解结果可以为底层多飞行器协同规划器提供一个较好的初值参考，提升NLP的求解效率。设由单飞行器快速规划器求解分别得到的各飞行器状态量和控制量分别为Xsi，Usi（i=1，2，N）。在多飞行器协同规划器增加配点数量以增加轨迹规划的精度，解决单飞行器快速规划器出现的精度差等问题。若多飞行器规划器LGL配点数为n2，根据单飞行器规划器的求解结果分别对各飞行器状态量和控制量进行Lagrange插值：xmi=i=1n2XsiLi()umi=i=1n2UsiLi()，i=1，2，N（26）将插值后的结果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所示。3.1单飞行器轨迹规划实验为了验证控制量变化率约束改进的有效性和可行性，使用表2中飞行器1参数进行单亚音速飞行器轨迹规划仿真实验。对于控制量变化率约束的处理方式对比文献 17 采用的有限差分法和文献 16 采用的增加控制量变化率方程方法。所有方法配点数设置为21，迭代次数设置为200。表1亚音速飞行器参数Table 1Parameters of subsonic vehicles参数质量/kg参考面积/m2阻力系数数值1500.032 40.016 9106第 1 期温岳等：引入控制量变化率约束的双层协同轨迹规划器各方法得到飞行器的三维轨迹曲线如图 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。根据实验结果可知，增加控制量变化率方程的方法明显增加了计算耗时。而本文方法相较于文献 17 的方法进一步提升了算法效率。对于有限差分法受限于区间分辨率，由于LGL配点分布并非等间距分布，因此有限差分法与伪谱法结合存在一定的局限性，造成了一定的优化困难。本文算法通过Lagrange多项式的计算得到控制量变化率作为约束条件，契合伪谱法理论推导，对控制量变化率约束的同时保证了算法效率。由实验结果可知，本文方法较文献 17 的有限差分法计算得到的控制量变化曲线更加平滑，同时有更优的计算速度。3.2多飞行器编队协同轨迹规划仿真实验在单飞行器情况下约束较少，优化变量较少，改进的控制量变化率约束方法和无控制量变化率约束的传统伪谱法优化效率大体相当。通过多飞行器协同轨迹规划仿真实验进一步体现本文双层飞行器协同轨迹规划器在求解性能上的提升。多飞行器仿真实验飞行器参数如表1所示，实验场景环境设定与各飞行器初始状态如表2所示。文献 16增加控制量变化率方程方法配点数设置为19，迭代次数设置为200；本文所提双层规划器顶层规划器配点数设置为9，迭代次数20次，底层规划器配点数为19，迭代次数200次。求解结果如图4和图5所示。实验结果表明，两种方法均可获得规划结果。在控制量限制阈值为50的条件下，增加控制量变化率方程后，控制量表2实验场景参数Table 2Parameters of experimental scene参数禁飞区1圆心/m禁飞区2圆心/m禁飞区3圆心/m禁飞区4圆心/m禁飞区半径R/m飞行器1初始位置/m飞行器2初始位置/m飞行器3初始位置/m所有飞行器初始速度/（ms-1）所有飞行器初始航迹角/rad所有飞行器初始航向角/rad所有飞行器终端位置/m数值（3 000，4 000）（1 500，3 500）（3 000，5 200）（1 500，4 700）300（5 000，5 000，1 000）（5 000，4 900，1 000）（5 000，4 800，1 000）3000（0，3 000，0）05001 0001 5002 000z/my/mx/m6 0006 0005 5005 0004 5004 0004 0003 5003 0002 50002 000文献17方法文献16方法本文方法图2三维轨迹结果Fig.23D trajectory results05101520时间/s20151050510纵向加速度/(ms2)(a)纵向加速度变化曲线时间/s(b)横向加速度变化曲线051015205051015252030横向加速度/(ms2)文献17方法文献16方法本文方法文献17方法文献16方法本文方法图3控制量变化曲线对比Fig.3Comparison of control variable change curves107宇航学报第 45 卷的变化曲线表明横向加速度在10 s左右时控制量明显超过阈值。这是由于拉格朗日插值法在两端点较稠密，而在中间段较稀疏，插值误差会导致控制量超过阈值。另外，增加控制量变化率方程会增加计算的复杂度，并且缺乏良好的初始值的情况下，导致优化困难。本文方法中顶层规划器可以提供底层规划器所需的优化初始值，从而更好地控制优化过程。其次，控制量变化率的约束可以进一步限制控制量的震荡，避免控制量的过度波动。算法运行时间统计如表3所示。可以看出，增加控制量变化率方程方法的消耗时间是本文方法的8.61倍。其主要原因是多飞行器情况下约束条件更复杂，待优化变量显著增加，双层规划器结构结合对控制量变化率约束的改进，避免增加新的约束方程和新的待优化变量，使算法更加高效。顶层规划器得到的结果尽管精度较低，可行性较差，但是其求解迅速，再通过底层规划器的进一步优化求解，保证了求解的精度，同时提高了规划的效率。因此，本文方法可以有效地解决轨迹规划问题中的初值敏感和高维度优化问题，并且具有更好的求解效率和精度。4结论本文提出了一种双层亚音速飞行器协同轨迹规划求解器，解决了多飞行器编队轨迹规划问题中的约束复杂和求解困难的问题。该方法首先在传统伪谱法的基础上，通过基于Lagrange插值多项式的方法计算控制量变化率，从而避免了引入虚拟控制变量和增加状态方程所带来的计算复杂度增加问题。然后，采用双层规划器，有效解决了伪谱法初值敏感问题，提高了算法的求解效率。通过仿真实验的结果表明，相比于增加控制量变化率方程的方法，本文提出的双层亚音速飞行器协同轨迹规划求解器在多飞行器编队飞行的轨迹规划问题中具有更高的优化效率。参 考 文 献1 王璟慧，姜毅，杨昌志.亚音速飞行器往复式滑翔增程弹道设计 J.弹道学报，2022，34（3）：1-10.WANG Jinghui，JIANG Yi，YANG Changzhi.Design on reciprocating glide extended range trajectory for subsonic aircraftJ.Journal of Ballistics，2022，34（3）：1-10.2 白瑞光，孙鑫，陈秋双，等.基于Gauss伪谱法的多UAV协同航迹规划 J.宇航学报，2014，35（9）：1022-1029.BAI Ruiguang，SUN Xin，CHEN Qiushuang，et al.Multiple UAV cooperative trajectory planning based on Gauss pseudospectral method J.Journal of Astronautics，2014，35（9）：1022-1029.3 杨希祥，杨慧欣，王鹏.伪谱法及其在飞行器轨迹优化设计05001 0001 5002 000z/my/mx/m6 0006 0005 5005 0004 5004 0004 0003 5003 0002 50002 000飞行器1本文算法飞行器2本文算法飞行器3本文算法飞行器1文献16算法飞行器2文献16算法飞行器3文献16算法图4三维轨迹结果Fig.43D trajectory results06824101214161820时间/s6040503020010102030405060纵向加速度/(ms2)(a)纵向加速度变化曲线06824101214161820时间/s6040503020010102030405060横向加速度/(ms2)(b)横向加速度变化曲线飞行器1本文算法飞行器2本文算法飞行器3本文算法飞行器1文献16算法飞行器2文献16算法飞行器3文献16算法飞行器1本文算法飞行器2本文算法飞行器3本文算法飞行器1文献16算法飞行器2文献16算法飞行器3文献16算法图5控制量变化曲线对比Fig.5Comparison of control variable change curves表3算法耗时Table 3Algorithm time consumption方法增加控制量变化率方程16本文方法顶层规划时间/s/0.764 142底层规划时间/s415.204 72947.451 982108第 1 期温岳等：引入控制量变化率约束的双层协同轨迹规划器领域的应用综述 J.国防科技大学学报，2015，37（4）：1-8.YANG Xixiang，YANG Huixin，WANG Peng.Overview of pseudo-spectral method and its application in trajectory optimum design for flight vehiclesJ.Journal of National University of Defense Technology，2015，37（4）：1-8.4 ZHOU J Y，SUN R Q，YANG W X，et al.Dynamic multi-target trajectory planning of aircraft based on Gaussian pseudo-spectral methodC.International Seminar on Computer Science and Engineering Technology，Indianapolis，USA，January 8-9，2022.5 TIAN B L，ZONG Q.Optimal guidance for reentry vehicles based on indirect Legendre pseudospectral methodJ.Acta Astronautica，2011，68（7/8）：1176-1184.6 LIAO Y X，LI H F，BAO W M.Indirect Radau pseudospectral method for the receding horizon control problemJ.Chinese Journal of Aeronautics，2016，29（1）：215-227.7 WANG J Y，LIANG H Z，QI Z，et al.Mapped Chebyshev pseudospectral methods for optimal trajectory planning of differentially flat hypersonic vehicle systemsJ.Aerospace Science and Technology，2019，89：420-430.8 MAO Y F，ZHANG D L，WANG L.Reentry trajectory optimization for hypersonic vehicle based on improved Gauss pseudospectral method J.Soft Computing，2017，21（16）：4583-4592.9 粟建波，张立丰，张甲奇.基于Radau伪谱法的无人作战飞机四维轨迹规划 J.飞行力学，2020，38（1）：41-45，53.SU Jianb