显式制导

总加速函数设计，F矩阵和E矩阵，还有你最爱的泰勒级数

基本知识

两个动力学常微分方程：

$$\begin{aligned} & \dot{\boldsymbol{r}}(t)=\boldsymbol{v}(t) \\ & \dot{\boldsymbol{v}}(t)=\boldsymbol{g}(\boldsymbol{r})+\boldsymbol{a}_P(t) \end{aligned}$$

两点边值问题：

当时时刻的位置和速度（导航系统提供）

$$\begin{aligned} & \boldsymbol{r}\left(t_0\right)=\left[\begin{array}{lll} x_0 & y_0 & z_0 \end{array}\right]^T \\ & \boldsymbol{v}\left(t_0\right)=\left[\begin{array}{lll} \dot{x}_0 & \dot{y}_0 & \dot{z}_0 \end{array}\right]^T \end{aligned}$$

终端时刻的位置和速度

$$\begin{aligned} & \boldsymbol{r}\left(t_{\mathrm{f}}\right)=\left[\begin{array}{lll} x_{\mathrm{f}} & y_{\mathrm{f}} & z_{\mathrm{f}} \end{array}\right]^T \\ & \boldsymbol{v}\left(t_{\mathrm{f}}\right)=\left[\begin{array}{lll} \dot{x}_{\mathrm{f}} & \dot{y}_{\mathrm{f}} & \dot{z}_{\mathrm{f}} \end{array}\right]^T \end{aligned}$$

实际控制量：

推力矢量——幅值+方向

提示

引力场很难计算，求导、积分等操作都很困难，所以可以直接设计加速度函数，选择“总”加速度作为控制量，先设计总角速度，然后调整推力角速度使其与重力加速度的和加速度等于总加速度，相当于分解了设计步骤

总加速度函数的设计

需要满足的条件

显然就是两点边值条件，积分后可知

$$\begin{gathered} \dot{x}_{\mathrm{f}}-\dot{x}_0=\int_{t_0}^{t_{\mathrm{f}}} \ddot{x}(t) \mathrm{d} t \\ x_{\mathrm{f}}-x_0-\dot{x}\left(t_0\right) t_{\mathrm{go}}=\int_{t_0}^{t_{\mathrm{f}}}\left[\int_{t_0}^t \ddot{x}(s) \mathrm{d} s\right] \mathrm{d} t \end{gathered}$$

描述

速度改变=加速度一次积分

位移改变-初速度*时间=加速度二次积分

显然满足这样条件的函数有无数个，我们可以使用任意一组函数空间的基来设计相应的函数，注意，这样的基函数可以不是正交函数，比方我们下面所用的幂级数就不是正交函数，但是保证他们线性无关就行了。（糟糕，我好像嗅到了泛函的气息）

两点边值条件下的总加速度函数设计

这里我们需要满足两个方程，所以我们把方程的自由度限定为两个，也就是说用两个线性无关函数的线性组合来表示总加速度：

$$\ddot{x}(t)=c_1 p_1(t)+c_2 p_2(t)$$

他的一次积分和两次积分仍然需要满足上面那个方程：

$$\begin{gathered} \dot{x}_{\mathrm{f}}-\dot{x}_0=\int_{t_0}^{t_{\mathrm{f}}} \ddot{x}(t) \mathrm{d} t=f_{11} c_1+f_{12} c_2 \\ x_{\mathrm{f}}-x_0-\dot{x}\left(t_0\right) t_{\mathrm{go}}=\int_{t_0}^{t_{\mathrm{f}}}\left[\int_{t_0}^t \ddot{x}(s) \mathrm{d} s\right] \mathrm{d} t=f_{21} c_1+f_{22} c_2 \end{gathered}$$

其中

