基于模糊LQR的智能汽車路徑跟蹤控制*

胡 杰，鐘鑫凱，陳瑞楠，朱令磊，徐文才，張敏超

（1.武漢理工大學，現(xiàn)代汽車零部件技術(shù)湖北省重點實驗室，武漢 430070；2.武漢理工大學，汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心，武漢 430070；3.新能源與智能網(wǎng)聯(lián)車湖北工程技術(shù)研究中心，武漢 430070）

前言

近年來，智能汽車發(fā)展迅速，在一些封閉場景中已實現(xiàn)商業(yè)化落地，具有廣闊的前景。智能汽車的路徑跟蹤控制是實現(xiàn)自動駕駛的關(guān)鍵技術(shù)之一，其主要功能是控制車輛的前輪轉(zhuǎn)角，使車輛能夠沿參考路徑行駛，盡可能地減少受控車輛與參考路徑之間的偏差。在非極限工況下，跟蹤精度是路徑跟蹤控制的首要目標。

國內(nèi)外研究人員針對智能汽車的路徑跟蹤控制問題進行了大量研究工作。現(xiàn)有的路徑跟蹤控制方法，可根據(jù)是否采用車輛模型分為兩類。無模型控制方法不需要建立車輛模型，忽略車輛特性，直接根據(jù)車輛與參考路徑之間的偏差計算前輪轉(zhuǎn)角，包括PID 控制方法和模糊控制方法等；基于模型的控制方法需要建立車輛運動學或動力學模型，充分考慮車輛的系統(tǒng)特性，包含純追蹤控制方法、Stanley 控制方法、MPC 控制方法和LQR 控制方法等。

上述路徑跟蹤控制方法中，LQR 控制方法能夠兼顧多項性能指標，在中低車速工況下具有較好的控制效果；并且具有較好的實時性，對硬件設(shè)備要求低，能夠滿足工程實踐的需要。因此，許多研究人員對LQR 路徑跟蹤控制方法開展了深入研究。文獻［11］中基于LQR 提出了前輪轉(zhuǎn)角與橫擺力矩的集成控制策略，該策略具有較好的跟蹤精度與橫向穩(wěn)定性。為了減小LQR 控制器的穩(wěn)態(tài)誤差，文獻［12］和文獻［13］中構(gòu)建了加入前饋控制的LQR 控制器，相較于未帶前饋控制的LQR 控制器，有效減少了跟蹤誤差。更進一步，文獻［14］中針對LQR 控制器權(quán)重固定的問題，提出了基于跟蹤偏差的權(quán)重調(diào)整策略，再次提升了LQR 控制器的精確性。

雖然加入前饋控制對LQR 控制器進行轉(zhuǎn)角補償，可以在一定程度上提高跟蹤精度，但前饋控制和LQR 控制器都是基于同一簡化系統(tǒng)模型，存在較大的局限性。實際車輛是一個復雜系統(tǒng)，前饋控制不能有效減少由于系統(tǒng)參數(shù)不確定性所導致的跟蹤誤差。同時，系統(tǒng)以質(zhì)心為控制點，前饋控制相對滯后，可能產(chǎn)生超調(diào)，導致車輛轉(zhuǎn)角頻繁變動，行駛穩(wěn)定性變差。此外，不同的車速條件下，同一控制轉(zhuǎn)角引起的車輛位置變動不同。因此，即使路徑偏差相同，也需要控制器根據(jù)車速變化輸出相應控制轉(zhuǎn)角。由于簡化建模、系統(tǒng)參數(shù)不確定以及執(zhí)行機構(gòu)響應滯后等問題，LQR 控制器自身并不能很好地適應車速變化，固定的權(quán)重參數(shù)無法保證不同車速下控制效果。

