基于改進RRT算法的快速路徑規(guī)劃

顧子侶，劉 宇，岳 廣,2，孫商文，李銅哨，付嚴(yán)宇

(1.中國人民解放軍空軍航空大學(xué)， 長春 130022； 2.中國人民解放軍78102部隊， 成都 610000; 3.中國人民解放軍32072部隊， 北京 100089)

1 引言

路徑規(guī)劃是實現(xiàn)無人設(shè)備自主導(dǎo)航的關(guān)鍵技術(shù)之一，它是指在具有障礙物威脅環(huán)境中，按一定評價標(biāo)準(zhǔn)，尋找一條從起點到目標(biāo)點的無碰撞路徑。目前常見的路徑規(guī)劃算法主要有Dijkstra算法、A算法、人工勢場法以及包括蟻群算法、遺傳算法、粒子群算法等在內(nèi)的群智能算法。但上述算法存在如下缺陷：Dijkstra算法和A算法需預(yù)先對環(huán)境信進行柵格化處理，算法計算量隨空間范圍擴大呈指數(shù)增長；人工勢場法易陷入局部最優(yōu)；群智能算法需通過預(yù)先確定的代價函數(shù)來進行優(yōu)化求解，強調(diào)路徑最優(yōu)性，當(dāng)規(guī)劃空間約束較多，代價函數(shù)較為復(fù)雜時，往往需要較長收斂時間。

快速擴展隨機樹(rapidly exploring random tree，RRT)算法是Lavalle教授于1998年提出的一種采用增量方式增長的隨機采樣算法。該算法具有如下優(yōu)點：通過隨機采樣點，可搜索整個狀態(tài)空間，具有概率完備性；擴展速度快，空間搜索效率高；通過碰撞檢測方式來使路徑避開障礙物威脅，避免對環(huán)境空間的建模。基于以上優(yōu)點，RRT算法被廣泛應(yīng)用于解決復(fù)雜路徑規(guī)劃問題中。然而由于RRT算法空間搜索的盲目性，使得算法目標(biāo)收斂性差、內(nèi)存計算量大。對此，學(xué)者們提出不同改進策略。文獻[11]借鑒人工勢場中引力場思想，通過給采樣點方向和目標(biāo)點方向分配不同權(quán)重來共同決定節(jié)點擴展方向，然而由于權(quán)重固定，導(dǎo)致算法在復(fù)雜障礙物環(huán)境中容易長時間陷入局部陷阱中。文獻[12]提出一種雙向RRT算法，通過同時構(gòu)建兩棵樹來加快算法搜索速度，但該算法本質(zhì)上仍是盲目搜索，算法的隨機性并未得到有效降低。文獻[13]引入距離約束來決定生成的新節(jié)點是否應(yīng)被添加于隨機樹中，有效縮短路徑長度，但算法規(guī)劃速度仍存在提升空間。文獻[14]提出一種基于概率的目標(biāo)偏置擴展策略，即通過以一定概率將目標(biāo)點作為采樣點，進而使隨機樹也以一定概率朝向目標(biāo)方向擴展，然而由于在每次節(jié)點擴展過程中，具體是選擇隨機擴展方式還是目標(biāo)偏置擴展方式仍是是隨機的，無法針對障礙物，仍存在改善空間。

結(jié)合已有研究成果，在RRT算法基礎(chǔ)上，通過對隨機樹中待擴展節(jié)點以及擴展方向的選取方式進行改進，實現(xiàn)提高算法規(guī)劃速度和降低算法內(nèi)存計算量的目的。另外，通過對初始路徑進行冗余點裁剪以及對冗余點裁剪后的路徑進行B樣條曲線平滑來改善路徑質(zhì)量。

2 RRT算法基本原理

RRT算法基本思想是通過不斷擴展葉節(jié)點的方式來搜索整個規(guī)劃空間，直至隨機樹中的葉節(jié)點到達目標(biāo)區(qū)域時，擴展停止。然后通過節(jié)點回溯便可找到一條由起點到目標(biāo)點的可行路徑，該算法具體擴展方式如圖1所示。

圖1 RRT算法節(jié)點擴展方式示意圖Fig.1 Schematic diagram of RRT algorithm node expansion

其中表示擴展隨機樹，樹上最初只有起點這一個節(jié)點，因此也被稱為根節(jié)點；為隨機采樣點，在每次節(jié)點擴展過程中，由采樣函數(shù)隨機產(chǎn)生；為待擴展節(jié)點，傳統(tǒng)RRT算法中將樹中離隨機采樣點最近的節(jié)點作為，即

