多參數優化MPC的自動駕駛軌跡跟蹤控制

李學慧，蘇 振，張俊友

（山東科技大學交通學院，山東 青島 266590）

0 引言

軌跡跟蹤控制是實現車輛自動駕駛的關鍵技術之一［1］，其原理是將車輛當前的實際位姿與目標位姿進行比較，并轉換到車身坐標系，得到橫向誤差，通過最小化橫向誤差計算得到前輪轉角，并作用于執行機構，使車輛沿著目標路徑行駛［2］。

在軌跡跟蹤控制中使用較多的控制算法有：PID控制、模型預測控制、預瞄控制和線性二次型調節器等［3］。模型預測控制能夠結合被控系統的動態模型與系統當前的狀態，預測系統未來的輸出，通過求解含約束條件的最優控制問題，獲得系統的最優控制輸入，并且具有處理多約束優化問題的能力［4］，更適用于復雜多變的自動駕駛應用場景。車輛在高速、低附著路面和大曲率路徑工況下，傳統MPC算法處理能力有限，會出現跟蹤誤差大、穩定性差等現象。近年來，通過改進MPC算法提高自動駕駛軌跡跟蹤精度和穩定性成為國內外研究熱點問題［5］。

改進MPC算法主要有參數優化和與其他算法相結合2種方式。通過參數優化改進的MPC算法可以不斷調整預測時域、控制時域和調整權重矩陣，從而獲得相對最優解，更好地作用于被控系統。李培慶等［6］使用權重矩陣自適應的MPC控制器，目標函數的權重矩陣根據參考路徑的曲率變化進行優化調節，提高了軌跡跟蹤精度和穩定性，但是沒有針對不同速度工況進行優化。李耀華等［7］建立了一種智能車軌跡跟蹤性能綜合評價指標，制定了不同車速下的預測時域和控制時域參數調節策略，獲取最優預測時域與控制時域，結合規劃層和控制層解決過度避障問題，減小跟蹤誤差。王銀等［8］使用自適應軌跡跟蹤控制策略，預測時域根據車速變化進行自適應優化調節，提高了低附著系數路面工況下車輛變速行駛時的軌跡跟蹤精度和穩定性。預測時域的產生是通過三次多項式擬合4組數據得到，由于數據較少，不能得到最優預測時域。Lin等［9］根據橫向速度和縱向速度變化，通過模糊控制調節預測時域和控制時域參數，并利用余弦相似度觸發狀態矩陣更新機制，驗證了該策略在變速工況下具有良好的適應性和跟蹤精度。Wang等［10］使用Mamdani模糊自適應算法對MPC控制器權重矩陣進行優化調節，與傳統MPC算法相比，在跟蹤精度和轉向平順性方面具有更好的跟蹤性能。李韶華等［11］根據橫向誤差和橫擺角誤差使用Takagi-Sugeno模糊變權重MPC控制算法，在線優化MPC權重矩陣，測試了該算法在不同速度下的跟蹤效果，但是沒有根據不同速度對時域進行優化。

在不同速度工況下，僅調節模型預測控制器的權重矩陣，能夠提高軌跡跟蹤精度，但受時域的限制，優化幅度有限，如果在此基礎上同時調節時域，會進一步提高跟蹤精度。根據車輛行駛速度、橫向位置誤差和橫擺角誤差作為模糊輸入，利用模糊控制對MPC控制器的預測時域、控制時域和權重矩陣等多個參數進行實時優化，通過權重矩陣優化提高跟蹤精度，同時進行時域優化，進一步提高車輛在不同速度工況下的適應性。

1 無人車輛系統建模

汽車是一個復雜的非線性多約束系統，在研究車輛運動控制相關的問題時，為了降低系統的復雜程度，提高算法的運算效率，在滿足車輛自身的結構約束和控制算法需求的前提下，一般會忽略部分非研究重點的因素［12］，本文中假設：忽略車輛的垂向運動，輪胎受力的橫縱向耦合關系，載荷的左右轉移以及橫縱向空氣動力學效應；質心側偏角和輪胎滑移率較??；車輛各個輪胎受力均處于線性區域?；谏鲜黾僭O，將車輛簡化為圖1所示的3自由度動力學模型。

圖1 車輛3自由度模型示意圖

