融合人工勢場蟻群算法的移動機器人路徑規劃

李志錕， 趙倩楠

(1.廣東科技學院機電工程學院，廣東 東莞 523000；2.高端裝備先進感知與智能控制教育部重點實驗室，安徽 蕪湖 241000)

0 引言

路徑規劃是移動機器人依賴智能算法從起點到目標位置規劃出一條最優路徑。智能算法的應用可有效提高移動機器人路徑規劃效率，目前已有大量智能算法應用于移動機器人的路徑規劃，如D*算法[1]、蟻群算法[2-6]、人工勢場法[7]、人工魚群算法[8-10]等。每種算法都有自己的優點和缺點，利用不同算法的優點進行融合，為解決移動機器人路徑規劃提供了新的方法，如人工勢場法與蟻群算法之間的結合、粒子群算法和A*算法之間的結合、模擬退火算法和改進人工魚群算法之間的結合等。蟻群算法是通過研究螞蟻覓食規律而得出的概率型算法，蟻群算法相比其他智能算法具有更強的魯棒性和正反饋性；分布式搜索方法使得蟻群算法擅長解決組合優化問題，信息素的分泌增強蟻群選擇局部最優路徑的概率，當全局非最優路徑上的信息素較多時，由于蟻群的正反饋性，將吸引越來越多的螞蟻選擇全局非最優路徑，降低了移動機器人路徑規劃能力；而蟻群算法和人工勢場算法的融合，能夠提高算法收斂速度以及縮短全局最優路徑長度。

人工勢場法[11]通過設計一種人造力干預移動機器人的運動，有利區域對移動機器人的吸引作用，障礙物及不利區域對移動機器人的排斥作用，最后通過合力來引導移動機器人完成最優路徑搜索，利用人工勢場法規劃出的路徑基本可以避免死鎖現象。作為分布式算法，傳統蟻群算法雖然有足夠強的魯棒性，但是收斂速度較慢，且利用蟻群算法[5，12-13]搜索出的最優路徑較長，甚至可能陷入局部最優。為解決傳統蟻群算法存在的這些問題，本文融合人工勢場算法，對傳統蟻群算法進行改進，構造勢場力啟發函數，同時改進信息素更新策略，融合人工勢場法的蟻群算法[14-17]相對傳統蟻群算法，具有較強的全局路徑搜索能力，能較快地完成起始點到終點的路徑規劃。

1 人工勢場法的基本原理

人工勢場法的基本原理為在移動機器人路徑規劃區域內引入模擬的勢力場。當移動機器人從起點向終點移動時，受到模擬的吸引力或排斥力的作用，影響移動機器人路徑的選擇。如圖1所示，終點產生一個吸引力，吸引移動機器人朝著終點移動，阻礙移動機器人通過的障礙物產生排斥力，避免移動機器人碰撞障礙物，多個排斥力和引力的綜合作用下，移動機器人將沿著合力方向移動。移動機器人受到的排斥力的大小和移動機器人與障礙物之間的距離有關，距離越長，則移動機器人受到障礙物的排斥力就越小；同理，終點對移動機器人引力的大小和移動機器人與終點的距離成正比，距離越長，則終點對移動機器人的引力就越大。移動機器人在搜索路徑時，目標點對它的引力保證移動機器人朝著終點方向移動，而障礙物對它的排斥力則避免移動機器人與障礙物發生碰撞。

圖1 人工勢場法路徑規劃圖Fig.1 Path planning diagram of artificial potential field method

設移動機器人在二維空間中的當前位置X坐標為(x,y)，目標點Xd坐標為(xd,yd)，當移動機器人在模擬的勢力場內移動時，引力勢場函數為

(1)

式中,kp表示位置增益系數。引力勢場對移動機器人產生的引力為引力勢能的負梯度方向，即

Fa=-kp·[(x-xd),(y-yd)]。

(2)

由式(2)可以看出，移動機器人距離終點越近，終點對移動機器人的引力則越小。定義斥力場函數為

(3)

式中：m為斥力場的增益系數；P0為障礙物的影響范圍；ρ(X,X0)為移動機器人和障礙物之間的最短直線距離；當移動機器人到障礙物的直線距離小于P0，障礙物對移動機器人產生的斥力有效。斥力勢場對移動機器人產生的斥力為

Fr=-grad(Ur(X))

(4)

(5)

由式(5)可以看出，移動機器人距離障礙物越近，則產生的排斥力越大，因此，人工勢場作用在移動機器人上的合力Ft為

