基于二次轉(zhuǎn)角約束的改進(jìn)RRT路徑規(guī)劃算法研究

鮑家定 鐘國安 馬果 徐海軍 景暉

【摘要】針對快速隨機搜索樹（RRT）算法存在節(jié)點擴(kuò)展冗余、生成路徑不滿足車輛轉(zhuǎn)角條件等問題，提出一種改進(jìn)的二次轉(zhuǎn)角約束RRT算法。首先，在傳統(tǒng)RRT算法基礎(chǔ)上對采樣空間進(jìn)行裁剪，引入目標(biāo)導(dǎo)向策略減少采樣時間；然后采用車輛膨脹處理和直線方法檢測障礙物，并引入第一次轉(zhuǎn)角約束得到粗解路徑；接著對粗解路徑建立二次轉(zhuǎn)角約束并進(jìn)行優(yōu)化處理，獲取優(yōu)化路徑后擬合，并進(jìn)行仿真驗證。結(jié)果表明，相比于引入目標(biāo)導(dǎo)向策略的RRT算法，所提出的算法路徑最大曲率降低了34.33%，平均曲率降低47.36%，擴(kuò)展節(jié)點數(shù)降低47.62%，路徑距離降低7.76%，規(guī)劃時間縮短14.98%。

主題詞：改進(jìn)RRT算法 轉(zhuǎn)向角度約束 路徑規(guī)劃 路徑曲率 路徑平滑性

中圖分類號：U461.99? ?文獻(xiàn)標(biāo)志碼：A? ?DOI： 10.19620/j.cnki.1000-3703.20230371

Research on Improved RRT Path Planning Algorithm Based on Two-Time Steer Angel Constraint

【Abstract】Aiming at the problems that the Rapid-exploration Random Tree （RRT） algorithm is easy to lead to node expansion redundancy and the generated path does not meet the vehicle rotation angle constraint， an improved secondary rotation angle constraint RRT algorithm is proposed. Based on the traditional RRT algorithm， the sampling space is first cut， and the target offset strategy is introduced to reduce the sampling time. Then， the vehicle expansion processing and the straight line method are used to detect the obstacle collision， and the first rotation angle constraint is introduced to obtain the coarse solution path. Then， the secondary angle constraint optimization processing is established for the rough solution path， and the vehicle optimization path is obtained and fitted， and the simulation verification is carried out. The results show that compared with the biased RRT algorithm with the goal-oriented strategy， the maximum curvature of the path is reduced by 34.33 %， the average curvature is reduced by 47.36 %， the number of extended nodes is reduced by 47.62 %， the path distance is reduced by 7.76 %， and the planning time is reduced by 14.98 %.

Key words： Improved RRT algorithm， Steering angle constrain， Path planning， Path curvature， Path smoothness

1 前言

路徑規(guī)劃技術(shù)作為無人駕駛汽車實現(xiàn)自主導(dǎo)航的關(guān)鍵技術(shù)之一，對車輛安全行駛起著至關(guān)重要的作用，逐漸成為無人駕駛車輛領(lǐng)域研究熱點[1-2]。

路徑規(guī)劃算法可分為基于圖搜索的算法（如A*算法、Dijkstra算法等）、基于隨機采樣的算法（如快速隨機搜索樹（Rapid-exploration Random Tree，RRT）算法、蒙特卡洛隨機采樣算法等），以及深度學(xué)習(xí)算法、蟻群算法等[3-7]。

