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

劉 瀛

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

1 引言

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

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

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

2 改進APF局部路徑規(guī)劃

2.1 運動環(huán)境模型構(gòu)建

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

圖1 算法分析采用的環(huán)境模型

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

(1)

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

2.2 傳統(tǒng)APF算法

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

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

(2)

(3)

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

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

(4)

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

2.2 改進APF算法

針對傳統(tǒng)APF存在的目標(biāo)不可達局限，根據(jù)已有文獻[11]研究，對傳統(tǒng)APF算法的斥力中引入距離值，同時約束合力勢場的全局極小值點為路徑目標(biāo)點，以使機器人穩(wěn)定停止在目標(biāo)位置，優(yōu)化后的斥力勢場函數(shù)為

(5)

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

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

(6)

式中，F(xiàn)rep1(X)和Frep2(X)的計算式為

(7)

(8)

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

(9)

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

(10)

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

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

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

(11)

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

2.3 路徑震蕩點過濾

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

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

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

(12)

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

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

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

(13)

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

3 改進蟻群全局規(guī)劃算法

3.1 經(jīng)典蟻群算法

(14)

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

信息素的局部更新規(guī)則為

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

(15)

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

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

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

(16)

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

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

3.2 揮發(fā)因子ρ的自適應(yīng)設(shè)置

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

自適應(yīng)設(shè)置基于算法的迭代次數(shù)，當(dāng)最優(yōu)解在若干次迭代后，仍無明顯改變時，ρ的取值計算式為

(17)

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

3.3 狀態(tài)轉(zhuǎn)移函數(shù)的改進

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

(18)

3.3 信息素更新改進

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

由神經(jīng)網(wǎng)絡(luò)模型的相關(guān)知識可知，其激勵函數(shù)f(x)=(ex-e-x)/(ex+e-x)為輸出為[-1，1]內(nèi)的tanh連續(xù)函數(shù)，經(jīng)過函數(shù)值調(diào)整后，可將函數(shù)輸出取值調(diào)整為[0，1]，即

(19)

這樣，當(dāng)次迭代完成后，全局信息素更新規(guī)則修改為

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

(20)

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

圖2 調(diào)節(jié)因子σ的函數(shù)圖像

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

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

4 仿真驗證

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

4.1 路徑規(guī)劃結(jié)果比較

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

圖4 簡單實驗環(huán)境實驗結(jié)果

圖5 各算法收束結(jié)果

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

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

表1 仿真結(jié)果

4.2 算法性能比較實驗

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

表2 不同柵格尺寸實驗結(jié)果

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

圖6 兩種算法的路徑覆蓋實驗結(jié)果

(21)

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

5 總結(jié)

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