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

徐勁力 許建寧 黃豐云 李征瑞

(武漢理工大學(xué)機(jī)電工程學(xué)院 湖北 武漢 430070)

0 引 言

路徑規(guī)劃技術(shù)在移動機(jī)器人領(lǐng)域中是一項尤為重要的技術(shù),是其自主移動完成各種復(fù)雜任務(wù)的前提,更是所有移動機(jī)器人可以進(jìn)行智能化技術(shù)應(yīng)用分析研究的一切前提。近年來,國內(nèi)外學(xué)者提出一系列算法應(yīng)用在路徑規(guī)劃研究當(dāng)中,主要包括人工勢場法[1-3]、蟻群算法[4]、遺傳分析算法[5]、A*算法[6]、RRT算法[7]等。相比于其他算法,人工勢場法比較簡單,計算量小,所規(guī)劃的路徑相對較為平坦,但是也存在障礙物附近目標(biāo)不可達(dá)以及陷入局部最小值無法運動等缺陷。為了解決這些問題,國內(nèi)外學(xué)者對其進(jìn)行了改進(jìn):楊凱等[8]通過對斥力場模型進(jìn)行改進(jìn)優(yōu)化,通過設(shè)置虛擬目標(biāo)點解決移動機(jī)器人在搜索路徑研究過程中可能出現(xiàn)的局部最小值問題;Orozco-Rosas等[9]提出采用遺傳算法與人工勢場算法融合以此來對人工勢場法進(jìn)行改進(jìn),搜索出一條無碰撞的路徑;汪四新等[10]通過改進(jìn)斥力場的人工勢場與模糊算法相結(jié)合,利用模糊決策系統(tǒng)調(diào)整機(jī)器人運動,使得規(guī)劃出的路徑平滑;李靖等[11]通過改進(jìn)人工勢場法中的勢場模型,去除斥力場,設(shè)置動態(tài)引力場,在機(jī)器人運動過程中不斷對引力場進(jìn)行修正,最后對生成的路徑進(jìn)行平滑;侯翔[12]通過改進(jìn)人工勢場法中的勢力場模型,引入指數(shù)因子,采用量子遺傳算法對規(guī)劃出的路徑進(jìn)行優(yōu)化平滑。

本文在現(xiàn)有研究基礎(chǔ)上,針對人工勢場法應(yīng)用在移動機(jī)器人路徑規(guī)劃領(lǐng)域存在的問題,提出一種改進(jìn)的人工勢場法。通過改進(jìn)斥力場函數(shù)模型,將人工勢場法與模擬退火算法融合,并通過路徑優(yōu)化算法規(guī)劃出一條無碰撞的平滑路徑。

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

人工勢場法是通過在機(jī)器人進(jìn)行運動時建立勢力場,并使得在目標(biāo)點出的合勢力為全局最優(yōu)值。對于移動機(jī)器人在二維空間中任意一點q=[xy],傳統(tǒng)人工勢場法通過在空間中目標(biāo)點qg=[xgyg]建立引力勢場,在各個障礙物qo=[xoyo]處建立斥力勢場。在目標(biāo)點引力Fatt以及障礙物排斥力Freq的作用下,通過兩者的合力F以及給定的步長λ0確定了移動機(jī)器人下一步的運動方向[13],如圖1所示。

圖1 移動機(jī)器人受力示意圖

傳統(tǒng)人工勢場法的目標(biāo)點引力場函數(shù)模型如下:

式中:Katt為所述引力勢場的增益系數(shù)。

對應(yīng)的引力為:

Fatt(q)=-grad(Uatt(q))=-Katt(q-qg)

(2)

障礙物點處斥力場函數(shù)模型如下:

式中:Kreq為所述斥力勢場的增益系數(shù);ρ為機(jī)器人當(dāng)前點q與障礙物點qo的直線距離;ρ0為障礙物點影響距離。

對應(yīng)的斥力為:

Freq(q)=-grad(Ureq(q))=

機(jī)器人所受的合勢場以及合力為:

U(q)=Uatt(q)+Ureq(q)

(5)

式中:N為障礙物的數(shù)目。

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

2.1 改進(jìn)勢力場函數(shù)

