CN112287525A - 固体运载火箭耗尽关机模式下的惯性落点控制闭环制导方法 - Google Patents

Abstract

本发明涉及一种固体运载火箭耗尽关机模式下的惯性落点控制闭环制导方法，根据瞬时轨道落点的微分导数关系，推导出自由飞行过程瞬时轨道落点相关导数的解析解，从而建立了当前点与轨道落点之间的映射关系。根据当前固体运载火箭剩余飞行时间确定当前加速度指令矢量，保证固体运载火箭发动机耗尽关机时瞬时轨道落点移动到目标点，运载火箭继续沿开普勒轨道滑行至目标落点，同时该发明具有很高的制导精度。

Description

固体运载火箭耗尽关机模式下的惯性落点控制闭环制导方法

技术领域

本发明属于航天技术领域，具体涉及一种远程固体火箭在耗尽关机方式下的惯性落点闭环制导技术。

背景技术

国内远程固体火箭的制导技术主要研究思想是以闭路制导结合速度控制的混合制导方法。对于耗尽关机方式，发动机能够产生的剩余视速度增量由剩余燃料决定，通过待增速度矢量以及剩余视速度增量给出火箭需要耗散的多余能量。现有的方法主要集中在以下几点：一、将需要速度矢量为迭代变量设计了一种适用于固体火箭的迭代制导方法，通过闭环迭代计算的形式提高了终端精度；二、考虑发动机性能参数散布特性，设计了需用速度增益曲面的概念并在大气层外对火箭进行导引，提高闭路制导方法的鲁棒性问题。但是，该方法从运动学求解需要速度矢量过程中存在无穷解的问题，即存在零射程线矢量又可以通过能量管理方式进行速度控制，并未形成统一的远程固体火箭闭环制导方法。

发明内容

要解决的技术问题

本发明主要解决目前采用经典闭路制导存在的不足，即在快速发射条件下无法通过离线设计在满足射程约束的椭圆轨道簇中预先装订最佳的椭圆轨道参数，这使得制导方法的自主适应性不足。因此，为了提高算法的自适应能力以及满足快速发射任务需求，根据瞬时轨道落点的微分导数关系，推导出自由飞行过程瞬时轨道落点相关导数的解析解，从而建立了当前点与轨道落点之间的映射关系。根据当前固体运载火箭剩余飞行时间确定当前加速度指令矢量，保证固体运载火箭发动机耗尽关机时瞬时轨道落点移动到目标点，运载火箭继续沿开普勒轨道滑行至目标落点，同时该发明具有很高的制导精度。

技术方案

一种固体运载火箭耗尽关机模式下的惯性落点控制闭环制导方法，其特征在于步骤如下：

步骤1：确定瞬时轨道落点射程角β_e：根据当前地心矢径大小r₀、速度大小v₀、动量矩大小h，当地弹道倾角

据下式计算得到当前瞬时轨道落点射程角β_e，其中A₁,A₂,A₃为与射程角计算相关的待定系数；

步骤2：确定地心惯性系下的瞬时轨道落点矢径1_p：根据当前位置矢量r₀、速度矢量v₀和当前落点射程角β_e，据下式计算得到当前瞬时轨道落点矢径1_p，其中

代表当地弹道倾角，

代表当前位置单位矢量，

代表当前速度单位矢量；

步骤3：确定瞬时轨道落点的经纬度φ_p和λ_p：根据瞬时轨道落点矢径1_p、和剩余飞行时间t_F，据下式计算得到瞬时轨道落点的经纬度φ_p和λ_p：

其中，1_px,1_py,1_pz为1_p在地心惯性坐标系下各坐标轴的分量，t是当前时间，t_ref是地心坐标系与地心惯性坐标系的对齐时间；

步骤4：确定地心惯性坐标系下的瞬时轨道落点矢径的导数

与径向、射向和轨道坐标系法向加速度分量对应的三个矢量d_r,d_θ,d_h：根据步骤2计算得到的瞬时轨道落点矢径1_p，当前位置矢量r₀、速度矢量v₀以及当前瞬时轨道落点射程角β_e，据下式计算得到三个矢量d_r,d_θ,d_h，其中

代表计算过程中涉及的中间导数量，A₁,A₂,A₃为步骤1计算得到的与射程角计算相关的待定系数；

