基于勢場法的路徑規(guī)劃算法特性分析和路徑優(yōu)化

李積云,許亞軍,蒲衛(wèi)華，任 輝

(寶雞職業(yè)技術(shù)學(xué)院智能制造創(chuàng)新中心，陜西 寶雞 721013)

0 引言

路徑規(guī)劃算法和軌跡控制是移動機器人在特定環(huán)境中開展有效工作的關(guān)鍵因素[1]。路徑規(guī)劃算法的種類眾多，人工勢場法因算法簡單、易于編程驗算、實用型和安全性好，應(yīng)用較為廣泛。許多學(xué)者在人工勢場法的算法研究和實踐應(yīng)用方面開展了眾多研究，內(nèi)容主要在于解決障礙物附近目標(biāo)不可達(dá)問題(CNRON)[2-3]。該問題的第一類是因斥力過大或引力與斥力相等，導(dǎo)致機器人無法到達(dá)目標(biāo)點，即陷入“勢場陷阱”[4]；另一類是環(huán)境因素，機器人的行進(jìn)路線被障礙物阻擋或目標(biāo)點被障礙物所包圍，導(dǎo)致無法到達(dá)目標(biāo)位置。

為解決目標(biāo)不可達(dá)問題，已有多種改進(jìn)型人工勢場法，但缺少評價路徑規(guī)劃算法和優(yōu)化路徑軌跡的方法。本文綜合各人工勢場法算法的特點和應(yīng)用情況，提出評價路徑規(guī)劃算法的代價函數(shù)，并結(jié)合輪式機器人的構(gòu)造和運動特性[5]，提出優(yōu)化路徑軌跡的算法，以提高算法在實際應(yīng)用中的效果和可移植性，完善人工勢場法在移動機器人路徑規(guī)劃中的應(yīng)用。

1 人工勢場法

1.1 傳統(tǒng)人工勢場法

該算法將機器人所處的運動環(huán)境虛擬成人造勢場，目標(biāo)點對機器人作用引力勢場，障礙物在某一限定范圍內(nèi)對機器人作用斥力勢場[4]。機器人在運行過程中受引力和斥力的合力作用，朝著目標(biāo)點移動，模型如圖1所示，q為機器人位置，qoi(i=1,2,…,n)為各障礙物所在位置[6]，n為障礙物的個數(shù);qg為機器人要達(dá)到的目標(biāo)位置；Frepi(i=1,2,…,n)為各障礙物對機器人的斥力；Fatt(q)為目標(biāo)對機器人的引力；F為目標(biāo)點作用的引力和所有范圍內(nèi)障礙物作用的斥力的合力。

圖1 人工勢場模型

1.1.1 引力的計算

引力的大小與目標(biāo)點到機器人當(dāng)前位置間的距離成正比，方向由當(dāng)前位置指向目標(biāo)點，引力勢場Uatt(q)為

(1)

α為引力增益系數(shù)；ρ(q,qg)為當(dāng)前位置到目標(biāo)點的距離。對應(yīng)引力Fatt(q)為引力勢場的負(fù)梯度為

Fatt(q)=-grad[Uatt(q)]=-αρ(q,qg)

(2)

1.1.2 斥力的計算

一般認(rèn)為距離機器人一定范圍內(nèi)的障礙物都會對其產(chǎn)生斥力，這些斥力相迭加形成斥力的合力，其中某個障礙物形成的斥力勢場Urepi(q)為

β為斥力增益系數(shù)，為正數(shù)；ρ(q,qo)為機器人當(dāng)前位置與障礙物之間的距離；ρ0為設(shè)定的障礙物起作用的范圍，在該范圍之外的障礙物形成的斥力勢場為零；斥力Frepi(q)為斥力勢場的負(fù)梯度，方向由障礙物指向當(dāng)前位置。

Frepi(q)=-grad[Urepi(q)]=

(4)

在ρo范圍內(nèi)若有n個障礙物，多個障礙物斥力矢量迭加后的斥力為

(5)

在上述勢場作用下的合力F為

F=Fatt+Frep

(6)

假設(shè)合力F與水平方向的夾角為θ，機器人便以單位步長Δd從當(dāng)前位置(xi,yi)移動到下一位置(xi+1,yi+1)，以此類推，通過傳統(tǒng)人工勢場法路徑各路徑點，到達(dá)目標(biāo)點。

(7)

1.2 改進(jìn)人工勢場法

