車輛自動(dòng)駕駛縱向運(yùn)動(dòng)控制優(yōu)化及求解算法

王 韜，曲大義，徐 勝，賈彥峰，宋 慧

（1.青島理工大學(xué)機(jī)械與汽車工程學(xué)院，山東 青島 266520；2.淄博職業(yè)學(xué)院人工智能與大數(shù)據(jù)學(xué)院，山東 淄博 255300；3.哲晰軟件技術(shù)（上海）有限公司自動(dòng)駕駛數(shù)據(jù)服務(wù)部，上海 201815）

為了實(shí)現(xiàn)自動(dòng)和安全駕駛，自動(dòng)駕駛車輛利用車載傳感器感知周圍環(huán)境，基于當(dāng)前的交通狀況動(dòng)態(tài)規(guī)劃路徑，通過操控車速進(jìn)行運(yùn)動(dòng)控制［1］。自動(dòng)駕駛應(yīng)用包括車輛的縱向和側(cè)向運(yùn)動(dòng)控制，而大多數(shù)縱向和側(cè)向控制問題都可以在模型預(yù)測(cè)控制（Model Predictive Control，MPC）的 框 架 下 表述［2-3］。本文考慮車輛的縱向運(yùn)動(dòng)控制，相應(yīng)的MPC 問題為約束線性二次MPC 問題。將MPC 應(yīng)用于車輛縱向運(yùn)動(dòng)控制，需要在短時(shí)間內(nèi)處理大量數(shù)據(jù)，完成高強(qiáng)度計(jì)算任務(wù)，因此，采用有效的求解算法是實(shí)現(xiàn)MPC的關(guān)鍵。

求解該MPC 問題的優(yōu)化方法有離線和在線方法，其中在線方法更為常見［4-5］。在線方法主要包括有效集方法和內(nèi)點(diǎn)法兩種迭代方法。這兩種方法均需求解比較復(fù)雜且容易引起數(shù)值問題的大規(guī)模線性方程組，當(dāng)進(jìn)入實(shí)時(shí)實(shí)現(xiàn)時(shí)，由于計(jì)算復(fù)雜使得在實(shí)時(shí)系統(tǒng)中編碼困難［6］。為減小計(jì)算負(fù)擔(dān)，可利用由典型的線性二次MPC 問題產(chǎn)生的二次規(guī)劃（Quadratic Programming，QP）問題的結(jié)構(gòu)來簡(jiǎn)化數(shù)值方法的實(shí)現(xiàn)，以及采用更有效的數(shù)值方法進(jìn)行求解［7］。

本文采用拉格朗日松弛法將等式約束引入到目標(biāo)函數(shù)中，并用拉格朗日乘子向量作為對(duì)違反約束的懲罰來軟化硬約束，通過歐氏投影移除不等式約束以減輕計(jì)算負(fù)擔(dān)，采用Nesterov 快速梯度法進(jìn)行求解［8］，避免了上述方法存在的各種問題，實(shí)現(xiàn)了對(duì)車輛縱向運(yùn)動(dòng)控制快速Q(mào)P 求解器的開發(fā)。

1 車輛縱向運(yùn)動(dòng)模型

在MPC 框架下，車輛自動(dòng)駕駛的縱向位移（位置）、速度和加速度等關(guān)鍵變量的最優(yōu)值均作為有限時(shí)域約束最優(yōu)控制問題的解得到，因此建立離散形式的車輛縱向運(yùn)動(dòng)模型如下：

式中：ts為采樣周期；xk、vk、ak和a′k分別為在預(yù)測(cè)時(shí)域內(nèi)第k點(diǎn)車輛的縱向位移、速度、加速度和加速度的變化率，并且各變量的取值受到其最大和最小值限制。

2 基于約束線性二次MPC 表達(dá)式建立QP模型

對(duì)于縱向參考跟蹤問題，引入狀態(tài)向量wk=[xk，vk，ak]T，輸入向量uk=[a′k]，狀態(tài)向量參考值wk(ref)=[xk(ref)，vk(ref)，ak(ref)]T，輸 入 向 量 參 考 值uk(ref)=[a′k(ref)]，則可得到相應(yīng)的約束線性二次MPC優(yōu)化問題為

式中：w、wf和U分別為狀態(tài)向量wk、wN和輸入向量uk的約束集；Q、Qf和R分別為相應(yīng)的權(quán)重矩陣；A、B分別為狀態(tài)矩陣和輸入矩陣。

由式（1）可得

式中：z為新的優(yōu)化向量，z∈R4N+3；H為新形成的海森矩陣，H∈R(4N+3)×(4N+3)；g∈R4N+3；C為新的約束矩 陣，C∈R3N×(4N+3)；b∈R3N；Z為z的閉凸集，z?R4N+3。此處Rn和Rm×n分別表示由n維實(shí)向量（n×1 實(shí)矩陣）和m×n實(shí)矩陣構(gòu)成的集合。有關(guān)矩陣和向量的表達(dá)式如下：

