3、姿态解算基础：欧拉角与四元数、旋转矩阵、坐标系定义
各位同学，欢迎来到第三讲。今天我们要啃的这块骨头，是所有飞控算法的基础——姿态解算。说白了，就是让飞控知道“飞机现在脑袋朝哪边”。
我记得刚入行那会儿，带我的老工程师跟我说过一句话：“姿态算不对，飞机必炸。” 当时觉得夸张，后来自己调参炸了几次机，才明白这句话的分量。嗯，咱们今天就把这块硬骨头啃透。
3.1 坐标系定义：先定规矩，再谈计算
做姿态解算，第一步不是算，而是定义坐标系。坐标系不统一，后面全是鸡同鸭讲。我在项目里见过最坑的事，就是两个模块用了不同定义的坐标系，结果融合出来的姿态直接翻了个个儿。
我们主要用两个坐标系：
3.1.1 地理坐标系（NED系）
也叫导航坐标系。你想象自己站在地面上：
- X轴：指向正北（North）
- Y轴：指向正东（East）
- Z轴：指向地心（Down），也就是垂直向下
为什么Z轴要向下？因为重力加速度是向下的，这样定义后，静止时加速度计读出的Z轴就是+1g，处理起来很直观。我个人习惯用NED系，因为PX4、ArduPilot这些主流飞控都这么用。
3.1.2 机体坐标系（Body系）
这个坐标系固定在飞机上，跟着飞机一起转：
- X轴：指向机头方向（前进方向）
- Y轴：指向飞机右侧（右翼方向）
- Z轴：指向飞机下方（符合右手定则）
你想想看，IMU（惯性测量单元）就焊在飞控板上，它测量的加速度和角速度，天然就是机体坐标系下的数据。而我们想让飞机保持水平，需要知道机体相对于地面的姿态——这就引出了坐标变换。
核心思想：姿态解算的本质，就是找到从机体坐标系到地理坐标系的旋转关系。
3.2 欧拉角：最直观，但有个大坑
欧拉角是描述姿态最直观的方式。三个角度，分别对应绕三个轴的旋转：
| 角度 | 符号 | 绕轴 | 范围 |
|---|---|---|---|
| 横滚角 | φ (Roll) | X轴 | -180° ~ 180° |
| 俯仰角 | θ (Pitch) | Y轴 | -90° ~ 90° |
| 偏航角 | ψ (Yaw) | Z轴 | -180° ~ 180° |
旋转顺序很重要。我们一般用Z-Y-X顺序（先偏航，再俯仰，最后横滚）。为什么是这个顺序？因为这样最符合直觉：先确定机头朝向，再低头抬头，最后左右倾斜。
⚠️ 万向锁（Gimbal Lock）：这是欧拉角最大的坑。当俯仰角达到±90°时，横滚和偏航会失去一个自由度。我曾经在调试一个垂直起降固定翼时，飞机在过渡阶段俯仰角接近90°，欧拉角直接炸了，姿态瞬间跳变。从那以后，我只要涉及全姿态飞行，一定用四元数。
3.3 旋转矩阵：数学上最严谨
旋转矩阵是一个3x3的正交矩阵，它能把机体坐标系下的向量，变换到地理坐标系下。
绕Z轴旋转ψ角的矩阵：
Rz(ψ) = [cosψ -sinψ 0]
[sinψ cosψ 0]
[0 0 1]绕Y轴旋转θ角的矩阵：
Ry(θ) = [cosθ 0 sinθ]
[0 1 0 ]
[-sinθ 0 cosθ]绕X轴旋转φ角的矩阵：
Rx(φ) = [1 0 0 ]
[0 cosφ -sinφ]
[0 sinφ cosφ]完整的旋转矩阵（Z-Y-X顺序）：
Cnb = Rz(ψ) * Ry(θ) * Rx(φ)展开后长这样：
Cnb = [cosθcosψ sinφsinθcosψ-cosφsinψ cosφsinθcosψ+sinφsinψ]
[cosθsinψ sinφsinθsinψ+cosφcosψ cosφsinθsinψ-sinφcosψ]
[-sinθ sinφcosθ cosφcosθ ]看着复杂，但实际用起来很稳。旋转矩阵没有万向锁问题，而且可以连续旋转。缺点就是计算量大——每次更新都要算9个三角函数，对单片机来说有点吃力。
💡 小技巧：在实际工程中，我们很少直接计算旋转矩阵。一般是用四元数做姿态更新，需要的时候再转成旋转矩阵或欧拉角。这样既避免了万向锁，又降低了计算量。
3.4 四元数：飞控的标配
四元数，说白了就是一个超复数。形式是：
q = w + xi + yj + zk其中w是实部，x、y、z是虚部。约束条件：w² + x² + y² + z² = 1（单位四元数）。
为什么飞控都用四元数？三个理由：
- 无万向锁：全姿态可用，飞机翻过来也能算
- 计算量小：只有乘法和加法，没有三角函数
- 便于插值：两个姿态之间可以平滑过渡
四元数更新姿态的公式（核心）：
q_new = q_old + 0.5 * q_old * ω * dt其中ω是陀螺仪测得的角速度，用四元数乘法。每次更新后，记得归一化：
q = q / norm(q)这一步不能省。我见过有人忘了归一化，结果姿态越飘越远，最后飞机自己都不知道自己在哪。
3.4.1 四元数与欧拉角的转换
从四元数转欧拉角：
roll = atan2(2*(w*x + y*z), 1 - 2*(x² + y²))
pitch = asin(2*(w*y - z*x))
yaw = atan2(2*(w*z + x*y), 1 - 2*(y² + z²))从欧拉角转四元数：
w = cos(φ/2)cos(θ/2)cos(ψ/2) + sin(φ/2)sin(θ/2)sin(ψ/2)
x = sin(φ/2)cos(θ/2)cos(ψ/2) - cos(φ/2)sin(θ/2)sin(ψ/2)
y = cos(φ/2)sin(θ/2)cos(ψ/2) + sin(φ/2)cos(θ/2)sin(ψ/2)
z = cos(φ/2)cos(θ/2)sin(ψ/2) - sin(φ/2)sin(θ/2)cos(ψ/2)工程建议：在飞控内部，全程用四元数做姿态解算和更新。只在需要输出给地面站或做控制律时，才转成欧拉角。这样既安全又高效。
3.5 三种表示方式的对比
| 特性 | 欧拉角 | 旋转矩阵 | 四元数 |
|---|---|---|---|
| 直观性 | ★★★★★ | ★★ | ★★★ |
| 计算量 | ★★★ | ★★ | ★★★★★ |
| 万向锁 | 有 | 无 | 无 |
| 插值平滑 | 差 | 一般 | 好 |
| 存储空间 | 3个浮点数 | 9个浮点数 | 4个浮点数 |
看到这个表，你应该明白为什么四元数是飞控的标配了。计算量小、无奇点、存储少，简直就是为嵌入式系统量身定做的。
3.6 实战经验：姿态解算的坑与避坑
最后分享几个我在项目中踩过的坑：
- 坐标系搞反：有一次我把NED和ENU搞混了，结果飞机起飞后直接往地里钻。后来我养成了习惯，每个模块开头先打印坐标系定义。
- 四元数忘归一化：这个坑我踩了不止一次。归一化看似简单，但代码写多了容易忘。建议在每次更新后都加一个断言检查模长。
- 欧拉角直接积分：有人直接用角速度积分欧拉角，这在俯仰角接近90°时会完全失效。记住，欧拉角只适合小角度姿态，大角度一定要用四元数。
💡 我的建议：初学者先用欧拉角理解姿态的概念，但写代码时直接上四元数。等你能把四元数玩转了，再回头理解旋转矩阵的几何意义，这样进步最快。
好了，这一讲的内容就到这里。姿态解算是飞控的基石，理解透了，后面的滤波、控制才能站得稳。下一讲，我们会把今天学的四元数，和IMU的数据结合起来，实现真正的姿态解算算法。