基于改進多步長蟻群算法的機器人路徑規劃

張原藝，章 政，王 泉

(武漢科技大學 信息科學與工程學院，湖北 武漢 430081)

0 引 言

傳統的移動機器人路徑規劃方法有柵格法、人工勢場法[1,2]、A*算法[3]等。隨著移動機器人技術的發展以及對機器人智能化的要求逐漸提高，許多智能優化算法已經逐漸應用到移動機器人研究領域中，如遺傳算法[4]、粒子群算法[5,6]以及蟻群算法[7-9]等。其中，蟻群算法因其具有良好的尋優能力、強魯棒性等優點，廣泛應用于移動機器人路徑規劃問題中。

近些年來，針對蟻群算法存在收斂速度慢、易陷入局部最優等問題，學者們展開了一系列研究工作[10-13]。通過分析現有的用于機器人路徑規劃的改進蟻群算法，還存在一些問題有待進一步研究，如：①采用單步長的搜索方式難以規劃出最簡化的路徑；②基于直線引導搜索策略的多步長蟻群算法因其搜索視野范圍受限而難以搜索到最優多步長路徑；③規劃出的最優路徑距離障礙物太近從而難以保證移動機器人安全到達目標點。針對上述問題，本文設計了一種改進多步長蟻群算法用于移動機器人路徑規劃。首先，設計了一種路徑引導搜索策略來確定機器人下一步移動的候選柵格。一方面擴大了算法的搜索范圍，另一方面提高了算法的搜索效率；然后，設計了一種多策略柵格選擇方法，根據選擇概率因子從不同的候選柵格集合中選擇出下一步移動的柵格，從而提高了算法的全局搜索能力。在搜索多步長的候選路徑時加入安全距離判斷策略以提高最優路徑的安全性。最后，將本文算法應用于移動機器人路徑規劃中并進行仿真實驗以驗證本文算法的有效性。

1 改進多步長蟻群算法描述

本文的改進多步長蟻群算法主要分為3個部分：①通過路徑引導的方式得到多步長候選柵格。②采用一種多策略柵格選擇機制來選擇螞蟻下一步移動的柵格。③通過改進的信息素更新策略對信息素進行更新。

1.1 環境建模

假設機器人在某個二維空間內移動，且該區域內存在有限個靜態障礙物，采用柵格法對機器人的運動環境建模。將機器人的運動區域的橫向和縱向以相等的間隔劃分為m和n等分，則該區域可表述為m×n個柵格構成的柵格圖，柵格的大小由移動機器人的尺寸而定，柵格的大小能保證容納移動機器人并且移動機器人與柵格的邊界保持適當的距離。柵格中無障礙物的柵格為可行柵格，有障礙物的柵格則為不可行柵格。通常有障礙物的柵格用黑色方格表示，為方便研究，當障礙物小于一個柵格時按一個柵格處理，可行柵格用白色方格表示。移動機器人在柵格之間的移動可看作一個質點，并且假設移動機器人每次都停留在柵格的中心位置。

以橫向為坐標系X軸，縱向為坐標系的Y軸，設X、Y方向的最大值分別為xmax和ymax。假設機器人移動的步長為λ(通常λ=1)，以λ為單位將X和Y軸分別進行劃分，則每行柵格數為Nx=xmax/λ,每列的柵格數為Ny=ymax/λ。地圖中左上角為1號柵格，柵格序號從左至右依次加1，序號從上至下依次加Ny。每個柵格的中心坐標與柵格的序號相對應，柵格i的中心坐標可表示為

(1)

式中：xi和yi分別為i號柵格中心點的橫坐標和縱坐標,且1≤i≤Nx·Ny。

基于上述描述本文定義的30×30環境模型如圖1所示。

圖1 30×30柵格環境模型

1.2 搜索候選柵格

傳統蟻群算法路徑規劃采用單步長方式進行搜索，即將當前位置相鄰的柵格作為下一步移動柵格，這種搜索方式不僅會使算法收斂速度慢而且容易產生多余的拐角。多步長搜索方式是采用一些策略從地圖中所有的自由柵格中篩選出下一步移動的候選柵格，采用多步長路徑搜索方式可有效減小路徑的長度、提高搜索效率、簡化移動路徑。但是采用多步長路徑搜索方式時需要考慮如何篩選多步長候選柵格，若將所有的自由柵格都作為候選柵格會大大降低算法的搜索效率，若采用直線引導的方式難以保證搜索到最優的候選柵格。因此本文設計了一種路徑引導的路徑搜索方式，將蟻群每次迭代搜索出的全局最優路徑作為引導路徑，將當前位置與引導路徑中連線不經過障礙物且距離目標點最近的柵格作為多步長候選柵格。路徑引導搜索方法具體如下：

