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

一、代码整体架构与核心目标

1. 核心目标

本套MATLAB源码针对多智能体协同路径跟踪（Cooperative Path Following, CPF） 问题，实现了受输入约束（如速度、航向率限制）的无人船（USV）、无人车等智能体编队控制。核心目标是让多智能体沿预设空间路径行驶，同时保持指定几何编队（三角形、圆形等），并通过事件触发通信（Event-Triggered Communication, ETC）机制减少数据传输量，提升通信效率。

2. 整体架构

代码采用双层控制结构，完全遵循参考文献中“路径跟踪解耦+多智能体协同”的设计思路，架构如下：

上层：多智能体协同层（协调路径参数，维持编队）
- 核心功能：通过分布式一致性控制同步各智能体路径参数，基于事件触发机制实现按需通信
- 关键模块：一致性控制器、事件触发判断、通信延迟补偿

下层：单智能体路径跟踪层（跟踪个体路径，满足约束）
- 核心功能：基于模型预测控制（MPC）实现输入约束下的精准路径跟踪
- 关键模块：MPC优化器、路径跟踪误差计算、Lyapunov稳定性约束

支撑模块：
- 可视化模块：智能体3D建模、轨迹动画、数据绘图
- 工具模块：RK4数值积分、变量存储、通信拓扑定义

二、核心文件功能解析

代码包包含7个核心文件，按功能可分为“主控制文件”“可视化文件”“工具文件”三类，具体功能如下：

（一）主控制文件（核心逻辑实现）

1. `MPC_CPF_5V_Circular.m`：圆形编队主控制程序

• 功能定位：实现5个智能体的圆形编队协同路径跟踪，对应参考文献中“嵌套圆周路径”场景。
• 核心流程：
  1. 初始化配置：
• 智能体状态：初始位置（如车辆1初始位置[20;0]）、航向角（如psi1_0=pi/4）；
• 路径参数：5个同心圆路径半径（30m、33m、36m、39m、42m），路径参数γ初始值；
• 约束条件：速度范围[0.2, 2]m/s，航向率范围[-0.2, 0.2]rad/s；
• 通信参数：事件触发阈值η=0.1e^(-0.2t)+5e-3，通信延迟Δ=2s。
  2. 协同层计算：
• 路径跟踪误差：通过epf()函数计算惯性系到平行传输帧的位置误差（ex, ey）和航向误差（e_psi）；
• 事件触发判断：当路径参数估计误差|γ̃|>η时，触发通信并广播当前γ值；
• 一致性控制：通过分布式控制律uci = -kctanh(l(γi - 邻居γ估计值))计算速度修正量，实现路径参数同步。
  3. MPC路径跟踪：
• 构建MPC优化问题：预测时域Np=10，采样时间Ts=0.2s，目标函数包含误差项和控制输入惩罚；
• 约束处理：显式考虑速度、航向率输入约束，通过Lyapunov稳定性约束保证闭环稳定性；
• 求解优化：调用CasADi工具包求解有限时域最优控制问题，输出控制量（航向率r、路径参数更新率u_gamma）。
  4. 状态更新：
• 智能体状态：通过update_vehicle()函数更新位置和航向；
• 路径状态：通过update_gamma()函数更新路径参数γ；
• 估计器更新：通过update_gammahat()函数更新邻居路径参数估计值。

2. `MPC_CPF_5V_Triangular.m`：三角形编队主控制程序

• 功能定位：实现5个智能体的三角形编队协同路径跟踪，对应参考文献中“平行直线路径”场景。
• 核心差异：
• 路径参数：平行直线路径定义为pd = [a*(γ-c); d]，其中a=50m（路径长度系数），c为路径偏移系数（如c1=0, c2=-0.1），d为横向距离（如d1=-10m, d2=-5m）；
• 仿真参数：仿真时长T=81s（短于圆形编队的201s），初始位置分布在不同横向距离上，确保形成三角形编队。

3. `mpcController.m`：MPC控制器构建函数

• 功能定位：为每个智能体生成独立的MPC控制器，是下层路径跟踪的核心模块。
• 关键实现：
  1. 状态与输入定义：
• 状态向量x = [s1, y1, psie, gamma, uc]（s1/y1为路径跟踪误差，psie为航向误差，gamma为路径参数，uc为协同层速度修正量）；
• 控制输入u = [r, ugamma]（r为航向率，ugamma为路径参数更新率）。
  2. 动力学模型：基于参考文献中的路径跟踪误差动力学方程，考虑路径曲率κ和速度耦合项，公式如下：
  matlab
