自適應預測時域參數MPC車輛軌跡跟蹤控制

吳長水，高紹元

（上海工程技術大學機械與汽車工程學院，上海 201620）

0 引言

軌跡跟蹤是無人車輛實現無人駕駛的重要環節之一，它是指通過根據某種控制理論，為車輛的控制系統設計一個控制輸入作用，使得無人駕駛車輛能夠以期望的速度跟蹤目標軌跡［1］。經典的無人車輛軌跡跟蹤控制算法主要有純跟蹤控制算法、滑?？刂扑惴ê蚉ID算法等。這些算法在處理由車身、輪胎、懸架等組成的汽車非線性系統時存在不足，往往面臨復雜、大量的調參工作［2］。對于多約束系統，MPC算法較其他控制算法具有先天性優勢，能夠很好地與路徑規劃、運動控制處理相結合，已成為無人車輛運動控制領域研究的熱點。

MPC算法具有處理多個約束條件的能力：北京理工大學的孫銀健等［3］為提高控制器的跟蹤效果對側偏角添加軟約束處理。龐輝等［4］通過建立2自由度運動學模型，提出一種基于線性時變的MPC軌跡跟蹤控制器，提升了自主車輛軌跡跟蹤的精確性。近年來興起的神經網絡算法［5］也被廣泛用于改善MPC算法的跟蹤精度。上述研究多專注于通過簡化車輛模型或優化約束條件來提高軌跡跟蹤效果，所提出的方法對控制器仍存在較高的計算性能要求，出于成本的考慮，工業界常用的控制器算力是有限的，因此，為了降低算法的計算量，模型大多需要進行簡化處理，如線性化、小角度假設等［6－7］。

預測模型是MPC算法用來描述整個系統動態行為的基礎，MPC根據預測時域參數調整需要計算出系統狀態量和控制量的次數。車輛的動力學響應是復雜多變的，參數自適應的MPC算法可以較好地適應這種特點：杜榮華等［8］提出了變預測時域MPC方法，通過模糊控制算法計算預測時域變化量實現預測時域的自適應調整；劉志強等［9］以最大橫向偏差作為精度指標，通過綜合評價法得到不同速度下預測時域和控制時域的最優取值。金輝等［10］通過改進粒子群算法優化權重參數和設計前饋神經網絡的路面識別算法識別路面附著系數從而提升自適應權重控制器的跟蹤效果。

基于上述研究，本文中提出一種自適應預測時域參數的MPC算法：以車輛在不同速度工況下軌跡跟蹤過程中的最大橫向偏差、最大橫擺角偏差、仿真計算時間為評價項，通過灰色關聯評價法確定預測時域參數與評價項的關聯度。選定最優預測時域參數通過傅里葉逼近法進行擬合，最終得到可隨車速變化的自適應調節預測時域參數的MPC控制器。通過仿真對比試驗與實車測試，結果表明：所提出的自適應預測時域參數MPC軌跡跟蹤控制器相較于傳統固定預測時域MPC控制器能夠有效減小軌跡跟蹤控制超調量，在跟蹤精度、求解速度和穩定性上都有明顯提升。

1 車輛動力學模型

無人車輛的軌跡跟蹤任務需要考慮車輛行駛過程的動力學特性，在保證跟蹤精度的同時簡化模型的復雜度可在一定程度上減小算法的計算量［11］。本文中所建立的車輛動力學模型如圖1所示。其中：XOY為大地坐標系；xoy為車身坐標系；Fcf、Fcr表示前、后輪的側向力；Flf、Flr分別為前、后輪縱向力；a、b分別為前、后輪到車輛質心的距離；δf為前輪轉角；φ、˙φ分別為車輛橫擺角、橫擺角速度；αf、αr分別為前后輪側偏角；β為質心側偏角；Vf、Vr分別為車身坐標系前后輪的速度；V為車輛質心位置的速度。

圖1 車輛動力學模型

根據牛頓第二定律和魔術輪胎公式，結合車身坐標轉換公式，得到的車輛動力學模型為：

