欠驱动AUV轨迹跟踪的全局积分滑模控制与Simulink仿真

做欠驱动AUV控制的人十有八九都会卡在同一个问题上：水面舰艇模型里横向可以靠侧推器硬掰回来，但普通鱼雷构型的水下机器人水平面上只有推进器和舵，能施加的只有纵向推力与偏航力矩，横荡方向根本使不上劲。偏偏水平轨迹跟踪要求AUV踩着时间轴精确走完一条参考轨迹，欠驱动约束、模型非线性、水动力参数不确定三个麻烦叠在一起，常规PID根本压不住。我前前后后在这个课题上折腾了近两个月，最终用全局积分滑模把整个链条跑通：从水平面三自由度建模、内外环滑模控制器推导，到Simulink仿真模型搭建和Matlab代码实现，再到调试阶段踩掉的几个大坑。这篇文章把整个研究过程原原本本整理出来，给正在做同类课题的同行当一份参考，少走几周弯路。

1. 欠驱动AUV模型：为什么水平轨迹跟踪这么难

1.1 水平面三自由度模型与欠驱动约束

要设计控制器，第一步是把被控对象的时间模型写清楚。本文研究水平轨迹跟踪，所以忽略纵倾、横摇和垂荡，认为AUV由单独的深度控制器保持在固定深度，剩下的就是水平面三自由度运动：纵向速度u、横荡速度v、艏摇角速度r，加上大地坐标系下的位置x、y和艏向角ψ。这个简化在工程上非常常见，把六自由度问题砍到三自由度，控制设计的工作量会小一个量级，而且对水平面轨迹跟踪来说损失的信息量很小。

运动学方程很直接：

ẋ = u cosψ - v sinψ ẏ = u sinψ + v cosψ ψ̇ = r

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

m11 u̇ = m22 v r - Xu u - Xu|u| |u|u + τu m22 v̇ = -m11 u r - Yv v - Yv|v| |v|v m33 ṙ = (m11 - m22) u v - Nr r - Nr|r| |r|r + τr

其中m11、m22、m33是包含附加质量的惯性项，Xu、Yv、Nr及对应的二次项是线性与非线性阻尼系数。注意横荡方程里没有控制力τv，这就是欠驱动约束的本质——你有x、y、ψ三个输出要控制，手上的输入却只有τu和τr两个。

我第一次搭这个模型时犯过一个低级错误：把m22 v r搬到了v方程那边，导致开环仿真时系统发散。后来仔细核对才发现，科氏力矩阵的写法在不同文献里符号习惯有差异，跨方程搬运时正负号特别容易出错。建议建模时以刚体动力学标准形式为准，不要凭记忆默写。

1.2 欠驱动必然带来横漂

欠驱动约束最直观的后果不是“少一个执行器”，而是横荡速度v不再可控。AUV转弯时，船体必然会产生侧滑，v不会自己归零，它由纵向速度u和艏摇角速度r的耦合动态决定。这意味着你希望AUV沿参考轨迹某个切线方向走，但实际上它的速度向量和艏向之间始终存在一个侧滑角。

这也引出一个很多新手容易混淆的点：轨迹跟踪（trajectory tracking）和路径跟踪（path following）不是一回事。轨迹跟踪要求AUV在特定时刻到达特定位置，参考信号带时间戳；路径跟踪只要求沿路径前进，至于何时走到哪个点可以由引导律自行调整。欠驱动系统做路径跟踪相对容易，因为可以放宽时间约束；做轨迹跟踪则必须把时间约束也满足，控制难度明显更高。本文用的是圆形参考轨迹，带解析的时间导数，属于典型的trajectory tracking。

横漂现象在仿真结果里很容易观察：画出v曲线，如果转弯过程中v明显不为零，说明系统确实处于欠驱动状态，而不是因为控制器出错在“硬掰”横向。这一点在后文结果分析中会当作判断模型是否正确的重要依据。

2. 全局积分滑模控制器的完整设计

2.1 普通滑模、积分滑模到全局积分滑模

