王宏偉，劉晨宇，李 磊，張昊天

（東北大學秦皇島分?？刂乒こ虒W院，秦皇島 066004）

前言

隨著無人駕駛技術(shù)的快速發(fā)展，無人車輛在減少交通事故、提高道路利用率、完成特殊作業(yè)、國防軍事應用等領域發(fā)揮著至關(guān)重要的作用［1］。無人車是一個非線性程度高、耦合性強、多輸入多輸出的系統(tǒng)。其中，無人車軌跡跟蹤控制是實現(xiàn)自動駕駛技術(shù)的關(guān)鍵，目前已成為國內(nèi)外學者研究的熱點問題，是無人車今后研究的重點方向。

為了克服無人車系統(tǒng)軌跡跟蹤控制面臨的挑戰(zhàn)，提高無人車軌跡跟蹤的可靠性、安全性和精確性，國內(nèi)外學者進行了大量的研究，提出了很多有效的控制方法。主要包括：經(jīng)典控制［2］、最優(yōu)化控制［3］、自適應控制［4］、魯棒控制［5］、滑?？刂疲?］等。這些控制方法在無人車控制領域取得了明顯的效果，但不能解決控制過程中車輛動力學多約束問題。模型預測控制（model predictive control，MPC）可以將無人車系統(tǒng)實際約束條件加入預測運動中，同時根據(jù)無人車狀態(tài)反饋量做出實時調(diào)整，達到最優(yōu)控制。但MPC控制算法在滾動優(yōu)化時需要對車輛模型線性化，不能準確建立車輛系統(tǒng)精準的非線性模型，容易造成模型失配問題［7］。其次MPC控制算法的約束條件通常被描述為優(yōu)化過程的硬約束，在極端工況下會增加系統(tǒng)計算負擔，導致無法實時求出可行解。

非線性模型預測控制（nonlinear model predictive control，NMPC）對非線性模型具有良好的控制性能，并且可以顯式處理系統(tǒng)約束，解決非凸規(guī)劃問題［8］。在NMPC的設計中，非線性系統(tǒng)會帶來繁重的計算負擔，高計算負荷和實時求解之間會產(chǎn)生沖突，計算效率是需要考慮的關(guān)鍵因素。文獻［9］中對車輛建立輪胎的高度非線性模型，利用序列二次規(guī)劃（sequential quadratic programming，SQP）算法求解NMPC控制問題，將一部分在線計算負擔轉(zhuǎn)移到離線過程。文獻［10］中提出了一種FP-SQP改進方法，用以解決非線性問題，但是該算法僅適用于多項式非線性系統(tǒng)，存在一定的局限性。文獻［11］中將MPC優(yōu)化問題分為離線和在線兩部分，其中優(yōu)化僅在在線部分進行，用來減少計算時間。但這種方法的有效性取決于約束問題的可分性，不適合靈活應用于不同場合。文獻［12］中提出基于直接多次打靶法的實時迭代算法，將計算時間大幅減少到每次迭代20 ms，但要求代價函數(shù)和約束至少兩次連續(xù)可微。文獻［13］中對打靶法的約束條件進行了改進，但只適用于約束都能轉(zhuǎn)換為特定形式，并滿足所有最優(yōu)化條件的場合，這會影響其進一步推廣和使用。文獻［14］中針對車輛在低速運行工況下，利用遺傳算法優(yōu)化求解過程，實現(xiàn)了NMPC控制器的實時尋優(yōu)，進一步提升了計算效率。文獻［15］中針對車輛輪胎在路徑跟蹤中存在的高度非線性問題，提出一種線性時變MPC控制器來解決非線性問題的方法。為了減少計算工作量，對非線性模型在初始狀態(tài)下通過泰勒展開進行線性化，但由于模型近似而導致一定誤差。文獻［16］中利用粒子群優(yōu)化算法和現(xiàn)場可編程門陣列芯片的并行計算優(yōu)勢，開發(fā)了實時NMPC策略，但芯片成本較高，難以實現(xiàn)大規(guī)模應用。文獻［17］中提出一種效果與NMPC類似的顯式MPC算法，控制問題采用離線優(yōu)化，并生成在線采用的查找表。這樣控制的性能依賴于查找表中定義的大量點，因此導致高內(nèi)存需求，從而限制了其適用性。與上述求解NMPC的算法相比，連續(xù)/廣義最小殘差算法（continuation/generalized minimal residual，C/GMRES）結(jié)合了連續(xù)法［18］和Krylov子空間法［19］，在不依賴于求解問題直接優(yōu)化方法所需的大量迭代的前提下，得到系統(tǒng)最優(yōu)控制規(guī)律，已經(jīng)發(fā)展成為一種普遍求解NMPC的算法［20］。文獻［21］在利用NMPC方法研究車輛偏航穩(wěn)定性時，在C/GMRES算法中引入了死區(qū)懲罰函數(shù)用于處理不等式約束，實現(xiàn)理想的軌跡跟蹤效果。但該算法求解的NMPC設計性能指標中不包括無人車系統(tǒng)終端狀態(tài)約束，計算出的NMPC控制缺乏穩(wěn)定性保證。

