陆平老师论文Closed-Loop Endoatmospheric Ascent Guidance读后总结

Closed-Loop Endoatmospheric Ascent Guidance

作者信息：
Ping Lu，爱荷华州立大学，爱荷华州艾姆斯市
Hongsheng Sun
Bruce Tsai

总结人： 鲁鹏，北京理工大学宇航学院
2019.05.09

文章结构

1. 引言
2. 大气层内上升制导问题的数学模型
   1. 上升段动力学方程
   2. 最优控制问题
   3. 确定$\Phi$和$\alpha$的符号
   4. 添加路径约束
3. 数值方法
   1. 有限差分方法
   2. 更新算法
   3. Continuation on Atmospheric Density和初值猜测
4. 仿真
   1. 终端条件和上升时间调整
   2. 开环解
   3. 闭环仿真
5. 结论

• 附录A：三维最优上升问题的协态微分方程
• 附录B：真空最优上升问题解析解

收获

1. 运载器的在惯性坐标系中的动力学方程中的$\mathbf{r}$是运载器的地心位置矢量
2. 质量变化率公式$\dot{m}=-\eta T_{vac}/(g_0{I}_{sp})$中的$g_0$是地球表面的重力加速度。探测器月球表面软着陆也使用这个质量变化公式，$g_0$ 依然是地球表面的重力加速度
3. 地球自传给垂直发射运载器带来水平方向（相对于发射惯性坐标系）的初始速度
4. 运载器上升段制导中，常用$J=1/r_f-V_f^2/2$ 作为目标函数，使$J$最小，则末端能量$-1/r_f+V_f^2/2$最大

优缺点

to be added …

难理解的部分

约束$S_1=q\alpha -Q_{\alpha }\le 0$零阶约束，因为控制量$1_b$直接出现在约束中（通过$\alpha$），约束$S_3=q-q_{max}\le 0$是一阶约束，因为控制量$1_b$出现在$S_3$对时间的一阶导数中。

公式推导

文章中的公式有很多值得注意的细节，文章中的错误也一并记录下来。

1. 真空最优上升解

真空主动段动力学方程

归一化后的方程[1]
$$\begin{array}{rl}\dot{\mathbf{r}}=& \mathbf{V}\\ \dot{\mathbf{V}}=-\mathbf{r}+& T(\tau )1_b\end{array}$$

动力学方程可以简化为这种形式的前提是引力加速度做如下近似
$$\mathbf{g}=-({\mu }_E/r_0^3)\mathbf{r}=-{\omega }^2\mathbf{r}$$

为了减小引力简化带来的误差，在每一次制导环的开始更新$r_0$.

该方程的解析解如下所示[1-3]
$$\left[\begin{array}{c}{\mathbf{p}}_v(\tau )\\ -{\mathbf{p}}_r(\tau )\end{array}\right]=\left[\begin{array}{cc}\cos \tau I_3& \sin \tau I_3\\ -\sin \tau I_3& \cos \tau I_3\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_{v0}\\ -{\mathbf{p}}_{r0}\end{array}\right]=\Omega (\tau )\left[\begin{array}{c}{\mathbf{p}}_{v0}\\ -{\mathbf{p}}_{r0}\end{array}\right]$$

$$\left[\begin{array}{c}\mathbf{r}(\tau )\\ \mathbf{V}(\tau )\end{array}\right]=\Omega (\tau )\left[\begin{array}{c}{\mathbf{r}}_0\\ {\mathbf{V}}_0\end{array}\right]+\Gamma (\tau )\mathrm{W}$$

$$\Gamma (\tau )=\left[\begin{array}{cc}\sin \tau I_3& -\cos \tau I_3\\ \cos \tau I_3& \sin \tau I_3\end{array}\right]$$

$$\mathrm{W}=\left[\begin{array}{c}{I}_c\\ I_s\end{array}\right]=\left[\begin{array}{c}{\int }_0^{\tau }{1}_{pv}(\zeta )\cos (\zeta )T(\zeta )d\zeta \\ {\int }_0^{\tau }{1}_{pv}(\zeta )\sin (\zeta )T(\zeta )d\zeta \end{array}\right]$$
其中，$I_3$是$3\times 3$的单位矩阵。通过公式可以看出初始协态变量的模$({\mathbf{p}}_{v0}^T{\mathbf{p}}_{v0}+{\mathbf{p}}_{r0}^T{\mathbf{p}}_{r0})$对协态变量之后的方向变化没有任何影响，只影响协态变量的大小。考虑到状态的解析解只和$1_{pv}$有关，所以初始协态变量的模对状态也没有影响。

质量推力和发动机阀门

首先声明，程序中我在求解质量推力和发动机阀门大小时未对质量和时间归一化，只是在每个时间区间用对应的引力加速度对推力加速度$T(t)$进行了归一化。

1. 当$T<T_{max}$时

$$\begin{array}{rl}m(t)=& m_0-\frac{T_{vac}}{c}t\\ T(t)=& \frac{T_{vac}}{m(t)g_0}\\ \eta (t)=& 1\end{array}$$

