基于改進勢場與蟻群算法的機器人路徑規劃法

劉 瀛

(遼東學院化工與機械學院，遼寧 丹東 118000)

1 引言

路徑規劃是機器人控制領域研究的重要方向和組成部分[1]，是指為移動機器人在已知或未知障礙環境下從起點規劃一條至目標點的能耗與時間最少的最優安全無碰撞路徑，目前，國內外在這一方面的諸多研究已取得顯著成果，如Dijkstra算法、人工勢場算法[2]、A*算法[3]、蟻群算法[4]及神經網絡算法等。

蟻群算法具有較強的魯棒性，采用正反饋機制和并行分布式計算，更適于障礙環境的路徑規劃，但傳統蟻群算法搜索效率較低，且易陷入局部最優[4]。人工勢場算法(Artificial Potential Field，APF)原理簡單且計算量小，具有較快的路徑規劃效率，但復雜障礙物環境下，易出現死鎖和目標不可達現象[2]，為此，近年來，研究人員綜合了勢場和蟻群兩種算法優點，提出基于人工勢場和蟻群算法的聯合算法[5]，但由于勢場與蟻群算法自身問題影響，勢場蟻群算法仍存在計算量大、收斂慢和最優解性能較差等問題[6]。張強[7]等以有效障礙物識別和邊界中間點設置改進APF，并以改進的APF優化蟻群初始路徑篩選，緩解初始路徑交叉影響收斂速度問題，以負反饋通道構建全局和局部信息素的自適應更新，保證算法的收斂速度與全局搜索能力的統一；王欽釗[8]等以勢場法得到的預規劃信息構建信息素矩陣優化蟻群初始路徑搜索，以自適應偽隨機轉移函數和代價函數平滑信息素更新過程，得到復雜障礙環境下的最優路徑規劃，新算法具有較好的收斂速度和全局尋優能力[9]；LIU等[10]以改進的APF算法優化蟻群算法的初始信息素設置，提高螞蟻在路徑搜索初期的方向性；Park等[11]以改進后的APF合力優化啟必信息和揮發因子的設置，并聯合APF目標距離避免算法陷入停滯；李憲強等[12]將APF算法引入到初始信息素分配中，避免迭代初期信息素與啟發信息不匹配而造成的路徑趨向于強啟發信息一側，其次以APF引導函數優化蟻群狀態轉移函數，避免盲目選擇影響算法的收斂速度。陳余慶等[13]將改進的APF引力和斥力附加到蟻群路徑尋徑過程的信息素增量計算中，提高了蟻群算法的全局路徑尋優的效率；王曉燕等[14]將APF初始預規劃路徑及不均衡分配策略附加到蟻群算法的啟發信息和初始信息素中，改善了蟻群算法的全局搜索效率。

在已有研究基礎上，提出了基于改進的勢場與蟻群算法的機器人路徑規劃算法。算法首先利用斥力距離值引入和增設虛擬子目標點改進經典人工勢場算法，以優化其目標不可達和局部欠優問題，并通過過濾振蕩點平滑最優路徑，其次，在改進勢場優化蟻群初始路徑篩選基礎上，通過揮發因子的自適應設置與全局搜索策略的改進，提高蟻群算法對不同復雜障礙環境的適應性和全局最優路徑的有效性。實驗結果驗證了該算法的有效性。

2 改進APF局部路徑規劃

2.1 運動環境模型構建

運動環境的合理建模是實現機器人行進路徑精確規劃的基礎，為此，將環境建模為二維平面，而移動機器人則為平面內的點狀移動物體。設環境柵格為M={Mi，i=0，1，2，3}，則柵格值i=0，1，2，3分別描述了起始位置、無障礙區域、障礙區域和目標位置，如圖1所示為環境建模結果。

圖1 算法分析采用的環境模型

環境模型使用的柵格大小影響路徑規劃的精確性，較大的柵格值可減少計算量，但路徑較為粗糙，且長度會增加，而過小的柵格尺寸，可以提高路徑的精細度，但計算過程緩慢，算法的實時性較差，為此，實驗中對環境進行建模時，通過需要根據實際環境情況確定建模柵格尺寸，即

(1)

式中，r與R分別為障礙物和機器人建模后的等效尺寸半徑，δ為預設的距離安全值。劃的準確度就會提高，但規劃過程緩慢。根據圖1和式(1)建模后，機器人路徑規劃可描述為在柵格化環境模型中搜索一條連續最短的無障礙路徑。

2.2 傳統APF算法

