基于優化快速搜索隨機樹算法的全局路徑規劃

【摘要】為了改善傳統快速搜索隨機樹（RRT）算法在全局路徑規劃中存在的平滑度差、具有潛在碰撞性等問題，提出了一種雙重優化的RRT算法。在傳統RRT算法基礎上，引入自適應目標偏向策略以縮短采樣時間，引入角度約束采樣策略以適應車輛極限轉角。得到初始路徑后，建立二項優化函數（即降低路徑曲率和遠離障礙物），并將其作為基點進行梯度下降二次優化，生成可供車輛行駛、平滑性良好且碰撞概率低的路徑，并進行仿真驗證。結果表明：優化RRT算法相比于傳統RRT算法、RRT-Connect算法和RRT*算法，平均曲率分別降低了38.1%、36.4%和24.7%，曲率均方差分別降低了38.4%、38.4%和27.2%。

主題詞：快速搜索隨機樹 全局路徑規劃 避障 梯度下降法

中圖分類號：TP242 " 文獻標志碼：A " DOI： 10.19620/j.cnki.1000-3703.20230346

The Global Path Planning Algorithm Based on Optimization RRT Algorithm

Yang Wei1， Tan Liang1， Sun Xue1， Du Yafeng1， Zhou Xiaobing1，2

（1. Chang’an University， Xi’an 710064; 2. Commercial Vehicle Development Institute， FAW Jiefang Automobile Co.， Ltd.， Changchun 130011）

【Abstract】In order to improve the shortcomings of poor smoothness and potential collision in traditional Rapidly-exploring Random Tree （RRT） algorithm for global path planning， the paper proposed a dual-optimization RRT algorithm. Based on the traditional RRT algorithm， an adaptive target bias strategy was introduced to shorten the sampling time， and an angle-constrained sampling strategy was introduced to adapt to the vehicle’s maximum steering angle. After the initial path was obtained， a binary optimization function （reducing path curvature and avoiding obstacles） was established and used as a basis for gradient descent secondary optimization， generating a path that can be driven by vehicles with good smoothness and low collision probability， which was then simulated and verified. The results show that compared with RRT algorithm， RRT-Connect algorithm and RRT* algorithm， the optimized RRT algorithm reduces average curvature by 38.1%， 36.4% and 24.7%， respectively; while reducing curvature variance by 38.4%， 38.4% and 27.2%， respectively.

Key words： Rapidly-exploring Random Tree （RRT）， Global path planning， Obstacle avoidance， Gradient descent method

【引用格式】 楊煒， 譚亮， 孫雪， 等. 基于優化快速搜索隨機樹算法的全局路徑規劃[J]. 汽車技術， 2024（3）： 31-36.

YANG W， TAN L， SUN X， et al. The Global Path Planning Algorithm Based on Optimization RRT Algorithm[J]. Automobile Technology， 2024（3）： 31-36.

1 前言

無人駕駛汽車的全局路徑規劃旨在利用路徑搜索算法，在已知起點位置、終點位置和環境信息的情況下，搜尋可行駛路徑并滿足安全、無碰撞和平滑等條件[1]。

目前，國內外常用的無人駕駛汽車全局路徑生成方法有基于搜索的迪杰斯特拉（Dijkstra）[2]和A*[3]算法，生成的路徑距離雖然較短，但沒有使用運動學約束，實際規劃效果并不理想。智能仿生類算法[4]、蟻群算法[5]由于計算量過大，無法滿足無人車對實時性的要求。