圖1中，a和b分別為車輛質心與前軸和后軸的距離；v表示車輛質心速度；φ表示車輛橫擺角；˙φ表示車輛橫擺角速度；δf表示車輛的前輪轉角；β表示質心側偏角；x和y為車身坐標系；X和Y表示大地坐標系；αf和αr分別為前后輪胎側偏角；Fyf表示前輪胎側向力的合力；Fyr表示后輪胎側向力的合力；Fxf表示前輪胎縱向力的合力；Fxr表示后輪胎縱向力的合力。

簡化后的車輛動力學方程如下：

式中：m表示車輛質量；Iz表示車輛的轉動慣量；Clf和Clr分別表示車輛前后輪胎的縱向剛度；Ccf和Ccr分別表示車輛前后輪的側偏剛度；sf和sr分別表示前后輪滑移率。

由車身坐標系和大地坐標系轉換得到：

此時可以將被控系統描述為如下的狀態空間表達式：

2 軌跡跟蹤控制器的設計

模型預測控制屬于過程控制類策略，在每一個控制周期內，對有限時域內開環控制問題進行優化求解，并不斷地滾動優化實現反饋控制。將車輛非線性動力學模型線性化，然后離散化，并加入約束條件，轉化為二次規劃問題求解。

2.1 非線性系統線性化與離散化

對系統的狀態方程＝f（ξ，u）在參考值ξ＝ξ0，u＝u0處利用泰勒展開式，對車輛非線性動力學模型線性化處理后，得到線性時變方程：

再利用向前歐拉法進行離散化后可得：

式中：Ak，t＝I＋TAt，0，Bk，t＝TBt，0，Dk，t＝TDt，0，I為單位矩陣，T為采樣周期。

令u（k）＝u（k－1）＋Δu（k），Δu（k）為控制增量，定義新的狀態變量ξ～（k）＝［ξ（k），u（k－1）］T，將式（5）轉化為以控制增量為輸入的狀態方程并生成新的狀態空間表達式：

自動駕駛軌跡跟蹤控制的目標是盡量減小車輛實際行駛軌跡與參考軌跡的誤差，使車輛平穩行駛。所以目標函數既要能表征跟蹤精度，又要能表征跟蹤穩定性［13］。采用控制增量作為狀態量，并加入松弛因子ε，目標函數如式（7）所示。

式中：η（·｜k）表示k時刻之后一段時間內系統的輸出；ηref（·｜k）是η（·｜k）的參考值；Δu為前輪轉角增量；ρ為權重系數；ε為松弛因子；Np為預測時域，Nc為控制時域；Q和R為權重矩陣。表達式如下：

在目標函數中還需計算控制系統未來時刻的輸出，系統的未來時刻輸出形式如下：

2.2 轉化為二次規劃問題求解

將預測模型和目標函數相結合，構建軌跡跟蹤二次規劃問題并設置約束條件，利用Matlab中的求解器quadprog進行求解［14］。

式中：x表示返回值向量；H表示二次目標矩陣，是實對稱矩陣；f表示線性目標向量；E＝Ψkξ（k）＋ΓkΦ－Yref（k），Yref（k）是Y（k）的參考值。對式（13）進行求解，獲得控制輸入增量序列Δ＝將該序列中第一個元素取出，當前時刻控制量與該元素相加后作用于被控系統，即u（k＋1）＝u（k）＋Δ。

在設計模型預測控制器時，需要對輸出量、控制量和控制量增量進行約束。輸出量根據車輛參考軌跡設置；對于控制量和控制量增量即車輛前輪轉角和前輪轉角增量約束可設置為：

2.3 優化方案

將車輛行駛速度、橫向位置誤差和橫擺角誤差作為模糊輸入，利用模糊控制對MPC控制器多個參數進行優化，在速度不變時通過調節權重矩陣提高軌跡跟蹤精度，在不同速度工況下通過調節預測時域和控制時域對控制器進一步優化，實現MPC控制器多參數調節，軌跡跟蹤控制器架構如圖2所示。

圖2 軌跡跟蹤控制器架構

2.3.1 權重矩陣優化

在同一速度工況下采用橫向誤差ΔY和橫擺角誤差Δφ作為Mamdani模糊控制［10］輸入，輸出權重矩陣調節系數rφ、rY和rR，通過式（15）計算改變權重矩陣Q和R，式（15）中設置初始值為：Qφ0＝200 000，QY0＝90 000，Rδ0＝500 000。Mamdani模糊控制輸入和輸出變量的隸屬度如圖3和圖4所示，Δφ的論域設置為［－15，15］，ΔY的論域設置為［－6，6］，rφ、rY和rR的論域設置為［0，1］。