xdot = [(vd+uc)hgicos(psie)-hgiug(1-cgiy1); % 沿路径误差动力学
(vd+uc)hgisin(psie)-cgis1hgiug; % 横向误差动力学
r-cgihgiug; % 航向误差动力学
ug; % 路径参数更新
0]; % 速度修正量不变

  其中hgi为路径长度系数，cgi为路径曲率相关系数，vd为期望速度。
  3. 优化目标：二次型目标函数，权衡误差最小化和控制输入平滑性：
  matlab
Q=diag([1 1 2 2 20]); % 误差权重（航向误差权重最高）
L = [s1, y1, psie, (vd+uc)cos(psie)-ug, r-cgihgiug]Q*[...]

  4. 稳定性约束：引入Lyapunov函数导数约束dVmpc ≤ dVnon，确保闭环系统全局渐近稳定（GAS），避免终端集设计，扩大吸引域。

（二）可视化文件（结果展示与动画）

1. `GTF_Simulink_PlotAUV.m`：智能体3D建模与绘制

• 功能定位：生成鱼雷型智能体（AUV/USV/无人车）的3D模型，支持位置、姿态、缩放比例配置。
• 核心组件：
• 机身：通过球体（nose/tailcap）和圆柱体（cyl1）组合，支持自定义颜色；
• 尾翼与螺旋桨： fins（水平尾翼）、rudders（垂直尾翼）、prop（螺旋桨，支持旋转动画）；
• 坐标变换：先旋转（X/Y/Z轴）、再缩放、最后平移，确保姿态与运动一致。

2. `animation_5vehicles_LMH.m`：编队动画生成

• 功能定位：基于GTFSimulinkPlotAUV.m绘制5个智能体的实时运动动画，支持视频录制。
• 关键特性：
• 颜色区分：5个智能体分别使用黑、蓝、红、青、紫5种颜色，与路径颜色一一对应；
• 姿态同步：实时更新智能体航向角（yaw1_1=90-yaw1），确保动画与实际运动一致；
• 播放控制：支持暂停、帧间隔调整，可通过vid=1启用视频录制。

3. `plotjournal_5vehicle.m`：仿真结果绘图

• 功能定位：生成参考文献风格的仿真结果图，用于验证算法性能。
• 绘图类型：
• 轨迹图：对比智能体实际轨迹（虚线）与期望路径（实线）；
• 输入约束图：绘制速度、航向率随时间变化，标注约束边界（黑色虚线）；
• 路径参数同步图：展示5个智能体γ参数收敛过程（一致性验证）；
• 事件触发图：标记各智能体通信触发时刻（+号）；
• 估计误差图：对比路径参数估计误差与触发阈值η；
• Lyapunov函数图：展示V_mpc单调递减，验证稳定性。

（三）工具文件（辅助功能支持）

1. `RK4_integrator.m`：RK4数值积分工具

• 功能定位：为智能体动力学模型提供四阶龙格-库塔（RK4）数值积分，确保状态更新精度。
• 核心接口：输入微分方程函数ode_fun、初始状态x0、控制输入u0、采样时间Ts，输出积分后的状态xEnd，支持灵敏度计算（可选）。

2. `save_to_base.m`：变量存储工具

• 功能定位：将主程序中的内部变量（如轨迹、误差、控制输入）保存到MATLAB基础工作区，方便后续分析。
• 特性：支持选择是否覆盖已有变量，自动排除ans等临时变量。

三、关键技术细节解析

1. 事件触发通信（ETC）机制

• 核心思想：仅当“路径参数估计误差超过阈值”时才进行通信，避免连续通信造成的带宽浪费。
• 触发条件：|γ̃i(t)| = |γ̂i(t) - γi(t)| > ηi(t)，其中η_i(t)=c1e^(-αt)+ε0（指数衰减阈值，兼顾收敛性与通信频率）。
• 延迟补偿：考虑2s通信延迟，接收端通过Gammahat21(end) = Data1D(2,idx1)+vd*Delta补偿延迟造成的估计偏差。
• 优势：仿真初期误差大时通信频繁，编队稳定后误差趋近于零，通信次数显著减少（参考文献图9B验证）。

2. 多智能体一致性控制

• 通信拓扑：采用无向连通图， Laplacian矩阵定义如下（5个智能体链式拓扑）：
  matlab
L = [1 -1 0 0 0;
-1 2 -1 0 0;
0 -1 2 -1 0;
0 0 -1 2 -1;
0 0 0 -1 1];

• 一致性控制律：基于双曲正切函数（tanh）设计饱和控制输入，避免速度修正量超出约束：
  matlab
uci = -kctanh(l(γi - sum(邻居γ估计值)))

  其中l为增益系数，kc满足0u/gmax（cu为速度调整余量，gmax为路径长度系数最大值）。

3. MPC约束处理

