基于蟻群算法的機(jī)器人路徑規(guī)劃MATLAB源碼

1、基于蟻群算法的機(jī)器人路徑規(guī)劃MATLAB源碼使用網(wǎng)格離散化的方法對(duì)帶有障礙物的環(huán)境建模，使用鄰接矩陣存儲(chǔ)該環(huán)境，使得問(wèn)題轉(zhuǎn)化為蟻群算法尋找最短路徑。functionROUTES,PL,Tau=ACASPS(G,Tau,K,M,S,E,Alpha,Beta,Rho,Q)%ACASP.m%蟻群算法動(dòng)態(tài)尋路算法%GreenSim團(tuán)隊(duì)原創(chuàng)作品，轉(zhuǎn)載請(qǐng)注明%GreenSim團(tuán)隊(duì)長(zhǎng)期從事算法設(shè)計(jì)、代寫程序等業(yè)務(wù)%歡迎訪問(wèn)GreenSim算法仿真團(tuán)隊(duì)-%輸入?yún)?shù)列表%G地形圖為01矩陣，如果為1表示障礙物%Tau初始信息素矩陣(認(rèn)為前面的覓食活動(dòng)中有殘留的信息素)%K迭代次數(shù)(指螞蟻出動(dòng)多少波)%M螞蟻個(gè)數(shù)

