前阵子有个做船舶控制方向的朋友拿了个标题来问我说希望把一个 IEEE 会议的算法主题复现成可跑的 Matlab 程序【IEEE会议】基于李雅普诺夫非线性的多欠驱动无人船协同路径跟踪Matlab程序带配套复现参考文献。我一看“多欠驱动”和“协同路径跟踪”这两个词组合在一起就意识到这活儿不简单它跟普通的单船航迹控制完全不是一个量级。这个标题概括的其实是当前无人船USV控制里非常典型的一类问题多艘船既要各自沿着期望路径走又要保持队形协同而且每艘船本身还是欠驱动系统——没有侧向推进器只能靠推进力跟艏摇力矩去控制。解决思路通常就是用李雅普诺夫稳定性理论去设计非线性控制器再配合制导律和协同协议把整个闭环系统的稳定性证明清楚。这类程序适合船舶控制、无人系统编队、非线性控制方向的研究生或者工程师去学习既能当成论文复现的参考模板也可以直接在此基础上改成自己的仿真框架。1. 项目概述多欠驱动无人船协同路径跟踪到底在解决什么问题1.1 从标题拆出四个关键技术词先不要急着打开 Matlab先把标题里每个词都抠出来看清楚因为它们决定了整个程序的设计方向。第一个词是“多欠驱动无人船”。无人船常见模型是三自由度运动纵荡、横荡、艏摇。但多数情况下为了简化结构和降低成本船上只装了主推进器和艏摇控制舵没有侧向推进器。换句话说横荡方向是没有直接控制力的系统控制输入个数小于自由度个数这就是“欠驱动”。第二个词是“协同”。这不是各跑各的而是多艘船之间要有耦合通常通过通信拓扑共享路径参数、速度信息让所有船在路径上的“进度”保持同步同时保持期望的队形间距。第三个词是“路径跟踪”。路径跟踪跟轨迹跟踪有区别轨迹跟踪要求严格按时间走完一条曲线路径跟踪只要求沿着空间路径走速度可以由制导律自行调节。这个区别非常重要也是设计时能松一口气的地方。第四个词是“李雅普诺夫非线性”。这是控制器的理论基础。设计一个候选函数 V让它正定再让它的导数负定或半负定从而证明系统误差收敛。在模拟程序里这一部分不只是一个数学证明还会直接体现为控制律中各项增益和误差组合的选取方式。这四个关键词串起来程序要解决的问题就很清晰了构造控制器让每一艘欠驱动无人船都收敛到自己的期望路径上同时通过协同项让编队参数同步整个过程用李雅普诺夫理论保证稳定性。1.2 这类程序的实际用途与适用人群这类仿真程序在学术界和工程界都经常出现。对科研人员来说它是验证算法有效性的第一站对于做实际项目的人来说它是算法走板之前必须过的一道预实验尤其是海上通信代价高、实船试验成本大先在 Matlab 里把控制器、制导律、扰动场景全部验证一遍比直接下水要稳妥得多。如果你刚接触这个方向这套程序能帮你回答几个很实际的问题欠驱动船的横荡速度到底是怎么被间接控制住的多船同步到底靠的是哪几个信号控制器增益调大调小会在曲线上表现出什么差异如果把这些细节都搞明白了你去看同类 IEEE 文献时基本上不会再被大段推导吓住。我个人的建议是复现时不要只满足于“跑通出图”要按论文里给出的稳定性证明去对照程序里的每一项。程序里某个增益为什么能镇定系统往往都能在 V 导数的推导中找到对应关系。2. 数学模型搭建把“欠驱动”三个字吃透2.1 三自由度模型的建立与欠驱动含义无人船运动学模型通常建立在两个坐标系下惯性坐标系和船体坐标系。忽略垂荡、横摇和纵摇只考虑水平面运动时运动学方程如下x_dot u·cos(ψ) - v·sin(ψ) y_dot u·sin(ψ) v·cos(ψ) ψ_dot r这里 (x, y) 是船在惯性系中的位置ψ 是艏摇角u 是纵荡速度v 是横荡速度r 是艏摇角速度。动力学方程常用简化形式m11·u_dot m22·v·r - d11·u τu m22·v_dot -m11·u·r - d22·v m33·r_dot (m11 - m22)·u·v - d33·r τr仔细看第二行v 的动态方程里没有控制输入只有 Coriolis 项和阻尼项。这意味着你只能通过改变 u 和 r让船体转动产生横向水流作用力间接改变 v。这个特性很像开车车子没法直接横向平移只能靠方向盘和油门配合让前轮转向来改变横向运动。这种欠驱动特性给控制器设计带来了本质性约束。你不能简单地对 v 单独设计一个镇定律而必须考虑整个系统的几何约束。很多初学者在建模时忽略了这个约束直接把 v 当成了有控制力的变量最后仿真出来的控制输入明显不合理或者在实船上根本无法执行。2.2 控制架构制导、控制、协同三层分工对于协同路径跟踪这种复杂任务尽量不要在一个模块里把所有逻辑写完最好把控制系统拆成三个层次制导层、控制层、协同层。制导层负责把“船当前位置”和“期望路径”之间的几何误差转换成期望航向角。最常用的方法是 LOSLine of Sight制导。传统 LOS 就是让船始终瞄着期望路径前方一个前视距离处的虚拟点走这个前视距离如果固定响应快慢会有明显差别。前视距离太大船“切”回路径的角速度就慢太小又容易在路径附近来回震荡。控制层负责跟踪制导层给出来的期望速度 u_d 和期望艏摇角速度 r_d。这一层是李雅普诺夫方法的重头戏常用反步法设计控制律 τu 和 τr把跟踪误差一步步反向解出来最后保证误差动态收敛。协同层则负责船与船之间的同步。最简单的是 leader-follower 结构选定一条虚拟路径作为领航者或者指定其中一艘船作为 leader其他船通过通信获取路径参数信息然后各自调整自己的前进速度让路径参数误差收敛到一个小邻域内。这三层合在一起程序里可以对应三个独立的函数模块方便单独调试。我先跑通单船的制导和控制再加协同层这个顺序能省下很多排查时间。2.3 李雅普诺夫候选函数的设计思路李雅普诺夫方法在程序里不等于“写一个函数去判断稳定性”而是控制律设计的核心依据。通常做法是先定义误差变量比如位置跟踪误差和速度跟踪误差然后选择一个形如下面的候选函数V (1/2)·z1² (1/2)·z2² (1/2)·z3²其中 z1、z2、z3 是不同层级的误差。对 V 求导代入控制律想办法让 V_dot 变成一个负定项加上一些有界残差项。如果能做到这一点就可以说闭环系统是一致最终有界的。在协同场景里V 里通常还会加一个协同误差项比如各船路径参数之间偏差的平方和。这一步在程序里看起来很不起眼但它是实现“协同”的关键。没有这一项编队中各船只是各自跟踪各自的路径参数一旦错开就很难自动纠正。设计候选函数时我建议先跟随参考文献的推导使用最常见的二次型候选函数。等程序跑通后如果遇到输入饱和或扰动过大的情况再考虑加入 tanh 饱和函数或者非线性增益不要一开始就追求复杂形式。3. 从公式到Matlab代码控制器与协同策略的实现要点3.1 制导律实现与航向角包裹问题制导层的目标是根据横向误差计算期望航向角 ψ_d。假设期望路径在当前位置的切向方向为 γ_p(θ)横向误差为 ye那么期望艏向角可以用 LOS 形式表示ψ_d γ_p(θ) atan(-ye / Δ)其中 Δ 是前视距离。程序里最容易出问题的是 tan 反函数的象限处理。如果直接使用 atan艏向角很可能在 π 和 -π 之间跳变导致控制输入瞬间大幅振荡。建议在 Matlab 里统一用 atan2并且在角度差处做如下的角度包裹处理wrap_to_pi (angle) mod(angle pi, 2*pi) - pi;这个函数要放在所有角度计算之后尤其是航向角误差、艏摇角速度误差的计算。我见过很多复现程序跑出来曲线发散不是控制律写错而是角度没有做 wrap导致误差在 ±π 附近来回跳。LOS 制导里的前视距离 Δ 也可以做成动态变化当横向误差较大时用小 Δ 让船快速转向回归路径当横向误差较小时用大 Δ 降低震荡。这种自适应 LOS 在参考文献中很常见实现起来就是给 Δ 一个关于 ye 的指数函数。3.2 反步控制器与虚拟速度律的编程映射经典反步法的思路在 Matlab 编程中其实不复杂但需要把推导中的中间变量在代码里拆清楚。假设位置误差已经转化到 Serret-Frenet 坐标系下得到沿路径方向的误差 se 和横向误差 ye。第一步是设计虚拟控制量。比如期望纵荡速度 u_d 可以设计成u_d k1·se / cos(ψ_d - γ_p)这里要注意 cos(ψ_d - γ_p) 不能接近零否则会出现奇异。解决办法是加一个小的常数偏差或者限制期望航向角与路径切向角的夹角不超过 60 度左右。仿真中如果看到速度指令突然跳到极大值多半就是这里产生了奇异。第二步是速度跟踪误差的定义eu u - u_d er r - r_d然后构造控制律τu m11·(u_d_dot - ku·eu) - m22·v·r d11·u τr m33·(r_d_dot - kr·er) - (m11 - m22)·u·v d33·r其中 ku 和 kr 是待调的正增益。这里的 u_d_dot 和 r_d_dot 可以解析求得也可以用数值差分近似。解析求导在程序里更干净但推到 w 误差项时会麻烦一点。我通常采用一个折中制导量解析求导速度虚拟控制量用差分近似只要差分步长足够小仿真结果几乎没区别。程序里要把 m11、m22、m33、d11、d22、d33 这些水动力参数统一定义在一个结构体里不要散落在各处。后续做参数扰动分析时只需要改这个结构体很大程度能避免改漏参数导致结果对不上。3.3 多船协同编队的程序框架多船协同的编程方式和拓扑结构直接相关。最常见的 leader-follower 结构里leader 船跟踪标称路径follower 船在标称路径上做偏移再通过通信获取 leader 的路径参数 θ_leader。实现协同的关键变量是路径参数速度 θ_dot。如果各船都按自己的节奏走队形会越拉越散。于是需要设计协同速度律θ_dot_i u_i / sqrt(x_θ² y_θ²) - k_c·(θ_i - θ_j)这里的 k_c 是协同增益θ_i - θ_j 是相邻船的路径参数误差。如果我把这个误差直接乘到速度指令里就相当于让走得快的船稍微等一下走得慢的船稍微追一下。如果 follower 不止一艘通信拓扑可能是链式、环式或全连接。程序里用一个邻接矩阵来表示拓扑关系最直观。比如三艘船全互通邻接矩阵是adj [0 1 1; 1 0 1; 1 1 0]然后在循环里遍历获取邻居信息累计同步误差。需要提醒一点协同项加入之后整个闭环稳定性必须重新考虑。不能只证明单船稳定就直接把协同项叠加进去。论文如果写了完整的协同稳定证明最好先读懂证明里 V 多出来的那部分再去调 k_c。否则 k_c 调太大会出现船队“为同步而同步”单船路径误差反而明显变差。4. 仿真场景搭建、参数调试与结果判读4.1 仿真场景与关键参数设置复现程序时我一般先搭一个最基础的场景两艘船走同一条闭合圆形路径leader 在初始位置有 5 米的前向偏差follower 在初始位置有 3 米的横向偏差。圆形路径的好处是几何简单方便观察路径跟踪误差的收敛过程而且切向角和位置量都是周期性的能充分检验制导律处理角度包裹的能力。水动力参数参考常用的标准模型大致范围如下参数符号典型数值说明船体惯性质量m1125.8 kg纵荡附加质量等效船体惯性质量m2233.8 kg横荡附加质量等效船体惯性质量m332.76 kg·m²艏摇附加惯性矩纵荡阻尼d110.72线性阻尼系数横荡阻尼d220.95线性阻尼系数艏摇阻尼d330.5线性阻尼系数这组参数来源于常见的小型无人船模型做仿真足够典型。实际论文可能给出具体数值建议优先使用参数所在论文里列出的原始值。如果没有再以这类典型值作为初始设置。仿真时长一般给 200 到 300 秒。求解器用 ode45但要注意把控制律写成单独函数不要全部塞进一个巨型脚本。仿真步长的控制也很讲究固定步长模式下步长取 0.1 秒在大部分算例里够用但遇到高增益控制器时建议降到 0.05 秒再观察结果。外部扰动不能从一开始就加。先做无扰动条件下的确定性仿真等所有曲线都收敛了再加海流或随机风力。这个顺序非常重要因为如果一开始就带扰动一旦发散根本分不清是控制器问题还是扰动场景设置问题。4.2 重点盯哪几条曲线程序跑完后曲线不要只看位置轨迹图那只能反映部分问题。建议统一输出以下几组数据并分别画图第一是路径跟踪误差曲线。包括沿路径误差 se、横向误差 ye、艏向角误差 ψe这三条曲线收敛到零附近的小邻域说明制导层和控制层的设计正确。第二是编队同步误差曲线。以 leader 和 follower 的路径参数差值为例这个参数差应当随时间收敛而不是持续线性增加。如果它一直增加说明协同速度律没有起到作用或者增益太小。第三是控制输入曲线。τu 和 τr 的数值要落在合理范围不能高频抖振也不能长时间贴着饱和上限。第四是李雅普诺夫函数 V 的实时值。这个在论文里不会直接画出来但我个人强烈推荐在仿真过程中把 V 值存下来。如果你设计的控制律正确V 曲线应该单调下降至少在总体趋势上递减。如果中间有一段明显上升说明中间过程某个误差变量被放大需要回到对应时间点检查是哪个误差爆发了。4.3 参数调优的几条实测经验增益参数不是越大越好。路径跟踪控制律中的位置误差增益 k1 如果取得太大船会在路径附近产生高频摆动看起来像“蛇形走位”实际上非常消耗推进器能量。一般先从小增益开始比如 k1 0.5观察误差收敛速度再逐步往上加。前视距离 Δ 的取值通常和船长相关一般的经验是取船长的 2 到 4 倍。取太小船会过于激进地修正横向误差导致路径附近振荡取太大收敛速度明显变慢尤其在圆形路径的弯道部分容易出现内切或外偏。协同增益 k_c 的调法不太一样。它主要影响编队同步速度和单船跟踪误差之间的平衡。k_c 从 0.1 开始如果同步误差曲线收敛太慢就加倍直到同步误差能在 50 秒内收敛到 0.1 米以内。但如果 k_c 超过某个上限后单船的横向误差反而会增大这时就必须回到协同项的设计去检查而不是继续加增益。5. 论文复现中常见的坑与排查技巧5.1 仿真发散和误差不收敛的典型原因如果你在复现时发现仿真直接发散不要先怀疑论文算法有问题先检查下面几个高频成因。第一个是角度未做包裹处理。艏向角误差在 ±π 之间跳变会导致控制输入符号翻转船会像疯了一样原地打转。这种问题看轨迹图非常明显船在期望路径附近画圈角度差曲线呈锯齿状。第二个是虚拟控制律导数求错。反步法里需要对 u_d 和 r_d 求导很多复现版本会用 diff 函数对离散序列求差分但差分步长如果和控制器步长不一致会产生数值噪声然后噪声被高增益放大。建议用解析求导或者使用带滤波的数值微分。第三个是初始状态设置不当。比如初始艏向角和期望航向角相差过大超过 90 度甚至接近 180 度制导律可能暂时失效。这在实际中很常见解决办法是把初始艏向角设置在期望航向角附近 30 度以内先验证控制器收敛性再逐步加大初始误差考验鲁棒性。第四是位置误差坐标系搞混了。路径点的切向角和法向角如果弄反横向误差的符号会反整个闭环等于在正反馈状态下运行必发散。排查时可以在仿真早期打印出 se、ye 和 ψe 的具体数值看它们相对初始误差的符号和量级是不是符合直觉。我把常见问题整理成了一张排查表方便在仿真失败时逐项对照现象可能原因排查方法轨迹画圈艏向角误差未做 wrap检查所有误差量是否调用 wrap_to_pi速度指令爆炸制导律奇异检查 cos(ψ_d - γ_p) 是否接近零控制输入高频抖振增益过大或数值差分噪声降低增益或改解析求导编队误差持续增大协同增益太小或拓扑错误打印邻接矩阵检查参数同步项单船收敛但编队发散协同层和路径跟踪层耦合不足检查协同项是否真正进入速度指令5.2 编队速度不同步的排查路径编队不同步的现象经常是leader 已经绕了大半圈follower 还在起点附近缓慢推进。绝大多数情况不是控制律失效而是路径参数 θ 的定义不一致。每艘船的路径参数可能是弧长、归一化参数或者时间参数。如果 leader 用的是弧长follower 用的是归一化参数两者的数值范围完全不同直接相减得到误差毫无意义。解决方法是统一路径参数的物理含义或者在协同误差计算前做一次尺度归一化。另一个常见问题是通信拓扑里存在孤立节点。如果你的编队有 3 艘船但邻接矩阵里第 2 艘船和第 3 艘船没有任何连接那么它们的协同误差永远是零同步效果自然为零。这种情况在程序里不容易看出来因为图表上 leader 和 follower 各走各的轨迹单独看都很正常。解决办法是在每次迭代时对邻接矩阵做一个连通性检查或者至少在初始化时打印一次拓扑信息。还有一个容易被忽略的坑协同项加入速度律后路径参数的更新方程也要相应修改。有些程序只改了控制输入公式却忘了路径参数的导数是基于合成速度来计算导致协同指令下发后几乎没有反应。路径参数更新公式里要显式引用协同速度指令而不是继续使用单船的纵向速度。5.3 复现工作的通用心得这篇参考文献的复现工作量并不在于看懂某个公式而在于把连续的数学推导拆成离散的数字仿真。我复现这类程序时有一个固定的推进顺序先读论文搞清符号定义再手写一个极简控制器不包含任何扰动和协同项只验证单船路径跟踪。跑通之后加扰动最后加协同。每一步都保存一个可独立运行的版本。版本管理这块强烈建议不要只用“最终版”、“最终版2”这种命名。用仿真日期加场景描述比如“20240510_circle_2ship_no_disturbance.m”这样出了问题还能回溯到对应配置。另外控制程序的模块化比很多人想象的重要。制导律、动力学模型、控制器、协同协议最好写成四个独立函数。这样可以单独对制导律做开环测试也可以只保留协同层去跑理想运动模型。最怕的是一个脚本从头走到尾几百行代码里既有控制又有绘图改一处参数要全局搜索既费时间又容易漏。在我个人的复现经验里还有一个小技巧值得分享把李雅普诺夫候选函数 V 的每项都单独存下来比如 z1_squared、syn_error_squared而不是只存 V 的总值。这样当控制器在某个瞬间表现得奇怪时你可以从每项的变化曲线里直接看出是跟踪误差在恶化还是协同误差在主导不必反复猜测。这个方法几乎每次都能帮我快速定位问题。这套流程跑下来之后你会发现原来标题里那串很长的概念组合最终落到代码层面就是几个函数、十几个增益、一组曲线和几处角度包裹的细节。能把这些细节稳稳拿住再回去看任何一篇基于李雅普诺夫方法的协同控制论文都会觉得思路清晰很多。
