張家旭 王志偉 郭 崇 趙 健 周時(shí)瑩

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

高級(jí)駕駛輔助技術(shù)的推廣應(yīng)用提高了汽車(chē)行駛的安全性、舒適性和經(jīng)濟(jì)性,泊車(chē)輔助系統(tǒng)作為高級(jí)駕駛輔助技術(shù)的重要組成部分,已有近30年研究歷史[1],其意在幫助駕駛員解決棘手的泊車(chē)難問(wèn)題.相對(duì)于傳統(tǒng)的泊車(chē)輔助系統(tǒng),自主代客泊車(chē)系統(tǒng)能夠代替駕駛員自主尋找車(chē)位并停放汽車(chē),具有更高的自動(dòng)化程度,徹底解決汽車(chē)最后一公里自動(dòng)駕駛問(wèn)題[2].

路徑規(guī)劃是自主代客泊車(chē)系統(tǒng)的重要環(huán)節(jié),目前最具代表性的路徑規(guī)劃方法有基于優(yōu)化的方法、基于采樣的方法和基于圖搜索的方法.文獻(xiàn)[3]將自主代客泊車(chē)路徑規(guī)劃問(wèn)題轉(zhuǎn)化為非線性規(guī)劃問(wèn)題,并將客觀數(shù)據(jù)作為初始解來(lái)加快非線性規(guī)劃問(wèn)題的求解效率.文獻(xiàn)[4]綜合考慮汽車(chē)運(yùn)動(dòng)學(xué)約束、機(jī)械約束和碰撞約束等,將泊車(chē)路徑規(guī)劃轉(zhuǎn)化為最優(yōu)化問(wèn)題,并利用同步動(dòng)態(tài)優(yōu)化方法求解該最優(yōu)優(yōu)化問(wèn)題.文獻(xiàn)[5]采用自適應(yīng)偽譜法求解泊車(chē)路徑規(guī)劃問(wèn)題,相對(duì)于傳統(tǒng)偽普法具有更快的收斂速度.基于優(yōu)化的路徑規(guī)劃方法得到的路徑滿足汽車(chē)運(yùn)動(dòng)學(xué)、汽車(chē)動(dòng)力學(xué)和碰撞約束,并且具有較好的可跟隨性,但優(yōu)化問(wèn)題求解過(guò)程復(fù)雜,對(duì)計(jì)算性能要求較高,難以工程應(yīng)用.

相對(duì)于基于優(yōu)化的方法,基于采樣的方法以快速擴(kuò)展隨機(jī)樹(shù)算法為代表,有一定的隨機(jī)性,具有迭代快、計(jì)算量小等特點(diǎn),而基于圖搜索的方法以A*算法為代表,通過(guò)設(shè)置不同的啟發(fā)函數(shù)和節(jié)點(diǎn)擴(kuò)展方式,可實(shí)現(xiàn)全局最優(yōu)或近似最優(yōu).文獻(xiàn)[6]提出利用停車(chē)場(chǎng)內(nèi)攝像頭識(shí)別空車(chē)位,再利用Dijkstra算法規(guī)劃全局自主代客泊車(chē)路徑,并通過(guò)平行換道策略躲避停放不標(biāo)準(zhǔn)的車(chē)輛.文獻(xiàn)[7]首先采用混合A*算法規(guī)劃出滿足汽車(chē)運(yùn)動(dòng)學(xué)約束的全局自主代客泊車(chē)路徑,隨后利用共軛梯度方法對(duì)全局自主代客泊車(chē)路徑的曲率、與障礙物距離等因素進(jìn)行優(yōu)化.文獻(xiàn)[8]改進(jìn)了混合A*算法的節(jié)點(diǎn)擴(kuò)展方式和代價(jià)函數(shù),并利用杜賓斯曲線直接連接自主代客泊車(chē)路徑的當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)的方式來(lái)提高路徑規(guī)劃的效率.文獻(xiàn)[9]從目標(biāo)偏向性、轉(zhuǎn)角約束和路徑平滑性3個(gè)角度對(duì)傳統(tǒng)快速擴(kuò)展隨機(jī)樹(shù)算法進(jìn)行優(yōu)化改進(jìn),提高了快速擴(kuò)展隨機(jī)樹(shù)算法對(duì)自主代客泊車(chē)路徑的規(guī)劃效率,并使規(guī)劃出的路徑具有更好的可跟隨性.文獻(xiàn)[10]將最優(yōu)泊車(chē)位姿融入到混合A*算法的啟發(fā)值中來(lái)優(yōu)化全局路徑搜索方向,減少時(shí)間消耗.文獻(xiàn)[11]利用可行駛區(qū)域場(chǎng)構(gòu)建快速擴(kuò)展隨機(jī)樹(shù)算法的概率密度函數(shù)來(lái)優(yōu)化隨機(jī)樹(shù)的生長(zhǎng)方向,并利用B樣條曲線平滑規(guī)劃出的路徑.上述2類(lèi)方法生成的路徑一般難以滿足汽車(chē)運(yùn)動(dòng)學(xué)約束,通常作為全局路徑應(yīng)用.