其中，$T_{vac}$是发动机真空推力，$g_0$是运载器所在某时间区间$[t_i,t_i+\tau ]$内的引力加速度值，喷嘴等效气流速度（nozzle exit velocity）$c=g_E{I}_{sp}$，$g_E$是地球表面重力加速度，$m(t)$是运载器质量，$m_0$是运载器质量初始值，$T(t)$是被$g_0$归一化后的推力加速度，$\eta$是发动机阀门。
随着$T$不断增大，假设在$t_c$时刻，推力加速度达到最大（取$T_{max}=4g_e$），此后推力加速度$T$保持最大，并且$t_c$时刻在$[t_i,t_i+\tau ]$内，那么
$$\frac{T_{vac}}{[m_0-(T_{vac}/c)t_c]g_0}=\frac{T_{max}}{g_0}$$

所以
$$t_c=c(\frac{m_0}{T_{vac}}-\frac{1}{T_{max}})$$

2. 当$T=T_{max}$后，即$t⩾t_c$时

$$\begin{array}{rl}m(t)=& m_{t_c}{e}^{-(T_{max}/c)t}\\ T(t)=& \frac{T_{max}}{g_0}\\ \eta (t)=& \frac{T_{max}m(t)}{T_{vac}}\end{array}$$

此时的$g_0$是某时间区间$[t_j,t_j+\tau ]$（注意$t_j+\tau ⩾t_c$）内引力加速度，$T(t)$是被$g_0$归一化后的推力加速度，$m_{t_c}$是$t_c$时刻运载器的质量。

垂直上升结束时运载器在发射点惯性坐标系中的位置和速度

运载器在垂直上升结束后制导率才开始工作，不同运载器有不同的垂直上升时间$t_0$。文献[5]中垂直上升时间$t_0$取9s，文献[1]中垂直上升时间$t_0$取5s。
垂直上升阶段运载器推力加速度满足最大推力加速度约束，此时运载器所受加速度
$$a_{vehicle}(t)=\frac{T_{vac}-m(t)g_E}{m(t)}$$

所以，对加速度积分可以得到竖直上升的速度$v_{0x}(t)$
$$\begin{array}{rl}{v}_0=& [v_{0x}(t_0),v_{0y},v_{0z}{]}^T\\ v_{0x}(t)=-c\ln (& m_0-\frac{T_{vac}}{c}t)-g_{E}t+c\ln (m_0)\end{array}$$

其中$v_{0y},v_{0z}$是由地球自转引起的水平面内的速度在y和z轴上的分量。对速度$v_{0x}(t)$积分可得$r_{0x}(t)$
$$\begin{array}{rl}{r}_0=[r_{0x}(t_0),v_{0y}& t_0,v_{0z}{t}_0{]}^T\\ r_{0x}(t)=\frac{c^2}{T_{vac}}(m_0-\frac{T_{vac}}{c}t)[\ln (& m_0-\frac{T_{vac}}{c}t)-1]-\frac{1}{2}{g}_E{t}^2+\\ c\ln (m_0)t+\frac{c^2}{T_{vac}}{m}_0[& 1-\ln (m_0)]\end{array}$$

终端约束和横截条件得到的约束方程$\Psi =0$

方程如下[1]
$$\frac{1}{2}{\mathbf{r}}_f^T{\mathbf{r}}_f-\frac{1}{2}{r}_f^{\ast 2}=0$$

$$1_N^T({\mathbf{r}}_f\times {\mathbf{V}}_f)-\Vert {\mathbf{r}}_f\times {\mathbf{V}}_f\Vert \cos i^{\ast }=0$$

$${\mathbf{r}}_f^T{\mathbf{V}}_f-r_f{V}_f\sin {\gamma }_f^{\ast }=0$$

$$({\mathbf{V}}_f^T{\mathbf{p}}_{rf})r_f^2-({\mathbf{r}}_f^T{\mathbf{p}}_{Vf})V_f^2+({\mathbf{r}}_f^T{\mathbf{V}}_f)(V_f^2-{\mathbf{r}}_f^T{\mathbf{p}}_{rf})=0$$

$${\mathbf{V}}_f^T{\mathbf{p}}_{Vf}-V_f^2=0$$

$$({\mathbf{h}}_f^T{\mathbf{p}}_{rf})[{\mathbf{h}}_f^T({\mathbf{r}}_f\times 1_N)]+({\mathbf{h}}_f^T{\mathbf{p}}_{Vf})[{\mathbf{h}}_f^T({\mathbf{V}}_f\times 1_N)]=0$$

求$\Psi$对状态和协态变量的导数

$f_1$对状态变量和协态变量求导
$$\frac{\partial f_1}{\partial {\mathbf{r}}_f}={\mathbf{r}}_f,\frac{\partial f_1}{\partial {\mathbf{V}}_f}=0,\frac{\partial f_1}{\partial {\mathbf{p}}_{Vf}}=0,\frac{\partial f_1}{\partial {\mathbf{p}}_{rf}}=0$$

