基于自適應(yīng)MPC算法的軌跡跟蹤控制研究

王 銀，張灝琦，孫前來(lái)，李小松，孫志毅

太原科技大學(xué) 電子信息工程學(xué)院，太原030024

自動(dòng)駕駛不僅可以減少因操作失誤而引起的交通事故的發(fā)生率，而且能夠提高貨物的運(yùn)輸效率，因而自動(dòng)駕駛技術(shù)成為當(dāng)前國(guó)內(nèi)外自動(dòng)控制領(lǐng)域研究的熱點(diǎn)[1]。

車(chē)輛的軌跡跟蹤控制環(huán)節(jié)是自動(dòng)駕駛系統(tǒng)中重要的一部分。軌跡跟蹤是指通過(guò)控制車(chē)輛轉(zhuǎn)向，使得車(chē)輛可以按照給定參考軌跡的路線(xiàn)行駛[2]。

目前，智能車(chē)輛的軌跡跟蹤控制問(wèn)題受到了國(guó)內(nèi)外廣大專(zhuān)家學(xué)者的重視。常用的軌跡跟蹤控制算法有魯棒控制、滑膜控制、模型預(yù)測(cè)控制（MPC）等。文獻(xiàn)[3]提出了一種增益調(diào)度、魯棒、共享的控制器來(lái)進(jìn)行軌跡跟蹤。文獻(xiàn)[4]中的控制器采用了模糊CMAC 控制方法，該控制器可以消除擾動(dòng)對(duì)系統(tǒng)的影響，使得機(jī)器人可以較為精確地跟蹤參考軌跡，并且一定程度上提高了行駛穩(wěn)定性。章仁燮等[5]采用了一種條件積分算法的魯棒控制方法，控制器由運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)兩部分構(gòu)成，保證側(cè)向偏差和橫擺角速度的漸進(jìn)穩(wěn)定，使得試驗(yàn)車(chē)輛可以實(shí)現(xiàn)跟蹤期望軌跡的目標(biāo)。文獻(xiàn)[6]提出的滑膜綜合控制器可以降低建模不確定性和擾動(dòng)時(shí)的魯棒性。而MPC相比于其他控制算法來(lái)說(shuō)，最大的優(yōu)勢(shì)是多種約束可以在控制過(guò)程中被添加進(jìn)去[7]。大多數(shù)學(xué)者都把提高軌跡跟蹤精度作為主要控制目標(biāo)[8-10]。其中，文獻(xiàn)[11]把車(chē)輛行駛的穩(wěn)定性也作為了控制目標(biāo)，通過(guò)把前輪轉(zhuǎn)角作為控制量，根據(jù)MPC設(shè)計(jì)相應(yīng)的控制器，可以實(shí)現(xiàn)較為穩(wěn)定的跟蹤參考軌跡。文獻(xiàn)[12]提出了一種基于MPC 的新型路徑跟蹤框架，是一個(gè)多層控制系統(tǒng)，包括三個(gè)具有固定速度的路徑跟蹤控制器和一個(gè)速度決策控制器，可以根據(jù)路徑信息主動(dòng)調(diào)整縱向速度，且有良好的跟蹤精度。文獻(xiàn)[13]采用運(yùn)動(dòng)學(xué)模型，基于MPC方法設(shè)計(jì)了考慮車(chē)輛動(dòng)力學(xué)、執(zhí)行器和狀態(tài)約束的路徑跟蹤控制器，以前輪轉(zhuǎn)向角為控制變量，使AGV的實(shí)際軌跡保持在一定的區(qū)域內(nèi)并滿(mǎn)足安全要求。文獻(xiàn)[14]設(shè)計(jì)了集成主動(dòng)前輪轉(zhuǎn)向和差動(dòng)制動(dòng)的MPC 控制器，實(shí)現(xiàn)了車(chē)輛的橫向穩(wěn)定控制。劉凱等[15]針對(duì)越野地形的復(fù)雜環(huán)境，建立了特殊的車(chē)輛動(dòng)力學(xué)模型，并且添加了車(chē)輛側(cè)傾安全約束。然后在考慮上述模型及約束條件的基礎(chǔ)上，設(shè)計(jì)了MPC 軌跡跟蹤控制器。眾多專(zhuān)家學(xué)者對(duì)MPC 軌跡跟蹤控制方面都做了大量的研究，且都有著不錯(cuò)的跟蹤精度。但大多數(shù)都是處于定速且良好工況，然而實(shí)際應(yīng)用中，車(chē)輛經(jīng)常會(huì)處于加速減速且低附著的工況中，使得車(chē)速變化幅度大，極易造成軌跡跟蹤失敗，發(fā)生安全事故。

