張家旭 周時(shí)瑩 李長龍 郭 崇 趙 健

1.吉林大學(xué)汽車仿真與控制國家重點(diǎn)實(shí)驗(yàn)室，長春，1300222.中國第一汽車集團(tuán)有限公司智能網(wǎng)聯(lián)研發(fā)院，長春，130011

0 引言

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

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

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

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

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

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

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

圖1 膨脹算法的偽代碼

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

2 全局路徑規(guī)劃

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

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

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

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

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

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

(a)原始能量場

3 局部路徑規(guī)劃

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

圖5 線控四輪轉(zhuǎn)向汽車運(yùn)動(dòng)學(xué)模型

(1)

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

(2)

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

(3)

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

(4)

其中，T為旋轉(zhuǎn)變換矩陣，可表示為

(5)

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

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

J=αJ1+βJ2

(6)

式中，J1、α分別為預(yù)測軌跡終端處汽車橫擺角與全局路徑目標(biāo)點(diǎn)處切線方向之間的角度偏差懲罰項(xiàng)及加權(quán)系數(shù)；J2、β分別為汽車預(yù)測軌跡與障礙物之間最近距離懲罰項(xiàng)及加權(quán)系數(shù)。

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

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

(7)

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

(8)

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

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

(9)

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

4 仿真分析

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

表1 仿真參數(shù)

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

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

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

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

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

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

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

6 結(jié)論

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

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