$f_2$对状态变量和协态变量求导
$$\frac{\partial f_2}{\partial {\mathbf{r}}_f}={\mathbf{V}}_f^{\times }{1}_N+({\mathbf{V}}_f^{\times }{)}^T({\mathbf{r}}_f\times {\mathbf{V}}_f)\frac{\cos i^{\ast }}{\Vert {\mathbf{r}}_f\times {\mathbf{V}}_f\Vert }$$

$$\frac{\partial f_2}{\partial {\mathbf{V}}_f}=({\mathbf{r}}_f^{\times }{)}^T{1}_N+{\mathbf{r}}_f^{\times }({\mathbf{r}}_f\times {\mathbf{V}}_f)\frac{\cos i^{\ast }}{\Vert {\mathbf{r}}_f\times {\mathbf{V}}_f\Vert }$$

$$\frac{\partial f_2}{\partial {\mathbf{p}}_{Vf}}=0,\frac{\partial f_2}{\partial {\mathbf{p}}_{rf}}=0$$

$f_3$对状态变量和协态变量求导
$$\frac{\partial f_3}{\partial {\mathbf{r}}_f}={\mathbf{V}}_f-(V_f\sin {\gamma }^{\ast }){\mathbf{r}}_f/r_f$$

$$\frac{\partial f_3}{\partial {\mathbf{V}}_f}={\mathbf{r}}_f-(r_f\sin {\gamma }^{\ast }){\mathbf{V}}_f/V_f$$

$$\frac{\partial f_3}{\partial {\mathbf{p}}_{Vf}}=0,\frac{\partial f_3}{\partial {\mathbf{p}}_{rf}}=0$$

$f_4$对状态变量和协态变量求导
$$\frac{\partial f_4}{\partial {\mathbf{r}}_f}=2({\mathbf{V}}_f^T{\mathbf{p}}_{rf}){\mathbf{r}}_f-V_f^2{\mathbf{p}}_{vf}+V_f^2{\mathbf{V}}_f-({\mathbf{r}}_f^T{\mathbf{p}}_{rf}){\mathbf{V}}_f-({\mathbf{r}}_f^T{\mathbf{V}}_f){\mathbf{p}}_{rf}$$

$$\frac{\partial f_4}{\partial {\mathbf{V}}_f}=r_f^2{\mathbf{p}}_{rf}-2({\mathbf{r}}_f^T{\mathbf{p}}_{Vf}){\mathbf{V}}_f+V_f^2{\mathbf{r}}_f+2({\mathbf{r}}_f^T{\mathbf{V}}_f){\mathbf{V}}_f-({\mathbf{r}}_f^T{\mathbf{p}}_{rf}){\mathbf{r}}_f$$

$$\frac{\partial f_4}{\partial {\mathbf{p}}_{Vf}}=-V_f^2{\mathbf{r}}_f,\frac{\partial f_4}{\partial {\mathbf{p}}_{rf}}=r_f^2{\mathbf{V}}_f-({\mathbf{r}}_f^T{\mathbf{V}}_f){\mathbf{r}}_f$$

$f_5$对状态变量和协态变量求导
$$\frac{\partial f_5}{\partial {\mathbf{r}}_f}=0,\frac{\partial f_5}{\partial {\mathbf{V}}_f}={\mathbf{p}}_{Vf}-2{\mathbf{V}}_f$$

$$\frac{\partial f_5}{\partial {\mathbf{p}}_{Vf}}={\mathbf{V}}_f,\frac{\partial f_5}{\partial {\mathbf{p}}_{rf}}=0$$

$f_6$对状态变量和协态变量求导
$$\begin{array}{rl}\frac{\partial f_6}{\partial {\mathbf{r}}_f}=& [{\mathbf{h}}_f^T({\mathbf{r}}_f\times 1_N)]{\mathbf{V}}_f^{\times }{\mathbf{p}}_{rf}+({\mathbf{h}}_f^T{\mathbf{p}}_{rf})[({\mathbf{V}}_f^{\times }{)}^T{1}_N^{\times }+(1_N^{\times }{)}^T{\mathbf{V}}_f^{\times }]{\mathbf{r}}_f\\ & +[{\mathbf{h}}_f^T({\mathbf{V}}_f\times 1_N)]{\mathbf{V}}_f^{\times }{\mathbf{p}}_{Vf}+({\mathbf{h}}_f^T{\mathbf{p}}_{Vf})[({\mathbf{V}}_f^{\times }{)}^2{1}_N]\end{array}$$

$$\begin{array}{rl}\frac{\partial f_6}{\partial {\mathbf{V}}_f}=& [{\mathbf{h}}_f^T({\mathbf{r}}_f\times 1_N)]({\mathbf{r}}_f^{\times }{)}^T{\mathbf{p}}_{rf}+({\mathbf{h}}_f^T{\mathbf{p}}_{rf})[-({\mathbf{r}}_f^{\times }{)}^2{1}_N]\\ & +[{\mathbf{h}}_f^T({\mathbf{V}}_f\times 1_N)]({\mathbf{r}}_f^{\times }{)}^T{\mathbf{p}}_{Vf}+({\mathbf{h}}_f^T{\mathbf{p}}_{Vf})({\mathbf{r}}_f^{\times }{1}_N^{\times }+1_N^{\times }{\mathbf{r}}_f^{\times }){\mathbf{V}}_f\end{array}$$

