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

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

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

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

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

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

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

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

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

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

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

11、000000000001110000000000000000011100000000000011100111000000000000111001110000000000001110011101111000000011100000011110000000000000000111100000000000001101111000000000000011000000000000000000000011101111000000000000111011110001100000001110111100011001110000000000000000011101100000110000000000011001

12、001100000000000000010000000000000000000000000;MM=size(G,1);%G地形圖為01矩陣，如果為1表示障礙物Tau=ones(MM*MM,MM*MM);%Tau初始信息素矩陣(認為前面的覓食活動中有殘留的信息素)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信息素蒸發(fā)系數(shù)Q

13、=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);%終止點橫坐標ifEx=-0.5Ex=MM-0.5;endEy=a*(MM+0.5-ceil(E/MM);%終止點縱坐標Eta=zeros(N);%啟發(fā)式信息，取為至目標點的直線距離的倒數(shù)%下面構造啟發(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(i

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

15、(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%第三步：轉輪賭法選擇下一步怎么走PP=zeros(Len_LJD);fori=1:Len_LJDPP(i)=(Tau(W,LJD(i)AAlpha)*(Eta(LJD(i)FBeta);endsumpp=sum(PP);PP=PP/sumpp;%建立概率分布Pcum

16、(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);forj=1:l

17、ength(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(ROUT)-

18、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ā)一部分，新增加一部分end%繪圖plotif=1;%是否繪圖的控制參數(shù)ifplotif=1%繪收斂曲線minPL=zeros(K);fori=1:KPLK=PL(i,:);Nonzero=find(PLK);PLKPLK=PLK(Nonzero);minPL(i)

19、=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-1;y1=

20、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