滑模控制在AUV轨迹跟踪里几乎成了标配，原因很简单：它对模型不确定性和外界扰动有天然的鲁棒性。但不同形式的滑模面，工程效果差距很大。

普通滑模面取的是误差及其导数组合，比如对位置误差取s = ė + λe。这种形式有个固有问题：系统从初始状态到达滑模面的过程叫“到达阶段”，在这个阶段里控制量尚未形成等效滑模控制，对扰动的抵抗能力很弱。另一个问题是，如果模型存在常值扰动或参数偏差，普通滑模会留下稳态误差。

积分滑模面在误差项后面引入误差的积分，s = ė + λe + β∫e。积分项的加入可以把稳态误差压到零，代价是需要处理积分饱和。但积分滑模并未解决“到达阶段鲁棒性差”的问题，它只是把误差的稳态性能改善了。

全局积分滑模面则更进一步，在滑模面中减去一个随时间衰减的初始项：

s(t) = ė + λe + β∫e - s(0)·e^(-γt)

由于s(0)被初始时刻定义为当前误差状态计算出的值，而积分项在t=0时为0，所以s(0) - s(0) = 0，系统从0时刻就落在滑模面上，彻底消除到达阶段。同时∫e项保留了对常值扰动的抑制能力，γ决定了全局项衰减多快。三者对比见表。

2.2 外环：位置滑模面生成期望速度与航向

控制器采用经典的内外环级联结构。外环根据位置误差生成期望纵向速度u_d和期望艏向ψ_d，内环分别跟踪这两个期望值，输出实际控制力τu和τr。

对位置误差分别定义两个全局积分滑模面：

sx = ẋe + λx·xe + βx·∫xe dt - sx(0)·e^(-γx·t) sy = ẏe + λy·ye + βy·∫ye dt - sy(0)·e^(-γy·t)

其中xe = x - xd，ye = y - yd。令sx=0、sy=0，解得期望的位置速度：

ẋ_des = ẋd - λx·xe - βx·∫xe dt + sx(0)·e^(-γx·t) ẏ_des = ẏd - λy·ye - βy·∫ye dt + sy(0)·e^(-γy·t)

再通过逆运动学解算期望纵向速度和期望艏向：

u_d = sqrt(ẋ_des² + ẏ_des²) ψ_d = atan2(ẏ_des, ẋ_des)

这里隐含一个工程假设：AUV主推进器只提供正向推力，u_d取正数。对于常规鱼雷构型AUV完全成立；如果是能倒车的水下平台，需要额外处理u_d的符号变化，复杂度会显著上升。

设计时要注意，λx、λy决定外环误差收敛速度，但也不能盲目加大。外环只是生成期望信号，内环是否追得上由内环带宽决定。如果λ太大，外环生成的u_d会很高，超出推进器极限，反而形成饱和。

2.3 内环：速度与航向滑模控制律

内环把u_d和ψ_d当作跟踪目标。纵向速度误差ue = u - u_d，艏向误差ψe = ψ - ψ_d。注意ψe要做角度归一化到[-π, π]，否则atan2在π边界跳变会导致控制器异常，这个问题在调试部分会细说。

速度环的全局积分滑模面：

su = u̇e + λu·ue + βu·∫ue dt - su(0)·e^(-γu·t)

把u方程代入，令ṡu满足指数趋近律ṡu = -ku·sat(su/φu) - qu·su，整理得到τu的控制律。这里用sat替代sgn是为了抑制抖振，边界层厚度φu一般取0.02~0.05。

航向环的滑模面：

sψ = ψ̇e + λψ·ψe + βψ·∫ψe dt - sψ(0)·e^(-γψ·t)

ψ̇e = r - ψ̇d，把r方程代入，同样令ṡψ = -kψ·sat(sψ/φψ) - qψ·sψ，整理得到τr。

两个控制律的核心结构如下：

τu = m11·(-ku·sat(su/φu) - qu·su + ü_d - λu·u̇e - βu·ue - γu·su(0)·e^(-γu·t)) + m22·v·r - Xu·u - Xu|u|·|u|·u

