視覺預(yù)瞄式智能車輛縱橫向協(xié)同控制研究*

汪若塵，魏振東，葉 青，蔡英鳳，陳 龍

（江蘇大學(xué)汽車工程研究院，鎮(zhèn)江 212013）

前言

路徑跟蹤控制是智能車輛領(lǐng)域的關(guān)鍵技術(shù)之一，研究內(nèi)容包括橫向控制及縱向控制。橫向控制通過控制車輛轉(zhuǎn)向?qū)崿F(xiàn)對期望路徑的跟蹤，縱向控制通過控制車輛驅(qū)動(dòng)／制動(dòng)系統(tǒng)實(shí)現(xiàn)對車速的跟蹤［1-2］。

針對路徑跟蹤的各種控制方法，諸多學(xué)者做了相關(guān)研究。Park等［3］通過對多個(gè)預(yù)瞄點(diǎn)的道路信息整合，基于汽車轉(zhuǎn)向幾何學(xué)模型建立了多點(diǎn)預(yù)瞄反饋控制器，其縱向速度采用了PID控制。尹曉麗等［4］基于車輛運(yùn)動(dòng)學(xué)模型設(shè)計(jì)了一種汽車運(yùn)動(dòng)狀態(tài)預(yù)測模型，在線計(jì)算車輛的橫向偏差和橫擺角度偏差，控制車輛穩(wěn)定跟蹤期望路徑。Walters等［5］在車輛與期望路徑之間建立一個(gè)動(dòng)態(tài)虛擬路徑，運(yùn)用最優(yōu)控制跟蹤動(dòng)態(tài)虛擬路徑的方法實(shí)現(xiàn)了路徑跟蹤。Rathgeber等［6］采用了狀態(tài)空間的方法對路徑跟蹤控制策略展開研究，建立了干擾觀測器，用以提升系統(tǒng)對車輛自身固有參數(shù)變化的魯棒性。Lee等［7］基于最優(yōu)控制理論，建立了線性二次最優(yōu)預(yù)瞄控制器實(shí)現(xiàn)對車輛橫向運(yùn)動(dòng)的控制，之后又進(jìn)行了實(shí)車驗(yàn)證。但上述研究均較少考慮車輛路徑跟蹤過程中的動(dòng)力學(xué)約束，隨著車速的上升或行駛環(huán)境的惡化，其跟蹤性能下降明顯。

Falcone等［8］首先基于車輛線性動(dòng)力學(xué)模型設(shè)計(jì)了MPC路徑跟蹤控制器，并將控制算法進(jìn)行了實(shí)車測試。Abbas等［9］綜合考慮路徑跟蹤過程中輪胎側(cè)偏角度約束，對車輛行駛穩(wěn)定性進(jìn)行了分析，但該控制算法實(shí)時(shí)性較差，且對硬件設(shè)備要求較高。孫銀建［10］基于模型預(yù)測控制算法，針對冰雪路面的路徑跟蹤控制進(jìn)行研究，但未考慮縱向車速對橫向控制的影響。明廷友等［11］將縱向加速度納入控制器設(shè)計(jì)，實(shí)現(xiàn)了車輛的縱橫向綜合控制，但并未給出如何通過車速的控制提升系統(tǒng)跟蹤性能的具體方案。上述基于模型預(yù)測控制算法的研究，控制器設(shè)計(jì)過程中對縱向車速的處理較為簡單。

為此，本文中將視覺預(yù)瞄控制的強(qiáng)魯棒性優(yōu)勢與MPC在處理約束方面的優(yōu)勢相結(jié)合，并綜合考慮縱向車速與預(yù)瞄距離對路徑跟蹤性能的影響，提出了一種預(yù)瞄式MPC路徑跟蹤控制方法。最后通過仿真和試驗(yàn)，對所設(shè)計(jì)的路徑跟蹤控制器進(jìn)行驗(yàn)證。

1 路徑跟蹤系統(tǒng)模型

