機器人的路徑規劃---蟻群算法蟻群算法眾所周知，蟻群算法是優化領域中新出現并逐漸引起重視的一種仿生進化算法它是群體智能的典型實現，是一種基于種群尋優的啟發式搜索算法。自從M.Dorigo等意大利學者在1991年首先提出蟻群算法(AntColonySystem,ACS)以來，這種新型的分布式智能模擬算法已逐漸引起人們的注意并得到廣泛的應用。蟻群算法的特點主要表現在以下五個方面：螞蟻群體行為表現出正反饋過程。蟻群在尋優的過程中會釋放一定量的信息素，蟻群的規模越大，釋放的信息素的量也就越大，而尋優路徑上存在的信息素濃度越高，就會吸引更多的螞蟻，形成一種正反饋機制，然后通過反饋機制的調整，可對系統中的較優解起到一個自增強的作用，從而使問題的解向著全局最優的方向演變，最終能有效地獲得全局相對較優解。蟻群算法是一種本質并行的算法。個體之間不斷進行信息交流和傳遞.有利于最優解的發現，并在很大程度上減少了陷于局部最優的可能。蟻群算法易于與其他方法結合。蟻族算法通過與其他算法的結合，能夠揚長避短，提高算法的性能。蟻群算法提供的解具有全局性的特點。一群算法是一種群只能算法，每只螞蟻巡游的過程相對獨立，他們會在自己的活動空間進行搜索，螞蟻在尋優過程中通過釋放信息素，相互影響，互相通信，保證了解的全局性。蟻群算法具有魯棒性。蟻族算法的數學模型易于理解，可以廣泛應用在很多復雜的優化問題中，蟻族算法區別于傳統優化算法的一個特點在于該算法不依賴于初始點的選擇，受初始點的影響相對較小，并且在整個算法過程中會自適應的調整尋優路徑。由此可見，在機器人尋找最優路徑的過程中，采用蟻群算法實現路徑的規劃問題，可以高效，準確的找到最優的路徑。移動機器人的路徑規劃2.1環境信息處理假設機器人運行環境為邊長分別為x和Y的矩形區域，在矩形區域內分布有n個異形障礙物，顯然對于該獲取的實際環境信息：首先，由于障礙物大小不一，而且形狀也各不相同，為了減少機器人處理地圖信息的負擔，需要對工作環境行一些必要的預處理；其次，在后續章節中，描述機器人的路徑規劃方法是基于把障礙物近似成質點的基礎上進行的，而要把障礙物近似成質點也同樣需要對工作環境的信息進行適當預處理。環境信息預處理遵循以下原則：1）移動機器人作二維平面運動，障礙物不考慮高度信息；2）小問距障礙物作合并處理，即如果兩個障礙物相距太近，障礙物之間距離小于機器人通過的最小安全距離。則將兩個障礙物合并作為一個障礙物處理；3）作出障礙物的外接矩形，并對障礙物外接矩形進行徑向擴張且對環境邊界向內作徑向擴張，因此可把移動機器人退化成運動質點處理。2.2環境建模設機器人工作空間為二維結構化空間記為RS，并且障礙物位置、大小已知。用尺寸相同的柵格對RS進行劃分，柵格大小以機器人能在其內自由運動為限，設機器人能自由運動的范圍為［0，R］。若某一柵格尺寸范圍內不含任何障礙物，則稱此柵格為自由柵格，反之稱為障礙柵格。自由空間和障礙物均可表示成柵格塊的集合，我們將障礙物柵格集記為OS。柵格標識可采用下述兩種方法：（1）直角坐標法。以柵格陣左上角為坐標原點，水平向右為x軸正方向，豎直向下為Y軸正方向，每一柵格區間對應坐標軸上的一個單位長度。任一柵格均可用直角坐標（x，y）唯一標識。（2）序號法。按從左到右，從上到下的順序，從柵格陣左上角第一個柵格開始，給每一個柵格一個序號n（從0開始計），則序號以與柵格塊一一對應。2.3具體方法給定一個有彈個節點的城市道路網的路徑規劃問題，我們可以把指定的起始點s假設為人工蟻群（以下簡稱為蟻群）的巢穴，把目標點t假設為要尋找的食物，則此路徑規劃問題就可以轉化為蟻群尋找食物的路徑尋優問題。假定人工螞蟻(以下簡稱為螞蟻)的數量為m只，則每只螞蟻的行為要符合以下的規則：能夠釋放出兩種類型的信息素：“食物”信息素和“巢穴”信息素；根據與當前節點相連接的各個路徑上的信息素濃度和路徑長度，以相應的概率來隨機選擇下一個節點；不再選擇已經走過的節點為下一個節點，這可以通過一個結構數組來實現；在尋找食物時，通過“食物”信息素尋找下一個節點，同時釋放“巢穴”信息素；在尋找巢穴時，通過“巢穴”信息素尋找下一個節點，同時釋放“食物”信息素；按一定的路徑長度釋放相應的信息素濃度，并且所釋放的信息素濃度會隨著時間的推移而逐步減少；程序設計流程在主程序流程中，地圖數據庫是從實際地圖中抽象出來的城市道路網相關的數據信息，其中包括城市道路網的節點信息、道路信息和相應道路的信息素信息，每部分信息都各自形成一個數據表。在節點表中，包括了各個節點的編號、對應的地點名稱和經緯度坐標等數據。在道路表中，包括了每條道路的起點和終點對應的編號、道路的長度、級別和所經過的路線等數據。在信息素表中，包括了對應道路上的“巢穴”信息素和“食物”信息素等數據。所需要輸入的參數包括：節點的數量n、起始節點的編號OriNode、目標節點的編號DesNode、最大循環次數MaxCount.螞蟻的數量m、蒸發信息素的相對重要程度、每只螞蟻所釋放的信息素總量Q、信息素濃度的相對重要程度口、啟發式信息的相對重要程。所需要初始化的參數包括：“巢穴”信息素和“食物”信息素等值，每條道路對應的“巢穴”信息素和“食物”信息素的值分別仞始化為1，這是為了在計算下一個節點的選擇概率時，分母不為0。在程序開始時，首先連接地圖數據庫，然后輸入、初始化各個參數并開始進行循環。在每次循環中，每只螞蟻依次進行尋食過程，如果有螞蟻找到了食物即找到了一條尋食路徑，將此路徑與本次循環中其它螞蟻找到的尋食路徑進行比較，將最小的尋食路徑更新為最優路徑，并判斷是否滿足所給定的精度，如果滿足則退出循環，否則進行下一次循環。當循環次數達到最大次數時，結束循環并判斷是否找到了最優路徑，如果找到了最優路徑，則輸出最優路徑的路線及其權值，否則顯示沒有找到最優路徑。最后，關閉地圖數據庫并結束程序。在每只螞蟻進行尋食的過程中，首先判斷螞蟻是否正在尋找食物，如果是則進行尋找食物的過程，否則進行尋找巢穴的過程。在進行尋找食物的過程中，首先從地圖數據庫的道路表中讀取與當前節點所連接的節點數，以及每個節點的編號和權值。判斷每個節點是否已經走過，如果此節點已經走過，則讀取下一個節點。從地圖數據庫的信息素表中讀取對應邊的“食物”信息素值，從當前點到下一可行點的轉移是由基于信息量的狀態轉移概率和和距離啟發式信息概率綜合決定的，而這里采用的綜合決定方法是基于比例選擇策略即“輪盤賭”的方式。從地圖數據庫的道路表中讀取對應邊的權值，并計算所走過路徑的權值。從地圖數據庫的信息素表中讀取對應邊的“巢穴”信息素值，并重新計算對應邊的“巢穴”信息素值。當所得的值小于1時，將此值設置為1，這是為了保證下一回計算選擇概率時分母不為0。將重新計算的“巢穴”信息素值更新到信息素表中。判斷下一個目標節點是否為食物，如果是的話，則保存各個記錄，如螞蟻所走過的節點、此螞蟻找到食物的次數以及整個路徑的總權值等數據，并為尋找巢穴做準備，如清空內存中的歷史數據，將食物作為起始節點等，否則設置下一個目標節點為當前節點。在進行尋找巢穴的過程中，大部分的操作都跟上面螞蟻進行尋找食物的過程一樣，只不過將“食物”信息素和“巢穴”信息素進行對調，在判斷下一個目標節點是否為巢穴的時候，不需要保存各個記錄，只需為尋找食物做準備，如清空內存中的歷史數據，將巢穴作為起始節點，并將此螞蟻上次找到食物的路徑記錄。程序流程圖如下：

