基于改進(jìn)RRT路徑規(guī)劃算法*

李金良，舒翰儒，劉德建，徐 磊

(山東科技大學(xué)機械電子工程學(xué)院，山東 青島 266560)

0 引言

隨著人工智能的不斷推進(jìn)，機器人技術(shù)得到了飛速的發(fā)展，智能化成為未來機器人領(lǐng)域的新航標(biāo)。其中，機器人技術(shù)重要的研究領(lǐng)域—路徑規(guī)劃，也開始迅速發(fā)展。

路徑規(guī)劃主要研究在障礙物空間環(huán)境下，機器人自主搜尋一條從起點到目標(biāo)點的可行路徑或最優(yōu)路徑[1]。機器人的路徑規(guī)劃算法有圖搜索法[2]、可視圖法[3]、人工勢場法[4]、概率路圖法[5]、快速擴展隨機樹法等。因快速擴展隨機樹法更適合移動機器人和多自由度工業(yè)機器人路徑規(guī)劃而被廣泛應(yīng)用及改進(jìn)。

王中玉等[6]改進(jìn)傳統(tǒng)A*算法的評價函數(shù)，減少了一定的計算量；通過改變評價函數(shù)的權(quán)重比例，減少了不必要的點的搜索，但在三維空間中依然存在計算量大的問題。宋宇等[7]通過人工勢場法和RRT算法相結(jié)合，以合力方向引導(dǎo)搜索樹的擴張，可以減少采樣點的個數(shù)和搜索路徑長度，但搜索時間明顯增加。代彥輝等[8]通過引入起始點到目標(biāo)點的方向向量，使得搜索樹擴展具有方向性，雖然大大減少了算法的迭代次數(shù)和搜索時間，但極容易陷入障礙物包圍圈內(nèi)，從而導(dǎo)致算法的失敗。

本文提出一種目標(biāo)偏向的RRT算法，結(jié)合上述一些算法作出以下改進(jìn)：基本RRT算法搜索具有盲目性，因此引入目標(biāo)偏向策略[9]，以減少采樣點的數(shù)目；通過對采樣點距離和速度約束，減少搜索范圍，提高計算效率；對規(guī)劃后的路徑采用貪心算法[10]簡化路徑并對其用三次B樣條曲線[11]進(jìn)行優(yōu)化。

1 RRT算法

RRT算法是以空間中起點為根節(jié)點，通過特殊的增量方式進(jìn)行擴展，生成一棵隨機擴展樹，判定隨機擴展樹上的采樣點是否包含目標(biāo)點或在目標(biāo)范圍內(nèi)，直到該條件滿足，隨機擴展樹就很快覆蓋起點和終點的所有位姿空間，當(dāng)樹長好后，路徑也就找到了，而且起點到終點有且僅有一條路徑?；綬RT算法擴展示意圖如圖1所示。

圖1 RRT算法擴展示意圖

RRT算法步驟：在空間環(huán)境中，定義一個起點xinit；在空間環(huán)境中，隨機確定一個點xrand；若點xrand不在障礙區(qū)范圍內(nèi)，則連接點xinit和點xrand，得到一條連接直線L；若直線L沒有和障礙物發(fā)生碰撞，則沿著直線L，從xinit向xrand的方向擴展一定的距離λ，得到新的點xnew，該點被添加到已經(jīng)存在的擴展樹上；重復(fù)以上步驟，直到目標(biāo)點被添加在擴展樹上或在一定誤差范圍內(nèi)，于是可以尋找到起點到終點唯一一條路徑。

在上述算法需要引起注意的是：在隨機擴展樹上，初始的隨機數(shù)只包含一個根節(jié)點；隨機選擇一個采樣點，在目標(biāo)點和這個采樣點中創(chuàng)建一個新的節(jié)點(如果這個新節(jié)點和障礙物之間發(fā)生碰撞則取消該節(jié)點，如果沒有碰撞則把這個節(jié)點加入到隨機擴展樹中)；在擴展過程中為了控制算法，加入了運行時間上限和搜索節(jié)點個數(shù)上限條件，如果沒有在規(guī)定條件內(nèi)達(dá)到目標(biāo)點，判定算法失敗。

