汽車軌跡跟蹤模型預測控制的加速求解方法

孫浩 杜煜 卜德旭 劉浩棟

摘? ?要：針對汽車軌跡跟蹤模型預測控制求解中存在的規模較大、求解效率較低的問題，提出一種基于時域分解的加速計算方法提高求解效率. 首先引入全局一致性變量，將模型預測控制中鄰接控制周期的時域耦合約束轉化為全局一致性約束，實現時域解耦;隨后在交叉方向乘子法框架下推導了時域分解后優化問題的分塊更新方法，并設計了分塊更新數值求解問題的停止準則，從而將大規模優化問題轉化為小規模子問題;最后搭建了Simulink-CarSim平臺進行了算法的仿真驗證. 仿真結果表明，在求解精度不變的情況下，求解耗時下降24.21%，從而實現模型預測控制問題的加速求解.

關鍵詞：模型預測控制;自動駕駛汽車;軌跡跟蹤;交叉方向乘子法;時域分解

中圖分類號：U461.99? ? ? ? ? ? ? ? ? ? ? ? ? ? 文獻標志碼：A

Accelerated Solution Method for Vehicle Trajectory

Tracking Based on Model Predictive Control

SUN Hao1，2，DU Yu1，2?，BU Dexu3，LIU Haodong1，2

（1. Beijing Key Laboratory of Information Service Engineering，Beijing Union University，Beijing 100101，China;

2. College of Robotics，Beijing Union University，Beijing 100027，China;

3. College of Traffic and Transportation Engineering，Central South University，Changsha 410076，China）

Abstract：To solve the problems of large scale and low efficiency in the solution of intelligent vehicle trajectory tracking algorithm based on model predictive control，this paper proposes a time-domain splitting method to accelerate the calculation. First，global consistency variables are introduced to transform the time-domain coupling constraints of the adjacent control cycles into global consistency constraints，thus to achieve time-domain decoupling. Then，under the framework of Alternating Direction Method of Multipliers，the block updating method of optimization problem after time domain splitting is derived，and the stop criterion of block updating numerical solution is designed. Therefore，the large-scale optimization problem is converted into several small-scale sub-problems. Finally，the Simulink-CarSim platform is set up and the algorithm is verified by simulation. The simulation results show that，under the same accuracy of the solution，the efficiency of solution is improved by 24.21% on average with the proposed method，achieving accelerated solution for vehicle trajectory tracking based on model predictive control.

Key words：model predictive control; autonomous vehicles; path tracking; alternating direction method of multipliers; time splitting

自動駕駛汽車是提高交通系統安全、效率、經濟等性能的有效途徑，而軌跡跟蹤是實現智能駕駛的核心技術之一[1]，其目標是在滿足汽車復雜動力學約束、行駛安全性約束前提下，以較小的偏差、較高的舒適性跟蹤所規劃的軌跡[2]. 模型預測控制（Model

Predictive Control，MPC）最早被應用在成本不敏感的大型工程中（如化工產業），并被認為是當年最具影響力的現代控制算法之一[3]. MPC通過引入參考模

型，對約束的處理直接簡單，不僅能夠有效處理被控模型特性、約束帶來影響，還能夠通過滾動求解優化問題找到當前的最優控制量[4].基于上述優點，MPC在汽車軌跡跟蹤問題的研究中得到國內外研究者的廣泛關注.

求解MPC問題的主流思路是將其轉化為二次規劃（Quadratic Programming，QP）問題. 其中，無約束MPC求解相對簡單. Xu等以跟蹤誤差為性能指標實現AGV的軌跡跟蹤控制[5]，Shen等則建立了綜合考慮跟蹤精度和舒適性的目標函數[6]，也有研究將輸入量的邊界建模為目標函數中代價值，構成“軟約束”，但其數學原問題仍是無約束二次規劃. 無約束二次規劃問題可通過梯度下降等方法直接求解，但其局限在于僅考慮車輛的運動及動力學約束，沒有考慮運動過程中所受到的其它約束，局限性較強. 因此，約束MPC更具備實際意義. 如Bo等建立了基于動力學的增量模型，考慮了執行機構的動力學特性約束[7]; Cai等則通過4自由度車輛模型加入車輛運動的側翻約束，提高車輛跟蹤控制的安全性[8]. 約束MPC的數學原問題是約束QP問題（MPC-QP問題），需要通過數值優化方法計算求解. 求解約束二次規劃問題主要采用主動集法、內點法等方法，迭代次數較多. Wang和Boyd的研究表明，對于狀態維數為m、控制輸入維數為n、求解時域為T的MPC問題，狀態轉移形式及控制輸入形式的MPC-QP問題求解的時間復雜度分別為O（[T（n+m）]3）和O（[Tn]3）[9]. 綜上，雖然MPC在軌跡跟蹤問題研究中得到廣泛關注，但MPC-QP問題存在規模大、求解所需的浮點運算多、求解效率低的缺點，制約了MPC的性能. 因此，提高軌跡跟蹤MPC問題的求解效率具有重要的意義.

