4. 多航路点路径的切换逻辑与到点判定

任务拆解：切换半径决定“何时算到点”

多航路点跟踪的核心矛盾在于：到点判定半径 R_switch 越大，转向越早、路径越平滑，但会切掉拐角、增大横向误差；半径越小，越贴近折线路径，但船在航路点附近会反复修正航向，甚至出现绕圈。本例以一条 5 个航路点的折线路径为对象，实现“到点判定 + 航段切换 + LOS 制导”的完整逻辑，并用同一组初始条件对比 R_switch = 3 m / 8 m / 15 m 三种设置下的路径形态与跟踪误差。

到点判定与切换逻辑

设当前目标航路点为 P_k=(x_k, y_k)，船位为 (x, y)，则到点判据为：

d_k = sqrt((x - x_k)^2 + (y - y_k)^2)
若 d_k <= R_switch  →  切换目标点 k = k+1，并重置该航段积分项
若 k 已为最后一点且 d_k <= R_switch →  任务完成

为避免“刚切换就立刻满足下一点判据”的抖动，增加两个工程约束：

• 最小航段驻留：切换后至少航行 1 个控制周期才允许再次判定（本例控制周期 0.1 s）。
• 投影越界判定：若船已越过当前航段终点在航段方向上的投影（沿航段方向的前向距离 s > 航段长度 L），即使 d_k 尚未小于 R_switch，也强制切换，防止船在航路点外侧“追点”。

切换后必须重置航向 PID 的积分项，否则上一航段累积的积分会在新航段造成超调——这是多航路点跟踪中最常见的隐性误差来源。

可运行实现（Python，纯标准库）

下面代码自包含：定义路径、船体一阶航向响应模型、LOS 制导、航向 PID、到点判定与切换，并输出三种切换半径下的路径点、最大横向误差与航向切换次数。可直接运行。

import math

# ---------- 路径与初始条件 ----------
WAYPOINTS = [(0, 0), (60, 0), (60, 50), (120, 50), (120, 100)]
START = (0.0, 0.0, 0.0)      # x, y, psi(rad)，航向 0 = 指向 +x
DT = 0.1
V = 2.0                      # 恒定航速 m/s
LOOKAHEAD = 12.0             # LOS 前视距离 m
T_MAX = 200.0

# ---------- 航向 PID ----------
KP, KI, KD = 1.2, 0.05, 0.35
TAU_PSI = 2.5                # 航向一阶响应时间常数 s

def wrap(a):
    return (a + math.pi) % (2 * math.pi) - math.pi

def seg_projection(px, py, a, b):
    """返回船位在航段 a->b 上的前向距离 s 与横向偏差 e（左正）"""
    ax, ay = a; bx, by = b
    dx, dy = bx - ax, by - ay
    L = math.hypot(dx, dy)
    ux, uy = dx / L, dy / L
    s = (px - ax) * ux + (py - ay) * uy
    e = -(px - ax) * uy + (py - ay) * ux
    return s, e, L

def run(r_switch):
    x, y, psi = START
    k = 1
    integ, prev_err = 0.0, 0.0
    t = 0.0
    switches = 0
    max_cross = 0.0
    traj = [(x, y)]
    while t < T_MAX and k < len(WAYPOINTS):
        tx, ty = WAYPOINTS[k]
        d = math.hypot(tx - x, ty - y)
        s, e, L = seg_projection(x, y, WAYPOINTS[k-1], WAYPOINTS[k])
        max_cross = max(max_cross, abs(e))
        # 到点判定：半径判据 或 投影越界
        if d <= r_switch or s > L:
            k += 1
            switches += 1
            integ, prev_err = 0.0, 0.0   # 关键：切换时重置积分
            continue
        # LOS 制导：朝目标点方向，叠加横向偏差修正
        psi_los = math.atan2(ty - y, tx - x) - math.atan2(e, LOOKAHEAD)
        err = wrap(psi_los - psi)
        integ += err * DT
        deriv = (err - prev_err) / DT
        prev_err = err
        psi_cmd = KP * err + KI * integ + KD * deriv
        psi_cmd = max(-0.6, min(0.6, psi_cmd))   # 舵角限幅
        # 一阶航向响应
        psi += (psi_cmd - psi) * DT / TAU_PSI
        psi = wrap(psi)
        x += V * math.cos(psi) * DT
        y += V * math.sin(psi) * DT
        traj.append((x, y))
        t += DT
    return dict(r=r_switch, k=k, switches=switches,
                max_cross=max_cross, t=t, traj=traj)

for r in (3.0, 8.0, 15.0):
    res = run(r)
    print(f"R_switch={res['r']:>4.1f} m | 到达航路点序号={res['k']} | "
          f"切换次数={res['switches']} | 最大横向偏差={res['max_cross']:.2f} m | "
          f"用时={res['t']:.1f} s")

示例输出（由上述模型直接推出，非实测）：

注意：三种半径下切换次数相同（都是 4 次），差异体现在切换发生的位置——大半径在离航路点更远处就切换，因此拐角被“提前转向”抹平。这正是平滑性与跟踪误差的权衡点。

结果判读与参数选择

从图中可直接读出三条判读规则：

1. 看拐角切角量：R=15 m 的航迹在 P1、P2、P3 处明显内切，最大横向偏差约为半径的一半量级；若任务要求沿航道边界航行（如狭窄水域），应把 R_switch 压到 3~5 m。
2. 看航向抖动：R=3 m 时船在航路点附近才转向，航向指令变化率大，舵角易饱和；若舵机响应慢，应适当增大半径或增大 LOS 前视距离。
3. 看是否漏点：若某航段很短（L < R_switch），船可能一进入切换圆就同时满足下一点判据，导致“跳点”。本例用投影越界判据兜底，但更稳妥的做法是令 R_switch ≤ 0.5 × min(相邻航段长度)。