Ft=Fa+Fr。

(6)

移動機器人將在引力及斥力的合力作用下移動，并最終到達終點Xd。

2 融合人工勢場法的蟻群算法

2.1 改進斥力函數

當終點附近的障礙物較多時，由于移動機器人距離障礙物較近，受到障礙物的排斥力較大，而此時，移動機器人與目標點距離較小，目標點對移動機器人的吸引力也隨之減小，可能導致移動機器人無法到達目標點。改進后的斥力場函數為

(7)

λ=(X-Xd)n

(8)

其中，n為正整數。從式(8)可以看出，當移動機器人靠近目標點時，同時也在靠近與目標點距離較近的障礙物，目標點距離影響因子λ的值也隨之變小，有效避免移動機器人在目標點附近受到過大的斥力，相應的斥力算式為

(9)

(10)

(11)

其中，Fr1及Fr2兩個分力表示的方向不同，Fr1是由障礙物指向移動機器人的矢量，Fr2是由移動機器人指向終點的矢量。

對改進后的人工勢場算法進行仿真實驗，移動機器人的運動軌跡如圖2所示。圖3所示為對目標點附近放大顯示。圖2、圖3中，小圓圈代表障礙物，實心點組成了移動機器人路徑規劃軌跡，三角形代表目標點位置。圖2、圖3中坐標均無單位。移動機器人起始點坐標為(0,0),目標點坐標為(10,10)，移動機器人避開了所有的障礙物，斥力函數有利于移動機器人避開障礙物，但也可能導致移動機器人偏離目標點，因此，為了解決勢場力的負面影響，引入了目標點距離影響因子λ，動態調整移動機器人受到的斥力，改善勢場力對于移動機器人路徑搜索的影響。即便在目標點附近放置較多障礙物，改進人工勢場算法通過改進斥力場函數，避免移動機器人受到較大的斥力而無法規劃出最優路徑。

圖2 改進人工勢場算法機器人軌跡圖Fig.2 Robot trajectory diagram of the improved artificial potential field algorithm

圖3 軌跡放大圖Fig.3 The enlarged trajectory

2.2 構造勢場力啟發函數

傳統蟻群算法構造出的啟發函數僅考慮節點之間的距離，即

(12)

式中，di j表示當前節點位置i與下一步可到達節點j之間的歐氏距離，位于節點i的螞蟻k選擇節點j作為下一步要到達的位置。位置選擇概率為

(13)

式中：ak表示螞蟻k下一步可選擇到達位置的集合；τi j表示路徑(i,j)上的信息素值；ηi j表示啟發函數；α表示信息素啟發因子；β表示啟發函數ηi j的權重參數。di j的值越小，該啟發函數的值就越大，則節點i選擇節點j的概率就越大。

根據式(12)可知，傳統蟻群算法的啟發函數僅僅與移動機器人當前位置到下一節點位置的距離有關，當終點附近存在大量障礙物時，由于傳統蟻群算法的啟發函數考慮因素較少，可能使得傳統蟻群算法陷入局部最優，由于蟻群算法的正反饋性，當初代螞蟻未能尋找到最優路徑，同時蟻群分泌大量的信息素吸引其他螞蟻選擇該路徑，最終螞蟻搜索出的路徑可能較長，甚至規劃出錯誤的路徑。為了解決傳統蟻群算法存在的問題，通過人工勢場法搜索到的節點與終點的距離信息來改進啟發函數，在移動機器人路徑搜索的后期，為了避免勢場蟻群算法陷入局部最優，降低啟發函數對移動機器人路徑搜索的影響，引入啟發函數遞減參數φ，移動機器人越接近終點，啟發函數起的作用就越小。在考慮距離啟發信息之外，當移動機器人在路徑搜索時遇到復雜的障礙物環境，移動機器人要繞開障礙物，同時要搜索出最優路徑，因此，也要考慮勢場啟發信息，改進后的啟發函數為

(14)

式中：ηF(t)為勢場啟發函數；ηd(t)為距離啟發函數；φ為大于0小于1的常數。為了保證移動機器人搜索到的路徑足夠短，在移動機器人的可選節點中，移動機器人優先考慮距離目標點更近的節點，用移動機器人下一步要選擇的節點j到目標點e的距離dje替代傳統蟻群算法的di j，改進后的距離啟發信息為

(15)

勢場啟發函數為

ηF(t)=aFt·cos θ

(16)

