做水下机器人控制仿真这几年欠驱动AUV的轨迹跟踪和路径跟随始终是个绕不开的话题。别以为这只是“跟踪一条线”那么简单实际做起来你会发现控制输入比状态量少、模型非线性强、还要同时应付静水力和海流扰动稍不注意仿真就发散了。这篇文章我把当时在Matlab和Simulink里做的整套仿真研究整理出来包含算法思路、模型搭建、控制器实现和踩坑记录希望能给同样在做欠驱动AUV运动控制的同学一些参考。先说明白一件事欠驱动AUV是工程中的常态因为多数水下航行器只装了尾部推进器和艏摇舵没有侧移推力器所以水平面内只有纵向推力和转艏力矩两个输入却要控制x、y和航向角三个自由度。这就是“欠驱动”三个字的含义。控制难点在于系统存在非完整约束没法简单地用全驱动方式处理。而轨迹跟踪和路径跟随这两种任务虽然听起来差不多但在数学定义和控制结构上差别很大轨迹跟踪要求AUV在每一时刻都精确踩在时间参数化的参考轨迹上路径跟随则只要求AUV收敛到一条空间曲线至于时间先后无所谓。这两种任务的分析方法也不同我分别用反步法、视线制导(LOS)、滑模控制和模型预测控制做了仿真对比下面逐步展开。1. 项目背景与控制需求拆解1.1 什么是欠驱动AUV为什么研究它AUV的全称是自主水下航行器常见形态是鱼雷型或回转体型。它的特点是在深海环境中自主完成导航、目标探测、管线巡检等任务但为了降低能耗和结构复杂度大部分AUV并不安装垂直面和水平面的侧推器而是只保留尾部推进器和舵面。水平面内AUV靠推进器提供纵向推力靠方向舵改变艏向垂直面内则通过艏舵控制深度。这样一来系统的独立控制输入个数就小于被控自由度个数数学上称为欠驱动系统。欠驱动系统听起来很酷但控制难度很大。以水平面三自由度模型为例状态量纵向速度u、横荡速度v、艏摇角速度r、位置x、位置y、航向角ψ控制输入纵向推力X、转艏力矩N缺的是直接产生横向加速度的力。因为无法直接控制v和y方向的运动控制器设计必须利用AUV的耦合动力学通过调节纵向推力和转艏力矩间接影响横向运动。这就像开车方向盘只能控制横摆但只要你掌握好车速和前轮转角汽车依然可以跑出任意想要的路径。AUV在水平面的运动学模型可以用如下形式描述x_dot u*cos(psi) - v*sin(psi) y_dot u*sin(psi) v*cos(psi) psi_dot r这个模型是后续所有控制器设计的基础。为了不引入太多参数仿真中我使用了典型的鱼雷形AUV模型质量m≈30kg附加质量和阻尼系数参考相关文献中的标准参数。实际建模时可以按照自己的研究对象调整这里更关注控制算法本身的对比。1.2 轨迹跟踪和路径跟随到底差在哪这是很多刚入门的人容易混淆的点。轨迹跟踪要求作为时间的函数来指定参考运动比如给定一组随时间变化的位置序列x_ref(t) 5*cos(0.1*t) y_ref(t) 5*sin(0.1*t)AUV必须保证在每一时刻都尽量接近这些坐标因此参考信号本身就是一条带“时间戳”的曲线。如果AUV中途因为扰动慢了半拍控制器会努力追赶参考点可能导致超调。这种任务适合定义明确的作业任务比如按预定时间到达指定区域。路径跟随则完全不同。它只定义一条不依赖于时间的空间几何曲线例如一条直线或一段圆弧AUV需要收敛到该曲线上并沿着它前进但对“何时经过哪个点”没有严格约束。控制器的目标是消除横向偏差而纵向进度是自由的。这样做的好处是控制动作更平稳不容易出现因时间错位导致的航向急转也更符合AUV长航程巡航的实际需求。这两种任务对控制器性能的评判标准也不同轨迹跟踪看时间上的误差积分或者最大偏差路径跟随看的是曲线横向偏差和航向误差。在仿真时我分别构造了两套参考输入体系轨迹跟踪用正弦组合曲线路径跟随则用直线段和圆弧组成的导航路径。不同分析方法的适用性也由此分开了。1.3 项目目标与整体仿真方案我的目标很明确在Matlab和Simulink环境中针对同一个欠驱动AUV模型用不同的分析方法设计控制器然后放在同一个仿真框架下进行横向对比。对比的维度包括稳态误差和瞬态响应抗海流扰动的能力控制器复杂度与计算开销参数整定难度。整体方案分成五层第一层是AUV动力学模型用S函数封装第二层是参考轨迹或路径生成器第三层是控制器模块可切换不同算法第四层是扰动注入模块第五层是数据记录与可视化。这样设计的好处是控制算法之间可以无缝切换不至于为了换一种算法重新搭一遍仿真。2. 算法选型不同分析方法的对比2.1 反步法最经典的Lyapunov设计路线反步法(backstepping)是处理非线性控制系统最经典的工具之一。它的核心思想是把复杂的非线性系统拆成若干个子系统然后一步一步地设计虚拟控制律每一步都构造Lyapunov函数来保证稳定性。对于欠驱动AUV思路是这样的先把位置误差转化为航向参考角ψ_ref也就是“往哪走才能减小位置误差”设计虚拟控制律让AUV的航向收敛到ψ_ref再设计推力控制律让纵向速度收敛到期望速度。在Matlab中实现反步法并不复杂关键是要把外环位置控制和内环速度控制串起来。以轨迹跟踪任务为例定义位置误差为xe x_ref - x ye y_ref - y根据运动学关系可以推导出期望航向psi_d atan2(ye_dot k2*ye, xe_dot k1*xe) // 简化示意这里k1、k2是正定的增益参数。实际实现时还要考虑航向角wrap到[-π,π]否则在跨越±180°时会出现跳变。反步法的优点是设计过程严谨在仿真中能把误差收敛到很小的范围缺点是对模型参数依赖较强参数整定比较靠经验增益太大会导致输入饱和增益太小又收敛慢。如果后续要扩展到真实AUV还需要处理推进器饱和和执行机构动态这一点在仿真里可以先用Saturation模块模拟。2.2 视线制导法工程最常用的路径跟随方案视线制导(LOSLine of Sight)是船舶和AUV路径跟随里最实用的方法它模拟的是船员看到目标点后调整船头的动作。LOS的核心是定义一个“虚拟目标点”这个点沿着参考路径往前跑其位置取决于AUV当前位置和路径的相对几何关系。对于直线路径很简单。假设参考直线段由起点P_s和终点P_e定义先把AUV的位置投影到这条线段上得到投影点然后沿着路径前视距离Δ往前取一个点P_los。控制器让AUV艏向对准P_los即可。前视距离Δ的影响非常直观Δ小AUV反应灵敏但容易振荡Δ大AUV动作平缓但路径收敛变慢。我一般取Δ为船长的2到3倍再根据路径弯曲程度调节。LOS的优势是计算量极小不需要复杂的非线性解算非常适合嵌入式部署。仿真中我用它做了直线和折线路径的跟随效果都很稳定。其不足是对动态模型速度变化不敏感尤其在强海流下会产生稳态横向偏差这时可以加上积分项构成ILOS。2.3 滑模控制处理模型不确定性的利器欠驱动AUV的动力学参数质量、阻尼、附加质量往往不容易精确获得海流干扰也是时变的。滑模控制(SMC)在这一点上有天然优势它对匹配不确定性和外部扰动具有不变性设计思路是把系统状态驱动到一个滑动面上然后靠等效控制和切换控制把状态维持在滑动面上。在我的仿真里滑动面取为位置跟踪误差的组合例如s x_dot lambda1 * x lambda2 * y 示意实际设计时我会把水平面的位置误差和速度误差加权构成滑模面控制律采用“等效控制符号切换项”的形式切换增益需要大于扰动上界。滑模控制的问题也很典型符号函数会带来高频抖振。仿真里可以看到控制输入输出频繁切换这不仅对执行机构是折磨还会在仿真步长较大时产生数值振荡。为了缓解我用了边界层法也就是把sign(s)换成sat(s/phi)但这样会牺牲一些鲁棒性。大家在做仿真时可以根据需求在“抗扰”和“平滑”之间做取舍。2.4 模型预测控制最近很热的优化控制思路MPC这几年在运动控制圈火得不行尤其在水下机器人领域处理约束和多目标优化很方便。它的基本思路是在每个采样时刻基于当前状态预测未来N步的系统输出然后求解一个有限时域优化问题把最优控制序列的第一个元素施加给系统下一时刻重新滚动求解。对AUV来说MPC最大的好处是把欠驱动约束、输入饱和、状态限制都显式放到优化问题里。但它也有明显的代价计算量大。欠驱动AUV模型非线性很强如果直接用非线性MPC(NMPC)求解耗时很长在Matlab里如果是纯代码仿真还能顶住但放到Simulink里做实时仿真就会吃力。我这里做的是线性化MPC在每个工作点把动力学方程线性化然后用二次规划(QP)求解。这样虽然牺牲了一部分大范围性能但足以展示MPC在轨迹跟踪上的潜力。MPC的整定主要涉及预测时域N和权重矩阵Q、R。N太短会失去预见性N太长计算时间翻倍Q矩阵中位置误差权重决定跟踪速度R矩阵中控制增量权重决定平滑度。仿真时我用四旋翼里常用的qpOASES来求解QP通过Matlab的mex接口也可以直接用fmincon但速度会慢不少。3. Matlab与Simulink仿真架构搭建3.1 被控对象建模三自由度水平面运动方程与参数整个仿真的核心是被控对象。我没有直接用Simulink自带的船舶模块而是自己写了S函数这样便于切换不同模型参数和扰动形式。水平面运动模型采用经典的分离模型m11 * u_dot m22 * v * r - d11 * u X m22 * v_dot -m11 * u * r - d22 * v m33 * r_dot (m11 - m22) * u * v - d33 * r N其中m11、m22、m33是包含附加质量的惯性系数d11、d22、d33是线性阻尼系数。注意横荡方程里没有控制力但v依旧通过耦合项影响u和r这就引入了欠驱动特性。为了让模型更贴近真实我在动力学方程右侧额外加了一个外部海流扰动项幅度可调。S函数采用两级流程update阶段更新状态导数output阶段输出状态。状态向量选取为[u, v, r, x, y, psi]。仿真步长设为0.1s比一般控制周期小方便控制器输出的连续性。还要注意在S函数里做航向角处理时psi要保持在(-pi, pi]区间否则仿真几十秒后会因为2π累积误差导致姿态解算混乱。下面是S函数的简化框架function [sys,x0,str,ts] auv_sfun(t,x,u,flag,param) switch flag case 0 sys [6 0 2 2 0 1]; % 6个连续状态2个输入2个输出 x0 [0.1; 0; 0; 0; 0; 0]; ts [0 0]; case 1 xu x(1); yv x(2); yr x(3); xpos x(4); ypos x(5); psi x(6); X u(1); N u(2); % 动力学方程 udot (param.m22*yv*yr - param.d11*xu X)/param.m11; vdot (-param.m11*xu*yr - param.d22*yv)/param.m22; rdot ((param.m11-param.m22)*xu*yv - param.d33*yr N)/param.m33; xdot xu*cos(psi) - yv*sin(psi); ydot xu*sin(psi) yv*cos(psi); psidot yr; sys [udot; vdot; rdot; xdot; ydot; psidot]; case 3 sys x; % 输出全部状态供控制器使用 end这里的参数结构体param在初始化时通过全局变量或者函数参数传入。我习惯在脚本里先定义参数再用sim指令跑模型这样改参数不用来回改S函数。3.2 参考轨迹生成与制导模块设计参考轨迹分两种。轨迹跟踪任务我生成了一条Lissajous曲线也就是正弦组合曲线目的是让AUV同时完成x和y方向的周期性跟踪。生成代码很简单t 0:0.1:300; x_ref 5*sin(0.08*t); y_ref 4*cos(0.06*t); psid_ref atan2(cos(0.06*t)*0.06*4*(-sin(0.08*t)*0.08*5), ...);对轨迹跟踪控制器需要知道参考速度和参考加速度所以不能只给位置还要求导。在Simulink里我用一个MATLAB Function模块接收时间t返回参考位置和参考线速度。这么做的好处是切换轨迹时不用动模型结构。路径跟随任务则不同我设计了包含直线段和圆弧的组合路径。制导模块负责根据当前位置计算投影点和LOS点。直线段投影要留意当AUV超出线段端点时投影点要锁定在离得近的那个端点否则会引导AUV往回走。对圆弧段投影点通过当前AUV相对圆心的角度来求同时要考虑从上一段路径切换过来时的路径索引更新问题。制导模块输出两个量期望艏向和期望纵向速度。期望速度可以简单设定为常值1.5 m/s也可以根据路径曲率自动调整曲率大的地方降速避免横向偏差过大。3.3 控制器模块搭建从S函数到普通Simulink框图控制器模块我做了多个版本。反步法和滑模控制直接写成S函数输入是状态反馈和参考信号输出是推力X和转艏力矩N。LOS则更偏制导它输出的航向参考值要先经过一个航向速度控制器再生成力矩。所以我把控制架构分成了外环(制导)和内环(动力学控制)两层这样便于插拔不同算法。对于反步法Simulink模型框图大致是误差计算模块位置误差和航向误差虚拟控制律模块输出期望航向和期望速度内环控制模块根据航向误差计算转艏力矩速度控制模块根据速度误差计算纵向推力。这里有个容易犯的错误直接在Simulink里用乘法器实现三角函数时角度信号如果不做wrap处理atan2输出在±π处跳变会让控制器瞬间饱和。所以我在所有用到角度减法的地方都加了一个wrap函数function e wrapToPi(angle) e mod(angle pi, 2*pi) - pi; endMPC控制器的搭建相对复杂。我没有完全用纯框图而是在Simulink里调用了MATLAB Function模块内部先求解QP再输出控制量。用MATLAB Function的好处是调试方便坏处是仿真速度会慢。如果追求速度建议用C MEX或者embedded coder把MPC代码转成C。对于我们这个研究目的MATLAB Function够了。Simulink总模型结构如下表模块作用说明AUV S-Function被控对象六状态动力学模型Trajectory Generator轨迹生成输出参考位置/速度LOS Guidance路径制导输出期望艏向和速度Controller_Backstepping反步控制器可切换Controller_SMC滑模控制器可切换Controller_MPC模型预测控制器可切换Disturbance Inject海流扰动加在动态方程上Scope/To Workspace数据记录输出到工作区绘图3.4 仿真参数设置与数据记录仿真参数我统一这样设置总时长300秒固定步长0.1秒求解器用ode4(Runge-Kutta)。虽然S函数是连续状态但固定步长更接近控制系统的离散化实现也方便后面做实时分析。AUV的初始位置设为(1, 1)初始航向0度初始速度0.1m/s这样一开始就存在初始误差可以观察控制器的收敛能力。数据记录用To Workspace模块把时间、状态、控制输入、参考信号都保存成结构体仿真结束后用一个统一的plot脚本绘图。绘图时我会画三个图第一是位置曲线对比第二是跟踪误差随时间变化第三是控制输入曲线。这三个图基本能评价一个控制器的性能。4. 仿真结果分析与算法横向对比4.1 轨迹跟踪场景谁跟得更紧谁更稳先说结论在无扰动情况下反步法和MPC的轨迹跟踪误差都能收敛到0附近但动态过程差异明显。反步法参数调整合适时最大超调量在0.3m左右稳态误差小于0.05m。MPC因为带有预测功能在轨迹曲率变化大的地方表现出更好的“预见性”位置误差峰值更小但是控制力矩波动更频繁尤其是预测时域较短时力矩会像锯齿一样一跳一跳。滑模控制在无扰动情况下优势不大它更擅长对付不确定性。如果我把模型质量参数m11故意调大20%反步法的位置误差会明显变大而滑模控制的误差变化很小这就是鲁棒性的体现。LOS因为本质上是一种制导方法在轨迹跟踪任务中需要额外配合内环控制器直接跟踪的效果不如反步法精细但不会发散。这里有一个重要经验轨迹跟踪性能不能只看稳态误差。你要把执行机构饱和加进去你会发现某些算法的理论优势实际是建立在“控制力可以无限大”的假设上。加了Saturation之后反步法容易因为积分饱和导致长时间超调MPC因为自带输入约束转移倒是很平滑。4.2 路径跟随场景LOS在实际航路下的表现路径跟随我用了一段折线路径航向依次为0°、45°、-30°并带一段圆弧过渡。LOS制导配合PID内环控制器在直线段上横向偏差能够从初始2m收敛到0.1m以内。难点在拐弯处如果前视距离Δ选得过大AUV会“抄近路”切弯导致路径偏差峰值达到0.8m如果Δ过小AUV到拐点时会频繁修正航向控制力抖动明显。经过反复试验我采用了可变前视距离直线段Δ取3倍船长圆弧部分取1.5倍船长并加入横向偏差的比例项。这样既保持了直线段的稳定性又改善了弯道跟随精度。ILOS方法也在仿真中验证过在存在常值海流时普通LOS会有一个稳态横向偏差ILOS因为增加了积分项能够把这个偏差压回零。不过积分项增益不能太大否则在初始阶段会出现较大的超调。MPC用于路径跟随同样可行。我把路径的投影点作为参考位置并在线性化模型中引入投影误差控制效果与LOS接近但计算量大很多。所以从工程角度看如果不需要处理复杂约束LOS仍然是性价比最高的路径跟随方案。4.3 抗扰动能力对比加风浪流干扰后的表现仿真中我加了两种扰动一是常值海流相当于在速度上叠加一个固定流速二是周期性扰动模拟波浪的影响。常值海流干扰下反步法和滑模控制表现差异最大。反步法如果模型中没有包含扰动估计会出现明显的稳态位置偏移而滑模控制由于切换项能够抵消扰动稳态误差依然很小。LOS加ILOS也能对抗常值海流但动态响应比滑模慢半拍。周期性扰动下滑模控制的响应最剧烈控制输入出现频繁抖振虽然位置误差不大但执行机构压力很大。MPC因为有约束优化控制输入相对温和不过对扰动的抑制速度不如滑模。换热词里正好有“mpc轨迹跟踪”我补充一句在连续海流扰动场景下MPC可以显式地把扰动模型加入预测方程实现动态前馈补偿效果会更好。但如果不加扰动估计MPC和普通反步法一样会产生静态误差。这一点在做仿真对比时要注意公平性要么大家都加扰动补偿要么都不加。4.4 计算量与实时性考量几类算法的计算量差异悬殊。反步法和滑模控制本质是解析公式在普通Intel i5上单步计算时间在1ms以内几乎可以忽略。LOS加PID也非常轻量。MPC则不然线性MPC每个步长需要求解一个QP在我的测试中预测时域N20时单步QP求解约需要15~30ms如果换用非线性NMPC求解时间会飙到几百毫秒。在Simulink里跑300秒仿真MPC版本总会比其他版本慢几倍。实时性考虑如果目标是半实物仿真或者嵌入式部署反步法和LOS更有优势MPC需要依赖高效的QP求解器和代码生成目前难度还是偏高。但如果AUV任务对约束要求很严格比如在狭窄管道或复杂作业中MPC仍然值得投入。5. 实操中的坑与解决记录5.1 欠驱动模型初始条件导致的数值抖动我第一次用S函数跑模型时把初始纵向速度设为了0导致仿真刚开始的几秒钟内横荡速度v出现高频振荡。原因在于欠驱动模型中的v是由u和r耦合驱动的当u非常接近0时v的动态变成了一种弱阻尼高增益环节数值积分容易不稳定。解决办法有两个一是初始速度不要设为0给一个小值比如0.1m/s二是在控制器设计中给纵向速度一个安全下限。这个坑在论文里不一定会写但实操中特别常见。5.2 Simulink代数环问题与延迟处理反步法控制器中外环位置误差会计算一个期望航向内环又要根据当前航向和期望航向之差计算力矩而期望航向本身可能直接包含当前航向状态的代数关系这就容易造成Simulink代数环。代数环一旦出现仿真每一步都要迭代求解速度慢且可能报错。我的解决方法是在期望航向计算后加一个memory模块或单位延迟把实时值变成上一拍的值。虽然这相当于引入了一拍延迟但工程上完全可接受还能避免代数环。类似的还有MPC优化问题需要当前状态x(k)但如果预测模型包含代数约束Simulink也容易报警。给MPC模块前串一个unit delay就解决。5.3 反步法参数整定的经验反步法的位置误差增益k1和k2不能简单取大就好。k1过大时期望航向变化剧烈内环转艏力矩会饱和导致实际航向跟不上参考反而出现“蛇形漂移”。我常用的策略是先让内环航向控制响应快于外环位置控制分频段设计。具体做法是内环PD增益先调好使得航向带宽约为外环位置带宽的3~5倍。之后整定k1、k2时从0.05开始逐步增大观察位置响应曲线找到临界振荡点再回退50%。这个经验在纯仿真里特别有效能省下很多盲目尝试的时间。5.4 MPC权重调整的直觉MPC的Q和R调整原则和LQR类似但更直观Q中位置误差权重增大跟踪更紧但控制动作更猛R中控制增量权重增大控制更平缓但跟踪会变慢。实际操作时我先把R固定为单位阵把Q中位置权重设为对角阵[10,10]跑一次仿真看误差幅值然后按误差峰值来缩放Q。如果想减少控制抖动就加大R或者限制控制增量的变化率。另外采样周期对MPC影响很大采样周期太长预测精度下降太短实时求解困难。对AUV这种慢系统0.5s到1s的采样周期比较合理。还有一个小技巧把MPC里预测模型的线性化点设置成当前状态而不是标称工作点效果会好很多。我在Simulink里用MATLAB Function实时计算雅可比矩阵然后用quadprog求解代价是每次仿真时间增加但结果明显更稳定。6. 后续扩展方向与个人体会6.1 从3自由度到6自由度水平面仿真只是第一步。真实AUV必须考虑垂面运动也就是要加上升沉速度、纵倾角和纵倾角速度控制输入变为推力、艏摇力矩、纵倾力矩。这时候欠驱动特性仍然保留但控制耦合更严重需要把水平面和垂直面的控制器协调起来。反步法可以推广到六自由度但公式推导量会大很多MPC则因为优化维度增加计算量水涨船高。我的建议是先用3自由度验证算法可行性再逐步扩展到6自由度不要在最初阶段就想一步到位。6.2 与强化学习结合的可能最近不少人在尝试用DQN、PPO、DDPG做AUV控制热词里也出现了dqn算法matlab、ppo算法matlab。从我的仿真经验看强化学习在路径跟随这样的低纬度任务中有一定优势因为它不需要精确模型可以通过仿真环境大量试错。但它很难在仿真里收敛得很稳定尤其是欠驱动系统奖励函数离散且稀疏训练容易卡住。如果要做建议先用本文提到的LOS或反步法生成一批专家数据再通过模仿学习初始化策略会比自己从头学靠谱得多。这个思路可以算作这个仿真项目的自然延伸我已经在部分场景里跑通了软硬件联合仿真。6.3 最后想说的话整个项目做下来最大的体会是不要迷信某一种算法。反步法严谨、滑模鲁棒、LOS简单、MPC灵活但放到真实约束下各有各的脾气。仿真研究的价值不在于证明“我的算法误差最小”而是通过对比看清每种方法在什么条件下成立、在什么条件下失效以及如何通过结构和参数调整让它们变得实用。如果你正在复现类似的工作我建议先跑通LOS基线再上反步法最后折腾MPC。这一步一步递进能帮你少走很多弯路。上面提到的Simulink模型结构和S函数框架都可以照着搭遇到问题时多往“延迟、饱和、角度wrap、初始条件”这几个方向排查。希望能给同样做欠驱动AUV仿真的你一些真正有用的参考。
