姿态表示方法：欧拉角、旋转矩阵、四元数与等效旋转矢量

各位同学，欢迎来到《姿态稳定控制全系统搭建教程》的第一章。今天咱们聊聊飞控里最基础、也最容易踩坑的话题——姿态表示。

你想想看，一个四轴在天上翻飞，我们怎么知道它当前是正着还是歪着？怎么告诉它「给我转30度」？这背后就是姿态表示方法在起作用。我做了这么多年飞控，见过太多新手在这上面栽跟头。嗯，咱们今天就把这四种方法彻底讲透。

1. 欧拉角——最直观，但暗藏杀机

欧拉角是什么？说白了就是用三个角度来描述一个物体的朝向。滚转(Roll)、俯仰(Pitch)、偏航(Yaw)，每个角度对应绕一个轴的旋转。

我个人习惯把欧拉角比作「人的点头、歪头、转头」。你想想看，你点个头就是俯仰，歪个头就是滚转，转个头就是偏航。是不是很直观？

但问题来了——万向锁(Gimbal Lock)。我在项目中遇到过好几次，新手用欧拉角做姿态控制，飞机飞到某个角度突然就「抽风」了。为什么会这样？

举个例子：假设飞机垂直向上飞（俯仰90°），此时你给它一个滚转指令，它实际表现出来的是偏航运动。这就乱套了。我记得有一次调试固定翼，就是因为没处理好万向锁，飞机在做筋斗动作时直接失控了。

欧拉角的另一个问题是角度跳变。偏航角从359°变到0°时，数值上跳了359度，但实际飞机只转了1度。这种不连续性在控制算法里非常讨厌。

2. 旋转矩阵——数学上完美，但计算量太大

旋转矩阵是一个3×3的正交矩阵，它把向量从一个坐标系变换到另一个坐标系。数学上它很完美——没有奇点，没有跳变，可以表示任意姿态。

但问题在于：9个元素，6个约束条件。每次更新姿态后，你都得重新正交化，否则矩阵会慢慢「漂移」，不再是一个合法的旋转矩阵。

// 旋转矩阵的正交化（我常用的方法）
// 输入：R - 3x3 矩阵
// 输出：正交化后的旋转矩阵
void orthonormalize(float R[3][3]) {
    float x[3], y[3], z[3];
    
    // 取三个列向量
    for(int i=0; i<3; i++) {
        x[i] = R[i][0];
        y[i] = R[i][1];
        z[i] = R[i][2];
    }
    
    // Gram-Schmidt 正交化
    // 先归一化 x
    float norm_x = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);
    for(int i=0; i<3; i++) x[i] /= norm_x;
    
    // 从 y 中减去 x 方向的分量
    float dot_xy = x[0]*y[0] + x[1]*y[1] + x[2]*y[2];
    for(int i=0; i<3; i++) y[i] -= dot_xy * x[i];
    float norm_y = sqrt(y[0]*y[0] + y[1]*y[1] + y[2]*y[2]);
    for(int i=0; i<3; i++) y[i] /= norm_y;
    
    // z = x × y
    z[0] = x[1]*y[2] - x[2]*y[1];
    z[1] = x[2]*y[0] - x[0]*y[2];
    z[2] = x[0]*y[1] - x[1]*y[0];
    
    // 写回矩阵
    for(int i=0; i<3; i++) {
        R[i][0] = x[i];
        R[i][1] = y[i];
        R[i][2] = z[i];
    }
}

你看，光是正交化就要做这么多计算。在嵌入式平台上，每次姿态更新都跑一遍这个，CPU开销不小。我早期做STM32F4的飞控时，就吃过这个亏——旋转矩阵更新占用了太多CPU时间，导致控制周期不稳定。

3. 四元数——飞控界的「万金油」

四元数，说白了就是一个标量加三个虚部：q = q0 + q1·i + q2·j + q3·k。它用4个参数表示旋转，没有奇点，计算效率高。

我个人最喜欢四元数的地方是：插值平滑。你想让飞机从姿态A平滑转到姿态B，用四元数的球面线性插值(SLERP)就能得到一条非常平滑的路径。这在云台控制和视觉跟踪里特别有用。

