FSAC 賽車軌跡跟蹤協同控制策略研究

張志強， 李剛， 李寧， 田甜， 白鴻飛

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

近年來，隨著科學技術的發展，世界各國對智能汽車的研究越來越深入，由中國汽車工程學會舉辦的中國大學生無人駕駛方程式汽車大賽應運而生。軌跡跟蹤控制是無人駕駛方程式賽車最重要的技術之一，通過控制車輛的橫向和縱向運動，準確、快速地跟蹤上層參考路徑，確保行駛的穩定性和安全性［1］。國內外對智能汽車的軌跡跟蹤控制問題進行了大量的研究。

目前，智能車輛的軌跡跟蹤控制因使用的方法不同，主要可以分為智能控制理論、經典控制理論和現代控制理論［2］：一是以深度學習和強化學習方法為代表智能控制理論［3］；二是以PID 為典型代表的經典控制理論［4-5］；三是以現代控制理論為基礎，其中以滑模控制［6-7］、自適應控制［8］、魯棒控制［9］等方法為代表。AMIR 等［10］考慮到MPC 算法計算量大的問題，提出了一種基于模型縮減方案的切換預測控制方法，以實現不同模型粒度的自動駕駛車輛路徑跟蹤。WANG Zejiang 等［11］為了提高MPC的工作效率，提出了一種基于查詢表的在線參數選擇方法，以幫助車輛在穩定性和計算能力約束下跟蹤參考路徑。LIMA 等［12］提出了一種用于平滑自主路徑跟蹤的平滑精確MPC 算法。HAMID 等［13］設計了非線性MPC 控制器，來提高控制器的魯棒性。杭鵬等［14］采用一種帶有四輪轉向和直接橫擺力矩控制的MPC 軌跡控制轉向算法，提高了車輛的行駛穩定性。郭應時等［15］研究了車輛路徑跟蹤控制的擬人程度和乘坐舒適性。ZHANG Xizheng等［16］提出了一種基于車道檢測和滑模控制的智能電動汽車自動跟蹤控制，實現智能電動汽車準確穩定的路徑跟蹤和電機間最佳的轉矩分配。劉志強等［17］采用基于自適應時域參數MPC 的軌跡跟蹤方法，解決車輛跟蹤控制的穩定性和精度問題。盧佳興等［18］提出了一種雙參數自適應優化的算法，解決控制器適應性差的問題。鄭鑫等［19］提出一種模型參考自適應路徑跟蹤控制器，提高了車輛的跟蹤穩定性。

這些軌跡跟蹤控制研究方法多數采用的是MPC，但該算法具有很高的計算量，對硬件要求很高。為了降低對大學生無人駕駛方程式賽車硬件的要求，同時考慮到精度和穩定性，本文在縱向控制上采用PID算法，在橫向控制上采用LQR算法，為了更好地進行橫縱向控制，設計了一種協同控制器。在經過仿真與實車的驗證后得出結論，該控制策略能使無人駕駛方程式賽車在行駛過程中具有良好的跟蹤精度和穩定性。

1 車輛參考動力學建模

無人駕駛方程式賽車是一個高度集成并且是非線性的約束系統，因此，需要搭建賽車的動力學模型來提高賽車的控制精度。無人駕駛方程式賽車是一個復雜的系統，具有非線性和不完整的運動學約束，其工作條件受到許多測量不準確和不確定因素的影響，這些因素會對無人駕駛賽車的軌跡跟蹤控制造成一定的影響。本文的研究目標主要是使無人駕駛方程式賽車能準確、快速并且穩定地跟蹤期望軌跡，因此，就要考慮到無人駕駛方程式賽車的動力學性能，正常情況下，一個動力學模型要考慮的參數越多，就越精確和復雜，這也會造成模型求解上的困難和控制上的遲滯，從而影響無人駕駛方程式賽車軌跡跟蹤的準確性。由于賽道平順良好并且是干燥的瀝青路面，對影響車輛平穩性的懸架系統沒有進行深入研究，為了減少計算量，本文采用了線性二自由度“自行車”模型為車輛動力學模型，如圖1所示。

圖1 車輛動力學模型

其中，定義XOY為大地坐標系，并將其設為慣性參考系，原點o為車輛質心，xoy為車體坐標系。車輛二自由度微分方程如式（1）所示。

式中：m為賽車質量，單位kg；ay為賽車在質心處的側向加速度，單位m/s2；Fyf為前輪側偏力，單位N；Fyr為后輪側偏力，單位N；Iz為車輛質心繞z軸的轉動慣量，單位kg/m2；φ¨ 為橫擺角加速度，單位rad/s2；a、b分別為賽車質心到前軸與后軸之間的距離，單位m。

