基于模型預測控制與離散線性二次型調節器的智能車橫縱解耦跟蹤控制*

段敏 孫小松 張博涵

（遼寧工業大學，錦州 121001）

主題詞：智能車 離散線性二次型調節器 模型預測控制 跟蹤控制 速度規劃

1 前言

跟蹤控制是無人駕駛汽車領域的研究熱點，控制器必須通過同時操縱轉向角度、油門和制動來跟蹤參考路徑和速度。目前，跟蹤問題的解決方法有：無模型反饋控制法，這類方法一般不考慮車輛系統的特性，直接根據系統反饋的跟蹤誤差通過控制律得到前輪轉角，例如比例積分微分（PID）控制法，但是該方法因魯棒性較差需要不斷調整參數；純跟蹤（Pure Pursuit）控制法，該控制方法簡單易行、實時性好，但需在特定工況下才能有效控制車輛，當面對連續變曲率道路工況時，此方法的跟蹤效果較差，難以投入實際工程應用；非線性模型預測控制（Nonlinear Model Predictive Control，NMPC）法，該方法依靠動力學模型設計懲罰函數進行轉向與加速，指引車輛實現路徑與速度跟蹤，此方法雖可在低速工況下保持較高跟蹤精度，但由于其在不斷循環優化求解的過程中會帶來較大計算量，車速較高時存在無法實現對車輛實時控制的風險。Su在軌跡跟蹤控制階段采用了模糊PID 控制，此方式雖然提高了控制的可靠性，但其忽略了對橫擺穩定性的控制。Wang等采用自適應模型預測控制提高軌跡的跟蹤與穩定性，但其無法適應大曲率工況。Chen等采用一種基于最優前輪側偏力的智能汽車線性二次型調節器（Linear Quadratic Regulator，LQR）路徑跟蹤橫向控制方法，但其在跟蹤小曲率的圓弧路徑時仍產生0.2 m 的橫向誤差，這對實際工程應用來說是難以接受的。

因此，本文在Frenet坐標系下建立二自由度動力學模型設計前饋補償離散線性二次型調節器（Discrete Linear Quadratic Regulator，DLQR）橫向控制器，在一階慣性系統下設計模型預測控制（Model Predictive Control，MPC）縱向控制器，并通過模糊速度規劃使控制系統滿足在不同曲率工況下的跟蹤精度與橫向穩定性，并通過仿真驗證其可靠性。

2 自行車模型的建立

2.1 二自由度車輛動力學模型

由于本文對低、高速工況均有涉及，而考慮到輪胎的側偏特性，高速工況下輪胎會產生較大形變，因此運動學模型不再適用，應以動力學模型作為控制模型，如圖1所示（本文坐標系皆采用右手坐標系）。

圖1 二自由度動力學模型

其中，v、v分別為模型質心處的縱、橫向車速，、分別為質心與前、后軸的距離，為質心側偏角，、分別為橫擺角與橫擺角速度，為前輪轉角，F、F分別為前、后輪所受的側向力，、分別為前、后輪側偏角。

假設較小，則cos≈1。對模型進行受力分析：

以該模型建立力矩方程為：

式中，為轉動慣量；為整車質量；a為側向加速度；C、C分別為前、后輪側偏剛度。

根據剛體運動學速度合成與分解可得：

將側偏角方程代入牛頓第二定律公式可得：

2.2 Frenet坐標系下的橫向誤差模型

在Frenet 坐標系中，以道路中心線為參考線，以參考線的切向量與法向量建立坐標系，以車輛在參考軌跡的投影點為坐標原點，坐標軸相互垂直，分為方向與方向。相比于笛卡爾坐標系，Frenet坐標系可與軌跡規劃的坐標系呼應，大幅簡化控制算法，并可起到使縱向控制與橫向控制解耦的作用。

圖2所示為車輛在Frenet坐標系下的映射，其中、分別為投影點處單位法向量和切向量，、分別為匹配點處單位法向量和切向量，為車輛投影位矢，為車輛真實位矢，為車輛匹配位矢，為橫向誤差，、分別為車輛在絕對坐標系和Frenet坐標系下的航向角，、′分別為映射前、后的速度。

