江 洪，蔣瀟杰

（江蘇大學(xué) 機(jī)械工程學(xué)院，江蘇 鎮(zhèn)江 212013）

隨著計(jì)算機(jī)云計(jì)算等新技術(shù)的快速普及，無人駕駛技術(shù)得到了大力發(fā)展。路徑規(guī)劃作為其關(guān)鍵技術(shù)之一，眾多學(xué)者對其進(jìn)行了理論與技術(shù)上的研究。路徑規(guī)劃問題通?？擅枋鰹椋涸诓加幸欢ㄕ系K物的環(huán)境中，給予車輛起始點(diǎn)以及目標(biāo)點(diǎn)，在遵循車輛運(yùn)動學(xué)、規(guī)劃時(shí)間最短、路徑最優(yōu)等一系列原則下，獲得一個車輛行駛的最優(yōu)方案［1］。經(jīng)典的路徑規(guī)劃算法主要有人工勢場法、數(shù)值規(guī)劃法、可視圖法以及基于隨機(jī)采樣的算法［2］。

人工勢場法（APFA）最早由Khatib提出［3］，其通過構(gòu)建虛擬力場，巧妙地運(yùn)用目標(biāo)點(diǎn)引力和障礙物斥力，規(guī)劃出一條無障礙路線，但此算法容易陷入局部極小值。趙東輝等［4］通過在斥力函數(shù)中添加逃逸力解決局部最小值問題，并利用遺傳算法得到平滑路徑。

隨機(jī)采樣的路徑規(guī)劃算法主要包括Steven.M.LaValle提出的快速隨機(jī)擴(kuò)展樹算法（rapidlyexploring random tree，RRT）［5－6］。其優(yōu)點(diǎn)在于無需對地圖進(jìn)行建模，同時(shí)考慮了無人駕駛汽車的客觀約束［7－8］。然而RRT算法依賴于隨機(jī)點(diǎn)選擇，從而產(chǎn)生的路徑不唯一、運(yùn)算耗時(shí)較長。

為提高RRT算法采樣時(shí)的目標(biāo)導(dǎo)向性，有學(xué)者提出基于目標(biāo)偏向策略的P概率RRT算法［9］，在采樣判斷時(shí)設(shè)定一個參數(shù)Pa，在每次擴(kuò)展前隨機(jī)得到一個（0，1）內(nèi)的隨機(jī)值P并進(jìn)行判斷，當(dāng)0＜P＜Pa時(shí)，隨機(jī)點(diǎn)隨機(jī)產(chǎn)生，當(dāng)Pa＜P＜1時(shí)，隨機(jī)點(diǎn)為目標(biāo)點(diǎn)，隨機(jī)樹朝目標(biāo)點(diǎn)生長，使隨機(jī)樹擴(kuò)展更具目標(biāo)性。龍建全等［10］提出一種基于路標(biāo)引導(dǎo)及增長采樣區(qū)域策略引導(dǎo)算法朝目標(biāo)點(diǎn)搜索。王坤等［11］在Kuffner等提出的RRT-Connect算法［12］基礎(chǔ)上進(jìn)行了一定改進(jìn)，在起始點(diǎn)和目標(biāo)點(diǎn)中間建立第3個節(jié)點(diǎn)，通過生成4棵搜索樹的方式來提高收斂速度。也有研究者在RRT-Connect算法中引入目標(biāo)吸引函數(shù)［13］，使隨機(jī)樹向目標(biāo)生長，縮短時(shí)間。以上改進(jìn)算法雖縮短了算法時(shí)間，但是仍存在路徑節(jié)點(diǎn)數(shù)較多以及路徑較長的問題。因此，針對RRT算法路徑過長問題，有研究者提出刪去路徑中安全的節(jié)點(diǎn)的方法進(jìn)行路徑縮短［14］；王素琴等［15］采用貪心算法去除多余節(jié)點(diǎn)。雖然上述方法縮短了路徑的長度，但依舊是基于原本的RRT路徑進(jìn)行縮減，因此路徑優(yōu)化不顯著。

