采用联立法求解大姿态终端约束的上升段轨迹优化

邱 丰，宋征宇

(北京航天自动控制研究所，北京100854)

采用联立法求解大姿态终端约束的上升段轨迹优化

邱 丰，宋征宇

(北京航天自动控制研究所，北京100854)

针对恒定推力和未配置辅助动力系统的运载火箭，提出了一种基于联立框架的直接法(“联立法”)来求解带大姿态终端约束的动态在线轨迹规划问题，解决了传统迭代制导方法中飞行轨迹线性规划的不足，并可根据姿态跟踪精度的需求来控制程序角速率，从而同时满足各种约束条件。该联立法结合了直接配点法和伪谱法的特点，采用有限元正交配置法将状态变量和控制变量进行离散化;结果表明与伪谱法相比，计算效率得到了大幅提升。在此基础上，提出了通过合理选择初值、调整入轨精度偏差、自适应有限元分段等提升计算速度的措施，进一步提高了这一算法的实时性，为未来解决更复杂多样的制导任务提供了基础。

运载火箭;姿态终端约束;联立法;轨迹规划

0 引 言

近年来，自主控制的需求在航天应用中愈发增强。上升段飞行中，对于目标变更和推力下降等异常情况，自主规划的适应性较跟踪标准轨迹的摄动制导要高很多。自载人航天交会对接任务起，我国运载火箭首次采用了迭代制导方法［1］。这是一种在线动态自主轨迹规划的方法，但由于飞行轨迹的控制是通过调整姿态角来实现的，使得入轨姿态的散布很大。

解决入轨姿态可以从总体方案着手，例如在火箭的末级增加辅助动力系统(也称调姿系统)，或者依靠有效载荷自身进行姿态调整，但这些方案均额外增加了系统资源和复杂性。为此，文献［2］提出了一种预测修正的迭代制导改进方法。但是，对于一些较为特殊的入轨姿态角，现有迭代制导中线性规划的前提条件不再存在，导致入轨误差很大。对于此类问题，即使传统摄动制导也难以满足精度要求(这将在后文进行分析)。

本文提出了一种基于联立框架的直接法(下文简称“联立法”)，将入轨时间最短作为性能指标，通过控制推力方向变化使得火箭以要求姿态进入预定轨道。该方法采用统一化的处理流程，非常适合计算机自动处理，在自主制导领域有较好的应用前景。

1 带终端姿态约束的最优入轨问题

对于推力大小不可调节的运载火箭上升段实时轨迹优化问题，其本质上是带有状态约束和控制约束的最优控制问题，间接法和直接法是最常用的两种方法，但间接法对于末端有姿态角约束且入轨时间浮动的情况无法完成求解［3］。而直接法将连续空间最优控制问题转化为非线性规划(NLP)问题，利用数值方法求解。文献［4］利用伪谱法对固体运载火箭上升段轨迹优化进行研究，但计算耗时无法满足实时控制要求;文献［5］改善了计算效率，但收敛性还需进一步验证。目前研究成果表明［6－8］，直接法的实时性和收敛性问题并没有完全解决。

本文研究更为一般性的带终端姿态约束的精确入轨问题，采用有限元正交配置法将状态变量和控制变量进行离散化，实现对原命题的分段低阶逼近处理，使原问题转化为NLP问题，同时提出了提高计算效率的措施。

1.1 火箭质心运动模型

在不考虑气动力影响的条件下，建立其相对于发射惯性坐标系(以下简称发惯系)的质心运动方程如下［9］:

式中:x，y，z为火箭当前点到发射点矢径在发惯系上的投影，Vx，Vy，Vz为火箭速度矢量在发惯系上的投影，gx，gy，gz为引力加速度在发惯系上的投影，m0为飞行器初始质量，m·为秒耗量，P为推力，φ为俯仰角，ψ为偏航角。对于定推力发动机，秒耗量及推力设为定值。

由于状态变量之间数量级相差较大，为了便于数值求解，需要对运动方程进行单位归一化处理:

1.2 需要满足的约束条件