(,)≤(,)

式中：為樹上由起點開始向外擴展的第個節(jié)點；為空間中任意兩點歐式距離的計算函數(shù)；為生成的新節(jié)點，由待擴展節(jié)點沿與連線方向擴展一個步長得到，即

如果與連線之間不存在障礙物威脅，則將添加于中，否則，舍棄該節(jié)點，重新產(chǎn)生采樣點來引導(dǎo)隨機樹的擴展方向。如此循環(huán)往復(fù)，直至隨機樹中的葉節(jié)點處于目標(biāo)區(qū)域時，擴展停止，進而找到一條從起點到目標(biāo)點的無碰撞路徑。從算法原理中可以看出，對隨機樹整體生長方向影響最大因素主要為待擴展節(jié)點和擴展方向的選取方式。但在傳統(tǒng)RRT算法中，這兩者主要由采樣點決定，導(dǎo)致算法在路徑搜索過程中具有較大盲目性。

3 改進RRT的路徑規(guī)劃算法

針對傳統(tǒng)RRT算法路徑搜索盲目性較大的缺點，提出基于目標(biāo)啟發(fā)的待擴展節(jié)點選擇策略，并對待擴展節(jié)點的擴展方向作出適當(dāng)改進，使算法具有較強目標(biāo)啟發(fā)性的同時，還能在遇到障礙物威脅時，具備充分隨機性，實現(xiàn)快速路徑規(guī)劃。另外，通過對初始路徑進行冗余點裁剪并對裁剪后的路徑再進行B樣條曲線平滑來改善路徑質(zhì)量。

3.1 基于目標(biāo)啟發(fā)的待擴展節(jié)點選擇策略

傳統(tǒng)RRT算法將擴展隨機樹中與采樣點距離最小的節(jié)點作為待擴展節(jié)點，即待擴展節(jié)點完全由采樣點所處空間位置來決定。由于每輪循環(huán)中，采樣點都是隨機產(chǎn)生，因此待擴展節(jié)點的選取也具有較大隨機性。甚至當(dāng)擴展隨機樹已經(jīng)快接近目標(biāo)點時，由于待擴展節(jié)點選取的隨機性，導(dǎo)致樹中各節(jié)點被選取作為待擴展節(jié)點的概率是相同的。這會使得隨機樹向四周等概率進行擴展，導(dǎo)致生成大量無效節(jié)點。由于每輪擴展均需要計算隨機樹中所有節(jié)點與采樣點的距離來確定待擴展節(jié)點，因此也進一步增加算法內(nèi)存計算量。

通過借鑒A算法中啟發(fā)距離函數(shù)的思想，提出了一種目標(biāo)啟發(fā)的待擴展節(jié)點選取策略。首先，設(shè)隨機樹上任一節(jié)點與采樣點距離為()，與目標(biāo)點距離為()，并令()為兩者距離之和，則：

()=(,)

()=(,)

()=()+()

最后，將具有()最小值的節(jié)點作為待擴展節(jié)點，記為。圖2顯示了2種方法選取待擴展節(jié)點的原理示意圖，可以看出，在待擴展節(jié)點的選取上，通過增加節(jié)點距目標(biāo)點距離()，可以增大距離目標(biāo)點較近節(jié)點被選取作為待擴展節(jié)點的概率，進而達到降低搜索盲目性和提高算法規(guī)劃速度的目的。

圖2 待擴展節(jié)點選取方式原理示意圖Fig.2 Method for selecting nodes to be expanded

3.2 擴展方向確定策略

傳統(tǒng)RRT算法以隨機采樣點所在位置作為擴展方向，隨機性大。為改善RRT算法中擴展方向選取的隨機性，受文獻[11]提出引力場思想(以該策略進行改進的RRT算法常稱為APF-RRT算法)以及文獻[14]中基于概率的目標(biāo)偏置擴展策略(以該策略進行改進的RRT算法常稱為hRRT算法)的啟發(fā)，對節(jié)點擴展方向提出如下改進思路：當(dāng)待擴展節(jié)點確定后，優(yōu)先嘗試以目標(biāo)方向作為擴展方向，若嘗試成功，則直接生成新節(jié)點；若嘗試失敗，則以產(chǎn)生的隨機采樣點方向作為擴展方向，生成新節(jié)點，同時將嘗試失敗的待擴展節(jié)點標(biāo)記為目標(biāo)方向擴展失敗節(jié)點，如圖3所示。若下次該節(jié)點被選取作為待擴展節(jié)點時，直接進行隨機擴展。