綜上分析可知,單一的路徑規(guī)劃方法難以應(yīng)對(duì)復(fù)雜的自主代客泊車(chē)工況.鑒于此,本文提出一種基于分層架構(gòu)的自主代客泊車(chē)路徑規(guī)劃算法.首先,利用柵格掃描算法快速準(zhǔn)確地將自主代客泊車(chē)環(huán)境地圖轉(zhuǎn)化成為Voronoi圖,量化自主代客泊車(chē)環(huán)境中任意柵格格網(wǎng)區(qū)域與其最近障礙物距離.隨后,利用A*算法規(guī)劃出全局自主代客泊車(chē)路徑,并采用優(yōu)先隊(duì)列數(shù)據(jù)實(shí)現(xiàn)A*算法的開(kāi)放列表來(lái)提升其計(jì)算效率.最后,基于自主代客泊車(chē)環(huán)境Voronoi圖實(shí)現(xiàn)汽車(chē)碰撞檢測(cè),并采用改進(jìn)動(dòng)態(tài)窗口法沿著全局自主代客泊車(chē)路徑規(guī)劃出滿足汽車(chē)非完整約束和機(jī)械約束的無(wú)碰撞路徑,擴(kuò)展傳統(tǒng)動(dòng)態(tài)窗口法的可行解空間和降低其保守性.

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

準(zhǔn)確高效獲取環(huán)境地圖信息是快速規(guī)劃出可執(zhí)行的自主代客泊車(chē)路徑的重要基礎(chǔ).本節(jié)利用Voronoi圖描述自主代客泊車(chē)環(huán)境地圖信息,并采用具有離散計(jì)算特征的柵格掃描算法生成Voronoi圖,快速準(zhǔn)確地量化自主代客泊車(chē)環(huán)境中任意柵格格網(wǎng)區(qū)域與其最近障礙物距離.如圖1所示,采用q1,q2,…,q8表示柵格格網(wǎng)p的8個(gè)鄰近格網(wǎng),由這8個(gè)鄰近格網(wǎng)構(gòu)成柵格格網(wǎng)p的鄰域模板.柵格掃描算法利用此模板對(duì)所有柵格格網(wǎng)分別進(jìn)行從左到右的逐行正向掃描,從右到左逐行反向掃描,從上到下逐列正向掃描和從下到上逐列反向掃描來(lái)生成Voronoi圖[12].

圖1 柵格格網(wǎng)p的鄰域模板

柵格格網(wǎng)p的8個(gè)鄰近格網(wǎng)的相對(duì)坐標(biāo)為C(q)=(Cx(q),Cy(q)),q∈{q1,q2,…,q8},則柵格格網(wǎng)p與8個(gè)鄰近格網(wǎng)之間的平方歐氏距離偏差d(p,q)和相對(duì)坐標(biāo)偏差M(p,q)可分別表示為[13]

