具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

实施例1

本实施例提供一种固体运载火箭入轨修正方法，所述方法包括：

S10、在三级发动机关机时，获取箭体的速度位置状态量；

S20、根据所述速度位置状态量及轨道参数信息确定滑行姿态指令和四级点火时间；所述轨道参数信息为四级入轨的轨道参数信息；

S30、在滑行调姿段根据所述滑行姿态指令对所述火箭进行姿态调整，直至调至四级定轴姿态；所述滑行调姿段为三级发动机关机后至四级发送机点火之间的阶段；

S40、在所述四级点火时间到达时，发出四级点火指令，使得火箭进入四级主动段；

S50、根据发动机的内弹道曲线及发动机状态参数，计算所述四级主动段全程每间隔时间Δt₄的预测入轨轨道根数；

S60、通过预测入轨轨道根数与目标轨道根数计算获得四级姿态角指令的补偿值；

S70、根据补偿值对四级姿态角指令进行更新，直到四级发动机耗尽关机。

如图2所示，本发明的计算起始点为三级固体发动机关机之后，此时箭体经过了三次主动段飞行的加速，已经具有了很高的高度和速度，向预定轨道的高度进行滑行。此时开始进行四级制导指令计算工作，使得固体火箭发动机实现和液体火箭发动机一样的关机。

图2中的四级制导指令计算即为本申请中的四级姿态角指令的计算；图2中的四级主动段发动机状态辨识指的是获取发动机状态参数。

其中，所述四级点火时间根据以下步骤确定：

S21、根据地心惯性系下的速度位置状态量[r_xei,r_yei,r_zei,v_xei,v_yei,v_zei]，计算得到制导指令计算时刻的轨道根数a_sub、e_sux；

地心惯性系状态与轨道根数相互转换为现有技术方法，a_sub表示轨道的半长轴，e_sub表示轨道的偏心率。

惯性坐标系下，火箭的绝对状态参数为[r_a,v_a]＝[r_xei,r_yei,r_zei,v_xei,v_yei,v_zei]。对于轨道要素的表示，需要转化为中间量，间接来描述：

轨道动量矩矢量：

H＝r_a×v_a

升交点矢量：

N＝[0 0 0]^T×H＝[-H_X -H_Y 0]^T

偏心率矢量：

通过这三个矢量表达式，进而可以求解出轨道要素。但是，矢量表达式并不能直观的反映出轨道要素的物理意义，需要通过其标量形式，来详细描述。

轨道动量矩标量式为：

H＝||H||₂

轨道能量的表达式为：

根据以上轨道动量矩和轨道能量，再根据三个矢量表达式可以得出轨道六个要素的具体形式：

半长轴为：

半通径为：

偏心率为：

e＝||e||₂

轨道倾角为：

升交点赤经为：

近地点幅角为：

偏近点角(火箭当前轨道位置t)为：

以上，轨道六要素具体表达式已经详细表述出来。在地心惯性坐标系下，轨道六要素与笛卡尔状态参数r_a,v_a的关系已经详细表述出来，通过终端轨道要素的约束，进行转换为状态参数的约束。

所述地心惯性系下的速度位置状态量[r_xei,r_yei,r_zei,v_xei,v_yei,v_zei]根据以下方法计算得到：

获取火箭在发射系的速度位置状态量[x,y,z,vx,vy,vz]；

将发射系的速度位置状态量[x,y,z,vx,vy,vz]转换为地心固连系的速度位置状态量[r_xef,r_yef,r_zef,v_xef,v_yef,v_zef]；

通过以下公式(八)将地心固连系下的速度位置状态量[r_xef,r_yef,r_zef,v_xef,v_yef,v_zef]转换为地心惯性系下的速度位置状态量[r_xei,r_yei,r_zei,v_xei,v_yei,v_zei]

[x_ef，y_ef，z_ef]表示地心固连系的位置参数，[x_ei，y_ei，z_ei]表示地心惯性系条件下的位置参数。

具体地：

根据发射方位角 A₀

发射点经度 λ₀

发射点高程 H₀

发射点地理纬度 B₀

发射点地心纬度

地垂线与地心矢径的夹角

发射点地心距

发射点地心矢径在发射坐标系分量：

则可以根据发射系的速度位置状态量[x,y,z,vx,vy,vz]获得地心固连系的速度位置状态量[r_xef,r_yef,r_zef,v_xef,v_yef,v_zef]

式中，转换矩阵

进一步将地心固连系下的状态量[r_xef,r_yef,r_zef,v_xef,v_yef,v_zef]转换为地心惯性系下的状态量[r_xei,r_yei,r_zei,v_xei,v_yei,v_zei]，该部分采用简化处理方法，认为

S22、根据以下公式(一)计算火箭当前滑行轨道的远地点高度R_ap；

R_ap＝a_sub(1+e_sub)； (一)

S23、判断R_ap小于目标轨道的轨道高度时，根据以下公式组(二)确定等效脉冲时间t_imp及四级点火时间t_ig：

t_imp＝t_{i_ap}

R_ap＝a_sub(1+e_sub)

t_ig＝t_{i_ap}-t_core4

t_core4＝t₄-R_M4/W_M4； (二)