針對上述問題，本文中設(shè)計了一種帶預瞄PID的模糊LQR 路徑跟蹤控制器。首先建立了路徑跟蹤誤差模型，在此基礎(chǔ)上設(shè)計了LQR 控制器，并采用預瞄PID 的方法對LQR 控制器進行轉(zhuǎn)角補償，減小穩(wěn)態(tài)誤差；然后分析車速變化對于控制效果的影響，提出了基于車速的權(quán)重參數(shù)模糊調(diào)節(jié)策略，對權(quán)重參數(shù)進行自適應調(diào)節(jié)，以此保證不同車速下的控制效果；最后，通過實車試驗平臺對設(shè)計的控制器進行對比分析，驗證其準確有效。

1 路徑跟蹤誤差模型

1.1 車輛動力學模型

智能汽車的路徑跟蹤控制，主要涉及車輛的側(cè)向運動與橫擺運動，為簡化計算，假設(shè)車輛同軸車輪具有相同的側(cè)偏剛度與轉(zhuǎn)角，由此可以合并同軸車輪，采用2 自由度單軌車輛模型作為車輛模型，如圖1所示。

圖1 車輛動力學模型

假設(shè)車輛勻速行駛，且前輪轉(zhuǎn)角較小，分析車輛受力情況，可以得到：

式中：為整車質(zhì)量；a為車輛的側(cè)向加速度；F、F分別為車輛的前、后輪胎所受側(cè)向力；I為車輛繞垂線方向的轉(zhuǎn)動慣量；為車輛的橫擺角速度；、分別為車輛前、后軸與質(zhì)心的距離。

在附著條件良好、車速較低的常規(guī)工況下，車輛的輪胎側(cè)偏角較小，可認為輪胎所受側(cè)向力與側(cè)偏角成線性關(guān)系。由此，得到車輛動力學模型：

式中：v、v分別為車輛的縱、橫向車速；、分別為車輛前、后軸車輪的側(cè)偏剛度；為前輪轉(zhuǎn)角。

1.2 路徑跟蹤誤差模型

智能汽車跟蹤參考路徑時，主要考慮側(cè)向誤差和航向誤差。如圖2 所示，定義車輛質(zhì)心到參考路徑的最短距離為側(cè)向誤差e，車輛航向角與參考航向角之差為航向誤差e。

圖2 路徑跟蹤誤差模型

假設(shè)航向誤差較小，可以得到航向誤差和側(cè)向速度誤差：

式中：為車輛當前航向角；為參考航向角。

假設(shè)參考路徑的轉(zhuǎn)彎半徑為，可以得到車輛的參考側(cè)向加速度為

車輛的實際加速度為

根據(jù)式（5）和式（6），可以得到側(cè)向加速度誤差為

綜合上述內(nèi)容，可以得到

其中：

2 路徑跟蹤控制器

路徑跟蹤控制器的整體結(jié)構(gòu)如圖3 所示。首先基于路徑跟蹤誤差模型，設(shè)計LQR 控制器作為路徑跟蹤控制器的主體部分，使車輛基本按照參考路徑行駛；在此基礎(chǔ)上，采用預瞄PID 方法作為轉(zhuǎn)角補償，消除由于系統(tǒng)簡化與參數(shù)失準導致的穩(wěn)態(tài)誤差，同時避免由于響應滯后而產(chǎn)生的超調(diào)；最后，對LQR控制器的權(quán)重參數(shù)進行模糊調(diào)節(jié)，提升控制器對于車速的適應性。

圖3 路徑跟蹤控制器結(jié)構(gòu)

2.1 LQR控制器

LQR 控制器針對離散系統(tǒng)進行控制，因此設(shè)定控制步長為，選擇雙線性離散化的方法，將連續(xù)的狀態(tài)空間方程轉(zhuǎn)換為離散系統(tǒng)，可以得到：

式中：=(-/2)(+/2)；=；()為時刻的狀態(tài)量；()為時刻的控制量。

LQR控制器的控制目的不僅需要減小車輛的路徑跟蹤誤差，還需要保持控制量盡可能小，以保證車輛行駛的穩(wěn)定性。由此，定義如下目標函數(shù)：

式中：為狀態(tài)量；為控制量；為狀態(tài)量權(quán)重矩陣；為控制量權(quán)重矩陣。

假設(shè)LQR控制器的控制律為

式中為控制增益系數(shù)。

將式（13）代入式（12），可以得到：

