改進人工勢場法的移動機器人路徑規(guī)劃

陳金鑫,董 蛟,朱旭芳

(海軍工程大學(xué)，湖北 武漢 430000)

Khatib在1986年提出了人工勢場法,它的基本思想是在機器人周圍設(shè)計一種類似于電場的勢場,機器人的運動依靠勢場力進行驅(qū)動。障礙物對機器人產(chǎn)生“斥力”,目標對機器人產(chǎn)生“引力”,障礙物斥力和目標引力的矢量和是機器人所受的合力,合力的方向即機器人下一步的運動方向。該方法由于數(shù)學(xué)原理簡單，計算速度快，硬件要求低，被廣泛應(yīng)用于移動機器人路徑規(guī)劃。

同時,傳統(tǒng)的人工勢場法也存在著缺陷,文獻[1]指出該方法依托局部環(huán)境信息進行運算,缺少全局信息的指導(dǎo),存在著陷入局部極小陷阱,產(chǎn)生目標不可達，規(guī)劃路徑徘徊震動等問題。為此,研究人員對此做了大量改進:文獻[2]重新定義了勢場函數(shù),加入了可調(diào)參數(shù),并使用進化算法確定最優(yōu)參數(shù);文獻[3-4]將距離項引入斥力勢場函數(shù),解決了局部極小引起的目標不可達問題;文獻[5-9]均對勢場函數(shù)進行了改進,提高了規(guī)劃路徑的質(zhì)量；文獻[10-12]通過增設(shè)虛擬目標點的方式引導(dǎo)機器人走出局部極小點;文獻[13-14]則從障礙物入手,增設(shè)虛擬障礙物,修改障礙物斥力的影響范圍,進而達到避碰的目的；文獻[15-19]將人工勢場法與其他路徑規(guī)劃算法進行融合,取得了不錯的避碰效果。以上研究均對人工勢場法的發(fā)展起到了促進作用,不過部分算法也存在著規(guī)劃過于復(fù)雜、實時性差等問題。

本文為解決人工勢場法存在的局部極小問題,提出一種改進模型,根據(jù)機器人和障礙物、目標的相對位置偏轉(zhuǎn)斥力的方向,同時自適應(yīng)調(diào)節(jié)斥力系數(shù),最后通過Matlab仿真驗證了算法的有效性。

1 傳統(tǒng)人工勢場模型

傳統(tǒng)人工勢場由兩種勢場疊加而成,目標點對機器人提供引力,形成引力勢場;障礙物則提供斥力,形成斥力勢場。障礙物處的勢能較高,而目標點處的勢能較低,在勢場力的驅(qū)動下,機器人從勢能高的地方向勢能低的方向運動,最終找到了一條可以到達目標點的無碰路徑。

傳統(tǒng)人工勢場的引力勢場函數(shù)為

Uatt(X)=0.5λ1ρ2(X,Xg)

(1)

斥力勢場函數(shù)為

(2)

式中,X=(x,y)是機器人的位置向量,λ1和λ2分別是引力與斥力增益系數(shù),X、X0、Xg分別代表機器人、障礙物、目標點的空間位置,ρ(X,X0)和ρ(X,Xg)分別代表機器人到障礙物和到目標點的距離,ρ0是障礙物對機器人的影響半徑。

對勢場函數(shù)求解負梯度,即可得到勢場力的表達式:

Fatt(X)=-

(3)

Frep(X)=-Urep(X)=

(4)

在上述勢場力的作用下,機器人路徑規(guī)劃存在著局部極小問題。一種改進方法是將機器人和目標距離的n次方引入到斥力勢場函數(shù)中

(5)

相應(yīng)的,斥力函數(shù)修改為:

Frep(X)=-Urep(X)=

(6)

式中,n的取值范圍是(0,∞)。修改后的斥力分力Frep1由障礙物指向機器人,分力Frep2由機器人指向目標點,且Frep1隨著機器人和目標點距離的減小而減小,如圖1所示。

圖1 引入距離項后機器人受力示意圖

