花曉峰，段建民，田曉生

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

1 引言

對于無人駕駛智能車輛的安全行車而言，障礙躲避是車輛行駛控制的核心任務(wù)。由于智能車輛只能在道路上行駛移動，而且路線也是嚴格受限的，遇到的只有車輛、行人等特定的規(guī)則障礙，因此移動式機器人避障的算法思維并不能完全應(yīng)用于車輛避障。由于車輛系統(tǒng)結(jié)構(gòu)復(fù)雜，存在高度的非線性時變特性，因此有效地進行系統(tǒng)建模，選取快速、可靠的控制策略是車輛控制的重要基礎(chǔ)。

模型預(yù)測控制（Model Predictive Control，MPC）是針對被控對象的線性化模型求解未來時刻的有約束最優(yōu)解，并通過反饋機制更新系統(tǒng)狀態(tài)來達到對系統(tǒng)實時在線控制的目的。預(yù)測控制以預(yù)測模型為基礎(chǔ)，理論上不受系統(tǒng)的時變與非時變、線性與非線性、有無時滯的影響，在復(fù)雜系統(tǒng)控制方面取得了很大成功[1-2]。在智能車避障控制應(yīng)用方面，文獻[3]提出了基于人類駕駛行為的預(yù)測避障方案，但在避障環(huán)節(jié)采用了預(yù)瞄點搜索、MPC控制、PID控制多重模塊，導(dǎo)致避障系統(tǒng)過于復(fù)雜；文獻[4-5]分別從受控自回歸積分滑動平均模型和模擬退火算法求解目標(biāo)函數(shù)方面闡述了軌跡跟蹤控制方案，但均把重點放在了單純的車輛軌跡規(guī)劃層面，對于以障礙檢測、障礙預(yù)測為基礎(chǔ)的軌跡重規(guī)劃方面內(nèi)容較少涉及；文獻[6]采用了預(yù)測觸覺反饋（Predictive Haptic Feedback）的思路實現(xiàn)了障礙躲避，但方案只完成了換道，并沒有進行避障換道之后的返回原車道實驗；文獻[7-10]通過模型預(yù)測與不同算法融合闡述了避障策略，但并未考慮車輛的長寬尺寸，亦無法判定車體是否能安全地避免與障礙發(fā)生碰撞。真實的行車環(huán)境中，障礙物和車體的尺寸都不可忽略，因此即便將車體抽象為質(zhì)點，也必須要考慮其與障礙拉開足夠距離的問題。因此障礙相對于車輛質(zhì)心而言必然會存在一個“禁區(qū)范圍”。

針對以上問題，本文以模型預(yù)測控制算法和車輛動力學(xué)模型為基礎(chǔ)，以前輪轉(zhuǎn)向車輛為實驗對象，對障礙物邊界點的確定引入以階躍函數(shù)為基礎(chǔ)的禁區(qū)邊界懲罰函數(shù)，禁區(qū)邊界長度以車輛尺寸為依據(jù)，以保證車身不與障礙邊界點觸碰；在避障路徑重規(guī)劃環(huán)節(jié)引入預(yù)測距離倍增的方案，以彌補因預(yù)測距離過短導(dǎo)致無法在遠距離處檢測到障礙物造成避障失敗的缺陷。

2 預(yù)測模型

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

車輛運動過程中存在多個狀態(tài)變量，因此需要建立復(fù)雜的動力學(xué)微分方程組。假設(shè)車輛是只可作橫向、縱向、橫擺運動的三自由度前輪驅(qū)動車輛，則可等效為二輪自行車單軌模型。分別建立相對地面的世界坐標(biāo)系OXYZ和相對車輛自身的車體坐標(biāo)系oxyz，如圖1，兩坐標(biāo)系以坐標(biāo)軸字母大小寫進行區(qū)分。車體系原點位于車輛質(zhì)心，x軸方向為車體縱向，y軸沿車體橫向與x軸垂直，z軸方向由右手系法則確定，橫擺角φ是車體系x軸與世界坐標(biāo)系X軸的夾角，其逆時針方向為正方向。

圖1 二輪單軌前輪驅(qū)動式車輛動力學(xué)模型

