花曉峰,段建民,田曉生
北京工業(yè)大學(xué) 城市交通學(xué)院,北京 100124
對(duì)于無人駕駛智能車輛的安全行車而言,障礙躲避是車輛行駛控制的核心任務(wù)。由于智能車輛只能在道路上行駛移動(dòng),而且路線也是嚴(yán)格受限的,遇到的只有車輛、行人等特定的規(guī)則障礙,因此移動(dòng)式機(jī)器人避障的算法思維并不能完全應(yīng)用于車輛避障。由于車輛系統(tǒng)結(jié)構(gòu)復(fù)雜,存在高度的非線性時(shí)變特性,因此有效地進(jìn)行系統(tǒng)建模,選取快速、可靠的控制策略是車輛控制的重要基礎(chǔ)。
模型預(yù)測控制(Model Predictive Control,MPC)是針對(duì)被控對(duì)象的線性化模型求解未來時(shí)刻的有約束最優(yōu)解,并通過反饋機(jī)制更新系統(tǒng)狀態(tài)來達(dá)到對(duì)系統(tǒng)實(shí)時(shí)在線控制的目的。預(yù)測控制以預(yù)測模型為基礎(chǔ),理論上不受系統(tǒng)的時(shí)變與非時(shí)變、線性與非線性、有無時(shí)滯的影響,在復(fù)雜系統(tǒng)控制方面取得了很大成功[1-2]。在智能車避障控制應(yīng)用方面,文獻(xiàn)[3]提出了基于人類駕駛行為的預(yù)測避障方案,但在避障環(huán)節(jié)采用了預(yù)瞄點(diǎn)搜索、MPC控制、PID控制多重模塊,導(dǎo)致避障系統(tǒng)過于復(fù)雜;文獻(xiàn)[4-5]分別從受控自回歸積分滑動(dòng)平均模型和模擬退火算法求解目標(biāo)函數(shù)方面闡述了軌跡跟蹤控制方案,但均把重點(diǎn)放在了單純的車輛軌跡規(guī)劃層面,對(duì)于以障礙檢測、障礙預(yù)測為基礎(chǔ)的軌跡重規(guī)劃方面內(nèi)容較少涉及;文獻(xiàn)[6]采用了預(yù)測觸覺反饋(Predictive Haptic Feedback)的思路實(shí)現(xiàn)了障礙躲避,但方案只完成了換道,并沒有進(jìn)行避障換道之后的返回原車道實(shí)驗(yàn);文獻(xiàn)[7-10]通過模型預(yù)測與不同算法融合闡述了避障策略,但并未考慮車輛的長寬尺寸,亦無法判定車體是否能安全地避免與障礙發(fā)生碰撞。真實(shí)的行車環(huán)境中,障礙物和車體的尺寸都不可忽略,因此即便將車體抽象為質(zhì)點(diǎn),也必須要考慮其與障礙拉開足夠距離的問題。因此障礙相對(duì)于車輛質(zhì)心而言必然會(huì)存在一個(gè)“禁區(qū)范圍”。
針對(duì)以上問題,本文以模型預(yù)測控制算法和車輛動(dòng)力學(xué)模型為基礎(chǔ),以前輪轉(zhuǎn)向車輛為實(shí)驗(yàn)對(duì)象,對(duì)障礙物邊界點(diǎn)的確定引入以階躍函數(shù)為基礎(chǔ)的禁區(qū)邊界懲罰函數(shù),禁區(qū)邊界長度以車輛尺寸為依據(jù),以保證車身不與障礙邊界點(diǎn)觸碰;在避障路徑重規(guī)劃環(huán)節(jié)引入預(yù)測距離倍增的方案,以彌補(bǔ)因預(yù)測距離過短導(dǎo)致無法在遠(yuǎn)距離處檢測到障礙物造成避障失敗的缺陷。
車輛運(yùn)動(dòng)過程中存在多個(gè)狀態(tài)變量,因此需要建立復(fù)雜的動(dòng)力學(xué)微分方程組。假設(shè)車輛是只可作橫向、縱向、橫擺運(yùn)動(dòng)的三自由度前輪驅(qū)動(dòng)車輛,則可等效為二輪自行車單軌模型。分別建立相對(duì)地面的世界坐標(biāo)系OXYZ和相對(duì)車輛自身的車體坐標(biāo)系oxyz,如圖1,兩坐標(biāo)系以坐標(biāo)軸字母大小寫進(jìn)行區(qū)分。車體系原點(diǎn)位于車輛質(zhì)心,x軸方向?yàn)檐圀w縱向,y軸沿車體橫向與x軸垂直,z軸方向由右手系法則確定,橫擺角φ是車體系x軸與世界坐標(biāo)系X軸的夾角,其逆時(shí)針方向?yàn)檎较颉?/p>
圖1 二輪單軌前輪驅(qū)動(dòng)式車輛動(dòng)力學(xué)模型
根據(jù)運(yùn)動(dòng)學(xué)坐標(biāo)轉(zhuǎn)換和牛頓第二定律可得受力平衡方程:Flf、Fcf分別為施加給前輪的縱、橫向力,F(xiàn)lr、Fcr分別為施加給后輪的縱、橫向力。前輪驅(qū)動(dòng)車輛的后輪不能轉(zhuǎn)向,故后輪偏角恒為0,即δr≡0。前輪的合速度即為車輛的輸入速度,即 vin=x˙2+y˙2,恒定速度時(shí) vin為常數(shù)。在側(cè)偏角較小時(shí),動(dòng)力學(xué)模型可進(jìn)行線性近似化,由無窮小定理和輪胎線性模型,可將式(1)轉(zhuǎn)化為前輪小偏角和線性化輪胎模型下的車輛動(dòng)力學(xué)非線性模型[11-12]:
m為車體質(zhì)量;Ccf、Ccr為前、后輪側(cè)偏剛度;Clf、Clr為前、后輪縱向剛度;a、b為質(zhì)心的前、后車軸距離;sf、sr為前、后輪縱向滑移率;Iz為車體關(guān)于z軸的轉(zhuǎn)動(dòng)慣量。角度單位全部為弧度。選取狀態(tài)量ξ為(˙˙φ φ˙YX)T,控制量 u 為 δf,輸出量 η=Cξ=(φ Y)T,式(2)可以寫為由ξ、u、η組成的非線性函數(shù)狀態(tài)空間式:
2.2.1 狀態(tài)方程
考慮無人駕駛車輛對(duì)控制器的實(shí)時(shí)性要求,首先需要對(duì)模型進(jìn)行線性化。采用近似線性化思想,將式(3)寫為標(biāo)準(zhǔn)線性模型式:
其中Ad(k)=I+TA,Bd(k)=TB。
式(5)出現(xiàn)了ξ(k+1),它表示的是以當(dāng)前k時(shí)刻為基準(zhǔn)未來下一時(shí)刻的狀態(tài)。模型預(yù)測的思想便以此為基礎(chǔ),通過遞推法逐步推導(dǎo)出預(yù)測時(shí)域內(nèi)未來某一時(shí)刻的狀態(tài)。
2.2.2 目標(biāo)函數(shù)
目標(biāo)函數(shù)需要保證車輛快速平穩(wěn)跟蹤規(guī)劃的期望軌跡,優(yōu)化對(duì)象為系統(tǒng)狀態(tài)量和控制量。采用偏差式目標(biāo)函數(shù)以消除靜態(tài)誤差,即[13]:
定義狀態(tài)偏差量:
可得:
得最終的廣義預(yù)測輸出表達(dá)式:
2.2.3 反饋環(huán)節(jié)
每一控制周期內(nèi)對(duì)式(10)求解完成后得到控制時(shí)域內(nèi)的一個(gè)控制輸入序列ΔU*:
根據(jù)反饋原理將第一元素作為控制輸入增量作用于系統(tǒng),即:
進(jìn)入下一控制周期后重復(fù)上述優(yōu)化過程,再次求得下一控制周期的最優(yōu)控制輸入增量。
障礙物點(diǎn)陣信息通常由雷達(dá)通過掃描障礙物得出,懲罰函數(shù)可以單獨(dú)針對(duì)障礙物點(diǎn)進(jìn)行設(shè)計(jì)。懲罰函數(shù)是通過與障礙物點(diǎn)之間的距離函數(shù)來調(diào)節(jié)避障優(yōu)化函數(shù)值。懲罰函數(shù)可采用關(guān)于距離的反比例函數(shù)實(shí)現(xiàn),如式(12)[12]:
k為預(yù)測時(shí)域內(nèi)的某一時(shí)刻,(x0,y0)是該時(shí)刻車輛質(zhì)心的坐標(biāo),(xi,yi)是第i個(gè)障礙物點(diǎn)的坐標(biāo)(均在世界坐標(biāo)系下),ε>0是一個(gè)防止分母為0的保護(hù)常數(shù),vk是當(dāng)前時(shí)刻的合速度,S是權(quán)重系數(shù)。
該方法的缺陷首先是對(duì)于障礙邊界的劃分較為模糊,其次是并且未能考慮車身尺寸。在避障預(yù)測時(shí)會(huì)出現(xiàn)無法正確確定障礙邊界、甚至規(guī)劃路徑穿過障礙物的錯(cuò)誤結(jié)果;即使規(guī)劃路徑能夠避開障礙物,也有可能因路徑過于靠近障礙物而導(dǎo)致車身與之發(fā)生碰撞。因此本文提出一種基于階躍函數(shù)的禁區(qū)邊界式懲罰函數(shù)策略,即:
式中M是一個(gè)充分大的正數(shù),R*是臨界(禁區(qū))半徑。車輛質(zhì)心位于車體幾何中心。令R*等于車身外接圓半徑,相應(yīng)地,每一個(gè)障礙物點(diǎn)都可以認(rèn)為存在一個(gè)以自身為圓心,半徑為R*的禁區(qū)圓。預(yù)測時(shí)域內(nèi)某一時(shí)刻得出的車輛質(zhì)心坐標(biāo)如果進(jìn)入了障礙物點(diǎn)禁區(qū)圓內(nèi),由于M的存在,會(huì)導(dǎo)致規(guī)劃目標(biāo)函數(shù)值急劇增大,迫使預(yù)測控制器重新搜索最優(yōu)控制量,如圖2。
圖2 障礙點(diǎn)與預(yù)測時(shí)域內(nèi)不同時(shí)刻的車輛位置的關(guān)系
由于軌跡重規(guī)劃環(huán)節(jié)計(jì)算量較大,為了簡化計(jì)算,規(guī)劃層采用運(yùn)動(dòng)學(xué)模型,可以更好地兼顧控制性能和計(jì)算速度[14]。在規(guī)劃層中,設(shè)狀態(tài)變量ξ 為(y ˙ x˙φ YX)T,離散化后有:
某一預(yù)測時(shí)刻k下得到的k+1時(shí)刻狀態(tài)變量為:
由于車輛輸入速度為恒定,因此在任意的k時(shí)刻,vin,d(k)為常數(shù),前輪偏角δf即為控制量。由于控制時(shí)域過長會(huì)導(dǎo)致MPC控制不穩(wěn)定,因此控制時(shí)域Nc應(yīng)盡可能短,這里取Nc=1,則令U=()δf,η(k)=[]Y(k),局部參考路徑ηr(k)=[ ]Yr(k),可得軌跡重規(guī)劃的目標(biāo)函數(shù):N為障礙物點(diǎn)的個(gè)數(shù)。式中的η、U、Q、R與第2章中的不同,它們是屬于規(guī)劃層環(huán)節(jié),并不用于車輛的運(yùn)動(dòng)控制策略。式中:
因此可知輸出量η是關(guān)于控制量U=(δf)的函數(shù)。由于控制時(shí)域僅為1,因此當(dāng)時(shí)刻i≥1時(shí),控制量的變化量恒為0,即 ΔU(k+i)≡0,i=1,2,…,Np-1,因此預(yù)測時(shí)域內(nèi)有U(k+i)≡U(k)≡(δf)。優(yōu)化完成后得到車輛在當(dāng)前位置點(diǎn)的最優(yōu)前輪偏角量,按照MPC反饋的定義,直接代入式(15),更新狀態(tài)量即可得到新時(shí)刻的車輛各個(gè)狀態(tài)值。
傳統(tǒng)的障礙檢測是將障礙物視作點(diǎn)障礙,并以該點(diǎn)構(gòu)造基于距離的反比例函數(shù)作為懲罰函數(shù)。規(guī)劃層采樣時(shí)間短,且預(yù)測時(shí)域一般不能過長。仿真實(shí)驗(yàn)觀測表明,車輛在某一時(shí)刻實(shí)際預(yù)測距離很短,對(duì)于點(diǎn)障礙物不考慮尺寸則可以完成路徑的避障規(guī)劃。而對(duì)于長寬不可忽略的大障礙物,其尺寸明顯大于所預(yù)測距離,使用同樣的方法會(huì)導(dǎo)致車輛較遠(yuǎn)時(shí)根本無法探測到障礙物。本文采用另一種策略,在規(guī)劃層進(jìn)行優(yōu)化期間,對(duì)采樣周期進(jìn)行倍數(shù)擴(kuò)增,例如3倍或5倍,然后再將優(yōu)化得到的結(jié)果直接用于狀態(tài)更新,狀態(tài)更新過程中不使用周期倍增,這樣便可使車輛在較遠(yuǎn)位置即能探測到障礙物,如圖3。
圖3 預(yù)測路徑與障礙物的關(guān)系
設(shè)擴(kuò)增倍數(shù)為W,則式(17)中η改寫為:
同理障礙懲罰函數(shù)中的X(k+i)和Y(k+i)分別為X(k+i)+TW[xd(k+i)cosφ(k+i)-yd(k+i)sinφ(k+i)]和 Y(k+i)+TW[xd(k+i)sinφ(k+i)+yd(k+i)cosφ(k+i)]。
這樣優(yōu)化過程中的預(yù)測位移X和Y相應(yīng)地被擴(kuò)大,完成后得到最優(yōu)前輪偏角量記為δ~*f,代入式(15)更新狀態(tài),在更新狀態(tài)時(shí)未使用擴(kuò)增倍數(shù)為W。通過這種手段便能使車輛在距離障礙物較遠(yuǎn)位置即可提前預(yù)知障礙物的存在從而及時(shí)調(diào)整當(dāng)前狀態(tài),為后續(xù)能夠得到正確的規(guī)劃結(jié)果奠定基礎(chǔ)。
軌跡重規(guī)劃得到的是一系列散點(diǎn),由于控制層輸入端口數(shù)量有限,顯然不能將離散點(diǎn)直接輸入控制層。曲線擬合是一種主要的替代處理方式,由于車輛的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)存在一階、二階特性以及連續(xù)特性,因此采用三次多項(xiàng)式作為擬合曲線,其表達(dá)式為[12]:
縱向位移與橫擺角擬合多項(xiàng)式的各項(xiàng)參數(shù)向量(a b c d)和(w x y z)將作為軌跡重規(guī)劃的輸出值。
選用CarSim車輛動(dòng)力學(xué)作為仿真平臺(tái),利用Matlab/Simulink搭建控制系統(tǒng)并通過S函數(shù)編寫控制策略,系統(tǒng)框圖如圖4。
CarSim輸入4個(gè)變量,依次為左、右前輪偏角和左、右后輪偏角;輸出6個(gè)變量依次為車體系橫向速度、車體系縱向速度、橫擺角、橫擺角速度、世界坐標(biāo)系下的縱向位移、世界坐標(biāo)系下的橫向位移,路面摩擦系數(shù)設(shè)定為0.85。
實(shí)驗(yàn)中采用雙移線軌跡作為跟蹤期望路徑[15]。模型預(yù)測控制需要添加相應(yīng)約束,控制量只有前輪偏角δf,設(shè)車輛方向盤在打死狀態(tài)時(shí)前輪偏角最大值為30°,前輪偏角變化量Δδf最大值設(shè)置為每采樣周期0.85°。因此控制量及控制量增量約束分別為:
(1)單一障礙物 I:障礙物中點(diǎn)坐標(biāo)(32.5,0);車輛速度為5 m/s(18 km/h);Np,pl=19,Nc,pl=1,Tpl=0.05 s;Np,ctrl=19,Nc,ctrl=1,Tctrl=0.03 s;
(2)單一障礙物Ⅱ:障礙物中點(diǎn)坐標(biāo)(32.5,2);車輛速度為 16.67 m/s(60 km/h);Np,pl=19,Nc,pl=1,Tpl=0.02 s;Np,ctrl=22,Nc,ctrl=1,Tctrl=0.095 s;
(3)雙障礙物:障礙物1中點(diǎn)坐標(biāo)(32.5,0),障礙物2中點(diǎn)坐標(biāo) (72.5,1);車輛速度為 10 m/s(36 km/h),Np,pl=19,Nc,pl=1,Tpl=0.02 s;Np,ctrl=25,Nc,ctrl=1,Tctrl=0.03 s。
圖4 系統(tǒng)框圖
黑色曲線為參考軌跡。實(shí)驗(yàn)每隔一定時(shí)間以矩形框顯示某一位置處的車體輪廓;藍(lán)色星號(hào)軌跡為傳統(tǒng)方法即使用反比例懲罰函數(shù)及不加預(yù)測倍增方法得到的實(shí)驗(yàn)結(jié)果;紅點(diǎn)軌跡為使用本文方法得到的結(jié)果。在縱坐標(biāo)位移圖中,本文方法得到的每一處實(shí)際位置點(diǎn)上均以一系列綠色須狀線條顯示不同種規(guī)劃方案。橫擺角曲線中的黑色曲線表示參考軌跡上對(duì)應(yīng)橫坐標(biāo)處的車體期望橫擺角。前輪偏角及變化量曲線使用藍(lán)色虛線劃定其約束的上下限。
工況1(單一障礙物 I),如圖5~8。
圖5 車輛行駛軌跡曲線
圖6 障礙物附近的局部放大圖
圖7 車輛橫擺角曲線
工況2(單一障礙物Ⅱ),如圖9~12。
工況3(雙障礙物),如圖13~16。
圖8 前輪偏角及變化量曲線
圖9 車輛行駛軌跡曲線
圖10 障礙物附近的局部放大圖
圖11 車輛橫擺角曲線
圖12 前輪偏角及變化量曲線
圖13 車輛行駛軌跡曲線
圖14 障礙物附近的局部放大圖
圖15 車輛橫擺角曲線
圖16 前輪偏角及變化量曲線
三種工況下分別繪制出車輛軌跡、橫擺角及前輪偏角曲線。與傳統(tǒng)方法進(jìn)行對(duì)比,可以發(fā)現(xiàn)通過本文方法車輛能夠完成避障,車輛橫擺角和前輪偏角均收斂于0°,且前輪偏角及前輪偏角變化量的值均在約束范圍內(nèi)。對(duì)于傳統(tǒng)方法,圖5和圖13可以看到工況1和3下車輛質(zhì)心能避開障礙;圖7和圖15的車輛橫擺角都能收斂于0°;圖8和圖16的前輪偏角亦收斂于0°且在約束范圍內(nèi);但是障礙鄰近放大圖(圖6和圖14)可見車輛質(zhì)心過于深入障礙物點(diǎn)禁區(qū)范圍,這樣將導(dǎo)致車身擦碰障礙物;圖9和圖10可以看到工況2傳統(tǒng)方法下車輛質(zhì)心觸碰障礙物,避障失敗,而且從橫擺角圖(圖11)、前輪偏角圖(圖12)可以發(fā)現(xiàn)橫擺角和前輪偏角都無法收斂,使得車身圍繞參考軌跡不斷搖擺。另外,在各個(gè)仿真工況下的障礙物附近區(qū)域局部放大圖中可以發(fā)現(xiàn),新方法車輛質(zhì)心軌跡由于預(yù)測規(guī)劃誤差的緣故略微擦過了障礙物的最外層禁區(qū)邊沿,但是通過車輛輪廓線可以發(fā)現(xiàn)車身已經(jīng)遠(yuǎn)離障礙區(qū),說明避障成功,因此軌跡的微小越界是可以接受的。
本文提出了劃定障礙物點(diǎn)禁區(qū)邊界的方案,使用分段函數(shù)法進(jìn)行障礙物點(diǎn)的禁區(qū)邊界劃定,解決了關(guān)于距離的反比例懲罰函數(shù)對(duì)于障礙邊界劃定不清的問題;在規(guī)劃層中進(jìn)行軌跡重規(guī)劃時(shí),采用了預(yù)測周期倍增法,將倍增后得到的預(yù)測位置點(diǎn)應(yīng)用于未施加倍增的運(yùn)動(dòng)學(xué)方程中進(jìn)行狀態(tài)更新,從而使得車輛在較遠(yuǎn)位置即可提前預(yù)知大尺寸障礙物,及時(shí)做出轉(zhuǎn)向調(diào)整;對(duì)不同工況進(jìn)行仿真測試,并與傳統(tǒng)方法進(jìn)行比對(duì),結(jié)果滿足預(yù)期要求,驗(yàn)證了算法的可行性。