• 输入约束：明确限制速度u∈[0.2,2]m/s、航向率r∈[-0.2,0.2]rad/s、路径参数更新率u_gamma∈[-0.05,0.05]；
• 稳定性约束：通过dVmpc ≤ dVnon确保MPC控制律的稳定性，其中dV_non为Lyapunov函数的负定导数（由非线性控制器推导）；
• 递归可行性：将非线性稳定控制器的输出作为MPC的可行解，确保优化问题始终有解。

4. 路径跟踪误差模型

• 坐标系转换：采用平行传输帧（Parallel Transport Frame），将惯性系位置误差转换为沿路径（ex）和横向（ey）误差，简化动力学建模；
• 误差动力学：考虑路径曲率κ(γ)对误差的影响，适用于任意平滑路径（直线、圆、椭圆等），突破弧长参数化限制。

四、仿真复现步骤

1. 环境准备

• 软件要求：MATLAB R2018b及以上；
• 依赖工具包：CasADi（非线性优化求解）、Robotics Toolbox（可选，用于坐标系转换验证）；
• 安装步骤：
  1. 下载CasADi工具包（https://web.casadi.org/），添加路径到MATLAB：addpath('casadiinstalldir')；
  2. 将所有代码文件放在同一目录，运行savetobase.m初始化变量存储功能。

2. 复现步骤

步骤1：选择编队类型

• 圆形编队：运行MPCCPF5V_Circular.m；
• 三角形编队：运行MPCCPF5V_Triangular.m。

步骤2：参数调整（可选）

• 编队参数：修改路径参数（如圆形编队半径、三角形编队横向距离）；
• 控制参数：调整MPC预测时域Np、权重矩阵Q、事件触发阈值η；
• 约束参数：修改速度/航向率约束（vehicle.rmax、vehicle.rmin）。

步骤3：查看结果

• 动画展示：运行animation5vehiclesLMH.m，查看智能体编队运动过程；
• 数据绘图：运行plotjournal_5vehicle.m，生成轨迹、误差、通信触发等结果图；
• 变量分析：基础工作区中可查看xvehicle1~5（智能体状态）、epf1~5（路径跟踪误差）、uc（一致性控制输入）等变量。

3. 预期结果

• 轨迹收敛：智能体在50~100s内收敛到期望路径，保持指定编队；
• 约束满足：速度、航向率始终在设定范围内；
• 通信优化：编队稳定后，通信触发次数趋近于零；
• 稳定性验证：Lyapunov函数V_mpc单调递减至零。

五、代码扩展性说明

1. 智能体类型扩展

• 无人车：修改vehicle.m中的动力学模型，替换为无人车运动学模型（如差速驱动）；
• 无人机（UAV）：添加高度维度（z轴），修改GTFSimulinkPlotAUV.m中的3D模型，适配无人机外形。

2. 编队类型扩展

• 自定义编队：修改路径参数生成函数path.m，定义新的期望路径（如矩形、菱形）；
• 时变编队：在uc计算中引入时间依赖项，实现编队形状动态变化。

3. 通信拓扑扩展

• 切换拓扑：修改Laplacian矩阵L，支持星形、全连通等拓扑；
• 时变拓扑：在主程序中动态更新L，模拟通信链路切换。

六、关键问题排查

1. 仿真报错：CasADi未找到

• 解决方案：确认CasADi路径添加正确，或重新下载对应MATLAB版本的CasADi。

2. 优化问题无解

• 原因：输入约束过紧或MPC权重设置不合理；
• 解决方案：放宽速度/航向率约束，调整Q矩阵中误差权重，或增大预测时域Np。

3. 编队无法收敛

• 原因：一致性增益kc过小，或事件触发阈值η过大；
• 解决方案：增大kc（不超过cu/gmax），减小η的初始值c1。

4. 动画无法显示

• 原因：MATLAB图形渲染设置问题；
• 解决方案：运行opengl info检查渲染支持，或修改GTFSimulinkPlotAUV.m中的renderpatch函数为renderwire（线框模式）。

七、总结

本套代码完整复现了参考文献中“基于MPC与事件触发通信的约束多智能体协同路径跟踪”算法，具有以下特点：

1. 理论严谨：严格遵循“路径跟踪+多智能体协同”双层架构，确保稳定性与收敛性；
2. 工程实用：显式处理输入约束、通信延迟，事件触发机制适用于低带宽场景；
3. 可视化完善：提供3D动画与专业结果图，方便算法验证与论文撰写；
4. 扩展性强：支持多种智能体类型、编队形式与通信拓扑，可快速适配不同应用场景。

代码可直接用于无人船、无人车等多智能体编队控制的仿真验证，也可作为实际系统开发的基础框架。