圖2 Frenet坐標系

為計算Frenet 坐標系下全局規劃路徑各離散點的信息，本文尋找與車輛距離最短的目標路徑點作為匹配點(,,,)，通過匹配點的信息對車輛在Frenet 坐標系下投影點的信息進行求解。設車輛坐標為(,)，則：

式中，為投影曲率；、為車輛投影位矢的數值坐標；、、、分別為離散軌跡與車距離最近的點在直角坐標系下對應的縱向位置、橫向位置、航向角、曲率。

同理，由幾何關系可得：

為建立與控制方程間的聯系，對求導后由Frenet公式可得：

又因為=+，(-)為小量，化簡可得：

將式（9）代入式（7）后，等式兩邊點乘得：

式中，e為航向誤差。

e的穩態誤差為-，因此，為在控制中使縱向誤差收斂于0，且航向誤差(-)收斂于0，令縱向誤差e=，航向誤差e=-以間接達到控制效果。由此，可構成：

代入二自由度動力學方程可得橫向誤差微分方程：

為將控制問題轉化為性能函數極小值求解問題，將式（12）按順序寫成：

3 跟蹤控制器

跟蹤控制器整體結構如圖3所示。

圖3 控制系統結構

3.1 橫向跟蹤控制器設計

3.1.1 DLQR橫向跟蹤控制設計

為避免違背連續狀態方程式（13）中所滿足的牛頓運動定律，實現DLQR 的反饋控制，忽略式（13）中的道路幾何信息，對等式兩邊積分后由中值定理得：

式中，為采樣周期；+、分別為積分的上、下限，且在此區間內；()為式（13）兩邊積分后由牛頓萊布尼茲公式所得移項。

對()采用中點法，對()采用前向歐拉法，可得離散化后的微分方程：

式中，為單位矩陣。

將式（15）按順序對應簡寫為離散化狀態表達式：

設計性能函數：

式中，、分別為性能函數對應于狀態量與控制量的權重矩陣。

為求二次型的最小值，利用拉格朗日乘子法將其表示為：

式中，=0,1,2,…,-1；λ為各約束的待定系數；為保證誤差的和最小的時段上限，其趨于無窮。

分別對X、u、X、λ求偏導等于0，可得：

設λ=2PX，(=1,2,…,)，且P=，則通過式（19）內等式間的相互代入可得著名的里卡蒂（Riccati）方程的解：

通過迭代求解里卡蒂方程使解收斂后，易得全狀態反饋控制：

3.1.2 前饋控制器設計

為消除穩態誤差，需要通過額外引入前饋控制，實現存在合適的車輪轉角前饋量使盡可能收斂于0的效果，則重新定義全狀態反饋控制與誤差微分方程：

當橫向誤差e=0時，易解式（23）得車輪轉角前饋量：

式中，(3)為反饋矩陣的第3列。

3.2 基于MPC的縱向跟蹤控制器設計

3.2.1 預測模型的建立

假設車輛橫擺運動幅度較小，則有：

以一階慣性環節來近似表示車輛縱向運動特性：

式中，a為縱向加速度；為a的期望值；為時間常數。

通過前向歐拉法構建離散狀態空間，并以速度v作為系統輸出，建立輸出方程：

式中，()=[v a]；()=；、+1、分別為當前采樣時刻、下一周期采樣時刻和采樣周期。

為了對加速度變化率（Jerk）進行約束，構造狀態空間表達式：

由式（28）進行遞推迭代可得系統未來時刻的輸出：

3.2.2 目標函數設計

為在確保速度跟蹤精度的同時避免產生大的加速度變化率，定義性能評價函數為：

式中，、為權重矩陣；為系統輸出參考量；||·||表示向量的歐幾里得范數和矩陣的誘導范數。

通過二次規劃的方法解決此優化問題，將式（30）轉化為標準二次型：

式中，為轉化后的常數項，在優化求解中可忽略不計。

該目標函數約束為：

式中，?表示克羅內克積；、Δ分別為控制時域內控制量、控制增量的最小值；、Δ分別為控制時域內控制量、控制增量的最大值。