路徑跟蹤系統(tǒng)模型是實(shí)現(xiàn)智能車輛路徑跟蹤的基礎(chǔ)。實(shí)現(xiàn)路徑跟蹤系統(tǒng)閉環(huán)仿真需要構(gòu)建以下模型：車輛動(dòng)力學(xué)模型、視覺模型和道路模型。

1.1 車輛動(dòng)力學(xué)模型

為滿足控制器實(shí)時(shí)性需求，建立車輛模型之初首先做如下假設(shè)：（1）車輛僅做平面運(yùn)動(dòng)功能，忽略懸架特性影響；（2）車輛為前輪轉(zhuǎn)向，后輪轉(zhuǎn)向角度為0°；（3）忽略空氣動(dòng)力學(xué)及坡道阻力影響；（4）左右車輪特性相同，轉(zhuǎn)角度恒等；（5）忽略輪胎回正力矩影響。建立的車輛單軌動(dòng)力學(xué)模型如圖1所示，圖中Flf和 Flr為前、后輪縱向力，F(xiàn)cf和 Fcr為前、后輪側(cè)向力，αf和αr為前、后輪側(cè)偏角，φ為當(dāng)前車輛實(shí)際航向角。

圖1 車輛單軌動(dòng)力學(xué)模型

根據(jù)車輛縱向、橫向及橫擺方向的動(dòng)力學(xué)及牛頓運(yùn)動(dòng)定律，車輛動(dòng)力學(xué)模型為

式中：ax為車輛縱向加速度；r為橫擺角速度；m為整車質(zhì)量；δf為前輪轉(zhuǎn)角；Iz為車輛繞Z軸轉(zhuǎn)動(dòng)慣量；lf和lr分別為車輛前后軸距；vx和vy分別為縱向和橫向車速；Caf和Car分別為前、后輪胎的側(cè)偏剛度。

1.2 視覺模型

圖2為視覺預(yù)瞄誤差模型示意圖。

圖2 視覺預(yù)瞄誤差模型

由圖2可知視覺預(yù)瞄誤差模型為

式中：φe為預(yù)瞄點(diǎn)（gx，gy）處的方向偏差；φp為預(yù)瞄點(diǎn)（gx，gy）處的期望航向角。

對式（2）求導(dǎo)可得

式中κ為預(yù)瞄點(diǎn)（gx，gy）處的道路曲率。

此外，根據(jù)視覺預(yù)瞄模型還可得出

式中：yef，yer和yeo分別為車輛前、后軸軸心和質(zhì)心相對于預(yù)瞄點(diǎn)的橫向偏差；ye為預(yù)瞄點(diǎn)處的橫向偏差；L為預(yù)瞄距離。

當(dāng)車輛沿著當(dāng)前路徑行駛時(shí)，方向偏差一般較小，可假設(shè) tanφe＝φe，因此式（4）可進(jìn)一步轉(zhuǎn)化為

對式（5）求導(dǎo)，并結(jié)合式（3）可得視覺預(yù)瞄誤差模型為

考慮到駕駛員駕駛車輛在大曲率路徑上行駛時(shí)，會(huì)更多關(guān)注近距離處的道路信息，車速較高時(shí)更多關(guān)注較遠(yuǎn)距離的道路信息。因此，在文獻(xiàn)［11］中提出的預(yù)瞄距離計(jì)算方法基礎(chǔ)上進(jìn)一步考慮道路曲率變化對預(yù)瞄距離的影響，設(shè)計(jì)預(yù)瞄距離計(jì)算公式為

式中：L0為初始預(yù)瞄距離；κ為路徑曲率；?和τ為經(jīng)驗(yàn)系數(shù)，?用以描述預(yù)瞄距離受車速變化的影響程度大小，τ用以描述預(yù)瞄距離受道路曲率變化的影響程度大小。

1.3 道路模型

