變權重矩陣下的MPC軌跡跟蹤研究

李培慶，楊正龍，陳一鍇，李昊

(1.浙江科技學院機械與能源工程學院，浙江杭州 310023；2.浙江大學機械工程學院，浙江杭州 310058；3.合肥工業(yè)大學汽車與交通工程學院，安徽合肥 230009)

0 前言

目前無人駕駛車輛面臨三大技術難題：環(huán)境感知、路徑規(guī)劃、軌跡跟蹤，軌跡跟蹤便是其中之一。無人駕駛車輛基于定位和高精度地圖，通過激光雷達、毫米波雷達、攝像頭等來收集數(shù)據(jù)，將這些數(shù)據(jù)融合，然后傳遞給路徑規(guī)劃層;規(guī)劃層根據(jù)數(shù)據(jù)規(guī)劃出一條符合當前時刻的參考路徑，再傳遞給控制層;控制層接收到信號后，控制車輛的速度和轉向盡可能地跟隨參考軌跡。

針對軌跡跟蹤，國內外學者進行了大量研究，目前主流的軌跡跟蹤控制算法主要有：預瞄控制算法、模型預測控制算法、PID控制、LQR控制跟蹤、純跟蹤控制(Pure Pursuit)等。FALCONE等提出了一種非線性的MPC路徑跟蹤控制算法，并建立了完整的十階車輛模型和簡化的自行車模型驗證了算法的有效性。汪佳興等提出了一種基于B樣條曲線的MPC軌跡重規(guī)劃算法，利用B樣條曲線的局部性和凸包性的優(yōu)勢，提高規(guī)劃路徑的平滑性。CHEN等建立了一種考慮道路寬度和車輛幾何結構的新型車路模型ribbon model，并在此基礎上提出了一種新的帶轉向的軌跡跟蹤控制器，通過仿真和實車驗證了車輛可以根據(jù)道路情況自主調整安全、舒適、快速等控制策略。吳飛龍、郭世永對控制器參數(shù)進行優(yōu)化，加入側偏角軟約束，從而保證了跟蹤精度和行駛穩(wěn)定性。梁忠超等設計了一種自適應MPC，通過不斷更新卡爾曼狀態(tài)估計器相關增益系數(shù)矩陣以及控制器的狀態(tài)來保證車輛跟隨的精度和穩(wěn)定性。唐傳茵等采用PSO算法對模型預測控制器中的預測時域和控制時域進行了優(yōu)化處理，得到了不同工況和不同速度下的最優(yōu)控制參數(shù)，從而使路徑跟蹤效果更進一步提高。章仁燮等提出了一種基于條件積分算法的無人駕駛車輛軌跡跟蹤魯棒控制方法，分別設計了運動學控制器和動力學控制器，通過仿真驗證了能夠實現(xiàn)無人車輛良好的軌跡跟蹤效果。宋曉華等提出了基于車輛二自由度動力學模型的車輛軌跡預測改進方法，采用改進控制增量約束的方式，解決了車輛軌跡預測求解過程中二次規(guī)劃方程變量上下限設置不當導致的車輛預測軌跡偏離期望軌跡的問題。邵毅明等設計了一種帶有側偏角軟約束的線性時變MPC控制器，保證了車輛在穩(wěn)定的前提下實現(xiàn)軌跡跟蹤。

基于以上研究，本文作者在傳統(tǒng)MPC理論的基礎上研究了一種基于參考軌跡曲率的自適應模型預測控制方法。首先建立阿克曼轉向特性的車輛運動學模型，通過速度和前輪轉角來控制車輛，然后建立目標路徑，根據(jù)軌跡曲率的變化動態(tài)改變權重矩陣大小。最后通過MATLAB/CarSim進行對比仿真，驗證控制效果。

1 車輛運動學模型的建立

車輛在自動駕駛場景下，按照規(guī)劃模塊規(guī)劃好的軌跡行駛，控制模塊控制車輛盡可能地按照規(guī)劃好的軌跡行駛，這就要求車輛的行駛要符合實際情況，滿足車輛的運動學約束。采用運動學模型進行建模分析，可以滿足車輛在軌跡跟蹤過程中的運動學約束。

1.1 阿克曼轉向