式中：I為3階單位矩陣。

采用新的優(yōu)化向量z可以使矩陣C為一帶狀矩陣，從而使后續(xù)有關(guān)計(jì)算更容易。

3 拉格朗日對(duì)偶問題及其求解

3.1 對(duì)偶問題的轉(zhuǎn)化及快速梯度法求解

對(duì)于式（4）的QP問題，應(yīng)用拉格朗日松弛法可以得到拉格朗日函數(shù)如下：

式中：λ為拉格朗日乘子向量，λ∈R3N。

拉格朗日函數(shù)的對(duì)偶函數(shù)為

d(λ)給出了原QP 問題的最優(yōu)值p*的下界，即有d(λ)≤p*，而且d(λ)是一個(gè)無約束凹函數(shù)［9-10］，通過求解拉格朗日對(duì)偶問題即優(yōu)化問題可以得到最優(yōu)拉格朗日乘子λ*，找到原問題最優(yōu)值的最好下界d*=d(λ*)，而如果原問題與其對(duì)偶問題保持強(qiáng)對(duì)偶性［11］，則p*=d*，原問題的最優(yōu)解可由z*(λ)得到，z*(λ)為

最終，可將原QP 問題轉(zhuǎn)化為包含兩層優(yōu)化問題的拉格朗日對(duì)偶問題，即

采用Nesterov 快速梯度法求解該對(duì)偶問題時(shí)，內(nèi)層QP 問題可通過將準(zhǔn)確解投影到箱型邊界經(jīng)一步計(jì)算準(zhǔn)確地求解，即有

式中：PZ為在凸集上的投影。

將z*(λ)代入拉格朗日函數(shù)，可得到拉格朗日對(duì)偶函數(shù)，進(jìn)而得到需要求解的拉格朗日對(duì)偶問題，再借助對(duì)偶函數(shù)的梯度即可完成快速梯度法求解，對(duì)偶函數(shù)的梯度為

本文設(shè)計(jì)的求解拉格朗日對(duì)偶問題的快速梯度法算法如表1 所示。其中，為λ的初始估計(jì)值，L為李普希茨常數(shù)，Kmax為最大迭代次數(shù)。

表1 求解拉格朗日對(duì)偶問題的快速梯度法算法Tab.1 Algorithm of fast gradient method for solving the Lagrange dual problem

3.2 最優(yōu)李普希茨常數(shù)的計(jì)算

在海森矩陣為正定的條件下，對(duì)偶函數(shù)d(λ)具有李普希茨連續(xù)梯度，即滿足

其李普希茨常數(shù)為［12］

式中：‖C‖為矩陣C的范數(shù)，它等于C的最大奇異值；λmin(H)為H的最小特征值。

引入變量的線性變換z=Py，此處P可逆并選用對(duì)角陣，則可得到求解最優(yōu)李普希茨常數(shù)的優(yōu)化問題為

為求解該優(yōu)化問題，引入如下結(jié)果：

若計(jì)算最優(yōu)李普希茨常數(shù)困難，則可用自適應(yīng)搜索法找到該常數(shù)一個(gè)合理的值。

3.3 最小迭代下界分析

求解拉格朗日對(duì)偶問題的要求為

式中：εd為控制誤差；最優(yōu)值d(λ*)事先未知，可用第k次迭代其值的下界代替。

利用快速梯度法收斂速度的上界則有

式中：μ、k分別為強(qiáng)凸函數(shù)的凸性參數(shù)和迭代次數(shù)。

在算法采用最優(yōu)李普希茨常數(shù)后，其達(dá)到控制誤 差εd的 充 分 條 件 是 經(jīng) 過kεd次 迭 代 后 停止［13］，kεd為

最小迭代下界kεd由L*、和εd決定，而只要優(yōu)化問題的框架確定了L*和εd就均為常數(shù)值，由λ0和Λ*決定，要得到Λ*比較困難，但如果選擇了合適的λ0則可以優(yōu)化kεd。

3.4 停止準(zhǔn)則的確定

選用對(duì)偶函數(shù)梯度的范數(shù)‖ ?d(λ) ‖作為停止準(zhǔn)則。從QP 的觀點(diǎn)看，如果‖ ?d(λ) ‖=0 得到完全滿足，那么就可以將由λ恢復(fù)的最終解z(λ)看作是最優(yōu)解。從MPC 的角度看，只要‖ ?d(λ) ‖能收斂于0，那么隨著迭代的進(jìn)行預(yù)測(cè)結(jié)果就將趨近于最優(yōu)計(jì)算值，這意味著與車輛模型運(yùn)動(dòng)學(xué)匹配得較好。在實(shí)際應(yīng)用中，因計(jì)算簡(jiǎn)單，綜合停止準(zhǔn)則可用‖ ?d(λ)‖2，而將其展開后可得

