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

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

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

4、為0.5，并且，假設(shè)路上信息素濃度為0，且各個(gè)螞蟻行進(jìn)速度相同，單位時(shí)間所走的長(zhǎng)度為1，每個(gè)單位時(shí)間內(nèi)在走過(guò)路徑上留下的信息素的量也相同。當(dāng)t=0時(shí)，從A 點(diǎn)，E 點(diǎn)同時(shí)各有30 只螞蟻從該點(diǎn)出發(fā)。當(dāng)t=1，從A 點(diǎn)出發(fā)的螞蟻?zhàn)叩紹 點(diǎn)時(shí)，由于兩條路BH 與BC 上的信息素濃度相同，所以螞蟻以相同的概率選擇BH 與BC，這樣就有15 只螞蟻選擇走BH，有15 只螞蟻選擇走BC。同樣的從E 點(diǎn)出發(fā)的螞蟻?zhàn)叩紻 點(diǎn)，分別有15 只螞蟻選擇DH 和DC。當(dāng)t=2 時(shí)，選擇BC 與DC的螞蟻分別走過(guò)了BCD 和DCB，而選擇BH 與DH 的螞蟻都走到了H 點(diǎn)。所有的螞蟻都在所走過(guò)的路上留下了相同濃度的

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

6、。 歸結(jié)蟻群算法有如下特點(diǎn)： （1）分布式計(jì)算：各個(gè)螞蟻獨(dú)立地構(gòu)造解，當(dāng)有螞蟻個(gè)體構(gòu)造的解較差時(shí)，并不會(huì)影響整體的求解結(jié)果。這使得算法具有較強(qiáng)的適應(yīng)性； （2）自組織性：系統(tǒng)學(xué)中自組織性就是系統(tǒng)的組織指令是來(lái)自系統(tǒng)的內(nèi)部。同樣的蟻群算法中的各個(gè)螞蟻的決策是根據(jù)系統(tǒng)內(nèi)部信息素的分布進(jìn)行的。這使得算法具有較強(qiáng)的魯棒性； （3）正反饋機(jī)制與負(fù)反饋機(jī)制結(jié)合：若某部分空間上分布的信息素越多，那么在這個(gè)空間上走過(guò)的螞蟻也就越多；走過(guò)的螞蟻越多，在那個(gè)空間上留下的信息素也就越多，這就是存在的正反饋機(jī)制。但蟻群算法中解的構(gòu)造是通過(guò)計(jì)算轉(zhuǎn)移概率實(shí)現(xiàn)的，也就是說(shuō)構(gòu)造解的時(shí)候可以接受退化解，這限制了正反饋機(jī)制，可以

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

8、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 0 0 0

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

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

11、其中為信息素?fù)]發(fā)系數(shù)。為信息量增加強(qiáng)度。為路徑長(zhǎng)度。（8）重復(fù)（3）-（7），直至n代螞蟻迭代結(jié)束。4 運(yùn)行結(jié)果（圖、表等）將上述矩陣輸入到程序中，畫(huà)出最短路徑的路線，并且輸入每一輪迭代的最短路徑，查看程序的收斂效果，在程序中設(shè)置plotif=1則輸出收斂和最短路徑圖，在程序中設(shè)置plotif2=1則輸出每一代螞蟻的路徑圖。 最終輸出的結(jié)果如圖 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 0 0 1

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

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

14、 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 初始信息素矩陣（認(rèn)為前面的覓食活動(dòng)中有殘留的信息素） Tau=8.*Tau; K=100; % K 迭代次數(shù)（指螞蟻出動(dòng)多少波） M=50; % M 螞蟻個(gè)數(shù)（每一波螞蟻有多少個(gè)） S=1 ; % S 起始點(diǎn)（最短路徑的起

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

16、Eta=zeros(N);%啟發(fā)式信息，取為至目標(biāo)點(diǎn)的直線距離的倒數(shù) %下面構(gòu)造啟發(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);%用細(xì)胞結(jié)構(gòu)存儲(chǔ)每一代的每一只螞蟻的爬行路線 PL=zeros(K,M);%用矩陣存儲(chǔ)每一代的每一只螞蟻的爬行路線長(zhǎng)度 % -啟動(dòng)K輪螞蟻覓食活動(dòng)，每輪派出M只螞蟻- for

17、k=1:K for m=1:M % 第一步：狀態(tài)初始化 W=S;%當(dāng)前節(jié)點(diǎn)初始化為起始點(diǎn) Path=S;%爬行路線初始化PLkm=0;%爬行路線長(zhǎng)度初始化 TABUkm=ones(N);%禁忌表初始化 TABUkm(S)=0;%已經(jīng)在初始點(diǎn)了，因此要排除 DD=D;%鄰接矩陣初始化 % 第二步：下一步可以前往的節(jié)點(diǎn) 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é)點(diǎn)的個(gè)數(shù) % 覓食停止條件：螞蟻未

18、遇到食物或者陷入死胡同 while W=E&Len_LJD=1 % 第三步：轉(zhuǎn)輪賭法選擇下一步怎么走 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); % 第四步：狀態(tài)更新和記錄Path=Path,t

19、o_visit;%路徑增加PLkm=PLkm+DD(W,to_visit);%路徑長(zhǎng)度增加W=to_visit;%螞蟻移到下一個(gè)節(jié)點(diǎn) for kk=1:N if TABUkm(kk)=0 DD(W,kk)=0; DD(kk,W)=0; end end TABUkm(W)=0;%已訪問(wèn)過(guò)的節(jié)點(diǎn)從禁忌表中刪除 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é)點(diǎn)的個(gè)數(shù) end % 第五步：記下每一代每一只螞蟻的

20、覓食路線和路線長(zhǎng)度 ROUTESk,m=Path; if Path(end)=E PL(k,m)=PLkm; if PLkmminkl 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;%跳數(shù) PL_km=PL(k,m); for s=1:TS x=ROUT(s); y=ROUT(s+1); Delta_Tau(x,y)=Delta_Tau(x,y)

21、+Q/PL_km; Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km; end end end Tau=(1-Rho).*Tau+Delta_Tau;%信息素?fù)]發(fā)一部分，新增加一部分 end % -繪圖- plotif=1;%是否繪圖的控制參數(shù) 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) plot(minPL); hold on grid on t

22、itle(收斂曲線（最小路徑長(zhǎng)度）); xlabel(迭代次數(shù)); ylabel(路徑長(zhǎng)度); %繪爬行圖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=MM-i; x2=j;y2=MM-i; x3=j;y3=MM-i+1; x4=j-1;y4=MM-i+1; fil

23、l(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 for j=1:MM if G(i,j)=1 x1=j-1;y1=MM-i; x2=j;y2=MM