無人駕駛車輛軌跡跟蹤控制

陳 誠

（西華大學 汽車與交通學院，四川 成都 610039）

近年來，智能交通、人工智能技術快速發(fā)展，自動駕駛已成為各領域的研究熱點。作為支撐汽車自動駕駛的關鍵技術之一，軌跡跟蹤要求控制車輛使之能夠按照參考軌跡行駛，如何確保跟蹤的精確性和穩(wěn)定性一直是無人駕駛控制技術重點與難點[1]。目前軌跡跟蹤控制的主流方法主要有基于模型預測控制[2]為代表的方法，基于汽車動力學的LQR算法以及基于運動學幾何追蹤的方法，其中幾何追蹤方法主要包含純跟蹤和Stanley兩種方法，純跟蹤方法已經廣泛應用于移動機器人的路徑跟蹤中，本文介紹兩種基于幾何追蹤的方法—純跟蹤控制算法和Stanley[3]控制算法。

1 車輛運動學自行車模型

運動學是從幾何學的角度研究物體的運動規(guī)律，包括物體在空間的速度、位置等隨時間而產生的變化，因此，車輛運動學模型能反映車輛位置、速度、加速度等與時間的關系。所以汽車運動學模型對于控制器來說是非常重要的。如果我們想要更好的去控制車輛的運動，那么我們就要首先建立起一個精確的汽車運動學模型。建立好的汽車運動學模型能讓控制器對汽車形成一個恰當合理的約束，自行車模型就是一個重要的汽車運動學模型。

我們一般做出如下假設：不引入任何會影響到車輛動作的力，車輛只在水平面上運動，在垂直方向上的側傾和前后俯仰等不考慮。車輛結構就像自行車，左右前輪的轉角和轉速一致，后輪也是一樣。所以兩個前輪和后輪各可只用一個輪胎來描述。前后輪的各自速度矢量和各自輪胎朝向一致，即假設輪胎無側滑。而一般只有在低速狀態(tài)下，輪胎產生的側向力很小可忽略，所以該模型也主要應用于低速場景。

2 純跟蹤算法原理

基于當前車輛后輪中心位置，在參考路徑上向距離ld匹配一個預瞄點，假設車輛后輪輪中心點可以安置按照一定的轉彎半徑R行駛抵達該預瞄點[4]。然后根據(jù)預瞄距離ld，轉彎半徑R，車輛坐標系下預瞄點的朝向角2α之間的幾何關系來確認前輪轉角。圖1為純跟蹤算法原理圖。

圖1 純跟蹤算法原理

根據(jù)在ΔOAC中，AB⊥AO，則∠AOC：

為了使車輛后輪跟蹤圓弧虛線軌跡到達C點，在ΔOAC中需要滿足的正弦定理關系為

化簡上式得

再看為了達到這種關系，作為控制變量的前輪轉角需要滿足的關系。在阿克曼轉向ΔOAC中

因此，聯(lián)立式（3）（4）得

另外，定義橫向誤差為車輛當前姿態(tài)和預瞄點在橫向上的誤差

聯(lián)立式（5）和式（6），并考慮小角度假設得

所以純跟蹤本質上是一個P控制器，那么跟蹤效果將由ld決定，通常定義ld為關于速度的一次多項式

式中，kv為瞄準距離系數(shù)；v為車速。

為了驗證預瞄距離ld是如何影響跟蹤效果的，現(xiàn)將算法通過MATLAB進行仿真試驗，由于純跟蹤算法本質上是一個P控制器，所以控制系數(shù)不宜過大，過大會影響系統(tǒng)的穩(wěn)定性，因此，在速度一定的情況下把預瞄距離系數(shù)kv分別設置為0.02、0.1、0.5。試驗結果如圖2所示。圖中的橫坐標x和縱坐標y分別表示縱向位移和橫向位移。

圖2 預瞄距離系數(shù)為0.02

如圖3、圖4所示，隨著預瞄距離增加，車輛對于期望軌跡的跟隨能力會減弱。

圖3 預瞄距離系數(shù)為0.1

圖4 預瞄距離系數(shù)為0.5

3 斯坦利算法原理

