復雜多障礙物環境下改進的RRT*路徑規劃算法

余艷碧

（重慶師范大學數學科學學院，重慶 401331）

0 引言

近幾年來，路徑規劃算法在移動機器人領域得到廣泛的研究，大量經典的路徑規劃算法被國內外學者提出。包括基于構建虛擬勢場的人工勢場法［1］，基于計算智能的蟻群算法［2］、遺傳算法［3］、基于圖搜索的A*算法［4］，等。然而，上述算法在解決復雜多障礙環境下或高維狀態空間下的路徑規劃問題時，搜索效率都明顯降低。

基于采樣的路徑規劃算法被提出，目的是提高算法在復雜多障礙物環境或高維狀態空間下搜索能力。快速擴展隨機樹（RRT）［5］是當前應用最廣泛的基于采樣的算法，作為概率完備的快速搜索算法，RRT可以彌補上述算法搜索速度慢等缺陷。然而，由于傳統的RRT算法是均勻采樣的，引導信息不足，從而算法在復雜多障礙物環境下搜索的路徑復雜曲折，且耗費時間較長［6］。為了解決基本RRT算法的缺陷，國內外學者提出了許多改進算法。LaValle和Kuffner通過從起始點和目標點兩個方向同時向對方擴展形成兩棵隨機樹，提出了Bi-RRT算法［5］，之后在此基礎上增加貪婪策略，提出了RRT-connect算法［7］，提高了節點擴展效率。

由于RRT的最大缺陷是沒有考慮可行解的成本，沒辦法搜索出最優路徑。Karaman和Frazoli在2011年提出了漸進最優的RRT*算法［8］，該算法在RRT擴展樹節點的基礎上根據最優標準來調整隨機樹中的節點。雖然確保了近似最優性，但是該算法仍需對所有狀態空間進行采樣來搜索路徑，增加了不必要的內存和計算。為了提高RRT*的搜索效率，可以通過搜索一條從起始點到目標點的初始引導路徑來確定采樣區域，比如從智能采樣和路徑優化兩方面改進RRT*的RRT*-smart算法［9］和借鑒RRT-Connect雙向擴展樹優勢的B-RRT*算法［10］，以及Gammell等提出的Informed-RRT*算法［11］，該算法利用RRT*找到初始引導路徑，然后生成由起始點、目標點和路徑長度決定的橢圓作為采樣區域，從而加速收斂以獲得最優路徑。然而，在復雜的多障礙環境中，Informed-RRT*算法在搜索初始引導路徑時容易因為過多的節點導致占用大量內存，搜索效率較低。

綜合已有研究成果和存在的問題，本文提出了一種針對復雜多障礙物環境下基于A*引導域的改進RRT*路徑規劃算法。該算法將A*算法的最優性和RRT*算法快速性結合起來，首先利用A*算法在柵格化后的低分辨率地圖中搜索出最優路徑，并將該路徑作為引導路徑生成引導域，限制RRT*算法的采樣區域，從而提高其采樣效率，然后在引導域中不斷迭代搜索，得到一條漸進最優且無碰撞的路徑，最后基于B樣條曲線的路徑優化，使移動機器人能夠平穩安全到達終點。通過對RRT*算法與Informed-RRT*算法的仿真比較實驗，驗證本文算法的有效性和優越性。

1 RRT*算法基本原理

1998年，LaValle教授根據最優控制、隨機采樣算法等理論提出了一種隨機采樣增量式運動規劃算法，即RRT算法［5］。該算法是從起始點開始在狀態空間中隨機采樣向目標點擴展的一個樹狀結構，直至找到一條連接起點與終點的路徑。RRT算法的搜索方法避免了對整個環境地圖的模型構建，加快了算法的收斂，在全局路徑規劃中有廣泛的應用和改進，其算法的有效性和完備性已經得到了充分的驗證，但算法仍然存在搜索效率低，路徑不優，占用內存過大等問題。

