基于MMPC的無人汽車軌跡跟蹤控制①

盧紅生 劉 斌 蔣 崢 胡慧中

(*武漢科技大學冶金自動化與檢測技術教育部工程研究中心 武漢 430080) (**冶金工業(yè)過程系統(tǒng)科學湖北省重點實驗室 武漢 430080)

0 引 言

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

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

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

1 車輛動力學模型

實際車輛是一個復雜的運動體，為了方便設計控制器，本文采用2自由度車輛模型[14]。首先做出如下假設：

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

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

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

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

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

(1)

(2)

圖1 車輛2自由度動力學模型

表1 車輛參數(shù)

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

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

Fy=cosδFxf+Fxr

(3)

MZ=lfcosδFxf-lrFxr

(4)

在短時采樣周期內前輪轉角δ非常小，所以可近似認為cosδ≈1，可得：

Fy=Fxf+Fxr

(5)

MZ=lfFxf-lrFxr

(6)

Vy=Vsinβ

(7)

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

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

(8)

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

=V(Δβ+Δψ)

(9)

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

(10)

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

(11)

(12)

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

(13)

圖2 車速隨時間變化的微分示意圖

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

(14)

其中，

為了方便設計多約束模型預測控制器，首先將上述連續(xù)模型進行離散化得到如下離散狀態(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控制中，為保證各個輸出變量的硬約束和軟約束，以及控制量和控制量增量的硬約束均得到滿足，需要對狀態(tài)方程式(15)進行增廣變換。

首先定義狀態(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 多約束模型預測控制器設計

2.1 預測方程

MMPC控制器設計的一個重要環(huán)節(jié)是基于預測模型對車輛在未來若干時刻的行為進行預測。

設當前時刻為k(k≥0)，預測時域為NP，控制時域為Nc(Nc≤NP)。用ΔUm(k)表示在k時刻計算所得未來輸入增量序列；用Ym(k)表示在k時刻計算所得預測輸出序列。

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

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

基于模型預測控制原理[17]，可以推導出預測方程的表達式如下：

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

(17)

其中，

G=

2.2 輸出變量的參考目標值

(1)車輛質心側偏角

(2)車輛橫擺角角速度

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

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

其中βmax為車輛質心側偏角的最大限制。

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

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

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

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

(18)

其中Q、R矩陣為適維加權矩陣。

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

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

(19)

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

(20)

將輸出量約束描述為

(21)

其中δmax和Δδmax分別為車輛轉向角的最大值和單個采樣周期內的最大變化值。

MΔUm(k)≤N

(22)

其中，

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

基于以上預測方程、目標函數(shù)及約束對未來控制量(前輪轉角)增量序列求解。將式(18)形式的目標函數(shù)改寫為二次規(guī)劃標準形式，由式(17)、(18)、(22)得：

(23)

其中，

Em=2(GTQG+R)

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

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

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

(24)

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

(25)

對式(24)求ΔUm的偏導得：

(26)

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

(27)

其中，

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

(28)

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

將(28)進一步展開為

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

…

(29)

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

(30)

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

3 數(shù)值仿真

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

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

(31)

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

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

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

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

然后，用這2種方法求解的MMPC控制器分別進行無人汽車軌跡跟蹤控制實驗。國內外也有較多學者以雙移線軌跡進行無人汽車的軌跡跟蹤能力測試。因此本次實驗也采用雙移線作為參考軌跡。它的表達形式如下：

(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為車的縱向位移。

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

表3 無人汽車仿真實驗參數(shù)表

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

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

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

圖6 控制器A、B的求解時長

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

圖中X、Y軸分別為大地坐標系下的橫、縱坐標。由圖4、圖5的控制效果可看出本文提出的控制器的跟蹤性能較好。由圖6的計算時長對比結果可看出，在實時性方面本文提出的倒序乘子求解法更好一些。需要說明的是，圖7中設計的約束條件(δmax=0.0684 rad)不是實際無人汽車轉向輸入量的物理限制，是為了測試本文設計的倒序乘子求解法在約束條件下的控制效果?？煽闯霰疚奶岢龅姆椒ㄔ跐M足約束條件情況下也能達到較好的控制效果。

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

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

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

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

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

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

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

4 結 論

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