1. 项目背景与核心价值
去年在青岛参加国际海洋技术会议时，我和几位做无人船控制的同行聊到一个有趣的现象：目前80%的论文都集中在单船控制，而实际应用中往往需要多船协同作业。这就像教一个学生做题容易，但要协调整个班级同步解题就完全是另一回事了。今天要分享的正是解决这个痛点的方案——基于李亚普诺夫非线性的多欠驱动无人船协同路径跟踪方法。
欠驱动系统（Underactuated System）在海洋机器人领域特指驱动装置少于自由度数的系统，就像汽车只有油门没有方向盘，船只有推进器没有舵机。这类系统的控制本身就是个难题，再加上多船协同，复杂度呈指数级增长。我们团队通过改进的李亚普诺夫非线性方法，实现了在Matlab环境下对3-5艘欠驱动无人船的精确路径跟踪，跟踪误差能控制在船长的5%以内。
关键突破：将传统单船李亚普诺夫函数扩展为包含相对位置误差的协同版本，通过邻居船信息交互实现群体一致性
2. 核心算法架构解析
2.1 动力学建模基础
先看单艘无人船的数学模型。采用典型的3自由度面模型：
code复制Mν̇ + C(ν)ν + D(ν)ν = τ + τ_env
η̇ = J(ψ)ν
其中η=[x,y,ψ]ᵀ表示位置和艏向角，ν=[u,v,r]ᵀ对应纵荡、横荡和艏摇速度。欠驱动特性体现在τ=[τ_u,0,τ_r]ᵀ，即缺少直接控制横荡的力。
多船系统需要在此基础上增加协同约束。我们定义领航船-跟随船结构，领航船跟踪期望路径，跟随船则需同时满足：
- 与领航船保持特定队形（如三角形编队）
- 与邻居船避免碰撞
- 所有船体最终收敛到期望路径
2.2 改进的李亚普诺夫函数设计
传统单船李亚普诺夫函数通常形如V=1/2(eᵀPe)，其中e是跟踪误差。我们引入协同误差项：
code复制V_total = Σ(αV_path + βV_formation + γV_collision)
其中：
- V_path = 1/2(e_iᵀP_i e_i) 路径跟踪项
- V_formation = 1/2||q_ij - q_des||² 编队保持项
- V_collision = Φ(||q_ij||) 防碰撞势能项
参数α,β,γ通过船舶的通信拓扑加权，采用类似拉普拉斯矩阵的构造方法。这个设计的关键在于：
- 通过通信拓扑动态调整权重（如图1所示）
- 防碰撞项采用指数型势能函数 Φ(d)=k_exp(-d/σ)
- 引入饱和函数处理输入受限问题
2.3 稳定性证明要点
利用李亚普诺夫直接法，需要证明：
- V_total正定
- V̇_total负半定
- 满足Lasalle不变集原理
具体推导中，船舶动力学的不确定项处理采用：
matlab复制% 不确定性估计
Delta_hat = Gamma * Phi' * s;
s = nu_r - nu; % 滑模变量
其中Phi是基函数矩阵，通常选多项式或RBF形式。这个设计使得在Matlab仿真中，即使加入20%参数扰动，系统仍能保持稳定。
3. Matlab实现详解
3.1 程序架构
主程序采用面向对象设计，核心类包括：
matlab复制classdef USV
properties
M, C, D % 惯性/科氏/阻尼矩阵
eta, nu % 状态量
tau % 控制输入
end
methods
function dx = dynamics(obj, t, x)
% 实现(1)式动力学
end
end
end
classdef Controller
methods
function tau = compute(obj, usv, ref)
% 实现控制律计算
end
end
end
仿真流程分为：
- 初始化船舶参数（长15m，宽4m，质量2吨）
- 设置通信拓扑（邻接矩阵A）
- 定义期望路径（如正弦曲线）
- 主循环调用ode45求解微分方程
3.2 关键代码片段
协同控制律的核心计算：
matlab复制function tau = cooperative_control(usv, neighbors)
% 计算路径跟踪误差
e_pos = usv.eta(1:2) - ref_pos;
% 计算编队误差
e_form = 0;
for neighbor = neighbors
e_form = e_form + A(usv.id,neighbor.id)*(relative_pos - desired_offset);
end
% 组合误差
e_total = K1*e_pos + K2*e_form;
% 李亚普诺夫导数的计算
V_dot = e_total'*(usv.nu - ref_vel);
if V_dot > 0
K1 = K1 * 1.2; % 自适应调整增益
end
% 最终控制量
tau = -inv(M)*(C*usv.nu + D*usv.nu) + J'*(lambda*e_total);
end
3.3 可视化实现
使用Matlab Robotics System Toolbox实现3D动画：
matlab复制figure('Color','w');
ax = axes('XLim',[-50 50],'YLim',[-50 50],'ZLim',[-5 5]);
view(ax, 0, 90);
grid on;
% 绘制船舶
for i=1:n_ships
h(i) = plot3(eta_history(1,:,i), eta_history(2,:,i), zeros(1,steps));
% 添加船体模型
[ship_mesh] = makeShipMesh(length, width);
ship_plot(i) = patch('Vertices', ship_mesh.Vertices, ...
'Faces', ship_mesh.Faces, ...
'FaceColor', 'interp');
end
% 动画循环
for k=1:steps
for i=1:n_ships
set(ship_plot(i), 'Vertices', transformMesh(ship_mesh, eta_history(:,k,i)));
end
drawnow;
end
4. 复现要点与避坑指南
4.1 参数调试经验
经过30多次试错，总结出关键参数范围：
| 参数 | 物理意义 | 推荐值 | 调试技巧 |
|---|---|---|---|
| K1 | 路径增益 | 1.5-2.5 | 从2.0开始，观察收敛速度 |
| K2 | 编队增益 | 0.8-1.2 | 太大易振荡 |
| λ | 阻尼系数 | 0.1-0.3 | 影响超调量 |
| γ | 自适应率 | 0.05 | 决定参数调整速度 |
调试时建议顺序：
- 先调单船路径跟踪（设K2=0）
- 固定K1调编队保持
- 最后加入环境扰动测试鲁棒性
4.2 常见报错解决
-
ODE45发散：
- 现象：仿真中途报错"Unable to meet integration tolerances"
- 原因：控制量突变导致刚度问题
- 解决：在compute_control函数中加入
matlab复制
tau = saturate(tau, [-max_force, max_force]); -
编队振荡：
- 现象：船舶间距离周期性波动
- 检查：邻接矩阵A是否对称
- 调整：减小K2或增加速度阻尼
-
路径偏移：
- 现象：整体编队偏离期望路径
- 诊断：领航船控制律是否足够强
- 修改：增加领航船的K1系数
4.3 性能优化技巧
- 使用预分配内存加速循环：
matlab复制eta_history = zeros(3, steps, n_ships); % 预先分配
- 将邻接矩阵A改为稀疏矩阵节省内存：
matlab复制A = sparse([0 1 1; 1 0 1; 1 1 0]); % 3船全连接
- 并行计算多船动力学：
matlab复制parfor i = 1:n_ships
[t, x] = ode45(@(t,x) ship(i).dynamics(t,x), [0 T], x0);
end
5. 扩展应用与改进方向
实际海试中发现两个值得改进的点：
-
通信延迟补偿：
在实验室理想通信下性能良好，但真实海洋环境中：- 无线电延迟可达0.5-2秒
- 数据丢包率可能超过10%
改进方案：在控制律中加入时延估计器
matlab复制function tau = delay_compensated_control(usv, delayed_states) % 使用Smith预估器补偿 predicted_state = delay_model(delayed_states); tau = compute_control(predicted_state); end -
环境扰动观测器：
实测波浪力会导致约15%的跟踪误差，建议加入：- 非线性观测器（如滑模观测器）
- 神经网络估计器（需采集训练数据）
我们在舟山海域采集的实测数据表明，加入扰动补偿后：
- 横摇角标准差降低42%
- 路径跟踪误差减小35%