假設(shè)存在一常量矩陣，使系統(tǒng)趨于穩(wěn)定，可以得到：

式中為如下Riccati方程的正定解：

2.2 預瞄PID轉(zhuǎn)角補償

預瞄PID 控制方法不依賴于系統(tǒng)建模，直接針對誤差進行控制，在小范圍內(nèi)具有良好的穩(wěn)定性，采用此方法進行小角度的轉(zhuǎn)角補償，能夠有效彌補由于系統(tǒng)簡化和參數(shù)失準導致的穩(wěn)態(tài)誤差。此外，由于加入了預瞄距離，控制器存在一定預見性，可以有效改善轉(zhuǎn)向滯后問題，避免超調(diào)。

如圖2 所示，智能汽車的預瞄距離為，其數(shù)值大小與車速相關(guān)，具體如下：

式中：為基礎(chǔ)預瞄距離；為預瞄時長。

由此，可以確定預瞄點位置：

式中：、為智能汽車質(zhì)心的縱、橫向位置；、為預瞄點縱、橫向位置；為車輛航向角。

定義預瞄點與參考路徑之間的最短距離為預瞄誤差，預瞄PID 控制器直接針對離散系統(tǒng)的誤差進行控制，控制步長與LQR 控制器保持一致，具體如下：

式中：為預瞄PID 控制器輸出的補償轉(zhuǎn)角；、、分別為比例、積分、微分常數(shù)；()為時刻的預瞄偏差。

綜合上述內(nèi)容，可以得到控制系統(tǒng)最終輸出至智能汽車的轉(zhuǎn)向盤轉(zhuǎn)角為

式中表示車輛轉(zhuǎn)向機構(gòu)的傳動比。

2.3 權(quán)重參數(shù)模糊調(diào)節(jié)

實車環(huán)境中，系統(tǒng)復雜，與理想條件差別較大，車速變化會對路徑跟蹤控制器的控制效果造成較大影響。固定權(quán)重的控制器無法較好地適應車速變化，在不同車速條件下保持良好的控制效果。針對此問題，本文中采用模糊控制方法實現(xiàn)對LQR 控制器權(quán)重參數(shù)的自適應調(diào)節(jié)，保證不同車速條件下的跟蹤性能。

2.3.1 變量設(shè)計

LQR 控制器包含兩個權(quán)重矩陣和，分別對應狀態(tài)量與控制量，可以描述為

式中：、、、分別為側(cè)向誤差、側(cè)向速度誤差、航向誤差、航向角速度誤差的權(quán)重參數(shù)；為前輪轉(zhuǎn)角的權(quán)重參數(shù)。

智能汽車跟蹤參考路徑時，控制器主要針對側(cè)向誤差、航向誤差和前輪轉(zhuǎn)角3 個變量進行控制，兼顧準確性與穩(wěn)定性。權(quán)重參數(shù)越大，其對相應變量的限制作用越強，權(quán)重參數(shù)的相對大小決定了控制器對各變量的控制程度。誤差的權(quán)重參數(shù)越大，跟蹤精度越高；轉(zhuǎn)角的權(quán)重參數(shù)越大，轉(zhuǎn)角變化越小，駕駛的穩(wěn)定性與舒適性更好。在低車速條件下，車輛行駛平穩(wěn)，應增大誤差權(quán)重，以保障跟蹤精度為主。在高車速條件下，車輛短時間的位置變動較大，轉(zhuǎn)角過大容易導致超調(diào)，所以應增大轉(zhuǎn)角權(quán)重，加強對前輪轉(zhuǎn)角的限制，防止車輛因超調(diào)而左右搖擺，導致車輛的跟蹤精度與行駛穩(wěn)定性變差。

根據(jù)上述分析，本文中將、置為0，同時設(shè)定為固定值，通過調(diào)節(jié)和，改變權(quán)重的相對大小，實現(xiàn)控制器對于車速變化的自適應。設(shè)定模糊調(diào)節(jié)的輸入變量為車速v，輸出變量為控制系數(shù)、的調(diào)整量，表示為Δ、Δ。各變量取值范圍如表1所示。