$$\frac{\partial f_6}{\partial {\mathbf{p}}_{Vf}}=[{\mathbf{h}}_f^T({\mathbf{V}}_f\times 1_N)]{\mathbf{h}}_f$$

$$\frac{\partial f_6}{\partial {\mathbf{p}}_{rf}}=[{\mathbf{h}}_f^T({\mathbf{r}}_f\times 1_N)]{\mathbf{h}}_f$$

计算雅克比矩阵

计算雅克比矩阵，用于牛顿迭代法求解协态变量初值，过程跟参考文献[2]方法相同。考虑到完整性，在下面列出推导过程

定义$U^T=\left[S^T{\lambda }^T\right]$，$J^T=\left[I_c^T(\tau )I_s^T(\tau )\right]$，其中$\tau =t-t_i$，则有下式成立
$$dU\left(t_i+\tau \right)=B(\tau )dU\left(t_i\right),d{U}^T\left(t_0\right)=\left[\begin{array}{ll}0& d{\lambda }_0^T\end{array}\right]$$

其中
$$B(\tau )=\left[\begin{array}{cc}\Omega & \Gamma \frac{d\mathbf{J}(\tau )}{d\mathbf{\lambda }\left(t_i\right)}\\ 0& \Omega \end{array}\right]=\left[\begin{array}{cc}\Omega & \Gamma (\tau )\left[\begin{array}{c}d{I}_c/d\lambda (t_i)\\ d{I}_s/d\lambda (t_i)\end{array}\right]\\ 0& \Omega \end{array}\right]$$

$$K(\tau )=\frac{\cos (\tau )T(\tau )}{\Vert {\mathbf{p}}_V(\tau )\Vert }[I_3-\frac{{\mathbf{p}}_V(\tau )({\mathbf{p}}_V(\tau ){)}^T}{{\Vert {\mathbf{p}}_V(\tau )\Vert }^2}][\cos (\tau )I_3\sin (\tau )I_3]$$

$$\frac{d{I}_c(\tau )}{d\lambda (t_i)}=\frac{\tau [7K(0)+32K(\delta )+12K(2\delta )+32K(3\delta )+7K(4\delta )]}{90}$$

$$\frac{d{I}_s(\tau )}{d\lambda (t_i)}=\frac{\tau [32K(\delta )\tan (\delta )+12K(2\delta )\tan (2\delta )+32K(3\delta )\tan (3\delta )+7K(4\delta )\tan (4\delta )]}{90}$$

$$\Theta =\prod _{i=1}^{M-1}B(t_{i+1}-t_i)$$

$$d\Psi ={\Psi }_{S}d{S}_f+{\Psi }_{\lambda }d{\lambda }_f=({\Psi }_S{\Theta }_{12}+{\Psi }_{\lambda }{\Theta }_{22})d{\lambda }_0$$

其中${\Theta }_{12}$和${\Theta }_{22}$是矩阵$\Theta$的分块矩阵，因此雅克比矩阵$\mathrm{J}\mathrm{a}\mathrm{c}\mathrm{o}\mathrm{b}$
$$\mathrm{J}\mathrm{a}\mathrm{c}\mathrm{o}\mathrm{b}=\frac{d\Psi }{d{\lambda }_0}={\Psi }_S{\Theta }_{12}+{\Psi }_{\lambda }{\Theta }_{22}$$

2. 大气层内上升解

作者将大气层内上升问题转化为了两点边值问题(TPBVPs)，并使用有限差分法求解两点边值问题。解两点边值问题之前先要推导状态方程和协态方程中涉及的相关方程。

过程约束的处理

处理不等式约束时，当约束$S<0$时，系统的协态变量微分方程取$-\partial H/\partial \mathbf{x}$当约束$S=0$时，才考虑将约束相关的项加入协态变量微分方程。
约束$S_1=q\alpha -Q_{\alpha }\le 0$
当$S_1=0$时
$$\begin{array}{rl}\frac{\partial q}{\partial \mathbf{r}}=& \frac{\partial (\frac{1}{2}\rho V_r^2)}{\partial \mathbf{r}}=\frac{1}{2}{V}_r^2\frac{\partial \rho }{\partial \mathbf{r}}+\rho \frac{\partial V_r}{\partial \mathbf{r}}\\ =& \frac{1}{2}{V}_r^2{\rho }_r\frac{\mathbf{r}}{r}+\rho {\bar{\omega }}_E^{\times }{\mathbf{V}}_r\end{array}$$

$$\begin{array}{rl}\cos \alpha =1_b^T{1}_{Vr}& \Rightarrow -\sin \alpha \frac{\partial \alpha }{\partial \mathbf{V}}=\frac{\partial (1_b^T{1}_{Vr})}{\partial \mathbf{V}}\\ & \Rightarrow -\sin \alpha \frac{\partial \alpha }{\partial \mathbf{V}}=\frac{1}{V_r}\frac{\partial (1_b^T{\mathbf{V}}_r)}{\partial \mathbf{V}}+1_b^T{\mathbf{V}}_r\frac{\partial \frac{1}{V_r}}{\partial \mathbf{V}}\\ & \Rightarrow \frac{\partial \alpha }{\partial \mathbf{V}}=\frac{1}{V_r\sin \alpha }(\cos \alpha 1_{Vr}-1_b)\end{array}$$

