基于改進快速探索隨機樹的復(fù)雜環(huán)境垂直泊車路徑規(guī)劃

徐遠征 吳長水

（上海工程技術(shù)大學(xué)，上海 201620）

主題詞：垂直泊車 路徑規(guī)劃 改進RRT算法 高斯分布采樣 Reeds-Shepp曲線

1 前言

隨著汽車保有量迅速增加，城市中車位不足、車位狹窄等問題日益突出。此外，常見的不規(guī)范停車行為使原來的單輛車位空間被兩側(cè)車輛占據(jù)，導(dǎo)致泊車場景更加復(fù)雜。開發(fā)適用于復(fù)雜環(huán)境的自動泊車系統(tǒng)是解決此類問題的關(guān)鍵。完整的自動泊車過程可分為感知、路徑規(guī)劃和控制執(zhí)行3個部分，因此研究適用于復(fù)雜場景的泊車路徑規(guī)劃算法具有重要意義。

常見的泊車路徑規(guī)劃算法主要包括幾何法與曲線擬合法。Plamen 等針對垂直泊車問題，結(jié)合杜賓斯（Dubins）曲線提出了基于最小轉(zhuǎn)彎半徑的無碰撞幾何路徑規(guī)劃算法。李韜根據(jù)幾何關(guān)系確定最短泊車路徑與泊車起始區(qū)域，然后采用五次多項式進行了路徑擬合，該路徑滿足車輛運動學(xué)約束。上述方法需對泊車區(qū)域進行詳細的幾何劃分，且泊車必須從特定的點或區(qū)域進行，算法適用性及可拓展性較差。針對這一問題，部分學(xué)者利用快速探索隨機樹（Rapidly-exploring Randomized Tree，RRT）算法無需對泊車場景進行建模、適用性強的特點，提出了基于RRT 算法的解決方案。Lone等使用基本RRT算法進行垂直、水平及傾斜泊車的路徑規(guī)劃，然而其存在規(guī)劃過程緩慢、路徑不平滑等缺點。Seho 等通過指定RRT 種子取向加快規(guī)劃速度，然而種子取向受泊車環(huán)境影響較大。

綜上，為確保算法具有良好的適用性及可拓展性，本文充分利用RRT 算法的優(yōu)勢并對其進行改進，提出一種基于改進RRT 的垂直泊車路徑規(guī)劃算法，以滿足任意初始位姿的路徑規(guī)劃需求。

2 車輛模型及泊車場景建模

2.1 車輛運動學(xué)模型

泊車是低速的運動過程（低于5 km/h），此時車輛可視為剛體，且車輪所受的側(cè)向力可忽略，因此不會發(fā)生滑動，在實際研究過程中通常會對車輛運動學(xué)模型進行簡化。基于此，本文以目標車位后端中點為世界坐標系原點，采用如圖1所示的二自由度兩輪車模型進行建模。

圖1 車輛運動學(xué)模型

本文建立的車輛運動學(xué)方程為：

式中，()為車輛后軸中心點坐標；為后軸車速（向前為正）；為軸距；為車輛航向角（逆時針方向為正）；為等效前輪轉(zhuǎn)向角（逆時針方向為正），其受最大等效前輪轉(zhuǎn)角約束，滿足||≤。

2.2 泊車場景設(shè)定

本文以城市普通垂直停車位為研究場景，綜合考慮常見不規(guī)范泊車現(xiàn)象對泊車過程的影響，將泊車場景劃分為有車位線場景及無車位線場景兩類，如圖2所示。

圖2 泊車場景

對于有車位線場景，車位兩旁的車輛對泊車過程沒有影響，車輛可以車位線為參考進行標準泊車，目標泊車位置即為車位的中心。對于無車位線場景，車位兩旁車輛占據(jù)了車位的部分泊車空間，此時車輛無法以車位線作為泊車參考依據(jù)，只能根據(jù)車位兩旁車輛的停靠情況進行泊車。考慮到狹小空間內(nèi)車輛泊入后駕駛員側(cè)車門開啟難的問題，本文規(guī)定當(dāng)右側(cè)車輛向右傾斜時，車輛泊入后應(yīng)與右側(cè)車輛保持平行。