表1 變量論域

2.3.2 模糊化處理

模糊語言變量集劃分為5 個模糊子集：負大（NB），負小（NS），零（O），正?。≒S），正大（PB）。隸屬度函數(shù)為高斯型函數(shù)，具體如下：

式中：為變量；和為參數(shù)。

分別對輸入與輸出變量進行模糊化處理，結(jié)果如圖4所示。

圖4 隸屬度函數(shù)

2.3.3 模糊規(guī)則與解模糊

智能汽車的車速越高，對前輪轉(zhuǎn)角的限制程度應越大，保持行駛穩(wěn)定，防止超調(diào)振蕩，具體模糊規(guī)則如表2所示。

表2 模糊規(guī)則

根據(jù)制定的模糊規(guī)則，輸入量經(jīng)模糊推理后，可得到模糊輸出量，選擇質(zhì)心法解模糊，得到精確輸出量Δ、Δ。

式中、分別為基準車速下側(cè)向誤差、前輪轉(zhuǎn)角的最優(yōu)權(quán)重參數(shù)。

最后，得到不同車速下的最優(yōu)權(quán)重矩陣：

式中、、分別為基準車速下側(cè)向速度誤差、航向誤差和航向角速度誤差的最優(yōu)權(quán)重參數(shù)。

3 實車試驗

為驗證設(shè)計的路徑跟蹤控制器，基于實車試驗平臺對其進行試驗測試。

所用的實車試驗平臺由純電動線控乘用車底盤改裝而來，加裝了攝像頭、毫米波雷達、激光雷達、GPS、IMU等高性能傳感器，如圖5（a）所示。

圖5 實車試驗平臺與軟件架構(gòu)

整車的結(jié)構(gòu)參數(shù)如表3所示。

表3 整車結(jié)構(gòu)參數(shù)

實車試驗平臺的軟件架構(gòu)如圖5（b）所示，為便于實時調(diào)整，采用分層控制結(jié)構(gòu)。上層路徑跟蹤控制程序采用Simulink 編寫，部署在PC 上，通過UDP與底層驅(qū)動程序通信；GPS、IMU、車輛VCU 控制等模塊的驅(qū)動程序采用LabVIEW 編寫，部署在NI控制器中，通過CAN 報文與硬件設(shè)備進行通信，實時接收傳感器信號并控制車輛。

試驗場地為校園內(nèi)一封閉練車場，采用Fishhook 型路徑。車輛轉(zhuǎn)角由路徑跟蹤器控制，車速采用PID控制，控制頻率均為50 Hz。

3.1 轉(zhuǎn)角補償效果試驗

為進行對比分析，分別設(shè)計了無轉(zhuǎn)角補償?shù)腖QR 控制器和帶前饋控制的LQR 控制器，與本文中設(shè)計的控制器同時進行路徑跟蹤試驗。車速設(shè)定為3 m/s，LQR 控制權(quán)重均為當前車速下的最優(yōu)權(quán)重參數(shù)。試驗結(jié)果如圖6～圖8所示。

圖6顯示了在3種控制器作用下，車輛的實際行駛路徑與參考路徑的對比。可以看出，帶預瞄PID的LQR 控制器對于參考路徑的跟蹤效果最好，而無轉(zhuǎn)角補償?shù)腖QR 控制和帶前饋控制的LQR 控制器均存在較大的穩(wěn)態(tài)誤差，其中無轉(zhuǎn)角補償?shù)腖QR 控制器穩(wěn)態(tài)誤差最大，與參考路徑出現(xiàn)了明顯的偏離，跟蹤效果最差。

圖6 不同轉(zhuǎn)角補償下的路徑跟蹤結(jié)果

