基于改進RRT路徑規劃算法*

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

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

0 引言

隨著人工智能的不斷推進，機器人技術得到了飛速的發展，智能化成為未來機器人領域的新航標。其中，機器人技術重要的研究領域—路徑規劃，也開始迅速發展。

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

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

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

1 RRT算法

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

圖1 RRT算法擴展示意圖

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

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

2 改進RRT算法

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

改進RRT算法步驟：

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

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

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

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

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

第5步：判斷擴展樹上的所有節點是否達到終點，若不能，返回第3步；若能，進行下一步。

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

圖2為改進RRT算法流程圖。

圖2 改進RRT算法流程圖

2.1 擴展步長

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

(1)

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

2.2 目標偏向策略

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

2.3 度量函數

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

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

圖3 改進RRT算法節點擴展圖

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坐標為(xi,yi)，xrand坐標為(xj,yj)，dmax和θmax為搜索最大值。

2.4 路徑的剪枝及平滑

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

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

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

K次B樣條曲線表示：

(7)

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

三次B樣條曲線的基函數：

(8)

3 仿真驗證

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

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

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

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

表1 不同算法搜索時間表

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

表3 不同算法迭代次數表

4 軌跡平滑仿真

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

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

5 結論

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

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