本文中關(guān)注的非線性問題主要為兩方面：一是在路面附著系數(shù)較低和車速較高情況下，車輛的輪胎模型非線性程度明顯增加；二是在求解過程中，因為巨大的計算量出現(xiàn)無可行解或極端解，造成非線性程度增加。針對以上問題提出一種改進的連續(xù)/廣義最小殘差算法（improved continuation/generalized minimal residual，improved-C/GMRES）的NMPC無人車軌跡跟蹤控制方法。首先，建立車輛非線性模型并在性能指標中加入多約束條件。然后，為提高求解平滑性，利用障礙罰函數(shù)法處理不等式約束，引入連續(xù)增加的懲罰因子降低運算負擔，提高數(shù)值求解的計算效率。最后，通過Simulink和CarSim的聯(lián)合仿真，驗證了無人車在高速和復雜工況條件下所提出算法結(jié)果的有效性和準確性，實現(xiàn)精確穩(wěn)定的軌跡跟蹤控制。

1 非線性車輛模型

1.1 動力學模型

為實現(xiàn)軌跡跟蹤控制，對車輛橫向和縱向建立2自由度車輛模型，定義XOY為大地坐標系，并且設為慣性參考系。定義xoy為車輛坐標系，其中原點o為車輛質(zhì)心。

建立2自由度動力學模型如圖1所示。

圖中：X和Y分別為車輛橫、縱坐標；vx為縱向速度；vy為橫向速度；β為質(zhì)心側(cè)偏角；γ為橫擺角速度；φ為航向角；Lf和Lr分別為質(zhì)心到前、后軸的距離；Fyf和Fyr分別為前、后輪胎的側(cè)向力；αf和αr分別為前、后輪輪胎側(cè)偏角；δ為車輛前輪轉(zhuǎn)角。

車輛坐標(X，Y)與行駛速度的關(guān)系為

根據(jù)圖1可以得到車輛的動力學方程：

式中Iz為繞z軸的轉(zhuǎn)動慣量。

車輛輪胎在前/后輪受到的側(cè)向力為

式中Fyfl、Fyfr、Fyrl和Fyrr分別為左前輪、右前輪、左后輪和右后輪受到的側(cè)向力。

當質(zhì)心側(cè)偏角很小的時候，tanβ≈β，由幾何關(guān)系可得：

建立的軌跡跟蹤示意圖如圖2所示。

圖中：φr為期望軌跡的參考航向角；φ為實際軌跡的航向角。

定義ey表示車輛位置和期望路徑之間的距離誤差，實際意義為從車輛質(zhì)心o到期望軌跡上正交投影點T的距離。定義eφ為航向角誤差，eφ=φ-φr，可以得到公式：

