羅 輝，蔣 濤，周 楠，許 林，譚學敏，程永杰

成都信息工程大學 控制工程學院，成都 610000

路徑規(guī)劃算法是機器人和無人駕駛領域的熱門研究之一[1-2]，是實現(xiàn)導航的關鍵技術[3]，規(guī)劃算法現(xiàn)目前存在許多問題，例如規(guī)劃路徑不符合車輛實際運動學和動力學約束，以及障礙物貼合擬合路徑，算法計算效率不高，拓展性不強等；路徑規(guī)劃算法主要分為兩大類，基于圖搜索算法，如A*算法，以及基于采樣的搜索算法，快速隨機搜索樹算法等[4]。為了更好地利用A*算法，武善平等人[5]提出一種自適應的啟發(fā)式A*搜索算法，但是在平滑效率上仍存在路徑曲率不連續(xù)等缺點。針對機器人在復雜多變的動態(tài)環(huán)境下的路徑規(guī)劃問題，封碩等人[6]提出了一種融合動態(tài)障礙物信息的動態(tài)窗口A*搜索算法。由于基于圖搜索算法在大規(guī)模搜索區(qū)域時計算量過大，搜索效率過低。所以后續(xù)出現(xiàn)基于采樣搜索的算法，能夠有效解決圖搜索算法在較大規(guī)模區(qū)域內(nèi)收斂效率不高的問題?；诓蓸拥乃阉魉惴≧RT因其快速的節(jié)點拓展機制和強大的搜索性能而被廣泛應用[7]。但是傳統(tǒng)快速搜索隨機數(shù)RRT算法存在隨機采樣、采樣的盲目性以及不確定性等問題，導致算法計算效率低下，因此產(chǎn)生了一系列衍生算法來解決這些問題。Mashayekhi等人[8]針對RRT搜索路徑時的收斂效率以及優(yōu)化路徑耗時問題，提出了一種Hybird-RRT。馮來春等人[9]提出了一種改進的RRT算法，考慮了車輛當前的環(huán)境約束和運動學約束，但是該方法未考慮車輛周圍障礙物的約束。針對RRT的實時性以及優(yōu)化角度等問題，劉紫燕等人[10]提出了一種目標偏向型的平滑RRT算法。Wang等人[11]提出了一種基于彈性帶的快速重規(guī)劃EB-RRT算法，有效地解決了RRT在動態(tài)環(huán)境中規(guī)劃的實時性和安全性問題。Hu等人[12]為了有效地提高RRT算法在平滑階段以及搜索路徑長度的質(zhì)量，提出了一種改進的RRT算法。同時為了更好地解決RRT容易陷入局部最優(yōu)的問題，Zhang等人[13]提出一種基于復雜環(huán)境下的改進RRT算法。Yuan等人[14]提出了一種在三維空間中有效規(guī)劃路徑的改進RRT算法，結合了啟發(fā)式概率和偏差目標因子，避免算法陷入局部最優(yōu)的同時，保證了算法在避障規(guī)劃中的有效性和可靠性。Xiang等人[15]提出了一種針對原快速探索隨機樹RRT的偏差最優(yōu)解、非目標和路徑非光滑問題，提出了一種改進RRT算法，解決了啟發(fā)式值函數(shù)引起的局部極小問題。為了更好地解決RRT非漸近優(yōu)化的問題，Karaman等人[16]提出RRT*算法，解決了RRT算法產(chǎn)生的路徑并非概率最優(yōu)解問題，但大規(guī)模搜索時運行效率極低。楊瑤等人[17]提出改進雙向RRT*路徑優(yōu)化算法，該算法利用Reeds-Shepp曲線優(yōu)化航向問題。Chen等人[18]針對RRT*收斂速度問題，提出一種結合角度和曲率過濾器篩選父節(jié)點的改進RRT*算法。Chen等人[19]提出了一種HL-RRT*算法，解決了RRT*在一些復雜多變的動態(tài)環(huán)境中的快速收斂問題，同時也縮短了路徑的長度。馬小陸等人[20]提出了均勻Logistic混沌序列采樣的RRT*路徑規(guī)劃算法，解決了RRT*算法收斂速度慢的問題。許萬等人[21]提出了簡化地圖區(qū)域采樣的思想，加快了RRT*算法的收斂速度。劉成菊等人[22]提出了一種動態(tài)規(guī)劃的方法，減弱了重規(guī)劃的隨機性。但上述所提及的方法并未同時有效解決RRT平滑性差，搜索效率低，以及搜索路徑過長等問題。

