【UUV编队控制】基于刚性图MPC对具有未知绝对位置的无人水下航行器编队控制附Matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

在海洋资源开发中，无人水下航行器（Unmanned Underwater Vehicle，UUV）编队凭借协同作业优势发挥着关键作用。然而，水下环境复杂，UUV 常面临绝对位置难以精确获取的问题。刚性图（Rigid Graph）理论可描述 UUV 间相对位置关系，模型预测控制（Model Predictive Control，MPC）能实现滚动优化控制。将两者结合，为解决 UUV 绝对位置未知下的编队控制问题提供了新思路。本文将深入研究基于刚性图 MPC 的 UUV 编队控制方法。

一、基础理论概述

（一）刚性图理论

刚性图理论是图论在多智能体系统编队控制中的应用，通过图的形式描述多智能体之间的连接关系和相对位置约束。在 UUV 编队中，将每个 UUV 视为图中的节点，UUV 之间的通信和相对位置测量关系视为边。刚性图的特性在于，当图满足一定条件时，给定节点间的相对距离信息，整个图的结构（即 UUV 编队的相对位置布局）在不考虑旋转和平移的情况下是唯一确定的 。这使得即使 UUV 无法获取自身绝对位置，也能通过相互间的相对位置信息维持特定的编队结构。

（二）模型预测控制（MPC）

模型预测控制是一种基于模型的滚动时域优化控制策略。其核心步骤包括：预测模型建立、滚动优化和反馈校正。在 UUV 编队控制中，首先根据 UUV 的运动学和动力学模型建立预测模型，用于预测 UUV 在未来一段时间内的状态；然后在每个采样时刻，基于预测模型对未来的控制输入进行优化，以最小化设定的性能指标（如编队误差、能量消耗等）；最后将优化得到的控制输入的第一个元素作用于 UUV，并根据 UUV 的实际输出对预测模型进行校正，进入下一个采样时刻的滚动优化 。

二、基于刚性图的 UUV 编队模型构建

（一）UUV 运动学与动力学模型

三、基于刚性图 MPC 的 UUV 编队控制算法设计

（一）预测模型建立

⛳️ 运行结果

📣 部分代码

clc;

clear;

close all;

addpath('./utilis/','./header/')

set(0,'defaultfigurecolor','w')

set(groot, 'DefaultAxesFontName', 'Times New Roman');

set(groot, 'DefaultTextFontName', 'Times New Roman');

%======================================================================%

%======================================================================%

err_model=0;

m=116;

Iz=13.1;

X_udot=-167.6*(1+err_model);

Y_vdot=-477.2*(1+err_model);

N_rdot=-15.9*(1+err_model);

Xu=26.9*(1+err_model);

Yv=35.8*(1+err_model);

Nr=3.5*(1+err_model);

Du=241.3*(1+err_model);

Dv=503.8*(1+err_model);

Dr=76.9*(1+err_model);

Mx=m-X_udot;

My=m-Y_vdot;

Mpsi=Iz-N_rdot;

disturbance=[0;0;0];

%======================================================================%

%======================================================================%

coef = [m; Iz; X_udot; Y_vdot; N_rdot; Xu; Yv; Nr; Du; Dv; Dr];

ndof = 3;

x1 = 0;

y1 = 0;

x2 = -7.42;

y2 = 6.7;  

x3 = 4.08;

y3 = 9.13;

x4 = 9.94;

y4 = -1.06;

x5 = 2.07;

y5 = -9.78;

x6 = -8.67;

y6 = -4.99;

X1_0 = [x1       ;y1       ;  0 + pi  ; 0; 0; 0];

X2_0 = [x2 - 2.76;y2 + 6.14;  0 + pi/2; 0; 0; 0];

X3_0 = [x3 + 7.3 ;y3 - 1.4 ;  0 + pi/3; 0; 0; 0];

X4_0 = [x4 + 2.3 ;y4 - 4.2 ;  0 - pi/2; 0; 0; 0];

X5_0 = [x5 + 10.4;y5 - 6.6 ;  0 + pi/4; 0; 0; 0];

X6_0 = [x6 - 5.4 ;y6 - 10.2;  0 - pi/3; 0; 0; 0];

U_0 = [0;0;0];