根據(jù)運動學(xué)坐標(biāo)轉(zhuǎn)換和牛頓第二定律可得受力平衡方程：Flf、Fcf分別為施加給前輪的縱、橫向力，F(xiàn)lr、Fcr分別為施加給后輪的縱、橫向力。前輪驅(qū)動車輛的后輪不能轉(zhuǎn)向，故后輪偏角恒為0，即δr≡0。前輪的合速度即為車輛的輸入速度，即 vin=x˙2+y˙2，恒定速度時 vin為常數(shù)。在側(cè)偏角較小時，動力學(xué)模型可進行線性近似化，由無窮小定理和輪胎線性模型，可將式（1）轉(zhuǎn)化為前輪小偏角和線性化輪胎模型下的車輛動力學(xué)非線性模型[11-12]：

m為車體質(zhì)量；Ccf、Ccr為前、后輪側(cè)偏剛度；Clf、Clr為前、后輪縱向剛度；a、b為質(zhì)心的前、后車軸距離；sf、sr為前、后輪縱向滑移率；Iz為車體關(guān)于z軸的轉(zhuǎn)動慣量。角度單位全部為弧度。選取狀態(tài)量ξ為(˙˙φ φ˙YX)T，控制量 u 為 δf，輸出量 η=Cξ=(φ Y)T，式（2）可以寫為由ξ、u、η組成的非線性函數(shù)狀態(tài)空間式：

2.2 車輛預(yù)測模型

2.2.1 狀態(tài)方程

考慮無人駕駛車輛對控制器的實時性要求，首先需要對模型進行線性化。采用近似線性化思想，將式（3）寫為標(biāo)準(zhǔn)線性模型式：

其中Ad(k)=I+TA，Bd(k)=TB。

式（5）出現(xiàn)了ξ(k+1)，它表示的是以當(dāng)前k時刻為基準(zhǔn)未來下一時刻的狀態(tài)。模型預(yù)測的思想便以此為基礎(chǔ)，通過遞推法逐步推導(dǎo)出預(yù)測時域內(nèi)未來某一時刻的狀態(tài)。

2.2.2 目標(biāo)函數(shù)

目標(biāo)函數(shù)需要保證車輛快速平穩(wěn)跟蹤規(guī)劃的期望軌跡，優(yōu)化對象為系統(tǒng)狀態(tài)量和控制量。采用偏差式目標(biāo)函數(shù)以消除靜態(tài)誤差，即[13]：

定義狀態(tài)偏差量：

可得：

得最終的廣義預(yù)測輸出表達式：

2.2.3 反饋環(huán)節(jié)

每一控制周期內(nèi)對式（10）求解完成后得到控制時域內(nèi)的一個控制輸入序列ΔU*：

根據(jù)反饋原理將第一元素作為控制輸入增量作用于系統(tǒng)，即：

進入下一控制周期后重復(fù)上述優(yōu)化過程，再次求得下一控制周期的最優(yōu)控制輸入增量。

3 避障規(guī)劃

3.1 懲罰函數(shù)

障礙物點陣信息通常由雷達通過掃描障礙物得出，懲罰函數(shù)可以單獨針對障礙物點進行設(shè)計。懲罰函數(shù)是通過與障礙物點之間的距離函數(shù)來調(diào)節(jié)避障優(yōu)化函數(shù)值。懲罰函數(shù)可采用關(guān)于距離的反比例函數(shù)實現(xiàn)，如式（12）[12]：

k為預(yù)測時域內(nèi)的某一時刻，(x0,y0)是該時刻車輛質(zhì)心的坐標(biāo)，(xi,yi)是第i個障礙物點的坐標(biāo)（均在世界坐標(biāo)系下），ε＞0是一個防止分母為0的保護常數(shù)，vk是當(dāng)前時刻的合速度，S是權(quán)重系數(shù)。

該方法的缺陷首先是對于障礙邊界的劃分較為模糊，其次是并且未能考慮車身尺寸。在避障預(yù)測時會出現(xiàn)無法正確確定障礙邊界、甚至規(guī)劃路徑穿過障礙物的錯誤結(jié)果；即使規(guī)劃路徑能夠避開障礙物，也有可能因路徑過于靠近障礙物而導(dǎo)致車身與之發(fā)生碰撞。因此本文提出一種基于階躍函數(shù)的禁區(qū)邊界式懲罰函數(shù)策略，即：

