简化舰船动力学模型¶
该模型为适配“任务级路径/兵棋推演”应用的二维模型：纯运动学 + 舵向一阶，配LOS 导引、转弯半径约束与航速限幅。符号采取右手系，艏向为 \(+x\)，偏航角 \(\psi\) 顺时针为正（可按需要改成逆时针正）。
1. 状态、输入与常量¶
- 状态：\(\eta=[x,\ y,\ \psi]^T\)（位置与艏向），\(v\)（航速，可恒速或一阶）
- 控制输入：期望艏向 \(\psi_d\)（由LOS产生），或直接给舵向指令的等效
- 执行器（艏向一阶）：时间常数 \(T_\psi\)、艏向角速率限幅 \(|\dot\psi|\le r_{\max}\)
- 航速限幅：\(v\in[v_{\min},,v_{\max}]\)（可选一阶速度环）
- 最小转弯半径：\(R_{\min}\)（由操纵品质/场景设定）
可选外扰
- 恒定/缓变海流 \(\boldsymbol{v}_c=[u_c,\ v_c]^T\)（地速 = 水中速度 + 海流）
- 噪声项（过程/量测）留给滤波器处理
2. 运动学与艏向一阶¶
连续时间
\[
\begin{aligned}
& \dot{x}=v \cos \psi+u_c, \\
& \dot{y}=v \sin \psi+v_c, \\
& \dot{\psi}=\operatorname{sat}\left(-\frac{\psi-\psi_d}{T_\psi}, r_{\max }\right),
\end{aligned}
\]
其中 \(r_{\max}=\dfrac{v}{R_{\min}}\)（转弯半径约束转化为角速率上限）。 若设恒速 \(v=v_0\)，则忽略 \(v\) 的动态；若需要速度环，可用：
\[
\dot v = \operatorname{sat}\left(-\frac{v-v_d}{T_v},\ a_{\max}\right),\qquad v\in[v_{\min},v_{\max}],
\]
其中 \(v_d\) 为期望航速（可按航段限速或情景切换给定）。
离散时间（Euler, 步长 \(\Delta t\)）
\[
\begin{aligned}
& \psi_{k+1}=\psi_k+\operatorname{sat}\left(-\frac{\operatorname{wrap}\left(\psi_k-\psi_{d, k}\right)}{T_\psi}, r_{\max , k}\right) \Delta t, \\
& x_{k+1}=x_k+\left(v_k \cos \psi_k+u_c\right) \Delta t, \\
& y_{k+1}=y_k+\left(v_k \sin \psi_k+v_c\right) \Delta t, \\
& v_{k+1}=\operatorname{clip}\left(v_k+\operatorname{sat}\left(-\frac{v_k-v_{d, k}}{T_v}, a_{\max }\right) \Delta t, v_{\min }, v_{\max }\right),
\end{aligned}
\]
\[
r_{\max,k}=\frac{v_k}{R_{\min}},\quad \operatorname{wrap}(\cdot)\in(-\pi,\pi]
\]
说明
- \(sat(z, zmax)=\operatorname{sign}(z)\min(|z|, zmax)\)；
clip为上下限裁剪。wrap保证角度误差取主值域，避免 \(2\pi\) 跳变。- 若不建速度环，直接令 \(v_{k+1}=v_k=v_0\)。
3. LOS 导引（航段—航点切换）¶
给定当前航段两个航点 \(W_i=(x_i,y_i),\ W_{i+1}=(x_{i+1},y_{i+1})\)，定义：
\[
\boldsymbol{t}=\frac{W_{i+1}-W_i}{|W_{i+1}-W_i|},\quad
\boldsymbol{p}=(x,y)^T,\quad
\boldsymbol{e}=\boldsymbol{p}-W_i
\]
沿线投影与横向误差
\[
s=\boldsymbol{e}\cdot \boldsymbol{t},\qquad
\boldsymbol{p}_\text{proj}=W_i + s,\boldsymbol{t},\qquad
\boldsymbol{n}=\begin{bmatrix}-t_y\ t_x\end{bmatrix},\qquad
e_\perp=\boldsymbol{e}\cdot\boldsymbol{n}
\]
前视点（lookahead） 距离 \(\Delta_\text{LOS}>0\)：
\[
\boldsymbol{p}_{\mathrm{LA}}=\boldsymbol{p}_{\mathrm{proj}}+\Delta_{\mathrm{LOS}} \boldsymbol{t}
\]
期望艏向（LOS航向）
\[
\psi_d=\operatorname{atan} 2\left(\left(y_{\mathrm{LA}}-y\right),\left(x_{\mathrm{LA}}-x\right)\right)+\beta\left(e_{\perp}\right)
\]
其中 \(\beta(e_\perp)\) 为可选的偏置项，用于交叉航迹误差收敛（无侧滑假设下常取 0；若希望更快贴线，可取小角：\(\beta=\arctan(k_e e_\perp)\)，\(k_e>0\)）。
航点切换规则（任取其一或并用）
- 基于投影：当 \(s\ge |W_{i+1}-W_i|\)，切换至下一段；
- 基于邻域：当 \(|\boldsymbol{p}-W_{i+1}|\le R_\text{sw}\)（切换半径），切换；
- 基于视线：当前视点越过 \(W_{i+1}\) 时切换（避免尖角）。
参数建议
- \(\Delta_\text{LOS}\) 一般取 \(2!~\sim!5\) 个船长或按期望转弯半径调度：\(\Delta_\text{LOS}\approx c\cdot R_{\min}\)，\(c\in[0.5,1.5]\)。
- 若设 \(\beta=\arctan(k_e e_\perp)\)，则 \(k_e \in [0.01,,0.2]\ \text{m}^{-1}\) 量级（视尺度而定）。
4. 转弯半径与角速率约束的一致性¶
由几何关系 \(R=\dfrac{v}{|\dot\psi|}\)，限制 \(R\ge R_{\min}\Rightarrow |\dot\psi|\le r_{\max}=\dfrac{v}{R_{\min}}\)。 因此在艏向一阶环上做速率饱和即可自动满足最小转弯半径。若 \(v\) 很小（靠泊/低速），建议设置 \(r_{\max}\le r_{\max}^{\text{abs}}\) 的绝对上限，避免数值过大。
5. 简易速度调度（可选）¶
- 恒速：\(v=v_0\)（兵棋/任务级常用）。
- 航段限速：靠近拐点或受限水域时降低 \(v_d\)；出段恢复巡航。
- 曲率感知：提前按目标航段的折角或样条曲率调小 \(v_d\)，减小 \(R_{\min}\) 需求。
6. 伪代码（单步更新）¶
Given: waypoints {W_i}, params (Δt, Tψ, Rmin, v_min/max, ΔLOS, ke, Tv, amax, vc=(uc,vc))
1) Determine active leg i → (Wi, Wi+1)
2) Compute t, n, s, p_proj, p_LA = p_proj + ΔLOS * t
3) e_perp = (p - Wi)·n
4) ψ_d = atan2(p_LA.y - y, p_LA.x - x) + atan(ke * e_perp) // ke=0 → 无偏置
5) r_max = min(v / Rmin, r_abs_max)
6) ψ̇ = sat( -wrap(ψ - ψ_d)/Tψ , r_max )
7) ψ ← ψ + ψ̇ Δt
8) if speed loop:
v̇ = sat( -(v - v_d)/Tv , amax )
v ← clip(v + v̇ Δt, v_min, v_max)
9) x ← x + (v cosψ + uc) Δt
y ← y + (v sinψ + vc) Δt
10) Switch leg if (s ≥ |Wi+1 - Wi|) or (‖p - Wi+1‖ ≤ R_sw)
7. 典型参数初值（示例）¶
- 巡航航速：\(v_0=6\ \text{m/s}\)（≈11.7节）；\(v_{\min}=0\)，\(v_{\max}=8\)
- 艏向一阶：\(T_\psi=15\ \text{s}\)；\(r_{\max}^{\text{abs}}=3^\circ/\text{s}\)
- 最小转弯半径：\(R_{\min}=150\ \text{m}\)（→ \(r_{\max}!=!v/R_{\min}\approx 0.04\ \text{rad/s}\)）
- LOS：\(\Delta_{\text{LOS}}=100\ \text{m}\)；误差增益 \(k_e=0.03\ \text{m}^{-1}\)；切换半径 \(R_{\text{sw}}=50\ \text{m}\)
- 速度环（可选）：\(T_v=20\ \text{s},\ a_{\max}=0.2\ \text{m/s}^2\)
- 海流：\((u_c,v_c)\) 依据场景给定（无流时取 0）
这套模型能以极低计算开销支撑路径跟随、编队路径复现、兵棋级态势演示。