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

MATLAB實(shí)現(xiàn)基于蟻算法的機(jī)器人路徑規(guī)劃

1、問題描述

移動(dòng)機(jī)器人路徑規(guī)劃是機(jī)器人學(xué)的一個(gè)重要研究領(lǐng)域。它要求機(jī)器人依據(jù)某個(gè)或某些優(yōu)化原則（如最

小能量消耗，最短行走路線，最短行走時(shí)間等），在其工作空間中到一條從起

始狀態(tài)到目標(biāo)狀態(tài)的能避開障礙物的最優(yōu)路徑。機(jī)器人路徑規(guī)劃問題可以建模為一個(gè)有約束的優(yōu)化問題，都

要完成路徑規(guī)劃、定位和避障等任務(wù)。

2算法理論

蟻算法（AntColonyAlgorithm，ACA），最初是由意大利學(xué)者DorigoM.博士于1991年首次提

出，其本質(zhì)是一個(gè)復(fù)雜的智能系統(tǒng)，且具有較強(qiáng)的魯棒性，優(yōu)良的分布式計(jì)算機(jī)制等優(yōu)點(diǎn)。該算法經(jīng)過十

多年的發(fā)展，已被廣大的科學(xué)研究人員應(yīng)用于各種問題的研究，如旅行商問題，二次規(guī)劃問題，生產(chǎn)調(diào)度

問題等。但是算法本身性能的評(píng)價(jià)等算法理論研究方面進(jìn)展較慢。

Dorigo提出了精英蟻模型（EAS），在這一模型中信息素更新按照得到當(dāng)前最優(yōu)解的螞蟻所構(gòu)造的

解來進(jìn)行，但這樣的策略往往使進(jìn)化變得緩慢，并不能取得較好的效果。次年Dorigo博士給出改進(jìn)模型

（ACS），文中改進(jìn)了轉(zhuǎn)移概率模型，并且應(yīng)用了全局搜索與局部搜索策略，來得進(jìn)行深度搜索。

Stützle與Hoos給出了最大-最小螞蟻系統(tǒng)（MAX-MIAS），所謂最大-最小即是為信息素設(shè)定上限與

下限，設(shè)定上限避免搜索陷入局部最優(yōu)，設(shè)定下限鼓勵(lì)深度搜索。螞蟻?zhàn)鳛橐粋€(gè)生物個(gè)體其自身的能力是十

分有限的，比如螞蟻個(gè)體是沒有視覺的，螞蟻?zhàn)陨眢w積又是那么渺小，但是由這些能力有限的螞蟻組成的

蟻卻可以做出超越個(gè)體螞蟻能力的超常行為。螞蟻沒有視覺卻可以尋覓食物，螞蟻體積渺小而蟻卻可

以搬運(yùn)比它們個(gè)體大十倍甚至百倍的昆蟲。這些都說明螞蟻體內(nèi)部的某種機(jī)制使得它們具有了體智能，

可以做到螞蟻個(gè)體無法實(shí)現(xiàn)的事情。經(jīng)過生物學(xué)家的長(zhǎng)時(shí)間觀察發(fā)現(xiàn)，螞蟻是通過分泌于空間中的信息素進(jìn)

行信息交流，進(jìn)而實(shí)現(xiàn)體行為的。

下面簡(jiǎn)要介紹蟻通過信息素的交流到最短路徑的簡(jiǎn)化實(shí)例。如圖2-1所示，AE之間有兩條路

ABCDE與ABHDE，其中AB，DE，HD，HB的長(zhǎng)度為1，BC，CD長(zhǎng)度為0.5，并且，假設(shè)路上信

息素濃度為0，且各個(gè)螞蟻行進(jìn)速度相同，單位時(shí)間所走的長(zhǎng)度為1，每個(gè)單位時(shí)間內(nèi)在走過路徑上留下的

