基于改進(jìn)人工魚(yú)群算法的移動(dòng)機(jī)器人路徑規(guī)劃

江 巖 黃宜慶

（1.安徽工程大學(xué)電氣工程學(xué)院 安徽省電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室；2.高端裝備先進(jìn)感知與智能控制教育部重點(diǎn)實(shí)驗(yàn)室 安徽蕪湖 241000）

路徑規(guī)劃是移動(dòng)機(jī)器人導(dǎo)航的關(guān)鍵技術(shù)，具有十分重要的研究意義。機(jī)器人路徑規(guī)劃是指在預(yù)先設(shè)定的條件下，從起點(diǎn)到終點(diǎn)的路徑當(dāng)中躲避障礙物尋找到一條最短的路徑[1]。根據(jù)環(huán)境信息是否已知，可將移動(dòng)機(jī)器人路徑規(guī)劃問(wèn)題分為兩類(lèi)：全局路徑規(guī)劃和局部路徑規(guī)劃[2]，前者為環(huán)境信息完全已知，后者為環(huán)境信息未知或者部分已知。因此，在局部路徑規(guī)劃過(guò)程中需要通過(guò)傳感器對(duì)環(huán)境信息進(jìn)行感知。

傳統(tǒng)的路徑規(guī)劃方法主要有人工勢(shì)場(chǎng)法[3]、可視圖法[4]、自由空間法等，但均存在不足之處。其中，人工勢(shì)場(chǎng)法結(jié)構(gòu)比較簡(jiǎn)單，但在全局路徑規(guī)劃時(shí)容易陷入局部最優(yōu)解的問(wèn)題；可視圖法的路徑規(guī)劃雖然能夠獲得最短路徑但效率比較低；自由空間法比較靈活，起點(diǎn)和終點(diǎn)的位置改變不會(huì)重構(gòu)可視圖，但是在障礙物較多時(shí)有時(shí)無(wú)法獲得最短路徑。隨著研究的進(jìn)一步深入，群智能算法成為目前機(jī)器人路徑規(guī)劃的熱門(mén)方法。主要包括人工魚(yú)群算法[5]、蟻群算法[6]、粒子群算法等。文獻(xiàn)[1]采用連接圖法進(jìn)行環(huán)境建模，得到起點(diǎn)和終點(diǎn)的網(wǎng)絡(luò)有權(quán)圖，再利用Dijkstra算法得到初始的路徑，最后利用遺傳算法優(yōu)化得到的路徑。文獻(xiàn)[2]將擾動(dòng)機(jī)制加入粒子群算法當(dāng)中，采用個(gè)體修正的方法代替進(jìn)入進(jìn)化停滯狀態(tài)的個(gè)體，從而引導(dǎo)算法搜索可行路徑，使算法避免陷入局部極值。文獻(xiàn)[3]分析了傳統(tǒng)人工勢(shì)場(chǎng)法路徑規(guī)劃時(shí)，由于局部最優(yōu)而導(dǎo)致路徑規(guī)劃失敗的原因。對(duì)于勢(shì)函數(shù)進(jìn)行改進(jìn)的基礎(chǔ)上又結(jié)合了構(gòu)造虛擬障礙物的方法，使得算法逃脫局部極小值點(diǎn)。文獻(xiàn)[4]針對(duì)靜態(tài)已知環(huán)境下的全局路徑規(guī)劃，在可視圖法建模的基礎(chǔ)上，利用遺傳算法進(jìn)行優(yōu)化，并加入了目標(biāo)導(dǎo)向啟發(fā)函數(shù)，使得算法的收斂時(shí)間得到改善。文獻(xiàn)[5]針對(duì)移動(dòng)機(jī)器人的全局路徑規(guī)劃問(wèn)題，提出了一系列的改進(jìn)人工魚(yú)群算法策略，引入多策略混合機(jī)制，實(shí)現(xiàn)了對(duì)于傳統(tǒng)人工魚(yú)群算法的改進(jìn)。文獻(xiàn)[6]對(duì)于傳統(tǒng)的蟻群算法進(jìn)行改進(jìn)，提出了更新優(yōu)質(zhì)螞蟻信息素的原則，對(duì)揮發(fā)系數(shù)進(jìn)行動(dòng)態(tài)調(diào)整，并對(duì)路徑進(jìn)行二次規(guī)劃，降低機(jī)器人運(yùn)行時(shí)間和能量消耗。

