(完整word版)基于蟻群算法的路徑規(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 算法理論蟻群算法（ Ant Colony Algorithm ，ACA ），最初是由意大利學(xué)者 Dorigo M. 博士于 1991 年首次提出， 其本質(zhì)是一個復(fù)雜的智能系統(tǒng)， 且具有較強的魯棒性， 優(yōu)良的分布式計算機制 等優(yōu)點。 該算法經(jīng)

2、過十多年的發(fā)展， 已被廣大的科學(xué)研究人員應(yīng)用于各種問題的研究， 如旅 行商問題， 二次規(guī)劃問題， 生產(chǎn)調(diào)度問題等。 但是算法本身性能的評價等算法理論研究方面 進展較慢。Dorigo 提出了精英蟻群模型（ EAS ），在這一模型中信息素更新按照得到當(dāng)前最優(yōu)解的 螞蟻所構(gòu)造的解來進行， 但這樣的策略往往使進化變得緩慢， 并不能取得較好的效果。 次年 Dorigo 博士給出改進模型（ ACS ），文中 改進了轉(zhuǎn)移概率模型，并且應(yīng)用了全局搜索與局 部搜索策略，來得進行深度搜索。St utzle與Hoos給出了最大-最小螞蟻系統(tǒng) （MAX-MINAS ），所謂最大 -最小即是為信息素設(shè)定上限與下限，設(shè)定

3、上限避免搜索陷入局 部最優(yōu)， 設(shè)定下限鼓勵深度搜索。 螞蟻作為一個生物個體其自身的能力是十分有限的， 比如 螞蟻個體是沒有視覺的， 螞蟻自身體積又是那么渺小， 但是由這些能力有限的螞蟻組成的蟻 群卻可以做出超越個體螞蟻能力的超常行為。 螞蟻沒有視覺卻可以尋覓食物， 螞蟻體積渺小 而蟻群卻可以搬運比它們個體大十倍甚至百倍的昆蟲。 這些都說明螞蟻群體內(nèi)部的某種機制 使得它們具有了群體智能， 可以做到螞蟻個體無法實現(xiàn)的事情。 經(jīng)過生物學(xué)家的長時間觀察 發(fā)現(xiàn)，螞蟻是通過分泌于空間中的信息素進行信息交流，進而實現(xiàn)群體行為的。下面簡要介紹蟻群通過信息素的交流找到最短路徑的簡化實例。如圖 2-1 所示， A

4、E 之間有兩條路 ABCDE 與 ABHDE ，其中 AB ， DE， HD， HB 的長度為 1， BC， CD 長度為 0.5，并且， 假設(shè)路上信息素濃度為 0，且各個螞蟻行進速度相同， 單位時間所走的長度為 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時，選擇BC與D

5、C的螞蟻分別走過了 BCD和DCB，而選擇BH與 DH 的螞蟻都走到了 H 點。所有的螞蟻都在所走過的路上留下了相同濃度的信息素，那么 路徑 BCD 上的信息素的濃度是路徑 BHD 上信息素濃度的兩倍，這樣若再次有螞蟻選擇走 BC 和 BH 時，或選擇走 DC 與 DH 時，都會以較大的概率選擇信息素濃度高的一邊。這 樣的過程反復(fù)進行下去， 最短的路徑上走過的螞蟻較多， 留下的信息素也越多， 蟻群這樣就 可以找到一條較短的路。這就是它們?nèi)后w智能的體現(xiàn)。蟻群算法就是模擬螞蟻覓食過程中可以找到最短的路的行為過程設(shè)計的一種仿生算法。 在用蟻群算法求解組合優(yōu)化問題時，首先要將組合優(yōu)化問題表達成與信息素

6、相關(guān)的規(guī)范形 式，然后各個螞蟻獨立地根據(jù)局部的信息素進行決策構(gòu)造解，并根據(jù)解的優(yōu)劣更新周圍的信息素，這樣的過程反復(fù)的進行即可求出組合優(yōu)化問題的優(yōu)化解。歸結(jié)蟻群算法有如下特點：（1）分布式計算：各個螞蟻獨立地構(gòu)造解，當(dāng)有螞蟻個體構(gòu)造的解較差時，并不會影 響整體的求解結(jié)果。這使得算法具有較強的適應(yīng)性；（2）自組織性：系統(tǒng)學(xué)中自組織性就是系統(tǒng)的組織指令是來自系統(tǒng)的內(nèi)部。同樣的蟻這使得算法具有較強的魯群算法中的各個螞蟻的決策是根據(jù)系統(tǒng)內(nèi)部信息素的分布進行的。 棒性；這就也就是說構(gòu)造解這是蟻群算法中隱（3）正反饋機制與負反饋機制結(jié)合：若某部分空間上分布的信息素越多，那么在這個 空間上走過的螞蟻也就越多；