有效载荷的运行轨道由五个轨道根数决定(如果考虑其某一时刻在轨道上的具体位置，则需要六个轨道根数)［10］。只有所处位置在轨道上的一点，并且速度大小及方向恰好是轨道上该点所需速度的情况下，才能实现对五个轨道根数的精确控制。

带姿态约束的最优入轨问题中，不仅要控制火箭精确进入预定轨道，而且需满足一定的入轨姿态。此外，由于火箭燃料有限，为了确保得到的解是可行的，还需要加入最大入轨时间的约束。

综上所述，带姿态约束的火箭最优入轨问题中要满足的约束如表1所示。

表1 命题约束列表Table 1 Constraints list of the proposition

2 基于有限元正交配置的联立法

2.1 有限元正交配置法

假定整个问题的时间区间为［t0，tf］，将上升段入轨问题描述为一般动态优化命题形式［11］:

式中:Φ(x(tf))是目标函数，x(t)∈Rnx为状态变量组成的nx维向量;u(t)∈Rnu为控制变量组成的nu维向量;p∈Rnp是模型参数;f为微分方程组的右边约束;gE为代数方程组;微分代数方程组中的状态变量和控制变量的边界约束均归结到gI中; x0∈Rnx为状态变量的初始状态，N1(x(tf))为终端目标集约束。

传统伪谱法采用全局插值多项式拟合，为了保证逼近精度要求多项式具有较高阶次，离散化后NLP问题规模很大，导致了计算难度和求解时间的增加。为了在保证计算精度的同时降低离散化后命题的复杂程度，可将整个时域划分为N个区间，每个区间称为有限元，在每段有限元［ti－1，ti］上，采用正交多项式对状态变量x及控制变量u进行逼近，采用拉格朗日插值多项式可使变量在配置点上的值正好等于其系数。这种方法称之为有限元正交配置法［12］。

此外，在有限元端点上需通过连接方程来保证状态变量的连续性;同时应满足初始和终端约束条件。结合以上离散化后的条件，可以得到有限元正交配置法处理后的NLP问题。

2.2 联立法对问题的重构

以本问题为例，将整个时域［t0，tf］分为N个有限元，每个有限元内用K阶拉格朗日插值多项式对变量和约束进行逼近。采用Gauss-Radau点［13］作为配置点进行配置。

目标函数为入轨时间:

状态变量为火箭的速度、位置:

控制变量为火箭的姿态程序角:

在推力大小不变的情况下，定点入轨是无法保证的。因此在求解过程中将真近点角f作为代数变量进行处理，通过五个轨道根数及f，可以确定入轨点速度、位置，从而形成六个等式约束方程。

(1)过程约束

火箭质心运动的状态变量微分方程表示如下:

式中，i∈［0，N)，j∈［0，K)。

状态变量的连续性方程表示为(以x为例):

实际飞行中，还应加入姿态角及姿态角速率的不等式约束(以俯仰角为例，ω为角速率上限):

(2)终端约束

状态变量的初始约束和终端约束(以x为例):

目标入轨时间最大值的约束:

综合式(7)～(11)可得带终端姿态约束最优入轨问题离散化后的大规模非线性规划命题。

由式(9)和(11)可以看出该非线性规划命题中含有不等式约束，直接对其求解非常困难，且约束个数随着有限元个数N和插值阶数K增加而成倍增加，故采用内点算法［12］对命题进行求解。

至此，联立法的计算过程可归纳为图1所示，分为设计输入、离散处理和实时规划三部分。通过利用基于内点算法的NLP求解器迭代求解，将求得变量代入离散化后的命题得到约束残差再返回求解器。通过迭代计算使残差渐渐逼近零，求得的变量也越来越逼近最优解。在该计算过程中，终端姿态角约束被统一处理，并不区分姿态角的大小，体现了该方法的通用性。

2.3 联立法的性能分析

在［t0，tf］内将整个时域分成N段，联立法通过有限元将状态变量x(t)分别定义到多个时段内。在有限元［ti－1，ti］上，由式(7)得到的近似最优解xk+1(t)和真实解x(t)之间的误差边界可以通过下式来描述［13］:

