車輛路徑跟蹤包括主車的縱向和橫向運(yùn)動(dòng)控制，縱向控制是通過調(diào)整主車的加速度，使得車輛以期望的速度行駛；橫向控制是通過調(diào)整主車的轉(zhuǎn)向，使主車沿預(yù)期的軌跡行駛。本文結(jié)合車輛運(yùn)動(dòng)學(xué)的知識(shí)對(duì)主車進(jìn)行橫縱向控制，基于PID控制策略設(shè)計(jì)路徑跟蹤控制器，并實(shí)現(xiàn)整個(gè)系統(tǒng)的閉環(huán)仿真與分析。

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

首先，利用車輛運(yùn)動(dòng)學(xué)模型，來估計(jì)車輛實(shí)際運(yùn)動(dòng)軌跡相應(yīng)點(diǎn)的位置。車輛轉(zhuǎn)向運(yùn)動(dòng)學(xué)模型如下圖所示。

其中坐標(biāo)系為慣性坐標(biāo)系XOY；δf為前輪轉(zhuǎn)角；(Xf,Yf)和(Xr,Yr)分別為前軸軸心和后軸軸心的坐標(biāo)；vf為前軸軸心速度，vr為后軸軸心速度；L為軸距；φ為航向角。

在后軸行駛軸心(Xr,Yr)處，速度為：

? ? ? ??? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

前后軸的運(yùn)動(dòng)學(xué)約束為：

? ? ? ? ? ?

由式（1）和式（2）聯(lián)合得：

? ? ? ? ? ? ? ? ? ? ? ? ? ?

由前后輪的幾何關(guān)系可得：

? ? ? ? ? ? ? ? ? ?? ? ??

根據(jù)式（2）、（3）和（4）可得車輛橫擺角速度為：

? ? ? ? ? ? ? ? ? ? ? ??

綜上，得到車輛運(yùn)動(dòng)學(xué)模型狀態(tài)方程如下：

? ? ? ? ? ? ? ? ? ? ? ??

由狀態(tài)方程可知，輸入為前輪轉(zhuǎn)角δf和后軸軸心速度vr，狀態(tài)為橫向位移Xr、縱向位移Yr和航向角φ。給定后軸軸心初速度為0，初始航向角為0。

二、預(yù)瞄駕駛員模型

明尼蘇達(dá)大學(xué)的Rajamani教授等指出，預(yù)瞄控制是提高車輛橫向控制魯棒性和穩(wěn)定性最好的方法之一。因此，采用預(yù)瞄控制思想，即以車輛前方預(yù)瞄點(diǎn)處的運(yùn)動(dòng)狀態(tài)作為反饋設(shè)計(jì)PID控制器。預(yù)瞄時(shí)距取0.1s，預(yù)瞄距離由以下公式計(jì)算：

? ? ? ? ? ? ? ? ? ? ? ??

我們記本時(shí)刻點(diǎn)的位置為(x0,y0)，將預(yù)瞄點(diǎn)的位置表示為(x1,y1)，則兩點(diǎn)之間的坐標(biāo)有以下關(guān)系

? ? ? ? ? ? ? ??

三、橫向控制

本題給定軌跡為一百個(gè)離散的點(diǎn)，利用for循環(huán)語句依次求預(yù)瞄點(diǎn)與軌跡上各點(diǎn)的距離比較得到最近點(diǎn)的坐標(biāo)。因?yàn)榫嚯x都是非負(fù)數(shù)，為了簡(jiǎn)便運(yùn)算，此處我們可直接比較距離的平方值，記d為預(yù)瞄點(diǎn)到軌跡上點(diǎn)距離的平方，計(jì)算公式為：

? ? ? ? ? ? ? ? ??

1）橫向誤差計(jì)算

此處的橫向誤差不是縱坐標(biāo)的差值，而是預(yù)瞄點(diǎn)到軌跡上最近點(diǎn)的距離。記橫向誤差為e1，計(jì)算公式如下：

? ? ? ? ??

2）航向誤差計(jì)算

如要計(jì)算航向誤差，需要得到期望軌跡相應(yīng)點(diǎn)的航向角，與實(shí)際航向角作差得到航向誤差。但給定的軌跡為一組離散點(diǎn)，為了得到航向角信息，我們可以對(duì)軌跡進(jìn)行擬合。利用MATLAB曲線擬合工具箱 （cftool）進(jìn)行擬合，發(fā)現(xiàn)擬合最佳曲線為正弦函數(shù)曲線，具體如下。