為有效解決RRT平滑性以及搜索效率，搜索路徑長度方面的問題，本文提出了一種二階段RRT算法（TSRRT），該算法在原RRT算法的基礎上，通過分階段分別融合不同擬合算法進行全局路徑規(guī)劃，來提高算法的收斂速度與規(guī)劃路徑的車輛跟隨性。第一階段，在整個可規(guī)劃環(huán)境范圍內(nèi)采取啟發(fā)式隨機采樣，隨機采點與結合最大轉(zhuǎn)向角度和曲率的貝塞爾曲線擬合。在曲線擬合時結合車輛最小轉(zhuǎn)彎半徑，使改進RRT算法規(guī)劃的路徑更加的平滑，并符合車輛運動的上邊界曲率約束，最終產(chǎn)生一條能夠滿足車輛實際行駛的軌跡。第二階段，杜賓斯（Dubins）曲線擬合。當?shù)谝浑A段采樣點拓展到目標點鄰域內(nèi)時，直接采用杜賓斯曲線進行連接，并通過碰撞檢測，產(chǎn)生一條能夠適合于車輛運動軌跡的路徑。本文所提出的算法在到達第二階段起點時，直接通過Dubins曲線連接第二階段起點到終點的路徑，從而減少算法的收斂時間，同時也滿足了車輛的模型約束，提高了原有RRT算法的實時性與路徑車輛跟隨性。通過對比實驗驗證，TSRRT算法相較于傳統(tǒng)RRT算法能取得更好的收斂速度及產(chǎn)生更符合車輛跟隨的全局路徑，并且TSRRT算法搜索的全局路徑滿足車輛運動學要求。

1 相關工作

1.1 車輛運動學

傳統(tǒng)的RRT路徑規(guī)劃算法僅僅是為了產(chǎn)生一條能夠從起點到終點的路徑，并沒有考慮加入車輛運動學約束使規(guī)劃的路徑能夠更好地滿足車輛行駛要求，為了讓自動駕駛車輛能夠更好地對所規(guī)劃的全局路徑進行跟蹤，TSRRT算法考慮在路徑優(yōu)化的過程中加入車輛運動學模型約束。算法將全局規(guī)劃路徑的曲率進行優(yōu)化，使路徑曲率能夠滿足參數(shù)連續(xù)，從而使TSRRT算法生成的全局路徑更加平滑。為了使規(guī)劃的路徑更容易結合車輛運動學模型，本文將車輛運動學模型簡化為單車模型，即模型將左右后輪合并為一個N點，即(x,y)；左右前輪合并為一個M點；車輛質(zhì)心為Q點，車輛軸距為L。車輛運動學模型如圖1所示，相關參數(shù)如表1所示。

表1 車輛運動學模型參數(shù)Table 1 Vehicle kinematics model parameters

圖1 車輛運動學模型圖Fig.1 Vehicle kinematics model diagram

車輛模型的運動學方程如下：

其中，轉(zhuǎn)彎半徑與前輪轉(zhuǎn)角存在如下關系：

根據(jù)公式（1）和公式（2）得到在當前位置的轉(zhuǎn)彎半徑，從而根據(jù)曲率和半徑的關系得到曲率計算公式：

即，當前位置的曲率可通過當前位置的前輪轉(zhuǎn)角和車輛軸距得到，優(yōu)化最小轉(zhuǎn)彎半徑即優(yōu)化最大曲率約束，TSRRT算法采用曲線優(yōu)化算法時融合前輪轉(zhuǎn)角和最大曲率進行車輛運動學優(yōu)化，讓車輛跟蹤TSRRT算法所規(guī)劃路徑時更穩(wěn)定和平順。

1.2 路徑曲率優(yōu)化

傳統(tǒng)的快速隨機搜索樹算法通常以直線連接兩個節(jié)點，導致算法全局規(guī)劃的路徑存在轉(zhuǎn)折點的曲率不連續(xù)、轉(zhuǎn)折角度過大等問題，所規(guī)劃的全局路徑不適宜進行無人駕駛車輛的軌跡跟蹤。本文所提出的TSRRT算法為了保證路徑曲率連續(xù)[23]，采用兩段三次貝塞爾曲線來對節(jié)點進行連接，同時將車輛的最大轉(zhuǎn)角與貝塞爾曲線的參數(shù)相結合，通過優(yōu)化貝塞爾曲線的相關參數(shù)來使得所規(guī)劃路徑的曲率連續(xù)且滿足無人駕駛車輛的最小轉(zhuǎn)彎半徑約束。貝塞爾曲線在路徑規(guī)劃中常用于路徑平滑，其n階貝塞爾表達式如下：

公式（4）中Pi表示第i控制點，n表示貝塞爾曲線的階數(shù)，Bn,i(s)為伯恩斯坦多項式。改進算法TSRRT中第一階段搜索路徑均由兩段三階貝塞爾曲線拼接而成，兩段三階貝塞爾曲線優(yōu)化路徑中的三個節(jié)點，如圖2所示，其中W1、W2、W3表示所優(yōu)化的節(jié)點，分別為RRT樹中的鄰近點的父節(jié)點、鄰近點、新產(chǎn)生的子節(jié)點。γ表示無人駕駛車輛的最大轉(zhuǎn)向角度，B0、B1、B2、B3為第一段三階貝塞爾曲線的控制點，E0、E1、E2、E3為第二段三階貝塞爾曲線的控制點，d為RRT樹的步長。

