孫浩 陳澤宇 吳思凡 潘峰

（1.北京聯(lián)合大學(xué)，北京市信息服務(wù)工程重點(diǎn)實(shí)驗(yàn)室，北京 100101；2.北京聯(lián)合大學(xué)，機(jī)器人學(xué)院，北京 100027；3.清華大學(xué)，汽車安全與節(jié)能國家重點(diǎn)實(shí)驗(yàn)室，北京 100084）

主題詞：智能車輛 運(yùn)動(dòng)控制 策略梯度 模型預(yù)測控制

1 前言

智能車輛運(yùn)動(dòng)控制的目標(biāo)是實(shí)現(xiàn)對(duì)期望軌跡的準(zhǔn)確跟蹤，是智能駕駛的基礎(chǔ)核心技術(shù)之一[1]。模型預(yù)測控制（Model Predictive Control，MPC）是一種最優(yōu)控制方法，其通過參考模型的引入直接處理被控對(duì)象的動(dòng)力學(xué)特性，且其基于模型的預(yù)測功能使算法擁有對(duì)環(huán)境的前饋能力，從而取得更好的控制效果，因此受到研究者的廣泛關(guān)注[2-5]。然而，MPC通過對(duì)優(yōu)化問題的在線求解計(jì)算當(dāng)前最優(yōu)控制量，求解效率較低。如Wang和Boyd的研究表明，對(duì)于線性時(shí)不變系統(tǒng)的MPC問題，當(dāng)其狀態(tài)維數(shù)為m，控制輸入維數(shù)為n，控制時(shí)域?yàn)門時(shí)，狀態(tài)轉(zhuǎn)移形式及控制輸入形式的二次凸優(yōu)化問題在線求解的時(shí)間復(fù)雜度分別為O([T(n+m)]3)和O([Tn]3)[6]，其求解復(fù)雜度呈三次指數(shù)增長，在線求解效率較低。交叉方向乘子法可以實(shí)現(xiàn)MPC問題的時(shí)域分解，降低問題的維數(shù)，從而提高在線求解效率[7-8]，但時(shí)域分解要求系統(tǒng)必須為線性。車輛運(yùn)動(dòng)為平面剛體運(yùn)動(dòng)，其運(yùn)動(dòng)控制問題為非線性控制問題，對(duì)其線性近似將導(dǎo)致控制精度的下降。

近年來，隨著神經(jīng)網(wǎng)絡(luò)的發(fā)展，其強(qiáng)大的表示及近似能力推動(dòng)了強(qiáng)化學(xué)習(xí)由表格型求解向函數(shù)近似型求解發(fā)展。強(qiáng)化學(xué)習(xí)的應(yīng)用領(lǐng)域也從有限的離散空間拓展到無限的連續(xù)空間，從而適用于解決復(fù)雜非線性系統(tǒng)的控制問題，典型的代表為基于策略梯度（Policy Gradient，PG）的深度確定性策略梯度（Deep Deterministic Policy Gradient，DDPG）方法[9]。該類方法屬于無模型強(qiáng)化學(xué)習(xí)方法，通過與環(huán)境的交互獲得回報(bào)值，并在“行動(dòng)器-評(píng)判器”（Actor-Critic，AC）框架下交替迭代更新策略評(píng)估與策略改進(jìn)環(huán)節(jié)，直至收斂。然而，無模型強(qiáng)化學(xué)習(xí)對(duì)環(huán)境的無目的隨機(jī)探索特性導(dǎo)致其采樣效率不高，收斂速度較慢。

針對(duì)當(dāng)前智能車輛模型預(yù)測運(yùn)動(dòng)控制研究中存在的問題，本文在策略梯度求解框架下提出一種融合預(yù)測模型的策略梯度MPC（Policy Gradient MPC，PGMPC）方法。該方法利用神經(jīng)網(wǎng)絡(luò)近似最優(yōu)策略，基于MPC 問題的預(yù)測模型及優(yōu)化目標(biāo)設(shè)計(jì)策略網(wǎng)絡(luò)的損失函數(shù)，將車輛運(yùn)動(dòng)控制的非線性MPC 問題轉(zhuǎn)化為神經(jīng)網(wǎng)絡(luò)問題。相比于傳統(tǒng)MPC方法，PGMPC方法通過離線求解、在線應(yīng)用的方式，同時(shí)提高了車輛運(yùn)動(dòng)控制的精度以及在線應(yīng)用的計(jì)算效率。

