完整版基于蟻群算法的路徑規(guī)劃

1、MATLAB實現(xiàn)基于蟻群算法的機器人路徑規(guī)劃1、問題描述移動機器人路徑規(guī)劃是機器人學(xué)的一個重要研究領(lǐng)域.它要求機器人依據(jù)某個或某些優(yōu)化原那么(如最小能量消耗,最短行走路線,最短行走時間等),在其工作空間中找到一條從起始狀態(tài)到目標(biāo)狀態(tài)的能避開障礙物的最優(yōu)路徑.機器人路徑規(guī)劃問題可以建模為一個有約束的優(yōu)化問題,都要完成路徑規(guī)劃、定位和避障等任務(wù).2算法理論蟻群算法(AntColonyAlgorithm,ACA),最初是由意大利學(xué)者DorigoM.博士于1991年首次提出,其本質(zhì)是一個復(fù)雜的智能系統(tǒng),且具有較強的魯棒性,優(yōu)良的分布式計算機制等優(yōu)點.該算法經(jīng)過十多年的開展,已被廣闊的科學(xué)研究人員應(yīng)用于

2、各種問題的研究,如旅行商問題,二次規(guī)劃問題,生產(chǎn)調(diào)度問題等.但是算法本身性能的評價等算法理論研究方面進展較慢.Dorigo提出了精英蟻群模型(EAS),在這一模型中信息素更新根據(jù)得到當(dāng)前最優(yōu)解的螞蟻所構(gòu)造的解來進行,但這樣的策略往往使進化變得緩慢,并不能取得較好的效果.次年Dorigo博士給出改良模型(ACS),文中改良了轉(zhuǎn)移概率模型,并且應(yīng)用了全局搜索與局部搜索策略,來得進行深度搜索.Stutzle與Hoos給出了最大-最小螞蟻系統(tǒng)(MAX-MINAS),所謂最大-最小即是為信息素設(shè)定上限與下限,設(shè)定上限防止搜索陷入局部最優(yōu),設(shè)定下限鼓勵深度搜索.螞蟻作為一個生物個體其自身的水平是十分有限的

3、,比方螞蟻個體是沒有視覺的,螞蟻自身體積又是那么渺小,但是由這些水平有限的螞蟻組成的蟻群卻可以做出超越個體螞蟻水平的超常行為.螞蟻沒有視覺卻可以尋覓食物,螞蟻體積渺小而蟻群卻可以搬運比它們個體大十倍甚至百倍的昆蟲.這些都說明螞蟻群體內(nèi)部的某種機制使得它們具有了群體智能,可以做到螞蟻個體無法實現(xiàn)的事情.經(jīng)過生物學(xué)家的長時間觀察發(fā)現(xiàn),螞蟻是通過分泌于空間中的信息素進行信息交流,進而實現(xiàn)群體行為的.下面簡要介紹蟻群通過信息素的交流找到最短路徑的簡化實例.如圖2-1所示,AE之間有兩條路ABCDE與ABHDE,其中AB,DE,HD,HB的長度為1,BC,CD長度為0.5,并且,假設(shè)路上信息素濃度為0,

4、且各個螞蟻行進速度相同,單位時間所走的長度為1,每個單位時間內(nèi)在走過路徑上留下的信息素的量也相同.當(dāng)t=0時,從A點,E點同時各有30只螞蟻從該點出發(fā).當(dāng)t=1,從A點出發(fā)的螞蟻走到B點時,由于兩條路BH與BC上的信息素濃度相同,所以螞蟻以相同的概率選擇BH與BC,這樣就有15只螞蟻選擇走BH,有15只螞蟻選擇走BC.同樣的從E點出發(fā)的螞蟻走到D點,分別有15只螞蟻選擇DH和DC.當(dāng)t=2時,選才iBC與DC的螞蟻分別走過了BCD和DCB,而選擇BH與DH的螞蟻都走到了H點.所有的螞蟻都在所走過的路上留下了相同濃度的信息素,那么路彳至BCD上的信息素的濃度是路徑BHD上信息素濃度的兩倍,這樣假

5、設(shè)再次有螞蟻選擇走BC和BH時,或選擇走DC與DH時,都會以較大的概率選擇信息素濃度高的一邊.這樣的過程反復(fù)進行下去,最短的路徑上走過的螞蟻較多,留下的信息素也越多,蟻群這樣就可以找到一條較短的路.這就是它們?nèi)后w智能的表達.蟻群算法就是模擬螞蟻覓食過程中可以找到最短的路的行為過程設(shè)計的一種仿生算法.在用蟻群算法求解組合優(yōu)化問題時,首先要將組合優(yōu)化問題表達成與信息素相關(guān)的標(biāo)準(zhǔn)形式,然后各個螞蟻獨立地根據(jù)局部的信息素進行決策構(gòu)造解,并根據(jù)解的優(yōu)劣更新周圍的信息素,這樣的過程反復(fù)的進行即可求出組合優(yōu)化問題的優(yōu)化解.歸結(jié)蟻群算法有如下特點：(1)分布式計算：各個螞蟻獨立地構(gòu)造解,當(dāng)有螞蟻個體構(gòu)造的解較

