孫浩 杜煜 卜德旭 劉浩棟

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

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

中圖分類號：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

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

Predictive Control，MPC）最早被應用在成本不敏感的大型工程中（如化工產(chǎn)業(yè)），并被認為是當年最具影響力的現(xiàn)代控制算法之一[3]. MPC通過引入?yún)⒖寄?/p>

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

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

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

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

1.1? ?問題建模

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

當汽車輪胎側(cè)偏特性處于線性范圍時，基于小角度假設(shè)可得到跟蹤控制系統(tǒng)的線性連續(xù)狀態(tài)方程，如式（1）所示.

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

那么，可得線性離散時間系統(tǒng)的狀態(tài)空間模型，如式（3）所示.

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

且連續(xù)與離散系統(tǒng)矩陣轉(zhuǎn)換關(guān)系如式（4）所示，其中Ts為離散系統(tǒng)的采樣時間.

不失一般性，假設(shè)當前時刻系統(tǒng)狀態(tài)為x0，根據(jù)離散系統(tǒng)的狀態(tài)方程（3），在預測時域N內(nèi)系統(tǒng)的輸入與系統(tǒng)狀態(tài)之間的轉(zhuǎn)移關(guān)系如圖 2所示，其中x∈Rm，u∈Rn.

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

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

1.2? ?時域分解

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

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

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

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

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

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

2? ?分塊求解方法

2.1? ?問題求解

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

為了方便地將式（10）轉(zhuǎn)換為ADMM標準構(gòu)型，引入塊變量：[x][～]t = （x（t）

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

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

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

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

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

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

2.2? ?停止準則

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

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

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

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

3? ?仿真結(jié)果與分析

3.1? ?仿真工況與參數(shù)

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

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

3.2? ?仿真結(jié)果及分析

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

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

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

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

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

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

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

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

4? ?結(jié)? ?論

針對傳統(tǒng)研究中汽車軌跡跟蹤的MPC求解效率低的問題，本文提出一種基于ADMM的時域分解加速求解方法，包括：1）基于汽車二自由度動力學模型的問題建模，并通過引入全局一致性變量將時域耦合約束轉(zhuǎn)變?yōu)槿忠恢滦约s束; 2）推導了時域分

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

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

參考文獻

[1]? ? 孫浩. 考慮交通車輛運動不確定性的軌跡規(guī)劃方法研究[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）