第四章 姿态解算基础：欧拉角与四元数、旋转矩阵
各位同学，欢迎来到姿态解算这一章。说实话，这部分内容是飞控算法的核心，也是很多新手觉得头疼的地方。我当年刚接触时也被绕得晕头转向，后来在项目里摔了几次机才真正搞明白。今天咱们就把这块硬骨头啃下来。
4.1 为什么要学姿态解算？
你想想看，无人机在天上飞，它怎么知道自己现在是正着还是歪着？怎么知道机头朝哪？靠的就是姿态解算。说白了，就是把传感器（陀螺仪、加速度计、磁力计）的原始数据，转换成我们能理解的欧拉角或者四元数。
我在做第一个飞控项目时，就犯了个低级错误——直接用陀螺仪积分算角度。结果飞机起飞后疯狂漂移，差点砸到人。嗯，从那以后我才明白，姿态解算不是简单的积分，而是一套严谨的数学体系。
4.2 欧拉角：最直观的表示方法
欧拉角用三个角度描述物体姿态：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。你可以想象成飞机绕三个轴依次旋转。
核心要点：欧拉角虽然直观，但有个致命问题——万向锁。当俯仰角接近±90°时，横滚和偏航会耦合，丢失一个自由度。我在做特技飞行时遇到过这个问题，飞机翻过来后姿态直接乱套了。
欧拉角的旋转顺序很重要。常用的顺序是Z-Y-X（偏航→俯仰→横滚），对应的旋转矩阵为：
R = Rz(yaw) * Ry(pitch) * Rx(roll)其中每个旋转矩阵都是3x3的。我个人习惯用Z-Y-X顺序，因为符合直觉——先转方向，再抬头，最后侧倾。
4.3 四元数：优雅的数学工具
四元数是个四维复数，形式为 q = w + xi + yj + zk。它没有万向锁问题，计算效率也高。说白了，就是用四个数表示三维旋转。
我记得第一次看到四元数时，心想这什么鬼？后来发现它其实很简单：
- w：标量部分，表示旋转角度的一半的余弦
- x, y, z：向量部分，表示旋转轴的方向
四元数的乘法规则：
q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2) +
(w1*x2 + x1*w2 + y1*z2 - z1*y2)i +
(w1*y2 - x1*z2 + y1*w2 + z1*x2)j +
(w1*z2 + x1*y2 - y1*x2 + z1*w2)k实用技巧：四元数归一化很重要！我见过有人忘记归一化，结果姿态越算越偏。每次更新后记得做 q = q / ||q||。
4.4 旋转矩阵：连接欧拉角和四元数的桥梁
旋转矩阵是3x3的正交矩阵，满足 R^T * R = I。它可以把一个向量从一个坐标系变换到另一个坐标系。
从四元数到旋转矩阵的转换：
R = [[1-2(y²+z²), 2(xy-wz), 2(xz+wy)],
[2(xy+wz), 1-2(x²+z²), 2(yz-wx)],
[2(xz-wy), 2(yz+wx), 1-2(x²+y²)]]从欧拉角到旋转矩阵：
R = Rz(yaw) * Ry(pitch) * Rx(roll)这里要注意，旋转矩阵的乘法顺序不能乱。我曾经在代码里把顺序写反了，结果飞机横滚和俯仰反着来，调试了整整两天才找到问题。
4.5 姿态解算的数学原理
4.5.1 方向余弦法
方向余弦法是最直接的方法。它用方向余弦矩阵（DCM）表示姿态，通过陀螺仪数据更新矩阵元素。
更新公式：
R_dot = R * [ω×]其中 [ω×] 是角速度的反对称矩阵：
[ω×] = [[0, -ωz, ωy],
[ωz, 0, -ωx],
[-ωy, ωx, 0]]方向余弦法的优点是精度高，但计算量大（9个元素）。我早期在STM32F1上跑DCM，CPU占用率直接飙到80%，根本跑不动别的任务。
避坑指南：DCM需要定期正交化，否则矩阵会漂移。我曾经偷懒没做正交化，结果姿态误差越来越大，飞机在天上画起了8字。
4.5.2 四元数法
四元数法是目前最主流的方法。它用四元数表示姿态，通过陀螺仪数据更新四元数。
更新公式：
q_dot = 0.5 * q * [0, ωx, ωy, ωz]离散化后：
q_new = q_old + q_dot * dt四元数法的优势：
- 只有4个参数，计算量小
- 没有万向锁问题
- 容易与加速度计、磁力计融合
我个人强烈推荐四元数法。现在几乎所有飞控都用它，包括PX4和ArduPilot。你想想看，如果它不好用，这些开源项目怎么会选它？
4.6 姿态解算的完整流程
下面我用SVG画一张流程图，展示姿态解算的核心逻辑：
4.7 实际项目中的选择建议
根据我的经验，不同场景适合不同方法：
| 场景 | 推荐方法 | 原因 |
|---|---|---|
| 普通航拍无人机 | 四元数法 + 互补滤波 | 计算量小，稳定性好 |
| 竞速穿越机 | 四元数法 + 卡尔曼滤波 | 高动态响应，精度要求高 |
| 固定翼无人机 | 方向余弦法 | 大角度机动，需要高精度 |
| 微型无人机 | 简化四元数法 | 算力有限，需要轻量化 |
我的建议：新手先从四元数法+互补滤波开始。这个组合简单可靠，足够应付大多数场景。等跑通了再考虑更复杂的卡尔曼滤波。
4.8 常见问题与避坑
最后分享几个我踩过的坑：
- 坐标系搞混：NED和ENU坐标系搞反，姿态全错。我建议统一用NED（北东地）坐标系。
- 单位问题：陀螺仪输出是rad/s还是deg/s？我见过有人混用，结果积分出来角度差了几十倍。
- 时间戳对齐：传感器数据时间戳不对齐，融合结果会抖动。我后来加了时间戳同步模块才解决。
嗯，这一章的内容就到这里。姿态解算是飞控的基石，搞懂了它，后面的控制算法才能站得住脚。大家回去后可以拿开源飞控的代码看看，对照着今天讲的内容，理解会更深刻。
公众号:蓝海资料掘金营，微信deep3321