趙港

改進(jìn)RRT*算法的智能車輛路徑規(guī)劃

趙港

（長安大學(xué) 汽車學(xué)院，陜西 西安 710064）

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

路徑規(guī)劃；RRT*算法；角度約束

前言

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

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

1 RRT*算法

RRT*算法基于隨即采樣的方式來搜索空間，通過連接每個(gè)相鄰的采樣點(diǎn)構(gòu)成隨機(jī)生成樹。根據(jù)地圖信息得到障礙物的位置和狀態(tài)信息，將全局配置信息記為，包含障礙物的空間記為obstacle，無障礙空間記為free，對象出發(fā)點(diǎn)記為init，目標(biāo)點(diǎn)記為goal，RRT*算法原理如圖1所示。

圖1 RRT*算法原理圖

（1）初始化隨機(jī)樹T，此時(shí)T中只有一個(gè)起始出發(fā)點(diǎn)init。

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

（3）在new的規(guī)定鄰域內(nèi)搜索滿足要求的所有節(jié)點(diǎn)集合，圖1中已用圓圈注明。此時(shí)將圓圈內(nèi)其他點(diǎn)與near之間的代價(jià)函數(shù)作比較，若near到new的代價(jià)函數(shù)比通過1、2再到new的代價(jià)函數(shù)低，則將near與new進(jìn)行連線。同時(shí)，還要對比從near出發(fā)到2的代價(jià)函數(shù)最低路線，若通過new到達(dá)2的代價(jià)函數(shù)最低，則將2的父節(jié)點(diǎn)更新為new，并更新代價(jià)函數(shù)。

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

2 改進(jìn)RRT*算法

2.1 最近次級節(jié)點(diǎn)的選擇

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

表1 改進(jìn)RRT*搜索方法的算法框架

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

2.2 路徑轉(zhuǎn)折點(diǎn)角度約束

RRT*算法生成節(jié)點(diǎn)具有隨機(jī)性，導(dǎo)致生成的路徑不平滑。在進(jìn)行智能車輛路徑規(guī)劃時(shí)，沒有考慮到汽車的動力學(xué)約束、最小轉(zhuǎn)彎半徑等因素，因此要盡可能控制路徑的曲率范圍。

圖2為生成路徑轉(zhuǎn)折角度示意圖。pre為當(dāng)前節(jié)點(diǎn)，2為pre的父節(jié)點(diǎn)，rand為本次隨機(jī)生成的節(jié)點(diǎn)。點(diǎn)pre與其父節(jié)點(diǎn)2連接構(gòu)成的線段為，點(diǎn)pre與rand連接構(gòu)成的線段為，兩條線段構(gòu)成的夾角為beta。當(dāng)角beta越大時(shí)，生成的曲線越平滑，曲率波動越小。

圖2 路徑轉(zhuǎn)折點(diǎn)角度示意圖

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

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

3 仿真驗(yàn)證

為驗(yàn)證改進(jìn)RRT*算法的性能，在Matlab仿真平臺上進(jìn)行試驗(yàn)。為了使改進(jìn)后的算法可控，加入循環(huán)次數(shù)的限制。如果在規(guī)定循環(huán)次數(shù)內(nèi)隨機(jī)生成樹T無法搜索到目標(biāo)點(diǎn)goal，則顯示算法返回失敗，重新設(shè)置參數(shù)再次進(jìn)行仿真驗(yàn)證。

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

圖3 仿真結(jié)果圖

智能汽車行駛環(huán)境如圖3(a)和圖3(b)所示，黑色區(qū)域?yàn)槠囆旭傔^程中的障礙物以及道路約束，空白區(qū)域?yàn)闊o障礙空間free。為驗(yàn)證改進(jìn)RRT*算法的有效性，設(shè)置完全相同的仿真環(huán)境，經(jīng)過隨機(jī)生成樹的擴(kuò)展后，規(guī)劃的路徑為圖3(a)和圖3(b)中的曲線。

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

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

4 結(jié)束語

本文在現(xiàn)有RRT*算法的基礎(chǔ)上提出了一種改進(jìn)算法，通過引入歐氏距離公式對次級節(jié)點(diǎn)的選擇進(jìn)行限制，優(yōu)化部分路徑段的長度。同時(shí)，考慮到汽車行駛運(yùn)動學(xué)的問題，通過設(shè)置轉(zhuǎn)折點(diǎn)角度約束，提高路徑平滑度?；贛atlab仿真平臺對改進(jìn)前后的RRT*算法進(jìn)行仿真驗(yàn)證，結(jié)果表明，改進(jìn)后的RRT*算法能夠有效地降低路徑規(guī)劃的曲率波動，相比于比原算法，可以生成更為合理的路徑。本文的仿真驗(yàn)證環(huán)節(jié)只在靜態(tài)環(huán)境下進(jìn)行，并沒有考慮到動態(tài)障礙物，未來將考慮在具有動態(tài)障礙物的環(huán)境中進(jìn)行路徑規(guī)劃，并能夠根據(jù)障礙物的情況自適應(yīng)的調(diào)整拓展步長，進(jìn)一步提高該算法在路徑規(guī)劃方面的適應(yīng)性。

[1] 劉琦.智能車輛駕駛行為決策與運(yùn)動規(guī)劃控制研究[D].西安:西安理工大學(xué),2019.

[2] 梁中一,程方曉,魏巍.改進(jìn)RRT～*路徑規(guī)劃算法[J].長春工業(yè)大學(xué)學(xué)報(bào),2020,41(06):602-607.

[3] 韓豐鍵,邱書波,馮超,等.基于目標(biāo)導(dǎo)向的雙向RRT路徑規(guī)劃算法[J].齊魯工業(yè)大學(xué)學(xué)報(bào),2021,35(01):35-43.

[4] 施楊洋,楊家富,布升強(qiáng),等.基于RRT改進(jìn)的智能車輛路徑規(guī)劃算法[J].計(jì)算技術(shù)與自動化,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

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

10.16638/j.cnki.1671-7988.2021.022.010