简介面向三自由度全驱动船舶的轨迹跟踪控制问题资源提供一套带扰动观测器的自适应动态面滑模控制方案适合研究船舶运动控制、非线性鲁棒控制的研究生或工程师。方案通过扰动观测器前馈补偿未知环境扰动结合σ修正自适应律处理观测误差界并利用动态面技术避免反演法微分爆炸从理论上保证闭环系统信号的最终有界。资源内含完整仿真代码与模型覆盖圆轨迹、直线轨迹等典型工况基于供给船舶的仿真可复现未知扰动估计、参数自适应和滑模控制等关键算法。压缩包共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ᵀRIRᵀ(ψ)R⁻¹(ψ)后面推导控制律时经常要用这个性质。三自由度动力学方程写成标准形式Mν̇ C(ν)ν D(ν)ν τ τ_dM 是惯性矩阵含附加质量C(ν) 是科氏力矩阵D(ν) 是阻尼矩阵τ 是推进器控制力/力矩τ_d 是外界环境扰动力。做控制器设计时有三个结构性质可以直接用M 对称正定C(ν) 斜对称即满足 xᵀCx0D(ν) 正定。这三个性质是后面李雅普诺夫证明中消项的基础仿真的 plant 模块也要确保 M、C、D 满足这些约束不要随便填矩阵。全驱动是什么意思意思是纵向、横向、艏摇三个自由度都有独立推进器或舵机组合来驱动控制输入 τ∈ℝ³ 可以独立设计。这一点直接把问题从欠驱动控制里摘出来后面用的反演法、动态面、滑模面都是针对全驱动形式的。资源包里的供给船仿真参数大概是这个量级参数含义中标量级m船体质量加附加质量5.5×10⁶ kgI_z艏摇转动惯量3.5×10⁷ kg·m²X_u纵荡线性阻尼1.0×10⁵ kg/sY_v横荡线性阻尼2.0×10⁵ kg/sN_r艏摇阻尼力矩系数6.0×10⁶ kg·m²/s实际仿真里换成你自己的船型参数就行量级保持在这个范围内控制器对参数变化有一定的鲁棒性。2.2 从跟踪误差到滑模面位置跟踪误差定义为 eη-η_dη_d 是期望轨迹。传统反演法第一步把 e 的导数展开ėR(ψ)ν-η̇_d。如果直接把 R(ψ)ν 虚拟控制 α_ν 取成 η̇_d-Λe那么 e 的误差动态就是指数收敛Λ 是正定对角矩阵决定误差收敛带宽。这一步需要坐标系变换操作。滑模面按一阶形式取s ė Λes 收敛到零之后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·sMα̇_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·VC₀ 的不等式形式。λ_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̂zK_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; endx 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 仿真步长稍大也不会卡在代数循环上。参数说明表参数符号说明调节方向Λlambda误差收敛带宽增大提高收敛速度过大会超出推力范围T_fTf动态面滤波时间常数增大抑制控制力突变恶化跟踪精度K_dKd速度观测器增益决定扰动估计收敛快慢过大放大噪声K_obsKobs扰动估计增益跟踪时变扰动的带宽受 K_d 约束Γ_sGamma_s自适应估计增益增大加快 δ̂ 响应σsigma泄漏系数防参数漂移过大会吞掉鲁棒项εepsilon饱和函数边界层决定稳态误差带宽度和抖振水平实际跑数据时把观测器增益 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这是因为轨迹从圆心看是顺时针方向艏向角按角速度负方向旋转。很多初次跑仿真的人直接给 ψ_datan2(ẏ_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 倍常值推力能明显看到扰动观测器的作用。直线轨迹下扰动主要是常值项在影响稳态误差圆轨迹下时变项持续激励扰动观测器和自适应项都要盯着跟踪。初始化脚本里要设的参数速查表仿真项值说明仿真时长300 s圆轨迹保证至少走完两圈固定步长0.01 s离散控制器必须用固定步长M 矩阵diag 量级见 2.1 节需对称正定Kd 观测器增益diag([1.0, 1.0, 1.0])先取小逐步增大Kobs 扰动增益diag([0.3, 0.3, 0.3])约为 Kd 的 1/3Tf 滤波时间常数5 s过大跟踪滞后过小微分爆炸又现sigma 泄漏系数0.01防自适应漂移epsilon 边界层0.05按误差量级取4.3 仿真曲线怎么判读跑完仿真先看四条曲线位置跟踪误差、艏向误差、控制输入、扰动估计对比。拿结果时把“有观测器”和“无观测器”的对照开关做出来通常源码里注释比较充分。对照关系是验证控制器设计的最有力证据现象原因修正方向直线稳态误差在 0.05m 以内控制量平稳观测器前馈起主导作用合格无需改动圆轨迹控制输入出现周期性调制向心加速度与扰动叠加正常现象幅度不超过推力上限即可关闭观测器后误差大会一倍以上切换增益兜底能力有限说明观测器前馈有效控制量出现密集高频抖动边界层太小或 Kd 增益过大先降 Kd再升 epsilon扰动估计曲线滞后真实扰动 2 秒以上Kobs 带宽不够升 Kobs但保持低于 Kd 的 1/3一个在结果判读里经常被忽略的细节扰动观测器的输出曲线不需要追求完美贴住真实扰动。因为控制律里还有自适应项 δ̂ 兜底观测器只要能估出主要低频分量δ̂ 就把剩余残差吸收掉。非要加大 K_d 把观测器做“准”噪声被放大后滑模项负担反而加重得不偿失。仿真里扰动是已知的可以直接计算估计误差对比曲线实际工程中扰动不可测只能通过控制量和状态的残差间接判断估计质量。这个资源包的仿真价值就在这里——它在已知真实扰动的条件下标定了观测器和自适应项的配合关系把结果迁移到实船时只需要替换模型参数。4.4 Simulink 仿真与代码验证资源包里的仿真程序通常是 Simulink 模型加 MATLAB 初始化脚本的组合。控制器部分用 MATLAB Function 或 S-Functionplant 部分用 M、C、D 矩阵建连续模型步长特意设成固定步长 0.01s保证离散控制器和连续 plant 的衔接稳定。若仿真速度过慢可以先把扰动时变频率调低到 0.1rad/s再用加速模式跑。验证模型的正确性有个快速办法把所有扰动为零、初始偏差为零时跑一次控制器理想情况下控制量应该完全为零——期望轨迹已经满足模型动态控制器没有“发现”任何误差。如果此时控制量波动很大大概率是滤波器初始状态或者参考轨迹导数给错了。这一步能在五秒内排除一半以上的建模问题。5. 参数整定顺序与仿真排错从微分爆炸到误差带调节拿到资源包之后不要直接改参数跑完就结束。这个控制器的参数有内在优先级正确整定顺序是先调动态面滤波 T_f再调观测器 K_d/K_obs最后调自适应律的 Γ_s/σ 和边界层 ε。顺序反了会出现一种很迷惑的现象怎么调 Γ_s 跟踪精度都上不去其实是 T_f 太大导致滤波输出滞后自适应项在追逐一个已经失真得目标。滤波时间常数 T_f 的整定标准是看控制量突变尖峰。初始偏差存在时虚拟控制 α 会瞬间跳一个大值α_f 经过滤波后会被拉平。T_f5s 时尖峰会比 T_f1s 小一个量级但同时误差收敛时间明显拉长。T_f 取系统期望时间常数的 1/5-1/10 是经验区间以控制量为准而不是以跟踪误差为准。观测器增益 K_d 的调节要观察扰动估计曲线。缺点工程量的做法是直接给一个阻尼响应先设 K_d1观察扰动估计达到真实扰动 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 秒出现一次大摆角波动。解决方式不是加大滤波器时间常数而是在初始化阶段让参考轨迹从当前实际位置重新规划等控制器偏航角对准之后再切入原始期望轨迹用两个阶段的轨迹切换完成平滑过渡。本文还有配套的精品资源点击获取
全驱动船舶轨迹跟踪：扰动观测器与动态面滑模控制设计
简介面向三自由度全驱动船舶的轨迹跟踪控制问题资源提供一套带扰动观测器的自适应动态面滑模控制方案适合研究船舶运动控制、非线性鲁棒控制的研究生或工程师。方案通过扰动观测器前馈补偿未知环境扰动结合σ修正自适应律处理观测误差界并利用动态面技术避免反演法微分爆炸从理论上保证闭环系统信号的最终有界。资源内含完整仿真代码与模型覆盖圆轨迹、直线轨迹等典型工况基于供给船舶的仿真可复现未知扰动估计、参数自适应和滑模控制等关键算法。压缩包共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ᵀRIRᵀ(ψ)R⁻¹(ψ)后面推导控制律时经常要用这个性质。三自由度动力学方程写成标准形式Mν̇ C(ν)ν D(ν)ν τ τ_dM 是惯性矩阵含附加质量C(ν) 是科氏力矩阵D(ν) 是阻尼矩阵τ 是推进器控制力/力矩τ_d 是外界环境扰动力。做控制器设计时有三个结构性质可以直接用M 对称正定C(ν) 斜对称即满足 xᵀCx0D(ν) 正定。这三个性质是后面李雅普诺夫证明中消项的基础仿真的 plant 模块也要确保 M、C、D 满足这些约束不要随便填矩阵。全驱动是什么意思意思是纵向、横向、艏摇三个自由度都有独立推进器或舵机组合来驱动控制输入 τ∈ℝ³ 可以独立设计。这一点直接把问题从欠驱动控制里摘出来后面用的反演法、动态面、滑模面都是针对全驱动形式的。资源包里的供给船仿真参数大概是这个量级参数含义中标量级m船体质量加附加质量5.5×10⁶ kgI_z艏摇转动惯量3.5×10⁷ kg·m²X_u纵荡线性阻尼1.0×10⁵ kg/sY_v横荡线性阻尼2.0×10⁵ kg/sN_r艏摇阻尼力矩系数6.0×10⁶ kg·m²/s实际仿真里换成你自己的船型参数就行量级保持在这个范围内控制器对参数变化有一定的鲁棒性。2.2 从跟踪误差到滑模面位置跟踪误差定义为 eη-η_dη_d 是期望轨迹。传统反演法第一步把 e 的导数展开ėR(ψ)ν-η̇_d。如果直接把 R(ψ)ν 虚拟控制 α_ν 取成 η̇_d-Λe那么 e 的误差动态就是指数收敛Λ 是正定对角矩阵决定误差收敛带宽。这一步需要坐标系变换操作。滑模面按一阶形式取s ė Λes 收敛到零之后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·sMα̇_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·VC₀ 的不等式形式。λ_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̂zK_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; endx 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 仿真步长稍大也不会卡在代数循环上。参数说明表参数符号说明调节方向Λlambda误差收敛带宽增大提高收敛速度过大会超出推力范围T_fTf动态面滤波时间常数增大抑制控制力突变恶化跟踪精度K_dKd速度观测器增益决定扰动估计收敛快慢过大放大噪声K_obsKobs扰动估计增益跟踪时变扰动的带宽受 K_d 约束Γ_sGamma_s自适应估计增益增大加快 δ̂ 响应σsigma泄漏系数防参数漂移过大会吞掉鲁棒项εepsilon饱和函数边界层决定稳态误差带宽度和抖振水平实际跑数据时把观测器增益 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这是因为轨迹从圆心看是顺时针方向艏向角按角速度负方向旋转。很多初次跑仿真的人直接给 ψ_datan2(ẏ_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 倍常值推力能明显看到扰动观测器的作用。直线轨迹下扰动主要是常值项在影响稳态误差圆轨迹下时变项持续激励扰动观测器和自适应项都要盯着跟踪。初始化脚本里要设的参数速查表仿真项值说明仿真时长300 s圆轨迹保证至少走完两圈固定步长0.01 s离散控制器必须用固定步长M 矩阵diag 量级见 2.1 节需对称正定Kd 观测器增益diag([1.0, 1.0, 1.0])先取小逐步增大Kobs 扰动增益diag([0.3, 0.3, 0.3])约为 Kd 的 1/3Tf 滤波时间常数5 s过大跟踪滞后过小微分爆炸又现sigma 泄漏系数0.01防自适应漂移epsilon 边界层0.05按误差量级取4.3 仿真曲线怎么判读跑完仿真先看四条曲线位置跟踪误差、艏向误差、控制输入、扰动估计对比。拿结果时把“有观测器”和“无观测器”的对照开关做出来通常源码里注释比较充分。对照关系是验证控制器设计的最有力证据现象原因修正方向直线稳态误差在 0.05m 以内控制量平稳观测器前馈起主导作用合格无需改动圆轨迹控制输入出现周期性调制向心加速度与扰动叠加正常现象幅度不超过推力上限即可关闭观测器后误差大会一倍以上切换增益兜底能力有限说明观测器前馈有效控制量出现密集高频抖动边界层太小或 Kd 增益过大先降 Kd再升 epsilon扰动估计曲线滞后真实扰动 2 秒以上Kobs 带宽不够升 Kobs但保持低于 Kd 的 1/3一个在结果判读里经常被忽略的细节扰动观测器的输出曲线不需要追求完美贴住真实扰动。因为控制律里还有自适应项 δ̂ 兜底观测器只要能估出主要低频分量δ̂ 就把剩余残差吸收掉。非要加大 K_d 把观测器做“准”噪声被放大后滑模项负担反而加重得不偿失。仿真里扰动是已知的可以直接计算估计误差对比曲线实际工程中扰动不可测只能通过控制量和状态的残差间接判断估计质量。这个资源包的仿真价值就在这里——它在已知真实扰动的条件下标定了观测器和自适应项的配合关系把结果迁移到实船时只需要替换模型参数。4.4 Simulink 仿真与代码验证资源包里的仿真程序通常是 Simulink 模型加 MATLAB 初始化脚本的组合。控制器部分用 MATLAB Function 或 S-Functionplant 部分用 M、C、D 矩阵建连续模型步长特意设成固定步长 0.01s保证离散控制器和连续 plant 的衔接稳定。若仿真速度过慢可以先把扰动时变频率调低到 0.1rad/s再用加速模式跑。验证模型的正确性有个快速办法把所有扰动为零、初始偏差为零时跑一次控制器理想情况下控制量应该完全为零——期望轨迹已经满足模型动态控制器没有“发现”任何误差。如果此时控制量波动很大大概率是滤波器初始状态或者参考轨迹导数给错了。这一步能在五秒内排除一半以上的建模问题。5. 参数整定顺序与仿真排错从微分爆炸到误差带调节拿到资源包之后不要直接改参数跑完就结束。这个控制器的参数有内在优先级正确整定顺序是先调动态面滤波 T_f再调观测器 K_d/K_obs最后调自适应律的 Γ_s/σ 和边界层 ε。顺序反了会出现一种很迷惑的现象怎么调 Γ_s 跟踪精度都上不去其实是 T_f 太大导致滤波输出滞后自适应项在追逐一个已经失真得目标。滤波时间常数 T_f 的整定标准是看控制量突变尖峰。初始偏差存在时虚拟控制 α 会瞬间跳一个大值α_f 经过滤波后会被拉平。T_f5s 时尖峰会比 T_f1s 小一个量级但同时误差收敛时间明显拉长。T_f 取系统期望时间常数的 1/5-1/10 是经验区间以控制量为准而不是以跟踪误差为准。观测器增益 K_d 的调节要观察扰动估计曲线。缺点工程量的做法是直接给一个阻尼响应先设 K_d1观察扰动估计达到真实扰动 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 秒出现一次大摆角波动。解决方式不是加大滤波器时间常数而是在初始化阶段让参考轨迹从当前实际位置重新规划等控制器偏航角对准之后再切入原始期望轨迹用两个阶段的轨迹切换完成平滑过渡。本文还有配套的精品资源点击获取
