基于禁區(qū)懲罰函數(shù)和MPC倍增預(yù)測的車輛避障研究

花曉峰，段建民，田曉生

北京工業(yè)大學(xué) 城市交通學(xué)院，北京 100124

1 引言

對(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)距離處檢測到障礙物造成避障失敗的缺陷。

2 預(yù)測模型

2.1 車輛動(dòng)力學(xué)建模

車輛運(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 車輛預(yù)測模型

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)控制輸入增量。

3 避障規(guī)劃

3.1 懲罰函數(shù)

障礙物點(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)值。

3.2 障礙檢測

傳統(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ǔ)。

3.3 軌跡擬合

軌跡重規(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ī)劃的輸出值。

4 實(shí)驗(yàn)方案設(shè)計(jì)

4.1 實(shí)驗(yàn)平臺(tái)

選用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。

4.2 實(shí)驗(yàn)參數(shù)與工況設(shè)定

實(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)框圖

5 結(jié)果與分析

5.1 實(shí)驗(yàn)圖像

黑色曲線為參考軌跡。實(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 車輛橫擺角曲線

5.2 結(jié)果分析

圖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ū)，說明避障成功，因此軌跡的微小越界是可以接受的。

6 結(jié)束語

本文提出了劃定障礙物點(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)證了算法的可行性。