步骤5：确定地心坐标系下的瞬时轨道落点矢径的导数

与径向、射向和轨道坐标系法向加速度分量对应的三个矢量

根据步骤3计算得到的瞬时轨道落点经纬度确定地心坐标系下的瞬时轨道落点矢径

并利用步骤4计算得到的d_r,d_θ,d_h及地心惯性坐标系与地心坐标系之间的转换矩阵

代入下式计算得到三个矢量

其中

代表计算过程中涉及的中间导数量；

其中

代表a_r,a_θ对

的偏导数，

和

代表计算过程中涉及的中间导数量；

步骤6：根据几何关系确定瞬时轨道移动模式下相关参数：根据最短路径方向矢量

确定瞬时轨道落点导数

的方向，并利用下式计算瞬时轨道落点移动模式下的相关参数；

其中

分别为

在x、y轴的分量；

分别为

在x、z轴的分量；

分别为

在z、x轴的分量；求得的M的符号与

的符号相同；

步骤7：确定瞬时轨道落点移动模式下的加速度指令

和

根据几何关系确定的瞬时轨道落点导数方向及固体运载火箭加速度大小a_m，利用下式计算得到瞬时轨道落点移动模式下的加速度指令：

步骤8：确定瞬时轨道落点保持模式下的相关参数p₁、p₂、p₃和加速度指令

根据步骤3计算得到的瞬时轨道落点经纬度φ_p和λ_p保持为零，即可计算相关参数p₁、p₂、p₃，同时引入d_r、d_θ和d_h利用下式即可得到瞬时轨道落点保持模式的加速度矢量指令：

p₃＝cosλ_pd_hy-sinλ_pd_hx

其中d_rx、d_ry分别为d_r在x、y轴的分量；d_θx为d_θ在x轴的分量；d_hy、d_hx分别为d_h在y、x轴的分量；

步骤9：确定耗尽关机情况下落点矢量导数指令

根据当前运载火箭射程角β_e和发动机剩余工作时间t_go确定耗尽关机情况下落点矢量导数指令，同时考虑运载火箭推力加速度限制对落点矢量导数指令进行限幅，利用下式即可得到耗尽关机情况下落点矢量导数指令：

其中

表示地心坐标系中的目标落点矢量；

步骤10：确定耗尽关机情况下落点控制加速度指令

和

根据落点矢量导数指令

和当前运载火箭落点矢量导数

利用下式即可得到耗尽关机情况下落点控制的加速度指令：

a^s＝a^**cosβ+a^H*sinβ

步骤11：计算发射坐标系火箭的姿态角指令：计算得到加速度矢量的分量后，即可由下式计算得到火箭的俯仰角指令和偏航角指令

ψ_c，其中a_x,a_y,a_z分别代表加速度矢量在发射坐标系x,y,z坐标轴的投影，ω_ex,ω_ey,ω_ez分别为地球自转矢量ω_e在发射坐标系的投影，下标c代表指令：

有益效果

本发明提出的一种固体运载火箭耗尽关机模式下的惯性落点控制闭环制导方法，照给定的初始速度矢量、位置矢量以及火箭的总体参数、并配置不同的期望目标落点进行仿真实验，目标落点任务分别为：沿射向将射程缩短150km/300km；沿射向将射程增加150km/300km以及沿横向将横程增加200km。根据当前瞬时轨道落点及目标期望落点，采用耗尽关机下的落点控制闭环制导方法沿着最短落点移动路径。耗尽关机下的落点控制闭环制导方法通过瞬时轨道落点及其时间导数的解析表达式，来产生使其瞬时轨道落点导数大小与瞬时轨道落点导数指令相等的加速度指令，将运载火箭的瞬时轨道落点移动到目标期望位置上。

此外，以2500km射程任务为例，在参数偏差及不确定性条件下的验证耗尽关机下的落点控制闭环制导方法的制导精度及鲁棒性。在耗尽关机方式下，利用闭环制导方法进行导引，将轨道惯性落点控制在目标射程附近。通过分析结果发现闭环制导方法的落点偏差缩小至216.6米，具有较高的落点控制的精度和鲁棒性。

附图说明

图1瞬时轨道落点及其导数原理示意图；

图2瞬时轨道落点移动模式原理示意图；