d(p,q)=

(1)

(2)

定義從左到右逐行正向掃描集合為N1(p)={q1,q2,q3,q4},從右到左逐行反向掃描集合為N2(p)={q5,q6,q7,q8},從上到下逐列正向掃描集合為N3(p)={q3,q2,q1,q8},從下到上逐列反向掃描集合為N4(p)={q7,q6,q5,q4}.采用f(q),q∈{q1,q2,…,q8}表示柵格格網(wǎng)p的8個(gè)鄰近格網(wǎng)的平方歐氏距離,則柵格掃描算法計(jì)算任意柵格格網(wǎng)p的平方歐氏距離f(p)和相對(duì)坐標(biāo)C(p)的具體流程如下:

① 從左到右逐行正向掃描柵格格網(wǎng)p.設(shè)置f(p)=∞,對(duì)于任意的鄰近格網(wǎng)q∈N1(q),如果f(p)>f(q)+d(p,q),更新f(p)=f(q)+d(p,q),更新C(p)=C(q)+M(p,q).

② 從右到左逐行反向掃描柵格格網(wǎng)p.對(duì)于任意的鄰近格網(wǎng)q∈N2(q),如果f(p)>f(q)+d(p,q),更新f(p)=f(q)+d(p,q),更新C(p)=C(q)+M(p,q).

③ 從上到下逐列正向掃描柵格格網(wǎng)p.對(duì)于任意的鄰近格網(wǎng)q∈N3(q),如果f(p)>f(q)+d(p,q),更新f(p)=f(q)+d(p,q),更新C(p)=C(q)+M(p,q).

④ 從下到上逐列反向掃描柵格格網(wǎng)p.對(duì)于任意的鄰近格網(wǎng)q∈N4(q),如果f(p)>f(q)+d(p,q),更新f(p)=f(q)+d(p,q),更新C(p)=C(q)+M(p,q).

2 全局路徑規(guī)劃

全局路徑規(guī)劃的任務(wù)是根據(jù)環(huán)境地圖信息規(guī)劃出從起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞全局自主代客泊車(chē)路徑.本節(jié)采用A*算法規(guī)劃全局自主代客泊車(chē)路徑.利用歐氏距離計(jì)算公式定義A*算法中從起始點(diǎn)到當(dāng)前節(jié)點(diǎn)的實(shí)際代價(jià)G,從當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的預(yù)估代價(jià)H,以及當(dāng)前節(jié)點(diǎn)的總代價(jià)F=G+H,則A*算法通過(guò)對(duì)保存待檢測(cè)節(jié)點(diǎn)的開(kāi)放列表反復(fù)執(zhí)行插入節(jié)點(diǎn)和提取最小節(jié)點(diǎn)操作,并采用封閉列表保存已檢測(cè)的節(jié)點(diǎn),實(shí)現(xiàn)全局自主代客泊車(chē)路徑的規(guī)劃,其具體流程如下:

① 將起始點(diǎn)加入開(kāi)放列表.

② 重復(fù)如下過(guò)程直到目標(biāo)點(diǎn)加入開(kāi)放列表或開(kāi)放列表為空.

③ 將開(kāi)放列表中最小F值節(jié)點(diǎn)取出作為當(dāng)前節(jié)點(diǎn),將其移至封閉列表.

④ 檢查當(dāng)前節(jié)點(diǎn)的8個(gè)鄰近節(jié)點(diǎn),若鄰近節(jié)點(diǎn)平方歐氏距離小于給定閾值或已在封閉列表中,不操作;若鄰近節(jié)點(diǎn)不在開(kāi)放列表中,將其加入開(kāi)放列表,將當(dāng)前節(jié)點(diǎn)作為其父節(jié)點(diǎn);若鄰近節(jié)點(diǎn)在開(kāi)放列表中且當(dāng)前節(jié)點(diǎn)能減小鄰近節(jié)點(diǎn)G值,更新鄰近節(jié)點(diǎn)G值和F值,將當(dāng)前節(jié)點(diǎn)作為其父節(jié)點(diǎn).

