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

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

0 引言

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

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

1 人工勢(shì)場(chǎng)法

1.1 傳統(tǒng)人工勢(shì)場(chǎng)法

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

圖1 人工勢(shì)場(chǎng)模型

1.1.1 引力的計(jì)算

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

(1)

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

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

(2)

1.1.2 斥力的計(jì)算

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

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

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

(4)

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

(5)

在上述勢(shì)場(chǎng)作用下的合力F為

F=Fatt+Frep

(6)

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

(7)

1.2 改進(jìn)人工勢(shì)場(chǎng)法

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

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

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

(8)

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

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

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

(9)

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

(10)

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

(11)

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

2 人工勢(shì)場(chǎng)法算法

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

圖2 基于勢(shì)場(chǎng)法的路徑規(guī)劃流程

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

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

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

3 奇點(diǎn)和代價(jià)函數(shù)

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

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

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

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

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

(12)

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

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

(13)

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

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

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

(14)

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

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

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

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

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

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

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

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

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

圖4 人工勢(shì)場(chǎng)法算法基本思路

4.2 算法驗(yàn)證

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

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

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

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

5 結(jié)束語(yǔ)

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