針對RRT算法中的一些不足，在算法節(jié)點(diǎn)選擇、擴(kuò)展方向、步長選擇上進(jìn)行了改進(jìn)，提出了偏向目標(biāo)點(diǎn)的新節(jié)點(diǎn)擴(kuò)展方法以及剪枝優(yōu)化方法，有效地提高了算法的效率以及優(yōu)化了路徑節(jié)點(diǎn)個數(shù)和路徑長度。首先介紹了傳統(tǒng)RRT算法，然后針對不同的問題，提出相應(yīng)的改進(jìn)規(guī)劃算法，最后通過仿真實(shí)驗(yàn)對比，驗(yàn)證改進(jìn)算法的有效性。

1 RRT算法

1.1 傳統(tǒng)RRT算法

RRT算法將狀態(tài)空間中的起始點(diǎn)qinit作為根節(jié)點(diǎn)，通過隨機(jī)采樣的方式向地圖中的自由空間（非障礙區(qū)）生成一個隨機(jī)點(diǎn)qrand，以隨機(jī)點(diǎn)qrand為目標(biāo)，遍歷隨機(jī)樹上的現(xiàn)存節(jié)點(diǎn)，計(jì)算每個節(jié)點(diǎn)到該隨機(jī)點(diǎn)qrand的距離，尋找到最近的節(jié)點(diǎn)qnear作為父節(jié)點(diǎn)，以固定步長向外擴(kuò)展生成一個新的節(jié)點(diǎn)qnew，新節(jié)點(diǎn)qnew與最近節(jié)點(diǎn)qnear連線之間無障礙物時(shí)，將新節(jié)點(diǎn)qnew添加到樹上成為子節(jié)點(diǎn)，否則重新生成隨機(jī)點(diǎn)qrand。當(dāng)隨機(jī)樹中的子節(jié)點(diǎn)進(jìn)入到目標(biāo)點(diǎn)qgoal的設(shè)定鄰域或者為目標(biāo)點(diǎn)qgoal本身時(shí)，停止擴(kuò)展，最后從目標(biāo)點(diǎn)qgoal開始依次回溯父節(jié)點(diǎn)直到起始點(diǎn)qinit結(jié)束，找到一條從起始點(diǎn)qinit到目標(biāo)點(diǎn)qgoal無障礙路徑。其擴(kuò)展方式如圖1所示。

圖1 傳統(tǒng)RRT擴(kuò)展示意圖

1.2 新節(jié)點(diǎn)擴(kuò)展方法

本文中提出一種將目標(biāo)引力、障礙物斥力和隨機(jī)點(diǎn)引力三合一的變步長RRT算法，并設(shè)計(jì)自適應(yīng)函數(shù)調(diào)節(jié)隨機(jī)點(diǎn)引力與目標(biāo)點(diǎn)引力在新節(jié)點(diǎn)擴(kuò)展過程中的占比，以此提高RRT算法路徑搜索速度和避障能力。具體過程為：隨機(jī)樹向可行區(qū)域隨機(jī)擴(kuò)展一個隨機(jī)點(diǎn)qrand后，尋找到樹上最近節(jié)點(diǎn)qnear，并對其產(chǎn)生隨機(jī)點(diǎn)引力R（q），目標(biāo)點(diǎn)qgoal對節(jié)點(diǎn)qnear產(chǎn)生目標(biāo)引力G（q），障礙物hob對節(jié)點(diǎn)qnear產(chǎn)生障礙物斥力H（q），將這3種力合成為F（q），則合力F（q）的大小作為新節(jié)點(diǎn)qnew步長的大小，合力方向?yàn)樾鹿?jié)點(diǎn)qnew生長方向，當(dāng)新節(jié)點(diǎn)qnew與最近節(jié)點(diǎn)qnear連線之間無障礙物時(shí)，將新節(jié)點(diǎn)qnew添加到樹上成為子節(jié)點(diǎn)，否則重新生成隨機(jī)點(diǎn)qrand。其中，在引力函數(shù)中設(shè)計(jì)了自適應(yīng)引力系數(shù)λ，使隨機(jī)點(diǎn)引力與目標(biāo)點(diǎn)引力在擴(kuò)展過程中的占比根據(jù)最近節(jié)點(diǎn)qnear與障礙物距離的遠(yuǎn)近自適應(yīng)調(diào)節(jié)。新節(jié)點(diǎn)qnew的擴(kuò)展公式為：