auv1 = AUV(coef, ndof, X1_0, U_0);

auv2 = AUV(coef, ndof, X2_0, U_0);

auv3 = AUV(coef, ndof, X3_0, U_0);

auv4 = AUV(coef, ndof, X4_0, U_0);

auv5 = AUV(coef, ndof, X5_0, U_0);

auv6 = AUV(coef, ndof, X6_0, U_0);

internal_auv1 = AUV(coef, ndof, X1_0, U_0);

internal_auv2 = AUV(coef, ndof, X2_0, U_0);

internal_auv3 = AUV(coef, ndof, X3_0, U_0);

internal_auv4 = AUV(coef, ndof, X4_0, U_0);

internal_auv5 = AUV(coef, ndof, X5_0, U_0);

internal_auv6 = AUV(coef, ndof, X6_0, U_0);

%======================================================================%

%======================================================================%

Adj = [0,1,1,1,1,1;

       1,0,1,0,0,1;

       1,1,0,1,0,0;

       1,0,1,0,1,0;

       1,0,0,1,0,0;

       1,1,0,0,0,0];

ref = [0,    1,    1,    1,    1,    1;

       1,    0, 1.18,    0,    0, 1.18;

       1, 1.18,    0, 1.18,    0,    0;

       1,    0, 1.18,    0, 1.18,    0;

       1,    0,    0, 1.18,    0,    0;

       1, 1.18,    0,    0,    0,    0]*10;

traj = [0,0,0,0,0,0];

%==========================================================================

%==========================================================================

dt = 0.1;

Tstep = 150;

X1plus = X1_0;

X2plus = X2_0;

X3plus = X3_0; 

X4plus = X4_0; 

X5plus = X5_0; 

X6plus = X6_0;

%======================================================================%

%======================================================================%

nx = length(X1_0);

nu = length(U_0);

[Xa1, Xa2, Xa3, Xa4, Xa5, Xa6] = deal(zeros(nx,Tstep+1));

[Ua1, Ua2, Ua3, Ua4, Ua5, Ua6] = deal(zeros(nu,Tstep));

[Va1, Va2, Va3, Va4, Va5, Va6] = deal(zeros(nu,Tstep));

[Vap1, Vap2, Vap3, Vap4, Vap5, Vap6] = deal(zeros(nu,Tstep));

Xa1(:,1) = X1_0; 

Xa2(:,1) = X2_0; 

Xa3(:,1) = X3_0; 

Xa4(:,1) = X4_0;

Xa5(:,1) = X5_0;

Xa6(:,1) = X6_0;

k_bs.v = 1e-5;

k_bs.a = 1e3;

k_bs.psi_v = 1;

k_bs.psi_a = 1;

upperbound_bs = 1000;

lowerbound_bs = -1000;

bs1 = rigidity_bs_controller(k_bs,coef,upperbound_bs,lowerbound_bs,Adj);

bs2 = rigidity_bs_controller(k_bs,coef,upperbound_bs,lowerbound_bs,Adj);

bs3 = rigidity_bs_controller(k_bs,coef,upperbound_bs,lowerbound_bs,Adj);

bs4 = rigidity_bs_controller(k_bs,coef,upperbound_bs,lowerbound_bs,Adj);

bs5 = rigidity_bs_controller(k_bs,coef,upperbound_bs,lowerbound_bs,Adj);

bs6 = rigidity_bs_controller(k_bs,coef,upperbound_bs,lowerbound_bs,Adj);

%======================================================================%

%======================================================================%

