改進RRT*算法的智能車輛路徑規劃

趙港

改進RRT*算法的智能車輛路徑規劃

趙港

（長安大學 汽車學院，陜西 西安 710064）

RRT*算法是智能車路徑規劃的常用方法，但傳統RRT*算法在汽車路徑規劃方面存在路徑轉折多、曲率波動較大等問題。針對這些問題，文章提出了一種改進RRT*算法，通過距離公式選取次級節點，同時添加路徑轉折點角度約束，從而達到限制最小路徑段長度和減小路徑曲率變化的目的。通過MATLAB仿真平臺對改進的RRT*算法進行仿真驗證。實驗結果表明，該算法能夠提高生成軌跡的平滑度，規劃的路徑更符合車輛在行駛過程中的軌跡特征。

路徑規劃；RRT*算法；角度約束

前言

隨著現代社會汽車智能化程度的不斷提高，路徑規劃已經成為智能駕駛汽車上的一個重要研究方面。路徑規劃主要目的是讓目標對象在規定范圍的區域內找到一條從起點到終點的無碰撞安全路徑，是智能汽車完成駕駛任務的必要條件[1]。目前針對智能車路徑規劃的相關研究成果，大部分都是對移動機器人路徑規劃方法進行修改和調整得來的。路徑規劃算法主要有：基于圖搜索的方法，以A*算法和D*算法為典型代表；基于采樣的方法，以概率路圖法（PRM）和RRT為代表；基于離散優化的方法；人工勢場法：通過構建虛擬力場的方法，假設障礙物與車輛之前存在斥力作用，而目標點與車輛之前存在引力作用，通過計算合力的方法為車輛規劃出一條可通過的安全路徑。

文獻[2]用復雜度低的度量函數以提高隨機樹的求解速度，嵌入Dijkstra算法進行優化，提高了算法的收斂速度。文獻[3]通過目標導向思想對隨機樹中采樣點的產生進行改進，引導隨機樹偏向目標點生長，降低了擴展的復雜度。文獻[4]采用雙向隨機樹和多棵局部隨機樹的探索與合并，增加引力分量，使雙向隨機樹朝著各自目標方向生長，減少了算法的隨機性。以上對于RRT*算法的優化并未考慮到生成軌跡的平滑性，因此，本文通過距離公式選取次級節點，避免因隨機生成樹的樹枝過短而導致曲率大幅度波動的問題，并添加路徑轉折點角度約束，從而確保生成的軌跡平滑，滿足智能駕駛汽車動力學約束。

1 RRT*算法

RRT*算法基于隨即采樣的方式來搜索空間，通過連接每個相鄰的采樣點構成隨機生成樹。根據地圖信息得到障礙物的位置和狀態信息，將全局配置信息記為，包含障礙物的空間記為obstacle，無障礙空間記為free，對象出發點記為init，目標點記為goal，RRT*算法原理如圖1所示。

圖1 RRT*算法原理圖

（1）初始化隨機樹T，此時T中只有一個起始出發點init。

（2）在無障礙空間free中隨機生成一個次級節點near，并且按照設定的擴展步長生成一個新的節點new，連接near與new，判斷兩點間的連線與obstacle是否沖突，此操作被稱為碰撞檢測，如果new通過碰撞檢測，則將新節點new添加到隨機樹T上。

（3）在new的規定鄰域內搜索滿足要求的所有節點集合，圖1中已用圓圈注明。此時將圓圈內其他點與near之間的代價函數作比較，若near到new的代價函數比通過1、2再到new的代價函數低，則將near與new進行連線。同時，還要對比從near出發到2的代價函數最低路線，若通過new到達2的代價函數最低，則將2的父節點更新為new，并更新代價函數。

（4）重復以上過程，直到直到達到迭代上限或找到目標點為止。通過對傳統RRT*算法的算法流程可知，該算法按照隨機采樣的方式來尋找采樣點，從而擴展隨機樹T。由于采樣的隨機性，會導致無障礙空間free上任意一點會被占據，而且采樣點距離隨機樹T上點的距離存在不確定性，擴展的子節點隨機分布將會導致以下問題：隨機擴展中每個節點間連線長度具有隨機性，從而導致連線段數過多，生成的路徑曲率變化隨機，很難滿足智能汽車相關動力學約束的問題。

2 改進RRT*算法

2.1 最近次級節點的選擇

RRT*算法在每次生成新的節點時都會根據距離公式選取距離隨機樹T上已知節點最近的點作為次級結點near。但是，如果選取兩點間的最短距離過小，就會導致隨機樹T的樹枝過短，生成的路徑大幅度波動。因此，本文引入歐氏距離公式對當前節點pre與隨機搜索到的節點rand之間的距離進行約束，規定兩點間的最小距離不得小于設定的探索步長step，如果距離滿足要求，就將新節點添加到隨機樹T上，否則重新生成新的節點進行判斷。表1為改進RRT*搜索方法的算法偽代碼：

表1 改進RRT*搜索方法的算法框架

次級結點qnear的選擇策略 function Selection（） qrand=Sample_point（） distance=（qpre，qrand） if distance>step return qnear=qrand；end else return 2

2.2 路徑轉折點角度約束

RRT*算法生成節點具有隨機性，導致生成的路徑不平滑。在進行智能車輛路徑規劃時，沒有考慮到汽車的動力學約束、最小轉彎半徑等因素，因此要盡可能控制路徑的曲率范圍。