τr = m33·(-kψ·sat(sψ/φψ) - qψ·sψ + ψ̈d - λψ·ψ̇e - βψ·ψe - γψ·sψ(0)·e^(-γψ·t)) + (m11-m22)·u·v - Nr·r - Nr|r|·|r|·r

注意公式里显式出现了γ·s(0)·e^(-γt)这一项，它来自对全局项e^(-γt)求导的负号翻转，很多论文公式里漏掉它，实际实现时会导致控制量含有一个持续的错误偏置。

3. Simulink仿真架构与模型参数

3.1 模块划分与信号流

Simulink模型建议按信号流方向拆成四个模块：参考轨迹生成、控制器、AUV被控对象、结果显示。

参考轨迹模块用Clock驱动Matlab Function，直接输出xd、yd、ẋd、ẏd以及二阶导数。推荐把参考轨迹写成解析形式，因为控制器需要ψ̇d甚至ψ̈d，解析求导最干净，别用Derivative模块去差分，噪声会直接影响控制品质。

控制器模块是整个模型的核心，我用一个Matlab Function实现，输入为AUV状态反馈和参考轨迹信号，输出τu和τr。Matlab Function比Level-2 S-Function直观得多，参数可以直接用工作空间的变量，调试修改也很方便。前提是提前在base工作空间定义好所有控制器参数。

AUV被控对象模块也有两种实现方式。第一种是用三个积分器搭建状态方程，把动力学方程拆开，每个状态用加法器和增益模块连起来，好处是直观、可以单步跟踪中间变量；缺点是连线一多容易乱。第二种是用S-Function封装整个非线性模型，代码更紧凑，但调起来没有积分器直观。我建议先搭积分器版本验证模型，稳定后再封装成S-Function。

初学容易卡住的还有一个问题：代数环。控制器里如果用到了ẋe = ẋ - ẋd，而ẋ并不是单独的输入信号，直接尝试从状态输出取导数就会形成代数环。正确的做法是把运动学方程写进控制器内部，直接用u、ψ、v计算ẋ = u·cosψ - v·sinψ，再减去参考轨迹的解析导数。这样既避免了代数环，也避免了数值微分放大噪声。

3.2 模型参数与仿真器设置

仿真参数我采用一组典型的帆船型AUV数值，惯性项和阻尼系数如表所示。这些参数不是某个特定实物的精确值，而是综合多篇文献的常见量级，对验证控制算法足够。如果你有自己平台的辨识数据，直接替换表中的数值即可。

参考轨迹选半径8米的圆形：

xd(t) = 8·cos(0.05t) yd(t) = 8·sin(0.05t)

这样起点在(8, 0)，方向沿切线逆时针运动。AUV初始位置设在(1, 0)，艏向0，初始速度u0=0.5m/s，故意制造较大的位置误差，用来检验控制器的收敛能力。

仿真器设置方面，变步长选ode45，相对误差1e-6，仿真时长120秒。如果发现控制量高频抖振且计算缓慢，可以切到固定步长ode4，步长0.01秒。我个人习惯先用变步长跑通逻辑，再换固定步长验证离散化影响，两种结果应该差异不大。

4. Matlab代码实现的核心细节

4.1 主程序框架

我强烈建议先用纯Matlab脚本把算法跑通，再去做Simulink可视化。因为脚本调试时可以随时打印中间变量、画出误差曲线，定位问题远比Simulink方便。Simulink模型只需要复用脚本里的同一个控制器函数即可，保证两种实现逻辑完全一致。

主程序的大致骨架如下：

