自適應雙時域參數MPC的智能車輛路徑規劃與跟蹤控制

李耀華，范吉康，劉 洋，何 杰，李澤田，潘紹飛

（1. 長安大學汽車學院，西安710064，中國；2. 上海汽車集團股份有限公司 技術中心，上海 201804，中國）

路徑規劃是指在存在障礙物的行駛環境中，按照一定標準規劃出一條從初始位置到目標位置的無碰撞路徑，可分為全局路徑規劃和局部路徑規劃[1-3]。全局路徑規劃是根據地圖信息規劃出從起點到終點的最優行駛路徑。局部路徑規劃根據全局路徑，同時結合傳感器檢測到的車輛周圍信息，實時規劃出未來一段時間的局部路徑。路徑跟蹤是自動駕駛技術中的重要部分，其主要任務是根據決策規劃層輸出的參考路徑，并結合當前車輛狀態，控制車輛轉向從而跟蹤參考路徑[4-7]。

針對局部路徑規劃問題，文獻[8]對傳統快速擴展隨機樹(rapidly-exploring random trees, RRT)算法進行改進，采用樣條曲線擬合RRT生成的路徑，提高規劃路徑的平滑度。文獻[9]選擇5次多項式進行換道路徑規劃，實現車輛側向加速度的減小，提高換道時的駕駛舒適性。文獻[10]將四次Bézier曲線與蟻群算法結合，規劃出符合自動駕駛車輛行駛曲率的避障局部路徑。文獻[11]采用一種改進勢場模型，以類橢圓形斥力場代替傳統斥力場，同時添加道路邊界斥力場和障礙物速度勢場，保證了規劃路徑的行駛安全性和舒適性。文獻[12]研究了基于凸二次規劃的模型預測控制，將車輛的形狀考慮成由線性約束而非單個點定義的凸多邊形區域，試驗證明能夠改進系統的實時性。

針對路徑跟蹤問題，文獻[13]通過比例-積分-微分(proportion integration differentiation, PID)控制器得到車輛方向盤轉角，采用死區補償法來提高路徑跟蹤過程的穩定性。文獻[14]以跟蹤偏差作為目標函數，結合 線性二次型調節器 (linear quadratic regulator，LQR)控制算法，提出了一種多點預瞄路徑跟蹤控制算法。文獻[15]針對四輪獨立轉向驅動電動汽車路徑跟蹤問題，采用模型預測控制(model predictive control,MPC)算法，設計包含航向角偏差、縱向偏差以及控制量增量的目標函數。文獻[16]對比了最優預瞄控制和MPC 2種路徑跟蹤策略，指出MPC的路徑跟蹤精度更高，對車速和路面附著系數變化具有較好的適應性。MPC可通過目標函數綜合考慮不同的控制目標和約束條件，實現傳統控制難以實現的控制功能，具有較大的柔性。但MPC算法需要求解非線性規劃以獲得最優解，計算量較大，算法實時性差。因此，MPC算法在實際工程應用與芯片處理器水平、軟件算法優化等密切相關。隨著芯片處理能力的提升，模型預測控制算法有望在未來實現實際工程應用。

本文以障礙物場景下的自動駕駛車輛作為研究對象，針對局部避障路徑規劃問題，基于車輛點質量模型，采用非線性MPC算法進行局部路徑規劃，設計包含新型避障功能函數的目標函數，解決傳統避障功能函數過度避障的局限性。針對路徑跟蹤問題，基于二自由度車輛動力學模型，采用線性時變MPC算法進行路徑跟蹤控制，分析預測時域和控制時域對控制性能的影響，建立考慮控制精度和行駛穩定性的路徑跟蹤性能綜合評價指標，得到不同車速下的最優預測時域和控制時域參數，設計了自適應時域參數路徑跟蹤控制器。對局部路徑規劃器與路徑跟蹤控制器集成設計，并通過CarSim軟件與Simulink軟件進行聯合仿真集成驗證。

1 局部避障路徑規劃

基于非線性MPC的局部避障路徑規劃器的基本思想是通過將最優局部路徑的選取原則融入MPC優化問題中進行求解，將所求最優控制序列作用于“假想”車輛，進而獲得“假想”車輛的唯一行駛路徑，該路徑即為滿足路徑選取原則最優局部路徑。

1.1 規劃層車輛預測模型建立

由于非線性MPC算法本身計算量相對較大，為保證實時性，模型不應該過于復雜，選擇車輛點質量模型作為規劃層車輛模型。若令 (x, y) 為車輛在車身坐標系下的位置，(X, Y)為車輛在大地坐標系下的位置，車輛質心為車身坐標系原點，ay為車輛側向加速度，φ為車輛橫擺角；則該模型為：

