火箭的控制原理——TVC与钱学森弹道

TVC姿态控制

TVC全称Thrust Vector Control，也就是推力矢量控制，让发动机喷管能”歪一歪”，靠推力的斜向分量产生控制力矩，从而在气动舵面几乎失效的高度和速度区间（比如主动段初期或者大气层外）把火箭的姿态掰回来。固体火箭用得最多，因为它的喷管舵机结构简单、响应也快，这期就以它为例。

开始之前先把符号说清楚：俯仰角记作$\theta$，偏航角记作$\psi$，滚转角记作$\gamma$，滚转角速度就是$\omega_x=\dot\gamma$。喷管在箭体坐标系里的两个偏角记作$\delta_z$（俯仰方向）和$\delta_y$（偏航方向），它们对应的纯俯仰/纯偏航控制效果就叫等效角$\delta_\theta$和$\delta_\psi$。

等效角

如果箭体以角速度$\omega_x$滚转，那么固定在箭体上的喷管自然也一起转，滚过的角度就是$\gamma=\omega_x t$。于是喷管的实际偏角$(\delta_z,\delta_y)$与等效角$(\delta_\theta,\delta_\psi)$之间就差一个旋转：

展开出来就是：

反过来解（两边乘$R(-\gamma)$）：

这个取逆就是滚转解耦：控制器在准箭体系算出来的指令，要先按滚转角转一下再发给喷管，不然滚转会把俯仰指令甩进偏航通道，后面有仿真演示。

姿态动力学

喷管偏转$\delta$之后，推力$T$会在质心外产生力矩。设喷管到质心的距离（力臂）为$L$，则：

俯仰和偏航两个通道分别写出来：

其中J是箭体的转动惯量。两边拉普拉斯变换（零初值）：

也就是一个双积分环节。意思很直接：推力越大、力臂越长、惯量越小，同样的偏角转得越猛；而且双积分本身一点阻尼都没有，不给反馈只会越转越快，所以必须闭环。

大气层内还有气动项：静稳定力矩像一根”弹簧”，气动阻尼力矩像阻尼器，写全大概是$J\ddot\theta=-K_a\theta-C_d\dot\theta+T L\delta_z$，参数整定时按最恶劣的状态点来就行。

伺服机构

喷管不可能瞬间偏转，伺服机构本身就有惯性，常见建模有三种：

  • 一阶惯性：$G_s(s)=\frac{1}{T_s s+1}$，$T_s$是时间常数，越小响应越快；
  • 二阶惯性：$G_s(s)=\frac{\omega_n^2}{s^2+2\zeta\omega_n s+\omega_n^2}$，用来描述舵机柔性传动或者大负载惯量；
  • 纯延迟：$e^{-\tau s}$，来自机械传动间隙和控制周期，工程上常用Pade近似展开成有理式方便计算。

把控制器、伺服、延迟乘在一起，就是完整的开环传递函数。

PID与稳定性

控制器最常用的是PID（或者在角度外环里面再套一层角速度内环）：

拿PD加双积分器闭环分析，特征方程是：

和标准二阶系统$s^2+2\zeta\omega_n s+\omega_n^2=0$对照：

这就是参数整定的依据：$K_p$管响应快慢，$K_d$管阻尼和超调，$K_i$消稳态误差但会拖相位。加上伺服惯性和延迟还要再吃掉一些相位，所以实际参数得看开环波特图的增益/相位裕度，留够裕度再上箭。

控制流程

回路结构

整个回路就一条线：指令和反馈相减，PID算出等效控制角，经滚转解耦转到喷管坐标系，伺服带动喷管，推力产生力矩，箭体姿态动起来，再被陀螺/IMU测回来。实际用的时候一般是双环，角速度内环提供阻尼、压住超调，角度外环负责跟踪指令，另外还得处理舵机限幅和积分饱和。

阶跃响应仿真

把上面的环节写成代码，看看PID参数对姿态响应的影响：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
# TVC俯仰通道闭环: 双积分器 + 一阶伺服 + 延迟 + PD
import numpy as np
import matplotlib.pyplot as plt

dt, T_end = 0.001, 8.0
tau_s, delay = 0.05, 0.02          # 伺服时间常数 / 纯延迟

def simulate(Kp, Kd):
    th = thd = servo = 0.0
    buf = [0.0] * int(delay / dt)
    log = []
    for i in range(int(T_end / dt)):
        u = Kp * (1.0 - th) - Kd * thd      # 期望阶跃到 1
        buf.append(u)
        servo += (buf.pop(0) - servo) * dt / tau_s   # 伺服惯性
        thd += servo * dt                    # 双积分器
        th += thd * dt
        log.append(th)
    return np.arange(len(log)) * dt, np.array(log)