1.2 輪胎模型

輪胎在高速行駛的過程中，非線性特性會尤為明顯，直接影響車輛的穩(wěn)定性和操縱性。

前、后輪輪胎側(cè)偏角計算公式為

魔術(shù)輪胎（magic formula，MF）模型對車輛輪胎力非線性特性擬合較好，其表達式為

式中：α為輪胎側(cè)偏角；μ為路面附著系數(shù)；FZ為輪胎垂直載荷；B為剛度系數(shù)；CFα為轉(zhuǎn)彎剛度；C為輪胎模型形狀因子；D為輪胎模型峰值因子；E為輪胎模型曲率因子；k1和k2分別為辨識系數(shù)。

將式（2）～式（8）代入式（1），得到車輛的偏航運動方程：

式中Fzf和Fzr分別為前、后輪胎垂直載荷。

2 NMPC控制器設計

NMPC對于求解車輛運動過程的非線性問題有著較好的計算速度和計算精度。本文對設計的性能指標函數(shù)添加非線性軟約束和硬約束條件，在保證車輛行駛穩(wěn)定性的前提下，同時提高對目標路徑的跟蹤性能。

2.1 預測模型

根據(jù)式（1）-式（6），得到非線性模型預測的狀態(tài)空間表達式，即

式中：f為系統(tǒng)的狀態(tài)方程；x=[β，γ，ey，eφ，X，Y]為系統(tǒng)狀態(tài)量；u=δ為控制變量；ω=φr為外部干擾；y為系統(tǒng)輸出方程；ξ為受控變量。

2.2 問題描述

NMPC控制性能指標函數(shù)最小化可表示為

式中：x0為車輛的初始狀態(tài)；t為初始采樣時間；T為系統(tǒng)的預測時域；τ為最小化預測時域。

式（12）第1式等式右側(cè)第1項為系統(tǒng)的終端狀態(tài)，第2項可以表示為

式中：xτ為τ時刻的實際軌跡；x r為期望參考軌跡；Q=diag{q1，…，q6}和R=diag{r1}為權(quán)重系數(shù)矩陣，q1～q6和r1為權(quán)重系數(shù)。

將式（12）簡化為如下形式：

式中U為計算得到的不同時刻最優(yōu)控制序列。

3 求解器設計

利用NMPC求解會使計算量增加，為提高無人車穩(wěn)定性和求解效率，本文在C/GMRES算法基礎上進行改進，得到improved-C/GMRES算法。

3.1 控制規(guī)律設計

為了保證NMPC控制器的穩(wěn)定性，對車輛前輪轉(zhuǎn)角控制設計了輔助控制規(guī)律。根據(jù)文獻［23］采用反步法得到控制規(guī)律：

式中：γaux(t)為車輛前輪轉(zhuǎn)角速度控制規(guī)律；ka和kb為調(diào)整因子。

kb＞vx ka是速度誤差和航向角誤差收斂為0的必要條件。

構(gòu)造對應的李雅普諾夫函數(shù)：

對于所構(gòu)造的李雅普諾夫函數(shù)，在定義域內(nèi)是連續(xù)可微的，并且是無界的。由李雅普諾夫定理可知，存在一組?i(i=1，2，3)滿足下列式子：

根據(jù)NMPC求解特點，在每個采樣時刻輸入一個采樣周期，只會對非線性模型預測的第一個更新狀態(tài)具有控制作用，得到以下不等式：

利用李雅普諾夫定理，得到無人車輛系統(tǒng)的終端狀態(tài)，將終端約束這個附加約束應用到在線優(yōu)化中，可以證明閉環(huán)系統(tǒng)的穩(wěn)定性。即在優(yōu)化過程中施加的約束要求系統(tǒng)在預測期結(jié)束時的狀態(tài)相對于預測期開始時的狀態(tài)在范數(shù)上收斂。將設定值選為優(yōu)化問題中的終端等式約束，既保證漸近收斂到期望軌跡，又保證了NMPC在線求解的優(yōu)化問題的可行性。

