基于動(dòng)力學(xué)模型預(yù)測控制的鉸接車輛多點(diǎn)預(yù)瞄路徑跟蹤方法*

李斯旭，徐 彪，胡滿江，邊有鋼，陳曉龍，孫 寧

（湖南大學(xué)機(jī)械與運(yùn)載工程學(xué)院，汽車車身先進(jìn)設(shè)計(jì)制造國家重點(diǎn)實(shí)驗(yàn)室，長沙410082）

前言

鉸接車輛指前后車體間存在擺動(dòng)環(huán)和鉸接點(diǎn)的汽車［1］，其前后車體間夾角可達(dá)45°，機(jī)動(dòng)能力好、越野能力強(qiáng)，加之其油耗小、成本低，在實(shí)際中被廣泛應(yīng)用。然而，鉸接車輛操作困難，多用于礦山、山地等惡劣地形環(huán)境中，易引發(fā)事故，且這些作業(yè)環(huán)境往往伴隨粉塵和噪聲，也易對駕駛員的安全健康造成威脅。近年來，車輛自動(dòng)駕駛技術(shù)快速發(fā)展，將其應(yīng)用于鉸接車輛，不但可替代駕駛員、避免人員損傷，還可有效提升工作效率和燃油經(jīng)濟(jì)性。

在車輛自動(dòng)駕駛中，路徑跟蹤控制是核心技術(shù)之一［2］，而鉸接車輛模型復(fù)雜、非線性強(qiáng)，其路徑跟蹤控制相比一般乘用車更加困難，也更具挑戰(zhàn)性。目前，車輛路徑跟蹤控制技術(shù)已得到廣泛研究，典型控制方法如模糊比例-積分-微分（PID）控制、線性二次調(diào)節(jié)控制、前饋-反饋控制、模型預(yù)測控制（model predictive control，MPC）等。Hayakawa等設(shè)計(jì)了PID路徑跟蹤控制器［3］，該方法結(jié)構(gòu)簡單，不依賴于精確的車輛模型，但無法對車輛未來狀態(tài)進(jìn)行預(yù)測，且對于非線性動(dòng)力學(xué)的控制效果欠佳。Levinson等將線性二次調(diào)節(jié)器應(yīng)用于路徑跟蹤控制［4］，提高了路徑跟蹤精度，但該方法難以顯式滿足車輛系統(tǒng)約束，在較復(fù)雜環(huán)境中難以達(dá)到理想的控制效果。Kritayakirana等針對賽車的路徑跟蹤問題設(shè)計(jì)了前饋-反饋控制器［5］，同樣也有較高的跟蹤精度，但其依賴于系統(tǒng)可逆性，應(yīng)用范圍有限。

近年來，MPC技術(shù)在車輛路徑跟蹤控制中得到了廣泛應(yīng)用。該技術(shù)能夠?qū)⑽磥砺窂叫畔⑷谌肟刂圃O(shè)計(jì)中，對速度和路徑曲率的變化有良好的魯棒性，且可實(shí)現(xiàn)多目標(biāo)優(yōu)化、顯式處理狀態(tài)變量約束及執(zhí)行器飽和約束［6］，在車輛路徑跟蹤方面具有很大優(yōu)勢。基于MPC進(jìn)行鉸接車輛路徑跟蹤控制，可充分考慮其實(shí)際物理特性，顯式處理前后車體間最大夾角、最大轉(zhuǎn)向力矩等約束，提升控制方法實(shí)用性。