2 基于MPC的車輛運(yùn)動(dòng)控制問題建模

2.1 車輛運(yùn)動(dòng)模型建立

假設(shè)汽車輪胎側(cè)偏特性處于線性范圍內(nèi)，建立如圖1所示的二自由度動(dòng)力學(xué)車輛運(yùn)動(dòng)預(yù)測模型。模型中，系統(tǒng)的輸入量為前輪轉(zhuǎn)角δ，即u=[δ]，狀態(tài)量為x=[y,θ,β,ω]，其中y、θ、β、ω分別為車輛的側(cè)向位置、航向角、質(zhì)心側(cè)偏角和橫擺角速度。

圖1 車輛運(yùn)動(dòng)預(yù)測模型

現(xiàn)有的研究通常基于小航向角假設(shè)，將車輛的平面運(yùn)動(dòng)模型建立為線性模型。然而，在大曲率工況下該假設(shè)不再成立，小航向角假設(shè)必然會(huì)導(dǎo)致軌跡跟蹤運(yùn)動(dòng)控制精度的降低。為解決此問題，本文建立車輛運(yùn)動(dòng)控制的非仿射非線性連續(xù)系統(tǒng)狀態(tài)方程：

式中，vx為車輛縱向速度；m為整車質(zhì)量；k1、k2分別模型的前、后輪胎等效側(cè)偏剛度；a、b分別為車輛質(zhì)心到前、后軸的距離；Iz為車輛的轉(zhuǎn)動(dòng)慣量。

對(duì)于式（1）所示的非線性動(dòng)力學(xué)系統(tǒng)，其狀態(tài)方程無法用狀態(tài)矩陣與輸入矩陣表達(dá)，僅可用一般形式表達(dá)：

對(duì)于上述具有一般形式的時(shí)不變動(dòng)力學(xué)系統(tǒng)，其離散狀態(tài)方程可表達(dá)為：

式中，dt為離散系統(tǒng)的采樣時(shí)間；xk、uk分別為k時(shí)刻系統(tǒng)的狀態(tài)量及控制量。

2.2 基于運(yùn)動(dòng)控制的MPC建模

為不失一般性，設(shè)當(dāng)前時(shí)刻為0 時(shí)刻，被控系統(tǒng)的初始狀態(tài)為x0。對(duì)于預(yù)測時(shí)域N內(nèi)的任意預(yù)測時(shí)刻k，系統(tǒng)的輸入量為uk-1、狀態(tài)量為xk、參考量為rk，預(yù)測時(shí)域內(nèi)系統(tǒng)狀態(tài)轉(zhuǎn)移過程如圖2所示。

圖2 預(yù)測時(shí)域狀態(tài)轉(zhuǎn)移示意

那么，系統(tǒng)在第k個(gè)控制周期的代價(jià)值Lk為uk-1、xk、rk的函數(shù)：

式中，lk為代價(jià)函數(shù)。

對(duì)于車輛運(yùn)動(dòng)控制MPC 問題而言，若給定初始狀態(tài)x0及預(yù)測時(shí)域內(nèi)參考量r1:N（1:N表示預(yù)測時(shí)域的第1步到第N步），則預(yù)測時(shí)域內(nèi)的代價(jià)函數(shù)為：

基于MPC的車輛運(yùn)動(dòng)控制的求解即為找到使預(yù)測時(shí)域內(nèi)代價(jià)函數(shù)最小的最優(yōu)控制序列u*，并將最優(yōu)控制序列的第1個(gè)控制量作用于系統(tǒng)后重置初始狀態(tài)，不斷通過滾動(dòng)優(yōu)化實(shí)現(xiàn)對(duì)系統(tǒng)的最優(yōu)控制。

綜上，車輛運(yùn)動(dòng)控制MPC問題可建模為最優(yōu)化問題：

其中：

當(dāng)式（3）所示系統(tǒng)為線性系統(tǒng)時(shí)，式（6）所示問題為二次規(guī)劃問題。然而，當(dāng)式（3）所示系統(tǒng)為非仿射非線性系統(tǒng)時(shí)，求解式（6）所示問題非常困難，甚至無法實(shí)現(xiàn)實(shí)時(shí)求解。

3 策略梯度MPC算法

3.1 策略存在性

