2. AUV深度与航向控制基础

在开始写PID之前，必须先回答一个更基础的问题：控制器到底在“控制什么”，它看到的量、它输出的量、以及这些量之间通过什么物理通道发生联系。AUV的深度与航向控制之所以容易在调试中反复振荡或长期稳态偏差，很大一部分原因不是PID参数本身，而是坐标系约定混乱、被控量与执行机构对应关系不清。本节把这条链路从定义到执行机构限制完整梳理一遍，为后续建模和调参提供统一参照。

一、两套坐标系：体坐标系与地面坐标系

AUV的运动描述必须同时使用两套坐标系。地面坐标系（也称惯性系或北东地系）固定于大地，原点可取水面某一点，X轴指北、Y轴指东、Z轴指向地心（北东地，NED）。深度就是AUV在地面系下的Z向位置，向下为正，因此“深度增大”对应Z值增大，这一点在写深度误差时非常关键：若期望深度为 z_d，当前深度为 z，则深度误差常取 e_z = z_d - z，控制器输出需使Z向速度为正才能下潜。

体坐标系固连于艇体，原点通常取艇体重心或浮心，X轴指向艏部，Y轴指向右舷，Z轴指向艇底。姿态角由此定义：纵倾角（pitch）θ 为绕体Y轴的转角，艏摇角（yaw）ψ 为绕体Z轴的转角，横滚角（roll）φ 为绕体X轴的转角。地面系与体系之间通过旋转矩阵联系，工程上常按Z-Y-X顺序分解。对深度与航向控制而言，真正需要牢记的是：深度是地面系量，纵倾和艏摇是体系姿态量，二者通过旋转关系耦合。

二、深度不是直接“推”出来的：纵倾与升沉的关系

很多初学者会直觉地认为“深度控制就是控制一个上下推进器”，但对多数欠驱动AUV而言，深度变化主要来自两条通道的叠加。第一条是升沉运动：当艇体存在净浮力差或垂直方向速度 w 时，深度直接变化。第二条是纵倾耦合：当AUV以巡航速度 u 前进并带有纵倾角 θ 时，艇体沿体X轴的速度会在地面系Z轴上产生分量 ż ≈ -u·sinθ + w·cosθ（按NED约定，θ为抬头正时下潜分量为负，需结合具体符号约定核对）。这意味着：让艇低头（θ为负）配合前进速度，就能下潜；抬头则上浮。这正是艉升降舵控制深度的物理基础。

由此带来一个重要的工程结论：深度环的输出不应直接给舵角，而通常先给期望纵倾角 θ_d，再由内环纵倾控制器跟踪 θ_d，形成“深度—纵倾”串级结构。若把深度误差直接映射为舵角，在低速时舵效不足会导致积分饱和，在高速时又容易过冲。实际调试中，常见做法是限制 θ_d 的幅值（例如 ±15°），并给深度环配置比纵倾环慢3～5倍的时间尺度。

三、航向与艏摇：方向舵的作用

航向控制相对独立。艏摇角 ψ 由方向舵偏角 δ_r 产生的艏摇力矩驱动，其简化动态可写为 I_z·ψ̈ = N_δ·δ_r - N_r·ψ̇，其中 N_δ 为舵效系数、N_r 为阻尼系数。航向环通常直接输出 δ_r，但必须注意两点：其一，舵效与航速强相关，N_δ 近似与 u² 成正比，低速时同样的舵角产生的力矩显著减小；其二，舵机存在死区和速率限制，小角度指令可能根本不动，调试时若发现稳态误差始终存在，应先排查死区而非盲目加积分。

航向误差需处理角度环绕问题：ψ 在 ±180° 处跳变，误差应归一化到 (−180°, 180°]，否则会出现“本应转10°却指令转350°”的异常。工程上常用 e_ψ = atan2(sin(ψ_d−ψ), cos(ψ_d−ψ)) 完成归一化。

四、执行机构的真实限制

建模与调参时若忽略执行机构限制，仿真里漂亮的响应在实艇上往往变成持续振荡。需要纳入的三类限制如下表所示（数值为典型中小型AUV的合理示例，具体以实际舵机手册为准）：

五、把约定固定下来：调试前的检查清单

在进入建模之前，建议先用一段静态检查把符号与通道固定，避免后续反复返工。以下清单可直接对照实艇或仿真配置逐项确认：

• 深度符号：确认深度计输出向下为正，且 e_z = z_d − z 时正误差对应下潜指令。
• 纵倾符号：确认抬头为正还是低头为正，并与深度环输出方向一致；若不一致，在接口处统一取反，切勿在多个环节各自取反。
• 舵角符号：给定正舵角，观察艇体实际转向，确认 δ_r 正方向与 ψ 增大方向一致。
• 角度归一化：航向误差必须经 atan2 归一化，禁止直接相减。
• 限幅位置：舵角限幅应放在控制器输出之后、执行机构之前，并同步实现积分抗饱和。
• 航速关联：记录调试时的巡航速度，后续增益若按低速整定，高速时需重新核对。

把上述概念理清后，深度与航向控制就从一个模糊的“调三个参数”问题，变成了两条边界清晰的通道：深度环通过期望纵倾驱动升降舵，航向环通过方向舵直接驱动艏摇，二者共享航速这一关键工况变量。后续的建模、增益整定与抗饱和设计，都将建立在这一统一约定之上。