因此針對(duì)智能車(chē)輛在低附著且變速條件下跟蹤控制精確性和穩(wěn)定性不高的問(wèn)題，提出一種新型控制框架。通過(guò)添加側(cè)偏角約束，以及設(shè)計(jì)預(yù)測(cè)時(shí)域（Hp）控制器，建立了自適應(yīng)的MPC 控制器，該控制器能夠根據(jù)車(chē)速的變化，實(shí)時(shí)產(chǎn)生Hp。在CarSim 與MATLAB/Simulink構(gòu)建的聯(lián)合仿真實(shí)驗(yàn)平臺(tái)進(jìn)行試驗(yàn)，仿真實(shí)驗(yàn)結(jié)果表明自適應(yīng)的MPC控制器能夠提高智能車(chē)輛在低附著且變速條件下跟蹤控制的精確性和穩(wěn)定性。

1 車(chē)輛動(dòng)力學(xué)模型

車(chē)輛的動(dòng)力學(xué)模型是要作為控制器的預(yù)測(cè)模型使用，需要在較為準(zhǔn)確地描述車(chē)輛動(dòng)力學(xué)過(guò)程的基礎(chǔ)上盡可能地進(jìn)行簡(jiǎn)化，以減少控制算法的計(jì)算量。因此在車(chē)輛動(dòng)力學(xué)建模時(shí)，要進(jìn)行以下理想化假設(shè)：（1）忽略車(chē)輛垂向運(yùn)動(dòng)。（2）忽略車(chē)輛的懸架特性。（3）忽略輪胎橫、縱向耦合關(guān)系。（4）不考慮輪胎的橫向載荷轉(zhuǎn)移，以及認(rèn)為輪距相對(duì)于轉(zhuǎn)彎半徑可以忽略不計(jì)，使用單軌模型來(lái)描述車(chē)輛。（5）忽略空氣阻力。

基于上述假設(shè)可以得到車(chē)輛的動(dòng)力學(xué)模型，如圖1所示。

圖1 中，F(xiàn)lf、Flr為前后輪受到的縱向力；Fcf、Fcr為前后輪受到的側(cè)向力；Fxf、Fxr為前后輪受到的x方向的力；Fyf、Fyr為前后輪受到的y方向的力；δf為前輪偏轉(zhuǎn)角；αf為前輪側(cè)偏角；φ為橫擺角速度。

圖1 車(chē)輛單軌模型

根據(jù)牛頓第二定律、車(chē)身坐標(biāo)轉(zhuǎn)換、輪胎受力，可得出車(chē)輛動(dòng)力學(xué)模型。

2 軌跡跟蹤控制

2.1 軌跡跟蹤控制架構(gòu)