一、基本人工魚(yú)群算法行為

基本的人工魚(yú)群算法[7]可以描述為：人工魚(yú)個(gè)體的狀態(tài)可以表示為向量X=(x1,x2,…,xn)，其中xi(i=1,2,…,n)為欲尋優(yōu)的每條人工魚(yú)，種群大小為N；Y=f(x)為當(dāng)前人工魚(yú)所在位置的食物濃度（Y為目標(biāo)函數(shù)值）；每條人工魚(yú)都試圖通過(guò)覓食行為、群聚行為、追尾行為、和隨機(jī)行為來(lái)找到最合適的位置來(lái)滿足它們的食物要求。四種基本行為概述如下[8]：

（一）覓食行為。這是魚(yú)群趨向食物的一種活動(dòng)，設(shè)當(dāng)前人工魚(yú)狀態(tài)為為其視野范圍內(nèi)的隨機(jī)狀態(tài)，的下一個(gè)時(shí)刻位置。如果食物濃度為則人工魚(yú)按照式（1）向的方向移動(dòng)一步。否則再次隨機(jī)選擇狀態(tài)并判斷是否滿足上述條件。如果重復(fù)trynum次后，仍不滿足前進(jìn)條件，則按照（2）式隨機(jī)移動(dòng)一步。綜上所述，覓食行為可以表示為：

（三）追尾行為。當(dāng)某一條魚(yú)或者幾條魚(yú)發(fā)現(xiàn)食物時(shí)，它的附近的魚(yú)會(huì)尾隨而來(lái)，進(jìn)而導(dǎo)致更遠(yuǎn)處的魚(yú)也會(huì)尾隨過(guò)來(lái)。設(shè)當(dāng)前的人工魚(yú)狀態(tài)為Xi，有NF條人工魚(yú)位于其視野范圍內(nèi)，其中Xj為最優(yōu)的個(gè)體對(duì)應(yīng)的狀態(tài)，若滿足Yj/NF>δYi,則按照（4）式向Xj方向移動(dòng)一步，否則繼續(xù)執(zhí)行覓食行為。

（四）隨機(jī)行為。當(dāng)人工魚(yú)未能找到較優(yōu)位置時(shí)，在其視野范圍內(nèi)隨機(jī)選擇一個(gè)狀態(tài)或位置，然后游向所選擇的位置。如（2）式所示，隨機(jī)行為是覓食行為的缺省行為，它增加樣本的多樣性，可在一定程度上避免陷入局部極值。

二、柵格環(huán)境下的人工魚(yú)群算法

采用柵格法建模，將機(jī)器人工作環(huán)境由三維環(huán)境轉(zhuǎn)換成二維環(huán)境，然后按照一定大小的柵格對(duì)二維環(huán)境進(jìn)行劃分。我們以xoy 平面建立坐標(biāo)系，規(guī)定黑色柵格表示障礙物環(huán)境，白色柵格表示機(jī)器人可以移動(dòng)的區(qū)域。劃分后的工作空間如下圖所示，柵格左上角為機(jī)器人移動(dòng)的起點(diǎn)，右下角為終點(diǎn)。

圖1 機(jī)器人工作環(huán)境柵格模型

柵格法相關(guān)基本定義。

定義1 機(jī)器人只可以在相鄰的柵格移動(dòng)，并且不可以越過(guò)障礙物，也不能碰撞障礙物，因此機(jī)器人的移動(dòng)步長(zhǎng)為

定義4:人工魚(yú)pi的視野其中R為視野區(qū)域半徑，設(shè)為常數(shù)，S表示整個(gè)區(qū)域。

定義5:定義δ為擁擠度因子，其取值范圍為（0,1）。

