1. 导弹六自由度运动学与动力学建模

任务定义

建立导弹六自由度（6-DOF）运动学与动力学数学模型，包含位置、速度、姿态角与角速度的微分方程，并在无控制输入条件下对模型进行数值积分，验证其响应满足物理约束（速度幅值变化符合重力与气动力作用、姿态角变化率与角速度的几何关系一致）。

坐标系与状态变量定义

采用地面惯性坐标系 O_g-x_gy_gz_g（原点为发射点，x_g指向正北，y_g指向天，z_g构成右手系）与弹体坐标系 O_b-x_by_bz_b（原点在导弹质心，x_b沿弹体纵轴向前）。状态向量取12维：

X = [x, y, z, u, v, w, φ, θ, ψ, p, q, r]^T

位置：(x, y, z) — 地面系下导弹质心坐标（m）
速度：(u, v, w) — 弹体系下速度分量（m/s）
姿态角：(φ, θ, ψ) — 滚转、俯仰、偏航角（rad），采用3-2-1旋转顺序
角速度：(p, q, r) — 弹体系下滚转、俯仰、偏航角速度（rad/s）

运动学方程

位置微分方程（地面系）：

[ẋ, ẏ, ż]^T = R_bg(φ,θ,ψ)·[u,v,w]^T

其中从弹体系到地面系的旋转矩阵：

R_bg = R_z(ψ)·R_y(θ)·R_x(φ)

姿态角微分方程（运动学关系）：

[φ̇, θ̇, ψ̇]^T = W(φ,θ)·[p,q,r]^T

其中：

W = [[1, sinφ·tanθ, cosφ·tanθ], [0, cosφ, −sinφ], [0, sinφ/cosθ, cosφ/cosθ]]

动力学方程

线运动动力学（弹体系，考虑重力与气动力，无控状态推力为零）：

[u̇,v̇,ẇ]^T = F/m − ω×[u,v,w]^T

其中 ω = [p,q,r]^T，F为弹体系下气动力与重力投影之和。重力在地面系恒为 [0, −mg, 0]^T，转换到弹体系：

F_grav,b = R_bg^T·[0, −mg, 0]^T

气动力采用简化线性模型（攻角与侧滑角小假设）：

F_aero,b = [−C_A·q̄·S, C_N·q̄·S·α, −C_N·q̄·S·β]^T

其中动压 q̄ = ½ρV²，速度幅值 V = √(u²+v²+w²)，攻角 α = atan2(w, u)，侧滑角 β = asin(v/V)。

角运动动力学（弹体系，忽略惯性积，对称弹体 I_xy=I_xz=I_yz=0）：

I_xṗ = (I_y−I_z)·q·r + L_aero
I_yq̇ = (I_z−I_x)·r·p + M_aero
I_zṙ = (I_x−I_y)·p·q + N_aero

气动力矩（参考长度 d，无控状态无舵偏）：

L_aero = C_lp·q̄·S·d·(p·d/(2V))
M_aero = C_mα·q̄·S·d·α + C_mq·q̄·S·d·(q·d/(2V))
N_aero = −C_nβ·q̄·S·d·β + C_nr·q̄·S·d·(r·d/(2V))

Python实现

以下代码完整实现上述12维状态微分方程，采用RK4积分器进行无控状态仿真。初始条件为：位置原点、速度 (200, 50, 0) m/s、零姿态角、零角速度，仿真时长10s。

import numpy as np
from scipy.integrate import solve_ivp

# 物理参数
m = 500.0          # 质量 kg
g = 9.81           # 重力加速度 m/s^2
S = 0.25           # 参考面积 m^2
d = 0.5            # 参考长度 m
rho = 1.225        # 空气密度 kg/m^3
Ix, Iy, Iz = 30.0, 800.0, 800.0   # 转动惯量 kg·m^2

# 气动系数（简化线性模型）
CA = 0.30          # 轴向力系数
CN = 2.5           # 法向力系数斜率 /rad
Clp = -0.05        # 滚转阻尼导数
Cma = -1.2         # 俯仰力矩斜率 /rad
Cmq = -8.0         # 俯仰阻尼导数
Cnb = 0.8          # 偏航力矩斜率 /rad
Cnr = -8.0         # 偏航阻尼导数

def rotation_matrix(phi, theta, psi):
    """弹体系到地面系的旋转矩阵 R_bg (3-2-1顺序)"""
    cp, sp = np.cos(phi), np.sin(phi)
    ct, st = np.cos(theta), np.sin(theta)
    cb, sb = np.cos(psi), np.sin(psi)
    R = np.array([
        [ct*cb, sp*st*cb - cp*sb, cp*st*cb + sp*sb],
        [ct*sb, sp*st*sb + cp*cb, cp*st*sb - sp*cb],
        [-st,   sp*ct,            cp*ct]
    ])
    return R

def attitude_kinematics_matrix(phi, theta):
    """姿态角速率与弹体角速度关系矩阵 W"""
    cp, sp = np.cos(phi), np.sin(phi)
    if abs(np.cos(theta)) < 1e-10:
        raise ValueError("俯仰角接近±90°，姿态奇异")
    tt = np.tan(theta)
    ct = np.cos(theta)
    W = np.array([
        [1.0, sp*tt, cp*tt],
        [0.0, cp,    -sp],
        [0.0, sp/ct, cp/ct]
    ])
    return W