圖2 新節(jié)點(diǎn)擴(kuò)展示意圖

在傳統(tǒng)RRT算法上通過隨機(jī)點(diǎn)引力、目標(biāo)點(diǎn)引力、障礙物斥力三力合一來引導(dǎo)新節(jié)點(diǎn)避開障礙物向目標(biāo)點(diǎn)變步長生長，并在隨機(jī)點(diǎn)引力與目標(biāo)點(diǎn)引力公式里添加與障礙物距離有關(guān)的自適應(yīng)函數(shù)，自適應(yīng)調(diào)節(jié)隨機(jī)點(diǎn)引力與目標(biāo)點(diǎn)引力在新節(jié)點(diǎn)擴(kuò)展過程中的占比，避免遇到連續(xù)障礙物時(shí)震蕩不前，使隨機(jī)樹靠近障礙物區(qū)域時(shí)盡快往遠(yuǎn)離障礙物區(qū)域擴(kuò)散，在無障礙區(qū)域時(shí)快速朝目標(biāo)點(diǎn)擴(kuò)展，縮短規(guī)劃時(shí)間，減少采樣點(diǎn)數(shù)。

1.3 剪枝優(yōu)化方法

偏向目標(biāo)點(diǎn)的新節(jié)點(diǎn)擴(kuò)展RRT算法雖然搜索速度很快，但由于是每一次采樣擴(kuò)展的子節(jié)點(diǎn)與父節(jié)點(diǎn)相連回溯生成路徑，使路徑中仍有冗余的節(jié)點(diǎn)，規(guī)劃出的路徑長度并非最優(yōu)。為此本文提出了一種新的RRT剪枝優(yōu)化方法。

基于改進(jìn)的新節(jié)點(diǎn)擴(kuò)展RRT算法，利用估價(jià)函數(shù)搜索出最佳采樣點(diǎn)，從而優(yōu)化出一條更短路徑。所設(shè)計(jì)的剪枝裁剪算法需不斷地更新3個列表，分別是：可選節(jié)點(diǎn)列表、路徑節(jié)點(diǎn)列表以及節(jié)點(diǎn)路徑長度列表。其中，可選節(jié)點(diǎn)列表存儲由算法得到一系列采樣點(diǎn)（起始點(diǎn)與目標(biāo)點(diǎn)不包含在內(nèi)）；路徑節(jié)點(diǎn)列表存儲根據(jù)估價(jià)函數(shù)從可選節(jié)點(diǎn)列表中選擇出具有最小成本的節(jié)點(diǎn)；節(jié)點(diǎn)路徑長度列表存儲起始點(diǎn)到優(yōu)化節(jié)點(diǎn)的回溯路徑長度。

其中，估價(jià)函數(shù)公式如式（8）所示。

式中：qgoal為目標(biāo)點(diǎn)；qselect（i）為可選節(jié)點(diǎn)列表中的預(yù)選節(jié)點(diǎn)；qpath（j）為路徑節(jié)點(diǎn)列表中儲存的路徑節(jié)點(diǎn)；d（i）是qpath（j）到qselect（i）的代價(jià)函數(shù)；h（i）是qselect（i）到目標(biāo)點(diǎn)的啟發(fā)式估計(jì)代價(jià)函數(shù)；L（j）是節(jié)點(diǎn)路徑長度列表中存儲的起始點(diǎn)到qpath（j）回溯路徑長度代價(jià)。

具體裁剪方法流程如下所述：

步驟1把改進(jìn)的RRT算法采集的節(jié)點(diǎn)存入可選節(jié)點(diǎn)列表，同時(shí)將起始點(diǎn)存入路徑節(jié)點(diǎn)列表中。