2 改進(jìn)RRT算法

在基本RRT算法的基礎(chǔ)上，采用目標(biāo)偏向策略減少樹擴展的隨機性；添加采樣點時角度約束，減少計算量；并對規(guī)劃后的節(jié)點進(jìn)行貪心策略，簡化搜索路徑長度。

改進(jìn)RRT算法步驟：

第1步：在空間地圖中，定義起點xinit，終點xgoal，偏向概率p，擴展步長λ，最大迭代次數(shù)kmax。

剛戀愛那會兒，林藍(lán)與大趙也有說不完的話。那時他倆都是初入職場的小白領(lǐng)，林藍(lán)和父母住，大趙在離林家兩站路的地方租了一間出租屋。每次約會大趙送林藍(lán)回家最有意思：往往都走到門口了，兩個人覺得剛剛聊的話題還沒說完，于是又手牽手往回走，由林藍(lán)送大趙回家去，大趙怎么會允許女朋友送自己呢？于是到地兒又把林藍(lán)送回來。

第2步：檢測xinit與xgoal之間的距離是否小于λ，若條件成立，則對擴展樹采用貪心策略剪枝，生成新的路徑節(jié)點；若條件不成立，進(jìn)行下一步。

第3步：以一定的偏向概率p選擇采樣點xrand，并根據(jù)改進(jìn)度量函數(shù)確定就近點xnear。

第4步：在xnew和xrand之間的連線L上找到新節(jié)點，xnew沿著直線L，從xinit向xrand的方向擴展一定的距離λ，得到新的節(jié)點xnew。

第5步：判斷擴展樹上的所有節(jié)點是否達(dá)到終點，若不能，返回第3步；若能，進(jìn)行下一步。

第6步：對已經(jīng)生成的擴展樹采用貪心策略，生成新的路徑點。

圖2為改進(jìn)RRT算法流程圖。

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

2.1 擴展步長

在RRT算法中，xnew是由xnear向xrand擴展一定步長得到的，下式為采樣點求得公式：

(1)

其中，λ為擴展步長，d(xrand,xnear)為點xrand與點xnear之間的歐幾里得距離。

2.2 目標(biāo)偏向策略

基本RRT算法的采樣點的范圍很大，且隨機性強，隨機樹在擴展方向上具有盲目性，缺乏一定的引導(dǎo)性，導(dǎo)致算法搜索時間較長。所以，采用目標(biāo)偏向策略，以一定的概率P把目標(biāo)點作為采樣點進(jìn)行隨機樹擴展，提高隨機樹向目標(biāo)點的擴展的概率，以達(dá)到減少采樣點的個數(shù)，加速隨機擴展樹的形成。

2.3 度量函數(shù)

度量函數(shù)的目的是在隨機擴展樹上找到一點xnear離采樣點xrand最近的哪一個。其中，基本RRT算法的度量函數(shù)是歐幾里得距離。在改進(jìn)的RRT算法中除了考慮兩點之間的距離，還引入了角度約束，使得節(jié)點的確立對整條路徑平滑有著重要作用。

由于距離和角度的量綱不同，所以將其進(jìn)行歸一化處理。下式中，D(d)為規(guī)劃后的距離函數(shù)，G(θ)為規(guī)劃后的偏角函數(shù)。在已形成的搜索擴展樹上的臨近的節(jié)點，使得M函數(shù)取得最小值的節(jié)點，即可作為xnear。如圖2所示節(jié)點選擇圖。

圖3 改進(jìn)RRT算法節(jié)點擴展圖

D(d)=(dmax-d)/dmax

(2)

G(θ)=(θmax-θ)/θmax

(3)

(4)

(5)

M(xi,xj)=D(d)d(i,j)+G(θ)Δθ(i,j)

(6)

其中，xnear坐標(biāo)為(xi,yi)，xrand坐標(biāo)為(xj,yj)，dmax和θmax為搜索最大值。

2.4 路徑的剪枝及平滑