$$\begin{array}{rl}\frac{\partial \alpha }{\partial \mathbf{r}}=& {\left(\frac{\partial \mathbf{V}}{\partial \mathbf{r}}\right)}^T\frac{\partial \alpha }{\partial \mathbf{V}}\\ =& ({\bar{\omega }}_E^{\times }{)}^T\frac{q}{V_r\sin \alpha }(\cos \alpha 1_{Vr}-1_b)\end{array}$$

$$\begin{array}{rl}\frac{\partial S_1}{\partial \mathbf{r}}=& \frac{\partial (q\alpha )}{\partial \mathbf{r}}=\alpha \frac{\partial q}{\partial \mathbf{r}}+q\frac{\partial \alpha }{\partial \mathbf{r}}\\ =& \frac{1}{2}\alpha {\rho }_r{V}_r^2\frac{\mathbf{r}}{r}+\alpha \rho {\bar{\omega }}_E^{\times }{\mathbf{V}}_r+({\bar{\omega }}_E^{\times }{)}^T\frac{q}{V_r\sin \alpha }(\cos \alpha 1_{Vr}-1_b)\end{array}$$

$$\begin{array}{rl}\frac{\partial S_1}{\partial \mathbf{V}}=& \frac{\partial (q\alpha )}{\partial \mathbf{V}}=\alpha \frac{\partial q}{\partial \mathbf{V}}+q\frac{\partial \alpha }{\partial \mathbf{V}}\\ =& \alpha \rho {\mathbf{V}}_r+\frac{q}{V_r\sin (\alpha )}(\cos \alpha 1_{Vr}-1_b)\end{array}$$

其中，${\rho }_r=\partial \rho /\partial r$。

约束$S_2=T-T_{max}\le 0$
当$S_2=0$时，发动机的阀门根据以下公式调节
$$\eta =T_{max}m(t)g_0/T_{vac}$$

约束$S_3=q-q_{max}\le 0$
文章中对约束$S_3$的处理那块儿，公式有点没弄懂！！当$S_3=0$时，作者将动压约束转化为了对发动机阀门大小$\eta$的约束，联立以下四个方程[1]
$$\begin{array}{rl}{\mathbf{V}}^{\prime }=& -\frac{1}{r^3}\mathbf{r}+\mathbf{A}+T{1}_b+\mathbf{N}\\ T=& \eta \left[T_{vac}+\Delta T(r)\right]/(m(t)g_E)\\ {\mathbf{V}}_r^{\prime }=& {\mathbf{V}}^{\prime }-{\bar{\omega }}_E\times {\mathbf{V}}^{\prime }-{\mathbf{V}}_w\\ S_3^{\prime }=& (1/(2r)){\rho }_r{V}_r^2{\mathbf{r}}^T\mathbf{V}+\rho {\mathbf{V}}_r^T{\mathbf{V}}_r^{\prime }\end{array}$$

可知
$$q^{\prime }(t)=a_q(\mathbf{x},1_b)\eta (t)+b_q(\mathbf{x},1_b)$$

其中
$$a_q(\mathbf{x},1_b)=...$$

$$b_q(\mathbf{x},1_b)=...$$

另$\delta >0$则有
$$\begin{array}{r}q(t+\delta )\approx q(t)+q^{\prime }(t)\delta \end{array}$$

根据动压约束$q(t+\delta )-q_{max}\le 0$
$$\begin{array}{r}\eta (t)\le \frac{q_{\max }-q(t)-b_q\delta }{a_q\delta }\triangleq {\eta }_q\end{array}$$

