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

姚相瀅,許倫輝,林世城

(華南理工大學(xué)土木與交通學(xué)院,廣東 廣州 510641)

1 引言

近年來,人工智能以及車聯(lián)網(wǎng)的發(fā)展允許車輛自主駕駛,并減輕駕駛員的負(fù)擔(dān),提高行駛的安全性[1]。路徑規(guī)劃作為自動駕駛控制系統(tǒng)最上游模塊,完成類似于人類駕駛員在駕駛過程中對路徑規(guī)劃的工作,是自動駕駛汽車核心的任務(wù)之一[2]。路徑規(guī)劃模塊需要收集來自定位、感知、數(shù)據(jù)庫等一系列基礎(chǔ)模塊的數(shù)據(jù),并對這些數(shù)據(jù)進(jìn)行綜合評估,給出在限定條件下的最優(yōu)路徑規(guī)劃[3]。路徑規(guī)劃是汽車完成駕駛決策及進(jìn)一步運動的基礎(chǔ),其在整個自動駕駛系統(tǒng)的框架中是必不可少且至關(guān)重要的部分?？偟膩碚f,路徑規(guī)劃的最終目的是實現(xiàn)智能車輛在有障礙物的環(huán)境中快速、準(zhǔn)確地找到一條無碰撞路徑,最終達(dá)到目標(biāo)點。目前,智能車輛在求解全局路徑規(guī)劃問題上已經(jīng)有許多成熟的算法,常見的算法包括Dijkstra、A*等基于搜索的算法和PRM、RRT等基于采樣的算法[4]。1998年,La Valle[5]第一次提出了快速擴展隨機樹(Rapidly Exploring Random Tree,RRT)算法,它是一種具有采樣概率完備性的路徑規(guī)劃算法。近些年人們對于RRT算法的研究很多。2002年Jim Bruce[6]提出了ERRT (Extend RRT)算法,2006年Dave Ferguson[7]提出DRRT (Dynamic RRT)算法,2012 年Islam[8]等提出快速收斂的RRT*-smart算法,但是采樣點較少,使得路徑棱角較大,不利于實際運用。2012年李威洲[9]提出目標(biāo)偏向RRT算法,雖然加快了收斂速度,但是易陷入局部最小解,對于復(fù)雜狹小路徑,更容易陷入死區(qū)。2016年王道威等[10]提出動態(tài)步長的RRT算法,但是只考慮簡單障礙物環(huán)境。2021年劉建宇[11]等在改進(jìn)的RRT*-connect算法中增加了區(qū)域約束,但是對空間利用率不高。

本文基于上述研究成果,針對RRT算法的采樣盲目性,在目標(biāo)節(jié)點附近遲遲搜索不出路徑,以及產(chǎn)生路徑較粗糙的問題,采用漸近區(qū)域采樣并融合目標(biāo)偏向采樣的方法,保證每次采樣趨近目標(biāo)點,減少不必要區(qū)域的搜索,使采樣更加高效,并且能加快對目標(biāo)點的搜索;考慮車輛幾何參數(shù),采用動態(tài)圓規(guī)則化處理車輛;對生成的粗糙路徑采用基于三角不等式的方法對路徑冗余點進(jìn)行修剪,在此基礎(chǔ)上采用三次B樣條線平滑路徑以達(dá)到曲率連續(xù),最終得到一條滿足車輛行駛軌跡的運動路徑。

2 問題描述與相關(guān)基本算法

2.1 路徑規(guī)劃問題描述

設(shè)X?Rn為路徑規(guī)劃問題的有界工作空間,其中n表示工作空間的維度,n?N且n≥2。Xobs?X為空間中的障礙區(qū)域,空閑區(qū)域為Xfree,滿足Xfree=XXobs,Xfree包含工作空間的所有狀態(tài)點集合。給定路徑規(guī)劃的起點xinit∈Xfree,目標(biāo)點xgoal∈Xfree,移動機器人路徑的連續(xù)性由滿足自身約束的函數(shù)s:[0,1]→Rn表示。如果對所有的τ∈[0,1],有s(0)=xinit,s(1)=xgoal,則路徑規(guī)劃問題可由(Xfree,xinit,xgoal)定義,表示為在Xfree中,通過路徑規(guī)劃算法計算出一條從起點xinit到目標(biāo)點xgoal滿足移動機器人自身約束條件的連續(xù)無碰撞路徑作為問題的解。

定義∑ 是在環(huán)境X中所有可形成路徑s解的集合,最優(yōu)路徑規(guī)劃問題通常定義為搜索∑ 中最短路徑s*的問題,定義通過Xfree連接xinit與xgoal的成本函數(shù)c:∑→R≥0,則最優(yōu)路徑規(guī)劃問題表示為搜索∑ 中最短的路徑解s*,如式(1)所示

