2. AUV运动学与动力学建模基础

在讨论任何滤波与融合算法之前，必须先回答一个更基础的问题：我们用什么样的数学对象来描述AUV的运动？如果位置、速度、姿态的定义不统一，或者坐标系之间的转换关系含糊不清，后续的卡尔曼滤波、惯导解算与多传感器融合都会建立在错误的前提上。本节围绕三个核心问题展开：AUV的六自由度运动如何分解，惯性系与载体系如何定义与转换，以及位置、速度、姿态这些运动参数如何表示并相互换算。

六自由度：把空间运动拆成可计算的分量

刚体在三维空间中的运动可以分解为六个独立分量：沿三个轴的平移和绕三个轴的旋转。工程上通常采用国际拖曳水池会议（ITTC）推荐的符号体系，用向量 η 表示惯性系下的位置与姿态，用 ν 表示载体系下的线速度与角速度。具体约定如下：

• 平移分量：纵荡（surge，沿载体系 x 轴）、横荡（sway，沿 y 轴）、垂荡（heave，沿 z 轴），对应位置 x、y、z 与线速度 u、v、w。
• 旋转分量：横滚（roll，绕 x 轴）、纵倾（pitch，绕 y 轴）、艏摇（yaw，绕 z 轴），对应姿态角 φ、θ、ψ 与角速度 p、q、r。

需要强调的是，这六个分量并非彼此独立。例如艏摇角速度 r 会与纵荡速度 u 耦合，产生载体系下的横向加速度；纵倾角 θ 会使重力在载体系上产生纵向分量。这种耦合正是AUV动力学建模不能简单套用线性系统的原因，也是后续误差方程中交叉项的来源。

坐标系定义：惯性系与载体系

惯性系（也称导航系或世界系）通常取当地水平面为参考：原点固定于水面某点，x 轴指向正北，y 轴指向正东，z 轴指向地心，构成北-东-地（NED）右手系。选择 NED 而非东北天，是因为它与航海习惯一致，且深度为正值，便于表示AUV的下潜深度。惯性系下位置记为 pⁿ = [x, y, z]^T，速度记为 vⁿ = [v_N, v_E, v_D]^T。

载体系固定于AUV本体：原点取在重心（或浮心，需在全文统一并说明），x 轴指向艏部，y 轴指向右舷，z 轴指向底部，同样构成右手系。载体系下速度记为 v^b = [u, v, w]^T，角速度记为 ω^b = [p, q, r]^T。姿态角按 Z-Y-X 顺序定义：艏摇角 ψ 绕 z 轴，纵倾角 θ 绕 y 轴，横滚角 φ 绕 x 轴，取值范围分别为 ψ∈(-π, π]、θ∈(-π/2, π/2)、φ∈(-π, π]。

姿态表示与坐标转换

从载体系到惯性系的转换由旋转矩阵 R_bⁿ 完成，按 Z-Y-X 顺序可写为三个基本旋转的乘积：

R_b^n = Rz(ψ) · Ry(θ) · Rx(φ)

展开后：
[ cψcθ   cψsθsφ - sψcφ   cψsθcφ + sψsφ ]
[ sψcθ   sψsθsφ + cψcφ   sψsθcφ - cψsφ ]
[ -sθ    cθsφ            cθcφ            ]

其中 c 表示 cos，s 表示 sin，角度取 ψ、θ、φ。

有了旋转矩阵，两个方向的换算就非常直接：惯性系速度 vⁿ = R_bⁿ · v^b，反过来 v^b = (R_bⁿ)^T · vⁿ。位置更新则在惯性系下积分：pⁿ(k+1) = pⁿ(k) + vⁿ · Δt。姿态更新稍复杂，欧拉角变化率与载体系角速度之间通过一个转换矩阵关联：

[φ̇]   [1  sφ·tanθ  cφ·tanθ] [p]
[θ̇] = [0  cφ        -sφ     ] [q]
[ψ̇]   [0  sφ/cθ     cφ/cθ   ] [r]