将由约束$S_2$求得的$\eta$记为${\eta }_{\mathrm{p}\mathrm{r}\mathrm{b}}$，$\eta$的最小值是${\eta }_{\min }$，那么同时考虑约束2和3，$\eta$的取值方法如下[1]
$$\begin{array}{r}\eta =\{\begin{array}{lll}{\eta }_{\mathrm{p}\mathrm{r}\mathrm{b}},& \text{ if }& {\eta }_q>{\eta }_{\mathrm{p}\mathrm{r}\mathrm{b}}\\ {\eta }_q,& \text{ if }& {\eta }_{\min }\le {\eta }_q\le {\eta }_{\mathrm{p}\mathrm{r}\mathrm{b}}\\ {\eta }_{\mathrm{m}\mathrm{i}\mathrm{n}},& \text{ if }& {\eta }_q<{\eta }_{\min }\end{array}\end{array}$$

密度、声速、推力对地心距求导

密度
已知是空气密度是关于运载器所在高度的函数，即${\rho }_1=f(h_1)$
$$\begin{array}{rl}{\rho }_r=\frac{\partial \rho }{\partial r}=& \frac{\partial \rho }{\partial (1+h)}=\frac{\partial \rho }{\partial h}\end{array}\text{(归一化方程)}$$

未归一化密度${\rho }_1={\rho }_0\rho$，单位$kg/m^3$，未归一化运载器高度$h_1=R_{0}h/1000$，单位$km$
$$\begin{array}{r}{\rho }_r=\frac{R_0}{1000{\rho }_0}\frac{\partial {\rho }_1}{\partial h_1}\end{array}$$

声速
声速是关于运载器所在高度的函数，即$V_{s1}=f(h_1)$，此$f(h)$非计算密度的$f(h)$。未归一化声速$V_{s1}=\sqrt{R_0{g}_E}{V}_s$，单位$m/s$，未归一化运载器高度$h_1=R_{0}h/1000$，单位$km$
$$\begin{array}{r}\frac{\partial V_s}{\partial r}=\frac{R_0}{1000\sqrt{R_0{g}_E}}\frac{\partial V_{s1}}{\partial h_1}\end{array}$$

推力
文献中计算推力的公式$T=\eta [T_{vac}+\Delta T(r)]/(m(t)g_0)$，其中$\Delta T(r)$怎么求呢？由文献[4]可知，在弹道计算中，通常采用下式计算推力
$$P=P_0+S_e(p_0-p_H)$$

其中，$P$是发动机推力，$P_0$是发动机地面推力，$S_e$是喷口载面积，$p_0$是地面大气压，$p_H$是发动机所在高度大气压。因此$\Delta T(r)=-S_e{p}_H$，进而可得
$$T_r=\frac{\partial T}{\partial r}=-\frac{\eta S_e}{m(t)g_E}\frac{\partial p_H}{\partial r}$$

求解最优体轴矢量$1_b^{\ast }$

在求$Y_0$过程中，我们已经求出一条满足终端约束的上升轨迹，根据该过程中求得的各量，求解$1_b^{\ast }$
计算$\Phi$
根据以下公式可以求解$\Phi$的绝对值[1]
$$\Phi =1_{p_V}^T{1}_{V_r}$$

再根据文章中的方法确定$\Phi$的正负。

用牛顿迭代法求解$\alpha$
$\partial H/\partial \alpha =0$可求得下式[1]
$$\tan (\Phi -\alpha )\left(T-A+N_{\alpha }\right)-\left(A_{\alpha }+N\right)=0$$

要使用牛顿迭代法求解$\alpha$还需求解${\partial }^{2}H/\partial {\alpha }^2$
$$\frac{{\partial }^{2}H}{\partial {\alpha }^2}=\frac{A-T-N_{\alpha }}{{\cos }^2(\Phi -\alpha )}+\tan (\Phi -\alpha )(-A_{\alpha }+N_{\alpha \alpha })-(A_{\alpha \alpha }+N_{\alpha })$$

其中，$A_{\alpha \alpha }={\partial }^{2}A/\partial {\alpha }^2,N_{\alpha \alpha }={\partial }^{2}N/\partial {\alpha }^2$。仿真时，气动力$A$和$N$被拟合成攻角$\alpha$的二次多项式形式，很容易求得气动力对攻角的一阶和二阶偏导数。求得$\alpha$后，根据下式求解最优体轴矢量[1]
$$1_b^{\ast }=\left(\frac{\sin \alpha }{\sin \Phi }\right)1_{p_V}+\left[\frac{\cos \alpha -\cos \Phi \cos (\Phi -\alpha )}{{\sin }^2\Phi }\right]1_{V_r}$$

有限差分法解TPBVPs

两点边值问题中的动力学方程由运载器的动力学方程和协态变量微分方程组成[1]
$$\begin{array}{rl}{\mathbf{r}}^{\prime }=& \mathbf{V}\\ {\mathbf{V}}^{\prime }=-\frac{1}{r^3}\mathbf{r}+\mathbf{A}& +T{1}_b+\mathbf{N}\end{array}$$

p r ′ = 1 r 3 p V − [ 3 a p v b r 4 − a p v n ( T r − A ρ r + 1 2 V r C ρ V s 2 C A M a c h ∂ V s ∂ r ) + a p v n ( N ρ r − 1 2 V r C ρ V s 2 C N M a c h ∂ V s ∂ r ) ] r r + C ρ ω ˉ E × { a p v b [ ( C A + 1 2 V r V s C A M a c h ) V r + 1 2 C A α V r 2 ∂ α ∂ V ] − a p v n [ ( C N + 1 2 V r V s C N M a c h ) V r + 1 2 C N α V r 2 ∂ α ∂ V ] } p V ′ = − p r + C ρ [ a p v b ( C A + 1 2 V r V s C A M a c h ) − a p v n ( C N + 1 2 V r V s C N M a c h ) + 1 2 ( a p v b C A α − a p v n C N α ) cos ⁡ α sin ⁡ α ] × V r − C ρ V r 2 sin ⁡ α ( a p v b C A α − a p v n C N α ) 1 b \begin{aligned} \boldsymbol{p}^{\prime}_{r} =& \frac{1}{r^{3}}\boldsymbol{p}_{V} - \lbrack \frac{3a_{pvb}}{r^{4}} -a_{pvn} ( T_{r}-A_{\rho r} + \frac{1}{2V_{r}}C_{\rho}V^{2}_{s}C_{A_{Mach}}\frac{\partial V_{s}}{\partial r} ) \\ &+ a_{pvn}(N_{\rho r} - \frac{1}{2V_{r}}C_{\rho}V^{2}_{s}C_{N_{Mach}}\frac{\partial V_{s}}{\partial r})]\frac{\boldsymbol{r}}{r} \\ &+ C_{\rho}\bar{\omega}_{E} \times \{ a_{pvb} [(C_{A} + \frac{1}{2V_{r}}V_{s}C_{A_{Mach}}) \boldsymbol{V}_{r} + \frac{1}{2}C_{A_{\alpha}}V^{2}_{r} \frac{\partial \alpha}{\partial \boldsymbol{V}} ] \\ &- a_{pvn}[(C_{N} + \frac{1}{2V_{r}}V_{s}C_{N_{Mach}}) \boldsymbol{V}_{r} + \frac{1}{2}C_{N_{\alpha}}V^{2}_{r} \frac{\partial \alpha}{\partial \boldsymbol{V}} ] \} \\ \boldsymbol{p}^{\prime}_{V} =& -\boldsymbol{p}_{r} + C_{\rho}[a_{pvb}(C_{A} + \frac{1}{2V_{r}}V_{s}C_{A_{Mach}}) \\ &- a_{pvn}(C_{N} + \frac{1}{2V_{r}}V_{s}C_{N_{Mach}}) + \frac{1}{2}(a_{pvb}C_{A_{\alpha}} - a_{pvn}C_{N_{\alpha}})\frac{\cos{\alpha}}{\sin{\alpha}}] \\ & \times \boldsymbol{V}_{r} - \frac{C_{\rho}V_{r}}{2\sin{\alpha}}(a_{pvb}C_{A_{\alpha}} - a_{pvn}C_{N_{\alpha}})\boldsymbol{1}_{b} \end{aligned} pr′​=pV′​=​r31​pV​−[r43apvb​​−apvn​(Tr​−Aρr​+2Vr​1​Cρ​Vs2​CAMach​​∂r∂Vs​​)+apvn​(Nρr​−2Vr​1​Cρ​Vs2​CNMach​​∂r∂Vs​​)]rr​+Cρ​ωˉE​×{apvb​[(CA​+2Vr​1​Vs​CAMach​​)Vr​+21​CAα​​Vr2​∂V∂α​]−apvn​[(CN​+2Vr​1​Vs​CNMach​​)Vr​+21​CNα​​Vr2​∂V∂α​]}−pr​+Cρ​[apvb​(CA​+2Vr​1​Vs​CAMach​​)−apvn​(CN​+2Vr​1​Vs​CNMach​​)+21​(apvb​CAα​​−apvn​CNα​​)sinαcosα​]×Vr​−2sinαCρ​Vr​​(apvb​CAα​​−apvn​CNα​​)1b​​