基于MPC的鉸接車輛路徑跟蹤控制研究尚不多見。Nayl等首先將基于運(yùn)動(dòng)學(xué)的MPC應(yīng)用于鉸接車輛的路徑跟蹤問題［7］，但這種基于運(yùn)動(dòng)學(xué)模型的方法難以在路徑曲率連續(xù)變化、低附著路面、高速行駛等復(fù)雜條件下保障良好的控制性能。竇鳳謙將基于動(dòng)力學(xué)的MPC應(yīng)用于鉸接車輛的路徑跟蹤控制［8］，取得了良好的控制效果，但該方法在模型線性化過程中作了較多簡化，且未對線性化誤差進(jìn)行補(bǔ)償，因此在較高車速、低附著和曲率連續(xù)變化情況下難以保證跟蹤精度。為解決上述問題，本文中設(shè)計(jì)了一種新的基于DMPC（dynamic MPC）的路徑跟蹤控制方法，該方法對線性化誤差進(jìn)行補(bǔ)償，從而得到一種改進(jìn)動(dòng)力學(xué)模型，并根據(jù)狀態(tài)軌跡和實(shí)際狀態(tài)量的偏差設(shè)計(jì)DMPC算法，以期提高路徑跟蹤控制精度。Nayl等設(shè)計(jì)了誤差預(yù)瞄模型用以預(yù)測跟蹤誤差，但該方法基于預(yù)測時(shí)域內(nèi)路徑曲率不變的假設(shè)和前后車體間夾角為小角度的假設(shè)，實(shí)際的變化曲率較大及前后車體間夾角會(huì)導(dǎo)致跟蹤精度的降低。為此，本文中直接根據(jù)路徑函數(shù)生成參考路徑點(diǎn)而非采用近似預(yù)瞄誤差方程，提出了考慮多點(diǎn)預(yù)瞄誤差的控制目標(biāo)，以提高控制精度。

本文中首先構(gòu)建鉸接車輛路徑跟蹤動(dòng)力學(xué)模型，而后設(shè)計(jì)DMPC控制器，最后通過Matlab/Simulink與Adams聯(lián)合仿真驗(yàn)證所提出路徑跟蹤控制算法的性能。

1 研究方法

本節(jié)介紹預(yù)測模型構(gòu)建和控制器設(shè)計(jì)方法。首先構(gòu)建鉸接車非線性車輛動(dòng)力學(xué)模型，采用基于狀態(tài)軌跡的線性化方法將其線性化，再近似離散化得到預(yù)測模型；而后基于預(yù)測模型，根據(jù)狀態(tài)軌跡和真實(shí)狀態(tài)變量的偏差設(shè)計(jì)約束條件、路徑多點(diǎn)預(yù)瞄和目標(biāo)函數(shù)，進(jìn)而給出MPC優(yōu)化問題。

1.1 預(yù)測模型

1.1.1 鉸接車輛動(dòng)力學(xué)模型

鉸接車輛質(zhì)量、尺寸參數(shù)如圖1（a）所示，將鉸接車輛液壓轉(zhuǎn)向系統(tǒng)等效為作用在前車和后車上的轉(zhuǎn)向力矩Tn，鉸接車輛受力情況如圖1（b）所示。其中，x1、y1為前車局部坐標(biāo)系坐標(biāo)軸，Xw1、Xw2為前車輪胎受到的縱向力，Yw1、Yw2為前車輪胎受到的橫向力，ω1、ω2為前、后車橫擺角速度，T1、T2為前、后車在Tn作用下質(zhì)心處受到的等效力矩，u1、v1分別為前車的縱向、橫向速度，φ為前后車在鉸接點(diǎn)處的夾角，φ?為前后車夾角加速度，Xw3、Xw4分別為后車輪胎所受到的縱向力，Yw3、Yw4分別為后車輪胎所受到的橫向力，u2、v2分別為后車的縱向、橫向速度。

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

如圖1（b）所示，由達(dá)朗貝爾原理：

式中：Y1=Yw1+Yw2；Y2=Yw3+Yw4；X2=Xw3+Xw4；ta為半個(gè)輪距。

在路徑跟蹤過程中，兩側(cè)輪胎受力可近似為相等，且輪胎力可用近似的線性描述：

式中：Fs、Fα分別為輪胎縱向力和橫向力；Cs、Cα分別為輪胎縱向剛度和側(cè)偏剛度；s為輪胎縱向滑移率；α為輪胎側(cè)偏角。

S2為后輪縱向滑移率；α1、α2分別為前、后輪側(cè)偏角；Cs2為后輪縱向滑移剛度；Cα1、Cα2為前、后輪側(cè)偏剛度；；u1為縱向速度，為定值，可控制鉸接車輛的轉(zhuǎn)向。

1.1.2 模型線性化