開(kāi)放列表插入節(jié)點(diǎn)操作和提取最小節(jié)點(diǎn)操作的運(yùn)行時(shí)間是影響A*算法搜索效率的重要因素.本節(jié)采用插入節(jié)點(diǎn)操作和提取最小節(jié)點(diǎn)操作最壞情形運(yùn)行時(shí)間均為O(logN)的優(yōu)先隊(duì)列數(shù)據(jù)結(jié)構(gòu)實(shí)現(xiàn)A*算法中的開(kāi)放列表.同時(shí),為了獲取A*算法規(guī)劃的全局自主代客泊車(chē)路徑,需要從目標(biāo)點(diǎn)開(kāi)始,沿著每個(gè)節(jié)點(diǎn)的父節(jié)點(diǎn)移動(dòng)直到起始點(diǎn).因此,為了便于快速高效地從起始點(diǎn)開(kāi)始訪問(wèn)規(guī)劃出的全局自主代客泊車(chē)路徑,本節(jié)采用具有后進(jìn)先出特征的棧數(shù)據(jù)結(jié)構(gòu)保存全局自主代客泊車(chē)路徑.

3 局部路徑規(guī)劃

局部路徑規(guī)劃的任務(wù)是沿著全局自主代客泊車(chē)路徑規(guī)劃出滿足汽車(chē)非完整約束和機(jī)械約束的無(wú)碰撞路徑.本節(jié)采用改進(jìn)動(dòng)態(tài)窗口法規(guī)劃局部自主代客泊車(chē)路徑.傳統(tǒng)的動(dòng)態(tài)窗口法采用精度較低的歐拉法離散化汽車(chē)運(yùn)動(dòng)學(xué)方程,并且將第1個(gè)計(jì)算步長(zhǎng)內(nèi)控制量允許的變化量作為整個(gè)預(yù)測(cè)路徑過(guò)程的約束條件,限制了可行解空間[14].為此,本節(jié)采用精度更高的四階龍格庫(kù)塔積分法離散化汽車(chē)運(yùn)動(dòng)學(xué)方程,并在汽車(chē)預(yù)測(cè)路徑計(jì)算過(guò)程中持續(xù)更新控制量,擴(kuò)大可行解空間.同時(shí),本節(jié)采用3個(gè)相同半徑的圓表示汽車(chē)外輪廓,并借助Voronoi圖柵格格網(wǎng)的平方歐氏距離實(shí)現(xiàn)汽車(chē)碰撞快速檢測(cè),避免了傳統(tǒng)動(dòng)態(tài)窗口法將汽車(chē)簡(jiǎn)化為質(zhì)點(diǎn)來(lái)檢測(cè)碰撞而產(chǎn)生的較大保守性.

汽車(chē)自主代客泊車(chē)過(guò)程中處于低速大轉(zhuǎn)角行駛狀態(tài),可認(rèn)為4個(gè)車(chē)輪無(wú)側(cè)偏地繞同一瞬時(shí)圓心作圓周運(yùn)動(dòng).如圖2所示,將汽車(chē)后軸中點(diǎn)橫坐標(biāo)xr、縱坐標(biāo)yr和汽車(chē)橫擺角φ作為系統(tǒng)狀態(tài)量x=[xryrφ]T,將汽車(chē)后軸中點(diǎn)速度vr和汽車(chē)前輪轉(zhuǎn)向角δf作為系統(tǒng)控制量u=[vrδf]T,由此建立汽車(chē)運(yùn)動(dòng)學(xué)方程為

圖2 汽車(chē)運(yùn)動(dòng)學(xué)模型

(3)

式中,L為汽車(chē)軸距.

利用四階龍格庫(kù)塔積分法將式(3)離散化為[15]

(4)

式中,Δt為計(jì)算步長(zhǎng);K1、K2、K3和K4為四階龍格庫(kù)塔積分法的系數(shù),可表示為

(5)