快速搜索隨機樹[7]（Rapidly-exploring Random Tree，RRT）算法是一種基于采樣的算法，通過在狀態空間進行隨機采樣到達目標點。但由于缺乏約束及采樣的隨機性，導致收斂速度慢，生成的路徑光滑性不足。針對傳統RRT算法的各類問題，國內外學者提出了不同的改進方法：Taheri等[7]提出模糊貪婪RRT算法，通過模糊控制實現了節點數量的減少與路徑曲率的減小；張衛波等[8]利用同心圓采樣策略和臨近點選擇方法改進RRT的采樣策略生成路徑，并利用三次B樣條曲線對路徑進行二次優化，試驗結果表明，該算法在路徑平滑性和長度上與其他改進算法相比顯著改善；Zaid等[9]通過將人工勢場（Artificial Potential Field，APF）法納入雙向勢場梯度啟發式的方法對PIB-RRT*（Probabilistic Informed Biased-Rapidly Exploring Random Trees*）和PB-RRT*（Probabilistic Roadmap Rapidly-Exploring Random Trees*）算法進行改進，使算法的效率和路徑的平滑性顯著提高；Ska?kauskas等[10]首先人為規定路徑規劃的起始點、終點及轉折點，連接后形成粗略路徑，將其轉換為杜賓斯（Dubins）路徑，通過實車試驗驗證了算法的有效性；臧強等[11]提出了一種基于RRT*的路徑規劃算法，并將RRT*算法與自適應人工勢場（Adaptive Artificial Potential Field，AAPF）法相結合，提高了路徑規劃效率和路徑的平滑度；胡小平等[12]提出了3種改進勢場方法，并通過試驗與仿真驗證了方法的有效性；Mohammed等[13]通過限制算法節點生成的區域或體積縮短了路徑生成的時間。

綜上，多數文獻只關注改善路徑平滑度和路徑生成時間，而未針對實際車輛的運動姿態進行優化。本文提出自適應偏向目標策略，以改善傳統RRT算法在全局路徑規劃中存在的平滑度差和潛在碰撞危險等問題為目標，引入角約束策略以滿足實際車輛的運動需求，并建立梯度優化函數進一步優化生成的路徑。

2 車輛行駛場景下的傳統RRT算法優化

2.1 傳統RRT算法

RRT算法首先以無人駕駛汽車初始狀態qinit作為初始節點生成隨機樹T，在可行空間qgoal中隨機采樣，得到隨機點qrand，遍歷隨機樹T，查找與隨機點qrand最近的節點qnear，由qnear向qrand以隨機步長D進行擴展得到節點qnew，判斷qnear向qnew擴展過程中是否與障礙物節點發生碰撞，若發生碰撞，則重新采樣，否則將節點qnew添加到隨機樹T中，并將qnear設為節點qnew的父節點，重復以上步驟，直到將目標點qgoal加入隨機樹時結束循環。由qgoal沿父節點回溯到qinit，即可得到可行路徑，如圖1所示。針對傳統RRT算法存在的一些不足，本文在RRT的基礎上加以改進。

2.2 基于車輛行駛場景的傳統RRT算法改進

2.2.1 基于偏向目標擴展的采樣策略

APF方法是一種局部優化算法，它以目標與障礙物的合力方向作為節點擴展的方向，從而加速了節點的擴張。然而，這也容易導致在擴展節點的力平衡處無法生成節點。傳統的人工勢場主要用于牽引小車的局部路徑規劃，本文利用該思想改進節點擴展策略，障礙物對擴展節點的斥力場Ur（p）為：

Ur（p）=kr·[ρ（p， pobs）]-2/2 " " " " " " " " " " " " （1）

式中：kr為斥力場增益系數，ρ（p， pobs）為父節點與最近的障礙物的距離。

則最近障礙物對節點的斥力Fr（p）為：

Fr（p）=kr·[ρ（p， pobs）]-2 " " " " " " " " " " " " " （2）

為了降低RRT算法搜索過程的盲目性，引入目標動態概率采樣。動態設置概率閾值obia∈（0，1），每次采樣前生成隨機值p∈（0，1），如果plt;obia，以目標點為擴展方向，否則按原定的隨機步長進行擴展，以跳出與目標的碰撞。

obia的計算公式為：

obia=1-Fr（p）/Fmax " " " " " " " " " " " " （3）

式中：Fmax為最大斥力；當斥力占比大時，以小步長生成節點，斥力占比小時以大步長生成擴展節點。

