蚁群算法路径规划MATLAB程序

  • 蚁群算法路径规划MATLAB程序已关闭评论
  • 242 views
  • A+
所属分类:AGV设计资料
摘要

蚁群算法路径规划MATLAB程序

  1. function [ROUTES,PL,Tau]=ACASP(G,Tau,K,M,S,E,Alpha,Beta,Rho,Q)   
  2. %% ---------------------------------------------------------------   
  3. %  ACASP.m   
  4. %  ACS for Path Planning   
  5. %% ---------------------------------------------------------------   
  6. %  input parameters   
  7. %  G        gragh, which is 01 matrix, 1 represents obstcles   
  8. %  Tau      initial pheromone matrix   
  9. %  K        iterative times   
  10. %  M        ant number for once iteration   
  11. %  S        Start Point   
  12. %  E        Goal point   
  13. %  Alpha    parameter indicates the importance of pheromone   
  14. %  Beta     parameter indicates the importance of visibility   
  15. %  Rho      pheromone evaporation rate   
  16. %  Q        pheromone accumulation parameter   
  17. %   
  18. %  output parameters   
  19. %  ROUTES   the route that every ant in every iteration   
  20. %  passes   
  21. %  PL       the length that every ant in every iteration   
  22. %  passes   
  23. %  Tau      output updated pheromone    
  24.    
  25. %% -------------------- variableinitialization-----------------------------   
  26. D=G2D(G);   
  27. N=size(D,1);%N is the peoblem scale(pixel number)   
  28. MM=size(G,1);   
  29. a=1;% edge length of pixel   
  30. Ex=a*(mod(E,MM)-0.5);% goal point coordinate   
  31. if Ex==-0.5   
  32.     Ex=MM-0.5;   
  33. end   
  34. Ey=a*(MM+0.5-ceil(E/MM));% Y coordinate of goal point   
  35. Eta=zeros(1,N);   
  36. %construct the initialization matrix   
  37. for i=1:N   
  38.     ix=a*(mod(i,MM)-0.5);   
  39.     if ix==-0.5   
  40.         ix=MM-0.5;   
  41.     end   
  42.     iy=a*(MM+0.5-ceil(i/MM));       
  43.     if i~=E   
  44.         Eta(1,i)=1/((ix-Ex)^2+(iy-Ey)^2)^0.5;   
  45.     else   
  46.         Eta(1,i)=100;   
  47.     end   
  48. end   
  49. ROUTES=cell(K,M);% use cell construct to load the passing route of every ant for every iteration       
  50. PL=zeros(K,M);% use matrix to load the passing length of every ant for every iteration   
  51. %% -----------iteration time is k, ant number is M--------------------   
  52. for k=1:K   
  53.     disp(k);   
  54.     for m=1:M   
  55. %%First step: status initialization   
  56.         W=S;%set current point as the starting point   
  57.         Path=S;%initialize the passing route   
  58.         PLkm=0;%initialize the passing length   
  59.         TABUkm=ones(1,N);%initialize the forbbiden table   
  60.         TABUkm(S)=0;% eliminate this point for it is the starting point   
  61.         DD=D;%initialize adjacency matrix   
  62. %%Second Step:Choose the nodes that can step forward   
  63.         DW=DD(W,:);   
  64.         DW1=find(DW<inf);   
  65.         for j=1:length(DW1)   
  66.             if TABUkm(DW1(j))==0   
  67.                 DW(j)=inf;   
  68.             end   
  69.         end   
  70.         LJD=find(DW<inf);% set of points can be selected   
  71.         Len_LJD=length(LJD);%number of points can be selected   
  72. %%Colony stop condition:there is no food or go into a impass   
  73.         while W~=E&&Len_LJD>=1   
  74. %%Third Step:the method to choose the next step   
  75.             PP=zeros(1,Len_LJD);   
  76.             for i=1:Len_LJD   
  77.                 PP(i)=(Tau(W,LJD(i))^Alpha)*(Eta(LJD(i))^Beta);   
  78.             end   
  79.             PP=PP/(sum(PP));%construct probability distributing   
  80.             Pcum=cumsum(PP);   
  81.             Select=find(Pcum>=rand);   
  82.             to_visit=LJD(Select(1));%the next step node   
  83. %%Fourth Step:status update and record   
  84.             Path=[Path,to_visit];%Paths accumulation   
  85.             PLkm=PLkm+DD(W,to_visit);%Path length accumulation   
  86.             W=to_visit;%move to the next point   
  87.             for kk=1:N   
  88.                 if TABUkm(kk)==0   
  89.                     DD(W,kk)=inf;   
  90.                     DD(kk,W)=inf;   
  91.                 end   
  92.             end   
  93.             TABUkm(W)=0;%eliminate the travelled point in the forbidden table   
  94.             DW=DD(W,:);   
  95.             DW1=find(DW<inf);   
  96.             for j=1:length(DW1)   
  97.                 if TABUkm(DW1(j))==0   
  98.                     DW(j)=inf;   
  99.                 end   
  100.             end   
  101.             LJD=find(DW<inf);%set of optional points   
  102.             Len_LJD=length(LJD);%number of optional points   
  103.         end   
  104. %%Fifth Step:record the pate and length that every ant for every   
  105. %%iteration passes   
  106.         ROUTES{k,m}=Path;   
  107.         if Path(end)==E   
  108.             PL(k,m)=PLkm;   
  109.         else   
  110.             PL(k,m)=inf;   
  111.         end   
  112.     end   
  113. %%Sixth Step: update pheromone   
  114.     Delta_Tau=zeros(N,N);%initialize updating ammount   
  115.     for m=1:M   
  116.         if PL(k,m)<inf   
  117.             ROUT=ROUTES{k,m};   
  118.             TS=length(ROUT)-1;   
  119.             PL_km=PL(k,m);   
  120.             for s=1:TS   
  121.                 x=ROUT(s);   
  122.                 y=ROUT(s+1);   
  123.                 Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km;   
  124.                 Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km;   
  125.             end   
  126.         end   
  127.     end   
  128.     Tau=(1-Rho).*Tau+Delta_Tau;%pheromone evaporates some and accumulates some   
  129. end   
  130. %% ---------------------------PLOT--------------------------------   
  131. plotif=1;%control parameter, determine if plot or not   
  132. if plotif==1   
  133.     %plot convergence curve   
  134.     meanPL=zeros(1,K);   
  135.     minPL=zeros(1,K);   
  136.     for i=1:K   
  137.         PLK=PL(i,:);   
  138.         Nonzero=find(PLK<inf);   
  139.         PLKPLK=PLK(Nonzero);   
  140.         meanPL(i)=mean(PLKPLK);   
  141.         minPL(i)=min(PLKPLK);   
  142.     end   
  143.     figure(1)   
  144.     plot(minPL);   
  145.     hold on   
  146.     plot(meanPL);   
  147.     grid on   
  148.     title('Convergence Curve(Average Length and Minimum Length)');   
  149.     xlabel('Iteration Times');   
  150.     ylabel('Path Length');    
  151.     %Plot Passing Graph   
  152.     figure(2)   
  153.     axis([0,MM,0,MM])   
  154.     for i=1:MM   
  155.         for j=1:MM   
  156.             if G(i,j)==1   
  157.                 x1=j-1;y1=MM-i;   
  158.                 x2=j;y2=MM-i;   
  159.                 x3=j;y3=MM-i+1;   
  160.                 x4=j-1;y4=MM-i+1;   
  161.                 fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);   
  162.                 hold on   
  163.             else   
  164.                 x1=j-1;y1=MM-i;   
  165.                 x2=j;y2=MM-i;   
  166.                 x3=j;y3=MM-i+1;   
  167.                 x4=j-1;y4=MM-i+1;   
  168.                 fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);   
  169.                 hold on   
  170.             end   
  171.         end   
  172.     end   
  173.     hold on   
  174.     ROUT=ROUTES{K,M};   
  175.     LENROUT=length(ROUT);   
  176.     Rx=ROUT;   
  177.     Ry=ROUT;   
  178.     for ii=1:LENROUT   
  179.         Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);   
  180.         if Rx(ii)==-0.5   
  181.             Rx(ii)=MM-0.5;   
  182.         end   
  183.         Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));   
  184.     end   
  185.     plot(Rx,Ry)   
  186. end   
  187. title('Shortest Path');   
  188. axis([0,MM,0,MM]);   
  189. plotif2=1;%Plot route for every iteration   
  190. if plotif2==1   
  191.     figure(3)   
  192.     axis([0,MM,0,MM])   
  193.     for i=1:MM   
  194.         for j=1:MM   
  195.             if G(i,j)==1   
  196.                 x1=j-1;y1=MM-i;   
  197.                 x2=j;y2=MM-i;   
  198.                 x3=j;y3=MM-i+1;   
  199.                 x4=j-1;y4=MM-i+1;   
  200.                 fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);   
  201.                 hold on   
  202.             else   
  203.                 x1=j-1;y1=MM-i;   
  204.                 x2=j;y2=MM-i;   
  205.                 x3=j;y3=MM-i+1;   
  206.                 x4=j-1;y4=MM-i+1;   
  207.                 fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);   
  208.                 hold on   
  209.             end   
  210.         end   
  211.     end   
  212.     for k=1:K   
  213.         PLK=PL(k,:);   
  214.         minPLK=min(PLK);   
  215.         pos=find(PLK==minPLK);   
  216.         m=pos(1);   
  217.         ROUT=ROUTES{k,m};   
  218.         LENROUT=length(ROUT);   
  219.         Rx=ROUT;   
  220.         Ry=ROUT;   
  221.         for ii=1:LENROUT   
  222.             Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);   
  223.             if Rx(ii)==-0.5   
  224.                 Rx(ii)=MM-0.5;   
  225.             end   
  226.             Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));   
  227.         end   
  228.         plot(Rx,Ry)   
  229.         hold on   
  230.     end   
  231. end   
  232. axis([0,MM,0,MM])   
  233. title('Routes that All the Ants Passed');   
  234. disp('Shortest Length is')   
  235. disp(minPLK);  
weinxin
微信公众号
agvba是一个分享AGV知识和agv案例视频的网站。