一、完整算法流程
1. 坐标系定义
- 地心惯性坐标系(ECI)：原点在地心，X轴指向春分点
- 发射坐标系：原点在发射点，X轴指向射向，Y轴垂直向上
- 体坐标系：原点在火箭质心，X轴沿纵轴向前
- 速度坐标系：基于速度方向定义
2. 状态变量定义
状态向量包含18个变量：
ini
X = [x, y, z, # 位置(ECI) [m]
vx, vy, vz, # 速度(ECI) [m/s]
q0, q1, q2, q3, # 姿态四元数
ωx, ωy, ωz, # 角速度(体坐标系) [rad/s]
m] # 质量 [kg]3. 主计算流程
ini
初始化：
- 读取火箭参数
- 设置初始状态
- 定义控制程序（如俯仰程序）
主循环（t=0 到 t_max）：
1. 环境参数计算
- 计算高度、经纬度
- 调用大气模型（温度、压强、密度）
- 计算重力（考虑J2项）
- 计算科氏加速度
2. 气动参数计算
- 计算马赫数、雷诺数
- 基于攻角、侧滑角、马赫数查表获取：
* 阻力系数 Cx
* 升力系数 Cy, Cz
* 力矩系数 Cl, Cm, Cn
- 计算动压：q = 0.5 * ρ * V²
- 计算气动力/力矩
3. 推进系统计算
- 计算燃料质量流量：ṁ = F_vac / (Isp * g0)
- 计算推力（随高度变化）：
F = F_vac - p * A_e
- 计算推力偏心矩
4. 力与力矩合成
- 总力：F_total = F_thrust + F_aero + F_gravity
- 总力矩：M_total = M_thrust + M_aero
5. 运动方程求解
a. 平动方程：
d²r/dt² = F_total/m + ω_earth × (ω_earth × r) + 2ω_earth × v
b. 转动方程：
I * dω/dt + ω × (I * ω) = M_total
dq/dt = 0.5 * Ω * q
c. 质量方程：
dm/dt = -ṁ
6. 数值积分
使用四阶龙格-库塔法：
k1 = f(t, X)
k2 = f(t + h/2, X + h*k1/2)
k3 = f(t + h/2, X + h*k2/2)
k4 = f(t + h, X + h*k3)
X_{n+1} = X_n + (h/6)*(k1 + 2k2 + 2k3 + k4)
7. 坐标系转换
- 计算欧拉角（俯仰、偏航、滚转）
- 体坐标系 ↔ 惯性坐标系转换
8. 终止条件检查
- 燃料耗尽
- 高度 < 0（落地）
- 程序时间结束
9. 记录与输出4. 主要数学模型
4.1 平动方程
ini
m * dv/dt = ΣF - m * (ω_earth × (ω_earth × r) + 2ω_earth × v)4.2 转动方程
css
I * dω/dt = ΣM - ω × (I * ω)4.3 四元数微分方程
css
dq/dt = 0.5 * [ 0 -ωx -ωy -ωz ωx 0 ωz -ωy ωy -ωz 0 ωx ωz ωy -ωx 0 ] * q4.4 质量变化
scss
m(t) = m0 - ∫₀ᵗ ṁ dt二、完整配套代码
ini
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from scipy.interpolate import interp1d
from dataclasses import dataclass
from typing import Tuple, List, Dict, Optional
import json
# ==================== 常量定义 ====================
G = 6.67430e-11 # 引力常数 [m³/kg/s²]
M_EARTH = 5.9722e24 # 地球质量 [kg]
R_EARTH = 6378137.0 # 地球赤道半径 [m]
OMEGA_EARTH = 7.292115e-5 # 地球自转角速度 [rad/s]
G0 = 9.80665 # 海平面重力加速度 [m/s²]
J2 = 1.08263e-3 # 地球扁率系数
# ==================== 数据类定义 ====================
@dataclass
class RocketConfig:
"""火箭配置参数"""
# 质量特性
dry_mass: float = 500.0 # 干质量 [kg]
prop_mass: float = 1500.0 # 推进剂质量 [kg]
prop_density: float = 1800.0 # 推进剂密度 [kg/m³]
# 发动机特性
thrust_vac: float = 200000.0 # 真空推力 [N]
thrust_sl: float = 180000.0 # 海平面推力 [N]
isp_vac: float = 280.0 # 真空比冲 [s]
isp_sl: float = 250.0 # 海平面比冲 [s]
burn_time: float = 120.0 # 工作时间 [s]
nozzle_area: float = 0.5 # 喷管面积 [m²]
throttle: List[float] = None # 油门程序
# 几何特性
length: float = 10.0 # 长度 [m]
diameter: float = 1.0 # 直径 [m]
ref_area: float = 0.7854 # 参考面积 [m²]
center_of_mass: float = 6.0 # 质心位置（距尾部）[m]
center_of_pressure: float = 5.5 # 压心位置（距尾部）[m]
# 惯性特性
inertia_roll: float = 100.0 # 滚转惯量 [kg·m²]
inertia_pitch: float = 10000.0 # 俯仰惯量 [kg·m²]
inertia_yaw: float = 10000.0 # 偏航惯量 [kg·m²]
def __post_init__(self):
if self.throttle is None:
self.throttle = [1.0] # 默认全油门
self.ref_area = np.pi * (self.diameter/2)**2
@property
def inertia_tensor(self) -> np.ndarray:
"""转动惯量张量"""
return np.diag([self.inertia_roll,
self.inertia_pitch,
self.inertia_yaw])
@dataclass
class AeroData:
"""气动数据"""
# 攻角范围 [deg]
alpha: np.ndarray = None
# 马赫数范围
mach: np.ndarray = None
# 阻力系数 Cx(alpha, mach)
Cx: np.ndarray = None
# 法向力系数 Cz(alpha, mach)
Cz: np.ndarray = None
# 俯仰力矩系数 Cm(alpha, mach)
Cm: np.ndarray = None
def __post_init__(self):
if self.alpha is None:
self.alpha = np.linspace(-20, 20, 9) # -20° 到 20°
if self.mach is None:
self.mach = np.array([0, 0.5, 1.0, 1.5, 2.0, 3.0, 5.0])
if self.Cx is None:
# 默认阻力系数
self.Cx = np.zeros((len(self.alpha), len(self.mach)))
for i, m in enumerate(self.mach):
if m < 1.0:
self.Cx[:, i] = 0.5 + 0.1 * np.abs(self.alpha) # 亚音速
else:
self.Cx[:, i] = 0.3 + 0.05 * np.abs(self.alpha) # 超音速
if self.Cz is None:
# 默认法向力系数
self.Cz = 0.05 * np.deg2rad(self.alpha).reshape(-1, 1) * np.ones((len(self.alpha), len(self.mach)))
if self.Cm is None:
# 默认力矩系数
self.Cm = -0.01 * np.deg2rad(self.alpha).reshape(-1, 1) * np.ones((len(self.alpha), len(self.mach)))
def get_coefficients(self, alpha: float, mach: float) -> Tuple[float, float, float]:
"""通过插值获取气动系数"""
alpha_rad = np.deg2rad(alpha)
alpha_deg = np.rad2deg(alpha_rad)
# 确保在数据范围内
alpha_clip = np.clip(alpha_deg, self.alpha[0], self.alpha[-1])
mach_clip = np.clip(mach, self.mach[0], self.mach[-1])
# 二维插值
from scipy.interpolate import RectBivariateSpline
Cx_interp = RectBivariateSpline(self.alpha, self.mach, self.Cx)
Cz_interp = RectBivariateSpline(self.alpha, self.mach, self.Cz)
Cm_interp = RectBivariateSpline(self.alpha, self.mach, self.Cm)
Cx_val = float(Cx_interp(alpha_clip, mach_clip))
Cz_val = float(Cz_interp(alpha_clip, mach_clip))
Cm_val = float(Cm_interp(alpha_clip, mach_clip))
return Cx_val, Cz_val, Cm_val
# ==================== 大气模型 ====================
class Atmosphere1976:
"""US Standard Atmosphere 1976"""
def __init__(self):
# 定义大气层分段 (高度: m, 温度梯度: K/m, 基础温度: K, 基础压强: Pa)
self.layers = [
(0, 11000, -0.0065, 288.15, 101325.0),
(11000, 20000, 0.0, 216.65, 22632.0),
(20000, 32000, 0.001, 216.65, 5474.9),
(32000, 47000, 0.0028, 228.65, 868.02),
(47000, 51000, 0.0, 270.65, 110.91),
(51000, 71000, -0.0028, 270.65, 66.939),
(71000, 84852, -0.002, 214.65, 3.9564)
]
self.R = 287.053 # 气体常数 [J/kg/K]
self.g0 = 9.80665
self.gamma = 1.4 # 空气比热比
def get_atmosphere(self, altitude: float) -> Tuple[float, float, float, float]:
"""获取大气参数
返回: (温度[K], 压强[Pa], 密度[kg/m³], 声速[m/s])
"""
h = max(0.0, altitude)
for i, (h0, h1, a, T0, p0) in enumerate(self.layers):
if h0 <= h <= h1 or (i == len(self.layers)-1 and h > h1):
if a == 0: # 等温层
T = T0
p = p0 * np.exp(-self.g0 * (h - h0) / (self.R * T0))
else: # 温度梯度层
T = T0 + a * (h - h0)
if abs(a) > 1e-10:
p = p0 * (T / T0) ** (-self.g0 / (a * self.R))
else:
p = p0 * np.exp(-self.g0 * (h - h0) / (self.R * T0))
rho = p / (self.R * T)
sound_speed = np.sqrt(self.gamma * self.R * T)
return T, p, rho, sound_speed
# 超过最高层，使用指数外推
h0, h1, a, T0, p0 = self.layers[-1]
T = T0
p = p0 * np.exp(-self.g0 * (h - h0) / (self.R * T0))
rho = p / (self.R * T)
sound_speed = np.sqrt(self.gamma * self.R * T)
return T, p, rho, sound_speed
# ==================== 火箭动力学模型 ====================
class Rocket6DOF:
"""火箭6自由度动力学模型"""
def __init__(self, config: RocketConfig, aero_data: AeroData = None):
self.config = config
self.aero_data = aero_data or AeroData()
self.atmosphere = Atmosphere1976()
self.g0 = G0
# 预计算惯性张量逆矩阵
self.I_inv = np.linalg.inv(config.inertia_tensor)
# 状态历史记录
self.history = {
'time': [], 'position': [], 'velocity': [], 'mass': [],
'euler': [], 'accel': [], 'alpha': [], 'mach': []
}
def ecef_to_eci(self, r_ecef: np.ndarray, t: float) -> np.ndarray:
"""ECEF到ECI坐标转换"""
theta = OMEGA_EARTH * t
R = np.array([
[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]
])
return R @ r_ecef
def eci_to_ecef(self, r_eci: np.ndarray, t: float) -> np.ndarray:
"""ECI到ECEF坐标转换"""
theta = -OMEGA_EARTH * t
R = np.array([
[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]
])
return R @ r_eci
def quaternion_to_dcm(self, q: np.ndarray) -> np.ndarray:
"""四元数转方向余弦矩阵"""
q0, q1, q2, q3 = q
return np.array([
[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)],
[2*(q1*q2+q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1**2+q2**2)]
])
def dcm_to_euler(self, dcm: np.ndarray) -> Tuple[float, float, float]:
"""DCM转欧拉角（3-2-1顺序）"""
phi = np.arctan2(dcm[2,1], dcm[2,2]) # 滚转
theta = np.arcsin(-dcm[2,0]) # 俯仰
psi = np.arctan2(dcm[1,0], dcm[0,0]) # 偏航
return phi, theta, psi
def gravity_acceleration(self, r_eci: np.ndarray) -> np.ndarray:
"""计算重力加速度（含J2项）"""
r_norm = np.linalg.norm(r_eci)
lat = np.arcsin(r_eci[2] / r_norm)
# 中心引力
g_magnitude = G * M_EARTH / r_norm**2
# J2项修正
J2_factor = 1.5 * J2 * (R_EARTH/r_norm)**2 * (3*np.sin(lat)**2 - 1)
g_magnitude_j2 = g_magnitude * (1 + J2_factor)
# 重力加速度向量（指向地心）
g_eci = -g_magnitude_j2 * r_eci / r_norm
return g_eci
def get_thrust(self, t: float, p: float) -> Tuple[float, float]:
"""计算推力和质量流量"""
if t > self.config.burn_time:
return 0.0, 0.0
# 推力随高度变化
thrust = self.config.thrust_vac - p * self.config.nozzle_area
# 质量流量
m_dot = self.config.thrust_vac / (self.config.isp_vac * self.g0)
return thrust, m_dot
def aero_forces_moments(self, v_body: np.ndarray, rho: float,
mach: float, dcm: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
"""计算气动力和力矩"""
v_norm = np.linalg.norm(v_body)
if v_norm < 1e-3:
return np.zeros(3), np.zeros(3)
# 计算攻角和侧滑角
alpha = np.arctan2(v_body[2], v_body[0]) # 俯仰平面
beta = np.arcsin(v_body[1] / v_norm) # 偏航平面
# 获取气动系数
Cx, Cz, Cm = self.aero_data.get_coefficients(np.rad2deg(alpha), mach)
# 动压
q_inf = 0.5 * rho * v_norm**2
# 气动力（体坐标系）
F_aero_body = np.array([
-q_inf * self.config.ref_area * Cx,
q_inf * self.config.ref_area * 0.0, # 侧向力系数暂设为0
-q_inf * self.config.ref_area * Cz
])
# 气动力矩（体坐标系）
M_aero_body = np.array([
0.0, # 滚转力矩
q_inf * self.config.ref_area * self.config.diameter * Cm, # 俯仰力矩
0.0 # 偏航力矩
])
return F_aero_body, M_aero_body
def dynamics(self, t: float, state: np.ndarray) -> np.ndarray:
"""6自由度动力学方程"""
# 解包状态向量
x, y, z = state[0:3]
vx, vy, vz = state[3:6]
q0, q1, q2, q3 = state[6:10]
omega_x, omega_y, omega_z = state[10:13]
m = state[13]
# 位置和速度向量
r_eci = np.array([x, y, z])
v_eci = np.array([vx, vy, vz])
# 计算高度
r_norm = np.linalg.norm(r_eci)
alt = r_norm - R_EARTH
# 大气参数
T, p, rho, sound_speed = self.atmosphere.get_atmosphere(alt)
# 四元数归一化
q = np.array([q0, q1, q2, q3])
q = q / np.linalg.norm(q)
q0, q1, q2, q3 = q
# 方向余弦矩阵
dcm = self.quaternion_to_dcm(q)
# 速度转换到体坐标系
v_body = dcm.T @ v_eci
v_norm = np.linalg.norm(v_body)
mach = v_norm / sound_speed if sound_speed > 0 else 0
# 气动力和力矩
F_aero_body, M_aero_body = self.aero_forces_moments(v_body, rho, mach, dcm)
# 推力和质量流量
thrust_body, m_dot = self.get_thrust(t, p)
F_thrust_body = np.array([thrust_body, 0.0, 0.0])
# 总力和力矩（体坐标系）
F_body = F_thrust_body + F_aero_body
M_body = M_aero_body # 简化，不考虑推力偏心
# 转换到惯性系
F_inertial = dcm @ F_body
# 重力
g_eci = self.gravity_acceleration(r_eci)
# 科氏加速度和离心加速度
omega_vec = np.array([0, 0, OMEGA_EARTH])
coriolis = 2 * np.cross(omega_vec, v_eci)
centrifugal = np.cross(omega_vec, np.cross(omega_vec, r_eci))
# 平动加速度
a_eci = F_inertial / m + g_eci - coriolis - centrifugal
# 转动动力学
omega = np.array([omega_x, omega_y, omega_z])
I = self.config.inertia_tensor
# 欧拉方程: I * dω/dt + ω × (I * ω) = M
omega_dot = self.I_inv @ (M_body - np.cross(omega, I @ omega))
# 四元数运动学
Omega = np.array([
[0, -omega_x, -omega_y, -omega_z],
[omega_x, 0, omega_z, -omega_y],
[omega_y, -omega_z, 0, omega_x],
[omega_z, omega_y, -omega_x, 0]
])
q_dot = 0.5 * Omega @ q
# 质量变化
m_dot_val = -m_dot if t <= self.config.burn_time else 0.0
# 组装状态导数
state_dot = np.zeros(14)
state_dot[0:3] = v_eci
state_dot[3:6] = a_eci
state_dot[6:10] = q_dot
state_dot[10:13] = omega_dot
state_dot[13] = m_dot_val
return state_dot
def simulate(self, t_span: Tuple[float, float], initial_state: np.ndarray,
dt: float = 0.1, rtol: float = 1e-6, atol: float = 1e-9) -> Dict:
"""执行仿真"""
# 记录初始状态
self.history['time'].append(t_span[0])
self.history['position'].append(initial_state[0:3].copy())
self.history['velocity'].append(initial_state[3:6].copy())
self.history['mass'].append(initial_state[13])
# 定义事件函数（落地检测）
def hit_ground(t, y):
r = np.linalg.norm(y[0:3])
return r - R_EARTH
hit_ground.terminal = True
hit_ground.direction = -1
# 积分
sol = solve_ivp(
fun=lambda t, y: self.dynamics(t, y),
t_span=t_span,
y0=initial_state,
method='RK45',
t_eval=np.arange(t_span[0], t_span[1], dt),
rtol=rtol,
atol=atol,
events=[hit_ground]
)
# 记录结果
for i in range(len(sol.t)):
t = sol.t[i]
state = sol.y[:, i]
# 记录基本状态
self.history['time'].append(t)
self.history['position'].append(state[0:3].copy())
self.history['velocity'].append(state[3:6].copy())
self.history['mass'].append(state[13])
# 计算附加信息
v_eci = state[3:6]
v_norm = np.linalg.norm(v_eci)
alt = np.linalg.norm(state[0:3]) - R_EARTH
T, p, rho, sound_speed = self.atmosphere.get_atmosphere(alt)
mach = v_norm / sound_speed if sound_speed > 0 else 0
dcm = self.quaternion_to_dcm(state[6:10])
v_body = dcm.T @ v_eci
alpha = np.arctan2(v_body[2], v_body[0])
self.history['mach'].append(mach)
self.history['alpha'].append(alpha)
# 计算欧拉角
phi, theta, psi = self.dcm_to_euler(dcm)
self.history['euler'].append([phi, theta, psi])
# 计算加速度
if i > 0:
dt_hist = self.history['time'][-1] - self.history['time'][-2]
dv = self.history['velocity'][-1] - self.history['velocity'][-2]
accel = dv / dt_hist if dt_hist > 0 else np.zeros(3)
self.history['accel'].append(accel)
else:
self.history['accel'].append(np.zeros(3))
return {
'time': np.array(self.history['time']),
'position': np.array(self.history['position']),
'velocity': np.array(self.history['velocity']),
'mass': np.array(self.history['mass']),
'euler': np.array(self.history['euler']),
'accel': np.array(self.history['accel']),
'mach': np.array(self.history['mach']),
'alpha': np.array(self.history['alpha'])
}
def plot_results(self, results: Dict):
"""绘制结果"""
fig, axes = plt.subplots(3, 3, figsize=(15, 12))
time = results['time']
# 位置
pos = results['position']
alt = np.linalg.norm(pos, axis=1) - R_EARTH
axes[0,0].plot(time, alt / 1000)
axes[0,0].set_ylabel('高度 (km)')
axes[0,0].set_xlabel('时间 (s)')
axes[0,0].grid(True)
axes[0,0].set_title('飞行高度')
# 速度
vel = results['velocity']
vel_mag = np.linalg.norm(vel, axis=1)
axes[0,1].plot(time, vel_mag)
axes[0,1].set_ylabel('速度 (m/s)')
axes[0,1].set_xlabel('时间 (s)')
axes[0,1].grid(True)
axes[0,1].set_title('飞行速度')
# 马赫数
axes[0,2].plot(time, results['mach'])
axes[0,2].set_ylabel('马赫数')
axes[0,2].set_xlabel('时间 (s)')
axes[0,2].grid(True)
axes[0,2].set_title('马赫数变化')
# 质量
axes[1,0].plot(time, results['mass'])
axes[1,0].set_ylabel('质量 (kg)')
axes[1,0].set_xlabel('时间 (s)')
axes[1,0].grid(True)
axes[1,0].set_title('质量变化')
# 攻角
axes[1,1].plot(time, np.rad2deg(results['alpha']))
axes[1,1].set_ylabel('攻角 (度)')
axes[1,1].set_xlabel('时间 (s)')
axes[1,1].grid(True)
axes[1,1].set_title('攻角变化')
# 欧拉角
euler = results['euler']
axes[1,2].plot(time, np.rad2deg(euler[:, 0]), label='滚转')
axes[1,2].plot(time, np.rad2deg(euler[:, 1]), label='俯仰')
axes[1,2].plot(time, np.rad2deg(euler[:, 2]), label='偏航')
axes[1,2].set_ylabel('角度 (度)')
axes[1,2].set_xlabel('时间 (s)')
axes[1,2].grid(True)
axes[1,2].legend()
axes[1,2].set_title('姿态角')
# 加速度
accel = results['accel']
accel_g = np.linalg.norm(accel, axis=1) / G0
axes[2,0].plot(time, accel_g)
axes[2,0].set_ylabel('过载 (g)')
axes[2,0].set_xlabel('时间 (s)')
axes[2,0].grid(True)
axes[2,0].set_title('过载')
# 弹道轨迹
axes[2,1].plot(pos[:, 0] / 1000, pos[:, 2] / 1000)
axes[2,1].set_xlabel('X (km)')
axes[2,1].set_ylabel('Z (km)')
axes[2,1].grid(True)
axes[2,1].set_title('弹道轨迹')
axes[2,1].axis('equal')
# 弹道倾角
gamma = np.arctan2(vel[:, 2], np.sqrt(vel[:, 0]**2 + vel[:, 1]**2))
axes[2,2].plot(time, np.rad2deg(gamma))
axes[2,2].set_ylabel('弹道倾角 (度)')
axes[2,2].set_xlabel('时间 (s)')
axes[2,2].grid(True)
axes[2,2].set_title('弹道倾角')
plt.tight_layout()
plt.show()
# ==================== 主程序 ====================
def main():
"""主仿真程序"""
# 1. 创建火箭配置
config = RocketConfig(
dry_mass=500.0,
prop_mass=1500.0,
thrust_vac=200000.0,
thrust_sl=180000.0,
isp_vac=280.0,
isp_sl=250.0,
burn_time=120.0,
length=10.0,
diameter=1.0
)
# 2. 创建气动数据
aero_data = AeroData()
# 3. 创建火箭模型
rocket = Rocket6DOF(config, aero_data)
# 4. 设置初始状态
# 初始位置：发射点（纬度0°，经度0°，高度0）
lat0 = np.deg2rad(0.0) # 赤道
lon0 = np.deg2rad(0.0)
alt0 = 0.0
# 地固坐标系位置
r_ecef = np.array([
(R_EARTH + alt0) * np.cos(lat0) * np.cos(lon0),
(R_EARTH + alt0) * np.cos(lat0) * np.sin(lon0),
(R_EARTH + alt0) * np.sin(lat0)
])
# 转换到惯性系
r_eci = rocket.ecef_to_eci(r_ecef, 0.0)
# 初始速度（考虑地球自转）
v_ecef = np.array([0.0, 0.0, 0.0])
omega_earth = np.array([0, 0, OMEGA_EARTH])
v_eci = v_ecef + np.cross(omega_earth, r_ecef)
# 初始姿态（垂直向上）
pitch0 = np.deg2rad(90.0) # 俯仰角90°（垂直）
yaw0 = np.deg2rad(0.0) # 偏航角0°
roll0 = np.deg2rad(0.0) # 滚转角0°
# 计算初始四元数
cy = np.cos(yaw0 * 0.5)
sy = np.sin(yaw0 * 0.5)
cp = np.cos(pitch0 * 0.5)
sp = np.sin(pitch0 * 0.5)
cr = np.cos(roll0 * 0.5)
sr = np.sin(roll0 * 0.5)
q0 = cr * cp * cy + sr * sp * sy
q1 = sr * cp * cy - cr * sp * sy
q2 = cr * sp * cy + sr * cp * sy
q3 = cr * cp * sy - sr * sp * cy
# 初始角速度
omega0 = np.array([0.0, 0.0, 0.0])
# 初始质量
m0 = config.dry_mass + config.prop_mass
# 初始状态向量
initial_state = np.concatenate([
r_eci, v_eci, [q0, q1, q2, q3], omega0, [m0]
])
# 5. 运行仿真
print("开始火箭6自由度弹道仿真...")
print(f"初始质量: {m0:.1f} kg")
print(f"干质量: {config.dry_mass:.1f} kg")
print(f"推进剂质量: {config.prop_mass:.1f} kg")
print(f"真空推力: {config.thrust_vac/1000:.1f} kN")
print(f"工作时间: {config.burn_time:.1f} s")
results = rocket.simulate(
t_span=(0.0, 300.0), # 仿真时间
initial_state=initial_state,
dt=0.1, # 输出步长
rtol=1e-6,
atol=1e-9
)
# 6. 输出结果
print(f"\n仿真完成，总时间: {results['time'][-1]:.2f} s")
print(f"最终高度: {np.linalg.norm(results['position'][-1]) - R_EARTH:.2f} m")
print(f"最终速度: {np.linalg.norm(results['velocity'][-1]):.2f} m/s")
print(f"最终马赫数: {results['mach'][-1]:.2f}")
print(f"剩余质量: {results['mass'][-1]:.2f} kg")
# 7. 绘制结果
rocket.plot_results(results)
return rocket, results
if __name__ == "__main__":
rocket, results = main()三、系统特性说明
1. 关键特性
- 6自由度动力学：完整的平动和转动方程
- 质量变化：考虑燃料消耗引起的质量和质心变化
- 大气模型：US Standard Atmosphere 1976标准大气
- 地球自转：考虑科氏力和离心力
- J2项重力：考虑地球扁率影响
- 气动模型：基于攻角和马赫数的插值表
- 推进系统：考虑推力随高度变化
2. 扩展建议
如需进一步扩展，可考虑：
- 增加风场模型
- 添加控制系统和制导算法
- 考虑弹性振动和液体晃动
- 加入多级分离逻辑
- 优化气动数据库
- 添加蒙特卡洛仿真功能
3. 使用说明
- 修改
RocketConfig参数定义火箭特性 - 准备气动数据表格（可选）
- 设置初始发射条件
- 运行仿真获取弹道数据
- 分析结果并进行优化
此系统提供了完整的火箭6自由度弹道计算框架，可用于火箭初步设计、弹道分析和控制系统设计。