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

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

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

前言

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

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

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

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

1 軌跡解算方法

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

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

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

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

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

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

下一刻的時間：

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

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

根據(jù)曲率公式：

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

2 跟蹤控制方法

線性二次型最優(yōu)控制算法，在系統(tǒng)運行過程中，當(dāng)系統(tǒng)狀態(tài)因任何因素偏離平衡狀態(tài)時，在盡量少消耗能量的情況下，能夠保持控制系統(tǒng)狀態(tài)的每個分量仍接近平衡狀態(tài)，有三個矩陣，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)時，實現(xiàn)用較小的控制保持較低的跟蹤誤差，達(dá)到能量和控制跟蹤誤差最優(yōu)的目的。

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

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

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

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

3 仿真試驗驗證

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

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

圖2 60km/h 仿真跟蹤軌跡點

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

4 結(jié)論

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

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