定義6:每條人工魚(yú)的下一時(shí)刻的隨機(jī)行，只可以發(fā)生在當(dāng)前人工魚(yú)pi的可行域(allow_area) 內(nèi), 且pi,pf∈Allow,pi,pf?Barrier，其中Allow表示機(jī)器人的可移動(dòng)區(qū)域，Barrier表示區(qū)域中的障礙物，pf表示可行域當(dāng)中的位置。

三、改進(jìn)人工魚(yú)群算法

（一）加權(quán)自適應(yīng)視野范圍和步長(zhǎng)。在傳統(tǒng)的AFSA中，人工魚(yú)的視野范圍被設(shè)為固定的常量，所以在局部極值附近的人工魚(yú)無(wú)法看到全局極值，從而使得算法后期的收斂速度變慢，并且不容易擺脫局部極值。因此提出了基于加權(quán)自適應(yīng)視野范圍和步長(zhǎng)的方法，在算法的不同階段自適應(yīng)調(diào)整視野范圍和移動(dòng)步長(zhǎng)，保證算法的收斂速度并防止陷入局部最優(yōu)。具體如下：以有N條人工魚(yú)為例，人工魚(yú)記為記Ki(i=1,2,…,N)第Ki條人工魚(yú)與j條人工魚(yú)之間的距離為Ri,j,定義距離權(quán)值為ωi,j，人工魚(yú)Ki的視野范圍Vi可以由加權(quán)平均[9]方法得到，具體為：

在算法進(jìn)行的初期，人工魚(yú)之間的距離較大，視野范圍也較大。隨著算法的進(jìn)行，人工魚(yú)都向著食物濃度高的地方移動(dòng)，使得魚(yú)群越來(lái)越聚集，個(gè)體之間的距離逐漸變小。由（7）式可以看出人工魚(yú)的視野范圍也在隨著算法的進(jìn)行逐漸變小，從而有利于提高算法的搜索效率。

步長(zhǎng)也是影響算法尋優(yōu)精度的一個(gè)非常重要的參數(shù)，步長(zhǎng)step越大，算法尋優(yōu)精度越低；步長(zhǎng)step越小，尋優(yōu)精度越高[10]。因此和加權(quán)自適應(yīng)的視野范圍的思想類(lèi)似，可在算法收斂的前期采用相對(duì)較大的移動(dòng)步長(zhǎng)，有利于加快算法前期的收斂速度，隨著算法的進(jìn)行，在算法后期采用較小的移動(dòng)步長(zhǎng)，可以提高算法尋優(yōu)精度。因此對(duì)步長(zhǎng)Step引入了一個(gè)權(quán)值如（8）式所示：

式中t為當(dāng)前時(shí)刻的迭代次數(shù)，TotalIter為總的迭代次數(shù)。從（8）式可以看出，算法在不同的階段，權(quán)值會(huì)相應(yīng)的調(diào)整，進(jìn)而調(diào)整移動(dòng)步長(zhǎng)。

（二）路徑優(yōu)化算子。針對(duì)路徑規(guī)劃的軌跡中存在的路徑冗長(zhǎng)的問(wèn)題，給出了一種路徑優(yōu)化算子來(lái)優(yōu)化得到的軌跡。設(shè)p(kx,ky)為第k 時(shí)刻求解的最優(yōu)魚(yú)群位置坐標(biāo)，n為柵格地圖矩陣的維數(shù)，kt 為已經(jīng)優(yōu)化的解的個(gè)數(shù)。goal(x,y) 是目標(biāo)點(diǎn)的位置坐標(biāo)。優(yōu)化算子R(t)可以描述為[11]：

（三）改進(jìn)人工魚(yú)群算法的實(shí)現(xiàn)步驟。

Step1.初始化人工魚(yú)群參數(shù)，包括魚(yú)群規(guī)模N、人工魚(yú)移動(dòng)步長(zhǎng)step、視野visual擁擠度因子δ以及最大迭代次數(shù)等。