輪式車輛的轉向形式一般有獨立轉向、阿克曼轉向、鉸鏈轉向、差速轉向。一般常用的轉向方式是阿克曼轉向，阿克曼轉向是通過連桿機構實現(xiàn)左右兩側車輪在運動學上的協(xié)調實現(xiàn)轉向。阿克曼轉向的特點就是車輛沿著彎道行駛時利用四連桿的相等曲柄使內側車輪的轉向角比外側車輪要大，從而使4個車輪行駛軌跡的圓心大致交匯于后軸延長線上的瞬時轉向中心，即每個車輪都繞同一轉向中心做純滾動，使車輛可以順暢地轉彎。從而解決了車輛在轉向的過程中由于兩個前輪轉角不同而具有不同的轉向中心，引起轉向不便和輪胎磨損的問題。

1.2 運動學模型

基于上述的阿克曼轉向特性在大地坐標系()下建立車輛運動學模型，如圖1所示。假設車輛是前輪轉向、后輪驅動，并忽略空氣阻力、懸架的作用。對于前輪轉向、后輪驅動的車輛來說，在實際行駛過程中，其前輪的轉向不一定等于當前車輛的速度方向。為了簡化建模過程，采用單軌二自由度，這樣其前輪方向就是當前車輛的速度方向。

圖1 車輛運動學模型

用3個狀態(tài)量(,,)表示當前車輛的位置信息，兩個控制量(,)控制車輛的運動。故可以得到運動學方程：

(1)

其中:(，)是車輛后軸中心坐標;為當前車輛的橫擺角;為后軸車速;為前輪轉角;為軸距。

將式(1)表示為狀態(tài)方程的形式：

(2)

2 模型預測控制

模型預測控制是一種最優(yōu)控制算法，優(yōu)勢在于它是一個多變量控制器，通過考慮所有因素同時控制輸出系統(tǒng)變量之間的交互，并且還可以處理多約束條件問題。模型預測控制主要分為三要素：預測模型、滾動優(yōu)化、反饋校正。預測模型指的是根據(jù)被控模型的歷史信息和將來的信息，預測控制對象未來的輸出響應。滾動優(yōu)化是指利用某一選定的參數(shù)指標達到最優(yōu)化，來求解將來有限時刻的最優(yōu)控制率。反饋校正是指根據(jù)被控對象的實際輸出來對基于模型的預測輸出進行校正并進行下一時刻的優(yōu)化。模型預測控制原理如圖2所示。

圖2 MPC控制原理示意

2.1 預測模型

選取車輛作為被控對象，將其狀態(tài)量表示為=(,,)，控制量表示為=(,)。此時整個系統(tǒng)可以表示成一個一般形式的控制系統(tǒng)：

(3)

軌跡跟蹤的參考軌跡用下標r代表參考量，此時一般形式變?yōu)?/p>

(4)

接下來進行線性化處理，線性的目的是為了使最終的誤差線性化。將式(3)在參考點采用泰勒級數(shù)展開并只展開到一階，于是得到：

(5)

再將式(5)與式(4)相減，用·表示誤差，于是得到：

(6)

(7)

(8)

然后再對式(8)中的車輛狀態(tài)和控制量用前向歐拉法進行離散化處理，得到：

(9)

(10)

得到離散化的結果后再進行預測處理，用表示預測時域，表示控制時域得出系統(tǒng)的預測輸出表達式：

=()+Δ()

(11)

2.2 優(yōu)化求解

每一時刻，都會進行求解，要使當前的軌跡趨近參考軌跡，必須對控制量和當前車輛的狀態(tài)進行約束和優(yōu)化，故根據(jù)式(15)建立代價函數(shù)，使得當前車輛的行駛軌跡和參考軌跡的誤差越小越好，更好地追蹤軌跡。參考文獻[10]采用如下的代價函數(shù)：

(12)

式中：和為權重矩陣；為權重系數(shù)；為松弛因子。此代價函數(shù)利于將問題轉換成二次規(guī)劃(QP)求解問題，利用MATLAB中的quadprog函數(shù)求解，此函數(shù)數(shù)學模型為

(13)

式中：、和為矩陣；、、、、和為向量。

將式(12)轉換成式(13)中的二次型標準形式，得到：

(14)

式中:為預測時域內的跟蹤誤差。

2.3 反饋校正

在每一個控制周期內，由式(14)進行求解，得到當前控制時域的控制輸入增量Δ()并作用于系統(tǒng)：

(15)

車輛隨之運動變化，重新獲得當前車輛的狀態(tài)量(,,)，再進行預測、優(yōu)化控制、反饋。重復上述過程，如此循環(huán)便實現(xiàn)了對車輛的軌跡跟蹤控制。