for i=1:1:Tstep

    state = [X1plus,X2plus,X3plus,X4plus,X5plus,X6plus];

    [tau1,Vdot1] = bs1.calc_control(ref,traj,1,state);

    [tau2,Vdot2] = bs2.calc_control(ref,traj,2,state);

    [tau3,Vdot3] = bs3.calc_control(ref,traj,3,state);

    [tau4,Vdot4] = bs4.calc_control(ref,traj,4,state);

    [tau5,Vdot5] = bs5.calc_control(ref,traj,5,state);

    [tau6,Vdot6] = bs6.calc_control(ref,traj,6,state);

    Ua1(:,i) = tau1;

    Ua2(:,i) = tau2;

    Ua3(:,i) = tau3;

    Ua4(:,i) = tau4;

    Ua5(:,i) = tau5;

    Ua6(:,i) = tau6;

    Va1(:,i) = Vdot1(1);

    Va2(:,i) = Vdot2(1);

    Va3(:,i) = Vdot3(1);

    Va4(:,i) = Vdot4(1);

    Va5(:,i) = Vdot5(1);

    Va6(:,i) = Vdot6(1);

    Vap1(:,i) = Vdot1(2);

    Vap2(:,i) = Vdot2(2);

    Vap3(:,i) = Vdot3(2);

    Vap4(:,i) = Vdot4(2);

    Vap5(:,i) = Vdot5(2);

    Vap6(:,i) = Vdot6(2);

    auv1.advance(tau1, disturbance, dt);

    X1plus = auv1.X;

    Xa1(:,i+1) = X1plus;

    auv2.advance(tau2, disturbance, dt);

    X2plus = auv2.X;

    Xa2(:,i+1) = X2plus;

    auv3.advance(tau3, disturbance, dt);

    X3plus = auv3.X;

    Xa3(:,i+1) = X3plus;

    auv4.advance(tau4, disturbance, dt);

    X4plus = auv4.X;

    Xa4(:,i+1) = X4plus;

    auv5.advance(tau5, disturbance, dt);

    X5plus = auv5.X;

    Xa5(:,i+1) = X5plus;

    auv6.advance(tau6, disturbance, dt);

    X6plus = auv6.X;

    Xa6(:,i+1) = X6plus;

end

%%

%======================================================================%

%======================================================================%

blue   = '#5F97D2';

lgreen = '#B1CE46';

red    = '#D76364';

yellow = '#F1D77E';

green  = '#63E398';

purple = '#7E2F8E';

pink   = '#A2142F';

cblue  = '#5F9EA0';

close all;

figure(1);

plot(Xa1(1,end), Xa1(2,end),'*','color',blue,'LineWidth',2);

hold on;

plot(Xa2(1,end), Xa2(2,end),'*','color',lgreen,'LineWidth',2);

hold on;

plot(Xa3(1,end), Xa3(2,end),'*','color',red,'LineWidth',2);

hold on;

plot(Xa4(1,end), Xa4(2,end),'*','color',yellow,'LineWidth',2);

hold on;

plot(Xa5(1,end), Xa5(2,end),'*','color',green,'LineWidth',2);

hold on;

plot(Xa6(1,end), Xa6(2,end),'*','color',purple,'LineWidth',2);

hold on;

b1 = plot([Xa1(1,1),Xa2(1,1)],[Xa1(2,1),Xa2(2,1)],'--','color',cblue);

hold on;

plot([Xa1(1,1),Xa3(1,1)],[Xa1(2,1),Xa3(2,1)],'--','color',cblue);

hold on;

plot([Xa1(1,1),Xa4(1,1)],[Xa1(2,1),Xa4(2,1)],'--','color',cblue);

hold on;

plot([Xa1(1,1),Xa5(1,1)],[Xa1(2,1),Xa5(2,1)],'--','color',cblue);

hold on;

plot([Xa1(1,1),Xa6(1,1)],[Xa1(2,1),Xa6(2,1)],'--','color',cblue);

hold on;

plot([Xa2(1,1),Xa3(1,1)],[Xa2(2,1),Xa3(2,1)],'--','color',cblue);

hold on;

plot([Xa3(1,1),Xa4(1,1)],[Xa3(2,1),Xa4(2,1)],'--','color',cblue);

hold on;

plot([Xa4(1,1),Xa5(1,1)],[Xa4(2,1),Xa5(2,1)],'--','color',cblue);

hold on;

plot([Xa5(1,1),Xa6(1,1)],[Xa5(2,1),Xa6(2,1)],'--','color',cblue);

hold on;

plot([Xa2(1,1),Xa6(1,1)],[Xa2(2,1),Xa6(2,1)],'--','color',cblue);

hold on;

b2 = plot([Xa1(1,end),Xa2(1,end)],[Xa1(2,end),Xa2(2,end)],'--','color',pink);

hold on;

plot([Xa1(1,end),Xa3(1,end)],[Xa1(2,end),Xa3(2,end)],'--','color',pink);

