岳麗姣,時(shí) 利,孟建平
(1.安徽江淮汽車集團(tuán)股份有限公司 智能網(wǎng)聯(lián)汽車研究院,安徽 合肥 230601;2.合肥工業(yè)大學(xué) 汽車與交通工程學(xué)院,安徽 合肥 230009)
自動(dòng)換道控制作為自動(dòng)駕駛技術(shù)的核心功能模塊之一,已成為智能汽車領(lǐng)域的重點(diǎn)[1]。實(shí)現(xiàn)自動(dòng)換道控制的基礎(chǔ)是進(jìn)行換道軌跡規(guī)劃與軌跡跟蹤控制[2]。
為實(shí)現(xiàn)換道控制,一些學(xué)者使用AI*算法[3]、RRT[4]等基于搜索的方法,雖然這類方法可以在復(fù)雜的道路環(huán)境下規(guī)劃出無(wú)碰撞軌跡,但是沒(méi)有考慮汽車的非完整約束,可能導(dǎo)致汽車無(wú)法跟蹤規(guī)劃出的軌跡。為獲得局部最優(yōu)軌跡,Huang等[5]將人工勢(shì)場(chǎng)(APF)與MPC 相結(jié)合通過(guò)模型預(yù)測(cè)控制的優(yōu)化算法來(lái)優(yōu)化選擇人工勢(shì)場(chǎng)定義下的規(guī)劃軌跡。雖然該方法能實(shí)現(xiàn)換道避礙,但是由于道路勢(shì)場(chǎng)在兩條車道中心處最低,換道時(shí)參考量相當(dāng)于從當(dāng)前車道中心突變?yōu)槟繕?biāo)車道中心,從而導(dǎo)致側(cè)向加速度較大,舒適性較差。基于多項(xiàng)式的換道軌跡模型具有形式簡(jiǎn)單、曲率連續(xù)、可以靈活調(diào)整軌跡等優(yōu)點(diǎn)[6],能有效地解決上述問(wèn)題。如LIE 等[7]學(xué)者在無(wú)障礙物環(huán)境下利用多項(xiàng)式規(guī)劃一條車道變換軌跡,基于積分反步法設(shè)計(jì)軌跡跟蹤控制器,實(shí)現(xiàn)安全換道。Yang[8]等根據(jù)換道起點(diǎn)和終點(diǎn)的邊界條件結(jié)合考慮避障設(shè)置的約束,求取多項(xiàng)式參數(shù),獲得換道軌跡。大多基于多項(xiàng)式換道軌跡的研究中,軌跡模型雖然考慮了與周圍車輛避撞的問(wèn)題,但只是在換道初始時(shí)刻進(jìn)行了參數(shù)計(jì)算,不能避免整個(gè)換道過(guò)程中因周圍車輛狀態(tài)發(fā)生變化而造成的碰撞危險(xiǎn)[9]。為此,Chebly Alia 等[10]在基于多項(xiàng)式軌跡的基礎(chǔ)上,引入基于回旋曲線的軌跡簇,并從中篩選出一條無(wú)碰撞軌跡,但是局部軌跡是否合適取決于軌跡簇的待選軌跡條數(shù),軌跡條數(shù)越多,選出的軌跡越接近最優(yōu)軌跡,但待選軌跡條數(shù)的增加會(huì)導(dǎo)致計(jì)算量增大,換道系統(tǒng)的實(shí)時(shí)性變差。
為使換道車輛能有效的應(yīng)對(duì)前方車輛的狀態(tài)變化,本文提出了一種基于滾動(dòng)優(yōu)化的換道動(dòng)態(tài)軌跡規(guī)劃和軌跡跟蹤控制方法。所設(shè)計(jì)方法的結(jié)構(gòu)如圖1 所示,其主要由換道距離模型、換道參考軌跡模塊、動(dòng)態(tài)軌跡生產(chǎn)模塊以及模型預(yù)測(cè)軌跡跟蹤控制器組成。換道距離模型用于判斷車輛是否需要換道。換道參考軌跡模塊基于車-路信息,利用五次多項(xiàng)式的軌跡方程和由換道軌跡評(píng)價(jià)指標(biāo)確定的換道時(shí)間共同構(gòu)建出換道參考軌跡。動(dòng)態(tài)軌跡生產(chǎn)模塊以換道動(dòng)態(tài)軌跡規(guī)劃函數(shù)得到的動(dòng)態(tài)軌跡和參考軌跡的偏離最小化以及相鄰周期內(nèi)所規(guī)劃軌跡的偏差最小化為目標(biāo),設(shè)計(jì)滾動(dòng)優(yōu)化函數(shù),結(jié)合包絡(luò)橢圓約束、道路邊界約束和穩(wěn)定性約束,規(guī)劃出動(dòng)態(tài)換道軌跡。設(shè)計(jì)的模型預(yù)測(cè)軌跡跟蹤控制器以規(guī)劃的動(dòng)態(tài)軌跡為目標(biāo),計(jì)算理想的前輪轉(zhuǎn)角。最后由電機(jī)控制器控制車輛轉(zhuǎn)向。
圖1 換道控制方法的結(jié)構(gòu)
本文采用包含沿y 軸方向側(cè)向運(yùn)動(dòng)和繞z 軸橫擺運(yùn)動(dòng)的二自由度的車輛模型。車身二自由度模型如圖2 所示,其動(dòng)力學(xué)模型可表示為:
圖2 線性二自由度汽車模型
式中,β為車輛質(zhì)心側(cè)偏角;ω為橫擺角速度;m為整車質(zhì)量;Cf、Cr分別為前、后輪側(cè)偏角剛度之和;lf、lr分別為質(zhì)心到前后軸之間的距離;Iz為整車?yán)@z 軸的轉(zhuǎn)動(dòng)慣量;δf為前輪轉(zhuǎn)角。
在地面坐標(biāo)系下,汽車運(yùn)動(dòng)學(xué)微分方程如下:
式中:x、y分別為車輛縱向和側(cè)向坐標(biāo);φ為汽車航向角。在車輛穩(wěn)定狀態(tài)下,由二自由度汽車穩(wěn)態(tài)轉(zhuǎn)向模型得到車輛穩(wěn)態(tài)橫擺角速度:
當(dāng)同車道前方存在低速障礙車輛時(shí),為保證行車安全并提高通行效率,如果目標(biāo)車道有充足的換道空間,被控車輛與前方障礙車輛之間的距離達(dá)到換道請(qǐng)求距離時(shí),被控車輛產(chǎn)生換道需求,并進(jìn)行換道參考軌跡規(guī)劃。換道請(qǐng)求距離為:
其中,εv為道路法規(guī)規(guī)定的最小行車間距。如圖3 所示,為了保證換道過(guò)程中的安全性,換道結(jié)束時(shí)在縱向上與原車道前方障礙車輛仍留有一定的安全裕度值D3,則:
圖3 換道距離
式中,v為自車車速,vobs為前方障礙車輛車速,T1為換道時(shí)間。
以換道起始時(shí)汽車質(zhì)心為坐標(biāo)原點(diǎn),沿車身縱軸方向?yàn)閤軸,沿橫軸方向?yàn)閥軸。要求規(guī)劃出的軌跡能保證車輛側(cè)向速度和側(cè)向加速度連續(xù),由換道起止點(diǎn)的6 個(gè)邊界條件,可以確定多項(xiàng)式軌跡的6 個(gè)系數(shù),所以本文采用5 次多項(xiàng)式來(lái)描述側(cè)向位移軌跡:
換道的起始和終止時(shí)刻汽車側(cè)向位移、側(cè)向速度、側(cè)向加速度分別為:
其中,H為車道寬度。結(jié)合公式(7)中邊界條件,可求得換道參考軌跡的系數(shù):
將所求系數(shù)代入到公式(9)中并對(duì)其進(jìn)行二階求導(dǎo),可得到換道過(guò)程中的側(cè)向加速度,即:
則側(cè)向加速度最大值和最小值對(duì)應(yīng)的時(shí)刻為:
為保證汽車在跟蹤換道參考軌跡過(guò)程中的穩(wěn)定性,對(duì)側(cè)向加速度進(jìn)行約束,側(cè)向加速度滿足約束條件:
聯(lián)合公式(13)-(15),可求得換道時(shí)間T1應(yīng)滿足:
本文中取ay,max=-ay,min,=-ay,min=0.4g,即可求得最小換道時(shí)間T1min=2.35s。將T1min代入公式(8),求得滿足側(cè)向加速度約束的最小換道安全距離Smin。
換道時(shí)間越短,最大側(cè)向加速度越大,換道時(shí)的乘坐舒適性變差;但換道時(shí)間越短,換道效率越高。為了綜合行車效率和換道乘坐舒適性,構(gòu)建換道軌跡評(píng)價(jià)指標(biāo):
換道軌跡評(píng)價(jià)指標(biāo)隨換道時(shí)間的變化曲線如圖4 所示,本文選擇換道時(shí)間為T1=3.6s。
圖4 換道軌跡評(píng)價(jià)指標(biāo)
2.3.1 換道動(dòng)態(tài)軌跡規(guī)劃函數(shù)
為了保證換道過(guò)程中側(cè)向加速度的連續(xù)性,較好的擬合出換道軌跡,結(jié)合當(dāng)前采樣時(shí)刻三個(gè)狀態(tài),要求動(dòng)態(tài)軌跡多項(xiàng)式的階數(shù)n≥3。階數(shù)越高,擬合精度越好,但隨階數(shù)的增加,待優(yōu)化參數(shù)數(shù)量增加,實(shí)時(shí)性變差。綜合考慮擬合精度和實(shí)時(shí)性要求,本文采用四次多項(xiàng)式來(lái)表示動(dòng)態(tài)換道軌跡:
當(dāng)前采樣時(shí)刻t0,結(jié)合汽車運(yùn)動(dòng)方程(5),則當(dāng)前被控車輛橫向行駛狀態(tài)為:
即可得到:
其中:
則動(dòng)態(tài)換道軌跡為:
式中,f(t)=[1t t2],g(t)=[t3t4]。即可將u作為優(yōu)化變量,實(shí)現(xiàn)對(duì)軌跡的優(yōu)化。
2.3.2 滾動(dòng)優(yōu)化目標(biāo)函數(shù)設(shè)計(jì)
為使動(dòng)態(tài)規(guī)劃的軌跡更接近換道參考軌跡,以保證汽車在換道過(guò)程中的舒適性和效率,對(duì)規(guī)劃時(shí)域T內(nèi)動(dòng)態(tài)軌跡與參考軌跡差值的平方進(jìn)行積分,從而獲得與參考軌跡偏差的代價(jià)函數(shù):
動(dòng)態(tài)換道軌跡與參考軌跡偏差越大,其代價(jià)值越高。但僅以上述代價(jià)函數(shù)作為優(yōu)化目標(biāo)函數(shù),則可能出現(xiàn)相鄰兩次規(guī)劃周期內(nèi)所規(guī)劃的軌跡出現(xiàn)較大偏差,從而造成軌跡跟蹤時(shí)方向盤出現(xiàn)較大抖動(dòng),為避免上述問(wèn)題,對(duì)本次規(guī)劃周期內(nèi)規(guī)劃的軌跡與上個(gè)規(guī)劃周期內(nèi)規(guī)劃的軌跡偏差的平方進(jìn)行積分,從而減小相鄰規(guī)劃周期內(nèi)軌跡之間的偏差:
其中:ypref(t)為上個(gè)規(guī)劃周期內(nèi)規(guī)劃出的軌跡。最終得到優(yōu)化目標(biāo)函數(shù):
并可以離線求出滾動(dòng)優(yōu)化函數(shù):
其中,ξ為與u無(wú)關(guān)的參數(shù)。
2.3.3 障礙車隨動(dòng)危險(xiǎn)區(qū)域包絡(luò)橢圓
實(shí)際情況下汽車行駛狀態(tài)是連續(xù)變化的,短時(shí)間內(nèi)不存在突變?;诖祟A(yù)測(cè)未來(lái)一段時(shí)間T內(nèi)障礙車位置:
被控汽車在時(shí)間T內(nèi)的縱向坐標(biāo)為:
式中,x0為當(dāng)前時(shí)刻被控車輛縱向坐標(biāo)。以障礙車位置為橢圓圓心,以最小換道安全距離Smin為長(zhǎng)半軸,對(duì)障礙車構(gòu)建跟隨其運(yùn)動(dòng)的危險(xiǎn)區(qū)域包絡(luò)橢圓,其邊界為:
式中,Sy為橫向安全距離。
2.3.4 約束條件
為防止與前方障礙車發(fā)生碰撞,將動(dòng)態(tài)規(guī)劃出的軌跡約束在前車隨動(dòng)危險(xiǎn)包絡(luò)橢圓范圍之外,即當(dāng)|x(t)-xobs(t)|<Smin時(shí)有:
假設(shè)被控車輛可以理想的跟蹤動(dòng)態(tài)規(guī)劃的軌跡,則換道過(guò)程中的側(cè)向加速度為:
為規(guī)避跟隨軌跡過(guò)程中因側(cè)向加速度過(guò)大而造成的失穩(wěn)可能,對(duì)汽車?yán)硐敫S軌跡過(guò)程中的側(cè)向加速度進(jìn)行約束,約束范圍為:
為避免汽車駛出道路,將動(dòng)態(tài)規(guī)劃軌跡約束在道路安全區(qū)域范圍內(nèi),防止被控車輛駛出車道邊界或進(jìn)入對(duì)向車道。
式中w為被控車輛寬度。將公式(18)分別代入以上約束公式中,即可求得對(duì)u的不等式約束,通過(guò)求解帶約束二次規(guī)劃問(wèn)題,即可求得優(yōu)化變量u,并將其代入公式(21),得到優(yōu)化后的未來(lái)一段時(shí)間T內(nèi)最優(yōu)軌跡。
為準(zhǔn)確的跟蹤規(guī)劃出的動(dòng)態(tài)軌跡,設(shè)計(jì)基于模型預(yù)測(cè)的軌跡跟蹤控制器來(lái)獲得理想的前輪轉(zhuǎn)角。結(jié)合公式(4)、(5),以X=[y φ ω β]T為系統(tǒng)狀態(tài)量,以前輪轉(zhuǎn)角δf為控制量uc,可得離散化后的線性時(shí)變系統(tǒng)狀態(tài)方程:
為保證控制量的連續(xù)性,防止控制量突變,需要對(duì)控制量增量進(jìn)行約束,為方便處理,采用控制增量代替原控制量作為系統(tǒng)控制輸入。并對(duì)系統(tǒng)狀態(tài)量進(jìn)行如下轉(zhuǎn)換:
轉(zhuǎn)換后新的系統(tǒng)控制輸入為:
得到新的離散線性化狀態(tài)方程:
其中,η(k|t)=Y(k|t)。設(shè)置控制器預(yù)測(cè)時(shí)域Np=8,控制時(shí)域Nc=5。
系統(tǒng)控制增量是未知的,通過(guò)設(shè)定適當(dāng)?shù)膬?yōu)化目標(biāo)函數(shù),對(duì)其進(jìn)行優(yōu)化求解,才能得到控制時(shí)域內(nèi)的控制序列,并將控制序列中第一個(gè)元素作為實(shí)際控制增量作用于系統(tǒng),實(shí)現(xiàn)車輛軌跡跟蹤控制。根據(jù)參考文獻(xiàn)[5],構(gòu)建以下形式的優(yōu)化目標(biāo)函數(shù):
其中,Q 和R分別為權(quán)重矩陣。第一項(xiàng)反應(yīng)軌跡跟蹤能力;第二項(xiàng)反應(yīng)對(duì)控制量平穩(wěn)變化的要求;第三項(xiàng)為在目標(biāo)函數(shù)中加入的松弛因子,防止無(wú)可行解的情況??紤]對(duì)控制量和控制增量的約束,控制量即前輪轉(zhuǎn)角應(yīng)被約束在機(jī)械結(jié)構(gòu)所允許的最大范圍之內(nèi),則有:
根據(jù)參考文獻(xiàn)[10]并結(jié)合本文離散化周期,取△ucmax=-△ucmin=1.7deg。
為準(zhǔn)確地評(píng)估所設(shè)計(jì)的換道控制方法的有效性,進(jìn)行CarSim/Simulink 硬件在環(huán)試驗(yàn)。根據(jù)設(shè)計(jì)的換道控制方法,進(jìn)行前車緊急制動(dòng)三種情況下的車輛換道硬件試驗(yàn)。采用基于LabVIEW/CarSim 聯(lián)合的自動(dòng)駕駛試驗(yàn)平臺(tái)進(jìn)行車輛換道的硬件在環(huán)試驗(yàn)研究。硬件在環(huán)試驗(yàn)臺(tái)如圖5 所示。
圖5 硬件在環(huán)試驗(yàn)平臺(tái)(換成LAC 的試驗(yàn)臺(tái))
試驗(yàn)中使用轉(zhuǎn)向臺(tái)架的實(shí)際轉(zhuǎn)角代替計(jì)算到的期望方向盤轉(zhuǎn)角,設(shè)置被控車輛車速為80 km/h、路面附著系數(shù)為0.85、仿真時(shí)間為6 s。在前方障礙車輛緊急制動(dòng)情況下進(jìn)行換道試驗(yàn)。
從圖6(a)、(b)和(d)中可以看出,有動(dòng)態(tài)軌跡規(guī)劃的換道過(guò)程相較于無(wú)動(dòng)態(tài)軌跡規(guī)劃的換道過(guò)程,由于受到前方障礙車輛的橢圓危險(xiǎn)包絡(luò)橢圓約束,方向盤轉(zhuǎn)角明顯增大,車輛實(shí)際軌跡較大的偏離了換道參考軌跡,從而避免換道過(guò)程中自車與前車發(fā)生碰撞。從圖6(c)中可以看出,在所設(shè)計(jì)的軌跡跟蹤控制器的作用下,被控車輛的軌跡誤差在換道過(guò)程中均保持在較小的范圍內(nèi)。另外,由圖6(e)可知,換道過(guò)程中車輛的側(cè)向加速度基本保持在約束的范圍內(nèi),且變化平穩(wěn)。
圖6 硬件在環(huán)試驗(yàn)結(jié)果
從硬件在環(huán)試驗(yàn)的結(jié)果分析可知,在基于滾動(dòng)優(yōu)化的汽車換道動(dòng)態(tài)軌跡規(guī)劃和軌跡跟蹤控制方法的作用下車輛具有良好的應(yīng)對(duì)不同換道環(huán)境的能力。
(1)針對(duì)換道過(guò)程中前車狀態(tài)變化問(wèn)題,本文在規(guī)劃出換道參考軌跡的基礎(chǔ)上,基于滾動(dòng)優(yōu)化思想,設(shè)計(jì)了以規(guī)劃軌跡和參考軌跡偏差最小化為目標(biāo)的動(dòng)態(tài)軌跡規(guī)劃方法,并基于模型預(yù)測(cè)控制設(shè)計(jì)了軌跡跟蹤控制器。
(2)對(duì)所設(shè)計(jì)的換道控制方法在前車緊急制動(dòng)工況下在基于CarSim/labview 的自動(dòng)駕駛試驗(yàn)平臺(tái)進(jìn)行了硬件在環(huán)試驗(yàn)。硬件在環(huán)試驗(yàn)結(jié)果表明,本文設(shè)計(jì)的基于滾動(dòng)優(yōu)化的汽車換道動(dòng)態(tài)軌跡規(guī)劃和軌跡跟蹤控制方法能使車輛有效的應(yīng)對(duì)不同換道環(huán)境,在前車狀態(tài)發(fā)生改變的情況下,能夠保證車輛的換道安全性和舒適性。