盧紅生 劉 斌 蔣 崢 胡慧中

(*武漢科技大學(xué)冶金自動(dòng)化與檢測(cè)技術(shù)教育部工程研究中心 武漢 430080) (**冶金工業(yè)過程系統(tǒng)科學(xué)湖北省重點(diǎn)實(shí)驗(yàn)室 武漢 430080)

0 引 言

隨著科技發(fā)展，人們對(duì)行車安全的要求越來(lái)越高，希望汽車能在無(wú)人操作的情況下完成自動(dòng)行駛、自動(dòng)避障等一系列自動(dòng)駕駛行為。無(wú)人汽車的概念逐漸成為研究熱點(diǎn)。

對(duì)于無(wú)人汽車，軌跡跟蹤控制是必不可少的。首先無(wú)人汽車是一個(gè)典型非完整約束的系統(tǒng)[1]，其次車輛系統(tǒng)是一個(gè)高度耦合的復(fù)雜非線性系統(tǒng)，并且存在著相當(dāng)多的不確定因素[2]，因此，無(wú)人汽車軌跡跟蹤控制面臨嚴(yán)峻挑戰(zhàn)。國(guó)內(nèi)外學(xué)者已經(jīng)進(jìn)行了相關(guān)研究，提出了用于無(wú)人地面車輛(unmanned ground vehicle，UGV)軌跡跟蹤的多種方法，包括常規(guī)的PID控制，模糊控制[3]，滑?？刂芠4]，模型參考自適應(yīng)控制[5]，神經(jīng)網(wǎng)絡(luò)控制[6]等。需要注意的是，這些控制方法均假設(shè)計(jì)算得出的控制量不會(huì)超過方向盤轉(zhuǎn)角的物理限制，也忽略了車輛與道路的相互作用導(dǎo)致的側(cè)滑可能性。無(wú)人汽車軌跡跟蹤控制必須考慮車輛行駛過程的物理約束特性以及車輛與道路的相互作用。模型預(yù)測(cè)控制(model predictive control，MPC)是解決這一問題很好的方法[7]。2007年Falcone等人[8]將模型預(yù)測(cè)控制分別應(yīng)用于無(wú)人汽車的線性模型和非線性模型的軌跡跟蹤控制。近年來(lái)有很多學(xué)者將非線性預(yù)測(cè)控制應(yīng)用于無(wú)人汽車軌跡跟蹤[9]，雖然能減少跟蹤誤差，但其中的非線性滾動(dòng)時(shí)域優(yōu)化問題導(dǎo)致算法的實(shí)時(shí)性較差。因此，線性預(yù)測(cè)控制方法在無(wú)人汽車控制中仍占優(yōu)勢(shì)。文獻(xiàn)[10]提出一種軌跡優(yōu)化算法，將參考軌跡上車輛橫向制導(dǎo)任務(wù)描述為有約束最優(yōu)控制問題，并通過線性時(shí)變模型預(yù)測(cè)控制方案實(shí)現(xiàn)軌跡跟蹤控制。Kim等人[11]提出了一種基于模型預(yù)測(cè)控制的軌跡跟蹤控制算法，應(yīng)用二次規(guī)劃優(yōu)化方法來(lái)計(jì)算最優(yōu)轉(zhuǎn)向控制輸入。Zanon等人[12]為了驗(yàn)證MPC控制無(wú)人汽車的安全性、平穩(wěn)性，在結(jié)冰路面上進(jìn)行了軌跡跟蹤實(shí)驗(yàn)。以上的研究均強(qiáng)調(diào)無(wú)人汽車在行駛過程中不違反物理限制，從而保證安全性及平穩(wěn)性，但未關(guān)注無(wú)人汽車在載人情況下的舒適性。文獻(xiàn)[13]提及了舒適性的概念，以及舒適性的判斷標(biāo)準(zhǔn)，但并未將此項(xiàng)納入無(wú)人汽車的軌跡跟蹤控制。

本文采用多約束模型預(yù)測(cè)控制(multi-constraint model predictive control，MMPC)方法實(shí)現(xiàn)無(wú)人汽車的軌跡跟蹤控制。將無(wú)人汽車的軌跡跟蹤過程中舒適性標(biāo)準(zhǔn)納入系統(tǒng)模型，將安全性描述為系統(tǒng)約束。為提高有約束滾動(dòng)時(shí)域優(yōu)化問題的求解效率，提出一種倒序乘子搜索算法，并通過CarSim-Matlab平臺(tái)模擬實(shí)車情況下的軌跡跟蹤控制，驗(yàn)證了本文所提出的多約束模型預(yù)測(cè)控制方法的有效性。

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