def eom(t, X):
    """六自由度运动学与动力学微分方程"""
    x, y, z, u, v, w, phi, theta, psi, p, q, r = X
    
    # 速度幅值与气动角
    V = np.sqrt(u*u + v*v + w*w)
    if V < 1e-6:
        V = 1e-6
    alpha = np.arctan2(w, u)          # 攻角
    beta = np.arcsin(np.clip(v/V, -1, 1))  # 侧滑角
    
    # 动压
    qbar = 0.5 * rho * V * V
    
    # 旋转矩阵
    R_bg = rotation_matrix(phi, theta, psi)
    
    # --- 位置运动学 ---
    pos_dot = R_bg @ np.array([u, v, w])
    
    # --- 姿态运动学 ---
    W = attitude_kinematics_matrix(phi, theta)
    att_dot = W @ np.array([p, q, r])
    
    # --- 线动力学 ---
    # 重力在弹体系投影
    F_grav_b = R_bg.T @ np.array([0.0, -m*g, 0.0])
    # 气动力（弹体系）
    F_aero_b = np.array([
        -CA * qbar * S,
        CN * qbar * S * alpha,
        -CN * qbar * S * beta
    ])
    F_total = F_grav_b + F_aero_b
    
    omega = np.array([p, q, r])
    vel_dot = F_total / m - np.cross(omega, np.array([u, v, w]))
    
    # --- 角动力学 ---
    L_aero = Clp * qbar * S * d * (p*d/(2*V))
    M_aero = Cma * qbar * S * d * alpha + Cmq * qbar * S * d * (q*d/(2*V))
    N_aero = -Cnb * qbar * S * d * beta + Cnr * qbar * S * d * (r*d/(2*V))
    
    p_dot = ((Iy - Iz)*q*r + L_aero) / Ix
    q_dot = ((Iz - Ix)*r*p + M_aero) / Iy
    r_dot = ((Ix - Iy)*p*q + N_aero) / Iz
    
    return np.concatenate([pos_dot, vel_dot, att_dot, [p_dot, q_dot, r_dot]])

# 初始状态：位置原点，速度(200,50,0)m/s，零姿态，零角速度
X0 = np.array([0.0, 0.0, 0.0, 200.0, 50.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

# 仿真时间
t_span = (0.0, 10.0)
t_eval = np.linspace(0.0, 10.0, 1001)

# RK45数值积分
sol = solve_ivp(eom, t_span, X0, method='RK45', t_eval=t_eval, rtol=1e-8, atol=1e-10)

# 提取结果
t = sol.t
x, y, z = sol.y[0], sol.y[1], sol.y[2]
u, v, w = sol.y[3], sol.y[4], sol.y[5]
phi, theta, psi = sol.y[6], sol.y[7], sol.y[8]
p, q, r = sol.y[9], sol.y[10], sol.y[11]

# 计算速度幅值
V_mag = np.sqrt(u**2 + v**2 + w**2)

# 输出关键结果
print("=== 无控状态6-DOF仿真结果 ===")
print(f"仿真时间: {t[-1]:.1f}s, 步数: {len(t)}")
print(f"最终位置: x={x[-1]:.2f}m, y={y[-1]:.2f}m, z={z[-1]:.2f}m")
print(f"最终速度: u={u[-1]:.2f}, v={v[-1]:.2f}, w={w[-1]:.2f} m/s")
print(f"速度幅值: 初始={V_mag[0]:.2f} m/s → 最终={V_mag[-1]:.2f} m/s")
print(f"最终姿态角: φ={np.degrees(phi[-1]):.2f}°, θ={np.degrees(theta[-1]):.2f}°, ψ={np.degrees(psi[-1]):.2f}°")
print(f"最终角速度: p={p[-1]:.4f}, q={q[-1]:.4f}, r={r[-1]:.4f} rad/s")

# 验证姿态运动学关系：数值微分姿态角 vs 角速度积分
dphi_dt_num = np.gradient(phi, t)
phi_dot_recon = p + np.sin(phi)*np.tan(theta)*q + np.cos(phi)*np.tan(theta)*r
max_phi_err = np.max(np.abs(dphi_dt_num - phi_dot_recon))
print(f"\n姿态运动学验证: 最大φ̇误差 = {max_phi_err:.2e} rad/s")

# 验证能量约束：无气动时机械能守恒（此处有气动，速度应单调下降）
print(f"速度变化验证: 末速度/初速度 = {V_mag[-1]/V_mag[0]:.4f} (应<1, 因气动阻力)")

运行方式与依赖

需要Python 3.8+及NumPy、SciPy库。安装依赖：

pip install numpy scipy

将上述代码保存为 missile_6dof.py，在终端运行：

python missile_6dof.py

验证方法与判定标准

模型正确性通过以下三项判定：

姿态运动学一致性：程序输出的最大φ̇误差应小于 1e-6 rad/s，证明姿态角微分方程与角速度定义自洽；
速度幅值单调衰减：由于存在气动阻力且无推力，末速度与初速度之比应小于1（预期约0.85～0.95），且速度曲线无振荡发散；
物理合理性：俯仰角θ在重力作用下应逐渐减小（弹道下坠），偏航角ψ保持小量（初始无侧向力），位置y先升后降符合弹道特征。

若上述条件满足，则证明六自由度模型建立正确且数值实现有效。

📚 课程目录