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

AUV 的避障算法最终要落到“艇体怎么动、为什么这样动”上。如果只把 AUV 当成一个在平面上移动的质点，避障仿真往往能跑通，但一旦加入深度保持、横滚抑制或强海流扰动，规划出的轨迹就会与真实响应严重脱节。本节围绕六自由度运动学、动力学模型以及面向避障仿真的简化与参数辨识展开，目标是让读者能写出一个“够用且可信”的仿真模型。

一、坐标系与六自由度运动学

建模的第一步是统一参照约定，全文及后续图示保持一致。采用两个右手坐标系：惯性坐标系 O-XYZ 固定于大地，Z 轴向下为正；艇体坐标系 o-xyz 固连于 AUV，x 轴指向艇首，y 轴指向右舷，z 轴指向艇底。六自由度状态量记为：位置 η₁=[x, y, z]ᵀ，姿态角 η₂=[φ, θ, ψ]ᵀ（横滚、纵倾、艏向）；艇体线速度 v₁=[u, v, w]ᵀ，角速度 v₂=[p, q, r]ᵀ。

运动学方程把艇体速度映射到惯性系：

η̇₁ = R(φ,θ,ψ) · v₁
η̇₂ = T(φ,θ) · v₂

R = Rz(ψ)Ry(θ)Rx(φ)   （Z-Y-X 欧拉顺序）
T = [[1, sinφ·tanθ, cosφ·tanθ],
     [0, cosφ,      -sinφ    ],
     [0, sinφ/cosθ,  cosφ/cosθ]]

这里有两个工程上必须注意的点。其一，T 矩阵在 θ=±90° 处奇异，AUV 正常航行纵倾很小，一般不会触及，但仿真中若姿态初值给得过大就会发散，建议把 θ 限制在 ±60° 内。其二，避障中最关心的是艏向 ψ 与位置 (x, y)，而深度 z 通常由独立的深度控制回路维持，因此运动学实现时可以按“水平面三自由度 + 垂直面深度”分层处理，但状态量仍保留完整六维，避免耦合项丢失。

二、动力学模型：附加质量、阻尼与恢复力

刚体动力学在艇体坐标系下写成通用的六自由度形式：

M·v̇ + C(v)·v + D(v)·v + g(η) = τ

M = M_RB + M_A      （刚体惯性 + 附加质量）
D(v) = D_l + D_n(v) （线性阻尼 + 非线性阻尼）
g(η) = 恢复力（重力与浮力）

附加质量是水下建模区别于空中飞行器的核心。AUV 加速时须带动周围水一起运动，等效为各轴方向上的附加惯性。工程上常取经验值：附加质量约为对应方向排开水的 10%~100%，纵向较小（细长体约 0.1m），横向与垂向较大（约 0.8~1.0m），转动方向附加惯量同样不可忽略。忽略附加质量会让加速段响应偏快，避障急转时轨迹过于“灵敏”，是仿真失真最常见的来源之一。

阻尼分为线性项与二次项。低速时以线性阻尼为主，高速或大攻角时二次阻尼占优。对避障仿真而言，艏向阻尼系数 r 相关项和横向阻尼 v 相关项最敏感，因为它们直接决定转向的收敛速度与超调。

恢复力由重力 W 与浮力 B 的合力及扶正力矩构成。若设计为中性浮力（W≈B），则垂向静差很小，恢复力主要体现为横滚与纵倾的扶正刚度，用稳心高 GM 描述。避障中若 AUV 需要大角度转向，横滚耦合会通过恢复力产生回正力矩，这一项在简化模型中若被删掉，转向后的姿态恢复过程就会缺失。

三、面向避障仿真的简化方法

完整六自由度模型参数多、辨识成本高，而避障算法验证主要关心水平机动性与转向响应，因此合理的简化是必要的，但简化必须有依据。推荐做法是：保留水平面三自由度（x, y, ψ 与 u, v, r）作为主模型，深度通道用一阶或二阶回路近似，同时保留附加质量与主导阻尼项，删除横滚与纵倾的快速动态，仅以静态扶正关系约束姿态。这样既能把状态维数从 12 降到 6 左右，又不至于让转向响应失真。

一个常见的错误是“为了跑得快”把附加质量整项删掉。以某中型 AUV 为例，纵向附加质量约为 0.12m、横向约 0.85m，若删除横向附加质量，同样的舵角指令下横向速度响应会偏大约 40%，避障时规划器会误判“转得过来”，实艇却出现明显超调甚至擦碰障碍。因此简化可以删高阶耦合项，但附加质量与转向阻尼这两类主导项应保留。

四、参数辨识的实操要点

参数辨识不是“调参凑曲线”，而是用可观测的试验数据反推系数。以艏向阻尼 N_r 与 N_rr 为例，可设计 Z 形操舵试验：给定 ±10° 的舵角阶跃，记录艏向角 ψ(t) 与角速度 r(t)，用最小二乘拟合一阶转向模型 ψ̇ = r，I_z·ṙ = N_r·r + N_rr·|r|·r + N_δ·δ。拟合时先固定 N_δ（可由稳态回转试验得到），再回归 N_r 与 N_rr，最后用另一组不同舵角的试验做交叉验证，若预测的 ψ 峰值误差在 10% 以内即可认为模型可用。

辨识中有三个易被忽略的细节：其一，试验数据须先做低通滤波，否则角速度噪声会严重污染二次项系数；其二，附加质量难以直接测量，通常由 CFD 或经验公式给出初值，再用加速段数据微调；其三，恢复力项中的 GM 应通过横滚衰减试验独立校核，不要与阻尼项混在一起拟合，否则两者会相互补偿，得到“拟合很好但物理错误”的参数组。

把上述模型接入避障仿真时，建议先用完整模型跑一条基准轨迹，再用简化模型跑同一场景，对比两者的最小障碍距离与转向超调量。若偏差在可接受范围内，简化模型即可用于后续大量算法迭代；若偏差过大，应回到附加质量与阻尼项检查，而不是直接调避障算法的安全阈值来“掩盖”模型误差。