=xgoal,?τ∈(0,1),s(τ)∈xfree}

(1)

2.2 RRT算法

RRT算法既是一種算法,同時也是一種數(shù)據(jù)結(jié)構(gòu),被設(shè)計用來高效地在高維空間中進(jìn)行搜索,它特別適用于在涉及非完整約束場合下的路徑規(guī)劃問題。

RRT原理如圖1所示。在工作環(huán)境中定義路徑規(guī)劃的起點qstart與終點qgoal,算法初始化根結(jié)點,以根結(jié)點開始生長,向四周進(jìn)行探索,在地圖中隨機采樣,采樣點為xrand,生成公式如式(2)所示,其中X,Y為地圖的邊界尺寸。然后找到樹中離xrand最近的葉子結(jié)點xnearest,接下來以theta為步長,往xrand方向進(jìn)行擴展,擴展新的葉子結(jié)點為xnew,公式如式(3)所示。對xnearest與xnew的連線進(jìn)行碰撞檢測,若連線不經(jīng)過障礙物,則將xnew加入樹中,進(jìn)行下一輪的迭代,如圖1(a)所示;若連線經(jīng)過障礙物,則放棄本次迭代,重新選取隨機點,如圖1(b)所示。

圖1 RRT擴展過程

xrand=(rand(0,X),rand(0,Y))

(2)

(3)

當(dāng)樹中的任意葉子結(jié)點與qgoal重合或xnearest與xnew的連線經(jīng)過qgoal時完成搜索,隨后算法在搜索樹中尋找一條連接起點和終點的最短路徑;若達(dá)到最大迭代次數(shù)也沒有找到目標(biāo)點,則搜索失敗。

2.3 偏向RRT算法

偏向RRT算法在RRT算法基礎(chǔ)上加入了目標(biāo)偏向采樣機制,使得隨機樹偏向目標(biāo)搜索,在簡單環(huán)境下具有較高的效率。目標(biāo)偏向采樣是指在采樣過程中先設(shè)定一個基準(zhǔn)值pstandard,然后隨機樹在每輪生成隨機點之前會生成一個隨機概率p,如果p>pstandard,則在空間內(nèi)隨機采樣,否則選取目標(biāo)點作為隨機點,如式(4)所示。

p←rand(0,1);

ifp

(4)

3 改進(jìn)RRT算法

3.1 采樣策略

由于RRT算法采樣具有隨機性,會導(dǎo)致路徑空間搜索范圍擴大;并且偏向型RRT在復(fù)雜環(huán)境中易陷入死區(qū)。針對上述問題,本文首先提出了一種漸近區(qū)域采樣的方法,能減少算法在不必要空間的搜索,加快搜索速度。假設(shè)隨機樹此時擴展到點xnew,隨機樹中橫向空間值最小的點為xmin,最大的點為xmax。

xeval=(xmin+xmax)·k

(5)

xrand=(rand(xeval,X),rand(0,Y))

(6)

其中k∈(0,1),為漸近因子,其值的大小表示了區(qū)域漸近的一個程度,xeval表示采樣區(qū)域的橫向空間起始值。xeval隨著k的增大而增大,經(jīng)多次測試,k值不宜過大,在復(fù)雜環(huán)境下取12為宜。漸近區(qū)域采樣按照式(6)所示生成采樣點。

漸近區(qū)域采樣原理如圖2所示。當(dāng)隨機樹初始化時,RRT算法有一定概率往區(qū)域①進(jìn)行擴展,而改進(jìn)RRT會在區(qū)域①之外進(jìn)行采樣,限制了采樣區(qū)域,當(dāng)擴展到xnew時,根據(jù)樹中結(jié)點的橫向空間最值,計算xeval,根據(jù)xeval采樣區(qū)域漸近到區(qū)域③。下一輪采樣區(qū)域便限制為區(qū)域③。隨著隨機樹的擴展,采樣區(qū)域會向目標(biāo)點逐漸漸近。

圖2 漸近區(qū)域采樣

漸近區(qū)域采樣確保每次采樣都趨于目標(biāo)點,有效防止隨機采樣點的反向搜索,從而保證采樣的方向性,減少資源浪費,提高采樣效率,同時在約束環(huán)境下不限制采樣點在縱向空間的隨機選擇,保留了采樣的隨機性,確保遇到障礙物時,能夠高效搜索出可行路徑。融合目標(biāo)偏向采樣使得隨機樹能在目標(biāo)點附近加快搜索,提高搜索效率。改進(jìn)算法采樣策略如式(7)所示。

p←rand(0,1);

ifp

elsexeval=(xmin+xmax)·k

xrand=(rand(xeval,X),rand(0,Y))