式中：a是大于零的常數；θ為移動機器人在當前位置i指向下一個節點j的方向與勢場合力Ft方向的夾角。人工勢場力的存在，引導移動機器人避開障礙物，朝向終點移動，提高了算法的收斂速度，但是也更容易陷入局部最優解，融合人工勢場的蟻群算法引入勢場合力的衰減系數ξ，隨著融合人工勢場的蟻群算法不斷迭代，勢場合力逐漸衰減至零，則根據式(16)可知，勢場啟發函數ηF(t)趨向于數值1，勢場合力的衰減系數ξ為

(17)

式中：Nmax為最大迭代次數；Nk為當下迭代次數。因此，改進后的啟發函數為

(18)

式中，F1為改進后的勢場合力，其算式為

(19)

2.3 信息素更新策略

傳統蟻群算法在蟻群初始化時，分布均勻的信息素，移動機器人在路徑規劃前期，各柵格位置的信息素值差異不大，導致移動機器人盲目搜索。本文提出信息素不均勻分配方式，增加移動機器人起始點指向終點方向所在區域的信息素濃度，信息素濃度沿著垂直于移動機器人起始點到終點方向逐步衰減，如圖4所示。

圖4 初始信息素分配方式Fig.4 Initial pheromone distribution mode

圖4中，移動機器人起始點到目標點連接的直線函數表達式為

y=-x+C

(20)

式中，C為正整數。當該直線遇到障礙物時，障礙物所在的柵格位置信息素濃度值設置為零，初始信息素分布方式為

(21)

式中：T為初始信息素調整參數；坐標系中y=-x+C代表的直線穿過所有非障礙物柵格的初始信息素濃度值為T，初始信息素的差異化分配對于提高算法的收斂速度具有明顯作用。

當移動機器人完成從節點i到節點j的移動，需要對移動機器人行走過的路徑進行局部信息素更新，局部信息素更新規則為

τi j=(1-ρ)τi j+ω·τ0

(22)

式中：ω為參數，ω在(0,1)中取值；ρ表示信息素揮發系數，且ρ在(0,1)中取值；τ0表示信息素初始值。在路徑搜索過程中，移動機器人每經過路徑(i,j)時，均按照式(22)進行局部信息素更新，當移動機器人完成一次路徑迭代后，對所有路徑上的信息素做全局更新，螞蟻系統的全局信息素更新方式為

(23)

(24)

隨著算法迭代次數的增加，當信息素集中于非最優路徑時，由于蟻群算法的正反饋作用，移動機器人在路徑搜索時更傾向于選擇信息素多的路徑，最終，算法收斂于非最優解，為解決這一問題，降低信息素的干擾，引入信息素調節系數，改進后的全局信息素更新方式為

(25)

式中，e1表示參數。隨著Nk的增加，信息素調節系數逐漸減小，避免算法收斂于非最優解。

3 融合人工勢場蟻群算法的實現流程

融合人工勢場蟻群算法的實現步驟如下：

1) 初始化融合人工勢場蟻群算法的所有參數，建立柵格地圖環境，設置蟻群的起始點、終點，根據式(21)進行信息素不均勻分布；

2)m只螞蟻從起始點開始搜索路徑；

3) 將構造的勢場力啟發函數算式(18)、重新設計的信息素更新算式(25)代入狀態轉移概率算式(13)，選擇螞蟻要到達的節點位置；

4) 判斷所有螞蟻是否避開所有障礙物到達終點位置，若是，存儲本次迭代產生的最優路徑，若不是，返回3)；

5) 根據算式(13)對每條路徑更新信息素濃度值，根據算式(18)更新勢場力啟發函數；

6) 判斷是否完成所有迭代，如果達到最大迭代次數，存儲并輸出最優路徑，否則返回2)。

4 算法仿真實驗

4.1 實驗1：20 m×20 m仿真環境

在20 m×20 m柵格仿真環境下，對本文提出的融合人工勢場蟻群算法及文獻[14]算法分別進行仿真實驗。對比路徑軌跡如圖5所示。移動機器人起點和終點坐標分別為(0.5 m,19.5 m)，(19.5 m,0.5 m)。圖5(a)為應用融合人工勢場蟻群算法規劃出的最優路徑軌跡，最優路徑長度為30.38 m，移動機器人在搜索路徑時轉折次數為12。圖5(b)為采用文獻[14]算法規劃出的最優路徑軌跡，最優路徑軌跡長度為30.38 m，移動機器人在搜索路徑時轉折次數為14。顯然，采用融合人工勢場蟻群算法規劃出的最優路徑轉折次數更少，路徑更加平滑。

