异构混合阶多智能体系统编队控制的分布式优化研究（Matlab代码实现）

异构混合阶多智能体系统编队控制的分布式优化研究一、引言在当今智能化和自动化技术快速发展的背景下，多智能体系统的研究日益受到关注。异构混合阶多智能体系统作为其中的一个重要研究方向，其编队控制的分布式优化具有重要意义。通过合理地设计编队控制策略并采用分布式优化方法，可以实现智能体系统的高效协作和优化控制，提高系统的整体性能和效益，在众多领域有着广泛的应用前景。二、异构混合阶多智能体系统概述。

yyds_2201

983人浏览 · 2025-01-25 09:09:35

yyds_2201 · 2025-01-25 09:09:35 发布

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

异构混合阶多智能体系统编队控制的分布式优化研究

一、引言

在当今智能化和自动化技术快速发展的背景下，多智能体系统的研究日益受到关注。异构混合阶多智能体系统作为其中的一个重要研究方向，其编队控制的分布式优化具有重要意义。通过合理地设计编队控制策略并采用分布式优化方法，可以实现智能体系统的高效协作和优化控制，提高系统的整体性能和效益，在众多领域有着广泛的应用前景。

二、异构混合阶多智能体系统概述

智能体特性差异
- 在异构混合阶多智能体系统中，智能体具有不同的特性和能力。这些特性可以是不同的传感器、执行器或决策规则等。例如，某些智能体可能配备高精度的传感器用于环境监测，而另一些智能体可能具有更强的执行能力来完成特定任务。
系统工作原理
- 异构混合阶多智能体系统可通过合理地分配任务和资源，使得整个系统能够高效地完成工作。各个智能体之间需要相互协作，根据自身的能力和接收到的信息，共同实现系统的目标。

三、分布式优化在编队控制中的意义

避免单点故障
- 分布式优化技术是实现异构混合阶多智能体系统编队控制的关键。通过分布式优化方法，可以将整个编队控制问题分解成多个较小的子问题，每个智能体只需要解决自身的局部优化问题。这种分布式的优化方式避免了中央控制节点的单点故障，提高了系统的稳定性和容错性。
降低负载提高效率
- 同时，分布式优化还可以降低通信和计算负载，提高系统的实时性和效率。由于每个智能体主要处理自身的局部优化问题，不需要频繁地与中央节点进行大量数据交互，从而减轻了系统的通信和计算负担。

四、编队控制目标与智能体协作

编队控制目标
- 在异构混合阶多智能体系统中，编队控制的目标是使所有智能体以一定的形态和策略进行协同工作。通过合理地设计编队控制策略，可以实现高效的任务分配和资源利用，提高系统的整体性能和效益。例如，在无人机编队飞行中，要使无人机保持特定的队形以完成侦查或投递任务。
智能体间协作
- 智能体之间的信息交互和合作是至关重要的。通过相互传递信息和协调动作，智能体能够实现分工合作、动态调整和自适应控制。同时，智能体之间的通信和协作也需要考虑安全性和保密性等因素，以保护系统的稳定性和可靠性。

五、面临的挑战与问题

适应性学习算法
- 对于异构混合阶多智能体系统编队控制的分布式优化，如何在系统中引入适应性学习算法，以提高系统的自适应能力和学习效果是一个挑战。例如，在复杂多变的环境中，智能体需要不断学习新的行为模式来适应环境变化。
有效协同与协作
- 如何实现智能体之间的有效协同与协作，以提高系统的整体性能和鲁棒性也是需要解决的问题。这涉及到如何优化智能体之间的通信协议、如何更好地分配任务等方面的研究。

六、相关控制方法示例 - Leader - Follower编队控制方法