本文制定的具體場景參數(shù)如表1 所示。根據(jù)國家標準車位尺寸以及實際泊車需求對車位基本尺寸進行了定義，考慮到車位兩旁車輛對目標泊車位姿()的影響，本文對兩旁車輛的傾斜角度進行了限定。若泊車場景參數(shù)滿足上述設(shè)定的取值范圍，則視為可泊車場景，否則視為不可泊車場景。最后，為使試驗過程更貼合實際應(yīng)用，本文對車身到車位前端的垂直距離、車輛初始航向角的取值范圍進行了限定。

表1 場景參數(shù)

3 垂直泊車路徑規(guī)劃算法

3.1 基本RRT算法

RRT算法是一種使用增長樹的搜索算法，其優(yōu)勢在于無需對泊車空間進行幾何劃分，若路徑存在則可以滿足任何初始位姿要求，且其可以滿足車輛約束?；綬RT算法偽代碼如下：

其中，為泊車起始點；為泊車目標點；為采樣點；為與采樣點最近的RRT 節(jié)點；為新的RRT節(jié)點；為采樣次數(shù)；Δ為拓展步長；為到的最短距離；為RRT路徑。

基本RRT 算法在應(yīng)用于泊車路徑規(guī)劃時，在采樣階段存在采樣點隨機性大的問題，在拓展階段存在不易通過狹窄通道、計算代價較大的問題，在路徑生成階段存在路徑不平滑的問題。因此有必要對其進行改進。

3.2 改進RRT算法

在采樣階段，基于高斯分布及目標偏置采樣，本文提出了一種融合采樣方法加快采樣速度。為解決拓展階段的通道狹窄問題，本文使用逆向樹生成新的RRT目標點，拓展過程中不斷嘗試用Reeds-Shepp（RS）曲線無碰撞連接該目標點。在路徑生成階段基于RS曲線對RRT 路徑進行平滑優(yōu)化。改進后的算法流程如圖3 所示。

圖3 改進RRT算法流程

3.2.1 融合采樣方法

傳統(tǒng)RRT 算法的采樣點均勻分布在搜索空間中，存在過多的冗余搜索，導(dǎo)致算法效率較低。因此，本文融合高斯分布采樣和目標偏置采樣對其進行改進，利用高斯分布采樣減少采樣的隨機性與盲目性，同時限制采樣的方向及范圍，使之更加符合車輛約束，采用目標偏置采樣解決高斯分布采樣存在的采樣死區(qū)問題。采用的高斯分布采樣表達式為：

式中，x、y為當(dāng)前第個采樣點的橫、縱坐標；、為當(dāng)前采樣點的父節(jié)點橫、縱坐標；μ為當(dāng)前采樣點與父節(jié)點的相對距離；σ為采樣區(qū)徑向范圍；μ為當(dāng)前采樣區(qū)中心線與軸的夾角；σ為采樣區(qū)角度范圍；(μ,σ)、(μ,σ)為隨機數(shù)。

融合采樣流程如圖4所示。

圖4 融合采樣流程

初始階段，隨機樹頂點只有，采樣區(qū)域中心線方向為當(dāng)前車輛行駛方向；采樣階段隨機樹頂點數(shù)大于1個，此時采樣區(qū)中心線方向為該節(jié)點與其父節(jié)點連線的方向。采樣區(qū)中心線與軸的夾角μ為：

由于高斯分布采樣具有一定的偏向性，其在采樣過程中存在采樣點落入死區(qū)的可能性，如圖5所示。為確保采樣過程的穩(wěn)定性，當(dāng)采樣點落入死區(qū)時，采樣算法將切換為目標點偏置，本文設(shè)置目標點偏置概率為0.1。

圖5 采樣區(qū)及采樣死區(qū)

3.2.2 狹窄通道問題優(yōu)化

由于車位較為狹窄，RRT拓展過程中存在狹窄通道難以進入的問題。此外，在復(fù)雜泊車環(huán)境下，由于RRT拓展的隨機性，其規(guī)劃的路徑在車位內(nèi)通常需要較多的位姿調(diào)整，容易造成車輛剮蹭等危險情況的發(fā)生?？紤]到泊車過程中的安全性，應(yīng)盡可能減少車輛在車位內(nèi)的位姿調(diào)整。基于此，本文引入逆向拓展樹及RS 曲線對RRT算法進行改進。

