無(wú)人駕駛汽車軌跡解算及跟蹤控制研究

張旭斌，李剛，冀同濤，任建平

（遼寧工業(yè)大學(xué) 汽車與交通工程學(xué)院，遼寧 錦州 121001）

前言

近年來(lái)，由于無(wú)人駕駛汽車在智能網(wǎng)聯(lián)交通系統(tǒng)中扮演著越發(fā)重要的角色，而軌跡跟蹤是無(wú)人駕駛汽車研究重點(diǎn)之一[1]。

求解軌跡曲線的方法有很多種，主要包括最優(yōu)軌跡曲線[2]、五次多項(xiàng)式生成軌跡曲線[3]等。最常用的是參數(shù)方程法，但方程次數(shù)太高會(huì)提高計(jì)算量且增加計(jì)算誤差，因此本文提出一種基于時(shí)間t 的分段三次參數(shù)方程法，可以減少以上所述的誤差。

在無(wú)人駕駛汽車軌跡跟蹤控制方面，主要有基于預(yù)瞄軌跡的跟蹤控制方法[4]、模型預(yù)測(cè)控制（MPC）[5]以及線性二次型最優(yōu)控制（LQR）[6]等。由于LQR 控制能用于嵌入式開(kāi)發(fā)系統(tǒng)中，因此采用LQR 實(shí)現(xiàn)無(wú)人駕駛汽車的軌跡跟蹤控制。

綜上所述，本文提出一種基于時(shí)間t 的分段三次參數(shù)方程方法，用其解算出軌跡點(diǎn)，利用最優(yōu)控制解算出方向盤(pán)反饋控制量，與解算出軌跡點(diǎn)轉(zhuǎn)向角一同輸入無(wú)人駕駛轉(zhuǎn)向控制器中，構(gòu)成閉環(huán)控制。

1 軌跡解算方法

本文按照無(wú)人駕駛汽車傳感器采集的道路中心坐標(biāo)，用五次樣條曲線擬合的方法形成路徑方程曲線：

由于速度、加速度分別是位移關(guān)于時(shí)間t 的一階導(dǎo)數(shù)、二階導(dǎo)數(shù)，因此，用含時(shí)間t 的三次參數(shù)方程組就可以完全表示某一段的軌跡曲線方程：

為了更進(jìn)一步的提高軌跡曲線方程的準(zhǔn)確性，在擬合出的路徑曲線上尋找等線長(zhǎng)的點(diǎn)來(lái)解算軌跡曲線方程的參數(shù)。依據(jù)擬合出的路徑曲線方程分別求出每一點(diǎn)上對(duì)應(yīng)的時(shí)間、速度、切線，如公式(4)～(7)。

設(shè)初始位置A1(t1,x1,y1,v1,r1)

下一時(shí)刻的坐標(biāo)點(diǎn)，按照求解曲線長(zhǎng)度的函數(shù)：

下一時(shí)刻車輛的速度，如果vi＞vmax，則vi+1= vmax；如果vi＜vmax，則vi+1= vi。

下一刻的時(shí)間：

初步計(jì)劃取含有10 個(gè)區(qū)間的參數(shù)方程函數(shù)，含X 的參數(shù)方程矩陣為G(10,4)，含Y 的參數(shù)方程矩陣為F(10,4)，時(shí)間的矩陣為T(mén)(4,4)，按照矩陣運(yùn)算就可求出G、F，如公式(8)～(11)。

按照上述方法求可以求出軌跡曲線方程，以時(shí)間t=0.05s為采樣間隔，分別求出含位置、速度、航向、前輪轉(zhuǎn)角、轉(zhuǎn)彎半徑信息的軌跡點(diǎn)，如公式(11)、(12)。

根據(jù)曲率公式：

注：此時(shí)y=f(x)，L=2.8m 為汽車的軸距，則按照阿克曼轉(zhuǎn)角原理可求出前輪轉(zhuǎn)向角如下式：

2 跟蹤控制方法