图3加速度矢量几何关系示意图；

图4满足约束的a_θ-a_r变化曲线；

图5耗尽关机下的加速度指令几何关系；

图6耗尽关机下的落点控制计算流程；

图7大小射程任务下瞬时弹道落点经纬度变化曲线；

图8大小射程任务下姿态角指令及经纬度导数变化曲线：(a)大小射程任务下俯仰角指令变化；(b)大小射程任务下偏航角指令变化；(c)瞬时轨道落点经度的导数变化；(d)瞬时轨道落点纬度的导数变化；

图9 500次打靶经纬高变化曲线；

图10(a)500次打靶俯仰角指令变化；(b)500次打靶偏航角指令变化；(c)500次打靶射程—高度变化；(d)500次打靶射程—当地弹道倾角变化；(e)500次打靶脱靶量散布；(f)500次打靶射程散布拟合。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

1、瞬时轨道落点及其导数的解析形式

固体运载火箭在没有推力或气动力等合外作用力的影响，开普勒轨道的瞬时轨道落点(定义为：当前时刻的飞行状态沿开普勒轨道映射至地面落点的经纬度)应保持不变。然而，一旦火箭受到外力的作用，瞬时轨道落点将会在地球表面移动，移动的方式将根据瞬时轨道落点纬度和经度的导数来确定。

1.1瞬时轨道落点的解析函数

根据运载火箭质点动力学式(1)，在没有推力和气动力的作用下瞬时弹道落点就是根据当前位置矢量r₀和速度矢量v₀寻找其弹道与地球表面的交点，如图1所示：

其中，r和v是运载火箭在地心惯性坐标系中的位置和速度，g_c(r)是运载火箭当前所受到的重力加速度，1_r、1_θ、1_h分别是位置、轨迹切线及角动量方向的单位矢量。

瞬时轨道落点r_p根据当前速度矢量v₀和位置矢量r₀得到非正交坐标表达式：

其中，

为运载火箭的飞行航迹角，下标0表示当前时刻，射程角β_e表示为：

其中，A₁、A₂、A₃表示为：

其中，

是运载火箭的角动量。瞬时轨道落点在地心惯性坐标系中的经度和纬度可以通过瞬时轨道落点的单位矢量1_p计算得到，如下所示：

考虑到地球在飞行时间内的自转，瞬时轨道落点在地心坐标系下的经度表示为：

其中，ω_e为地球的自转角速度，t是当前时间，t_ref是地心坐标系与地心惯性坐标系的对齐时间。此外，t_F是到达瞬时轨道落点的飞行时间，且由自由飞行段特性解析公式得：

其中，

表示运载火箭当前速度大小v₀与所在圆轨道速度大小

之比的平方。至此，固体运载火箭由当前位置矢量r₀和速度矢量v₀解析推导得到瞬时轨道落点表达

1.2瞬时轨道落点的解析导数

当运载火箭受到式(1)中控制力的作用时，瞬时轨道落点将发生变化且移动方式将根据瞬时轨道落点的解析导数来确定。瞬时轨道落点的解析导数表示为控制力加速度的线性组合，具体表达式如下所示：

式中矢量导数为：

其中，

和

分别为加速度a_r和a_θ对射程角导数

的导数。射程角导数

由重力及外部加速度共同决定，表达式如下：

其中：

飞行时间t_F的时间导数表示为a_r和a_θ的线性组合，详细表达式如下：

其中：

式中，a、e、p、n分别为当前运载火箭所在轨道的半长轴、偏心率、半通径和平均速度；E₀和E_p为当前位置和落点位置的偏近点角，其表达式如下：

瞬时轨道落点矢量在地心坐标系下的时间导数为：

其中，

为在地心坐标系中表示的瞬时轨道落点单位向量，

为地心惯性坐标系到地心坐标系的转换矩阵，Ω_e为表示地球自转的向量，分别表示为：

Ω_e＝[0,0,ω_e]^T (20)

联立式(8)、式(14)和式(17)，可以得到瞬时轨道落点矢量在地心坐标系下的时间导数表达式：

其中：

2、瞬时轨道落点控制模式

根据瞬时轨道落点的解析导数推导出考虑地球自转的瞬时轨道落点移动模式和瞬时轨道落点保持模式。

