全驱动船舶轨迹跟踪：扰动观测器与动态面滑模控制设计

简介：面向三自由度全驱动船舶的轨迹跟踪控制问题，资源提供一套带扰动观测器的自适应动态面滑模控制方案，适合研究船舶运动控制、非线性鲁棒控制的研究生或工程师。方案通过扰动观测器前馈补偿未知环境扰动，结合σ修正自适应律处理观测误差界，并利用动态面技术避免反演法微分爆炸，从理论上保证闭环系统信号的最终有界。资源内含完整仿真代码与模型，覆盖圆轨迹、直线轨迹等典型工况，基于供给船舶的仿真可复现未知扰动估计、参数自适应和滑模控制等关键算法。压缩包共21个文件，其中15个m为算法主体、2个mdl为Simulink模型、2个txt为辅助说明，另有论文PDF及r2012a格式数据，整体仅1.27MB，结构清晰。该资源已有431人学习/下载，对船舶轨迹跟踪控制器设计与算法对比具有实用参考价值。

1. 全驱动船舶轨迹跟踪控制里，扰动观测器补上了什么

全驱动船舶在锚泊定位、靠离泊和近海作业时，需要让纵向、横向、艏摇三个自由度同时跟踪期望轨迹，但风、浪、流合起来的低频扰动会持续注入船体，位置误差刚被压下去又被推回来。传统滑模控制用大切换增益硬扛扰动，跟踪误差确实能压住，控制力却抖得厉害，执行机构磨损严重。张晓玲这篇文章的思路是把“抗扰动”拆成两段：扰动观测器先估计出等效扰动并做前馈抵消，滑模项只负责对付观测器没跟上残差，切换增益因此可以选得比较小，抖振被边缘化。资源包里除了论文 PDF，还带了 MATLAB/Simulink 仿真程序，内置直线和圆轨迹两组工况，适合做船舶运动控制、非线性鲁棒控制的研究生或工程师直接复现，也可以作为模板把扰动观测器、动态面滤波、σ修正自适应律组合进自己的控制器里。

2. 三自由度模型、动态面滤波与滑模控制律的推导链路

2.1 模型怎么建：运动学与动力学结构

先统一符号。船舶在北东坐标系下的位置和艏向写成 η=[x,y,ψ]ᵀ，船体坐标系下的速度写成 ν=[u,v,r]ᵀ，分别是纵荡速度、横荡速度和艏摇角速度。运动学关系是：

η̇ = R(ψ)ν

其中 R(ψ) 是旋转矩阵：

R(ψ) = [cos(ψ), -sin(ψ), 0; sin(ψ), cos(ψ), 0; 0, 0, 1]

这里 R(ψ) 是正交阵，满足 RᵀR=I，Rᵀ(ψ)=R⁻¹(ψ)，后面推导控制律时经常要用这个性质。

三自由度动力学方程写成标准形式：

Mν̇ + C(ν)ν + D(ν)ν = τ + τ_d

M 是惯性矩阵（含附加质量），C(ν) 是科氏力矩阵，D(ν) 是阻尼矩阵，τ 是推进器控制力/力矩，τ_d 是外界环境扰动力。做控制器设计时有三个结构性质可以直接用：M 对称正定，C(ν) 斜对称（即满足 xᵀCx=0），D(ν) 正定。这三个性质是后面李雅普诺夫证明中消项的基础，仿真的 plant 模块也要确保 M、C、D 满足这些约束，不要随便填矩阵。

全驱动是什么意思？意思是纵向、横向、艏摇三个自由度都有独立推进器或舵机组合来驱动，控制输入 τ∈ℝ³ 可以独立设计。这一点直接把问题从欠驱动控制里摘出来，后面用的反演法、动态面、滑模面都是针对全驱动形式的。资源包里的供给船仿真参数大概是这个量级：

实际仿真里换成你自己的船型参数就行，量级保持在这个范围内，控制器对参数变化有一定的鲁棒性。

2.2 从跟踪误差到滑模面

位置跟踪误差定义为 e=η-η_d，η_d 是期望轨迹。传统反演法第一步把 e 的导数展开：ė=R(ψ)ν-η̇_d。如果直接把 R(ψ)ν 虚拟控制 α_ν 取成 η̇_d-Λe，那么 e 的误差动态就是指数收敛，Λ 是正定对角矩阵，决定误差收敛带宽。

这一步需要坐标系变换操作。滑模面按一阶形式取：

s = ė + Λe