RRT算法作為一種常用的路徑規(guī)劃算法，通過隨機采樣狀態(tài)空間生成新節(jié)點，并不斷擴(kuò)展樹形結(jié)構(gòu)，搜索可行路徑，具有較強的靈活性。欒添添等[8]提出了一種動態(tài)變采樣區(qū)域RRT路徑規(guī)劃算法，減少了節(jié)點的采樣次數(shù)并降低了搜索的盲目性。Zhang等[9]利用人工勢場法，對采樣目標(biāo)進(jìn)行啟發(fā)式引導(dǎo)，再根據(jù)障礙物密度動態(tài)調(diào)整步長，提出了動態(tài)步長RRT算法。Chen等[10]為了解決RRT算法搜索效率低的問題，提出了一種改進(jìn)的RRT連接路徑規(guī)劃（Improved Rapid-exploration Random Tree-Connect）算法。Ghosh等[11]在不影響精度的情況下限制生成的節(jié)點數(shù)量，并結(jié)合動力學(xué)約束生成平滑的軌跡，提出了一種基于運動學(xué)約束的雙向RRT（Kinematically Constrained Bidirectional-RRT）算法。許萬等[12]針對RRT*全局路徑規(guī)劃算法在多障礙物復(fù)雜環(huán)境中搜索效率低、占用內(nèi)存過大、搜索路徑不平滑等問題，提出一種基于簡化地圖的區(qū)域采樣RRT*算法（Simplified Map-based Regional Sampling RRT*，SMRS-RRT*）。朱冰等[13]針對RRT和RRT*算法存在路徑抖動大、易陷入局部區(qū)域和計算效率低等缺點，提出了一種基于安全場改進(jìn)RRT*算法的路徑規(guī)劃方法。Shi等[14]針對RRT算法隨機性大、收斂速度慢、易產(chǎn)生偏差的問題，采用循環(huán)交替迭代搜索法和雙向隨機樹搜索法對RRT算法進(jìn)行了改進(jìn)和優(yōu)化。Tahir等[15]為了克服復(fù)雜環(huán)境的算法收斂速度，提出了潛在性引導(dǎo)智能雙向RRT*（Potentially Guided Intelligent Bi-directional RRT*）方法。

綜上所述，目前RRT算法已提高了算法收斂速度，但仍存在獲取路徑不平滑、路徑曲折及采樣點數(shù)量過多等問題。為解決上述問題，本文提出基于二次轉(zhuǎn)角約束改進(jìn)的RRT路徑規(guī)劃算法，通過對采樣點進(jìn)行第一次轉(zhuǎn)角約束獲得粗解路徑，然后對粗解路徑進(jìn)行第二次轉(zhuǎn)角約束獲得優(yōu)化路徑，以此提高路徑的平滑性。最后通過仿真試驗驗證所提出算法的有效性。

2 偏置RRT算法

2.1 基本RRT算法原理

RRT算法是一種基于隨機空間采樣的路徑規(guī)劃算法，可以直接應(yīng)用于非完整約束系統(tǒng)的路徑規(guī)劃，且在多維復(fù)雜環(huán)境中具有極高的搜索效率，算法原理如圖1所示。

RRT算法基本流程為：首先確定起始點Xinit和目標(biāo)點Xgoal，并以Xinit為起點生成擴(kuò)展隨機樹Tnode，隨后進(jìn)行狀態(tài)空間采樣；然后在空間中選定一隨機點Xrand，以Xrand的最近點Xnear和Xrand的連線為生長方向，若Xnear與Xrand的連線未經(jīng)過障礙物，則確定Xrand；其次以Xnear和Xrand連線作為擴(kuò)展方向，設(shè)定步長[l]生成一個新的節(jié)點Xnew。若Xnear向Xnew方向擴(kuò)展過程中不與障礙物發(fā)生碰撞，則將Xnew放入Tnode中，若發(fā)生碰撞，則重新采樣隨機點。最后重復(fù)上述步驟，直到新產(chǎn)生的節(jié)點Xnew與目標(biāo)點Xgoal的距離小于設(shè)定閾值時，算法停止，并回溯獲取路徑。

2.2 目標(biāo)約束采樣

為了降低RRT算法拓展節(jié)點的盲目性，使隨機樹尋找過程更迅速，本文對隨機點的選取進(jìn)行約束?；诟怕蕄對目標(biāo)節(jié)點進(jìn)行偏置采樣，若隨機率p產(chǎn)生的概率小于設(shè)定偏置概率閾值p1，則在自由空間里進(jìn)行隨機采樣，反之，隨機點即為目標(biāo)點Xgoal，偏置采樣公式為：

式中：R為隨機采樣函數(shù)獲取的采樣點。

3 改進(jìn)的RRT算法原理

為避免RRT算法搜索的盲目性，對其進(jìn)行偏置導(dǎo)向處理，但偏置RRT算法獲取的路徑不一定滿足車輛轉(zhuǎn)向要求且路徑不夠平滑。為了獲取滿足車輛運動參數(shù)的行駛路徑，針對車輛轉(zhuǎn)向角度及采樣點約束對RRT算法路徑生成過程進(jìn)行優(yōu)化，改進(jìn)后的RRT算法流程如圖2所示。