實(shí)際車輛是一個(gè)復(fù)雜的運(yùn)動(dòng)體，為了方便設(shè)計(jì)控制器，本文采用2自由度車輛模型[14]。首先做出如下假設(shè)：

(1)車輛的縱向速度是恒定的；

(2)忽略懸架運(yùn)動(dòng)，只考慮X-Y軸的平面運(yùn)動(dòng)以及空氣動(dòng)力影響；

(3)把車輛的方向盤轉(zhuǎn)角直接作為系統(tǒng)輸入，并且假定左右輪轉(zhuǎn)角相等。

基于以上假設(shè)，根據(jù)牛頓定律獲得了如圖1所示的車輛的2自由度動(dòng)力學(xué)模型。需要注意的是，圖1中x-y軸是根據(jù)ISO8855規(guī)定的車輛坐標(biāo)系，以車輛質(zhì)心o為原點(diǎn)，x軸指向車輛前方，y軸指向車輛左側(cè)，與大地坐標(biāo)系X-Y不同。圖1中各物理量的含義如表1所示。

對(duì)于較小的輪胎側(cè)偏角，橫向輪胎力可以近似為關(guān)于輪胎側(cè)偏角的線性函數(shù)。前后輪輪胎側(cè)偏角αf、αr和前后輪的橫向輪胎力Fxf、Fxr定義為

(1)

(2)

圖1 車輛2自由度動(dòng)力學(xué)模型

表1 車輛參數(shù)

參數(shù)含義o車輛質(zhì)心Fxf、Fxr前后輪的橫向輪胎力δ前輪轉(zhuǎn)角lf、lr質(zhì)心到前后軸的距離β質(zhì)心側(cè)偏角(速度方向與車身夾角)ψ橫擺角αf、αr前后輪胎側(cè)偏角(車輛輪胎在橫向輪胎力的作用下,使車輪的中心線與垂直于地面的車輪平面錯(cuò)開了一定距離而形成的傾斜角)Cf、Cr前后輪胎側(cè)偏剛度V縱向速度m車的質(zhì)量IZ車輛繞Z軸的轉(zhuǎn)動(dòng)慣量

根據(jù)圖1，可以得到汽車所受到的沿y軸的合外力以及繞質(zhì)心的合力矩分別為

Fy=cosδFxf+Fxr

(3)

MZ=lfcosδFxf-lrFxr

(4)

在短時(shí)采樣周期內(nèi)前輪轉(zhuǎn)角δ非常小，所以可近似認(rèn)為cosδ≈1，可得：

Fy=Fxf+Fxr

(5)

MZ=lfFxf-lrFxr

(6)

Vy=Vsinβ

(7)

+V′cos(β+Δβ)sinΔψ

=V′sin(β+Δβ+Δψ)

(8)

由于β在實(shí)際中較小，Δβ、Δψ均為微分量，且模型假設(shè)車輛縱向速度為恒定大小，取ΔVy為兩時(shí)刻的橫向車速差，則由式(7)和式(8)可得：

=V(Δβ+Δψ)

(9)

由式(9)微分可得車輛的橫向位移加速度ay為

(10)

根據(jù)式(5)、(6)和(10)可得車輛橫向動(dòng)力學(xué)方程：

(11)

(12)

聯(lián)立式(2)、(11)、(12)可以得到車輛橫向運(yùn)動(dòng)以及橫擺運(yùn)動(dòng)的動(dòng)力學(xué)方程為

(13)

圖2 車速隨時(shí)間變化的微分示意圖

根據(jù)上述車輛動(dòng)力學(xué)模型，并取系統(tǒng)控制輸入量uc=δ得到連續(xù)的系統(tǒng)狀態(tài)方程為

(14)

其中，

為了方便設(shè)計(jì)多約束模型預(yù)測(cè)控制器，首先將上述連續(xù)模型進(jìn)行離散化得到如下離散狀態(tài)方程：

X(k+1)=AX(k)+Bu(k)

Y(k)=CX(k)+Du(k)

(15)

其中A、B分別為矩陣Ac、Bc離散后得到的矩陣，C=Cc，D=Dc。

