宋 宇, 王志明

(長(zhǎng)春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 吉林 長(zhǎng)春 130012)

0 引 言

由于具有廣泛的應(yīng)用場(chǎng)景，如機(jī)械臂運(yùn)動(dòng)規(guī)劃、機(jī)器人運(yùn)動(dòng)規(guī)劃等，近年來(lái)路徑規(guī)劃得到國(guó)內(nèi)外學(xué)者的關(guān)注，路徑規(guī)劃的相關(guān)算法被不斷提出。其中人工勢(shì)場(chǎng)法由于簡(jiǎn)便、高效而受到許多學(xué)者的青睞，但人工勢(shì)場(chǎng)法存在局部極值點(diǎn)的問(wèn)題。RRT算法具有概率完備性，在給定任意隨機(jī)地圖的情況下，總能得到一條從起點(diǎn)到終點(diǎn)的路徑，但由于此路徑往往不是最優(yōu)，即與最優(yōu)路徑長(zhǎng)度相比長(zhǎng)度過(guò)長(zhǎng)。文獻(xiàn)[1]結(jié)合貪婪算法(樹(shù)枝每次向隨機(jī)點(diǎn)移動(dòng)的過(guò)程中嘗試多步移動(dòng)，直到與障礙物碰撞或到達(dá)隨機(jī)點(diǎn)為止)與雙向搜索機(jī)制(同時(shí)從起點(diǎn)與目標(biāo)點(diǎn)生長(zhǎng)2顆樹(shù))提出了RRT-connect算法；文獻(xiàn)[2]在RRT樹(shù)枝擴(kuò)展過(guò)程中引入了改線機(jī)制(比較經(jīng)由距離隨機(jī)點(diǎn)最近點(diǎn)的樹(shù)枝點(diǎn)的某鄰域內(nèi)的樹(shù)枝點(diǎn)到達(dá)新點(diǎn)是否得到更小的新點(diǎn)代價(jià)以及經(jīng)由新點(diǎn)到達(dá)此鄰域內(nèi)的點(diǎn)是否產(chǎn)生更小的代價(jià)),提出了RRT-star算法；文獻(xiàn)[3]通過(guò)智能采點(diǎn)與路徑優(yōu)化多次迭代RRT算法得到了更短的最終路徑;文獻(xiàn)[4]結(jié)合目標(biāo)偏向采樣與角度度量函數(shù)，并進(jìn)行了剪枝操作與路徑平滑。

通過(guò)引入樹(shù)枝節(jié)點(diǎn)向合力方向擴(kuò)展的機(jī)制提出了改進(jìn)RRT算法，仿真實(shí)驗(yàn)分別對(duì)比了傳統(tǒng)RRT算法、RRT-connect算法、RRT-star算法，結(jié)果表明,改進(jìn)算法得到的路徑更優(yōu)。

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

人工勢(shì)場(chǎng)法是通過(guò)計(jì)算合力機(jī)器人在一個(gè)虛擬勢(shì)場(chǎng)環(huán)境中受到的合力來(lái)決定機(jī)器人的下一步方向，目標(biāo)點(diǎn)在環(huán)境中任一點(diǎn)x產(chǎn)生的吸引力勢(shì)場(chǎng)值為:

(1)

式中:xd----目標(biāo)點(diǎn)坐標(biāo);

kp----引力增益系數(shù)。

如電場(chǎng)力等于電勢(shì)的負(fù)梯度一樣，引力為吸引力勢(shì)場(chǎng)的負(fù)梯度，方向由機(jī)器人指向目標(biāo)點(diǎn),吸引力的大小為:

Fatt(x)=-grad[Uxd(x)]=-kp(x-xd)(2)

單個(gè)障礙物在環(huán)境中任一點(diǎn)x產(chǎn)生的排斥力勢(shì)場(chǎng)值為:

(3)

式中:xobs----目標(biāo)點(diǎn)坐標(biāo);

ε----斥力增益系數(shù);

ρ(x,xobs)----機(jī)器人與障礙物之間的距離。

如電場(chǎng)力等于電勢(shì)的負(fù)梯度一樣，斥力為斥力勢(shì)場(chǎng)的負(fù)梯度，方向由障礙物指向機(jī)器人,機(jī)器人排斥力的大小為:

Frep(x)= -grad[Uo(x)]=

RRT算法是一種隨機(jī)采樣算法，首先將起始點(diǎn)加入樹(shù)中，然后隨機(jī)取一個(gè)點(diǎn)，選擇距離此隨機(jī)點(diǎn)最近的樹(shù)節(jié)點(diǎn)作為待生長(zhǎng)節(jié)點(diǎn)，向此隨機(jī)點(diǎn)方向移動(dòng)一步，若與障礙物相撞則重新選擇隨機(jī)點(diǎn)重新移動(dòng)，重復(fù)此過(guò)程直到有樹(shù)枝節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)附近為止。

2 改進(jìn)RRT算法

2.1 改進(jìn)算法流程圖