步驟2依次選取可選節(jié)點(diǎn)列表中的節(jié)點(diǎn)qselect（i）和路徑節(jié)點(diǎn)列表中的節(jié)點(diǎn)qpath（j），并對選取的節(jié)點(diǎn)作如下處理：

1）判斷預(yù)選節(jié)點(diǎn)qselect（i）與節(jié)點(diǎn)qpath（j）之間有無障礙物，同時(shí)計(jì)算兩節(jié)點(diǎn)間的距離，得到d（i）；

2）計(jì)算預(yù)選節(jié)點(diǎn)qselect（i）與目標(biāo)點(diǎn)之間的代價(jià)h（i）；

3）從節(jié)點(diǎn)路徑長度列表找到從起始點(diǎn)到節(jié)點(diǎn)qpath（j）的回溯路徑長度L（j）；

4）計(jì)算預(yù)選節(jié)點(diǎn)qselect（i）的代價(jià)函數(shù)f（i）；

5）比較所有預(yù)選節(jié)點(diǎn)的代價(jià)函數(shù)f（i），得到代價(jià)最小的節(jié)點(diǎn)qselect_min，將其作為此次的擴(kuò)展節(jié)點(diǎn)，同時(shí)刪除可選節(jié)點(diǎn)列表中的該節(jié)點(diǎn)信息；

步驟3將當(dāng)前擴(kuò)展點(diǎn)存入路徑列表中，并且判斷其與目標(biāo)點(diǎn)之間有無障礙物。若無障礙物，擴(kuò)展點(diǎn)與目標(biāo)點(diǎn)相連，將目標(biāo)點(diǎn)存入路徑列表，結(jié)束搜索進(jìn)入步驟4，否則計(jì)算該節(jié)點(diǎn)至起始點(diǎn)的回溯路徑長度，存入節(jié)點(diǎn)路徑長度列表中，然后返回步驟2繼續(xù)搜索；

步驟4從目標(biāo)點(diǎn)開始回溯，由子節(jié)點(diǎn)連接父節(jié)點(diǎn)，直到起始點(diǎn)為止。

2 仿真與分析

為了驗(yàn)證本文方法的可行性，在同一臺筆記本中通過Matlab2020a軟件進(jìn)行仿真實(shí)驗(yàn)。分析比較以下幾點(diǎn)：①尋路時(shí)間；②路徑節(jié)點(diǎn)數(shù)；③路徑長度。對地圖進(jìn)行仿真，地圖中空白區(qū)域代表安全區(qū)，黑色代表障礙物區(qū)，灰色點(diǎn)代表起始點(diǎn)位置，黑色點(diǎn)代表目標(biāo)點(diǎn)位置。

2.1 新節(jié)點(diǎn)擴(kuò)展方法仿真分析

為了分析新節(jié)點(diǎn)擴(kuò)展方法的優(yōu)劣性，本文分別與傳統(tǒng)RRT算法和基于P概率的RRT算法進(jìn)行比較。

圖3是以（4，4）為起點(diǎn)坐標(biāo)，（85，85）為目標(biāo)點(diǎn)坐標(biāo)為例的仿真結(jié)果。從圖3中可以看出，相較于其他2種算法，改進(jìn)算法生成的路徑更具有導(dǎo)向性且生成的節(jié)點(diǎn)數(shù)更少。

圖3 仿真結(jié)果

在實(shí)驗(yàn)中，由于RRT算法生成的路徑具有隨機(jī)性，造成每次生成的時(shí)間、路徑均不相同，單比較一次的實(shí)驗(yàn)結(jié)果無任何意義，因此對每種算法以不同的起始點(diǎn)與目標(biāo)點(diǎn)分別進(jìn)行50次仿真實(shí)驗(yàn)并取其平均值作為比較，重點(diǎn)對尋路時(shí)間、生成路徑節(jié)點(diǎn)數(shù)和路徑長度進(jìn)行分析。圖4為仿真結(jié)果，圖中組號1、2、3、4、5、6分別對應(yīng)起始點(diǎn)與目標(biāo)點(diǎn)坐標(biāo)：（4，4）－（85，85），（10，90）－（85，10），（10，90）－（85，50），（4，4）－（50，90），（5，50）－（85，50），（5，50）－（85，85）。