由于跟蹤的目標(biāo)為路徑的X、Y坐標(biāo)及路徑的航向角，需要對式（6）模型進(jìn)行擴(kuò)維處理，使得車輛X、Y坐標(biāo)及車輛航向角成為狀態(tài)變量，選擇χ=作為狀態(tài)變量，u=Tn為系統(tǒng)輸入控制量，其中X、Y為前車參考點(diǎn)全局坐標(biāo)，θ為前車航向角，可得適用于模型預(yù)測控制的鉸接車輛動(dòng)力學(xué)模型：

式中：f=M-1F；

采用基于狀態(tài)軌跡的線性化方法將鉸接車輛動(dòng)力學(xué)模型線性化［9］，該方法優(yōu)點(diǎn)是不需要提供每一時(shí)刻的狀態(tài)變量參考值。假設(shè)在某一采樣時(shí)刻t，車輛狀態(tài)變量為χ0，輸入控制量為u0，假設(shè)鉸接車輛在預(yù)測時(shí)域內(nèi)的控制輸入量為u0不變，得到一條狀態(tài)軌跡?，對前文推導(dǎo)的式（7）非線性模型在狀態(tài)軌跡點(diǎn)進(jìn)行泰勒展開，并忽略高階項(xiàng)：

將式（8）重新整理，可得以χ作為狀態(tài)變量、以u作為控制量的表達(dá)式：

1.1.3 模型離散化

取采樣時(shí)間T，對鉸接車輛動(dòng)力學(xué)模型進(jìn)行近似離散化［6］，取狀態(tài)變量χ，控制量u，則k+1時(shí)刻狀態(tài)變量χ(k+1)及用于表示目標(biāo)函數(shù)需要的輸出η(k)的表達(dá)式為

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

1.2.1 約束條件

根據(jù)實(shí)際工況賦予以下約束：

式（12）表示鉸接點(diǎn)處液壓轉(zhuǎn)向系統(tǒng)輸入的轉(zhuǎn)向力矩極值約束；式（13）表示鉸接點(diǎn)處輸入的轉(zhuǎn)向力矩增量極值約束，其中ε≥0為松弛因子［6］，對控制量增量約束進(jìn)行松弛，其能夠?qū)η蠼饪尚杏蜻M(jìn)行擴(kuò)展，避免在突然轉(zhuǎn)彎等極限工況下無法在硬約束上下界內(nèi)求得可行解，保證可行解存在。

1.2.2 路徑多點(diǎn)預(yù)瞄

為消除基于預(yù)測時(shí)域內(nèi)曲率不變假設(shè)和前后車體間夾角為小角度假設(shè)的誤差模型產(chǎn)生的跟蹤誤差，提高路徑跟蹤精度，本文中直接根據(jù)期望路徑函數(shù)生成路徑點(diǎn)，由預(yù)測時(shí)域內(nèi)期望路點(diǎn)的狀態(tài)軌跡與預(yù)測的車輛行駛狀態(tài)軌跡之差獲得跟蹤誤差，設(shè)計(jì)考慮路徑多點(diǎn)預(yù)瞄誤差的目標(biāo)函數(shù)。此外，基于文獻(xiàn)［6］中方法，為綜合考慮前方路徑轉(zhuǎn)向特性、提高鉸接車輛轉(zhuǎn)彎時(shí)的跟蹤精度，計(jì)當(dāng)前時(shí)刻 為t，以[t，t+N?T]時(shí)域內(nèi)路點(diǎn)位置及[t+k?T，t+(k+N)?T]時(shí)域內(nèi)路點(diǎn)航向角為車輛期望狀態(tài)軌跡，如圖2所示，N為預(yù)測時(shí)域長度，各預(yù)瞄點(diǎn)間的距離為v?T。

圖2 跟蹤誤差示意圖

將預(yù)瞄誤差分為位置偏差ed與航向角偏差eh，其表達(dá)式為

式中：xr(t)為t時(shí)刻預(yù)瞄點(diǎn)的x坐標(biāo)；x(t)為t時(shí)刻車輛的x坐標(biāo)；yr(t)為t時(shí)刻預(yù)瞄點(diǎn)的y坐標(biāo)；y(t)為t時(shí)刻車輛的y坐標(biāo)；θr(t)為t時(shí)刻預(yù)瞄點(diǎn)的航向角；θ(t)為t時(shí)刻的車輛航向角。