為實(shí)現(xiàn)式（6）所示的非仿射非線性系統(tǒng)MPC 問題的實(shí)時(shí)求解，本文使用函數(shù)近似的方法，將求解最優(yōu)化問題的方式由在線求解轉(zhuǎn)化為離線訓(xùn)練、在線應(yīng)用，從而大幅提高求解效率。該方法在提出過程中使用了如下2個(gè)定理：

引理1 萬能近似定理：若一個(gè)前饋神經(jīng)網(wǎng)絡(luò)具有至少一層含有任意一種“擠壓”性質(zhì)激活函數(shù)隱藏層的線性輸出層，那么當(dāng)網(wǎng)絡(luò)具有足夠數(shù)量的隱藏單元（即網(wǎng)絡(luò)過參數(shù)化）時(shí)，該網(wǎng)絡(luò)能夠以任意精度近似任何一個(gè)有限維空間指向另一個(gè)有限維空間的函數(shù)[10]。

引理2近似最優(yōu)性：當(dāng)近似函數(shù)為過參數(shù)化的神經(jīng)網(wǎng)絡(luò)時(shí)，可利用優(yōu)化算法（如梯度下降法和隨機(jī)梯度下降法）在多項(xiàng)式時(shí)間內(nèi)找到參數(shù)化損失函數(shù)的全局最小值[11-12]。

設(shè)參數(shù)化策略π由策略神經(jīng)網(wǎng)絡(luò)的參數(shù)θ決定，且策略π為當(dāng)前狀態(tài)x0與預(yù)測時(shí)域內(nèi)參考量r1:N到控制序列的映射。若可獲得一組最優(yōu)的參數(shù)θ*，使得策略π與最優(yōu)控制序列u*之間滿足如式（8）所示的等價(jià)關(guān)系，則MPC問題的最優(yōu)解可被策略π近似：

因此，若能求解出使式（8）成立的參數(shù)θ*，便可將MPC的在線求解轉(zhuǎn)為參數(shù)化映射。引理1保證等式（8）的存在性，引理2保證式（8）參數(shù)的可求解性。基于此，本文提出一種以神經(jīng)網(wǎng)絡(luò)作為近似函數(shù)的求解方法，實(shí)現(xiàn)非仿射非線性系統(tǒng)MPC問題的近似求解。

3.2 策略梯度MPC問題求解

對(duì)于連續(xù)系統(tǒng)的控制問題，傳統(tǒng)策略梯度類方法（如DDPG 方法）通常采用Actor-Critic 框架求解，如圖3所示。圖中：Q為值網(wǎng)絡(luò)輸出；r為回報(bào)值；γ為折扣因子；ε為均方根函數(shù)；qw為參數(shù)w下的值網(wǎng)絡(luò)函數(shù)。DDPG 算法通過最小化上述2 個(gè)網(wǎng)絡(luò)的損失函數(shù)Lπ、Lq進(jìn)行求解，其中Actor 的目標(biāo)是最小化值網(wǎng)絡(luò)輸出Qw，Critic 的目標(biāo)是最小化輸出值與實(shí)際回報(bào)值的均方差。通過環(huán)境的不斷交互，基于梯度下降實(shí)現(xiàn)Actor與Critic交替更新，直至收斂。

圖3 DDPG方法求解框圖

可發(fā)現(xiàn)，在傳統(tǒng)DDPG 等的無模型策略梯度方法中，需要通過對(duì)環(huán)境的探索不斷取得回報(bào)值，通過“自舉法”實(shí)現(xiàn)梯度下降。然而，在這個(gè)過程中，僅考慮了策略網(wǎng)絡(luò)的權(quán)值向量對(duì)值函數(shù)估計(jì)的影響，忽略了其對(duì)目標(biāo)值函數(shù)的影響，下降方向僅包含一部分梯度。因此，傳統(tǒng)無模型的更新過程稱為半梯度方法，這導(dǎo)致收斂過程不穩(wěn)健，收斂速度較慢。

在MPC 問題中，被控對(duì)象模型及代價(jià)函數(shù)均是先驗(yàn)已知信息（如式（3）、式（5）所示）。因此，本文在利用策略梯度類方法求解最優(yōu)策略π時(shí)，在傳統(tǒng)Actor-Critic框架基礎(chǔ)上引入系統(tǒng)預(yù)測模型及代價(jià)函數(shù)，提出PGMPC 方法，其框架如圖4 所示。圖中V函數(shù)為式（5）所示的代價(jià)函數(shù)。

圖4 PGMPC方法求解框圖

相比于傳統(tǒng)的策略梯度方法，PGMPC 方法由于結(jié)合了被控對(duì)象的模型及代價(jià)函數(shù)，其目標(biāo)值函數(shù)及估計(jì)值函數(shù)總是確定的。因此，通過梯度下降直接更新策略網(wǎng)絡(luò)即可完成訓(xùn)練，具有更高的魯棒性。

為了找到最優(yōu)的參數(shù)θ*，并使式（8）對(duì)全狀態(tài)空間都成立，就需要對(duì)原有代價(jià)函數(shù)在狀態(tài)空間上取期望。此時(shí)，構(gòu)造出神經(jīng)網(wǎng)絡(luò)的損失函數(shù)為：

式中，πk-1為由策略π產(chǎn)生的控制序列中的第(k-1)個(gè)控制量；χ為策略網(wǎng)絡(luò)輸入的樣本空間；E為期望函數(shù)。

那么，損失函數(shù)J(θ)對(duì)于參數(shù)θ的梯度為：

將式（8）代入式（5）所示的預(yù)測時(shí)域代價(jià)函數(shù)，可得：

由于參考量rk與參數(shù)無關(guān)，而πk-1、xk均為θ的函數(shù)，根據(jù)鏈?zhǔn)角髮?dǎo)法則，式（11）右端代價(jià)函數(shù)可展開為：

式中，（12）-1、（12）-3項(xiàng)由式（4）決定。

對(duì)于（12）-2 項(xiàng)，由于xk、πk-1、xk-1滿足式（3）所示的函數(shù)關(guān)系，繼續(xù)利用鏈?zhǔn)椒▌t對(duì)其進(jìn)行展開，可得：

記式（12）-2 項(xiàng)為函數(shù)φ，那么相鄰時(shí)刻的φk與φk-1存在如式（14）所示的遞歸關(guān)系，其中f為式（3）中的車輛動(dòng)力學(xué)模型函數(shù)：

將式（11）～式（14）代入式（10）中，即可求得神經(jīng)網(wǎng)絡(luò)損失函數(shù)相對(duì)參數(shù)θ的梯度。以梯度下降法為例，第K次迭代中網(wǎng)絡(luò)的更新公式為：

式中，aK為當(dāng)前迭代周期的學(xué)習(xí)率。

根據(jù)引理2，通過對(duì)參數(shù)的不斷迭代更新，θ最終收斂到θ*。

注意到，由于在神經(jīng)網(wǎng)絡(luò)訓(xùn)練過程中，收斂條件對(duì)系統(tǒng)狀態(tài)及代價(jià)函數(shù)特征沒有要求，所以，理論上對(duì)于任意能夠顯式表達(dá)系統(tǒng)動(dòng)力學(xué)特性及代價(jià)函數(shù)的控制系統(tǒng)，均可在本文提出的PGMPC框架下進(jìn)行近似求解。

4 仿真驗(yàn)證

4.1 仿真對(duì)比驗(yàn)證總體設(shè)計(jì)

為驗(yàn)證所提出方法的有效性，本文選擇大曲率道路（半徑15 m）的軌跡跟蹤控制作為仿真工況，對(duì)比PGMPC算法與傳統(tǒng)在線求解MPC（Online MPC，OMPC）的跟蹤精度、求解實(shí)時(shí)性等性能指標(biāo)（其中OMPC 采用Boyd團(tuán)隊(duì)所開發(fā)的OSQP求解器[6]）。在此工況下，由于道路航向變化較大，道路航向角的小角度假設(shè)所導(dǎo)致的模型偏差也較大。

為了進(jìn)行有效對(duì)比，仿真驗(yàn)證中PGMPC 與OMPC使用相同的凸優(yōu)化目標(biāo)函數(shù)。將式（4）所示代價(jià)函數(shù)修改為式（16），該代價(jià)函數(shù)同時(shí)考慮跟蹤精度與行駛舒適性，被廣泛應(yīng)用為智能車輛軌跡跟蹤的目標(biāo)函數(shù)：

式中，Q為狀態(tài)量懲罰權(quán)重，R為輸入量懲罰權(quán)重。

仿真驗(yàn)證中以CarSim 搭建的動(dòng)力學(xué)模型為被控對(duì)象，搭建聯(lián)合仿真環(huán)境，模擬車輛的大曲率軌跡跟蹤控制。仿真中使用的主要參數(shù)設(shè)置如表1示。

