迭代制导总结

鲁 鹏，北京理工大学宇航学院
2019.05.17

本文主要参考文献[1-2]学习迭代制导（Iterative Guidance Mode, IGM）技术，源代码已上传到码云 https://gitee.com/olupengo/IterativeGuidance/，仅供参考。文献[3]是迭代制导技术的原始文献，写得很乱，不适入门者学习。

一般情况下，对火箭在大气层内（低于$90km$高度）飞行采用固定程序控制，火箭进入真空飞行后，开始加入制导控制[1]。本文会使用原点在地心$O$的发射惯性坐标系$Oxyz$，原点在发射点的发射点惯性坐标系，入轨点轨道坐标系（又被称为制导坐标系）$O{x}_{ocf}{y}_{ocf}{z}_{ocf}$和升交点轨道坐标系$O{x}_{rcf}{y}_{rcf}{z}_{rcf}$，这几个坐标系的定义见参考文献[2]。

空间运动方程

使用迭代制导时，一般会把火箭运动方程表示在制导坐标系下，考虑到运载火箭在实际飞行过程中，其控制系统是可以保证滚转角${\gamma }_{ocf}\approx 0$，由于是在真空飞行段加入制导控制，可以不考虑大气作用。因此火箭的质心运动可以视为由推力和重力驱动下的质点运动[1-2]
$$\left[\begin{array}{c}{\ddot{x}}_{ocf}\\ {\ddot{y}}_{ocf}\\ {\ddot{z}}_{ocf}\end{array}\right]=\frac{F}{m}\left[\begin{array}{c}\cos {\phi }_{ocf}\cos {\psi }_{ocf}\\ \sin {\phi }_{ocf}\cos {\psi }_{ocf}\\ -\sin {\psi }_{ocf}\end{array}\right]+\left[\begin{array}{l}{g}_{ocfx}\\ g_{ocfy}\\ g_{ocfz}\end{array}\right]\text{\hspace{1em}(1)}$$
公式（1）中俯仰角${\phi }_{ocf}$和偏航角${\psi }_{ocf}$定义如图 1 ，$F$是火箭恒值推力，$m$是火箭质量。文献[7]有空间运动方程的详细推导。

由于入轨点不断更新，所以入轨点轨道坐标系$O{x}_{ocf}{y}_{ocf}{z}_{ocf}$在不断变化，但是每个制导周期中$O{x}_{ocf}{y}_{ocf}{z}_{ocf}$是惯性坐标系。

最优控制问题

取状态变量$\mathbf{x}={\left[\begin{array}{lllllc}{\dot{x}}_{ocf}& x_{ocf}& {\dot{y}}_{ocf}& y_{ocf}& {\dot{z}}_{ocf}& z_{ocf}\end{array}\right]}^T$，将上升制导转化为最优控制问题[1-2]
$$\begin{array}{c}J={\int }_0^{t_g}dt\\ \dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u}+C\\ {\mathbf{x}}_0={\left[{\dot{x}}_{ocfi}{x}_{ocfi}{\dot{y}}_{ocfi}{y}_{ocfi}{\dot{z}}_{ocfi}{z}_{ocfi}\right]}^T\\ {\mathbf{x}}_f={\left[{\dot{x}}_{ocff}{x}_{ocff}{\dot{y}}_{ocff}{y}_{ocff}{\dot{z}}_{ocff}{z}_{ocff}\right]}^T\end{array}\text{\hspace{1em}(2)}$$
其中$A=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]，B=F/m，\mathbf{u}=\left[\begin{array}{c}\cos {\phi }_{ocf}\cos {\psi }_{ocf}\\ 0\\ \sin {\phi }_{ocf}\cos {\psi }_{ocf}\\ 0\\ -\sin {\psi }_{ocf}\\ 0\end{array}\right]，C=\left[\begin{array}{c}{g}_{ocfx}\\ 0\\ g_{ocfy}\\ 0\\ g_{ocfz}\\ 0\end{array}\right]$

根据连续系统最优控制泛函求极值的必要条件可知[4]

