function [TT,Pbest,a,fv]=PSO(fitness,w,c1,c2,N,D,M,Lsg,alfa,o,r)
format long;
vmax=1.5;
for i=1:N
for j=1:D
x(i,j)=unifrnd(-12,12);
while(((j*Lsg/(D+1)-o(1,1))^2+(x(i,j)-o(2,1))^2)<r^2||((j*Lsg/(D+1)-o(1,2))^2+(x(i,j)-o(2,2))^2)<r^2)
x(i,j)=unifrnd(-12,12);
end
v(i,j)=(2*rand-1)*vmax;
if v(i,j)>vmax
v(i,j)=vmax;
end
end
p(i)=fitness(D,x(i,:));
I(i,:)=x(i,:);
end
pg=x(N,:);
for i=1:(N-1)
if fitness(D,x(i,:))<fitness(D,pg)
pg=x(i,:);
end
end
TT=[];
for t=1:M;
for i=1:N
v(i,:)=w*v(i,:)+c1*rand*(I(i,:)-x(i,:))+c2*rand*(pg-x(i,:));
if v(i,:)>vmax
v(i,:)=vmax;
end
for j=1:D
x(i,j)=x(i,j)+v(i,j);
if ((j*Lsg/(D+1)-o(1,1))^2+(x(i,j)-o(2,1))^2)<r^2;
if abs(x(i,j)-o(2,1)-sqrt(r^2-(j*Lsg/(D+1)-o(1,1))^2))<sqrt(r^2-(j*Lsg/(D+1)-o(1,1))^2)
v(i,j)=o(2,1)+sqrt(r^2-(j*Lsg/(D+1)-o(1,1))^2)-x(i,j);
x(i,j)=x(i,j)+v(i,j);
v(i,j)=0;
else
v(i,j)=o(2,1)-sqrt(r^2-(j*Lsg/(D+1)-o(1,1))^2)-x(i,j);
x(i,j)=x(i,j)+v(i,j);
v(i,j)=0;
end
end
if ((j*Lsg/(D+1)-o(1,2))^2+(x(i,j)-o(2,2))^2)<r^2;
if abs(x(i,j)-o(2,2)-sqrt(r^2-(j*Lsg/(D+1)-o(1,2))^2))<sqrt(r^2-(j*Lsg/(D+1)-o(1,2))^2)
v(i,j)=o(2,2)+sqrt(r^2-(j*Lsg/(D+1)-o(1,2))^2)-x(i,j);
x(i,j)=x(i,j)+v(i,j);
v(i,j)=0;
else
v(i,j)=o(2,2)-sqrt(r^2-(j*Lsg/(D+1)-o(1,2))^2)-x(i,j);
x(i,j)=x(i,j)+v(i,j);
v(i,j)=0;
end
end
end
if fitness(D,x(i,:))<p(i);
p(i)=fitness(D,x(i,:));
I(i,:)=x(i,:);
end
if p(i)<fitness(D,pg)
pg=I(i,:);
end
end
Pbest(t)=fitness(D,pg);
TT=[TT,t];
end
ybest = pg';
for m=1:D;
xbest(m)=Lsg/(D+1).*m;
end
T=[cos(alfa),-sin(alfa);sin(alfa),cos(alfa)];
a=T*[xbest;ybest'];
fv = fitness(D,pg); %1随机量选择较大2安全区3 100米内考虑避障，速度3kn，障碍物200~500米可测大小
