作者：HongXiao

本文旨在构建一种通用自动入轨算法.

我们常用的入轨方法是重力转弯+远地点附近点火, 这种方法简单可控, 但是缺点是它并不像想象中一样省油, 而且精度较差. 而在真实的火箭入轨过程中, 二级发动机不会在到达远地点附近时二次点火, 通常关机即入轨, 它们采用了一些制导算法, 其中线性正切律迭代制导是一种平衡优化程度与计算复杂度的算法. 其最早被应用于土星系列火箭, 成为执行地月转移轨道注入 (TLI) 的关键技术; 后来成为航天飞机动力显式制导 (PEG) 的基础.

我们给出的自动入轨算法包含两个阶段, 第一阶段是在稠密大气中, 火箭遵循重力转弯, 以减小空气阻力, 增强运行稳定性; 第二阶段开始于火箭到达高空后, 通常是第一级分离后, 火箭按照线性正切制导律进行闭环控制, 直到到达预定高度和速度. 本文聚焦第二阶段的具体控制算法.

我们将问题简化, 假设火箭在二维平面运动, 由于火箭入轨过程中位移相对于地球半径很小, 可以假设地面是一条直线, 由于火箭已到达高空, 可以假设无大气阻力, 推力大小恒定. 我们也假设火箭质量恒定, 重力加速度恒定, 这确实过度简化了实际情况, 因为火箭会消耗大量燃料, 且水平速度会产生离心作用抵消重力加速度, 导致我们设计的算法实际上不是最优的, 但是这种简化能让我们得到解析方程组, 便于实时的迭代求解.

问题描述

在平面直角坐标系中, 重力方向沿 yy 轴负半轴, 当前时间为 tot_o, 火箭当前位置为 (xo,yo)(x_o,y_o), 当前速度为 (uo,vo)(u_o,v_o), 比推力大小恒为 ff, 目标是达到高度为 hh, 速度方向水平, 大小为 VV 的轨道. 我们要控制火箭推力方向角 θ\theta, 使终止时间 tft_f 最小.

我们提出最优控制问题:

寻找控制函数 θ(t)\theta(t) 和终端时刻 tft_f 使得代价函数 J=tfJ = t_f 最小, 满足约束:

初始条件:

终端条件:

应用庞特里亚金极大值原理, 我们可以得到:

这就是线性正切制导律, 此结论的证明不在本文讨论范围内, 请参考文献[1]. 现在的问题是 a, tan⁡θoa , \, \tan\theta_o 的计算方式.

解析方程推导

对约束条件应用链式法则改变自变量

对 (5b), (5c) 积分

将 (6b) 代入 (5a) , 对 (5a) 积分

利用 (6), (7) 代入终端条件, 我们得到了关于 θo, θf, a\theta_o, \, \theta_f, \, a 三个未知数的方程组

现在要将它们化为关于 θo, θf\theta_o, \, \theta_f 的方程组, 为了简洁我们记

由 (8b)

代入 (8a)

由 (8b) 和 (8c) 消去 aa

(11), (12) 是关于 θo, θf\theta_o, \, \theta_f 的方程组, 可以用于数值求解.

数值求解方法

设

令 θf\theta_f 为 F(θf)=F(θo)F(\theta_f) = F(\theta_o) 的解, 设

现在只需求 G(θo)G(\theta_o) 的零点, 根据物理意义, 可以发现 G(θo)G(\theta_o) 在 (−π2,π2)\left(-\frac{\pi}{2}, \frac{\pi}{2}\right) 上严格增, 可以用二分法求解其零点, 前提是对于每一个 θo\theta_o, 我们能解出 F(θf)=F(θo)F(\theta_f) = F(\theta_o), 为此, 我们考察 F(θf)F(\theta_f) 的单调性, 引入双曲函数代换 γ=γ(θ):(−π,π)→R\gamma =\gamma(\theta): \quad \left(-\pi,\pi\right) \to \mathbb{R} 简化表达式, 代换后不改变函数的单调性, 并且避免了边界奇点, 由于 θ∈(−π2,π2)\theta \in \left(-\frac{\pi}{2},\frac{\pi}{2}\right), 有

代入 (13)

求导

I. 当 μ<1\mu < 1 时, 有

F(γf)F(\gamma_f) 是严格凸函数, 至多有两个零点, 令 dFdγf=0\frac{\mathrm{d}F}{\mathrm{d}\gamma_f} = 0, 可以找到最小值点