Karaman和Frazzoli于2011年提出漸進最優性（最短距離）的RRT*算法［8］，該算法基本結構和RRT算法相似，都將初始狀態作為根節點增量式地擴展隨機樹，在避開障礙物的同時，當樹的節點到達預設目標狀態附近區域則結束對狀態空間的搜索。隨機樹T最初只有一個根節點qinit［圖1（a）中節點0］，每次迭代都會在狀態空間中隨機采樣一個點的qrand，并計算隨機樹T中距離qrand最近的節點qnear［圖1（a）中節點5］，如果從最近鄰節點qnear向qrand擴展一定預設步長移動可達即無碰撞，那么這個可達的點記為新節點qnew［圖1（a）中節點9］，并將qnew加入隨機樹T中，此時qnear成為qnew的父節點，RRT算法便是重復上述過程最終規劃出路徑。但RRT*算法在這里會啟動重選父節點過程，即qnear不一定是最后的父節點。當確定qnew后，根據近鄰準則找出qnew周圍一組近鄰節點［圖1（b）中節點4、5、7、8］，并計算Qnew中每一個近鄰節點到根節點qinit的累積成本與該近鄰節點到qnew的距離成本的和，取這些和的最小值所對應的近鄰節點作為qnew的新父節點q′near［例如圖1（a）中，qnew經過節點5索引到qinit的路徑為9-5-2-0，累積成本為9；qnew經過節點7索引至qinit的路徑為9-7-2-0，累積成本為7，代價最小，故節點7為qnew新的父節點q′near，如圖1（b）所示］，并以q′near作為父節點將qnew加入隨機樹T中。接下來優化Qnew中每一個近鄰節點，具體是讓qnew代替每一個近鄰點的父節點，如果此時近鄰點到根節點qinit的累積成本比qnew代替前更小，則放棄原先的父節點而把qnew作為該點的父節點重新連接到近鄰節點上［如圖1（c）中，節點8以qnew為父節點時索引至qinit的路徑為8-9-7-2-0，累積成本為8，而不以qnew為父節點時索引至qinit的路徑為8-5-2-0，累積成本為10，故修剪隨機樹T，將節點8的父節點改為qnew，如圖1（d）所示］，從而對隨機樹T實現逐步優化。

圖1 RRT*算法修剪節點示意圖

2 A*算法

2.1 A*算法基本原理

A*算法是一種基于深度優先算法和廣度優先算法的啟發式融合搜索算法，基于深度優先算法能以最快速度找到一條從起始點到目標點的路徑，但不能保證最優性，基于廣度優先算法則必然能找到最短路徑，但需要遍歷所有結點，計算復雜。而A*算法融合了這兩種算法的優點，基于啟發函數構建了代價函數，考慮了新結點距離初始點和目標點的代價［12］。

2.2 低分辨率環境地圖下的A*算法

A*算法是在柵格化后的環境地圖中搜索，搜索過程中將每一個柵格作為一個狀態，當柵格尺寸越大，即地圖分辨率越低，則環境地圖分解成的狀態數就越少，使得當起始點和目標點坐標不變時，A*算法執行的時間會變短，規劃出來的路徑也會發生相應變化［6］。圖2是大小為128×128的環境地圖，起始點坐標（11，11），目標點坐標（100，100）。對于圖中復雜的障礙物環境，本文分別進行128×128，64×64和43×43的柵格化，A*算法在三種不同分辨率下的規劃結果如圖2（a）、（b）、（c）所示。

圖2 三種分辨率下A*算法規劃結果

在三種不同分辨率的環境地圖下，A*算法規劃時間分別為477 ms、64 ms和23 ms，由此可以看出柵格地圖分辨率越小，A*算法的規劃時間大幅減少，更重要的是，A*算法在不同分辨率下規劃出來的路徑趨勢是一致的。

3 基于A*引導域的RRT＊算法

3.1 A*引導域

記A*算法規劃路徑經過的第i個柵格中心點坐標為A i（x i,y i），則引導域上界點集定義為：

下界點集定義為：

k為單個引導路徑點橫縱坐標移動長度，再將上界點集和下界點集分別按順序連接成一條線得到上界和下界，最后連接上下界形成封閉引導域。圖3為引導域示意圖，藍色線條為A*算法規劃出的引導路徑，紅色線條內的區域為生成的引導域。

圖3 A*算法生成的引導域

3.2 采樣策略

由于RRT*算法采樣范圍大且隨機性強，缺乏一定的引導性，因此本文在3.1節引導域的基礎上，采用目標偏向策略，以一定的概率p選擇目標點作為采樣點進行隨機樹擴展，提高隨機樹向目標點擴展的概率，以達到減少采樣點的個數，加速隨機樹的形成。