道路模型描述的是路徑跟蹤時(shí)車輛的參考路徑，通過計(jì)算路徑跟蹤過程中車輛行駛偏差的大小評價(jià)所設(shè)計(jì)路徑跟蹤控制器性能的優(yōu)劣。為便于對本文中所設(shè)計(jì)路徑跟蹤控制器進(jìn)行評價(jià)，本文中參考GB／T 6323—2014標(biāo)準(zhǔn)及 ISO／3888技術(shù)報(bào)告設(shè)定雙移線（double lane change，DLC）目標(biāo)路徑，如圖3所示。

圖3 雙移線道路模型

雙移線道路模型參數(shù)如表1所示。

表1 雙移線路徑參數(shù) m

雙移線路徑的軌跡方程為

式中：x，y分別為縱、橫向位置坐標(biāo)；D為路徑側(cè)向位移。

2 控制系統(tǒng)設(shè)計(jì)

根據(jù)式（1）和式（7）可知，車速vx僅依賴于縱向加速度ax，由于預(yù)瞄點(diǎn)處道路曲率κ由仿真和試驗(yàn)道路決定，?和τ是反映駕駛員駕駛特性的特征參數(shù)，均取定值，因此預(yù)瞄距離L實(shí)際僅取決于縱向加速度。但橫向、橫擺動(dòng)力學(xué)性能及預(yù)瞄模型與車速存在耦合關(guān)系，因此，為反映此耦合關(guān)系，考慮單位控制步長較短，在每個(gè)控制周期內(nèi)將系統(tǒng)各控制變量視為固定，縱向加速度ax與預(yù)瞄距離L視為已知輸入，此時(shí)，多輸入多輸出系統(tǒng)就轉(zhuǎn)化為僅有前輪轉(zhuǎn)角輸入的單輸入多輸出系統(tǒng)。因此，設(shè)計(jì)預(yù)瞄式智能車輛縱橫向協(xié)同控制系統(tǒng)結(jié)構(gòu)如圖4所示。

圖4 路徑跟蹤控制系統(tǒng)結(jié)構(gòu)圖

智能車輛的控制輸入為驅(qū)動(dòng)／制動(dòng)力矩Tdriv及前輪轉(zhuǎn)角δf，輸出為車輛當(dāng)前狀態(tài)信息ξk。其中，驅(qū)動(dòng)力Tdriv由車速跟蹤控制器根據(jù)車輛當(dāng)前狀態(tài)信息及期望車速控制得出；前輪轉(zhuǎn)角δf由路徑跟蹤控制器根據(jù)目標(biāo)路徑、預(yù)瞄距離L、車輛加速度ax及當(dāng)前狀態(tài)信息ξk控制得出。

2.1 預(yù)瞄式跟蹤模型離散化

將車輛動(dòng)力學(xué)模型與視覺模型相結(jié)合，得到可用以路徑跟蹤系統(tǒng)的預(yù)瞄式跟蹤系統(tǒng)模型：

方便起見，將上式描述為如下狀態(tài)空間形式：

式中：ξ＝[ φ vxvyr φeyeL]T為系統(tǒng)狀態(tài)變量；u＝[ axδfL]T為系統(tǒng)控制量。

根據(jù)系統(tǒng)狀態(tài)空間表達(dá)式（12）可將任意時(shí)刻系統(tǒng)狀態(tài)變量與控制變量關(guān)系做如下表示：

將式（12）在任意工作時(shí)刻（ξt，ut）泰勒展開，保留1階項(xiàng)忽略高階項(xiàng)可得

式中：Jf（ξ）為系統(tǒng)對狀態(tài)變量 ξ的雅克比矩陣；Jf（u）為系統(tǒng)對于控制輸入量u的雅克比矩陣。

結(jié)合式（13）與式（14）可得

另外為滿足 MPC控制器設(shè)計(jì)需求，做如下假設(shè)：

因此，結(jié)合式（15）與式（16），即可得到用于MPC控制器的系統(tǒng)離散線性時(shí)變形式：