圖4 50次仿真直方圖統(tǒng)計(jì)

從仿真結(jié)果中可以看出，所提出的新節(jié)點(diǎn)擴(kuò)展方法與傳統(tǒng)RRT算法以及基于P概率的RRT算法相比，有效地減少了尋路時(shí)間和路徑節(jié)點(diǎn)數(shù)，縮短了路徑長度。與基于P概率RRT算法相比，在尋路時(shí)間上縮短了33.2%以上，路徑節(jié)點(diǎn)個數(shù)減少了54.2%以上，路徑長度縮減了9.21%以上。

搜索時(shí)間和路徑節(jié)點(diǎn)減少的原因在于本文改進(jìn)算法為自適應(yīng)變步長算法。在無障礙區(qū)域時(shí)，隨機(jī)樹由目標(biāo)點(diǎn)引力引導(dǎo)朝目標(biāo)點(diǎn)快速發(fā)展，減少了不必要區(qū)域的搜索；當(dāng)靠近障礙物區(qū)域時(shí)，目標(biāo)點(diǎn)引力逐漸減小，隨機(jī)點(diǎn)引力逐漸增大，與斥力函數(shù)一起將隨機(jī)樹新節(jié)點(diǎn)向無障礙區(qū)引導(dǎo)擴(kuò)展跳出局部陷阱，不僅與障礙物保持了安全距離而且避免了不必要的碰撞檢驗(yàn)，縮短了大量時(shí)間。路徑長度縮短則是因?yàn)楸疚乃惴ǜ哂心繕?biāo)偏向性，加之自適應(yīng)步長使路徑節(jié)點(diǎn)大量減少，避免了不必要的路徑的產(chǎn)生。

2.2 剪枝優(yōu)化方法仿真分析

為了分析剪枝優(yōu)化方法的優(yōu)劣性，在新節(jié)點(diǎn)擴(kuò)展方法搜索出基礎(chǔ)路徑上進(jìn)行裁剪，并選擇了相關(guān)度較高的文獻(xiàn)［14］所提方法做對比仿真，主要從路徑長度這一指標(biāo)進(jìn)行對比分析。圖5為路徑裁剪效果仿真圖，表1為裁剪數(shù)據(jù)結(jié)果。

表1 裁剪數(shù)據(jù)結(jié)果

圖5 裁剪方法仿真效果圖

從圖表信息可以看出，剪枝優(yōu)化方法對基礎(chǔ)路徑長度進(jìn)行了明顯的縮減，并優(yōu)于文獻(xiàn)［14］所提方法。由仿真結(jié)果可知，文獻(xiàn)［14］方法將基礎(chǔ)路徑長度縮短了16.12%，而本文的剪枝方法將基礎(chǔ)路徑長度縮短了18.9%。主要是因?yàn)槲墨I(xiàn)［14］的方法是基于隨機(jī)樹上路徑點(diǎn)進(jìn)行裁剪，主體上并沒有偏離基礎(chǔ)路徑的本身，然而路徑上的點(diǎn)并非最優(yōu)點(diǎn)，在RRT算法所有的采樣點(diǎn)中可能存在更優(yōu)節(jié)點(diǎn)，所以本文基于隨機(jī)樹上所有采樣點(diǎn)，通過剪枝優(yōu)化方法搜索出最佳采樣點(diǎn)，從而快速裁剪出一條無障礙物的更優(yōu)路徑。

2.3 融合算法仿真分析

為了分析新節(jié)點(diǎn)擴(kuò)展方法與剪枝優(yōu)化方法融合后算法的優(yōu)劣性，設(shè)置了2種仿真環(huán)境：簡單仿真環(huán)境和目標(biāo)區(qū)域存在較多障礙物的復(fù)雜仿真環(huán)境，本文選擇文獻(xiàn)［13］改進(jìn)方法進(jìn)行對比仿真。

圖6是以（4，4）為起點(diǎn)坐標(biāo)，（85，85）為目標(biāo)點(diǎn)坐標(biāo)的簡單仿真環(huán)境結(jié)果，其中（a）、（b）分別為文獻(xiàn)［13］改進(jìn)算法和本文融合改進(jìn)算法仿真圖。