線性二次型最優(yōu)控制算法，在系統(tǒng)運(yùn)行過(guò)程中，當(dāng)系統(tǒng)狀態(tài)因任何因素偏離平衡狀態(tài)時(shí)，在盡量少消耗能量的情況下，能夠保持控制系統(tǒng)狀態(tài)的每個(gè)分量仍接近平衡狀態(tài)，有三個(gè)矩陣，Q 狀態(tài)誤差加權(quán)矩陣、R 控制加權(quán)矩陣、QN終端誤差加權(quán)矩陣，利用變分法原理使目標(biāo)函數(shù)J 取得最小值，尋求最優(yōu)控制δ(t)，將系統(tǒng)從初始狀態(tài)量轉(zhuǎn)移到終端狀態(tài)時(shí)，實(shí)現(xiàn)用較小的控制保持較低的跟蹤誤差，達(dá)到能量和控制跟蹤誤差最優(yōu)的目的。

結(jié)合上兩節(jié)的道路軌跡點(diǎn)模型和車輛模型，使用線性二次型最優(yōu)控制消除車輛當(dāng)前狀態(tài)和參考路徑上參考點(diǎn)的狀態(tài)誤差，定義了如下的評(píng)價(jià)函數(shù)：

式中，Q 為車輛狀態(tài)誤差權(quán)重矩陣，R 為轉(zhuǎn)角控制量權(quán)重矩陣，QN為車輛終端狀態(tài)誤差權(quán)重矩陣，xk為無(wú)人駕駛車輛第k 時(shí)刻的預(yù)測(cè)行駛狀態(tài)，rk為參考路徑上第k 時(shí)刻的目標(biāo)行駛狀態(tài)，δk+ Δδk為第k 時(shí)刻的轉(zhuǎn)向角控制量。

上式中，δk為反饋控制量，Δδk為參考軌跡與實(shí)際軌跡處的差值。

最終設(shè)定Q=[55 0 0;0 100 0;0 0 350]；R=[7]；QN=[1 0 0;0 1 0;0 0 1]。

3 仿真試驗(yàn)驗(yàn)證

為了驗(yàn)證論文中的軌跡點(diǎn)生成方法及軌跡跟蹤效果，在MATLAB 中編寫(xiě)了M 語(yǔ)言，設(shè)定車輛軸距 L=2.8m，以車速60km/h 進(jìn)行了驗(yàn)證。設(shè)置車的初始狀態(tài)(0,0,pi/3)，以t=0.05s的軌跡點(diǎn)采樣時(shí)間，每?jī)蓚€(gè)采樣點(diǎn)之間車輛模型分10 步運(yùn)行，仿真結(jié)果表明：由圖1 可以看出依據(jù)路徑點(diǎn)結(jié)算出的軌跡點(diǎn)曲線，圖2、圖3 表明給算法能有效的跟蹤軌跡，最大偏差在0.19m。

圖1 依據(jù)路徑點(diǎn)生成軌跡點(diǎn)

圖2 60km/h 仿真跟蹤軌跡點(diǎn)

圖3 60km/h 前輪轉(zhuǎn)角

4 結(jié)論

（1）本文針對(duì)無(wú)人駕駛汽車中的軌跡解算與跟蹤控制這一關(guān)鍵技術(shù)，采用了一種關(guān)于時(shí)間t 的三次分段參數(shù)軌跡方程方法，計(jì)算出每一時(shí)刻的軌跡點(diǎn)信息。

（2）搭建了車輛二自由度運(yùn)動(dòng)學(xué)模型，結(jié)合線性二次型最優(yōu)控制算法構(gòu)成反饋控制，在MATLAB 中編寫(xiě)了控制程序，進(jìn)行了實(shí)驗(yàn)仿真驗(yàn)證。實(shí)驗(yàn)表明，這一方法能夠解決無(wú)人駕駛車輛中的軌跡解算與跟蹤控制這一關(guān)鍵技術(shù)，并且具有良好的跟蹤精度。