其中E_imp为等效脉冲点的偏近点角；R_orb为目标轨道的地心距；t_core4为四级型心时间，t₄为四级发动机工作时间，R_M4为四级视位移增量，W_M4为四级视速度增量，E为偏近点角；

判断R_ap大于等于目标轨道的轨道高度时，根据以下公式(三)确定等效脉冲时间t_imp及四级点火时间t_ig：

t_ig＝t_imp-t_core4

t_core4＝t₄-R_M4/W_M4； (三)

其中E_imp为等效脉冲点的偏近点角；R_orb为目标轨道的地心距。

若根据公式组(三)确定的t_imp与根据公式组(二)确定的t_{i_ap}满足t_{i_ap}-t_imp≤R_M4/W_M4，则令t_imp＝t_{i_ap}-R_M4/W_M4。

其中，所述四级视位移增量R_M4、四级视速度增量W_M4根据以下公式(四)计算得到：

式中，Isp₄为四级发动机平均比冲，m₄₀为四级点火时刻的起始质量，m_4p为四级推进剂装药量，m_4f为四级发动机关机后剩余箭体质量，T_s4为四级发动机工作时间，k_m4为四级增量修正系数。

其中，所述滑行姿态指令根据以下步骤确定：

S24、根据开普勒轨道递推火箭在重力作用下滑行至四级等效脉冲点时间t_imp的速度位置状态量r_sub，v_sub。四级等效脉冲点的三个位置量和三个速度量：

r_sub＝[r_xei，r_yei，r_zei]；

v_sub＝[v_xei，v_yei，v_zei]；

S25、根据状态量r_sub，v_sub，轨道根数a_sub、e_sub，及目标轨道的轨道根数a_orb、e_orb、i_orb确定过渡轨道的轨道根数；

四级轨道根数a_orb、e_orb、i_orb已知，对于其余轨道根数，由轨道参数几何关系可知

u＝ω+f

则过渡轨道剩余轨道根数计算如下

ω_4.inj＝u_4.inj-f_4.inj；

S26、根据过渡轨道的轨道根数算出过渡轨道的状态量r_orb，v_orb；

该算法为上面介绍的状态量计算轨道根数算法的逆运算，属于已知算法

输入：半长轴a、离心率e、轨道倾角i、升交点赤经Ω、近地点幅角ω和真近点角Θ，μ为地球引力常数。

输出：地惯系状态量，包括位置：x_EI、y_EI、z_EI，速度：v_EIx、v_EIy、v_EIz。

(1)计算出比角动量的模

(2)计算近焦点坐标系下的位置矢量

(3)计算近焦点坐标系下的速度矢量

(4)计算近焦点坐标系到地心赤道坐标系的变换矩阵

(5)计算地惯系下的位置矢量

(6)计算地惯系下的速度矢量

S27、根据以下公式(五)计算出地心惯性系下的需用速度增量v_pa：

v_pa＝v_orb-v_sub； (五)

S28、将地心惯性系下的需用速度增量v_pa转化为发射系下的需用速度增量v_x'、v_y'、v_z'；

其中，v_pax为v_pa在x方向的分量，v_pay为v_pa在y方向的分量,v_paz为v_pa在z方向的分量，A₀为火箭的发射方位角，B₀为火箭发射点的地理纬度，E₀为火箭发射点的地心距；，L表示方向余弦矩阵；

S29、根据以下公式组(六)计算四级姿态角指令：

ψ_cmd＝-arcsinv′_z

γ_cmd＝0 (六)；

其中，ψ_cmd、γ_cmd分别表示俯仰、偏航、滚转方向指令姿态角。

因此本实施例中的轨道参数信息包括步骤S21-S29中的涉及的除去速度位置状态量以外的其他参数。

本申请实施例提供的技术方案，所述预测入轨轨道根数根据以下方法确定：

四级主动段飞行过程中，每隔一定的时间Δt₄，获取当前的位置产生的实际视速度增量ΔW_M4和实际视位移增量ΔR_M4；发动机状态参数指的就是发动机的当前位置的实际视速度增量ΔW_M4和实际视位移增量ΔR_M4；

根据发动机内弹道曲线、实际视速度增量ΔW_M4、实际视位移增量ΔR_M4，计算得到全程的视速度增量和视位移增量每间隔时间Δt₄的预测视位移增量预测视速度增量

预测视位移增量预测视速度增量与四级视位移增量R_M4、四级视速度增量W_M4的计算方法一致，只是将各个参数的时间替换为间隔时间Δt₄后的时间，在此不赘述；

根据预测视位移增量预测视速度增量计算出当前状态下关机后的预测入轨轨道根数；该处预测入轨轨道根数的算法与上述入轨轨道根数的算法一致，在此不赘述。

其中，本实施例中，根据以下方法计算获得四级姿态角指令的补偿值：

根据预测入轨轨道根数与目标轨道根数计算四级姿态角指令的补偿值和Δψ_cmd。计算方法同公式(六)，在此不赘述。

据本申请实施例提供的技术方案，根据以下方法通过补偿值对四级姿态角指令进行更新，直到四级发动机耗尽关机：

根据以下公式(七)对四级姿态角指令进行更新：

γ_cmd＝0 (七)。

以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。