圖5 兩種算法的路徑軌跡(實驗1)Fig.5 Path trajectory diagram of two algorithms(Experiment 1)

圖6為采用人工勢場蟻群算法規劃出的各代最優路徑軌跡，可以看出，各代最優路徑均避開所有障礙物，且各代軌跡均集中于起點指向終點的直線附近，實驗數據如表1所示。

圖6 融合人工勢場蟻群算法各代最優路徑軌跡(實驗1)

表1 20 m×20 m柵格環境實驗結果對比

兩種算法收斂曲線如圖7所示。由圖7(a)可知,移動機器人在第6次搜索路徑時達到穩定狀態,由圖7(b)可知,移動機器人在第30次搜索路徑時達到穩定狀態。采用人工勢場蟻群算法在收斂速度方面比文獻[14]算法提高了80%，由于人工勢場蟻群算法采用全新的信息素不均勻分布策略，對距離啟發信息及勢場啟發信息同時改進，構造了全新的勢場力啟發函數。因此，人工勢場蟻群算法在迭代初期搜索出的最優路徑長度已經比較接近全局最優路徑長度；而文獻[14]算法在迭代初期振蕩較大，且收斂速度較慢，效率較低，不利于移動機器人路徑規劃。因此，采用人工勢場蟻群算法規劃路徑速度更快，搜索效率更高。

圖7 兩種算法的收斂曲線Fig.7 Convergence curves of two algorithms

4.2 實驗2：30 m×30 m仿真環境

為了驗證融合人工勢場蟻群算法在復雜環境下的路徑規劃能力，在30 m×30 m柵格仿真環境下，采用與文獻[15]同樣的柵格地圖進行仿真實驗及對比。移動機器人起點和終點坐標分別為(0.5 m,29.5 m)和(29.5 m,0.5 m)。圖8為移動機器人采用融合人工勢場蟻群算法規劃出的最優路徑軌跡，路徑長度為43.355 3 m，最優路徑轉折次數為9;同一幅柵格地圖下，文獻[15]規劃出的最優路徑長度為44.526 9 m，最優路徑轉折次數為12。在最優路徑長度方面，本文提出的融合人工勢場蟻群算法相比文獻[15]算法縮短了2.6%。在路徑平滑度方面，本文算法相比文獻[15]算法縮短了25%。圖9為融合人工勢場蟻群算法的收斂曲線，可以看出，第1代蟻群搜索出的最優路徑長度為62.669 0 m,第2代蟻群搜索出的最優路徑長度為49.598 0 m,第2代蟻群搜索出的最優路徑長度相比第1代蟻群搜索出的最優路徑長度大幅縮短，第3代蟻群搜索出的最優路徑長度為43.355 3 m,達到全局最優路徑長度。文獻[15]算法的收斂次數為9，相比文獻[15]算法，本文算法在收斂速度方面提高了66.7%，實驗數據如表2所示。體現了本文提出的融合人工勢場蟻群算法具有極強的路徑搜索能力。

圖8 融合人工勢場蟻群算法路徑軌跡(實驗2)

圖9 融合人工勢場蟻群算法收斂曲線

圖10為移動機器人采用融合人工勢場蟻群算法規劃出的各代最優路徑軌跡，可以看出，從起點到終點存在多條路徑實現最短路徑長度，各代最優路徑均集中于起點到終點方向的直線附近，表明本文算法的有效性及優越性。

圖10 融合人工勢場蟻群算法各代最優路徑軌跡(實驗2)

表2 30 m×30 m柵格環境實驗結果對比

5 結束語

本文首先介紹了人工勢場法的基本原理。為了解決勢場力的負面影響，引入目標點距離影響因子，通過改進斥力場函數，避免移動機器人受到較大斥力而無法實現路徑規劃；為了降低啟發函數對于移動機器人路徑搜索的影響，引入啟發函數遞減參數，同時兼顧勢場啟發信息及距離啟發信息的改進，來構造全新的勢場力啟發函數；為了避免融合勢場蟻群算法陷入局部最優解，構造勢場合力衰減系數，隨著算法的不斷迭代，勢場合力逐漸衰減至零；采用全新的信息素不均勻分配策略，引入信息素調節系數，改進全局信息素更新方式，促進移動機器人朝著最優路徑方向搜索，提高了移動機器人全局路徑搜索能力。