本文采用如圖2示的柵格地圖生成路徑。由圖2可以看出，采取目標偏向策略后的RRT有目的性地向目標節點生長，隨機樹的采樣節點也比未采取目標偏向策略的RRT少很多，但仍存在潛在的碰撞危險及路徑曲率較大、不易于車輛行駛等弊端。

2.2.2 基于車輛轉角約束的重采樣策略

算法生成的路徑要符合傳統車輛模型的實際要求，規劃的路徑才會具有現實指導意義。車輛模型如圖3所示，在車輛勻速運動的狀態下，且側向加速度限定在0.4 g以下、輪胎側偏特性處于線性范圍時，可以將汽車模型簡化為二自由度模型[14]。設（x，y）為車輛后軸中心點的位置坐標，R為車輛的轉彎半徑，l為軸距，δf為前輪轉角，vf、vr分別為前、后輪的瞬時速度，基于車輛運動學模型可知，在已知軸距l與最小轉彎半徑Rmin的情況下，可求得車輛的前輪最大轉向角：

δfmax=arctan（l/Rmin） " " " " " " " " " " " " " （4）

由于每一次采樣均在可行域中隨機采樣，很可能會出現生成大轉角節點的情況，如圖4所示，其中qnear為父節點，qrand為臨時節點，qnew為新節點。可以看出，傳統RRT算法采樣的新節點超出了車輛的轉向極限，理應被去除，因此，在RRT節點生成時應加入轉向約束條件，限制新節點生成時的方向，使其滿足車輛的最大轉向極限。

對于每個擴展的節點qnew，邊E（qnear，qnew）與邊E（qn，qnear）所形成的夾角δ都不應超過車輛前輪最大轉角δfmax。

要想使生成的新節點qnew滿足前輪最大轉向角δfmax約束，應采用如下方式生成新節點：

θ=θnear+rand（-1，1）δfmax " " " " " " " " " " " （5）

qnew=（xnear，ynear）+（cos（θ），sin（θ））D " " " " " " " " （6）

式中：θ、θnear分別為邊E（qnear，qnew）、邊E（qn，qnear）與全局坐標系X軸的夾角，rand（-1，1）為[-1，1]范圍內的隨機數，D為隨機樹節點的擴展隨機步長，（xnear，ynear）為qnear在全局坐標系下的坐標。

該方法可以使生成的節點滿足車輛的最大轉向角約束，同時保持RRT算法的隨機性。

如圖5所示為未采用運動學約束和采用運動學約束時產生的路徑。由圖5可知，采用運動學約束后部分節點的轉角明顯比未加約束時的轉角小，且未采用運動學約束時部分路徑存在尖銳轉角，無法滿足車輛的實際行駛需求。雖然經過轉角優化后曲率略有減小，但路徑仍不夠理想。

通過對比可以發現，在本文提出的運動學約束下生成的路徑相較于傳統RRT算法更加平滑，但仍存在較高的碰撞概率。

3 梯度下降法的再次優化

3.1 梯度下降法優化原理

梯度下降法是求解無約束優化問題最常采用的方法之一，通過迭代求解得到最小化優化函數，從而實現對于目標值的優化。

本文根據梯度下降法原理設計合理的優化函數對RRT算法求解得的初始路徑進行優化，優化函數P由2個部分組成：

P=wo·Pobs+wc·Pcur " " " " " " " " " " " " " "（7）

式中：Pobs為障礙物函數，用于懲罰車輛與障礙物的碰撞，從而引導車輛遠離不可行駛區域來保證安全性；Pcur為曲率函數，用于約束每個路徑點的最大曲率，避免路徑曲率過大；wo、wc分別為障礙物函數、曲率函數的權重，用于限制各函數的影響。

3.2 優化函數設計

3.2.1 路徑安全性優化函數設計

障礙物函數Pobs可表示為：

[Pobs=i=1Nxi-oi-dobs2xi-oi-dobs] " " " " " （8）

式中：xi=（xi，yi）為路徑點的向量坐標，oi為與路徑點xi最近的障礙物點的向量坐標，dobs為最大距離的閾值。