极值条件：
$$\left[\begin{array}{c}\frac{\partial H}{\partial {\phi }_{ocf}}\\ \frac{\partial H}{\partial {\psi }_{ocf}}\end{array}\right]=0⟹\left[\begin{array}{c}{\lambda }_1\sin {\phi }_{ocf}\cos {\psi }_{ocf}-{\lambda }_3\cos {\phi }_{ocf}\cos {\psi }_{ocf}\\ {\lambda }_1\cos {\phi }_{ocf}\sin {\psi }_{ocf}+{\lambda }_3\sin {\phi }_{ocf}\sin {\psi }_{ocf}+{\lambda }_5\cos {\psi }_{ocf}\end{array}\right]=0\text{\hspace{1em}(3)}$$
其中$H$是与最优控制问题（2）对应的哈密顿函数。

伴随条件：
$$\dot{\mathbf{\lambda }}=-\frac{\partial H}{\partial \mathbf{x}}⟹\left[\begin{array}{c}{\dot{\lambda }}_1\\ {\dot{\lambda }}_2\\ {\dot{\lambda }}_3\\ {\dot{\lambda }}_4\\ {\dot{\lambda }}_5\\ {\dot{\lambda }}_6\end{array}\right]=\left[\begin{array}{c}-{\lambda }_2\\ 0\\ -{\lambda }_4\\ 0\\ -{\lambda }_6\\ 0\end{array}\right]\text{\hspace{1em}(4)}$$
横截条件：
$${\mathbf{\lambda }}^T\delta \mathbf{x}{|}_{t_f}=0⟹\left[\begin{array}{c}{\lambda }_1\delta {\dot{x}}_{ocf}\\ {\lambda }_2\delta x_{ocf}\\ {\lambda }_3\delta {\dot{x}}_{ocf}\\ {\lambda }_4\delta y_{ocf}\\ {\lambda }_5\delta {\dot{z}}_{ocf}\\ {\lambda }_6\delta z_{ocf}\end{array}\right]{|}_{t_f}=0\text{\hspace{1em}(5)}$$
解最优控制问题，可得[2]
$$\{\begin{array}{rl}\tan {\phi }_{ocf}=& \frac{\lambda 30-\lambda 40t}{\lambda 10}\\ \tan {\psi }_{ocf}=& \frac{-(\lambda 50-\lambda 60t)}{\lambda 10\cos {\phi }_{ocf}+(\lambda 30-\lambda 40t)\sin {\phi }_{ocf}}\end{array}\text{\hspace{1em}(6)}$$
俯仰角${\phi }_{ocf}$和偏航角${\psi }_{ocf}$的近似解[1-3]
$$\{\begin{array}{c}{\phi }_{ocf}\approx {\overline{\phi }}_{ocf}+K_{\phi 2}t-K_{\phi 1}\\ {\psi }_{ocf}\approx {\overline{\psi }}_{ocf}+K_{\psi 2}t-K_{\psi 1}\end{array}\text{\hspace{1em}(7)}$$
其中${\overline{\phi }}_{ocf}$和${\overline{\psi }}_{ocf}$是为了满足目标入轨点速度矢量产生的控制角，$K_{\phi 2}$ 、$K_{\phi 1}$、$K_{\psi 2}$、$K_{\psi 1}$是为了达到目标位置矢量产生的附加控制角参数[1-2]。下一节详述如何求解${\overline{\phi }}_{ocf}$、$K_{\phi 2}$、$K_{\phi 1}$等系数。

最优控制解的计算

本节将计算剩余飞行时间以及俯仰角${\phi }_{ocf}$和偏航角${\psi }_{ocf}$的近似解中的系数。

计算剩余飞行时间

利用雅克比迭代法求解下面这个非线性方程，可得剩余飞行时间[2]
$$t_g=t_h\left(1-e^{-\frac{\Delta V}{Vex}}\right)\text{\hspace{1em}(8)}$$
其中$\Delta V=\sqrt{{\left({\dot{x}}_{ocff}-{\dot{x}}_{ocfi}-g_{ocfx}{t}_g\right)}^2+{\left({\dot{y}}_{ocff}-{\dot{y}}_{ocfi}-g_{ocfy}{t}_g\right)}^2+{\left({\dot{z}}_{ocff}-{\dot{z}}_{ocfi}-g_{ocfz}{t}_g\right)}^2}$

传统的迭代制导方法使用下式计算引力[1-3]
$${\mathbf{g}}_{ocf}=\left[\begin{array}{l}{g}_{ocfx}\\ g_{ocfy}\\ g_{ocfz}\end{array}\right]=\frac{1}{2}(\left[\begin{array}{l}{g}_{ocfix}\\ g_{ocfiy}\\ g_{ocfiz}\end{array}\right]+\left[\begin{array}{l}{g}_{ocffx}\\ g_{ocffy}\\ g_{ocffz}\end{array}\right])\text{\hspace{1em}(9)}$$
其中${\mathbf{g}}_{ocfi}=[g_{ocfix}{g}_{ocfiy}{g}_{ocfiz}{]}^T$是火箭瞬时引力加速度，导航计算而得；${\mathbf{g}}_{ocff}=[g_{ocffx}{g}_{ocffy}{g}_{ocffz}{]}^T$是入轨点处引力加速度，迭代更新获取。

地心角计算

地心角$\varphi$是相应点在轨道平面投影形成的地心夹角。计算地心角是为了将目标轨道的轨道根数转化为制导坐标系中的速度和位置约束。

为了满足轨道倾角$i$，升交点赤经$\Omega$，要保证$z_{ocff}=0$，${\dot{z}}_{ocff}=0$。在入轨点轨道坐标系中$x_{ocff}=0$，为了满足半长轴$a$，偏心率$e$，则入轨点速度${\dot{x}}_{ocff}$、${\dot{y}}_{ocff}$和位置$y_{ocff}$要满足如下约束

$${\dot{x}}_{ocff}=(1+e\cos f)\sqrt{\frac{\mu }{a\left(1-e^2\right)}}\text{\hspace{1em}(10)}$$
$${\dot{y}}_{ocff}=e\sin f\sqrt{\frac{\mu }{a\left(1-e^2\right)}}\text{\hspace{1em}(11)}$$

$$y_{ocff}=\frac{a\left(1-e^2\right)}{1+e\cos f}\text{\hspace{1em}(12)}$$

火箭在入轨点的速度$V_{ocff}={\left[{\dot{x}}_{ocff}{\dot{y}}_{ocff}{\dot{z}}_{ocff}\right]}^T$
$$V_{ocff}=\sqrt{\mu \left(\frac{2}{y_{ocff}}-\frac{1}{a}\right)}\text{\hspace{1em}(13)}$$
定义入轨点处火箭的速度和当地水平面的夹角，即飞行路径角为$\beta$，则飞行路径角$\beta$和真近点角$f$的关系如下
$$\begin{array}{r}\cos \beta =\frac{r}{v}\dot{f}=\frac{1+e\cos f}{\sqrt{1+e^2+2e\cos f}}\\ \sin \beta =\frac{1}{v}\dot{r}=\frac{e\sin f}{\sqrt{1+e^2+2e\cos f}}\end{array}\}$$

$${\dot{x}}_{ocff}=V_{ocff}\cos \beta =(1+e\cos f)\sqrt{\frac{\mu }{a\left(1-e^2\right)}}\text{\hspace{1em}(14)}$$

$${\dot{y}}_{ocff}=V_{ocff}\sin \beta =e\sin f\sqrt{\frac{\mu }{a\left(1-e^2\right)}}\text{\hspace{1em}(15)}$$

如图 2 所示，真近点角$f$和地心角$\varphi$、近地点角距$\omega$有如下关系[2]
$$f=\varphi -\omega \text{\hspace{1em}(16)}$$
计算地心角$\varphi$的方法文献[2]写的好乱，这是文献[2]最让我头疼的地方，总结如下

地心角$\varphi$由两部分组成（见图 3），一部分是火箭瞬时在目标轨道平面投影点与目标轨道升交点的地心夹角${\varphi }_1$，另一部分是剩余飞行时间内火箭能飞过的地心角${\varphi }_2$

${\varphi }_1$按如下公式计算[2]
$${\varphi }_1=\arctan \frac{x_{rcf}}{y_{rcf}}\text{\hspace{1em}(17)}$$
其中$x_{rcf}$和$y_{rcf}$是运载火箭瞬时在升交点轨道坐标系中$x$轴和$y$轴的地心矢径分量。

${\varphi }_2$采用以下方法估算见图 4 ：通过火箭在剩余飞行时间里的轨迹估算以入轨点高度为半径圆弧的长度，圆弧对的圆心角就是${\varphi }_2$，因此[2]
$${\varphi }_2\approx \frac{[V{t}_g+S_{thrust}-k{t}_g(V+\Delta V-V_{ocff})]\cos \theta }{y_{ocff}}\text{\hspace{1em}(18)}$$
其中$V$是瞬时总速度，$\Delta V$是推力产生的速度增量，$V_{ocff}$是入轨点速度，$S_{thrust}$是$[0t_g]$时间段内推力产生的位移，$k$是重力损失修正常数[2]，$k$的求法下一小节讲，$\theta$是入轨点当地轨道倾角，若目标轨道为圆轨道则$\theta =0$。通过等式（18）计算${\varphi }_2$时要用到$y_{ocff}$，但是由等式（12）和（16）可知，计算$y_{ocff}$又需要$\varphi$，每一次迭代过程中计算${\varphi }_2$时，使用上一次迭代计算出的入轨点距离地心轨道高度$y_{ocff}$

姿态控制角计算

对推力的一次积分[2]
$${\mathbf{V}}_{thrust}=\left[\begin{array}{l}F0\left(t_g\right)\cos \left({\overline{\phi }}_{ocf}\right)+F0\left(t_g\right)K_{\phi 1}\sin \left({\overline{\phi }}_{ocf}\right)-F1\left(t_g\right)K_{\phi 2}\sin \left({\overline{\phi }}_{ocf}\right)\\ F0\left(t_g\right)\sin \left({\overline{\phi }}_{ocf}\right)-F0\left(t_g\right)K_{\phi 1}\cos \left({\overline{\phi }}_{ocf}\right)+F1\left(t_g\right)K_{\phi 2}\cos \left({\overline{\phi }}_{ocf}\right)\\ F0\left(t_g\right)K_{\psi 1}\cos \left({\overline{\psi }}_{ocf}\right)-F1\left(t_g\right)K_{\psi 2}\cos \left({\overline{\psi }}_{ocf}\right)+F0\left(t_g\right)\sin \left({\overline{\psi }}_{ocf}\right)\end{array}\right]\text{\hspace{1em}(19)}$$
其中$F0(t)$、$F1(t)$定义如下
$$F0(t)\triangleq {\int }_0^t\frac{V_{ex}}{t_h-t}dt=V_{ex}\ln \frac{t_h}{t_h-t}\text{\hspace{1em}(20)}$$
$$F1(t)\triangleq {\int }_0^t\frac{V_{ex}}{t_h-t}tdt=t_{h}F0(t)-V_{ex}t\text{\hspace{1em}(21)}$$

因为${\overline{\phi }}_{ocf}$和${\overline{\psi }}_{ocf}$是为了满足目标入轨点速度矢量产生的控制角，不希望$K_{\phi 2}t-K_{\phi 1}$和$K_{\psi 2}t-K_{\psi 1}$对入轨速度矢量产生影响，故下式成立
$${\mathbf{V}}_{thrust}=\left[\begin{array}{c}F0\left(t_g\right)\cos {\overline{\phi }}_{ocf}\cos {\overline{\psi }}_{ocf}\\ F0\left(t_g\right)\sin {\overline{\phi }}_{ocf}\cos {\overline{\psi }}_{ocf}\\ 0\end{array}\right]\text{\hspace{1em}(22)}$$
求解方程（22），可得$K_{\phi 2}$与$K_{\phi 1}$以及$K_{\psi 2}$与$K_{\psi 1}$之间的关系[5]
$$\begin{array}{l}F0\left(t_g\right)K_{\rho 1}=F1\left(t_g\right)K_{\phi 2}\\ F0\left(t_g\right)K_{\psi 1}=F1\left(t_g\right)K_{\psi 2}\end{array}\text{\hspace{1em}(23)}$$
对等式（1）进行二次积分可得[2]
$${\mathbf{R}}_{ocff}={\mathbf{R}}_{thrust}+{\mathbf{R}}_{gravity}+{\mathbf{V}}_{ocfi}{t}_g+{\mathbf{R}}_{ocfi}\text{\hspace{1em}(24)}$$
分量形式为[2]
$$\begin{gathered} \left[\begin{array}{c}{x}_{ocff}\\ y_{ocff}\\ z_{ocff}\end{array}\right]=\left[\begin{array}{c}F2\left(t_g\right)\cos \left({\overline{\phi }}_{ocf}\right)+F2\left(t_g\right)K_{\phi 1}\sin \left({\overline{\phi }}_{ocf}\right)-F3\left(t_g\right)K_{\phi 2}\sin \left({\overline{\phi }}_{ocf}\right)\\ F2\left(t_g\right)\sin \left({\overline{\phi }}_{ocf}\right)-F2\left(t_g\right)K_{\phi 1}\cos \left({\overline{\phi }}_{ocf}\right)+F3\left(t_g\right)K_{\phi 2}\cos \left({\overline{\phi }}_{ocf}\right)\\ F2\left(t_g\right)K_{\psi 1}\cos \left({\overline{\psi }}_{ocf}\right)-F3\left(t_g\right)K_{\psi 2}\cos \left({\overline{\psi }}_{ocf}\right)+F2\left(t_g\right)\sin \left({\overline{\psi }}_{ocf}\right)\end{array}\right] \\ +\left[\begin{array}{c}\frac{1}{2}{g}_{ocfx}{t}_g^2\\ \frac{1}{2}{g}_{ocfy}{t}_g^2\\ \frac{1}{2}{g}_{ocfz}{t}_g^2\end{array}\right]+\left[\begin{array}{c}{\dot{x}}_{ocfi}{t}_g\\ {\dot{y}}_{ocfi}{t}_g\\ {\dot{z}}_{ocfi}{t}_g\end{array}\right]+\left[\begin{array}{c}{x}_{ocfi}\\ y_{ocfi}\\ z_{ocfi}\end{array}\right]\text{\hspace{1em}(25)} \end{gathered}$$

其中$F2(t)$、$F3(t)$定义如下
$$F2(t)\triangleq {\int }_0^t{\int }_0^s\frac{V_{ex}}{t_h-t}dtds=F0(t)\cdot t-F1(t)\text{\hspace{1em}(26)}$$

$$F3(t)\triangleq {\int }_0^t{\int }_0^s\frac{V_{ex}}{t_h-t}tdtds=F2(t)\cdot t_h-\frac{V_{ex}}{2}{t}^2\text{\hspace{1em}(27)}$$

根据等式（23）和（25）可得[2][5]
$$K_{\phi 1}=\frac{y_{ocff}-F2\left(t_g\right)\sin \left({\overline{\phi }}_{ocf}\right)-\frac{1}{2}{g}_{ocfy}{t}_g^2-{\dot{y}}_{ocfi}{t}_g-y_{ocfi}}{\left[-F2\left(t_g\right)+\frac{F3\left(t_g\right)F0\left(t_g\right)}{F1\left(t_g\right)}\right]\cos \left({\overline{\phi }}_{ocf}\right)}\text{\hspace{1em}(28)}$$

$$K_{\phi 2}=\frac{\left[y_{ocff}-F2\left(t_g\right)\sin \left({\overline{\phi }}_{ocf}\right)-\frac{1}{2}{g}_{ocfy}{t}_g^2-{\dot{y}}_{ocfi}{t}_g-y_{ocfi}\right]F0\left(t_g\right)}{\left[-F2\left(t_g\right)F1\left(t_g\right)+F3\left(t_g\right)F0\left(t_g\right)\right]\cos \left({\overline{\phi }}_{ocf}\right)}\text{\hspace{1em}(29)}$$

$$K_{\psi 1}=\frac{z_{ocff}-\frac{1}{2}{g}_{ocfz}{t}_g^2-{\dot{z}}_{ocfi}{t}_g-z_{ocfi}+F2\left(t_g\right)\sin \left({\overline{\psi }}_{ocf}\right)}{\left[F2\left(t_g\right)-\frac{F3\left(t_g\right)F0\left(t_g\right)}{F1\left(t_g\right)}\right]\cos \left({\overline{\psi }}_{ocf}\right)}\text{\hspace{1em}(30)}$$

$$K_{\psi 2}=\frac{\left[z_{ocff}-\frac{1}{2}{g}_{ocfz}{t}_g^2-{\dot{z}}_{ocfi}{t}_g-z_{ocfi}+F2\left(t_g\right)\sin \left({\overline{\psi }}_{ocf}\right)\right]F0\left(t_g\right)}{\left[F2\left(t_g\right)F1\left(t_g\right)-F3\left(t_g\right)F0\left(t_g\right)\right]\cos \left({\overline{\psi }}_{ocf}\right)}\text{\hspace{1em}(31)}$$

计算重力损失修正常数$k$

火箭瞬时速度矢量在目标轨道平面的投影记为${\mathbf{V}}_{xy}$，如果火箭以速度${\mathbf{V}}_{xy}$匀速飞行，飞行的航程是$\Vert {\mathbf{V}}_{xy}\Vert t$，但是飞行中还有推力和引力用，公式（26）计算出了推力产生的位移为$F2(t_g)$，迭代制导原始文献[3]中将引力对位移的影响设为$k{t}_g^2$，其中$k$就是重力损失修正常数，该常数与飞行任务有关。记火箭轨迹在轨道平面内投影的长度为$S_1$，则$S_1=\Vert {\mathbf{V}}_{xy}\Vert t_g+F2(t_g)+k{t}_g^2$

文献[1]使用了另一种方法估计火箭轨迹在轨道平面内投影的长度。求视加速度$F/m$在升交点轨道坐标系中的分量$W_{rcfx}、W_{rcfy}、W_{rcfz}$（实际时由导航系统输入），在轨道面内的耗尽时间$t_{horbit}=V_{ex}/\sqrt{W_{rcfx}^2+W_{rcfy}^2}$，用$t_{horbit}$替换$F0(t)$、$F1(t)$、$F2(t)$中的$t_h$可得
$$F0(t{)}_{orbit}\triangleq {\int }_0^t\frac{V_{ex}}{t_{horbit}-t}dt=V_{ex}\ln \frac{t_{horbit}}{t_{horbit}-t}\text{\hspace{1em}(32)}$$

$$F1(t{)}_{orbit}\triangleq {\int }_0^t\frac{V_{ex}}{t_{horbit}-t}tdt=t_{horbit}F0(t{)}_{orbit}-V_{ex}t\text{\hspace{1em}(33)}$$

$$F2(t{)}_{orbit}\triangleq {\int }_0^t{\int }_0^s\frac{V_{ex}}{t_{horbit}-t}dtds=F0(t{)}_{orbit}\cdot t-F1(t{)}_{orbit}\text{\hspace{1em}(34)}$$

下图中，${\mathbf{r}}_{xy}$表示火箭瞬时位置矢量在目标轨道平面的投影，${\mathbf{V}}_{xy}$表示火箭瞬时速度矢量在目标轨道平面的投影，火箭瞬时的当地轨道倾角记为${\theta }_H$

$$\cos (90^{\circ }-{\theta }_H)=\frac{{\mathbf{r}}_{xy}^T{\mathbf{V}}_{xy}}{\Vert {\mathbf{r}}_{xy}\Vert \Vert {\mathbf{V}}_{xy}\Vert }$$

$$⟹\cos ({\theta }_H)=\frac{|x\dot{y}-y\dot{x}|}{\Vert {\mathbf{r}}_{xy}\Vert \Vert {\mathbf{V}}_{xy}\Vert }\text{\hspace{1em}(35)}$$

其中${\mathbf{r}}_{xy}=[xy{]}^T$，${\mathbf{V}}_{xy}=[\dot{x}\dot{y}{]}^T$

已知入轨点当地的轨道倾角$\theta$，用文献[1]的方法求解火箭轨迹在轨道平面内投影的长度$S_2$的公式如下[1]
$$S_2\approx \Vert {\mathbf{V}}_{xy}\Vert t_g\cos {\theta }_H+F2(t_g{)}_{orbit}\cos \theta \text{\hspace{1em}(36)}$$
用$S_2$近似$S_1$，即$S_1\approx S_2$，可得[2]
$$\Vert {\mathbf{V}}_{xy}\Vert t_g+F2(t_g)+k{t}_g^2\approx \Vert {\mathbf{V}}_{xy}\Vert t_g\cos {\theta }_H+F2(t_g{)}_{orbit}\cos \theta$$

$$⟹k\approx \frac{\Vert {\mathbf{V}}_{xy}\Vert t_g(\cos {\theta }_H-1)+F2(t_g{)}_{orbit}\cos \theta -F2(t_g)}{t_g^2}\text{\hspace{1em}(37)}$$

仿真

用低地球轨道（LEO）任务进行仿真验证，任务简图如图 6，仿真参数如下表[8]

装订项	值
制导开始时刻的质量$m_0$	$86500kg$
发动机真空推力$T_{vac}$	$8\times 10^{5}N$
秒耗量$\dot{m}$	$272kg/s$
发射点赤经	$10^{\circ }$
发射点经度	$100^{\circ }\text{E}$
发射点地理维度$B_0$	$41.1906^{\circ }\text{N}$
目标轨道半长轴$a$	$6622785.34m$
目标轨道偏心率$e$	$0$
目标轨道倾角$i$	$68.846^{\circ }$
目标轨道升交点赤经$\Omega$	$-9.964^{\circ }$
目标轨道近地点幅角$\omega$	$0^{\circ }$
目标入轨点对应的真近点角$f$	$49.6823^{\circ }$
制导开始时刻火箭相对于发射惯性坐标系的位置${\mathbf{r}}_0$	$\left[2.9058\times 10^{5}92126-33401\right]m$
制导开始时刻火箭相对于发射惯性坐标系的速度${\mathbf{v}}_0$	$\left[2815.75494.70370.953\right]m/s$
制导周期$\Delta t$	$2s$
制导开始时刻火箭俯仰角	$61.3383^{\circ }$
制导开始时刻火箭偏航角	$0.3105^{\circ }$

仿真结果

入轨点轨道根数如下表

入轨点轨道六根数	仿真得到的数值
轨道半长轴$a$	$6.5608\times 10^6$
偏心率$e$	0.0162
轨道倾角$i$	$68.8340^{\circ }$
升交点赤经$\Omega$	$-9.9840^{\circ }$
近地点幅角$\omega$	$-68.3610^{\circ }$
入轨点真近点角$f$	$57.5899^{\circ }$

从图7可看出，火箭俯仰角$\phi$随时间变化的趋势在开始时刻有点异常。进行单步调试相关参数变化如下变

count	$f/deg$	${\bar{\phi }}_{ocf}/deg$	$t_{g0}/s$	$t_g/s$	$\varphi /deg$
1	57.6823	14.9774	260	261.2839	57.6823
2	58.0049	14.8186	258	259.4437	58.0049
3	57.9859	14.4737	256	257.5097	57.9859
4	579736	14.3217	254	255.6799	57.9736
5	57.9615	14.1649	252	253.8467	57.9610
…	…	…	…	…	…

从表中数据可以看出入轨点真近点角$f$是产生该突变的原因，如果调整仿真初始条件表中的入轨点真近点角$f$可以消除突变。当入轨点真近点角$f=58.0149^{\circ }$时火箭俯仰角变化如图11

参考文献

[1] 韩祝斋. 用于大型运载火箭的迭代制导方法[J]. 宇航学报, 1983, 4(1):12-24.
[2] 李伟. 基于精确控制解的运载火箭迭代制导自适应性分析研究[D]. 哈尔滨工业大学, 2012.
[3] Chandler D C , Smith I E . Development of the iterative guidance mode with its application to various vehicles and missions.[J]. Journal of Spacecraft and Rockets, 1967, 4(7):898-903.
[4] 杨军, 朱学平,等. 飞行器最优控制[M]. 西北工业大学出版社, 2011.
[5] 韩业鹏. 运载火箭上升段动力故障自适应制导研究[D]. 哈尔滨工业大学, 2016.
[6] 陈新民, 余梦伦. 迭代制导在运载火箭上的应用研究[J]. 宇航学报, 2003, 24(5):484-489.
[7] 周国财. 运载火箭迭代制导方法研究[D]. 西北工业大学, 2003.
[8] 升力式天地往返飞行器自主制导方法研究[D]. 哈尔滨工业大学, 2012.