姿态表示方法：三种工具，一个核心

做飞控这么多年，我越来越觉得姿态表示是EKF的基石。选错了表示方法，后面全是坑。今天咱们就把欧拉角、旋转矩阵、四元数这三兄弟聊透。

一、欧拉角：直观但危险

欧拉角大家最熟悉。俯仰、横滚、偏航，三个角度搞定。我刚开始做飞控时，第一版代码用的就是欧拉角。为什么？直观啊！调试时直接看角度变化，多舒服。

但问题很快就来了。

为什么会这样？因为欧拉角的旋转顺序是固定的。比如按Z-Y-X顺序旋转，当Y轴旋转90°时，Z轴和X轴就重合了。你想想看，两个轴重合，你还能独立控制吗？

另外，欧拉角还有个大问题——三角函数计算。每次更新都要算sin、cos，计算量不小。而且角度变化不连续，比如从359°到0°，数值跳变，EKF处理起来很麻烦。

二、旋转矩阵：稳定但笨重

旋转矩阵用3×3的矩阵表示姿态。它没有万向锁问题，这是最大的优势。

但旋转矩阵有个硬约束——正交性。说白了，矩阵的列向量必须互相垂直，且模长为1。这个约束在数值计算中很难保持。

我遇到过这种情况：EKF跑着跑着，旋转矩阵慢慢变形了。矩阵不再正交，姿态就歪了。怎么办？需要定期做正交化修正。这个过程叫Gram-Schmidt正交化，代码写起来不复杂，但增加了计算量。

// 旋转矩阵正交化示例
void normalizeRotationMatrix(float R[3][3]) {
    // 对列向量进行Gram-Schmidt正交化
    // 第一列归一化
    float norm = sqrt(R[0][0]*R[0][0] + R[1][0]*R[1][0] + R[2][0]*R[2][0]);
    R[0][0] /= norm; R[1][0] /= norm; R[2][0] /= norm;
    
    // 第二列减去在第一列上的投影
    float dot = R[0][0]*R[0][1] + R[1][0]*R[1][1] + R[2][0]*R[2][1];
    R[0][1] -= dot * R[0][0];
    R[1][1] -= dot * R[1][0];
    R[2][1] -= dot * R[2][0];
    // 再归一化...
}

旋转矩阵还有一个问题：9个参数。你想想看，EKF的状态向量里塞9个参数，协方差矩阵就是9×9，计算量直接爆炸。对于嵌入式MCU来说，这太奢侈了。

三、四元数：飞控界的宠儿

四元数，说白了就是四个数表示旋转。一个标量加三个矢量，没有奇异点，计算高效。我个人习惯在EKF里用四元数，原因有三：

• 无奇异：不存在万向锁问题，任意姿态都能表示
• 计算高效：只有4个参数，乘法比矩阵快
• 易于插值：姿态平滑过渡很方便

四元数的形式是这样的：

q = [w, x, y, z]^T
其中 w^2 + x^2 + y^2 + z^2 = 1

注意这个约束——单位四元数。模长必须为1。这和旋转矩阵的正交性约束类似，但处理起来简单多了。只需要归一化就行：

void normalizeQuaternion(float q[4]) {
    float norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    q[0] /= norm; q[1] /= norm; q[2] /= norm; q[3] /= norm;
}

就这么简单。每次EKF更新后，做一次归一化，约束就保持了。

四、三种方法对比与选择

咱们直接上表格，一目了然：

特性	欧拉角	旋转矩阵	四元数
参数数量	3	9	4
奇异问题	有（万向锁）	无	无
计算效率	中等（三角函数）	低（9参数运算）	高（4参数运算）
约束条件	无	正交性（难保持）	单位模长（易保持）
直观性	高	低	中
EKF适用性	差（奇异+不连续）	一般（计算量大）	最佳

从表格能看出来，四元数在EKF里是综合最优的选择。我做了这么多年飞控，几乎所有项目都用四元数做核心姿态表示。欧拉角只用来做显示和日志记录，旋转矩阵偶尔用在一些特殊计算中。

五、我的选择策略

如果你问我具体怎么选，我的建议是这样的：

1. EKF核心状态：用四元数。4个参数，无奇异，计算快，约束好保持。
2. 用户交互层：用欧拉角。显示给用户看，或者做简单的角度控制。
3. 特殊计算：用旋转矩阵。比如需要多次旋转叠加时，矩阵乘法更直观。

你想想看，EKF里状态向量用四元数，观测模型里把四元数转成欧拉角，这样既保证了核心的稳定性，又方便了用户理解。两全其美。

好了，这一章就聊到这里。下一章咱们深入四元数的数学细节，看看它到底怎么旋转的。

---