基本思想
- 在由多智能体组成的群组中，其中某个智能体被指定为领航者，其余的智能体被指定为跟踪领航者运动的跟随者，跟随者以设定的距离或速度等参量跟踪领航智能体的位置和方向。进一步来讲，对同一个多智能体系统，领航者可以仅仅指定一个，也可以存在多个，但控制群组编队形状的领航者只能有一个。通过设定领航者智能体与跟随智能体间不同的位置关系，便可得到不同的网络拓扑结构，即不同的编队队形。
优点与缺点
- 优点：领航者作为控制整个智能体系统运动的主导，仅凭给定的运行轨迹便可以控制整个智能体群组的行为，从而大大简化了控制作用。
- 缺点：系统中不存在明确的反馈控制作用，也就是说，一旦系统中领航者发生错误行为时，会直接导致跟随智能体行为的紊乱，进而让整个系统陷入崩馈状态。

七、结论

异构混合阶多智能体系统编队控制的分布式优化是一项具有挑战性和重要意义的研究课题。虽然已经取得了一定的研究成果，但仍然存在许多问题需要解决。未来的研究工作应该进一步深入探索编队控制问题，解决实际应用中的挑战，并进一步推动异构混合阶多智能体系统编队控制技术的发展和应用。

📚2 运行结果

部分代码：

subplot(2,2,4)
set(gca,'position',[0.55 0.05 0.43 0.43]);
plot(t,d(4,:), 'r', 'linewidth',1); hold on;
plot(t,d(8,:), 'g', 'linewidth',1); hold on;
plot(t,d(12,:),'b', 'linewidth',1); hold on;
plot(t,d(16,:), '--r', 'linewidth',1); hold on;
plot(t,d(20,:), '-.r', 'linewidth',1); hold on;
plot(t,d(24,:), '--g', 'linewidth',1); hold on;
plot(t,d(28,:), '-.g', 'linewidth',1); hold on;
plot(t,d(32,:), ':g', 'linewidth',1); hold on;
plot(t,d(36,:), '--b', 'linewidth',1); hold on;
plot(t,d(40,:), '-.b', 'linewidth',1); hold on;
ylim([-40 40]);
title("Y velocity error",'Interpreter','latex');
xlabel("t (s)",'Interpreter','latex'); ylabel("Error $v^y$ (m/s)",'Interpreter','latex');
legend('l1', 'l2', 'l3', 'f4', 'f5', 'f6', 'f7', 'f8', 'f9', 'f10','Interpreter','latex');
grid on; box on;

figure(7)