2.1瞬时轨道落点移动模式

基于瞬时轨道落点的解析导数，通过几何关系推导出满足运载火箭落点约束的能量最优控制模式，即瞬时轨道落点移动模式。为了将当前瞬时轨道落点移动到目标位置，在由这两点以及地心构成的平面内，通过控制瞬时落点的移动矢量来确定运载火箭当前所需要的加速度矢量，各矢量的定义如图2所示。显然，该控制模式可以使瞬时轨道落点速度向量与连接当前瞬时轨道落点和目标位置的圆弧方向保持一致并且使落点速度最大。

从当前瞬时碰撞点到目标瞬时碰撞点沿地球表面的最短路径为连接两点的圆弧。

表示地心坐标系中的目标落点矢量，q_z定义为

和

所决定平面的法向量，

为其单位向量，表示如下：

1_qz＝q_z/||q_z|| (26)

为实现能量最优，瞬时轨道落点解析导数方向被定义为连接当前瞬时轨道落点和目标位置的球面在最短路径方向上的单位向量：

瞬时轨道落点解析导数矢量在x和z方向的比值定义如下：

将式(21)代入式(28)得：

如果假定运载火箭的推力或加速度是已知的，则加速度的大小如下所示：

运载火箭所能产生的所有加速度矢量集合被定义为一个球体。在球面上的点之间，满足运载火箭瞬时轨道落点在射面内变化的加速度矢量集合形成一个圆。位于该圆相对于基准线一侧的加速度矢量将瞬时轨道落点移动到目标点，而圆上的其他加速度矢量将瞬时轨道落点移动到相反的方向。根据当前运载火箭瞬时轨道落点和目标位置确定适当的一侧，如图3所示：

将式(29)按照图3中加速度矢量形式进行恒等变形，具体表达式如下：

a_h＝Aa_r+Ba_θ (31)

其中：

在满足将瞬时轨道落点移动到目标位置的加速度矢量族中，利用几何关系确定使瞬时轨道落点解析导数最大的加速度矢量。将式(31)代入式(30)得到：

式(34)为以a_θ-a_r为坐标轴的椭圆，如图4所示。

通过将式(31)代入式(21)，可以得到瞬时轨道落点解析导数

的x分量，如下所示：

将式(35)进行恒等变换，并定义一个变量M：

a_r+sa_θ＝M (36)

其中：

当式(36)表示的直线与式(34)表示的椭圆相切时，|M|最大(即

最大)。将式(36)代入式(34)得到的二元一次方程有重根时，直线与椭圆相切，从而得到|M|的值：

另外，M的符号可根据式(37)进行确定。

同时，得到基于瞬时轨道落点移动模式的能量最优加速度指令

如下：

综上所述，固体运载火箭由于推力大小不可控制，加速度指令取决于当前固体运载火箭的视加速度大小a_m。为了使实际的瞬时轨道落点单位矢量与期望的瞬时轨道落点单位矢量(沿着连接当前和目标瞬时轨道落点的最佳路径)方向一致，等式(31)确定了瞬时轨道落点单位矢量位于当前位置单位矢量和目标位置单位矢量所决定的平面内，并根据几何关系来决定最优的加速度分量，使得瞬时轨道落点能够尽快移动到目标位置。

2.2瞬时轨道落点保持模式

瞬时轨道落点保持模式为固体运载火箭在推力或气动力等合外作用力的影响下，瞬时轨道落点保持模式仍能使瞬时轨道落点保持不变，即地心坐标系下的瞬时轨道落点经纬度的导数为零。

通过求解由式(30)、式(42)和式(43)组成的方程组，得到瞬时轨道落点保持模式的加速度指令

如下：

其中：

p₃＝cosλ_pd_hy-sinλ_pd_hx (49)

综上所述，通过对瞬时轨道落点经纬度导数进行约束即可保证固体运载火箭在推力或气动力等合外作用力的影响下仍然能保持当前瞬时轨道落点不变。同时，由于该加速度指令有解析公式构成不涉及任何迭代运算，具有较强的可行性和鲁棒性。

3、耗尽关机下的落点控制闭环制导方法

固体运载火箭为了提高质量比及可靠性取消了推力终止机构。由于不能进行制导关机，发动机性能偏差无法通过“物理”手段消除，加大了制导算法的难度。为保证固体运载火箭的制导精度，提出了耗尽关机下的落点控制闭环制导方法。