2.4 約束條件

車輛在每個控制時域內都需要滿足以下的約束條件：

(16)

式中：、分別為控制量的最小值與最大值；Δ、Δ分別為控制增量的最小值和最大值；、分別為輪胎側偏角的最小值與最大值；、分別為質心側偏角的最小值與最大值。

3 自適應MPC控制器

式(12)中權重矩陣和的選取非常重要。、一般選取對角矩陣，=diag(,…,=×)為狀態(tài)誤差矩陣，反映了對參考軌跡的跟隨能力；=diag(,…,=×)為控制矩陣，反映了對控制量變化的約束或要求。其中為狀態(tài)量的個數(shù)，為控制量的個數(shù)；和分別是狀態(tài)誤差權重系數(shù)和控制權重系數(shù)，文中都為同一值，都為同一值。

3.1 不同權重系數(shù)下的軌跡跟蹤對比仿真

不同權重系數(shù)、的雙移線軌跡跟蹤對比仿真結果如圖3和圖4所示。

圖3 不同r時的軌跡跟蹤

圖4 不同q時的軌跡跟蹤

從圖3中可以看出：當參考軌跡較趨于直線，曲線曲率較小時，不同的值下都可以比較好地跟蹤軌跡，但是當參考軌跡由直線變?yōu)榍€，曲線曲率變大時，較小的值跟蹤精度更高，橫向誤差更??；當值太大超過一定范圍時，跟蹤效果變差，跟蹤軌跡出現(xiàn)震蕩現(xiàn)象，行駛穩(wěn)定性變差。從圖4中可以看出：當參考軌跡由直線變?yōu)榍€，曲線曲率變大時，不同值下都可以相對較好地跟蹤軌跡；當值和值相等時，跟蹤軌跡到高點時出現(xiàn)了較大誤差，并且在軌跡趨于直線時出現(xiàn)了波動；當曲線曲率較小，軌跡趨于直線時，值越大，跟蹤效果越差，上下波動震蕩越大，車輛穩(wěn)定性越差。因此，可以考慮當參考軌跡曲率較小時，適當減小權重系數(shù)值，當參考軌跡曲率較大時，應該減小權重系數(shù)值。

3.2 變權重系數(shù)

基于以上分析，提出一種改進的算法，根據(jù)參考軌跡曲率更新權重矩陣、中的權重系數(shù)和。曲率是曲線彎曲程度的一種度量，軌跡曲率越大，曲線越彎曲越陡，表示車輛跟蹤更為困難。當車輛速度較低時，轉向容易，轉向能力好；當車輛速度較大時，轉向能力變弱，向心力變大，容易發(fā)生側滑。車輛最大轉向曲率應滿足以下公式：

(17)

式中:為最大前輪轉角;為車長。

將參考軌跡雙移線擬合成以下函數(shù)：

=·sin(·+)+·sin(·+)+·

sin(·+)+·sin(·+)+·sin(·+

)+·sin(·+)+·sin(·+)+·

sin (·+)

(18)

擬合后的曲線和原參考軌跡相比，擬合程度較好，參考軌跡和擬合后的曲線基本吻合，如圖5所示。

圖5 擬合曲線

車輛在慣性坐標系下的縱向速度可以表示為

_DOT=·cos-·sin

(19)

當前時刻的參考軌跡的縱向位置為

()=+_DOT·

(20)

并根據(jù)以下公式算出參考軌跡曲率：

(21)

式中：()為當前時刻參考軌跡的縱向位置。將當前參考軌跡的縱向位置信息代入式(21)，算出當前時刻的參考軌跡曲率，調整權重矩陣的大小，從而影響控制量的大小，實時改變前輪轉角和車速，使車輛能夠較好地跟隨參考軌跡，并提高行駛穩(wěn)定性。

為了簡便優(yōu)化處理，首先將參考軌跡曲率進行歸一化處理：

(22)

式中：為參考軌跡曲率的最小值；為參考軌跡曲率的最大值。

當權重系數(shù)較小時，控制系統(tǒng)響應整體趨于平緩，控制量變化??；當權重系數(shù)較小時，控制系統(tǒng)響應更加靈敏，控制量變化大。根據(jù)曲率的大小調整權重矩陣中權重系數(shù)、的大小。當較小時，表明參考軌跡曲率較小，軌跡趨于直線，較不彎曲，不需要大幅度調整前輪轉向和速度，因此權重系數(shù)應適當減小一些；當較大時，表明參考軌跡曲率較大，軌跡較彎曲，跟蹤困難，前輪轉角和速度應及時調整，因此權重系數(shù)應減小。故采用如下的算式調整權重系數(shù)、：當曲率越小時，值減少得越多；當曲率越大時，值減少得越多。