式中:C为只与阶次K有关的常数，hi为第i个有限元的长度，为真实解K+1次导数在区间［ti－1，ti］上的无穷范数。因此，当有界时，可以得到:

也就是说当有限元的划分无限加密的时候，联立法得到的近似解无限逼近真实的解，故可以通过控制离散化精度来间接保证解的最优性。但工程实际应用中不能为了少量精度的提高而增加大量的有限元，在线规划时可根据实际用例选取合适的有限元，以达到求解精度和运算时间的平衡。

3 仿真校验

3.1 仿真场景

表2～表3以某火箭上升段飞行为例列出了仿真场景的各项参数。暂不考虑大气影响，即从火箭二级飞行段开始。表4为终端姿态要求，其中终端俯仰角为－110°，正是这一特殊约束，导致传统算法无法同时兼顾入轨精度和终端姿态的要求。

表2 初始参数Table 2 Initial parameters

表3 目标轨道参数Table 3 Target orbit parameters

表4 姿态角约束Table 4 Attitude constraints

平均取30段有限元和3阶Radau配点进行离散化，初值取法如下:

1)各配点速度、位置初值均设为初始点值;

2)真近点角变量的初值取为0°;

3)目标函数(总飞行时间)的初值取标准弹道入轨时间376.4 s;

4)由于俯仰角变化大，其初值采用等差选取的方法，而偏航角变化不大，初值取初始状态的值。

3.2 仿真结果与分析

本次数值实验是基于 IntelRCoreTMi5－3470 CPU处理器、3.2 GHz主频的计算机进行的，求解器为IPOPT(3.6.1版)［14］。由于决定运算速度的主要因素是处理器主频，所以不同硬件平台下的计算耗时可以根据CPU的主频按比例近似折算。

3.2.1 与标准弹道进行对比分析

图2给出了标准弹道设计与联立法计算结果的飞行程序角对比:对于终端俯仰角－110°的约束条件，相当于火箭向下飞行，这对运载能力是有损失的，因此标准弹道设计时将姿态调整段压缩在飞行的末断，时间要尽可能短，俯仰角速率最大达到9.35°/s，如图3所示。六自由度仿真结果表明，在该角速率条件下，姿态跟踪误差达到了2°，这反过来影响了入轨精度。这就是引言中所提采用摄动制导跟踪标准轨迹飞行也难以满足入轨精度的原因。

为此在联立法计算中将角速率约束定为4°/s，终端姿态和入轨精度均能满足要求，入轨时间为377.5 s，比标准弹道多 1.1 s(多消耗推进剂约118.6 kg，在安全余量范围内)，如图3所示。且轨迹可以实时动态调整，及时修正之前的误差，即误差不具有积累的效果。

从图4速度和位置曲线可以看出，该飞行轨迹尽管前后段均近似线性，但整体难以用线性规划来统一描述;而大角速率转弯飞行的起点是浮动的，无法提前获知，使得分段线性化处理也不可能，这是导致现有迭代制导无法满足该算例入轨精度要求的主要原因。

3.2.2 与伪谱法求解结果进行对比分析

在同一运算平台下，分别用全局30和300个Radau配置点的伪谱法对命题进行求解，得到的结果与30段3阶Radau配置点的联立法求解情况对比，如表5和图5所示。

表5 联立法与伪谱法结果对比Table 5 Comparison of the simultaneous method and the pseudo-spectral method

由图5可见，求解时间近似的情况下，联立法求解精度高于使用全局近似的伪谱法;而求解精度相似时，伪谱法的耗时远远超过联立法。因此联立法在求解效率上优于全局插值近似的伪谱法。

3.3 提升算法实时性的几种手段

3.3.1 选取合理的初值

由于每个控制周期均迭代计算，即使在一个周期内受到干扰等影响，实际飞行轨迹也不会偏离过大。因此可采用当前点速度位置作为起始点初值，上一次规划的相关参数作为其他状态及控制变量的初值，以缩短优化时间，这对实时飞行的迭代计算意义十分重大。

针对本算例，将所有变量的初值均设置为已有优化结果得到的最优值附近，假设偏差为3%，则各个变量的初值取为:

计算结果对比如表6所示。