(7)

3.2 規(guī)則化車輛

RRT算法將采樣點視為質(zhì)點,并未考慮機器人本身的幾何大小,但在實際運用中的移動機器人具有幾何形狀,如果不考慮車輛本身的大小,規(guī)劃出來的路徑在實車實驗時可能會發(fā)生碰撞。為了保證所擴展的結(jié)點適用于車輛的行駛,需要對車輛進(jìn)行規(guī)則化處理。

文瓊[12]用包絡(luò)圓的方法對車輛進(jìn)行處理,如圖3(a)所示,這樣雖然保證了安全,但是會把大部分非行駛區(qū)域也包含進(jìn)來,本文采用動態(tài)圓的方法進(jìn)行規(guī)則化處理車輛,如圖3(b)所示,先以車輛寬度畫圓,考慮實際情況和障礙物,適當(dāng)放大動態(tài)圓半徑,根據(jù)車輛的幾何模型,動態(tài)圓心關(guān)系如式(8)所示。

圖3 車輛規(guī)則化處理

(8)

(9)

其中(x0,y0)表示動態(tài)圓的圓心,u∈[0,1],(xf,yf)表示車輛最前端圓的圓心坐標(biāo),(xr,yr)表示車輛最后端圓的圓心坐標(biāo)。式(9)為動態(tài)圓的方程,b為車輛的車身寬度,ε為放大系數(shù),在車身寬度上適當(dāng)放大一個距離,以保證較好的避障效果,保留相對富裕的空間。動態(tài)圓在仿真地圖上的效果如圖4所示。

圖4 規(guī)則化車輛仿真結(jié)果

圖5 路徑剪枝

圖6 改進(jìn)RRT算法流程

圖7 實驗環(huán)境

3.3 路徑優(yōu)化

3.3.1 路徑修剪

在仿真環(huán)境下RRT算法生成的路徑如圖8(a)所示,由于RRT采用的是隨機采樣的方式,生成的路徑較為曲折,冗余點較多,刪除這些冗余點不僅可以大大的縮短路徑的長度,還可以減少路徑轉(zhuǎn)向的次數(shù),轉(zhuǎn)向次數(shù)的減少可以提高車輛行駛的舒適性。修剪過程用于在不與障礙物碰撞的情況下刪除路徑中的冗余點得到一條新的路徑,為此根據(jù)三角不等式的概念對路徑進(jìn)行修剪處理。

圖8 實驗環(huán)境下三種算法仿真結(jié)果

路徑剪枝原理如圖6所示,這個過程從起點qstart開始,然后找到之后的第二個結(jié)點q2,連接qstart與q2,若兩結(jié)點之間沒有發(fā)生碰撞,則刪除q1,繼續(xù)尋找q2之后的下一個連續(xù)點q3,連接該點與qstart發(fā)生碰撞,則退出此輪迭代,下一輪迭代則從q2為起點,一直持續(xù),直至連接到qgoal,此時對路徑進(jìn)行重連。

3.3.2B樣條線平滑處理

路徑經(jīng)過剪枝處理后,路徑曲率不是連續(xù)的,需要對路徑進(jìn)行平滑處理。對于汽車運動規(guī)劃和控制系統(tǒng),需要基于樣條插值使得汽車運動軌跡能經(jīng)過控制點,增加擬合曲線與目標(biāo)軌跡的匹配程度。B樣條曲線具有曲率連續(xù)的優(yōu)點,特別是在相鄰曲線段之間的結(jié)點處曲率也是連續(xù)的,因此B樣條曲線在路徑規(guī)劃中有著較為廣泛的應(yīng)用[13]。軌跡規(guī)劃中常用的是三階B樣條曲線,以滿足汽車運動學(xué)約束。

設(shè)有控制頂點P1,P2,P3,…,Pn,則k階(k-1)次B樣條曲線的數(shù)學(xué)表達(dá)式如式(10)所示

(10)

其中,Ni,k(t)是B樣條基函數(shù),也稱為B樣條的分段混合函數(shù),是一個k階(k-1)次分段多項式,參數(shù)t是一組被稱為結(jié)點矢量的非遞增序列。根據(jù)德布爾-考克斯遞推定義:

(11)

(12)

Ni,k(t)中每一部分被稱為B樣條,依次連接全部k階B樣條曲線段就組成了整條B樣條曲線。

3.4 改進(jìn)算法具體步驟

根據(jù)上述方法進(jìn)行改進(jìn),改進(jìn)RRT算法具體步驟如圖6所示。

4 仿真結(jié)果對比分析

4.1 實驗環(huán)境