軌跡跟蹤控制架構(gòu)主要由3 部分組成：MPC 控制器、CarSim車(chē)輛模型、Hp控制器。其中MPC控制器是實(shí)現(xiàn)軌跡跟蹤的核心部分，遵循預(yù)測(cè)模型、滾動(dòng)優(yōu)化和反饋校正的基本原理[16]。MPC 控制器根據(jù)參考軌跡和目標(biāo)函數(shù)求解出最優(yōu)的控制量δf，輸入到被控車(chē)輛模型中，得到當(dāng)前的狀態(tài)，作為預(yù)測(cè)模型的輸入，預(yù)測(cè)時(shí)域控制器根據(jù)當(dāng)前車(chē)速v求解出對(duì)應(yīng)預(yù)測(cè)時(shí)域Hp，輸入到線(xiàn)性誤差預(yù)測(cè)模型中，然后重復(fù)上述過(guò)程，最終實(shí)現(xiàn)軌跡跟蹤。圖2為基于自適應(yīng)MPC軌跡跟蹤控制器。

圖2 基于自適應(yīng)MPC軌跡跟蹤控制器

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

2.2.1 線(xiàn)性誤差模型的建立

根據(jù)車(chē)輛非線(xiàn)性動(dòng)力學(xué)模型可建立狀態(tài)空間表達(dá)式：

在t時(shí)刻將式（2）在工作點(diǎn)(ξ(t),μ(t-1))處線(xiàn)性化并離散化，得到離散的線(xiàn)性時(shí)變系統(tǒng)，如下：

根據(jù)式（3）、（5）設(shè)計(jì)模型預(yù)測(cè)控制器，首先將控制輸入由控制量μ(t)經(jīng)過(guò)數(shù)學(xué)變換轉(zhuǎn)化為控制增量Δμ(t)，再做如下設(shè)定：

式中，0m×n是m×n維0矩陣；0m是m維列矩陣；Im是m維單位矩陣。

得到線(xiàn)性誤差表達(dá)式：

進(jìn)一步地，可以得到在預(yù)測(cè)時(shí)域內(nèi)Hp系統(tǒng)輸出量可用如下公式計(jì)算：

由式（8）可知預(yù)測(cè)時(shí)域Hp內(nèi)輸出量的預(yù)測(cè)值Ytr(t)可通過(guò)當(dāng)前t時(shí)刻已知的狀態(tài)量ξ(t|t)、上一時(shí)刻的控制量μ(t-1|t)以及控制時(shí)域Hc內(nèi)未知的控制增量ΔU(t)計(jì)算得到。

2.2.2 目標(biāo)函數(shù)的設(shè)計(jì)

由于系統(tǒng)的控制增量ΔU(t)是未知的，必須通過(guò)設(shè)定合理的優(yōu)化目標(biāo)進(jìn)行求解，通過(guò)求解目標(biāo)函數(shù)，可以得到一系列最優(yōu)控制增量，并將序列中的第一個(gè)元素加入到狀態(tài)量和輸出量的求解中。

在預(yù)測(cè)控制中，一般考慮如下目標(biāo)函數(shù)：

其中，Hp和Hc分別為預(yù)測(cè)時(shí)域和控制時(shí)域；ηref(t+i|t)為參考輸出量；Q、R、ρ為權(quán)重矩陣，ε為松弛因子。

式中第一項(xiàng)體現(xiàn)了控制系統(tǒng)對(duì)參考軌跡的跟蹤能力，第二項(xiàng)反映了系統(tǒng)對(duì)控制量平穩(wěn)變化的要求，第三項(xiàng)反映了系統(tǒng)對(duì)控制量最值的要求。為了防止出現(xiàn)無(wú)解的情況，因此在目標(biāo)函數(shù)中加入了松弛因子ε。

2.2.3 約束條件

為了保證系統(tǒng)對(duì)期望軌跡的精確跟蹤，以及保證行駛的穩(wěn)定性，需要對(duì)控制量、控制增量進(jìn)行約束，約束條件表達(dá)式如下：

此外，為了解決車(chē)輛在低附著環(huán)境下行駛時(shí)因側(cè)滑而造成軌跡跟蹤失敗，應(yīng)添加側(cè)偏角約束。

輪胎側(cè)偏角α與質(zhì)心橫向速度、質(zhì)心縱向速度、橫擺角速度以及前輪轉(zhuǎn)角δf之間的關(guān)系如下：