圖3 目標(biāo)方向擴展嘗試失敗示意圖Fig.3 Schematic diagram of failed extension attempt in target

3.3 冗余點裁剪

利用RRT算法規(guī)劃得到初始路徑后，由于算法的隨機性，導(dǎo)致生成的初始路徑存在諸多冗余點。若兩不相鄰路徑點之間連線不受障礙物影響，則將兩點之間的路徑點稱為冗余點，如圖4中路徑點+1和+2即為冗余點。

圖4 冗余點示意圖Fig.4 Schematic diagram of redundant points

因此冗余點裁剪原理為：從初始路徑起點(目標(biāo)點)開始，逐一進行碰撞檢測，找出可以無碰撞連接目標(biāo)點(起點)的中間節(jié)點，并將該點作為新一輪起點，再依次尋找能夠無碰撞連接的節(jié)點，直到每段路徑之間不存在冗余點。最后，比較從起點連接目標(biāo)點與從目標(biāo)點連接起點這2種方式所得路徑長度，將具有較短長度的路徑作為冗余點裁剪后的路徑。以從起點開始為例，其具體實現(xiàn)步驟：

1) 輸入初始路徑點序列為{,,…,}，其中為起點位置，為目標(biāo)點的位置；

2) 構(gòu)建冗余點裁剪后的路徑點序列為，初始為空集；

3) 將起點加入中，并令=1，=；

4) 檢測路徑點與之間是否存在障礙物，若存在轉(zhuǎn)入步驟5)，否則，轉(zhuǎn)入步驟6)；

5) 若的值等于+1，則令=+1，=，并轉(zhuǎn)入到步驟4)；否則，=-1，并轉(zhuǎn)入步驟4);

6) 將路徑點加入到集合中，并令=，=;

7) 若=，則裁剪結(jié)束，輸出裁剪冗余點后的序列，否則轉(zhuǎn)入步驟4)。

3.4 基于B樣條曲線的路徑平滑

考慮到路徑平滑性要求，需進一步對冗余點裁剪后的路徑進行平滑處理。B樣條曲線可以將產(chǎn)生的路徑節(jié)點作為B樣條基函數(shù)控制點，生成曲率連續(xù)的平滑路徑。本文采用三次B樣條曲線來對初始路徑進行平滑處理，三次B樣條曲線的表達式為

(1)

式中:+為基函數(shù)控制點，,3()為三次B樣條曲線的基函數(shù)，其表達式為

(2)

令冗余點裁剪后的節(jié)點序列集合為，即={,,…}，其中為起始路徑點位置，為目標(biāo)路徑點位置。將路徑點作為基函數(shù)控制點，并與式(2)一同代入式(1)中可得路徑點與+3之間的三次B樣條曲線段為

3.5 改進算法流程

Step 1：初始化隨機樹，將起點作為的根節(jié)點；

Step 2：在規(guī)劃空間中隨機產(chǎn)生采樣點；

Step 3：計算中節(jié)點與采樣點以及與目標(biāo)點之間的距離之和()，將具有()最小值的節(jié)點作為待擴展節(jié)點；

Step 4：判斷待擴展節(jié)點是否存在目標(biāo)方向擴展失敗的歷史記錄，若存在則轉(zhuǎn)到Step 6；否則轉(zhuǎn)到Step 5；

Step 5：沿與方向以為步長擴展得到，若擴展成功，則將添加于中，并轉(zhuǎn)到Step 7，否則轉(zhuǎn)到Step 6，并將該節(jié)點標(biāo)記為目標(biāo)方向擴展失敗節(jié)點。

Step 6：沿著與方向以為步長擴展得到；若擴展成功則將添加于隨機樹中，否則轉(zhuǎn)到Step 2。

Step 7：判斷(,)≤，若滿足則轉(zhuǎn)到Step 8，否則轉(zhuǎn)到Step 2；

Step 8：跳出循環(huán)，回溯得到初始路徑；

Step 9：對初始路徑進行冗余點裁剪；

Step 10：對裁剪后路徑進行B樣條曲線平滑，得到最終路徑。

4 計算結(jié)果及分析

為驗證改進算法可行性，在MATLAB R2016b中進行算法仿真實驗。實驗所用計算機配置如下：處理器為Intel Core-i5-7200U，主頻為2.71 GHz，內(nèi)存為12 GB，操作系統(tǒng)為64位Windows 10。并設(shè)置2種不同的地圖環(huán)境，地圖大小為800*800(無量綱)，如圖5所示。另外，起點和目標(biāo)點坐標(biāo)分別設(shè)為(100，700)和(700，100)，擴展步長為20。