1.2.3 MPC優(yōu)化問題

為實(shí)現(xiàn)鉸接車輛的精確跟蹤，設(shè)置以下優(yōu)化目標(biāo)函數(shù)：

式中：ΔU(t)=[Δu(t)，Δu(t+1)，…，Δu(t+Np)]；Δη(t+i|t)=η(t+i|t)-ηref(t+i|t)，表示車輛的跟蹤誤差，可分解為如1.2.2節(jié)中的位置偏差ed和航向角偏差eh；ηref(t+i|t)為t+i時(shí)刻的系統(tǒng)輸出量參考值，由該時(shí)刻參考路徑的x坐標(biāo)、y坐標(biāo)、前車航向角組成；Δu(t+i|t)為在t時(shí)刻預(yù)測的t+i時(shí)刻的系統(tǒng)控制量增量；ε為1.2.1節(jié)中的松弛因子，為找到偏離硬約束最小的可行解，防止約束上下界被松弛因子過分放大，將松弛因子加入目標(biāo)函數(shù)中進(jìn)行限制；Q、R、ρ為權(quán)重矩陣。

設(shè)定新的狀態(tài)量［6］：

可得預(yù)測時(shí)域內(nèi)所有輸出量的表達(dá)式為

將目標(biāo)函數(shù)轉(zhuǎn)變?yōu)闃?biāo)準(zhǔn)二次型，在每個(gè)控制周期求解上述優(yōu)化問題，得到最優(yōu)控制增量序列：

取最優(yōu)序列的第1項(xiàng)作為當(dāng)前時(shí)刻的鉸接車輛控制量增量。如此進(jìn)行滾動(dòng)時(shí)域優(yōu)化控制至跟蹤結(jié)束。

2 仿真分析

為驗(yàn)證所設(shè)計(jì)控制器的性能，建立Adams鉸接車輛模型與Matlab/Simulink進(jìn)行聯(lián)合仿真，如圖3所示，Adams模型各參數(shù)如表1所示，各參數(shù)含義見圖1，輪胎選用UA輪胎，路面選用二維平路面。

圖3 Adams鉸接車輛模型

表1 Adams模型參數(shù)

仿真環(huán)境中設(shè)置螺旋線、左轉(zhuǎn)、雙圓3種仿真工況。鉸接車輛初始X、Y坐標(biāo)、前車航向角、前后車鉸接點(diǎn)處夾角都為0，由于鉸接車輛在不同前后車夾角、路徑曲率等情況具有不同的速度上限，本文中選取縱向速度為3 m/s，可涵蓋大部分轉(zhuǎn)向情況速度上限。且每一種工況都將本文中提出的DMPC算法與文獻(xiàn)［8］中提出的MPC算法進(jìn)行比較。由于本文中只考慮鉸接車輛的橫向控制，故將用于對比的MPC控制器的輸出量稍作調(diào)整，假設(shè)預(yù)測時(shí)域內(nèi)車輛縱向速度不變，只對橫向位置進(jìn)行控制。仿真均使用i5?7200U處理器，采用內(nèi)點(diǎn)法在Matlab中使用quadprog求解器進(jìn)行MPC優(yōu)化問題求解。

仿真中，設(shè)采樣時(shí)間為0.05 s，兩控制器參數(shù)均調(diào)節(jié)至最優(yōu)，其中DMPC權(quán)重矩陣Q=5×1010I，R=I，ρ=1000I，預(yù)測時(shí)域Np=11，k=10?？刂屏縏n的約束設(shè)為-100 kN?m≤u(k+t|t)≤100 kN?m，控制 量 增 量 的 約 束 設(shè) 為-20 kN?m≤Δu(k+t|t)≤20 kN?m。輪胎縱向滑移率根據(jù)仿真測試數(shù)據(jù)采用估計(jì)值0.15，車輛縱向速度變化量在7%以內(nèi)，本文控制器在每一采樣時(shí)間都對車輛最近路點(diǎn)進(jìn)行搜索并更新，以減小其影響。

