% 本文參照文獻：Flocking for Multi-Agent Dynamic Systems:Algorithms and Theory

clear;
close all;
clc;
%% Parameters 初始化參數
num_agents = 100;
t_gap=1; ? ? ? ? ? ? ? % 迭代間隔
queue_gap=15; ? ? ? ? ?% 隊形間隔
queue_vy=12;
queue_vx=13;
queue_r=40;
r_c=20; ? ? ? ? ? ? ? ?% 交互范圍（半徑）
k=1.2; ? ? ? ? ? ? ? ? % 晶格的ratio
d=r_c/k; ? ? ? ? ? ? ? % 晶格的scale(表示兩兩智能體之間的距離（論文中公式5）)
v_0=2; ? ? ? ? ? ? ? ? % 初始速度
v_limit=0; ? ? ? ? ? ? % 最大速度
efs = 1; ? ? ? ? ? ? ? % sigma-norms parameter?
h=0.4; ? ? ? ? ? ? ? ? % 設置bump function的分割點（公式10）
d_o = r_c; ? ? ? ? ? ??
r_c_sigma = sigma_norm(r_c,efs); ? ? ? ? % r_c的σ范數
d_sigma = sigma_norm(d,efs); ? ? ? ? ? ? % d的σ范數
map_width = 400; ? ? ? ? ? ? ? ? ? ? ? ? % width of a squre map
map_res = 0.5; ? ? ? ? ? ? ? ? ? ? ? ? ? % width of a grid to play obstacles pixel
c1=0.2;c2=0.5;c3=0.2;c4=0.1;c5=10;c6=0.01;
x = zeros(num_agents,2); ? ? ? ? ? ? ? ? % current position
x_1 = zeros(num_agents,2); ? ? ? ? ? ? ? % previous position
v_1 = zeros(num_agents,2); ? ? ? ? ? ? ? % previous velocity
x_r0= zeros(num_agents,2); ? ? ? ? ? ? ? % x_r0：用來存儲指定的隊形信息
v_r_1st_point=300;
path_num = zeros(1000,2,num_agents);
v_r=[1,0]; ?
%% 生成不同編隊隊形的智能體坐標

% % 使α-agent群集呈現豎“一”(指定預期坐標)
for a=1:1:num_agents
? ? b=v_r_1st_point-(a-1)*queue_gap; ? ? ?
? ? x_r1(a,2)=b;
end

% % 使α-agent群集呈現V字形
for a=1:1:num_agents
? ? queue_vy=12;
? ? b=250-(a-1)*queue_vy;
? ? x_r2(a,2)=b;
? ? if a<=num_agents/2
? ? ? ? c=100+queue_vx*(a-1);
? ? ? ? x_r2(a,1)=c;
? ? else
? ? ? ? c=100+queue_vx*(num_agents-a);
? ? ? ? x_r2(a,1)=c;
? ? end
end

% % 坐標更新后的豎“一”
for a=1:1:num_agents
? ? b=v_r_1st_point-(a-1)*queue_gap;
? ? x_r3(a,2)=b;
? ? x_r3(a,1)=200+queue_vx*ceil(num_agents/2);
end

% % 使α-agent群集呈現倒V字形
for a=1:1:num_agents
? ? b=250-(a-1)*queue_vy;
? ? x_r4(a,2)=b;
? ? if a<=num_agents/2
? ? ? ? c=340+queue_vx*ceil(num_agents/2)-queue_vx*(a-1);
? ? ? ? x_r4(a,1)=c;
? ? else
? ? ? ? c=340+queue_vx*ceil(num_agents/2)-queue_vx*(num_agents-a);
? ? ? ? x_r4(a,1)=c;
? ? end
end

% % 使α-agent群集呈現圓形
for a=1:num_agents
? ? theta=2*pi/num_agents;
? ? xo=[450+queue_vx*ceil(num_agents/2),240];
? ? x_r5(a,1)=xo(1)+queue_r*sin(a*theta);
? ? x_r5(a,2)=xo(2)-queue_r*cos(a*theta);
end