s 收敛到零之后，e 的动态由 ė=-Λe 决定，误差单调收敛且不人为引入超调。这里 Λ 的选取不能太大，否则期望速度 Rᵀ(η̇_d-Λe) 会超过推进器能力范围。工程上我一般按期望跟踪误差时间常数的 3-5 倍取 Λ，比如希望误差 10 秒内衰减完，λ 取 0.3 左右。

继续反演就需要对虚拟控制 α_ν 求导。二阶系统里 α_ν 的导数已经包含 η̈_d、Ṙ(ψ)ν 以及 Ṙ 相关的耦合项，到了三阶、四阶系统，项数成倍增加，这就是传统反演法的“微分爆炸”问题。每多一层状态，公式翻倍，代码实现极易出错。

2.3 动态面技术：用一阶滤波器替代求导

动态面技术的核心非常朴素：虚拟控制 α 先过一个一阶低通滤波器，滤波器输出的 α_f 取代 α 直接参与控制律计算，α_f 的导数由滤波器代数式直接给出，不再做解析求导。滤波器形式：

T_f α̇_f + α_f = α, α_f(0) = α(0)

一阶惯性环节，T_f 是滤波时间常数。实现代码很短：

alpha_f_dot = (alpha - alpha_f) / Tf; alpha_f = alpha_f + Ts * alpha_f_dot;

这里 alpha 是当前拍反演出来的虚拟控制，alpha_f 是上一拍滤波输出，alpha_f_dot 是滤波器状态导数。注意第一行用的是上一拍的 alpha_f，第二行才更新当前值，这是离散实现的一步延迟，仿真步长 Ts 足够小时误差可以忽略。

T_f 的物理意义很直接：它限制虚拟控制变化率能有多快。T_f 越大，滤波输出滞后越明显，控制力变化放缓，但误差收敛变慢，极端情况会产生明显相位滞后；T_f 太小，滤波效果消失，动态面退化成普通反演法，微分爆炸问题又回来了。我一般先把 T_f 设定成期望闭环时间常数的 1/10，再根据仿真曲线微调。这个参数是动态面控制器里第一个要整定的量，后面会专门讲调节顺序。

滤波付出的代价是 α_f 和 α 之间存在滤波误差 y=α_f-α。这个误差不会消失，但它会进入李雅普诺夫分析中，最终被动态面参数 T_f 拉进一致最终有界的小界里。这个“有界而非渐近收敛”的性质是整个控制器设计里最难向新手解释的一点，但看到后面 σ修正自适应律和扰动观测误差的影响，就会明白这是工程妥协的必然结果。

2.4 控制律组装与李雅普诺夫证明链路

控制律的最终结构是把运动学虚拟控制、动力学模型补偿、扰动前馈、自适应鲁棒项拼在一起：

τ = Mα̇_f + C(ν)ν + D(ν)ν - d̂ - δ̂·sat(s/ε) - K_s·s

Mα̇_f 是动态面滤波后的期望加速度前馈；Cν+Dν 是模型补偿；-d̂ 由扰动观测器给出的前馈补偿项；δ̂·sat(s/ε) 是自适应滑模项，对付扰动观测残差；K_s·s 是阻尼项，加快收敛。sat(s/ε) 是饱和函数，替代符号函数之后控制量在边界层内连续变化，抖振在这个链路上被压到最低。

闭环系统的稳定性分析可以按李雅普诺夫函数逐项展开：

V = 1/2eᵀe + 1/2sᵀMs + 1/2yᵀy + 1/(2Γ_s)δ̃²

四项分别对应位置误差、滑模面、滤波误差、自适应估计误差。求导后用到 M 对称正定、C 斜对称消项、ε-sat 的性质，最后得到 V̇≤-λ_min·V+C₀ 的不等式形式。λ_min 由控制增益决定，C₀ 是扰动界、滤波时间常数、σ修正项共同贡献的常数。这个不等式说明所有状态被压进一个原点的邻域内，即一致最终有界，而不是渐近稳定到零。谁会关心这个区别？当你看到仿真曲线里的误差不是严格归零而是在厘米级波动时，就知道这是设计本身决定的稳态误差带，不是程序 bug。

3. 扰动观测器与σ修正自适应律：公式到可运行代码

3.1 扰动观测器：把环境力估计出来做前馈

扰动观测器的目标是用可测的速度 ν 和控制输入 τ 反推出 τ_d 的估计值 d̂。工程上最常用的是扩张状态观测器形式，把扰动当作一个慢变扩张状态，用速度估计残差驱动扰动估计。状态方程写成：

