4. 姿态表示与转换

水下载体（AUV、ROV、潜航器）在导航解算中必须持续回答一个问题：机体坐标系相对参考坐标系（通常为当地水平坐标系 NED 或 ENU）转了多少、怎么转的。姿态表示方法决定了这个问题的表达效率与数值稳定性。本节围绕三种主流表示——欧拉角、旋转矩阵、四元数——讲清定义、相互转换、万向节锁的成因，以及四元数在插值平滑中的实际优势。

一、三种表示的定义与约定

先统一约定，全文保持一致：机体轴为 X 前、Y 右、Z 下（前右下，FRD）；参考系取北东地（NED）。欧拉角采用 Z-Y-X 内旋顺序（即先航向 yaw ψ，再俯仰 pitch θ，最后横滚 roll φ），这是水下导航中最常见的约定。三个角的含义是：

• 横滚 φ（roll）：绕机体 X 轴旋转，右倾为正，范围约 ±180°；
• 俯仰 θ（pitch）：绕机体 Y 轴旋转，抬头为正，范围 ±90°；
• 航向 ψ（yaw）：绕参考系 Z 轴旋转，北偏东为正，范围 ±180°。

旋转矩阵 R 是 3×3 正交矩阵，满足 RᵀR=I、det(R)=+1，把机体系向量映射到参考系。四元数 q=[w, x, y, z]（标量在前）是单位四元数，‖q‖=1，用四个参数无奇异表达任意姿态。三者描述的是同一个旋转，只是参数化方式不同。

二、相互转换方法

欧拉角 → 旋转矩阵：按 Z-Y-X 顺序连乘，R = Rz(ψ)·Ry(θ)·Rx(φ)。展开后可直接写入代码：

R11 = cosψcosθ
R12 = cosψsinθsinφ - sinψcosφ
R13 = cosψsinθcosφ + sinψsinφ
R21 = sinψcosθ
R22 = sinψsinθsinφ + cosψcosφ
R23 = sinψsinθcosφ - cosψsinφ
R31 = -sinθ
R32 = cosθsinφ
R33 = cosθcosφ

旋转矩阵 → 欧拉角：由上式反解，θ = asin(-R31)，φ = atan2(R32, R33)，ψ = atan2(R21, R11)。注意 θ 接近 ±90° 时 R11、R21 同时趋零，atan2 的输入退化，这正是万向节锁的数值表现。

欧拉角 → 四元数：令半角 cφ=cos(φ/2) 等，则

w = cφcθcψ + sφsθsψ
x = sφcθcψ - cφsθsψ
y = cφsθcψ + sφcθsψ
z = cφcθsψ - sφsθcψ

四元数 → 旋转矩阵：R 的元素可由 q 的二次型直接给出，例如 R11 = 1-2(y²+z²)，R12 = 2(xy-wz)，R13 = 2(xz+wy)，其余元素按循环对称写出。四元数 → 欧拉角则先转矩阵再反解，或直接用 q 分量代入 atan2 表达式，避免中间矩阵误差累积。

工程上还有一个高频操作：旋转向量（轴角）↔ 四元数。绕单位轴 n 转 α 角对应 q=[cos(α/2), n·sin(α/2)]，这在处理 IMU 角增量积分时几乎每帧都要用到。

三、万向节锁：不是"锁死"，而是自由度丢失

万向节锁常被误解为姿态无法转动，实际是参数化退化。当俯仰 θ=+90°（机头垂直向上）时，机体 X 轴与参考系 Z 轴重合，此时绕机体 X 的横滚与绕参考系 Z 的航向变成绕同一根轴的旋转，两个角只能确定其和或差，无法独立区分。数学上表现为 R31=-1，R11=R21=0，反解公式中 atan2(0,0) 无定义。

对水下载体而言，这个问题的现实意义在于：ROV 做大角度俯仰作业（如贴底观察、垂直上浮下潜）时，若导航滤波器的状态量直接取欧拉角，协方差会在 θ≈±90° 附近急剧膨胀，滤波发散或姿态跳变。因此工程惯例是滤波器内部用四元数或误差状态四元数，欧拉角只作为人机界面输出，并在输出时对 ψ 做 ±180° 环绕展开（unwrap），避免显示值在 ±180° 处来回跳。

四、四元数在插值与平滑中的优势

姿态插值的需求在轨迹规划、传感器时间对齐、动画回放中普遍存在。若直接对欧拉角线性插值，会遇到两个问题：一是 θ 跨 ±90° 时路径错误；二是 ψ 从 179° 到 -179° 线性插值会绕远路转 358°。四元数插值则天然规避：

• 归一化线性插值 NLERP：q(t)=normalize((1-t)q₀+tq₁)，计算量小，短弧段近似够用，但角速度不均匀；
• 球面线性插值 SLERP：q(t)=[sin((1-t)Ω)q₀+sin(tΩ)q₁]/sinΩ，Ω=acos(q₀·q₁)，角速度恒定，路径最短；
• 符号一致性：q 与 -q 表示同一旋转，插值前须检查 q₀·q₁<0 时取反，否则会走 360° 长弧。

平滑滤波同理。对多帧姿态做滑动平均时，欧拉角平均在跨 ±180° 或接近奇异时失效；四元数可先做符号对齐，再对分量加权平均后归一化，得到几何上合理的"平均姿态"。这也是姿态平滑、IMU 与视觉/声学观测融合时的标准做法。

五、水下载体姿态变化案例练习

设某 AUV 在定深巡航中做一次"抬头爬升—右转—回平"机动，采样到三个关键时刻的姿态（示例数据，用于练习转换）：

练习一（转换）：把 t1 的欧拉角转为四元数。取半角 φ/2=2.5°、θ/2=30°、ψ/2=22.5°，代入半角公式得 q≈[0.891, 0.034, 0.259, 0.371]（示例计算值，保留三位小数），归一化校验 ‖q‖≈1.000。再用四元数→矩阵公式算 R31=-sinθ=-0.866，与直接由欧拉角计算一致，说明转换链路正确。

练习二（奇异判断）：t2 的 θ=88°，R31=-0.9994，R11、R21 已很小。此时若用 atan2 反解 ψ，结果对输入噪声极其敏感——φ 或 θ 的 0.1° 测量误差可能让 ψ 跳变数度。判断依据：当 |R31|>0.999（即 |θ|>87.4°）时，应标记为"欧拉角输出不可靠区"，改用四元数或矩阵传递姿态。

练习三（插值对比）：在 t1 与 t2 之间做 10 步插值。若对欧拉角线性插值，θ 从 60° 到 88° 均匀变化，看似合理；但若把 t2 改为 θ=92°（越过奇异点），欧拉角插值会经过 90° 奇异区，ψ 出现非物理跳变。改用 SLERP，先检查 q₁·q₂ 符号，再按球面路径插值，中间姿态的角速度恒定且路径最短，横滚不会出现虚假摆动。这就是四元数在平滑中的直接价值。

小结：欧拉角胜在直观、便于人机交互；旋转矩阵胜在直接作用于向量、无奇异；四元数胜在紧凑、无奇异、插值平滑。三者转换关系必须记牢并在代码中验证一致性（如用 R 反解回欧拉角与原始值比对），而万向节锁的应对策略——内部四元数、外部欧拉角——应作为水下载体姿态处理的基本工程习惯。