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

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

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

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

4、，CD 長度為0.5，并且，假設路上信息素濃度為0，且各個螞蟻行進速度相同，單位時間所走的長度為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 時，選擇BC 與DC的螞蟻分別走過了BCD 和DCB，而選擇BH 與DH 的螞蟻都走到了H 點。所有的螞蟻都在所走過的路上留下

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

6、問題的優(yōu)化解。 歸結蟻群算法有如下特點： （1）分布式計算：各個螞蟻獨立地構造解，當有螞蟻個體構造的解較差時，并不會影響整體的求解結果。這使得算法具有較強的適應性； （2）自組織性：系統學中自組織性就是系統的組織指令是來自系統的內部。同樣的蟻群算法中的各個螞蟻的決策是根據系統內部信息素的分布進行的。這使得算法具有較強的魯棒性； （3）正反饋機制與負反饋機制結合：若某部分空間上分布的信息素越多，那么在這個空間上走過的螞蟻也就越多；走過的螞蟻越多，在那個空間上留下的信息素也就越多，這就是存在的正反饋機制。但蟻群算法中解的構造是通過計算轉移概率實現的，也就是說構造解的時候可以接受退化解，這限制了正反

7、饋機制，可以使得搜索范圍擴大，這是蟻群算法中隱含的負反饋機制。 3求解步驟應用蟻群算法求解機器人路徑優(yōu)化問題的主要步驟如下：（1）輸入由0和1組成的矩陣表示機器人需要尋找最優(yōu)路徑的地圖的地圖，其中0表示此處可以通過的，1表示此處為障礙物。上圖的表示矩陣為：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 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0

8、0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0; 0 1 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 1 1 0 0

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

10、0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;(2)輸入初始的信息素矩陣，選擇初始點和終止點并且設置各種參數。在此次計算中，我們設置所有位置的初始信息素相等。 (3)選擇從初始點下一步可以到達的節(jié)點，根據每個節(jié)點的信息素求出前往每個節(jié)點的概率，并利用輪盤算法選取下一步的初始點。其中為析取圖中弧上的信息素的濃度。為與弧相關聯的啟發(fā)式信息。，分別為，的權重參數。 （4）更新路徑，以及路程長度。 (5) 重復（3）（4）過程，直到螞蟻到達終點或者無路可走。（6）重復（3）（4）（5），直到某一代m只螞蟻迭代結束。（7）更新信息素矩陣，其中沒有到達的螞蟻

11、不計算在內。其中為信息素揮發(fā)系數。為信息量增加強度。為路徑長度。（8）重復（3）-（7），直至n代螞蟻迭代結束。4 運行結果（圖、表等）將上述矩陣輸入到程序中，畫出最短路徑的路線，并且輸入每一輪迭代的最短路徑，查看程序的收斂效果，在程序中設置plotif=1則輸出收斂和最短路徑圖，在程序中設置plotif2=1則輸出每一代螞蟻的路徑圖。 最終輸出的結果如圖 5 MATLAB程序function m_main() 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

12、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 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0; 0 1 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 1

13、 1 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 1 1 0 1 1 1 1 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 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 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

14、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; MM=size(G,1); % G 地形圖為01矩陣，如果為1表示障礙物 Tau=ones(MM*MM,MM*MM);% Tau 初始信息素矩陣（認為前面的覓食活動中有殘留的信息素） Tau=8.*Tau; K=100; % K 迭代次數（指螞蟻出動多少波） M=50; % M 螞蟻個數（每一波螞蟻有多少個） S=1 ; % S 起始點（

