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

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

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

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

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

1 車輛縱向運動模型

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

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

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

對于縱向參考跟蹤問題，引入狀態(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)]，則可得到相應的約束線性二次MPC優(yōu)化問題為

式中：w、wf和U分別為狀態(tài)向量wk、wN和輸入向量uk的約束集；Q、Qf和R分別為相應的權(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維實向量（n×1 實矩陣）和m×n實矩陣構(gòu)成的集合。有關(guān)矩陣和向量的表達式如下：

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

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

3 拉格朗日對偶問題及其求解

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.3 最小迭代下界分析

求解拉格朗日對偶問題的要求為

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

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

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

在算法采用最優(yōu)李普希茨常數(shù)后，其達到控制誤 差ε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 停止準則的確定

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

4 計算結(jié)果分析

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

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

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

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

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

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

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

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

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

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

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

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

4.3 縱向運動控制跟蹤結(jié)果

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

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

5 結(jié)語

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