改進(jìn)人工勢場法的移動(dòng)機(jī)器人路徑規(guī)劃

陳金鑫,董 蛟,朱旭芳

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

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

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

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

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

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

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

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

(1)

斥力勢場函數(shù)為

(2)

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

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

Fatt(X)=-

(3)

Frep(X)=-Urep(X)=

(4)

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

(5)

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

Frep(X)=-Urep(X)=

(6)

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

圖1 引入距離項(xiàng)后機(jī)器人受力示意圖

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

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

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

a. 機(jī)器人、障礙物、目標(biāo)點(diǎn)共線,障礙物在中間。

b. 機(jī)器人、障礙物、目標(biāo)點(diǎn)共線,目標(biāo)點(diǎn)在中間。

c. 機(jī)器人和目標(biāo)陷入障礙物包圍圈中。

d. 目標(biāo)在障礙物包圍圈中。

圖2 典型局部極小點(diǎn)場景示意圖

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

2.1 改進(jìn)1——改變斥力分力的方向

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

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

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

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

d=l·sin(β)

(7)

θ=θ0-μ·d·l

(8)

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

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

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

圖4 目標(biāo)點(diǎn)周圍障礙物及其勢場分布示意圖

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

(9)

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

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

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

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

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

(10)

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

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

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

(11)

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

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

設(shè)第i個(gè)時(shí)段的航向?yàn)镃i,第(i+1)個(gè)時(shí)段的航向?yàn)镃i+1,那么航向差分為

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

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

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

(12)

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

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

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

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

(13)

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

(14)

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

4 仿真校驗(yàn)及分析

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

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

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

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

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

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

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

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

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

5 結(jié)束語

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