經二次規劃（Quadratic Programming，QP）求解器對式（31）、式（32）求解，得到內的控制輸入增量：

根據MPC 的基本原理，將控制序列內首元素Δ()作為實際控制輸入增量作用于系統：

該控制量被計算至下一時刻，并在新時刻根據狀態信息重新預測下一的輸出，通過優化過程獲得新控制增量序列，如此循環至控制過程結束。

3.2.3 油門制動標定表的制作

基于CarSim-Simulink 聯合仿真平臺，以油門踏板開度、制動主缸壓力為自變量得到(v,a,k)、(v,a,P)的三維數據點，并擬合模型油門踏板開度=(v,a)，制動主缸壓力=(v,a)后合并，油門制動標定表如圖4所示。

圖4 油門制動標定

3.3 基于模糊算法的速度規劃

根據駕駛經驗，當前方道路曲率較大時，處于高速工況的車輛會產生較大的離心力，且在車輛轉向輪的轉向角速度約束影響下，被控車輛無法及時轉向，易導致車輛操縱穩定性降低與側向誤差增加，甚至造成跟蹤失敗，嚴重危及道路安全。因此，車輛在跟蹤大曲率目標路徑時若出現側向誤差增加的現象，應適當降低車速以提高跟蹤效能。反之，可適當提高行車速度。

目前，隸屬函數的確定尚未有較為成熟的方法，因此本文通過不斷調試確定輸入信號采用鐘形隸屬函數()，輸出信號采用三角形隸屬函數()時控制效果較為良好：

式中，、、為廣義鐘形隸屬函數的參數，其分別決定了隸屬度為0.5時的橫軸位置、隸屬度變化速度和隸屬函數中心位置；、、為三角形隸屬函數的參數，決定了隸屬度曲線3個端點對應的橫軸位置。

隸屬度函數與模糊規則曲面分別如圖5、圖6所示，其中橫向誤差輸入的模糊語言為S（小）、M（中）、B（大）、VB（非常大），道路曲率輸入的模糊語言為S（小）、M（中）、B（大）、VB（非常大），參考速度輸出的模糊語言為VS（非常小）、S（小）、M（中）、B（大）、VB（非常大）、UB（極其大）。

圖5 隸屬度函數示意

圖6 模糊規則曲面

4 仿真驗證

4.1 仿真參數

本文控制器使用由Fiala 開發的、由Pacejka 提出的刷式輪胎模型，且忽略在大加速度下軸荷轉移引起的前、后輪垂向力分配程度的改變，近似認為輪胎側偏剛度不發生變化。車輛所跟隨的路徑信息（道路的橫、縱向位置）通過PreScan中的采集車進行采集，仿真路面采用交通量較小的混凝土路面，其路面附著系數為0.75，仿真的主要參數如表1所示，其中依靠以往的經驗對泛函、中的權重矩陣選取，權重矩陣的元素取值越大，意味著對應變量的約束要求越高。

表1 主要參數設置

4.2 橫縱聯合控制效果驗證

在變曲率（僅小曲率突變）路徑下對比分析智能車在低速10 m/s、中速16.666 m/s、高速25 m/s 下跟蹤精度，結果如圖7 所示，在小曲率工況下車輛低速條件下的跟蹤誤差在8 mm 左右，中速條件下的跟蹤誤差在10 mm附近，高速條件下跟蹤誤差在50 mm以內。可以看出，本文控制器的跟蹤控制精度非常高。本文仿真時車輛起始位置并不在路徑中心線上，Frenet坐標系以車輛在路徑上的投影為原點，因此在仿真開始時有固定偏移。

圖7 不同車速下的路徑跟蹤結果

圖8～圖10所示分別為不同車速下車輛油門踏板開度與橫向跟蹤誤差變化曲線、路徑曲率、速度跟蹤與縱向加速度變化曲線。可以看出，控制器輸出期望加速度后通過油門制動標定表可以精確地實現對速度的跟蹤，其中圖8中車輛在高速下跟蹤路徑時橫向誤差出現的瞬態波動是由于道路曲率突然變大產生的。此外，可以看出該控制器符合速度越快，橫向誤差越大的客觀規律，當車速較小時橫向誤差可以達到穩態收斂。在10 m/s車速下，橫向誤差隨著路徑曲率的逐漸變大，其誤差變化范圍為0～28 mm，最終趨于穩定。在16.666 m/s 與25 m/s車速下，橫向誤差也同樣遵循此規律，且車速越高，橫向偏差波動越大。此外，圖9 反映了路徑曲率的變化情況，可以看出仿真處于小曲率工況，曲率范圍為0～0.011 m。

