基于二次轉角約束的改進RRT路徑規劃算法研究

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

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

主題詞：改進RRT算法 轉向角度約束 路徑規劃 路徑曲率 路徑平滑性

中圖分類號：U461.99? ?文獻標志碼：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 前言

路徑規劃技術作為無人駕駛汽車實現自主導航的關鍵技術之一，對車輛安全行駛起著至關重要的作用，逐漸成為無人駕駛車輛領域研究熱點[1-2]。

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

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

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

2 偏置RRT算法

2.1 基本RRT算法原理

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

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

2.2 目標約束采樣

為了降低RRT算法拓展節點的盲目性，使隨機樹尋找過程更迅速，本文對隨機點的選取進行約束。基于概率p對目標節點進行偏置采樣，若隨機率p產生的概率小于設定偏置概率閾值p1，則在自由空間里進行隨機采樣，反之，隨機點即為目標點Xgoal，偏置采樣公式為：

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

3 改進的RRT算法原理

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

改進RRT算法路徑生成步驟如下：

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

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

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

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

3.1 目標偏置策略和約束采樣

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

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

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

3.2 采樣空間約束

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

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

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

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

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

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

3.4 二次轉角約束

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

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

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

當獲得粗解路徑時，各個節點之間的曲率以及轉向角度并不同時滿足要求，可能存在較為尖銳的路徑點，為此，本文對粗解路徑進行第二次轉角約束優化以獲取優化路徑，如圖7所示。

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

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

3.4 B樣條曲線的路徑平滑

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

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

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

最終得到3次均勻B樣條曲線的基函數表達式為：

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

4 仿真試驗

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

4.1 擬合前路徑

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

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

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

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

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

4.2 B樣條曲線擬合路徑

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

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

5 結束語

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

參 考 文 獻

[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] 欒添添， 王皓， 孫明曉， 等.基于動態變采樣區域RRT的無人車路徑規劃[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] 許萬， 楊曄， 余磊濤， 等. 一種基于改進RRT*的全局路徑規劃算法[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] 朱冰， 韓嘉懿， 趙健， 等. 基于安全場改進RRT*算法的智能汽車路徑規劃方法[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.