針對GNRON問題,許多學(xué)者提出了行之有效的方法，有改進(jìn)引力函數(shù)的，有引入安全距離調(diào)節(jié)因子的，有分段規(guī)劃的。還有結(jié)合其他路徑規(guī)劃算法，諸如柵格法、人工神經(jīng)法。

1.2.1 改進(jìn)引力函數(shù)法

如前所述，當(dāng)機器人距離目標(biāo)點較遠(yuǎn)時，引力較大，斥力小，可能會使機器人在路徑上碰上障礙物。這種情況可修正引力函數(shù)，引入限定目標(biāo)位置與當(dāng)前位置之間的距離參數(shù)d，改進(jìn)后的引力函數(shù)為[7]

(8)

α,ρ(q,qg)與傳統(tǒng)人工勢場法中的參數(shù)相同

1.2.2 改進(jìn)斥力函數(shù)法

當(dāng)目標(biāo)點附近有障礙物時，斥力變得非常大，引力相對很小，機器人很難達(dá)到目標(biāo)點，此時采用改進(jìn)斥力函數(shù)法。在傳統(tǒng)斥力函數(shù)基礎(chǔ)上，增加當(dāng)前位置和目標(biāo)點的距離對斥力的影響，對障礙物的作用范圍進(jìn)行分段：設(shè)ρ1≤ρ≤ρ0區(qū)為可控域，障礙物的斥力為ρ的線性收斂區(qū)；ρ<ρ1區(qū)域為機器人的危險區(qū)域，即將當(dāng)機器人在目標(biāo)附近時，斥力增大，但距離在減小，可實現(xiàn)對斥力場的拖拽作用[8-9]。

(9)

當(dāng)ρ1≤ρ(q,qo)≤ρ0時

(10)

當(dāng)ρ(q,qo)<ρ1時

(11)

m和n為大于零的實數(shù)；ρ1一般為ρ0的整數(shù)倍。

2 人工勢場法算法

人工勢場法算法原理如圖2所示，機器人在某一位置，遍歷限定范圍內(nèi)所有的障礙物，得到作用的合力，確定機器人下一步移動的方向和位置。以此類推，并時刻監(jiān)控機器人在限定的迭代次數(shù)內(nèi)是否能到達(dá)目標(biāo)點。如果達(dá)不到，對參數(shù)進(jìn)行調(diào)整，再次循環(huán)。

圖2 基于勢場法的路徑規(guī)劃流程

圖3為分別對應(yīng)用傳統(tǒng)勢場法、改進(jìn)斥力勢場法模擬得到的規(guī)劃路徑。本例中設(shè)定的相應(yīng)參數(shù)如下：引力增益系數(shù)a=15；增益斥力系數(shù)β=5；步長λ=0.25；障礙物影響距離ρ0=0.25 mm；改進(jìn)斥力函數(shù)設(shè)定距離ρ1=ρ0/2；m=n=2。由圖3b可知，改進(jìn)型人工勢場法增強了障礙物對機器人的斥力作用，降低行進(jìn)過程中與障礙物的碰撞可能性。

圖3 基于勢場法的算法對應(yīng)規(guī)劃路徑

人工勢場法采用的是迭代法，盡管改進(jìn)勢場法在路徑安全性和實效性有所改善，但仍然存在圖3中所示點1、點2、點3、點4、點5一樣的拐點或折點，這勢必會影響機器人的操控性和運動性。

3 奇點和代價函數(shù)

3.1 規(guī)劃路徑的奇點

機器人規(guī)劃路徑上若某軌跡點附近路徑的曲率出現(xiàn)突變，機器人在這些點附近運動時會出現(xiàn)抖動或停頓，將這樣的路徑軌跡點定義為奇點。圖3路徑中的軌跡點1、點2、點3、點4、點5可定義為奇點。機器人若按照規(guī)劃路徑行駛，運行到這些點附近時，機器人必須減速或轉(zhuǎn)向。

3.2 路徑規(guī)劃算法的代價函數(shù)

通過比對規(guī)劃路徑，將路徑安全性、算法耗時、路徑“奇點”個數(shù)作為評價規(guī)劃路徑的標(biāo)準(zhǔn)，從而構(gòu)造如下所示的代價函數(shù)。

p(i)=EsQs(i)+EtQt(i)+EnQn(i)

(12)

p(i)為算法對應(yīng)路徑的總代價，i為路徑標(biāo)號；Qs(i)為安全代價函數(shù)；Qt(i)為時間代價函數(shù)；Qn(i)為路徑平順代價函數(shù)；Es，Et和En為各代價函數(shù)的調(diào)節(jié)參數(shù)。

