坐标系与姿态表示：欧拉角、旋转矩阵、四元数、轴角之间的转换关系及面试常见陷阱

在飞控算法面试中，姿态表示与坐标系转换是最高频的基础考点之一。面试官通常不会直接问“四元数乘法怎么算”，而是通过一个具体的工程场景——比如“无人机在悬停时受到风扰，如何用姿态误差计算控制量”——来考察你对四种姿态表示（欧拉角、旋转矩阵、四元数、轴角）的理解深度。这四种表示在数学上等价，但在工程实现中各有优劣：欧拉角直观但有万向锁，旋转矩阵适合坐标变换但冗余，四元数紧凑且无奇异性，轴角适合插值与增量计算。本章将系统梳理它们之间的转换公式、代码实现要点，以及面试中常见的“陷阱题”。

姿态表示转换关系与工程陷阱 姿态状态 R(q) = Rz·Ry·Rx 欧拉角 (φ, θ, ψ) 直观·万向锁 旋转矩阵 R ∈ SO(3) 9元素·冗余 四元数 q = (w, x, y, z) 紧凑·无奇异 轴角 (n, θ) 适合插值 R = Rz(ψ)Ry(θ)Rx(φ) 提取欧拉角 q → R(q) 罗德里格斯 核心：所有表示均描述同一旋转，转换需注意奇异性与符号约定

核心原理：四种姿态表示的数学本质与适用场景

姿态描述的本质是建立“机体坐标系”与“参考坐标系（如NED）”之间的旋转关系。四种表示方式在数学上是同构的，但工程特性差异显著。

表示方式 参数数量 优点 缺点 典型应用
欧拉角 3 直观、物理意义明确 万向锁、三角函数计算慢 遥控器输入、姿态显示
旋转矩阵 9 坐标变换直接、无奇异 冗余、需正交化修正 坐标变换、IMU标定
四元数 4 紧凑、无奇异、插值平滑 不直观、需归一化 姿态解算、控制律
轴角 4 物理意义清晰、适合增量旋转 在θ=0时方向不确定 角速度积分、姿态误差

面试中常问的一个关键点是：为什么飞控代码中普遍使用四元数而不是欧拉角？答案有三层：第一，欧拉角在俯仰角接近±90°时出现万向锁，导致姿态解算发散；第二，四元数仅需4个参数且无奇异，计算效率高于9元素的旋转矩阵；第三，四元数乘法可以方便地组合旋转，适合姿态反馈控制中的误差计算。

转换公式与代码实现：从数学到C语言

四种表示之间的转换是面试手写代码的高频题。以下给出最核心的转换公式及C语言实现要点。

1. 欧拉角 → 旋转矩阵（ZYX顺序）

// 输入：roll(φ), pitch(θ), yaw(ψ) 单位弧度
// 输出：3x3旋转矩阵 R
double R[3][3];
double cφ = cos(roll), sφ = sin(roll);
double cθ = cos(pitch), sθ = sin(pitch);
double cψ = cos(yaw), sψ = sin(yaw);

R[0][0] = cθ*cψ;
R[0][1] = sφ*sθ*cψ - cφ*sψ;
R[0][2] = cφ*sθ*cψ + sφ*sψ;
R[1][0] = cθ*sψ;
R[1][1] = sφ*sθ*sψ + cφ*cψ;
R[1][2] = cφ*sθ*sψ - sφ*cψ;
R[2][0] = -sθ;
R[2][1] = sφ*cθ;
R[2][2] = cφ*cθ;

注意：这里的旋转顺序是Z-Y-X（先偏航、再俯仰、最后横滚），这是航空航天领域的标准约定。面试时务必确认旋转顺序，否则结果完全不同。

2. 旋转矩阵 → 欧拉角（从R提取）

// 从R提取欧拉角，注意pitch=±90°时的退化情况
pitch = asin(-R[2][0]);  // 注意符号
if (fabs(pitch - M_PI/2) < 1e-6) {
    // 万向锁：roll和yaw无法唯一分离
    roll = 0;
    yaw = atan2(R[0][1], R[1][1]);
} else {
    roll = atan2(R[2][1], R[2][2]);
    yaw = atan2(R[1][0], R[0][0]);
}

这个提取过程是面试陷阱高发区：当pitch接近±90°时，atan2的参数会退化，导致roll和yaw无法唯一确定。工程上通常用四元数避免这个问题。

3. 四元数 → 旋转矩阵

// 输入：四元数 q = (w, x, y, z)，已归一化
double R[3][3];
R[0][0] = 1 - 2*(y*y + z*z);
R[0][1] = 2*(x*y - w*z);
R[0][2] = 2*(x*z + w*y);
R[1][0] = 2*(x*y + w*z);
R[1][1] = 1 - 2*(x*x + z*z);
R[1][2] = 2*(y*z - w*x);
R[2][0] = 2*(x*z - w*y);
R[2][1] = 2*(y*z + w*x);
R[2][2] = 1 - 2*(x*x + y*y);

这里有一个关键细节：四元数必须归一化。如果直接使用未归一化的四元数，旋转矩阵将不再是正交矩阵，导致坐标变换后向量长度发生变化。飞控代码中通常每100ms做一次归一化。

4. 旋转矩阵 → 四元数

// 从R提取四元数，注意数值稳定性
double trace = R[0][0] + R[1][1] + R[2][2];
double w, x, y, z;
if (trace > 0) {
    double s = sqrt(trace + 1.0) * 2;
    w = 0.25 * s;
    x = (R[2][1] - R[1][2]) / s;
    y = (R[0][2] - R[2][0]) / s;
    z = (R[1][0] - R[0][1]) / s;
} else if (R[0][0] > R[1][1] && R[0][0] > R[2][2]) {
    double s = sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2;
    w = (R[2][1] - R[1][2]) / s;
    x = 0.25 * s;
    y = (R[0][1] + R[1][0]) / s;
    z = (R[0][2] + R[2][0]) / s;
} else if (R[1][1] > R[2][2]) {
    // 类似分支...
}