%% 参数定义（节选） m = 30; Iz = 4.1; Xud = 2.8; Yvd = 12.0; Nrd = 3.0; m11 = m - Xud; m22 = m - Yvd; m33 = Iz - Nrd; Xu = 5.5; Yv = 10.0; Nr = 6.0; Xu2 = 2.0; Yv2 = 4.0; Nr2 = 2.0; %% 控制器参数 lambda_x = 0.8; beta_x = 0.4; gamma_x = 1.0; lambda_y = 0.8; beta_y = 0.4; gamma_y = 1.0; lambda_u = 1.5; beta_u = 0.6; gamma_u = 1.0; lambda_psi = 1.2; beta_psi = 0.6; gamma_psi = 1.0; k_u = 8; qu = 2; phi_u = 0.05; k_psi = 3; qpsi = 1; phi_psi = 0.05; %% 初始状态 x0 = 1; y0 = 0; psi0 = 0; u0 = 0.5; v0 = 0; r0 = 0; init = [x0 y0 psi0 u0 v0 r0]; %% 全局项初值 % 参考轨迹及其导数在t=0处的值 xd0 = 8; yd0 = 0; xd_dot0 = 0; yd_dot0 = 0.4; % 位置误差及其导数 xe0 = x0 - xd0; ye0 = y0 - yd0; dxe0 = u0*cos(psi0) - v0*sin(psi0) - xd_dot0; dye0 = u0*sin(psi0) + v0*cos(psi0) - yd_dot0; s_x0 = dxe0 + lambda_x*xe0; s_y0 = dye0 + lambda_y*ye0;

注意这里的位置误差导数是用运动学方程算出来的，而不是数值差分。这一步直接决定了全局项s(0)是否准确，也是很多代码里最容易出错的地方。

4.2 控制器函数与关键算法

控制器函数建议写成独立m文件，输入状态向量和当前时刻，输出两个控制力。函数内部要能同时支持ode45脚本调用和Simulink的Matlab Function调用，输入输出全部用向量传递。

核心计算逻辑如下：

function [tau_u, tau_r] = auv_controller(state, t, params) % 状态解包 x = state(1); y = state(2); psi = state(3); u = state(4); v = state(5); r = state(6); % 参考轨迹及解析导数 xd = 8*cos(0.05*t); yd = 8*sin(0.05*t); xd_dot = -0.4*sin(0.05*t); yd_dot = 0.4*cos(0.05*t); xd_ddot = -0.02*cos(0.05*t); yd_ddot = -0.02*sin(0.05*t); % 位置误差 xe = x - xd; ye = y - yd; dxe = u*cos(psi) - v*sin(psi) - xd_dot; dye = u*sin(psi) + v*cos(psi) - yd_dot; % 外环滑模面，含全局衰减项 sx = dxe + params.lambda_x*xe + params.beta_x*params.int_x ... - params.s_x0*exp(-params.gamma_x*t); sy = dye + params.lambda_y*ye + params.beta_y*params.int_y ... - params.s_y0*exp(-params.gamma_y*t); % 期望速度分量（令sx=0, sy=0） vx_des = xd_dot - params.lambda_x*xe - params.beta_x*params.int_x ... + params.s_x0*exp(-params.gamma_x*t); vy_des = yd_dot - params.lambda_y*ye - params.beta_y*params.int_y ... + params.s_y0*exp(-params.gamma_y*t); u_d = sqrt(vx_des^2 + vy_des^2); psi_d = atan2(vy_des, vx_des); psi_d_dot = (vy_des_dot*vx_des - vy_des*vx_des_dot) / ... (vx_des^2 + vy_des^2); % 内环误差 ue = u - u_d; psie = wrapToPi(psi - psi_d); due = (m22*v*r - Xu*u - Xu2*abs(u)*u)/m11 + tau_u/m11 - u_d_dot; % 这里tau_u是上一控制周期的值，实际实现中可用解析方式替代 d_psie = r - psi_d_dot; % 速度环滑模面和趋近律 su = due + params.lambda_u*ue + params.beta_u*params.int_u ... - params.s_u0*exp(-params.gamma_u*t); sdot_u = -params.k_u*sat(su/params.phi_u) - params.q_u*su; tau_u = m11*(sdot_u - params.lambda_u*due - params.beta_u*ue ... - params.gamma_u*params.s_u0*exp(-params.gamma_u*t)) ... + m22*v*r - Xu*u - Xu2*abs(u)*u; % 注意此处简化了u_d的二阶导数项，完整实现需要解析推导 % 航向环滑模面和趋近律 s_psi = d_psie + params.lambda_psi*psie + params.beta_psi*params.int_psi ... - params.s_psi0*exp(-params.gamma_psi*t); sdot_psi = -params.k_psi*sat(s_psi/params.phi_psi) - params.q_psi*s_psi; tau_r = m33*(sdot_psi - params.lambda_psi*d_psie - params.beta_psi*psie ... - params.gamma_psi*params.s_psi0*exp(-params.gamma_psi*t)) ... + (m11-m22)*u*v - Nr*r - Nr2*abs(r)*r; end