显然至少有一个平凡的零点 γf=γo\gamma_f = \gamma_o, 我们要找到另一个零点, 这个零点满足

且 F(γf)F(\gamma_f) 在 (−∞,γinf⁡)(-\infty,\gamma_{\inf}) 和 (γinf⁡,+∞)(\gamma_{\inf},+\infty) 上单调, 因此可以在 ⁡γinf⁡⁡\gamma_{\inf} 的一侧用牛顿迭代法寻找 γf\gamma_f. 初值设定为

第 n+1n+1 次近似值为

II. 当 μ>1\mu >1 时, dFdγf\frac{\mathrm{d}F}{\mathrm{d}\gamma_f} 先增后减. 若 ν2−μ2+1<0\nu^2 - \mu^2 +1 < 0, 有

此时只有平凡解, 这个情况下火箭不可能达到目标速度. 若 ν2−μ2+1>0\nu^2 - \mu^2 + 1 > 0, 则 d2Fdγf2\frac{\mathrm{d}^2F}{\mathrm{d}\gamma_f^2} 有零点 γ∗\gamma^\ast, dFdγf\frac{\mathrm{d}F}{\mathrm{d}\gamma_f} 有零点

F(γf)F(\gamma_f) 先减再增后减, 此时存在非平凡解的充要条件是 F(γinf⁡)≤F(γo)≤F(γsup⁡)F(\gamma_{\inf}) \le F(\gamma_o) \le F(\gamma_{\sup}), 那么共三个解 γo, γf1, γf2 (γf1<γf2)\gamma_o, \, \gamma_{f1}, \, \gamma_{f2} \, (\gamma_{f1} < \gamma_{f2}), 考虑到

关于 γf\gamma_f 严格增, 因此 γf\gamma_f 应取较小的非平凡解 γf1\gamma_{f1}, 满足

我们希望初值仍然为 γf(0)=2γinf⁡−γo\gamma_f^{(0)} = 2 \gamma_{\inf} - \gamma_o, 事实上, 可以证明对于满足 F(γinf⁡)≤F(γo)≤F(γsup⁡)F(\gamma_{\inf}) \le F(\gamma_o) \le F(\gamma_{\sup}) 的 γo\gamma_o

这意味着 dFdγf\frac{\mathrm{d}F}{\mathrm{d}\gamma_f} 和 d2Fdγf2\frac{\mathrm{d}^2F}{\mathrm{d}\gamma_f^2} 在区间 [γf(0),γinf⁡)\left[\gamma_f^{(0)},\gamma_{\inf}\right) 和 (γinf⁡,γf(0)]\left(\gamma_{\inf},\gamma_f^{(0)}\right] 均保号, 因此可以在这两个区间内进行牛顿迭代法求解.

末端误差补偿

请注意此制导律对于比推力敏感, 意味着我们不能在即将圆轨时随意减小推力, 因此在即将结束的几秒内需要选用其他算法以避免速度过大. 可以选择将 ΔV\Delta V 和 vov_o 等比例降为零, 由此引入 hh 的误差, 但是 hh 的误差是可以估算的.

假设线性正切率制导在 te=tf−Δtt_e = t_f - \Delta t 时刻结束, 用线性正切率, 最后这段较短的时间 Δt\Delta t 内可忽略 θ\theta 的变化, 由于此时接近圆轨, 重力加速度可忽略不计, 为匀减速运动, 竖直方向加速度大小为 fy=fsin⁡θe<0f_y = f\sin\theta_e < 0, 竖直方向初始速度为 ve=−fyΔtv_e = -f_y \Delta t

现在我们使推力正比于速度减小, tet_e 时仍有竖直方向加速度 fye=fsin⁡θef_{ye} = f\sin\theta_e, 竖直方向速度 ve=−fyΔtv_e = -f_y \Delta t, 显然竖直方向加速度也正比于竖直方向速度, 即

解得竖直方向速度

当 t→+∞t \to +\infty 时 v→0v \to 0, 对速度积分求高度改变量

则高度误差为

可以在线性正切律求解过程中给 v→0v \to 0 减去 Δherr\Delta h_{err}, 以补偿误差.

演示代码

#include <cmath>
#include <cstdio>
double pi = std::acos(-1.);
using std::sqrt;
using std::log;
using std::exp;
using std::atan;

