何佳，夏海鵬，劉修知

（中國汽車技術研究中心有限公司，天津 300300）

前言

隨著無人駕駛技術的發(fā)展，無人車的路徑規(guī)劃技術作為其中的重要技術獲得了迅速的發(fā)展[1]。其中傳統(tǒng)的路徑規(guī)劃算法包括人工勢場法、模糊規(guī)則法、遺傳算法、神經(jīng)網(wǎng)絡、模擬退火算法、蟻群優(yōu)化等算法[2]。但這些方法都需要在一個確定的空間內對障礙物進行建模，計算復雜度與機器人自由度呈指數(shù)關系，不適合解決多自由度機器人在復雜環(huán)境中的規(guī)劃問題。

本文主要研究RRT 算法及其改進方法，快速擴展隨機樹（RRT/rapi-dly exploring random tree）算法在1998 年由LaValle 教授提出[3]，通過環(huán)境空間的隨機采樣點，把搜索導向空白區(qū)域，從而尋找到一條從起始點到目標點的規(guī)劃路徑。通過對環(huán)境空間中的采樣點進行碰撞檢測，避免了對環(huán)境空間的建模，能夠有效地解決高維空間和復雜約束的路徑規(guī)劃問題，適合解決多自由度機器人在復雜環(huán)境下和動態(tài)環(huán)境中的路徑規(guī)劃[4]。RRT 算法存在節(jié)點擴展時缺乏引導信息，存在較大的盲目性，導致生成的路徑曲折不連續(xù)，且容易陷入局部極小值等不足。關于RRT 算法的改進提出了很多解決方法，大致可以分為兩大類，一類是基于基本RRT 算法節(jié)點選擇、步長選擇、擴展方向等的改進，另一類與其他路徑規(guī)劃算法融合的改進。

1 基本RRT 算法

RRT 算法以一個初始點作為根節(jié)點，通過隨機采樣增加葉子節(jié)點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節(jié)點包含了目標點或進入了目標區(qū)域，便可以在隨機樹中找到一條從初始點到目標點的路徑?；綬RT 算法流程圖如圖1 所示。

（1）初始化樹空間

初始化隨機樹，輸入初始節(jié)點qstart和目標節(jié)點qgoal，對初始節(jié)點與目標節(jié)點進行連接嘗試，如果初始節(jié)點與目標節(jié)點能夠連接成功，則返回成功生成的路徑。

圖1 基本RRT 算法流程圖

（2）構造搜索樹空間

在環(huán)境空間中隨機生成一隨機節(jié)點qrand，在qrand附近找到距離其最近的qnear，生成沿qnear和qrand方向上，距qnear為Δq的qnew，對qnear到qnew做碰撞檢測，如果沒有碰撞，則將qnew和其與qnear連接的邊添加到搜索樹空間，如此循環(huán)，直至遍歷整個環(huán)境空間。

（3）生成路徑

如果qnew與qgoal重合或者qnew和qnear將qgoal包圍，則已到達終點，在搜索樹空間中將從起點到終點生成的邊連接起來，即為生成的路徑。

2 基于基本RRT 算法改進

2.1 偏向目標的RRT 算法

為了加快隨機樹到達目標點的速度，在隨機樹每次的生長過程中，根據(jù)隨機概率來決定qrand是隨機點還是目標點，使樹的擴展有一個偏向目標節(jié)點的趨勢[5]。具體思想為：在選擇隨機點時，設置參考值ref(在0 到1 之間)，每次得到一個0 到1 的隨機值p，當0

圖2 偏向目標的RRT 算法

2.2 雙向RRT 算法

