基于自適應MPC的無人駕駛車輛軌跡跟蹤控制

梁忠超， 張 歡， 趙 晶， 王永富

(東北大學 機械工程與自動化學院， 遼寧 沈陽 110819)

我國汽車數(shù)量逐年增加，所帶來的社會問題愈加明顯.我國每年因為駕駛員操作不當造成死亡人數(shù)高達數(shù)十萬，而且解決城市交通擁擠問題也迫在眉睫.對于未來的智能交通系統(tǒng)，無人駕駛車輛的迅速發(fā)展已成為一種必然趨勢.無人駕駛車輛涉及的技術領域廣泛，其中主要有環(huán)境探知、定位導航、行為決策、路徑規(guī)劃與跟蹤控制等.

無人駕駛實現(xiàn)的重要環(huán)節(jié)是跟蹤控制，在滿足安全條件下，控制算法應該最大程度保證無人車的跟蹤精度與行駛穩(wěn)定性.實際的車輛模型是高度非線性的，致使無人駕駛車輛的精確控制很難實現(xiàn).控制技術的主要目標就是為了提高自適應性、魯棒性以及精確度，所以控制算法的應用尤為重要.控制技術主要分為側向控制、縱向控制與雙向綜合控制[1-2].側向控制包括模型預測控制(MPC)[3]、純追蹤軌跡跟蹤控制[4]、線性二次高斯控制(LQG)[5]、基于線性矩陣不等式方法的魯棒增益調度自動轉向控制[6]、H∞魯棒控制[7-8]等.由于模型預測控制能在控制過程中考慮多種約束，所以應用廣泛.文獻[9]提出一種基于多種約束的最優(yōu)軌跡跟蹤控制策略，通過連續(xù)線性化誤差模型，利用二次規(guī)劃求解MPC，獲得良好的軌跡跟蹤精度.文獻[10]提出了一種基于隨機約束情形的MPC控制方法，在解決眾多決策變量的大規(guī)模問題上取得良好的效果.文獻[11]應用模型預測控制算法設計出一種新型的向后駕駛的轉向控制器，與斯坦利轉向控制算法相比，它有較好的魯棒性與跟蹤精度.

綜合目前的研究成果，車輛橫向控制研究較少地考慮無人車輛模型失配、時變、非線性以及狀態(tài)測量偏差等問題帶來的影響.無人駕駛車輛實際工況復雜多變，隨時受隨機性因素影響.因此本文采用自適應MPC算法來解決上述問題，并搭建軌跡跟蹤控制仿真模型，驗證控制效果.

1 車輛三自由度動力學建模

本文選用具有x，y，Iz3個自由度的動力學模型.如圖1所示，坐標系{o-xyz}為無人駕駛車輛質心處的坐標系，坐標系{O-XYZ}為全局大地坐標系，均滿足右手法則.則無人駕駛車輛四輪簡化模型[3]與輪胎的受力情況定義如下.

圖1 車輛動力學模型

根據(jù)牛頓第二定律，計算得到x軸、y軸與繞z軸3個方向的受力平衡方程，其中，F(xiàn)x,Lf，F(xiàn)y,Lf，F(xiàn)x,Rf，F(xiàn)y,Rf，F(xiàn)x,Lr，F(xiàn)y,Lr，F(xiàn)x,Rr，F(xiàn)y,Rr分別為4個輪胎所受縱向與側向的力，其動力學方程分別如下所示：

(1)

坐標系{o-xy}與慣性坐標系{O-XY}之間的轉換關系公式：

(2)

應用線性輪胎模型可得到輪胎的縱向力與側向力[7]分別為

(3)

式中：Cl，Cc分別為輪胎的縱向與側向的剛度.

經(jīng)簡化與推導，得到車輛三自由度動力學模型為

(4)

式中：Ccf，Ccr，Clf，Clr分別為前、后輪胎的側向剛度與縱向剛度；sf，sr為前、后輪胎的滑移率；δf為前輪轉角.

2 軌跡跟蹤控制器設計

由文獻[3]推導出的非線性控制器可知，非線性模型以及復雜的控制器約束對于無人駕駛車輛的求解具有一定的難度.因此為了確保車輛在實際工況中求解的實時性，本文采用線性控制器對控制問題進行描述，并且把狀態(tài)量偏差引入預測模型中，不斷更新狀態(tài)工作點，對系統(tǒng)模型進行線性化.

