姿态表示基础：欧拉角、旋转矩阵、四元数的定义、转换关系及奇异性问题

在卡尔曼滤波姿态估计中，选择何种姿态表示方式直接决定了状态向量的维度、系统方程的线性化复杂度以及滤波器的数值稳定性。欧拉角、旋转矩阵与四元数是工程中最常用的三种数学工具，它们各有优劣，且相互之间存在明确的转换关系。本章将系统梳理三者的定义、数学性质、转换公式，并重点剖析欧拉角固有的奇异性（万向节锁）问题，为后续章节中卡尔曼滤波的状态建模与观测方程设计奠定基础。

2.1 欧拉角：直观但危险的表示

欧拉角通过三次绕坐标轴的旋转来描述刚体姿态，最常见的工程约定是 Z-Y-X 顺序（即先绕 Z 轴偏航 Yaw，再绕 Y 轴俯仰 Pitch，最后绕 X 轴横滚 Roll）。这种表示的最大优势在于物理意义明确——飞行员、机器人工程师和传感器开发者都能直接理解每个角度的含义。例如，在无人机姿态控制中，Yaw 角表示机头朝向，Pitch 角表示俯仰角度，Roll 角表示左右倾斜角度。

然而，欧拉角的数学结构存在一个致命缺陷：奇异性（Singularity），又称万向节锁（Gimbal Lock）。当中间旋转轴（Pitch）达到 ±90° 时，第一次和第三次旋转的轴会重合，导致系统丢失一个旋转自由度。此时，Yaw 和 Roll 的旋转效果变得不可区分，姿态微分方程中的分母项趋近于零，数值计算出现病态。

2.2 旋转矩阵：无奇异但冗余

旋转矩阵 R 是一个 3×3 的正交矩阵（R^TR = I，det(R) = +1），它完整描述了刚体在三维空间中的姿态。旋转矩阵的最大优势是不存在奇异性，且旋转组合可以通过矩阵乘法直接实现（R_total = R₂·R₁）。在卡尔曼滤波中，旋转矩阵常用于坐标变换和观测方程的构建。

但旋转矩阵的代价是参数冗余：9 个元素只有 3 个自由度，必须满足 6 个正交约束。在滤波过程中，由于数值误差累积，矩阵会逐渐偏离正交性，需要频繁进行正交化修正（如 Gram-Schmidt 过程或 SVD 分解）。这增加了计算负担，也使得协方差矩阵的更新变得复杂。

2.3 四元数：紧凑且鲁棒的选择

四元数由爱尔兰数学家哈密顿于 1843 年提出，形式为 q = q₀ + q₁i + q₂j + q₃k，其中 q₀ 为标量部分，[q₁, q₂, q₃] 为矢量部分。单位四元数（||q||=1）可以表示任意三维旋转，其物理意义为：绕单位轴 [q₁, q₂, q₃] 旋转角度 θ = 2·arccos(q₀)。

四元数在卡尔曼滤波中的优势极为突出：无奇异、参数紧凑（仅 4 维）、组合旋转只需一次四元数乘法。其唯一的约束是单位范数，在滤波更新后只需做一次归一化即可保持稳定性。这使得四元数成为惯性导航和姿态估计系统的事实标准。

// 给定 R = [r11 r12 r13; r21 r22 r23; r31 r32 r33]
// 计算四元数 q = [q0, q1, q2, q3]
trace = r11 + r22 + r33
if (trace > 0) {
  s = 0.5 / sqrt(trace + 1.0);
  q0 = 0.25 / s;
  q1 = (r32 - r23) * s;
  q2 = (r13 - r31) * s;
  q3 = (r21 - r12) * s;
} else if (r11 > r22 && r11 > r33) {
  s = 2.0 * sqrt(1.0 + r11 - r22 - r33);
  q1 = 0.25 * s;
  q0 = (r32 - r23) / s;
  q2 = (r12 + r21) / s;
  q3 = (r13 + r31) / s;
} else if (r22 > r33) {
  s = 2.0 * sqrt(1.0 + r22 - r11 - r33);
  q2 = 0.25 * s;
  q0 = (r13 - r31) / s;
  q1 = (r12 + r21) / s;
  q3 = (r23 + r32) / s;
} else {
  s = 2.0 * sqrt(1.0 + r33 - r11 - r22);
  q3 = 0.25 * s;
  q0 = (r21 - r12) / s;
  q1 = (r13 + r31) / s;
  q2 = (r23 + r32) / s;
}
// 最后归一化：q = q / ||q||

2.4 奇异性问题的本质与工程应对