若k時(shí)刻汽車(chē)后軸中點(diǎn)速度和汽車(chē)前輪轉(zhuǎn)向角的設(shè)定值分別為vr,set∈[Vmin,Vmax]和δf,set∈[-δmax,δmax],綜合考慮k-1時(shí)刻汽車(chē)后軸中點(diǎn)速度和汽車(chē)前輪轉(zhuǎn)向角實(shí)際值與汽車(chē)機(jī)械約束,可得

vr(k)=

(6)

δf(k)=

(7)

式中,amax和ωmax分別為汽車(chē)后軸中點(diǎn)加速度最大值和汽車(chē)前輪轉(zhuǎn)向角速度最大值.

為了提高局部自主代客泊車(chē)路徑規(guī)劃過(guò)程中汽車(chē)碰撞檢測(cè)效率,采用3個(gè)相同半徑的圓包含汽車(chē)外輪廓,通過(guò)3個(gè)圓圓心所在Voronoi圖柵格格網(wǎng)的平方歐氏距離實(shí)現(xiàn)汽車(chē)碰撞快速檢測(cè).如圖3所示,已知汽車(chē)的長(zhǎng)度、寬度和后懸長(zhǎng)度分別為L(zhǎng)c、W和Lr,則3個(gè)圓半徑R和k時(shí)刻圓心坐標(biāo)可分別表示為

圖3 汽車(chē)圓包絡(luò)

(8)

(9)

(10)

(11)

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

(12)

因此,若k時(shí)刻3個(gè)圓圓心所在Voronoi圖柵格格網(wǎng)的平方歐氏距離大于R2,則汽車(chē)在k時(shí)刻與障礙物不相交.

在汽車(chē)后軸中點(diǎn)速度可行空間[Vmin,Vmax]和汽車(chē)前輪轉(zhuǎn)向角可行空間[-δmax,δmax]均勻采樣,得到k時(shí)刻一簇汽車(chē)后軸中點(diǎn)速度和汽車(chē)前輪轉(zhuǎn)向角的設(shè)定值,利用式(3)～(7)預(yù)測(cè)k時(shí)刻到k+N時(shí)刻的汽車(chē)行駛軌跡.為了實(shí)現(xiàn)局部自主代客泊車(chē)路徑規(guī)劃,本節(jié)設(shè)計(jì)如下評(píng)價(jià)函數(shù)從無(wú)碰撞的汽車(chē)預(yù)測(cè)軌跡中篩選出最優(yōu)軌跡:

J(vr,set,δf,set)=αJ1(vr,set,δf,set)+βJ2(vr,set,δf,set)

(13)

式中,J1(vr,set,δf,set)為汽車(chē)k+N時(shí)刻橫擺角與全局路徑切線方向之間的角度偏差;J2(vr,set,δf,set)為汽車(chē)預(yù)測(cè)軌跡與障礙物之間最近距離;α和β為加權(quán)系數(shù).

在采用改進(jìn)動(dòng)態(tài)窗口法規(guī)劃出局部自主代客泊車(chē)路徑來(lái)引導(dǎo)汽車(chē)到達(dá)泊車(chē)位附近后,再利用文獻(xiàn)[5]提出的方法規(guī)劃泊車(chē)路徑,引導(dǎo)汽車(chē)進(jìn)入泊車(chē)位,進(jìn)而實(shí)現(xiàn)汽車(chē)自主代客泊車(chē)功能.

4 仿真分析

本節(jié)在VC++6.0編輯和編譯環(huán)境中實(shí)現(xiàn)自主代客泊車(chē)路徑規(guī)劃算法.針對(duì)圖4(a)所示的停車(chē)場(chǎng)原始地圖,所提出的自主代客泊車(chē)路徑規(guī)劃算法參數(shù)設(shè)置為:Δt=0.1 s,Lc=4.155 m,L=2.405 m,Lf=0.800 m,Lr=0.950 m,W=1.645 m,δmax=0.53 rad,Vmin=1 m/s,Vmax=3 m/s,amax=2 m/s2,ωmax=0.53 rad/s,柵格格網(wǎng)距離為0.5 m,泊車(chē)位長(zhǎng)度和寬度分別為4.5和2.5 m.仿真計(jì)算結(jié)果如圖4(b)～(d)所示.

