无人船编队 无人车编队 MPC 模型预测控制 多智能体协同控制 一致性 MATLAB 无人车 USV 带原文献

一、代码核心定位与应用场景

本套MATLAB源码针对自主水下航行器（AUV）、无人船（USV）、无人车等多智能体系统，实现了基于事件触发机制和非线性模型预测控制（MPC）的编队跟随控制。核心目标是通过分布式协同策略，使多智能体在保持预设几何编队的同时，精准跟踪领航者路径或预设参考轨迹，并通过事件触发通信减少数据交互量，提升系统通信效率。

代码支持圆形、三角形等多种编队构型，可直接复现参考文献中"受输入约束的多智能体协同路径跟踪"场景，适用于海洋探测、智能交通等多智能体协同作业领域。

二、核心文件组成与功能解析

1. 主控制文件

（1）`MPC_CPF_5V_Circular.m`——圆形编队控制主程序

• 功能描述：实现5个智能体的圆形编队协同控制，是代码核心执行入口。
• 关键流程：
• 初始化配置：定义智能体初始状态（位置、航向角）、圆形路径参数（半径30m-42m递增）、控制约束（速度0.2-2m/s，航向率±0.2rad/s）及通信参数（事件触发阈值η=0.1e^(-0.2t)+5e-3）。
• 协同控制逻辑：
  matlab
% 路径跟踪误差计算
[epf, ~] = epf(vehicle, p, pd, psid, kappa);
% 事件触发判断（当估计误差超阈值时通信）
trigger = abs(gammahati - gammai) > eta;
% 一致性控制律（同步路径参数）
uci = -kc tanh(l (gammai - sum(neighbor_gamma)));

• MPC控制器调用：每0.2s采样周期调用mpcController求解带约束的优化问题，输出航向率和路径参数更新率。
• 状态更新：通过RK4积分器更新智能体位置、航向及路径参数，循环至仿真结束（201s）。

（2）`MPC_CPF_5V_Triangular.m`——三角形编队控制主程序

• 功能描述：实现5个智能体的三角形编队控制，与圆形编队程序的核心差异在于：
• 路径类型：采用平行直线路径，定义为pd = [a*(gamma-c); d]（a=50m为路径长度系数，c/d为偏移参数）。
• 仿真参数：缩短仿真时长至81s，调整初始位置分布以形成三角形构型。

2. 控制算法模块

（1）`mpcController.m`——非线性MPC控制器

• 核心功能：为每个智能体构建带输入约束的模型预测控制器，是路径跟踪的核心模块。
• 模型构建：
• 状态向量：x = [s1, y1, psie, gamma, uc]（沿路径误差、横向误差、航向误差、路径参数、协同控制量）。
• 控制输入：u = [r, u_gamma]（航向率、路径参数更新率）。
• 动力学模型：基于路径跟踪误差方程，考虑路径曲率影响：
  matlab
xdot = [
(vd+uc)hgicos(psie) - hgiug(1 - cgiy1); % 沿路径误差导数
(vd+uc)hgisin(psie) - cgis1hgiug; % 横向误差导数
r - cgihgiug; % 航向误差导数
ug; % 路径参数更新
0 % 协同控制量不变
];

• 优化目标：二次型成本函数，权重矩阵Q=diag([1 1 2 2 20])（航向误差权重最高），兼顾误差最小化与控制平滑性。
• 约束处理：显式约束速度、航向率等输入，并通过Lyapunov稳定性约束dVmpc ≤ dVnon保证闭环稳定。

（2）事件触发与一致性模块

• 事件触发机制：通过trigger = abs(gammahati - gammai) > eta判断是否触发通信，阈值η随时间指数衰减，减少稳定后的通信频率。
• 一致性控制：基于无向连通拓扑（Laplacian矩阵定义），通过分布式控制律同步各智能体路径参数，确保编队几何关系保持。

3. 可视化与数据处理模块

（1）`GTF_Simulink_PlotAUV.m`——智能体3D建模

• 功能：生成AUV/USV三维模型，包含机身（球体+圆柱体）、尾翼、螺旋桨等组件，支持颜色、姿态、缩放比例配置。
• 坐标变换：通过旋转-缩放-平移矩阵实现模型姿态与实际运动的同步。

（2）`animation_5vehicles_LMH.m`——编队动画生成

• 功能：实时绘制5个智能体的运动轨迹，支持颜色区分（黑/蓝/红/青/紫）和视频录制，动画帧率与仿真采样周期匹配。

（3）`plotjournal_5vehicle.m`——仿真结果绘图

• 输出图表：包含轨迹对比图、控制输入约束图、路径参数同步曲线、事件触发时刻标记、估计误差曲线及Lyapunov函数变化趋势，完整复现参考文献中的结果展示形式。

4. 工具函数

（1）`RK4_integrator.m`——四阶龙格-库塔积分器

• 功能：对智能体动力学模型进行数值积分，输入微分方程、初始状态和控制量，输出积分后的状态，确保状态更新精度。

（2）`save_to_base.m`——变量存储工具

• 功能：将仿真过程中的关键变量（轨迹、误差、控制输入等）保存至MATLAB工作区，方便后续分析与绘图。

三、核心技术特性

1. 双层控制架构：
   - 上层：事件触发式分布式一致性控制，同步路径参数以维持编队。
   - 下层：非线性MPC实现单智能体路径跟踪，处理输入约束。

1. 事件触发通信优势：
   - 相比周期通信减少30%-50%数据传输量（参考文献验证）。
   - 动态阈值设计兼顾收敛速度与通信效率。

1. 约束处理能力：
   - 硬约束：速度、航向率等物理限制直接嵌入MPC优化问题。
   - 稳定性约束：通过Lyapunov函数导数条件保证闭环系统渐近稳定。

1. 路径跟踪适应性：
   - 支持任意平滑路径（直线、圆等），通过路径参数化实现通用跟踪。
   - 平行传输帧变换简化误差建模，提高跟踪精度。

四、使用说明

1. 环境依赖：MATLAB R2018b+，CasADi优化工具包（用于求解MPC问题）。
2. 运行步骤：
   - 圆形编队：直接运行MPCCPF5VCircular.m。
   - 三角形编队：直接运行MPCCPF5VTriangular.m。
   - 查看结果：仿真结束后运行animation5vehiclesLMH.m生成动画，plotjournal_5vehicle.m生成分析图表。
3. 参数调整：可修改路径参数（半径、偏移量）、控制权重、约束范围等，通过工作区变量实时观察调整效果。

五、代码扩展性

• 智能体类型扩展：修改动力学模型可适配无人车（差速驱动）、无人机（添加高度控制）。
• 编队构型扩展：调整路径生成函数可实现矩形、菱形等自定义编队。
• 通信拓扑扩展：修改Laplacian矩阵可切换星形、全连通等通信结构。

本代码完整复现了文献提出的控制算法，代码结构清晰，注释详尽，可作为多智能体编队控制研究的基础框架或工程应用原型。