將上式進(jìn)行線(xiàn)性化與離散化處理，得到離散線(xiàn)性時(shí)變系統(tǒng)，如下：

然后對(duì)側(cè)偏角添加約束條件：

下面對(duì)比未添加側(cè)偏角約束控制器與添加側(cè)偏角約束控制器進(jìn)行軌跡跟蹤的效果。本文參考軌跡如圖3所示。表達(dá)式如式（13）：

圖3 參考軌跡和參考航向

本文車(chē)輛參數(shù)如表1 所示，MPC 控制器參數(shù)如表2所示。

表1 車(chē)輛參數(shù)

表2 控制器參數(shù)

在Carsim 軟件中設(shè)置路面附著系數(shù)為0.2，速度設(shè)置為36 km/h，控制器中預(yù)測(cè)時(shí)域設(shè)置為8。下面對(duì)添加側(cè)偏角約束的MPC控制器和未添加側(cè)偏角約束的MPC控制器分別進(jìn)行軌跡跟蹤控制測(cè)試，仿真結(jié)果如圖4。

在上面的仿真結(jié)果中，根據(jù)圖4（a）和圖4（b）可知，控制器添加側(cè)偏角約束能夠有效提升車(chē)輛軌跡跟蹤的精度，其中未添加側(cè)偏角約束的最大橫向位置偏差為0.386 3 m，而添加了側(cè)偏角約束的最大橫向位置偏差為0.180 2 m。根據(jù)圖4（c）可知添加側(cè)偏角約束能夠有效提升車(chē)輛的行駛穩(wěn)定性，其中未添加側(cè)偏角約束的最大質(zhì)心側(cè)偏角和橫擺角速度分別為3.390 9°和14.081 4（°）/s，而添加了側(cè)偏角約束的最大質(zhì)心側(cè)偏角和橫擺角速度分別為0.729 5°和8.027 9（°）/s。綜上所述控制器添加側(cè)偏角約束可以有效提高車(chē)輛軌跡跟蹤控制的行駛穩(wěn)定性。

2.3 Hp 控制器的設(shè)計(jì)

2.3.1 Hp 對(duì)軌跡跟蹤控制的影響

預(yù)測(cè)時(shí)域作為MPC 的重要參數(shù)之一，表示控制器對(duì)未來(lái)狀態(tài)的預(yù)測(cè)程度。若車(chē)速一定時(shí)，由式（8）以及式（9）中的第一項(xiàng)可知，當(dāng)Hp較大時(shí)，控制器可以預(yù)測(cè)較遠(yuǎn)的距離，但會(huì)產(chǎn)生較大的誤差，降低軌跡跟蹤精度；當(dāng)Hp較小時(shí)，在文獻(xiàn)[17]提出車(chē)輛轉(zhuǎn)向輪的轉(zhuǎn)向角速度約束的影響下，車(chē)輛又會(huì)因無(wú)法及時(shí)轉(zhuǎn)向而造成軌跡跟蹤失敗。所以不同的預(yù)測(cè)時(shí)域產(chǎn)生的控制效果不同。

圖4（a）橫向位置和橫向偏差

圖4（b）航向角和航向偏差

圖4（c）質(zhì)心側(cè)偏角和橫擺角速度

下面在添加側(cè)偏角約束的基礎(chǔ)上，分析不同恒定速度下Hp取不同的值對(duì)軌跡跟蹤控制的影響。通過(guò)CarSim和MATLAB/Simulink聯(lián)合仿真進(jìn)行了四組定速下不同Hp的軌跡跟蹤效果的測(cè)試，其中Hp也采用了四組。

（1）試驗(yàn)一：36 km/h 下Hp取4 組不同定值的軌跡跟蹤。