(a) 停車(chē)場(chǎng)原始地圖

圖4(b)為柵格掃描算法生成的停車(chē)場(chǎng)Voronoi圖,圖4(c)為A*算法規(guī)劃的全局自主代客泊車(chē)路徑,圖4(d)為改進(jìn)動(dòng)態(tài)窗口法規(guī)劃的以圓形為起始點(diǎn)、五角星為目標(biāo)點(diǎn)的局部自主代客泊車(chē)路徑和文獻(xiàn)[5]方法規(guī)劃的連接2個(gè)五角星的垂直泊車(chē)路徑.如圖4(b)所示,基于柵格掃描算法將停車(chē)場(chǎng)原始地圖轉(zhuǎn)換為包含勢(shì)能場(chǎng)信息的停車(chē)場(chǎng)Voronoi圖,利用該信息可以更易判斷車(chē)輛與障礙物之間的距離,為解決自動(dòng)代客泊車(chē)路徑規(guī)劃的避障問(wèn)題奠定了基礎(chǔ).如圖4(c)所示,本文將平方歐氏距離小于一定閾值的柵格格網(wǎng)考慮成不可行區(qū)域,使得規(guī)劃的全局自主代客泊車(chē)路徑與障礙物保持一定的安全距離,提升自主代客泊車(chē)過(guò)程的安全性,并且便于局部路徑規(guī)劃過(guò)程中障礙物處理.如圖4(d)所示,采用改進(jìn)動(dòng)態(tài)窗口法規(guī)劃的局部自主代客泊車(chē)路徑滿足汽車(chē)運(yùn)動(dòng)學(xué)約束和避障約束,可以安全地引導(dǎo)汽車(chē)到達(dá)目標(biāo)泊車(chē)位附近.當(dāng)汽車(chē)到達(dá)目標(biāo)泊車(chē)位附近時(shí),采用文獻(xiàn)[5]提出的泊車(chē)路徑方法規(guī)劃垂直泊車(chē)路徑,引導(dǎo)汽車(chē)進(jìn)入泊車(chē)位,進(jìn)而實(shí)現(xiàn)汽車(chē)自主代客泊車(chē)功能.

5 結(jié)論

1) 基于分層架構(gòu)提出了一種自主代客泊車(chē)路徑規(guī)劃算法.采用Voronoi圖描述自主代客泊車(chē)環(huán)境地圖信息,量化自主代客泊車(chē)環(huán)境中任意柵格格網(wǎng)區(qū)域與其最近障礙物距離.基于自主代客泊車(chē)環(huán)境Voronoi圖,利用A*算法規(guī)劃出了全局自主代客泊車(chē)路徑,并采用優(yōu)先隊(duì)列數(shù)據(jù)實(shí)現(xiàn)A*算法的開(kāi)放列表來(lái)提升其計(jì)算效率.

2) 在全局自主代客泊車(chē)路徑的基礎(chǔ)上,采用改進(jìn)動(dòng)態(tài)窗口法規(guī)劃出了滿足汽車(chē)非完整約束和機(jī)械約束的無(wú)碰撞路徑,有效地?cái)U(kuò)展了傳統(tǒng)動(dòng)態(tài)窗口法的可行解空間,并且降低了其保守性.

3) 在VC++6.0環(huán)境中驗(yàn)證所提出的自主代客泊車(chē)路徑規(guī)劃算法的可行性和有效性,結(jié)果表明，所提出的自主代客泊車(chē)路徑規(guī)劃算法可以安全、快速地引導(dǎo)汽車(chē)到達(dá)目標(biāo)泊車(chē)位附近,為汽車(chē)后續(xù)執(zhí)行泊車(chē)操作奠定基礎(chǔ).