首先采用以固定步長沿直線生長的逆向拓展樹調(diào)整RRT目標點，基本流程如圖6所示。逆向拓展樹的起始點為車輛最終的泊車位姿，終止點為車輛能夠以最小轉(zhuǎn)彎半徑泊出且無碰撞的點，該點即為調(diào)整后的RRT目標點。在此基礎(chǔ)上，在RRT樹拓展的過程中，每當(dāng)生成新節(jié)點時，直接將其用RS曲線連接到目標點，并對生成的RS 路徑進行碰撞檢測。若沒有發(fā)生碰撞則RRT樹拓展結(jié)束，將RS路徑、RRT路徑及逆向樹路徑合并為改進RRT 算法的初始規(guī)劃路徑；若發(fā)生碰撞，則RRT 樹進行新一輪的拓展，直至能通過RS 曲線無碰撞地連接新節(jié)點與目標點。

圖6 RRT目標點遷移

RS 曲線以Dubins 曲線為基礎(chǔ)，其假定車輛始終按照最小轉(zhuǎn)彎半徑進行轉(zhuǎn)向且盡可能使用直線，起點到終點間的路徑只由圓弧及直線構(gòu)成。由于本文在規(guī)劃過程中嘗試采用RS 曲線對目標點進行直接連接，故最終在車位空間內(nèi)的泊車路徑將只由圓弧和直線組成，車輛在車位內(nèi)沒有多余的位姿調(diào)整，提高了泊車過程的安全性。

3.2.3 碰撞檢測

為確保所規(guī)劃路徑的安全性，有必要對其進行碰撞檢測?？紤]到車輛和障礙物的幾何形狀均可抽象為矩形，本文采用方向包圍盒（Oriented Bounding Box，OBB）檢測方法，將碰撞檢測轉(zhuǎn)化為OBB 相交檢測問題。其檢測原理為：對于任意的2 個凸多邊形物體，若能找到任意一個分離軸使這2 個物體在該軸上的投影不發(fā)生重疊，則判定2個物體之間不發(fā)生碰撞。

OBB檢測原理如圖7所示，4條中心線代表分離軸，矩形、分別代表車輛和障礙物。A、A為以A為坐標原點的局部坐標系下的單位向量，B、B為以B為坐標原點的局部坐標系下的單位向量。為矩形、中心點間的距離；為全局坐標系下的車輛航向角；為兩矩形中心連線與全局坐標系軸的夾角。根據(jù)OBB 碰撞檢測原理，只需判斷2 個矩形在上述4 個分離軸A、A、B、B上的投影是否滿足如下條件：

圖7 OBB檢測原理

式中，L、L分別為矩形、的長度；W、W分別為矩形、的寬度。

如果同時滿足式（4）中的4 個檢測條件，即可判定車輛和障礙物發(fā)生碰撞。

3.2.4 路徑優(yōu)化

針對RRT 路徑存在的曲率不連續(xù)、路徑代價較大等問題，本文采用RS 曲線替換原有的RRT 路徑，使優(yōu)化后的路徑能夠滿足車輛運動約束。當(dāng)兩點間不存在障礙物時，RS 路徑即為最短路徑，根據(jù)這一特性，本文基于貪婪思想，將每一段的局部最優(yōu)路徑進行合并，以此獲得相對較小的路徑代價。

路徑優(yōu)化過程如圖8所示，本文以RRT路徑索引的終點為優(yōu)化起始點，根據(jù)索引編號由小到大依次通過RS 曲線連接優(yōu)化起點與索引點，若在兩點間能找到無碰撞的RS路徑，則剔除兩點間的其他索引點，并用新的路徑替代原有的路徑。然后以當(dāng)前索引點為新的起始點進行下一輪優(yōu)化，直至將原有的RRT 路徑均轉(zhuǎn)化為RS 路徑。最后將優(yōu)化后的RRT 路徑進行合并，生成最終的垂直泊車規(guī)劃路徑。