奇异性并非欧拉角独有的数学缺陷，而是任何三维旋转表示在拓扑学上的必然结果。根据 hairy ball 定理，三维球面上不存在全局连续的向量场，因此任何用三个参数表示三维旋转的方案都必然存在至少一个奇异点。欧拉角将奇异点集中在 Pitch=±90° 处，而旋转矩阵和四元数通过增加冗余维度规避了这一问题。

在卡尔曼滤波的实际工程中，处理奇异性的策略通常有三种：

1. 限制姿态范围：在飞行控制中，通过机械限位或软件约束，使 Pitch 角始终远离 ±90°。适用于固定翼飞机等运动范围受限的场景。
2. 切换表示方式：当检测到 Pitch 接近奇异点时，临时将状态向量从欧拉角切换为四元数，滤波完成后在转换回欧拉角输出。需要处理切换瞬间的协方差映射。
3. 全程使用四元数：状态向量直接采用四元数，观测方程中再转换为欧拉角或旋转矩阵。这是现代工程的主流做法，避免了切换带来的复杂性。

dq/dt = 0.5 * q ⊗ [0, ωx, ωy, ωz]

2.5 案例拆解：无人机垂直爬升时的滤波器发散问题

背景与约束：某四旋翼无人机在自主巡检任务中需要执行垂直爬升动作，爬升角度达到 88°。飞控系统采用扩展卡尔曼滤波（EKF）进行姿态估计，状态向量为欧拉角 [Roll, Pitch, Yaw]，陀螺仪零偏已通过在线估计补偿。

初始现象：当无人机 Pitch 角超过 85° 时，滤波输出的 Roll 角出现剧烈振荡，Yaw 角漂移速率异常增大，约 2 秒后滤波器完全发散，无人机触发保护性降落。

关键数据分析：通过日志回放发现，在 Pitch=87° 时，状态转移矩阵中的 cos(Pitch) 项已接近 0.05，导致 Roll 和 Yaw 的耦合项急剧放大。协方差矩阵 P 的对应元素在 0.5 秒内增长了 3 个数量级，卡尔曼增益 K 出现病态。

分析判断过程：工程师首先排除了陀螺仪故障和振动干扰，确认问题源于欧拉角表示在接近奇异点时的数值不稳定性。进一步验证：将同一段传感器数据离线用四元数 EKF 重放，滤波器全程稳定，姿态误差保持在 0.5° 以内。

实际操作步骤：

1. 将状态向量从 3 维欧拉角扩展为 4 维四元数，同时增加陀螺仪零偏估计（共 7 维状态）。
2. 状态转移方程改为四元数微分方程，使用一阶龙格-库塔积分。
3. 观测方程中，将加速度计解算的 Roll/Pitch 和磁力计解算的 Yaw 转换为四元数观测值。
4. 滤波更新后执行四元数归一化，并限制四元数实部为正（消除双覆盖歧义）。

结果对比：修改后的滤波器在相同飞行轨迹下，Pitch 达到 89.5° 时仍保持稳定，姿态误差小于 1°，协方差矩阵条件数始终在 10³ 以内。后续 200 次重复飞行测试中，未再出现发散现象。

风险与代价：状态维度从 3 增加到 7，计算量增加约 60%，但 STM32F4 处理器仍有余量。四元数观测值的构造需要额外的三角函数计算，但整体实时性满足 500Hz 控制频率要求。

复盘经验：欧拉角表示在常规飞行（Pitch < 60°）时完全够用，但任何可能接近 ±90° 的场景都应提前切换为四元数。切换时机应设置安全裕度（如 ±75°），而非等到奇异点附近才处理。

2.6 常见误区与总结

在姿态表示的学习和工程应用中，以下误区值得特别注意：

• 误区一：认为欧拉角只有万向节锁一个问题。实际上，欧拉角的三角函数运算在滤波中还会引入非线性误差，且不同旋转顺序（ZYX、ZYZ、XYZ）对应的转换公式完全不同，混用会导致严重错误。
• 误区二：忽略四元数的双覆盖特性。q 和 -q 表示相同的旋转，但在滤波中如果符号跳变，会导致状态突变。解决方法是强制 q₀ ≥ 0，或在更新后检查相邻时刻四元数的点积符号。
• 误区三：旋转矩阵的正交化过于频繁。每次滤波迭代都做 SVD 分解会显著增加计算负担。实际工程中，每 100 步或当 ||R^TR - I|| > 10^-6 时才进行修正即可。
• 误区四：混淆旋转矩阵与坐标变换矩阵。旋转矩阵描述刚体自身的旋转，而坐标变换矩阵描述同一向量在不同坐标系下的表示。两者互为转置关系，方向搞反会导致姿态完全错误。