張原藝，章 政，王 泉

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

0 引 言

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

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

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

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

1.1 環(huán)境建模

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

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

(1)

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

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

圖1 30×30柵格環(huán)境模型

1.2 搜索候選柵格

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

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

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

圖2 路徑引導(dǎo)搜索策略

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

1.3 多步長路徑選擇策略

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

(2)

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

(3)

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

1.4 信息素更新

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

(4)

式中：ρ為信息素濃度衰減系數(shù)；?為多步長路徑連線經(jīng)過的柵格信息素增量的權(quán)值，且0

(5)

(6)

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

2 安全距離判斷策略

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

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

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

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

y=kpathx-x1+y1

(7)

設(shè)直線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都大于δ，則說明該路徑滿足安全距離條件，路徑對應(yīng)的柵格可作為下一步移動的候選柵格。若路徑中有一處li小于δ，則該路徑不滿足安全距離條件。該路徑不可行。

3 算法流程

本文算法流程如下：

步驟1 采用柵格法進行地圖構(gòu)建，將機器人初始位置設(shè)為起始點，設(shè)置目標(biāo)點，各項參數(shù)初始化。

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

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

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

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

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

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

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

圖4 本文算法流程

4 仿真實驗及結(jié)果分析

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

4.1 仿真結(jié)果分析

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

圖5 路徑長度與迭代次數(shù)的關(guān)系

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

圖6 路徑比較結(jié)果

表1 算法仿真結(jié)果對比

4.2 路徑多樣性分析

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

(9)

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

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

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

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

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

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

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

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

5 結(jié)束語

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