式中：m為整車質量；和分別為車輛在大地坐標系下縱、橫向速度；Iz為車輛繞z軸的轉動慣量；和分別為車輛在車身坐標系中的縱向和橫向速度；Ccf和Ccr分別為前后輪的側偏剛度；Clf和Clr分別為前后輪的縱向剛度；sf和sr分別為前后輪的縱向滑移率。選取系統的狀態量為：ξ＝［，φ，，Y，X］T；控制量為u＝［δf］。

2 自適應預測時域參數MPC控制器設計

2.1 預測模型

根據式（1）的車輛動力學模型建立非線性動力學方程：

式中：f（ξ，u）＝［fy fx fφ f˙φ fY fX］T；Yc為控制輸出量，Yc＝［Y φ］T；C為輸出矩陣，C＝［0 0 1 0 0 0； 0 0 0 0 1］T令當前時刻系統的狀態量和控制量分別為ξ0、u0，得到非線性模型：

式（3）中對任意點（ξ0，u0）進行泰勒展開并忽略高階項，得到：

式（3）與式（4）作差，離散化處理后得到新的狀態方程：

模型預測控制需要計算出預測時域內所有的狀態量和控制量，通常采用增量作為實際控制量［12］，在此構建新的狀態量如式（6）所示：

式中：η（k｜k）＝［ξ（k｜k）u（k－1｜k）］T，其中ξ（k｜k）為車輛在k時刻的狀態量，u（k－1｜k）為車輛在k－1時刻的控制輸入量，k時刻控制增量Δu（k｜k）＝u（k｜k）－u（k－1｜k）；Ck＝［C0］。

定義Np為預測時域參數，Nc為控制時域參數，在整個預測時域內對于預測模型的輸出進行迭代計算，得到未來某時刻k的輸出為：

2.2 目標函數及約束條件設計

在實際的控制對象中，系統的控制量偏差無法通過測試直接獲得［13］。本文中軌跡跟蹤控制器以控制車輛前輪轉角為目標，這里將目標函數的控制量設置為轉角增量，設計目標函數的目的是讓車輛軌跡跟蹤時能在擁有較高精度同時考慮車輛的行駛穩定性［14］，控制器跟蹤的目標函數為：

為便于計算機求解，將式（8）轉化為標準二次規劃形式進行求解：

控制器是通過控制車輛前輪轉角來執行軌跡跟蹤任務，這里對每個計算周期內的轉角范圍和轉角增量進行如下約束：

控制器在每個周期滾動優化求解，得到控制時域內一系列控制增量：

將式（11）中的第一項作為前輪轉角的控制量輸入給被控車輛，重復求解過程，實現軌跡跟蹤的閉環控制。

2.3 自適應預測時域參數模型

2.3.1 控制器架構設計

在基于MPC的軌跡跟蹤任務中，預測時域參數Np的選取對于跟蹤的實時性和精度有較大影響。由于跟蹤過程中車輛的實際行駛速度、道路曲率等復雜因素是時刻變化的，固定預測時域的MPC控制器處理這類變化的能力較弱［15］。如圖2所示，自適應預測時域參數MPC控制器主要由3部分組成：MPC控制器、被控車輛模型、自適應預測時域參數模型。其中MPC控制器根據目標跟蹤軌跡和目標函數求解出最優控制量作用于被控車輛，被控車輛將當前的車輛狀態作為預測模型的輸入，自適應預測時域參數模型根據目標車速求解出當前速度工況下的最優預測時域輸入給MPC控制器，重復上述過程，最終實現變預測時域軌跡跟蹤控制。

圖2 自適應預測時域MPC控制器架構

2.3.2 預測時域與評價參數關聯性分析

MPC預測時域參數往往依靠調試經驗選取。當預測時域參數很大時，控制器可以預測較遠的距離，同時也會造成車輛提前轉向，降低軌跡跟蹤精度；當預測時域參數較小時，在前輪的轉向角度約束的影響下，車輛又會因無法及時轉向而造成軌跡跟蹤失敗。為了探究預測時域參數與被控車輛狀態響應的關系，采用灰色關聯法［16］，選取預測時域參數Np項作為母序列，以最大橫向偏差、最大橫擺角偏差、仿真計算時間作為特征序列（評價項）來分析各評價項對母序列的關聯程度。其中，最大橫向偏差反映跟蹤精度，最大橫擺角偏差反映乘坐舒適性，仿真計算時間反映控制器求解速率。以10 km／h作為速度間隔，在10～120 km／h速度區間內設置不同的預測時域參數對控制器進行離線仿真實驗，總實驗組數為300組。同時，為了保證控制器時域參數的有效性，對預測時域參數進行以下篩選：1）選取的預測時域參數在跟蹤試驗中有較高的跟蹤精度。2）選取的預測時域參數在跟蹤試驗中未發生超調現象。3）選取的預測時域參數在跟蹤實驗中未發生橫擺角震蕩現象?；谝陨虾Y選條件，得到滿足要求的試驗138組。為方便指標數據進行量綱一體化，對值較小的評價項乘以10，分辨系數取0.5，得到的母序列預測時域參數Np與各子序列評價項的關聯系數如圖3所示。