兩條貝塞爾曲線中八個控制點位由下式確定：

其中，u1、u2、u3分別表示沿著W2W1、W2W3、B2E2方向的單位向量，其余變量為：

根據(jù)式（8）的相關常量，可由式（4）計算得到圖2中的優(yōu)化曲線。

圖2 三控制點中的貝塞爾曲線圖Fig.2 Bezier curve in three control points

1.3 終點范圍約束曲線

Reeds_Shepp曲線[24]和Dubins曲線[25]最大的差別是Reeds_Shepp曲線有考慮車輛倒車的情況，而Dubins曲線僅考慮車輛前進的情況。由于全局路徑規(guī)劃并未考慮車輛倒退，所以本文所提出的TSRRT算法采用Dubins曲線進行目標點附近的節(jié)點連接。

傳統(tǒng)RRT算法以及部分改進RRT算法當搜索到目標點附近時，便以集中采樣的方式找到滿足特定要求的點，但集中采樣易陷入采樣迷失終點的困境，且比較耗時，當步長不合適宜時，比較難連接到目標點，也無法滿足目標狀態(tài)的角度約束。而本文所提出的TSRRT算法當有新的節(jié)點Znew在目標點附近時，便以滿足航向的Dubins曲線連接目標點與點Znew，若該Dubins曲線沒有經(jīng)過障礙物，則算法搜索結束，返回相應的路徑，否則繼續(xù)采樣，生成新的節(jié)點，添加到樹中。

本文受原RRT算法終點結束時判斷啟發(fā)，采用Dubins曲線進行第二階段曲線連接代替原RRT算法搜索到終點距離范圍結束方式。首先是能夠有效的在長距離范圍內(nèi)進行滿足始端和終端航向的最短路徑直連，解決了原RRT在相同終點范圍距離的冗余采樣和搜索路徑過多以及原RRT算法搜索路徑不滿足航向要求的問題，有效地提高了算法的收斂效率和航向約束問題，同時也結合Dubins曲線滿足曲率約束的優(yōu)點，滿足了TSRRT算法在智能車行駛方面的橫向控制穩(wěn)定和舒適性要求。

在原RRT算法中，一般采用當前節(jié)點到終點距離小于1倍步長才判定RRT算法已經(jīng)搜索到終點，但是TSRRT算法是在距離終點10～20倍步長時進行Dubins曲線連接，相比于原RRT算法能夠縮短10～20倍步長的距離的隨機搜索，有效地提高收斂效率，同時由于原RRT在到達末尾區(qū)域時，如果搜索步長和終點范圍距離閾值設置不合適，會造成搜索節(jié)點和路徑過于冗余。所以TSRRT算法采用Dubins曲線能夠有效地減少收斂時間，同時Dubins曲線是在滿足曲率約束以及規(guī)定的始端和末端切線方向條件下，連接兩個端點的最短路徑，有效地提高了TSRRT算法的收斂效率和滿足了路徑的航向要求。

Dubins曲線采用Dubins車輛運動模型，即只允許車輛前進，不允許車輛倒退的情況。Dubins曲線是在滿足起始狀態(tài)、末端狀態(tài)約束與最小轉(zhuǎn)彎半徑的情況下，連接二維平面的最短路徑。Dubins曲線共有6種可能最短路徑，分別為LSL、RSR、RSL、LSR、RLR與LRL，L為以最小轉(zhuǎn)彎半徑向左轉(zhuǎn)彎，S為直線行駛，R為以最小轉(zhuǎn)彎半徑向右轉(zhuǎn)彎。由于類型太多，本文只列舉其中一種情況進行闡述，如圖3所示，連接P0、P1兩個控制點的最短可能路徑RSL，其由直線與圓弧構成，圓弧與直線的連接處相切。

圖3 RSL示意圖Fig.3 Schematic diagram of RSL

圖4為Dubins曲線的應用實驗結果，其中深紅色曲線為Dubins曲線，其連接第一階段終點和最終目標點的局部放大圖如右圖所示，根據(jù)計算得出連接處均滿足方向角約束，證明改進后的算法TSRRT滿足末端狀態(tài)約束，且曲線具有C1連續(xù)性。

圖4 改進TSRRT算法末端局部放大圖Fig.4 Improved TSRRT algorithm end local enlargement

1.4 快速拓展隨機數(shù)（RRT）

