1. 摘要 (Abstract)坐标系定义与运动学建模是6-DOF仿真的基石。本文将系统介绍航空航天仿真中最常用的坐标系惯性系、体轴系重点对比欧拉角、旋转矩阵与四元数在描述姿态时的优劣。我们将揭示欧拉角的“万向节锁Gimbal Lock”缺陷并论证四元数为何在工程实践中成为首选。文章最后将通过Python代码实现旋转矩阵与四元数的相互转换并可视化演示万向节锁现象。2. 坐标系体系 (Coordinate Systems)在飞机与导弹仿真中混乱的坐标系定义是导致Bug的主要来源。我们必须严格遵守约定。2.1 北-东-地坐标系 (NED - North-East-Down)用途惯性系Inertial Frame用于描述物体的绝对位置和速度。定义x轴指向北 (North)。y轴指向东 (East)。z轴指向地 (Down)垂直于参考椭球体向下。特点随着地球曲率变化但在大多数战术导弹和小范围飞机机动仿真中我们将其视为平面大地忽略地球曲率。2.2 机体坐标系 (Body Frame)用途体轴系用于描述气动力、发动机推力、惯性张量等。定义航空惯例xb轴指向机头Roll轴滚转。yb轴指向右机翼Pitch轴俯仰。zb轴指向下方Yaw轴偏航。关键关系气动参数如升力系数 CL、阻力系数 CD通常是在体轴系下定义的。惯性系与体轴系的关系。旋转矩阵 Cbn负责将体轴系下的矢量转换到惯性系。3. 姿态表示方法 (Attitude Representations)如何将体轴系“贴”在惯性系上主要有三种方法3.1 欧拉角 (Euler Angles)定义通过三次连续旋转来描述姿态。航空领域标准序列为Z-Y-X缺点万向节锁 (Gimbal Lock)当俯仰角 θ±90∘时第一次旋转轴和第三次旋转轴重合导致失去一个自由度。此时无法通过微分变化区分偏航和滚转数值求解会趋于无穷大。3.2 旋转矩阵 (Rotation Matrix)优点直观无奇点。缺点有9个元素但只有3个自由度存在6个约束正交性计算量大不适合积分。3.3 四元数 (Quaternion) ——工程首选优点无奇点完美避开万向节锁。计算高效仅需积分4个变量比旋转矩阵少。数值稳定易于归一化适合长时间仿真。4. 理论推导从角速度到四元数 (Kinematics)在6-DOF模型中我们测量或设定的是体轴系下的角速度我们需要通过运动学方程更新姿态。4.1 四元数微分方程四元数对时间的导数与角速度的关系如下将其展开为矩阵形式便于编程实现4.2 旋转矩阵与四元数的转换我们需要将体轴系下的气动力转换到惯性系进行位置更新这需要旋转矩阵 Cbn。它由四元数构造而来5. 代码实现与可视化5.1 四元数工具库我们首先构建一个基础的代数库包含四元数乘法、归一化和旋转矩阵生成。import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # ---------------------------------------------------- # 1. 四元数代数库 # ---------------------------------------------------- def quaternion_normalize(q): 归一化四元数 return q / np.linalg.norm(q) def quaternion_multiply(p, q): 四元数乘法 p ⊗ q p0, p1, p2, p3 p q0, q1, q2, q3 q return np.array([ p0*q0 - p1*q1 - p2*q2 - p3*q3, p0*q1 p1*q0 p2*q3 - p3*q2, p0*q2 - p1*q3 p2*q0 p3*q1, p0*q3 p1*q2 - p2*q1 q3*p0 ]) def quaternion_to_rotation_matrix(q): 将四元数转换为旋转矩阵 C_b^n q0, q1, q2, q3 q return np.array([ [q0**2q1**2-q2**2-q3**2, 2*(q1*q2 q0*q3), 2*(q1*q3 - q0*q2)], [2*(q1*q2 - q0*q3), q0**2-q1**2q2**2-q3**2, 2*(q2*q3 q0*q1)], [2*(q1*q3 q0*q2), 2*(q2*q3 - q0*q1), q0**2-q1**2-q2**2q3**2] ]) def euler_to_quaternion(phi, theta, psi): 欧拉角 (roll, pitch, yaw) 转四元数 cy np.cos(psi * 0.5) sy np.sin(psi * 0.5) cp np.cos(theta * 0.5) sp np.sin(theta * 0.5) cr np.cos(phi * 0.5) sr np.sin(phi * 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 return np.array([q0, q1, q2, q3]) # ---------------------------------------------------- # 2. 可视化辅助函数 # ---------------------------------------------------- def plot_coordinate_frame(ax, R, originnp.zeros(3), scale1.0, label): 在3D轴上绘制坐标系 x_axis R[:, 0] * scale y_axis R[:, 1] * scale z_axis R[:, 2] * scale ax.quiver(origin[0], origin[1], origin[2], x_axis[0], x_axis[1], x_axis[2], colorr, labelf{label}-X if label else None) ax.quiver(origin[0], origin[1], origin[2], y_axis[0], y_axis[1], y_axis[2], colorg, labelf{label}-Y if label else None) ax.quiver(origin[0], origin[1], origin[2], z_axis[0], z_axis[1], z_axis[2], colorb, labelf{label}-Z if label else None)5.2 Demo 1修正上一节的动力学方程现在我们用正确的运动学方程替换第01篇中的简化版本。def derivatives_corrected(state, mass, gravity): 修正后的导数计算正确处理坐标变换 pos, vel_b, quat, omega_b state.pos, state.vel, state.quat, state.omega # 1. 位置导数: dp/dt C_b^n * v_b C_bn quaternion_to_rotation_matrix(quat) vel_n C_bn vel_b pos_dot vel_n # 2. 速度导数 (体轴系): dv/dt F/m - omega x v # 重力在体轴系的投影: C_n^b * [0, 0, g] C_nb C_bn.T # 转置即逆矩阵 gravity_force_n np.array([0, 0, gravity]) gravity_force_b C_nb gravity_force_n # 假设除了重力没有其他力 forces_b gravity_force_b vel_dot_b forces_b / mass - np.cross(omega_b, vel_b) # 3. 四元数导数 p, q, r omega_b Omega np.array([ [0, -p, -q, -r], [p, 0, r, -q], [q, -r, 0, p], [r, q, -p, 0] ]) quat_dot 0.5 * Omega quat # 4. 角速度导数 (仍为0因为没有力矩) omega_dot np.zeros(3) return pos_dot, vel_dot_b, quat_dot, omega_dot # 运行修正后的仿真代码片段复用第01篇的rk4_step和run_simulation逻辑 # 注意现在pos_dot使用的是vel_n而不是vel_b轨迹的物理意义才是正确的。5.3 Demo 2万向节锁可视化这个Demo将直观展示欧拉角在 θ90∘时的奇异现象。def demonstrate_gimbal_lock(): fig plt.figure(figsize(10, 8)) ax fig.add_subplot(111, projection3d) # 初始姿态俯仰角 89度 (接近锁死) phi np.deg2rad(0) theta np.deg2rad(89.9) # 接近90度 psi np.deg2rad(0) q euler_to_quaternion(phi, theta, psi) R quaternion_to_rotation_matrix(q) # 绘制惯性系 plot_coordinate_frame(ax, np.eye(3), scale2, labelInertial) # 绘制体轴系 plot_coordinate_frame(ax, R, scale1, labelBody) ax.set_xlim([-3, 3]) ax.set_ylim([-3, 3]) ax.set_zlim([-3, 3]) ax.set_title(fGimbal Lock Demonstration\nPitch{np.rad2deg(theta):.1f}°\nNotice X and Z axes alignment) ax.legend() ax.set_box_aspect([1,1,1]) plt.show() if __name__ __main__: # 运行万向节锁演示 demonstrate_gimbal_lock()结果分析当俯仰角接近90度时你会发现机体的X轴机头方向与Z轴机腹方向几乎与惯性系的Z轴对齐。此时无论你如何调整偏航角ψ或滚转角ϕ你都无法恢复对某个轴向的独立控制。这就是万向节锁的几何本质。6. 总结与展望 (Conclusion)本篇解决了6-DOF仿真中最核心的几何问题明确了坐标系确立了NED惯性与Body体轴的双坐标系体系。攻克了姿态表示通过对比确立了四元数在数值仿真中的统治地位。完善了运动学方程给出了从角速度 ω到四元数导数的微分方程并提供了旋转矩阵的转换代码。修正了历史遗留问题在第01篇Demo的基础上引入了 Cbn旋转矩阵使得位置更新逻辑符合物理实际。