通常情況下，速度矢量的夾角很小，對其進行小角度假設，如式（2）所示。

式中：αf，αr為賽車前后輪側偏角，單位rad；vx為賽車縱向速度，單位m/s；vy為賽車側向速度，單位m/s；φ˙為賽車橫擺角速度，單位rad/s。賽車輪胎側偏力與側偏角的關系為：

式中：Cαf，Cαr為賽車前后輪側偏剛度，單位N/rad；δ為前輪轉角，單位rad。

聯立式（1）～（3），可得到車輛動力學方程，如式（4）所示。

2 橫縱向控制策略設計

2.1 橫向控制策略設計

在實際比賽過程中，通過感知模塊中賽車前方的激光雷達和賽車主環燈下方的攝像頭，采集到賽道兩側的錐桶信息，經過處理發送給路徑規劃模塊，進而規劃一條可行駛軌跡。為了使賽車能較好地跟蹤已規劃的軌跡，設計了LQR 控制器，對車輛進行橫向跟蹤控制，實現在橫向距離上準確且穩定的跟蹤。通過組合慣性導航裝置和輪速里程計來獲取賽車的實時定位和狀態信息，該控制算法分為預測模塊、誤差與曲率計算模塊、前饋控制計算模塊以及控制計算模塊4 個主要部分，預測模塊通過輸入賽車的狀態信息進行下一時刻賽車狀態的預測，并將預測出的下一時刻狀態信息與規劃的賽車狀態信息同時作為誤差與曲率計算模塊的輸入，將輸出的k值與賽車參數信息同時作為輸入發送給前饋控制計算模塊，計算出前輪轉角，將此時計算出的前輪轉角與誤差計算模塊計算出的誤差同時輸入給控制計算模塊，最終輸出前輪轉角給無人駕駛賽車。橫向控制算法流程如圖2所示。

圖2 橫向控制算法流程

2.1.1 預測模塊

由于駕駛員駕駛時會知道下一時刻的路徑，所以會做出是否撥動方向盤的駕駛行為，來對車輛進行橫向控制。但是無人駕駛方程式賽車在無人駕駛時是由控制算法進行控制的，如果當前的位置與規劃的軌跡之間誤差等于0 時，即使該時刻的速度方向與軌跡的方向不同，算法也不會及時轉動方向盤，從而導致車輛產生偏離已規劃好的軌跡的趨勢，即算法控制具有滯后性。因此，為了使賽車具有一定的可預見性，對賽車進行更好的控制，在LQR算法中增加了一個預測模塊。預測時間記為ts，如式（5）所示。

式中：xpre，vxpre，ypre，vypre，φpre，φ˙pre分別為預測后的橫向位置、橫向速度、縱向位置、縱向速度、橫擺角以及橫擺角速度。

2.1.2 誤差與曲率計算模塊

求解步驟如下。

1）根據規劃的(xr，yr)，找到與(x，y)最近的規劃點，將其定義為匹配點。將該點的序列記為dmin。則匹配點沿切線方向的向量則為：

匹配點沿法線方向的向量為：

2）根據真實的位置向量減去匹配點的位置向量得到誤差的距離向量，記為：

3）根據匹配點切線方向向量、法線方向向量與誤差的距離向量分別求出賽車在運動過程中的橫縱向位置誤差，即ed與es，如式（9）所示。

4）由于匹配點不等于投影點，所以在求解投影點切線方向與x軸之間的夾角，即投影點處所在切線的航向角θr時，要進行相應的運算，如式（10）所示。

5）假設匹配點到投影點的曲率kr=kdmin，根據橫、縱向速度、橫擺角、投影點處所在切線的航向角、橫擺角速度以及橫向位置誤差來求出橫擺角誤差、橫向位置誤差和橫擺角誤差隨時間變化的關系，如式（11）所示。

2.1.3 前饋控制模塊

根據賽車參數以及賽車在規劃的軌跡上投影點的曲率計算得出前輪轉角，如式（12）所示。

最后通過一個控制模塊計算得出最終的前輪轉角，如式（13）所示。

2.2 縱向控制策略設計

2.2.1 雙PID控制算法原理

