基于改進RRT算法的智能車輛路徑規劃研究

姚相瀅,許倫輝,林世城

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

1 引言

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

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

2 問題描述與相關基本算法

2.1 路徑規劃問題描述

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

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

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

(1)

2.2 RRT算法

RRT算法既是一種算法,同時也是一種數據結構,被設計用來高效地在高維空間中進行搜索,它特別適用于在涉及非完整約束場合下的路徑規劃問題。

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

圖1 RRT擴展過程

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

(2)

(3)

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

2.3 偏向RRT算法

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

p←rand(0,1);

ifp

(4)

3 改進RRT算法

3.1 采樣策略

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

xeval=(xmin+xmax)·k

(5)

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

(6)

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

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

圖2 漸近區域采樣

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

p←rand(0,1);

ifp

elsexeval=(xmin+xmax)·k

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

(7)

3.2 規則化車輛

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

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

圖3 車輛規則化處理

(8)

(9)

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

圖4 規則化車輛仿真結果

圖5 路徑剪枝

圖6 改進RRT算法流程

圖7 實驗環境

3.3 路徑優化

3.3.1 路徑修剪

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

圖8 實驗環境下三種算法仿真結果

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

3.3.2B樣條線平滑處理

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

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

(10)

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

(11)

(12)

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

3.4 改進算法具體步驟

根據上述方法進行改進,改進RRT算法具體步驟如圖6所示。

4 仿真結果對比分析

4.1 實驗環境

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

設置實驗參數如表1所示。

表1 實驗參數

表2 實驗環境下30次仿真平均結果

4.2 實驗結果與分析

用RRT算法、偏向RRT算法和改進RRT算法在圖7所示的實驗環境下分別進行30次仿真,并將三種算法的收斂時間,迭代次數和成功次數進行對比分析。圖8為三種算法在實驗環境下的表現,圖9為路徑優化的仿真結果。

圖9 實驗環境下改進RRT路徑優化結果

圖10 三種算法迭代次數對比

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

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

5 結語

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