4 計(jì)算結(jié)果分析

車輛縱向運(yùn)動(dòng)控制參數(shù)如表2 所示，其中一些參數(shù)用于為QP問題構(gòu)造箱型邊界。

表2 車輛縱向運(yùn)動(dòng)控制參數(shù)Tab.2 Parameters for vehicle longitudinal motion control

為了獲得良好的車輛縱向運(yùn)動(dòng)控制性能，經(jīng)過仔細(xì)調(diào)試得到權(quán)重矩陣如下：

4.1 不同停止準(zhǔn)則迭代結(jié)果

求解過程中綜合停止準(zhǔn)則值隨迭代次數(shù)的變化情況如圖1 所示。經(jīng)過約500 次迭代‖ ?d(λ)‖2收斂于10-2，而對(duì)于位移和速度不需要達(dá)到這樣小的值。

圖1 綜合停止準(zhǔn)則值隨迭代次數(shù)的變化Fig.1 Relation between the comprehensive stopping criterion values and iterations

求解過程中分解的停止準(zhǔn)則值隨迭代次數(shù)的變化情況如圖2 所示。與加速度停止準(zhǔn)則相比，位移和速度停止準(zhǔn)則可以分別設(shè)置較大的控制誤差。

圖2 分解的停止準(zhǔn)則值隨迭代次數(shù)的變化Fig.2 Relation between the separate stopping criterion values and iterations

雖然由‖ ?d(λ)‖2提取分解的停止準(zhǔn)則需要花費(fèi)一些額外的流程時(shí)間，但可以靈活地設(shè)置3個(gè)不同的控制誤差，這對(duì)于提高求解效率是非常有益的。

4.2 熱起動(dòng)迭代結(jié)果

對(duì)于車輛縱向運(yùn)動(dòng)控制，傳感器的采樣頻率很高，兩個(gè)連續(xù)的數(shù)據(jù)集之間的差別很小，可以選擇前一數(shù)據(jù)集的最優(yōu)解作為計(jì)算后一數(shù)據(jù)集最優(yōu)解的一個(gè)初始估計(jì)，以減少迭代次數(shù)。用試驗(yàn)數(shù)據(jù)集對(duì)此熱起動(dòng)技術(shù)進(jìn)行試驗(yàn)，停止準(zhǔn)則為‖ ?d(λ)‖2=10-2，采用和不用熱起動(dòng)的迭代結(jié)果比較，如圖3 所示。若采用熱起動(dòng)技術(shù)，則可以顯著地減少求解迭代次數(shù)，求解器性能可以得到很大改進(jìn)。

圖3 采用和不用熱起動(dòng)的迭代結(jié)果Fig.3 Number of iterations for method with and without hot start

4.3 縱向運(yùn)動(dòng)控制跟蹤結(jié)果

用試驗(yàn)數(shù)據(jù)集對(duì)采用前述各項(xiàng)技術(shù)開發(fā)的QP求解器進(jìn)行試驗(yàn)，得到車輛縱向位移、速度和加速度的跟蹤性能，如圖4 所示。車輛縱向運(yùn)動(dòng)控制跟蹤性能較好，可以滿足預(yù)期要求。

圖4 縱向位移、速度和加速度跟蹤性能Fig.4 Tracking performance of the displacement，velocity and acceleration in longitudinal direction

5 結(jié)語

針對(duì)車輛自動(dòng)駕駛縱向運(yùn)動(dòng)MPC 實(shí)現(xiàn)問題，建立了車輛縱向運(yùn)動(dòng)模型和縱向參考跟蹤約束線性二次MPC 表達(dá)式；通過變量重排序和對(duì)MPC 表達(dá)式進(jìn)行重構(gòu)，建立了QP 模型，為優(yōu)化求解約束MPC 問題時(shí)簡(jiǎn)化數(shù)值方法的實(shí)現(xiàn)打下了基礎(chǔ)。采用拉格朗日松弛法將QP模型轉(zhuǎn)化為包含兩層優(yōu)化問題的拉格朗日對(duì)偶問題，設(shè)計(jì)了求解該對(duì)偶問題的快速梯度法算法，對(duì)算法的參數(shù)進(jìn)行了分析，并采用試驗(yàn)數(shù)據(jù)集對(duì)QP求解器進(jìn)行了運(yùn)行試驗(yàn)。通過分析和運(yùn)行試驗(yàn)，明確了最小李普希茨常數(shù)、拉格朗日乘子初始值、熱起動(dòng)、綜合和分解的停止準(zhǔn)則，對(duì)求解器性能的影響和應(yīng)用條件，驗(yàn)證了快速梯度法的有效性。開發(fā)的求解器，可以在車輛縱向運(yùn)動(dòng)控制采樣率足夠高的情況下實(shí)時(shí)運(yùn)行。