表6 不同初值结果对比Table 6 Comparison of different initial value

由表6可以看出，该方法可以大大缩短运算时间;且两次计算之间的时间间隔越短，初值与最优解之间的偏差也越小。

3.3.2 选择合适的入轨点精度偏差

联立法是将轨道根数转化得到的速度、位置作为终端等式约束条件，即要求入轨点的速度、位置与理论值的差值为0。而实际应用中，不同任务对不同轨道根数的精度要求也不同;并且在合理的偏差范围内，即差值不为0也是可以接受的。因此可以对不同轨道根数区别处理以提升计算效率。

以本算例为例，在保持整体容差设置为1×10－8不变的情况下，将变量值与要求值的差除以10000，则相当于把六个约束放宽到1×10－4，结果对比如表7和表8所示。

表7 放宽误差约束后计算效率对比Table 7 Comparison of computational efficiency after the relaxation of error constraints

从表7中看出，放宽误差约束可以缩短优化时间，但对入轨精度的影响在可接受的范围之内，如表8所示。因此，未来可以根据不同算例的特点对约束进行区别调整，在不影响精度的情况下进一步提高实时性。

表8 放宽误差约束后入轨精度对比Table 8 Comparison of orbit injection accuracy after the relaxation of error constraints

3.3.3 自适应有限元分段

减少有限元的分段能有效减小离散后NLP命题的规模，但会降低解的精度和最优性。通过上述飞行曲线的观察可以发现，控制变量的大幅变化基本集中在初始和终端位置，中间很长的时间变化平缓。根据这一特点，在变量快速变化的区间内加密有限元划分，而在变化平缓的区域减少有限元分段，这种方法可称作自适应有限元分段法。

为此，将整个飞行过程分为三个时间段:［0，20 s］、［20 s，350 s］和［350，tf］，三段内各取5个有限元，每个有限元内同样取3阶Radau点进行配置，结果如表9所示。

表9 不同有限元分段结果对比Table 9 Comparison of different finite element configurations

在实际使用中，可以利用起飞前离线规划的轨迹，通过姿态角变化率的求解，调整有限元分段;在飞行过程中，也可以利用这一规律自适应地动态调整分段。

4 结 论

本文针对传统迭代制导方法无法保证终端大姿态约束的问题，利用有限元正交配置进行低阶逼近和联立求解，与伪谱法等直接法相比计算效率得到大幅提高。并以某火箭上升段最优入轨任务为例，对算法的效果进行了仿真验证。结果表明，入轨精度和终端大姿态约束均能满足，求解时间也缩短到百毫秒级，为实时在线规划打下了基础。

［1］ 宋征宇.从准确、精确到精益求精——载人航天推动运载火箭制导方法的发展［J］.航天控制，2013，31(1):4－10.［Song Zheng-yu.From accurate，precise to perfect-manned space promotes the development of guidance method on launch vehicle［J］.Aerospace Control，2013，31(1):4－10.］

［2］ 吕新广，宋征宇，巩庆海.运载火箭轨迹预测制导方法研究［J］.载人航天，2013，19(6):24－28.［Lv Xin-guang;Song Zheng-yu;Gong Qing-hai.Research on trajectory prediction guidance for launch vehicle［J］.Manned Spaceflight，2013，19 (6):24－28.］

［3］ 宋征宇.信息集成在航天运输控制系统中的应用研究［J］.宇航学报，2015，36(4)，365－374.［Song Zheng-yu.Application study ofinformation integration in aerospace transportation control system［J］.Journal of Astronautics，2015，36(4):365－374.］

［4］ 杨希祥，张为华.基于Gauss伪谱法的固体运载火箭上升段轨迹快速优化研究［J］.宇航学报，2011，32(1)，15－21.［Yang Xi-xiang，Zhang Wei-hua.Rapid optimization of ascent trajectory for solid launch vehicles based on gauss pseudo-spectral method［J］.Journal of Astronautics，2011，32(1):15－21.］