这个转换的陷阱在于：当trace接近-1时（旋转180°），数值误差会放大。工程上通常采用Shepperd方法，通过比较对角线元素选择最稳定的分支。

5. 轴角 ↔ 四元数（罗德里格斯公式）

// 轴角(n, θ) → 四元数
q.w = cos(θ/2);
q.x = n.x * sin(θ/2);
q.y = n.y * sin(θ/2);
q.z = n.z * sin(θ/2);

// 四元数 → 轴角
double sinHalf = sqrt(q.x*q.x + q.y*q.y + q.z*q.z);
if (sinHalf > 1e-8) {
    θ = 2 * atan2(sinHalf, q.w);
    n.x = q.x / sinHalf;
    n.y = q.y / sinHalf;
    n.z = q.z / sinHalf;
} else {
    // 零旋转，轴方向任意
    θ = 0;
    n = (1, 0, 0);
}
关键陷阱：当旋转角度接近0°时，轴方向变得不确定。在飞控中，如果直接用轴角做姿态误差，小角度时会出现数值抖动。正确做法是使用“旋转向量”（即轴角乘以角度），而不是单独使用轴和角。

案例拆解：姿态误差计算中的表示选择

我们来看一个典型的飞控面试题：“无人机悬停时，期望姿态为水平（roll=0, pitch=0, yaw=0），当前姿态由四元数q_cur表示。请计算姿态误差，并转换为控制量。”

初始现象与约束：飞控运行在200Hz控制环，姿态解算输出四元数。期望姿态q_des = (1,0,0,0)。需要计算误差并转换为角速度指令。

错误做法（常见陷阱）：直接用欧拉角相减——err_roll = roll_des - roll_cur。这在悬停小角度时近似成立，但一旦无人机倾斜超过30°，欧拉角之间的非线性耦合会导致控制发散。

正确做法：使用四元数误差。

// 四元数误差：q_err = q_des^(-1) ⊗ q_cur
// 由于q_des = (1,0,0,0)，其逆为(1,0,0,0)
// 所以 q_err = q_cur（但需要取共轭？不，这里q_des是单位四元数）
// 实际计算：
Quaternion q_err = quat_multiply(quat_conjugate(q_des), q_cur);
// 由于q_des是单位四元数，q_err = q_cur

// 将四元数误差转换为旋转向量（轴角）
double sinHalf = sqrt(q_err.x*q_err.x + q_err.y*q_err.y + q_err.z*q_err.z);
double angle = 2 * atan2(sinHalf, q_err.w);
Vector3f rot_vec;
if (sinHalf > 1e-8) {
    rot_vec = Vector3f(q_err.x, q_err.y, q_err.z) / sinHalf * angle;
} else {
    rot_vec = Vector3f(0, 0, 0);
}

// 控制律：角速度指令 = Kp * rot_vec
Vector3f rate_cmd = Kp * rot_vec;

结果对比：使用四元数误差方法，在任意姿态下都能正确计算误差方向；而欧拉角相减在大角度时会出现“绕远路”的问题。例如，期望yaw=170°，当前yaw=-170°，欧拉角差值为-340°，但实际最短旋转路径是+20°。四元数误差自动处理了这种环绕问题。

风险与代价：四元数误差计算需要一次四元数乘法和一次归一化，计算量略高于欧拉角减法。但在200Hz控制环上，这多出的几微秒完全可以接受。

常见误区与面试陷阱总结

面试高频陷阱清单：
  1. 旋转顺序未确认：欧拉角有12种旋转顺序约定（ZYX、XYZ、ZYZ等），面试时务必先确认约定。PX4使用ZYX（从NED到机体），而某些使用XYZ。
  2. 四元数归一化遗漏：连续四元数乘法会导致范数漂移，必须定期归一化。面试手写代码时，忘记归一化是扣分点。
  3. 万向锁处理不当：从旋转矩阵提取欧拉角时，pitch=±90°需要特殊分支。很多候选人直接写atan2(R[2][1], R[2][2])，没有处理退化情况。
  4. 轴角与旋转向量混淆：轴角是(n, θ)四参数，旋转向量是n*θ三参数。在姿态误差计算中，应该用旋转向量而不是轴角。
  5. 四元数左右乘混淆：q_des^(-1) ⊗ q_cur 与 q_cur ⊗ q_des^(-1) 结果不同。前者表示“从当前到期望的旋转”，后者表示“在机体坐标系下的误差”。
  6. 忽略小角度近似条件：当|θ|<0.1 rad时，sin(θ)≈θ，cos(θ)≈1。但超过这个范围，近似误差会显著影响控制精度。

总结复盘：在飞控面试中，姿态表示问题考察的是候选人对“数学表示”与“工程实现”之间差距的理解。建议准备时重点练习：①从四元数到旋转矩阵的代码；②从旋转矩阵提取欧拉角（含万向锁分支）；③四元数误差计算与控制量转换。同时，要能清晰解释为什么飞控内部用四元数、对外接口用欧拉角、调试时用轴角——这体现了对工程权衡的理解。

实战建议：在回答姿态转换问题时，先明确“旋转顺序”和“坐标系定义”（NED还是ENU），再写公式。面试官更看重你能否识别出题目中隐含的约定，而不是单纯背公式。
← 上一章 📖 返回目录 下一章 →