改進RRT移動機器人路徑規劃算法

2018-12-26 09:58:12王志明

長春工業大學學報 2018年6期

宋宇, 王志明

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

0 引言

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

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

1 人工勢場法與RRT算法

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

(1)

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

kp----引力增益系數。

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

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

單個障礙物在環境中任一點x產生的排斥力勢場值為:

(3)

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

ε----斥力增益系數;

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

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

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

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

2 改進RRT算法

2.1 改進算法流程圖

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

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

2.2 檢測是否陷入局部最優

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

圖1 改進算法流程圖

2.3 改線

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

3 仿真實驗

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