$$\begin{aligned} & f_{11}=\int_{t_0}^{t_f} p_1(t) \mathrm{dt}, \quad f_{21}=\int_{t_0}^{t_f}\left[\int_{t_0}^t p_1(s) \mathrm{d} s\right] \mathrm{d} t \\ & f_{12}=\int_{t_0}^{t_f} p_2(t) \mathrm{dt}, \quad f_{22}=\int_{t_0}^{t_f}\left[\int_{t_0}^t p_2(s) \mathrm{d} s\right] \mathrm{d} t \end{aligned}$$

即：

$$\left[\begin{array}{c} \dot{x}_{\mathrm{f}}-\dot{x}_0 \\ x_{\mathrm{f}}-\left(x_0+\dot{x}_0 t_{\mathrm{go}}\right) \end{array}\right]=\left[\begin{array}{ll} f_{11} & f_{12} \\ f_{21} & f_{22} \end{array}\right]\left[\begin{array}{l} c_1 \\ c_2 \end{array}\right] \triangleq \boldsymbol{F}\left[\begin{array}{l} c_1 \\ c_2 \end{array}\right]$$

我们的目的是确定系数$c_1$和$c_2$，而现在我们已经有两点边值条件了，所以：

$$\left[\begin{array}{l} c_1 \\ c_2 \end{array}\right]=\left[\begin{array}{ll} e_{11} & e_{12} \\ e_{21} & e_{22} \end{array}\right]\left[\begin{array}{c} \dot{x}_{\mathrm{f}}-\dot{x}_0 \\ x_{\mathrm{f}}-\left(x_0+\dot{x}_0 t_{\mathrm{go}}\right) \end{array}\right] \triangleq \boldsymbol{E}\left[\begin{array}{c} \dot{x}_{\mathrm{f}}-\dot{x}_0 \\ x_{\mathrm{f}}-\left(x_0+\dot{x}_0 t_{\mathrm{go}}\right) \end{array}\right]$$

其中E是F的逆

本例中，令

$$p_1(t)=1, \quad p_2(t)=t_{\mathrm{f}}-t$$

$f_{11}$、$f_{12}$、$f_{21}$、$f_{22}$分别是p1、p2的一次积分和二次积分，且

$$t_f-t_0=t_{go}$$

则

$$\boldsymbol{F}=\left[\begin{array}{cc} t_{g o}^2 & \frac{1}{2} t_{g o}^3 \\ \frac{1}{2} t_{g o}^3 & \frac{1}{3} t_{g o}^4 \end{array}\right]$$

即

$$\boldsymbol{E}=\boldsymbol{F}^{-1}= \left[\begin{array}{cc} 4 / t_{\mathrm{go}} & -6 / t_{\mathrm{go}}^2 \\ -6 / t_{\mathrm{go}}^2 & 12 / t_{\mathrm{go}}^3 \end{array}\right]$$

最后的表达式为

$$\left[\begin{array}{c} c_1 \\ c_2 \end{array}\right]=\left[\begin{array}{cc} 4 / t_{\mathrm{go}} & -6 / t_{\mathrm{go}}^2 \\ -6 / t_{\mathrm{go}}^2 & 12 / t_{\mathrm{go}}^3 \end{array}\right]\left[\begin{array}{c} \dot{x}_{\mathrm{f}}-\dot{x}_0 \\ x_{\mathrm{f}}-\left(x_0+\dot{x}_0 t_{\mathrm{go}}\right) \end{array}\right]$$

x轴需要的加速度为

$$a_{P x}(t)=\ddot{x}(t)-g_x(t)=c_1+c_2\left(t_{\mathrm{f}}-t\right)-g_x(t)$$

补充

显式制导中实际上是“预测-矫正”框架，我们观察这个式子

$$\left[\begin{array}{c} c_1 \\ c_2 \end{array}\right]=\left[\begin{array}{cc} 4 / t_{\mathrm{go}} & -6 / t_{\mathrm{go}}^2 \\ -6 / t_{\mathrm{go}}^2 & 12 / t_{\mathrm{go}}^3 \end{array}\right]\left[\begin{array}{c} \dot{x}_{\mathrm{f}}-\dot{x}_0 \\ x_{\mathrm{f}}-\left(x_0+\dot{x}_0 t_{\mathrm{go}}\right) \end{array}\right]$$