非線性動力學系統(tǒng)為

(5)

式中：f為系統(tǒng)的狀態(tài)轉移函數(shù)；h為系統(tǒng)的輸出函數(shù)；x(t)為n維的狀態(tài)變量；u(t)為m維的控制變量；η(t)為p維的輸出變量.

對系統(tǒng)進行線性化與離散化：

(6)

式中：k為采樣時刻；d(t)，v(t)分別為過程狀態(tài)偏差與測量輸出偏差.

自適應MPC控制原理如圖2所示.

圖2 自適應MPC控制原理

基于以上原理圖，首先對系統(tǒng)狀態(tài)進行估計.由于車輛系統(tǒng)是高度非線性，而且傳感器也處于高度集中，控制器的狀態(tài)量不是全部可以測量或有測量噪聲，需要估計狀態(tài)或濾波.在默認情況下，傳統(tǒng)MPC使用的是靜態(tài)卡爾曼濾波器(KF)估計軌跡跟蹤控制器中的狀態(tài)，這樣導致軌跡跟蹤精度會有一定的誤差.因此本文在每一個控制采樣時刻基于動態(tài)卡爾曼濾波器(LTV-KF)設計兩個增益矩陣L和M，并且不斷更新L和M以適應當下時刻的無人車輛系統(tǒng)[12].

狀態(tài)預測方程如下：

(7)

e(k)=η(k)-(Ckx(k|k-1)+Du,ku(k)+Dv,kv(k)) .

(8)

LTV-KF的狀態(tài)估計器所設計的增益矩陣更新方程如下：

(9)

式中：Q，R和N是MPC狀態(tài)估計器中的噪聲協(xié)方差矩陣；Pk|k-1是狀態(tài)估計器基于k-1時刻獲得k時刻信息的狀態(tài)估計誤差協(xié)方差矩陣；Lk，Mk為卡爾曼狀態(tài)估計器設計的基于k時刻的更新增益矩陣.狀態(tài)估計器相關參數(shù)的取值參考文獻[13].

w(k)=Dv,kv(k) .

(10)

把控制量u(k)轉化為控制增量Δu(k)，方便模型預測控制器對Δu(k)進行直接限制，因此對系統(tǒng)狀態(tài)方程中的輸入、輸出以及相關矩陣等變量作出相應的變化.得到新的狀態(tài)空間表達式為

(11)

Δu(k)=u(k)-u(k-1)；Im是m維單位矩陣；0m是m維值為0的列向量；0m×n是m×n維的零矩陣.

通過不斷進行迭代求解，可得到基于時刻k的系統(tǒng)輸出量，則變換后的空間方程的系統(tǒng)輸出量表達式如下：

(12)

設模型預測控制器的預測時域為Np，控制時域為Nc，則根據(jù)以上推導，在預測時域Np內系統(tǒng)的輸出量表達式如下：

(13)

式中：Y為系統(tǒng)的輸出量向量；Ψk，Θk，Υk分別為此系統(tǒng)中定義的參數(shù)矩陣；ΔU為系統(tǒng)控制時域Nc內的控制量向量；Φ為系統(tǒng)預測時域Np內的輸入偏差向量；Ζ為系統(tǒng)預測時域Np內的偏差向量.定義的變量如下：

(13.1)

(13.2)

(13.3)

(13.4)

(13.5)

(13.6)

(13.7)

現(xiàn)定義輸出變量η(k)如下：

η(k)=[ηtr(k),ηhc(k),ηsc(k)] .

(14)

式中：ηtr，ηhc，ηsc分別為控制輸出量、硬約束輸出量、軟約束輸出量.

由此經(jīng)過向量矩陣變化，可推導出在預測時域Np內控制輸出量Ytr(k)，硬約束輸出量Yhc(k)，軟約束輸出量Ysc(k)的表達式：

(15)

式中：下標tr，hc，sc分別表示系統(tǒng)參數(shù)矩陣的轉化量、硬約束量和軟約束量.

由于車輛動力學模型以及約束比較復雜，在一個控制周期內可能出現(xiàn)無解的情形，因此在目標函數(shù)中加入可放寬約束的松弛因子ε，保證求解器在每一個控制周期都可以找到解[14].則采用如下形式的目標函數(shù)：

