欠驱动船舶路径跟踪仿真复现：双曲LOS制导与有限时间控制

说实话，刚开始拿到这篇IEEE Trans.期刊论文时，我第一反应是“这个能复现吗”。文中公式一带而过，参数表只给了模型系数，仿真条件靠猜，主图上的收敛曲线漂亮得让人心里打鼓。但等我把制导、控制、仿真三层逻辑拆开之后，发现这条技术路线其实非常清晰：欠驱动船舶，只有螺旋桨和舵两个输入，要控制它在平面里走出一条期望路径，靠的是双曲-切线LOS制导生成参考航向，再用有限时间控制器把航向和速度收敛到期望值。中间断断续续花了两周做仿真复现、调参、对图，最终在MATLAB/Simulink里把整条链路跑通，直线和曲线路径跟踪的效果都能和论文主图对上。

这篇文章我就围绕复现过程写，把三个核心概念、仿真模型搭建、Simulink实现细节、容易踩的坑一次说清楚。正在做船舶运动控制、制导律设计，或者纯粹想学习论文复现的人，应该都能从中找到能直接复制到你自己项目里的内容。整个工程的落脚点是仿真复现，所以我会把重点放在“如何把论文公式变成能跑的模型”上，而不是停留在理论推导。

1. 复现前必须搞懂的三件事

复现论文最忌讳上来就敲代码。特别是控制领域，机理没想透，仿真模型就是一堆漂移的发散曲线。我先把论文背后的三个基础问题拆开讲，把基础打牢，后面调参时你才知道哪个环节出了问题。

1.1 欠驱动船舶动力学：为什么只有两个输入还敢叫“路径跟踪”

“欠驱动”不是说船不听话，而是说自由度数多于控制输入数。水面船舶的平面运动有三个自由度：纵荡（surge）、横荡（sway）、艏摇（yaw）。常规配置下，推进器管纵向力，舵管转艏力矩，横向（sway）方向没有任何直接执行器。这就是问题的核心：要控制三个自由度，却只有两个输入。换句话说，船不能像全驱动系统那样“横着走”，只能在前进中通过转艏来间接控制横向运动。

动力学模型用经典的三自由度形式表示。运动学方程是：

[
\dot{x} = u \cos\psi - v \sin\psi
]
[
\dot{y} = u \sin\psi + v \cos\psi
]
[
\dot{\psi} = r
]

动力学方程是：

[
m_{11}\dot{u} = m_{22} v r - d_{11} u + \tau_u
]
[
m_{22}\dot{v} = -m_{11} u r - d_{22} v
]
[
m_{33}\dot{r} = (m_{11} - m_{22}) u v - d_{33} r + \tau_r
]

这里 (u, v, r) 分别是纵荡速度、横荡速度、艏摇角速度；(\tau_u) 是推进力，(\tau_r) 是转艏力矩；(m_{ii}) 是包含附加质量的惯性系数，(d_{ii}) 是阻尼系数。注意第二个方程，横荡速度 (v) 的输入为零，它是被 (u r) 项“带”着走的。仿真的成败，很大程度取决于对这个方程的理解——控制器设计绕不开它，Simulink建模更是直接把方程组搬进模块。

这也是为什么很多新手复现出来，船不走直线、跟丢路径甚至原地打转，就是因为没有把耦合项 ((m_{11} - m_{22})uv) 和 (m_{22}vr) 处理正确。后面我会专门讲怎么验算模型是否写对。

1.2 双曲-切线LOS制导：从“看得见”到“走得好”

路径跟踪里的“制导律”决定了船该朝哪个方向走。传统视线法（Line-of-Sight, LOS）的思路很简单：选一个前视点，让船头指向它。想象你在海边走路，远处有一个灯塔，你只要一直朝着灯塔走，就能慢慢靠近目标航线。标准LOS的期望航向公式是：

[
\psi_d = \psi_p + \arctan\left(-\frac{e}{\Delta}\right)
]

其中 (\psi_p) 是路径切线角，(e) 是横向偏差，(\Delta) 是前视距离。(e) 越大，校正角越大；(\Delta) 越大，校正越平缓。这个公式很直观：船偏离路径越远，就要把船头转得越猛。

双曲正切LOS是标准LOS的一个改进：把横向误差先经过 (\tanh) 函数做有界映射，再代入制导公式：

[
\psi_d = \psi_p + \arctan\left(-\frac{k_t \cdot \tanh(e)}{\Delta}\right)
]

