4、姿态表示（欧拉角与旋转矩阵）

姿态表示，说白了就是回答一个问题：飞行器现在朝哪边？ 这个问题看似简单，但在飞控算法里，选错表示方式，轻则控制抖动，重则直接炸机。我见过不少新手，上来就用欧拉角做姿态插值，结果在90度俯仰时直接翻车——嗯，这就是万向锁在作怪。

今天咱们就把欧拉角和旋转矩阵彻底聊透。我会从定义讲起，再到那个让人头疼的万向锁，最后给出代码实现。你想想看，面试官问这个问题，其实就想看你有没有踩过坑。

4.1 欧拉角定义

欧拉角用三个角度描述旋转：滚转（Roll）、俯仰（Pitch）、偏航（Yaw）。你可以想象成飞机先转偏航，再转俯仰，最后转滚转。这个顺序很重要，我习惯用Z-Y-X顺序（即偏航→俯仰→滚转）。

常用欧拉角定义（Z-Y-X顺序）：

  • 偏航角 ψ (Yaw)：绕机体Z轴旋转，范围[-π, π]
  • 俯仰角 θ (Pitch)：绕机体Y轴旋转，范围[-π/2, π/2]
  • 滚转角 φ (Roll)：绕机体X轴旋转，范围[-π, π]

为什么俯仰角范围只有±90度？因为超过这个范围，飞机就倒过来了，这时候偏航和滚转的定义会变得模糊。我在项目中遇到过，有人把俯仰角设成±180度，结果姿态解算直接发散——这就是没理解欧拉角的物理含义。

4.2 万向锁问题

万向锁（Gimbal Lock）是欧拉角最致命的缺陷。当俯仰角接近±90度时，偏航和滚转的旋转轴会重合，导致丢失一个自由度。

为什么会这样？你想想看：

  • 当俯仰角=90度时，机头朝上
  • 此时偏航轴（Z轴）和滚转轴（X轴）都指向垂直方向
  • 你无法区分是偏航在转还是滚转在转

避坑指南： 我曾经在无人机特技飞行项目中，用欧拉角做姿态控制。当飞机做筋斗动作（俯仰超过90度）时，控制器突然剧烈抖动。查了两天才发现是万向锁导致姿态解算奇异。从那以后，我所有涉及大角度机动的地方，一律改用四元数或旋转矩阵。

万向锁的本质是：欧拉角在特定角度下，旋转矩阵的雅可比矩阵奇异。这意味着你无法从角速度唯一地解算出欧拉角速率。说白了，就是数学上无解了。

4.3 旋转矩阵推导

旋转矩阵是3x3的正交矩阵，满足RTR = I，det(R)=1。它描述了一个坐标系到另一个坐标系的旋转关系。

从欧拉角到旋转矩阵，其实就是三个基本旋转矩阵的乘积。以Z-Y-X顺序为例：

// 基本旋转矩阵
// 绕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(φ)
R = [cosθ*cosψ,  sinφ*sinθ*cosψ - cosφ*sinψ,  cosφ*sinθ*cosψ + sinφ*sinψ;
     cosθ*sinψ,  sinφ*sinθ*sinψ + cosφ*cosψ,  cosφ*sinθ*sinψ - sinφ*cosψ;
    -sinθ,       sinφ*cosθ,                   cosφ*cosθ]

个人习惯： 我写代码时，从来不会手算这个矩阵。我会用库函数，比如Eigen或PX4的math库。但面试时，面试官大概率会让你手推一遍。记住一个技巧：先写三个基本矩阵，再按顺序乘起来。注意乘法顺序，左乘还是右乘，取决于你用的是固定坐标系还是随体坐标系。

4.4 从旋转矩阵到欧拉角

反过来，给定一个旋转矩阵R，如何提取欧拉角？这其实是个反三角函数求解问题。

从上面的R矩阵可以看出：

  • R[2][0] = -sinθ → θ = asin(-R[2][0])
  • R[2][1] / R[2][2] = tanφ → φ = atan2(R[2][1], R[2][2])
  • R[1][0] / R[0][0] = tanψ → ψ = atan2(R[1][0], R[0][0])

但这里有个坑：当cosθ接近0时（即θ接近±90度），R[2][1]和R[2][2]都接近0，atan2会变得不稳定。这就是万向锁在数值上的体现。

// C++代码：从旋转矩阵提取欧拉角（Z-Y-X顺序）
// 注意处理万向锁情况
void rotationMatrixToEuler(const Matrix3x3& R, double& roll, double& pitch, double& yaw) {
    // 检查是否接近万向锁
    double pitch_val = -asin(R[2][0]);
    
    if (fabs(cos(pitch_val)) > 1e-6) {
        // 正常情况
        roll = atan2(R[2][1], R[2][2]);
        yaw = atan2(R[1][0], R[0][0]);
    } else {
        // 万向锁！俯仰角接近±90度
        // 此时只能假设滚转角为0，或者用其他方法
        roll = 0.0;  // 或者用 atan2(-R[1][2], R[1][1])
        yaw = atan2(-R[0][1], R[1][1]);
    }
    
    pitch = pitch_val;
}

注意： 万向锁情况下，提取的欧拉角不是唯一的。你可以选择不同的滚转角，得到不同的偏航角。这就是为什么飞控里很少直接用欧拉角做姿态控制——你无法保证姿态的唯一性。

4.5 知识体系图

下面这张图展示了姿态表示的核心逻辑：

姿态表示核心逻辑 欧拉角 Roll, Pitch, Yaw 旋转矩阵 3x3正交矩阵 四元数 q0+q1i+q2j+q3k Rz*Ry*Rx atan2 迹法 Rodrigues ⚠ 万向锁问题 俯仰角=±90°时，偏航与滚转轴重合 → 旋转矩阵奇异 → 欧拉角不唯一 ✅ 工程推荐：四元数 + 旋转矩阵 欧拉角仅用于人机交互和可视化

4.6 面试常见追问

面试官可能会问这几个问题，我帮你梳理一下：

问题 回答要点
为什么飞控不用欧拉角做姿态控制？ 万向锁导致奇异，且角速度到欧拉角速率的变换矩阵在俯仰±90°时不可逆。说白了就是控制会失效。
旋转矩阵为什么是正交的？ 因为旋转不改变向量长度和夹角，所以RTR=I。物理意义是：旋转前后，坐标系的基向量保持正交且单位长度。
如何判断一个矩阵是不是旋转矩阵？ 检查两点：RTR是否接近单位矩阵，det(R)是否接近+1。如果det(R)=-1，那是反射矩阵，不是旋转。
万向锁能完全避免吗？ 用欧拉角就无法避免。但用四元数或旋转矩阵做姿态表示，可以完全绕过这个问题。我建议所有内部计算都用四元数，只在最后输出给地面站时转成欧拉角。

我的经验： 面试时如果被问到万向锁，不要只背定义。你可以说：「我在做某型无人机时，用欧拉角做姿态控制，结果在俯仰80度时就开始抖动。后来换成四元数，问题就解决了。」这样既展示了技术深度，又体现了实战经验。

好了，姿态表示这块就聊到这。记住一句话：欧拉角用于人看，旋转矩阵用于机算，四元数用于插值。下次面试官问你，你就知道该怎么答了。

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

← 上一章 📖 返回目录 下一章 →