📄 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 + -