耗尽关机下的落点控制闭环制导方法先采用瞬时轨道落点移动模式，并逐渐切换为瞬时轨道落点保持模式。瞬时轨道落点移动模式计算得到的加速度指令始终垂直于瞬时轨道落点保持模式计算得到的加速度指令，即运载火箭改变制导模式需要近90度的姿态机动，但由于运载火箭角速度的限制无法在短时间内完成。因此，引入固体运载火箭发动机剩余工作时间t_fuel，在该段时间内完成相应的模式切换。

该方法的加速度矢量指令为上述两个模式下的加速度指令的线性组合(a^s＝a^**cosβ+a^H*sinβ)，如图5所示：

其中β由瞬时轨道落点矢量变化指令

和当前瞬时轨道落点移动模式中的

确定，其表达式如下：

根据当前瞬时轨道落点矢量

目标落点矢量

及剩余飞行时间t_go确定瞬时轨道落点矢量变化指令

由于瞬时轨道落点矢量导数的大小受固体运载火箭加速度大小的限制，因此需对瞬时轨道落点矢量变化指令

进行限幅。其表达式如下：

其中，为保证在末段剩余飞行时间t_go过小而导致指令出现奇异，令t_go在末段一个制导周期t_G内保持不变，其表达式如下：

由此便得到了固体运载火箭在耗尽关机模式下的落点控制闭环制导指令。为保证运载火箭制导指令的统一性，将地心惯性坐标系中的加速度指令转变为发射坐标系的姿态角指令。固体运载火箭的加速度大小为a_m＝T/m以及箭体矢量方向为x′_b＝a/a_m，故在发射惯性坐标系内运载火箭的姿态角指令为：

其中，G_E为地心惯性坐标系到发射惯性坐标系的转换矩阵。由于地球旋转会影响发射坐标系与发射惯性坐标系之间的坐标轴方向发生变化，相应姿态角产生一定的差值。故在发射坐标系内运载火箭的姿态角指令为：

其中：ω_ex、ω_ey、ω_ez为地球自转矢量在发射坐标系内的投影，其表达式如下：

耗尽关机下的落点控制闭环制导方法的实际输入条件是由惯性导航提供的当前运载火箭的位置矢量r₀、速度矢量v₀以及当前加速度模量a_m，用于计算当前瞬时轨道落点和瞬时轨道落点导数的方向d(1_p)/dt，并根据几何关系得到移动模式和保持模式下的加速度矢量。由固体运载火箭发动机的剩余工作时间，确定瞬时轨道落点导数的大小，输出满足瞬时轨道落点导数大小约束的制导加速度矢量的分量(以此计算姿态角指令)。在解算过程中，根据解析函数及其导数关系并不涉及迭代循环运算，同时由期望落点解算姿态角指令构成闭环格式，具有实际飞行过程中可靠性、可实现性的重要特性。

远程固体火箭耗尽关机下的落点控制闭环制导方法根据当前位置矢量r₀、速度矢量v₀以及视加速度模量a_m来实时计算火箭的姿态角指令，在制导过程中根据目标落点与当前瞬时轨道落点构成闭环格式，而且在解算过程中以解析的形式进行求解不涉及迭代运算，耗尽关机下的落点控制的计算流程如图6所示，详细计算步骤如下：

(1)确定瞬时轨道落点射程角β_e：根据当前地心矢径大小r₀、速度大小v₀、动量矩大小h，当地弹道倾角

据下式计算得到当前瞬时轨道落点射程角β_e，其中A₁,A₂,A₃为与射程角计算相关的待定系数；

(2)确定地心惯性系下的瞬时轨道落点矢径1_p：根据当前位置矢量r₀、速度矢量v₀和当前落点射程角β_e，据下式计算得到当前瞬时轨道落点矢径1_p，其中

代表当地弹道倾角，

代表当前位置单位矢量，

代表当前速度单位矢量；

(3)确定瞬时轨道落点的经纬度φ_p、λ_p和地心坐标系下的瞬时轨道落点矢径

根据瞬时轨道落点矢径1_p、和剩余飞行时间t_F，据下式计算得到瞬时轨道落点的经纬度φ_p和λ_p，其中1_px,1_py,1_pz为1_p在地心惯性坐标系下各坐标轴的分量：

