基于改進快速探索隨機樹的復雜環境垂直泊車路徑規劃

徐遠征 吳長水

（上海工程技術大學，上海 201620）

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

1 前言

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

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

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

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

2.1 車輛運動學模型

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

圖1 車輛運動學模型

本文建立的車輛運動學方程為：

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

2.2 泊車場景設定

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

圖2 泊車場景

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

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

表1 場景參數

3 垂直泊車路徑規劃算法

3.1 基本RRT算法

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

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

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

3.2 改進RRT算法

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

圖3 改進RRT算法流程

3.2.1 融合采樣方法

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

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

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

圖4 融合采樣流程

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

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

圖5 采樣區及采樣死區

3.2.2 狹窄通道問題優化

由于車位較為狹窄，RRT拓展過程中存在狹窄通道難以進入的問題。此外，在復雜泊車環境下，由于RRT拓展的隨機性，其規劃的路徑在車位內通常需要較多的位姿調整，容易造成車輛剮蹭等危險情況的發生。考慮到泊車過程中的安全性，應盡可能減少車輛在車位內的位姿調整。基于此，本文引入逆向拓展樹及RS 曲線對RRT算法進行改進。

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

圖6 RRT目標點遷移

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

3.2.3 碰撞檢測

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

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

圖7 OBB檢測原理

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

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

3.2.4 路徑優化

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

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

圖8 路徑優化

4 仿真分析

4.1 仿真參數設置

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

圖9 測試用例

表2 車輛及車位參數

4.2 不同泊車場景下的路徑規劃結果

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

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

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

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

圖11 不同方法的路徑規劃情況

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

表3 批處理仿真結果

5 結束語

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

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