1. 自动舵PID控制律的离散化实现与基础航向保持仿真

连续PID到离散PID：先定采样周期，再定差分形式

自动舵的航向保持回路中，舵角指令由PID控制律给出。连续形式为

δ(t) = K_p·e(t) + K_i∫e(τ)dτ + K_d·de(t)/dt，其中 e(t) = ψ_ref − ψ(t) 为航向误差（单位：度），δ 为舵角指令（单位：度）。

实际自动舵以固定周期 T_s 采样航向，因此必须离散化。本例采用位置式离散PID：

• 积分项用矩形累加：I(k) = I(k−1) + K_i·T_s·e(k)
• 微分项用后向差分：D(k) = K_d·(e(k) − e(k−1))/T_s
• 输出：δ(k) = K_p·e(k) + I(k) + D(k)，再经舵角限幅 |δ| ≤ 35°

采样周期取 T_s = 0.5 s。船舶Nomoto一阶模型参数取 K = 0.18 (1/s)、T = 12 s，即 ψ̇ = (K·δ − ψ)/T（此处以航向角速度形式表达，δ 为舵角输入）。仿真用一阶欧拉法推进，步长与采样周期一致。

仿真程序实现

下面给出可直接运行的Python实现。程序包含离散PID类、Nomoto模型推进和航向保持仿真，并输出关键指标（超调量、稳态误差、调节时间）。

import numpy as np

class DiscretePID:
    def __init__(self, kp, ki, kd, ts, u_min=-35.0, u_max=35.0):
        self.kp, self.ki, self.kd, self.ts = kp, ki, kd, ts
        self.u_min, self.u_max = u_min, u_max
        self.integral = 0.0
        self.prev_error = 0.0

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0

    def step(self, error):
        # 积分项：矩形累加
        self.integral += self.ki * self.ts * error
        # 微分项：后向差分
        derivative = self.kd * (error - self.prev_error) / self.ts
        # 位置式输出
        u = self.kp * error + self.integral + derivative
        # 舵角限幅
        u = max(self.u_min, min(self.u_max, u))
        self.prev_error = error
        return u

def simulate(kp, ki, kd, ts=0.5, t_end=120.0, psi_ref=20.0,
             K=0.18, T=12.0, psi0=0.0):
    n = int(t_end / ts)
    pid = DiscretePID(kp, ki, kd, ts)
    psi = psi0
    log = {'t': [], 'psi': [], 'delta': [], 'error': []}
    for k in range(n):
        t = k * ts
        error = psi_ref - psi
        delta = pid.step(error)
        # Nomoto 一阶模型：T*psi_dot + psi = K*delta
        psi_dot = (K * delta - psi) / T
        psi += psi_dot * ts
        log['t'].append(t)
        log['psi'].append(psi)
        log['delta'].append(delta)
        log['error'].append(error)
    return {k: np.array(v) for k, v in log.items()}

def metrics(log, psi_ref=20.0, ts=0.5):
    psi = log['psi']
    peak = np.max(psi)
    overshoot = max(0.0, (peak - psi_ref) / psi_ref * 100)
    # 稳态误差：最后10%时间段均值
    tail = psi[int(len(psi)*0.9):]
    ss_error = psi_ref - np.mean(tail)
    # 调节时间：进入±2%带并保持
    band = 0.02 * psi_ref
    settle_idx = len(psi) - 1
    for i in range(len(psi)-1, -1, -1):
        if abs(psi[i] - psi_ref) > band:
            settle_idx = i + 1
            break
    settle_time = settle_idx * ts
    return overshoot, ss_error, settle_time

if __name__ == '__main__':
    log = simulate(kp=2.0, ki=0.05, kd=8.0)
    os_, sse, st = metrics(log)
    print(f"超调量: {os_:.2f}%")
    print(f"稳态误差: {sse:.3f}°")
    print(f"调节时间(±2%): {st:.1f} s")

参数对收敛过程的影响

用同一组Nomoto参数，分别改变Kp、Ki、Kd，观察航向误差收敛。下表为示例仿真结果（教学示例数据，非实测）：

判读要点：Kp增大加快上升但增大超调；Ki消除稳态误差但过大会引起积分超调；Kd提供阻尼、抑制超调，但对航向测量噪声敏感，实际自动舵中需配合滤波。本例为静水无干扰场景，稳态误差主要由积分项消除。