hold on;

plot([Xa1(1,end),Xa4(1,end)],[Xa1(2,end),Xa4(2,end)],'--','color',pink);

hold on;

plot([Xa1(1,end),Xa5(1,end)],[Xa1(2,end),Xa5(2,end)],'--','color',pink);

hold on;

plot([Xa1(1,end),Xa6(1,end)],[Xa1(2,end),Xa6(2,end)],'--','color',pink);

hold on;

plot([Xa2(1,end),Xa3(1,end)],[Xa2(2,end),Xa3(2,end)],'--','color',pink);

hold on;

plot([Xa3(1,end),Xa4(1,end)],[Xa3(2,end),Xa4(2,end)],'--','color',pink);

hold on;

plot([Xa4(1,end),Xa5(1,end)],[Xa4(2,end),Xa5(2,end)],'--','color',pink);

hold on;

plot([Xa5(1,end),Xa6(1,end)],[Xa5(2,end),Xa6(2,end)],'--','color',pink);

hold on;

plot([Xa2(1,end),Xa6(1,end)],[Xa2(2,end),Xa6(2,end)],'--','color',pink);

hold on;

a1=plot(Xa1(1,:), Xa1(2,:),'color',blue,'LineWidth',2);

hold on;

a2=plot(Xa2(1,:), Xa2(2,:),'color',lgreen,'LineWidth',2);

hold on;

a3=plot(Xa3(1,:), Xa3(2,:),'color',red,'LineWidth',2);

hold on;

a4=plot(Xa4(1,:), Xa4(2,:),'color',yellow,'LineWidth',2);

hold on;

a5=plot(Xa5(1,:), Xa5(2,:),'color',green,'LineWidth',2);

hold on;

a6=plot(Xa6(1,:), Xa6(2,:),'color',purple,'LineWidth',2);,'RGMPC UUV6'},'NumColumns',3);

legend('boxoff');

set(gca,'FontSize',12);

nexttile

m1=plot(0.1:0.1:Tstep/10,U1(2,:),'color',blue,'LineWidth',2);

hold on;

m2=plot(0.1:0.1:Tstep/10,U2(2,:),'color',lgreen,'LineWidth',2);

hold on;

m3=plot(0.1:0.1:Tstep/10,U3(2,:),'color',red,'LineWidth',2);

hold on;

m4=plot(0.1:0.1:Tstep/10,U4(2,:),'color',yellow,'LineWidth',2);

hold on;

m5=plot(0.1:0.1:Tstep/10,U5(2,:),'color',green,'LineWidth',2);

hold on;

m6=plot(0.1:0.1:Tstep/10,U6(2,:),'color',purple,'LineWidth',2);

xlabel('\bf{time}\boldmath{$[s]$}', 'Interpreter', 'latex','FontSize',14);

ylabel('\textbf{control input} \boldmath{$\tau^{y}[N]$}', 'Interpreter', 'latex','FontSize',14);

ylim([-1000 1000]);

set(gca,'FontSize',12);

nexttile

m1=plot(0.1:0.1:Tstep/10,U1(3,:),'color',blue,'LineWidth',2);

hold on;

m2=plot(0.1:0.1:Tstep/10,U2(3,:),'color',lgreen,'LineWidth',2);

hold on;

m3=plot(0.1:0.1:Tstep/10,U3(3,:),'color',red,'LineWidth',2);

hold on;

m4=plot(0.1:0.1:Tstep/10,U4(3,:),'color',yellow,'LineWidth',2);

hold on;

m5=plot(0.1:0.1:Tstep/10,U5(3,:),'color',green,'LineWidth',2);

hold on;

m6=plot(0.1:0.1:Tstep/10,U6(3,:),'color',purple,'LineWidth',2);

xlabel('\bf{time}\boldmath{$[s]$}', 'Interpreter', 'latex','FontSize',14);

ylabel('\textbf{control input} \boldmath{$\tau^{r}[N{\cdot}m]$}', 'Interpreter', 'latex','FontSize',14);

set(gca,'FontSize',12);

ylim([-1000 1000]);

set(gcf,'unit','normalized','position', [0.2,0.2,0.5,0.5]);

🔗 参考文献

