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

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

0 前言

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

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

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

1 車輛運(yùn)動(dòng)學(xué)模型的建立

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

1.1 阿克曼轉(zhuǎn)向

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

1.2 運(yùn)動(dòng)學(xué)模型

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

圖1 車輛運(yùn)動(dòng)學(xué)模型

用3個(gè)狀態(tài)量(,,)表示當(dāng)前車輛的位置信息，兩個(gè)控制量(,)控制車輛的運(yùn)動(dòng)。故可以得到運(yùn)動(dòng)學(xué)方程：

(1)

其中:(，)是車輛后軸中心坐標(biāo);為當(dāng)前車輛的橫擺角;為后軸車速;為前輪轉(zhuǎn)角;為軸距。

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

(2)

2 模型預(yù)測控制

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

圖2 MPC控制原理示意

2.1 預(yù)測模型

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

(3)

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

(4)

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

(5)

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

(6)

(7)

(8)

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

(9)

(10)

得到離散化的結(jié)果后再進(jìn)行預(yù)測處理，用表示預(yù)測時(shí)域，表示控制時(shí)域得出系統(tǒng)的預(yù)測輸出表達(dá)式：

=()+Δ()

(11)

2.2 優(yōu)化求解

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

(12)

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

(13)

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

將式(12)轉(zhuǎn)換成式(13)中的二次型標(biāo)準(zhǔn)形式，得到：

(14)

式中:為預(yù)測時(shí)域內(nèi)的跟蹤誤差。

2.3 反饋校正

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

(15)

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

2.4 約束條件

車輛在每個(gè)控制時(shí)域內(nèi)都需要滿足以下的約束條件：

(16)

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

3 自適應(yīng)MPC控制器

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

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

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

圖3 不同r時(shí)的軌跡跟蹤

圖4 不同q時(shí)的軌跡跟蹤

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

3.2 變權(quán)重系數(shù)

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

(17)

式中:為最大前輪轉(zhuǎn)角;為車長。

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

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

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

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

sin (·+)

(18)

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

圖5 擬合曲線

車輛在慣性坐標(biāo)系下的縱向速度可以表示為

_DOT=·cos-·sin

(19)

當(dāng)前時(shí)刻的參考軌跡的縱向位置為

()=+_DOT·

(20)

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

(21)

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

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

(22)

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

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

(23)

(24)

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

4 仿真實(shí)驗(yàn)

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

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

表1 整車參數(shù)

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

4.1 兩種控制器下的對(duì)比仿真

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

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

圖6 車輛軌跡跟蹤對(duì)比

圖7 橫擺角速度

圖8 質(zhì)心側(cè)偏角

4.2 變權(quán)重矩陣控制器對(duì)速度的魯棒性

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

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

圖9 車輛軌跡跟蹤對(duì)比

圖10 車輛軌跡最高點(diǎn)局部放大

圖11 橫擺角速度

圖12 質(zhì)心側(cè)偏角

5 結(jié)論

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