在試驗(yàn)一的仿真結(jié)果中，由圖5（c）可知，車(chē)速為36 km/h時(shí)，Hp取不同定值時(shí)軌跡跟蹤控制器操作穩(wěn)定性均較為良好。但隨著Hp的增大，軌跡跟蹤的精確性隨之降低。由圖5（a）、圖5（b）可知，Hp=8 時(shí)，橫向偏差和航向偏差最小，Hp=26 時(shí)，橫向偏差和航向偏差最大。

圖5（a）36 km/h下的橫向位置和橫向偏差

圖5（b）36 km/h下的航向角和航向偏差

圖5（c）36 km/h下的質(zhì)心側(cè)偏角和橫擺角速度

同理，分別進(jìn)行72 km/h、90 km/h、108 km/h 下Hp取不同定值的軌跡跟蹤。

（2）試驗(yàn)二：72 km/h 下Hp取4 組不同定值的軌跡跟蹤，跟蹤情況見(jiàn)表3。

表3 試驗(yàn)二跟蹤情況

在試驗(yàn)二的仿真結(jié)果中，車(chē)速為72 km/h時(shí)，Hp=8時(shí)，雖然橫向偏差和航向偏差最小，但質(zhì)心側(cè)偏角和橫擺角速度的數(shù)值最大，說(shuō)明有明顯抖動(dòng)，軌跡跟蹤控制器操作穩(wěn)定性較差。Hp=20 和Hp=26 時(shí)，雖然質(zhì)心側(cè)偏角和橫擺角速度較小，但軌跡跟蹤的核心參數(shù)最大橫向偏差較大。而Hp=14 時(shí)，雖然橫向偏差相較Hp=8來(lái)講要大，但在可接受范圍內(nèi)，且操作穩(wěn)定性良好。

（3）試驗(yàn)三：90 km/h 下Hp取4 組不同定值的軌跡跟蹤，跟蹤情況見(jiàn)表4。

表4 試驗(yàn)三跟蹤情況

在試驗(yàn)三的仿真結(jié)果中，當(dāng)車(chē)速為90 km/h 時(shí)，Hp=8 和Hp=14 時(shí)，軌跡跟蹤失敗。Hp=20 和Hp=26時(shí)，質(zhì)心側(cè)偏角和橫擺角速度相差不大，都有良好行駛穩(wěn)定性，但Hp=20 時(shí)，軌跡跟蹤的核心參數(shù)最大橫向偏差最小，說(shuō)明與參考軌跡的誤差更小，有更好的跟蹤精確性。

（4）試驗(yàn)四：108 km/h下Hp取4組不同定值的軌跡跟蹤，跟蹤情況見(jiàn)表5。

表5 試驗(yàn)四跟蹤情況

在試驗(yàn)四的仿真結(jié)果中，車(chē)速為108 km/h 時(shí)，由仿真結(jié)果可知，Hp分別為8、14、20 時(shí)軌跡跟蹤失敗。而Hp=26 時(shí)，有良好的跟蹤精確性和行駛穩(wěn)定性。

綜上所述，綜合考慮跟蹤精確性和行駛穩(wěn)定性，可以得出結(jié)論低速時(shí)采用較小的Hp，而高速情況下采用較大的Hp有良好的跟蹤精確性和行駛穩(wěn)定性。這是因?yàn)樵谀Ｐ皖A(yù)測(cè)算法中，當(dāng)Hp較大時(shí)，控制器會(huì)考慮更多的未來(lái)軌跡趨勢(shì)變化，造成跟蹤精度下降。

2.3.2 Hp 控制器的設(shè)計(jì)

通過(guò)上一節(jié)的試驗(yàn)，可知低速工況下，控制器應(yīng)采用較小的Hp，高速工況下，控制器應(yīng)采用較大的Hp。然而在軌跡跟蹤過(guò)程中，行駛工況有時(shí)會(huì)有變速的要求，那么此時(shí)控制器若采用固定的Hp將會(huì)無(wú)法滿(mǎn)足低速、高速、變速的行駛工況。

因此，下面將設(shè)計(jì)Hp的控制器，能夠根據(jù)車(chē)速度的變化，實(shí)時(shí)產(chǎn)生Hp。