雙向RRT 算法，顧名思義是在原來單向RRT 算法基礎上又多擴展了一個從目標節(jié)點擴展的隨機樹[6]。具體思想為：首先同時從初始節(jié)點qstart和目標節(jié)點qgoal分別生成兩顆隨機樹，在狀態(tài)空間中均勻的隨機選取一個狀態(tài)節(jié)點qrand，尋找離狀態(tài)節(jié)點最近的節(jié)點進行擴展，通過碰撞檢測嘗試尋找新的節(jié)點qnew加入到第一棵樹中，然后使第二顆樹朝向第一棵樹所生成的新節(jié)點qnew進行多步擴展，試圖將第二顆樹連接上第一棵樹，如果連接成功，從已經(jīng)連接上的兩棵樹中返回一條連接起始點和目標點的路徑。算法仿真如圖3 所示，其中上圖為單樹生成路徑，下圖為雙樹生成路徑，藍色路徑為第一棵樹生成路徑，紅色路徑為第二顆樹生成路徑。且分別采集100 次仿真時間對比如圖4，雙樹比單樹仿真時間平均快一倍。

圖3 雙向RRT 算法

2.3 動態(tài)步長RRT 算法

由于擴展點的隨機選取，規(guī)劃出來的隨機路徑具有很大的隨機性。動態(tài)步長的RRT 算法是在對基本RRT 算法的基礎上，添加動態(tài)步長的改進，改善了RRT 算法生成路徑的不確定性，提高了RRT 算法的避障等能力。具體思想為：基本的RRT 算法新節(jié)點位置的計算公式如（1）所示。

其中，ρ為RRT 生長的最小單位長度，成為步長。

加入目標引力思想的RRT 算法新節(jié)點位置的計算公式如（2）所示。

其中，ρ1為隨機點方向上的步長，ρ2為目標點方向上的步長。

在目標引力思想RRT 算法的基礎上添加了動態(tài)步長的概念。當沒有遇到障礙物時取ρ2>ρ1，使隨機擴展樹朝著目標節(jié)點方向生長；當遇到障礙物時取ρ2<ρ1，使隨機擴展樹朝著隨機點方向生長。算法仿真如下圖4 所示，其中上圖為基本RRT 算法生成的路徑，下圖為動態(tài)步長RRT 算法生成的路徑。

圖4 單樹、雙樹仿真時間對比

圖5 動態(tài)步長RRT 算法

3 RRT 算法與其他算法結合

3.1 融合A＊算法的改進

參數(shù)化RRT 算法在環(huán)境空間中隨機采樣，缺乏引導信息，導致規(guī)劃出的路徑長度、時間都有很大的隨機性。而結合A*算法的最優(yōu)型、完備性和RRT 算法的快速性、便于考慮運動學約束的特性可以解決此問題。具體思想為：首先，對環(huán)境空間進行低分辨率的柵格化處理，然后利用A*算法在低分辨率的柵格地圖上進行路徑規(guī)劃，實時地規(guī)劃出一條最短路徑，然后根據(jù)A*算法規(guī)劃的結果生成引導域[7]。在RRT采樣的過程中，結合A*算法，加入引導域偏向采樣策略，在隨機樹構建過程中，只在引導域中生成路徑，既可以偏向目標節(jié)點生成，又可以生成最優(yōu)路徑。算法仿真如上圖5 所示，其中上圖為基于A*算法生成的引導域，下圖為在引導域內生成的路徑。

圖6 融合A＊算法引導域的RRT 算法

3.2 融合曲線平滑連接的改進

由于基本RRT 算法的隨機性導致生成的路徑不平滑，無法被車輛等實際應用，因此融合曲線平滑連接進行改進[8]，具體思想為：首先采用基于最大曲率約束的剪枝函數(shù)對整棵樹進行處理，刪去不必要的節(jié)點，添加必要的節(jié)點，接著使用B 樣條曲線、回旋曲線、貝賽爾曲線等曲線對RRT 算法生成路徑進行平滑連接，既保證曲率連續(xù)的同時又能滿足車輛等的非完整約束。算法仿真如圖7 所示，其中上圖紅色連線為基本RRT 算法生成路徑，圖7 黑色連線為曲線優(yōu)化后的的路徑。

圖7 平滑連接后的RRT 生成路徑

4 總結

RRT算法雖然具有快速性、便于考慮運動學約束等優(yōu)勢，但是構建擴展樹的隨機性、容易陷入局部極小值等方面還有待改進，總的來說改進方法分為兩大類，一類是基于基本RRT算法節(jié)點選擇、步長選擇、擴展方向等的改進，二是與其它路徑規(guī)劃算法融合的改進。