四元数和旋转矩阵的转换也很简单：

// 四元数转旋转矩阵
void quat_to_rotmat(float q[4], float R[3][3]) {
    float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
    
    R[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
    R[0][1] = 2*(q1*q2 - q0*q3);
    R[0][2] = 2*(q1*q3 + q0*q2);
    
    R[1][0] = 2*(q1*q2 + q0*q3);
    R[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
    R[1][2] = 2*(q2*q3 - q0*q1);
    
    R[2][0] = 2*(q1*q3 - q0*q2);
    R[2][1] = 2*(q2*q3 + q0*q1);
    R[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
}

// 旋转矩阵转四元数
void rotmat_to_quat(float R[3][3], float q[4]) {
    float tr = R[0][0] + R[1][1] + R[2][2];
    
    if(tr > 0) {
        float S = sqrt(tr + 1.0) * 2;
        q[0] = 0.25 * S;
        q[1] = (R[2][1] - R[1][2]) / S;
        q[2] = (R[0][2] - R[2][0]) / S;
        q[3] = (R[1][0] - R[0][1]) / S;
    } else if(R[0][0] > R[1][1] && R[0][0] > R[2][2]) {
        // ... 其他分支处理
    }
}

四元数还有一个好处——乘法就是旋转的复合。你想把两次旋转合并，直接做四元数乘法就行，比旋转矩阵的乘法快得多。

4. 等效旋转矢量——惯性导航的「秘密武器」

等效旋转矢量，也叫轴角表示法。它用一个向量来表示旋转：向量的方向是旋转轴，向量的长度是旋转角度。说白了就是「绕某个轴转多少度」。

这个表示法在惯性导航(INS)里特别重要。为什么？因为陀螺仪直接测量的是角速度，而角速度积分得到的就是等效旋转矢量。

我记得在做高精度惯导时，用四元数直接积分角速度会有不可交换性误差——因为旋转不满足交换律，先绕X轴转再绕Y轴转，和反过来转，结果不一样。而等效旋转矢量通过圆锥补偿算法可以很好地处理这个问题。

四元数和等效旋转矢量的转换也很直接：

// 等效旋转矢量转四元数
void rotvec_to_quat(float phi[3], float q[4]) {
    float theta = sqrt(phi[0]*phi[0] + phi[1]*phi[1] + phi[2]*phi[2]);
    
    if(theta < 1e-10) {
        q[0] = 1.0;
        q[1] = q[2] = q[3] = 0.0;
        return;
    }
    
    float half_theta = theta * 0.5;
    float sin_half = sin(half_theta);
    
    q[0] = cos(half_theta);
    q[1] = sin_half * phi[0] / theta;
    q[2] = sin_half * phi[1] / theta;
    q[3] = sin_half * phi[2] / theta;
}

5. 四种方法的对比与选择

好了，四种方法都讲完了。咱们来做个对比：

特性	欧拉角	旋转矩阵	四元数	等效旋转矢量
参数数量	3	9	4	3
奇异性	有（万向锁）	无	无	无
计算效率	高	低	高	中
插值平滑性	差	中	好	好
物理直观性	好	差	中	中
适合场景	人机交互、小角度	多次向量变换	姿态控制、插值	惯导积分

我个人在实际项目中的选择策略是这样的：

• 姿态控制核心循环：用四元数。没有奇点，计算快，插值平滑。
• 传感器数据融合：用等效旋转矢量。特别是陀螺仪积分，用圆锥补偿算法精度更高。
• 地面站显示：转成欧拉角。操作员看着直观，但内部算法千万别用欧拉角。
• 需要多次坐标变换：用旋转矩阵。比如把多个传感器的数据统一到同一个坐标系。

好了，这一章的内容就到这儿。四种姿态表示方法，各有各的脾气。你只要记住：欧拉角看、四元数算、旋转矩阵转、等效矢量积，基本就不会用错。

---

公众号:蓝海资料掘金营，微信deep3321