根據(jù)上一節(jié)的試驗(yàn)分析，下面根據(jù)4組速度對(duì)應(yīng)的Hp，設(shè)計(jì)變Hp控制器，如表6所示。

表6 不同速度對(duì)應(yīng)的Hp

具體控制規(guī)律如式（14）所示，是通過(guò)表6選取的四組數(shù)據(jù)進(jìn)行三次多項(xiàng)式擬合得到。圖6 為不同速度對(duì)應(yīng)的Hp。

圖6 不同速度對(duì)應(yīng)的Hp

3 仿真分析

前面驗(yàn)證了添加側(cè)偏角約束可以提高車(chē)輛在低附著路面軌跡跟蹤的精度與行駛穩(wěn)定性，以及分析了不同速度下Hp取不同值對(duì)軌跡跟蹤控制的影響。在添加側(cè)偏角約束的基礎(chǔ)上結(jié)合變Hp控制器，共同構(gòu)成了自適應(yīng)MPC軌跡跟蹤控制器，下面對(duì)自適應(yīng)MPC軌跡跟蹤控制器在變速且低附著環(huán)境下進(jìn)行仿真分析。通過(guò)CarSim 和MATLAB/Simulink 搭建聯(lián)合仿真平臺(tái)，地面附著系數(shù)設(shè)置為0.2，為了模擬變速環(huán)境，CarSim中設(shè)置為超車(chē)工況，超車(chē)工況速度變化如圖7 所示，車(chē)輛參數(shù)和控制器參數(shù)如表1和表2所示。

圖7 超車(chē)工況下速度的變化

在上面的仿真結(jié)果中，由圖8（a）知Hp分別為8、14、20 時(shí)軌跡跟蹤失敗。Hp=26 時(shí)最大橫向偏差為0.457 3 m，而自適應(yīng)的MPC 控制器最大橫向偏差為0.385 6 m。由圖8（b）可知Hp=26 的最大航向偏差為10.771 1°，而自適應(yīng)的MPC 控制器的最大航向偏差為2.236 4°。由圖8（c）可知Hp=26 最大質(zhì)心側(cè)偏角和最大橫擺角速度分別為11.017 3°和39.867 0（°）/s，而自適應(yīng)的MPC控制器最大質(zhì)心側(cè)偏角和最大橫擺角速度分別為3.418 9°和26.653 1（°）/s。綜上所述，基于自適應(yīng)的MPC控制器可以有效提高跟蹤精度和行駛穩(wěn)定性。

圖8（c） 超車(chē)工況下的質(zhì)心側(cè)偏角和橫擺角速度

圖8（b） 超車(chē)工況下的航向角和航向偏差

圖8（a） 超車(chē)工況下的橫向位置和橫向偏差

4 結(jié)論

本文研究了車(chē)輛在低附著且變速的環(huán)境下軌跡跟蹤控制問(wèn)題。以車(chē)輛動(dòng)力學(xué)模型為基礎(chǔ)，設(shè)計(jì)了自適應(yīng)的MPC控制器，分別由添加側(cè)偏角的MPC控制器和變Hp控制器組成。通過(guò)MATLAB/Simulink 和Carsim 聯(lián)合仿真，所得結(jié)論如下：

（1）仿真數(shù)據(jù)表明，添加側(cè)偏角使得質(zhì)心側(cè)偏角最大偏差減小了2.661 4°，橫擺角速度最大偏差減小了6.053 6（°）/s，有效提高車(chē)輛行駛的穩(wěn)定性。

（2）采用自適應(yīng)的MPC 控制器，相比無(wú)法根據(jù)車(chē)速更新Hp的控制器，使得最大航向偏差減小了8.534 7°，質(zhì)心側(cè)偏角減小了7.598 4°，橫擺角速度減小了13.215 7（°）/s，有效提高了軌跡跟蹤精度和行駛穩(wěn)定性。