摘要：
在第一篇中，我们通过3-DOF质点模型探讨了导弹的质心运动，但这仅仅是飞行力学的冰山一角。真实的导弹是一个具有转动惯量的刚体，它在空中不仅会平移，还会滚转、俯仰和偏航。本篇将带领读者跨越"质点"到"刚体"的认知鸿沟，深入构建六自由度（6-DOF） 全量仿真系统。我们将重点攻克四元数（Quaternion） 这一数学工具，彻底解决欧拉角在俯仰角±90°时的"万向节死锁（Gimbal Lock）"问题，并详细推导刚体的欧拉转动方程。文章将提供一套完整的单文件Python代码，涵盖从姿态更新、转动惯量处理到6-DOF全状态方程求解的全过程，为后续引入气动耦合与飞控系统打下坚实的数学与工程基础。
使用场景介绍：
6-DOF模型是导弹飞行力学的最高形态，适用于以下核心场景：
-
自动驾驶仪（Autopilot）设计：验证控制器在面对导弹转动惯量时的响应速度与超调量。
-
气动弹性与耦合分析：研究滚转通道的操纵为何会引起偏航通道的耦合运动（惯性耦合）。
-
引战配合（Fuze-Warhead Coordination）：精确计算战斗部引爆时，弹头指向与目标的相对几何关系。
-
HIL/SIL仿真：硬件在回路仿真必须基于6-DOF模型，因为传感器（IMU）测量的是角速度，而非角度。
不适用场景（红线警告）：
-
超大规模集群对抗：由于计算量激增（状态数是3-DOF的两倍，且涉及矩阵运算），不适合数千个实体的实时推演。
-
快速射表生成：如果只是想粗略看看导弹能飞多远，用6-DOF是杀鸡用牛刀，效率极低。
问题讨论：
-
**为什么必须放弃欧拉角？** 欧拉角（Pitch, Yaw, Roll）直观但存在数学奇点。当导弹进行大机动导致俯仰角接近90度时，欧拉角微分方程会产生除零错误，导致仿真崩溃。
-
**四元数为什么能解决死锁？** 四元数本质上是四维单位球面上的旋转表示，它没有奇点，且插值平滑，是航天器姿态解算的工业标准。
-
**转动惯量矩阵是恒定的吗？** 对于变质心导弹（如燃料消耗导致质量分布变化），转动惯量 I是时间的函数，本篇将讨论其处理方法。
公式推导：
1. 坐标系与姿态表示
我们引入弹体坐标系（Body Frame, b），原点位于导弹质心，xb指向弹头，yb指向弹体右侧，zb按右手定则指向弹体下方。
四元数定义：
四元数运动学方程
2. 刚体转动动力学（欧拉方程）
根据动量矩定理，在弹体坐标系下，外力矩 M与角动量的关系为：
3. 全状态向量与微分方程
4. 坐标变换（方向余弦矩阵 DCM）
为了计算气动力，需要将速度从地面系转换到弹体系，或者将推力从弹体系转换到地面系。利用四元数构建DCM：
代码结构：
我们将代码设计为单文件结构，严格区分平动与转动逻辑，并引入四元数工具类。
python
# ============================================================================
# MissileSim-Py: 6-DOF Rigid Body Simulation
# Blog Part 2: Quaternions and Full Attitude Dynamics
# ============================================================================
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
import time
# ----------------------------------------------------------------------------
# 1. Utility: Quaternion Math
# ----------------------------------------------------------------------------
class QuatTools:
@staticmethod
def multiply(q, r):
"""Quaternion multiplication q x r."""
w1, x1, y1, z1 = q
w2, x2, y2, z2 = r
return np.array([
w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2
])
@staticmethod
def normalize(q):
"""Normalize quaternion to unit length. CRITICAL FOR STABILITY."""
norm = np.linalg.norm(q)
if norm < 1e-12:
return np.array([1.0, 0.0, 0.0, 0.0])
return q / norm
@staticmethod
def derivative(q, omega):
"""Kinematics: dq/dt = 0.5 * q x omega_quat."""
qw, qx, qy, qz = q
wx, wy, wz = omega
# Matrix form: 0.5 * Omega_matrix * q
dq = 0.5 * np.array([
-qx*wx - qy*wy - qz*wz,
qw*wx + qy*wz - qz*wy,
qw*wy - qx*wz + qz*wx,
qw*wz + qx*wy - qy*wx
])
return dq
@staticmethod
def to_rotation_matrix(q):
"""Convert quaternion to Direction Cosine Matrix (DCM)."""
qw, qx, qy, qz = q
return np.array([
[1-2*(qy**2+qz**2), 2*(qx*qy-qw*qz), 2*(qx*qz+qw*qy)],
[2*(qx*qy+qw*qz), 1-2*(qx**2+qz**2), 2*(qy*qz-qw*qx)],
[2*(qx*qz-qw*qy), 2*(qy*qz+qw*qx), 1-2*(qx**2+qy**2)]
])
# ----------------------------------------------------------------------------
# 2. Missile 6-DOF Dynamics
# ----------------------------------------------------------------------------
class Missile6DOF:
def __init__(self, config):
self.cfg = config
# State: [x,y,z, vx,vy,vz, qw,qx,qy,qz, p,q,r]
self.state = np.zeros(13)
self.state[6] = 1.0 # Quaternion identity [1,0,0,0]
# Inertia Tensor (Diagonal for simplicity)
self.I = np.diag([config['Ixx'], config['Iyy'], config['Izz']])
self.I_inv = np.linalg.inv(self.I)
def set_initial_state(self, pos, vel, quat, omega):
self.state[0:3] = pos
self.state[3:6] = vel
self.state[6:10] = QuatTools.normalize(quat)
self.state[10:13] = omega
def dynamics(self, t, state):
"""Full 6-DOF Equations of Motion."""
# Unpack state
x, y, z = state[0:3]
vx, vy, vz = state[3:6]
q = state[6:10]
p, q_ang, r = state[10:13] # Angular velocities
# --- 1. Translational Kinematics ---
dx, dy, dz = vx, vy, vz
# --- 2. Translational Dynamics ---
# Forces in Inertial Frame (Simplified: Gravity + Thrust)
# Thrust in Body Frame
thrust_body = np.array([self.cfg['thrust'], 0.0, 0.0])
# Convert Thrust to Inertial Frame
R = QuatTools.to_rotation_matrix(q)
thrust_inertial = R.T @ thrust_body # R^T transforms body to inertial
gravity = np.array([0, -self.cfg['mass'] * 9.81, 0])
forces = thrust_inertial + gravity
accel = forces / self.cfg['mass']
dvx, dvy, dvz = accel
# --- 3. Rotational Kinematics (Quaternion Derivative) ---
omega_vec = np.array([p, q_ang, r])
dq = QuatTools.derivative(q, omega_vec)
dqw, dqx, dqy, dqz = dq
# --- 4. Rotational Dynamics (Euler's Equation) ---
# M = I * dw/dt + w x (I * w)
# Assuming no external moments for this basic demo (M_ext = 0)
M_ext = np.array([0.0, 0.0, 0.0])
I_omega = self.I @ omega_vec
gyro_term = np.cross(omega_vec, I_omega)
domega = self.I_inv @ (M_ext - gyro_term)
dp, dq_ang, dr = domega
# Return derivatives
return [dx, dy, dz, dvx, dvy, dvz, dqw, dqx, dqy, dqz, dp, dq_ang, dr]
# ----------------------------------------------------------------------------
# 3. Simulation Runner & Visualization
# ----------------------------------------------------------------------------
def main():
# Configuration
CONFIG = {
'mass': 50.0,
'thrust': 1200.0,
'Ixx': 2.0, # Roll inertia
'Iyy': 50.0, # Pitch inertia
'Izz': 50.0 # Yaw inertia
}
missile = Missile6DOF(CONFIG)
# Initial Conditions
# Position: Ground level
init_pos = np.array([0.0, 0.0, 0.0])
# Velocity: 300 m/s forward
init_vel = np.array([300.0, 0.0, 0.0])
# Attitude: Level flight (no rotation, identity quaternion)
init_quat = np.array([1.0, 0.0, 0.0, 0.0])
# Angular Velocity: Give it a slight pitch rate (q=0.5 rad/s) and roll rate (p=1.0 rad/s)
init_omega = np.array([1.0, 0.5, 0.0])
missile.set_initial_state(init_pos, init_vel, init_quat, init_omega)
print("Starting 6-DOF Simulation...")
start_time = time.perf_counter()
# Run Solver
sol = solve_ivp(
fun=missile.dynamics,
t_span=(0, 20.0),
y0=missile.state,
method='DOP853',
max_step=0.01,
rtol=1e-9,
atol=1e-12
)
end_time = time.perf_counter()
print(f"Simulation finished in {end_time - start_time:.4f} seconds.")
print(f"Status: {sol.status}, Steps: {len(sol.t)}")
# --- Visualization ---
fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d')
# Plot trajectory (X vs Z vs Y for standard aerospace view)
ax.plot(sol.y[0], sol.y[2], sol.y[1], linewidth=2)
ax.set_xlabel("Range (m)")
ax.set_ylabel("Side Distance (m)")
ax.set_zlabel("Altitude (m)")
ax.set_title("6-DOF Missile Trajectory with Rolling Motion")
plt.show()
if __name__ == "__main__":
main()控制流程：
6-DOF仿真的控制流比3-DOF复杂得多，因为它涉及两个独立的动力学回路（平动与转动）及其耦合。
效果演示：
运行上述代码，你将看到：
-
3D轨迹 ：由于初始给了俯仰角速度 (q=0.5) 和滚转角速度 (p=1.0)，导弹不会直线飞行，而是会画出一个螺旋上升或盘旋的轨迹。这证明了转动自由度对质心运动的影响。
-
姿态变化：如果在代码中打印四元数，你会看到 qw,qx,qy,qz随时间不断变化，而欧拉角（可以通过四元数转换得到）会显示出持续的俯仰和滚转变化。
(此处应插入一张3D螺旋轨迹图，显示导弹在爬升的同时在水平面内盘旋)
问题总结分析与提高：
-
四元数归一化的重要性 ：数值积分会导致四元数模长偏离1，导致旋转矩阵不再是正交矩阵，进而引发能量不守恒。必须在
dynamics函数返回前或积分器每一步之后强制归一化。 -
惯性积（Products of Inertia）：本文假设导弹是完全轴对称的，因此惯性张量 I是对角阵。真实的导弹由于内部设备安装，存在非对角项（如 Ixy），这会引起更复杂的耦合。
-
气动力矩缺失：目前的代码只有推力，没有气动力矩（如静稳定力矩、阻尼力矩）。下一阶段引入气动数据库后，导弹将表现出"自稳定"或"发散"的趋势。
-
计算性能：6-DOF涉及大量的矩阵和四元数运算，如果进行蒙特卡洛打靶，建议使用Numba或C++重写核心循环。
结语：
本篇完成了从"质点"到"刚体"的飞跃。掌握了四元数与欧拉方程，你就拥有了模拟任何飞行器全姿态运动的能力。在下一篇文章中，我们将引入空气动力学数据库（AeroDB），让导弹在大气中受到真实的升力与阻力作用，并探讨如何计算攻角与侧滑角。