Matlab仿真4.1參數介紹地圖數據庫是從實際地圖中抽象出來的城市道路網相關的數據信息，其中包括城市道路網的節點信息、道路信息和相應道路的信息素信息，每部分信息都各自形成一個數據表。在節點表中，包括了各個節點的編號、對應的地點名稱和經緯度坐標等數據。在道路表中，包括了每條道路的起點和終點對應的編號、道路的長度、級別和所經過的路線等數據。在信息素表中，包括了對應道路上的“巢穴”信息素和“食物”信息素等數據。本仿真系統的靜態地圖數據假設在機器人出發之前就已經得到，而動態地圖數據假設按一定的頻率可以得到。在機器人整個仿真系統中所需要輸入的參數包括：節點的數量櫛、起始節點的編號OriNode、目標節點的編號DesNode、最大循環次數MaxCount、螞蟻的數量m、蒸發信息素的相對重要程度、每只螞蟻所釋放的信息素總量Q、信息素濃度的相對重要程度〃、啟發式信息的相對重要程。所需要初始化的參數包括：“巢穴”信息素和“食物”信息素等值，每條道路對應的“巢穴”信息素和“食物”信息素的值分別初始化為1，這是為了在計算下一個節點的選擇概率時，分母不為0。4.2程序介紹4.2.1G2D.m用于把障礙物分布圖轉化為圖的賦權鄰接矩陣，地形圖矩陣是一個01矩陣，里面的所有元素要么為0，要么為1。為0即表示機器人可以到達的節點，為1即表示其對應的柵格是障礙物。源程序如下：functionD=G2D(G)a=1;N=size(G,1);D=inf*ones(NA2,NA2);fori=1:(NA2)x=ceil(i/N-0.00001);y=mod(i,N);ify==0y=N;endx1=x-1;y1=y-1;ifx1>=1&&y1>=1j=(x1-1)*N+y1;D(i,j)=1.414*a;D(j,i)=1.414*a;endx2=x-1;y2=y;ifx2>=1j=(x2-1)*N+y2;D(i,j)=a;D(j,i)=a;endx3=x-1;y3=y+1;ifx3>=1&&x3<=Nj=(x3-1)*N+y3;D(i,j)=1.414*a;D(j,i)=1.414*a;endx4=x;y4=y-l;ify4>=lj=(x4-l)*N+y4;D(i,j)=a;D(j,i)=a;endx5=x;y5=y+l;ify5v=Nj=(x5-l)*N+y5;D(i,j)=a;D(j,i)=a;endx6=x+l;y6=y-l;ifx6v=N&&y6>=lj=(x6-l)*N+y6;D(i,j)=1.414*a;D(j,i)=1.414*a;endx7=x+l;y7=y;ifx7v=Nj=(x7-l)*N+y7;D(i,j)=a;D(j,i)=a;endx8=x+l;y8=y+l;ifx8v=N&&y8v=Nj=(x8-l)*N+y8;D(i,j)=1.414*a;D(j,i)=1.414*a;endendforx=l:Nfory=l:NifG(x,y)=lJ=(x-l)*N+y;D(:,J)=inf*ones(NA2,l);D(J,:)=inf*ones(l,NA2);endendendfori=l:(N-l)x=i*N+1;y=(i+1)*N;D(x,y)=inf;D(y,x)=inf;end4.2.2ACASP.m障礙物可以動的情況設計的蟻群算法，其主要功能就是通過派遣若干批螞蟻，來搜索動態環境下的最短路。程序內部設計準則完全按照前面的設計要求進行，包括啟發式信息規則、信息素更新規則，等等。當然，此程序可以單獨運行，主要用于解決靜態環境下的螞蟻尋路問題。程序把各批次所有螞蟻的行走路線都記錄下來，可以據此繪出螞蟻尋路的動態圖形。源程序如下：function[ROUTES,PL,Tau]=Route(GTau,K,M,S,E,Alpha,Beta,Rho,Q)D=G2D(G);N=size(D,1);MM=size(G,1);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(1,N);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(1,i)=1/((ix-Ex)A2+(iy-Ey)A2)A0.5;elseEta(1,i)=100;endendROUTES=cell(K,M);PL=zeros(K,M);fork=1:Kdisp(k);form=1:MW=S;Path=S;PLkm=0;TABUkm=ones(1,N);TABUkm(S)=0;DD=D;DW=DD(W,:);DW1=find(DW<inf);forj=1:length(DW1)ifTABUkm(DW1(j))==0DW(j)=inf;endendLJD=find(DW<inf);Len_LJD=length(LJD);whileW?=E&&Len_LJD>=1PP=zeros(1,Len_LJD);fori=1:Len_LJDPP(i)=(Tau(W,LJD(i))AAlpha)*(Eta(LJD(i))ABeta);endPP=PP/(sum(PP));Pcum=cumsum(PP);Select=find(Pcum>=rand);to_visit=LJD(Select(1));Path=[Path,to_visit];PLkm=PLkm+DD(W,to_visit);W=to_visit;%movetothenextpointforkk=1:NifTABUkm(kk)==0DD(W,kk)=inf;DD(kk,W)=inf;endendTABUkm(W)=0;DW=DD(W,:);DW1=find(DW<inf);forj=1:length(DW1)ifTABUkm(DW1(j))==0DW(j)=inf;endendLJD=find(DW<inf);Len_LJD=length(LJD);endROUTES{k,m}=Path;ifPath(end)==EPL(k,m)=PLkm;elsePL(k,m)=inf;endendDelta_Tau=zeros(N,N);form=1:MifPL(k,m)<infROUT=ROUTES{k,m};TS=length(ROUT)-1;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;%pheromoneevaporatessomeandaccumulatessomeendplotif=1;%controlparameter,determineifplotornotifplotif==1%plotconvergencecurvemeanPL=zeros(1,K);minPL=zeros(1,K);fori=1:KPLK=PL(i,:);Nonzero=find(PLK<inf);PLKPLK=PLK(Nonzero);meanPL(i)=mean(PLKPLK);minPL(i)=min(PLKPLK);endfigure(1)plot(minPL,'r');holdonplot(meanPL,'g');legend('最小路徑長度','平均路徑長度')；gridontitle('收斂曲線(平均路徑長度和最小路徑長度)');xlabel('迭代次數');ylabel('路徑長度')；%PlotPassingGraphfigure(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=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=ROUTES{K,M};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,'LineWidth',2)endtitle('ShortestPath');axis([0,MM,0,MM]);plotif2=1;%Plotrouteforeveryiterationifplotif2==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]);holdonelsex1=j-1;y1=MM-i;x2=j;y2=MM-i;x3=j;y3=MM-i+