该矩阵在 θ 接近 ±90° 时出现奇异，这是欧拉角表示的固有缺陷。对于纵倾角较大的AUV（如大角度下潜或爬升），工程上常改用四元数表示姿态，避免奇异并减少三角函数计算量。四元数 q = [q₀, q₁, q₂, q₃]^T 的更新方程为 q̇ = 0.5 · q ⊗ [0, ω^b]，其中 ⊗ 表示四元数乘法。本节后续以欧拉角为主讲解，但读者应知道四元数是实际系统中的常用替代。

一个可手算的转换示例

假设某AUV在载体系下的速度 v^b = [2.0, 0.5, -0.3]^T m/s（纵荡 2 m/s、横荡 0.5 m/s、垂荡 -0.3 m/s，负值表示向上），当前姿态为 ψ = 30°、θ = 10°、φ = 0°。代入旋转矩阵计算惯性系速度：

cψ=0.866, sψ=0.500, cθ=0.985, sθ=0.174, cφ=1, sφ=0

v_N = 0.866×0.985×2.0 + (0.866×0.174×0 - 0.500×1)×0.5 + (0.866×0.174×1 + 0.500×0)×(-0.3)
    ≈ 1.706 - 0.250 - 0.045 ≈ 1.411 m/s
v_E = 0.500×0.985×2.0 + (0.500×0.174×0 + 0.866×1)×0.5 + (0.500×0.174×1 - 0.866×0)×(-0.3)
    ≈ 0.985 + 0.433 - 0.026 ≈ 1.392 m/s
v_D = -0.174×2.0 + 0.985×0×0.5 + 0.985×1×(-0.3)
    ≈ -0.348 - 0.296 ≈ -0.644 m/s

结果说明：AUV虽然艏向偏北偏东 30°，但由于存在横荡和垂荡分量，惯性系下的北向与东向速度并不等于纵荡速度的简单投影，深度方向还出现了 -0.644 m/s 的上升速度。若忽略横荡项，北向速度会被高估约 0.25 m/s，在 100 s 的航程中累积位置误差可达 25 m——这正是融合算法必须精确建模坐标转换的直接原因。

运动学与动力学的分工

运动学只关心几何关系，不涉及力和质量，其核心是上述坐标转换与积分更新。动力学则描述力、力矩与加速度的关系，标准形式为 M·ν̇ + C(ν)·ν + D(ν)·ν + g(η) = τ，其中 M 为包含附加质量的惯性矩阵，C 为科氏力与向心力矩阵，D 为阻尼矩阵，g 为恢复力（重力与浮力），τ 为推进器与舵产生的控制力。对融合而言，运动学提供状态传播的骨架，动力学提供过程噪声与模型不确定性的物理依据；两者结合，才能让滤波器知道“状态应该怎么变”以及“变化有多可信”。

建模时的几个工程要点

第一，原点选择要明确。若原点取在重心，恢复力矩计算简洁；若取在浮心，重力与浮力的力臂关系更直观。两种选择本身都正确，但混用会导致力矩项符号错误。第二，附加质量不可忽略。水下航行器加速时带动周围流体，等效质量可达本体的 30%～80%，M 矩阵中非对角项（如纵荡与纵倾的耦合附加质量）在机动时影响显著。第三，阻尼矩阵通常是非线性的，D(ν)·ν 中既有线性项也有二次项，低速时线性项主导，高速时二次项主导，建模时应保留二次项以匹配实际阻力特性。

对后续滤波而言，本节建立的模型直接决定了状态转移矩阵 F 的形式。以位置-速度状态为例，离散化后 F ≈ I + A·Δt，其中 A 由运动学关系填充；姿态通道则通过 T(φ,θ) 矩阵与角速度关联。过程噪声协方差 Q 的量级也应参考动力学模型的不确定度设定，而不是随意取一个经验值。把这些基础打牢，后续的惯导误差抑制与多源融合才有可靠的模型支撑。