🎈 部分理论引用网络文献，若有侵权联系博主删除

 👇 关注我领取海量matlab电子书和数学建模资料 

🏆团队擅长辅导定制多种科研领域MATLAB仿真，助力科研梦：

🌈 各类智能优化算法改进及应用

生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化、背包问题、 风电场布局、时隙分配优化、 最佳分布式发电单元分配、多阶段管道维修、 工厂-中心-需求点三级选址问题、 应急生活物质配送中心选址、 基站选址、 道路灯柱布置、 枢纽节点部署、 输电线路台风监测装置、 集装箱调度、 机组优化、 投资优化组合、云服务器组合优化、 天线线性阵列分布优化、CVRP问题、VRPPD问题、多中心VRP问题、多层网络的VRP问题、多中心多车型的VRP问题、 动态VRP问题、双层车辆路径规划（2E-VRP）、充电车辆路径规划（EVRP）、油电混合车辆路径规划、混合流水车间问题、 订单拆分调度问题、 公交车的调度排班优化问题、航班摆渡车辆调度问题、选址路径规划问题、港口调度、港口岸桥调度、停机位分配、机场航班调度、泄漏源定位

🌈 机器学习和深度学习时序、回归、分类、聚类和降维

2.1 bp时序、回归预测和分类

2.2 ENS声神经网络时序、回归预测和分类

2.3 SVM/CNN-SVM/LSSVM/RVM支持向量机系列时序、回归预测和分类

2.4 CNN|TCN|GCN卷积神经网络系列时序、回归预测和分类

2.5 ELM/KELM/RELM/DELM极限学习机系列时序、回归预测和分类

2.6 GRU/Bi-GRU/CNN-GRU/CNN-BiGRU门控神经网络时序、回归预测和分类

2.7 ELMAN递归神经网络时序、回归\预测和分类

2.8 LSTM/BiLSTM/CNN-LSTM/CNN-BiLSTM/长短记忆神经网络系列时序、回归预测和分类

2.9 RBF径向基神经网络时序、回归预测和分类

2.10 DBN深度置信网络时序、回归预测和分类

2.11 FNN模糊神经网络时序、回归预测

2.12 RF随机森林时序、回归预测和分类

2.13 BLS宽度学习时序、回归预测和分类

2.14 PNN脉冲神经网络分类

2.15 模糊小波神经网络预测和分类

2.16 时序、回归预测和分类

2.17 时序、回归预测预测和分类

2.18 XGBOOST集成学习时序、回归预测预测和分类

2.19 Transform各类组合时序、回归预测预测和分类

方向涵盖风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、用电量预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

🌈图像处理方面

图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

🌈 路径规划方面

旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、 充电车辆路径规划（EVRP）、 双层车辆路径规划（2E-VRP）、 油电混合车辆路径规划、 船舶航迹规划、 全路径规划规划、 仓储巡逻

🌈 无人机应用方面

无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配、无人机安全通信轨迹在线优化、车辆协同无人机路径规划

🌈 通信方面

传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化、水声通信、通信上传下载分配

🌈 信号处理方面

信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化、心电信号、DOA估计、编码译码、变分模态分解、管道泄漏、滤波器、数字信号处理+传输+分析+去噪、数字信号调制、误码率、信号估计、DTMF、信号检测

🌈电力系统方面

微电网优化、无功优化、配电网重构、储能配置、有序充电、MPPT优化、家庭用电、电/冷/热负荷预测、电力设备故障诊断、电池管理系统（BMS）SOC/SOH估算（粒子滤波/卡尔曼滤波）、 多目标优化在电力系统调度中的应用、光伏MPPT控制算法改进（扰动观察法/电导增量法）

🌈 元胞自动机方面

交通流 人群疏散 病毒扩散 晶体生长 金属腐蚀

🌈 雷达方面

卡尔曼滤波跟踪、航迹关联、航迹融合、SOC估计、阵列优化、NLOS识别

🌈 车间调度

零等待流水车间调度问题NWFSP 、 置换流水车间调度问题PFSP、 混合流水车间调度问题HFSP 、零空闲流水车间调度问题NIFSP、分布式置换流水车间调度问题 DPFSP、阻塞流水车间调度问题BFSP

👇