FSAC 賽車軌跡跟蹤協(xié)同控制策略研究

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

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

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

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

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

1 車輛參考動力學(xué)建模

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

圖1 車輛動力學(xué)模型

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

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

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

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

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

聯(lián)立式（1）～（3），可得到車輛動力學(xué)方程，如式（4）所示。

2 橫縱向控制策略設(shè)計

2.1 橫向控制策略設(shè)計

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

圖2 橫向控制算法流程

2.1.1 預(yù)測模塊

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

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

2.1.2 誤差與曲率計算模塊

求解步驟如下。

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

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

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

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

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

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

2.1.3 前饋控制模塊

根據(jù)賽車參數(shù)以及賽車在規(guī)劃的軌跡上投影點的曲率計算得出前輪轉(zhuǎn)角，如式（12）所示。

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

2.2 縱向控制策略設(shè)計

2.2.1 雙PID控制算法原理

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

圖3 縱向雙PID控制原理

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

2.2.2 電門、制動標(biāo)定

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

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

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

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

2.3 橫縱向協(xié)同控制策略設(shè)計

2.3.1 協(xié)同控制原理

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

圖4 橫縱向協(xié)同控制策略原理

將規(guī)劃的參考路徑信息、期望車速、期望加速度以及賽車的實際狀態(tài)信息分別作為橫向控制器與縱向控制器的輸入，橫、縱向控制器的輸出分別為無人駕駛賽車的前輪轉(zhuǎn)角和目標(biāo)驅(qū)動力矩，同時作為協(xié)同控制器的輸入，經(jīng)過協(xié)同控制器的處理，輸出最終的前輪轉(zhuǎn)角和驅(qū)動力矩給無人駕駛方程式賽車。

2.3.2 工況分析

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

2.3.3 協(xié)同控制器設(shè)計

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

圖6 協(xié)同控制邏輯

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

3 仿真分析

3.1 參考軌跡設(shè)計

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

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

3.2 賽車參數(shù)

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

表1 賽車基本參數(shù)

3.2 仿真結(jié)果分析

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

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

圖7 橫擺角

圖8 前輪轉(zhuǎn)角

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

圖9 期望速度與跟蹤速度

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

圖10 縱向位置

圖11 縱向位置偏差

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

圖12 橫向位置

圖13 橫向位置偏差

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

圖14 有協(xié)同控制器與無協(xié)同控制器車速對比

4 實車驗證

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

圖15 試驗賽車跑動畫面

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

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

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

5 結(jié)論

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

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