快速擴展隨機樹（rapidly-exploring random trees，RRT）算法[26]由美國愛荷華州立大學的LaValle教授在1998年提出，是一種基于采樣的快速搜索的規(guī)劃方法[27]，解決了基于圖搜索算法中當搜索維度和搜索區(qū)域過大時，搜索時間和效率大幅度下降的問題，近十幾年得到廣泛發(fā)展與應用。傳統(tǒng)的RRT算法是通過一個初始點作為根節(jié)點，進行隨機采樣，增加葉子節(jié)點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節(jié)點包含了目標點或進入目標區(qū)域，便可以在隨機樹中找到一條由樹節(jié)點組成的從初始點到目標點的路徑[28]。

RRT算法是一種具備概率完備性的增量式采樣算法，該算法適合用于解決高維環(huán)境與非完整性約束機器人的路徑規(guī)劃問題。原RRT算法通過對狀態(tài)空間中的采樣點進行檢測，避免了對搜索空間進行建模，同時在狀態(tài)空間中隨機采樣，快速把搜索方向?qū)蚩瞻讌^(qū)域，從而尋找一條從起始點到目標點無障礙路徑。拓展樹是從狀態(tài)搜索空間中隨機抽取的樣本進行逐步構建的，并且本質(zhì)上傾向于朝向大部分未探測區(qū)域進行生長。同時由于它可以輕松處理障礙物和差分約束（非完整性和動力學）的問題，并被廣泛應用于自主機器人運動規(guī)劃。傳統(tǒng)RRT算法流程如圖5所示，其仿真結果示意如圖6，圖7表示RRT算法節(jié)點拓展細節(jié)圖，Xrand為隨機采樣節(jié)點，Xnearest為當前樹中距離Xrand最近節(jié)點，沿XnearestXrand方向拓展一定步長得到Xnew，通過障礙物檢測即可加入RRT樹。

圖5 RRT算法流程圖Fig.5 RRT algorithm flow chart

圖6 RRT搜索示意圖Fig.6 RRT search diagram

圖7 RRT拓展示意圖Fig.7 Schematic diagram of RRT expansion

原RRT算法主要存在以下優(yōu)點：

（1）RRT算法無需對環(huán)境進行建模，適用于高維問題。

（2）RRT算法的隨機性雖然存在缺陷，但是算法不容易陷入局部最優(yōu)。特別是在障礙物較多的情況下，不易陷入局部最優(yōu)。

（3）RRT算法容易處理障礙物和非完整性約束問題。

（4）RRT算法雖然在路徑質(zhì)量上存在過于曲折，路徑非漸近最優(yōu)，但是其屬于概率完備性算法，能夠很好地適應各類存在路徑解的復雜環(huán)境。

由圖6與圖7以及算法原理易知RRT算法存在以下三個方面不足：

（1）RRT算法產(chǎn)生的路徑存在過于曲折，由圖6和圖7中可以看出，在生成路徑的拓展過程中，由于在狀態(tài)空間隨機進行子節(jié)點拓展，導致拓展過程中出現(xiàn)較多超過最大轉(zhuǎn)向角度的轉(zhuǎn)折點，則車輛需要在此處停下原地旋轉(zhuǎn)后才能繼續(xù)前進，故導致該全局路徑不適合不能原地旋轉(zhuǎn)的車輛進行路徑跟蹤或者局部規(guī)劃。

（2）RRT算法產(chǎn)生路徑的時間過于冗長，由于產(chǎn)生大量隨機點的同時，也在不停地隨機拓展節(jié)點，由于搜索節(jié)點的隨機性和盲目性導致搜索區(qū)域擴大，從而產(chǎn)生過長搜索時間。由圖6所示，在終點的右上方還有部分搜索樹。原RRT算法的冗余搜索部分導致搜索時間過長，現(xiàn)實場景的應用存在局限性，沒有達到一定的實時效果。

（3）RRT算法起點的采樣容易發(fā)生大角度轉(zhuǎn)向甚至反向情況，如圖7起點位置處采樣所示。當起始采樣不符合車輛初始位置航向且超過車輛最大轉(zhuǎn)向角度時，若車輛跟隨全局路徑，則需要原地轉(zhuǎn)向，但是大部分智能車不能原地旋轉(zhuǎn)。

為了解決上述問題，本文提出了一種TSRRT算法：第一階段采用原RRT算法結合啟發(fā)式函數(shù)進行快速搜索，到達距離終點10～20步長圓形附近區(qū)域；第二階段采用Dubins曲線直接連接第一階段的終點和目標點，并判斷連接曲線是否經(jīng)過障礙物，最終到達目標點。經(jīng)過實驗驗證，本文提出的TSRRT算法可以快速地搜索出一條同時滿足安全與運動學約束的平滑路徑。

2 二階段RRT（TSRRT）算法

2.1 虛擬起始節(jié)點

