2. 水下航行器运动学基础

组合导航滤波器的输入是传感器测量值，输出是位置、速度和姿态。要把这两端正确连接起来，必须先建立一套自洽的运动学描述：用哪个坐标系表达矢量、姿态用什么参数表示、姿态如何随时间演化、速度与位置如何由加速度积分得到。本节围绕这四个问题展开，并给出可直接用于EKF状态建模的约定与公式。

一、坐标系定义与转换

水下航行器导航中最常用三个右手正交坐标系：

• 导航坐标系（n系，NED）：原点取航行器初始位置，x轴指北、y轴指东、z轴指地。重力矢量在n系中恒为 [0, 0, g]ᵀ，这是选择NED而非ENU的主要理由——深度增加对应z增大，与压力深度计读数方向一致。
• 载体坐标系（b系）：原点取航行器质心，x轴沿纵轴指艏、y轴指右舷、z轴沿垂轴指下。IMU的三个加速度计和三个陀螺仪就安装在该系上，其输出天然是b系分量。
• 地球坐标系（e系，ECEF）：用于长航程时将经纬度与局部n系衔接，短航程组合导航中通常只作为中间量出现。

两系之间的方向余弦矩阵（DCM）记为 C_bⁿ，满足 vⁿ = C_bⁿ v^b。按Z-Y-X顺序（先绕z转航向ψ，再绕y转纵摇θ，最后绕x转横滚φ）展开，得到：

C_b^n =
[ cθcψ            cθsψ           -sθ    ]
[ sφsθcψ - cφsψ   sφsθsψ + cφcψ    sφcθ ]
[ cφsθcψ + sφsψ   cφsθsψ - sφcψ    cφcθ ]

其中c、s分别代表cos与sin。该矩阵是正交阵，故 C_n^b = (C_bⁿ)ᵀ，反向转换只需转置，不必重新推导。工程上必须注意两点：一是φ、θ、ψ的符号约定要与IMU安装方向一致，否则滤波会出现系统性偏差；二是θ接近±90°时上述矩阵元素仍良定义，但欧拉角本身出现奇异，这正是后文引入四元数的原因。

二、姿态表示：欧拉角与四元数

欧拉角便于人机交互与结果展示，航向ψ、纵摇θ、横滚φ可直接由DCM元素反解：θ = −arcsin(C₃₁)，φ = atan2(C₃₂, C₃₃)，ψ = atan2(C₂₁, C₁₁)。但它在θ=±90°时φ与ψ耦合，且三角函数运算在滤波中代价较高，因此EKF内部通常不把欧拉角作为状态。

四元数用四个参数表示姿态，定义 q = [w, x, y, z]ᵀ，对应绕单位轴 n 旋转角 α 时 q = [cos(α/2), n·sin(α/2)]。其运动学方程为：

q̇ = 0.5 * q ⊗ ω_nb^b
其中 ω_nb^b = ω_ib^b - C_n^b * ω_in^n
离散更新（一阶）：q_{k+1} = q_k + 0.5*T*q_k⊗ω_k，随后归一化

实际实现中，采样周期T=10 ms、角速度约0.5 rad/s时，一阶近似误差可接受；若机动剧烈（如横滚速率超过2 rad/s），应改用二阶或旋转矢量补偿。每次更新后必须执行 q ← q/‖q‖，否则数值漂移会破坏正交性，导致C_bⁿ不再正交、重力投影出现虚假分量。

四元数与DCM的转换关系为：

C_b^n =
[1-2(y²+z²)   2(xy-wz)     2(xz+wy) ]
[2(xy+wz)     1-2(x²+z²)   2(yz-wx) ]
[2(xz-wy)     2(yz+wx)     1-2(x²+y²)]

三、运动学方程与导航中的使用方式

把上述内容串起来，就得到EKF预测步所需的连续时间方程。以NED系为例，速度微分方程为 v̇ⁿ = C_bⁿf^b + gⁿ − (2ω_ieⁿ + ω_enⁿ)×vⁿ。对于航程几十公里、航速几节的水下航行器，哥氏项与运输率项量级约为10^-5 m/s²，相对加速度计量测噪声可忽略，工程上常简化为 v̇ⁿ = C_bⁿf^b + gⁿ。但若做长航时高精度组合，应保留完整项并在文档中注明简化条件。

一个具体的建模示例（参数为教学假设）：设IMU输出比力 f^b = [0.02, 0.01, −0.05]ᵀ m/s²，当前姿态 φ=2°、θ=1°、ψ=30°，采样周期T=0.01 s。先用DCM把比力转到n系，加上重力 [0,0,9.81]ᵀ，得到n系加速度；再积分一次得速度增量，二次得位置增量。若DVL在同期测得前向速度3.0 kn（约1.543 m/s），则把该速度作为观测量 z，与预测速度比较得到残差，送入EKF更新。这里的关键是：DVL测的是b系或n系速度，必须明确其输出坐标系，否则残差中会混入姿态误差，滤波器会把速度偏差错误地归因于姿态。

姿态观测同样需要坐标系统一。磁航向计给出的是相对磁北的航向，使用前要加磁偏角修正到真北；深度计给出的是z轴正向深度，与NED的z向下一致，可直接作为p_z的观测。若使用USBL给出n系位置，则观测矩阵对位置状态直接取单位阵，对速度与姿态状态为零，结构清晰。

最后需要强调，运动学方程本身不含力与力矩，它只描述几何关系，因此EKF的预测精度取决于角速度和比力的测量精度以及积分方法，而不取决于航行器质量、水动力参数。这也解释了为什么组合导航中陀螺零偏和加速度计零偏必须作为状态在线估计——它们直接进入运动学方程的输入项，是预测误差的主要来源。