为什么这么改？因为标准LOS的校正角没有上界，船离路径很远时，(\arctan) 算出来的角度会很大，对舵效和控制力要求极高，甚至激发非线性振荡。加上 (\tanh) 之后，无论横向偏差多大，校正角都被限制在一个有限范围内。这个思路在工程上特别有用：论文里写的是有界制导律，实际效果就是船在初始偏差很大时也能平稳地回归路径，不会产生剧烈的机动。

我在仿真里对比过标准LOS和双曲正切LOS：初始横向偏差5米，标准LOS一上来给的期望航向变化率非常大，舵角直接顶到饱和；双曲正切版本则让船以更平滑的弧线回归路径。两者最终都能收敛，但控制力和收敛过程中的舒适度完全不同。

1.3 有限时间控制与渐近控制的本质区别

传统控制律大多保证“渐近稳定”，意思是误差随时间趋于零，但理论上要无限长时间才能完全收敛。有限时间控制则保证在某个有限时刻 (T) 之前误差严格归零。虽然实际仿真的收敛时间总有个“差不多”，但两者在收敛速度、抗扰性能上的差异是实打实的。

怎么实现有限时间收敛？一个核心手段是引入分数幂次。看这个标量系统：

[
\dot{x} = -k |x|^{\alpha} \mathrm{sign}(x), \quad 0<\alpha<1
]

当 (x) 远离零时，(|x|^{\alpha}) 比 (|x|) 收敛更快；当 (x) 接近零时，虽然 (|x|^{\alpha}) 变小，但幂次小于1意味着它比 (|x|) “减少得更慢”，系统能以更快速度到达零点。这就是有限时间收敛的直观机理。

论文里的控制器设计在此基础上引入了终端滑模面。先通过制导律生成期望航向 (\psi_d)，再定义航向误差 (\psi_e = \psi - \psi_d)，并构造滑模面：

[
s_r = r - r_d, \quad r_d = \dot{\psi}d - k\psi |\psi_e|^{\alpha} \mathrm{sign}(\psi_e)
]

当 (s_r = 0) 时，有

[
\dot{\psi}e = -k\psi |\psi_e|^{\alpha} \mathrm{sign}(\psi_e)
]

这正好是有限时间收敛的标准形式。然后对 (s_r) 再设计更高层的控制律，通常也会用分数幂次和符号函数项。这样整个航向误差闭环就是有限时间稳定。

从复现的角度，这一步最大的价值是：你不需要理解论文里全部稳定性证明，只需把这几条带指数和符号函数的表达式精确抄到代码里，控制器就已经完成了一半。剩下的就是参数整定和仿真调试。

2. 整体仿真方案设计与工具准备

理解了原理，下面就要落地。第一个问题是：用什么工具复现？我选的是MATLAB/Simulink，这也是论文仿真部分最常用的环境。原因非常实际：控制律里全是微分方程和代数运算，Simulink的框图方式直观，改参数方便，而且能直接出图对比论文曲线。如果有更复杂的场景需要做硬件在环，也可以直接复用这套模型。

2.1 工具选型：为什么绕不开MATLAB/Simulink

有人说“我用Python也能复现”。可以，但复现控制论文时，Simulink有几个先天优势。第一是基于时间步长的仿真环境天然适合状态空间模型，不需要自己写积分器；第二是内置的S函数、MATLAB Function模块能无缝嵌入自定义控制律；第三是后面如果要加扰动、加海洋环境模型，模块库直接拖。Python做数据处理和画图更灵活，但要把微分方程、控制律、离散采样、防抖振模块全部手搓一遍，效率低不少。

如果你的电脑没装Simulink，退而求其次用纯MATLAB脚本也能跑通，本质就是自己写RK4积分器。我在上面的实现环节会同时给出模型方程和Simulink模块接线思路，用脚本或框图都能复现。

2.2 仿真架构与模块划分

整个仿真链路分四层，每层都有独立输入输出，调试时可以单独验证：

模块划分清楚后，调试时你可以单独验证每一层。比如先给制导模块一个固定位置输入，看它输出的 (\psi_d) 是否符合公式；再给控制器一个固定的误差，看输出的控制力是否符合预期。一层一层打通，比把整套系统全接好再去debug要高效得多。

2.3 关键参数与设定

我仿真用的参数综合自论文常见设定与标准欠驱动船型数据，可以直接作为初值使用：