其中，${\rho }_r=\partial \rho /\partial r,T_r=\partial T/\partial r,C_{\rho }={\rho }_0\rho S_{ref}{R}_0/m(t)$，并且有
$$A_{\rho r}=\frac{V_r^2{S}_{ref}{R}_0{\rho }_0{C}_A{\rho }_r}{2m(t)},N_{\rho r}=\frac{V_r^2{S}_{ref}{R}_0{\rho }_0{C}_N{\rho }_r}{2m(t)}$$

$$C_{A_{\alpha }}=\frac{\partial C_A}{\partial \alpha },C_{N_{\alpha }}=\frac{\partial C_N}{\partial \alpha },C_{A_{Mach}}=\frac{\partial C_A}{\partial Mach}$$

$$C_{N_{Mach}}=\frac{\partial C_N}{\partial Mach},a_{pvb}={\mathbf{p}}_V^T{1}_b=p_V\cos (\Phi -\alpha )$$

$$a_{pvn}={\mathbf{p}}_V^T{1}_n=p_V\sin (\Phi -\alpha )$$

$$\frac{\partial \alpha }{\partial \mathbf{V}}=\frac{1}{V_r\sin \alpha }(\cos \alpha 1_{Vr}-1_b)$$

当加入约束$S_1=q\alpha -Q_{\alpha }=0$后，协态方程
$$\begin{array}{r}{\mathbf{p}}^{\prime }=-\frac{\partial H}{\partial \mathbf{x}}-{\lambda }_{q\alpha }\frac{\partial S_1}{\partial \mathbf{x}}\end{array}$$

初始条件$({\mathbf{x}}_0,{\mathbf{p}}_0)$已知，6个终端条件由入轨条件和最优控制理论求得的横截条件组成。
将时间分成相等的$M$段，一共$M+1$个节点，则每一段的长度$h=tf/M$。我们取$M=100$，那么两点边值问题就转换为如下方程组求解问题