在采樣過程中，為了判斷采樣點是否在引導域外，即判斷采樣點qnew和其父節點qnear連線與引導域邊界是否發生碰撞，需進行碰撞檢測。如果發生碰撞，則放棄此次采樣重新選擇采樣點。如圖4，檢測是否碰撞，即判斷線段qnearqnew和線段p1p2是否相交可以利用向量的叉乘。先固定線段qnearqnew，然后以qnear為軸，計算與是否異號；再固定線段p1p2，然后以p1為軸，計算是否異號。當上述的叉乘都異號的時候，說明兩條線段相交，發生碰撞，則重新選擇采樣點。

圖4 碰撞檢測

3.3 基于B樣條曲線的路徑優化

由于RRT*算法隨機采樣的特性，導致算法最終搜索出來的路徑一般是曲折、不平滑的，不利于移動機器人進行路徑追蹤。而B樣條曲線具有連續性、局部可調整性等優點，使得該曲線在路徑規劃中被廣泛的使用［13］，因此本文提出了基于三次B樣條曲線的路徑優化處理，與算法相結合生成一條平穩光滑的路徑，確保移動機器人運動的連續性和平穩性。

Riesenfeld等人在1972年構造了B樣條曲線，將Bernstein基函數用B樣條基函數來代替。B樣條曲線是分段組成的，每一段的參數區間為[0,1]，因此克服了貝塞爾曲線改變任意控制點其曲線上所有點也改變的缺陷，使得樣條曲線具有局部修改的特性［14］，這種優點使得B樣條曲線廣泛應用于路徑規劃中。

設有n+1個控制點P0,P1,…,P n，則k階B樣條曲線的數學表達式為：

其中，B i,k（u）是第i個k階B樣條基函數，u是自變量。

基函數具有如下Cox-deBoor遞推式

式（4）和式（5）中u i是一組被稱為節點矢量的非遞減序列的連續變化值。

4 仿真實驗與結果分析

為驗證本文提出的復雜多障礙物環境下基于A*引導域的RRT*算法搜索路徑的有效性和快速性，本文在復雜環境地圖下對該算法的規劃結果與RRT*、Informed-RRT*兩種算法的規劃結果進行仿真對比，最后給出所有算法仿真實驗的結果與分析。

本文將仿真地圖大小設置為128×128，起始點坐標（11，11），目標點坐標（100，100），障礙物由多個矩形和若干線條組成，分別對本文算法，RRT*算法和Informed-RRT*算法進行10次仿真，最大迭代次數為1000次，擴展步長為5，目標采樣率為10%，并將三種算法找到初始路徑時的初始路徑平均長度和初始平均搜索時間以及到達最大迭代次數后的最終路徑平均長度進行對比分析。圖5為三種算法最終規劃出的路徑示意圖，其中紅色曲線為搜索的最終路徑。

圖5 三種算法的最終規劃結果

表1為仿真實驗數據對比。從表中可以看出，本文算法搜索出的初始路徑平均長度和初始平均搜索時間比另外兩種算法的都要少，與Informed-RRT*算法的初始路徑長度和初始搜索時間對比分別減少了5.4%和46.01%，搜索時間大幅下降，這說明本文算法的收斂速度更快。而從達到最大迭代次數后后的最終路徑平均長度上來看，本文算法也是優于另外兩種算法的，并且對比本文算法的初始路徑長度和最終路徑長度，在大部分情況下，本文算法搜索出的初始路徑已經趨于最優。以此證明了本文算法的有效性和優越性。

表1 仿真實驗數據對比

5 結語

針對RRT*算法和其一些改進算法存在的搜索效率低、占用內存過多等不足，本文提出了一種復雜多障礙物環境下基于A*引導域的改進RRT*路徑規劃算法。該算法有效地將A*算法的最優性和RRT*算法快速性結合起來，第一步利用A*算法在柵格化后的低分辨率仿真地圖中搜索出最優路徑，并將該路徑作為引導路徑生成引導域，限制RRT*算法的采樣區域，從而提高其采樣效率，然后在引導域中不斷迭代搜索，得到從起始點到目標點的漸進最優且無碰撞的路徑，最后基于B樣條曲線的路徑優化，生成一條平滑且曲率連續的優化路徑，使移動機器人能夠平穩安全到達終點。最后，通過對RRT*算法與Informed-RRT*算法的仿真比較實驗，驗證本文算法的有效性和優越性。