% % 坐標再次更新后的橫“一”
for a=1:1:num_agents
? ? b=v_r_1st_point-(a-1)*queue_gap;
? ? x_r6(a,2)=b;
? ? x_r6(a,1)=580+2*queue_r;
end
% ? x_r=[0,150;0,130;0,110];?
?
% %initializing agents 隨機生成坐標
x_1(:,1)=20*rand(num_agents,1); ? ? ? ? ? ? ? ? ? ? ? ? ?% x_1第一列是agent的x初始坐標?
x_1(:,2) = map_width/2*rand(num_agents,1)+map_width/4; ? % x_1第二列是agent的y初始坐標
counter = 0; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? % 用于記錄循環次數
%% main loop 主循環
while(true)
? ? counter=counter+1;
? ? u_alpha=zeros(num_agents,2); ? ? ? ? ? ? ? ?% alpha-agents correspond to UAVs ? 初始化alpha agent
? ? u_gamma=zeros(num_agents,2); ? ? ? ? ? ? ? ?% gamma-objects which model the effect of “collective objective” of a group ?初始化gamma agent
? ? dist_gap = get_gap(x_1); ? ? ? ? ? ? ? ? ? ?% 獲取間隔距離
? ? dist_2 = squd_norm(dist_gap); ? ? ? ? ? ? ? % 距離間隔平方和
? ? dist = sqrt(dist_2); ? ? ? ? ? ? ? ? ? ? ? ?% 對距離平方和開方 ?獲得距離
? ? dist_sigma=sigma_norm(dist_gap,efs);
? ? adj = bump_func(dist_sigma/r_c_sigma,h); ? ?% 求鄰接矩陣
? ? nbr = zeros(num_agents); ? ? ? ? ? ? ? ? ? ?% 初始化相鄰agent矩陣參數
? ? nbr(dist<=r_c) = 1; ? ? ? ? ? ? ? ? ? ? ? ? % 如果兩個agent距離小于r_c就算是鄰居
? ? nbr = nbr - diag(diag(nbr)); ? ? ? ? ? ? ? ?% set diagonal to 0 取中心元素
? ? adj = nbr.*adj; ? ? ? ? ? ? ? ? ? ? ? ? ? ? % 得到鄰接矩陣
%% 計算過α-agent的控制輸入
% % 這個控制輸入主要包括兩部分
% % 1.目標對α-agent的吸引力
% % 2.α-agent之間的相互作用力
? ?
? ?% % 計算γ-agent對每個α-agent的吸引力
? ?% u_gamma=-c1*limit(x_1-repmat(x_r,size(u_gamma,1),1),1)-c2*(v_1-repmat(v_r,size(u_gamma,1),1));?
? ? ?u_gamma=-c1*limit(x_1-x_r0,size(u_gamma,1))-c2*(v_1-repmat(v_r,size(u_gamma,1),1));?
? ? ?
? ?% % 計算α-agent之間的相互作用力
? ? for a=1:1:num_agents ? ? ? ? ? ? ? ?% 選定一個α-agent
? ? ? ? for b=1:num_agents ? ? ? ? ? ? ?% 通過循環的方式依次計算每個α-agent對選定α-agent的作用力的合力
? ? ? ? ? ? kk=x_1(b,:)-x_1(a,:);
? ? ? ? ? ? u_alpha(a,:)=u_alpha(a,:)+c3*adj(a,b)*phi_func(sigma_norm_2(kk,efs)-d_sigma)*sigma_norm_gradient(kk,efs)+c4*adj(a,b)*(v_1(b,:)-v_1(a,:)); ? % 將選定α-agent受到的合力一次疊加
% ? ? ? ? ? u_alpha(a,:)=u_alpha(a,:)+c3*nbr(a,b)*action_function( sigma_norm_2(x_1(a,:)-x_1(b,:),efs),r_c_sigma,d_sigma,h )*sigma_norm_gradient(x_1(b,:)-x_1(a,:),efs)+c4*nbr(a,b)*(v_1(b,:)-v_1(a,:));
? ? ? ? end
? ? end
%% 根據gamma和alpha agent得到v和x（卡爾曼濾波），前提在一個運動周期內速度不變。
? ? u=u_gamma+u_alpha; ? ? ? ? ? ? ? ? ? % 計算控制輸入 ? ? ? ??
? ? v = v_1 + u*t_gap; ? ? ? ? ? ? ? ? ? % 當前時刻的速度估計 = 前一時刻的速度 + 速度增量
? ? v_1=v; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? % 不考慮噪聲，將估計值直接認為當前時刻的真實值
? ? x(:,1:2) = x_1(:,1:2) + 0.8*v_1*t_gap; ? % 根據速度計算當前時刻的位置x，x_1表示前一時刻的位置
? ? x_1 = x;
? ? for nn = 1:1:num_agents
? ? ? ?path_num(counter,1,nn) = x(nn,1);
? ? ? ?path_num(counter,2,nn) = x(nn,2);
? ? end?
%% 隊形變換
? ? ?if (counter<100)
? ? ? ? x_r1=x_r1+v_r*t_gap;
? ? ? ? x_r0=x_r1;
? ? ?elseif (counter<200)
? ? ? ? x_r2=x_r2+v_r*t_gap;
? ? ? ? x_r0=x_r2;
? ? ?elseif counter<300
? ? ? ? x_r3=x_r3+v_r*t_gap;
? ? ? ? x_r0=x_r3;
? ? ?elseif counter<400
? ? ? ? x_r4=x_r4+v_r*t_gap;
? ? ? ? x_r0=x_r4;
? ? ?elseif counter<500
? ? ? ? x_r5=x_r5+v_r*t_gap;
? ? ? ? x_r0=x_r5;
? ? ?else
? ? ? ? x_r6=x_r6+v_r*t_gap;
? ? ? ? x_r0=x_r6;
? ? ?end

%% plot
? ? hold off
? ? plot(x(:,1),x(:,2),'ro');
? ? hold on
? ? axis([0 800 0 map_width]);
? ? plot(x_r0(:,1),x_r0(:,2),'k*');
? ? if counter>650
? ? ? ? ?break;
? ? end
? ? hold on

% ? ? text(340,150,'★');
? ? %% 通信agent畫線
? ? for ii=1:1:num_agents
? ? ? ? for jj=1:1:num_agents
? ? ? ? ? ? if nbr(ii,jj)>0
? ? ? ? ? ? ? ? plot([x(ii,1);x(jj,1)],[x(ii,2);x(jj,2)]);
? ? ? ? ? ? end
? ? ? ? end
? ? end
? ? %%?
? ? scatter(x(:,1),x(:,2),20,'black')
% ? ? ?plot(x_r1(:,1),x_r1(:,2),'r*');

? ? pause(0.1);
??

end
figure(1)
%hold off
for tt = 1:1:num_agents
for t = 1:1:1000
? ? plot(path_num(t,1,tt),path_num(t,2,tt),'k.','MarkerSize',1);
? ? hold on;
end
end

d128

?

總結

以上是生活随笔為你收集整理的Flocking for Multi-Agent Dynamic Systems:Algorithms and Theory的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。