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

趙港

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

趙港

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

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

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

前言

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

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

1 RRT*算法

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

圖1 RRT*算法原理圖

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

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

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

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

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

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

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

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

次級(jí)結(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í)，沒(méi)有考慮到汽車的動(dòng)力學(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í)，生成的曲線越平滑，曲率波動(dòng)越小。

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

因此在RRT*算法中加入角度判別函數(shù)，只有當(dāng)角beta大于90°時(shí)，才能將rand加入隨機(jī)樹(shù)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仿真平臺(tái)上進(jìn)行試驗(yàn)。為了使改進(jìn)后的算法可控，加入循環(huán)次數(shù)的限制。如果在規(guī)定循環(huán)次數(shù)內(nèi)隨機(jī)生成樹(shù)T無(wú)法搜索到目標(biāo)點(diǎn)goal，則顯示算法返回失敗，重新設(shè)置參數(shù)再次進(jìn)行仿真驗(yàn)證。

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

圖3 仿真結(jié)果圖

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

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

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

4 結(jié)束語(yǔ)

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

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

[2] 梁中一,程方曉,魏巍.改進(jìn)RRT～*路徑規(guī)劃算法[J].長(zhǎng)春工業(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ù)與自動(dòng)化,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

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

10.16638/j.cnki.1671-7988.2021.022.010