圖1所示模型中,斥力隨著機器人到目標點距離縮小而減小,同時增加了一個由機器人指向目標點的分力,因此理論上可以解決一類局部極小問題——目標點周圍有障礙物而導(dǎo)致目標點不可達的問題。文獻[7]對n不同取值對避障效果的影響進行了分析,結(jié)果顯示確定參數(shù)n的過程較為復(fù)雜而且不能保證絕對安全的避碰。參數(shù)調(diào)節(jié)不佳時,會出現(xiàn)提前停車或者直接撞向障礙物的情況[12]。第二節(jié)針對上述模型的缺陷進行改進,第四節(jié)仿真實驗中n值取1。

2 基于斥力偏轉(zhuǎn)的改進人工勢場模型

基于對極小點問題的幾種典型場景的分析,本文總結(jié)陷入勢場極小點的規(guī)律,提出了基于斥力偏轉(zhuǎn)的改進人工勢場模型。局部極小點問題可以表現(xiàn)為以下四種典型場景,如圖2所示。

a. 機器人、障礙物、目標點共線,障礙物在中間。

b. 機器人、障礙物、目標點共線,目標點在中間。

c. 機器人和目標陷入障礙物包圍圈中。

d. 目標在障礙物包圍圈中。

圖2 典型局部極小點場景示意圖

圖2所示場景中,機器人在障礙物前停住,或者沿著障礙物邊緣某位置徘徊,實際上是勢場對機器人施加的引力和斥力相等,方向相反。此時機器人所處的位置是一個局部勢能低點,但不是在目標位置形成的勢能低點,這叫做“勢場陷阱”。那么只要通過調(diào)整,破壞掉引力和斥力這種在數(shù)值和方向上的平衡,機器人就能走出“勢場陷阱”?；诔饬ζD(zhuǎn)的改進人工勢場模型,針對第一節(jié)中的勢場模型作了兩步改進：修改斥力的指向以及自適應(yīng)調(diào)節(jié)斥力系數(shù),破壞局部極小點處勢場引力和斥力的平衡。

2.1 改進1——改變斥力分力的方向

勢場斥力偏轉(zhuǎn)如圖3所示。

圖3 勢場斥力偏轉(zhuǎn)示意圖

斥力分力Frep1和引力Fatt之間有一個夾角,當夾角為鈍角時,機器人存在陷入局部極小點的可能性。進一步分析,當合力F和引力Fatt之間的夾角為鈍角時,機器人存在較大可能性陷入局部極小點。通過將Frep1的方向旋轉(zhuǎn)一個角度θ,使得合力F與引力Fatt的夾角小于等于90°,機器人即可避開局部極小點。

旋轉(zhuǎn)角θ的大小和兩個要素有關(guān),即角β和距離l。由于角度β不夠直觀,引入中間變量d來刻畫角度θ,表達式為:

d=l·sin(β)

(7)

θ=θ0-μ·d·l

(8)

2.2 改進2——修改斥力增益系數(shù)

在保證安全的前提下,當在機器人航向上,障礙物被目標點遮擋,機器人沒有必要調(diào)整航向;當機器人駛過障礙物時,機器人同樣沒有必要調(diào)整航向。

同時,分析目標點不可達問題,發(fā)現(xiàn)機器人之所以難以到達目標點,主要是難以翻越障礙物造成的高勢場。如圖4,顯示的是目的地周圍存在四個障礙物的情形下的勢場分布。雖然目標點處的勢場低,但是障礙物連線附近的勢場很高,形成類似勢場“高墻”導(dǎo)致機器人難以翻越。如果斥力增益系數(shù)能夠隨著與障礙物的距離增加而遞減的話,那么障礙物周圍的勢場下降的很快,障礙物之間形成的勢場“高墻”將不再能攔住障礙物。

圖4 目標點周圍障礙物及其勢場分布示意圖

綜合以上分析,斥力增益系數(shù)λ2的值應(yīng)當根據(jù)一個函數(shù)確定,表達式如下:

(9)

式中,λ0是系數(shù)基數(shù),m是非負實數(shù),s是機器人與障礙物之間的距離。

