PSO.zip_PSO_PSO 路径_pso 规划_粒子路径规划_路径规划 pso

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米可测大小