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

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

1 引言

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

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

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

2 RRT算法基本原理

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

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

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

(,)≤(,)

式中：為樹(shù)上由起點(diǎn)開(kāi)始向外擴(kuò)展的第個(gè)節(jié)點(diǎn)；為空間中任意兩點(diǎn)歐式距離的計(jì)算函數(shù)；為生成的新節(jié)點(diǎn)，由待擴(kuò)展節(jié)點(diǎn)沿與連線(xiàn)方向擴(kuò)展一個(gè)步長(zhǎng)得到，即

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

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

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

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

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

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

()=(,)

()=(,)

()=()+()

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

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

3.2 擴(kuò)展方向確定策略

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

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

3.3 冗余點(diǎn)裁剪

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

圖4 冗余點(diǎn)示意圖Fig.4 Schematic diagram of redundant points

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

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

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

3) 將起點(diǎn)加入中，并令=1，=；

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

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

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

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

3.4 基于B樣條曲線(xiàn)的路徑平滑

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

(1)

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

(2)

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

3.5 改進(jìn)算法流程

Step 1：初始化隨機(jī)樹(shù)，將起點(diǎn)作為的根節(jié)點(diǎn)；

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

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

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

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

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

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

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

Step 9：對(duì)初始路徑進(jìn)行冗余點(diǎn)裁剪；

Step 10：對(duì)裁剪后路徑進(jìn)行B樣條曲線(xiàn)平滑，得到最終路徑。

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

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

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

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

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

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

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

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

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

5 結(jié)論

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

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