式中M是一個充分大的正數(shù)，R*是臨界（禁區(qū)）半徑。車輛質(zhì)心位于車體幾何中心。令R*等于車身外接圓半徑，相應(yīng)地，每一個障礙物點都可以認為存在一個以自身為圓心，半徑為R*的禁區(qū)圓。預(yù)測時域內(nèi)某一時刻得出的車輛質(zhì)心坐標(biāo)如果進入了障礙物點禁區(qū)圓內(nèi)，由于M的存在，會導(dǎo)致規(guī)劃目標(biāo)函數(shù)值急劇增大，迫使預(yù)測控制器重新搜索最優(yōu)控制量，如圖2。

圖2 障礙點與預(yù)測時域內(nèi)不同時刻的車輛位置的關(guān)系

由于軌跡重規(guī)劃環(huán)節(jié)計算量較大，為了簡化計算，規(guī)劃層采用運動學(xué)模型，可以更好地兼顧控制性能和計算速度[14]。在規(guī)劃層中，設(shè)狀態(tài)變量ξ 為(y ˙ x˙φ YX)T，離散化后有：

某一預(yù)測時刻k下得到的k+1時刻狀態(tài)變量為：

由于車輛輸入速度為恒定，因此在任意的k時刻，vin,d(k)為常數(shù)，前輪偏角δf即為控制量。由于控制時域過長會導(dǎo)致MPC控制不穩(wěn)定，因此控制時域Nc應(yīng)盡可能短，這里取Nc=1，則令U=()δf，η(k)=[]Y(k)，局部參考路徑ηr(k)=[ ]Yr(k)，可得軌跡重規(guī)劃的目標(biāo)函數(shù)：N為障礙物點的個數(shù)。式中的η、U、Q、R與第2章中的不同，它們是屬于規(guī)劃層環(huán)節(jié)，并不用于車輛的運動控制策略。式中：

因此可知輸出量η是關(guān)于控制量U=(δf)的函數(shù)。由于控制時域僅為1，因此當(dāng)時刻i≥1時，控制量的變化量恒為0，即 ΔU(k+i)≡0,i=1,2,…,Np-1，因此預(yù)測時域內(nèi)有U(k+i)≡U(k)≡(δf)。優(yōu)化完成后得到車輛在當(dāng)前位置點的最優(yōu)前輪偏角量，按照MPC反饋的定義，直接代入式（15），更新狀態(tài)量即可得到新時刻的車輛各個狀態(tài)值。

3.2 障礙檢測

傳統(tǒng)的障礙檢測是將障礙物視作點障礙，并以該點構(gòu)造基于距離的反比例函數(shù)作為懲罰函數(shù)。規(guī)劃層采樣時間短，且預(yù)測時域一般不能過長。仿真實驗觀測表明，車輛在某一時刻實際預(yù)測距離很短，對于點障礙物不考慮尺寸則可以完成路徑的避障規(guī)劃。而對于長寬不可忽略的大障礙物，其尺寸明顯大于所預(yù)測距離，使用同樣的方法會導(dǎo)致車輛較遠時根本無法探測到障礙物。本文采用另一種策略，在規(guī)劃層進行優(yōu)化期間，對采樣周期進行倍數(shù)擴增，例如3倍或5倍，然后再將優(yōu)化得到的結(jié)果直接用于狀態(tài)更新，狀態(tài)更新過程中不使用周期倍增，這樣便可使車輛在較遠位置即能探測到障礙物，如圖3。

圖3 預(yù)測路徑與障礙物的關(guān)系

設(shè)擴增倍數(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)地被擴大，完成后得到最優(yōu)前輪偏角量記為δ~*f，代入式（15）更新狀態(tài)，在更新狀態(tài)時未使用擴增倍數(shù)為W。通過這種手段便能使車輛在距離障礙物較遠位置即可提前預(yù)知障礙物的存在從而及時調(diào)整當(dāng)前狀態(tài)，為后續(xù)能夠得到正確的規(guī)劃結(jié)果奠定基礎(chǔ)。

3.3 軌跡擬合

軌跡重規(guī)劃得到的是一系列散點，由于控制層輸入端口數(shù)量有限，顯然不能將離散點直接輸入控制層。曲線擬合是一種主要的替代處理方式，由于車輛的運動學(xué)和動力學(xué)存在一階、二階特性以及連續(xù)特性，因此采用三次多項式作為擬合曲線，其表達式為[12]：

縱向位移與橫擺角擬合多項式的各項參數(shù)向量(a b c d)和(w x y z)將作為軌跡重規(guī)劃的輸出值。