傳統(tǒng)人工勢場法中,機(jī)器人在建立好的勢力場中受到障礙物的斥力以及目標(biāo)的吸引力,兩者合力的方向決定了機(jī)器人的運動方向。從式(2)、式(4)可以看出,當(dāng)移動機(jī)器人愈靠近目標(biāo)點(位置在障礙物影響范圍內(nèi))時,受到的斥力會遠(yuǎn)遠(yuǎn)大于引力,機(jī)器人無法到達(dá)目標(biāo)點,因此對勢場函數(shù)模型進(jìn)行改進(jìn),在斥力場函數(shù)中引入當(dāng)前點與目標(biāo)點的歐氏距離。同時,為減小路徑的振蕩性,添加最小引力勢能。

此時改進(jìn)引力場函數(shù)模型為:

式中:Umin為最小引力勢能。

對應(yīng)的引力為:

Fatt(q)=-grad(Uatt(q))=

改進(jìn)后的斥力場函數(shù)模型如下:

對應(yīng)的斥力為:

式中:

此時移動機(jī)器人的受力如圖2所示。

圖2 改進(jìn)斥力場移動機(jī)器人受力示意圖

2.2 局部最小值問題

為處理采用人工勢場法時所出現(xiàn)的局部最小值問題導(dǎo)致機(jī)器人不可移動的情況,本文采用變步長的模擬退火算法逃逸當(dāng)前局部最小點。模擬退火算法是模擬工程中物質(zhì)退火的算法,當(dāng)加熱后的高溫材料溫度逐漸冷卻時,材料內(nèi)能逐漸降低,分子逐漸趨向穩(wěn)定,能夠以一定概率跳出當(dāng)前的局部最小值[14]。算法步驟如下:

步驟1初始化算法中如下設(shè)計參數(shù):初始工作溫度為T=10,退火速率為α=0.9。

步驟2隨機(jī)產(chǎn)生新解:在范圍內(nèi)給移動機(jī)器人一隨機(jī)角度θ、步長λ=βλ0,其中β為步長增益系數(shù),判斷機(jī)器人以該隨機(jī)角度以及增益后的步長運動后是否與障礙物碰撞,若碰撞,重復(fù)步驟2;否則,進(jìn)行下一步。

步驟3計算隨機(jī)點的合勢能Urand、前一個點的合勢能Ubef、勢能變化量ΔU=Urand-Ubef。

步驟4采用Metropolis準(zhǔn)則,判斷是否接受當(dāng)前隨機(jī)解,若ΔU<0,則接受當(dāng)前隨機(jī)解,否則產(chǎn)生一個隨機(jī)數(shù)r,若exp(-ΔU/T)>r,則接受當(dāng)前隨機(jī)解,進(jìn)行下一步;否則,重復(fù)步驟4。

步驟5按照退火策略T=αT更新參數(shù),判斷是否跳出局部最小值,若已逃逸,退出模擬退火算法,否則,重復(fù)步驟2-步驟4直至逃逸出局部最優(yōu)解。

3 路徑優(yōu)化算法

3.1 路徑優(yōu)化策略

由于改進(jìn)人工勢場法中獲得的路徑點存在冗余的路徑點與振蕩點,當(dāng)運動到障礙物附近時合力產(chǎn)生突變,生成的路徑點存在振蕩的情況,生成的路徑點在實際中無法應(yīng)用,因此本文研究提出一種路徑優(yōu)化平滑算法,其步驟描述如下:

步驟1移動機(jī)器人通過改進(jìn)后的人工勢場法獲得從起始點到目標(biāo)點的無碰撞路徑點序列為Qi(xi,yi),其中J為路徑點的總數(shù),初始化空數(shù)組LQ。

步驟2設(shè)障礙物點為Ci,i∈[0,N],路徑起始點A為Qm,m=1,選取路徑點的后兩個點B為Qn,n=m+2開始,計算點Ci,i∈[0,N]到直線AB的距離,若大于給定距離參數(shù)D,則n=n+1,否則判斷CA與CB的夾角,若為銳角,則n=n+1,否則,將Qm放入數(shù)組LQ中,m=m+1。

步驟3重復(fù)步驟2,直至m=J。

步驟4將獲得折線路徑的拐角左右兩條折線上取得中點以及靠近拐點的四等分點,通過四次貝塞爾樣條插值進(jìn)行平滑處理,當(dāng)對n個頂點進(jìn)行p次B樣條插值時,其曲線方程式為:

式中:P(i)(i=0,1,…,n)為控制點;Ni,p(t)為p次B樣條基函數(shù)。

3.2 技術(shù)路線