式中：dk，t（k）＝ξt（k＋1）-Ak，tξt（k）-Bk，tut（k），k≥0；Δu（k）為控制量增量。

2.2 MPC控制器設(shè)計(jì)

模型預(yù)測控制是一類循環(huán)求解帶約束條件最優(yōu)化問題的控制方法［12］，路徑跟蹤過程中，各種車輛動(dòng)力學(xué)約束對其跟蹤性能的影響至關(guān)重要。因此，設(shè)定包含輪胎側(cè)偏角、輸入量及輸出量等約束在內(nèi)的MPC路徑跟蹤控制器如下：

式中：NP為系統(tǒng)預(yù)測時(shí)域；NC為系統(tǒng)控制時(shí)域；χref＝[φrefrrefYref] 為期望控制輸出量；Q∈Rp×p為輸出加權(quán)矩陣；R∈Rm×m為控制增量加權(quán)矩陣；S∈Rm×m為控制增量加權(quán)矩陣；ρ∈R為松弛因子加權(quán)系數(shù)；ΔU＝[ Δu（t） … Δu（t＋NP-1）]；式（18a）為系統(tǒng)的離散線性時(shí)變動(dòng)力學(xué)模型；式（18b）為系統(tǒng)的輸出量約束；式（18c）和式（18d）為系統(tǒng)的控制量約束；式（18e）和式（18f）為系統(tǒng)控制增量約束；式（18g）為輪胎側(cè)偏角軟約束。

式（18）所示的MPC路徑跟蹤控制器無法直接進(jìn)行仿真計(jì)算，需要對該式進(jìn)行適當(dāng)轉(zhuǎn)化。

首先，為避免無可行解情況發(fā)生，將系統(tǒng)模型式（17）轉(zhuǎn)換為控制增量形式，引入以下矩陣：

式中：n為狀態(tài)變量個(gè)數(shù)；m為控制變量個(gè)數(shù)。

得到轉(zhuǎn)換后的系統(tǒng)模型為

另外，為推導(dǎo)系統(tǒng)未來狀態(tài)，進(jìn)一步提升MPC控制器運(yùn)行效率，做如下假設(shè)：

（1）控制時(shí)域外，控制量不變，即

（2）預(yù)測時(shí)域內(nèi)，增量矩陣保持不變，即

因此，通過迭代可得系統(tǒng)在預(yù)測時(shí)域內(nèi)的輸出量表達(dá)式：

為將MPC最優(yōu)化問題轉(zhuǎn)化為二次規(guī)劃求解問題，引入以下轉(zhuǎn)換：

其中：Q＝diag（q11q22q33）；R＝r；S＝s

令M＝K?I，其中?表示兩個(gè)矩陣的克羅內(nèi)克積，因此可得

另外設(shè)定：

最終，結(jié)合式（2）～式（30）可得到轉(zhuǎn)化為標(biāo)準(zhǔn)二次規(guī)劃形式的MPC路徑跟蹤控制器如下：

每一個(gè)仿真時(shí)刻，通過式（31）求解出一個(gè)最優(yōu)控制增量序列ΔUopt。為充分利用每一時(shí)刻系統(tǒng)的狀態(tài)信息，僅將ΔUopt中第一個(gè)控制增量作為實(shí)際控制增量作用于系統(tǒng)，循環(huán)上述過程，即可實(shí)現(xiàn)智能車輛的路徑跟蹤控制。

2.3 縱向車速跟蹤控制

智能車輛路徑跟蹤性能受車速的影響很大，在大曲率路況行駛時(shí)適當(dāng)?shù)亟档蛙囁賹ο到y(tǒng)的跟蹤精度及穩(wěn)定性尤為重要，實(shí)際駕駛員在遇彎道時(shí)選擇減速行駛也驗(yàn)證了這一點(diǎn)。因此本節(jié)中選擇車速的指數(shù)模型［13］對期望車速進(jìn)行描述：