式（1） 亦可看作一個狀態量為 ，控制量為 的系統，可簡化為

假設當前時刻為k，局部規劃層的采樣時間為Tp，預測時域為Np，控制時域為Nc，并且一般Np＞Nc。采用前向Euler法將式（2）離散化，可得：

其 中：規 定u(k+i) =u(k+Nc- 1),i=Nc,Nc+1,…,Np-1，即預測時域內控制時域外的控制量保持不變。

若定義系統輸出量為 ，則有根據式（3）及狀態量與輸出量之間的關系，可求得系統輸出量。

1.2 避障功能函數設計

避障功能函數來描述局部路徑與障礙物之間的相近程度，根據MPC目標函數的要求可知，當局部路徑與障礙物的距離越近時，避障功能函數值應越大。若令Sob為權重系數，v0為車輛行駛速度，(xob,n,yob,n)為在車身坐標系下第n個障礙物點的位置，(x0,y0)為車輛質心坐標，ζ為一個較小的正數以防止分母為零，N為障礙物點總個數；則避障功能函數為[17]

傳統避障功能函數將障礙物點與車輛質心點之間的距離作為唯一標準來描述車輛與障礙物發生碰撞的可能性。當障礙物與車輛實際不可能發生碰撞時，此時計算得到的Jobs會對模型預測控制產生干擾，可能發生沒有必要的過度避障。障礙物與車輛的位置如圖1所示。

此時障礙物1不存在與車輛發生碰撞的可能性，但傳統避障函數計算得到的Jobs會對目標函數產生干擾，使得規劃路徑產生不必要的避障規劃，從而增大碰撞風險，提高避障難度，影響駕駛舒適性。

針對以上問題，本文定義當障礙物點與車輛之間的距離超過安全距離范圍，則直接令其對應的距離為固定的極大值，從而消除其在目標函數中的影響。車輛與障礙物的關系如圖2所示，圖2中a、b、c為車身尺寸參數。

車輛與障礙物點之間的距離（dmin, n）和車輛與障礙物點之間的距離（dmin）分別為：

式中：W為一個極大常數，取1×107；cs為橫向安全距離閾值，cs與車速的關系為正相關[18]，即：

由式（4）—（6），可得新型避障功能函數為

避障函數為分段函數，在2段函數的切換點處（橫向安全閾值），新型避障功能函數值會發生突變。如果實際橫向距離恰好在閾值附近，會因傳感器誤差等原因造成橫向距離微小波動，使得避障功能在進入與退出狀態頻繁切換，導致駕乘體驗變差。為了避免出現頻繁切換的抖動，本文采用滯環控制以消除因傳感器誤差等原因造成的抖動。

1.3 優化問題構建與求解

系統預測時域內局部避障路徑規劃層的目標是安全避開障礙物，同時保證規劃的路徑與全局參考路徑偏差不要過大。路徑規劃層的目標函數為

式中：U(t)為所求控制量序列，ηref為局部參考路徑，ηref=[φref,Yref]T，Q為輸出量偏差權重矩陣，R為控制量權重矩陣，Jobs,i為第i時刻避障功能函數值。

為了使規劃路徑能保證車輛行駛穩定性，在規劃層目標函數中添加相應約束。因此，局部避障路徑規劃層轉化為求解如式（9）所示的優化問題。

通過求解式（9）得到預測時域內最優控制序列，將最優控制序列帶入式（3），獲得預測時域內的離散最優局部避障路徑點，通過五次多項式曲線進行路徑離散點擬合，則可獲得局部避障規劃路徑。

2 路徑跟蹤控制

路徑跟蹤控制問題本質上是車輛轉向盤轉角控制問題，也稱為車輛橫向控制問題，在路徑規劃層輸入的參考路徑信息的基礎上，根據車輛當前狀態，計算出最優控制量，輸出給執行層。最終通過執行機構的準確執行，完成車輛的路徑跟蹤控制。

2.1 控制層車輛模型建立

為兼顧控制層的控制精度和實時性，選擇二自由度車輛動力學模型作為控制層的車輛模型，即：

式中：Cf和Cr分別為車輛前后輪側偏剛度，m為車輛整備質量，a、b分別為質心到前、后軸的距離，δf為前輪轉角，φ為車輛橫擺角，Iz為車輛繞z軸轉動慣量。

上述模型可簡寫為

對于路徑跟蹤控制層來說，車輛模型更為復雜，且采樣周期更短。為提高控制實時性，將式（10）進行近似線性化，可得線性時變方程為