至此,改進(jìn)算法的技術(shù)流程如圖3所示。當(dāng)給定起點、目標(biāo)點、移動步長時,采用改進(jìn)的人工勢場法進(jìn)行路徑規(guī)劃搜索,通過相應(yīng)的路徑優(yōu)化策略對已經(jīng)規(guī)劃出來的路徑進(jìn)行優(yōu)化,并在拐點處采用四次貝塞爾曲線進(jìn)行平滑。

圖3 算法技術(shù)路線

3.3 實驗仿真分析

以在二維空間內(nèi)的路徑搜索為例,以此驗證本文算法以及路徑平滑策略的有效性。基于MATLAB設(shè)置仿真實驗,通過機(jī)器人的形態(tài)特征設(shè)置路徑平滑距離參數(shù),設(shè)置算法參數(shù)如表1所示。

表1 算法參數(shù)表

在二維空間內(nèi)設(shè)置起點S(0,0)與終點G(10,10),通過圓形模擬靜態(tài)障礙物。采用傳統(tǒng)人工勢場法以及本文算法進(jìn)行模擬仿真在以下三種典型障礙物(平衡點、障礙物附近目標(biāo)、槽型障礙物)進(jìn)行模擬仿真,其仿真結(jié)果對比分別如圖4-圖6所示。

圖4 平衡點出仿真對比

圖5 障礙物附近目標(biāo)不可達(dá)

圖6 槽型障礙物

可以看出,改進(jìn)后的算法均能克服傳統(tǒng)人工勢場法應(yīng)用到相應(yīng)典型障礙物情況下無法完成路徑規(guī)劃仿真的缺點。為進(jìn)一步說明算法的有效性,在復(fù)雜障礙物場景下采用傳統(tǒng)人工勢場法的路徑仿真示意圖如圖7所示。分別采用文獻(xiàn)[15]中所提出的算法以及本文算法在復(fù)雜障礙物環(huán)境中進(jìn)行仿真分析,得到機(jī)器人的路徑軌跡,并采用本文提出的路徑優(yōu)化策略將得到的路徑節(jié)點分別進(jìn)行路徑優(yōu)化處理。文獻(xiàn)[15]所得到的優(yōu)化前后的路徑仿真示意圖如圖8所示,本文算法所得到的優(yōu)化前后的路徑仿真示意圖如圖9所示。

圖7 傳統(tǒng)人工勢場法的路徑仿真示意圖

圖8 路徑優(yōu)化前后的仿真算法示意圖

圖9 路徑優(yōu)化前后本文算法仿真示意圖

從圖8可以看出,機(jī)器人在當(dāng)前障礙物環(huán)境中無法完成路徑軌跡規(guī)劃。同樣的環(huán)境中利用文獻(xiàn)[15]以及本文算法進(jìn)行仿真,從圖9和圖10可以看出,兩種算法均能使得移動機(jī)器人克服傳統(tǒng)人工勢場法的缺點,兩種算法均能在障礙空間環(huán)境內(nèi)從設(shè)置的起點S與終點G之間搜索出一條無碰撞的路徑,相比之下本文算法能夠更快地逃出局部最小值點,但是在經(jīng)過路徑優(yōu)化策略處理前,兩種算法所得到的路徑并不平滑且均存在振蕩點。在經(jīng)過本文提出的路徑優(yōu)化策略進(jìn)行處理后,兩種算法均能消除無碰撞搜索路徑的振蕩特性,都能得到一條平滑無碰撞的軌跡,表明本文算法的有效性。

圖10 隨機(jī)障礙物環(huán)境1

在保證算法參數(shù)相同的情況下,在地圖中隨機(jī)生成障礙物進(jìn)行多次仿真分析如圖10所示,仿真數(shù)據(jù)如表2所示,改進(jìn)算法在進(jìn)行路徑優(yōu)化后相比未進(jìn)行優(yōu)化之前平均減少了48.8%,表明了本文算法的有效性。

表2 隨機(jī)障礙物地圖下算法路徑優(yōu)化前后對比

4 結(jié) 語

本文結(jié)合人工勢場法中現(xiàn)有問題,通過對人工勢場法的函數(shù)模型進(jìn)行修改,采用模擬退火算法對局部最優(yōu)問題進(jìn)行逃離,結(jié)合機(jī)器人的形態(tài)特征,采用提出的路徑優(yōu)化算法去除路徑的冗余點與振蕩點。并通過MATLAB仿真分析,本文算法能夠生成一條平滑的無碰撞路徑,仿真結(jié)果證明了本文算法的有效性。