2. AUV航向阶跃指令跟踪性能仿真评估

航向阶跃指令跟踪性能仿真评估

本节任务是在已有仿真环境中，向AUV航向控制回路施加一个阶跃指令，记录航向角响应曲线，并从中提取超调量、上升时间与稳态误差三项指标，据此判断航向回路的跟踪性能。与定深巡航基线不同，本节关注的是横向控制通道的动态品质，输入是航向指令的突变，输出是航向角的时域响应。

阶跃指令设计与仿真记录

设AUV初始航向为0°（正北），仿真步长取0.05 s，仿真时长60 s。在t=5 s时刻将航向指令由0°阶跃至30°，保持至仿真结束。记录量包括时间戳、航向指令、实际航向角、舵角指令与舵角反馈。为便于指标计算，航向角统一取相对初始航向的偏差值，逆时针为正，与仿真坐标系约定一致。

# 阶跃指令与记录配置（示例参数）
sim_dt      = 0.05      # 仿真步长 s
sim_time    = 60.0      # 仿真时长 s
t_step      = 5.0       # 阶跃施加时刻 s
psi_cmd_0   = 0.0       # 初始航向指令 deg
psi_cmd_1   = 30.0      # 阶跃后航向指令 deg
record_vars = ["t", "psi_cmd", "psi", "delta_r_cmd", "delta_r"]

仿真运行后得到航向响应序列。下面给出指标计算的关键实现，输入为记录得到的t与psi数组，输出为三项性能指标。

import numpy as np

def eval_heading_step(t, psi, t_step, psi_cmd_1, psi_cmd_0=0.0, tol=0.02):
    """计算航向阶跃响应的超调量、上升时间与稳态误差。
    tol: 稳态带宽度，取阶跃幅值的2%。
    """
    amp = psi_cmd_1 - psi_cmd_0
    band = tol * abs(amp)
    # 只取阶跃之后的响应段
    mask = t >= t_step
    tt, pp = t[mask], psi[mask]

    # 稳态值：取最后10%时间段均值
    n_tail = max(1, int(0.1 * len(pp)))
    psi_ss = np.mean(pp[-n_tail:])

    # 超调量
    peak = np.max(pp)
    overshoot = (peak - psi_cmd_1) / abs(amp) * 100.0 if peak > psi_cmd_1 else 0.0

    # 上升时间：10%到90%阶跃幅值
    lo, hi = psi_cmd_0 + 0.1 * amp, psi_cmd_0 + 0.9 * amp
    t_lo = tt[np.argmax(pp >= lo)] if np.any(pp >= lo) else np.nan
    t_hi = tt[np.argmax(pp >= hi)] if np.any(pp >= hi) else np.nan
    t_rise = t_hi - t_lo

    # 稳态误差
    ss_error = psi_cmd_1 - psi_ss
    return overshoot, t_rise, ss_error, psi_ss

以一组示例仿真记录代入上述函数，得到：超调量约8.6%，上升时间约3.4 s，稳态误差约0.4°。这些数值属于示例参数下的预期结果，实际值随控制器增益、舵机限速与阻尼设置变化。

响应曲线与指标判读

下图把阶跃指令、航向响应与三项指标的提取位置放在同一时间轴上，便于对照判读。曲线为示例参数下的典型二阶欠阻尼响应形态。

判读要点：曲线在阶跃后约3.4 s完成10%–90%上升，峰值超出指令约2.6°（相对30°幅值约8.6%），随后在稳态带内小幅收敛，末段均值与指令相差约0.4°。若超调量明显偏大，通常指向航向PID比例增益过高或阻尼不足；若上升时间过长而超调很小，则多为增益偏低或舵机限速过紧；稳态误差持续偏大则需检查积分项是否被限幅或存在常值扰动未补偿。

指标记录与评估结论

将三项指标与设计门限对照，形成可复用的评估记录。下表为示例参数下的评估结果，门限值按一般航向回路要求设定，实际项目应替换为任务书规定值。

三项指标均落在门限内，可判定该组控制器参数下航向回路对30°阶跃指令具备可接受的跟踪性能。若需进一步验证鲁棒性，可在同一评估脚本中改变阶跃幅值（如15°、45°）重复计算，观察指标随工作点的变化趋势，但幅值扫描与扰动注入属于后续独立任务，本节不展开。