圖3 預測時域參數N p與評價項關聯系數圖

根據關聯系數計算各評價項對Np的灰色關聯度：

式中：rn評價項對預測時域的灰色關聯度；ˉωi為第i個指標變量的權重，這里取等權重1／138；ζNp為預測時域關聯系數。

根據式（12）得到的灰色關聯度排名結果如表1所示。

表1 灰色關聯度排名

結合圖3和表1可以看出：針對3個評價項，最大橫擺角偏差與預測時域Np關聯度最高，且控制器求解時間很大程度上受預測時域參數的影響，其數據值的分布呈現一定的周期性。

以上文的時域灰色關聯度結果為基礎，在實驗數據中以最大橫擺角偏差的最小值為參考項選取出不同車速下的預測時域最優數據組。采用傅里葉逼近法［17］對預測時域參數和速度參數進行擬合，同時為防止過擬合，以仿真計算時間作為權重量，最終得到基于車速變化自適應預測時域參數模型如式（13）所示：

式中：a、b、ω為時域傅里葉擬合系數；x為車輛在大地坐標系下的縱向速度。

3 試驗與分析

3.1 預測時域仿真對比試驗分析

為了直觀看出不同預測時域對軌跡跟蹤控制過程的影響，搭建Matlab／Simulink與CarSim聯合仿真平臺進行仿真試驗分析。在CarSim中選擇軸距為3 050 mm的E級車作為被控車輛模型，車輛基本參數如表2所示，MPC控制器參數如表3所示。以50 km／h和100 km／h為代表車速，設計3組不同的Np值進行仿真測試試驗，參考軌跡選擇雙移線避障軌跡，仿真試驗結果如圖4和圖5所示。

表2 車輛基本參數

表3 控制器參數

圖4 50 km／h軌跡跟蹤控制效果

圖5 100 km／h軌跡跟蹤控制效果

由圖4可知：不同的預測時域對控制器軌跡跟蹤的效果有所差異。預測時域參數為10時，控制器跟蹤雙移線軌跡的橫向偏差最小，但在跟蹤直線軌跡時出現了嚴重的超調量，橫向偏差逐漸增大為6.5 m，軌跡和航向已經完全脫離目標值。當預測時域參數為30時，控制器能夠完成跟蹤任務，但相比于預測時域參數為20時，其橫向偏差較大，跟蹤存在一定的滯后性。

結合圖4和圖5可以看出：隨著目標跟蹤車速的增加，當預測時域參數仍取10時，車輛在參考軌跡大曲率處出現了較大的偏移，這表明在不同速度工況下，固定預測時域參數MPC控制器對復雜路況的跟蹤效果與車速變化具有較強的相關性。

3.2 自適應預測時域控制器仿真試驗分析

式（13）的自適應預測時域參數模型的本質作用是在保證控制器跟蹤精度和穩定性的基礎上通過選取相對最優的預測時域參數來減小算法的計算量，降低控制器SOC的算力要求。本文中所設計加自適應預測時域參數控制器在雙移線軌跡下的預測時域參數與目標車速、計算時間的變化規律如圖6所示。

圖6 預測時域參數變化規律

為了驗證自適應預測時域參數MPC控制器的跟蹤性能，在雙移線避撞軌跡下與定時域MPC控制器仿真結果做對比，選取50、100 km／h為代表車速，經自適應預測時域模型計算出的預測時域參數取整后的值為：當車速為50 km／h時，Np＝14；當車速為100 km／h時，Np＝24；其他參數取值保持不變，仿真結果如圖7和圖8所示。

