基于雙向生長改進的RRT 機器人路徑規劃算法

李曉偉，于會山

（聊城大學物理科學與信息工程學院，聊城252059）

0 引言

在移動機器人的研究領域中，路徑規劃算法是最為重要而且不可缺少的組成部分，是移動機器人在障礙物環境下實現自主移動導航的基礎。隨著路徑規劃算法研究的不斷深入，研究人員已經改善并優化了很多傳統的路徑規劃算法，并不斷提出了很多新的路徑規劃算法，使得路徑規劃算法的研究趨勢已經逐步偏向復雜化和智能化[1]。常用的路徑規劃算法包括A*、人工勢場法、概率路線圖（PRM）算法、蟻群算法、遺傳算法以及諸多啟發式算法等[2]。這些算法的收斂速度過于緩慢，需要提前對未知的障礙物空間環境進行建模，當環境改變時無法在未知的障礙空間中使用。

快速擴展隨機樹算法由Steven M.LaValle 于1998年首次提出來[3]。RRT（快速探索隨機樹）算法能有效解決這些傳統路徑規劃算法存在的問題。與其他傳統路徑規劃算法相比，RRT 算法不僅具有快速、高效的特點，而且不需要依賴于環境建模就能夠有效地解決未知復雜障礙物空間和高維動態環境的路徑規劃問題。RRT 是基于空間中樹木分支的創建，它迭代地對新的狀態進行采樣，然后將離每個樣本更近的現有節點指向一個新的樣本，從而形成一個具有分支的樹。即在空間里，從根節點開始，進行任意采樣，當采樣形成的新點為目標點或新點與目標點的距離小于一定步長時，則形成一條由根節點至目標點的路徑[4]。

1 RRT算法

在快速擴展隨機樹中，qinit為樹的根節點（起始點），qgoal為目標點，qrand為任意擴展的隨機節點，qnear為每次擴展時距離qrand最近的節點，qnew為新節點[5]。首先在搜索空間中采用隨機的方式生成隨機樹的隨機擴展節點qrand，然后遍歷當前已有的隨機樹，從樹中的節點里尋找距離qrand最近的節點qnear，在qnear向qrand延伸一定步長之后可以得到新節點qnew，之后需要對新節點進行碰撞檢測，若qnew碰到障礙物便把這個節點舍去；反之若qnew沒有碰到障礙物，且在規定空間內，即把qnew添加到擴展樹中，可知此時qnew的父節點為qnear，按照上述方式繼續擴展，直到qnew和qgoal之間的距離小于一定步長時，便規劃出了一條由根節點至目標點的路徑[6]。基本算法如下所示：

基本的RRT 算法是從根節點開始擴展的RRT 樹搜索規定空間。若有兩棵隨機樹，分別從根節點和目標點來搜索狀態空間，速度會相對較快。因此，提出一種基于雙向擴展的算法，即RRT-connect 算法。

在搜索空間內，定義了兩棵隨機樹。一棵從起始點開始擴展，另一棵從目標點擴展，兩棵隨機樹分別是Ta和Tb，任意選取兩隨機樹Ta和Tb中離qrand最近節點qnear，再從qnear向qrand擴展得到qnew，直到兩棵樹的qnew小于一定步長，則可確定Ta和Tb連通，即路徑規劃成功[7]。其算法如下所示：

RRT 算法在采樣時，使用啟發式采樣，即當隨機樹在擴展時，通過隨機概率p1（0≤p1＜1）確定qrand為qgoal的可能性。當目標點就是采樣點P（qgoal=qrand）=p1，能夠提高RRT 樹對目標點擴展的能力，使隨機樹在生長時更有指向性，樹的生長過程會相對收斂。

隨機樹在擴展新節點時，基本RRT 中qnew的計算如下：

qnew=qnear＋ρ（qrand-qnear）/||qrand-qnear||

ρ 為隨機樹生長時的步長，（qrand-qnear）表示兩向量單位化，||qrand-qnear|||qrand-qnear||為qrand和qnear的歐氏距離。

當把目標引力思想添加到基本RRT 后，qnew的計算如下：

qnew=qnear＋ρ1（qrand-qnear）/||qrand-qnear||+ρ2（qgoal-qnear）/||qgoal-qnear||

ρ1是隨機樹向任意點方向擴展的步長，ρ2是隨機樹向目標點方向擴展的步長，為qgoal和qnear的歐氏距離。

基本RRT 算法中的新節點是通過最近節點向隨機點延伸一個步長得到的，新節點只朝著隨機點生長，可能會偏離目標點，規劃出的路徑耗時長，距離遠。當添加目標引力函數后，qnew也會向qgoal方向生長。當ρ1＞ρ2時，新節點會偏向隨機點方向，這就比較接近基本RRT 算法；當ρ1＜ρ2時，新節點會偏向于目標點方向，加快隨機樹的收斂性。

2 基于雙向生長改進的RRT算法

雙向RRT 算法嘗試擴展兩棵樹到彼此，相比于基本RRT 提高了算法的收斂速度。不過因基本RRT 在擴展時有一定的隨機性，從而導致了雙向RRT 無指向性，缺乏穩定性。針對于雙向生長具有不穩定的問題，提出一種雙向生長改進的路徑規劃算法[8]。該算法將雙向生長和目標引力思想相結合起來。在RRT 雙向生長算法中引入目標吸引函數，目標吸引函數使隨機樹向目標生長。這不僅避免了隨機樹搜索全局空間，很大程度上降低算法的計算復雜度，提高了實時性，改善了路徑的平穩性[9]。

其主要思想是兩棵隨機樹Ta和Tb分別從起始點和目標點開始擴展，若Ta從qinit開始生長，通過隨機擴展的方式得到采樣點，之后再結合目標引力思想擴展得到新節點。Tb從qgoal開始生長，生長時把Ta擴展得到的新節點當作目標點，采用和Ta相同的方式來擴展。Ta和Tb接下來在生長時都把彼此上一步擴展得到的新節點當成目標點，來進行下一步生長，直到兩棵樹的距離小于一定步長，即路徑規劃成功。

圖1 改進RRT構建過程

3 仿真實驗

通過用MATLAB 軟件來驗證文中改進的算法，設置根節點坐標是（0,0），目標點坐標是（490,490）。規劃一條由根節點至目標點的有效路徑。

圖2

表1

圖3

表2

圖4

表3

通過在不同環境下進行仿真試驗，得到如各表中的實驗數據。從而可以得出，相對于基本RRT 算法來說，Bi-RRT 路徑規劃用時和路徑長度更優些。但本文改進之后的算法相比其余兩種算法路徑更優，運行時間更短，接近最優路徑。

4 結語

本文介紹了路徑規劃的原始算法、雙向擴展的方式和加入目標引力思想的雙向擴展算法。本文改進的路徑規劃算法在擴展時減少了路徑的隨機性，讓隨機樹生長更具有目標性和方向性，但其仍然存在許多不足，例如規劃出的路徑不平滑，需要進一步進行完善。