这段代码是简化的示意版本，有几个地方需要你根据自己的模型补全：u_d_dot和psi_d_dot需要用外环信号的解析导数计算，不能直接差分；积分状态int_x、int_y等需要在主循环中累加或另开积分器。实际实现时，我会把参考轨迹计算封装成独立函数，避免重复代码。

4.3 全局项初值s(0)的处理

全局积分滑模与传统积分滑模的差异全部体现在s(0)上。s(0)必须在仿真启动前算好，而不是在控制器里实时计算。原因是控制器在每个采样周期看到的只是当前误差，无法知道“初始时刻”的误差值。

具体做法前面骨架已经展示：在初始化阶段用给定的初始状态和参考轨迹在t=0处的值计算xe0、ẋe0，进而得到s_x0、s_y0。对于内环的s_u0、s_psi0，由于外环生成的u_d和ψ_d在初始时刻通常会等于AUV当前速度与艏向（因为位置误差项在初始时刻包含了状态信息），实际操作中我直接令s_u0=0、s_psi0=0，效果并无明显差异。严格的做法是把u_d(0)和ψ_d(0)代入计算，但级联结构的特性决定了内环初始误差本来就很小，这部分可以简化。

5. 仿真调试中容易踩的坑

5.1 抖振问题：sgn与sat的取舍

最早版本我用sgn函数做趋近律，仿真结果惨不忍睹：控制量在滑模面附近高频切换，τr的曲线像锯齿一样，仿真步长被迫缩到极小，计算速度慢得离谱。这是滑模控制的老毛病，工程上必须用饱和函数替代符号函数。

sat函数的实现很简单：

function out = sat(s_over_phi) if abs(s_over_phi) <= 1 out = s_over_phi; else out = sign(s_over_phi); end end

边界层厚度φ不能拍脑袋定。φ太小，抖振压不下来；φ太大，控制精度变差，稳态误差会跟着涨。我调试下来的经验是，先取φ=0.05观察曲线，如果控制量仍有高频毛刺，逐步提高到0.1；如果稳态误差超标，再往回调到0.02。抖振问题同时和趋近律增益k有关，k越大到达越快但抖振越强，k的调节优先级排在φ前面。

5.2 角度跳变与积分饱和

第二个让我抓狂的问题是艏向角跳变。圆形轨迹上AUV跟踪的ψ_d是随时间连续变化的，理论上不会跳，但在某些时刻ψ_d会从接近π直接跳到接近-π，如果直接用这个值做误差计算，ψe会瞬间变成约2π，控制器会误以为航向偏差巨大，输出一个猛烈的偏航力矩。

解决办法是角度归一化。把ψe映射到(-π, π]区间：

function angle = wrapToPi(angle) angle = mod(angle + pi, 2*pi) - pi; end

Matlab自带的wrapToPi函数在Mapping Toolbox里，如果没装这个工具箱就自己写，功能完全一致。这个归一化必须放在误差计算之后、滑模面计算之前，顺序错了等于白做。

积分饱和的问题同样隐蔽。由于AUV初始位置离参考轨迹较远，位置误差积分项在最初几秒会快速累积，控制器很容易出现输出饱和。等误差回来之后积分项还迟迟退不下来，响应就会出现明显的超调甚至来回震荡。

我的处理办法有两个，组合使用效果最好：一是给积分项加限幅，比如约束在±5以内；二是采用“条件积分”，只在误差小于某个阈值时启动积分，大误差阶段关掉积分。后者在AUV这类启动阶段误差大的场景里特别有效，可以显著缩短收敛时间。