圖3 輸入變量隸屬度

圖4 輸出變量隸屬度

模糊規則設置如表1—表3所示，模糊輸入輸出三維圖如圖5所示。

表1 rφ模糊規則

表2 rY模糊規則

表3 rR模糊規則

圖5 Mamdani模糊輸入輸出三維圖

2.3.2 時域優化

MPC控制器中的預測時域與控制時域對軌跡跟蹤效果的影響如圖6所示。若目標函數中其他參數不變，當預測時域Np過大時，控制器可以預測更遠的距離，會考慮到更遠位置處的偏差，從而削弱對車輛當前位置偏差的控制，導致過早轉向，出現距離車輛當前位置較近處的跟蹤誤差較大的現象；當預測時域Np過小時，預測的未來車輛狀態信息過少，車輛無法及時轉向，容易失控。當控制時域Nc增大時，有利于控制靈敏度和精度，但會降低系統穩定性和實時性；當控制時域Nc減小時，情況則相反。

圖6 預測時域和控制時域對軌跡跟蹤的影響

為了使車輛在不同速度工況下控制器具有最優時域參數，設置預測時域初始值Np0，控制時域初始值Nc0，通過Takagi－Sugeno模糊控制獲得系數rNp和rNc，使用式（16）計算Np和Nc。因為系數rNp和rNc的論域設置為［0，1］，Np和Nc的值不超過Np0和Nc0，所以Np0和Nc0的設置不能太小，結合相關文獻，并反復仿真實驗后選取Np0＝33、Nc0＝16。

Takagi-Sugeno模糊控制的輸入為速度v，輸出為系數rNp和rNc，速度v論域設置為［0，12］，速度v論域與真實車速v*的數量關系為v＝v*／10，v*單位為km／h，速度v隸屬度如圖7所示。T-S模糊規則如表4所示，時域調節模糊規則二維關系如圖8所示。

表4 時域調節模糊規則

圖7 速度隸屬度

圖8 Takagi-Sugeno模糊輸出二維圖

3 仿真驗證

在自動駕駛車輛行駛過程中，需要將可行性、安全性和穩定性作為優化目標?？尚行员憩F為車輛前輪轉角控制在規定范圍，安全性表現為車輛行駛過程中軌跡跟蹤精度，穩定性表現為車輛行駛過程中橫擺角誤差的變化。為了檢驗改進的MPC控制器的跟蹤效果，使用Matlab和Carsim進行軌跡跟蹤控制聯合仿真，在Matlab中搭建MPC控制器，輸出控制量作用于Carsim，Carsim將車輛實時狀態變量輸出作用于Matlab。

3.1 工況選擇

參考軌跡選擇雙移線方程，路徑設置如下：

式中：Yref為橫向參考坐標；φref為參考橫擺角；Xref為參考縱向坐標；dx1、dx2、dy1、dy2均為軌跡參數。

3.2 仿真參數設置

MPC傳統算法使用如表5所示的5個固定參數，MPC多參數調節算法對這5個參數進行實時優化，MPC僅時域調節算法只優化Np和Nc，MPC僅權重調節算法只優化Qφ、QY和Rδ。Carsim中均使用B-Class車型，車輛參數如表6所示。

表5 模型預測控制器參數

表6 車輛主要參數

3.3 仿真結果評價

在高附著系數μ＝0.8的路面上進行36、54、72 km／h三種典型車速仿真實驗；在低附著系數μ＝0.4的路面進行54 km／h車速仿真實驗。將使用改進MPC控制器的車輛與傳統MPC控制器的車輛進行仿真對比。

3.3.1 高附著路面仿真分析

1）橫向誤差對比。不同車速下軌跡如圖9所示，在X＝53 m和85 m附近大曲率路徑處，采用MPC多參數優化算法的控制器具有更高的跟蹤精度，采用其余3種算法的控制器都由于過早轉向導致跟蹤誤差較大。為了更清晰地觀察橫向軌跡誤差大小的變化，設置ΔY＝Yref－Y，如圖10所示，可以直觀地顯示橫向誤差的變化。

圖9 不同車速下軌跡曲線

