線控四輪轉向汽車自主代客泊車路徑規劃

張家旭 周時瑩 李長龍 郭 崇 趙 健

1.吉林大學汽車仿真與控制國家重點實驗室，長春，1300222.中國第一汽車集團有限公司智能網聯研發院，長春，130011

0 引言

隨著V2X技術和自動駕駛技術的高速發展，自主代客泊車系統已成為解決最后一公里自動駕駛問題的有效手段。在已知停車場靜態環境地圖的基礎上，快速高效地規劃出面向動態障礙物場景的可行路徑是自主代客泊車系統的核心技術之一，且已得到了許多學者和主機廠商的廣泛關注[1-2]。

自主代客泊車系統現有的路徑規劃算法依據搜索空間類型可劃分為基于連續狀態空間的規劃算法和基于離散狀態空間的規劃算法。

文獻[3]將多個車輛自主代客泊車過程描述成約束優化問題，并采用內點法求解該約束優化問題，得到了最優的自主代客泊車路徑。文獻[4-5]以最小化自主代客泊車過程的能量消耗和所需時間為優化目標，基于優化理論規劃自主代客泊車路徑。文獻[6]借助V2X技術和云計算技術，基于優化理論規劃滿足約束要求的最優自主代客泊車路徑。上述基于連續狀態空間的規劃算法具有完備性優勢，但算法的高計算復雜度限制了其工程應用。

基于離散狀態空間的規劃算法通過降低完備性要求來提高算法的計算效率和實用性。文獻[7]采用具有解析度完備的Dijkstra算法規劃自主代客泊車路徑。文獻[8]基于車輛位姿信息離散化搜索空間，采用混合A*算法規劃滿足汽車運動學約束的自主代客泊車路徑。文獻[9]利用虛擬勢能場思想建立自主代客泊車環境地圖，并采用概率完備的快速擴展隨機樹搜索算法規劃自主代客泊車路徑。文獻[10]采用將A*算法和改進的動態窗口法相結合的方式規劃自主代客泊車路徑。當汽車沿著基于離散狀態空間的規劃算法得到的自主代客泊車路徑行駛時，遇到動態障礙物則需要重新規劃路徑，而上述算法的路徑重規劃效率較低。

鑒于此，本文針對線控四輪轉向汽車自主代客泊車路徑規劃問題，提出一種對動態障礙物具有自適應能力的自主代客泊車路徑規劃方法。首先，采用柵格地圖描述停車場環境信息，將連續狀態空間中的自主代客泊車路徑規劃問題轉化為離散狀態空間中的自主代客泊車路徑規劃問題，并采用各向同性的圓形結構元素對柵格地圖進行膨脹處理，使規劃的自主代客泊車路徑與障礙物之間保持一定的安全距離。隨后，采用D*Lite算法規劃出對動態障礙物具有自適應能力的全局自主代客泊車路徑，并采用動態窗口法和圓弧-直線組合方式沿著全局路徑規劃出滿足線控四輪轉向汽車運動學和機械約束的局部自主代客泊車路徑，引導汽車安全無碰撞地進入斜向泊車位。最后，通過仿真對所提方法的可行性進行了驗證。所提出的方法可以在動態障礙物存在的自主代客泊車場景中，規劃出安全無碰撞的可行路徑，引導汽車進入目標斜向泊車位。

1 環境地圖構建

快速、準確地表征停車場環境信息是線控四輪轉向汽車實現自主代客泊車路徑規劃的必要前提[11]。本節采用柵格地圖描述停車場環境信息，將連續狀態空間中的自主代客泊車路徑規劃問題轉化為離散狀態空間中的自主代客泊車路徑規劃問題，以降低路徑規劃算法的計算復雜度。為了使規劃的自主代客泊車路徑與障礙物之間保持一定的安全距離，本節采用各向同性的圓形結構元素對柵格地圖進行膨脹處理，膨脹算法的偽代碼如圖1所示。

圖1 膨脹算法的偽代碼

如圖1所示，行1為利用原始柵格地圖初始化膨脹柵格地圖；在利用行2和行3遍歷原始柵格地圖過程中，若當前原始柵格節點為障礙物節點，且當前原始柵格節點的上下左右四個鄰居節點中至少有一個為可行節點時，則利用行5將膨脹柵格地圖中圓形結構元素覆蓋的柵格節點設置為障礙物節點。相對于傳統的膨脹算法，該算法僅需將障礙物邊界區域包含的柵格節點重新設置為障礙物節點，很大程度上減少了柵格節點的訪問次數，降低了算法的計算復雜度。同時，在算法實現過程中，采用數組保存圓形結構元素中所有柵格節點相對于中心柵格節點(錨點)的偏移量，提高了圓形結構元素中所有柵格節點的有效性判斷效率。

2 全局路徑規劃