ν̂̇ = M⁻¹(τ - Cν - Dν + d̂) + K_d(ν-ν̂) d̂̇ = K_obs·(ν-ν̂)

第一行是速度估计器，第二行是扰动估计律，K_d 是速度估计增益，K_obs 是扰动估计增益。物理直觉：如果速度估计误差 ν-ν̂ 长期为正，说明模型里少了一项推力，扰动估计就往上调。两个增益都取正定对角矩阵。这种结构的优点是不需要对扰动建立任何数学模型，也不需要假设扰动是常数，时变扰动只要变化率不快，观测器就能跟住。

参数选取上有个基本约束：K_obs 对应的扰动估计带宽要低于速度估计环带宽，否则扰动的响应会和速度估计互相耦合，整条估计链路出现振荡。工程上常用的做法是让 K_d 取控制闭环带宽的 5-10 倍，K_obs 取 K_d 的 1/3 到 1/5。仿真里的时变扰动如果频率达到 0.3 rad/s 以上，K_obs 太小会导致扰动估计有明显幅值衰减和相位延迟，需要按扰动频率往上提。

还有一类更紧凑的实现是把扰动观测器写成 d̂=z+K_pν 的降维 Luenberger 形式，z 是辅助状态。这种形式少一个动态方程，但辅助状态 z 的初值如果不和 K_pν 配平，仿真第一拍会产生一个虚假的大扰动估计。资源包源码里若出现 z 变量，优先检查 z(0)+K_pν(0) 是否设成零。

3.2 σ修正泄漏项：防止自适应参数漂移

扰动观测器不是万能的，d̃=d-τ_d-？写成扰动观测误差 d̃=d-d̂ 之后，这个残差仍然是有界的，但界的大小不确定。为了对付残差，引入一个自适应参数 δ̂，用来估计残差界的大小。自适应律写成：

δ̂̇ = Γ_s(‖s‖ᵀ · sat_s - σδ̂)

Γ_s 是自适应增益，σ 是泄漏系数，sat_s 是饱和函数项。这里 σδ̂ 就是σ修正泄漏项。为什么需要它？因为纯积分形式的自适应律在持久激励下会让 δ̂ 无限增大，扰动消失后仍然保持一个极高增益，导致控制量被“记忆”撑大。σ项让 δ̂ 在没有激励时以 e^(-σt) 速度衰减，对应工程里的“遗忘”功能。

σ取太大，δ̂ 会被快速拉回零，鲁棒项失去作用；取太小，泄漏效果不明显。我一般从 σ=0.01 起步，根据自适应参数曲线观察衰减速率再调整。Γ_s 决定估计的响应速度，Γ_s 太大时 δ̂ 会抖动，仿真里还会出现积分器饱和，一般要配合限幅使用。

3.3 控制器完整循环的可运行代码

把动态面滤波、扰动观测器、σ修正自适应律、滑模控制律拼成一个完整控制器函数，是资源包里可以直接替换的核心模块。用 Level-2 MATLAB Function 每步调用一次，或者写成外部函数循环调用都是常见做法：

function [tau, d_hat, delta_hat, alpha_f] = dsc_controller(eta, nu, ref, param, Ts) % 输入 eta=[x;y;psi], nu=[u;v;r], ref为期望轨迹结构体, param为参数结构体 persistent alpha_f_int d_hat_int delta_hat_int if isempty(alpha_f_int) % 关键：滤波器初始状态与虚拟控制初始值一致，避免首拍冲击 alpha_0 = R(eta)' * (ref.eta_d_dot - param.Lambda * (eta - ref.eta_d)); alpha_f_int = alpha_0; d_hat_int = zeros(3,1); delta_hat_int = 0; end

