姿态控制基础：四元数与欧拉角、旋转矩阵、姿态表示方法

姿态控制，说白了就是让飞行器知道自己「脑袋朝哪」。你想想看，如果连自己正着还是歪着都不知道，那还谈什么控制？

我刚开始做飞控时，觉得姿态表示就是个数学工具，随便选一种就行。直到有一次在SITL仿真里，一架四旋翼突然在天上翻起了跟头——后来查出来是欧拉角万向锁导致的。嗯，从那以后我再也不敢轻视这个基础了。

为什么需要多种姿态表示？

飞行器在三维空间中的旋转，其实只有三个自由度。但不同的表示方法，各有各的脾气。

表示方法	优点	缺点	我的使用场景
欧拉角	直观，人脑好理解	万向锁，插值不平滑	遥控器输入、地面站显示
旋转矩阵	无奇点，可叠加	9个参数，冗余大	坐标变换、刚体动力学
四元数	无奇点，计算快，可插值	不直观，需归一化	姿态解算、控制律核心
轴角	物理意义清晰	不可直接叠加	姿态误差计算

欧拉角：最直观，但最危险

欧拉角用三个角度描述旋转：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。我习惯用Z-Y-X顺序，也就是先转偏航，再转俯仰，最后转横滚。

但这里有个坑——万向锁。当俯仰角接近±90°时，横滚和偏航会变成同一个轴，丢失一个自由度。我在SITL里测试大机动动作时，就遇到过这个问题。飞行器突然失控，日志里显示欧拉角解算直接炸了。

旋转矩阵：数学上的瑞士军刀

旋转矩阵是一个3×3的正交矩阵，行列式为+1。它可以把一个向量从一个坐标系变换到另一个坐标系。

比如，从机体坐标系到世界坐标系的变换：

// 绕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φ |

// 完整旋转矩阵：R = Rz * Ry * Rx

我个人习惯在SITL仿真里用旋转矩阵做坐标变换，因为它每一步都很清晰。但注意，矩阵乘法有累积误差，长时间运行需要重新正交化。

四元数：飞控的幕后英雄

四元数是个超复数，形式是 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。它用四个参数表示旋转，没有奇点。

为什么飞控都用四元数？三个原因：

• 无万向锁——随便你怎么转，不会丢自由度
• 计算快——乘法比矩阵快，适合嵌入式
• 可插值——球面线性插值（SLERP）让姿态过渡平滑

但四元数有个硬性要求：必须归一化。模长必须为1，否则姿态会漂移。我在代码里每次更新四元数后都会加一行归一化，已经成了肌肉记忆。

// 四元数乘法（旋转叠加）
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

// 归一化
norm = sqrt(w*w + x*x + y*y + z*z)
w /= norm; x /= norm; y /= norm; z /= norm

三种表示方法的转换

实际开发中，这三种表示方法经常需要互相转换。我整理了一份速查表：

转换方向	公式/方法	注意事项
欧拉角 → 四元数	分别计算各轴四元数，再相乘	注意旋转顺序
四元数 → 欧拉角	用atan2和asin提取角度	俯仰角接近±90°时精度下降
四元数 → 旋转矩阵	用四元数元素构造矩阵	确保四元数已归一化
旋转矩阵 → 四元数	从矩阵迹中提取	注意数值稳定性

知识体系总览

下面这张图，是我自己总结的姿态表示方法知识结构。每次带新人时，我都会先让他们看这张图。

实战中的选择建议

在SITL仿真里调参时，我一般这样用：

• IMU数据融合——用四元数做姿态解算，配合Mahony或Madgwick滤波
• 控制律计算——用四元数算误差，转成轴角形式再算PID
• 日志记录——转成欧拉角存下来，方便人看
• 可视化——用旋转矩阵把机体坐标转成世界坐标，渲染3D模型