?? sm_5_2.m
字號:
%------------------------------------------------------------------------
% SWARM SIMULATION PROGRAM
% DESIGNED BY Jun Lu
% Intelligent and Complex Systems Lab
% HuaZhong University of Science and Technology
%------------------------------------------------------------------------
cla;
clear all;
dimensation=25;
x_min = -dimensation;
y_min = -dimensation;
x_max = dimensation;
y_max = dimensation;
L0= 10.0; ds = 2*L0/101;
x = -L0:ds:+L0; y = -L0:ds:+L0;
[xx,yy] = meshgrid(x,y);
%initial positions of agents
xini =[ -37.3127 -1.7026
-35.6539 -4.9508
-34.8340 -2.3068
-38.8444 -3.3499
-36.8089 1.9830
-35.5775 -1.4473
-32.1440 -0.5999
-33.5098 4.1988
-34.1841 0.6252
-36.5392 0.7334
-37.5426 -2.6241
-31.5493 -3.9901
-35.5878 1.4199
-34.0984 5.7537
-30.9944 2.7939
];
%initial parameters of leader
leader=[-30,0];
vl=0.66;
x=[leader
xini]
xt=x;
%parameters of obstacles
Cord_obs=[-10+i*0;-5-i*5;0+i*3.8;8+i*3];%障礙物圓點
Rad_obs=[4.0;1.5;3;2.5];%障礙物半徑
a=1;b=10;c=2.5;%k1,k2,k3的值，inter-agent effert的參數
lo=[32;4.5;4.5;32;32];% lo的值,表3所示
Ad=[-38 -36 -36 -38 -38];% mo的值,表3所示
%sensor range of agents
sensor_r=16;
safe_r=0.860
%Strength of uniform flow
U0=10;
no_obstacle=length(Rad_obs);%length:數組長度，障礙物的個數
no_robots =length(x);%機器人的個數
Distance_Matrix=zeros(no_robots);%zeros:產生全為0的矩陣 ，計算LOS連接的agent的距離,間接反應LOS的情況，沒有連接，就不用判斷距離
GC=ones(no_robots);%ones:產生全為1的矩陣 ，產生機器人個數的全1矩陣：用來判斷機器人之間的聯系（get connection）
for tc=1:no_obstacle %對每個障礙物分別判斷LOS，求&運算，得出agent之間的聯系
GT=TC(real(Cord_obs(tc)),imag(Cord_obs(tc)),Rad_obs(tc),x);%輸入障礙物的圓點，半徑，x為機器人的初始化坐標，對障礙物分別判斷LOS
GC=GC & GT;
end
for i=1:no_robots % this part calculate the distance between those robots which have connectivity with each other計算連接的距離
for j=1:no_robots
if (GC(i,j)==1)
Distance_Matrix1(i,j)=sqrt((x(i,1)-x(j,1))^2+(x(i,2)-x(j,2))^2)*GC(i,j);
end
end
end
for i=1:no_robots % 計算相連接的x軸距離
for j=1:no_robots
if (GC(i,j)==1)
Distance_Matrix(i,j)=abs(x(i,1)-x(j,1));
end
end
end
PM=TProb(Distance_Matrix,x) %---call function TProb to decide the agent for associating，由x軸距離判斷連接
G2=PM;%x選取了每個agent相連的agent！！！！
%初始化畫圖
ahua=0;
for nn=1:no_obstacle
line('color', 'r', 'Marker', '.', 'markersize', Rad_obs(nn)*33, 'erase', 'background', 'xdata', real(Cord_obs(nn)), 'ydata', imag(Cord_obs(nn)));
end
line('color', 'b', 'Marker' ,'s', 'markersize', 4.3, 'erase', 'background', 'MarkerFaceColor','b','xdata', x(1, 1), 'ydata', x(1, 2));
for mm = 2: no_robots
line('color', 'r', 'Marker' ,'>', 'markersize', 3.5, 'erase', 'background', 'MarkerFaceColor','r','xdata', x(mm, 1), 'ydata', x(mm, 2));
end
axis([x_min x_max y_min y_max]);
drawnow;
nf=1;
rstep=1;
endt=0.54;
h=0.010;
for t=0:h:endt
ts =0.015;%時間間隔，每隔ts秒就更新機器人的位置
%計算作用力
for j=2:no_robots
%-----------------------calculate the fluid effect----------------------------------------
temp = [0 0];
for k=1:no_obstacle
rr2p(k)=(x(j,1)-real(Cord_obs(k)))^2 + (x(j,2)-imag(Cord_obs(k)))^2;%agent與障礙物圓點的距離的平方
rr2n(k)=(x(j,1)-real(Cord_obs(k)))^2 - (x(j,2)-imag(Cord_obs(k)))^2;
rr2m(k)=2*(x(j,1)-real(Cord_obs(k)))*(x(j,2)-imag(Cord_obs(k)));
if rr2p(k)<=400%距離太遠不考慮 流函數
Fadd1(k)=(Rad_obs(k)^2)*rr2n(k)/(rr2p(k))^2;
Fadd2(k)=(Rad_obs(k)^2)*rr2m(k)/(rr2p(k))^2;
else
Fadd1(k)=0;
Fadd2(k)=0;
end
end
temp(1)=-U0*(sum(Fadd1)-1);
temp(2)=-U0*(sum(Fadd2));
xt(j,1) = xt(j,1) + ts*temp(1);
xt(j,2) = xt(j,2) + ts*temp(2);
%-----------------------calculate the swarm force----------------------------------------
for q=1:no_robots
distance_nab(q) = sqrt((x(j,1)-x(q,1))^2 + (x(j,2)-x(q,2))^2);%agent之間的距離，用來判斷是否處于sensor的探測范圍內
if distance_nab(q)<sensor_r
naber_factor=1;
else
naber_factor=0;
end
for ni=1:2
temp(ni) = -(x(j,ni)-x(q,ni))*(a-b*exp(-distance_nab(q)^2/c))*naber_factor*G2(j,q);
xt(j,ni)=xt(j,ni)+ts*temp(ni);
end
end
%-----------------------歸隊作用力----------------------------------------
m=0;
for p=1:no_robots
distance_nab(p) = sqrt((x(j,1)-x(p,1))^2 + (x(j,2)-x(p,2))^2);
if naber_factor*G2(j,p)==0%如果對于agent j,沒有agent和它相連（G2(j,q)）或者不處于相連agent探測范圍內，則受到leader的作用
m=m+1;
end
end
if m==no_robots;
if GC(1,j)==1%如果處于leader的LOS范圍內
for ni=1:2
temp(ni) = -(x(j,ni)-x(1,ni))*(a-b*exp(-distance_nab(1)^2/c));
xt(j,ni)=xt(j,ni)+ts*temp(ni);
end
end
end
%-------------------------靠近排斥作用力---------------------------
for n=1:no_robots
distance_nab(n) = sqrt((x(j,1)-x(n,1))^2 + (x(j,2)-x(n,2))^2);%agent之間的距離，用來判斷是否處于sensor的探測范圍內
if distance_nab(n)<safe_r
naber_factor=1;
else
naber_factor=0;
end
for ni=1:2
temp(ni) = -naber_factor*(x(j,ni)-x(n,ni))*(a-b*exp(-distance_nab(n)^2/c));;
xt(j,ni)=xt(j,ni)+ts*temp(ni);
end
end
%-----------------------------APF forces of obstacle----------------------
for k=1:no_obstacle
dq(k)=sqrt((x(j,1)-real(Cord_obs(k)))^2 + (x(j,2)-imag(Cord_obs(k)))^2);
if dq(k)<1.5*Rad_obs(k)%當agent與障礙物的距離小于1.5倍的障礙物半徑時，考慮障礙物的斥力
temp_obs(1)=-Ad(k)*(x(j,1)-real(Cord_obs(k)))*exp(-dq(k)^2/lo(k))/lo(k) ;
temp_obs(2)=-Ad(k)*(x(j,2)-imag(Cord_obs(k)))*exp(-dq(k)^2/lo(k))/lo(k) ;
else
temp_obs(1)=0 ;
temp_obs(2)=0 ;
end
xt(j,1)=xt(j,1)+ts*temp_obs(1);
xt(j,2)=xt(j,2)+ts*temp_obs(2);
end
%-----------------------------APF forces of obstacle----------------------
xt(1,1)=xt(1,1)+ts*vl;
xt(1,2)=xt(1,2);
end
%----------------------------------
x=xt;
GC=ones(no_robots);
Distance_Matrix=zeros(no_robots);
for tc=1:no_obstacle
GT=TC(real(Cord_obs(tc)),imag(Cord_obs(tc)),Rad_obs(tc),x);
GC=GC & GT;
end
GC;
for i=1:no_robots % this part calculate the distance between those robots which have connectivity with each other
for j=1:no_robots
if (GC(i,j)==1)
Distance_Matrix1(i,j)=sqrt((x(i,1)-x(j,1))^2+(x(i,2)-x(j,2))^2);
end
end
end
for i=1:no_robots % 計算相連接的x軸距離
for j=1:no_robots
if (GC(i,j)==1)
Distance_Matrix(i,j)=abs(x(i,1)-x(j,1));
end
end
end
PM=TProb(Distance_Matrix,x)%---call function test_probability.m---------------------------------
G2=PM;
cla;
for nn=1:no_obstacle
line('color', 'r', 'Marker', '.', 'markersize', Rad_obs(nn)*33, 'erase', 'background', 'xdata', real(Cord_obs(nn)), 'ydata', imag(Cord_obs(nn)));
end
line('color', 'b', 'Marker' ,'s', 'markersize', 4.3, 'erase', 'background', 'MarkerFaceColor','b','xdata', x(1, 1), 'ydata', x(1, 2));
for mm = 2: no_robots
line('color', 'r', 'Marker' ,'>', 'markersize',3.5, 'erase', 'background', 'MarkerFaceColor','r','xdata', x(mm, 1), 'ydata', x(mm, 2));
end
%plot the inter-agent lines
for grax=1:no_robots
for gray=1:no_robots
if Distance_Matrix1(grax,gray)<=sensor_r
if GC(grax,gray)==1
if G2(grax,gray)==1
AA=[x(grax,1),x(gray,1)];
BB=[x(grax,2),x(gray,2)];
line(AA,BB,'Color','g','LineWidth',0.5);
else
AA=[x(grax,1),x(gray,1)];
BB=[x(grax,2),x(gray,2)];
end
end
end
end
end
stepn=t/h;
rob4x=x(1,1);
xlabel(stepn);% x軸卷標描述更新的次數
title(mean(x(:,1)));%標題描述x軸坐標的均值
RFX(rstep)=rob4x;
rstep=rstep+1;
nf=nf+1;
pause(0.0168); %this can be adjust to control the simulation speed
end?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -