汽車軌跡跟蹤模型預(yù)測(cè)控制的加速求解方法

孫浩 杜煜 卜德旭 劉浩棟

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

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

中圖分類號(hào)：U461.99? ? ? ? ? ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)志碼：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

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

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

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

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

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

1? ?模型預(yù)測(cè)控制問(wèn)題的時(shí)域分解

1.1? ?問(wèn)題建模

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

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

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

那么，可得線性離散時(shí)間系統(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í)間.

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

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

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

1.2? ?時(shí)域分解

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

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

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

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

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

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

2? ?分塊求解方法

2.1? ?問(wèn)題求解

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

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

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

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

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

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

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

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

2.2? ?停止準(zhǔn)則

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

問(wèn)題的原始?xì)埐钕蛄縭k+1及對(duì)偶?xì)埐钕蛄縮k+1如式（20）所示：

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

原始?xì)埐钸吔缂皩?duì)偶?xì)埐钸吔缛缡剑?2）所示. 其中：m為系統(tǒng)狀態(tài)變量維數(shù)，n為控制變量維數(shù)，N為預(yù)測(cè)時(shí)域.

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

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

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

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

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

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

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

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

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

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

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

最后，在計(jì)算平臺(tái)相同的情況下（CPU：Intel i5

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

4? ?結(jié)? ?論

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

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

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

參考文獻(xiàn)

[1]? ? 孫浩. 考慮交通車輛運(yùn)動(dòng)不確定性的軌跡規(guī)劃方法研究[D]. 長(zhǎng)春： 吉林大學(xué)汽車工程學(xué)院，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）