2、(每一波螞蟻有多少個(gè))%S起始點(diǎn)(最短路徑的起始點(diǎn))%E終止點(diǎn)(最短路徑的目的點(diǎn))%Alpha表征信息素重要程度的參數(shù)%Beta表征啟發(fā)式因子重要程度的參數(shù)%Rho信息素蒸發(fā)系數(shù)%Q信息素增加強(qiáng)度系數(shù)%輸出參數(shù)列表%ROUTES每一代的每一只螞蟻的爬行路線%PL每一代的每一只螞蟻的爬行路線長(zhǎng)度%Tau輸出動(dòng)態(tài)修正過(guò)的信息素%變量初始化%loadD=G2D(G);N=size(D,1);%N表示問(wèn)題的規(guī)模(象素個(gè)數(shù))MM=size(G,1);a=1;%小方格象素的邊長(zhǎng)Ex=a*(mod(E,MM)-0.5);%終止點(diǎn)橫坐標(biāo)ifEx=-0.5Ex=MM-0.5;endEy=a*(MM+0.5-ce

3、il(E/MM);%終止點(diǎn)縱坐標(biāo)Eta=zeros(1,N);%啟發(fā)式信息，取為至目標(biāo)點(diǎn)的直線距離的倒數(shù)%下面構(gòu)造啟發(fā)式信息矩陣fori=1:Nix=a*(mod(i,MM)-0.5);ifix=-0.5ix=MM-0.5;endiy=a*(MM+0.5-ceil(i/MM);ifi=EEta(1,i)=1/(ix-Ex)A2+(iy-Ey)A2)A0.5;elseEta(1,i)=100;endendROUTES=cell(K,M);%用細(xì)胞結(jié)構(gòu)存儲(chǔ)每一代的每一只螞蟻的爬行路線PL=zeros(K,M);%用矩陣存儲(chǔ)每一代的每一只螞蟻的爬行路線長(zhǎng)度%啟動(dòng)K輪螞蟻覓食活動(dòng)，每輪派出M只螞蟻fo

4、rk=1:K%disp(k);form=1:M%第一步：狀態(tài)初始化W=S;%當(dāng)前節(jié)點(diǎn)初始化為起始點(diǎn)Path=S;%爬行路線初始化PLkm=0;%爬行路線長(zhǎng)度初始化TABUkm(S)=0;%已經(jīng)在初始點(diǎn)了，因此要排除DD=D;%鄰接矩陣初始化%第二步：下一步可以前往的節(jié)點(diǎn)DW=DD(W,:);DW1=find(DW<inf);forj=1:length(DW1)ifTABUkm(DW1(j)=0endendLJD=find(DW<inf);%可選節(jié)點(diǎn)集Len_LJD=length(LJD);%可選節(jié)點(diǎn)的個(gè)數(shù)%覓食停止條件：螞蟻未遇到食物或者陷入死胡同whileW=E&&

5、;Len_LJD>=1%第三步：轉(zhuǎn)輪賭法選擇下一步怎么走PP=zeros(1,Len_LJD);fori=1:Len_LJDendPP=PP/(sum(PP);%建立概率分布Pcum=cumsum(PP);Select=find(Pcum>=rand);to_visit=LJD(Select(1);%下一步將要前往的節(jié)點(diǎn)%第四步：狀態(tài)更新和記錄Path=Path,to_visit;%路徑增加PLkm=PLkm+DD(W,to_visit);%路徑長(zhǎng)度增加W=to_visit;%螞蟻移到下一個(gè)節(jié)點(diǎn)forkk=1:NifTABUkm(kk)=0DD(W,kk)=inf;DD(kk,W)

6、=inf;endendTABUkm(W)=0;%已訪問(wèn)過(guò)的節(jié)點(diǎn)從禁忌表中刪除DW=DD(W,:);LJD=find(DW<inf);%可選節(jié)點(diǎn)集Len_LJD=length(LJD);%可選節(jié)點(diǎn)的個(gè)數(shù)end%第五步：記下每一代每一只螞蟻的覓食路線和路線長(zhǎng)度ROUTESk,m=Path;ifPath(end)=EPL(k,m)=PLkm;elsePL(k,m)=inf;endend%第六步：更新信息素Delta_Tau=zeros(N,N);%更新量初始化form=1:MifPL(k,m)<infROUT=ROUTESk,m;TS=length(ROUT)-1;%跳數(shù)PL_km=PL

7、(k,m);fors=1:TSx=ROUT(s);y=ROUT(s+1);Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km;Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km;endendendTau=(1-Rho).*Tau+Delta_Tau;%信息素?fù)]發(fā)一部分，新增加一部分end%繪圖plotif=0;%是否繪圖的控制參數(shù)ifplotif=1%繪收斂曲線meanPL=zeros(1,K);minPL=zeros(1,K);fori=1:KPLK=PL(i,:);Nonzero=find(PLK<inf);PLKPLK=PLK(Non

8、zero);meanPL(i)=mean(PLKPLK);minPL(i)=min(PLKPLK);endfigure(1)plot(minPL);holdonplot(meanPL);gridontitle('收斂曲線(平均路徑長(zhǎng)度和最小路徑長(zhǎng)度)');xlabel('迭代次數(shù));ylabel('路徑長(zhǎng)度');%繪爬行圖figure(2)axis(0,MM,0,MM)fori=1:MMforj=1:MMifG(i,j)=1x1=j-1;y1=MM-i;x2=j;y2=MM-i;x3=j;y3=MM-i+1;x4=j-1;y4=MM-i+1;fill(x

9、1,x2,x3,x4,y1,y2,y3,y4,0.2,0.2,0.2);holdonelsex1=j-1;y1=MM-i;x2=j;y2=MM-i;x3=j;y3=MM-i+1;x4=j-1;y4=MM-i+1;fill(x1,x2,x3,x4,y1,y2,y3,y4,1,1,1);holdonendendendholdonROUT=ROUTESK,M;Rx=ROUT;Ry=ROUT;forii=1:LENROUTRx(ii)=a*(mod(ROUT(ii),MM)-0.5);ifRx(ii)=-0.5Rx(ii)=MM-0.5;endRy(ii)=a*(MM+0.5-ceil(ROUT(ii

10、)/MM);endplot(Rx,Ry)endplotif2=0;%繪各代螞蟻爬行圖ifplotif2=1figure(3)axis(0,MM,0,MM)fori=1:MMforj=1:MMifG(i,j)=1x1=j-1;y1=MM-i;x2=j;y2=MM-i;x4=j-1;y4=MM-i+1;fill(x1,x2,x3,x4,y1,y2,y3,y4,0.2,0.2,0.2);holdonelsex1=j-1;y1=MM-i;x2=j;y2=MM-i;x3=j;y3=MM-i+1;x4=j-1;y4=MM-i+1;fill(x1,x2,x3,x4,y1,y2,y3,y4,1,1,1);holdonendendendfork=1:KPLK=PL(k,:);minPLK=min(PLK);pos=find(PLK=minPLK);m=pos(1);ROUT=ROUTESk,m;LENROUT=leng