(23)

(24)

式中：、、、為權重系數(shù)基本值；、為常數(shù)。

4 仿真實驗

為了驗證文中所提出的控制器的效果，基于MATLAB/CarSim進行聯(lián)合仿真，通過CarSim搭建無人駕駛車輛運動仿真工況，設置主要輸入及輸出參數(shù)，利用MATLAB建立仿真模型以及S-Function函數(shù)編寫。CarSim的輸出作為MATLAB的輸入，MATLAB的輸出作為CarSim的輸入，從而實現(xiàn)MPC控制算法，形成一個閉環(huán)反饋控制系統(tǒng)。

文中仿真車輛選取CarSim中E-Class/SUV車型，具體參數(shù)如表1所示。

表1 整車參數(shù)

為模擬車輛在現(xiàn)實中真實的行駛狀況，選取雙移線工況進行軌跡跟蹤仿真實驗。

4.1 兩種控制器下的對比仿真

仿真實驗中設定車速為36 km/h，控制器采用的參數(shù)為：預測時域=30，控制時域=15。MPC控制器權重系數(shù)經(jīng)過多次仿真測試后選取軌跡跟隨效果較好的一組數(shù)據(jù)：=2 000,=50 000。改進的MPC控制器自適應調整值和值。

圖6為兩種控制器下的車輛軌跡跟蹤對比結果，可以看出：改進的MPC控制器最大橫向誤差為0.09 m，MPC控制器最大橫向誤差為0.17 m，兩種控制器都能較好地跟蹤參考軌跡，但是在參考軌跡曲率較大處、曲線較彎曲時，改進的MPC控制器能夠更好地跟蹤軌跡，精度更高，橫向誤差減小了0.08 m。這是由于當曲率較大時，改進的MPC控制器權重系數(shù)值減小，從而能更好地跟蹤軌跡。圖7為橫擺角速度對比結果，可以看出：MPC控制器最大橫擺角速度為8.19°/s，改進的MPC控制器的最大橫擺角速度為6.80°/s，減小了1.39°/s，并且在參考軌跡由曲線變?yōu)橹本€、曲率變小時，改進的MPC控制器更加穩(wěn)定，車輛行駛更加平穩(wěn)舒適。這是因為權重系數(shù)值減小的原因，從而減少了波動。圖8為質心側偏角對比結果，可以看出：兩種控制器的質心偏角的最大值均在極限范圍內，小于8.91°，在穩(wěn)定范圍內，說明車輛在兩種控制器下都能較好地跟隨軌跡并保持穩(wěn)定性。

圖6 車輛軌跡跟蹤對比

圖7 橫擺角速度

圖8 質心側偏角

4.2 變權重矩陣控制器對速度的魯棒性

在不同的速度下對變權重矩陣控制器進行仿真實驗，分析它對速度的魯棒性?？刂破鞑捎玫膮?shù)為：預測時域=25，控制時域=10。

圖9—圖10為變權重矩陣控制器在不同速度下的軌跡跟蹤對比結果，可以看出：控制器在不同速度下能夠較好地完成軌跡跟蹤，并且在低速下精度更高，對速度具有較好的魯棒性。圖11為控制器在不同速度下的橫擺角速度，可以看出：隨著車速的增加，橫擺角速度也隨之增大。圖12為控制器在不同車速下的質心側偏角，3種車速下的質心側偏角都在約束范圍內，并且隨著速度的增加，質心側偏角隨之變小。綜上所述，變權重矩陣控制器對速度具有較好的魯棒性。

圖9 車輛軌跡跟蹤對比

圖10 車輛軌跡最高點局部放大

圖11 橫擺角速度

圖12 質心側偏角

5 結論

針對無人駕駛車輛軌跡跟蹤問題，基于模型預測控制算法提出了一種改進優(yōu)化的算法，即根據(jù)參考軌跡曲率來動態(tài)調整權重矩陣大小。MATLAB/CarSim仿真驗證表明改進的MPC控制器相比權重矩陣是固定值的傳統(tǒng)MPC控制器具有更高的跟蹤精度和穩(wěn)定性。