QPSO移動(dòng)機(jī)器人路徑規(guī)劃

1、Ax63oEiOA-for i=1:popsizeAx63oEiOA-for i=1:popsizefor j=1:vartotal angle(i,j)=2*pi*rand;chrom(i,1,j)=cos(angle(i,j);chrom(i,2,j)=sin(angle(i,j); selfangle(i,j)=2*pi*rand;selfchrom(i,1,j)=cos(angle(i,j);selfchrom(i,2,j)=sin(angle(i,j); dangle(i,j)=0;endend%pUiibAx6(Aex6EEia)pAuQ6ald%pUii6Ax6(Aex6EEia

2、)pAuQdyld%y0i,oAx6x0Eixi6AfOApAU坯c6aYd%y0i,oAx6x0Eixi6AfOApAu坯fOyTd%36E_xaQ0bAi(EUiE)aOaa-%3iBddyIA clear all;clear all;%aAe2eOpopsize=50;%OOE1Avartotal=2;%aAioEyinertia=0.5;%iB00ox6wselfw=2.0;%x0總d6x6c1globalw=2.0;%EOOdx6c2mutatep=0.05;%aOiiAAEmaxgen=500;%IPzuEy%Eyxe2iO varrange(1,1)=-512; %xx!BjOp

3、 varrange(1,2)=512; %xxizoOp varrange(2,1)=-512; %xx!BjOp varrange(2,2)=512; %xxizoOp for i=1:popsizefor j=1:2for k=1:vartotalchromx(i,j,k)=0.5*(varrange(k,2)*(1+chrom(i,j,k)+varrange(k,1)*(1- chrom(i,j,k);selfchromx(i,j,k)=0.5*(varrange(k,2)*(1+selfchrom(i,j,k)+varrange(k,1) *(1-selfchrom(i,j,k);en

4、dendend%EaEE6|iE一一for i=1:popsizefor j=1:2fitness(i,j)=-chromx(i,j,1)*sin(abs(chromx(i,j,2)+1-chromx(i,j,1)A0.5)*cos(abs(chromx(i,j,2)+1+chromx(i,j,1)A0.5) -(chromx(i,j,2)+1)*cos(abs(chromx(i,j,2)+1- chromx(i,j,1)A0.5)*sin(abs(chromx(i,j,2)+1+chromx(i,j,1)A0.5);selffitness(i,j)=- selfchromx(i,j,1)*s

5、in(abs(selfchromx(i,j,2)+1- selfchromx(i,j,1)A0.5)*cos(abs(selfchromx(i,j,2)+1+selfchromx(i,j,1) )7.5)(selfchromx(i,j,2)+1)*cos(abs(selfchromx(i,j,2)+1- selfchromx(i,j,1)A0.5)*sin(abs(selfchromx(i,j,2)+1+selfchromx(i,j,1) )7.5);endend%總總0；?總(3吻一minfit=fitness(1,1);if fitness(1,1)fitness(1,2)minfit=f

6、itness(1,2);endmaxfit=fitness(1,2);bestchain=2;if fitness(1,1)fitness(1,2)maxfit=fitness(1,1);bestchain=1;endminfitindex=1;maxfitindex=1; avgfit=(fitness(1,1)+fitness(1,2)/(2*popsize);for i=2:popsizefor j=1:2if minfitfitness(i,j) minfit=fitness(i,j);minfitindex=i;endif maxfitfitness(i,j) maxfit=fitn

7、ess(i,j);maxfitindex=i;bestchain=j;end avgfit=avgfit+fitness(i,j)/(2*popsize);endend%一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一36E_E0 xiQAagloangle(1,:)=angle(maxfitindex,:);%npAE0 xi0AIaiglochrom(1,:,:)=chrom(maxfitindex,:,:);%36E E%0 xi0Af0Aglochromx(1,:,:)=chromx(maxfitindex,:,:);%3

8、6E E0 xi0Ai0Aglofit=fitness(maxfitindex,bestchain);%npAE0 xiz6EE0；?E%一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一一3lBd0N-eaEfor gen=1:maxgengenglofitfor i=1:popsizefor j=1:vartotalt1=selfangle(i,j)-angle(i,j);if t1pit1=t1-2*pi;endt2=gloangle(1,j)-angle(i,j);if t2pit2=t2-2*pi;enddangle(i,j

9、)=inertia*dangle(i,j)+selfw*rand*t1+globalw*rand*t2;angle(i,j)=angle(i,j)+dangle(i,j); %0i6pAIaichrom(i,1,j)=cos(angle(i,j);%0i_6pAi0Achrom(i,2,j)=sin(angle(i,j);%di_6pAi0A%aOaachromx(i,1,j)=0.5*(varrange(j,2)*(1+chrom(i,1,j)+varrange(j,1)*(1- chrom(i,1,j);chromx(i,2,j)=0.5*(varrange(j,2)*(1+chrom(i

10、,2,j)+varrange(j,1)*(1- chrom(i,2,j);endfor j=1:1:2 fitness(i,j)=-chromx(i,j,1)*sin(abs(chromx(i,j,2)+1- chromx(i,j,1)A0.5)*cos(abs(chromx(i,j,2)+1+chromx(i,j,1)A0.5) -(chromx(i,j,2)+1)*cos(abs(chromx(i,j,2)+1- chromx(i,j,1)A0.5)*sin(abs(chromx(i,j,2)+1+chromx(i,j,1)A0.5);endend%Ax0I0Aa0ifor i=1:pop

11、sizefor j=1:vartotalif randfitness(1,2)minfit=fitness(1,2);endmaxfit=fitness(1,2);bestchain=2;if fitness(1,1)fitness(1,2)maxfit=fitness(1,1); bestchain=1;endminfitindex=1;maxfitindex=1; avgfit=(fitness(1,1)+fitness(1,2)/(2*popsize); for i=2:popsizefor j=1:2if minfitfitness(i,j) minfit=fitness(i,j);m

12、infitindex=i;endif maxfitfitness(i,j) maxfit=fitness(i,j);maxfitindex=i;bestchain=j;end avgfit=avgfit+fitness(i,j)/(2*popsize);endend%v Zs/z O zs 3AxOEixiOA1OAfor i=1:popsizet1=fitness(i,1);if t1fitness(i,2) t1=fitness(i,2);endt2=selffitness(i,1);if t2t2selfangle(i,:)=angle(i,:); selfchrom(i,:,:)=chrom(i,:,:); selfchromx(i,:,:)