傳統(tǒng)RRT算法沒有對起始航向進行約束，即在起點后的第一個新節(jié)點可以在360°的范圍內(nèi)產(chǎn)生，由于智能車本身是具備朝向信息的，即在起點時有航向信息，如果不進行航向約束，則存在較大概率的采樣點不符合起始航向，導致車輛需要原地旋轉(zhuǎn)才能滿足路徑的航向約束要求。所以為滿足起始狀態(tài)的角度約束，增加距離車輛起點航向的一個步長距離的虛擬節(jié)點Zfstart作為第一個采樣點，同時將其加入到TSRRT搜索樹T中，作為樹T的起始節(jié)點。起點位置采樣方式如圖8所示。

圖8 起始位置采樣圖Fig.8 Start position sampling diagram

2.2 鄰近節(jié)點優(yōu)選

原RRT算法拓展節(jié)點時采用歐氏距離方式找到最優(yōu)鄰近節(jié)點，當節(jié)點數(shù)量較多時，容易產(chǎn)生大量計算從而耗時。而改進算法TSRRT采用搜索隨機節(jié)點周圍固定K個鄰近節(jié)點方式，保證當節(jié)點數(shù)過多時也不會產(chǎn)生較多計算量，從而提高算法的收斂速度。改進算法TSRRT首先采用隨機搜索策略進行節(jié)點采樣，同時融合啟發(fā)式函數(shù)進行周圍K個鄰近節(jié)點代價值計算，G為當前總代價值最小的鄰近節(jié)點：

其中，H(Q)為起始位置到當前鄰近節(jié)點位置的已花費代價值，H(G)用于計算從當前鄰近節(jié)點位置到目標點的預估代價值：

Lbci為第i鄰近節(jié)點與其父節(jié)點的貝塞爾曲線近似長度，公式（12）中n為貝塞爾曲線的階數(shù)，Lc為貝塞爾曲線的弦長，Lp為貝塞爾曲線相鄰點之間的距離，以三階貝塞爾曲線為例，如圖9所示。

圖9 Bezier曲線連接圖Fig.9 Bezier curve connection diagram

公式（12）中的Lc和Lp計算公式如下：

預估代價值H(G)為當前鄰近節(jié)點到目標點的Dubins曲線連接長度。初始節(jié)點Zfstart的已花費代價值設置為0。公式（10）既考慮到了當前鄰近節(jié)點的已消耗代價，同時也計算了該鄰近節(jié)點的預估代價值，能夠快速找到隨機點周圍固定K個距離代價值較小的鄰近節(jié)點，將這些節(jié)點按代價值進行升序排序，再依次判斷該采樣節(jié)點和鄰近節(jié)點以及鄰近父節(jié)點合成的角度是否超過最大轉(zhuǎn)向角度，當獲取到?jīng)]有超過最大轉(zhuǎn)向角度γ的鄰近節(jié)點時，則選取該鄰近節(jié)點作為隨機采樣點的父節(jié)點，然后進行節(jié)點拓展，而原RRT算法計算鄰近節(jié)點時，采用歐式距離選取整棵RRT樹中最近的節(jié)點作為鄰近節(jié)點，當搜索空間過大時，其選取鄰近節(jié)點的計算量會大幅度增加，導致收斂效率過低，相比于改進算法TSRRT搜索鄰近節(jié)點方式，原RRT算法鄰近節(jié)點選取方式存在條件判斷不足，計算量過大等缺陷。

2.3 障礙物檢測

本文結合柵格地圖與貝塞爾曲線的凸包性質(zhì)做障礙物檢測，貝塞爾的凸包性即貝塞爾曲線一定在其控制點所圍成的最小凸多邊形內(nèi)，如圖10所示。該地圖提供地圖中任意一點到其最近障礙物的距離與到最近的泰森多邊形邊的位置。如圖10所示，為保證兩段貝塞爾曲線不在障礙物中，兩段貝塞爾曲線分別做障礙物檢測，以第一段貝塞爾曲線為例，只需保證其凸包內(nèi)的任意一點均不在障礙物內(nèi)即可，即dh＞0。Pobs表示離貝塞爾曲線控制點最近的障礙物且dc表示相應的距離，Pr表示貝塞爾曲線凸包內(nèi)任意一點，rh表示該控制點到Pr的距離，r01、r12、r23分別表示相應控制點之間的距離。

圖10 碰撞檢測示意圖Fig.10 Schematic diagram of collision detection

由三角不等式易得：

根據(jù)公式（18）即可判斷當前Bezier曲線部分是否避開障礙物。

2.4 TSRRT算法

TSRRT算法流程圖如圖11所示。

圖11 TSRRT算法流程圖Fig.11 TSRRT algorithm flow chart

