4. 水下载体姿态角定义与坐标变换链构建

水下导航解算的第一步，是把姿态传感器测得的“载体相对什么姿态”讲清楚。AUV 上常见的姿态传感器组合是光纤陀螺惯导（输出姿态角）或 AHRS（输出横滚、俯仰、航向），它们给出的角度都定义在载体坐标系与当地水平坐标系之间。本节以一台典型 AUV 的 AHRS 输出为例，建立从载体坐标系到当地水平系的姿态变换链，并明确三个角的物理含义与符号约定。

一、两个坐标系与三个姿态角

先约定坐标系。载体坐标系 b 系固连于 AUV：x_b 指向艏部，y_b 指向右舷，z_b 指向底部，构成右手系。当地水平坐标系 n 系取“北—东—地”（NED）：x_n 指北，y_n 指东，z_n 指向地心。姿态角就是描述 b 系相对 n 系转动的三个欧拉角，按 Z-Y-X 顺序（先航向、再俯仰、最后横滚）定义：

• 航向角 ψ（yaw）：b 系 x_b 轴在水平面内的投影与北向的夹角，绕 z_n 轴转动。约定北偏东为正，范围 0°～360°，正北为 0°，正东为 90°。
• 俯仰角 θ（pitch）：x_b 轴与水平面的夹角，绕转动后的 y 轴。约定艏部抬起为正（抬头），范围 −90°～+90°。
• 横滚角 φ（roll）：y_b 轴与水平面的夹角，绕转动后的 x 轴。约定右舷下沉为正（右倾），范围 −180°～+180°。

这三个约定必须与传感器内部定义一致，否则后续变换会整体反号。工程上最常见的错误，是把航向按“北偏西为正”或把横滚按“左倾为正”理解，导致 AUV 实际转向与解算航迹镜像对称。

二、姿态变换链的构建

从 n 系到 b 系的旋转，可分解为三次基本旋转的连乘。按 Z-Y-X 顺序，先把 n 系绕 z_n 转 ψ 得到中间系，再绕新 y 轴转 θ，最后绕新 x 轴转 φ，于是方向余弦矩阵为：

C_n^b = Rx(φ) · Ry(θ) · Rz(ψ)

其中：
Rz(ψ) = [ cosψ  sinψ  0 ]
        [-sinψ  cosψ  0 ]
        [  0     0    1 ]

Ry(θ) = [ cosθ  0  -sinθ ]
        [  0    1    0   ]
        [ sinθ  0   cosθ ]

Rx(φ) = [ 1   0      0    ]
        [ 0  cosφ   sinφ  ]
        [ 0 -sinφ   cosφ  ]

若要把载体坐标系下的量（如 DVL 测得的对地速度 v^b）转换到当地水平系，则用转置矩阵：vⁿ = C_bⁿ · v^b，其中 C_bⁿ = (C_n^b)^T。这条“传感器输出角 → 构造 C → 转置 → 乘载体矢量”的链路，就是姿态变换链的核心。

三、用一组示例走通变换链

设某次下潜中 AHRS 输出 φ = 5°、θ = 10°、ψ = 30°，DVL 在载体系测得对地速度 v^b = [2.0, 0.5, 0.1]^T m/s（前向 2.0、右向 0.5、向下 0.1）。按上述链路，先算 C_n^b 再转置，得到 vⁿ ≈ [1.44, 1.55, 0.42]^T m/s。判读时注意两点：其一，v_N 与 v_E 均为正，说明载体朝北偏东运动，与 ψ = 30° 的“北偏东”约定吻合；其二，v_D 为正表示存在下沉分量，这与 θ = 10° 抬头时载体实际应上浮的直觉相反——原因是载体前向速度在抬头时会产生向上的 n 系分量，而 v^b 中 w = 0.1 本身向下，两者叠加后仍略向下。这类“直觉与结果不一致”的情况，正是需要用矩阵而非单角近似来判断的地方。

四、工程实现中的关键检查点

实际写代码时，建议把变换链封装成一个函数，输入三个角与载体矢量，输出 n 系矢量，并在单元测试中固定几组边界值：ψ = 0°/90°/180°、θ = ±90°（万向节锁附近）、φ = ±180°。特别要检查 θ 接近 ±90° 时，横滚与航向的耦合会退化，此时矩阵仍可计算，但欧拉角本身不再唯一，解算器应避免在该区域做角度差分。另一个检查点是角度单位：AHRS 常以度输出，而三角函数库多用弧度，转换遗漏会造成量级错误。最后，若传感器安装存在安装偏差（如 AHRS 的 x 轴与 x_b 有 2° 夹角），应在变换链最前端插入一个固定的安装矩阵 C_b'^b，而不是事后修正角度。