$$\begin{array}{rl}{E}_0=& \left[\begin{array}{c}{y}_0(1:3)\\ y_0(4:6)\end{array}\right]-\left[\begin{array}{c}{\mathbf{r}}_0\\ {\mathbf{V}}_0\end{array}\right]\\ E_1=& y_1-y_0-hf(t_{1-\frac{1}{2}},\frac{(y_1+y_0)}{2})\\ E_2=& y_2-y_1-hf(t_{2-\frac{1}{2}},\frac{(y_2+y_1)}{2})\\ & ...\\ E_{100}=& y_{100}-y_{99}-hf(t_{100-\frac{1}{2}},\frac{(y_{100}+y_{99})}{2})\\ E_{101}=& \Psi (y_{100})\end{array}$$

其中$y_0(1:3)$表示$y_0$向量的前三个元素组成的向量。记方程的解为$Y=[y_0^T,y_1^T,y_2^T,...,y_{100}^T{]}^T$，其中$y=[{\mathbf{r}}^T,{\mathbf{V}}^T,{\mathbf{p}}_r^T,{\mathbf{p}}_v^T{]}^T$。这里需要注意的是前面的推导中我们用过$[{\mathbf{r}}^T,{\mathbf{V}}^T,{\mathbf{p}}_v^T,-{\mathbf{p}}_r^T{]}^T$这种顺序。

数值方法求$\partial E/\partial Y$
$$\begin{array}{r}\frac{\partial E}{\partial Y}=[...{]}_{1212\times 1212}=\left[\begin{array}{cccc}\frac{\partial E_0}{y_0}& \frac{\partial E_0}{y_1}& ...& \frac{\partial E_0}{y_{100}}\\ \frac{\partial E_1}{y_0}& \frac{\partial E_1}{y_1}& ...& \frac{\partial E_1}{y_{100}}\\ & ...& & \\ \frac{\partial E_{101}}{y_0}& \frac{\partial E_{101}}{y_1}& ...& \frac{\partial E_{101}}{y_{100}}\end{array}\right]\end{array}$$

$$\begin{array}{r}=\left[\begin{array}{cccccc}[I_6,0{]}_{6\times 12}& 0_{6\times 12}& ...& ...& 0_{6\times 12}& \\ -I_{12\times 12}-h\frac{\partial f(t_{1-\frac{1}{2}},\frac{(y_1+y_0)}{2})}{\partial y_0}& I_{12\times 12}-h\frac{f(,)}{y_1}& 0_{12\times 12}& ...& 0_{12\times 12}& \\ 0_{12\times 12}& -I_{12\times 12}-h\frac{\partial f(,)}{\partial y_1}& I_{12\times 12}-h\frac{f(,)}{y_2}& ...& 0_{12\times 12}& \\ ...& ...& ...& ...& ...& \\ 0_{12\times 12}& ...& 0_{12\times 12}& -I_{12\times 12}-h\frac{\partial f(,)}{\partial y_{99}}& I_{12\times 12}-h\frac{f(,)}{y_{100}}& \\ 0_{6\times 12}& ...& ...& 0_{6\times 12}& \frac{\partial \mathbf{\Psi }}{\partial y_{100}}& \end{array}\right]\end{array}$$

其中，$f(,)$是对应时刻$f(t_{k-\frac{1}{2}},\frac{(y_k+y_{k-1})}{2})$的简写，$\frac{\partial \mathbf{\Psi }}{\partial y_{100}}$之前有个小节推导过，是有解析形式的，除此以外其它的偏导数由数值的方法计算。

$$\frac{\partial f(t_{k-\frac{1}{2}},\frac{(y_k+y_{k-1})}{2})}{\partial y_k}=\frac{\partial f(t_{k-\frac{1}{2}},\frac{(y_k+y_{k-1})}{2})}{\partial (\frac{(y_k+y_{k-1})}{2})}\frac{\partial (\frac{(y_k+y_{k-1})}{2})}{\partial y_k}$$

高斯消元法求解搜索方向${\mathbf{d}}_j$
$$\begin{array}{r}\left[\frac{\partial E(Y_{j-1})}{\partial Y}\right]{\mathbf{d}}_j=-E(Y_{j-1})\end{array}$$

由于$\frac{\partial E}{\partial Y}$的稀疏特性，我们可以用高斯消元法求解${\mathbf{d}}_j$

参考文献

[1] Lu P , Sun H , Tsai B . Closed-Loop Endoatmospheric Ascent Guidance[J]. Journal of Guidance, Control, and Dynamics, 2003, 26(2):283-294.
[2] Calise A J , Melamed N , Lee S . Design and Evaluation of a Three-Dimensional Optimal Ascent Guidance Algorithm[J]. Journal of Guidance, Control, and Dynamics, 1998, 21(6):867-875.
[3] Mcadoo S F J , Jezewski D J , Dawkins G S . Development of a method for optimal maneuver analysis of complex space missions[J]. Nasa Sti/recon Technical Report N, 1975, 75.
[4] 王志刚, 施志佳. 远程火箭与卫星轨道力学基础[M].西安：西北工业大学出版社，2006.
[5] 傅瑜. 升力式天地往返飞行器自主制导方法研究[D]. 哈尔滨工业大学, 2012.