TSRRT算法主要以提高原算法搜索效率和讓搜索路徑滿足車輛的運動學規(guī)律為目的。TSRRT算法分為兩個階段，第一階段是結合原RRT算法搜索方式進行隨機采樣，保留原RRT算法的拓展狀態(tài)空間的隨機性，避免陷入局部最優(yōu)。其次采用代價值方式查詢采樣節(jié)點周圍固定K個鄰近節(jié)點，相比于原RRT算法進行除采樣節(jié)點以外的所有節(jié)點進行計算篩選的方式，此改進算法不會隨著搜索樹節(jié)點的增多而增加計算量，所有能夠更高效地找到鄰近節(jié)點。同時算法采用啟發(fā)式函數(shù)進行搜索，能夠更快地讓搜索方向趨于目標點方向，有效地提高TSRRT算法第一階段的收斂效率。TSRRT算法在搜索鄰近節(jié)點的同時采用已消耗代價和預估代價的啟發(fā)式函數(shù)以及角度剔除計算方式，相比于原RRT算法找歐式距離最近節(jié)點的方式能夠更加精準地找到角度和距離合適的鄰近節(jié)點，有效地提高TSRRT算法的收斂精度。當完成搜索后融合兩段三次Bezier曲線進行擬合以及障礙物檢測之后，保證了搜索路徑曲率連續(xù)以及路徑的安全性，讓車輛在行駛過程中能夠以不停車的方式進行連續(xù)平穩(wěn)安全地轉(zhuǎn)向。第二階段采用不同于原RRT算法方式的直連算法，這也是第一次提出利用Dubins曲線快速連接第一階段終點和目標點的方式，極大地提高了RRT算法的收斂速度，同時也滿足了車輛的航向約束。最終完成一、二階段搜索之后進行角度檢測算法剪枝，完成路徑搜索。

相較于傳統(tǒng)的RRT算法，TSRRT算法有以下五個部分進行改進：

（1）起始點增加虛擬節(jié)點如圖8所示，滿足車輛起始位航向搜索，避免算法在起始位置搜索隨機性問題，從而避免讓車輛在原地旋轉(zhuǎn)后跟蹤路徑。

（2）增加距離和角度以及曲率代價值啟發(fā)式搜索如公式（9）所示，快速搜索出代價值最低且滿足角度約束的鄰近節(jié)點，提高了鄰近節(jié)點的搜索效率以及鄰近節(jié)點質(zhì)量，不再是以單純的歐式距離作為度量方式，使算法能夠快速收斂，同時提高路徑搜索質(zhì)量。

（3）增加結合最小轉(zhuǎn)彎半徑和最大轉(zhuǎn)向角度的兩段貝塞爾曲線如公式（4）所示進行曲率優(yōu)化，保證路徑曲率連續(xù)，從而能夠讓車輛在不停止運動的情況下進行連續(xù)轉(zhuǎn)彎，滿足車輛運動學規(guī)律。

（4）增加Dubins曲線如圖4所示，當算法搜索到終點范圍10～20倍步長時直接連接Dubins曲線，提高了TSRRT算法搜索效率，Dubins曲線既保證了起始點和終止點間的距離最短，同時滿足曲率約束也滿足了車輛在起始點和終點航向約束要求。

（5）最后通過剪枝算法得到路徑：存在P0、P1、P2、P3、P4節(jié)點，分別計算∠P1P2P4和∠P2P4P5的角度是否同時超過最大轉(zhuǎn)向角度，若沒有超過，并且通過障礙物檢測，即可刪除P3節(jié)點，否則繼續(xù)下一節(jié)點判斷。

本文提出的TSRRT算法第一階段是結合原RRT算法搜索方式進行隨機采樣，保留原RRT算法拓展狀態(tài)空間的隨機性，避免陷入局部最優(yōu)。第二階段采用Dubins曲線直連方式約束曲率以及始端和終端的航向，同時也是始端和終端間的最短路徑。相比于原RRT算法仍需在此階段進行隨機搜索，由于其隨機性搜索拓展以及較長路徑需要探索，使其在此階段搜索時間將比TSRRT算法搜索時間更多，同時原RRT算法搜索路徑時采樣的隨機性導致拓展路徑過于曲折，也不滿足路徑的航向和曲率要求。所以TSRRT算法在兩個階段均能夠有效地提高算法的搜索效率，同時由于啟發(fā)式搜索和Dubins曲線的路徑最短特性，能夠有效地提升路徑的質(zhì)量，縮短路徑的長度，從而提高TSRRT算法整體的收斂精度。

3 實驗

3.1 實驗設計

為了充分研究所提出算法的有效性，實驗主要考慮以下三個場景，分別是簡單障礙物場景、復雜隨機障礙物場景（考慮有人或者其他障礙物出現(xiàn)）、迷宮類障礙物場景。地圖大小為500 m×500 m，目標偏置率Pgoal=0.2、最大曲率kmax=0.1、最大轉(zhuǎn)向角γ=0.4π、采樣步長d=10.08，起始點和終點設置如表2所示。