double H;
double V;
double y_o;
double u_o;
double v_o;
double t_o;
double f;
double g;

double Delta_t = 5.;
double epsilon = pi / 2046;

double Delta_V, nu, mu, gamma_inf;
double gamma_o, gamma_f;
double R_o, T_o, S_o, R_f, T_f, S_f, G_gamma_o;
double a, tan_theta_o, t_f;

double cal_F_gamma_f() {
	return nu * gamma_f + S_f - mu * T_f;
}

double cal_dF_gamma_f() {
	return T_f - mu * S_f + nu;
}

void find_gamma_f() {
	gamma_f = gamma_o;
	T_f = T_o;
	S_f = S_o;
	double F_gamma = cal_F_gamma_f();
	gamma_f = 2 * gamma_inf - gamma_o;
	R_f = exp(gamma_f);
	T_f = (R_f - 1 / R_f) / 2;
	S_f = (R_f + 1 / R_f) / 2;
	double Delta_gamma_f = (cal_F_gamma_f() - F_gamma) / cal_dF_gamma_f();
	while (Delta_gamma_f > epsilon || Delta_gamma_f < -epsilon) {
		gamma_f -= Delta_gamma_f;
		R_f = exp(gamma_f);
		T_f = (R_f - 1 / R_f) / 2;
		S_f = (R_f + 1 / R_f) / 2;
		Delta_gamma_f = (cal_F_gamma_f() - F_gamma) / cal_dF_gamma_f();
	}
}

void cal_G_gamma_o() {
	R_o = exp(gamma_o);
	T_o = (R_o - 1 / R_o) / 2;
	S_o = (R_o + 1 / R_o) / 2;
	find_gamma_f();
	double Delta_T = T_f - T_o;
	double Delta_S = S_f - S_o;
	double Delta_gamma = gamma_f - gamma_o;
	double Delta_h = H + f * (T_f / S_f) * (Delta_t * Delta_t) / 2 - y_o;
	G_gamma_o = (T_f * Delta_S - S_o * Delta_T - mu * Delta_T * Delta_T) / (Delta_gamma * Delta_gamma)
		+ (2 * nu * Delta_T + 1) / Delta_gamma - (2 * f * Delta_h) / (Delta_V * Delta_V);
}

void cal_a_T_o_t_f() {
	Delta_V = V - u_o;
	nu = v_o / Delta_V;
	mu = g / f;
	gamma_inf = log((nu - sqrt(nu * nu - mu * mu + 1)) / (mu - 1));
	gamma_o = -pi;
	cal_G_gamma_o();
	while (G_gamma_o > 0) {
		gamma_o *= 2;
		cal_G_gamma_o();
	}
	double lo = gamma_o;
	gamma_o = pi;
	cal_G_gamma_o();
	while (G_gamma_o < 0) {
		gamma_o *= 2;
		cal_G_gamma_o();
	}
	double hi = gamma_o;
	gamma_o = (hi + lo) / 2;
	cal_G_gamma_o();
	while (hi - lo > epsilon) {
		if (G_gamma_o > 0) hi = gamma_o;
		else lo = gamma_o;
		gamma_o = (hi + lo) / 2;
		cal_G_gamma_o();
	}
	a = f * (gamma_f - gamma_o) / Delta_V;
	tan_theta_o = T_o;
	t_f = (T_f - T_o) / a + t_o;
}

double theta_t(double t) {
	return atan(a * (t - t_o) + T_o) * 180 / pi;
}

int main() {
	y_o = 55565.8438706764;
	u_o = 1328.33816599409;
	v_o = 694.614420440756;
	t_o = 123.883340353146;
	f = 10.3611261003202;
	g = 7.66935518541919;
	H = 150000;
	V = 3341.16908017303;
	cal_a_T_o_t_f();
	for (double t = t_o; t <= t_f; t += 1.) std::printf("%f: %f\n", t, theta_t(t));
}

Vizzy 代码

计算 F(gamma_f) 及其导数
根据当前状态和目标轨道计算 a, tan theta_o, t_f

主函数1: 重力转弯
主函数2: 线性正切制导律
主函数3: 末端调整

相关作品已上传官网 Juno: New Origins | Auto Launch 可供参考.

[1] Longuski, J.M., Guzmán, J.J., Prussing, J.E. (2014). Application of the Euler-Lagrange Theorem. In: Optimal Control with Aerospace Applications. Space Technology Library, vol 32. Springer, New York, NY.