3. 基于航路点的直线航段跟踪控制器实现

任务对象与控制器结构

本案例的跟踪对象是一条由两个航路点 WP0=(0,0)、WP1=(120,80)（单位：m，NED 坐标系，x 向北、y 向东）确定的直线航段。无人船初始位置 (8,-6)，初始航向 ψ=0.35 rad，以恒定前进速度 u=1.5 m/s 航行。控制目标是让船体收敛到该直线并沿其前进，同时记录横向偏差 e_y 的收敛过程。

控制器采用串联结构：LOS 制导律根据船与航段的几何关系计算期望航向 ψ_d，航向 PID 控制器根据航向误差 ψ_e = ψ_d − ψ 输出舵角指令 δ，再由一阶 Nomoto 船舶模型积分得到实际航向与位置。前视距离 Δ 是 LOS 的核心参数，直接决定收敛速度与超调之间的权衡。

LOS 制导律与航向 PID 的实现

LOS 制导的核心是把船位投影到航段上，再沿航段方向前移一个前视距离 Δ 得到前视点，期望航向即指向该前视点。设航段起点 P0、终点 P1，航段方向角 α = atan2(y1−y0, x1−x0)，船位相对起点的横向偏差 e_y = −(x−x0)·sinα + (y−y0)·cosα（右正）。则：

ψ_d = α + atan2(−e_y, Δ)

航向 PID 以 ψ_e = wrap(ψ_d − ψ) 为输入，输出舵角 δ，并对舵角做限幅。下面给出可直接运行的 Python 仿真实现（仅依赖标准库与 numpy）。

import math
import numpy as np

# ---------- 航段与船舶参数 ----------
P0 = np.array([0.0, 0.0])      # WP0 (x_north, y_east)
P1 = np.array([120.0, 80.0])   # WP1
U  = 1.5                       # 前进速度 m/s
DT = 0.1                       # 仿真步长 s
T_SIM = 120.0                  # 仿真时长 s

# Nomoto 一阶模型参数
T_N = 2.5
K_N = 0.8

# 航向 PID 参数
KP, KI, KD = 1.2, 0.05, 0.4
DELTA_MAX = math.radians(30.0)  # 舵角限幅

# LOS 前视距离（待调整）
DELTA_LOS = 15.0

def wrap(a):
    """把角度归一化到 (-pi, pi]"""
    return (a + math.pi) % (2 * math.pi) - math.pi

def los_heading(px, py, p0, p1, delta_los):
    """返回期望航向 psi_d 与横向偏差 e_y"""
    dx, dy = p1[0] - p0[0], p1[1] - p0[1]
    alpha = math.atan2(dy, dx)
    # 船位相对起点的横向偏差（右正）
    e_y = -(px - p0[0]) * math.sin(alpha) + (py - p0[1]) * math.cos(alpha)
    psi_d = alpha + math.atan2(-e_y, delta_los)
    return wrap(psi_d), e_y

def simulate(delta_los, kp=KP, ki=KI, kd=KD):
    x, y = 8.0, -6.0
    psi = 0.35
    r = 0.0            # 偏航角速率
    integ = 0.0
    prev_e = 0.0
    log = []
    steps = int(T_SIM / DT)
    for k in range(steps):
        psi_d, e_y = los_heading(x, y, P0, P1, delta_los)
        e = wrap(psi_d - psi)
        integ += e * DT
        deriv = (e - prev_e) / DT
        prev_e = e
        delta = kp * e + ki * integ + kd * deriv
        delta = max(-DELTA_MAX, min(DELTA_MAX, delta))
        # Nomoto: T*r_dot + r = K*delta
        r_dot = (K_N * delta - r) / T_N
        r += r_dot * DT
        psi = wrap(psi + r * DT)
        x += U * math.cos(psi) * DT
        y += U * math.sin(psi) * DT
        log.append((k * DT, x, y, psi, psi_d, e_y, delta))
    return np.array(log)

# 对比三组前视距离
for d in (8.0, 15.0, 30.0):
    log = simulate(d)
    final_ey = abs(log[-1, 5])
    max_ey = np.max(np.abs(log[:, 5]))
    print(f"Δ={d:5.1f} m | 末段|e_y|={final_ey:.3f} m | 峰值|e_y|={max_ey:.3f} m")

运行后应观察到：Δ=8 时横向偏差收敛最快但峰值与末段残差可能偏大（易振荡）；Δ=30 时轨迹平滑但收敛慢；Δ=15 在两者之间取得折中。这正是前视距离参数调整的直观依据。

横向偏差收敛过程与参数判读

仿真日志中第 6 列即横向偏差 e_y。判读时关注三个量：峰值偏差（反映初始超调）、收敛时间（|e_y| 首次进入 0.5 m 并保持的时间）、末段残差（稳态跟踪精度）。下表为示例参数下的预期趋势（教学示例，非实测）：