改進(jìn)RRT算法路徑生成步驟如下：

a. 初始化起始點Xinit、目標(biāo)點Xgoal，以及第一次轉(zhuǎn)向角度約束值θ1、第二次轉(zhuǎn)向角度約束值θ2和車輛膨脹尺寸R。

b. 空間剪枝以約束采樣點的生成。以一定概率p選擇目標(biāo)點為采樣點生成Xrand。

c. 選取距離Xrand最近節(jié)點Xnear以及Xnear的父節(jié)點Xp進(jìn)行第一次轉(zhuǎn)角約束判斷，若滿足θ1，則擴(kuò)展生成Xnew。利用車輛碰撞圓和偏置直線碰撞融合方法檢測Xnear與Xrand之間是否存在碰撞。如果不存在碰撞，則連接節(jié)點。重復(fù)上述步驟，當(dāng)Xnew與目標(biāo)節(jié)點的距離小于設(shè)定值時，獲得粗解路徑。

d. 為獲得平滑的路徑，采用等距離散點對粗解路徑進(jìn)行數(shù)值優(yōu)化，計算轉(zhuǎn)向角度差值并進(jìn)行障礙物碰撞判斷，若轉(zhuǎn)向角度均滿足θ2，則優(yōu)化成功。

3.1 目標(biāo)偏置策略和約束采樣

當(dāng)隨機點Xrand產(chǎn)生后，在隨機樹Tnode尋找與隨機點最近的Xnear，同時對兩點之間進(jìn)行障礙物檢測。若存在障礙物，則重新生成隨機點Xrand；如果兩者之間不存在障礙物，在兩者連線的方向以步長l進(jìn)行拓展，得到新節(jié)點Xnew：

[Xnew=Xnear+l×[sinαcosα]] （2）

式中：[α]為Xnear與Xrand之間的夾角。

3.2 采樣空間約束

為了提高隨機點的選取效率，對隨機點進(jìn)行了空間約束處理，如圖3所示。Xinit為隨機樹采樣起點，Xgoal為搜索目標(biāo)點，綠色區(qū)域為障礙物，實線為自由空間邊界，虛線為空間剪枝后的采樣區(qū)域，隨機點Xrand在虛線區(qū)域內(nèi)獲得。

3.3 車輛膨脹及障礙物避障策略

針對擴(kuò)展節(jié)點未考慮車輛真實尺寸導(dǎo)致碰撞障礙物的情況，本文采用車輛碰撞圓和偏置直線融合方法對障礙物進(jìn)行檢測，碰撞檢測原理如圖4、圖5所示。

車輛膨脹圓的計算公式為：

式中：[δ]為松弛因子，[L]為車輛長度，[B]為車寬度，R為車輛膨脹半徑。

為使車輛能夠更好地避開障礙物，采用偏置直線策略對當(dāng)前節(jié)點Xcur與新生成的節(jié)點Xnew連線進(jìn)行障礙物碰撞邏輯判斷，見圖5。lleft為左邊偏置直線，lright為右邊偏置直線，R為車輛膨脹半徑，當(dāng)lleft和lright兩條直線都不與障礙物發(fā)生碰撞時，表明車輛可以通行。

3.4 二次轉(zhuǎn)角約束

為獲取符合車輛轉(zhuǎn)向角度的路徑，計算出Xrand、Xrand、Xnew3個點間的航向角度θ，若θ滿足θ1約束，獲取第一次轉(zhuǎn)向約束的隨機點Xrand，然后依據(jù)式（2）獲得新節(jié)點Xnew：

式中：Xp為Xnear的父節(jié)點，Xnear為距離Xrand的最近點，[θ]為路徑航向角。

通過對Xrand的角度約束，不斷生成Xnew，當(dāng)Xnew與Xgoal的距離小于θ1時，獲得第一次轉(zhuǎn)角約束后的粗解路徑，如圖6所示。在圖6a中，θs1滿足θ1，其對應(yīng)的Xrand滿足約束，所以擴(kuò)展該方向節(jié)點生成Xnew，否則如圖6b所示，θs2未滿足約束要求，則不拓展Xnew。