針對上述問題，本文提出一種加速求解方法以提高求解效率. 該方法通過交叉方向乘子法（Alternating Direction Method of Multipliers，ADMM）實現汽車軌跡跟蹤問題的時域分解，將集中大規模的優化問題轉為分塊小規模優化問題，從而降低汽車軌跡跟蹤MPC問題的時間復雜度，實現求解加速.

1? ?模型預測控制問題的時域分解

1.1? ?問題建模

為了考慮車輛的動力學特性，MPC控制器依賴如圖1所示的軌跡跟蹤模型預測車輛的未來狀態. 其中，系統輸入為前輪轉角δ;系統狀態量x=[y，θ，ω，β]T（在x中，y表示側向偏差，θ表示航向角，ω表示橫擺角速度，β表示質心側偏角）;系統的輸入u=[δ] .

當汽車輪胎側偏特性處于線性范圍時，基于小角度假設可得到跟蹤控制系統的線性連續狀態方程，如式（1）所示.

式（1）中各矩陣如式（2）所示. 其中，k1、k2分別為二自由度模型中車輛前、后輪等效側偏剛度;a、b分別為二自由度模型中車輛直線到前、后軸的距離.

那么，可得線性離散時間系統的狀態空間模型，如式（3）所示.

x（k+1） = Ad x（k） + Bd u（k）? ? ? （3）

且連續與離散系統矩陣轉換關系如式（4）所示，其中Ts為離散系統的采樣時間.

不失一般性，假設當前時刻系統狀態為x0，根據離散系統的狀態方程（3），在預測時域N內系統的輸入與系統狀態之間的轉移關系如圖 2所示，其中x∈Rm，u∈Rn.

設離散時刻t下系統的參考狀態為rt，那么綜合考慮跟蹤精度、舒適性的優化目標函數J可用式（5）表示. 式中Q∈Rm × m、R∈Rn × n分別為狀態變量、輸入變量的權重對角矩陣.

在軌跡跟蹤問題中，代價函數越小越好. 對式（5）進行展開，軌跡跟蹤的MPC問題可轉化為如式（6）所示的約束二次規劃問題. 在目標函數中，省略了不影響極值求解的常數項. 在約束條件中，（6a）表示汽車的動力學約束，（6b）表示不同時刻下狀態變量及控制變量的可行域約束. 可以看出，雖然目標函數J在時域上解耦，但由于約束條件（6a）的存在（即t周期的終止狀態為t+1周期的初始狀態），并不能在求解過程中實現問題的分塊解耦計算.

1.2? ?時域分解

為了實現問題的時域解耦，本文引入全局一致性變量，將汽車動力學帶來的時域耦合約束轉為全局一致性約束，從而實現問題的分塊求解. 圖 3為ADMM時域分解示意圖.

首先，以控制周期為單位對狀態變量、控制變量進行分塊表達. 對于所關注預測時域內的N個控制周期，按照次序給予編號1到N. 對于每個控制周期，該周期初始狀態量、輸入狀態量和輸出狀態量可分別用x（t）

t表達，其中上角標（t）表示為變量所屬的分塊編號，且塊內變量之間的關系由汽車動力學特性決定，如式（7）所示：

而塊間關系由時域耦合約束決定，滿足式（8）：

為解耦式（8）所示的時域耦合約束，引入全局一致性變量[z][～] = （z0，z1，…，zN）T? ，并使x與z之間滿足如式（9）所示的關系，其中：（9a）表示分塊（t）的輸入變量與zt-1的一致性約束;（9b）表示分塊（t）的輸出變量與zt的一致性約束.

因此，聯合式（7）～式（9），問題（6）可重寫為如式（10）所示形式. 此時，鄰接塊之間不存在直接關系，其耦合約束通過一致性變量間接表達，從而實現控制周期之間時域解耦.

2? ?分塊求解方法

2.1? ?問題求解

綜上所述，本文已經構建了MPC問題的時域分解范式，本節的工作則是基于ADMM求解時域分解后的問題. ADMM的標準構型如式（11）所示.

為了方便地將式（10）轉換為ADMM標準構型，引入塊變量：[x][～]t = （x（t）

t-1）T對一個控制周期內所有變量統一表達. 其中，塊變量的下角標表示其所對應塊編號，與塊內變量的上角標（t）相同. 問題（10）可被構建為ADMM標準構型，如式（12）所示.

（12a）為系統的動力學約束;（12b）為系統的可行域約束;（12c）為一致性約束. 式中變量的含義如下所示：

問題（12）的增廣拉格朗日函數如式（14）所示，其中yt為拉格朗日對偶變量，ρ為懲罰因子.

至此，本文已經完成MPC問題的時域分解，并構建了其ADMM的標準形式. 求解ADMM問題過程中，每個迭代周期需要依次更新x、z、y變量. 更新公式如式（15）～式（17）所示.

