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

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

（遼寧工業大學 汽車與交通工程學院，遼寧 錦州 121001）

前言

近年來，由于無人駕駛汽車在智能網聯交通系統中扮演著越發重要的角色，而軌跡跟蹤是無人駕駛汽車研究重點之一[1]。

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

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

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

1 軌跡解算方法

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

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

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

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

下一時刻的坐標點，按照求解曲線長度的函數：

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

下一刻的時間：

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

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

根據曲率公式：

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

2 跟蹤控制方法

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

結合上兩節的道路軌跡點模型和車輛模型，使用線性二次型最優控制消除車輛當前狀態和參考路徑上參考點的狀態誤差，定義了如下的評價函數：

式中，Q 為車輛狀態誤差權重矩陣，R 為轉角控制量權重矩陣，QN為車輛終端狀態誤差權重矩陣，xk為無人駕駛車輛第k 時刻的預測行駛狀態，rk為參考路徑上第k 時刻的目標行駛狀態，δk+ Δδk為第k 時刻的轉向角控制量。

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

最終設定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 語言，設定車輛軸距 L=2.8m，以車速60km/h 進行了驗證。設置車的初始狀態(0,0,pi/3)，以t=0.05s的軌跡點采樣時間，每兩個采樣點之間車輛模型分10 步運行，仿真結果表明：由圖1 可以看出依據路徑點結算出的軌跡點曲線，圖2、圖3 表明給算法能有效的跟蹤軌跡，最大偏差在0.19m。

圖1 依據路徑點生成軌跡點

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

圖3 60km/h 前輪轉角

4 結論

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

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