全局路徑規劃的任務是規劃出連接自主代客泊車起始點到目標點的無碰撞路徑[12]。本節采用D*Lite算法規劃線控四輪轉向汽車自主代客泊車全局路徑。采用c(M，N)表示節點M與節點N之間的當前代價，采用cold(M，N)表示節點M與節點N之間的歷史代價，采用K值表示柵格估計量，采用g值表示任意節點與目標節點之間的歷史代價，采用r值表示任意節點與目標節點之間的當前代價，則基于節點的g值和r值可將節點的狀態劃分為局部一致、局部過一致和局部欠一致。具體而言，若節點的g值等于r值，則節點的狀態為局部一致；若節點的g值大于r值，則節點的狀態為局部過一致；若節點的g值小于r值，則節點的狀態為局部欠一致。D*Lite算法通過維護節點的g值和r值來實現動態障礙物場景的路徑規劃，包含三個主要模塊的D*Lite算法的動態路徑規劃流程如圖2所示[13]。

圖2 基于D* Lite算法的動態路徑規劃流程

具體而言，D*Lite算法首先將自主代客泊車場景靜態環境地圖包含的所有節點的g值設置為極大值，將自主代客泊車場景靜態環境地圖包含的去除目標節點之外的所有節點的r值設置為極大值，隨后采用從目標點向起始點反向搜索的方式來最小化各個節點的r值，進而將自主代客泊車場景靜態環境地圖轉化為一個“靜態路徑場”，為汽車逐漸靠近目標點提供了擇優依據。在已建立的“靜態路徑場”基礎上，模塊一可以實現汽車自主代客泊車過程中依據所在的當前節點信息貪婪地選擇最佳下一個節點；模塊二可以在汽車所在的當前節點的后繼節點由可行節點變為障礙物節點或者由障礙物節點變為可行節點時，通過重新選擇汽車所在的當前節點的最佳后繼節點來更新當前節點的r值，進而將后繼節點的變化情況傳播到汽車所在的當前節點；模塊三可以將汽車所在的當前節點的r值變化傳播到其前繼節點，優化當前節點及其前繼節點到目標節點的代價。

圖3為基于D*Lite算法的動態路徑規劃示意圖?；贒*Lite算法建立的“靜態路徑場”可得到圖3a所示的從起始節點13到目標節點8的最優路徑為：節點13→節點10→節點7→節點8。如圖3b所示，在汽車利用模塊一貪婪地選擇最優路徑節點來逐漸靠近目標點的過程中，發現當前節點10的歷史最佳節點7變為障礙物節點時，則激活模塊二將當前節點10的最佳節點變為節點6，并且更新當前節點10的r值后將節點10插入到開放列表中。如圖3c所示，從開放列表中取出節點10作為模塊三的待處理節點，將節點10的g值設置為極大值，遍歷節點10的前繼節點，將節點10的前繼節點13的歷史最佳節點由節點10變為節點9，并且遍歷節點10的后繼節點來確定節點6是否依然為其歷史最佳節點。如圖3d所示，執行完模塊三后再次激活模塊一來將汽車從節點10移動到節點6，考慮到當前節點6的歷史最佳節點7已經標記為障礙物節點，則再次激活模塊一使汽車沿著路徑“節點6→節點3→節點8”移動到目標節點8。

(a)相對位置1 (b)相對位置2

通過上述分析可知，D*Lite算法的核心是通過維護節點的g值和r值形成圖4所示的以目標節點為穩定點的能量場，汽車沿著能量場的最速下降方向行駛，并且當能量場中障礙物信息發生變化時，通過快速修正能量場來規劃出可行的動態路徑。同時，D*Lite算法運行過程需要反復執行開放列表插入節點操作和提取最小節點操作，這兩類操作的運行時間是影響D*Lite算法搜索效率的重要因素。為了提高D*Lite算法的搜索效率，本節采用優先隊列數據結構構建D*Lite算法中的開放列表[14]。

(a)原始能量場

3 局部路徑規劃

局部路徑規劃的任務是在全局路徑的基礎上規劃出滿足線控四輪轉向汽車運動學約束的無碰撞路徑。本節采用動態窗口算法規劃線控四輪轉向汽車自主代客泊車局部路徑[15]。如圖5所示，已知線控四輪轉向汽車的前輪轉向角為δf、后輪轉向角為δr、方位角為φ、軸距為L以及后軸中點橫坐標、縱坐標和速度分別為xr、yr和vr，則線控四輪轉向汽車運動學模型可描述為[16]

圖5 線控四輪轉向汽車運動學模型

(1)

若采用x=(xr，yr，φ)T表示系統狀態向量，采用u=(vr,δf,δr)T表示系統控制向量，則利用四階龍格庫塔積分法可將線控四輪轉向汽車運動學模型離散化為

(2)

