【路径规划-二维路径规划】基于粒子群求解二维路径规划含路障附matlab代码 上传.zip

%------基本粒子群优化算法（Particle Swarm Optimization）----------- function result = PSO(dim) %--------声明全局--------------------------- global N; global m; global MaxDT; global Qmax; global Kmax; global w; global wmax; global wmin; global c1; global c2; global curpoint; global segR; global pathta; global pamoveta; global numta; global pathpoint; global hadsteps; global curstep; global pointIndex; %----------------------- %------初始化种群的个体(可以在这里限定位置和速度的范围)------------ result = true; %psosued = ones(1,N); %粒子群成功 Xmax = dim*pathta; %最大x值 Xmin = (dim-1)*pathta; %最小x值 Vmax = (Xmax - Xmin)*0.1; for i=1:N [temp,psosued(i)] = initX(Xmax,Xmin); %如果初始化不成功则不做任何的v变化 if psosued(i) x(i,:) = temp; for j=1:m v(i,j) = -Vmax+2*rand*Vmax; end end end %判断是否有条可走路径 for i=1:N if psosued(i) break; end end if psosued(i) == false result = false; return; end %------先计算各个粒子的适应度，并初始化Pi和Pg---------------------- %如果最大的 best_fitness = 10000; for i=1:N %如果粒子群初始化不成功的话将其适应值改为10000 if psosued(i) p(i)=fitness(x(i,:),m); y(i,:)=x(i,:); %y记录前一次迭代，粒子的位置 if p(i)<best_fitness pg = x(i,:); best_fitness = p(i); end end end %------进入主要循环，按照公式依次迭代，直到满足精度要求------------ for t=1:MaxDT for i=1:N %如果psosued为false的话，就让这个粒子不迭代 if psosued(i) %----cjss更新x for q = 1:Qmax %保护现场如果不通，直接返回 yuanx = x(i,:); yuanv = v(i,:); for j = 1:m for k=1:Kmax %保护现场，如果更新后不连通，返回原来的x，v值 tempv = v(i,j); tempx = x(i,j); %%%粒子群更新公式%%%%%%%%%%% w=wmax-(t-1)*(wmax-wmin)/(MaxDT-1); v(i,j)=w*v(i,j)+c1*rand*(y(i,j)-x(i,j))... +c2*rand*(pg(j)-x(i,j)); %看v是否越界 if v(i,j)>Vmax v(i,j) = Vmax; else if v(i,j)<-Vmax; v(i,j) = -Vmax; end end x(i,j)=x(i,j)+v(i,j); %看x是否越界 if x(i,j)>Xmax x(i,j) = Xmax; else if x(i,j)<Xmin x(i,j) = Xmin; end end %%%%%%%%%%%%%%%%%%%%%%%% %如果j是1的话就将curpoint赋值给purPoint %如果不是的话讲将前一个的tempPoint赋值给purPoint if 1 == j purPoint = curpoint; else purPoint = tempPoint; end %将极坐标转换为直角坐标 [tempPoint(1),tempPoint(2)]=plorTozhijiao(j*segR,x(i,j)); %判断这个点和前面的点是否相通 r = Conn(purPoint(1),purPoint(2),tempPoint(1),tempPoint(2)); if r break; else v(i,j) = tempv; x(i,j) = tempx; end end %如果k太大的话，说明此路不通，直接重新开始 if r == false break ; end end if r break; else %保护现场如果不通，直接返回 x(i,:)=yuanx; v(i,:)=yuanv; end end %---更新x % x(i,:)=x(i,:)+v(i,:); if fitness(x(i,:),m)<p(i) p(i)=fitness(x(i,:),m); y(i,:)=x(i,:); end if p(i)<fitness(pg,m) pg=y(i,:); end end end %记录每次迭代的最好fitness Pbest(t)=fitness(pg,m); end %----------更新链表 %将这一维的字赋值维2,然后初始化下一个节点 pamoveta(curstep,dim) = 2;%2表示已经走过了 curstep = curstep+1;%将走的点赋值给当前值，继续搜索 pamoveta(curstep,:)= 1; %将下一段的维值 tempta = mod(numta/2+dim,numta); if tempta == 0 tempta = 6; end pamoveta(curstep,tempta) = 3;%3表示来的路 %------------ %------更新点------------- for j=1:m [tempPoint(1),tempPoint(2)]=plorTozhijiao(j*segR,pg(j)); pointIndex = pointIndex+1; pathpoint(pointIndex,1) = tempPoint(1); %路径规划得出的点 pathpoint(pointIndex,2) = tempPoint(2); end pathpoint curpoint(1) = tempPoint(1); curpoint(2) = tempPoint(2); hadsteps(curstep,1) = curpoint(1); hadsteps(curstep,2) = curpoint(2); % %------最后给出计算结果 % % disp('*************************************************************') % disp('函数的全局最优位置为：') % Solution=pg' % disp('最后得到的优化极值为：') Result=fitness(pg,m) % disp('*************************************************************') % % %------算法结束---DreamSun GL & HF-----------------------------------