APF算法為基于運行環境中的斥力與引力合力作用的一種人工力場虛擬力法[9]，運動聲中的障礙物對機器人產生斥力作者，而規劃路徑的最終目標點則對機器人產生引力作用，兩種人工力場的共同牽引作用下，機器人以最優路徑到過目標點。

根據圖1所示，設機器人與目標點的位置分別為M1=X=[x，y]和M3=XM=[xm，ym]，則APF的引力為[9]：

(2)

(3)

式中，k0>0為斥力增益系數，ξ為機器人與障礙物之間的距離，ξ0為障礙物作用范圍，ξ≤ξ0，則運動環境中，機器人的受力合力為

F(X)=FG(X)+F0(X)

(4)

傳統APF算法通常存在局部最小值、引力斥力不足及目標不可達等問題，為此，提出改進的APF算法。

2.2 改進APF算法

針對傳統APF存在的目標不可達局限，根據已有文獻[11]研究，對傳統APF算法的斥力中引入距離值，同時約束合力勢場的全局極小值點為路徑目標點，以使機器人穩定停止在目標位置，優化后的斥力勢場函數為

(5)

式中，ρo為目標點附近障礙物的影響范圍值，ρ(X，Xo)為某一時刻機器與障礙物之間的歐氏距離，當ρ(X，Xo)≤ρo時，上式成立，反之有Urep(X)=0，ρ(X，Xg)不某一時刻機器人與目標點之間的歐氏距離，n為某正的調節常數。對式(5)所示斥力勢場函數求負梯度，則得到改進算法的斥力計算式為

Frep(X)=Frep1(X)-Frep2(X)

(6)

式中，Frep1(X)和Frep2(X)的計算式為

(7)

(8)

從式(7)和式(8)計算式可以看出，式(6)中的Frep1(X)和Frep2(X)分別描述了障礙物指向機器人的斥力和機器人指向目標點的引力，這樣，當機器人向目標移動時，Frep1(X)減少至0，而當0

(9)

式中，m為目標點附件的存在有效斥力的障礙物數。設F與坐標x軸夾角為θ，k為路徑序號點，則機器人的下一路徑點(xk+1，yk+1)為

(10)

式中，l為機器人運動步長。

針對傳統APF算法易陷入局部極小值問題，文中采用增設虛擬子目標的改進方法，通過虛擬外力引入，使陷入局部極小值點的機器人擺脫陷阱，抵達目標點。

首先以機器人移動的連續5個步長作為其是否陷入局部最小的判斷依據，當連續步長各小于βl，β∈[2，4]時，通過調節β值，可較快的檢測機器人是否陷入局部最小。當機器人判斷出已經陷入局部最小時，存儲相關障礙物信息，并將這些障礙物視為一個障礙物群體，然后以障礙物群與目標之間的連線為中心，以障礙物較多的一側作為有效障礙物群，按式(11)計算虛擬子目標位置，即

(11)

式中，(x，y)和(xg，yg)為機器人當前位置和目標位置，(xb，yb)為有效障礙物群位置，β1>0和β2>0為調節系數。根據式(11)機器人跳出局部極小值益后，撤回虛擬子目標，機器在原合力作用下斷續進行路徑規劃。

2.3 路徑震蕩點過濾

由于步長與障礙物作用距離的不匹配及解算過程的離散化，使得APF算法難以避免路徑振蕩點，即使使用虛擬子目標方法也只是減少震蕩次數，而無法避免，為此，文中對震蕩路徑點進行過濾，以進一步平滑最優路徑。

設k時刻路徑點為qk，當與qk-2路徑點之間的距離滿足ρ(qk，qk-2)l時，記為震蕩結束點qe=qk-1，由qb到qe組成震蕩簇。

當qe與qb位于簇異側時，有ρ(qe，qb)>l，此時，將直線L(qb，qe)等分后獲得n+1個新路徑點，即

(12)

式中，q1=qb，qn+1=qe，n=〖ρ(〗qe，qb)/l〗，〖·〗〗為取整數操作。

當qe與qb位于簇同側時，根據震蕩點形成原因，此時的震蕩突變點位于障礙物一側，當ρ(qe，qb)

S=(K-1)×l+∑md+∑ms

(13)

式中，K為未過濾總路徑點數，md和ms的計算式為md=ρ(qe，qb)-n×l，ms=l-ρ(qe，qb)。

3 改進蟻群全局規劃算法

3.1 經典蟻群算法

(14)

式中，Ak為螞蟻的下一可選節點集，ηij為節點邊權重的倒數，α和β為信息素與ηij關系的調節參數，q0∈(0，1)為給定的識別參數，q∈(0，1)為某隨機變量，其服務均勻分布。