圖7（a）和圖7（b）精確描述了車輛與參考路徑的側(cè)向偏差與航向偏差?？梢钥闯?，車輛貼近參考路徑穩(wěn)定行駛后，帶預瞄PID 的LQR 控制器的跟蹤精度最高，最大側(cè)向偏差在0.14 m 以內(nèi)，最大航向偏差在1.35°以內(nèi)；無轉(zhuǎn)角補償?shù)腖QR 控制器跟蹤精度最差，最大側(cè)向偏差達到0.71 m，最大航向偏差在3.6°左右；帶前饋控制的LQR 控制器雖然最大側(cè)向偏差為0.38 m，但最大航向偏差達到9.25°，并且車輛航向角出現(xiàn)了高達16°的擺動幅度，轉(zhuǎn)角在短時間內(nèi)變動過大，車輛行駛的穩(wěn)定性較差。

圖7 不同轉(zhuǎn)角補償下的跟蹤誤差

圖8 描述了3 種控制器的輸出控制轉(zhuǎn)角與車輛實際轉(zhuǎn)角。結(jié)合圖7和圖8可以看出，帶有前饋控制的LQR 控制器雖然減小了一定的穩(wěn)態(tài)誤差，但是由于前饋控制是以車輛質(zhì)心為基準點，控制存在一定的滯后，當存在外界擾動或曲率增加，導致誤差增大時，轉(zhuǎn)向盤轉(zhuǎn)角出現(xiàn)較大幅度的調(diào)整，出現(xiàn)超調(diào)問題。由圖8（a）可以看出，帶有前饋控制的LQR 控制器的最大輸出轉(zhuǎn)角大達到500°左右，并且變動迅速，而車輛的實際轉(zhuǎn)角存在一定的響應滯后，從而導致車輛不能及時調(diào)整，進一步加劇了超調(diào)問題，使得車輛航向角發(fā)生較大變動，出現(xiàn)左右擺動的現(xiàn)象。

由圖8（b）和圖8（c）可以看出，無轉(zhuǎn)角補償?shù)腖QR 控制器和帶預瞄PID 的LQR 控制器輸出的轉(zhuǎn)向盤轉(zhuǎn)角都比較平滑，沒有大幅跳變，車輛的實際轉(zhuǎn)角與之吻合較好，車輛無明顯擺動，穩(wěn)定性較好。

圖8 不同轉(zhuǎn)角補償下的轉(zhuǎn)向盤轉(zhuǎn)角

3.2 權(quán)重參數(shù)模糊調(diào)節(jié)效果試驗

為試驗對LQR 控制器權(quán)重參數(shù)進行模糊調(diào)節(jié)的效果，在帶預瞄PID 的LQR 控制器的基礎(chǔ)上，分別采用固定權(quán)重、模糊調(diào)節(jié)權(quán)重兩種方法，在不同車速下試驗控制器的控制效果。其中，固定權(quán)重和模糊調(diào)節(jié)的基礎(chǔ)權(quán)重參數(shù)均采用車速為3 m/s 時的最優(yōu)控制權(quán)重，試驗車速為1.5 和4.5 m/s。試驗結(jié)果如圖9～圖13所示。

圖9 不同車速下的路徑跟蹤結(jié)果

圖10 1.5 m/s時的跟蹤誤差

圖11 1.5 m/s時的轉(zhuǎn)向盤轉(zhuǎn)角

圖12 4.5 m/s時的跟蹤誤差

圖13 4.5 m/s時的轉(zhuǎn)向盤轉(zhuǎn)角

圖9 顯示了車輛在不同車速、不同控制器作用下，實際行駛路徑和參考路徑的對比。可以看出，在兩種車速條件下，權(quán)重參數(shù)采用模糊調(diào)節(jié)的控制器均具有較好的路徑跟蹤效果，跟蹤精度明顯優(yōu)于同車速下的固定權(quán)重控制器。此外，在同一控制器作用下，車速為4.5 m/s時的跟蹤精度比1.5 m/s時差，尤其是采用固定權(quán)重控制器時，4.5 m/s 車速下車輛出現(xiàn)了明顯的振蕩，轉(zhuǎn)角頻繁變化，嚴重影響到車輛的穩(wěn)定性。這是由于固定權(quán)重控制器沒有隨著車速提高而相應增加對控制量的限制導致的，較高車速下，即使保持低速時相同的轉(zhuǎn)向盤轉(zhuǎn)角，車輛在控制時域內(nèi)產(chǎn)生的位置變動也會更大，從而發(fā)生超調(diào)，產(chǎn)生新的偏差，使得控制器反復調(diào)整，轉(zhuǎn)角頻繁變動，車輛出現(xiàn)左右搖擺現(xiàn)象。而權(quán)重參數(shù)采用模糊調(diào)節(jié)的控制器，其控制量權(quán)重隨著車速增加而增大，雖然跟蹤精度相較于低速時有所降低，但保持了車輛行駛的穩(wěn)定性。

