姚相瀅,許倫輝,林世城

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

1 引言

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

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

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

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

設(shè)X?Rn為路徑規(guī)劃問題的有界工作空間,其中n表示工作空間的維度,n?N且n≥2。Xobs?X為空間中的障礙區(qū)域,空閑區(qū)域?yàn)閄free,滿足Xfree=XXobs,Xfree包含工作空間的所有狀態(tài)點(diǎn)集合。給定路徑規(guī)劃的起點(diǎn)xinit∈Xfree,目標(biāo)點(diǎn)xgoal∈Xfree,移動(dòng)機(jī)器人路徑的連續(xù)性由滿足自身約束的函數(shù)s:[0,1]→Rn表示。如果對(duì)所有的τ∈[0,1],有s(0)=xinit,s(1)=xgoal,則路徑規(guī)劃問題可由(Xfree,xinit,xgoal)定義,表示為在Xfree中,通過路徑規(guī)劃算法計(jì)算出一條從起點(diǎn)xinit到目標(biāo)點(diǎn)xgoal滿足移動(dòng)機(jī)器人自身約束條件的連續(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í)也是一種數(shù)據(jù)結(jié)構(gòu),被設(shè)計(jì)用來高效地在高維空間中進(jìn)行搜索,它特別適用于在涉及非完整約束場(chǎng)合下的路徑規(guī)劃問題。

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

圖1 RRT擴(kuò)展過程

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

(2)

(3)

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

2.3 偏向RRT算法

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

p←rand(0,1);

ifp

(4)

3 改進(jìn)RRT算法

3.1 采樣策略

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

xeval=(xmin+xmax)·k

(5)

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

(6)

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

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

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

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

p←rand(0,1);

ifp

elsexeval=(xmin+xmax)·k

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

(7)

3.2 規(guī)則化車輛

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

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

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

(8)

(9)

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

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

圖5 路徑剪枝

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

圖7 實(shí)驗(yàn)環(huán)境

3.3 路徑優(yōu)化

3.3.1 路徑修剪

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

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

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

3.3.2B樣條線平滑處理

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

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

(10)

其中,Ni,k(t)是B樣條基函數(shù),也稱為B樣條的分段混合函數(shù),是一個(gè)k階(k-1)次分段多項(xiàng)式,參數(shù)t是一組被稱為結(jié)點(diǎn)矢量的非遞增序列。根據(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é)果對(duì)比分析

4.1 實(shí)驗(yàn)環(huán)境

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

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

表1 實(shí)驗(yàn)參數(shù)

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

4.2 實(shí)驗(yàn)結(jié)果與分析

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

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

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

圖11 三種算法迭代時(shí)間對(duì)比

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

5 結(jié)語

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