会发现最右侧的那个矩阵，其中

• 第一行是速度$\dot x$在$t=t_f$时的误差
• 第二行是位移$x$在$t=t_f$时的误差

所以式子实际上实在预测终端误差，并且基于E矩阵来矫正$c_1、c_2$，可以料想，如果导航和控制系统完全理想的工作，那么$c_1、c_2$是不变了，但是实际过程中必然要不断计算和矫正$c_1、c_2$

另外，注意到E矩阵中各个元素分母都是$t_{go}$的幂级数，所以当$t_{go}$变小时，E矩阵各个元素会越来越来，为了避免数据溢出，实际设计的时候，动力飞行的最后几秒会停止更新$E$矩阵及c_1、c_2

三个边值条件下的总加速度函数设计

如果约束不止是“两点”边值条件，而是三点，比方对目标点的加速度也有要求，那么就要取3个参数：

$$p_1(t)=1, \quad p_2(t)=t_{\mathrm{f}}-t,\quad p_3=(t_{\mathrm{f}}-t)^2$$

提示

这里的三个函数仍然是互相线性无关的，因为幂级数本身是函数空间的基，他可以表示任何一个函数。至于为什么要用幂级数，也许可以理解我们在用多项式去拟合总加速度函数（?）在老师PPT第3小节还说这种选取是最优显示制导，应该也是有比较严谨的理由吧。

不过最直观的体验是，这样写出来的F和E矩阵确实很有规律也很好用，下面会计算说明

相应的，加速度的表达式变为：

$$\ddot{x}(t)=c_0+c_1\left(t_{\mathrm{f}}-t\right)+c_2\left(t_{\mathrm{f}}-t\right)^2$$

系数需要满足的条件有：

$$c_0=\ddot{x}_{\mathrm{f}}\\ \begin{gathered} \dot{x}_{\mathrm{f}}-\dot{x}_0=\int_{t_0}^{t_{\mathrm{f}}} \ddot{x}(t) \mathrm{d} t\\ x_{\mathrm{f}}-x_0-\dot{x}_0 t_{\mathrm{go}}=\int_{t_0}^{t_{\mathrm{f}}}\left[\int_{t_0}^t \ddot{x}(s) \mathrm{d} s\right] \mathrm{d} t \end{gathered}$$

比较令人欣慰的是$c_0$可以直接由约束条件得到，而后面两个式子组成的二阶矩阵也与之前的情况类似，他们系数的变化非常的有规律

$$\begin{gathered} \dot{x}_{\mathrm{f}}-\dot{x}_0=\int_{t_0}^{t_{\mathrm{f}}} \ddot{x}(t) \mathrm{d} t=c_0 t_{g o}+\frac{1}{2} c_1 t_{g o}^2+\frac{1}{3} c_2 t_{g o}^3 \\ x_{\mathrm{f}}-x_0-\dot{x}_0 t_{\mathrm{go}}=\int_{t_0}^{t_{\mathrm{f}}}\left[\int_{t_0}^t \ddot{x}(s) \mathrm{d} s\right] \mathrm{d} t=\frac{1}{2} c_0 t_{g o}^2+\frac{1}{3} c_1 t_{g o}^3+\frac{1}{4} c_2 t_{g o}^4 \end{gathered}$$

可以把已知的$c_0$拿到等式左边，此时的等式写成矩阵形式则为

$$\left[\begin{array}{c} \dot{x}_{\mathrm{f}}-\dot{x}_0-c_0 t_{g o} \\ x_{\mathrm{f}}-x_0-\dot{x}_0 t_{\mathrm{go}}-\frac{1}{2} c_0 t_{g o}^2 \end{array}\right]=\left[\begin{array}{cc} \frac{1}{2} t_{g o}^2 & \frac{1}{3} t_{g o}^3 \\ \frac{1}{3} t_{g o}^3 & \frac{1}{4} t_{g o}^4 \end{array}\right]\left[\begin{array}{l} c_1 \\ c_2 \end{array}\right]$$