3 規(guī)劃路徑評價模型

機器人路徑規(guī)劃問題,首要目標是規(guī)劃出一條無碰路徑。因此機器人根據(jù)規(guī)劃路徑巡航的結(jié)果有兩個,發(fā)生碰撞和未發(fā)生碰撞,這是一個0-1問題。但是,僅僅根據(jù)是否發(fā)生碰撞來衡量路徑規(guī)劃的優(yōu)劣,很難比較各種規(guī)劃方法的優(yōu)劣。建立規(guī)劃路徑量化評價模型,模型主要包括四個參數(shù)。

參數(shù)1:規(guī)劃路徑是否發(fā)生碰撞

由于無碰路徑是路徑規(guī)劃的首要要求,機器人與障礙物一旦發(fā)生碰撞,那么當次路徑規(guī)劃是失敗的。

(10)

參數(shù)2:規(guī)劃總時間

實際巡航時,路徑總長度的長短,意味著消耗資源的多少。因此,路徑總長度越短,那么當次路徑規(guī)劃結(jié)果越好。由于人工勢場模型未涉及對速度的要求,因此路徑長度和巡航時間是成正比的。由于路徑是分段規(guī)劃的,每段規(guī)劃的路徑航向不變,因此巡航總時間可以根據(jù)每一段的時間求和得到。

假設(shè)路徑規(guī)劃一共分為N段進行,第i個時段的時間長度為Ti,那么規(guī)劃總時間為

(11)

參數(shù)3:最大轉(zhuǎn)彎角速度

假設(shè)機器人某次任務(wù)規(guī)劃的起點和終點之間,沒有任何障礙物,那么起點和終點之間的直線連線可以作為規(guī)劃路徑,且直線航向時機器人的航向是不變的。實際過程中,由于障礙物的存在,機器人的路徑往往不是直線,因此航向也是實時變化的。航向角對時間的變化率,即機器人轉(zhuǎn)彎的角速度。轉(zhuǎn)彎角速度反映了,規(guī)劃的路徑對機器人機動性能提出的要求。由于路徑規(guī)劃分段進行,因此使用相鄰規(guī)劃時段的航向差分,作為航向角速度。

設(shè)第i個時段的航向為Ci,第(i+1)個時段的航向為Ci+1,那么航向差分為

式中,stepi是第i個規(guī)劃時段的時長。

在整個規(guī)劃時間內(nèi),取航向差分的絕對值的最大值作為最大轉(zhuǎn)彎角速度,即

ωmax=max{ΔC1,ΔC2,…,ΔCi,…, ΔCn-1},
(i=1,2,…,N-1)

(12)

參數(shù)4:保持直線航行時間

從參數(shù)1和參數(shù)2的確定過程來看,在路徑總長度相同的情況下,直線航行時間越長,意味著規(guī)劃路徑對機器人機動性要求越低。由于航向?qū)崟r變化,航向保持不變的概率較低,因此設(shè)定航向變化閾值R。

若存在ΔCi≤R,那么第i個時段視為“保持直線航行”;若ΔCi>R,那么第i個時段視為 “未保持直線航行”。設(shè)定屬性變量flagkeep,取值如下

統(tǒng)計“保持直線航行”的時段數(shù)目,得到機器人保持直線航行的時間

(13)

根據(jù)公式(10)-(13),給出規(guī)劃路徑的評價值

(14)

式中,r1、r2、r3為三個正系數(shù),且滿足r1+r2+r3=1。評價值value越大,表明規(guī)劃路徑的質(zhì)量越高。

4 仿真校驗及分析

使用Matlab軟件編寫避碰仿真系統(tǒng),對基于斥力偏轉(zhuǎn)的模型進行仿真驗證,并利用路徑評價模型對規(guī)劃的路徑進行評價,和傳統(tǒng)人工勢場模型、加入距離項后的勢場模型的避碰效果進行對比。

根據(jù)前文給出的四種障礙物環(huán)境進行仿真,結(jié)果如圖5所示。

圖5 典型局部極小點環(huán)境下避障規(guī)劃仿真圖