在軌跡跟蹤的MMPC控制中，為保證各個(gè)輸出變量的硬約束和軟約束，以及控制量和控制量增量的硬約束均得到滿足，需要對(duì)狀態(tài)方程式(15)進(jìn)行增廣變換。

首先定義狀態(tài)向量、輸入量的增量形式：

ΔX(k)=X(k)-X(k-1)

Δu(k)=u(k)-u(k-1)

可得：

Y(k)-Y(k-1)=CΔX(k)+DΔu(k)

Za(k+1)=AaZa(k)+BaΔu(k)

Ya(k)=CaZa(k)+DΔu(k)

(16)

其中，

2 多約束模型預(yù)測(cè)控制器設(shè)計(jì)

2.1 預(yù)測(cè)方程

MMPC控制器設(shè)計(jì)的一個(gè)重要環(huán)節(jié)是基于預(yù)測(cè)模型對(duì)車輛在未來(lái)若干時(shí)刻的行為進(jìn)行預(yù)測(cè)。

設(shè)當(dāng)前時(shí)刻為k(k≥0)，預(yù)測(cè)時(shí)域?yàn)镹P，控制時(shí)域?yàn)镹c(Nc≤NP)。用ΔUm(k)表示在k時(shí)刻計(jì)算所得未來(lái)輸入增量序列；用Ym(k)表示在k時(shí)刻計(jì)算所得預(yù)測(cè)輸出序列。

ΔUm(k)=[Δu(k),…,Δu(k+Nc-1)]T

Ym(k)=[Ya(k+1),…,Ya(k+Np)]T

基于模型預(yù)測(cè)控制原理[17]，可以推導(dǎo)出預(yù)測(cè)方程的表達(dá)式如下：

Ym(k)=FZa(k)+GΔUm(k)

(17)

其中，

G=

2.2 輸出變量的參考目標(biāo)值

(1)車輛質(zhì)心側(cè)偏角

(2)車輛橫擺角角速度

(3)車輛橫向位移加速度

按照以上計(jì)算方法得到的參考目標(biāo)值也不能直接應(yīng)用到參考目標(biāo)向量中，因?yàn)檫@些參考目標(biāo)值可能超出了車輛行駛的物理約束，或安全性和舒適性要求引起的約束。因此在參考給定向量化的過程中需要對(duì)每個(gè)時(shí)刻的每個(gè)參考目標(biāo)值都進(jìn)行約束校驗(yàn)，最終得到預(yù)測(cè)控制目標(biāo)函數(shù)中的參考目標(biāo)向量。以下是以車輛質(zhì)心側(cè)偏角為例的校驗(yàn)方法。

其中βmax為車輛質(zhì)心側(cè)偏角的最大限制。

2.3 目標(biāo)函數(shù)及約束

取k時(shí)刻的目標(biāo)函數(shù)為以下形式：

J(k)=[Rr(k)-Ym(k)]TQ[Rr(k)-Ym(k)]

+ΔUm(k)TRΔUm(k)

(18)

其中Q、R矩陣為適維加權(quán)矩陣。

在無(wú)人汽車軌跡跟蹤控制過程中系統(tǒng)主要服從3類約束，即控制量、控制增量和輸出量的約束。將控制量及其增量約束描述為

-C1Δδmax≤ΔUm(k)≤C1Δδmax

(19)

-C1δmax≤C1u(k-1)+C2ΔUm(k)≤C1δmax

(20)

將輸出量約束描述為

(21)

其中δmax和Δδmax分別為車輛轉(zhuǎn)向角的最大值和單個(gè)采樣周期內(nèi)的最大變化值。

MΔUm(k)≤N

(22)

其中，

2.4 二次規(guī)劃求解問題

基于以上預(yù)測(cè)方程、目標(biāo)函數(shù)及約束對(duì)未來(lái)控制量(前輪轉(zhuǎn)角)增量序列求解。將式(18)形式的目標(biāo)函數(shù)改寫為二次規(guī)劃標(biāo)準(zhǔn)形式，由式(17)、(18)、(22)得：

(23)

其中，

Em=2(GTQG+R)

Fm=-2GTQ[Rr(k)-FZa(k)]

顯然，Em為正定對(duì)稱的矩陣。為了最小化不等式約束下的目標(biāo)函數(shù)值，本文考慮下面的拉格朗日表達(dá)式：

+λT(MΔUm(k)-N)

(24)