7、走過的螞蟻越多，在那個空間上留下的信息素也就越多，是存在的正反饋機制。 但蟻群算法中解的構(gòu)造是通過計算轉(zhuǎn)移概率實現(xiàn)的， 的時候可以接受退化解， 這限制了正反饋機制， 可以使得搜索范圍擴大， 含的負反饋機制。3求解步驟 應(yīng)用蟻群算法求解機器人路徑優(yōu)化問題的主要步驟如下：（1）輸入由0和1組成的矩陣表示機器人需要尋找最優(yōu)路徑的地圖的地圖，其中 示此處可以通過的，1表示此處為障礙物。100 000000000;100 000000000;100 000000000;100 000000000;100 000000000;1 0 10 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0

8、;0 0 0 0 0 0;上圖的表示矩陣為:0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 0 0 0 10 0 0 0 0 0 1 0 0 0 0 0 0 10 10 10 10 10 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;0 0 1 1 0 0 0 0 0 0 0 1

9、1 1 0 1 1 1 1 0;0 0 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 1 1 1 0 1 1 0 0 0 0 0 1 1 0;0 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 1 1 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;(2) 輸入初始的信息素矩陣，選擇初始點和終止點并且設(shè)置各種參數(shù)。在此次計算中， 我們設(shè)置所有位置的初始信息素相等。(3) 選擇從初始點下一步可以到達的節(jié)點，根據(jù)每個節(jié)