5.3 参数整定顺序与经验值

参数整定顺序比参数大小本身更重要。我踩过的教训是，一开始上来就调内环增益，结果无论怎么调都感觉系统“跟不上”，其实是外环λx、λy没调好，期望信号本身就太激进。

正确的顺序应该是：先定外环λx、λy，观察位置误差收敛曲线，目标是把收敛时间控制在30~40秒左右；然后调内环速度环的λu、ku，确保u对u_d的跟踪误差在10秒内衰减到0.1m/s以内；最后调航向环的λψ、kψ。航向环带宽应该高于速度环，速度环带宽高于外环，形成清晰的级联关系，这也是级联控制器的通用设计准则。

一组实测比较稳定的参数组合是：

注意外环没有k和q，因为它不直接输出控制力，只生成期望信号。速度环的k比航向环大，因为纵向推力响应需要更“硬”一些才能跟上圆的切线速度变化。

6. 仿真结果分析与改进方向

6.1 从曲线判断控制质量

仿真结束后要看的不是一张漂亮位置图就完事，而是三组曲线：位置误差、航向误差、控制输入。位置误差曲线应该单调衰减到0附近，120秒仿真末端的稳态误差如果控制在±0.05米以内，对于欠驱动AUV来说已经不错了。圆形轨迹跟踪图上，AUV的轨迹会先快速逼近参考圆，然后贴合运行，不会出现明显的“抄近路”或“画外圈”现象。

航向误差曲线是判断控制器品质的第二指标。稳态误差应小于0.05rad，约3度。如果航向误差出现周期性波动，多半是ψ_d的导数计算有误，或者航向环带宽不足。

控制输入曲线则要重点检查两个问题：一是τr是否有高频抖振，二是τu是否存在长时间饱和。τr理想情况下应该是一条相对平滑的曲线，伴随转弯时有规律的正负交替；τu则平稳波动，绝不长时间钉在推进器上限。如果在稳态阶段控制量仍然频繁大幅切换，说明饱和函数边界层太薄，或者趋近律增益偏大。

顺带看一眼v曲线。AUV贴着圆形轨迹匀速转弯时，v应该稳定在一个非零的小值上，这正是欠驱动系统转弯时的横漂表现。如果v长期为0，说明你的闭环系统可能被控制器“锁死”了横向自由度，这在物理上是不可能的，大概率是控制器里出现了数值问题。

6.2 鲁棒性测试与可扩展方向

基础仿真跑通之后，我建议立刻做鲁棒性测试，否则很难说服别人这套算法在真实环境中能干活。最简单的做法是在动力学方程里叠加时变扰动，比如d_u = 5·sin(0.1t)，d_r = 2·cos(0.3t)，模拟海流扰动。保持控制器参数不变重新跑一遍，观察位置误差是否仍能收敛到可接受范围。

我实测下来，全局积分滑模对这类常值加谐波扰动有明显的抑制作用，稳态误差只会小幅恶化，但不会发散。相比之下，不加积分项的普通滑模在同样的扰动下稳态误差会明显增大。这组对比是做报告、写论文时很有说服力的素材。

如果还想进一步扩展，可以考虑几个方向。一是用扰动观测器估计海流项，把估计值前馈补偿到控制律中，可以进一步压缩稳态误差。二是把固定的趋近律增益改成自适应形式，误差大时增益大、误差小时增益小，缓解抖振与快速收敛之间的矛盾。三是将方法推广到三维空间，与深度控制解耦后仍然成立，只需在垂向通道叠加一个独立的深度控制器。

我在实际使用中还有一个体会：仿真参数和实机参数往往差距不小，如果手头有自己平台的辨识数据，第一步就是替换模型参数重跑整定流程，不要拿着别人的参数硬套。水动力系数的数量级变化对控制器参数的影响比我预想的大得多，尤其是二次阻尼项，可以说牵一发而动全身。做这类课题，仿真只是开始，真正的考验在上岸之后的水池实验。