信息素的量也相同。當(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的螞蟻分別走過了BCD和DCB，而選擇BH

與DH的螞蟻都走到了H點(diǎn)。所有的螞蟻都在所走過的路上留下了相同濃度的信息素，那么路徑BCD上的

信息素的濃度是路徑BHD上信息素濃度的兩倍，這樣若再次有螞蟻選擇走BC和BH時(shí)，或選擇走DC與

DH時(shí)，都會(huì)以較大的概率選擇信息素濃度高的一邊。這樣的過程反復(fù)進(jìn)行下去，最短的路徑上走過的螞蟻

較多，留下的信息素也越多，蟻這樣就可以到一條較短的路。這就是它們體智能的體現(xiàn)。

蟻算法就是模擬螞蟻覓食過程中可以到最短的路的行為過程設(shè)計(jì)的一種仿生算法。在用蟻算法求

解組合優(yōu)化問題時(shí)，首先要將組合優(yōu)化問題表達(dá)成與信息素相關(guān)的規(guī)范形式，然后各個(gè)螞蟻獨(dú)立地根據(jù)局部的

信息素進(jìn)行決策構(gòu)造解，并根據(jù)解的優(yōu)劣更新周圍的信息素，這樣的過程反復(fù)的進(jìn)行即可求出組合優(yōu)化問題

的優(yōu)化解。

歸結(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)的組織指令是來自系統(tǒng)的內(nèi)部。同樣的蟻算法中的各個(gè)螞

蟻的決策是根據(jù)系統(tǒng)內(nèi)部信息素的分布進(jìn)行的。這使得算法具有較強(qiáng)的魯棒性；

（3）正反饋機(jī)制與負(fù)反饋機(jī)制結(jié)合：若某部分空間上分布的信息素越多，那么在這個(gè)空間上走過的螞蟻

也就越多；走過的螞蟻越多，在那個(gè)空間上留下的信息素也就越多，這就是存在的正反饋機(jī)制。但蟻算

法中解的構(gòu)造是通過計(jì)算轉(zhuǎn)移概率實(shí)現(xiàn)的，也就是說構(gòu)造解的時(shí)候可以接受退化解，這限制了正反饋機(jī)制，

可以使得搜索范圍擴(kuò)大，這是蟻算法中隱含的負(fù)反饋機(jī)制。

3求解步驟

應(yīng)用蟻算法求解機(jī)器人路徑優(yōu)化問題的主要步驟如下：

（1）輸入由0和1組成的矩陣表示機(jī)器人需要尋最優(yōu)路徑的地圖的地圖，其中

示此處可以通過的，1表示此處為障礙物。

0表

圖的表示矩陣為：

00000000;000

00000;00000000;000000111

;00000000;011

10000;000000

00;11000000;

11000000;11000000;000000

00;

00000000;

11011110;

11011110;

11011110;

00000000;

00000110;

00100110;

00100000;

00000000;

(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)的概率，并利用輪

盤算法選取下一步的初始點(diǎn)。

p

k

ij

t

ijtijktabu

0

ijij

k

ifjtabuk

otherwise

其中ijt為析取圖中弧i,j上的信息素的濃度。ij為與弧i,j相關(guān)聯(lián)的啟發(fā)式信息。，分別為ijt，ij的權(quán)

重參數(shù)。

（4）更新路徑，以及路程長(zhǎng)度。

（5）重復(fù)（3）（4）過程，直到螞蟻到達(dá)終點(diǎn)或者無路可走。

（6）重復(fù)（3）（4）（5），直到某一代m只螞蟻迭代結(jié)束。

（7）更新信息素矩陣，其中沒有到達(dá)的螞蟻不計(jì)算在內(nèi)。

ijt11ijtij

ijtQ

Lkt，如果螞蟻k經(jīng)過i，j

0，如果螞蟻k不經(jīng)過i，j

其中為信息素?fù)]發(fā)系數(shù)。Q為信息量增加強(qiáng)度。Lkt為路徑長(zhǎng)度。

（8）重復(fù)（3）-（7），直至n代螞蟻迭代結(jié)束。

4運(yùn)行結(jié)果（圖、表等）將上述矩陣輸入到程序中，畫出最短路徑的路線，并且輸入每一輪迭代的最短路

徑，

查看程序的收斂效果，在程序中設(shè)置plotif=1則輸出收斂和最短路徑圖，在程序中設(shè)置plotif2=1則輸出每一

代螞蟻的路徑圖。

最終輸出的結(jié)果如圖

20

18

16

14

12

10

8

6

4

2

24681

收斂曲線（最小路徑長(zhǎng)度）

35

30

25

20

15

度

長(zhǎng)

徑

路

5

0

5MATLAB程序functionm_main()

G=[

108090100迭代次數(shù)

00000000;

0

00000000;

00000000;

00000000;

00000000;

00000000;

00000000;

11000000;

11000000;

11000000;

11000000;

00000000;

11011110;

11011110;

11011110;

00000000;

00000110;

00100110;

00100000;

00000000;];

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)(最短路徑的起始點(diǎn))

E=MM*MM;%E終止點(diǎn)(最短路徑的目的點(diǎn))

Alpha=1;

Beta=7;

%Alpha表征信息素重要程度的參數(shù)

%Beta表征啟發(fā)式因子重要程度的參數(shù)

Rho=0.3;%Rho信息素蒸發(fā)系數(shù)

Q=1;

minkl=inf;

mink=0;

minl=0;

D=G2D(G);

=size(D,1);%表示問題的規(guī)模(象素個(gè)數(shù))a=1;%小方格象素的邊長(zhǎng)

Ex=a*(mod(E,MM)-0.5);%終止點(diǎn)橫坐標(biāo)

ifEx==-0.5

Ex=MM-0.5;end

Ey=a*(MM+0.5-ceil(E/MM));%終止點(diǎn)縱坐標(biāo)

Eta=zeros();%啟發(fā)式信息，取為至目標(biāo)點(diǎn)的直線距離的倒數(shù)%下面構(gòu)造啟發(fā)式信息矩陣

fori=1:ix=a*(mod(i,MM)-0.5);

