基于MPC的自動(dòng)泊車系統(tǒng)軌跡跟蹤控制分析

李世豪，付子揚(yáng)，呂 洋

（長(zhǎng)安大學(xué) 汽車學(xué)院，陜西 西安 710064）

進(jìn)入二十一世紀(jì)以來(lái)，我國(guó)汽車銷量持續(xù)增長(zhǎng)，2009 年我國(guó)轎車的總銷售額第一次超過(guò)了美國(guó)市場(chǎng)，成為世界上第一大汽車生產(chǎn)市場(chǎng)。截至2020 年第一季度，中國(guó)汽車保有量仍達(dá)到了2.6億，其中乘用車保有量將達(dá)到2.1 億輛[1]。隨著汽車普及程度的不斷提升，引發(fā)了諸如碰撞事故、交通擁堵、環(huán)境多變、泊車?yán)щy等一系列問(wèn)題，尤其對(duì)于一些經(jīng)驗(yàn)不足的乘用車駕駛員而言，在遇到狹窄車位時(shí)，泊車過(guò)程會(huì)變得愈發(fā)困難，不僅影響交通流暢、耗費(fèi)大量時(shí)間，而且長(zhǎng)時(shí)間的怠速工況會(huì)減少發(fā)動(dòng)機(jī)壽命、污染環(huán)境，甚至?xí)霈F(xiàn)交通事故。美國(guó)交通安全研究調(diào)查指出，在泊車過(guò)程中出現(xiàn)的車禍占了各種事故的44%，其中約有半數(shù)以上的泊車事故是由駕駛員經(jīng)驗(yàn)不足、操作不當(dāng)導(dǎo)致的[2]。因此，為改善交通環(huán)境，減少事故發(fā)生，對(duì)于自動(dòng)泊車系統(tǒng)的研究具有重要意義。

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

設(shè)計(jì)的模型預(yù)測(cè)控制器精確與否，受制于是否建立合理的車輛運(yùn)動(dòng)學(xué)模型或動(dòng)力學(xué)模型，而泊車工況中車輛處于低速運(yùn)動(dòng)，一般不需要考慮動(dòng)力學(xué)問(wèn)題。車輛運(yùn)動(dòng)學(xué)模型如圖1 所示。

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

在大地坐標(biāo)系XOY中，為了得到汽車的轉(zhuǎn)向運(yùn)動(dòng)模型，假定汽車在某一時(shí)刻作直線運(yùn)動(dòng)，或是圍繞著一個(gè)點(diǎn)作圓周運(yùn)動(dòng)，并忽略懸架的影響，其中，（Xr,Yr）和（Xf,Yf）分別為車輛后軸中心和前軸中心在慣性坐標(biāo)系下的坐標(biāo)；δf為汽車前輪偏角；vr為車輛在后軸中心處的速度；L為軸距。根據(jù)車輛運(yùn)動(dòng)學(xué)關(guān)系，建立如下方程：

2 預(yù)測(cè)模型建立

由式（1）可知，系統(tǒng)輸入為u（v,δ），狀態(tài)量為χ（x,y,φ）。其一般形式為

對(duì)于已知的參考路徑，其上的每個(gè)點(diǎn)都滿足方程（2），r表示參考量，則可表示為

其中，χr=[xryrφr]T，ur=[vrδr]T。

對(duì)式（2）采用泰勒級(jí)數(shù)在參考軌跡點(diǎn)展開(kāi)，忽略高階項(xiàng)：

將式（4）與式（3）相減，可以得到：

式（5）即為線性化的無(wú)人駕駛車輛誤差模型，對(duì)其進(jìn)行離散化處理：

3 目標(biāo)函數(shù)設(shè)計(jì)

目標(biāo)函數(shù)的設(shè)置要能夠確保無(wú)人駕駛汽車準(zhǔn)確而迅速地跟蹤上目標(biāo)軌跡。所以必須優(yōu)化系統(tǒng)狀態(tài)量的偏差和控制量[3]。采用如下形式的目標(biāo)函數(shù)[4]：