創建引導路徑集合guidepath,將目標點G放入引導路徑集合中，即guidepath=G。判斷當前點與目標點的連線經過的柵格是否存在障礙物，若無障礙物則當前點和目標點是連通路徑，否則為不連通路徑。若當前點和目標點是連通路徑，則可將目標柵格作為下一步移動柵格，若不是連通路徑，則將當前點到目標點連線上離目標點最近且與當前點連通的柵格作為下一步移動的候選柵格。將所有螞蟻第一次搜索結束后得到的全局最短路徑包含的所有柵格加入到guidepath中，則guidepath=S,p1,p2,……,pn-1,pn,G。進行第二次搜索時則以新的guidepath集合作為引導路徑。從目標點G開始依次判斷當前柵格到guidepath中各個元素的連線是否是連通路徑，將guidepath集合中與當前點S連通且距離目標點G最近的柵格作為候選柵格。

圖2所示為采用路徑引導搜索策略搜索路徑方法示意圖。圖中S為當前點，G為目標點，虛線為引導路徑guidepath=S,p1,p2,p3,p4,G，圖中S點到G點、S點到p4點都不是連通路徑，而S點到p1點、S點到p2點、S點到p3點都是連通路徑，其中p3點距離G點最近，因此可將p3作為候選柵格。

圖2 路徑引導搜索策略

在每次迭代后，將本次迭代中的最短路徑與引導路徑進行比較，若本次迭代的最短路徑長度更短，則將新的路徑替代原來的引導路徑。采用上述路徑引導的搜索方法可以迅速找到距離目標點最近且與當前點連通的多步長候選柵格，從而有效提高路徑搜索的效率。

1.3 多步長路徑選擇策略

為了增加搜索路徑的多樣性，提高算法的全局搜索能力，在選擇下一步移動柵格時引入多策略路徑選擇機制。假設在t時刻，螞蟻k位于柵格i，設q1和q2是(0,1)內服從均勻分布的隨機變量，設置選擇概率因子q0，將q0與q1和q2進行比較。當q1≤q0時，將當前點周圍的柵格和路徑引導策略選出的柵格作為候選柵格，即多步長候選柵格。當q1>q0且q2≤q0時，直接將路徑引導策略選擇出的柵格作為下一步移動柵格。當q1>q0且q2>q0時，將當前點周圍的柵格作為下一步移動柵格，即單步長候選柵格。每次選擇下一個移動柵格X的公式可表示為

(2)

式中：V和V′分別表示多步長候選柵格集合和單步長候選柵格集合。pn是通過路徑引導搜索策略選擇的下一移動柵格，j和j′是根據式(3)選擇出的下一移動柵格

(3)

式中：τij(t)為t時刻從柵格i到柵格j的信息素濃度；ηij(t)為t時刻從柵格i到柵格j的啟發值，其表達式為ηij(t)=1/djG，即柵格j到目標柵格G的歐式距離的倒數；α為信息素濃度啟發因子，α的值越大，表明螞蟻更有可能選擇多數螞蟻走過的路徑；β為能見度啟發因子，β的值越大，表明螞蟻更有可能選擇離終點更近的柵格。

1.4 信息素更新

采用多步長蟻群算法進行路徑規劃時，搜索出的路徑通常含有單步長路徑和多步長路徑，單步長路徑信息素的更新方式是只更新柵格i到柵格j的信息素，而多步長路徑還包含柵格i到柵格j中心點的連線所經過的柵格。若使用單步長的信息素更新方式來對多步長路徑進行更新，會降低信息素的連續性，減少了路徑搜索的多樣性，因此需要采用多步長信息素更新方式。多步長信息素更新方式可表示為

(4)

式中：ρ為信息素濃度衰減系數；?為多步長路徑連線經過的柵格信息素增量的權值，且0

(5)

(6)