表2 三種場景狀態(tài)設置表Table 2 Three scene states setting table

3.2 實驗結果

3.2.1 簡單障礙物場景實驗結果

簡單障礙物場景路徑搜索結果如圖12所示，其中圖（a）紅色曲線為傳統(tǒng)RRT方法搜索結果。圖（b）為TSRRT在相同條件下的實驗結果，紅色曲線為第一階段搜索結果，深紅色曲線為第二階段搜索結果。從實驗結果可以看出TSRRT算法搜索路徑長度明顯變短，并且平滑度上明顯優(yōu)于傳統(tǒng)RRT算法結果。

圖12 簡單障礙物場景路徑搜索結果圖Fig.12 Path search results of simple obstacle scene

為了更好地展示所提出TSRRT算法對于路徑平滑的有效性，將對比的兩種方法所生成路徑的曲率變化展示如圖13所示。其中圖（a）為傳統(tǒng)RRT方法生成路徑的曲率變化情況，圖（b）為TSRRT方法所生成路徑的曲率變化情況。橫坐標代表曲率在整條路徑相應比例位置，縱坐標代表曲率。從實驗結果看出傳統(tǒng)RRT方法最大曲率已經(jīng)超過0.1，而TSRRT方法并沒有超過0.035，同時傳統(tǒng)RRT搜索路徑曲率突變過多，而TSRRT算法搜索路徑曲率連續(xù)。

圖13 簡單障礙物場景路徑曲率變化圖Fig.13 Path curvature variation of simple obstacle scene

將兩種對比方法的相關性能指標展示如表3所示，其中各項數(shù)據(jù)為100次實驗結果的均值。從表中可以看出傳統(tǒng)RRT與TSRRT算法均能在1 s內(nèi)找到有效的路徑，但傳統(tǒng)RRT算法的路徑偏長，而TSRRT算法搜索路徑長度減少了23.60%；TSRRT算法采用啟發(fā)式搜索和Dubins曲線連接第一階段終點和目標點，提高了算法搜索效率和搜索質(zhì)量，平均耗時減少40.38%，擴展的節(jié)點數(shù)減少31.49%。

表3 簡單障礙物實驗結果表Table 3 Experimental results of simple obstacles

3.2.2 隨機障礙物場景實驗結果

隨機障礙物場景實驗結果如圖14所示。圖（a）為傳統(tǒng)RRT方法實驗結果，最終搜索路徑如紅色曲線所示。圖（b）為TSRRT在相同條件下同場景的實驗結果，紅色路徑為第一階段搜索結果，深紅色路徑為第二階段搜索結果。改進RRT算法明顯在路徑點角度和搜索路徑長度上優(yōu)于傳統(tǒng)RRT算法。

圖14 隨機障礙物場景路徑搜索結果圖Fig.14 Path search results of random obstacle scene

傳統(tǒng)RRT算法生成路徑的曲率變化如圖15（a）所示，TSRRT算法生成路徑的曲率變化如圖15（b）所示。從實驗結果看出原RRT算法已經(jīng)多次超過最大曲率0.1，小于車輛最小轉(zhuǎn)彎半徑，而TSRRT算法并沒有超過0.071。同時由于原RRT搜索路徑曲率突變過多且超過0.1，造成車輛跟隨時需要在該點位停下進行原地旋轉(zhuǎn)，而TSRRT算法搜索路徑曲率連續(xù)，車輛無需在原地旋轉(zhuǎn)即可進行平滑穩(wěn)定的轉(zhuǎn)向，滿足了車輛運動學穩(wěn)定舒適的橫向控制要求。

圖15 隨機障礙物場景路徑曲率變化圖Fig.15 Path curvature variation of random obstacle scene

隨機障礙物實驗場景下的算法性能指標數(shù)據(jù)如表4所示，其中各項數(shù)據(jù)指標為100次實驗結果的均值。從表中可以看出傳統(tǒng)RRT算法、TSRRT算法均能在1 s內(nèi)找到有效的路徑，但是傳統(tǒng)RRT算法的路徑偏長，TSRRT算法搜索路徑長度減少了23.81%；改進后的TSRRT算法采用啟發(fā)式搜索和Dubins曲線連接終點，平均耗時減少42.80%，擴展的節(jié)點數(shù)減少21.83%。實驗結果證明提出的RRT改進方式應用于隨機障礙物場景有效。

表4 隨機障礙物實驗結果表Table 4 Results of random obstacle experiment

3.2.3 迷宮障礙物場景實驗結果

迷宮障礙物場景實驗結果如圖16所示。圖（a）紅色路徑為傳統(tǒng)RRT方法搜索實驗結果。圖（b）為TSRRT算法在相同條件下的實驗結果，紅色路徑為第一階段搜索結果，深紅色路徑為Dubins曲線直連結果，從實驗結果可以看出改進后的算法TSRRT在搜索路徑的長度上明顯變短，并且平滑度上明顯優(yōu)于RRT算法。