(16)

式中：O∈Rp×p，P∈Rm×m，W∈Rm×m分別為三個優(yōu)化目標的權重矩陣；ηref為參考軌跡變量；ρ為權重系數(shù)；ε為松弛因子.

為方便計算機仿真驗證，將上述目標函數(shù)轉換為標準二次型，利用自適應MPC算法對其進行求解，則優(yōu)化問題公式如下：

s.t.

(17)

式中：

(18)

(19)

(20)

U(k-1)=1Nc?Δu(k-1) .

(21)

其中:Hk為正定的Hessian矩陣；Ξ(k)為預測時域內的跟蹤誤差；1Nc是行數(shù)為Nc的列向量；M=K?Im，?為克羅內克積，K為Nc×Nc維的下三角單位矩陣，Im是m維的單位矩陣.

每一個跟蹤控制周期對式(17)進行求解，得到控制時域內一系列最優(yōu)的控制變量：

(22)

將上述最優(yōu)控制變量第一個元素作用系統(tǒng)：

(23)

每一個控制周期都進行以上計算步驟，通過前輪轉角控制量實現(xiàn)軌跡跟蹤控制.

3 仿真實驗與結果分析

表1為選定的車輛模型的參數(shù).本文設定控制器的預測時域Np=10，控制時域Nc=3，仿真步長Ts=0.05 s，權重系數(shù)O=diag(900,20,30,30)，P=20，W=100，ρ=1 000，-12°≤δf≤12°，-0.9°≤Δδf≤0.9°.

表1 車輛模型參數(shù)

在MATLAB/Simulink環(huán)境下搭建仿真模型并進行驗證.在所搭建的仿真模型中，縱向速度由基于模式切換的增量式PID進行控制，并在每一個控制周期把控制量傳給橫向控制器，實現(xiàn)分層控制，對系統(tǒng)進行縱橫向解耦.本文選定車輛以縱向速度為10，20 m/s,初速度10 m/s,加速度0.5 m/s2行駛時的3種工況進行驗證，分別對軌跡跟蹤精度與穩(wěn)定性進行比較.參考軌跡選擇國際標準的雙移線工況.仿真結果如圖3所示.

圖3a，3b表明:3種速度工況下，自適應MPC能較好實現(xiàn)軌跡跟蹤控制.軌跡跟蹤誤差伴隨著車速的提高而增大，但始終沒有超過0.25m.當車輛以0.5 m/s2的加速度加速行駛時，依舊保持良好的軌跡跟蹤精度，表明該算法對于車輛速度的實時變化具有較強的魯棒性.

圖3 仿真結果

圖3c表明:在3種速度工況下，車輛前輪轉角伴隨著車速的提高相應變化位置向前移動，最大轉角始終沒有超過控制器約束的范圍.雖然加速行駛和20 m/s兩種行駛工況下的前輪轉角有波動，但轉角變化量都在約束范圍之內，并未發(fā)生突變，表明在雙移線測試工況下本文所設計的控制器性能是可接受的.

圖3d表明:3種速度工況下，速度越低，軌跡跟蹤精度越高，則相對應的質心側偏角越大，但質心側偏角始終處于控制器約束的范圍之間，表明車輛控制過程是比較穩(wěn)定的，能夠確保車輛的操穩(wěn)性.當車輛以20 m/s的速度行駛時，質心側偏角出現(xiàn)微小波動，可能原因是較大的側向力導致輪胎處于非線性工作區(qū)域.

綜合以上分析可知，本文提出的控制策略是有效的，并表現(xiàn)出快速的響應與收斂能力.自適應MPC軌跡跟蹤控制算法具有良好的速度魯棒性與軌跡跟蹤性能.

4 結 論

本文考慮車輛系統(tǒng)的非線性以及測量與過程噪聲的影響來設計自適應MPC軌跡跟蹤控制器，在每一個控制周期不斷更新卡爾曼狀態(tài)估計器中增益矩陣來適應當前的系統(tǒng)工作環(huán)境.在MATLAB/Simulink中搭建仿真模型并進行仿真驗證，結果表明該算法在不同速度下依然保持良好的跟蹤精度與穩(wěn)定性，對于無人駕駛車輛軌跡跟蹤控制的研究具有一定的參考價值.