6、差時,并不會影響整體的求解結(jié)果.這使得算法具有較強的適應(yīng)性；(2)自組織性：系統(tǒng)學(xué)中自組織性就是系統(tǒng)的組織指令是來自系統(tǒng)的內(nèi)部.同樣的蟻群算法中的各個螞蟻的決策是根據(jù)系統(tǒng)內(nèi)部信息素的分布進行的.這使得算法具有較強的魯棒性；(3)正反應(yīng)機制與負(fù)反應(yīng)機制結(jié)合：假設(shè)某局部空間上分布的信息素越多,那么在這個空間上走過的螞蟻也就越多；走過的螞蟻越多,在那個空間上留下的信息素也就越多,這就是存在的正反應(yīng)機制.但蟻群算法中解的構(gòu)造是通過計算轉(zhuǎn)移概率實現(xiàn)的,也就是說構(gòu)造解的時候可以接受退化解,這限制了正反應(yīng)機制,可以使得搜索范圍擴大,這是蟻群算法中隱含的負(fù)反應(yīng)機制.3求解步驟應(yīng)用蟻群算法求解機器人路徑優(yōu)化問題

7、的主要步驟如下：(1)輸入由0和1組成的矩陣表示機器人需要尋找最優(yōu)路徑的地圖的地圖,其中0表示此處可以通過的,1表示此處為障礙物.上圖的表示矩陣為:0000000000000000000001100000000000000000011000111000000000000000001110000000000000000011100000000000011100111000000000000111001110000000000001110011101111000000011100000011110000000000000000111100000000000001101111000000000000

8、0110000000000000000000000111011110;00000000000111011110;00110000000111011110;00110011100000000000;00000011101100000110;00000000001100100110;00000000000000100000;00000000000000000000;(2)輸入初始的信息素矩陣,選擇初始點和終止點并且設(shè)置各種參數(shù).在此次計算中,我們設(shè)置所有位置的初始信息素相等.選擇從初始點下一步可以到達的節(jié)點,根據(jù)每個節(jié)點的信息素求出前往每個節(jié)點的概率,并利用輪盤算法選取下一步的初始點.kPij二小

9、Jj.tPj:k-.N-tabuk:'0ifjIN-tabukJotherwise其中q(t為析取圖中弧(i,j)上的信息素的濃度.*為與弧(i,j)相關(guān)聯(lián)的啟發(fā)式信息.a,P分別為(t),的權(quán)重參數(shù).(4)更新路徑,以及路程長度.(5)重復(fù)(3)(4)過程,直到螞蟻到達終點或者無路可走.(6)重復(fù)(3)(4)(5),直到某一代m只螞蟻迭代結(jié)束.(7)更新信息素矩陣,其中沒有到達的螞蟻不計算在內(nèi).ijt1=1-7ijt,-Q-,如果螞蟻k經(jīng)過i,j-'-ijt=Lkt00,如果螞蟻k不經(jīng)過i,j其中P為信息素?fù)]發(fā)系數(shù).Q為信息量增增強度.Lk(t)為路徑長度.(8)重復(fù)(3)-

10、(7),直至n代螞蟻迭代結(jié)束.4運行結(jié)果(圖、表等)將上述矩陣輸入到程序中,畫出最短路徑的路線,并且輸入每一輪迭代的最短路徑,查看程序的收斂效果,在程序中設(shè)置plotif=1那么輸出收斂和最短路徑圖,在程序中設(shè)置plotif2=1那么輸出每一代螞蟻的路徑圖.最終輸出的結(jié)果如圖:IKIaL'iIflI55050505044332211度長徑路1012161820度長徑路小最/|線曲斂收102030405060708090100迭代次數(shù)5MATLAB程序functionm_main()G=00000000000000000000;011000000000000000000110001110

11、000000000000000011100000000000000000111000000000000111001110000000000001110011100000000000011100111011110000000111000000111100000000000000001111000000000000011011110000000000000110000000000000000000000111011110000000000001110111100011000000011101111000110011100000000000000000111011000001100000000000