注意 (\alpha) 和 (\rho) 都取0到1之间。它们要过小，收敛过程会有明显的抖动；过大又失去有限时间优势。论文里一般只给 (0<\alpha<1) 这种不等式，具体数值需要自己仿真调整。我经历了大约20轮调参才找到一组在收敛速度和控制力幅值之间比较均衡的值。后面的“调参经验”部分会详细说。

3. 从零搭建仿真模型

这里开始进入实操。我按模块顺序讲，先搭船舶动力学，再做制导，最后接控制器。每个模块我给出Simulink搭建要点和对应的数学模型，也会给出可以直接复制的MATLAB代码片段。

3.1 船舶动力学模型搭建

在Simulink里，建议用积分器（Integrator）模块串联搭建状态方程，不要用传递函数，因为系统是强非线性的。状态变量取：

[
\mathbf{X} = [x, y, \psi, u, v, r]^T
]

搭建顺序是“从速度到位置”：

1. 先把 (u, v, r) 作为状态，计算动力学方程右侧：
   [
   \dot{u} = \frac{m_{22} v r - d_{11} u + \tau_u}{m_{11}}
   ]
   [
   \dot{v} = \frac{-m_{11} u r - d_{22} v}{m_{22}}
   ]
   [
   \dot{r} = \frac{(m_{11} - m_{22})uv - d_{33} r + \tau_r}{m_{33}}
   ]

2. 三个积分器分别得到 (u, v, r)。

3. 再根据运动学方程，用 (u, v, \psi) 计算：
   [
   \dot{x} = u\cos\psi - v\sin\psi
   ]
   [
   \dot{y} = u\sin\psi + v\cos\psi
   ]
   [
   \dot{\psi} = r
   ]

4. 两个积分器得到 (x, y)，一个积分器得到 (\psi)。

搭建完成后，先用开环测试验证：给 (\tau_u = 5)，(\tau_r = 0)，船应该直线前进，纵向速度 (u) 趋向某个稳定值；再给 (\tau_r) 一个阶跃，船应该原地转向。如果这两个基本动态都不正常，后面所有结果都不可信。

注意：若从零初始速度启动，(\dot{v} = 0)，横荡速度不会自己变。这是正常现象；等加上转艏力矩后，由于横荡耦合项 (-m_{11}ur/m_{22}) 的存在，(v) 才会被激励起来。

3.2 期望路径生成模块

路径跟踪不是轨迹跟踪，它不要求船在特定时间到达特定点，只要求船最终走在几何路径上。因此路径通常用参数 (\theta) 描述：

直线路径：

[
x_p(\theta) = \theta, \quad y_p(\theta) = 0
]

曲线路径（正弦路径）：

[
x_p(\theta) = \theta, \quad y_p(\theta) = A \sin(\omega \theta)
]

路径切线角：

[
\psi_p = \arctan\left(\frac{y_p'(\theta)}{x_p'(\theta)}\right)
]

在Simulink里，(x_p, y_p, \psi_p) 都可以用MATLAB Function模块直接写成表达式。路径参数 (\theta) 我直接取为船舶纵荡方向的投影长度，也可以用一个低速积分器生成，代表路径点在向前移动。简单起见，我采用给定常数前进速度的路径参数更新方式，即：

[
\dot{\theta} = U > 0
]

这样船既能跟踪路径，又不会出现路径参数停滞导致的奇异性。

3.3 双曲-切线LOS制导律实现

制导模块的核心是计算横向偏差 (e) 和期望航向 (\psi_d)。

在路径坐标系中，先把船舶位置变换到路径局部坐标系：

[
x_e = (x - x_p)\cos\psi_p + (y - y_p)\sin\psi_p
]
[
e = -(x - x_p)\sin\psi_p + (y - y_p)\cos\psi_p
]

然后计算期望航向：

[
\psi_d = \psi_p + \arctan\left(-\frac{k_t \tanh(e)}{\Delta}\right)
]

这段在Simulink里用MATLAB Function最方便，输入是 (x, y, x_p, y_p, \psi_p)，输出是 (\psi_d) 和 (e)。实际代码是这样：

matlab复制function [psi_d, e] = los_guidance(x, y, xp, yp, psi_p)
    Delta = 3.0;
    k_t = 1.5;
    
    dx = x - xp;
    dy = y - yp;
    
    % 横向跟踪误差
    e = -dx * sin(psi_p) + dy * cos(psi_p);
    
    % 双曲正切LOS制导
    psi_d = psi_p + atan2(-k_t * tanh(e), Delta);
    
    % 角度归一化到[-pi, pi]
    psi_d = mod(psi_d + pi, 2*pi) - pi;