PID 控制器在技術實踐中應用非常廣泛，因為其設計簡單，參數容易設置。基于此優勢，本文對PID 控制算法進行改進，將其應用在無人駕駛方程式賽車的縱向控制上。設計了一種以輸入為規劃的縱向速度、縱向位置誤差以及軌跡的跟蹤速度，輸出為電機轉矩和制動壓力的雙PID 控制器，通過控制位置偏差以及速度偏差來分別控制賽車縱向位置跟蹤和速度跟蹤。縱向控制過程原理如圖3所示。

圖3 縱向雙PID控制原理

圖3 中：x為加減速信號；s˙為軌跡的跟蹤速度；vp為規劃出的速度；v為期望速度。在本控制器設計中，通過多次調整參數，最終確定各系數為：KP= 3，KI= 0.02，Kd= 1 時，能較好地兼顧控制的精度和穩定性。

2.2.2 電門、制動標定

由于無人駕駛方程式賽車的動力形式為雙電機獨立驅動形式，所以對于驅動標定要進行電機轉矩的標定以及制動的標定。電機模型如式（14）所示。

式中：P為電機所需的功率；T為電機的轉矩；n為電機的轉速。轉矩與轉速的關系，如式（15）所示。

式中：Tmax為電機最大轉矩；apo為加速踏板開度；nb為電機最大功率與最大轉矩的比值。

基于此電機模型，通過Matlab/Simulink 進行仿真試驗，得到大量的速度、加速度與加速踏板開度的三維點，從而擬合出加速踏板開度關于速度與加速度所對應的值，進而制作電門與制動標定表。

2.3 橫縱向協同控制策略設計

2.3.1 協同控制原理

因為賽車運動時橫、縱向之間有較強的耦合關系，所以橫向控制器或縱向控制器單獨進行作用時，不能很好地表現出賽車的實際運動特性。通過分析本文所設計的橫向控制策略與縱向控制策略，發現二者有一個共同的影響因素，即縱向速度。由于賽車的縱向速度同時作為橫、縱向控制器的輸入，所以為了提高整體的控制效果，本文以縱向速度為結合點，設計了一種協同控制器。協同控制策略原理如圖4所示。

圖4 橫縱向協同控制策略原理

將規劃的參考路徑信息、期望車速、期望加速度以及賽車的實際狀態信息分別作為橫向控制器與縱向控制器的輸入，橫、縱向控制器的輸出分別為無人駕駛賽車的前輪轉角和目標驅動力矩，同時作為協同控制器的輸入，經過協同控制器的處理，輸出最終的前輪轉角和驅動力矩給無人駕駛方程式賽車。

2.3.2 工況分析

在比賽過程中，無人駕駛賽車的實際運動往往會出現以下4 種情況：（a）無人駕駛賽車與參考軌跡之間產生了橫向位置誤差，此時賽車在參考軌跡的左側并且向著遠離參考軌跡的方向行駛；（b）無人駕駛賽車與參考軌跡之間產生了橫向位置誤差，此時賽車在參考軌跡的右側并且向著靠近參考軌跡的方向行駛；（c）無人駕駛賽車與參考路徑之間產生橫向位置誤差，此時賽車在參考軌跡左側并且向著靠近參考軌跡的方向行駛；（d）無人駕駛賽車與參考軌跡之間產生橫向位置誤差，此時賽車在參考軌跡的右側并且向著遠離參考軌跡的方向行駛。4種運動情況如圖5所示。

2.3.3 協同控制器設計

通過對以上4 種工況進行分析，得出以下規律：當無人駕駛方程式賽車與參考軌跡之間產生了橫向位置偏差并且向著遠離參考軌跡的方向行駛時，此時應進行適當減速，讓賽車在橫向控制算法的作用下跟蹤參考軌跡；當無人駕駛方程式賽車與參考軌跡之間產生了橫向位置偏差并且向著靠近參考軌跡的方向行駛時，此時應進行適當加速，讓賽車在縱向控制算法的作用下快速地跟蹤參考軌跡。設定賽車在參考軌跡右側時，橫向偏差為正，反之，橫向偏差為負。首先對無人駕駛賽車的橫向位置偏差進行判定，來知曉無人駕駛賽車相對于參考軌跡的位置，進而將橫向位置偏差與無人駕駛賽車在橫向位置上可容忍的最大偏差進行比較，最后通過判定所參考的角度值的正負來進行加減速的控制。橫、縱向協同控制器的工作邏輯如圖6所示。

圖6 協同控制邏輯

圖6中：Y為橫向誤差可容忍最大值；φ為參考軌跡和半徑為r的預測范圍圓周的交點處參考軌跡的切線速度v1正方向與縱向車速v的正方向之間的夾角，以v1為定軸，如果車速v與v1之間的夾角φ是以v繞v1順時針方向轉動形成的，則假設其為正值，否則為負值。

