前段时间在复现一篇关于欠驱动USV无人水面艇路径跟踪的顶刊论文时我踩了不少坑也收获很大。最开始我照着论文公式用MATLAB脚本搭了一个纯数值仿真能跑但总觉得差点意思后来把整个方案移植到Simulink里才发现很多在公式里看不出来的问题比如代数环、艏向角跳变、增益选多大才稳。这里不绕弯子直接聊怎么用Lyapunov非线性控制方法配合Simulink把路径跟踪做扎实从数学模型、控制律设计到建模调参按我实际复现的顺序捋一遍所有代码和参数都能直接用。这篇文章适合正在做USV、AUV或者地面移动机器人控制的学生和工程师尤其是想从PID/线性控制转向非线性控制、又不太清楚Simulink里怎么落地的人。跟着跑一遍你会发现欠驱动船舶的路径跟踪没有想象中那么玄关键是把模型写对、把控制器结构理清、把Simulink细节处理好。1. 先把欠驱动USV的数学模型吃透1.1 为什么说“欠驱动”是难点所在欠驱动这个词翻译成大白话就是你想控制的自由度数量比实际能用的执行器数量多。对绝大多数USV来说尾部只有一个螺旋桨和一个舵或者采用两个差速推进器能给的物理量基本只有纵向推力τu和转艏力矩τr。但船在水面上有纵荡surge、横荡sway和艏摇yaw三个自由度也就是说横荡方向没有直接执行器你想让船像小汽车一样横向平移物理上做不到。这个特性直接导致了两件麻烦事。第一系统的动力学模型是非完整约束系统不能用普通的线性状态反馈直接设计控制器很多经典的线性方法在这里会失效。第二期望路径跟踪任务必须在运动学层面“间接”实现比如通过调整艏向角让船产生横向速度分量来逼近期望路径。这也是Lyapunov方法在USV控制里如此流行的根本原因它能直接处理这种非线性、强耦合、欠驱动的系统结构。1.2 3自由度运动学与动力学方程在做仿真之前首先要约定坐标系。我习惯用两个系大地固定坐标系惯性系和船体坐标系随动系。位置和艏向角在大地坐标系下表示速度分量在船体坐标系下表示。运动学方程如下x_dot u * cos(psi) - v * sin(psi) y_dot u * sin(psi) v * cos(psi) psi_dot r其中x、y是船在大地坐标系下的位置psi是艏向角u是纵荡速度v是横荡速度r是艏摇角速度。动力学方程采用常用的简化形式m11 * (u_dot - v * r) tau_u - d11(u) tau_wu m22 * (v_dot u * r) -d22(v) tau_wv m33 * r_dot tau_r - d33(r) tau_wr这里的m11、m22、m33是包含附加质量的惯性参数d11(u)、d22(v)、d33(r)是水动力阻尼项tau_wu、tau_wv、tau_wr是风浪流等外部干扰。很多论文里阻尼项会写成线性加非线性组合比如d11(u) du d_uu * |u| * u d22(v) dv d_vv * |v| * v d33(r) dr d_rr * |r| * r我第一次复现时就在这里吃过亏把阻尼全当成线性项处理仿真结果表面看没问题但一旦把期望速度提高或者加入扰动系统就变得过于理想化控制器的鲁棒性完全看不出来。所以建议从一开始就保留非线性阻尼Simulink里实现起来不费事无非多乘一个绝对值项。1.3 仿真模型参数怎么选论文复现最头疼的就是参数从哪来。我用的是一组公开论文里常见的小型实验USV参数长度1.4米左右质量约23.8公斤适合实验室环境下做运动控制验证。这样一组参数既不会过于理想也不会因为太多水动力系数而难以梳理。参数符号数值单位含义m23.8kg船体质量I_z1.76kg*m^2艏摇转动惯量X_u_dot-2.0kg纵荡附加质量Y_v_dot-10.0kg横荡附加质量N_r_dot-1.0kg*m^2艏摇附加转动惯量X_u-2.0kg/s纵荡线性阻尼Y_v-10.0kg/s横荡线性阻尼N_r-1.0kg*m^2/s艏摇线性阻尼X_uu-1.5kg/m纵荡非线性阻尼Y_vv-20.0kg/m横荡非线性阻尼N_rr-10.0kg*m^2艏摇非线性阻尼注意m11、m22、m33并不是直接等于m和I_z而是包含了附加质量m11 m - X_u_dot m22 m - Y_v_dot m33 I_z - N_r_dot由于附加质量是负值减负等于加所以m11、m22、m33比原始质量更大。这个细节如果漏掉控制律里的前馈补偿项就会差一截仿真时可能还能凑合做实船验证时误差会被放大得很明显。2. Lyapunov路径跟踪控制律是怎么设计出来的2.1 路径跟踪误差模型和LOS导引路径跟踪和轨迹跟踪是两个概念。轨迹跟踪要求船在指定时间到达指定位置路径跟踪只要求船收敛到期望路径上不关心时间。做欠驱动USV路径跟踪时工程上几乎首选LOSLine-of-Sight导引思路因为它直观、参数少、配合Lyapunov函数能直接给出稳定性证明。先定义期望路径为一条参数化曲线这里用最常用的直线路径举例。设直线路径方向角为alpha路径起点为(x0, y0)那么船到路径的切向误差x_e和法向误差y_e可以写成x_e cos(alpha) * (x - x0) sin(alpha) * (y - y0) y_e -sin(alpha) * (x - x0) cos(alpha) * (y - y0)x_e表示船沿路径方向超出/落后起点的距离y_e表示船偏离路径的距离也就是我们最关心的横向跟踪误差。LOS期望艏向角取psi_d alpha - atan2(k_y * y_e, Delta)这里的Delta是一个前视距离k_y是横向误差增益。当船偏在路径右侧时y_e为正psi_d会小于alpha也就是说船要往左转回来这是非常直觉的控制逻辑。Delta选得大趋近路径时艏向变化平缓、超调小但收敛慢Delta选得小收敛快但容易振荡。一般推荐取船长的3到5倍作为初值。2.2 反步法搭建控制律得到期望艏向之后还需要设计真实的控制输入tau_u和tau_r。这里用反步法Backstepping来处理。先定义误差变量psi_tilde psi - psi_d u_tilde u - u_du_d是期望纵荡速度可以根据任务自由设定比如速度1 m/s。然后构造候选Lyapunov函数V 0.5 * y_e^2 0.5 * psi_tilde^2 0.5 * u_tilde^2对V求导代入运动学和动力学方程目的是让V_dot尽量呈现出负定形式。经过一番整理细节建议对照具体论文不同论文对符号约定略有差异可以得到如下形式的控制律tau_u m11 * (u_dot_d - k_u * u_tilde) - m11 * v * r d11(u) tau_r m33 * (psi_d_dot_dot - k_r * (r - psi_d_dot) - k_psi * psi_tilde) - (m22 - m11) * u * v d33(r)其中k_u、k_r、k_psi是正的控制增益psi_d_dot和psi_d_dot_dot是期望艏向的一阶和二阶导数在实际代码里通过数值差分或者解析求导得到。这个控制律的逻辑是很清晰的tau_u补偿纵荡通道的动力学让实际纵荡速度收敛到期望速度tau_r既做艏向误差的比例反馈又补偿艏摇通道的非线性耦合项和阻尼项让船头稳稳对准期望艏向。把这套控制律代入Lyapunov函数只要增益都取正值在无扰动条件下V_dot就严格小于0系统渐近稳定。2.3 稳定性解释和使用心得我经常被问一个问题既然PID也能做路径跟踪为什么非要强调Lyapunov我的回答是PID不是不能用而是你没法系统地回答“为什么PID参数取这个值能稳、换一个场景还能不能稳”这个问题。Lyapunov方法通过构造V函数把系统的收敛性变成一个数学上可验证的结论哪怕模型不精确增益选择也有明确的方向。当然这套基础控制律在实际中不是万能的。顶刊论文通常会在基础反步法上再做改进比如加自适应参数去在线估计水动力系数或者加扰动观测器去补偿风浪流干扰。我在复现时先把基础版本跑通再逐步加入扰动补偿项这样出现问题很容易定位不会一开始就被一堆公式淹没。3. Simulink建模把控制律变成能跑的模型3.1 顶层模型结构设计Simulink建模不是把公式一股脑拖进去用加法器拼而是要有清晰的层次。我习惯把整个模型分成五个模块路径生成模块、误差计算模块、LOS导引模块、控制器模块、USV动力学模块最后统一送进Scope或To Workspace做后处理。顶层模型相当于“控制器-被控对象-反馈”闭环结构。反馈回来的状态是x、y、psi、u、v、r六个量控制器根据这些量和期望路径信息计算出tau_u、tau_r再输入给被控对象模块。这个结构看起来简单但每个模块内部都有值得注意的细节逐个说。3.2 核心USV动力学用Level-2 S-Function实现为什么不推荐用纯Simulink积木搭动力学因为状态方程一旦写错很难排查而且写论文用的公式和Simulink模块之间没有直接对应关系。用Level-2 MATLAB S-Function把状态方程原原本本写进去公式和代码一一对应调试时清晰得多。下面是一个可以直接套用的USV动力学S-Function骨架状态量顺序为[x, y, psi, u, v, r]输入为[tau_u, tau_r]function usv_sfun(block) setup(block); function setup(block) block.NumInputPorts 1; block.NumOutputPorts 1; block.SetPreCompInpPortInfoToDynamic; block.SetPreCompOutPortInfoToDynamic; block.InputPort(1).Dimensions 2; block.OutputPort(1).Dimensions 6; block.NumContStates 6; block.SampleTime [0 0]; block.SetAccelRunOnTLC(true); block.SimStateCompliance DefaultSimState; block.RegBlockMethod(InitializeConditions, InitConditions); block.RegBlockMethod(Derivatives, Derivatives); block.RegBlockMethod(Outputs, Outputs); function InitConditions(block) block.ContStates.Data [0; 0; 0; 0; 0; 0]; function Derivatives(block) x block.ContStates.Data(1); y block.ContStates.Data(2); psi block.ContStates.Data(3); u block.ContStates.Data(4); v block.ContStates.Data(5); r block.ContStates.Data(6); tau_u block.InputPort(1).Data(1); tau_r block.InputPort(1).Data(2); % 模型参数 m 23.8; Iz 1.76; Xu -2.0; Yv -10.0; Nr -1.0; Xu_dot -2.0; Yv_dot -10.0; Nr_dot -1.0; Xuu -1.5; Yvv -20.0; Nrr -10.0; m11 m - Xu_dot; m22 m - Yv_dot; m33 Iz - Nr_dot; d11 Xu Xuu * abs(u) * u; d22 Yv Yvv * abs(v) * v; d33 Nr Nrr * abs(r) * r; % 运动学 x_dot u * cos(psi) - v * sin(psi); y_dot u * sin(psi) v * cos(psi); psi_dot r; % 动力学 u_dot (tau_u - d11 m22 * v * r) / m11; v_dot (-d22 - m11 * u * r) / m22; r_dot (tau_r - d33 (m11 - m22) * u * v) / m33; block.Derivatives.Data [x_dot; y_dot; psi_dot; u_dot; v_dot; r_dot]; function Outputs(block) block.OutputPort(1).Data block.ContStates.Data;这段代码里有一个容易踩的坑动力学方程里u_dot表达式中m22 * v * r前面是加号还是减号取决于你的惯性矩阵是怎么定义的。在不同论文里这一项的符号可能差一个负号对不上会导致开环仿真都发散。建议用一个小技巧验证只给tau_u一个常数让船直线加速观察sway方向的v是否在一个小范围内变化如果v在无激励情况下快速变大说明耦合项符号反了。3.3 LOS导引和控制器模块实现LOS导引模块我用一个MATLAB Function块实现输入是当前状态和路径信息输出是期望艏向角psi_d。function psi_d los_guidance(x, y, alpha, x0, y0, ky, Delta) y_e -sin(alpha) * (x - x0) cos(alpha) * (y - y0); psi_d alpha - atan2(ky * y_e, Delta); end控制器模块同样用MATLAB Function块。这里需要注意psi_d_dot和psi_d_dot_dot的计算常见做法是在连续系统里用传递函数近似微分或者直接在仿真中通过求导模块得到。为了简单我通常假设路径是直线时alpha不变psi_d的导数主要来源于y_e的导数可以用解析法算出。但更省事的做法是在MATLAB Function块里用sample time做数值差分配合小的延迟模块也能保证精度。控制器核心代码function [tau_u, tau_r] controller(u, v, r, u_d, psi, psi_d, psi_dot_ref, psi_ddot_ref, k_u, k_r, k_psi) m 23.8; Iz 1.76; Xu -2.0; Yv -10.0; Nr -1.0; Xu_dot -2.0; Yv_dot -10.0; Nr_dot -1.0; Xuu -1.5; Yvv -20.0; Nrr -10.0; m11 m - Xu_dot; m22 m - Yv_dot; m33 Iz - Nr_dot; d11 Xu Xuu * abs(u) * u; d33 Nr Nrr * abs(r) * r; psi_tilde wrapToPi(psi - psi_d); u_tilde u - u_d; tau_u m11 * (-k_u * u_tilde) - m11 * v * r d11; tau_r m33 * (psi_ddot_ref - k_r * (r - psi_dot_ref) - k_psi * psi_tilde) - (m22 - m11) * u * v d33 * r; end这里有两个关键细节。一是psi_tilde必须用wrapToPi把误差限制在[-pi, pi]否则艏向角跨越正负180度时控制器会突然给出巨大的反向力矩导致船在那里像个陀螺一样转个不停。二是tau_r的表达式里包含了非线性耦合项(m22 - m11) * u * v这恰恰是欠驱动系统控制里“人为制造转艏力矩”的关键部分实际效果是利用纵荡和横荡速度的耦合来帮助转向千万不能删。3.4 Solver配置和仿真参数Simulink仿真设置里我推荐使用固定步长求解器类型选ode4四阶龙格库塔步长0.01秒。为什么不选变步长因为后续如果要走代码生成、外部模式或者硬件在环固定步长是几乎唯一的选择。而且变步长在控制器切换或饱和时会突然缩小步长导致仿真时间暴增对于调参这种需要反复跑的场景非常不友好。步长选0.01是基于经验的折中再大一些比如0.05控制效果看起来还行但控制输入曲线会有明显毛刺再小0.001仿真速度变慢但精度提升有限。如果是快速验证控制器逻辑先跑0.01足够如果论文里需要漂亮的曲线最后可以降到0.001并开启过零检测。4. 调试中的坑和排查实录4.1 艏向角跳变wrapToPi不能少我在第一次仿真时眼睁睁看着船已经在沿直线路径走了但艏向控制器输出突然出现一个巨大的尖峰船猛地转了一圈又回正。查了很久才发现是psi_tilde没有做角度回绕。当实际艏向是179度、期望艏向是-179度时两者之差是358度普通减法会认为误差异常大控制器自然拼命转艏向。加了wrapToPi之后误差变成2度控制器正常动作。这个坑在实船里更危险因为实船艏向传感器比如惯导输出的角度定义五花八门有的从0到360度有的从-180到180度接口对接时一定要统一。建议在Simulink模型里单独用一个模块把所有角度信号都wrapToPi一次收口统一。4.2 增益太大导致控制发散Lyapunov方法给了稳定性结论但不代表增益可以随便给。我把k_psi设成10之后仿真直接发散速度曲线飞到天上。原因倒不是理论错了而是仿真里存在离散化误差和输入饱和限制理论上的连续系统稳定不代表离散实现也稳定。调试增益我遵循一套顺序先固定位置外环LOS里的ky和Delta把速度内环调稳再加航向环。这里给出一组我实测能稳定起步的参数之后按需求微调增益符号推荐初值作用ky1.0横向误差反馈强度Delta4.0LOS前视距离k_u2.0纵荡速度误差反馈k_psi1.5艏向误差反馈k_r2.0艏摇角速度反馈整体思路是先让u稳定在期望速度附近再让艏向跟上期望值最后观察横向误差y_e是否平滑收敛到0。如果v出现高频振荡多半是k_psi太大或者Delta太小先把Delta加大到船长的5倍以上再试。4.3 代数环问题这是Simulink仿真里一个极易出现且折磨人的问题。当被控对象状态的一部分比如x、y反馈回控制器控制器输出又直接影响被控对象状态导数时如果模型中存在“纯直通”路径Simulink会检测到代数环求解器需要迭代才能解出当前时刻的值轻则减慢仿真速度重则每次步长都要迭代失败报错“不能求解代数环”。最有效的办法是在反馈回路上加一个Memory模块或者把控制器模块设置成离散采样时间比如Sample Time为0.01这样控制器输出延迟一拍断开了代数环代价是仿真结果与纯连续系统差一个采样周期的延迟对路径跟踪这种慢动态系统来说几乎没影响。提示加了离散采样后Simulink会弹提示说模型中存在混合采样率不要直接忽略。检查一下采样时间设置是否合理尤其要确保所有模块的采样周期匹配否则后面做代码生成时会多出很多自动插入的零阶保持器模型会变得很难读。4.4 常见Simulink报错速查表报错现象常见原因解决办法“找不到数据字典xx.sldd”模型引用了外部数据字典文件目录下没有检查路径或者把参数直接硬编码在模型里“LAPACK加载错误: mllapack.dll”MATLAB安装路径或dll依赖问题重装/修复MATLAB运行库或升级到较新版本“State derivatives returned by S-Function are not finite”状态方程发散出现NaN或Inf检查增益是否过大、初始条件是否合理“Algebraic loop detected”控制器和被控对象纯直通加Memory模块或给控制器设置离散采样时间“S-Function does not have a valid C MEX S-Function”用了Level-2 M文件S-Function但环境兼容问题确认函数名与文件名一致并检查路径权限“Cannot solve algebraic loop involving ...”闭环存在迭代收敛困难调整初始条件或使用固定步长求解器这些报错我在第一次搭建时几乎全部遇到过最坑的是S-Function报错原因是文件名大小写和函数名不一致MATLAB在Windows下偶尔不敏感但在Linux或代码生成时就会报错建议从一开始就保持文件名和函数名严格一致。5. 从仿真到实船应用还能怎么扩展5.1 联合仿真、C代码生成和外部模式如果你手头有比较精确的船舶水动力模型比如通过CFD或者拘束模试验辨识出来的可以把Simulink里的USV动力学S-Function替换成联合仿真模块和船舶水动力软件或机械系统仿真工具连起来做更精细的验证。仿真调通之后控制器模块可以直接用Simulink Coder生成C代码部署到单片机或者机载电脑。这里强烈推荐在开发阶段使用外部模式External Mode它允许你在一台上位机上实时调整控制器增益不用每次改参数都重新编译烧录。具体操作是在Simulink里设置外部模式仿真把代码下载到目标硬件运行后就可以在线改参数、看曲线。我实测下来整个流程最耗时间的不是生成代码而是目标硬件尤其单片机的I/O配置建议先花一天时间把PWM输出和传感器读数的Simulink驱动模块调好再谈控制算法验证。5.2 外界干扰怎么进去基础反步法在仿真时往往太过理想因为水面上的风浪流是随机且持续的。我给模型加干扰有两种做法。最简单的是在动力学方程右侧直接加一个低频正弦扰动tau_wu 1.0 * sin(0.1 * t); tau_wv 0.5 * cos(0.15 * t); tau_wr 0.2 * sin(0.2 * t);干扰幅值一开始可以取期望推力的百分之五到十仿真时观察控制器能否让跟踪误差保持有界。升级做法是用一个二阶马尔可夫过程生成更接近真实海况的干扰或者加一个扩展状态观测器去估计并补偿扰动。多数顶刊论文会在控制器前馈部分加入扰动项我的建议是先让风浪流干扰从零开始逐步加大观察Lyapunov函数的数值仿真时可以把V值输出观察如果V始终在下降说明控制器还能扛住如果V开始上升且持续不回落就说明干扰已经超出控制器的补偿能力。5.3 路径从直线到曲线控制律要改什么直线路径是练手项目真正实用化要处理曲线路径。曲线路径的定义需要引入路径参数s误差模型从直线场景的常系数alpha变成随s变化的切向角alpha(s)质点在路径上的运动方程也要相应改。控制律主体结构不变主要是psi_d的表达式需要加上路径曲率项psi_d alpha(s) - atan2(ky * y_e c_e * ... , Delta)具体公式不同论文写法各异但核心思想是期望艏向角不仅取决于横向偏差还要考虑路径本身在往前转弯控制器需要预留一部分转艏能力。建议先把直线路径跑稳再升级到圆弧路径最后才是自由曲线一步步来不要一上来就挑战复杂路径。我在实际调试里感受最深的一点是Lyapunov方法给你的是稳定性的“下限”保证了增益在一定范围内系统必然收敛但好的响应还是要靠具体调参和模型细节打磨。比如我后来在模型干扰项加了一点点噪声一开始控制器输出抖动得厉害排查后发现是psi_d的导数用数值差分产生的高频噪声被转艏力矩放大后来改用低通滤波加上更平滑的参考路径参数化问题才解决。这种细节论文里不会写只在真正的Simulink仿真过程中才暴露出来。