10、點的信息素求出前往每個節(jié)點的 概率，并利用輪盤算法選取下一步的初始點。kpij =無L(tij卩 kN -tabUk /0ifj n -tabUkotherwise（4）（6）（7）其中Uj為與弧（i, j湘關(guān)聯(lián)的啟發(fā)式信其中Tj （t為析取圖中?。╥, j ）上的信息素的濃度。息。a，P分別為Tij （t ）, n j的權(quán)重參數(shù)。更新路徑，以及路程長度。重復(fù)（3）（ 4）過程，直到螞蟻到達終點或者無路可走。 重復(fù)（3）（ 4）（ 5），直到某一代 m只螞蟻迭代結(jié)束。 更新信息素矩陣，其中沒有到達的螞蟻不計算在內(nèi)。Tij(t +1 )=(1 - P 卜 Tj(t )+人g（t金），如果螞蟻k經(jīng)

11、過i，j0,如果螞蟻k不經(jīng)過i，jP為信息素揮發(fā)系數(shù)。Q為信息量增加強度。Lk（t ）為路徑長度。重復(fù)（3） - （7）,直至n代螞蟻迭代結(jié)束。（8） 4運行結(jié)果（圖、表等）將上述矩陣輸入到程序中，畫出最短路徑的路線，并且輸入每一輪迭代的最短路徑，查看程序的收斂效果，在程序中設(shè)置plotif=1則輸出收斂和最短路徑圖，在程序中設(shè)置plotif2=1則輸出每一代螞蟻的路徑圖。最終輸出的結(jié)果如圖204518161412108642068101214161820度長徑路:iI11II il 111 1II I收斂曲線（最小路徑長度）4035302520151050010203040506070809

12、0100迭代次數(shù)5 MATLAB 程序fun ctio n m_mai n()G=0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 0 0 0 1Tau 初始信息素矩陣（認為前面的覓食活動中有殘留的信K=100;M=50;S=1 ;指螞蟻出動多少波） 每一波螞蟻有多少個）E=MM*MM; Alpha=1;Beta=7;Rho=0.3 ; Q=1;minkl=inf; mink=0; minl=0;D=G2D(G);N=size(D,1);%N% K 迭代次數(shù)% M 螞蟻個

13、數(shù)% S 起始點（最短路徑的起始點）% E 終止點（最短路徑的目的點） % Alpha 表征信息素重要程度的參數(shù) % Beta 表征啟發(fā)式因子重要程度的參數(shù)% Rho 信息素蒸發(fā)系數(shù)% Q 信息素增加強度系數(shù)表示問題的規(guī)模（象素個數(shù)）0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0;1 0 0 0 0 0 0;1 0 0 0 0 0 0;1 0 0 0 0 0 0;1 00 0 00 0;0 0 0 000 01 1

14、00 000 00 0 00 0;0 0 0 000 00 0 00 111 01 1 11 0;0 0 0 000 00 0 00 111 01 1 11 0;0 0 1 100 00 0 00 111 01 1 11 0;0 0 1 100 11 1 00 000 00 0 00 0;0 0 0 000 11 1 01 100 00 0 11 0;0 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 1 1 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;MM=

15、size(G,1); % G 地形圖為 01 矩陣，如果為 1 表示障礙物 Tau=ones(MM*MM,MM*MM);% 息素)Tau=8.*Tau;a=1;%小方格象素的邊長 Ex=a*(mod(E,MM)-0.5);% 終止點橫坐標(biāo) if Ex=-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ā)式信息矩陣for i=1:Nix=a*(mod(i,MM)-0.5);if ix=-0.5ix=MM-0.5;endiy=a*(MM+0.5-ceiI(i/MM);i

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

17、接矩陣初始化% 第二步：下一步可以前往的節(jié)點DW=DD(W,:);DW1=find(DW);for j=1:length(DW1)if TABUkm(DW1(j)=0DW(DW1(j)=0;endendLJD=find(DW);Len_LJD=length(LJD);% 可選節(jié)點的個數(shù)% 覓食停止條件：螞蟻未遇到食物或者陷入死胡同while W=E&&Len_LJD>=1% 第三步：轉(zhuǎn)輪賭法選擇下一步怎么走PP=zeros(Len_LJD);for i=1:Len_LJDPP (i)=(Tau(W,LJD(i)AAI pha)*(Eta(LJD(i)ABeta);ends

18、umpp=sum(PP);PP=PP/su mpp;%建立概率分布Pcum(1)=PP(1);for i=2:Len_LJD Pcum(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é)點for kk=1:N if TABUkm(kk)=0 DD(W,kk)=0; DD(kk,W)=0; end endTABU

19、km(W)=0;% 已訪問過的節(jié)點從禁忌表中刪除 DW=DD(W,:);DW1=find(DW);for j=1:length(DW1)if TABUkm(DW1(j)=0 DW(j)=0;endend LJD=find(DW); Len_LJD=length(LJD);% 可選節(jié)點的個數(shù) end% 第五步：記下每一代每一只螞蟻的覓食路線和路線長度 ROUTESk,m=Path;if Path(end)=E PL(k,m)=PLkm; if PLkm<minklmink=k;minl=m;minkl=PLkm;endelsePL(k,m)=0; endend% 第六步：更新信息素Delt

20、a_Tau=zeros(N,N);% 更新量初始化 for m=1:Mif PL(k,m)ROUT=ROUTESk,m;TS=length(ROUT)-1;% 跳數(shù) PL_km=PL(k,m);for s=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ù)if plotif=1 %

21、 繪收斂曲線 minPL=zeros(K);for i=1:K PLK=PL(i,:); Nonzero=find(PLK); PLKPLK=PLK(Nonzero); minPL(i)=min(PLKPLK);endfigure(1) plot(minPL); hold on grid on title(' 收斂曲線(最小路徑長度) '); xlabel(' 迭代次數(shù) ');ylabelC路徑長度');%繪爬行圖figure(2) axis(0,MM,0,MM) for i=1:MM for j=1:MM if G(i,j)=1 x1=j-1;y1=M

22、M-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);hold onelsex1=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);hold on end end end hold onROUT=ROUTESmink,minl;LENROUT=length(ROUT);Rx=ROUT;Ry=ROUT;for ii=1:LENROUTRx(ii)=a*(mod(ROUT(ii),MM)-0.5);if Rx(ii)=-0.5Rx(ii)=MM-0.5;endRy(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM);end plot(Rx,Ry) endplotif2=0;% 繪各代螞蟻爬行圖 if plotif2=1figure(3) axis(0,MM,0,MM)