做欠驱动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)被初始时刻定义为当前误差状态计算出的值而积分项在t0时为0所以s(0) - s(0) 0系统从0时刻就落在滑模面上彻底消除到达阶段。同时∫e项保留了对常值扰动的抑制能力γ决定了全局项衰减多快。三者对比见表。滑模面形式稳态误差到达阶段鲁棒性起点适用场景普通滑模 s ė λe有存在到达后精度要求不高的场合积分滑模 s ė λe β∫e无存在到达后需消除稳态误差全局积分滑模 s ė λe β∫e - s(0)e^(-γt)无无初始时刻高精度轨迹跟踪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 - xdye y - yd。令sx0、sy0解得期望的位置速度ẋ_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数值惯性项和阻尼系数如表所示。这些参数不是某个特定实物的精确值而是综合多篇文献的常见量级对验证控制算法足够。如果你有自己平台的辨识数据直接替换表中的数值即可。参数数值说明m30 kg质量Iz4.1 kg·m²艏摇转动惯量X_u̇2.8 kg纵向附加质量Y_v̇12.0 kg横荡附加质量N_ṙ3.0 kg·m²艏摇附加质量Xu5.5 kg/s纵向线性阻尼Yv10.0 kg/s横荡线性阻尼Nr6.0 kg·m²/s艏摇线性阻尼Xuu2.0 kg/mYvv4.0 kg/mNrr2.0 kg·m²参考轨迹选半径8米的圆形xd(t) 8·cos(0.05t) yd(t) 8·sin(0.05t)这样起点在(8, 0)方向沿切线逆时针运动。AUV初始位置设在(1, 0)艏向0初始速度u00.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]; %% 全局项初值 % 参考轨迹及其导数在t0处的值 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); % 期望速度分量令sx0, sy0 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)必须在仿真启动前算好而不是在控制器里实时计算。原因是控制器在每个采样周期看到的只是当前误差无法知道“初始时刻”的误差值。具体做法前面骨架已经展示在初始化阶段用给定的初始状态和参考轨迹在t0处的值计算xe0、ẋe0进而得到s_x0、s_y0。对于内环的s_u0、s_psi0由于外环生成的u_d和ψ_d在初始时刻通常会等于AUV当前速度与艏向因为位置误差项在初始时刻包含了状态信息实际操作中我直接令s_u00、s_psi00效果并无明显差异。严格的做法是把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; endMatlab自带的wrapToPi函数在Mapping Toolbox里如果没装这个工具箱就自己写功能完全一致。这个归一化必须放在误差计算之后、滑模面计算之前顺序错了等于白做。积分饱和的问题同样隐蔽。由于AUV初始位置离参考轨迹较远位置误差积分项在最初几秒会快速累积控制器很容易出现输出饱和。等误差回来之后积分项还迟迟退不下来响应就会出现明显的超调甚至来回震荡。我的处理办法有两个组合使用效果最好一是给积分项加限幅比如约束在±5以内二是采用“条件积分”只在误差小于某个阈值时启动积分大误差阶段关掉积分。后者在AUV这类启动阶段误差大的场景里特别有效可以显著缩短收敛时间。5.3 参数整定顺序与经验值参数整定顺序比参数大小本身更重要。我踩过的教训是一开始上来就调内环增益结果无论怎么调都感觉系统“跟不上”其实是外环λx、λy没调好期望信号本身就太激进。正确的顺序应该是先定外环λx、λy观察位置误差收敛曲线目标是把收敛时间控制在30~40秒左右然后调内环速度环的λu、ku确保u对u_d的跟踪误差在10秒内衰减到0.1m/s以内最后调航向环的λψ、kψ。航向环带宽应该高于速度环速度环带宽高于外环形成清晰的级联关系这也是级联控制器的通用设计准则。一组实测比较稳定的参数组合是回路λβγkqφ位置外环x0.80.41.0———位置外环y0.80.41.0———速度环1.50.61.0820.05航向环1.20.61.0310.05注意外环没有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)模拟海流扰动。保持控制器参数不变重新跑一遍观察位置误差是否仍能收敛到可接受范围。我实测下来全局积分滑模对这类常值加谐波扰动有明显的抑制作用稳态误差只会小幅恶化但不会发散。相比之下不加积分项的普通滑模在同样的扰动下稳态误差会明显增大。这组对比是做报告、写论文时很有说服力的素材。如果还想进一步扩展可以考虑几个方向。一是用扰动观测器估计海流项把估计值前馈补偿到控制律中可以进一步压缩稳态误差。二是把固定的趋近律增益改成自适应形式误差大时增益大、误差小时增益小缓解抖振与快速收敛之间的矛盾。三是将方法推广到三维空间与深度控制解耦后仍然成立只需在垂向通道叠加一个独立的深度控制器。我在实际使用中还有一个体会仿真参数和实机参数往往差距不小如果手头有自己平台的辨识数据第一步就是替换模型参数重跑整定流程不要拿着别人的参数硬套。水动力系数的数量级变化对控制器参数的影响比我预想的大得多尤其是二次阻尼项可以说牵一发而动全身。做这类课题仿真只是开始真正的考验在上岸之后的水池实验。