式中：v0為直線行駛的期望最高車速；R為道路曲率半徑；ζ為經(jīng)驗(yàn)因子。

經(jīng)驗(yàn)因子ζ的選取與駕駛員的駕駛習(xí)慣有關(guān)，較大的ζ表示駕駛員對道路曲率的變化反應(yīng)較小，彎道行駛傾向于選擇較高車速；較小的ζ表示駕駛員對道路曲率的變化敏感，彎道行駛傾向于選擇較低車速。

縱向車速的控制在自適應(yīng)巡航領(lǐng)域發(fā)展的已相當(dāng)成熟，為保證車輛速度穩(wěn)定變化，一般是通過對加速度的控制實(shí)現(xiàn)。假設(shè)車速從當(dāng)前位置定加速度行駛到預(yù)瞄點(diǎn)位置同時(shí)實(shí)現(xiàn)對期望車速的跟蹤，根據(jù)加速度定律可知

式中：vref為預(yù)瞄點(diǎn)處的期望車速；vx為當(dāng)前車速；aref為期望加速度。

考慮到車輛系統(tǒng)參數(shù)的變化及存在一定程度的模型失配問題，基于PI控制理論引入加速度的反饋控制：

3 仿真及試驗(yàn)驗(yàn)證

為驗(yàn)證本文中所設(shè)計(jì)控制器的有效性，在Carsim和Matlab／Simulink環(huán)境下搭建了系統(tǒng)的聯(lián)合仿真模型。系統(tǒng)中車輛仿真參數(shù)根據(jù)實(shí)車試驗(yàn)標(biāo)定得到，部分參數(shù)如表2所示。

表2 整車參數(shù)

3.1 仿真驗(yàn)證分析

分別采用低速36和高速72 km／h兩種縱向車速進(jìn)行仿真，仿真結(jié)果如圖5～圖10所示，雙移線路徑跟蹤的行駛偏差峰值對比結(jié)果如表3所示。

圖5 36 km／h時(shí)路徑跟蹤結(jié)果對比

圖6 36 km／h時(shí)橫向偏差對比

圖7 36 km／h時(shí)方向偏差對比

圖8 72 km／h時(shí)路徑跟蹤結(jié)果對比

圖9 72 km／h時(shí)橫向偏差對比

圖10 72 km／h時(shí)方向偏差對比

表3 仿真結(jié)果對照表

由圖5～圖10可知，相較于傳統(tǒng)MPC路徑跟蹤控制器，本文中所設(shè)計(jì)的預(yù)瞄式MPC路徑跟蹤控制器在36和72 km／h行駛車速下，均能夠有效提升路徑跟蹤精度。此外，根據(jù)表3所示的仿真結(jié)果對比可知，相比于傳統(tǒng)MPC控制器，預(yù)瞄式控制器在兩種車速下，橫向偏差優(yōu)化效果明顯，均降低了20%以上，且相比于 36 km／h，高速 72 km／h時(shí)，預(yù)瞄式MPC控制器路徑跟蹤橫向偏差優(yōu)化效果更加顯著，橫向偏差降低了27.52%，方向偏差降低了11.98%。因此本文中所提出的預(yù)瞄式MPC路徑跟蹤控制方法能夠有效降低路徑跟蹤行駛偏差，提升路徑跟蹤性能，相比于傳統(tǒng)MPC路徑跟蹤方法，高速跟蹤優(yōu)勢更加明顯。

為進(jìn)一步驗(yàn)證所設(shè)計(jì)控制系統(tǒng)的縱橫向協(xié)同控制效果，針對固定車速54 km／h與初始參考車速為54 km／h的變速跟蹤進(jìn)行仿真對比，其仿真結(jié)果如圖11～圖13所示。由圖12可知，因雙移線路徑的第一次換道曲率小于第二次換道，因此縱向車速在第一次換道過程中沒有明顯變動(dòng)，其協(xié)同控制優(yōu)勢主要體現(xiàn)在第二次換道過程中，可以看出，本文中所設(shè)計(jì)的縱向控制器能夠?qū)崿F(xiàn)對車速的良好跟蹤。由圖13可知，相較于定速跟蹤控制，本文中所設(shè)計(jì)的縱橫向協(xié)同控制跟蹤性能進(jìn)一步得到優(yōu)化。