信息素的局部更新規則為

τij(t+n)=(1-ρ)τij(t)+ρΔτij(t)

(15)

式中，ρ為揮發系數，Δτij(t)為螞蟻經過邊(i，j)后的信息素增加量，Δτij(t)=Q/Lk，Q為某給定值，Lk為當前迭代路徑長路。

信息素全局更新規則用于對當次迭代所有螞蟻完成路徑搜索后的整體信息素優化更新，以增加優質解對下一路徑迭代的貢獻度，提高算法收斂速度，其計算式為

τij(t+1)=(1-ρ)τij(t)+ρΔτij(t)

(16)

其中，當邊(i，j)位于當次迭代的最優路徑時，Δτij(t)=Q/Lbs，否則Δτij(t)=0。

可以看出，全局更新策略加強了節點的信息素濃度，有利于算法的快速收斂而局部更新策略則削弱濃度，以減少重復訪問，增強算法的全局尋優性能。經典蟻群算法在迭代初期，受ηij(t)影響，易限入局部最優，而迭代后期，局部信息素更新增強全局搜索能力，但影響算法的收斂性，為此，對經典蟻群算法進行了改進，以適于機器人全局路徑規劃。

3.2 揮發因子ρ的自適應設置

揮發因子ρ直接影像不同路徑的信息素濃度，進而影響最優路徑的質量，較大的ρ值可提高信息素更新效率，從而較快的實現算法的收斂，找到最優解，但較快的迭代收斂會導致邊間信息素濃度差異增大，而使算法陷入局部最優；另一方面，較小的ρ值可提高算法的全局搜索性能，但不明顯的信息素更新效率，影響了算法的收斂速度，為此，文中提出揮發因子ρ的自適應設置算法。

自適應設置基于算法的迭代次數，當最優解在若干次迭代后，仍無明顯改變時，ρ的取值計算式為

(17)

式中，ie與iemax分別為當前迭代次與最大迭代次數，λ為調節因子。根據式(17)，算法在初始迭代時，ρ值較朋以增加算法的收斂速度；而當算法趨于平衡，最優解變化較小時，ρ值將根據式(17)隨迭代次數進行動態調整，以增加解空間范圍，避免可能存在的局部最優解。

3.3 狀態轉移函數的改進

經典蟻群算法中對初始信息素濃度采用均勻分配的方式，導致初期搜索的盲目性和較慢的收斂速度，為此，文中首先以改進的APF確定一個較高的信息素濃度初值，然后以虛擬合力確定下一個路徑點方向，從而提高規劃方向的準確性。加入改進APR在狀態轉移函數為

(18)

3.3 信息素更新改進

信息素濃度在算法的不同迭瓦片階段要求不同，經典蟻群算法采用固定的信息素濃度更新策略，影響路徑規劃的最優解。同時，經典算法中全局信息素更新需要完成當次迭代所有螞蟻的路徑規劃后，結合最優解進行全局信息素優化調整，這將導致局部較優解的信息素濃度得到反而被加強，為此，在全局信息素更新策略中增加tanh函數為模型的動態因子σ，提出一種自適應信息素更新算法。

由神經網絡模型的相關知識可知，其激勵函數f(x)=(ex-e-x)/(ex+e-x)為輸出為[-1，1]內的tanh連續函數，經過函數值調整后，可將函數輸出取值調整為[0，1]，即

(19)

這樣，當次迭代完成后，全局信息素更新規則修改為

τij(t+n)=τij(t)+μσΔτij(t)

(20)

從圖2函數圖像可以看出中，當局部權值和與其路徑均值相差越小，調節因子越接近于1，反之則直接于零，說明路徑中局部最優解較小時，該路徑全局信息素優化時獲得的濃度增加就越多。另外，當x∈[-2，2]時，σ具有對局部解的高敏感性，此時，各迭代次數下獲得的最優路徑的信息素更新效果會因較小的局部解而表現出較大的差異性，從而加快全局最優路徑的篩選，當x>2時，σ的變化較為緩慢，且趨近于1，有利于消除局部最優解信息素濃度的過量增加對最終最優解的影響。

圖2 調節因子σ的函數圖像

根據上述對經典蟻群算法中揮發因子、轉換概率函數和信息素更新策略的改進，文中改進蟻群算法進行全局路徑規劃計算流程如圖3所示。

圖3 改進蟻群算法全局路徑規劃流程

4 仿真驗證