Stanley算法是斯坦福大學開發(fā)的無人車，通過這樣設計橫向控制器，Stanley算法相比較純跟蹤算法，優(yōu)點就是既考慮了車身偏航角，又考慮了車與跟蹤路徑的橫向誤差距離，Stanley控制又稱前輪反饋控制，圖5為斯坦利算法原理圖。其核心思想是基于前輪中心的路徑跟蹤偏差量對方向盤轉向控制量進行計算。Stanley方法是一種基于橫向跟蹤誤差為前軸中心到最近路徑點的距離的非線性反饋函數(shù)，并且能實現(xiàn)橫向跟蹤誤差指數(shù)收斂于0。根據(jù)車輛位姿與給定路徑的相對幾何關系可以直觀地獲得控制車輛方向盤轉角的控制變量。

圖5 斯坦利算法原理

前輪轉角控制變量由兩部分構成:一部分是航向誤差引起的轉角，即當前車身方向與參考軌跡最近點的切線方向的夾角；另一部分是橫向誤差引起的轉角，即前輪中心到參考軌跡最近點的橫向距離。

不考慮橫向跟蹤誤差的情況下，前輪偏角應當與給定路徑參考點的切線方向一致。其中，θφ表示車輛航向與最近路徑點切線方向之間的夾角，在沒有任何橫向誤差的情況下，前輪方向與所在路徑點的方向相同，即δφ=θφ。

在不考慮航向跟蹤偏差的情況下，橫向跟蹤誤差越大，前輪轉向角越大。假設車輛預期軌跡在距離前輪d(t)處與給定路徑上最近點相交，根據(jù)幾何關系得出如下非線性比例函數(shù)：

式中，d(t)與車速相關，用車速v(t)，控制系數(shù)k表示。

所以，前輪轉角為

另外，橫向誤差的變化率為

其中，sinδy根據(jù)幾何關系可知

故有

當橫向跟蹤誤差ey很小時，上式改寫為

積分上式，得

因此，橫向誤差指數(shù)收斂于ey(0)，Stanley算法是一種直接對前輪轉角進行調整來消除橫向偏差的橫向控制算法。為了驗證k是如何影響跟蹤效果的，現(xiàn)將算法通過matlab進行仿真試驗，由于控制系數(shù)k決定了橫向誤差的收斂速度，所以k值過大或者過小都會影響跟蹤的效果，因此，在速度一定的情況下把k值分別設置為2，5，15。試驗結果如圖6—圖8所示。

圖6 控制系數(shù)k=2

圖7 控制系數(shù)k=5

圖8 控制系數(shù)k=15

由上圖可以看出無論控制系數(shù)過大還是過小都無法取得良好的控制效果。因此，選取合適的控制系數(shù)是取得良好跟蹤效果的關鍵。

4 MATLAB對比仿真分析

圖10 車速10 m/s下的Stanley算法跟蹤效果

圖11 車速25 m/s下的純跟蹤算法跟蹤效果

為了對比驗證本文所提出的純跟蹤控制算法和Stanley控制算法在不同工況下的跟蹤精度，通過MATLAB軟件進行仿真，在對比仿真中，純跟蹤算法中的預瞄距離系數(shù)kv取在上文仿真效果較好的0.02，同理Stanley算法中的控制系數(shù)k取仿真效果較好的5，仿真工況為車速10 m/s和25 m/s，仿真結果如圖9—圖12所示，其中橫縱向坐標分別表示縱向位移和橫向位移。

圖9 車速10 m/s下的純跟蹤算法跟蹤效果

圖12 車速25 m/s下的Stanley算法跟蹤效果

5 結論

純跟蹤和Stanley兩個方法都是基于對前輪轉角進行控制來消除橫向誤差，以達到車輛跟蹤期望軌跡的效果。這兩種算法實際都是基于運動學模型的純幾何跟蹤方法。在低速場景時，兩種算法都有較好的跟蹤效果，但是隨著速度的增加，兩種算法的跟蹤效果都會變差。另外純跟蹤算法的關鍵在于預瞄系數(shù)的選取，盡量選取一個較小的值，但是具體是多少要結合實際。Stanley算法的控制效果取決于控制系數(shù)，而且缺少純跟蹤算法的規(guī)律性，通過仿真可以發(fā)現(xiàn)，這個值無論是偏大還是偏小都很難有一個較好的控制效果，調試時需要花一定精力去找那個合適的值。