圖7 50 km／h軌跡跟蹤控制效果

圖8 100 km／h軌跡跟蹤控制效果

從圖7和圖8易看出，不同速度工況下，自適應預測時域參數MPC控制器的軌跡跟蹤效果要優于定預測時域控制器。在目標車速為50 km／h時，定預測時域參數控制器的最大橫擺角偏差為6.66°，實測仿真計算時間為9.17 s，而自適應預測時域參數MPC控制器的最大橫擺角偏差為5.33°，實測仿真計算時間為7.79 s，控制精度提高了19.9%，控制器求解速度提升了15%，自適應預測時域參數控制器的最大橫向偏差、橫擺角震蕩幅度均小于定預測時域控制器。當速度達到100 km／h時，自適應預測時域參數控制器有效地減少了軌跡跟蹤控制的超調量，其最大橫擺角偏差和最大橫向偏差較定時域控制器分別減小了0.5%、26.9%。說明在高速行駛工況下，自適應預測時域控制器的控制效果更好，車輛穩定性更高。

3.3 實車試驗分析

3.3.1 試驗方案

實車試驗平臺采用基于線控底盤車改裝的無人駕駛車輛，如圖9所示。該平臺的車載感知層包括16線激光雷達、組合慣導、攝像頭，執行器部分為線控底盤，控制器部分為基于Arm64架構的NVIDIA AGX orin域控制器和車載顯示器組成。平臺采用Ubuntu 20.04操作系統，通過Autoware.ai開源自動駕駛框架，可實現建圖、定位、感知、規劃和運動控制等一系列的自動駕駛關鍵功能。平臺參數如表4所示。

表4 平臺參數

3.3.2 試驗結果

為驗證所提出的自適應預測時域參數MPC控制器在真實道路環境下的軌跡跟蹤效果，實車試驗使用激光雷達采集道路環境的點云數據，通過NDT配準算法得到車輛在點云地圖中的位姿信息，使用Autoware路徑規劃模塊生成和保存車輛實際行駛的軌跡點［18］。試驗車跟蹤的軌跡如圖10所示。

圖10 試驗車目標跟蹤軌跡

由于中高速工況下的實車驗證過于危險，實車測試試驗設定的跟蹤車速為5 km／h，每次試驗從同一起點出發，測試結果如圖11所示。

圖11 實車測試驗證結果

綜合圖11易看出：實車測試中，自適應預測時域參數控制器的軌跡跟蹤效果要明顯好于定預測時域控制器。在目標車速5 km／h下，對于與預測時域參數關聯度最高的橫擺角偏差，定預測時域參數控制器在軌跡點上的橫擺角偏差均值為9.918°，而自適應預測時域參數控制器的橫擺角偏差均值為8.46°，控制精度提高了14.7%。自適應預測時域參數控制器在軌跡點上的平均橫向偏差為0.307 5 m，最大縱向速度偏差為1.716 2 km／h，而定預測時域控制器的平均橫向偏差為0.392 9m，最大縱向速度偏差為4.023 1 km／h，行駛過程中出現明顯的縱向速度偏離期望值過大現象。這表明自適應預測時域控制器在實際控制過程中，車輛的跟蹤精度和乘坐舒適性均要優于定預測時域控制器控制。

4 結論

提出了一個基于自適應預測時域參數MPC的無人車輛軌跡跟蹤控制器，以跟蹤精度、乘坐舒適性、控制器求解時間為評價項，在傳統固定預測時域MPC的基礎上增加了能夠根據目標車速自適應調整預測時域參數的控制模塊。通過仿真試驗和實車測試，所得結論如下：

1）相較于傳統的定預測時域MPC控制器，自適應預測時域參數MPC控制器能夠在保證車輛行駛穩定的前提下進一步提升軌跡跟蹤的精度，對不同目標速度表現出較強的適應性，在減小各項誤差的同時，提升了求解速度，減少了算法層所占用的計算資源。

2）在實車測試中，自適應預測時域參數MPC控制器有效減少了軌跡跟蹤過程中橫向位置的偏差量，但因為橫縱向控制的強耦合關系導致車輛在跟蹤過程中出現縱向速度偏差過大，將以改善控制器這方面的控制效果作為后續的研究重點。