a. 安全代價函數(shù)。

(13)

D為該路徑中障礙物到路徑的最小距離,R0為障礙物半徑[10]。

b. 時間代價函數(shù)。Qt(i)以軟件模擬時間為準(zhǔn)，按照算法耗時的實際精準(zhǔn)程度進(jìn)行度量。

c. 路徑平順代價函數(shù)。

(14)

Qn(i)為標(biāo)號i的路徑上出現(xiàn)“奇點”的總次數(shù)；Δη為各奇點所在路徑的曲率變化量；η0為一常數(shù)，可根據(jù)機器人的結(jié)構(gòu)和路徑的復(fù)雜程度取值。

表1為圖3a和圖3b對應(yīng)2種規(guī)劃算法的代價函數(shù)結(jié)果，其中各代價函數(shù)的調(diào)節(jié)參數(shù)為Es=0.2，Et=0.3，En=0.5。

表1 路徑規(guī)劃算法估價函數(shù)

由表1可知，在相同的工作環(huán)境條件下，改進(jìn)人工勢場法評價結(jié)果優(yōu)于傳統(tǒng)人工勢場法。這也表明改進(jìn)人工勢場法在運算速率、路徑安全性和平順性方面具有更強的優(yōu)勢，結(jié)果與實際相符。因此，該代價函數(shù)可應(yīng)用于路徑算法的特性分析。

4 路徑優(yōu)化算法驗證

移動機器人常在后面左右兩側(cè)各安裝一個驅(qū)動輪，在“奇點”附近，機器人的兩輪由連續(xù)轉(zhuǎn)動突變?yōu)橐煌Ｒ晦D(zhuǎn)狀態(tài)，車輪與地面相對運動狀態(tài)發(fā)生變化，影響機器人的運行狀態(tài)[11]。為了使運行更為平順，可設(shè)計算法對路徑進(jìn)行優(yōu)化。

4.1 優(yōu)化算法設(shè)計

“奇點”的出現(xiàn)不但造成時間和能耗損失，同時對機器人的控制系統(tǒng)和執(zhí)行系統(tǒng)提出了更高的要求：不但控制系統(tǒng)需及時響應(yīng)外徑路徑的變化，而且在機械結(jié)構(gòu)上，還要求機器人的質(zhì)量分布、動力分布及結(jié)構(gòu)布局合理[12-13]。因此，對規(guī)劃路徑進(jìn)行平滑優(yōu)化具有重要的意義。

算法的基本思路如圖4所示，以已記錄的“奇點”為依據(jù)，在其左右搜索路徑上的點，以這些點組成點集，重新構(gòu)造與障礙物邊緣具有一定安全距離的過渡路徑，該路徑能保證機器人沿著平順的路徑安全準(zhǔn)確地抵達(dá)目標(biāo)。

圖4 人工勢場法算法基本思路

4.2 算法驗證

為了對路徑優(yōu)化算法進(jìn)行驗證，選用改進(jìn)人工勢場法的規(guī)劃路徑，通過對路徑上的軌跡點進(jìn)行分析，找出奇點，之后進(jìn)行路徑優(yōu)化。

圖5a為改進(jìn)人工勢場法對應(yīng)規(guī)劃路徑，圖5b為該路徑上各軌跡點所在位置的曲率差。以圖5a中的路徑算法的驗證對象，按照式(14)，判定“奇點”位置。然后應(yīng)用路徑優(yōu)化算法重新規(guī)劃路徑，得到圖5c所示經(jīng)過優(yōu)化算法處理的路徑。

通過對比，經(jīng)算法優(yōu)化后的路徑在保證安全性和準(zhǔn)確性的前提下，可實現(xiàn)路徑的平順處理，更符合機器人運動特性的需求。

圖5 路徑優(yōu)化算法驗證

5 結(jié)束語

隨著傳感器技術(shù)、視覺技術(shù)的發(fā)展，基于單片機控制的機器人應(yīng)用將非常廣泛[14],也取得豐富的研究成果。本文基于人工勢場法在機器人路徑規(guī)劃中的應(yīng)用，提出常用幾種算法的評價函數(shù)，并結(jié)合移動機器人的運動特性，給出路徑優(yōu)化算法。從而保證機器人在安全、無碰撞中的路徑軌跡中，以最優(yōu)的能耗和時間損耗平順地抵達(dá)目標(biāo)。