则

$$E=\left[\begin{array}{ll} \frac{1}{2} t_{g o}^2 & \frac{1}{3} t_{g o}^3 \\ \frac{1}{3} t_{g o}^3 & \frac{1}{4} t_{g o}^4 \end{array}\right]^{-1}=\left[\begin{array}{cc} \frac{18}{t_{g o}^2} & -\frac{24}{t_{g o}^3} \\ -\frac{24}{t_{g o}^3} & -\frac{36}{t_{g o}^4} \end{array}\right]$$

反解出的$c_1、c_2$为

$$\begin{aligned} {\left[\begin{array}{l} c_1 \\ c_2 \end{array}\right] } & =E\left[\begin{array}{c} \dot{x}_{\mathrm{f}}-\dot{x}_0-c_0 t_{g o} \\ x_{\mathrm{f}}-x_0-\dot{x}_0 t_{\mathrm{go}}-\frac{1}{2} c_0 t_{g o}^2 \end{array}\right]\\ & =\left[\begin{array}{c} \frac{18}{t_{g o}^2}\left(\dot{x}_{\mathrm{f}}-\dot{x}_0-c_0 t_{g o}\right)-\frac{24}{t_{g o}^3}\left(x_{\mathrm{f}}-x_0-\dot{x}_0 t_{\mathrm{go}}-\frac{1}{2} c_0 t_{g o}^2\right) \\ -\frac{24}{t_{g o}^3}\left(\dot{x}_{\mathrm{f}}-\dot{x}_0-c_0 t_{g o}\right)-\frac{36}{t_{g o}^4}\left(x_{\mathrm{f}}-x_0-\dot{x}_0 t_{\mathrm{go}}-\frac{1}{2} c_0 t_{g o}^2\right) \end{array}\right] \\ & =\left[\begin{array}{l} -\frac{24}{t_{g o}^3}\left(x_{\mathrm{f}}-x_0\right)+\frac{6}{t_{g o}^2}\left(3 \dot{x}_{\mathrm{f}}+\dot{x}_0\right)-\frac{6 c_0}{t_{g o}} \\ \frac{36}{t_{g o}^4}\left(x_{\mathrm{f}}-x_0\right)-\frac{12}{t_{g o}^3}\left(2 \dot{x}_{\mathrm{f}}+\dot{x}_0\right)+\frac{6 c_0}{t_{g o}} \end{array}\right] \end{aligned}$$

注意

三轴运动是互相解耦的，为了简便，上面的所有公式和说明都是针对x轴来说的，其他轴其实也是一模一样

推力不可调火箭的总加速度函数设计

已知量

• 当前时刻$t_0$的位置和速度
• 终端时刻$t_f$的目标位置和速度

注意到此时有“推力不可调”这样一个限制条件，所以不能直接设计“总加速函数”，而是要把他写的更详细一点

下面是我们的动力学方程

$$\ddot{y}(t)=a_P(t) \sin \varphi(t)+g_y(t)$$

备注

这个例子中我们是在y轴上设计的制导律

观察一下，其中我们能控制的不过是这个角度$\varphi$而已，其他的都木得机会

所以这里我们对控制量$\varphi$做参数化设计

$$\sin \varphi(t)=c_1+c_2\left(t-t_0\right)-g_y(t) / a_P(t)$$

这样可以使得“总”加速度形式变得和我们原先熟悉的一样，之后的计算也就流程化了

$$\ddot{y}(t)=c_1 a_P(t)+c_2 a_P(t)\left(t-t_0\right)=c_1 \frac{u_e}{\tau-\left(t-t_0\right)}+c_2 \frac{u_e}{\tau-\left(t-t_0\right)}\left(t-t_0\right)$$

需要注意的是，推力加速度也是时间的函数，所以积分的时候也要带上他

之后的计算虽然差不多，但是也很繁琐，这里就不多列出了，详见PPT第36面