ifix==-0.5

%Q信息素增加強(qiáng)度系數(shù)

ix=MM-0.5;

endiy=a*(MM+0.5-ceil(i/MM));

ifi~=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只螞蟻-------------------------------------------

fork=1:Kform=1:M

%%第一步：狀態(tài)初始化W=S;%當(dāng)前節(jié)點(diǎn)初始化為起始點(diǎn)Path=S;%爬行路線初始化

PLkm=0;%爬行路線長(zhǎng)度初始化

TABUkm=ones();%禁忌表初始化

TABUkm(S)=0;%已經(jīng)在初始點(diǎn)了，因此要排除

DD=D;%鄰接矩陣初始化

%%第二步：下一步可以前往的節(jié)點(diǎn)DW=DD(W,:);

DW1=find(DW);

forj=1:length(DW1)

ifTABUkm(DW1(j))==0DW(DW1(j))=0;

end

end

LJD=find(DW);

Len_LJD=length(LJD);%可選節(jié)點(diǎn)的個(gè)數(shù)

%%覓食停止條件：螞蟻未遇到食物或者陷入死胡同

whileW~=E&&Len_LJD>=1

%%第三步：轉(zhuǎn)輪賭法選擇下一步怎么走PP=zeros(Len_LJD);

fori=1:Len_LJDPP(i)=(Tau(W,LJD(i))^Alpha)*((Eta(LJD(i)))^Beta);

end

sumpp=sum(PP);

PP=PP/sumpp;%建立概率分布

Pcum(1)=PP(1);

fori=2:Len_LJDPcum(i)=Pcum(i-1)+PP(i);end

Select=find(Pcum>=rand);to_visit=LJD(Select(1));%%第四步：狀態(tài)更新和記錄

Path=[Path,to_visit];%路徑增加

PLkm=PLkm+DD(W,to_visit);%路徑長(zhǎng)度增加W=to_visit;%螞蟻移到下一個(gè)節(jié)點(diǎn)

forkk=1:ifTABUkm(kk)==0DD(W,kk)=0;DD(kk,W)=0;endend

TABUkm(W)=0;%已訪問過的節(jié)點(diǎn)從禁忌表中刪除DW=DD(W,:);

DW1=find(DW);

forj=1:length(DW1)

ifTABUkm(DW1(j))==0

DW(j)=0;

end

endLJD=find(DW);Len_LJD=length(LJD);%可選節(jié)點(diǎn)的個(gè)數(shù)end

%%第五步：記下每一代每一只螞蟻的覓食路線和路線長(zhǎng)度ROUTES{k,m}=Path;

ifPath(end)==EPL(k,m)=PLkm;ifPLkm

elsePL(k,m)=0;

end

end

%%第六步：更新信息素Delta_Tau=zeros(,);%更新量初始化form=1:M

ifPL(k,m)ROUT=ROUTES{k,m};

TS=length(ROUT)-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;

end

end

end

Tau=(1-Rho).*Tau+Delta_Tau;%信息素?fù)]發(fā)一部分，新增加一部分

end

%%-------------------------繪圖------------------------

plotif=1;%是否繪圖的控制參數(shù)

ifplotif==1%繪收斂曲線

minPL=zeros(K);

fori=1:K

PLK=PL(i,:);

onzero=find(PLK);

PLKPLK=PLK(onzero);

minPL(i)=min(PLKPLK);

end

figure(1)

plot(minPL);

holdon

gridon

title('收斂曲線(最小路徑長(zhǎng)度)');

xlabel('迭代次數(shù)');

ylabel('路徑長(zhǎng)度');%繪爬行圖

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]);

holdon

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;

fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);

holdonendendendholdon

ROUT=ROUTES{mink,minl};

LEROUT=length(ROUT);

Rx=ROUT;

Ry=ROUT;

forii=1:LEROUT

Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);ifRx(ii)==-0.5

Rx(ii)=MM-0.5;endRy(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));end

plot(Rx,Ry)end

plotif2=0;%繪各代螞蟻爬行圖

ifplotif2==1figure(3)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]);

holdonelse

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],[1,1,1]);holdon

endendendfork=1:K

PLK=PL(k,:);minPLK=min(PLK);

pos=find(PLK==minPLK);

m=pos(1);

ROUT=ROUTES{k,m};

LEROUT=length(ROUT);

Rx=ROUT;

Ry=ROUT;

forii=1:LEROUT

Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);

ifRx(ii)==-0.5

Rx(ii)=MM-0.5;

end

Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));

end

plot(Rx,Ry)

holdon

end

end

functionD=G2D(G)

l=size(G,1);

D=zeros(l*l,l*l);

fori=1:l

forj=1:l

ifG(i,j)==0

form=1:l

forn=1:l

ifG(m,n)==0

im=abs(i-m);jn=abs(j-n);

ifim+jn==1||(im==1&&jn==1)

D((i-1)*l+j,(m-1)*l+n)=(im+jn)^0.5;endendend

end

end

end

end