圖16 迷宮障礙物場景路徑搜索結果圖Fig.16 Maze obstacle scene path search results

傳統(tǒng)RRT算法生成路徑的曲率變化如圖17（a）所示，TSRRT生成路徑的曲率變化如圖17（b）所示。從實驗結果看出傳統(tǒng)RRT算法已經(jīng)多次超過最大曲率0.1，而改進后的算法TSRRT并沒有超過0.075，同時原RRT搜索路徑曲率突變過多，而TSRRT算法搜索路徑曲率連續(xù)，從而讓車輛能夠平滑穩(wěn)定的轉(zhuǎn)向，不再需要停止原地旋轉(zhuǎn)。實驗結果證明提出的RRT改進方式應用于隨機障礙物場景有效。

圖17 迷宮障礙物場景路徑曲率變化圖Fig.17 Path curvature variation of maze obstacle scene

迷宮障礙物實驗場景下的算法性能指標數(shù)據(jù)如表5所示，其中各項數(shù)據(jù)均為100次實驗結果的均值。從表中可以看出傳統(tǒng)RRT、改進后的算法TSRRT均能在1 s內(nèi)找到有效的路徑，但是傳統(tǒng)RRT算法的路徑偏長，TSRRT算法搜索路徑長度減少27.66%；改進后的算法TSRRT采用啟發(fā)式搜索和Dubins曲線連接終點，平均耗時減少46.05%，擴展的節(jié)點數(shù)減少33.75%。由于兩種算法均采用目標偏置采樣的策略，所以在迷宮環(huán)境下，能快速找到有效路徑。實驗結果證明提出的RRT改進方式應用于迷宮障礙物場景有效。

表5 迷宮障礙物實驗結果表Table 5 Maze obstacle test results

3.3 實驗總結

從三個場景實驗結果可以得出：TSRRT算法相比于原RRT算法，TSRRT算法搜索的路徑能夠在橫向控制進行更好的優(yōu)化，因為其能夠搜索出一條曲率連續(xù)的全局路徑，能夠讓智能車在不停車的情況下進行連續(xù)平穩(wěn)的橫向控制，然而原RRT算法搜索的路徑曲折，產(chǎn)生超過車輛最小轉(zhuǎn)彎半徑的控制點，路徑點轉(zhuǎn)折角度過大且頻繁變向，曲率不連續(xù)，導致智能車在跟隨過程中會出現(xiàn)較大轉(zhuǎn)向角度波動以及橫向控制波動，容易出現(xiàn)車輛控制不平穩(wěn)，剎車掉頭轉(zhuǎn)向等問題。

從實驗結果中的表格可以得出，原RRT算法采用隨機性搜索時，會產(chǎn)生較多的冗余節(jié)點，導致算法計算時間增加，同時也產(chǎn)生了一條不符合曲率和航向約束的路徑，不能滿足智能車的運動學要求。而TSRRT算法在第一階段采用啟發(fā)式函數(shù)引導式搜索以及K個固定鄰近節(jié)點選取，有效地提高了鄰近節(jié)點的選取效率，同時也讓搜索方向能夠更快地偏向目標點區(qū)域。第二階段Dubins曲線直連第一階段終點和目標點，刪除了原RRT算法在終點范圍的隨機搜索方式，有效地減少了算法的收斂時間，同時Dubins曲線也是滿足始端和終端航向以及曲率約束的最短路徑，而RRT在終點區(qū)域搜索時因其隨機性而不能保證是最短路徑，并且不滿足航向以及曲率要求，所以TSRRT算法有效地提高了原RRT算法的搜索效率和搜索質(zhì)量。

4 結束語

本文針對原RRT算法存在搜索路徑過于曲折，不滿足車輛運動學約束，路徑搜索實時性不高，以及搜索路徑過于冗長等問題，提出了一種二階段RRT算法（TSRRT），該算法融合最大轉(zhuǎn)向角度的三次Bezier曲線進行路徑曲率優(yōu)化，使生成的路徑滿足車輛運動學約束。與此同時TSRRT還進行分階段引導式采樣搜索與Dubins曲線直連以及最大轉(zhuǎn)向角剪枝，增強算法搜索實時性和縮短路徑長度。通過三種仿真場景對比實驗驗證得到，本文所提出的TSRRT算法在搜索時間上比原RRT算法平均節(jié)省近43%，能夠更加快速地實現(xiàn)全局路徑搜索，并且搜索出的全局路徑長度平均減少25%，同時全局路徑曲率連續(xù)，能夠讓車輛在不停車的情況下連續(xù)平穩(wěn)地轉(zhuǎn)向，因此能夠滿足車輛運動學約束，以便智能車輛更好地進行路徑跟蹤。