圖8 路徑優(yōu)化

4 仿真分析

4.1 仿真參數(shù)設(shè)置

本文使用MATLAB 2020a 軟件編寫相應(yīng)算法并進行仿真，并在相同的仿真條件下與基本RRT 及基于目標偏好的RRT（Goal-biasing RRT）算法進行比較。為驗證算法的有效性及可拓展性，需測試車輛在不同初始位姿下的路徑規(guī)劃情況。本文采用批處理的方式，根據(jù)表1的場景參數(shù)所設(shè)計的測試用例如圖9所示。其中車輛初始坐標范圍為-5.2～5.2 m，步進0.4 m；車輛初始坐標范圍為6.3～7.9 m，步進0.4 m；車輛初始角度范圍為-10°～10°，步進10°。表2 所示為本文采用的車輛及車位參數(shù)。

圖9 測試用例

表2 車輛及車位參數(shù)

4.2 不同泊車場景下的路徑規(guī)劃結(jié)果

不同場景下的部分泊車路徑規(guī)劃結(jié)果如圖10 所示。由圖10 可知，不同泊車場景下所規(guī)劃的路徑均較為平滑，且車輛在泊車過程中的位姿調(diào)整較少。在泊車過程中車輛均能沿規(guī)劃的路徑泊入車位且與周圍障礙物無碰撞，表明本文所設(shè)定的垂直泊車路徑規(guī)劃方法能夠滿足車輛及場景約束。此外，對于圖10b所示的無車位線場景，車輛的最終泊車位姿在左、右兩車的中間且車身與右側(cè)車輛平行，確保了更大的車門開啟空間，達到了預(yù)定的設(shè)計目標。

圖10 不同泊車場景下的泊車路徑

4.3 與RRT及其常見變體的比較

圖11 顯示了相同條件下基本RRT、Goal-biasing RRT與本文提出的改進RRT的路徑規(guī)劃情況。在采樣次數(shù)方面，基本RRT和Goal-biasing RRT 采樣次數(shù)多且有很多無效采樣，而本文提出的改進RRT 算法采樣次數(shù)改善較為明顯，證明了本文提出的聯(lián)合采樣方法的有效性。在路徑方面：基本RRT和Goal-biasing RRT 規(guī)劃的路徑不夠平滑，且在車位內(nèi)有較多的位姿調(diào)整，很難滿足車輛運動及泊車安全的要求；改進RRT 規(guī)劃的路徑相對平滑，在本文提出的逆向生長樹的作用下，車位內(nèi)車輛無多余的位姿調(diào)整，有效提高了泊車過程的安全性。

圖11 不同方法的路徑規(guī)劃情況

批處理的平均時間、平均采樣次數(shù)及路徑代價如表3所示。由表3可知，在相同仿真條件下，基本RRT算法的消耗時間最長且路徑代價最大，Goal-biasing RRT 相較于傳統(tǒng)RRT 算法改進效果并不明顯。相較于基本RRT 及Goal-biasing RRT，本文提出的路徑規(guī)劃方法平均規(guī)劃時間分別縮短52.3%和41.7%，路徑代價分別減少17.7%和13.9%，性能提升較為顯著，證明了本文算法具有較好的適用性及可拓展性。

表3 批處理仿真結(jié)果

5 結(jié)束語

本文對RRT 算法進行了針對性改進，使之更適用于復(fù)雜環(huán)境下的垂直泊車路徑規(guī)劃。通過融合采樣方法減少了采樣的隨機性及盲目性；通過引入逆向樹及RS曲線克服了狹窄通道難以進入的問題；通過RS曲線進行路徑優(yōu)化，平滑了路徑并減小了路徑代價。

仿真結(jié)果表明，本文提出的改進RRT 算法適用于復(fù)雜環(huán)境下的垂直泊車路徑規(guī)劃，確保了較短的規(guī)劃時間、較少的采樣次數(shù)以及相對較短的路徑代價，所規(guī)劃的路徑較為平滑，滿足車輛約束要求，也盡可能避免了車輛在車位內(nèi)多余的位姿調(diào)整，確保了車輛在復(fù)雜環(huán)境下泊車過程的安全。