其中，對于所有的頂點xi，有|xi-oi|≤dobs，即當路徑點與最近障礙物的距離大于dobs時，Pobs不起作用。為了在接近障礙物時加重懲罰，令σobs=（|xi-oi|-dobs）2，采用二次懲罰函數。可求得σobs在點xi處的梯度為：

[?σobs?xi=2xi-oi-dobsxi-oixi-oi] " " " " " （9）

3.2.2 路徑舒適性優化函數設計

路徑點處的最大曲率主要由相鄰的2個節點決定，分別用xi-1、xi、xi+1表示3個點的向量坐標，Δφi為路徑在點xi處的角度改變量。則曲率函數Pcur可表示為：

[Pcur=i=1N-1σcurΔφiΔxi-κmax] " " " " " " " " " "（10）

式中：σcur為懲罰函數，Δxi=xi-xi-1為xi處的位移向量，κmax為最大允許曲率。

路徑點處切向角的變化表示為：

[Δφi=arccosΔxTiΔxi+1ΔxiΔxi+1] " " " " " " " " " " "（11）

曲率κcur=Δφi/Δxi，當κcur≥κmax時，最大允許曲率的偏差用懲罰函數σcur進行懲罰。可求得κcur在3個點處的梯度分別為：

[?κcur?xi=-1Δxi?Δφi?cos（Δφi）?cos（Δφi）?xi-?ΔφiΔx2i?Δxi?xi] " "（12）

[?κcur?xi-1=-1Δxi?Δφi?cos（Δφi）?cos（Δφi）?xi-1-?ΔφiΔx2i?Δxi?xi-1] " （13）

[?κcur?xi+1=-1Δxi?Δφi?cos（Δφi）?cos（Δφi）?xi+1] " " " " "（14）

通過這3項加權求和得到曲率懲罰梯度：

[?Δφi?cos（Δφi）=?arccoscos（Δφi）?cos（Δφi）=-11-cos2（Δφi）1/2] " " （15）

對生成的路徑采用本文所提出的梯度優化法進行二次優化，經過梯度下降優化前、后的路徑如圖6所示。由對比結果可知，經過梯度下降法優化后的路徑曲率更加平滑且無突變產生。

4 驗證與分析

為了充分說明改進后的RRT算法的優化效果，將其與RRT算法、RRT*算法和RRT-Connect算法所規劃的路徑進行對比。在圖7所示的場景下，每個算法各進行12次仿真，去掉最優和最差值后統計平均曲率、時間、曲率均方差以及節點數量，結果如圖8和表1所示。相較于其他3種方法，本文提出的改進RRT算法在平均曲率和曲率均方差上存在明顯優勢：平均曲率為0.009 m-1，曲率均方差為0.008 m-1，并具有良好的穩定性。小的平均曲率和曲率均方差意味著路徑點之間沒有過大的轉向突變，有利于提高車輛乘坐舒適度和跟蹤效果；此外，改進RRT算法在4種方法中計算時間較短，能夠提高規劃效率。

5 結束語

本文提出了一種雙重優化RRT算法，采用角約束采樣策略來改善路徑生成，并使用梯度下降法對產生的路徑進行二次優化，從而得到更加平滑、易于車輛行駛的路徑，提高實際車輛行駛的安全性和舒適性，并通過仿真驗證將本文算法與RRT及其衍生算法的路徑規劃結果進行了對比分析。仿真結果表明，相比于其他算法，本文算法在路徑曲率方面更具優勢，有效改善了車輛在實際行駛工況下的安全舒適性。

參 考 文 獻

[1] 朱冰， 韓嘉懿， 趙健， 等. 基于安全場改進RRT～*算法的智能汽車路徑規劃方法[J]. 汽車工程， 2020， 42（9）： 1145-1150+1182.

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

[2] 葉穎詩， 魏福義， 蔡賢資. 基于并行計算的快速Dijkstra算法研究[J]. 計算機工程與應用， 2020， 56（6）： 58-65.

YE Y S， WEI F Y， CAI X Z. Research on Fast Dijkstra’s " Algorithm Based on Parallel Computing[J]. Computer " " " "Engineering and Applications， 2020， 56（6）： 58-65.