式中，Q和R都是權(quán)重矩陣，式（7）中等號(hào)右端第一項(xiàng)表達(dá)了系統(tǒng)對(duì)軌跡跟蹤的響應(yīng)能力，而第二項(xiàng)表達(dá)了對(duì)控制量變化率的約束。為防止因被控系統(tǒng)控制量突變而導(dǎo)致控制量連續(xù)性[5]受到影響，將目標(biāo)函數(shù)做如下轉(zhuǎn)化：

式中，Nc為控制時(shí)域；Np為預(yù)測(cè)時(shí)域；ε為松弛因子；ρ為權(quán)重系數(shù)。

通過(guò)推導(dǎo)能夠得出系統(tǒng)的預(yù)測(cè)輸出表達(dá)式：

4 約束條件設(shè)計(jì)

確定控制量極限約束和控制量增量約束：

通過(guò)對(duì)代價(jià)函數(shù)求解，可得到控制增量，因此，對(duì)于約束條件而言，也應(yīng)以控制增量或者控制增量與轉(zhuǎn)換矩陣相乘的形式來(lái)表示，所以對(duì)式（10）做如下轉(zhuǎn)化：

將式（8）目標(biāo)函數(shù)轉(zhuǎn)換為標(biāo)準(zhǔn)二次型，并結(jié)合約束條件可得

在完成各個(gè)控制周期內(nèi)對(duì)式（13）的計(jì)算后，在控制時(shí)域中可得到一組控制增量：

將控制序列中的第一個(gè)元素作為實(shí)際的控制增量作用于系統(tǒng)，隨即進(jìn)入下一個(gè)控制周期后，重復(fù)以上步驟，這樣便實(shí)現(xiàn)了對(duì)車輛的軌跡跟蹤控制。

5 基于運(yùn)動(dòng)學(xué)的MPC軌跡跟蹤仿真分析

在仿真過(guò)程中取車速v=1 m/s，設(shè)置初始時(shí)刻t0=0，末端時(shí)刻tfinal=14 s；將初始狀態(tài)設(shè)置為x0=0，y0=0，Simulink 利用（from workspace）模塊調(diào)用參考路徑信息；車位尺寸取wd=6.5 m，Lt=2.5 m；預(yù)測(cè)時(shí)域Np=20，控制時(shí)域Nc=5，控制周期T=0.02 s。仿真工具采用CarSim/Simulink，仿真模型如圖2 所示。

圖2 CarSim/Simulink 聯(lián)合仿真平臺(tái)

參考現(xiàn)有無(wú)人駕駛車輛平臺(tái)的整車配置與參數(shù)，在CarSim 中搭建了整車模型，主要車輛參數(shù)如表1 所示。

表1 參考車輛參數(shù)

仿真結(jié)果如圖3 所示，其中圖3(a)表示車輛跟蹤參考軌跡時(shí)坐標(biāo)的變化情況，圖3(b)表示軌跡跟蹤的偏差。在圖中可以分析出，車輛在控制器的作用下能準(zhǔn)確地跟蹤上期望軌跡，系統(tǒng)跟蹤的偏差最終無(wú)限接近于0，跟蹤效果良好。圖3(c)和圖3(d)反映了跟蹤過(guò)程中控制量隨時(shí)間的變化，在整個(gè)跟蹤過(guò)程中控制量始終保持在合理值約束范圍，不但跟蹤響應(yīng)速度快，而且保證了平穩(wěn)性。

圖3 跟蹤軌跡結(jié)果與跟蹤偏差

6 結(jié)論

本文主要研究了MPC 軌跡跟蹤控制器對(duì)于路徑的跟蹤效果，由于泊車時(shí)車輛速度保持在較低水平，一般不需要著重考慮操縱穩(wěn)定性，但在泊車時(shí)需要考慮駕駛員和乘客的感受，因此，需要將控制量增量添加到代價(jià)函數(shù)中，來(lái)兼顧汽車平順性進(jìn)行研究。由仿真結(jié)果圖可以得出，MPC 軌跡跟蹤控制算法能夠使車輛快速且平穩(wěn)地跟蹤上期望軌跡。