4 實驗方案設(shè)計

4.1 實驗平臺

選用CarSim車輛動力學(xué)作為仿真平臺，利用Matlab/Simulink搭建控制系統(tǒng)并通過S函數(shù)編寫控制策略，系統(tǒng)框圖如圖4。

CarSim輸入4個變量，依次為左、右前輪偏角和左、右后輪偏角；輸出6個變量依次為車體系橫向速度、車體系縱向速度、橫擺角、橫擺角速度、世界坐標(biāo)系下的縱向位移、世界坐標(biāo)系下的橫向位移，路面摩擦系數(shù)設(shè)定為0.85。

4.2 實驗參數(shù)與工況設(shè)定

實驗中采用雙移線軌跡作為跟蹤期望路徑[15]。模型預(yù)測控制需要添加相應(yīng)約束，控制量只有前輪偏角δf，設(shè)車輛方向盤在打死狀態(tài)時前輪偏角最大值為30°，前輪偏角變化量Δδf最大值設(shè)置為每采樣周期0.85°。因此控制量及控制量增量約束分別為：

（1）單一障礙物 I：障礙物中點坐標(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）單一障礙物Ⅱ：障礙物中點坐標(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中點坐標(biāo)(32.5,0)，障礙物2中點坐標(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 實驗圖像

黑色曲線為參考軌跡。實驗每隔一定時間以矩形框顯示某一位置處的車體輪廓；藍色星號軌跡為傳統(tǒng)方法即使用反比例懲罰函數(shù)及不加預(yù)測倍增方法得到的實驗結(jié)果；紅點軌跡為使用本文方法得到的結(jié)果。在縱坐標(biāo)位移圖中，本文方法得到的每一處實際位置點上均以一系列綠色須狀線條顯示不同種規(guī)劃方案。橫擺角曲線中的黑色曲線表示參考軌跡上對應(yīng)橫坐標(biāo)處的車體期望橫擺角。前輪偏角及變化量曲線使用藍色虛線劃定其約束的上下限。

工況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)方法進行對比，可以發(fā)現(xiàn)通過本文方法車輛能夠完成避障，車輛橫擺角和前輪偏角均收斂于0°，且前輪偏角及前輪偏角變化量的值均在約束范圍內(nèi)。對于傳統(tǒng)方法，圖5和圖13可以看到工況1和3下車輛質(zhì)心能避開障礙；圖7和圖15的車輛橫擺角都能收斂于0°；圖8和圖16的前輪偏角亦收斂于0°且在約束范圍內(nèi)；但是障礙鄰近放大圖（圖6和圖14）可見車輛質(zhì)心過于深入障礙物點禁區(qū)范圍，這樣將導(dǎo)致車身擦碰障礙物；圖9和圖10可以看到工況2傳統(tǒng)方法下車輛質(zhì)心觸碰障礙物，避障失敗，而且從橫擺角圖（圖11）、前輪偏角圖（圖12）可以發(fā)現(xiàn)橫擺角和前輪偏角都無法收斂，使得車身圍繞參考軌跡不斷搖擺。另外，在各個仿真工況下的障礙物附近區(qū)域局部放大圖中可以發(fā)現(xiàn)，新方法車輛質(zhì)心軌跡由于預(yù)測規(guī)劃誤差的緣故略微擦過了障礙物的最外層禁區(qū)邊沿，但是通過車輛輪廓線可以發(fā)現(xiàn)車身已經(jīng)遠離障礙區(qū)，說明避障成功，因此軌跡的微小越界是可以接受的。

6 結(jié)束語

本文提出了劃定障礙物點禁區(qū)邊界的方案，使用分段函數(shù)法進行障礙物點的禁區(qū)邊界劃定，解決了關(guān)于距離的反比例懲罰函數(shù)對于障礙邊界劃定不清的問題；在規(guī)劃層中進行軌跡重規(guī)劃時，采用了預(yù)測周期倍增法，將倍增后得到的預(yù)測位置點應(yīng)用于未施加倍增的運動學(xué)方程中進行狀態(tài)更新，從而使得車輛在較遠位置即可提前預(yù)知大尺寸障礙物，及時做出轉(zhuǎn)向調(diào)整；對不同工況進行仿真測試，并與傳統(tǒng)方法進行比對，結(jié)果滿足預(yù)期要求，驗證了算法的可行性。