改進RRT移動機器人路徑規(guī)劃算法

宋 宇, 王志明

(長春工業(yè)大學 計算機科學與工程學院， 吉林 長春 130012)

0 引 言

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

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

1 人工勢場法與RRT算法

人工勢場法是通過計算合力機器人在一個虛擬勢場環(huán)境中受到的合力來決定機器人的下一步方向，目標點在環(huán)境中任一點x產(chǎn)生的吸引力勢場值為:

(1)

式中:xd----目標點坐標;

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

如電場力等于電勢的負梯度一樣，引力為吸引力勢場的負梯度，方向由機器人指向目標點,吸引力的大小為:

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

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

(3)

式中:xobs----目標點坐標;

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

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

如電場力等于電勢的負梯度一樣，斥力為斥力勢場的負梯度，方向由障礙物指向機器人,機器人排斥力的大小為:

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

RRT算法是一種隨機采樣算法，首先將起始點加入樹中，然后隨機取一個點，選擇距離此隨機點最近的樹節(jié)點作為待生長節(jié)點，向此隨機點方向移動一步，若與障礙物相撞則重新選擇隨機點重新移動，重復此過程直到有樹枝節(jié)點到達目標點附近為止。

2 改進RRT算法

2.1 改進算法流程圖

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

改進算法流程如圖1所示。

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

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

圖1 改進算法流程圖

2.3 改線

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

3 仿真實驗

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

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

隨機地圖下各算法運行所得到的樹枝與路徑如圖3所示。

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

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

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

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

4 結(jié) 語

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