運用仿真軟件對本文提出的改進(jìn)RRT算法與RRT算法、偏向RRT算法進(jìn)行仿真,來驗證改進(jìn)算法的有效性和優(yōu)越性。硬件平臺為安裝64位的Win10筆記本(Intel(R) Core(TM) i5-10210U主頻1.60GHz內(nèi)存8GB),軟件平臺為MATLAB2018b編程平臺。實驗環(huán)境為800×800的二維平面,搜索起點坐標(biāo)為(50,50),目標(biāo)點坐標(biāo)為(700,700),黑色區(qū)域為障礙區(qū)域,藍(lán)色部分為搜索生成的隨機樹,紅色為隨機樹規(guī)劃出來的路徑。

設(shè)置實驗參數(shù)如表1所示。

表1 實驗參數(shù)

表2 實驗環(huán)境下30次仿真平均結(jié)果

4.2 實驗結(jié)果與分析

用RRT算法、偏向RRT算法和改進(jìn)RRT算法在圖7所示的實驗環(huán)境下分別進(jìn)行30次仿真,并將三種算法的收斂時間,迭代次數(shù)和成功次數(shù)進(jìn)行對比分析。圖8為三種算法在實驗環(huán)境下的表現(xiàn),圖9為路徑優(yōu)化的仿真結(jié)果。

圖9 實驗環(huán)境下改進(jìn)RRT路徑優(yōu)化結(jié)果

圖10 三種算法迭代次數(shù)對比

圖11 三種算法迭代時間對比

從仿真結(jié)果可以看出,在復(fù)雜環(huán)境下,改進(jìn)RRT算法搜索的表現(xiàn)明顯比RRT算法和偏向RRT算法好,由于RRT算法的隨機性,采樣點會全局生成,導(dǎo)致算法的迭代時間和次數(shù)明顯增多;偏向RRT在復(fù)雜環(huán)境下表現(xiàn)并不明顯,且易陷入死區(qū);而改進(jìn)RRT算法能有效提高空間利用率,使得采樣點趨向于目標(biāo)點區(qū)域。根據(jù)仿真結(jié)果,在30次試驗中,RRT算法平均迭代時間為2.2s,偏向RRT算法平均迭代時間為1.61s,改進(jìn)RRT算法平均迭代時間為0.66s,改進(jìn)RRT算法在RRT算法的平均迭代時間上減少了70%,在偏向RRT算法平均迭代時間上減少了59%;RRT算法平均迭代次數(shù)為1416次,偏向RRT算法平均迭代次數(shù)為1311次,改進(jìn)RRT算法平均迭代次數(shù)為862次,改進(jìn)RRT算法在RRT算法的平均迭代次數(shù)上減少了39.1%,在偏向RRT算法的平均迭代次數(shù)上減少了34.2%;綜合來看,改進(jìn)RRT不管是在時間和迭代次數(shù)上效果提升都較為明顯,且在30次實驗中每次都能成功的找到目標(biāo)點。路徑優(yōu)化仿真結(jié)果表明,經(jīng)過修剪和平滑處理后,路徑長度明顯縮短,路徑更為平滑,符合曲率連續(xù)約束,如圖9所示,規(guī)劃出來的路徑質(zhì)量明顯提高,適用于智能車輛的行駛。

5 結(jié)語

針對RRT算法在路徑規(guī)劃中存在目標(biāo)導(dǎo)向性差、空間利用率低、路徑粗糙的問題,提出了一種漸近區(qū)域采樣融合目標(biāo)偏向采樣的RRT算法。該算法能有效的防止隨機采樣點的反向搜索,從而保證采樣的方向性,減少資源浪費,加快了對目標(biāo)點的搜索,提高了路徑搜索效率;考慮車輛的幾何約束,用動態(tài)圓規(guī)則化處理車輛有效避免碰撞和通過狹窄區(qū)域;采用基于三角不等式的方法對路徑冗余點進(jìn)行修剪,在此基礎(chǔ)上采用三次B樣條線平滑路徑以達(dá)到曲率連續(xù),最終得到一條滿足車輛行駛軌跡的路徑。最后通過仿真對比分析三種算法的迭代時間和次數(shù),證明了改進(jìn)RRT算法在復(fù)雜環(huán)境下搜索的優(yōu)越性,并且搜索路徑更短,用時更少。但是該算法也存在一些問題,漸近區(qū)域的程度是根據(jù)漸近因子k值的大小決定,當(dāng)k值趨近于0時,改進(jìn)RRT算法蛻變?yōu)槠騌RT算法,當(dāng)k值趨近于1時,算法搜索會極大程度陷入死區(qū)導(dǎo)致隨機樹搜索失敗。接下來將會研究k值根據(jù)環(huán)境的復(fù)雜程度進(jìn)行動態(tài)變化,使得區(qū)域動態(tài)漸近。