圖8 油門踏板開度與橫向誤差

圖9 路徑曲率

圖10 速度跟蹤與加速度

圖11 為不同車速下轉向盤轉角的變化曲線，反映了為保證更高的跟蹤精度，橫向控制器細膩的控制方式使轉向盤轉角無大幅度、長時間的抖動，也從側面反映了該控制方式不會對車輛的穩定性造成嚴重影響。此外，當車速為10 m/s 時轉向盤轉角在轉向時趨于平穩，無明顯抖動，隨著車速的提高，轉向時轉向盤的抖動略有增強。

圖11 轉向盤轉角

圖12所示為不同車速下的橫向穩定性對比，反映了車輛在經過變曲率的路徑時質心側偏角與橫擺角速度隨車速的提高而增加，但始終在可控范圍內，低速時質心側偏角、橫擺角速度在0.01 rad、0.17 rad/s 內循環，高速時質心側偏角、橫擺角速度在0.03 rad、0.32 rad/s 內循環。可以看出本文控制器實現了很好的車輛橫向穩定性。

圖12 不同車速下的橫向穩定性對比

4.3 大曲率工況控制效果驗證

在變曲率（大曲率突變）路徑下對比分析智能車在10 m/s 與模糊速度規劃下的跟蹤效果，結果如圖13、圖14 所示。由圖13、圖14 可知：在具有模糊速度規劃的控制器控制下，車輛跟蹤突變大曲率路徑時的跟蹤誤差穩定在100 mm 以內，橫、縱向跟蹤誤差均在50 mm 左右；無模糊速度規劃的車輛在道路曲率突變增大時跟蹤誤差較大，且橫向誤差在160 mm 左右，縱向誤差在300 mm 左右。此外，圖13 也反映了在保證跟蹤精度的同時，車輛可以更短的時間完成總路徑的跟蹤。

圖13 路徑跟蹤結果

圖14 橫縱向跟蹤效果

圖15所示為有速度規劃條件下的速度跟蹤與縱向加速度結果，每一時刻對速度的控制皆可達到快速、平穩的效果，速度響應曲線全過程無超調，其中圖15a 反映了道路曲率苛刻的變化情況（0～0.08 m）。圖15 體現了控制器可對車輛進行精確的縱向控制與其性能的可靠性和優越性。

圖15 有速度規劃下的速度跟蹤與加速度

圖16 所示為2 種控制方法下橫向誤差與轉向盤轉角的變化曲線，在跟蹤過程中無模糊速度規劃時的路徑跟蹤在通過變曲率（大曲率突變）路徑時橫向誤差突增至超過1 m，而模糊速度規劃下的控制器緩解了這一問題，在轉向盤轉角方面，其轉向弧度更小，約為-4.1 rad，間接反映了具有較好的橫向穩定性。

圖16 橫向誤差與轉向盤轉角

圖17 所示為不同控制方式下的橫向穩定性對比，其中在速度規劃下產生橫擺角速度變動區域更小，在-0.55～0.30 rad/s 范圍內，其在路徑后半段質心側偏角略有增加，但仍可控，車輛仍具有較高的橫向穩定性。

圖17 橫向穩定性對比

5 結束語

本文為智能車設計了一種路徑跟蹤控制器，采用DLQR前反饋補償的方式進行橫向控制，同時在模糊速度規劃下以MPC 方式進行縱向控制，既保證了小曲率工況下不同車速的跟蹤效果，又確保了大曲率工況下仍具有良好的跟蹤性能。仿真驗證結果表明，橫縱跟蹤控制器具有較強的魯棒性，保證了車輛在不同曲率工況下的跟蹤精度與橫向穩定性，滿足實際工程應用要求。