2. 六自由度运动学方程：坐标系定义、欧拉角与四元数表示法
各位同学，咱们今天聊聊六自由度运动学里最基础、也最容易让人栽跟头的部分——坐标系和姿态表示。
说实话，我刚开始做飞行器建模那会儿，觉得坐标系嘛，不就是三个轴嘛，有什么难的？结果第一次做高攻角仿真，数据全乱套了。后来才发现，坐标系定义不清晰，后面全是白干。所以这节课，咱们把坐标系、欧拉角、四元数这几个硬骨头啃透。
2.1 坐标系定义——别小看这一步
做六自由度建模，第一步就是定坐标系。我个人习惯，至少定义三个坐标系：
- 地面坐标系（惯性系）：通常取北东地或东北天。我建议用北东地，因为和大多数飞控的惯导输出一致。
- 机体坐标系：原点在飞行器质心，X轴指向机头，Y轴指向右翼，Z轴指向机腹。
- 气流坐标系：也叫速度坐标系，X轴沿来流方向。
嗯，这里要注意：不同坐标系之间的转换矩阵，一定要反复验证。我在项目中遇到过，因为坐标系定义搞反了，导致仿真里飞机一直往反方向飞，排查了两天才发现是Y轴方向定义反了。
核心原则：所有坐标系都遵循右手定则。角速度正方向按右手螺旋定则确定。
2.2 欧拉角表示法——直观但有个大坑
欧拉角大家应该都熟悉，就是俯仰角θ、滚转角φ、偏航角ψ。为什么用这三个？说白了，就是绕机体三轴依次旋转。
旋转顺序我建议用Z-Y-X（偏航→俯仰→滚转），这是航空航天领域的标准。对应的旋转矩阵是：
C_b^n = R_x(φ) * R_y(θ) * R_z(ψ)其中R_x、R_y、R_z分别是绕X、Y、Z轴的基本旋转矩阵。
但是！欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，滚转和偏航的旋转轴会重合，丢失一个自由度。你想想看，大迎角飞行时，俯仰角很容易超过60°，甚至接近90°，这时候欧拉角就不好使了。
避坑指南：我曾经用欧拉角做全包线仿真，飞到90°迎角附近，姿态解算直接发散。后来改用四元数，问题才解决。所以，如果你的飞行器会做大迎角机动，建议直接上四元数。
2.3 四元数表示法——优雅但需要理解
四元数是什么？说白了就是一个超复数，q = q0 + q1*i + q2*j + q3*k。它用四个参数表示三维旋转，没有奇异性。
我刚开始学四元数时，觉得这东西很抽象。后来发现，你只要记住两个核心公式就行：
- 旋转公式：p' = q * p * q^(-1)，其中p是三维向量对应的纯四元数
- 运动学方程：dq/dt = 0.5 * q * ω，其中ω是角速度四元数
嗯，这里要特别强调：四元数必须归一化。数值积分过程中，四元数的模会漂移，需要定期归一化。我一般每步积分后都做一次归一化，虽然增加了一点计算量，但保证了稳定性。
个人经验：四元数转欧拉角时，注意判断象限。我写过一个转换函数，因为没处理atan2的边界情况，导致在ψ=180°附近跳变。后来加了角度归一化处理，问题解决。
2.4 欧拉角与四元数的转换
实际工程中，我们经常需要在两者之间切换。比如，飞控输出的是欧拉角，但姿态解算用四元数。转换公式如下：
| 转换方向 | 公式 |
|---|---|
| 欧拉角→四元数 | q0 = cos(φ/2)cos(θ/2)cos(ψ/2) + sin(φ/2)sin(θ/2)sin(ψ/2) q1 = sin(φ/2)cos(θ/2)cos(ψ/2) - cos(φ/2)sin(θ/2)sin(ψ/2) q2 = cos(φ/2)sin(θ/2)cos(ψ/2) + sin(φ/2)cos(θ/2)sin(ψ/2) q3 = cos(φ/2)cos(θ/2)sin(ψ/2) - sin(φ/2)sin(θ/2)cos(ψ/2) |
| 四元数→欧拉角 | φ = atan2(2(q0q1+q2q3), 1-2(q1²+q2²)) θ = asin(2(q0q2-q3q1)) ψ = atan2(2(q0q3+q1q2), 1-2(q2²+q3²)) |
注意：四元数转欧拉角时，当θ接近±90°，asin的参数可能略超出[-1,1]范围，需要做截断处理。我曾经因为这个bug，仿真到一半程序崩溃，排查了好久才发现是数值误差导致的。
2.5 六自由度运动学方程
有了姿态表示，我们就可以写出完整的运动学方程了。这里我直接给出四元数形式的方程：
位置方程：
dP/dt = v
速度方程（体坐标系下）：
dv/dt = F/m - ω × v
姿态方程（四元数形式）：
dq/dt = 0.5 * q ⊗ ω
角速度方程：
dω/dt = I^(-1) * (M - ω × Iω)其中，F是合力（包括气动力、推力、重力），M是合力矩，I是惯性张量，ω是角速度向量。
你想想看，这组方程其实就是在描述：力改变速度，力矩改变角速度，然后速度和角速度再改变位置和姿态。就这么简单。
核心要点：大迎角下，气动力和力矩的非线性很强，但运动学方程本身是精确的。难点在于气动数据的准确性和数值积分的稳定性。
2.6 数值积分注意事项
最后，聊几句数值积分。我建议用四阶龙格-库塔法，步长取0.001秒左右。对于四元数，每步积分后记得归一化。
另外，角速度积分时，注意惯性张量的耦合效应。大迎角下，飞机的转动惯量可能会因为燃油消耗而变化，如果做高精度仿真，建议实时更新惯性张量。
避坑指南：我曾经用固定步长做仿真，结果在高速滚转时，步长太大导致数值发散。后来改用自适应步长，问题解决。所以，如果你的飞行器机动性很强，建议用变步长积分器。
好了，这节课的内容就到这里。坐标系定义、欧拉角、四元数，这三者是六自由度建模的基石。搞清楚了，后面的气动建模、控制律设计才能站得住脚。