更新塊變量[x][～]：

可以看出，塊變量及對偶變量均可直接分塊更新，且更新過程相對獨立. 全局一致性變量中的元素更新如式（18）所示，其中G0 = [I，0，0]，G1=[0，0，I]. 全局一致性變量的更新過程僅涉及小規模矩陣的乘加運算，沒有迭代，所以更新速度很快.

2.2? ?停止準則

在數值迭代求解過程中，當原始變量殘差和對偶變量殘差均足夠小時，就可認為求解結果收斂. 本文根據Boyd的經典算法設計停止準則，將問題（12）視為一個整體設計停止準則，設該大問題停止條件所涉及的變量如式（19）所示.

問題的原始殘差向量rk+1及對偶殘差向量sk+1如式（20）所示：

其中，矩陣Ares、Bres的定義如下：

原始殘差邊界及對偶殘差邊界如式（22）所示. 其中：m為系統狀態變量維數，n為控制變量維數，N為預測時域.

3? ?仿真結果與分析

3.1? ?仿真工況與參數

為了驗證所提出方法的有效性，本文搭建了基于Simulink/CarSim聯合仿真平臺，其中Simulink用來構建控制策略，CarSim用來提供動力學模型. 仿真中所使用的動力學模型主要參數數值[10]如表 1所示.

基于相同的動力學模型，開展對原MPC（origin MPC，OMPC）和本文所提出的時域分解MPC（Time Splitting MPC，TSMPC）的仿真對比工作，OMPC問題構型采用控制輸入方式. 算法所涉及的相關參數設置如表 2所示. 為了保證仿真對比的有效性，本文仿真實驗在使用相同的算法參數的同時，也采用相同的二次優化求解器OSQP[11]對OMPC與TSMPC進行求解. OSQP求解器可以返回迭代次數，便于開展結果分析.

3.2? ?仿真結果及分析

本文以典型單移線工況為例開展仿真實驗，證明相比于傳統的OMPC，本文所提出的TSMPC能夠有效降低求解的時間復雜度.

仿真實驗中的前輪轉角計算結果對比如圖4所示. 由于采用了相同的數值求解停止參數，且OMPC與TSMPC約束二次規劃問題是等價的，所以兩算法計算結果，即最優前輪轉角完全相同，從而說明時域分解方法求解結果的正確性.

TSMPC方法的軌跡跟蹤效果如圖5所示，其中實線為跟蹤控制的車輛運動路徑，點劃線表示期望單移線路徑. 由跟蹤效果可以看出，TSMPC可以實現對期望軌跡的有效跟蹤.

具體地，OMPC與TSMPC的跟蹤誤差如圖6所示. 由于仿真工況相同，最優控制輸入相同，所以兩算法的跟蹤控制誤差也相同，且最大誤差約為0.023 m，方法能夠保證較高的跟蹤精度.

TSMPC及OMPC算法仿真過程中車輛其他主要狀態量，包括橫擺角速度ω、航向角θ如圖7所示. TSMPC算法能夠保證軌跡跟蹤過程狀態量處于合理的范圍內.

在相同的求解精度下，OMPC與TSMPC中迭代次數對比如圖8所示. TSMPC的迭代次數相對較高， 這是因為時域耦合約束轉為一致性約束后，相當于給全局一致性變量一個松弛范圍. 因此，相比于OMPC，TSMPC的迭代求解需要在松弛范圍的收斂過程中額外增加一致性變量，進而增加了迭代次數.

最后，在計算平臺相同的情況下（CPU：Intel i5

8 600 k;內存16 G），對比分析傳統OMPC與TSMPC的求解效率，計算結果如圖9所示，TSMPC的總運算量明顯下降. 經計算可得OMPC的平均求解耗時為12.02 ms，TSMPC的平均耗時為9.11 ms，平均耗時下降24.21%.

4? ?結? ?論

針對傳統研究中汽車軌跡跟蹤的MPC求解效率低的問題，本文提出一種基于ADMM的時域分解加速求解方法，包括：1）基于汽車二自由度動力學模型的問題建模，并通過引入全局一致性變量將時域耦合約束轉變為全局一致性約束; 2）推導了時域分

解問題的分塊數值求解方法，建立了分塊數值求解的停止準則，將大規模優化問題轉化為小規模子問題;3）搭建Simulink-Carsim仿真平臺，通過仿真對比證明本文所提出的算法可以降低24.21%的計算耗時.

本文算法中一致性約束的引入增加了數值求解的迭代次數，未來工作可研究時域分解模塊之間信息傳遞拓撲對收斂速度影響的機理，從而減少迭代次數.

參考文獻

[1]? ? 孫浩. 考慮交通車輛運動不確定性的軌跡規劃方法研究[D]. 長春： 吉林大學汽車工程學院，2017：1—2.

SUN H. Research on vehicle trajectory planning considering traffic uncertainties [D]. Changchun：College of Automotive，Jilin University，2017：1—2. （In Chinese）