(4)确定地心惯性坐标系下的瞬时轨道落点矢径的导数

与径向、射向和轨道坐标系法向加速度分量对应的三个矢量d_r,d_θ,d_h：根据第二步计算得到的瞬时轨道落点矢径1_p，当前位置矢量r₀、速度矢量v₀以及当前瞬时轨道落点射程角β_e，据下式计算得到三个矢量d_r,d_θ,d_h，其中

代表计算过程中涉及的中间导数量，A₁,A₂,A₃为第一步计算得到的与射程角计算相关的待定系数；

(5)确定地心坐标系下的瞬时轨道落点矢径的导数

与径向、射向和轨道坐标系法向加速度分量对应的三个矢量

根据第三步计算得到的瞬时轨道落点经纬度确定地心坐标系下的瞬时轨道落点矢径

并利用第四步计算得到的d_r,d_θ,d_h及地心惯性坐标系与地心坐标系之间的转换矩阵

代入下式计算得到三个矢量

其中

代表a_r,a_θ对

的偏导数，

和

代表计算过程中涉及的中间导数量；

(6)根据几何关系确定瞬时轨道移动模式下相关参数：根据最短路径方向矢量

确定瞬时轨道落点导数

的方向，并利用下式计算瞬时轨道落点移动模式下的相关参数，其中M的符号与

的符号相同；

(7)确定瞬时轨道落点移动模式下的加速度指令

和

根据几何关系确定的瞬时轨道落点导数方向及固体运载火箭加速度大小a_m，利用下式计算得到瞬时轨道落点移动模式下的加速度指令：

(8)确定瞬时轨道落点保持模式下的相关参数p₁、p₂、p₃和加速度指令

根据第三步计算得到的瞬时轨道落点经纬度φ_p和λ_p保持为零，即可计算相关参数p₁、p₂、p₃，同时引入d_r、d_θ和d_h利用下式即可得到瞬时轨道落点保持模式的加速度矢量指令：

p₃＝cosλ_pd_hy-sinλ_pd_hx

(9)确定耗尽关机情况下落点矢量导数指令

根据当前运载火箭射程角β_e和发动机剩余工作时间t_go确定耗尽关机情况下落点矢量导数指令，同时考虑运载火箭推力加速度限制对落点矢量导数指令进行限幅，利用下式即可得到耗尽关机情况下落点矢量导数指令：

(10)确定耗尽关机情况下落点控制加速度指令a_x、a_y和a_z：根据落点矢量导数指令

和当前运载火箭落点矢量导数

利用下式即可得到耗尽关机情况下落点控制的加速度指令：

(11)计算发射坐标系火箭的姿态角指令：计算得到加速度矢量的分量后，即可由下式计算得到火箭的俯仰角指令和偏航角指令

ψ，其中

ψ_Tc为地心惯性坐标系下火箭的俯仰角指令和偏航角指令，a_m为当前火箭加速度大小，ω_ex,ω_ey,ω_ez分别为地球自转矢量ω_e在发射坐标系的投影，下标c代表指令：

本发明的实施实例为：

1)初始条件：火箭的初始质量为2100kg，时间比冲为210s，秒流量为19.5725kg/s，额定工作时间32s，初始飞行状态为：

2)偏差条件：按照给定的初始速度矢量、位置矢量以及火箭的总体参数、并配置不同的期望目标落点进行仿真实验，目标落点任务分别为：沿射向将射程缩短150km/300km；沿射向将射程增加150km/300km以及沿横向将横程增加200km。

采用本发明的制导方法，测试得出的结果如图7和图8所示：

以2500km射程任务为例，在参数偏差及不确定性条件下的验证瞬时轨道落点闭环制导方法的制导精度及鲁棒性。

Claims (1)

1.一种固体运载火箭耗尽关机模式下的惯性落点控制闭环制导方法，其特征在于步骤如下：

步骤1：确定瞬时轨道落点射程角β_e：根据当前地心矢径大小r₀、速度大小v₀、动量矩大小h，当地弹道倾角θ_l，据下式计算得到当前瞬时轨道落点射程角β_e，其中A₁,A₂,A₃为与射程角计算相关的待定系数；