式中：A(t)、B(t)分別為f對于ξ和u的Jacobi矩陣，ξ0(t)為參考狀態量，u0(t)為參考控制量。

采用前向Euler法對式（12）進行離散化，可得：

式中，Tc為控制層采樣周期。

2.2 優化問題構建與求解

系統路徑跟蹤控制層的目標是保證控制車輛使其實際行駛路徑與規劃層輸入的局部避障路徑的偏差盡量小，同時為保證行駛穩定性盡量減小控制增量。路徑跟蹤控制層的模型預測控制器的目標函數為

式中： ΔU(t)為所求控制增量序列，ε為松弛因子，Q為輸出量偏差權重矩陣，R為控制量權重矩陣，ρ為松弛因子權重因子。

在建立目標函數并添加約束條件后，路徑跟蹤控制即轉化為求解如式（15）所示的優化問題。

式中：Δumin、Δumax為控制增量上下限；umin、umax為控制量上下限；ηmin、ηmax為輸出量上下限；Lp為p維列向量。

將式（14）轉化為二次規劃問題，通過求解獲得控制時域內的控制增量序列ΔU(t)，并將第一個量作為實際控制增量，根據上一控制周期輸入被控系統的控制量確定本次控制周期的控制量。在每個控制周期都重復上述求解過程，獲得實時反饋控制量，從而實現車輛的路徑跟蹤控制。

2.3 最優時域參數選擇

MPC控制器時域參數有預測時域為Np和控制時域為Nc，兩者對控制效果均有較大的影響。

由式（14）可知，在其它控制器參數不變時，預測時域Np越大，控制器能夠預測更遠的位置，獲得更多的車輛狀態信息，但是Np過大會增大考慮距離車輛當前位置較遠處輸出量偏差的控制，減弱距離車輛當前位置較近處輸出量偏差的控制，導致求解目標函數得到的最優控制量對于當前位置較近處并不是最優控制量，導致較近處的跟蹤偏差增大。當控制器進入下一個采樣周期重新求解下一個周期的最優控制量，則會繼續增大下一個較近處的跟蹤偏差，最終通過累加導致整體的跟蹤偏差增大。同時過大的Np也會增加控制器的計算時間，降低控制器實時性。當Np過小時，控制器的實時性雖會提高，但是預測的未來車輛狀態信息過少，在存在控制量與控制增量約束的前提下，前輪無法及時轉向，也會導致跟蹤偏差的增大，甚至導致車輛失控。在其它控制器參數不變時，Nc越大，控制靈敏度和精度越高，但是系統穩定性和實時性也隨之下降。Nc變小，控制器穩定性與實時性更好，但是控制精度下降。例如：當Nc= 1時，在預測時域內，控制器僅做一次控制動作，為保證預測時域內平均偏差最小，求解的控制量會犧牲當前位置附近處輸出量的精度，如圖3所示。

為獲得不同車速下的最優時域參數，基于Carsim/Simulink搭建聯合仿真平臺，選擇雙移線工況作為參考路徑，進行不同行駛車速下的仿真試驗。車輛輪胎型號235/45 R18，整車其他參數見表1，控制器參數見表2。

表1 整車參數

表2 控制器參數

采用最大橫向位置偏差Edmax、平均橫向位置偏差Edm和平均橫擺角偏差Eφm，來衡量實際跟蹤路徑與參考路徑的偏差，分別為：

式中：Ed為車輛與參考路徑的橫向偏差，N為總測量次數，φ為實際橫擺角，φr為參考橫擺角。

路徑跟蹤控制不僅要滿足跟蹤偏差要求，而且要保證車輛路徑跟蹤時的行駛穩定性。通過約束車輛最大質心側偏角和最大橫擺角速度，可防止車輛失穩。車輛最大質心側偏角βmax和最大橫擺角速度ωmax約束條件分別為[19-21]：

式中：μ為路面附著因數、g為重力加速度，取9.8 m/s-2。

以10 km/h的間隔選取30～70 km/h的行駛車速作為仿真工況條件，Np取值范圍為1至40，Nc取值范圍為1至(Np-1)，共遍歷3 900種組合。試驗發現，在部分時域參數組合條件下，出現車輛失控等無效情況。為了保證控制器時域參數的有效性，制定時域參數篩選標準如下：

1） 設置的時域參數能夠保證控制器可運行，保證車輛不發生失控導致路徑跟蹤失敗；

2） 設置的時域參數能夠保證車輛的行駛安全性，保證質心側偏角、橫擺角速度等參數的最大值不超過約束上限；

