第1章 姿态解算基础：欧拉角与旋转矩阵
各位同学，欢迎来到《多旋翼无人机姿态解算算法实战》的第一课。
姿态解算，说白了就是回答一个问题：飞机现在朝哪边？ 你想想看，无人机在天上飞，它得知道自己是不是歪了、是不是在转。这个「知道自己姿势」的过程，就是姿态解算。
我个人习惯把姿态解算比作「给飞机装一个虚拟的陀螺仪和加速度计，然后让数学帮我们算出角度」。今天这一章，我们先搞定最基础的两个工具：欧拉角和旋转矩阵。
1.1 欧拉角：最直观的「歪了多少」
欧拉角，其实就是三个角度：俯仰角（Pitch）、横滚角（Roll）、偏航角（Yaw）。
我在项目中遇到过不少新手，一上来就问：「老师，这三个角到底怎么转？」其实很简单——你想象自己坐在飞机里：
- 俯仰角：你抬头看天，或者低头看地。飞机机头抬起来或压下去。
- 横滚角：你向左歪身子，或者向右歪身子。飞机左翼抬起或右翼抬起。
- 偏航角：你原地向左转，或者向右转。飞机机头指向变化。
嗯，这里要注意：欧拉角的旋转顺序很重要。我刚开始做飞控时，就因为这个顺序搞反了，飞机一解锁直接翻了个跟头……
常见的旋转顺序是：Z-Y-X（先偏航，再俯仰，最后横滚）。为什么是这个顺序？因为偏航改变的是机头指向，俯仰改变的是抬头低头，横滚改变的是左右倾斜——这个顺序最符合物理直觉。
核心公式：
欧拉角到旋转矩阵的转换（Z-Y-X顺序）：
R = Rz(yaw) * Ry(pitch) * Rx(roll)其中：
Rz(yaw) = [cos(yaw) -sin(yaw) 0;
sin(yaw) cos(yaw) 0;
0 0 1]
Ry(pitch) = [cos(pitch) 0 sin(pitch);
0 1 0;
-sin(pitch) 0 cos(pitch)]
Rx(roll) = [1 0 0;
0 cos(roll) -sin(roll);
0 sin(roll) cos(roll)]避坑指南：
我曾经在调试时发现，明明欧拉角算对了，但飞机姿态显示却不对。后来发现是旋转顺序写反了。记住：矩阵乘法不满足交换律，Rz * Ry * Rx 和 Rx * Ry * Rz 完全是两码事。
1.2 旋转矩阵：数学上的「怎么转」
欧拉角虽然直观，但它有个致命问题——万向锁。当俯仰角达到±90度时，横滚和偏航会变得无法区分。你想想看，飞机垂直朝上时，你还能分清「向左转」和「向右歪」吗？
所以，在实际飞控中，我们更多用旋转矩阵或四元数。旋转矩阵就是一个3x3的矩阵，它描述了从一个坐标系到另一个坐标系的旋转关系。
说白了，旋转矩阵就是「把机体坐标系下的向量，转换到地面坐标系下」的数学工具。
1.3 从几何直观到数学表达
我们来看一个具体例子。假设飞机当前姿态是：
- 偏航角 = 30°
- 俯仰角 = 10°
- 横滚角 = 5°
那么旋转矩阵怎么算？
// 角度转弧度
float yaw = 30 * M_PI / 180.0;
float pitch = 10 * M_PI / 180.0;
float roll = 5 * M_PI / 180.0;
// 计算各轴旋转矩阵
float Rz[3][3] = {
{cos(yaw), -sin(yaw), 0},
{sin(yaw), cos(yaw), 0},
{0, 0, 1}
};
float Ry[3][3] = {
{cos(pitch), 0, sin(pitch)},
{0, 1, 0},
{-sin(pitch), 0, cos(pitch)}
};
float Rx[3][3] = {
{1, 0, 0},
{0, cos(roll), -sin(roll)},
{0, sin(roll), cos(roll)}
};
// 最终旋转矩阵 R = Rz * Ry * Rx
// 注意：矩阵乘法顺序不能错！
float R[3][3];
matrix_multiply(Rz, Ry, temp); // temp = Rz * Ry
matrix_multiply(temp, Rx, R); // R = temp * Rx关键点：
旋转矩阵的每一列，其实就代表了机体坐标系的三个轴在地面坐标系下的投影。比如：
- R的第1列：机体X轴（机头方向）在地面坐标系下的坐标
- R的第2列：机体Y轴（右翼方向）在地面坐标系下的坐标
- R的第3列：机体Z轴（垂直向下）在地面坐标系下的坐标
1.4 实战中的选择：欧拉角 vs 旋转矩阵
| 特性 | 欧拉角 | 旋转矩阵 |
|---|---|---|
| 直观性 | 非常直观，人脑容易理解 | 不直观，但数学上完美 |
| 万向锁 | 存在，俯仰±90°时失效 | 不存在 |
| 计算量 | 小，只需3个角度 | 大，9个元素需要维护 |
| 插值 | 困难，角度变化不线性 | 困难，但可用四元数替代 |
| 飞控常用场景 | 用户界面显示、遥控器输入 | 姿态解算内部计算 |
重要提醒：
在实际飞控代码中，我建议：
- 内部计算用旋转矩阵或四元数，避免万向锁问题
- 对外输出用欧拉角，方便调试和显示
- 欧拉角转旋转矩阵时，务必确认旋转顺序，不同飞控可能用不同顺序
1.5 本章小结
这一章我们搞定了两件事：
- 欧拉角：三个角度，直观但有限制
- 旋转矩阵：3x3矩阵，数学上严谨
下一章，我们会引入四元数——它结合了欧拉角的直观和旋转矩阵的严谨，是飞控姿态解算的终极武器。到时候你会发现，四元数其实没那么神秘，它就是一个「带约束的复数」而已。
嗯，今天就到这里。记得动手写代码验证一下旋转矩阵的计算，纸上得来终觉浅嘛。