圖11 54 km／h初始車速路徑跟蹤結(jié)果對比

圖12 變速跟蹤時(shí)車速跟蹤響應(yīng)

圖13 變速跟蹤時(shí)橫向偏差對比

3.2 試驗(yàn)驗(yàn)證

為驗(yàn)證本文中設(shè)計(jì)控制系統(tǒng)的可行性，以圖14所示的無人駕駛電動(dòng)車為試驗(yàn)樣車進(jìn)行了試驗(yàn)驗(yàn)證。該試驗(yàn)使用的智能車輛視覺傳感器如圖15所示。

圖14 試驗(yàn)車輛

圖15 視覺傳感器

選取的試驗(yàn)場地如圖16所示，箭頭表示車輛行駛方向，圖17為轉(zhuǎn)換后的道路離線坐標(biāo)數(shù)據(jù)。考慮到實(shí)車試驗(yàn)的安全性，試驗(yàn)車速僅選取20 km／h的低速下進(jìn)行，試驗(yàn)結(jié)果如圖18和圖19所示。

圖16 試驗(yàn)道路真實(shí)場景

圖17 試驗(yàn)道路離線數(shù)據(jù)

圖18 轉(zhuǎn)向盤響應(yīng)曲線

圖19 橫向偏差響應(yīng)曲線

由圖18和圖19可知，在整個(gè)路徑跟蹤過程中，轉(zhuǎn)向盤轉(zhuǎn)向平穩(wěn)，無明顯振蕩，且車速為20 km／h路徑跟蹤的橫向偏差均控制在±0.3 m。造成試驗(yàn)誤差略大于仿真誤差的原因主要有以下兩點(diǎn)：（1）Mobileye攝像頭所采集的車道線信息存在一定的誤差；（2）Mobileye攝像頭處理器的數(shù)據(jù)處理能力有限。

考慮到上述無法避免的因素，且跟蹤過程中轉(zhuǎn)向盤無明顯抖動(dòng)、跟蹤誤差在合理范圍之內(nèi)，這表明控制器控制車輛實(shí)現(xiàn)了對真實(shí)道路的良好跟蹤，進(jìn)而證明了本文中所設(shè)計(jì)預(yù)瞄式MPC控制器在真實(shí)環(huán)境下的有效性。

4 結(jié)論

為解決傳統(tǒng)MPC路徑跟蹤控制方法時(shí)域參數(shù)的敏感性問題，本文中在傳統(tǒng)MPC路徑跟蹤控制方法的基礎(chǔ)上，結(jié)合預(yù)瞄控制理論的強(qiáng)魯棒性優(yōu)勢，提出了一種預(yù)瞄式MPC路徑跟蹤控制方法。首先，構(gòu)建了預(yù)瞄式路徑跟蹤非線性系統(tǒng)模型，采用局部線性化的方法將非線性系統(tǒng)轉(zhuǎn)化為可用于預(yù)瞄式MPC控制器設(shè)計(jì)的線性時(shí)變系統(tǒng)。另外，基于合理假設(shè)的基礎(chǔ)上設(shè)計(jì)了車輛的縱橫向協(xié)同控制系統(tǒng)結(jié)構(gòu)，并設(shè)計(jì)了縱向車速的調(diào)節(jié)及跟蹤控制策略。仿真和試驗(yàn)表明，相比于傳統(tǒng)MPC路徑跟蹤控制方法，本文中所提出的預(yù)瞄式MPC路徑跟蹤控制方法能夠?qū)崿F(xiàn)車輛的縱橫向協(xié)同控制，且能夠有效降低車輛行駛偏差，提升路徑跟蹤性能。