其中，λ為拉格朗日乘子向量，其維數(shù)為4Nc+8Np。 因此，原問題被轉(zhuǎn)化為如下對(duì)偶問題：

(25)

對(duì)式(24)求ΔUm的偏導(dǎo)得：

(26)

將式(26)代入式(25)可將式(25)指出的對(duì)偶問題轉(zhuǎn)換為以λ為決策變量的二次規(guī)劃問題：

(27)

其中，

本文提出一種倒序乘子求解法求取次優(yōu)解，減少算法的計(jì)算時(shí)間，增強(qiáng)控制系統(tǒng)實(shí)時(shí)性。將式(27)展開為

(28)

其中n=4Nc+8Np為λ的維數(shù)，hij,ki分別為矩陣Hm,Km中的元素，且由式(27)可得Hm為正定對(duì)稱矩陣。

將(28)進(jìn)一步展開為

+(h23λ3+h24λ4+…+h2nλn)λ2+k2λ2

…

(29)

由式(29)看出，JL(k)可分解為n個(gè)開口向上的二次函數(shù)，各二次函數(shù)的對(duì)稱軸λi(i∈(1,n))對(duì)應(yīng)該函數(shù)的最小值。式(29)中前n-1個(gè)二次函數(shù)的對(duì)稱軸計(jì)算如下：

(30)

如果λn為一確定值，式(30)從下往上逆序求解可得由對(duì)稱軸組成的λ向量。因?yàn)棣薾可唯一確定λi(i∈(1,n-1))，從而將求解多維向量λ的問

3 數(shù)值仿真

3.1 倒序乘子求解法求解二次規(guī)劃問題

在進(jìn)行無(wú)人汽車軌跡跟蹤控制仿真前，首先分別用本文提出的倒序乘子求解法與有效集法對(duì)二次規(guī)劃問題求解，從而對(duì)兩者的求解精度以及求解速度進(jìn)行對(duì)比。實(shí)驗(yàn)采用Matlab仿真平臺(tái)，計(jì)算機(jī)CPU頻率為2.3 GHz。求解的二次規(guī)劃問題形式為

(31)

其中，JT為目標(biāo)函數(shù)值，X為二次規(guī)劃問題的解序列，ET、FT、MT及NT為系數(shù)矩陣。

圖3 倒序乘子求解法流程圖

表2所示為本文倒序乘子求解法與有效集法的求解結(jié)果對(duì)比。與有效集法相比，本文倒序乘子求解法是將二次規(guī)劃問題轉(zhuǎn)化為以一個(gè)拉格朗日乘子為決策變量的單變量尋優(yōu)問題。而有效集法則是從初始解出發(fā)不斷調(diào)整有效集，每次有效集的調(diào)整都需要按照多項(xiàng)規(guī)則判斷，最終求得最優(yōu)解。因此本文算法可大幅提高求解效率，由表2看出本文倒序乘子求解法的計(jì)算速度是有效集法的4倍左右。另外，本文求取最優(yōu)解與有效集法求解精度接近。上表中目標(biāo)函數(shù)值JT存在負(fù)數(shù)的原因是，在性能指標(biāo)函數(shù)轉(zhuǎn)化為式(31)的二次規(guī)劃標(biāo)準(zhǔn)形式的過程中去掉了不影響解序列的無(wú)關(guān)正項(xiàng)。

表2 二次規(guī)劃求解結(jié)果對(duì)比

然后，用這2種方法求解的MMPC控制器分別進(jìn)行無(wú)人汽車軌跡跟蹤控制實(shí)驗(yàn)。國(guó)內(nèi)外也有較多學(xué)者以雙移線軌跡進(jìn)行無(wú)人汽車的軌跡跟蹤能力測(cè)試。因此本次實(shí)驗(yàn)也采用雙移線作為參考軌跡。它的表達(dá)形式如下：

(32)

式中，z1=(2.4/25)×(X-27.19)-1.2，z2=(2.4/21.95)×(X-56.46)-1.2，dy1=4.05，dy2=5.7，dx1=25，dx2=21.95，X為車的縱向位移。

取預(yù)測(cè)時(shí)域Np=20，控制時(shí)域Nc=5，權(quán)重矩陣Q=INp×Np、R=50×INc×Nc，控制器采樣周期T=0.05 s，車速為36 km/h，用本文提出的MMPC軌跡跟蹤控制對(duì)式(14)的2自由度線性汽車動(dòng)力學(xué)模型進(jìn)行控制，模型具體參數(shù)見表3。令基于倒序乘子求解法的MMPC控制器為控制器A，有效集法求解的為控制器B。

