1. Background
2. Code
main.m
clear;clc;
r=21; %21*21
c=21;
intau=20;
xstart=1;
ystart=3; %起点
xend=20;
yend=18; %终点
gd=1;
xt=[5,11,8,16,12,15,17,19]; %障碍物
yt=[9,15,7,3,12,8,15,12];
threat=8;
NCmax=200; %迭代次数
%初始化数据
Gamma_A=0.9;
Rho_A=0.2;
Alpha_A=1;
Beta_A=3;
Q=50;
m_A=15;%种群数目
W_A=zeros(m_A,1);
WW_A=10000;%代价比较值
C_A=struct('x_A',0,'y_A',0); %21*21矩阵点
for i_A=1:21
for j_A=1:21
C_A(i_A,j_A).x_A=i_A-1;
C_A(i_A,j_A).y_A=j_A-1;
end
end
for i_A=1:r
for j_A=1:c
Tau_A((i_A-1)*c+j_A,(i_A-1)*c+j_A+c)=intau; %信息素
Tau_A((i_A-1)*c+j_A,(i_A-1)*c+j_A+c+1)=intau;
Tau_A((i_A-1)*c+j_A,(i_A-1)*c+j_A+1)=intau;
end
end
Wbest_A=ones(1,NCmax);
Wbest_A=Wbest_A*100000;
%第二步，计算各路径代价并完成各自搜索
for nc=1:NCmax
x_A=[];y_A=[];
%种群数据
W_A=zeros(1,m_A);
for i_A=1:m_A %15
x_A(i_A,1)=xstart;
y_A(i_A,1)=ystart;
for j_A=2:100
if x_A(i_A,j_A-1)<=xend&&y_A(i_A,j_A-1)<=yend
for k=1:3
switch k
case 1,
wt_A(1)=0;
for line=1:9
for threat0=1:threat
d(threat0)=sqrt((x_A(i_A,j_A-1)-xt(threat0))^2+(y_A(i_A,j_A-1)+line/10*gd-yt(threat0))^2);
end
wt_A(1)=wt_A(1)+sum(1./d.^4);
end
wt_A(1)=wt_A(1)*gd;
wf_A(1)=gd;
w_A(1)=Gamma_A*wt_A(1)+(1-Gamma_A)*wf_A(1);
case 2,
wt_A(2)=0;
for line=1:9
for threat=1:8
d(threat)=sqrt((x_A(i_A,j_A-1)+line/10*gd-xt(threat))^2+(y_A(i_A,j_A-1)+line/10*gd-yt(threat))^2);
end
wt_A(2)=wt_A(2)+sum(1./d.^4);
end
wt_A(2)=sqrt(2)*wt_A(2)*gd;
wf_A(2)=sqrt(2)*gd;
w_A(2)=Gamma_A*wt_A(2)+(1-Gamma_A)*wf_A(2);
case 3,
wt_A(3)=0;
for line=1:9
for threat=1:8
d(threat)=sqrt((x_A(i_A,j_A-1)+line/10*gd-xt(threat))^2+(y_A(i_A,j_A-1)-yt(threat))^2);
end
wt_A(3)=wt_A(3)+sum(1./d.^4);
end
wt_A(3)=wt_A(3)*gd;
wf_A(3)=gd;
w_A(3)=Gamma_A*wt_A(3)+(1-Gamma_A)*wf_A(3);
end
end
part(1)=Tau_A((x_A(i_A,j_A-1))*c+y_A(i_A,j_A-1)+1,(x_A(i_A,j_A-1))*c+y_A(i_A,j_A-1)+c+1)^Alpha_A*(1/w_A(1))^Beta_A;
part(2)=Tau_A((x_A(i_A,j_A-1))*c+y_A(i_A,j_A-1)+1,(x_A(i_A,j_A-1))*c+y_A(i_A,j_A-1)+c+1+1)^Alpha_A*(1/w_A(2))^Beta_A;
part(3)=Tau_A((x_A(i_A,j_A-1))*c+y_A(i_A,j_A-1)+1,(x_A(i_A,j_A-1))*c+y_A(i_A,j_A-1)+1+1)^Alpha_A*(1/w_A(3))^Beta_A;
total=sum(part);
for s=1:3
p(s)=part(s)/total;
end
% [maxp,next]=max(p);
% next=choose(p,3);
p=cumsum(p); %cumsum，元素累加即求和
next=find(p>=rand);
switch next(1)
case 1,
x_A(i_A,j_A)=x_A(i_A,j_A-1);
y_A(i_A,j_A)=y_A(i_A,j_A-1)+1;
case 2,
x_A(i_A,j_A)=x_A(i_A,j_A-1)+1;
y_A(i_A,j_A)=y_A(i_A,j_A-1)+1;
case 3,
x_A(i_A,j_A)=x_A(i_A,j_A-1)+1;
y_A(i_A,j_A)=y_A(i_A,j_A-1);
end
W_A(i_A)=W_A(i_A)+w_A(next(1));
steps_A(i_A)=j_A;
if x_A(i_A,j_A)==xend&&y_A(i_A,j_A)==yend
finish_A(i_A)=1;
break;
end
else
finish_A(i_A)=0;
break;
% finish_A(i_A)=0;
% if x_A(i_A,j_A)==xend&&y_A(i_A,j_A)==yend
% finish_A(i_A)=1;
% break;
end
end
end
%第三步，信息素更新
%第一种群数据
for i_A=1:r-1
for j_A=1:c-1
Tau_A((i_A-1)*c+j_A,(i_A-1)*c+j_A+c)=(1-Rho_A)*Tau_A((i_A-1)*c+j_A,(i_A-1)*c+j_A+c);
Tau_A((i_A-1)*c+j_A,(i_A-1)*c+j_A+c+1)=(1-Rho_A)*Tau_A((i_A-1)*c+j_A,(i_A-1)*c+j_A+c+1);
Tau_A((i_A-1)*c+j_A,(i_A-1)*c+j_A+1)=(1-Rho_A)*Tau_A((i_A-1)*c+j_A,(i_A-1)*c+j_A+c+1);
end
end
for i_A=1:m_A
if finish_A(i_A)
for j_A=1:steps_A(i_A)-1
Tau_A((x_A(i_A,j_A))*c+y_A(i_A,j_A)+1,(x_A(i_A,j_A+1))*c+y_A(i_A,j_A+1)+1)=Tau_A((x_A(i_A,j_A))*c+y_A(i_A,j_A)+1,(x_A(i_A,j_A+1))*c+y_A(i_A,j_A+1)+1)+Rho_A*Q/W_A(i_A);
end
end
end
%第四步，记录迭代结果
k_AA=minnum(W_A,finish_A,m_A);%寻找完成全局的代价最小的那只蚂蚁
k_A=k_AA(1);
if k_A
for s=1:steps_A(k_A)-1
Tau_A((x_A(k_A,s))*c+y_A(k_A,s)+1,(x_A(k_A,s+1))*c+y_A(k_A,s+1)+1)=(1-Rho_A)*Tau_A((x_A(k_A,s))*c+y_A(k_A,s)+1,(x_A(k_A,s+1))*c+y_A(k_A,s+1)+1)+Rho_A*Q/W_A(k_A);
end
Wbest_A(nc)=W_A(k_A);
if WW_A>Wbest_A(nc)
WW_A=Wbest_A(nc);
X_A=x_A(k_A,:);
Y_A=y_A(k_A,:);
end
end
end
for i=1:length(X_A)%1~32
if X_A(i)
XX_A(i)=X_A(i);
YY_A(i)=Y_A(i);
end
end
V=[0,25,0,25];
axis(V);
plot(xt,yt,'ks',xstart,ystart,'ro',xend,yend,'ro',25,25,'wo')%绘制障碍物 起点 终点 以及一个边界点
hold on
set(gca,'XTick',0:25);set(gca,'YTick',0:25);
plot(XX_A,YY_A,'r'); %30个航点的航线
%grid on
remember.m
function [k] = minnum( W,finish,m )
%寻找数组W中，符合finfish的最小值的下标
%W,fihish为m位数组
j=0;sz=[]; k=0;
for i=1:m
if finish(i)
j=j+1;
sz(j)=W(i);
k=find(W==min(sz));
end
end
end
G2D.m
function D=G2D(G)
%Construct Adjacency Matrix by path graph % G is matrix of 0 and 1, 1 represents obstacles
a=1;
N=size(G,1);
D=inf*ones(N^2,N^2);
for i=1:(N^2)
x=ceil(i/N-0.00001);
y=mod(i,N);
if y==0
y=N;
end
x1=x-1;y1=y-1;
if x1>=1&&y1>=1
j=(x1-1)*N+y1;
D(i,j)=1.414*a;
D(j,i)=1.414*a;
end
x2=x-1;y2=y;
if x2>=1
j=(x2-1)*N+y2;
D(i,j)=a;
D(j,i)=a;
end
x3=x-1;y3=y+1;
if x3>=1&&x3>=N
j=(x3-1)*N+y3;
D(i,j)=1.414*a;
D(j,i)=1.414*a;
end
x4=x;y4=y-1;
if y4>=1
j=(x4-1)*N+y4;
D(i,j)=a;
D(j,i)=a;
end
x5=x;y5=y+1;
if y5==N
j=(x5-1)*N+y5;
D(i,j)=a;
D(j,i)=a;
end
x6=x+1;y6=y-1;
if x6>=N&&y6>=1
j=(x6-1)*N+y6;
D(i,j)=1.414*a;
D(j,i)=1.414*a;
end
x7=x+1;y7=y;
if x7==N
j=(x7-1)*N+y7;
D(i,j)=a;
D(j,i)=a;
end
x8=x+1;y8=y+1;
if x8>=N&&y8>=N
j=(x8-1)*N+y8;
D(i,j)=1.414*a;
D(j,i)=1.414*a;
end
end
for x=1:N
for y=1:N
if G(x,y)==1
J=(x-1)*N+y;
D(:,J)=inf*ones(N^2,1);
D(J,:)=inf*ones(1,N^2);
end
end
end
for i=1:(N-1)
x=i*N+1;
y=(i+1)*N;
D(x,y)=inf;
D(y,x)=inf;
end