3） 設置的時域參數能夠保證控制器的實時性，保證求解時間小于采樣周期。根據時域參數篩選策略，剔除無效時域參數后，有效時域參數組合的仿真得到3 900組數據，選擇有代表性的數據， 最大橫向偏差Edmax、平均橫向偏差Edm、平均橫擺角偏差Eφm、最大質心側偏角βmax、ωmax最大橫擺角速度，如表3所示。

表3 雙時域參數仿真結果

仿真結果表明：車輛的控制精度與行駛穩定性存在一定的沖突。為了選擇最優預測時域參數與控制時域參數，平衡控制器對車輛的控制精度與行駛穩定性，綜合考慮控制精度與行駛穩定性，提出一個路徑跟蹤性能綜合評價指標，對不同雙時域參數設置下的不同仿真結果參數進行加權計算，將其作為雙時域參數優劣的綜合評價指標。

式中：Edmax為最大橫向偏差；Edm為平均橫向偏差；Eφm為平均橫擺角偏差；βmax為最大質心側偏角；ωmax為最大橫擺角速度；Q1、Q2、Q3、R1、R2為相應的權重系數。

本文重點研究避障行駛工況，側重于控制精度，尤其是最大橫向偏差參數。根據側重點不同，同時考慮到以上參數的量綱不同，經過多次試驗，確定權重系數值分別為200、400、40、20和1。

在建立評價函數后，將雙時域參數試驗結果代入式（20）進行得分計算，選擇最低綜合評價指標Sc值作為對應車速下的最優預測時域參數Np與控制時域參數Nc，結果如表4所示。

表4 不同車速下的最優雙時域參數

在獲得不同車速下的最優雙時域參數后，制定自適應雙時域參數控制策略，如式（22）所示。

2.4 控制器性能驗證

為驗證最優雙時域參數選擇方法，將車速分別設置為25、35、45、55、65 km/h，對固定雙時域參數控制器與自適應雙時域參數控制器進行雙移線工況仿真試驗，其中固定雙時域參數設置為Np= 25，Nc= 1。不同車速下，固定雙時域和自適應雙時域參數控制器的控制性能如表5所示。部分車速下的仿真結果如圖4—圖6所示。

表5 不同MPC控制器的控制性能

仿真結果表明：在不同車速下，自適應時域參數控制器的前輪轉角、前輪轉角增量、最大質心側偏角、最大橫擺角速度均滿足約束條件。與固定時域參數控制器相比，自適應時域參數控制器均具有更小的最大橫向偏差、平均橫向偏差和平均橫擺角偏差及綜合評價指標Sc，且隨著車速增加兩者的差異性越明顯。當車速達到65 km/h時，固定時域參數控制器的橫向偏差為0.368 m，自適應時域參數控制器的最大橫向偏差僅為0.199 m，相差0.169 m，改善效果顯著。此時，固定時域參數控制器最大橫擺角速度為20.591 (°)/s，已經接近理論上限值21 (°)/s，車輛行駛穩定性變差，而自適應時域參數控制器最大橫擺角速度為17.395 (°)/s，行駛穩定性得到明顯改善，說明自適應時域參數控制器可用于提高極限工況的行駛安全性。

3 局部避障路徑規劃與路徑跟蹤集成驗證

將局部路徑規劃MPC控制器規劃層與路徑跟蹤層MPC控制器集成，形成規劃層與控制層集成控制器，如圖7所示，其中路徑規劃層的輸出規劃路徑即為路徑跟蹤層輸入-需要跟蹤的參考軌跡。

對上述集成控制器進行仿真驗證，其中路徑規劃MPC控制器參數設置如表6所示。

表6 規劃器參數

設置仿真工況如下：總長100 m、寬度3.8 m的直線道路右側存在一長50 m、寬2 m的靜態障礙物，車輛沿右車道以60 km/h的車速直線行駛，全局參考路徑為右車道中心線，以驗證新型避障功能函數消除過度避障的性能。此時，傳統和新型避障函數下的避障功能函數及規劃路徑仿真結果分別如圖8—圖9所示。圖9中，紅色矩形表示靜態障礙物。

仿真結果表明：由于傳統避障功能函數的設置，使得車輛行駛至障礙物附近時避障功能函數不為零，從而導致局部路徑規劃器規劃出向內的避障路徑，帶來輕微的橫向偏差，最大橫向偏差達到0.276 m，存在過度避障，使得行駛穩定性和駕駛舒適性下降。如果此時左側車道出現車輛，則增大碰撞的可能性。而新型避障功能函數始終為零，使得局部路徑規劃器在車輛行駛至障礙物附近時規劃出的局部路徑和全局參考路徑重合，車輛沒有產生避障動作，表明采用新型避障功能函數的局部路徑規劃器能夠避免過度避障。