3.2 約束條件

根據(jù)車輛實際轉(zhuǎn)向系統(tǒng)，可以得到如式（20）所示的約束條件：

式中：角標max和min分別表示為約束的上、下邊界；β和γ的最大值滿足式（21）條件。

式中：g為重力加速度；δf∈[-6°，6°]。

為提高NMPC控制方法的穩(wěn)定性，使得到的最優(yōu)性保證在有限時域下的閉環(huán)穩(wěn)定，對NMPC優(yōu)化問題增加一個穩(wěn)定性約束條件：

給定每個時刻的初始狀態(tài)x(t)，可以根據(jù)式（12）求解最優(yōu)控制序列。由于僅取最優(yōu)控制序列的第一個元素作為實際控制值，所以僅考慮在預測時間[t，t+τ]中的穩(wěn)定性約束。

3.3 障礙罰函數(shù)

在NMPC求解過程中，求解問題是一個帶有不等式約束的非線性規(guī)劃問題，無法利用常規(guī)方法得到解析解。為了解決不等式約束局限，利用障礙罰函數(shù)法［23］來計算數(shù)值解。

在性能指標函數(shù)中引入違反約束懲罰項。定義一個新的理想障礙函數(shù)：

由于障礙函數(shù)在ζ(c(x(τ)，u(τ)))=0處缺乏連續(xù)可微性，導致該函數(shù)在實數(shù)域上不可微。為解決這個問題，引入近似函數(shù)，其表達式為

式中κ為近似系數(shù)。當κ→∞時，在c(x(τ)，u(τ))=0處可以用式（24）代替。

在不等式約束中加入障礙函數(shù)，既將控制變量限制在合理范圍之內(nèi)，可以遵從約束條件，同時可以兼顧性能指標中最小值條件，保證了性能指標的優(yōu)越，在多約束條件下新的優(yōu)化問題可以成功求解，這種方法特別適合于具有多狀態(tài)約束的無人車系統(tǒng)。此外，這種方法不需要引入額外的優(yōu)化變量，在保證計算精度的前提下，可以將計算工作量保持在相對較低的水平。

3.4 最優(yōu)化條件

為了保證NMPC優(yōu)化問題中車輛的穩(wěn)定性，加入一個穩(wěn)定性約束條件，將原來的優(yōu)化問題改寫為

上述軌跡跟蹤問題可以等價最優(yōu)控制必要條件，為解決最優(yōu)控制問題，本文引入哈密頓函數(shù)：

式中λ為共態(tài)向量。

基于龐特里亞金最大化原理（Pontryagin’s maximum principle，PMP），無人車軌跡跟蹤最優(yōu)控制問題可以轉(zhuǎn)化為兩點邊值問題（two-point boundary value problem，TPBVP），將連續(xù)的兩點邊值問題離散化，在預測時域[0，T]內(nèi)得到離散的兩點邊值問題為

通過式（27）計算可以得到：

取最優(yōu)控制的第一個元素(t)作為實際控制輸入。為了求解t時刻式（28）矩陣，利用數(shù)值延拓法和前向差分廣義最小殘差（forward-difference generalized minimal residual，F(xiàn)DGMRES）方法。文獻［20］和文獻［24］中給出了算法的具體描述。該算法避免求解大量的雅可比矩陣、Hessian矩陣和逆矩陣，將這種函數(shù)計算方法簡寫為

式中：ηmax為設置的最大迭代次數(shù)；U?為一個特定的初始猜測。

3.5 高效NMPC優(yōu)化算法

在初始化過程中，首先在傳統(tǒng)C/GMRES方法的基礎上加入變量參數(shù)求解PMP條件，只求解一次得到可行的初始控制輸入序列。