分析圖5得到,應(yīng)用基于斥力偏轉(zhuǎn)的改進人工勢場模型后,機器人能夠避開路徑上的障礙物,并且在目的地被障礙物包圍的時候,仍然可以尋找路徑到達目的地。

本文以“機器人與障礙物會遇”這一情形為例,分別針對傳統(tǒng)人工勢場模型、加入距離項后的勢場模型、基于斥力偏轉(zhuǎn)的改進人工勢場模型,使用仿真系統(tǒng)進行仿真,結(jié)果如圖6所示。應(yīng)用規(guī)劃路徑評價模型對各條規(guī)劃出的路徑進行評價,結(jié)果見表1。

表1 相同環(huán)境下三種模型的規(guī)劃效果評價數(shù)據(jù)表

圖6 相同環(huán)境下三種模型的規(guī)劃效果對比圖

根據(jù)圖6分析,三種人工勢場模型規(guī)劃的路徑均是無碰的,說明人工勢場法應(yīng)用于機器人巡航任務(wù)是比較安全的。圖6中三幅子圖對比可以得到,傳統(tǒng)人工勢場法的總路徑較長,而且機器人巡航在做轉(zhuǎn)彎動作時基本是急轉(zhuǎn)彎;修改斥力勢場函數(shù)的模型規(guī)劃出的路徑,轉(zhuǎn)彎動作稍緩;而基于斥力偏轉(zhuǎn)的模型規(guī)劃出的路徑是相對比較平滑的。

在表1中,參數(shù)flagcollision顯示,規(guī)劃出的三條路徑均是無碰的,和圖中實際情況符合。對比參數(shù)Ttotal,發(fā)現(xiàn)傳統(tǒng)人工勢場模型的規(guī)劃時間最長,基于斥力偏轉(zhuǎn)的改進模型規(guī)劃時間最短,說明模型的改進縮短了機器人的巡航路徑的長度。對比參數(shù)ωmax發(fā)現(xiàn),對應(yīng)傳統(tǒng)模型和修改斥力勢場函數(shù)的改進模型的數(shù)值較大,模型對機器人的轉(zhuǎn)向能力提出了很高的要求。而基于斥力偏轉(zhuǎn)的改進模型,ωmax值大幅減小,說明模型規(guī)劃的路徑非常平緩,對機器人的轉(zhuǎn)向能力要求較低。對比參數(shù)Tkeep,發(fā)現(xiàn)對應(yīng)基于斥力偏轉(zhuǎn)的改進模型的值最大,說明規(guī)劃路徑中更大部分是直線巡航,進一步降低了對機器人自身機動性能的要求。最終路徑評價值value顯示,基于斥力偏轉(zhuǎn)的改進模型規(guī)劃路徑的評價值,明顯高于其他兩種模型的規(guī)劃路徑的評價值,說明基于斥力偏轉(zhuǎn)的改進模型提高了巡航路徑的質(zhì)量。

5 結(jié)束語

本文提出的基于斥力偏轉(zhuǎn)的改進人工勢場模型,主要完善了傳統(tǒng)人工勢場法的幾個缺陷:第一,機器人巡航過程中,出現(xiàn)與障礙物、目的地共線的情況時,易陷入局部勢場最低點而提前停車?；诔饬ζD(zhuǎn)的改進模型,通過提前轉(zhuǎn)彎解決了這個問題,同時解決了傳統(tǒng)模型中出現(xiàn)的規(guī)劃航向變化過大導(dǎo)致機器人不能及時反應(yīng)的問題。第二,機器人巡航過程中,目的地周圍有障礙物群形成的高勢場,導(dǎo)致機器人無法靠近目的地。即使是修改斥力勢場函數(shù)的改進模型,在斥力增益系數(shù)不變的情況下,也易陷入局部勢場最低點,或者直接撞上障礙物。基于斥力偏轉(zhuǎn)的改進模型,自適應(yīng)調(diào)整斥力增益系數(shù),降低了障礙物連線之間的勢場“門檻”,使得機器人可以順利到達目的地。