其中，Lk為螞蟻k在本次循環中所走的路徑長度，Q為信息素強度，通常是一個常量。

2 安全距離判斷策略

為了提高路徑的安全性，在選擇下一步移動的候選柵格時加入安全距離判斷策略，使規劃出的路徑與障礙物的距離大于安全距離。圖3為一段多步長路徑。設當前位置s為1號柵格，終點e為15號柵格。從圖中可以看出該路徑為連通路徑，但是在某些情況下連通的路徑不能保證機器人能完全避開障礙物。因此本文設計了一種安全距離判斷策略以提高路徑的安全性。安全距離判斷策略方法如下所示。

圖3 加入安全距離判斷策略

首先確定判斷的主方向。計算多步長路徑的斜率kpath，若0

然后判斷路徑的安全距離。設圖3中1號柵格中心坐標為x1,y1，多步長路徑與2號柵格的交點分別為a和b。路徑的直線方程可表示為

y=kpathx-x1+y1

(7)

設直線x=x1+0.5與直線y=y1-0.5的交點為d，線段de為點d到路徑ab的距離，線段de可表示為路徑到6號柵格的安全距離。de的長度lde表示安全距離的大小。lde的計算方式如式(8)所示

lde=ladcosθ

(8)

最后比較li(i=1,2,…,n)與δ的大小判斷路徑是否滿足安全距離。若路徑中所有的li都大于δ，則說明該路徑滿足安全距離條件，路徑對應的柵格可作為下一步移動的候選柵格。若路徑中有一處li小于δ，則該路徑不滿足安全距離條件。該路徑不可行。

3 算法流程

本文算法流程如下：

步驟1 采用柵格法進行地圖構建，將機器人初始位置設為起始點，設置目標點，各項參數初始化。

步驟2 將M只螞蟻放在起點，將起點加入禁忌表中。

步驟3 根據路徑引導搜索策略搜索多步長候選路徑同時判斷候選路徑是否滿足安全距離，將滿足條件的多步長候選柵格與當前位置周圍的柵格一起作為機器人下一步可移動的候選柵格集合，根據式(2)從候選柵格集合中選擇下一步移動柵格并將當前柵格加入到禁忌表中。

步驟4 重復步驟3直至本次迭代中除“迷失”螞蟻外所有螞蟻到達終點柵格，保存螞蟻所走的路徑及路徑長度。

步驟5 找出本次迭代中除“迷失”螞蟻外所有螞蟻所走路徑長度最短的路徑。將最短路徑經過的柵格集合作為引導路徑。

步驟6 根據式(4)進行多步長信息素更新。

步驟7 判斷是否滿足停止條件。如果達到停止條件則結束，否則令迭代次數加1并重復步驟2～步驟6。最終保存的最短路徑作為規劃出的最優路徑。

本文算法流程如圖4所示。

圖4 本文算法流程

4 仿真實驗及結果分析

為驗證本文算法的可行性和有效性，將蟻群算法、基于直線引導的多步長蟻群算法以及本文算法用于機器人路徑規劃并進行仿真分析。

4.1 仿真結果分析

在圖1環境下，對蟻群算法、直線引導多步長蟻群算法以及本文算法規劃出的路徑結果進行比較分析。算法各項參數設置為最大迭代次數Nmax=70，蟻群數量M=30，α=1，β=7，Q=50，q0=0.5。圖5為上述3種算法迭代次數與搜索出的路徑長度關系，從圖中可以看出多步長蟻群算法比單步長蟻群算法收斂速度快，得到的最優路徑長度更短。本文算法相比于直線引導的多步長蟻群算法提高了算法的收斂速度，且減小了路徑長度。

圖5 路徑長度與迭代次數的關系

圖6為在圖1環境下分別采用上述3種算法路徑規劃出的路徑結果。從圖中可以看出多步長的路徑搜索方式能夠搜索出比單步長的路徑搜索方式更簡化且路徑長度更短的路徑。相比于直線引導多步長蟻群算法，采用本文算法規劃出的路徑長度更短且拐點更少，路徑更平滑。其中直線引導多步長蟻群算法是當前點到目標點的連線經過的自由柵格中進行搜索，因此算法的搜索范圍較小，從圖6(b)的路徑可以看出有些路徑時可以直接一步到達，但是受到算法的搜索范圍的限制而無法搜索到。本文基于路徑引導的搜索方式擴大了多步長路徑的搜索范圍，因此相比于直線引導的搜索方式，本文算法能夠搜索到可以直接一步到達的路徑，所以規劃出的路徑更為簡化，路徑長度更短。