其中，h為計算步長，k為離散時刻，K1、K2、K3和K4為四階龍格庫塔積分法系數，可表示為[17]

(3)

圖6 柵格空間覆蓋枚舉方法原理

(4)

其中，T為旋轉變換矩陣，可表示為

(5)

至此，利用式(4)和式(5)，并結合汽車后軸中點坐標可快速定位需要柵格空間覆蓋枚舉方法檢測的柵格節點。

若汽車預測軌跡經柵格空間覆蓋枚舉方法檢測后為無碰撞軌跡，則采用評價函數J從無碰撞軌跡中篩選出最優的汽車預測軌跡，并將最優的汽車預測軌跡片段作為當前時刻與下一時刻之間的線控四輪轉向汽車自主代客泊車局部路徑[18]，其表達式為

J=αJ1+βJ2

(6)

式中，J1、α分別為預測軌跡終端處汽車橫擺角與全局路徑目標點處切線方向之間的角度偏差懲罰項及加權系數；J2、β分別為汽車預測軌跡與障礙物之間最近距離懲罰項及加權系數。

當汽車位于斜向泊車位附近時，基于圓弧-直線組合方式規劃連接泊車起始點和目標點的泊車路徑[19]。如圖7所示，已知泊車起始點的坐標為(xG1，yG1)，泊車目標點的坐標為(xG3，yG3)，斜向泊車位的傾斜角度為θ1，則有

圖7 泊車路徑規劃

(7)

其中，R1為線控四輪轉向汽車轉彎半徑，由式(7)可得其表達式為

(8)

由式(8)可得泊車路徑包含的圓弧段和直線段的交點坐標為

(xG2，yG2)=(xG1-R1sinθ1，yG1-R1(1-cosθ1))

(9)

至此，已規劃出由圓弧段和直線段組成的泊車路徑。

4 仿真分析

圖8 自主代客泊車場景靜態環境地圖

表1 仿真參數

在采用膨脹算法建立的圖9a所示的自主代客泊車場景靜態環境柵格地圖基礎上，基于D*Lite算法規劃出的無碰撞原始全局路徑如圖9b所示。在汽車行駛過程中，當自主代客泊車場景地圖中出現圖9c中紅色區域標識的動態障礙物時，再次激活膨脹算法可得到圖9d所示的存在動態障礙物的自主代客泊車場景柵格地圖，并且D*Lite算法僅需遍歷圖9e所示的障礙物周圍較少的節點便可重規劃出圖9f所示的無碰撞全局路徑，提高了全局路徑重規劃的效率。如圖9g～圖9i所示，在重規劃的全局路徑基礎上，基于動態窗口法得到的局部路徑可以安全無碰撞地引導汽車行駛到目標斜向泊車位附近，而基于圓弧-直線組合方式得到的泊車路徑可以安全無碰撞地引導汽車進入目標斜向泊車位。

(a)自主代客泊車場景靜態環境柵格地圖 (b)規劃的原始全局路徑 (c)存在動態障礙物的自主代客泊車場景地圖

5 硬件在環試驗驗證

本節基于圖10所示的硬件在環試驗平臺對自主代客泊車路徑規劃方法的有效性進行驗證。硬件在環試驗平臺由自主代客泊車系統感知模塊、待驗證的規劃模塊、控制模塊和虛擬車輛組成。其中，自主代客泊車系統感知模塊用于回放實車測試數據，待驗證的規劃模塊基于感知回放數據規劃自主代客泊車路徑，控制模塊基于規劃路徑與實際路徑的偏差控制虛擬車輛完成自主代客泊車任務。

圖10 硬件在環試驗平臺

基于該硬件在環試驗平臺得到的自主代客泊車路徑規劃方法驗證結果如圖11所示，當自主代客泊車場景地圖中出現紅色標識的動態障礙物時，本文所提出的自主代客泊車路徑規劃方法可重規劃全局路徑，并且基于動態窗口法和圓弧-直線組合方式得到的局部路徑可以安全無碰撞地引導汽車進入目標斜向泊車位。

(a)原始全局路徑和重規劃全局路徑

6 結論

(1)利用柵格地圖表征停車場環境信息，將連續狀態空間中的自主代客泊車路徑規劃問題轉化為離散狀態空間中的自主代客泊車路徑規劃問題，降低了路徑規劃算法的計算復雜度，并采用各向同性的圓形結構元素對柵格地圖進行膨脹處理，使規劃的自主代客泊車路徑與障礙物之間保持一定的安全距離。

(2)基于D*Lite算法規劃出對動態障礙物具有自適應能力的全局自主代客泊車路徑，并采用動態窗口法和圓弧-直線組合方式沿著全局路徑規劃出滿足線控四輪轉向汽車運動學和機械約束的局部自主代客泊車路徑，引導汽車安全無碰撞地進入斜向泊車位。