當(dāng)獲得粗解路徑時，各個節(jié)點之間的曲率以及轉(zhuǎn)向角度并不同時滿足要求，可能存在較為尖銳的路徑點，為此，本文對粗解路徑進(jìn)行第二次轉(zhuǎn)角約束優(yōu)化以獲取優(yōu)化路徑，如圖7所示。

通過對粗解路徑當(dāng)前節(jié)點Xnear分別與Xp及Xnew兩者之間的連線進(jìn)行等距離離散化，可分別求得離散點n1、n2、n3、n4，將n3、n4兩離散點連接，取兩點的平均值，即可得到優(yōu)化節(jié)點Xpoint，通過式（4）計算三點之間的航向角θ，若θ滿足二次轉(zhuǎn)向角度約束的閾值θ2范圍內(nèi)，即用Xpoint代替Xnear，重復(fù)上述步驟即可得到優(yōu)化路徑，Xpoint選取計算方式為：

式中：i=（1，2，…，n-1）；N為離散點數(shù)量；m1、m2分別為第i點與第（i-1）點之間的單位向量及第（i+1）點與第i點之間的單位向量，nj與nk為離散點的坐標(biāo)信息，其中j=（1，2，…，N），k=（2N，2N-1，…，N）；?Si為Xnear與Xp的均勻離散距離；?Si+1為Xnear與Xnew的均勻離散距離。

3.4 B樣條曲線的路徑平滑

B樣條函數(shù)通過插值、逼近和擬合的方式應(yīng)用于路徑平滑處理。

本文采用3次B樣條曲線作為路徑平滑方法，三階B樣條函數(shù)計算如下：

式中：u為自變量，取值范圍為[0，1]；i為控制點索引；k為B樣條階數(shù)；Pi為第i個控制點；Bi，k（u）為第i個k階B樣條基函數(shù)；j為起始的控制點下標(biāo)索引，相當(dāng)于控制點從Pj到Pj+k；[Cjk+1=（k+1）！/（j?。╧+1-j）?。。

最終得到3次均勻B樣條曲線的基函數(shù)表達(dá)式為：

將其帶入式（6），可得：

4 仿真試驗

為了驗證所提出算法的有效性，對偏置RRT算法以及改進(jìn)的RRT算法分別在常規(guī)場景Map1、狹窄場景Map2、復(fù)雜場景Map3進(jìn)行20次和30次仿真試驗。其中，對隨機點Xrand的選取采用90°、75°、60°進(jìn)行第一次轉(zhuǎn)角約束確定Xnew，生成粗解路徑，之后對獲取的粗解路徑采用20°二次轉(zhuǎn)向角約束獲取優(yōu)化路徑，仿真試驗參數(shù)如表1所示。

4.1 擬合前路徑

圖8、圖9、圖10分別Map1、Map2、Map3地圖下偏置RRT算法與二次轉(zhuǎn)角約束的改進(jìn)RRT算法規(guī)劃路徑對比結(jié)果。從圖中可看出，改進(jìn)的RRT算法減小了樹節(jié)點的冗余，減少了無效節(jié)點的生成，且改進(jìn)的RRT算法都能夠?qū)φ系K物進(jìn)行避障。從圖8a、圖9a及圖10a可以看出，偏置RRT算法產(chǎn)生的路徑節(jié)點之間較為曲折，路徑不夠平滑。圖8b、圖9b以及圖10b中實線為第一次轉(zhuǎn)角約束獲取的粗解路徑，虛線為第二次轉(zhuǎn)向約束路徑的優(yōu)化路徑，由此可以看出，改進(jìn)的RRT算法大幅度地減小了路徑的曲折性，提高了節(jié)點之間連接的平滑性。

不同轉(zhuǎn)向角度約束下Map1、Map2、Map3的改進(jìn)RRT算法粗解路徑與優(yōu)化路徑仿真結(jié)果分析如圖11、圖12及圖13所示，實線為以90°、75°和60°為第一次轉(zhuǎn)向角度約束獲取的粗解路徑，虛線為以20°為第二次轉(zhuǎn)向角度約束的優(yōu)化路徑，可以明顯看出粗解路徑在節(jié)點連接某處轉(zhuǎn)向角度過大，路徑過于曲折，但當(dāng)轉(zhuǎn)向角角度逐漸減小時，轉(zhuǎn)角角度約束加強，節(jié)點之間的曲折性有所減弱，避免了尖銳節(jié)點的產(chǎn)生。