x = eta(1); y = eta(2); psi = eta(3); R_mat = [cos(psi), -sin(psi), 0; sin(psi), cos(psi), 0; 0, 0, 1]; % --- 1. 位置误差和滑模面 --- e = eta - ref.eta_d; e_dot = R_mat * nu - ref.eta_d_dot; s = e_dot + param.Lambda * e; % --- 2. 虚拟控制及动态面滤波 --- alpha = R_mat' * (ref.eta_d_dot - param.Lambda * e); alpha_f_dot = (alpha - alpha_f_int) / param.Tf; alpha_f = alpha_f_int + Ts * alpha_f_dot; % --- 3. 模型补偿项 --- C_nu = param.C(nu); D_nu = param.D(nu); % --- 4. 当前拍扰动估计（上一步持久变量） --- d_hat = d_hat_int; % --- 5. σ修正自适应律更新 --- sat_s = min(max(s / param.epsilon, -1), 1); delta_hat_dot = param.Gamma_s * (norm(s) - param.sigma * delta_hat_int); delta_hat = delta_hat_int + Ts * delta_hat_dot; % --- 6. 控制律 --- tau = param.M * alpha_f_dot + C_nu * nu + D_nu * nu ... - d_hat - delta_hat .* sat_s - param.Ks * s; % --- 7. 扰动观测器状态更新（扩张状态形式） --- nu_hat_dot = param.M_inv * (tau - C_nu * nu + d_hat) ... + param.Kd * (nu - nu_hat); d_hat_dot = param.Kobs * (nu - nu_hat); d_hat_int = d_hat + Ts * d_hat_dot; alpha_f_int = alpha_f; delta_hat_int = max(0, min(delta_hat, param.delta_max)); end

代码逻辑分三段看：第 1-3 行处理运动学误差和滤波，第 4-6 行先拿上一拍的扰动估计组装控制律，第 7 行再用当前控制律更新扰动观测器。这里有一个实现细节：d_hat 用的是上一拍的持久变量，不引入当前拍的代数环，Simulink 仿真步长稍大也不会卡在代数循环上。

参数说明表：

实际跑数据时，把观测器增益 K_obs 调大，扰动估计曲线会明显贴合真实扰动但叠加细碎噪声；边界层 ε 收窄，跟踪误差变小但控制量开始密集跳动。这套控制器里所有参数都有明确的物理含义，不像纯神经网络那样黑箱。

4. 直线与圆轨迹仿真：初始化、参数配置和结果判读

4.1 参考轨迹怎么给

资源包里的两组仿真工况，一组直线、一组圆，正好覆盖“定常航向”和“时变航向”两类典型任务。直线轨迹取：

U_d = 5; % 期望纵向速度 m/s eta_d = [U_d * t; 0; 0]; % x匀速前进，y保持0，艏向0 eta_d_dot = [U_d; 0; 0]; eta_d_ddot = zeros(3,1);

圆轨迹的参数写法更需要注意，艏向不能随位置硬切，必须沿轨迹切线方向给参考值：

R = 100; w_d = 0.05; % 半径100m，角速度0.05rad/s eta_d = [xc + R * sin(w_d * t); yc - R * cos(w_d * t); -w_d * t]; eta_d_dot = [R * w_d * cos(w_d * t); R * w_d * sin(w_d * t); -w_d];

位置轨迹、速度轨迹、艏向轨迹要同时给出，控制器里的反演链路每一层都用得到。圆轨迹的期望艏向是 -ω_d·t，这是因为轨迹从圆心看是顺时针方向，艏向角按角速度负方向旋转。很多初次跑仿真的人直接给 ψ_d=atan2(ẏ_d,ẋ_d)，在 MATLAB 里会因为 arctan 的象限不连续导致艏向跳变，控制器跟随瞬间产生巨大冲击。

船舶初始状态我建议不要直接放在期望轨迹上，而是给一个横向偏差和艏向偏差，比如位置偏差 [10m, -5m]，艏向偏差 20°，这样能看出滑模项在观测器收敛之前是如何兜底的。

4.2 仿真配置与扰动注入

扰动按“常值偏置+时变正弦”给，模拟海流加波浪的等效作用：

tau_d = [1e6 * sin(0.15*t) + 6e5; 5e5 * cos(0.20*t) + 3e5; 2e5 * sin(0.35*t) + 1e5];

这个扰动幅值相对于供给船的控制能力来说不算小，纵荡方向接近 1.5 倍常值推力，能明显看到扰动观测器的作用。直线轨迹下扰动主要是常值项在影响稳态误差，圆轨迹下时变项持续激励，扰动观测器和自适应项都要盯着跟踪。

初始化脚本里要设的参数速查表：

4.3 仿真曲线怎么判读

跑完仿真先看四条曲线：位置跟踪误差、艏向误差、控制输入、扰动估计对比。拿结果时把“有观测器”和“无观测器”的对照开关做出来，通常源码里注释比较充分。对照关系是验证控制器设计的最有力证据：