subplot(4,2,1)
set(gca,'position',[0.05 0.79 0.43 0.19]);
plot(t,d(41,:), 'm', 'linewidth',1); hold on;
plot(t,d(53,:), 'c', 'linewidth',1); hold on;
plot(t,d(65,:), '--m', 'linewidth',1); hold on;
plot(t,d(77,:), '-.m', 'linewidth',1); hold on;
plot(t,d(89,:), '--c', 'linewidth',1); hold on;
plot(t,d(101,:), '-.c', 'linewidth',1); hold on;
ylim([-40 40]);
title("X position error",'Interpreter','latex');
xlabel("t (s)",'Interpreter','latex'); ylabel("Error $x$ (m)",'Interpreter','latex');
legend('l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
grid on; box on;

subplot(4,2,2)
set(gca,'position',[0.55 0.79 0.43 0.19]);
plot(t,d(42,:), 'm', 'linewidth',1); hold on;
plot(t,d(54,:), 'c', 'linewidth',1); hold on;
plot(t,d(66,:), '--m', 'linewidth',1); hold on;
plot(t,d(78,:), '-.m', 'linewidth',1); hold on;
plot(t,d(90,:), '--c', 'linewidth',1); hold on;
plot(t,d(102,:), '-.c', 'linewidth',1); hold on;
ylim([-40 40]);
title("Y position error",'Interpreter','latex');
xlabel("t (s)",'Interpreter','latex'); ylabel("Error $y$ (m)",'Interpreter','latex');
legend('l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
grid on; box on;

subplot(4,2,3)
set(gca,'position',[0.05 0.54 0.43 0.19]);
plot(t,d(44,:), 'm', 'linewidth',1); hold on;
plot(t,d(56,:), 'c', 'linewidth',1); hold on;
plot(t,d(68,:), '--m', 'linewidth',1); hold on;
plot(t,d(80,:), '-.m', 'linewidth',1); hold on;
plot(t,d(92,:), '--c', 'linewidth',1); hold on;
plot(t,d(104,:), '-.c', 'linewidth',1); hold on;
ylim([-40 40]);
title("X velocity error",'Interpreter','latex');
xlabel("t (s)",'Interpreter','latex'); ylabel("Error $\dot x$ (m/s)",'Interpreter','latex');
% ylabel('$\dot t_2 $','Interpreter','latex');
legend('l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
grid on; box on;

subplot(4,2,4)
set(gca,'position',[0.55 0.54 0.43 0.19]);
plot(t,d(45,:), 'm', 'linewidth',1); hold on;
plot(t,d(57,:), 'c', 'linewidth',1); hold on;
plot(t,d(69,:), '--m', 'linewidth',1); hold on;
plot(t,d(81,:), '-.m', 'linewidth',1); hold on;
plot(t,d(93,:), '--c', 'linewidth',1); hold on;
plot(t,d(105,:), '-.c', 'linewidth',1); hold on;
ylim([-40 40]);
title("Y velocity error",'Interpreter','latex');
xlabel("t (s)",'Interpreter','latex'); ylabel("Error $\dot y$ (m/s)",'Interpreter','latex');
legend('l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
grid on; box on;

subplot(4,2,5)
set(gca,'position',[0.05 0.29 0.43 0.19]);
plot(t,d(47,:), 'm', 'linewidth',1); hold on;
plot(t,d(59,:), 'c', 'linewidth',1); hold on;
plot(t,d(71,:), '--m', 'linewidth',1); hold on;
plot(t,d(83,:), '-.m', 'linewidth',1); hold on;
plot(t,d(95,:), '--c', 'linewidth',1); hold on;
plot(t,d(107,:), '-.c', 'linewidth',1); hold on;
ylim([-40 40]);
title("Pitch angles error",'Interpreter','latex');
xlabel("t(s)",'Interpreter','latex'); ylabel("Error $\theta ~ (^{\circ})$",'Interpreter','latex');
legend('l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
grid on; box on;

subplot(4,2,6)
set(gca,'position',[0.55 0.29 0.43 0.19]);
plot(t,d(48,:), 'm', 'linewidth',1); hold on;
plot(t,d(60,:), 'c', 'linewidth',1); hold on;
plot(t,d(72,:), '--m', 'linewidth',1); hold on;
plot(t,d(84,:), '-.m', 'linewidth',1); hold on;
plot(t,d(96,:), '--c', 'linewidth',1); hold on;
plot(t,d(108,:), '-.c', 'linewidth',1); hold on;
ylim([-40 40]);
title("Roll angles error",'Interpreter','latex');
xlabel("t(s)",'Interpreter','latex'); ylabel("Error $\phi ~ (^{\circ})$",'Interpreter','latex');
legend('l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
grid on; box on;

subplot(4,2,7)
set(gca,'position',[0.05 0.04 0.43 0.19]);
plot(t,d(50,:), 'm', 'linewidth',1); hold on;
plot(t,d(62,:), 'c', 'linewidth',1); hold on;
plot(t,d(74,:), '--m', 'linewidth',1); hold on;
plot(t,d(86,:), '-.m', 'linewidth',1); hold on;
plot(t,d(98,:), '--c', 'linewidth',1); hold on;
plot(t,d(110,:), '-.c', 'linewidth',1); hold on;
ylim([-40 40]);
title("Pitch rates error",'Interpreter','latex');
xlabel("t(s)",'Interpreter','latex'); ylabel("Error $\dot \theta ~ (^{\circ}/s)$",'Interpreter','latex');
legend('l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
grid on; box on;

subplot(4,2,8)
set(gca,'position',[0.55 0.04 0.43 0.19]);
plot(t,d(51,:), 'm', 'linewidth',1); hold on;
plot(t,d(63,:), 'c', 'linewidth',1); hold on;
plot(t,d(75,:), '--m', 'linewidth',1); hold on;
plot(t,d(87,:), '-.m', 'linewidth',1); hold on;
plot(t,d(99,:), '--c', 'linewidth',1); hold on;
plot(t,d(111,:), '-.c', 'linewidth',1); hold on;
ylim([-40 40]);
title("Roll rates error",'Interpreter','latex');
xlabel("t(s)",'Interpreter','latex'); ylabel("Error $\dot \phi ~ (^{\circ}/s)$",'Interpreter','latex');
legend('l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
grid on; box on;

% Dynamic formation states
% figure(8)
% for i=1:10:(size(X,2))
% subplot(1,2,1)
% scatter3(H(1,i), H(2,i), 0, 150,'p', 'r'); hold on;
% scatter3(H(5,i), H(6,i), 0, 150,'p', 'g');
% scatter3(H(9,i), H(10,i),0, 150,'p', 'b');
% scatter3(H(13,i),H(14,i),0, 'r');
% scatter3(H(17,i),H(18,i),0, 'r');
% scatter3(H(21,i),H(22,i),0, 'g');
% scatter3(H(25,i),H(26,i),0, 'g');
% scatter3(H(29,i),H(30,i),0, 'g');
% scatter3(H(33,i),H(34,i),0, 'b');
% scatter3(H(37,i),H(38,i),0, 'b');
% scatter3(H(41,i),H(42,i),H(43,i), 150,'p', 'm');
% scatter3(H(53,i),H(54,i),H(55,i), 150,'p', 'c');
% scatter3(H(65,i),H(66,i),H(67,i), 'm');
% scatter3(H(77,i),H(78,i),H(79,i), 'm');
% scatter3(H(89,i),H(90,i),H(91,i), 'c');
% scatter3(H(101,i),H(102,i),H(103,i),'c');
% grid on;
% axis equal;
% axis([-80, 80, -50,300, 0,50]);
% xlabel("X(m)"); ylabel("Y(m)"); zlabel("H(m)");
% title("Virtual agents’ states");
%
% subplot(1,2,2)
% scatter3(X(1,i), X(2,i), 0, 150,'p', 'r'); hold on;
% scatter3(X(5,i), X(6,i), 0, 150,'p', 'g');
% scatter3(X(9,i), X(10,i),0, 150,'p', 'b');
% scatter3(X(13,i),X(14,i),0, 'r');
% scatter3(X(17,i),X(18,i),0, 'r');
% scatter3(X(21,i),X(22,i),0, 'g');
% scatter3(X(25,i),X(26,i),0, 'g');
% scatter3(X(29,i),X(30,i),0, 'g');
% scatter3(X(33,i),X(34,i),0, 'b');
% scatter3(X(37,i),X(38,i),0, 'b');
% scatter3(X(41,i),X(42,i),X(43,i), 150,'p', 'm');
% scatter3(X(53,i),X(54,i),X(55,i), 150,'p', 'c');
% scatter3(X(65,i),X(66,i),X(67,i), 'm');
% scatter3(X(77,i),X(78,i),X(79,i), 'm');
% scatter3(X(89,i),X(90,i),X(91,i), 'c');
% scatter3(X(101,i),X(102,i),X(103,i),'c');
% grid on;
% axis equal;
% axis([-80, 80, -50,300, 0,50]);
% xlabel("X(m)"); ylabel("Y(m)"); zlabel("H(m)");
% title("Actual agents’ states");
%
% disp(i)
% pause(0.1)
% if (i<990)
% clf
% end
% end

% Snapshots
figure(9)

subplot(2,2,1)
set(gca,'position',[0.05 0.55 0.43 0.43]);
i = 50;
scatter3(X(1,i), X(2,i), 0, 150,'p', 'r'); hold on;
scatter3(X(5,i), X(6,i), 0, 150,'p', 'g');
scatter3(X(9,i), X(10,i),0, 150,'p', 'b');
scatter3(X(13,i),X(14,i),0, 'r');
scatter3(X(17,i),X(18,i),0, 'r');
scatter3(X(21,i),X(22,i),0, 'g');
scatter3(X(25,i),X(26,i),0, 'g');
scatter3(X(29,i),X(30,i),0, 'g');
scatter3(X(33,i),X(34,i),0, 'b');
scatter3(X(37,i),X(38,i),0, 'b');
scatter3(X(41,i),X(42,i),X(43,i), 150,'p', 'm');
scatter3(X(53,i),X(54,i),X(55,i), 150,'p', 'c');
scatter3(X(65,i),X(66,i),X(67,i), 'm');
scatter3(X(77,i),X(78,i),X(79,i), 'm');
scatter3(X(89,i),X(90,i),X(91,i), 'c');
scatter3(X(101,i),X(102,i),X(103,i),'c');
line([X(1,i),X(13,i)], [X(2,i),X(14,i)]);
line([X(13,i),X(17,i)], [X(14,i),X(18,i)]);
line([X(17,i),X(1,i)], [X(18,i),X(2,i)]);
line([X(5,i),X(21,i)], [X(6,i),X(22,i)]);
line([X(21,i),X(25,i)], [X(22,i),X(26,i)]);
line([X(25,i),X(29,i)], [X(26,i),X(30,i)]);
line([X(29,i),X(5,i)], [X(30,i),X(6,i)]);
line([X(9,i),X(33,i)], [X(10,i),X(34,i)]);
line([X(33,i),X(37,i)], [X(34,i),X(38,i)]);
line([X(37,i),X(9,i)], [X(38,i),X(10,i)]);
line([X(41,i),X(65,i)], [X(42,i),X(66,i)], [X(43,i),X(67,i)]);
line([X(65,i),X(77,i)], [X(66,i),X(78,i)], [X(67,i),X(79,i)]);
line([X(77,i),X(41,i)], [X(78,i),X(42,i)], [X(79,i),X(43,i)]);
line([X(53,i),X(89,i)], [X(54,i),X(90,i)], [X(55,i),X(91,i)]);
line([X(89,i),X(101,i)], [X(90,i),X(102,i)], [X(91,i),X(103,i)]);
line([X(101,i),X(53,i)], [X(102,i),X(54,i)], [X(103,i),X(55,i)]);
grid on;
legend('l1', 'l2', 'l3', 'f4', 'f5', 'f6', 'f7', 'f8', 'f9', 'f10', 'l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
xlabel("X(m)",'Interpreter','latex'); ylabel("Y(m)",'Interpreter','latex'); zlabel("H(m)",'Interpreter','latex');
title("All agents position (t="+i*dT+"s)",'Interpreter','latex');

subplot(2,2,2)
set(gca,'position',[0.55 0.55 0.43 0.43]);
i = 100;
scatter3(X(1,i), X(2,i), 0, 150,'p', 'r'); hold on;
scatter3(X(5,i), X(6,i), 0, 150,'p', 'g');
scatter3(X(9,i), X(10,i),0, 150,'p', 'b');
scatter3(X(13,i),X(14,i),0, 'r');
scatter3(X(17,i),X(18,i),0, 'r');
scatter3(X(21,i),X(22,i),0, 'g');
scatter3(X(25,i),X(26,i),0, 'g');
scatter3(X(29,i),X(30,i),0, 'g');
scatter3(X(33,i),X(34,i),0, 'b');
scatter3(X(37,i),X(38,i),0, 'b');
scatter3(X(41,i),X(42,i),X(43,i), 150,'p', 'm');
scatter3(X(53,i),X(54,i),X(55,i), 150,'p', 'c');
scatter3(X(65,i),X(66,i),X(67,i), 'm');
scatter3(X(77,i),X(78,i),X(79,i), 'm');
scatter3(X(89,i),X(90,i),X(91,i), 'c');
scatter3(X(101,i),X(102,i),X(103,i),'c');
line([X(1,i),X(13,i)], [X(2,i),X(14,i)]);
line([X(13,i),X(17,i)], [X(14,i),X(18,i)]);
line([X(17,i),X(1,i)], [X(18,i),X(2,i)]);
line([X(5,i),X(21,i)], [X(6,i),X(22,i)]);
line([X(21,i),X(25,i)], [X(22,i),X(26,i)]);
line([X(25,i),X(29,i)], [X(26,i),X(30,i)]);
line([X(29,i),X(5,i)], [X(30,i),X(6,i)]);
line([X(9,i),X(33,i)], [X(10,i),X(34,i)]);
line([X(33,i),X(37,i)], [X(34,i),X(38,i)]);
line([X(37,i),X(9,i)], [X(38,i),X(10,i)]);
line([X(41,i),X(65,i)], [X(42,i),X(66,i)], [X(43,i),X(67,i)]);
line([X(65,i),X(77,i)], [X(66,i),X(78,i)], [X(67,i),X(79,i)]);
line([X(77,i),X(41,i)], [X(78,i),X(42,i)], [X(79,i),X(43,i)]);
line([X(53,i),X(89,i)], [X(54,i),X(90,i)], [X(55,i),X(91,i)]);
line([X(89,i),X(101,i)], [X(90,i),X(102,i)], [X(91,i),X(103,i)]);
line([X(101,i),X(53,i)], [X(102,i),X(54,i)], [X(103,i),X(55,i)]);
grid on;
legend('l1', 'l2', 'l3', 'f4', 'f5', 'f6', 'f7', 'f8', 'f9', 'f10', 'l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
xlabel("X(m)",'Interpreter','latex'); ylabel("Y(m)",'Interpreter','latex'); zlabel("H(m)",'Interpreter','latex');
title("All agents position (t="+i*dT+"s)",'Interpreter','latex');

subplot(2,2,3)
set(gca,'position',[0.05 0.05 0.43 0.43]);
i = 300;
scatter3(X(1,i), X(2,i), 0, 150,'p', 'r'); hold on;
scatter3(X(5,i), X(6,i), 0, 150,'p', 'g');
scatter3(X(9,i), X(10,i),0, 150,'p', 'b');
scatter3(X(13,i),X(14,i),0, 'r');
scatter3(X(17,i),X(18,i),0, 'r');
scatter3(X(21,i),X(22,i),0, 'g');
scatter3(X(25,i),X(26,i),0, 'g');
scatter3(X(29,i),X(30,i),0, 'g');
scatter3(X(33,i),X(34,i),0, 'b');
scatter3(X(37,i),X(38,i),0, 'b');
scatter3(X(41,i),X(42,i),X(43,i), 150,'p', 'm');
scatter3(X(53,i),X(54,i),X(55,i), 150,'p', 'c');
scatter3(X(65,i),X(66,i),X(67,i), 'm');
scatter3(X(77,i),X(78,i),X(79,i), 'm');
scatter3(X(89,i),X(90,i),X(91,i), 'c');
scatter3(X(101,i),X(102,i),X(103,i),'c');
line([X(1,i),X(13,i)], [X(2,i),X(14,i)]);
line([X(13,i),X(17,i)], [X(14,i),X(18,i)]);
line([X(17,i),X(1,i)], [X(18,i),X(2,i)]);
line([X(5,i),X(21,i)], [X(6,i),X(22,i)]);
line([X(21,i),X(25,i)], [X(22,i),X(26,i)]);
line([X(25,i),X(29,i)], [X(26,i),X(30,i)]);
line([X(29,i),X(5,i)], [X(30,i),X(6,i)]);
line([X(9,i),X(33,i)], [X(10,i),X(34,i)]);
line([X(33,i),X(37,i)], [X(34,i),X(38,i)]);
line([X(37,i),X(9,i)], [X(38,i),X(10,i)]);
line([X(41,i),X(65,i)], [X(42,i),X(66,i)], [X(43,i),X(67,i)]);
line([X(65,i),X(77,i)], [X(66,i),X(78,i)], [X(67,i),X(79,i)]);
line([X(77,i),X(41,i)], [X(78,i),X(42,i)], [X(79,i),X(43,i)]);
line([X(53,i),X(89,i)], [X(54,i),X(90,i)], [X(55,i),X(91,i)]);
line([X(89,i),X(101,i)], [X(90,i),X(102,i)], [X(91,i),X(103,i)]);
line([X(101,i),X(53,i)], [X(102,i),X(54,i)], [X(103,i),X(55,i)]);
grid on;
legend('l1', 'l2', 'l3', 'f4', 'f5', 'f6', 'f7', 'f8', 'f9', 'f10', 'l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
xlabel("X(m)",'Interpreter','latex'); ylabel("Y(m)",'Interpreter','latex'); zlabel("H(m)",'Interpreter','latex');
title("All agents position (t="+i*dT+"s)",'Interpreter','latex');

subplot(2,2,4)
set(gca,'position',[0.55 0.05 0.43 0.43]);
i = 500;
scatter3(X(1,i), X(2,i), 0, 150,'p', 'r'); hold on;
scatter3(X(5,i), X(6,i), 0, 150,'p', 'g');
scatter3(X(9,i), X(10,i),0, 150,'p', 'b');
scatter3(X(13,i),X(14,i),0, 'r');
scatter3(X(17,i),X(18,i),0, 'r');
scatter3(X(21,i),X(22,i),0, 'g');
scatter3(X(25,i),X(26,i),0, 'g');
scatter3(X(29,i),X(30,i),0, 'g');
scatter3(X(33,i),X(34,i),0, 'b');
scatter3(X(37,i),X(38,i),0, 'b');
scatter3(X(41,i),X(42,i),X(43,i), 150,'p', 'm');
scatter3(X(53,i),X(54,i),X(55,i), 150,'p', 'c');
scatter3(X(65,i),X(66,i),X(67,i), 'm');
scatter3(X(77,i),X(78,i),X(79,i), 'm');
scatter3(X(89,i),X(90,i),X(91,i), 'c');
scatter3(X(101,i),X(102,i),X(103,i),'c');
line([X(1,i),X(13,i)], [X(2,i),X(14,i)]);
line([X(13,i),X(17,i)], [X(14,i),X(18,i)]);
line([X(17,i),X(1,i)], [X(18,i),X(2,i)]);
line([X(5,i),X(21,i)], [X(6,i),X(22,i)]);
line([X(21,i),X(25,i)], [X(22,i),X(26,i)]);
line([X(25,i),X(29,i)], [X(26,i),X(30,i)]);
line([X(29,i),X(5,i)], [X(30,i),X(6,i)]);
line([X(9,i),X(33,i)], [X(10,i),X(34,i)]);
line([X(33,i),X(37,i)], [X(34,i),X(38,i)]);
line([X(37,i),X(9,i)], [X(38,i),X(10,i)]);
line([X(41,i),X(65,i)], [X(42,i),X(66,i)], [X(43,i),X(67,i)]);
line([X(65,i),X(77,i)], [X(66,i),X(78,i)], [X(67,i),X(79,i)]);
line([X(77,i),X(41,i)], [X(78,i),X(42,i)], [X(79,i),X(43,i)]);
line([X(53,i),X(89,i)], [X(54,i),X(90,i)], [X(55,i),X(91,i)]);
line([X(89,i),X(101,i)], [X(90,i),X(102,i)], [X(91,i),X(103,i)]);
line([X(101,i),X(53,i)], [X(102,i),X(54,i)], [X(103,i),X(55,i)]);
grid on;
legend('l1', 'l2', 'l3', 'f4', 'f5', 'f6', 'f7', 'f8', 'f9', 'f10', 'l11', 'l12', 'f13', 'f14', 'f15', 'f16','Interpreter','latex');
xlabel("X(m)",'Interpreter','latex'); ylabel("Y(m)",'Interpreter','latex'); zlabel("H(m)",'Interpreter','latex');
title("All agents position (t="+i*dT+"s)",'Interpreter','latex');