for Kp, Kd, label in [(2, 0.8, 'Kp=2, Kd=0.8 (欠阻尼)'),
                      (4, 4.0, 'Kp=4, Kd=4.0 (接近临界阻尼)'),
                      (9, 3.0, 'Kp=9, Kd=3.0 (高增益)'),
                      (2, 5.0, 'Kp=2, Kd=5.0 (过阻尼)')]:
    t, th = simulate(Kp, Kd)
    plt.plot(t, th, label=label)
plt.legend(); plt.grid(True); plt.show()

阶跃响应仿真

图里看得很清楚：$K_d$太小（蓝线）超调大、振荡半天；$K_d$合适（橙线）基本没超调；加大$K_p$（绿线）响应更快，但也更容易振；$K_d$太大（红线）稳是稳，就是慢。伺服0.05s的时间常数和0.02s的延迟都会让实际阻尼比比理论值低，所以工程里解析计算和仿真校核缺一不可。

滚转耦合仿真

解耦到底有没有用，直接开环试一下：给一个恒定的等效俯仰指令，看两个通道各自的表现：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
# 恒定俯仰指令下的滚转耦合 (开环)
import numpy as np

wx, cmd = 0.5, 0.1        # 滚转角速度 / 俯仰偏角指令
dt, T_end = 0.001, 10.0

def run(decoupled):
    gamma = th = thd = ps = psd = 0.0
    for _ in range(int(T_end / dt)):
        gamma += wx * dt
        if decoupled:     # 指令先旋转到箭体系
            dz, dy = np.cos(gamma) * cmd, -np.sin(gamma) * cmd
        else:             # 直接下发
            dz, dy = cmd, 0.0
        thd += (np.cos(gamma) * dz - np.sin(gamma) * dy) * dt
        psd += (np.sin(gamma) * dz + np.cos(gamma) * dy) * dt
        th += thd * dt; ps += psd * dt
    return th, ps

滚转耦合仿真

结果很直观：不解耦的时候（实线），俯仰指令被滚转”搅拌”掉，俯仰角根本涨不起来（蓝线来回晃），偏航反而被持续激发、越偏越大（红线）；按滚转角解耦之后（虚线），俯仰角正常增长（橙线），偏航老老实实保持零（绿线）。所以滚转解耦是必须做的。

TVC的部分到这里就差不多了：动力学是双积分、控制用PID、别忘了滚转解耦，剩下的就是按裕度慢慢调参。接下来解释一下钱学森弹道

钱学森弹道

钱学森弹道是一个抛物线路径，曲线方程为：

其中y是竖直方向位移，x是水平方向位移，a、b、c、d是控制曲线形状和落点的常数。

这个公式的用处不大，我们需要利用一些简单方法进行推导：

  • 初速度：设导弹初速度为 $v_0$，发射角为 $ \theta_0$。
  • 空气阻力：假设空气阻力$F_d$与速度v的平方成正比，即 $F_d = c_d v^2$，其中$c_d$为阻力系数。

  • 运动分解：导弹的运动分为水平方向和竖直方向，分别考虑。

  • 水平方向运动

水平方向上，假设不考虑空气阻力的情况，导弹的初始水平速度为 $v_{0x} = v_0 \cos\theta_0 $。

考虑空气阻力的情况下，水平速度的变化率可由以下方程表示：

其中m为导弹质量，v是导弹瞬时速度 $v = \sqrt{v_x^2 + v_y^2}$。

由于空气阻力会减小水平速度，所以速度方程为：

这一步做了简化：把总速度$v$近似为水平分量$v_x$，阻力较小、弹道比较平的时候误差可以接受，要精确就交给后面的数值仿真。
水平位移xt由速度积分得出：

假设 t=0时x=0，则$C = \frac{m v_{0x}}{c_d}$，所以：

  • 竖直方向运动

竖直方向上，导弹初始速度为 $v_{0y} = v_0 \sin\theta_0$，受重力 mg 和空气阻力Fd的共同作用。

竖直速度的变化率为：

同样的，通过解微分方程，可以得到竖直速度 $v_y(t)$ 和竖直位移 y(t)的表达式。这里同样做了线性化近似（把阻力项里的速度当作缓变量处理），所以下面给出的解析解适合用来理解趋势，精确定量还是要靠数值仿真。我们给出解的形式：

竖直位移 y(t) 为速度的积分：

同样假设 y(0) = 0，可以求得：

  • 消除时间参数

为了得到导弹的轨迹方程 y(x)，我们需要消去时间 t。通过解水平方程 x(t) 得到 t 作为 x 的函数：

将其代入竖直位移方程 y(t)，即可得到y关于x的表达式：

简化得到最终的轨迹方程：

  • 进一步简化（小空气阻力近似）

在空气阻力较小的情况下，即$ c_d \to 0$，轨迹方程可进一步简化为常见的抛物线形式：