圖10～圖13精確地描述了不同車速下兩種控制器的跟蹤精度與轉(zhuǎn)角變化。

由圖10 可以看出，在1.5 m/s 車速下，權(quán)重參數(shù)采用模糊調(diào)節(jié)的控制器將側(cè)向偏差保持在0.07 m以內(nèi)，航向偏差保持在0.8°以內(nèi)。相較于固定權(quán)重控制器，其較好地減小了側(cè)向偏差，實現(xiàn)了更高的跟蹤精度。圖11 描述了1.5 m/s 車速下，兩種控制器的控制轉(zhuǎn)角與車輛實際轉(zhuǎn)角。可以看出，為保證更高的跟蹤精度，權(quán)重參數(shù)采用模糊調(diào)節(jié)的控制器對于轉(zhuǎn)向盤轉(zhuǎn)角的調(diào)節(jié)更加精細，在低車速下，這種微小的調(diào)節(jié)不會引起車輛左右擺動，不會對車輛的穩(wěn)定性造成影響。

由圖12可以看出，在4.5 m/s車速下，固定權(quán)重控制器的側(cè)向偏差和航向偏差均出現(xiàn)了大幅度的振蕩現(xiàn)象，側(cè)向偏差最大振幅達到0.64 m，航向偏差最大振幅達到19°，且跟蹤誤差的收斂時間較長，車輛長時間未進入穩(wěn)定狀態(tài)，跟蹤精度與跟蹤穩(wěn)定性很差。而權(quán)重參數(shù)采用模糊調(diào)節(jié)的控制器可以快速收斂跟蹤誤差，無大幅波動，最大側(cè)向偏差為0.15 m，最大航向偏差在3°左右，路徑跟蹤的精度與穩(wěn)定性都比較好。

由圖13（a）可以看出，固定權(quán)重控制器為了減小跟蹤誤差，其輸出的轉(zhuǎn)向盤轉(zhuǎn)角過大，而車輛的轉(zhuǎn)向機構(gòu)的響應較為滯后，在車速較高的工況下，這導致車輛與理想位置存在較大偏差，使得控制器對轉(zhuǎn)向盤轉(zhuǎn)角進行反復調(diào)節(jié)，從而引起振蕩，難以收斂。而由圖13（b）可以看出，權(quán)重參數(shù)采用模糊調(diào)節(jié)的控制器由于針對高速調(diào)節(jié)了控制器權(quán)重，其輸出的轉(zhuǎn)向盤轉(zhuǎn)角較小，有效避免了響應滯后與超調(diào)現(xiàn)象，不僅保證了車輛行駛的平穩(wěn)，同時還保證了足夠的跟蹤精度。

4 結(jié)論

本文中設(shè)計了一種智能汽車的路徑跟蹤控制器，采用LQR 控制器作為主體部分，并使用預瞄PID方法進行轉(zhuǎn)角補償，提高跟蹤精度，同時根據(jù)車速對LQR 控制器的權(quán)重參數(shù)進行模糊調(diào)節(jié)，保證控制器在不同車速下的跟蹤效果。實車試驗結(jié)果表明，該路徑跟蹤控制器能夠有效克服由于系統(tǒng)簡化建模和響應滯后引起的誤差問題，實現(xiàn)更加精準的路徑跟蹤控制，且避免了傳統(tǒng)前饋補償?shù)某{(diào)現(xiàn)象；同時，控制器在不同車速工況下，均能保持較好的跟蹤精度與行駛穩(wěn)定性，具有較強的適應性。