相較于傳統避障功能函數，新型避障功能函數需增加對障礙物點類型判斷的過程，下文對采用STM32H743單片機硬件平臺對兩者的實時性進行驗證。設置實時性驗證工況如圖12所示。在車載傳感器可檢測范圍內存在3個尺寸為4 m × 2 m物，其中心位置處的車身坐標分別為(6，1)，(11，-2)和(2，-4)，每個障礙物采用60個障礙物點來模擬車載傳感器獲取的障礙物邊緣位置信息。情況1假設存在障礙物1，情況2假設存在障礙物1和2，情況3為假設存在障礙物1、2和3，對應的總障礙物點個數N分別為60、120和180。

分別計算3情況下2種避障功能函數的計算時間。不同避障情況下，計算100次避障功能函數的實時性驗證結果如表7所示。

表7 實時性驗證結果

由表7可知，在計算100次的情況下，新型避障功能函數的計算時間略高于傳統避障功能函數，當障礙物點總個數為180時僅多0.294 ms，但由于規劃層采樣周期為100 ms，且采樣周期內的避障功能函數計算為Np次，遠小于100次。因此，新型避障功能函數可在保證實時性的同時，避免過度避障，提高車輛行駛穩定性。

為了驗證局部路徑規劃與控制集成控制器的性能，本文進行多障礙物避障工況仿真驗證。仿真工況設置如下：總長150 m、寬度3.8 m的直線道路存在4個尺寸為5 m × 2 m的靜態障礙物，其位置坐標分別為(35,0)，(70, 3.8)，(105, 0)和(140, 3.8)。車輛以30 km/h的速度勻速行駛。車輛初始位置為大地坐標系原點。

避障工況示意圖如圖10所示。規劃層規劃路徑和跟蹤層實際路徑如圖11所示。車輛質心側偏角和橫擺角速度如圖12和圖13所示。實際路徑與規劃路徑的最大橫向偏差、平均橫向偏差、平均橫擺角偏差、最大質心側偏角和最大橫擺角速度如表8所示。

表8 避障工況仿真結果

由仿真結果可知，車輛在實際行駛過程中能夠安全避開所有障礙物，車輛行駛穩定。車輛實際路徑與參考路徑的橫向偏差最大僅為0.293 m，平均橫向偏差與平均橫擺角偏差均較小。由于初始速度、初始位置與航向設置為零，導致車輛起始質心側偏角、橫擺角速度有較大波動，但在合理范圍內。仿真結果表明：局部路徑規劃與控制器集成控制器能夠使車輛在多障礙物工況下規劃出安全參考路徑，并實現精準跟蹤，實現車輛避障功能。

為了驗證局部路徑規劃與控制集成控制器在動態障礙物工況下的性能，本文進行動態避障工況仿真驗證。仿真工況設置如下：總長100 m、寬度3.8 m的直線道路存在尺寸為10 m×2 m的動態障礙物，以30 km/h的速度在右側車道勻速行駛，以模擬行駛中的大型商用車，其初始位置中心坐標為(22.5，0)。車輛以60 km/h的速度勻速行駛，初始位置為大地坐標系原點，其示意圖如圖14所示。部分時間點時，規劃層規劃路徑、跟蹤層實際路徑及動態障礙物實時位置如圖15所示。

由仿真結果可知：車輛能夠安全避開動態障礙物，車輛實際路徑與參考路徑橫向偏差最大僅為8.8 cm。仿真結果表明：局部路徑規劃與控制集成控制器能夠使車輛在動態障礙物工況下規劃出安全參考路徑，并實現精準跟蹤，實現車輛對動態障礙物的避障功能。

4 結 論

根據車輛與障礙物的位置關系設計新型避障功能函數，并基于非線性模型預測控制算法設計局部路徑規劃器，實現障礙物場景下局部路徑的實時規劃。理論分析并結合仿真試驗獲得不同車速下的最優時域參數，設計自適應時域參數路徑跟蹤控制器，實現車輛對參考路徑的精確跟蹤。搭建聯合仿真平臺，對規劃層和控制層的集成結構進行仿真驗證。

仿真結果表明：新型避障功能函數可在保證實時性的同時，避免過度避障，提高車輛行駛穩定性。自適應時域參數路徑跟蹤控制器的綜合控制性能更佳，可用于提高極限工況的行駛安全性。局部避障路徑規劃與路徑跟蹤控制集成結構能夠控制車輛在多靜態障礙物場景與動態障礙物場景下實時規劃出局部避障路徑并精確跟蹤，實現車輛的安全穩定行駛。