第4章：飞控数学模型：六自由度刚体动力学模型、四元数与欧拉角、地球坐标系与机体坐标系
各位同学，欢迎来到第四章。
说实话，飞控数学模型是整个硬件在环仿真的地基。地基没打好，后面跑再漂亮的算法也是空中楼阁。我自己带项目时，见过太多团队在仿真里飞得挺好，一上真机就炸——十有八九是模型这里出了问题。
这一章，我们就把三个核心概念掰开揉碎：六自由度刚体动力学、四元数与欧拉角、坐标系变换。嗯，都是硬骨头，但啃下来之后，你会发现飞控的世界清晰很多。
4.1 六自由度刚体动力学模型
什么叫六自由度？说白了，就是飞机在三维空间里能做的六种独立运动。
我习惯把它分成两组：
- 线运动（3个）：前后（X轴）、左右（Y轴）、上下（Z轴）
- 角运动（3个）：滚转（绕X轴）、俯仰（绕Y轴）、偏航（绕Z轴）
你想想看，一架四旋翼在天上飞，不就是这六个量的组合吗？
核心公式（牛顿-欧拉方程）：
// 线运动方程
m * dV/dt = F_gravity + F_thrust + F_aero
// 角运动方程
I * dω/dt = M_thrust + M_gyro - ω × (I * ω)其中 m 是质量，I 是惯性张量矩阵，ω 是角速度向量。
我在项目中遇到过一个问题：仿真里飞机悬停很稳，但真机总是慢慢漂移。查了两天才发现，是惯性张量矩阵里的交叉耦合项没设对。嗯，这种坑，踩过一次就记住了。
4.2 四元数与欧拉角
说到姿态表示，很多新手第一反应就是欧拉角。确实直观——俯仰30度、滚转15度，一听就懂。
但欧拉角有个致命问题：万向锁。
避坑指南：
我曾经在某个项目中，用欧拉角做姿态插值。结果飞机在俯仰90度附近时，滚转和偏航突然耦合，姿态直接飞了。后来换成四元数，问题立刻解决。
记住：凡是涉及大角度机动或连续旋转，优先用四元数。
四元数长什么样？其实就是一个四维向量：
q = [q0, q1, q2, q3]其中 q0 是标量部分，q1~q3 是矢量部分。它没有奇点，计算效率也高。
欧拉角和四元数之间的转换，我建议你直接背下来：
| 转换方向 | 公式 |
|---|---|
| 欧拉角 → 四元数 | q = f(φ, θ, ψ) // 三个旋转的乘积 |
| 四元数 → 欧拉角 | φ = atan2(2*(q0*q1+q2*q3), 1-2*(q1²+q2²)) |
我的个人习惯：
在飞控代码里，内部运算全部用四元数。只在给地面站发数据时，才转成欧拉角方便人看。这样既避免了奇点，又保留了可读性。
4.3 地球坐标系与机体坐标系
坐标系变换，说白了就是回答一个问题：飞机自己感觉到的力，在地球上看是什么方向？
常用的两个坐标系：
- 地球坐标系（NED）：北-东-地，固定在地面上
- 机体坐标系（Body）：前-右-下，固定在飞机上
从机体到地球的变换，靠的就是旋转矩阵。这个矩阵由四元数或欧拉角构造：
// 由四元数构造旋转矩阵
R = [
[1-2*(q2²+q3²), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)],
[2*(q1*q2+q0*q3), 1-2*(q1²+q3²), 2*(q2*q3-q0*q1)],
[2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1²+q2²)]
]你想想看，GPS给的是地球坐标系下的位置，加速度计测的是机体坐标系下的加速度。没有这个变换矩阵，你根本没法把两者对上。
实战要点：
在硬件在环仿真中，我建议你这样做：
- 用四元数维护姿态状态
- 用旋转矩阵做坐标系变换
- 只在日志和调试时转成欧拉角
这样既高效又安全。
4.4 三者的关系
这三个东西不是孤立的。我画个逻辑链给你看：
六自由度动力学 → 输出加速度和角加速度
↓
积分得到速度和角速度
↓
四元数更新姿态
↓
旋转矩阵做坐标系变换
↓
反馈给动力学模型嗯，这就是一个完整的闭环。我在做第一个HIL仿真平台时，就是按这个链条一步步搭起来的。每一步都单独测试，最后联调时基本没出大问题。
给新手的建议：
别急着把代码写全。先用手算一个简单的例子——比如飞机只做俯仰运动——验证四元数更新和坐标系变换是否正确。这一步花不了半小时，但能帮你省下后面几天的调试时间。
好了，这一章的内容就到这里。下一章我们会把这些模型放到仿真环境里，看看怎么用代码实现。到时候我会分享一些具体的代码片段和调试技巧。