一个在结果判读里经常被忽略的细节：扰动观测器的输出曲线不需要追求完美贴住真实扰动。因为控制律里还有自适应项 δ̂ 兜底，观测器只要能估出主要低频分量，δ̂ 就把剩余残差吸收掉。非要加大 K_d 把观测器做“准”，噪声被放大后滑模项负担反而加重，得不偿失。仿真里扰动是已知的，可以直接计算估计误差对比曲线；实际工程中扰动不可测，只能通过控制量和状态的残差间接判断估计质量。这个资源包的仿真价值就在这里——它在已知真实扰动的条件下标定了观测器和自适应项的配合关系，把结果迁移到实船时只需要替换模型参数。

4.4 Simulink 仿真与代码验证

资源包里的仿真程序通常是 Simulink 模型加 MATLAB 初始化脚本的组合。控制器部分用 MATLAB Function 或 S-Function，plant 部分用 M、C、D 矩阵建连续模型，步长特意设成固定步长 0.01s，保证离散控制器和连续 plant 的衔接稳定。若仿真速度过慢，可以先把扰动时变频率调低到 0.1rad/s，再用加速模式跑。

验证模型的正确性有个快速办法：把所有扰动为零、初始偏差为零时跑一次控制器，理想情况下控制量应该完全为零——期望轨迹已经满足模型动态，控制器没有“发现”任何误差。如果此时控制量波动很大，大概率是滤波器初始状态或者参考轨迹导数给错了。这一步能在五秒内排除一半以上的建模问题。

5. 参数整定顺序与仿真排错：从微分爆炸到误差带调节

拿到资源包之后不要直接改参数跑完就结束。这个控制器的参数有内在优先级，正确整定顺序是先调动态面滤波 T_f，再调观测器 K_d/K_obs，最后调自适应律的 Γ_s/σ 和边界层 ε。顺序反了会出现一种很迷惑的现象：怎么调 Γ_s 跟踪精度都上不去，其实是 T_f 太大导致滤波输出滞后，自适应项在追逐一个已经失真得目标。

滤波时间常数 T_f 的整定标准是看控制量突变尖峰。初始偏差存在时，虚拟控制 α 会瞬间跳一个大值，α_f 经过滤波后会被拉平。T_f=5s 时尖峰会比 T_f=1s 小一个量级，但同时误差收敛时间明显拉长。T_f 取系统期望时间常数的 1/5-1/10 是经验区间，以控制量为准而不是以跟踪误差为准。

观测器增益 K_d 的调节要观察扰动估计曲线。缺点工程量的做法是直接给一个阻尼响应：先设 K_d=1，观察扰动估计达到真实扰动 63% 的时间，按期望的三倍提速，同时看速度噪声是否被放大。K_d 与 K_obs 的比例固定 3:1 起步，不要两个一起乱调。

再往下的自适应参数和边界层 ε 更像一对跷跷板。σ 增大导致 δ̂ 减小，滑模鲁棒项收缩，边界层 ε 需要收紧来保住精度；反过来 ε 放宽，控制量平滑，σ 要适当加大防止自适应参数在边界层内持续累积。这两项配合的调试标志是：跟踪误差曲线平稳，不存在自激振荡，自适应参数 δ̂ 在常值扰动下收敛到某个数值附近且有微小呼吸波动，而不是持续单调增长。

实际运行中最容易踩的坑是滤波器初始状态和虚拟控制不匹配。alpha_f 初值为零时，首拍滤波输出为零而虚拟控制接近期望速度，差值全部化作控制冲击，力矩瞬间顶到饱和限制。解决办法是把 alpha_f(0)=alpha(0) 写进初始化，这在 3.3 节代码里已经处理。另一个坑是扰动观测器的速度残差反馈将测量噪声直接引入控制量，当 K_d 调得很大时输出曲线出现虫蛀般的密集颤动，先检查噪声滤波模块的带宽是否高于观测器带宽，再考虑降低 K_d。

自适应参数 δ̂ 的限幅必须做在积分器输出端，否则在强时变参考轨迹的持续激励下，Γ_s 可能把 δ̂ 推到远超真实残差界，鲁棒项反过来成为主要扰动源。上限按最大控制能力的 20%-30% 取，仿真里用 sat 函数限制，工程上也有直接改自适应增益做死区的方式。

最后一个容易忽略的点是初始艏向与参考轨迹之间的差值。初始艏向偏差超过 45° 时，滑模面 s 的初值很大，动态面滤波输出在一开始跟不上虚拟控制变化，控制量会在前 10 秒出现一次大摆角波动。解决方式不是加大滤波器时间常数，而是在初始化阶段让参考轨迹从当前实际位置重新规划，等控制器偏航角对准之后再切入原始期望轨迹，用两个阶段的轨迹切换完成平滑过渡。

本文还有配套的精品资源，点击获取