4. 姿态解算：四元数、欧拉角、旋转矩阵的关系与转换

各位同学，今天我们来聊聊飞控里最绕不开的一个话题——姿态解算。

说白了，就是怎么描述无人机现在「脑袋朝哪」。你想想看，飞控要控制飞机，首先得知道它当前是平飞、侧倾还是倒扣着。这个「知道」的过程，就是姿态解算。

我刚开始做飞控那会儿，被四元数、欧拉角、旋转矩阵这三个东西搞得晕头转向。后来在项目里摔过几次机，才真正搞明白它们之间的关系。今天我把这些经验梳理出来，希望能帮你少走弯路。

4.1 三种姿态表示方法

先简单介绍一下这三种方法，它们各有各的脾气。

4.1.1 欧拉角

欧拉角是最直观的。它用三个角度来描述姿态：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。你可以想象成飞机先绕X轴转一个角度，再绕Y轴转一个角度，最后绕Z轴转一个角度。

优点很明显：一看就懂。调试的时候，我习惯把欧拉角打印出来，一眼就能看出飞机是不是歪了。

但欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，横滚和偏航会耦合在一起，丢失一个自由度。我在做固定翼飞控时遇到过这个问题，飞机垂直爬升时姿态突然乱跳，查了半天才发现是万向锁在作怪。

4.1.2 旋转矩阵

旋转矩阵是一个3×3的矩阵，它能把一个向量从一个坐标系变换到另一个坐标系。比如，把机体坐标系下的加速度转换到地面坐标系。

旋转矩阵的好处是：没有奇点，计算稳定。但它有9个元素，计算量大，而且不够直观。你看着一个3×3的矩阵，很难想象飞机到底转成了什么样子。

在实际工程中，我很少直接用旋转矩阵做姿态解算，更多是用它来做坐标变换。

4.1.3 四元数

四元数是个「神奇」的东西。它用四个数来表示旋转：一个实部和三个虚部。形式是 q = w + xi + yj + zk。

四元数没有万向锁，计算量比旋转矩阵小，而且可以方便地进行插值。现在的飞控，绝大多数都用四元数来做姿态解算。

我第一次接触四元数时觉得它很抽象，后来在PX4源码里看到它的实现，才慢慢理解。说白了，四元数就是用「轴+角」的方式描述旋转，只不过用了一种数学技巧避免了三角函数。

4.2 三种表示方法的转换

在实际飞控中，我们经常需要在三者之间来回转换。比如，遥控器输入的是欧拉角，但姿态解算用的是四元数，最后输出控制时又要转成旋转矩阵。

4.2.1 欧拉角 → 四元数

这个转换公式我背过很多次，但每次写代码还是得查一下。这里给出一个常用的实现：

// 欧拉角转四元数
// roll, pitch, yaw 单位为弧度
void euler_to_quaternion(float roll, float pitch, float yaw, float *q) {
    float cr = cos(roll * 0.5f);
    float sr = sin(roll * 0.5f);
    float cp = cos(pitch * 0.5f);
    float sp = sin(pitch * 0.5f);
    float cy = cos(yaw * 0.5f);
    float sy = sin(yaw * 0.5f);

    q[0] = cr * cp * cy + sr * sp * sy;  // w
    q[1] = sr * cp * cy - cr * sp * sy;  // x
    q[2] = cr * sp * cy + sr * cp * sy;  // y
    q[3] = cr * cp * sy - sr * sp * cy;  // z
}

注意这里的顺序：先绕Z轴（偏航），再绕Y轴（俯仰），最后绕X轴（横滚）。这个顺序很重要，搞反了姿态就全乱了。

4.2.2 四元数 → 欧拉角

反过来转换也很常用，比如把解算出的四元数转成欧拉角发给地面站显示。

// 四元数转欧拉角
void quaternion_to_euler(float *q, float *roll, float *pitch, float *yaw) {
    float w = q[0], x = q[1], y = q[2], z = q[3];

    // 俯仰角
    float sinp = 2.0f * (w * y - x * z);
    if (sinp > 1.0f) sinp = 1.0f;
    if (sinp < -1.0f) sinp = -1.0f;
    *pitch = asin(sinp);

    // 横滚角
    *roll = atan2(2.0f * (w * x + y * z), 1.0f - 2.0f * (x * x + y * y));

    // 偏航角
    *yaw = atan2(2.0f * (w * z + x * y), 1.0f - 2.0f * (y * y + z * z));
}

这里有个坑：当俯仰角接近±90°时，asin函数的输入会超过1，需要做限幅处理。我曾经在这个地方吃过亏，解算出的角度直接跳变，导致飞机剧烈抖动。

4.2.3 四元数 → 旋转矩阵

这个转换在坐标变换时特别有用。比如，要把机体坐标系下的加速度转换到地面坐标系：

// 四元数转旋转矩阵
void quaternion_to_rotation_matrix(float *q, float R[3][3]) {
    float w = q[0], x = q[1], y = q[2], z = q[3];
    float w2 = w * w, x2 = x * x, y2 = y * y, z2 = z * z;

    R[0][0] = w2 + x2 - y2 - z2;
    R[0][1] = 2.0f * (x * y - w * z);
    R[0][2] = 2.0f * (x * z + w * y);

    R[1][0] = 2.0f * (x * y + w * z);
    R[1][1] = w2 - x2 + y2 - z2;
    R[1][2] = 2.0f * (y * z - w * x);

    R[2][0] = 2.0f * (x * z - w * y);
    R[2][1] = 2.0f * (y * z + w * x);
    R[2][2] = w2 - x2 - y2 + z2;
}

4.3 知识体系总览

为了让你更直观地理解这三者的关系，我画了一张图：

4.4 工程中的避坑指南

做姿态解算这几年，我踩过不少坑。这里分享几个最常见的：

• 四元数归一化：每次更新完四元数后，一定要做归一化。否则累积误差会让四元数越来越「胖」，最终导致姿态发散。我习惯在每次姿态更新后都调用一次归一化函数。
• 欧拉角顺序：不同飞控可能使用不同的旋转顺序。PX4用的是ZYX顺序，ArduPilot也是。但有些库可能用XYZ顺序。转换前一定要确认清楚。
• 角度单位：这是个老生常谈的问题。我见过有人把度当弧度传进去，结果飞机直接翻了个跟头。建议在代码里统一用弧度，只在显示时转成度。
• 万向锁处理：如果一定要用欧拉角做控制，建议在俯仰角接近±90°时切换到其他控制方式。我在做云台控制时就是这么处理的。

4.5 总结

姿态解算是飞控的基石。三种表示方法各有优劣，关键是要理解它们之间的关系，并在合适的场景用合适的方法。

我个人建议初学者先从欧拉角入手，理解姿态的物理含义。然后学习四元数，掌握它的运算规则。最后再搞明白旋转矩阵，用于坐标变换。

等你真正理解了这三者的关系，你会发现飞控的姿态解算其实没那么神秘。说白了，就是一组数学工具，帮我们把物理世界的旋转，变成计算机能处理的数据。

---