擬合得到的函數(shù)表達(dá)式為：

? ? ? ? ? ?

記導(dǎo)數(shù)為ds，利用diff求導(dǎo)函數(shù)得到導(dǎo)數(shù)表達(dá)式：

繼而可以得到對(duì)應(yīng)每個(gè)x取值得到的導(dǎo)數(shù)值，再對(duì)導(dǎo)數(shù)進(jìn)行反正切計(jì)算，得到軌跡上對(duì)應(yīng)點(diǎn)的航向角，記航向誤差為e2，計(jì)算公式為：

? ? ? ? ? ? ? ? ? ?

以橫向誤差e1和航向誤差e2作為反饋對(duì)方向盤轉(zhuǎn)角或前輪轉(zhuǎn)角進(jìn)行PID控制，PID控制原理為：

? ? ? ? ? ??

其中，Kp、Ki、Kd 分別為誤差的比例系數(shù)、積分系數(shù)和微分系數(shù)，調(diào)節(jié)三個(gè)參數(shù)來對(duì)系統(tǒng)進(jìn)行控制。

當(dāng)以橫向誤差e1為反饋部分取Kp、Ki均為1.5，微分系數(shù)為0；以航向誤差e2為反饋部分取Kp為3，其余兩系數(shù)均為0。得到軌跡如下：

四、縱向控制

繼而進(jìn)行縱向控制，車輛縱向控制系統(tǒng)的目的是車輛的縱向行駛速度跟蹤，車輛初始速度為0，到達(dá)指定位置時(shí)速度也為0，當(dāng)開始行駛時(shí)目標(biāo)速度為Vset，當(dāng)車輛即將到達(dá)目標(biāo)位置時(shí)其目標(biāo)速度為0。車輛縱向控制系統(tǒng)是一個(gè)簡(jiǎn)單的單輸入單輸出系統(tǒng)，以速度跟蹤誤差為反饋實(shí)現(xiàn)車輛速度跟蹤控制。跟蹤誤差計(jì)算公式如下：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

其中，目標(biāo)參考速度為：

? ? ? ? ? ? ? ? ? ? ??

其中，d0為判斷車輛是否接近目標(biāo)位置的距離閾值，d為車輛當(dāng)前位置到目標(biāo)位置的距離，記目標(biāo)位置坐標(biāo)為(xdes,ydes)，則d的計(jì)算公式為：

? ? ? ? ? ? ? ??

以速度跟蹤誤差為反饋進(jìn)行PID控制，取比例系數(shù)為0.8，積分系數(shù)為15，得到速度跟蹤圖如下：

至此，實(shí)現(xiàn)了對(duì)給定期望軌跡的跟蹤控制，并用PID進(jìn)行橫縱向閉環(huán)控制，形成了車輛運(yùn)動(dòng)學(xué)的閉環(huán)系統(tǒng)仿真，得到了較為理想的運(yùn)動(dòng)軌跡。

關(guān)于清研車聯(lián)

清研車聯(lián)，是清華大學(xué)蘇州汽車研究院重點(diǎn)孵化打造的汽車職業(yè)教育企業(yè)。

專注于汽車行業(yè)垂直產(chǎn)業(yè)鏈，致力于中國汽車產(chǎn)業(yè)新型人才的培養(yǎng)，為汽車產(chǎn)業(yè)的轉(zhuǎn)型升級(jí)大規(guī)模輸送合格人才，為院校和企業(yè)的人才培養(yǎng)提供綜合解決方案。

積極應(yīng)對(duì)汽車產(chǎn)業(yè)變革中科技、人才、服務(wù)等快速發(fā)展趨勢(shì)，充分依托清華大學(xué)優(yōu)質(zhì)的教育資源，重點(diǎn)聚焦新能源汽車、智能網(wǎng)聯(lián)汽車、汽車智能制造，汽車輕量化與NVH等汽車新技術(shù)領(lǐng)域，推陳出新，引領(lǐng)教育變革，從內(nèi)容和模式方面協(xié)同國內(nèi)汽車職業(yè)教育轉(zhuǎn)型升級(jí)。

在線教育MOOC官網(wǎng)：www.auto-mooc.com?