对于有空气阻力确定落点的情况下：

初始速度 $v_0$ 和 发射角度 $\theta_0$ 已知,空气阻力 $F_d = c_d v^2$，其中 $c_d$ 是阻力系数，v 是速度。

弹道方程

弹道方程的垂直和水平分量分别为：

其中，$v_x$和 $v_y$ 是速度的水平和垂直分量。

接下来用python代码完成路径仿真：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
import numpy as np
import matplotlib.pyplot as plt

# 参数定义
g = 9.81  # 重力加速度 (m/s^2)
c_d = 0.005  # 阻力系数
m = 100  # 质量 (kg)
dt = 0.01  # 时间步长 (s)

# 定义不同的初速度 (m/s) 和发射角度 (度)
v0_values = [50, 70, 90]
theta_values = [30, 45, 60]

# 创建绘图
plt.figure(figsize=(10, 6))

# 绘制不同初速度和发射角度下的曲线
for v0 in v0_values:
    for theta0 in theta_values:
        # 将角度转换为弧度
        theta_rad = np.radians(theta0)

        # 初始条件
        v_x = v0 * np.cos(theta_rad)
        v_y = v0 * np.sin(theta_rad)
        x, y = 0, 0  # 初始位置

        # 用于存储轨迹的数组
        x_vals = []
        y_vals = []

        # 数值积分以确定弹道轨迹和落点
        while y >= 0:
            v = np.sqrt(v_x ** 2 + v_y ** 2)

            # 更新速度
            v_x -= (c_d / m) * v * v_x * dt
            v_y -= (g + (c_d / m) * v * v_y) * dt

            # 更新位置
            x += v_x * dt
            y += v_y * dt

            # 保存当前的 (x, y) 值
            x_vals.append(x)
            y_vals.append(y)

        # 绘制弹道轨迹
        plt.plot(x_vals, y_vals, label=f'v0 = {v0} m/s, θ = {theta0}°')

        # 标注落点位置
        plt.scatter(x, 0, color='red')
        plt.text(x, 0, f'({x:.1f}, 0)', fontsize=9, ha='right', color='red')

# 添加标题和标签
plt.title('ballistic trajectory')
plt.xlabel('distance (m)')
plt.ylabel('height (m)')
plt.legend()
plt.grid(True)

# 显示图形
plt.show()

11

不要疑惑，钱学森弹道在突破大气层过程中是图中的轨道，剩下的大气层打水漂过程我们无法完全仿真，因为需要考虑空气阻力，飞行器姿态控制，所以我们只能假设一下。

修改一下初始位置得到：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
import numpy as np
import matplotlib.pyplot as plt

# 参数定义
g = 9.81  # 重力加速度 (m/s^2)
c_d = 0.005  # 阻力系数
m = 100  # 质量 (kg)
dt = 0.01  # 时间步长 (s)

# 定义不同的初速度 (m/s) 和发射角度 (度)
v0_values = [50, 70, 90]  # 初速度列表
theta_values = [30, 45, 60]  # 发射角度列表

# 创建绘图
plt.figure(figsize=(12, 8))

# 绘制不同初速度和发射角度下的轨迹
for v0 in v0_values:
    for theta0 in theta_values:
        # 将角度转换为弧度
        theta_rad = np.radians(theta0)

        # 初始条件
        v_x = v0 * np.cos(theta_rad)
        v_y = v0 * np.sin(theta_rad)
        x, y = 0, 100000  # 初始位置

        # 用于存储轨迹的数组
        x_vals = []
        y_vals = []

        # 数值积分以确定弹道轨迹和大气层内过程
        while y >= 0:
            v = np.sqrt(v_x ** 2 + v_y ** 2)

            # 更新速度
            v_x -= (c_d / m) * v * v_x * dt
            v_y -= (g + (c_d / m) * v * v_y) * dt

            # 更新位置
            x += v_x * dt
            y += v_y * dt

            # 保存当前的 (x, y) 值
            x_vals.append(x)
            y_vals.append(y)

        # 绘制弹道轨迹
        plt.plot(x_vals, y_vals, label=f'v0 = {v0} m/s, θ = {theta0}°')

        # 标注落点位置
        plt.scatter(x, 0, color='red')
        plt.text(x, 0, f'({x:.1f}, 0)', fontsize=9, ha='right', color='red')

# 添加标题和标签
plt.title('ballistic trajectory')
plt.xlabel('distance (m)')
plt.ylabel('height (m)')
plt.legend()
plt.grid(True)

# 显示图形
plt.show()

12

技术
#python #数学
火箭的控制原理——TVC与钱学森弹道
https://blog.minloha.cn/posts/11152241a306b22024081544.html
作者
Minloha
发布于
2024年8月15日
更新于
2024年8月15日
许可协议