圖10 橫向誤差變化曲線

在測試的速度工況下，隨著速度提高，采用MPC多參數優化算法的控制器在減小橫向誤差方面優于采用MPC僅時域調節和僅權重調節的控制器，特別是在72 km／h工況下，MPC多參數優化算法優勢更加明顯。

對橫向誤差仿真結果進行數據統計，統計結果如表7所示，數據表明：在減小橫向誤差方面，不同速度工況下，采用MPC多參數優化算法的控制器均表現最優，平均優化比例可達27.4%。

表7 最大橫向誤差

2）橫擺角誤差對比。橫擺角誤差可以反映車輛行駛的穩定性，為了清晰地觀察橫擺角誤差大小及誤差變化，設置Δφ＝φref－φ，橫擺角誤差變化如圖11所示。在低速36 km／h時，4種控制器的橫擺角誤差變化區別不明顯；在54 km／h時，采用MPC多參數優化算法控制器的車輛行駛過程中橫擺角誤差較??；在72 km／h時，4種MPC控制器的車輛橫擺角誤差曲線都出現明顯波動，采用MPC多參數優化算法控制器的車輛行駛過程中橫擺角波動幅度較小。

圖11 橫擺角誤差變化

對橫擺角誤差仿真結果進行數據統計，統計結果如表8所示，在減小最大橫擺角誤差方面，3種改進算法都比傳統算法有優勢，采用MPC多參數優化算法的控制器在減小最大橫擺角誤差方面表現最優，車輛行駛穩定性最好，特別是72 km／h的時候，最大橫擺角誤差優化明顯，誤差在1°以內。

表8 最大橫擺角誤差

3）歐氏距離對比。為了能夠評價軌跡跟蹤全過程的整體誤差，基于歐式距離的相似度算法［15］對采用不同MPC控制器的車輛實際行駛軌跡與參考軌跡進行對比。歐式距離模型如圖12所示，計算Ni和Mi兩點間的歐氏距離：

圖12 歐式距離模型

實際軌跡與參考軌跡的歐式距離可表示為：

歐氏距離綜合考慮軌跡的整體誤差，用于評價跟蹤效果更準確，車輛在雙移線路徑行駛時，跟蹤誤差主要分布在縱向位移0～100 m處，在此范圍內均勻取點100個，即式（19）中n＝100。對歐氏距離計算結果進行數據統計，統計結果如表9所示，數據表明：采用MPC多參數優化算法控制器的歐氏距離明顯低于其余3種控制器，平均優化比例可達16.5%。

表9 歐氏距離

3.3.2 低附著路面仿真分析

為了驗證多參數優化MPC控制器在低附著路面工況下的優勢，選取附著系數μ＝0.4，車速為54 km／h進行仿真實驗。低附著路面仿真結果如圖13所示，將仿真結果進行數據統計，統計結果如表10—表12所示。仿真結果表明：與其余3種控制算法相比，采用MPC多參數優化算法控制器的車輛行駛軌跡更接近參考軌跡，且橫擺角誤差波動較小，在低附著路面工況下具有良好的操縱穩定性；采用僅時域優化算法的MPC控制器由于過度減小最大橫向誤差，導致軌跡在X＝85 m附近出現明顯波動，且橫擺角誤差在X＝90 m附近出現明顯波動，操縱穩定性較差。采用MPC多參數優化算法控制器的車輛行駛軌跡具有更小的歐氏距離，軌跡跟蹤整體誤差減小。

表10 最大橫向誤差

表11 最大橫擺角誤差

表12 歐氏距離

圖13 低附著路面仿真結果

4 結論

1）針對自動駕駛車輛橫向控制在大曲率路徑處跟蹤誤差較大的問題，提出了多參數優化MPC的自動駕駛軌跡跟蹤控制策略，進行了仿真驗證。

2）采用MPC多參數優化算法的控制器在高附著路面，能夠更好地減小最大橫向誤差，提高跟蹤精度，保持良好的操縱穩定性，在車速72 km／h雙移線工況下將最大橫擺角誤差控制在1°以內。

3）采用MPC多參數優化算法的控制器在低附著路面，能夠更好地平衡跟蹤精度與操縱穩定性，避免因提高跟蹤精度而降低操縱穩定性，在車速為54 km／h工況下，比MPC傳統算法的最大橫擺角誤差減小27.3%。