📚 课程目录第1章 AUV定深巡航任务仿真基线搭建与验证

1. AUV定深巡航任务仿真基线搭建与验证

任务场景与基线定义

本例要搭建的是一条“定深直航”基线：AUV 从初始位置出发，沿指定航向以恒定航速航行 600 m，全程保持目标深度 30 m。基线场景的意义在于给后续的航向阶跃、深度指令跟踪等评估提供一个可复现的参照——如果连定深直航都无法稳定保持，后续控制性能评估就失去意义。

场景参数按教学示例设定如下（非实测数据）：

参数	符号	取值	说明
目标深度	z_ref	30.0 m	深度向下为正
指令航速	u_cmd	1.5 m/s	纵向对地速度
指令航向	ψ_cmd	90°	正东，北向为 0°、顺时针为正
航段长度	L	600 m	沿航向的直线距离
仿真步长	dt	0.1 s	定步长积分
初始深度	z0	0 m	水面起航，需下潜

基线场景的仿真结构

基线不是“随便跑一下”，而是由指令层、被控对象、约束监测三层组成的闭环。下图给出本例搭建的仿真数据流：指令层产生深度/航速/航向设定值，深度与航向控制器输出舵角与推力，AUV 六自由度简化模型积分出状态，监测模块逐拍比对约束并累计航程，最终输出通过/不通过结论。

基线仿真实现

下面给出一个可直接运行的 Python 基线脚本。它用简化的三通道模型（垂向、艏向、纵向）代替完整六自由度，足以验证定深保持能力，同时保持代码短小可读。深度向下为正，艏向以北为 0°、顺时针为正。

import math

# ---------- 场景参数 ----------
DT      = 0.1
T_END   = 600.0
Z_REF   = 30.0
U_CMD   = 1.5
PSI_CMD = math.radians(90.0)
L_SEG   = 600.0

# ---------- 简化模型参数（教学示例值） ----------
M_W   = 120.0    # 垂向等效质量 kg
D_W   = 90.0     # 垂向阻尼 N·s/m
K_W   = 40.0     # 浮力/压载配平刚度 N/m
M_PSI = 25.0     # 艏向转动惯量
D_PSI = 18.0     # 艏向阻尼
K_PSI = 12.0     # 舵效系数
M_U   = 150.0    # 纵向等效质量
D_U   = 60.0     # 纵向阻尼

# ---------- 控制器 ----------
class PID:
    def __init__(self, kp, ki, kd, lim):
        self.kp, self.ki, self.kd, self.lim = kp, ki, kd, lim
        self.i = 0.0
        self.prev = None
    def step(self, err, dt):
        self.i += err * dt
        d = 0.0 if self.prev is None else (err - self.prev) / dt
        self.prev = err
        out = self.kp*err + self.ki*self.i + self.kd*d
        return max(-self.lim, min(self.lim, out))

pid_z   = PID(kp=2.2, ki=0.05, kd=1.8, lim=math.radians(25))
pid_psi = PID(kp=1.6, ki=0.02, kd=0.9, lim=math.radians(25))

# ---------- 初始状态 ----------
z, w   = 0.0, 0.0          # 深度 m，垂向速度 m/s（向下为正）
psi, r = 0.0, 0.0          # 艏向 rad，艏摇角速度 rad/s
u      = 0.0               # 纵向速度 m/s
x, y   = 0.0, 0.0
s      = 0.0               # 累计航程

t = 0.0
log = []
violations = []

while t < T_END:
    # 深度环：误差 = 目标 - 当前（向下为正）
    delta_s = pid_z.step(Z_REF - z, DT)
    # 航向环：角度误差归一化到 [-pi, pi]
    e_psi = (PSI_CMD - psi + math.pi) % (2*math.pi) - math.pi
    delta_r = pid_psi.step(e_psi, DT)

    # 垂向动力学
    w_dot = (K_W*(Z_REF - z) - D_W*w + M_W*9.81*math.sin(delta_s)) / M_W
    w += w_dot * DT
    z += w * DT

    # 艏向动力学
    r_dot = (K_PSI*delta_r - D_PSI*r) / M_PSI
    r += r_dot * DT
    psi += r * DT

    # 纵向：一阶趋近指令航速
    u += (U_CMD - u) * DT / 2.0

    # 位置与航程
    x += u * math.cos(psi) * DT
    y += u * math.sin(psi) * DT
    s += u * DT

    # 约束监测
    if abs(z - Z_REF) > 1.0:
        violations.append((round(t,1), 'depth', round(z,2)))
    if abs(math.degrees(psi) - 90.0) > 5.0:
        violations.append((round(t,1), 'heading', round(math.degrees(psi),2)))

    if abs(t % 10.0) < 1e-6:
        log.append((round(t,1), round(z,2), round(math.degrees(psi),1), round(u,2), round(s,1)))

    t += DT
    if s >= L_SEG:
        break

print("结束时刻 t =", round(t,1), "s, 航程 s =", round(s,1), "m")
print("最终深度 z =", round(z,2), "m, 航向 =", round(math.degrees(psi),1), "°")
print("深度越界次数 =", sum(1 for v in violations if v[1]=='depth'))
print("航向越界次数 =", sum(1 for v in violations if v[1]=='heading'))

脚本的关键点有三处：深度误差定义为 Z_REF - z（向下为正，目标大于当前时为正误差，舵角使 AUV 下潜）；航向误差用取模运算归一化到 ±π，避免 90° 与 −270° 被误判为大误差；航程 s 用 u·dt 累加，达到 600 m 即终止，保证“指定航段”这一约束被真正执行。

运行结果判读

按上述参数运行，预期现象是：起航阶段 AUV 从 0 m 下潜到 30 m，深度误差在初始几十秒内超出 ±1 m 容差带，属于正常的过渡过程；进入稳态后深度围绕 30 m 小幅波动，航向从 0° 转向 90° 后保持，航速逐步趋近 1.5 m/s。判读时不能只看终点值，要区分“过渡段越界”和“稳态越界”：

若越界只发生在 t < 60 s 的下潜段，且之后不再越界，判为“过渡过程正常，基线通过”。
若稳态段仍周期性越界，说明深度环增益或阻尼不匹配，需先调 PID 再谈基线。
若航向在 90° 附近持续振荡超过 ±5°，优先检查 pid_psi 的微分项与 D_PSI 是否过小。
若航程始终达不到 600 m，检查 u 是否被错误初始化或纵向时间常数过大。

基线脚本的模型参数是教学示例值，不代表任何真实 AUV 的水动力系数。实际使用时应用水池试验或辨识数据替换 M_W、D_W、K_W、M_PSI、D_PSI、K_PSI，但判据结构与监测逻辑可直接沿用。

基线检查清单

检查项	通过条件	不通过时的首要排查
深度保持	稳态 \|z−30\| ≤ 1.0 m	深度 PID 增益、垂向阻尼 D_W
航向保持	稳态 \|ψ−90°\| ≤ 5°	航向 PID 微分项、舵效 K_PSI
航速保持	\|u−1.5\| ≤ 0.1 m/s	纵向时间常数、推力限幅
航段完成	累计航程 ≥ 600 m	航向偏差导致投影航程不足
可复现性	同参数两次运行结果一致	随机种子、积分步长是否固定

完成以上检查后，这条定深直航基线即可作为后续控制性能评估的参照场景：任何新的控制器或任务改动，都应先在这条基线上复跑，确认没有破坏基本的定深与航段完成能力。