曹昊天,宋曉琳,黃 江
(湖南大學(xué),汽車車身先進(jìn)設(shè)計(jì)與制造國(guó)家重點(diǎn)實(shí)驗(yàn)室,長(zhǎng)沙 410082)
目前研發(fā)的車輛主動(dòng)避障系統(tǒng)有碰撞預(yù)警系統(tǒng)(collision warning system, CWS)、自適應(yīng)巡航控制系統(tǒng)(adaptive cruise control system, ACC)和車輛停走系統(tǒng)(stop & go)[1]等。歐洲EuroFOT提供的研究數(shù)據(jù)顯示,裝有ACC系統(tǒng)和前碰撞預(yù)警系統(tǒng)(FCW)等先進(jìn)駕駛員輔助系統(tǒng)(ADAS)的車輛,能減少交通事故的發(fā)生概率達(dá)5.7%[2]。根據(jù)2012年中國(guó)統(tǒng)計(jì)年鑒交通事故的數(shù)據(jù),我國(guó)2011年由于交通事故而死亡的人數(shù)達(dá)到六萬余人[3],盡管與前幾年比較死亡人數(shù)呈下降趨勢(shì),但總體而言,這個(gè)傷亡數(shù)目仍然巨大。而車輛主動(dòng)避障系統(tǒng)可以輔助駕駛員對(duì)潛有危險(xiǎn)的交通場(chǎng)景規(guī)劃出安全的避障路徑,從而可減少甚至避免交通事故的發(fā)生,因此車輛主動(dòng)避障系統(tǒng)的研究具有理論意義和實(shí)用價(jià)值。
文獻(xiàn)[4]中提出的人工勢(shì)場(chǎng)理論和彈性繩理論最早運(yùn)用于機(jī)器人領(lǐng)域,文獻(xiàn)[5]和文獻(xiàn)[6]中加以改進(jìn)運(yùn)用到自主車輛路徑規(guī)劃領(lǐng)域,并成功實(shí)現(xiàn)了在低風(fēng)險(xiǎn)路況下動(dòng)態(tài)車輛的避障路徑規(guī)劃算法。為擴(kuò)大障礙車勢(shì)場(chǎng)的作用范圍,作者曾對(duì)影響避障路徑生成的幾個(gè)因素,即障礙車引導(dǎo)勢(shì)場(chǎng)的形狀、引導(dǎo)勢(shì)場(chǎng)因子、彈性繩剛度和主車速度進(jìn)行了探討,結(jié)果表明彈性繩剛度和主車車速的影響最大,然后是引導(dǎo)勢(shì)場(chǎng)因子,而引導(dǎo)勢(shì)場(chǎng)形狀的影響最小[7]。本文中在先前研究和文獻(xiàn)[6]算法的基礎(chǔ)上,分別在障礙車的前后增加一個(gè)引導(dǎo)勢(shì)場(chǎng),研究引入引導(dǎo)勢(shì)場(chǎng)后對(duì)主車避障路徑生成的改善情況。通過在直道和彎道兩種不同道路環(huán)境下,結(jié)合Carsim車輛動(dòng)力學(xué)模型進(jìn)行仿真分析。結(jié)果表明,添加引導(dǎo)勢(shì)場(chǎng)后,對(duì)于直線道路上行駛的車輛作用最為明顯,車輛能夠提前避障,從而給駕駛員更為寬裕的操作時(shí)間,并且避障過程中在縱向上與障礙車輛保持充分的安全距離,從而使避障過程更安全。
道路環(huán)境的信息由車載雷達(dá)、GPS和車載攝像頭獲得。在路徑?jīng)Q策的初始階段,位于道路上的慣性坐標(biāo)系的原點(diǎn)選取在道路中心線上與車輛質(zhì)心縱向?qū)?yīng)的位置,如圖1所示。道路中心線由一回旋線構(gòu)造,其曲率變化率為一常數(shù),即其曲率與弧長(zhǎng)成正比。假定一條總弧長(zhǎng)為lD的回旋線線段,其起始曲率為c0,則自起點(diǎn)計(jì)弧長(zhǎng)為l處一點(diǎn)的曲率為c(l)=c0+c1l。由于回旋線的精確表達(dá)須計(jì)算Fresnel積分,因此更常用的方法是采用多項(xiàng)式逼近來表達(dá),即道路中心線的橫向位移可表示為
(1)
式中:x為道路坐標(biāo)系下的縱向位移;y0為道路中心線初始的橫向偏移;τ0為初始方向角;c1為曲率變化率或曲率常數(shù),c1=(cf-c0)/lD,cf為終點(diǎn)曲率。
道路中心線上的點(diǎn)在道路坐標(biāo)系中可表示為
Rc=xex+ycey
(2)
式中:ex、ey為道路坐標(biāo)系的縱向和橫向單位向量。
根據(jù)圖1,結(jié)合邊界條件yc(0)=0和yc′(0)=0可將y0和τ0消掉。為計(jì)算簡(jiǎn)便,引入建立在道路中心線上的自然坐標(biāo)系,其切向和法向單位向量為
(3)
因此對(duì)于雙車道,道路邊界上的點(diǎn)可表示為
(4)
式中:B為道路總寬度;下標(biāo)q∈[l,r],表示左右邊界。
此外考慮到構(gòu)成道路中心線的回旋線方向角較小,基于小曲率的假設(shè)下,整個(gè)道路上任意點(diǎn)的位置R近似表示為
R=Rc+(y-yc)en
(5)
根據(jù)文獻(xiàn)[5]中提出的彈性繩模型,假定車輛的緊急避障路徑是由一根虛擬的彈性繩模擬,它由N個(gè)節(jié)點(diǎn)的線性彈簧串聯(lián)在一起,如圖2所示。節(jié)點(diǎn)的位置向量在道路坐標(biāo)系中可表示為
ri=xiex+yiey
(6)
路徑規(guī)劃開始時(shí),確定好起始點(diǎn)和目標(biāo)點(diǎn),這樣彈性繩的首尾處的節(jié)點(diǎn)坐標(biāo)是已知的。假設(shè)彈簧的拉伸滿足胡克定律,那么彈性繩節(jié)點(diǎn)處的彈性勢(shì)能可由與相鄰兩個(gè)節(jié)點(diǎn)之間的拉伸長(zhǎng)度給出,即
(7)
式中:i=1,2,3,…,N-1,N為節(jié)點(diǎn)個(gè)數(shù);ki為第i段彈簧的剛度;li為第i段彈簧的自然長(zhǎng)度。li定義為歐式距離,即
道路邊界勢(shì)場(chǎng)的建立是為確保車輛在道路上正常行駛而不至于出界,因此勢(shì)場(chǎng)模型的選擇是關(guān)鍵之一。根據(jù)人工勢(shì)場(chǎng)模型在機(jī)器人領(lǐng)域運(yùn)用的經(jīng)驗(yàn),選取如式(8)所示的勢(shì)場(chǎng),很顯然,當(dāng)車輛接近于道路邊界,它的勢(shì)能值趨向于無窮大,以達(dá)到限制車輛運(yùn)動(dòng)的目的。
(8)
式中ηq為道路邊界剛度值。
本文中采用的人工勢(shì)場(chǎng)法考慮的是車輛在R2空間中的路徑規(guī)劃,且障礙車采用凸多邊形表示,具體而言,采用矩形來描述障礙車的幾何結(jié)構(gòu)。構(gòu)建障礙車勢(shì)場(chǎng)的目的是防止它與其他車輛發(fā)生碰撞,并保持一定的安全距離。因此在設(shè)計(jì)障礙車勢(shì)場(chǎng)函數(shù)時(shí),應(yīng)使主車越接近該障礙車時(shí),障礙車的勢(shì)能值越大,即
(9)
式中:kobst為障礙車勢(shì)場(chǎng)的剛度值;r0(m)為第m個(gè)障礙車輛的質(zhì)心坐標(biāo);d為安全圓直徑。所謂安全圓就是以障礙車輛的質(zhì)心為圓心包含整輛車在內(nèi)的一個(gè)圓,以保證主車不會(huì)與障礙車發(fā)生接觸,見圖3。
為使主車能夠在運(yùn)動(dòng)中盡力避免與障礙車接近,考慮在障礙車前后加入引導(dǎo)勢(shì)場(chǎng),并且將勢(shì)場(chǎng)的影響范圍“放大”,從而使主車能夠提前做出避障措施。如圖3所示,在障礙車前后加入半圓形的引導(dǎo)勢(shì)場(chǎng),對(duì)于后引導(dǎo)勢(shì)場(chǎng),選取其勢(shì)場(chǎng)表達(dá)式為
(10)
式中:kgr為車輛勢(shì)場(chǎng)剛度值;τ為引導(dǎo)勢(shì)場(chǎng)半徑;xr為障礙車后方邊界的中心位置的橫坐標(biāo)。引導(dǎo)勢(shì)場(chǎng)的形式與障礙車勢(shì)場(chǎng)的表達(dá)式相近,但是R′中多出了一個(gè)放大因子ξ,通過在以障礙車后方邊界中點(diǎn)為原點(diǎn)的坐標(biāo)系中進(jìn)行縮放,擴(kuò)大引導(dǎo)勢(shì)場(chǎng)在道路縱向上的影響范圍(側(cè)向上保持不變)。經(jīng)“放大”后的縱向坐標(biāo)值在道路坐標(biāo)系中用x*′表示,在相應(yīng)的道路坐標(biāo)系中表示成x′。此外以左邊界中點(diǎn)為原點(diǎn)的坐標(biāo)系中的X方向的數(shù)值表示成x*,該坐標(biāo)系的單位向量與道路坐標(biāo)系中單位向量的轉(zhuǎn)換關(guān)系為
(11)
(12)
式中:ψ0為障礙車的方向角,如圖3所示。后方放大因子ξr與主車和障礙車的速度差v-vm有直接關(guān)系,將βr稱作后方引導(dǎo)勢(shì)場(chǎng)因子。當(dāng)障礙車在直線道上行駛,ψ0=π。式(11)和式(12)可簡(jiǎn)化為
(13)
同樣,對(duì)于前方引導(dǎo)勢(shì)場(chǎng)也有類似的設(shè)定,ξf稱作前方放大因子,βf稱作前方引導(dǎo)勢(shì)場(chǎng)因子??偠灾ㄟ^引導(dǎo)勢(shì)場(chǎng)的“放大”作用使原障礙車勢(shì)場(chǎng)的影響范圍更大,原本離障礙車較遠(yuǎn)的點(diǎn),經(jīng)過坐標(biāo)變化后離障礙車更近了,從而促使車輛在離障礙車更遠(yuǎn)的地方就開始采取避障措施。β的取值決定了引導(dǎo)勢(shì)場(chǎng)的影響程度,β越大,所能影響的距離也就越遠(yuǎn)。
對(duì)于M個(gè)障礙車,將所有的障礙車勢(shì)場(chǎng)和前后引導(dǎo)勢(shì)場(chǎng)疊加起來得到最終的障礙車勢(shì)場(chǎng),如式(14)所示。外界勢(shì)場(chǎng)為道路邊界勢(shì)場(chǎng)和障礙車勢(shì)場(chǎng)之和,如式(15)所示。
(14)
Uext=Uborder+Uobst
(15)
Uext的大小體現(xiàn)了道路風(fēng)險(xiǎn)的情況。由于障礙車的位置和狀態(tài)隨時(shí)間而變化,所以Uext大小也隨著時(shí)間而變化。圖4為初始時(shí)刻在引入引導(dǎo)勢(shì)場(chǎng)前后外界勢(shì)場(chǎng)圖的對(duì)比。
由向量微積分的相關(guān)知識(shí)可知,勢(shì)場(chǎng)作用于每一個(gè)節(jié)點(diǎn)產(chǎn)生的虛擬力可由對(duì)勢(shì)場(chǎng)進(jìn)行梯度運(yùn)算得到,即
(16)
Fobj,i= -iUobst,i=
(17)
(18)
根據(jù)彈性繩理論的假設(shè),作用于每一個(gè)節(jié)點(diǎn)的合力應(yīng)處于平衡狀態(tài),由此可得到方程式為
(19)
對(duì)于N個(gè)彈性繩節(jié)點(diǎn),由于首尾節(jié)點(diǎn)均已固定,坐標(biāo)已知,將式(19)分解到橫向和縱向兩個(gè)方向,從而產(chǎn)生2(N-2)個(gè)方程組成的聯(lián)立方程組,因此節(jié)點(diǎn)位置的求解需解此非線性方程組。為使計(jì)算更加簡(jiǎn)便,將節(jié)點(diǎn)的x方向位移進(jìn)行約束,因?yàn)樵谲囕v進(jìn)行車道保持、換道或緊急避障時(shí),節(jié)點(diǎn)的縱向位移并非必要,因此可以將其進(jìn)行約束[6],即
ri·ex=xi=i·Δx,i=1,2,…,N
式中:Δx為節(jié)點(diǎn)之間在縱向上的間隔。這樣,式(19)可以簡(jiǎn)化為
(20)
本文中采用基本Newton-Raphson算法,它是多維求根方法中最為簡(jiǎn)單的一種,編程簡(jiǎn)易,但該算法對(duì)方程表達(dá)式的要求較高,求解的表達(dá)式必須連續(xù)可微,并且其1階導(dǎo)數(shù)是連續(xù)的。該方法雖然不具有全局收斂性質(zhì),但是對(duì)于足夠好的初始值仍然有著明顯的收斂效果。
Newton-Raphson法是一種通過表達(dá)式的泰勒級(jí)數(shù)的1階近似,在方程根可能存在的區(qū)間上進(jìn)行迭代尋根的方法。一般而言方程可以用下式來描述:
F(z)=0,dimz=N
Fitotal=Fiext+Fiinc
選取一組初始值為
F(z)≈F(z(k))+J(z(k))(z-z(k))
(21)
式中:J為F的雅克比矩陣;k為迭代序號(hào)。將式(21)中F(z)=0的解記為z(k+1),因此有
z(k+1)=z(k)-J-1(z(k))F(z(k))
(22)
直到所得到的搜索步長(zhǎng)Δz(k)=z(k+1)-z(k)在預(yù)期的誤差范圍ε內(nèi)停止迭代。此外,由于道路的危險(xiǎn)勢(shì)場(chǎng)隨著時(shí)間的變化而變化,因此路徑規(guī)劃須以一定的時(shí)間間隔進(jìn)行更新。這樣上一次規(guī)劃的路徑可作為下一次路徑規(guī)劃的初始值,通過Newton-Raphson算法來進(jìn)一步修正得到某規(guī)劃時(shí)刻規(guī)劃的路徑值。表1給出Newton-Raphson算法的偽代碼。
表1 Newton-Raphson迭代法偽代碼
用Newton-Raphson算法解方程組時(shí)須獲得一組初始解,即須得到一組初始路徑。為此本文中采取一種簡(jiǎn)單的幾何方法來獲得。如圖5所示,獲取探測(cè)到障礙車邊界與離它較遠(yuǎn)的車道邊界的中點(diǎn),然后將起始點(diǎn)P0和這些中點(diǎn)與目標(biāo)點(diǎn)PN進(jìn)行線性插值得到對(duì)應(yīng)的插值函數(shù),然后彈性繩各節(jié)點(diǎn)的取值便依照該插值函數(shù)來取點(diǎn)。這樣就得到一組可用來迭代求解的初始值。
本文中模擬的障礙車設(shè)定為一長(zhǎng)為4m,寬為2m的矩形,此外主車通過車載激光雷達(dá)來獲得與前方障礙車的間隔距離。其最大探測(cè)距離可達(dá)150m,取決于激光功率和人眼安全規(guī)定;最小0.5m,取決于脈沖長(zhǎng)度。在防碰撞預(yù)警系統(tǒng)中,考慮到自車和前車的絕對(duì)速度和相對(duì)速度、路況、兩車完全停止時(shí)的實(shí)際車間距離等因素,一般要求傳感器的探測(cè)距離不小于130m,分辨率為1m,激光雷達(dá)滿足該要求[8]。但在智能車的行車環(huán)境中,雷達(dá)的測(cè)量值可能來源于目標(biāo)車輛,也可能來源于其他干擾物;此外由于雷達(dá)自身工作的不穩(wěn)定和目標(biāo)回波能量的分布不均,還可能出現(xiàn)虛假目標(biāo),同時(shí)隨著車輛行駛中的隨機(jī)顛簸和擺動(dòng),雷達(dá)測(cè)量信號(hào)可能出現(xiàn)短暫丟失,從而導(dǎo)致目標(biāo)物信息的較大波動(dòng)[9]。因此,須對(duì)雷達(dá)測(cè)量信號(hào)進(jìn)行處理,以準(zhǔn)確選取有效目標(biāo)。為了簡(jiǎn)便,假定主車可準(zhǔn)確地捕捉到前方運(yùn)動(dòng)障礙車,且在仿真場(chǎng)景的設(shè)定中主車與障礙車有足夠的安全距離,方便主車進(jìn)行避障操作。為驗(yàn)證改進(jìn)算法的有效性,分別就以下兩種情形進(jìn)行仿真分析。
在單向雙車道的直線高速公路上,車道寬度為3.5m,主車的初始位置為右車道中心線上,障礙車起始位置同樣在右車道中心線離主車前方60m處。主車速度為25m/s,障礙車速度為15m/s,均勻速行駛,如圖6所示。主車探測(cè)到前方出現(xiàn)的障礙車開始采取避障措施,繞過障礙后繼續(xù)前進(jìn)。利用Mathematica軟件編寫算法程序得到避障路徑,然后結(jié)合Carsim動(dòng)力學(xué)軟件將路徑導(dǎo)入進(jìn)行仿真,如圖7所示,觀察車輛在避障過程中的穩(wěn)定性。仿真參數(shù)設(shè)置如表2所示。
參數(shù)數(shù)值描述ηl,ηr/(N/m)30道路左右邊界勢(shì)場(chǎng)剛度b/m35單車道寬度βf,βr020前后引導(dǎo)勢(shì)場(chǎng)影響因子N166彈性繩節(jié)點(diǎn)數(shù)l/m250路徑規(guī)劃長(zhǎng)度l0/m12彈性繩自然長(zhǎng)度k/(N/m)350000彈性繩剛度kobst/(N/m)1000障礙車勢(shì)場(chǎng)剛度kr,kf/(N/m)1000前后引導(dǎo)勢(shì)場(chǎng)剛度ε/m0001Newton?Raphson法求解精度W/m2障礙車橫向?qū)挾萀/m4障礙車縱向長(zhǎng)度
避障路徑生成的動(dòng)態(tài)過程如圖8所示。仿真結(jié)果表明,在引入前后引導(dǎo)勢(shì)場(chǎng)后,規(guī)劃出的路徑改善明顯,具體表現(xiàn)在:障礙車勢(shì)場(chǎng)對(duì)主車的影響范圍變大,因此主車在避障開始駛?cè)肓硪卉嚨佬旭?,且避障時(shí)側(cè)向與障礙車保持著更為安全的距離。沒有加引導(dǎo)勢(shì)場(chǎng)的模型雖然也能“成功”避障,但是近乎與障礙車“擦身而過”,因而實(shí)際駕駛中非常容易導(dǎo)致與障礙車的碰撞,如圖7(b)所示。所以引導(dǎo)勢(shì)場(chǎng)的引入很好地避免了這類情況的發(fā)生,從而使主車在避障過程中更為安全。
圖9(a)給出了加入和不加入引導(dǎo)勢(shì)場(chǎng)這兩種情況下生成的避障路徑在橫向上距離之差的絕對(duì)值??梢钥闯觯诩尤胍龑?dǎo)勢(shì)場(chǎng)后,主車與障礙車的橫向距離比未加入引導(dǎo)勢(shì)場(chǎng)時(shí)更遠(yuǎn),其最大差值達(dá)到約1.4m,可見效果明顯。
此外,由圖9(b)給出的車輛在路徑跟隨過程中主車橫擺角速度隨時(shí)間變化的關(guān)系可知,主車在加入和未加入引導(dǎo)勢(shì)場(chǎng)在避障過程中都能夠保持車輛的穩(wěn)定前行,但加入引導(dǎo)勢(shì)場(chǎng)后,由于避障初始轉(zhuǎn)向角的增大,橫擺角速度也會(huì)增大,但是整個(gè)過程中相差并不太多,且沒有出現(xiàn)失穩(wěn)的情況。因此該方法的可行性和有效性得到驗(yàn)證。
彎道場(chǎng)景如圖10(a)所示,道路參數(shù)設(shè)置如表3所示。規(guī)劃初始時(shí)刻主車在右車道距離車道中心線2.5m處,以25m/s的速度勻速行駛。障礙車的初始位置為水平距離50m,與車道中心線相距2m的地方,并且以15m/s的速度勻速行駛。
從圖10(b)的仿真結(jié)果來看,兩種情形下都能夠安全成功避障,雖然加入引導(dǎo)勢(shì)場(chǎng)的效果與不加入引導(dǎo)勢(shì)場(chǎng)沒有直線車道情形下差別明顯,但在加入引導(dǎo)勢(shì)場(chǎng)后,主車的避障路徑在橫向上距離最大差值約為0.4m,見圖10(c)。
參數(shù)數(shù)值描述b/m35單車道寬度l/m200路徑規(guī)劃長(zhǎng)度x0/m-10彎道開始處曲率xf/m-1000125彎道末尾處曲率
在彎道情形中,由于彈性繩起始位置的差異,在沒有加入引導(dǎo)勢(shì)場(chǎng)下主車也能夠較早駛?cè)肓硪卉嚨溃遗c障礙車在縱向上保持足夠的安全距離,是否加入引導(dǎo)勢(shì)場(chǎng)的區(qū)別不大。在直線車道情形中,由于引導(dǎo)勢(shì)場(chǎng)的加入也是為使主車在縱向上與障礙車保持更為安全的距離,所以位置會(huì)離障礙車更遠(yuǎn)。由此可見引導(dǎo)勢(shì)場(chǎng)的加入對(duì)直線道路上的避障作用更為明顯。此外從圖10(d)中也可以觀察到在避障過程中主車橫擺角速度大小,可知主車在整個(gè)過程中狀態(tài)穩(wěn)定。
在人工勢(shì)場(chǎng)和彈性繩模型的基礎(chǔ)上,分別在障礙車前后添加引導(dǎo)勢(shì)場(chǎng)模型,實(shí)現(xiàn)了在低風(fēng)險(xiǎn)交通場(chǎng)景的緊急避障路徑規(guī)劃。仿真實(shí)驗(yàn)表明,改進(jìn)后的模型與改進(jìn)前相比,特別在直線道路情形下,會(huì)有更加安全的避障路徑。通過Carsim仿真模擬可知,主車在避障過程中可保持車輛的動(dòng)力學(xué)穩(wěn)定性。
[1] 宋曉琳,馮廣剛,楊濟(jì)匡.汽車主動(dòng)避撞系統(tǒng)的發(fā)展現(xiàn)狀及趨勢(shì)[J].汽車工程,2008,30(4):285-290.
[2] 中華人民共和國(guó)統(tǒng)計(jì)局.中國(guó)統(tǒng)計(jì)年鑒(2012年):交通事故統(tǒng)計(jì)情況(2011年)[DB/OL].http://www.stats.gov.cn/tjsj/ndsj/2012/indexch.htm,2012-09-19.
[3] SAE International. Driver Assistance Technology: What Really Matters[EB/OL].www.sae.org/mags/sve/11152/,2012-06-02.
[4] Quinlan S. Real-time Modification of Collision-free Path[D]. Stanford University, USA,1995.
[5] Hilgert J, Hirsch K, Bertram T, et al. Emergency Path Planning for Autonomous Vehicles Using Elastic Band Theory[C]. IEEE/ASME International Conference on Advanced Intelligent Mechatronics AIM 2003, Kobe, Japan,20 -24 July 2003.
[6] Sattel T Brandt. From Robotics to Automotive: Lane-keeping and Collision Avoidance Based on Elastic Bands[J]. Vehicle System Dynamics,2008,46(7):597-619.
[7] Song Xiaolin, Cao Haotian, Huang Jiang. Influencing Factors Research on Vehicle Path Planning Based on Elastic Bands for Collision Avoidance[J]. SAE International Journal of Passenger Cars- Electronic and Electrical Systems,2012,5(2):625-637.
[8] 李曉霞,江宗法,李百川,等.車載距離探測(cè)技術(shù)比較[J].長(zhǎng)安大學(xué)學(xué)報(bào)(自然科學(xué)版),2002,22(2):73-76.
[9] 劉志峰,王建強(qiáng),李克強(qiáng).具有魯棒特性的車載雷達(dá)有效目標(biāo)確定方法[J].清華大學(xué)學(xué)報(bào)(自然科學(xué)版),2008,48(5):875-878.