3 仿真分析

3.1 參考軌跡設計

在進行仿真之前，首先要有一條參考軌跡來進行跟蹤，由于賽車存在運動學約束，如車輛位置連續則要求曲線也是連續的，橫擺角的連續性則要求曲線是連續到一階，而加速度約束則要求曲線是連續到二階。因此，為了更好地實現無人駕駛方程式賽車在行駛過程中的跟蹤效果，本文以五次多項式作為本次算法驗證中軌跡的擬合曲線，五次多項式的表達式如式（16）所示。

式 中 ：a0，a1，a2，a3，a4，a5，b0，b1，b2，b3，b4，b5均為待求參數。

3.2 賽車參數

本次仿真的被控對象為遼寧工業大學萬得無人駕駛電動方程式賽車，為了提高仿真精度，車輛參數嚴格按照遼寧工業大學萬得無人駕駛電動方程式賽車的參數進行設置，具體參數見表1。

表1 賽車基本參數

3.2 仿真結果分析

在循跡項目工況下進行仿真驗證，仿真結果如下。

由圖7 可知，整個跟蹤過程中橫擺角變化在0～0.2 rad 之間，由圖8 可知，前輪轉角在±0.5°之間。從整個跟蹤過程中可以看出，車輛的狀態都表現得很穩定，表明該控制算法具有很好的穩定性。

圖7 橫擺角

圖8 前輪轉角

由圖9 可知，無人駕駛賽車的速度能很好地跟蹤規劃的期望車速。速度誤差最大在0.2 m/s2左右，在速度變化較快時速度誤差在0.1 m/s2左右，可以看出，在速度跟蹤方面，該控制算法也具有較好的準確性和穩定性。

圖9 期望速度與跟蹤速度

由圖10～11 可知，無人駕駛方程式賽車在循跡工況下的縱向位置跟蹤誤差在±0.1 之間，具有較好的跟蹤效果。說明該縱向控制算法具有很好的可靠性。

圖10 縱向位置

圖11 縱向位置偏差

由圖12～13 可知，無人駕駛方程式賽車在循跡工況下的橫向位置跟蹤誤差在±0.03 m 之間，由于比賽中的賽道寬度至少為3 m，所以從仿真結果來看，使用該橫向控制算法具有很好的橫向位置跟蹤效果。

圖12 橫向位置

圖13 橫向位置偏差

圖14 是沒有橫縱向協同控制器與有協同控制器的車速跟蹤對比圖，在沒有協同控制器，即在對車輛進行橫向和縱向解耦控制的情況下，速度一般可以遵循期望的速度，但當車輛處于相對于期望軌跡的不同位置時，會出現較大的偏差；當加入協同控制器后，賽車的車速會通過協同控制器來進行實時調整，由圖14 可知，無人駕駛方程式賽車的實際速度與參考速度幾乎一致。可以說明本文所設計的橫、縱向協同控制器具有較好的協同效果，對無人駕駛賽車的軌跡跟蹤起到了一定的作用。

圖14 有協同控制器與無協同控制器車速對比

4 實車驗證

為了驗證本文設計的控制策略在實際應用中能起到良好的效果，以實驗室的FSAC 賽車為試驗平臺進行算法驗證。試驗賽車的行駛場景如圖15所示。

圖15 試驗賽車跑動畫面

由圖16 可知，橫向位置跟蹤相比于仿真得出的結果誤差會偏大一些，但在可接受范圍之內，誤差變大的原因可能包括通信的實時性較差和執行器機構的反應遲滯等情況。由圖17 可知，縱向位置跟蹤過程中的誤差情況與橫向位置跟蹤的情況相似，其誤差也在可接受范圍之內，滿足比賽的要求，驗證了本文所設計的控制策略的有效性。

圖16 實車與期望軌跡橫向位置對比

圖17 實車與期望軌跡縱向位置對比

5 結論

1）將本文所設計的控制算法運用在實驗室的賽車上，通過試驗驗證可知，本文所設計的控制算法具有跟蹤精度較高，穩定性良好的特點，且通過實車驗證了誤差在可接受范圍之內。

2）根據賽車相對參考軌跡的位置設計了協同控制器，解決了在賽車相對期望軌跡不同位置時車速會出現較大偏差的問題，跟蹤速度與期望速度基本一致，該研究方法也適合其他無人駕駛車輛。