基于求解TPBVP的方法，在最優(yōu)解的鄰域中，找到收斂更快且精度更高的解。所以在約束條件下需要找到一組更好的初始控制輸入序列U。于是引入連續(xù)增加的障礙項權(quán)重因子ε求解式（28），并將當前時刻求解的輸入序列作為下次迭代的初始值。高效NMPC算法引入連續(xù)增加的障礙項權(quán)重因子ε，可以通過迭代次數(shù)的增加，尋找使代價函數(shù)最小的權(quán)重因子ε，找到滿足條件的最優(yōu)控制序列，從而減小誤差，提高系統(tǒng)的控制精度。

分別利用傳統(tǒng)的C/GMRES算法和本文提出improved-C/GMRES算法進行計算求解，通過對比這兩種算法的計算結(jié)果，驗證improved-C/GMRES算法，在保證求解效率的前提下，可以保證無人車軌跡跟蹤的穩(wěn)定性，表1和表2給出這兩種算法的具體步驟。

表1 C/GMRES算法

表2 improved-C/GMRES算法

4 仿真與分析

為了驗證本文所提出的高效NMPC方法在無人車軌跡跟蹤控制的精度和求解速度，基于Simulink和CarSim搭建的聯(lián)合仿真平臺進行仿真分析，在2.3GHz筆記本電腦上的Intel Core i7 8750H CPU下對所提出的策略進行驗證，將利用improved-C/GMRES算法求解的高效NMPC與利用C/GMRES求解的NMPC進行對比。

本文選擇的復雜路況是在標準的雙移線和蛇行工況基礎上進行調(diào)整，使原有曲線具有更低的路面附著系數(shù)和更大的轉(zhuǎn)彎弧度，同時讓車輛保持較高的速度通過設計路段，驗證算法性能。表3和表4分別表示車輛動力學模型參數(shù)和控制器參數(shù)。

表3 車輛動力學模型參數(shù)

表4 控制器參數(shù)

4.1 雙移線工況仿真

為了驗證對比兩種算法的控制效果，在雙移線工況進行仿真驗證。道路附著系數(shù)μ為0.6，目標車速為60 km/h，并假設車輛初始狀態(tài)和偏航角都為零。

圖3表示軌跡跟蹤曲線圖，圖4表示軌跡誤差曲線圖。

由圖3和圖4可知，標準NMPC控制軌跡誤差在±0.15 m之內(nèi)，在5～6 s之間出現(xiàn)較大的誤差波動；而高效NMPC控制軌跡誤差在±0.07 m之內(nèi)，誤差更小，在控制過程中未產(chǎn)生劇烈的誤差波動?？梢姼咝MPC控制器在高速條件下跟蹤控制效果更好。

圖5表示航向角跟隨曲線圖，圖6表示航向角誤差曲線圖。

由圖5和圖6可知，標準NMPC控制角度誤差較大，在3～4 s和5～6 s之間都出現(xiàn)了明顯的角度誤差；而高效NMPC控制角度誤差更小，且控制過程中角度變化量也更加平緩，沒有出現(xiàn)劇烈波動的情況。可見高效NMPC控制器在路面附著系數(shù)較低，轉(zhuǎn)彎弧度較大的工況下，可以更精準實現(xiàn)角度跟蹤控制，并且可以提高車輛穩(wěn)定性和安全性。

4.2 蛇行工況仿真

為了進一步驗證本文所提方法的精確性和穩(wěn)定性，在更為復雜的蛇行工況下進行仿真。蛇行工況在短距離內(nèi)有多個大幅度轉(zhuǎn)彎，道路附著系數(shù)μ為0.45，目標車速為60 km/h，并假設車輛初始狀態(tài)以及偏航角都為零。

圖7表示軌跡跟蹤曲線圖，圖8表示軌跡誤差曲線圖。由圖可見，標準NMPC控制軌跡誤差在±0.4 m之內(nèi)，在3～4 s和6～7 s之間出現(xiàn)較大的誤差波動；而高效NMPC控制軌跡誤差在±0.1 m之內(nèi)，誤差更小且未產(chǎn)生劇烈的誤差波動?？梢姼咝MPC控制器在連續(xù)轉(zhuǎn)彎的情況下仍可以保持較好的控制效果。

