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

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

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

0 引言

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

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

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

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

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

1 環(huán)境地圖構建

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

圖1 膨脹算法的偽代碼

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

2 全局路徑規(guī)劃

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

圖2 基于D* Lite算法的動態(tài)路徑規(guī)劃流程

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

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

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

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

(a)原始能量場

3 局部路徑規(guī)劃

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

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

(1)

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

(2)

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

(3)

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

(4)

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

(5)

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

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

J=αJ1+βJ2

(6)

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

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

圖7 泊車路徑規(guī)劃

(7)

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

(8)

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

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

(9)

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

4 仿真分析

圖8 自主代客泊車場景靜態(tài)環(huán)境地圖

表1 仿真參數

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

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

5 硬件在環(huán)試驗驗證

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

圖10 硬件在環(huán)試驗平臺

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

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

6 結論

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

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