步骤2：确定地心惯性系下的瞬时轨道落点矢径1_p：根据当前位置矢量r₀、速度矢量v₀和当前落点射程角β_e，据下式计算得到当前瞬时轨道落点矢径1_p，其中

代表当地弹道倾角，

代表当前位置单位矢量，

代表当前速度单位矢量；

步骤3：确定瞬时轨道落点的经纬度φ_p和λ_p：根据瞬时轨道落点矢径1_p、和剩余飞行时间t_F，据下式计算得到瞬时轨道落点的经纬度φ_p和λ_p：

其中，1_px,1_py,1_pz为1_p在地心惯性坐标系下各坐标轴的分量，t是当前时间，t_ref是地心坐标系与地心惯性坐标系的对齐时间；

步骤4：确定地心惯性坐标系下的瞬时轨道落点矢径的导数

与径向、射向和轨道坐标系法向加速度分量对应的三个矢量d_r,d_θ,d_h：根据步骤2计算得到的瞬时轨道落点矢径1_p，当前位置矢量r₀、速度矢量v₀以及当前瞬时轨道落点射程角β_e，据下式计算得到三个矢量d_r,d_θ,d_h，其中

代表计算过程中涉及的中间导数量，A₁,A₂,A₃为步骤1计算得到的与射程角计算相关的待定系数；

步骤5：确定地心坐标系下的瞬时轨道落点矢径的导数

与径向、射向和轨道坐标系法向加速度分量对应的三个矢量

根据步骤3计算得到的瞬时轨道落点经纬度确定地心坐标系下的瞬时轨道落点矢径

并利用步骤4计算得到的d_r,d_θ,d_h及地心惯性坐标系与地心坐标系之间的转换矩阵

代入下式计算得到三个矢量

其中

代表计算过程中涉及的中间导数量；

其中

代表a_r,a_θ对

的偏导数，

和

代表计算过程中涉及的中间导数量；

步骤6：根据几何关系确定瞬时轨道移动模式下相关参数：根据最短路径方向矢量

确定瞬时轨道落点导数

的方向，并利用下式计算瞬时轨道落点移动模式下的相关参数；

其中

分别为

在x、y轴的分量；

分别为

在x、z轴的分量；

分别为

在z、x轴的分量；求得的M的符号与

的符号相同；

步骤7：确定瞬时轨道落点移动模式下的加速度指令

和

根据几何关系确定的瞬时轨道落点导数方向及固体运载火箭加速度大小a_m，利用下式计算得到瞬时轨道落点移动模式下的加速度指令：

步骤8：确定瞬时轨道落点保持模式下的相关参数p₁、p₂、p₃和加速度指令

根据步骤3计算得到的瞬时轨道落点经纬度φ_p和λ_p保持为零，即可计算相关参数p₁、p₂、p₃，同时引入d_r、d_θ和d_h利用下式即可得到瞬时轨道落点保持模式的加速度矢量指令：

p₃＝cosλ_pd_hy-sinλ_pd_hx

其中d_rx、d_ry分别为d_r在x、y轴的分量；d_θx为d_θ在x轴的分量；d_hy、d_hx分别为d_h在y、x轴的分量；

步骤9：确定耗尽关机情况下落点矢量导数指令

根据当前运载火箭射程角β_e和发动机剩余工作时间t_go确定耗尽关机情况下落点矢量导数指令，同时考虑运载火箭推力加速度限制对落点矢量导数指令进行限幅，利用下式即可得到耗尽关机情况下落点矢量导数指令：

其中

表示地心坐标系中的目标落点矢量；

步骤10：确定耗尽关机情况下落点控制加速度指令

和

根据落点矢量导数指令

和当前运载火箭落点矢量导数

利用下式即可得到耗尽关机情况下落点控制的加速度指令：

a^s＝a^**cosβ+a^H*sinβ

步骤11：计算发射坐标系火箭的姿态角指令：计算得到加速度矢量的分量后，即可由下式计算得到火箭的俯仰角指令和偏航角指令

ψ_c，其中a_x,a_y,a_z分别代表加速度矢量在发射坐标系x,y,z坐标轴的投影，ω_ex,ω_ey,ω_ez分别为地球自转矢量ω_e在发射坐标系的投影，下标c代表指令：

Images