圖6、圖7分別表示在簡單和復(fù)雜障礙物威脅環(huán)境下利用基本RRT算法、引入目標(biāo)引力策略的改進RRT算法(APF-RRT算法)、基于概率的目標(biāo)偏向擴展策略的改進RRT算法(hRRT算法)以及本文算法得到的路徑規(guī)劃結(jié)果圖，其中APF-RRT算法目標(biāo)方向擴展權(quán)重設(shè)為0.5，hRRT算法中目標(biāo)方向擴展概率設(shè)為0.5。圖中黑色實線為隨機樹的擴展路徑，藍(lán)色實線為得到的初始路徑，紅色實線為經(jīng)冗余點裁剪和B樣條曲線平滑后的最終路徑。

圖5 路徑規(guī)劃空間示意圖Fig.5 Route planning space

圖6 簡單障礙物環(huán)境下4種算法路徑規(guī)劃示意圖Fig.6 Schematic diagram of route planning with four algorithms in simple obstacle environment

圖7 復(fù)雜障礙物環(huán)境下4種算法路徑規(guī)劃示意圖Fig.7 Schematic diagram of route planning with four algorithms in complex obstacle environment

從圖6和圖7可以看出，在離障礙物威脅較遠(yuǎn)區(qū)域，本文算法中隨機樹的路徑擴展分支最少，RRT算法最多，其他2種算法次之。這是由于本文算法在對隨機樹的待擴展節(jié)點以及擴展方向的選取上具有較強的目標(biāo)啟發(fā)性，因此相比于其他3種算法而言，隨機樹在離障礙物威脅較遠(yuǎn)區(qū)域可以更迅速地朝向目標(biāo)區(qū)域生長；另外，在障礙物威脅區(qū)域附近，本文算法產(chǎn)生了大量不同方向的路徑擴展分支，這是因為將目標(biāo)擴展失敗的節(jié)點及時轉(zhuǎn)變?yōu)殡S機方向進行擴展，使得本文算法在障礙物威脅附近隨機樹的擴展方向作出了巨大的改變，以此來使隨機樹的生長快速跳出障礙物威脅的影響，但這也導(dǎo)致了規(guī)劃所得的路徑距離較長、且整體路徑較為曲折震蕩，如圖中藍(lán)色實線所示。

為彌補此不足，對得到的初始路徑進行冗余點裁剪并將裁剪后的路徑進行B樣條曲線平滑，得到圖6、圖7中紅色實線所示的最終路徑，從圖中可以看出路徑質(zhì)量得到有效提高。另外，為更好的驗證算法性能，分別對4種算法在2種障礙物環(huán)境下進行200次仿真實驗，得到的實驗數(shù)據(jù)如表1、表2所示，其中各統(tǒng)計參數(shù)均為平均值。

表1 簡單障礙物環(huán)境下算法路徑規(guī)劃性能實驗數(shù)據(jù)Table 1 Performance comparison of algorithm path planning in simple obstacle environment

表2 復(fù)雜障礙物環(huán)境下算法路徑規(guī)劃性能實驗數(shù)據(jù)Table 2 Performance comparison of algorithm path planning in complex obstacle environment

從表1、表2中可看出，不管是在簡單障礙物威脅還是復(fù)雜障礙物威脅環(huán)境下，本文算法的規(guī)劃時間和節(jié)點擴展數(shù)均是是最少的。且相比于hRRT算法，規(guī)劃速度在簡單威脅環(huán)境中提升36%左右，節(jié)點擴展數(shù)減少29%左右；在復(fù)雜威脅環(huán)境中，規(guī)劃速度提升65%左右，擴展節(jié)點數(shù)減少50%左右。另外，初始路徑經(jīng)冗余點裁剪以及進一步的B樣條曲線平滑處理后，路徑長度得到明顯縮短。

5 結(jié)論

1) 將RRT算法應(yīng)用到路徑規(guī)劃時，針對原有算法規(guī)劃速度慢、內(nèi)存計算量大，對算法中隨機擴展樹的待擴展節(jié)點以及擴展方向的選取上作了一定改進，使得算法在具備目標(biāo)啟發(fā)性的同時，又能在障礙物威脅環(huán)境下具備較大隨機性。

2) 本文提出的改進策略不僅可以提高算法的路徑規(guī)劃速度，還降低了算法的內(nèi)存計算量，存在的不足是規(guī)劃得到的初始路徑長度較長、彎曲度較大。然而，通過對得到的初始路徑進行冗余點裁剪后的路徑進行B樣條曲線平滑，可以有效提高路徑質(zhì)量。