圖6 仿真結(jié)果

圖7為采用6組不同的起始點(diǎn)與目標(biāo)點(diǎn)各做50次仿真實(shí)驗(yàn)取其平均值的統(tǒng)計(jì)圖，圖中組號1、2、3、4、5、6分別對應(yīng)起始點(diǎn)與目標(biāo)點(diǎn)坐標(biāo)：（4，4）－（85，85），（10，90）－（85，10），（10，90）－（85，50），（4，4）－（50，90），（5，50）－（85，50），（5，50）－（85，85）。

圖7 50次仿真直方圖統(tǒng)計(jì)

圖8是以（5，5）為起點(diǎn)坐標(biāo)，（85，85）為目標(biāo)點(diǎn)坐標(biāo)的復(fù)雜環(huán)境仿真結(jié)果，其中（a）（b）分別為文獻(xiàn)［13］改進(jìn)算法和本文融合改進(jìn)算法仿真。表2為50次仿真實(shí)驗(yàn)取其平均值統(tǒng)計(jì)。

圖8 仿真結(jié)果

表2 50次仿真實(shí)驗(yàn)各項(xiàng)平均值

從仿真結(jié)果對比圖表中可以看出，改進(jìn)的融合算法雖在時(shí)間優(yōu)化上劣于文獻(xiàn)［13］算法，但差距并不明顯，簡單環(huán)境最大差距僅為0.724 s，復(fù)雜環(huán)境相差4.457 s。相比于文獻(xiàn)［13］算法，本文融合算法在路徑節(jié)點(diǎn)個數(shù)方面，簡單環(huán)境下減少了76.2%以上，復(fù)雜環(huán)境下減少了87.2%以上；路徑長度方面，簡單環(huán)境下縮短了13.4%以上，復(fù)雜環(huán)境下縮短了21.3%以上。這是因?yàn)楸疚母倪M(jìn)算法在新節(jié)點(diǎn)擴(kuò)展方法上結(jié)合了剪枝優(yōu)化方法，雖增加了時(shí)間成本，但去除了大量冗余路徑節(jié)點(diǎn)和多余的路徑，使規(guī)劃出的軌跡更適合無人駕駛汽車行駛。上述結(jié)果表明：本文改進(jìn)的RRT算法規(guī)劃效果總體優(yōu)于其他算法。

3 結(jié)論

采用傳統(tǒng)RRT算法進(jìn)行路徑規(guī)劃可以規(guī)劃得到一條從起始點(diǎn)到目標(biāo)點(diǎn)的無障礙安全路徑，但由于采樣點(diǎn)的隨機(jī)性導(dǎo)致采樣效率低，使搜索時(shí)間過長，路徑較長。針對這些問題，首先提出了一種新節(jié)點(diǎn)擴(kuò)展方法，對改進(jìn)算法的原理及實(shí)現(xiàn)過程進(jìn)行了詳細(xì)分析，并通過與傳統(tǒng)RRT算法和基于P概率的RRT算法的仿真比較，驗(yàn)證了本文算法有效提高了采樣效率，去除了不必要的區(qū)域搜索，減少了搜索時(shí)間和縮短路徑長度；其次，還提出了一種剪枝優(yōu)化路徑方法，通過與已有方法比較，驗(yàn)證了方法的有效性，最后將2種方法融合與文獻(xiàn)［13］所提方法進(jìn)行對比，驗(yàn)證了本文改進(jìn)算法具有更佳的效果。

本文中所提的方法主要在路徑長度、時(shí)間和路徑節(jié)點(diǎn)個數(shù)上進(jìn)行了優(yōu)化，但忽略了路徑的曲折性，優(yōu)化過程中有些地方轉(zhuǎn)折角度可能較大，不符合車輛運(yùn)動學(xué)約束，故后續(xù)研究工作將考慮加入車輛運(yùn)動學(xué)約束，從而使路徑更加平滑，便于車輛的跟蹤控制。