end

需要注意，(atan) 要用 (atan2) 或直接做四象限反正切，否则航向在±180°附近跳变时仿真会出现尖刺。我复现时在这里踩了一个大坑：一开始用了单参数 (\arctan)，结果船在跨越路径垂线时，(\psi_d) 突然跳变180°，导致舵角瞬间饱和，整个路径跟踪振荡。改成四象限反正切并做好角度归一化后，问题立刻消失。

3.4 有限时间控制器实现

控制器分成纵荡和艏摇两个通道，但核心思想一致：用分数幂次误差加上符号函数，实现有限时间收敛。

纵荡速度控制目标是让 (u \to u_d)。定义速度误差：

[
u_e = u - u_d
]

控制律：

[
\tau_u = m_{22} v r - d_{11} u + m_{11}\dot{u}d - m\left(k_{u1} |u_e|^{\alpha_u}\mathrm{sign}(u_e) + k_{u2} |u_e|^{\beta_u}\mathrm{sign}(u_e)\right)
]

其中 (0 < \alpha_u < 1)，(\beta_u > 1)。前两项是模型补偿，让控制器不依赖缓慢的误差积分；后两项是有限时间反馈项。(k_{u2}) 那项用大于1的指数，是为了避免系统状态远离零时收敛速度不足。

艏摇控制需要分两步。先根据航向误差 (\psi_e = \psi - \psi_d) 设计虚拟角速度：

[
r_d = \dot{\psi}d - k\psi |\psi_e|^{\alpha}\mathrm{sign}(\psi_e)
]

再定义滑模变量：

[
s_r = r - r_d
]

则艏摇控制律：

[
\tau_r = (m_{11} - m_{22})uv - d_{33} r + m_{33}\dot{r}d - m\left(k_{r1} |s_r|^{\rho}\mathrm{sign}(s_r) + k_{r2} s_r\right)
]

MATLAB Function实现如下：

matlab复制function [tau_u, tau_r] = finite_time_controller(state, psi_d, psi_dot_d, u_d, u_dot_d)
    x = state(1); y = state(2); psi = state(3);
    u = state(4); v = state(5); r = state(6);
    
    % 纵荡控制
    u_e = u - u_d;
    tau_u = m22 * v * r - d11 * u + m11 * u_dot_d ...
            - m11 * (ku1 * abs(u_e)^alpha_u * sign(u_e) + ku2 * abs(u_e)^beta_u * sign(u_e));
    
    % 艏摇控制
    psi_e = mod(psi - psi_d + pi, 2*pi) - pi;
    r_d = psi_dot_d - k_psi * abs(psi_e)^alpha * sign(psi_e);
    s_r = r - r_d;
    
    tau_r = (m11 - m22) * u * v - d33 * r + m33 * r_dot_d ...
            - m33 * (kr1 * abs(s_r)^rho * sign(s_r) + kr2 * s_r);
end

这两条控制律在MATLAB Function里写出来不超过30行。但实现细节有几个坑：

• (\dot{\psi}_d) 和 (\dot{r}_d) 需要用滤波器或数值微分得到。(\psi_d) 本身是分段光滑函数，直接差分会产生大量噪声。我用一阶低通滤波器：

  [
  \dot{\psi}_{d,f} = \frac{s}{\tau_p s + 1} \psi_d
  ]

  在Simulink里就是两个模块：一个求导（Derivative）加一个滤波（Transfer Fcn）。虽然论文里写的是解析求导，实际工程中滤波微分更稳。

• 符号函数 (\mathrm{sign}(x)) 在高频切换时会造成抖振。仿真中我用饱和函数替代：

  [
  \mathrm{sat}(x) =
  \begin{cases}
  \mathrm{sign}(x), & |x| > \epsilon \
  x / \epsilon, & |x| \le \epsilon
  \end{cases}
  ]

  (\epsilon) 取0.01。这样既能保留符号函数的有界性，又避免了死区附近控制力的剧烈抖动。

4. 仿真结果复现与分析

跑通仿真后，最重要的环节来了：把结果和论文主图对比，判断复现是否成功。这一步需要耐心，因为论文不会把所有实验细节都给你，很多条件要自己推断。

4.1 直线路径跟踪结果

第一个实验是直线路径 (y