15、最短路徑的起始點） E=MM*MM; % E 終止點（最短路徑的目的點） Alpha=1; % Alpha 表征信息素重要程度的參數 Beta=7; % Beta 表征啟發(fā)式因子重要程度的參數 Rho=0.3 ; % Rho 信息素蒸發(fā)系數 Q=1; % Q 信息素增加強度系數 minkl=inf; mink=0; minl=0; D=G2D(G); N=size(D,1);%N表示問題的規(guī)模（象素個數） a=1;%小方格象素的邊長 Ex=a*(mod(E,MM)-0.5);%終止點橫坐標 if Ex=-0.5 Ex=MM-0.5; end Ey=a*(MM+0.5-ceil(E/MM);%終

16、止點縱坐標 Eta=zeros(N);%啟發(fā)式信息，取為至目標點的直線距離的倒數 %下面構造啟發(fā)式信息矩陣 for i=1:N ix=a*(mod(i,MM)-0.5); if ix=-0.5 ix=MM-0.5; end iy=a*(MM+0.5-ceil(i/MM); if i=E Eta(i)=1/(ix-Ex)2+(iy-Ey)2)0.5; else Eta(i)=100; end end ROUTES=cell(K,M);%用細胞結構存儲每一代的每一只螞蟻的爬行路線 PL=zeros(K,M);%用矩陣存儲每一代的每一只螞蟻的爬行路線長度 % -啟動K輪螞蟻覓食活動，每輪派出M只螞蟻

17、- for k=1:K for m=1:M % 第一步：狀態(tài)初始化 W=S;%當前節(jié)點初始化為起始點 Path=S;%爬行路線初始化PLkm=0;%爬行路線長度初始化 TABUkm=ones(N);%禁忌表初始化 TABUkm(S)=0;%已經在初始點了，因此要排除 DD=D;%鄰接矩陣初始化 % 第二步：下一步可以前往的節(jié)點 DW=DD(W,:); DW1=find(DW); for j=1:length(DW1) if TABUkm(DW1(j)=0 DW(DW1(j)=0; end end LJD=find(DW); Len_LJD=length(LJD);%可選節(jié)點的個數 % 覓食停止

18、條件：螞蟻未遇到食物或者陷入死胡同 while W=E&&Len_LJD>=1 % 第三步：轉輪賭法選擇下一步怎么走 PP=zeros(Len_LJD); for i=1:Len_LJD PP(i)=(Tau(W,LJD(i)Alpha)*(Eta(LJD(i)Beta); end sumpp=sum(PP); PP=PP/sumpp;%建立概率分布Pcum(1)=PP(1); for i=2:Len_LJD Pcum(i)=Pcum(i-1)+PP(i); end Select=find(Pcum>=rand); to_visit=LJD(Select(1); %

19、 第四步：狀態(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 end TABUkm(W)=0;%已訪問過的節(jié)點從禁忌表中刪除 DW=DD(W,:); DW1=find(DW); for j=1:length(DW1) if TABUkm(DW1(j)=0 DW(j)=0; end end LJD=find(DW); Len_LJD=length(LJD);%可選節(jié)點的個

20、數 end % 第五步：記下每一代每一只螞蟻的覓食路線和路線長度 ROUTESk,m=Path; if Path(end)=E PL(k,m)=PLkm; if PLkm<minkl mink=k;minl=m;minkl=PLkm; end else PL(k,m)=0; end end % 第六步：更新信息素Delta_Tau=zeros(N,N);%更新量初始化 for m=1:M if PL(k,m) ROUT=ROUTESk,m; TS=length(ROUT)-1;%跳數 PL_km=PL(k,m); for s=1:TS x=ROUT(s); y=ROUT(s+1); De

21、lta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km; Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km; end end end Tau=(1-Rho).*Tau+Delta_Tau;%信息素揮發(fā)一部分，新增加一部分 end % -繪圖- plotif=1;%是否繪圖的控制參數 if plotif=1 %繪收斂曲線 minPL=zeros(K); for i=1:K PLK=PL(i,:); Nonzero=find(PLK); PLKPLK=PLK(Nonzero); minPL(i)=min(PLKPLK); end figure(1) plo

22、t(minPL); hold on grid on title('收斂曲線（最小路徑長度）'); xlabel('迭代次數'); ylabel('路徑長度'); %繪爬行圖figure(2) axis(0,MM,0,MM) for i=1:MM for j=1:MM if G(i,j)=1 x1=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); hold on else x1=j-1;y1=

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