[3] HONG Z H， SUN P F， TONG X H， et al. Improved A-Star Algorithm for Long-Distance Off-Road Path Planning Using Terrain Data Map[J]. ISPRS International Journal of " " " " " " Geo-Information， 2021， 10（11）.

[4] SANGEETHA V， RAVICHANDRAN K S， SHEKHAR S， et al. An Intelligent Gain-Based Ant Colony Optimisation Method for Path Planning of Unmanned Ground Vehicles[J]. Defence Science Journal， 2019， 69（2）： 167-172.

[5] 李根， 李航， 張帥陽， 等. 基于蟻群算法的最優路徑規劃及參數研究[J]. 中國科技論文， 2018， 13（16）： 1909-1914.

LI G， LI H， ZHANG S Y， et al. Research on Optimal Path Planning and Parameters Based on ant Colony Algorithm[J]. China Science and Technology Papers， 2018， 13（16）： 1909-1914.

[6] LAVALLE S M. Rapidly-Exploring Random Trees： A New Tool for Path Planning[J]. Algorithmic and Computational Robotics New Directions， 1998： 293-308.

[7] TAHERI E， FERDOWSI M H， DANESH M. Fuzzy Greedy RRT Path Planning Algorithm in a Complex Configuration Space[J]. International Journal of Control Automation and Systems， 2018， 16（6）： 3026-3035.

[8] 張衛波， 肖繼亮. 改進RRT算法在復雜環境下智能車路徑規劃中的應用[J]. 中國公路學報， 2021， 34（3）： 225-234.

ZHANG W B， XIAO J L. Application of Improved RRT " " "Algorithm in Intelligent Vehicle Path Planning Complex " "Environment[J]. Chinese Journal of Highways， 2021， 34（3）： 225-234.

[9] ZAID T， QURESHI A H， YASAR A， et al. Potentially " " Guided Bidirectionalized RRT* for Fast Optimal Path " "Planning in Cluttered Environments[J]. Robotics and Autonomous Systems， 2018， 108： 13-27.

[10] SKA?KAUSKAS P， SOKOLOVSKIJ E. Analysis of the " "Hybrid Global Path Planning Algorithm for Different " " " Environments[J]. Transport and Telecommunication， 2019， 20（1）： 1-11.

[11] 臧強， 張國林， 靳雨桐， 等. 一種基于動態步長的AAPF-RRT*移動機器人路徑規劃新算法[J]. 中國科技論文， 2021， 16（11）： 1227-1233+1270.

ZANG Q， ZHANG G L， JIN Y T， et， al. A New AAPF-RRT* Mobile Robot Path Planning Algorithm Based on " Dynamic Step Size[J]. China Science and Technology " " "Paper， 2021， 16（11）： 1227-1233+1270.

[12] 胡小平， 曹敬. 一種多機器人的改進勢場路徑規劃算法[J]. 機械科學與技術， 2018， 37（8）： 1207-1216.

HU X P， CAO J. An Improved Potential Field Path " " " "Planning Algorithm for Multiple Robots[J]. Mechanical " "Science and Technology， 2018， 37（8）： 1207-1216.

[13] MOHAMMED H， JARADAT M A， ROMDHANE L B. RRT*N： An Improved Rapidly-Exploring Random Tree Approach for Reduced Processing Times[C]// 2018 11th " International Symposium on Mechatronics and Its " " " " " Applications （ISMA）. Sharjah， United Arab Emirates： IEEE， 2018.

[14] 陳特， 陳龍， 徐興， 等. 基于Hamilton理論的無人車路徑跟蹤控制[J]. 北京理工大學學報， 2019， 39（7）： 676-682.

CHEN T， CHEN L， XU X， et al. Unmanned Vehicle Path Tracking Control Based on Hamilton Theory[J]. " " " " " " Transactions of Beijing Institute of Technology， 2019， 39（7）： 676-682.

（責任編輯 斛 畔）

修改稿收到日期為2023年6月8日。