4. AUV定深巡航抗海流扰动仿真验证

任务设定与仿真对象

本例验证 AUV 在恒定海流扰动下的定深巡航保持能力。仿真对象采用一个简化的三自由度（纵向、垂向、艏向）运动模型，深度控制回路为“深度误差 → PID 深度控制器 → 俯仰指令 → 俯仰内环 → 垂向速度 → 深度”。海流以恒定速度沿水平面某一方向注入，通过改变相对流速影响 AUV 的纵向与横向运动，从而产生航迹偏移；同时海流引起的垂向扰动分量会带来深度偏差。

为便于复现，设定如下教学示例参数（均为假设值，非实测）：目标深度 z_ref = 50 m，初始深度 z0 = 50 m，巡航速度 u = 1.5 m/s，仿真步长 dt = 0.05 s，仿真时长 300 s。海流在 t = 60 s 时注入，流速 Vc = 0.4 m/s，流向与 AUV 航向夹角 β = 30°，因此海流在 AUV 体坐标系下的分量为：纵向 Vc·cosβ ≈ 0.346 m/s，横向 Vc·sinβ ≈ 0.200 m/s。深度控制器参数取 Kp = 0.8、Ki = 0.02、Kd = 0.15（俯仰角指令限幅 ±15°）。

仿真实现要点

核心是把海流作为“相对速度修正”注入运动方程，而不是直接叠加到位置。下面给出可运行的关键实现（Python，纯标准库即可运行，便于无第三方依赖复现）：

import math

# ---- 教学示例参数（假设值）----
z_ref   = 50.0      # 目标深度 m
u_cruise= 1.5       # 巡航速度 m/s
dt      = 0.05      # 步长 s
T_end   = 300.0     # 仿真时长 s
t_inject= 60.0      # 海流注入时刻 s
Vc      = 0.4       # 海流流速 m/s
beta    = math.radians(30.0)  # 海流与航向夹角

Kp, Ki, Kd = 0.8, 0.02, 0.15
pitch_limit = math.radians(15.0)

# ---- 状态初始化 ----
z, w      = z_ref, 0.0     # 深度、垂向速度
x, y      = 0.0, 0.0       # 水平位置
psi       = 0.0            # 艏向（沿 +x）
theta     = 0.0            # 俯仰角
integ     = 0.0
prev_e    = 0.0

log = []
t = 0.0
while t <= T_end + 1e-9:
    # 海流分量（体坐标系）
    if t >= t_inject:
        vc_u = Vc * math.cos(beta)
        vc_v = Vc * math.sin(beta)
    else:
        vc_u = vc_v = 0.0

    # 深度 PID → 俯仰指令
    e = z_ref - z
    integ += e * dt
    deriv = (e - prev_e) / dt
    theta_cmd = Kp*e + Ki*integ + Kd*deriv
    theta_cmd = max(-pitch_limit, min(pitch_limit, theta_cmd))
    prev_e = e

    # 俯仰一阶响应
    theta += (theta_cmd - theta) * min(1.0, 2.0*dt)

    # 垂向速度由俯仰与相对流速决定
    w = u_cruise * math.sin(theta) + vc_v * math.sin(theta)
    z += w * dt

    # 水平运动：相对流速 + 海流分量
    u_rel = u_cruise - vc_u
    x += (u_rel*math.cos(psi) - vc_v*math.sin(psi)) * dt
    y += (u_rel*math.sin(psi) + vc_v*math.cos(psi)) * dt

    log.append((t, z, y, theta))
    t += dt

# ---- 指标统计 ----
z_dev  = [abs(r[1]-z_ref) for r in log if r[0] >= t_inject]
y_drift= [abs(r[2]) for r in log if r[0] >= t_inject]
print("最大深度偏差(m):", round(max(z_dev), 3))
print("末段深度偏差(m):", round(z_dev[-1], 3))
print("航迹横向偏移(m):", round(y_drift[-1], 3))

运行后应观察三类量：深度偏差 |z - z_ref| 的时间曲线、横向偏移 |y| 的累积趋势、以及俯仰角 theta 的调节过程。海流注入瞬间深度偏差会先增大，随后由 PID 积分项逐步补偿，偏差回落并稳定在一个非零残差附近——这个残差正是恒定海流下“保持能力”的量化体现。

扰动响应与恢复过程判读

下图给出该仿真的信号链与关键响应关系：海流分量如何进入垂向与水平通道，深度偏差如何经控制器形成俯仰指令并最终反馈到深度，以及注入时刻、峰值偏差、恢复段与稳态残差在时间轴上的位置关系。

结果判读与保持能力评估

基于上述实现，判读时关注四个量：

• 峰值深度偏差：海流注入后偏差上升的最大值，反映控制器对突加扰动的瞬态抑制能力。若峰值超过任务允许的深度带（例如 ±2 m），说明增益偏小或积分作用不足。
• 恢复时间：从峰值回落到稳态残差 ±10% 范围内所需时间，反映调节速度。恢复过慢通常与 Ki 偏小或俯仰限幅过严有关。
• 稳态残差：恒定海流下 PID 积分项最终会抵消垂向扰动分量，但受俯仰限幅与模型非线性影响，残差通常不为零。残差大小是“保持能力”的直接指标。
• 航迹横向偏移：横向海流分量 vc_v ≈ 0.2 m/s 在 240 s 内可累积约 48 m 的横向偏移（示例估算），说明仅靠深度控制无法抑制航迹漂移，需要航向/横向控制协同或前馈补偿。