基于RRT 路徑規(guī)劃算法的改進(jìn)方法研究＊

何佳，夏海鵬，劉修知

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

前言

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

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

1 基本RRT 算法

RRT 算法以一個(gè)初始點(diǎn)作為根節(jié)點(diǎn)，通過隨機(jī)采樣增加葉子節(jié)點(diǎn)的方式，生成一個(gè)隨機(jī)擴(kuò)展樹，當(dāng)隨機(jī)樹中的葉子節(jié)點(diǎn)包含了目標(biāo)點(diǎn)或進(jìn)入了目標(biāo)區(qū)域，便可以在隨機(jī)樹中找到一條從初始點(diǎn)到目標(biāo)點(diǎn)的路徑。基本RRT 算法流程圖如圖1 所示。

（1）初始化樹空間

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

圖1 基本RRT 算法流程圖

（2）構(gòu)造搜索樹空間

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

（3）生成路徑

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

2 基于基本RRT 算法改進(jìn)

2.1 偏向目標(biāo)的RRT 算法

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

圖2 偏向目標(biāo)的RRT 算法

2.2 雙向RRT 算法

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

圖3 雙向RRT 算法

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

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

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

加入目標(biāo)引力思想的RRT 算法新節(jié)點(diǎn)位置的計(jì)算公式如（2）所示。

其中，ρ1為隨機(jī)點(diǎn)方向上的步長，ρ2為目標(biāo)點(diǎn)方向上的步長。

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

圖4 單樹、雙樹仿真時(shí)間對比

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

3 RRT 算法與其他算法結(jié)合

3.1 融合A＊算法的改進(jìn)

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

圖6 融合A＊算法引導(dǎo)域的RRT 算法

3.2 融合曲線平滑連接的改進(jìn)

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

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

4 總結(jié)

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