［5］ 闫循良，廖守亿，张金生，等.基于节点改善策略的伪谱轨迹优化［J］.宇航学报，2013，34(7):891－900.［Yan Xun-liang，Liao Shou-yi，Zhang Jin-sheng，et al.Trajectory optimization using pseudospectral method based on a grid node refinement strategy［J］.Journal of Astronautics，2013，34(7):891－900］

［6］ Calise A J，Brqandt N.Generation of launch vehicle abort trajectories using a hybrid optimization method［J］.Journal of Guidance，Control，and Dynamics［J］.2004，27(6)，930－937.

［7］ Mease K D，Chen D T，Teufel P，et al.Reduced-order entry trajectory planning for acceleration guidance［J］.Journal of Guidance，Control，and Dynamics，2002，25(2):257－266.

［8］ 雍恩米，陈磊，唐国金，等.飞行器轨迹优化数值方法综述［J］.宇航学报，2008，29(2):397－406.［Yong En-mi，Chen Lei，Tang Guo-jin，et al.A survey of numerical methods for trajectory optimization of spacecraft［J］.Journal of Astronautics，2008，29(2):397－406.］

［9］ 徐延万.弹道导弹、运载火箭控制系统设计与分析［M］.北京:宇航出版社，1999:41－53.

［10］ 张蒿.固体运载火箭弹道设计与优化［J］.固体火箭技术，1997，20(1):1－5.［Zhang Song.Calculation and optimization of solid launch vehicle trajectory［J］.Journal of Solid Rocket Technology，1997，20(1):1－5.］

［11］ Betts J T.Survey of numerical methods for trajectory optimization［J］.Journal of Guidance，Control and Dynamics，1998，21 (2):193－207.

［12］ 邱丰，宋征宇.基于联立框架的轨迹优化算法［J］.航天控制，2015，33(2):9－15.［Qiu Feng，Song Zheng-yu.A trajectory optimization algorithm based on simultaneous strategies［J］.Aerospace Control，2015，33(2):9－15.］

［13］ 陈伟锋.大规模复杂过程系统的高性能优化理论与方法研究［D］.杭州:浙江大学，2011.［Chen Wei-feng.Research on high-performance optimization for large-scale complex process systems［D］.Hangzhou:Zhejiang University，2011.］

［14］ Wachter A，Biegler L T.On the implementation of an interiorpoint filterline search algorithm forlarge-scale nonlinear programming［J］.Mathematical Programming，2006，106(1):25－57.

通信地址:北京142信箱402分箱(100854)

电话:(010)88525917

E-mail:qfeng1991@126.com

宋征宇(1970－)，男，硕士，研究员，主要从事飞行器控制、制导与仿真，系统集成、智能自主控制技术等研究。本文通信作者。

通信地址:北京142信箱402分箱(100854)

电话:(010)68389311

E-mail:zycalt12@sina.com

(编辑:张宇平)

Large Terminal Attitude Constrained Trajectory Optimization of Ascent Stage Via Simultaneous Method

QIU Feng，SONG Zheng-yu
(Beijing Aerospace Automatic Control Institute，Beijing 100854，China)

For launch vehicles with constant thrust and without auxiliary propulsion system，a direct method on simultaneous framework(simultaneous method)is presented to solve the dynamic online trajectory planning problem with large terminal attitude constraint，making up for the deficiency of linear programming in the traditional iterative guidance method，meanwhile，the angular rate of the trajectory can be limited according to the requirement of the attitude tracking error，so as to satisfy all constraint conditions.By combing the characteristics of the direct collocation method and pseudo-spectral method，and based on orthogonal collocation on finite element，this paper adopts the simultaneous method and discretized state variables and control variables to solve this problem，which is much more efficient than the pseudo-spectral method.On this basis，the paper puts forward some measures to increase the calculation efficiency，such as initial value selection，orbit precision error adjustment，and adaptive finite element configuration，further improving the real-time performance and providing feasible solutions to more complex and diverse guidance tasks in the future.

Launch Vehicles;Terminal attitude constraint;Simultaneous method;Trajectory design

V4

A

1000-1328(2017)01-0018-08

10.3873/j.issn.1000-1328.2017.01.003

邱 丰(1991－)，女，硕士，主要从事导航、制导与控制。

2016-05-23;

2016-10-16