表3 無(wú)人汽車仿真實(shí)驗(yàn)參數(shù)表

圖4、圖5分別為用本文倒序乘子求解法和有效集法求解的MMPC控制器對(duì)無(wú)人汽車軌跡跟蹤系統(tǒng)的仿真結(jié)果。圖6為2種求解方法的計(jì)算時(shí)長(zhǎng)。圖7為約束條件下基于MMPC(倒序乘子求解法)的無(wú)人汽車軌跡跟蹤系統(tǒng)仿真圖。

圖4 基于控制器A的無(wú)人汽車軌跡跟蹤系統(tǒng)仿真圖

圖5 基于控制器B的無(wú)人汽車軌跡跟蹤系統(tǒng)仿真圖

圖6 控制器A、B的求解時(shí)長(zhǎng)

圖7 約束條件下控制器A的無(wú)人汽車軌跡跟蹤系統(tǒng)仿真圖

圖中X、Y軸分別為大地坐標(biāo)系下的橫、縱坐標(biāo)。由圖4、圖5的控制效果可看出本文提出的控制器的跟蹤性能較好。由圖6的計(jì)算時(shí)長(zhǎng)對(duì)比結(jié)果可看出，在實(shí)時(shí)性方面本文提出的倒序乘子求解法更好一些。需要說(shuō)明的是，圖7中設(shè)計(jì)的約束條件(δmax=0.0684 rad)不是實(shí)際無(wú)人汽車轉(zhuǎn)向輸入量的物理限制，是為了測(cè)試本文設(shè)計(jì)的倒序乘子求解法在約束條件下的控制效果。可看出本文提出的方法在滿足約束條件情況下也能達(dá)到較好的控制效果。

3.2 CarSim-Matlab環(huán)境下的無(wú)人汽車軌跡跟蹤控制系統(tǒng)仿真

為了驗(yàn)證本文提出的控制器的有效性，必須進(jìn)行實(shí)車實(shí)驗(yàn)，但考慮到安全性和經(jīng)濟(jì)性等問題，本文采用CarSim-Matlab仿真來(lái)模擬實(shí)車的軌跡跟蹤。CarSim是專門針對(duì)車輛動(dòng)力學(xué)的仿真軟件，可以詳細(xì)定義整車各系統(tǒng)的特性參數(shù)。

將表3中汽車參數(shù)以及控制器所需的狀態(tài)量設(shè)置到CarSim中，并在CarSim-Matlab平臺(tái)上按圖8所示的系統(tǒng)框圖搭建基于MMPC控制器的無(wú)人汽車軌跡跟蹤控制系統(tǒng)。參考軌跡仍為雙移線軌跡，設(shè)定預(yù)測(cè)時(shí)域Np=25，控制時(shí)域Nc=7，權(quán)重矩陣Q=INp×Np、R=50×INc×Nc，控制器周期T=0.05 s，車速為72 km/h，得到相應(yīng)的控制狀態(tài)和跟蹤曲線見圖9和圖10。

圖8 基于MMPC控制器的無(wú)人汽車軌跡跟蹤控制系統(tǒng)框圖

圖9 無(wú)人汽車行駛過程各狀態(tài)圖

圖10 基于MMPC控制器的無(wú)人汽車軌跡跟蹤控制系統(tǒng)仿真圖

綜上，本文設(shè)計(jì)的MMPC控制器能使用于無(wú)人汽車軌跡跟蹤控制，具有良好的實(shí)時(shí)性，且無(wú)人汽車行駛過程中的安全性、舒適性均可以保障。

4 結(jié) 論

本文著重研究了基于MMPC方法的無(wú)人汽車軌跡跟蹤控制，及車輛行駛過程中的安全性、舒適性問題。為解決行車舒適性問題，本文將橫向位移加速度納入系統(tǒng)模型，并加以約束；為改善算法實(shí)時(shí)性，提出了倒序乘子求解法對(duì)二次規(guī)劃問題求解。通過與有效集法的對(duì)比實(shí)驗(yàn)，驗(yàn)證了本文倒序乘子求解法的有效性。最終，在CarSim-Matlab平臺(tái)實(shí)現(xiàn)了無(wú)人汽車軌跡跟蹤控制，檢驗(yàn)了本文提出的MMPC控制器的實(shí)用性。