為驗證文中基于改進人工勢場和蟻群算法的聯合最優路徑規劃算法的有效性，以不同復雜程度和障礙物環境作為仿真環境，采用Matlab R2016a開發實驗算法，計算機環境為：Windows10，Intel? Xeon? W-10855M @ 2.80G Hz CPU，32G 內存，采用已有改進勢場蟻群(簡記為Imp-ACPF)算法[7]及零點優化的自適應勢場蟻群算法(記為ZaA-ACPF算法)[14]作為實驗比較算法，主要參數設置為蟻群數為30，最大迭代次數設置為70，信息素初始值為0.0003，α=2、β=6、ξ=0.2、ε=0.5。

4.1 路徑規劃結果比較

圖4所示為實驗所用三種算法的多次路徑規劃實驗結果的平均值，圖5為相應的的收斂曲線均值。

圖4 簡單實驗環境實驗結果

圖5 各算法收束結果

由圖4和圖5實驗結果可以看出，ZaA-ACPF算法的迭代搜索陷入局部最優，主要是其在中間區域第一次避障時，受啟發信息誤導，其規劃路徑選擇了歐氏距離最小的右邊節點，并在后續迭代收斂到該局部解；Imp-ACPF算法與文中算法對蟻群啟發信息進行了改進化，引導螞蟻以較優的初始路徑進行迭代，從而有效提高算法的收斂速度和避免局部最優，但Imp-ACPF算法僅采用改進人工勢場優化啟發信息，其初始路徑得到優化，但仍與最優路徑存在一定的差別，從而其后續的收斂速度受到影響；文中算法在改進人工勢場優化算法初始路徑準確性基礎上，通過揮發因子自適應設置和全局搜索方法的優化改進，進一步提高了算法對各種障礙物環境的適應性，有利于收斂速度的優化，因而在收斂速率和最優路徑均取得較優的實驗結果。

表1所示為3種算法的仿真結果，可以看出，文中算法取得最高的收斂速度，其運行時間為0.735 s。

表1 仿真結果

4.2 算法性能比較實驗

由于Imp-ACPF算法與文中算法的最優路徑搜索結果相近，為進一步分析文中算法的性能優勢，將文中算法與Imp-ACPF算法分別應用于不同柵格尺寸的實驗環境，分析兩種算法的最優路徑和收斂時間，其結果如表2所示，表中，實驗環境隨著柵格尺寸的增加，障礙物也變得更加復雜，且實驗結果為多次實驗結果的平均值。從表中實驗結果可以看出，在環境柵格尺寸較小時，文中算法與Imp-ACPF算法的最優路徑長度及算法的平均收斂時間相差不大，但隨著柵格尺寸增加及環境復雜性增加，文中算法的最優路徑及收斂時間優勢逐漸突出，主要因為文中算法通過增加斥力距離值和增設虛擬子目標點優化人工勢場的避障性能，通過振蕩點消除平滑最優路徑，通過揮發因子的自適應設置與全局搜索的改進，優化蟻群算法對不同復雜障礙環境的適應性。

表2 不同柵格尺寸實驗結果

如圖6所示為兩種算法在全局蟻群搜索時的所有中間路徑，從實驗結果可以看出，兩種算法的中間路徑具有較好的全局覆蓋性能，從而保證了搜索到全局最優路徑的準確率，進一步分析根據式(21)兩種算法的環境覆蓋率φ量化指標，可得兩種算法的φ值分別為70.95%和72.59%，說明文中算法具有更好的全局尋優性能。即

圖6 兩種算法的路徑覆蓋實驗結果

(21)

式中，Npass為算法中間路徑柵格數，Nfree為所有可行路徑柵格數。

5 總結

針對經典勢場與蟻群算法聯合機器人路徑規劃算法，易陷入局部最優、路徑震蕩且對不同復雜障礙物環境適應性差等等問題，提出了基于改進的勢場與蟻群算法的聯合機器人路徑規劃算法。算法在地圖環境柵格化基礎上，首先利用斥力距離值引入和增設虛擬子目標點改進經典人工勢場算法，以優化其目標不可達和局部欠優問題，其次，在改進勢場優化蟻群初始路徑篩選基礎上，通過揮發因子的自適應設置與全局搜索策略的改進，提高蟻群算法對不同復雜障礙環境的適應性和全局最優路徑的有效性。實驗結果表明，所提算法能夠有效避免經典算法的局部最優問題，路徑較為平滑，對不同障礙物環境具有較好的適應性，全局搜索能力和收斂速度優于實驗采用的已有算法，從而驗證了該算法的有效性。