圖2為生成路徑轉折角度示意圖。pre為當前節點，2為pre的父節點，rand為本次隨機生成的節點。點pre與其父節點2連接構成的線段為，點pre與rand連接構成的線段為，兩條線段構成的夾角為beta。當角beta越大時，生成的曲線越平滑，曲率波動越小。

圖2 路徑轉折點角度示意圖

因此在RRT*算法中加入角度判別函數，只有當角beta大于90°時，才能將rand加入隨機樹T中。設2坐標為（1，1），pre坐標為（2，2），rand坐標為（3，3），此時線段的斜率1表示為：

由式（1）和（2）可求得角beta的值：

3 仿真驗證

為驗證改進RRT*算法的性能，在Matlab仿真平臺上進行試驗。為了使改進后的算法可控，加入循環次數的限制。如果在規定循環次數內隨機生成樹T無法搜索到目標點goal，則顯示算法返回失敗，重新設置參數再次進行仿真驗證。

設置大小為500×500像素的正方形仿真環境，被研究對象的初始位置坐標為[20,120]，目標位置的坐標為[490,490]，規定拓展步長為20，迭代次數最大值設定為10 000。分別采用原始RRT*算法和本文改進的算法進行路徑規劃，得到的仿真結果如圖3所示。

圖3 仿真結果圖

智能汽車行駛環境如圖3(a)和圖3(b)所示，黑色區域為汽車行駛過程中的障礙物以及道路約束，空白區域為無障礙空間free。為驗證改進RRT*算法的有效性，設置完全相同的仿真環境，經過隨機生成樹的擴展后，規劃的路徑為圖3(a)和圖3(b)中的曲線。

為分析改進RRT*算法的性能，在圖3(c)給出兩種算法路徑規劃的結果圖。在初始位置附近，由于周圍障礙物單一，對隨機生成樹的擴展影響較小，本文算法與原始RRT*算法所規劃的路徑基本重合。到達橫坐標X為150附近，開始靠近第一個障礙物，原始RRT*算法所生成的路徑曲率開始大幅度波動，軌跡間的連線角度接近90°。與此相比，本文算法生成的路徑曲率變化較為平緩，軌跡間的連線角度接近160°。到達橫坐標X為220之后，開始從兩個障礙物中間通過，存在多個障礙物的干擾，可以看出兩種算法規劃的路徑在此處有明顯差異。原始RRT*算法由于沒有路徑轉折點角度約束，生成的路徑有尖銳的夾角(夾角小于60°），沒有考慮到車輛的動力學約束。而本文算法生成的路徑轉折點最小角度為100°，考慮到了汽車在行駛過程中的軌跡特征。橫坐標X為350之后，由于周圍只存在一個障礙物，對算法規劃路徑的影響較小，本文算法與原始RRT*算法所規劃的路徑基本一致。

可以看出，相比于原始算法，經過改進后的算法規劃的軌跡的曲率波動明顯減小，軌跡也更平滑，能夠更好的滿足智能汽車在行駛過程中的運動學要求。

4 結束語

本文在現有RRT*算法的基礎上提出了一種改進算法，通過引入歐氏距離公式對次級節點的選擇進行限制，優化部分路徑段的長度。同時，考慮到汽車行駛運動學的問題，通過設置轉折點角度約束，提高路徑平滑度。基于Matlab仿真平臺對改進前后的RRT*算法進行仿真驗證，結果表明，改進后的RRT*算法能夠有效地降低路徑規劃的曲率波動，相比于比原算法，可以生成更為合理的路徑。本文的仿真驗證環節只在靜態環境下進行，并沒有考慮到動態障礙物，未來將考慮在具有動態障礙物的環境中進行路徑規劃，并能夠根據障礙物的情況自適應的調整拓展步長，進一步提高該算法在路徑規劃方面的適應性。

[1] 劉琦.智能車輛駕駛行為決策與運動規劃控制研究[D].西安:西安理工大學,2019.

[2] 梁中一,程方曉,魏巍.改進RRT～*路徑規劃算法[J].長春工業大學學報,2020,41(06):602-607.

[3] 韓豐鍵,邱書波,馮超,等.基于目標導向的雙向RRT路徑規劃算法[J].齊魯工業大學學報,2021,35(01):35-43.

[4] 施楊洋,楊家富,布升強,等.基于RRT改進的智能車輛路徑規劃算法[J].計算技術與自動化,2019,38(04):81-86.

Vehicle Path Planning Based on Improved RRT* Algorithm

ZHAO Gang

( School of Automobile, Chang'an University, Shaanxi Xi'an 710064 )

RRT* algorithm is a common method for intelligent vehicle path planning, but the traditional RRT* algorithm has many problems in vehicle path planning, such as path turning and curvature fluctuation. To solve these problems, this paper proposes an improved RRT*algorithm, which selects secondary nodes through the distance formula and adds the angle constraint of path turning point, so as to limit the minimum length of path segment and reduce the change of path curvature. The improved RRT* algorithm was simulated and verified by MATLAB simulation platform. Experimental results show that the algorithm can improve the smoothness of the generated trajectory, and the planned path is more consistent with the trajectory characteristics of the vehicle in the process of driving.

Path planning; RRT*algorithm; Angle constraint

A

1671-7988(2021)22-41-03

U495

A

1671-7988(2021)22-41-03

CLC NO.: U495

趙港，碩士研究生，就讀于長安大學汽車學院車輛工程專業。

10.16638/j.cnki.1671-7988.2021.022.010