圖9表示航向角跟隨曲線圖，圖10表示航向角誤差曲線圖。

由圖9和圖10的對比結(jié)果可知，標準NMPC控制角度誤差較大，在8～9 s都出現(xiàn)了明顯的角度誤差，車輛在行駛過程中的穩(wěn)定性較差；而高效NMPC角度誤差較小且較為平緩，在復雜工況下可以保持穩(wěn)定性。

4.3 計算效率驗證

本文在驗證兩種工況的控制精度時，為了避免實驗數(shù)據(jù)出現(xiàn)偶然性，在路況參數(shù)和控制器參數(shù)設置相同的條件下分別各進行了50次驗證實驗，將實驗數(shù)據(jù)的平均誤差范圍頻數(shù)次數(shù)分別統(tǒng)計到表5和表6中。

由表5和表6的統(tǒng)計數(shù)據(jù)可知，標準NMPC控制的誤差要明顯大于高效NMPC控制，高效NMPC控制誤差范圍更小，控制精度更高。

表5 工況1平均跟蹤誤差頻數(shù)統(tǒng)計

表6 工況2平均跟蹤誤差頻數(shù)統(tǒng)計

同時分別統(tǒng)計兩種方法的每一步迭代計算所需時間，結(jié)果如表7所示。表7中表示在不同工況不同算法的計算耗時情況，表中數(shù)據(jù)為計算仿真的單次優(yōu)化平均計算耗時。

由表7的統(tǒng)計數(shù)據(jù)可知，高效NMPC控制在兩種工況下單次優(yōu)化時間遠遠低于標準NMPC控制，在工況1和工況2中將計算時間大幅減少到每次迭代18.9和25.6 ms，大大提升了計算效率。

表7 單次優(yōu)化計算耗時對比表

為了驗證本文提出improved-C/GMRES控制算法的高效性，利用兩種算法分別完成工況1和工況2的軌跡跟蹤，將50次計算模擬時間求取平均值作為最終的仿真時間，兩種算法仿真時間如圖11所示。從圖中可以看出：C/GMRES算法求解上述兩種工況所需平均時間分別為1.663和7.921 s；improved-C/GMRES算法所需時間分別為0.353和1.632 s?？梢姡捎胕mproved-C/GMRES算法求解所需時間遠遠低于C/GMRES算法的求解時間。表明所提出的NMPC控制器可以實現(xiàn)實時優(yōu)化，提高計算效率，充分證明improved-C/GMRES算法處理非線性問題的高效性。

5 結(jié)論

本文中基于無人車的軌跡跟蹤問題設計了一種高效NMPC控制算法。首先建立車輛非線性模型，利用障礙罰函數(shù)法處理非線性不等式多約束條件，對原有性能指標進行優(yōu)化。然后通過引入連續(xù)增加的懲罰因子加快數(shù)值計算的求解效率。最后通過Simulink和CarSim的聯(lián)合仿真，在路面附著系數(shù)較低，車速較高的兩種工況下驗證算法的控制效果。結(jié)果表明：與C/GMRES算法的NMPC相比，利用improved-C/GMRES算法的高效NMPC在兩種工況下軌跡跟蹤最大誤差大幅度減少，軌跡控制精度得到明顯提高；航向角最大誤差大幅度減小，穩(wěn)定性得到明顯提高；計算效率提高78.77%和79.40%。improved-C/GMRES控制算法在保證軌跡跟蹤精度的同時，使車輛航向角跟蹤達到理想的效果，并提高車輛轉(zhuǎn)彎過程的穩(wěn)定性，可以大幅度提高計算效率，對計算負擔大的非線性問題同樣可以快速求解，滿足無人車實時性要求。