圖6 路徑比較結果

表1 算法仿真結果對比

4.2 路徑多樣性分析

為驗證多策略路徑選擇機制中選擇概率因子q0對算法全局搜索能力的影響，將q0取不同的值來觀察路徑多樣性的變化。路徑多樣性函數的計算公式如下

(9)

式中：m表示蟻群的個數，Lk(n)表示螞蟻k在第n次迭代搜索的路徑長度，avg(L(n))表示第n次迭代所有螞蟻搜索的路徑的平均長度。DIV(n)函數反映了不同螞蟻個體之間搜索出的路徑差異的大小，若路徑差異較大則說明算法的全局搜索能力較強。

圖7為q0在0.2、0.5和0.8這3種取值下得到的路徑多樣性曲線。從圖中可以看出，DIV(n)的取值隨著q0的增大而增大，這說明q0越大路徑的多樣性越強。隨著迭代次數的增大，DIV(n)的值逐漸減小，這是由于經過數次迭代后搜索出的路徑逐漸收斂。當q0=0.2時雖然路徑的多樣性較強，但算法的收斂速度相對較慢，當q0=0.8時，雖然收斂速度較快但是路徑的多樣性相對較差，容易產生局部最優解，因此q0的取值在0.5左右比較合適。

圖7 不同q0條件下解的多樣性曲線

4.3 加入安全距離的多步長蟻群算法路徑規劃

為了提高路徑的安全性，在搜索多步長候選柵格時加入安全距離判斷策略。圖8為δ=0.3時采用路徑引導多步長蟻群算法搜索到的路徑。比較圖8與圖6(c)的路徑可以看出加入安全距離判斷策略后規劃出的路徑與障礙物的距離始終大于安全距離，從而保證移動機器人能夠安全無碰撞到達目標點。

圖8 加入安全距離判斷策略后的路徑

本文算法加入安全距離判斷條件前后的路徑參數對比見表2。從表中可以看出加入安全距離判斷策略后規劃出的路徑長度有少量增加，這是因為未加入安全距離判斷策略進行路徑搜索時，只需搜索出路徑長度最短的自由柵格作為候選柵格。而加入安全距離判斷后使得原來的短的路徑可能會離障礙物太近而無法保證移動機器人能夠安全無碰撞地通過，因此需要規劃出即滿足安全距離又使得移動距離最短的路徑。未加入安全距離判斷策略搜索出的路徑在障礙物附近走的是直線，加入安全距離后，之前部分走直線的地方變成了拐角，從而減小了路徑的平滑度，增加了拐點個數。

表2 加入安全距離判斷策略前后對比

5 結束語

本文提出了一種基于路徑引導的多步長蟻群算法。首先設計了一種路徑引導搜索策略來選擇多步長候選柵格，將引導路徑中與當前位置的連線不存在障礙物且與目標點最近的柵格作為候選柵格。在每次迭代后搜索出的最優路徑與引導路徑長度進行比較，若新的最優路徑長度小于引導路徑的長度，則將本次迭代搜索到的最優路徑作為新的引導路徑。采用路徑引導搜索方式有效改善了多步長蟻群算法搜索范圍受限制的問題，從而提高了算法的搜索效率。在采用路徑引導搜索策略搜索多步長候選柵格的過程中加入安全距離判斷策略，使規劃出的路徑與障礙物的距離始終大于安全距離。然后設計了一種多策略的柵格選擇方式從候選柵格集合中選擇下一步移動柵格以提高算法的全局搜索能力，并分析了選擇概率因子q0在不同取值情況下對算法全局搜索能力的影響。最后將本文提出的改進多步長蟻群算法與傳統蟻群算法和直線引導多步長蟻群算法進行仿真對比實驗。實驗結果表明，本文改進的算法有效減少路徑的長度和拐彎次數，提高了算法的收斂速度。比較加入安全距離判斷策略前后規劃出的路徑可以看出，加入安全距離判斷策略后規劃出的路徑雖然路徑的長度有所增加、平滑度降低，但是保證了路徑的安全性。