2.1 螺旋線工況

圖4為路徑跟蹤結(jié)果，可見兩控制器均能控制鉸接車輛跟蹤參考路徑。

圖4 螺旋線工況路徑跟蹤結(jié)果

圖5為位置誤差及控制量隨時(shí)間的變化。基于DMPC和MPC的控制器產(chǎn)生的最大位置誤差分別為0.079和0.208 m，降低了62.0%，平均位置誤差分別為0.041和0.143 m，下降了71.3%?；贒MPC和MPC的兩控制器在仿真過程中最大控制量分別為32.486和48.594 kN·m，平均控制量分別為9.881和13.474 kN·m，平均控制量增量分別為0.770和1.189 kN·m。

圖5 螺旋線工況位置誤差及控制量變化

2.2 左轉(zhuǎn)工況

圖6為路徑跟蹤結(jié)果，可見兩控制器均能控制鉸接車輛跟蹤參考路徑。

圖6 左轉(zhuǎn)工況路徑跟蹤結(jié)果

圖7為位置誤差及控制量隨時(shí)間的變化?；贒MPC和MPC控制器產(chǎn)生的最大位置誤差分別為0.098和0.185 m，降低了47.0%，平均位置誤差分別為0.043和0.094 m，降低了54.3%。基于DMPC和MPC的兩控制器在仿真過程中最大控制量分別為42.515和63.627 kN·m，平均控制量分別為11.599和16.144 kN·m，平均控制量增量分別為0.968和1.502 kN·m。

圖7 左轉(zhuǎn)工況位置誤差及控制量變化

2.3 雙圓工況

圖8為路徑跟蹤結(jié)果，可見兩控制器均能控制鉸接車輛跟蹤參考路徑。

圖8 雙圓工況路徑跟蹤結(jié)果

圖9為位置誤差及控制量隨時(shí)間的變化?；贒MPC和MPC的控制器產(chǎn)生的最大位置誤差分別為0.088和0.165 m，降低了46.7%，基于DMPC和MPC的控制器產(chǎn)生的平均位置誤差分別為0.043和0.096 m，降低了55.2%?；贒MPC和MPC的兩控制器在仿真過程中最大控制量分別為49.264和76.280 kN·m，平均控制量分別為6.235和8.590 kN·m，平均控制量增量分別為0.417和0.697 kN·m。

圖9 雙圓工況位置跟蹤誤差及控制量變化

2.4 統(tǒng)計(jì)結(jié)果

為直觀地體現(xiàn)本文中提出算法與對比算法相比在跟蹤精度上的提高，將各工況下兩控制器產(chǎn)生的最大位置誤差與平均位置誤差進(jìn)行統(tǒng)計(jì)，如圖10和圖11所示。

圖10 各工況最大位置誤差

圖11 各工況平均位置誤差

本文中提出的DMPC算法相比于MPC算法在各工況下的最大位置誤差和平均位置誤差均有效降低，說明本文中設(shè)計(jì)的基于狀態(tài)軌跡的動(dòng)力學(xué)模型和考慮路徑多點(diǎn)預(yù)瞄誤差的控制目標(biāo)有效地提高了路徑跟蹤的控制精度。

3 結(jié)論

針對鉸接車輛路徑跟蹤控制問題，建立了適用于模型預(yù)測控制的鉸接車輛動(dòng)力學(xué)模型，基于狀態(tài)軌跡進(jìn)行線性化，對線性化誤差進(jìn)行補(bǔ)償。在此基礎(chǔ)上，構(gòu)建了考慮多點(diǎn)預(yù)瞄誤差的控制目標(biāo)，并設(shè)計(jì)了基于動(dòng)力學(xué)模型預(yù)測控制的算法。Matlab/Simulink和Adams聯(lián)合仿真結(jié)果顯示，所設(shè)計(jì)的方法可有效提升控制精度和控制平順性。

本文中假定鉸接車輛縱向速度恒定且線性化誤差在預(yù)測時(shí)域內(nèi)不變。未來將進(jìn)一步考慮變速工況與動(dòng)態(tài)線性化誤差，以提升控制方法的實(shí)用性。