對粗解路徑進(jìn)行約束后獲得的優(yōu)化路徑相比于粗解路徑，可以明顯看出所改進(jìn)的基于二次轉(zhuǎn)角約束的RRT算法能更好地改善路徑節(jié)點之間的曲折性，獲取路徑更加平滑，符合車輛轉(zhuǎn)向要求。

為了更好地驗證改進(jìn)RRT算法的優(yōu)化效果，對偏置RRT算法及改進(jìn)的RRT算法得到的仿真結(jié)果進(jìn)行統(tǒng)計，表2所示為第一次轉(zhuǎn)角約束90°、75°和60°，第二次轉(zhuǎn)角約束20°生成的優(yōu)化路徑與偏置RRT算法生成路徑的各參數(shù)對比。在Map1常規(guī)地圖中，可看到在不同轉(zhuǎn)角約束條件下，優(yōu)化路徑最大曲率降低43.33%、路徑平均曲率降低48.33%、平均擴(kuò)展節(jié)點降低44.59%、平均距離降低8.56%、平均時間降低23.81%；在Map2狹窄地圖中，優(yōu)化路徑最大曲率降低38.88%、路徑平均曲率降低了48.41%、平均擴(kuò)展節(jié)點降低59.13%、平均距離降低9.59%，但由于Map2的復(fù)雜性及空間的狹窄性，可以看到隨著第一次轉(zhuǎn)角約束的加強，雖然導(dǎo)致采樣節(jié)點增多，平均時間逐漸增大，但提高了節(jié)點之間的連接平滑性；在Map3復(fù)雜地圖中，優(yōu)化路徑最大曲率降低39.76%、路徑平均曲率降低51.61%、平均擴(kuò)展節(jié)點降低72.59%、平均距離降低11.61%、平均時間降低了72.05%。

由此可知，本文所提出的算法各項指標(biāo)明顯提升，能夠在不同角度約束下規(guī)劃出滿足車輛正常行駛的路徑，減小道路曲率及路徑節(jié)點數(shù)量，提高路徑平滑性。

4.2 B樣條曲線擬合路徑

采用3次均勻B樣條曲線將90°、75°、60°3種轉(zhuǎn)向角度約束下獲取得到的粗解路徑與優(yōu)化后的路徑進(jìn)行擬合，驗證所提出算法的有效性。

Map1、Map2、Map3擬合的路徑如圖14～圖16所示，其中虛線為粗解路徑擬合結(jié)果，點線為優(yōu)化路徑擬合結(jié)果。分析可知，經(jīng)過第一次轉(zhuǎn)角約束后，不同場景下擬合的粗解路徑仍存在尖銳點，導(dǎo)致路徑不夠平滑。從圖中的3種約束角度對比可知，隨著約束角度降低，路徑的曲折性相對降低，圖14c的粗解擬合路徑曲線并未出現(xiàn)更大的曲折點，對比粗解擬合路徑和優(yōu)化擬合后路徑可知，優(yōu)化擬合后的路徑在不同的場景都較為平滑，路徑的轉(zhuǎn)角角度得到了更好的優(yōu)化，符合車輛的轉(zhuǎn)角角度約束，減小了路徑的曲率，擬合路徑更加平滑，驗證了所提出算法的有效性。

5 結(jié)束語

本文針對傳統(tǒng)RRT算法搜索速率慢、節(jié)點擴(kuò)展冗余、路徑點不滿足車輛轉(zhuǎn)向角度條件及曲率不平滑問題，提出了二次轉(zhuǎn)角約束的改進(jìn)的RRT算法?；贛ATLAB平臺搭建了常規(guī)地圖、狹窄地圖和復(fù)雜地圖，對所提出的改進(jìn)RRT算法與偏置RRT算法進(jìn)行仿真分析。仿真結(jié)果表明，所提出的改進(jìn)RRT算法總體上使路徑最大曲率降低了34.33%，平均曲率降低了47.36%，擴(kuò)展節(jié)點數(shù)降低47.62%，路徑距離降低了7.76%，規(guī)劃時間降低14.98%，擬合后的路徑更加平滑，在滿足車輛轉(zhuǎn)角的同時，減少了行駛距離，提高了車輛行駛舒適性。