基本RRT算法采樣點的選擇具有隨機性，生成的路徑節(jié)點眾多，存在一些不必要的冗余節(jié)點。在障礙物較多的復(fù)雜空間下，隨著采樣節(jié)點的增多，會導(dǎo)致機器人運動無法得到有效的跟蹤。為此，改進(jìn)RRT算法對已生成的路徑節(jié)點進(jìn)行了剪枝，刪除了冗余節(jié)點，最后通過貪心算法對剪枝后的路徑進(jìn)行簡化。

具體修剪步驟：對初次規(guī)劃出的節(jié)點集合，從起點xinit開始，依次遍歷后面的節(jié)點xk，如果兩個節(jié)點的連線沒有與障礙物發(fā)生碰撞，則刪除點xinit與點xk之間的所有節(jié)點；如果兩個節(jié)點的連線與障礙物發(fā)生碰撞，則要從點xi的父節(jié)點開始重復(fù)上述過程，直到終點xgoal添加到新路徑中，最后將新的節(jié)點連接成路徑。

上述形成的路徑并不符合機器人運動學(xué)規(guī)律，因此在此基礎(chǔ)上對曲線進(jìn)行平滑。B樣條曲線的曲率具有一定的連續(xù)性，能夠更好地控制曲線的平滑度，并在實際工程中得到廣泛應(yīng)用。

K次B樣條曲線表示：

(7)

其中，pi為曲線控制的節(jié)點，Bi,k(u)為K次B樣條基函數(shù)，節(jié)點向量U=[u0,u1,…,un+k+1]確定K次分段曲線。

三次B樣條曲線的基函數(shù)：

(8)

3 仿真驗證

為了驗證改進(jìn)RRT算法的可行性和優(yōu)越性，通過MATLAB語言進(jìn)行了仿真驗證。通過基本RRT算法與改進(jìn)RRT算法在步長λ=0.4的條件下，得到在地圖1、地圖2、地圖3路徑規(guī)劃的長度、節(jié)點個數(shù)以及搜索時間。起點坐標(biāo)(0,0)，終點坐標(biāo)(10,10)，紅色代表最終規(guī)劃出的可行路徑，圖4為各個算法在不同地圖的路徑規(guī)劃圖。

(a) 地圖1基本RRT算法 (b) 地圖2基本RRT算法 (c) 地圖3基本RRT算法

(d) 地圖1改進(jìn)RRT算法 (e) 地圖2改進(jìn)RRT算法 (f) 地圖3改進(jìn)RRT算法圖4 不同算法路徑規(guī)劃圖

在仿真結(jié)果中，如表1、表2、表3所示仿真數(shù)據(jù)，基本RRT算法和改進(jìn)RRT算法在搜索時間、搜索路徑長度、迭代次數(shù)上有著明顯的差異。改進(jìn)RRT算法在這三個維度衡量標(biāo)準(zhǔn)下有著更顯著的優(yōu)勢。隨著地圖障礙物的增加，改進(jìn)后的RRT算法在路徑規(guī)劃中更加具有優(yōu)勢，在地圖3中，與基本RRT算法相比：搜索時間縮短53.8%，路徑長度縮短了14%，迭代次數(shù)減少了74.9%。以此證明了改進(jìn)RRT算法的可行性和優(yōu)越性。

表1 不同算法搜索時間表

表2 不同算法搜索路徑長度表

表3 不同算法迭代次數(shù)表

4 軌跡平滑仿真

為了使得路徑更符合機器人運動學(xué)規(guī)律，將改進(jìn)RRT算法規(guī)劃出的路徑進(jìn)行三次B樣條曲線擬合，如圖5所示。

(a) 地圖1路徑優(yōu)化 (b) 地圖2路徑優(yōu)化 (c) 地圖3路徑優(yōu)化圖5 三次B樣條曲線優(yōu)化圖

5 結(jié)論

針對靜態(tài)全局空間下機器人的路徑規(guī)劃問題，提出了一種改進(jìn)RRT算法。該算法通過搜索范圍的約束、剪枝運算以及貪心算法的應(yīng)用達(dá)到了對路徑的修正，最后通過三次B樣條曲線對路徑進(jìn)行優(yōu)化，使其更符合機器人運動學(xué)規(guī)律。

通過仿真驗證，與基本RRT算法對比，證明了改進(jìn)RRT算法的正確性和可行性，對機器人運動規(guī)劃具有一定的參考價值。