Step2.公告板初始化，計(jì)算出每條人工魚(yú)的適應(yīng)度值，通過(guò)比較將最優(yōu)的個(gè)體的值及其位置信息在公告板當(dāng)中記錄下來(lái)。

Step3.參數(shù)調(diào)整，根據(jù)式（7）、（8）計(jì)算視野和步長(zhǎng)。

Step4.人工魚(yú)執(zhí)行覓食行為、群聚行為、追尾行為和隨機(jī)行為并更新位置。

Step5.路徑優(yōu)化，根據(jù)(9)、（10）、（11）式對(duì)算法獲得的路徑進(jìn)行優(yōu)化。

Step6.檢查終止條件。若滿足終止條件，則終止迭代過(guò)程并輸出最優(yōu)解，否則返回到step3。

四、仿真驗(yàn)證

使用柵格法分別建立（10*10）和（20*20）的環(huán)境，障礙物覆蓋率分別為20%和35%。設(shè)定參數(shù)如下：魚(yú)群規(guī)模N=50，視野和擁擠度因子的初始值分別為visual=10,δ=0.618，蟻群算法中螞蟻的個(gè)數(shù)Nant=50。由圖2、圖3和圖4的對(duì)比可以看出（10*10）柵格環(huán)境下，改進(jìn)后算法規(guī)劃的路徑明顯優(yōu)于傳統(tǒng)魚(yú)群算法和蟻群算法，并且解決了傳統(tǒng)魚(yú)群算法和蟻群算法存在的局部路徑冗長(zhǎng)現(xiàn)象。從表1 的數(shù)據(jù)可以看出，改進(jìn)后算法的最優(yōu)路徑比傳統(tǒng)算法優(yōu)化了9.23%，比蟻群算法規(guī)劃的路徑優(yōu)化了12.58%。

圖2 10＊10傳統(tǒng)AFSA規(guī)劃結(jié)果

圖3 10＊10改進(jìn)AFSA規(guī)劃結(jié)果

圖4 10＊10蟻群算法規(guī)劃結(jié)果

圖5 20＊20蟻群算法規(guī)劃結(jié)果

圖6 20＊20傳統(tǒng)AFSA規(guī)劃結(jié)果

在（20*20）柵格環(huán)境下，如圖6所示，傳統(tǒng)人工魚(yú)群算法雖然可以規(guī)劃出路徑但是仍然存在路徑冗長(zhǎng)現(xiàn)象。同樣，蟻群算法規(guī)劃出的路徑也存在這種現(xiàn)象，并且用時(shí)較長(zhǎng)。改進(jìn)后的人工魚(yú)群算法在最優(yōu)路徑長(zhǎng)度上相比傳統(tǒng)人工魚(yú)群算法優(yōu)化了9.11%，比蟻群算法優(yōu)化了8.1%。這些數(shù)據(jù)充分表明改進(jìn)人工魚(yú)群算法在移動(dòng)機(jī)器人路徑規(guī)劃中的優(yōu)越性。

圖7 20＊20改進(jìn)AFSA規(guī)劃結(jié)果

表1 柵格環(huán)境算法比較

五、結(jié)論

針對(duì)傳統(tǒng)人工魚(yú)群算法收斂速度慢、尋優(yōu)精度低、易陷入局部最優(yōu)等不足進(jìn)行改進(jìn)，設(shè)計(jì)了加權(quán)自適應(yīng)的視野范圍和步長(zhǎng)，使得算法在前期進(jìn)行快速的大范圍搜索，后期則著重最優(yōu)值的收斂，提高了算法的全局搜索能力和精度。針對(duì)算法規(guī)劃的局部路徑存在冗長(zhǎng)的現(xiàn)象，采用了路徑優(yōu)化算子策略，對(duì)于規(guī)劃后的曲線進(jìn)行進(jìn)一步優(yōu)化，以消除局部路徑冗長(zhǎng)的現(xiàn)象。仿真結(jié)果表明，改進(jìn)算法在收斂速度以及尋優(yōu)精度上相比傳統(tǒng)算法和蟻群算法均有明顯的改善。