參 考 文 獻(xiàn)

[1] TAHA A E， ABUALI N. Route Planning Considerations for Autonomous Vehicles[J]. IEEE Communications Magazine， 2018， 56（10）： 78-84.

[2] BADUE C， GUIDOLINI R， CARNEIRO R V， et al. Self-Driving Cars： A Survey[J]. Expert Systems with Applications， 2021， 165.

[3] GONZ?LEZ D， P?REZ J， MILAN?S V， et al. A Review of Motion Planning Techniques for Automated Vehicles[J]. IEEE Transactions on Intelligent Transportation Systems， 2015， 17（4）： 1135-1145.

[4] LAVALLE S M， KUFFNER J J. Randomized Kinodynamic Planning[J]. The International Journal of Robotics Research， 2001， 20（5）： 378-400.

[5] CHEN C， JIANG J， LV N， et al. An Intelligent Path Planning Scheme of Autonomous Vehicles Platoon Using Deep Reinforcement Learning on Network Edge[J]. IEEE Access， 2020， 8： 99059-99069.

[6] DORIGO M， MANIEZZO V， COLORNI A. Ant System： Optimization by A Colony of Cooperating Agents[J]. IEEE Transactions on Systems， Man， and Cybernetics， Part B （Cybernetics）， 1996， 26（1）： 29-41.

[7] V?RAS L G D O， MEDEIROS F L L， GUIMAR?ES L N F. Systematic Literature Review of Sampling Process in Rapidly-Exploring Random Trees[J]. IEEE Access， 2019， 7： 50933-50953.

[8] 欒添添， 王皓， 孫明曉， 等.基于動態(tài)變采樣區(qū)域RRT的無人車路徑規(guī)劃[J]. 控制與決策， 2023， 38（6）： 1721-1729.

LUAN T T， WANG H， SUN M X， et al. Path Planning of Unmanned Vehicle Based on Dynamic Variable Sampling Area RRT[J]. Control and Decision， 2023， 38（6）： 1721-1729.

[9] ZHANG Y， WANG R， SONG C， et al. An Improved Dynamic Step Size RRT Algorithm in Complex Environments[C]. 2021 33rd Chinese Control and Decision Conference （CCDC）. Kunming， China： IEEE， 2021.

[10] CHEN J， ZHAO Y， XU X. Improved RRT-Connect Based Path Planning Algorithm for Mobile Robots[J]. IEEE Access， 2021， 9： 145988-145999.

[11] GHOSH D， NANDAKUMAR G， NARAYANAN K， et al. Kinematic Constraints Based Bi-Directional RRT （KB-RRT） with Parameterized Trajectories for Robot Path Planning in Cluttered Environment[C]// 2019 International Conference on Robotics and Automation （ICRA）. Montreal， Canada： IEEE， 2019.

[12] 許萬， 楊曄， 余磊濤， 等. 一種基于改進(jìn)RRT*的全局路徑規(guī)劃算法[J]. 控制與決策， 2022， 37（4）： 829-838.

XU W， YANG Y， YU L T， et al. A Global Path Planning Algorithm Based on Improved RRT*[J]. Control and Decision， 2022， 37（4）： 829-838.

[13] 朱冰， 韓嘉懿， 趙健， 等. 基于安全場改進(jìn)RRT*算法的智能汽車路徑規(guī)劃方法[J]. 汽車工程， 2020， 42（9）： 1145-1150.

ZHU B， HAN J Y， ZHAO J， et al. Safety Field-Based Improved RRT* Algorithm for Path Planning of Intelligent Vehicle[J]. Automotive Engineering， 2020， 42（9）： 1145-1150.

[14] SHI Y， LI Q， BU S， et al. Research on Intelligent Vehicle Path Planning Based on Rapidly-Exploring Random Tree[J]. Mathematical Problems in Engineering， 2020， 2020： 1-14.

[15] TAHIR Z， QURESHI A H， AYAZ Y， et al. Potentially Guided Bidirectionalized RRT* for Fast Optimal Path Planning in Cluttered Environments[J]. Robotics and Autonomous Systems， 2018， 108： 13-27.