3. 船舶运动学约束建模与参数标定

约束建模对象与标定思路

本节针对一艘假定的内河集装箱船（船长 L=98 m，设计航速 12 kn）建立运动学约束模型。约束模型不追求水动力精度，而是为后续航迹优化提供三类硬边界：最小转弯半径、最大转向角速度、加减速限制。这三类边界共同决定优化器生成的航迹是否可被该船实际跟随。

建模采用“机理初值 + 实测标定”的两步法：先用操纵性经验公式给出参数初值，再用一段实测航迹反算实际能力，最后用标定后的参数检验约束是否与实测行为一致。

三类约束的数学表达

设航迹离散点为 (x_i, y_i)，航速为 v_i，航向为 ψ_i。约束模型写成优化器可直接调用的形式：

• 最小转弯半径：由三点曲率 κ_i 反算 R_i = 1/|κ_i|，要求 R_i ≥ R_min。等价于曲率上限 |κ_i| ≤ 1/R_min。
• 最大转向角速度：ω_i = |ψ_{i+1} − ψ_i| / Δt，要求 ω_i ≤ ω_max。注意 ω_max 与航速相关，低速时转向能力更强，因此更严格的形式是 ω_max(v) = min(ω_0, v/R_min)。
• 加减速限制：a_i = (v_{i+1} − v_i)/Δt，要求 −d_max ≤ a_i ≤ a_max。

三者并非独立：转弯半径与转向角速度通过 ω = v/R 耦合。若优化器同时约束 R_min 和 ω_max，在高速段 ω_max 往往先起作用，低速段则由 R_min 主导。

用实测航迹标定参数

标定的核心是从实测航迹中提取每点的曲率、转向角速度和加速度，再取稳健分位数作为能力上限。以下代码完成从航迹到标定参数的完整流程，输入为清洗后的航迹点序列（时间、经度、纬度、对地航速）。

import numpy as np

# 教学示例：假设已从AIS清洗得到航迹点
# t: 秒; lon/lat: 度; sog: 节
t = np.array([0, 30, 60, 90, 120, 150, 180, 210, 240, 270, 300], dtype=float)
lon = np.array([121.500, 121.503, 121.506, 121.509, 121.511,
                121.512, 121.512, 121.511, 121.509, 121.506, 121.503])
lat = np.array([31.200, 31.201, 31.202, 31.203, 31.205,
                31.207, 31.209, 31.211, 31.212, 31.213, 31.213])
sog = np.array([12.0, 12.0, 11.8, 11.5, 11.0,
                10.5, 10.2, 10.0, 10.0, 10.2, 10.5])

# 1) 经纬度转局部平面坐标（小范围近似，米）
R_earth = 6371000.0
lat0 = np.deg2rad(lat.mean())
x = np.deg2rad(lon - lon[0]) * R_earth * np.cos(lat0)
y = np.deg2rad(lat - lat[0]) * R_earth

# 2) 航向与航速（m/s）
v = sog * 0.514444
psi = np.arctan2(np.gradient(y), np.gradient(x))  # 弧度
psi_unwrap = np.unwrap(psi)

# 3) 曲率（三点法）
dx = np.gradient(x); dy = np.gradient(y)
ddx = np.gradient(dx); ddy = np.gradient(dy)
kappa = np.abs(dx * ddy - dy * ddx) / (dx**2 + dy**2)**1.5
R = 1.0 / np.maximum(kappa, 1e-9)

# 4) 转向角速度（deg/s）与加速度（m/s^2）
dt = np.gradient(t)
omega = np.abs(np.gradient(psi_unwrap)) / dt * 180.0 / np.pi
acc = np.gradient(v) / dt

# 5) 稳健标定：取P95作为能力上限，剔除首尾边界效应
R_min_cal = np.percentile(R[2:-2], 5)      # 半径取小分位
omega_max_cal = np.percentile(omega[2:-2], 95)
a_max_cal = np.percentile(acc[2:-2], 95)
d_max_cal = -np.percentile(acc[2:-2], 5)

print(f"R_min  = {R_min_cal:.1f} m")
print(f"omega_max = {omega_max_cal:.3f} deg/s")
print(f"a_max  = {a_max_cal:.3f} m/s^2")
print(f"d_max  = {d_max_cal:.3f} m/s^2")

约束合理性验证

标定完成后必须回答两个问题：约束是否覆盖了实测行为，以及约束是否过紧。验证方法如下：

1. 覆盖率检验：统计实测航迹中满足全部约束的点的比例。合理结果应接近 100%（允许 P95 以外的少量点越界）。若覆盖率明显低于 95%，说明标定参数偏紧或航迹含异常。
2. 紧度检验：用标定参数跑一次简单优化（如最短路径 + 约束），若优化器频繁报“无可行解”，说明约束过紧；若优化结果与无约束结果几乎一致，说明约束过松、未起作用。
3. 物理一致性：检查 ω_max · R_min 是否与设计航速匹配。若 ω_max · R_min 远大于 v，说明两个约束中有一个明显偏松。

交付物：约束参数标定表

将标定结果整理为优化器可直接读取的参数表，字段如下，可直接填入目标船数据后使用：

完成标定后，后续航迹优化器在生成候选航段时，只需对每个航段检查 R ≥ R_min、|ω| ≤ ω_max、a ∈ [−d_max, a_max] 三条不等式，即可保证输出航迹在运动学上可被目标船跟随。若某航段越界，优化器应调整该段的曲率或速度剖面，而不是简单删除该段。