改進(jìn)算法在樹(shù)枝節(jié)點(diǎn)擴(kuò)展的過(guò)程中加入了向合力方向移動(dòng)的步驟。首先隨機(jī)產(chǎn)生一個(gè)點(diǎn)，選擇距離此點(diǎn)最近的樹(shù)枝節(jié)點(diǎn)作為待生長(zhǎng)節(jié)點(diǎn)，然后讓這個(gè)被選擇的樹(shù)節(jié)點(diǎn)按照合力方向移動(dòng)(此時(shí)的引力是由目標(biāo)點(diǎn)產(chǎn)生的，斥力是由落在障礙物中的隨機(jī)點(diǎn)產(chǎn)生的)，合力方向中的斥力是由每次落在障礙物中的隨機(jī)點(diǎn)或樹(shù)節(jié)點(diǎn)的所有坐標(biāo)提供的，即將所有隨機(jī)點(diǎn)產(chǎn)生在障礙物中的點(diǎn)記錄下來(lái)當(dāng)作障礙物位置,每移動(dòng)一步，同時(shí)記錄當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)與移動(dòng)代價(jià)(移動(dòng)代價(jià)等于父節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的直線距離加父節(jié)點(diǎn)的代價(jià)值)，直到此樹(shù)枝節(jié)點(diǎn)與障礙物碰撞或陷入局部極值而停滯不前為止。若此情況(樹(shù)枝節(jié)點(diǎn)與障礙物碰撞或停滯不前)出現(xiàn)，則隨機(jī)產(chǎn)生點(diǎn)選擇最近的樹(shù)枝節(jié)點(diǎn)，將最近的樹(shù)枝節(jié)點(diǎn)向此隨機(jī)點(diǎn)方向成功移動(dòng)一步，同時(shí)記錄當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)(即距離此隨機(jī)點(diǎn)最近的樹(shù)枝節(jié)點(diǎn))與移動(dòng)代價(jià)(移動(dòng)代價(jià)等于父節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的直線距離加父節(jié)點(diǎn)的代價(jià)值)，之后此擴(kuò)展過(guò)程結(jié)束，產(chǎn)生下一個(gè)隨機(jī)點(diǎn)，循環(huán)執(zhí)行上述過(guò)程。

改進(jìn)算法流程如圖1所示。

2.2 檢測(cè)是否陷入局部最優(yōu)

通過(guò)判斷人工勢(shì)場(chǎng)法執(zhí)行時(shí)產(chǎn)生的下一點(diǎn)的坐標(biāo)與當(dāng)前距離最近的樹(shù)節(jié)點(diǎn)的距離，若小于給定值d,如d=1，則判斷為此節(jié)點(diǎn)陷入局部最優(yōu)。

圖1 改進(jìn)算法流程圖

2.3 改線

上述過(guò)程執(zhí)行完畢后，在從目標(biāo)點(diǎn)向起點(diǎn)不斷找父節(jié)點(diǎn)的過(guò)程中，基本RRT算法不斷遍歷父節(jié)點(diǎn)直到起始點(diǎn)，這里改為從目標(biāo)點(diǎn)開(kāi)始，檢測(cè)所有與當(dāng)前節(jié)點(diǎn)直線連通的樹(shù)節(jié)點(diǎn)，找到滿足條件即與當(dāng)前節(jié)點(diǎn)直線連通的所有樹(shù)節(jié)點(diǎn)中代價(jià)最小的節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)，然后令此父節(jié)點(diǎn)為當(dāng)前節(jié)點(diǎn)，直到起始點(diǎn)。這里的代價(jià)指的是從起始點(diǎn)到這個(gè)點(diǎn)的目前發(fā)現(xiàn)的經(jīng)過(guò)的樹(shù)枝路徑長(zhǎng)度代價(jià)。

3 仿真實(shí)驗(yàn)

利用Matlab2014a分別比較了改進(jìn)算法與傳統(tǒng)人工勢(shì)場(chǎng)法在步長(zhǎng)為10的情況下，得到了傳統(tǒng)RRT算法、RRT-connect算法、RRT-star算法，改進(jìn)RRT算法的路徑長(zhǎng)度與算法運(yùn)行時(shí)間。起點(diǎn)坐標(biāo)為左下角的點(diǎn)(5,5)，終點(diǎn)坐標(biāo)為右上角的點(diǎn)(95,95)，黑線代表最終路徑，給定地圖下各算法運(yùn)行結(jié)果如圖2所示。

(a) RRT算法搜索結(jié)果 (b) RRT-connect算法搜索結(jié)果

隨機(jī)地圖下各算法運(yùn)行所得到的樹(shù)枝與路徑如圖3所示。

圖2算法結(jié)果對(duì)比見(jiàn)表1。

表1 算法結(jié)果對(duì)比

圖3算法結(jié)果對(duì)比見(jiàn)表2。

表2 算法結(jié)果對(duì)比

4 結(jié) 語(yǔ)

將合力引導(dǎo)機(jī)制加入RRT算法的樹(shù)節(jié)點(diǎn)擴(kuò)展過(guò)程中，以RRT算法中檢測(cè)到的障礙物坐標(biāo)為產(chǎn)生排斥力的坐標(biāo)位置，采用了改線機(jī)制。仿真結(jié)果顯示，改進(jìn)算法在路徑長(zhǎng)度、采樣點(diǎn)數(shù)量、算法運(yùn)行時(shí)間方面都得到了可行的運(yùn)行結(jié)果。