表1 聯(lián)合仿真參數(shù)

4.2 PGMPC訓(xùn)練結(jié)果

仿真驗(yàn)證中采用深度全連接神經(jīng)網(wǎng)絡(luò)作為PGMPC算法的近似函數(shù)。其中輸入層由跟蹤控制系統(tǒng)的狀態(tài)構(gòu)成，并與后續(xù)的4 層隱藏層全連接，隱藏層使用整流非線性單元作為激活函數(shù)，每層有27個(gè)神經(jīng)元，網(wǎng)絡(luò)的輸出層為tanh層，以界定控制量的邊界。離線訓(xùn)練過程中超參數(shù)設(shè)置情況如表2所示。

表2 神經(jīng)網(wǎng)絡(luò)參數(shù)設(shè)計(jì)

圖5 所示為訓(xùn)練過程中神經(jīng)網(wǎng)絡(luò)的誤差與訓(xùn)練次數(shù)之間的變化關(guān)系。由圖5可以看出：在訓(xùn)練次數(shù)小于1 000次時(shí)，隨著訓(xùn)練次數(shù)的增加，策略誤差值迅速降低到10-3以下；隨著訓(xùn)練次數(shù)繼續(xù)增加，誤差值略有上升，然后以較小的斜率繼續(xù)下降，最終保持在10-5左右，滿足誤差要求，訓(xùn)練停止并得到策略網(wǎng)絡(luò)。

圖5 訓(xùn)練過程中的誤差變化

4.3 仿真對(duì)比結(jié)果

仿真中，車輛以(15 m,0 m)為起始點(diǎn)，沿半徑為15 m的期望路徑逆時(shí)針行駛，軌跡跟蹤結(jié)果對(duì)比如圖6 所示。結(jié)果表明，PGMPC 與OMPC 均可達(dá)到較高的跟蹤精度，但PGMPC的跟蹤精度更高。

圖6 軌跡跟蹤結(jié)果

仿真得到的軌跡跟蹤誤差對(duì)比如圖7所示。由圖7可以看出，對(duì)于試驗(yàn)工況中大曲率軌跡跟蹤，由于無須基于小角假設(shè)建立參考模型，PGMPC的模型誤差更小，明顯優(yōu)于OMPC算法。

圖7 軌跡跟蹤誤差

仿真得到的被控車輛側(cè)向加速度對(duì)比如圖8所示，PGMPC 與OMPC 的平均側(cè)向加速度均為1.67 m/s2，但PGMPC的最大側(cè)向加速度更小、控制更為平滑。

圖8 側(cè)向加速度

算法求解耗時(shí)對(duì)比如圖9所示（計(jì)算平臺(tái)配置均為英特爾i5-8600K CPU，內(nèi)存16 GB）。由于PGMPC在線應(yīng)用僅需做神經(jīng)網(wǎng)絡(luò)前向推理，其計(jì)算平均耗時(shí)為0.405 ms，與之相比，需要在線求解的OMPC算法的平均計(jì)算耗時(shí)為15.72 ms。

圖9 計(jì)算耗時(shí)

5 結(jié)束語

針對(duì)目前智能車輛模型預(yù)測運(yùn)動(dòng)控制因在線求解效率的制約，多使用線性參考模型而引起的跟蹤精度下降問題，本文提出了一種策略梯度模型預(yù)測控制方法，建立車輛運(yùn)動(dòng)控制的非仿射非線性參考模型，在模型預(yù)測控制框架下將模型預(yù)測運(yùn)動(dòng)控制轉(zhuǎn)化為最優(yōu)化問題，并提出一種基于模型的策略梯度求解方法，通過離線求解、在線應(yīng)用的方式提高預(yù)測型運(yùn)動(dòng)控制的求解效率。仿真驗(yàn)證結(jié)果表明，相比于傳統(tǒng)在線求解方法，所提算法的跟蹤誤差由152 mm下降到1.99 mm，單步求解耗時(shí)由15.72 ms 下降到0.405 ms，驗(yàn)證了本文所提出算法的有效性。

未來工作將圍繞2個(gè)方面開展：研究具有自學(xué)習(xí)能力的求解方法，提高算法的環(huán)境自適應(yīng)能力；研究輸入狀態(tài)有噪聲情況下的求解算法，提高算法的魯棒性。