12、11001001100000000000000010000000000000000000000000;MM=size(G,1);%G地形圖為01矩陣,如果為1表示障礙物Tau=ones(MM*MM,MM*MM);%Tau初始信息素矩陣(認(rèn)為前面的覓食活動中有殘留的信息素)Tau=8.*Tau;K=100;%K迭代次數(shù)(指螞蟻出動多少波)M=50;%M螞蟻個數(shù)(每一波螞蟻有多少個)S=1;%S起始點(最短路徑的起始點)E=MM*MM;%E終止點(最短路徑的目的點)Alpha=1;%Alpha表征信息素重要程度的參數(shù)Beta=7;%Beta表征啟發(fā)式因子重要程度的參數(shù)Rho=0.3;%Rho信息素

13、蒸發(fā)系數(shù)Q=1;%Q信息素增增強度系數(shù)minkl=inf;mink=0;minl=0;D=G2D(G);N=size(D,1);%N表示問題的規(guī)模(象素個數(shù))a=1;%小方格象素的邊長Ex=a*(mod(E,MM)-0.5);%終止點橫坐標(biāo)ifEx=-0.5Ex=MM-0.5;endEy=a*(MM+0.5-ceil(E/MM);%終止點縱坐標(biāo)Eta=zeros(N);%啟發(fā)式信息,取為至目標(biāo)點的直線距離的倒數(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-=E

14、Eta(i)=1/(ix-Ex)A2+(iy-Ey)A2)A0.5;elseEta(i)=100;endendROUTES=cell(K,M);%用細(xì)胞結(jié)構(gòu)存儲每一代的每一只螞蟻的爬行路線PL=zeros(K,M);%用矩陣存儲每一代的每一只螞蟻的爬行路線長度%啟動K輪螞蟻覓食活動,每輪派出M只螞蟻fork=1:Kform=1:M%第一步：狀態(tài)初始化W=S;%當(dāng)前節(jié)點初始化為起始點Path=S;%爬行路線初始化PLkm=0;%爬行路線長度初始化TABUkm=ones(N);%禁忌表初始化TABUkm(S)=0;%已經(jīng)在初始點了,因此要排除DD=D;%鄰接矩陣初始化%第二步：下一步可以前往的節(jié)點

15、DW=DD(W,:);DW1=find(DW);forj=1:length(DW1)ifTABUkm(DW1(j)=0DW(DW1(j)=0;endendLJD=find(DW);Len_LJD=length(LJD);%可選節(jié)點的個數(shù)%覓食停止條件：螞蟻未遇到食物或者陷入死胡同whileW=E&&Len_LJD>=1%第三步：轉(zhuǎn)輪賭法選擇下一步怎么走PP=zeros(Len_LJD);fori=1:Len_LJDPP(i)=(Tau(W,LJD(i)AAlpha)*(Eta(LJD(i)FBeta);endsumpp=sum(PP);PP=PP/sumpp;%建立概率分

16、布Pcum(1)=PP(1);fori=2:Len_LJDPcum(i)=Pcum(i-1)+PP(i);endSelect=find(Pcum>=rand);to_visit=LJD(Select(1);%第四步：狀態(tài)更新和記錄Path=Path,to_visit;%路徑增加PLkm=PLkm+DD(W,to_visit);%路徑長度增加W=to_visit;%螞蟻移到下一個節(jié)點forkk=1:NifTABUkm(kk)=0DD(W,kk)=0;DD(kk,W)=0;endendTABUkm(W)=0;%已訪問過的節(jié)點從禁忌表中刪除DW=DD(W,:);DW1=find(DW);for

17、j=1:length(DW1)ifTABUkm(DW1(j)=0DW(j)=0;endendLJD=find(DW);Len_LJD=length(LJD);%可選節(jié)點的個數(shù)end%第五步：記下每一代每一只螞蟻的覓食路線和路線長度ROUTESk,m=Path;ifPath(end)=EPL(k,m)=PLkm;ifPLkm<minklmink=k;minl=m;minkl=PLkm;endelsePL(k,m)=0;endend%第六步：更新信息素Delta_Tau=zeros(N,N);%更新量初始化form=1:MifPL(k,m)ROUT=ROUTESk,m;TS=length(R

18、OUT)-1;%跳數(shù)PL_km=PL(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=1;%是否繪圖的限制參數(shù)ifplotif=1%繪收斂曲線minPL=zeros(K);fori=1:KPLK=PL(i,:);Nonzero=find(PLK);PLKPLK=PLK(Nonzero);min

19、PL(i)=min(PLKPLK);endfigure(1)plot(minPL);holdongridontitle('收斂曲線(最小路徑長度),);xlabel('迭代次數(shù)');ylabel('路徑長度');%繪爬行圖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(x1,x2,x3,x4,y1,y2,y3,y4,0.2,0.2,0.2);holdonelsex1=j-

20、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=ROUTESmink,minl;LENROUT=length(ROUT);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)/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