基于模型預(yù)測(cè)控制的智能汽車目標(biāo)路徑跟蹤方法研究＊

段建民 田曉生 夏天 宋志雪

（北京工業(yè)大學(xué)，北京 100022）

基于模型預(yù)測(cè)控制的智能汽車目標(biāo)路徑跟蹤方法研究＊

段建民 田曉生 夏天 宋志雪

（北京工業(yè)大學(xué)，北京 100022）

為避免智能汽車在目標(biāo)路徑跟蹤過程中發(fā)生側(cè)滑，改善自動(dòng)駕駛性能，通過改進(jìn)目標(biāo)函數(shù)、增加輪胎側(cè)偏角動(dòng)力學(xué)約束，以車輛動(dòng)力學(xué)模型作為模型預(yù)測(cè)控制的預(yù)測(cè)模型，提出了一種改進(jìn)的智能汽車目標(biāo)路徑跟蹤方法。在CarSim與MATLAB/Simulink聯(lián)合仿真平臺(tái)對(duì)該方法進(jìn)行了驗(yàn)證，結(jié)果表明，該方法有效地降低了智能汽車跟蹤目標(biāo)路徑時(shí)發(fā)生側(cè)滑的幾率，提高了智能汽車目標(biāo)路徑跟蹤的準(zhǔn)確性和安全性。

1 前言

路徑跟蹤指無人駕駛車輛沿著設(shè)定的路徑安全、穩(wěn)定、準(zhǔn)確地行駛，其性能直接影響智能車自主行駛的能力[1]。早期的路徑跟蹤控制方法（如純跟蹤算法Pure pursuit[2]、環(huán)形預(yù)瞄法Circular Look-Ahead[3]等）使用幾何方法進(jìn)行路徑跟蹤，但不同于以往的四輪機(jī)器人，車輛由于尺寸較大，受最小轉(zhuǎn)彎半徑、最大角速度等動(dòng)力學(xué)條件的約束[4]，傳統(tǒng)的基于幾何的路徑跟蹤方法并不能很好地解決車輛的這些約束問題。

基于模型預(yù)測(cè)控制的路徑跟蹤控制算法已在國內(nèi)外得到了廣泛應(yīng)用，文獻(xiàn)[4]依據(jù)被控對(duì)象的運(yùn)動(dòng)學(xué)模型，對(duì)系統(tǒng)狀態(tài)量的偏差和控制量進(jìn)行優(yōu)化，該方法雖簡單實(shí)用，但無法對(duì)每個(gè)采樣周期內(nèi)的控制增量進(jìn)行限制，無法避免被控系統(tǒng)狀態(tài)量發(fā)生突變。文獻(xiàn)[5]對(duì)該系統(tǒng)的空間方程進(jìn)行了轉(zhuǎn)換，實(shí)現(xiàn)了對(duì)控制增量的約束，但仍采用車輛運(yùn)動(dòng)學(xué)模型，不能對(duì)車輛動(dòng)力學(xué)指標(biāo)進(jìn)行約束，無法滿足車輛的安全行駛要求。文獻(xiàn)[6]提出了基于線性時(shí)變動(dòng)力學(xué)模型預(yù)測(cè)控制的路徑跟蹤算法，但并未考慮到車輛在高速行駛時(shí)的穩(wěn)定性問題，只對(duì)狀態(tài)量進(jìn)行約束，沒有對(duì)車輛進(jìn)行動(dòng)力學(xué)約束。

本文針對(duì)以上路徑跟蹤研究中存在的問題，以車輛動(dòng)力學(xué)模型作為預(yù)測(cè)模型，充分發(fā)揮模型預(yù)測(cè)控制解決多約束問題的優(yōu)點(diǎn)，通過改進(jìn)優(yōu)化目標(biāo)函數(shù)減少優(yōu)化目標(biāo)的數(shù)量來提高控制算法的實(shí)時(shí)性，通過加入車輪側(cè)偏角的動(dòng)力學(xué)約束來提高智能車跟蹤目標(biāo)路徑時(shí)的平穩(wěn)性和安全性。

2 車輛動(dòng)力學(xué)模型及其線性化

2.1 車輛動(dòng)力學(xué)模型

本文用車輛四輪模型來分析車輛在運(yùn)動(dòng)過程中的受力情況[7]，為簡化計(jì)算，假設(shè)車輛在水平路面上行駛，不考慮橫向和縱向空氣阻力。滿足以上條件的車輛動(dòng)力學(xué)模型如圖1所示，其中，oxy為車體坐標(biāo)系，規(guī)定x軸正向?yàn)檐囶^方向，y軸為車體橫向，OXY為大地坐標(biāo)系。

圖1 車輛動(dòng)力學(xué)模型

根據(jù)牛頓第二定律，分別得到沿x軸、y軸和繞z軸的受力平衡方程：

式中，a、b分別為質(zhì)心到前、后軸的距離；m為車輛的質(zhì)量；Iz為車輛繞z軸的轉(zhuǎn)動(dòng)慣量；φ為車輛航向角；Fxf、Fxr為車輛前、后車輪受到的x方向的力；Fyf、Fyr為車輛前、后車輪受到的y方向的力。

經(jīng)過簡化整理，可以得到較小前輪偏角假設(shè)下的車輛動(dòng)力學(xué)非線性模型：

式中，Ccf、Ccr分別為車輛前、后輪側(cè)偏剛度；Clf、Clr分別為車輛前、后輪輪胎縱向剛度；δf為前輪偏角；sf、sr分別為前、后輪滑移率；X、Y分別為慣性坐標(biāo)系坐標(biāo)下車輛坐標(biāo)。

2.2 模型線性化

由于智能車在高速行駛過程中對(duì)車輛運(yùn)動(dòng)控制器實(shí)時(shí)性要求很高，非線性模型預(yù)測(cè)控制難以滿足，因此，在參考點(diǎn)進(jìn)行泰勒展開并只保留一階項(xiàng)，將車輛的動(dòng)力學(xué)模型線性化。

2.3 狀態(tài)空間方程轉(zhuǎn)換

線性化得到的空間狀態(tài)方程中，udyn（t）是車輛的控制量，以這種形式的空間狀態(tài)方程得到的預(yù)測(cè)方程并不能對(duì)車輛控制量的增量進(jìn)行有效的約束，因此還需要對(duì)上一節(jié)線性化后的空間狀態(tài)方程進(jìn)行進(jìn)一步轉(zhuǎn)換。

本文取狀態(tài)變量

結(jié)合式（5）和式（6），可以得到新的狀態(tài)空間方程：

3 模型預(yù)測(cè)控制器

模型預(yù)測(cè)控制器作為車輛路徑跟蹤的決策機(jī)構(gòu)，通過狀態(tài)采集器獲取上一時(shí)刻車輛狀態(tài)，通過預(yù)測(cè)模型對(duì)車輛預(yù)測(cè)時(shí)域內(nèi)的狀態(tài)進(jìn)行預(yù)測(cè)，構(gòu)建二次型目標(biāo)函數(shù)，并結(jié)合約束條件，優(yōu)化求解得到控制時(shí)域內(nèi)各個(gè)周期的控制量，最后將控制序列的第一個(gè)元素作用于車輛，在下一個(gè)采樣時(shí)刻重新獲取車輛的狀態(tài)，繼續(xù)進(jìn)行下一輪的滾動(dòng)優(yōu)化[8]。智能車模型預(yù)測(cè)控制原理如圖2所示。

圖2 模型預(yù)測(cè)控制原理

3.1 目標(biāo)函數(shù)設(shè)計(jì)

在智能車的目標(biāo)路徑跟蹤中，目標(biāo)函數(shù)的作用是使其準(zhǔn)確、快速地跟隨目標(biāo)路徑。因此，目標(biāo)函數(shù)必須包含系統(tǒng)的狀態(tài)量和控制量。參考文獻(xiàn)[4]，目標(biāo)函數(shù)為：

式中，Xdyn為狀態(tài)量；Q為狀態(tài)加權(quán)矩陣；u為輸入控制量；R為控制加權(quán)矩陣；P為預(yù)測(cè)時(shí)域。

該形式的目標(biāo)函數(shù)使用系統(tǒng)狀態(tài)量和控制量進(jìn)行優(yōu)化求解，當(dāng)系統(tǒng)使用動(dòng)力學(xué)空間狀態(tài)模型時(shí)，由于約束量較多，會(huì)導(dǎo)致系統(tǒng)計(jì)算時(shí)間成本過高，而直接使用系統(tǒng)控制量來進(jìn)行約束也會(huì)導(dǎo)致無法對(duì)控制增量進(jìn)行有效約束[9]。因此，本文采用的模型預(yù)測(cè)控制器目標(biāo)函數(shù)為：

式中，ηdyn為輸出量；ηdyn,ref為輸出量參考量；Δudyn為控制偏差增量；ρ為松弛因子權(quán)重系數(shù)；N為控制時(shí)域。

由于使用的是車輛動(dòng)力學(xué)模型，并且增加了約束條件的數(shù)量，為了避免無最優(yōu)解情況的出現(xiàn)，為目標(biāo)函數(shù)添加了松弛因子ε[10]。

結(jié)合式（7）和式（9），消除系統(tǒng)狀態(tài)量ξ（k,t）可得：

3.2 條件約束

模型預(yù)測(cè)控制最突出的特點(diǎn)就是能很方便地處理多約束問題[11]，為了保證智能車平穩(wěn)地跟蹤目標(biāo)路徑，本文采用前輪偏角約束、前輪偏角增量約束以及輪胎側(cè)偏角動(dòng)力學(xué)約束。

3.2.1 前輪偏角及其增量約束

對(duì)于車輛的前輪偏角和前輪偏角增量的約束，可以根據(jù)車輛的實(shí)際物理參數(shù)進(jìn)行設(shè)置?？刂屏考s束表達(dá)式為：

控制增量約束的表達(dá)式為：

在目標(biāo)函數(shù)以及約束條件中，被優(yōu)化變量為控制時(shí)域內(nèi)的控制增量，因此，控制量要首先轉(zhuǎn)化為Δu的矩陣形式。

由控制增量和控制量的關(guān)系可得：

將式（11）轉(zhuǎn)化為Umin≤A?U+Ut≤Umax的形式，其中，

控制量和控制增量的約束能夠保證模型預(yù)測(cè)控制器生成的控制輸出是物理可實(shí)現(xiàn)的，但為了保證智能車在道路上的平穩(wěn)、安全行駛，還需要增加車輛的動(dòng)力學(xué)約束。

3.2.2 輪胎側(cè)偏角動(dòng)力學(xué)約束

智能車在行駛時(shí)，可能因路面濕滑或者轉(zhuǎn)彎過急而導(dǎo)致車輛發(fā)生側(cè)滑，進(jìn)而引發(fā)各種事故。因此，增加車輛動(dòng)力學(xué)約束、降低車輛發(fā)生側(cè)滑的可能性尤為重要。

[12]可知，車輛的側(cè)滑和輪胎側(cè)偏角密切相關(guān)。當(dāng)車輛在水平道路直線行駛時(shí)，輪胎側(cè)偏角α=0；當(dāng)輪胎受到橫向的力發(fā)生彈性形變而未發(fā)生側(cè)滑時(shí)，α≤αmax；當(dāng)輪胎受到橫向的力過大而導(dǎo)致車輛側(cè)滑時(shí)，α＞αmax。由此可以得出，車輛輪胎的側(cè)偏角直接反映了車輛是否發(fā)生側(cè)滑，限制了輪胎側(cè)偏角就限制了側(cè)滑的發(fā)生。

而由于建立的車輛動(dòng)力學(xué)空間方程并沒有將輪胎側(cè)偏角作為狀態(tài)量，無法對(duì)輪胎側(cè)偏角進(jìn)行直接約束，所以本文需要尋找輪胎側(cè)偏角與狀態(tài)量ξ（k,t）之間的關(guān)系，通過對(duì)狀態(tài)量施加特定關(guān)系的約束來達(dá)到約束輪胎側(cè)偏角的目的。

由車輛動(dòng)力學(xué)模型中各速度之間的關(guān)系可得輪胎側(cè)偏角為：

式中，vc和vl分別為輪胎的橫向和縱向速度。

vc和vl又可以通過車輛坐標(biāo)系的速度vx和vy來表示：

但由于vx和vy仍然不是系統(tǒng)的狀態(tài)量，因此還要經(jīng)過轉(zhuǎn)換：

式中，vxf、vxr、vyf、vyr分別為車身坐標(biāo)系下前、后輪x方向速度以及前、后輪y方向速度。

對(duì)式（14）～式（16）進(jìn)行簡化和合并，可得輪胎前、后輪側(cè)偏角為：

以ξdyn為狀態(tài)量，以u(píng)dyn為控制量，對(duì)上式進(jìn)行線性化，可得：

式中，α=[αfαr]T為輪胎側(cè)偏角矩陣；F=[-1 0]T為直接傳遞矩陣；為輸出矩陣。

對(duì)式（18）進(jìn)行空間轉(zhuǎn)換，可得：

綜合以上目標(biāo)函數(shù)和約束條件，基于動(dòng)力學(xué)模型預(yù)測(cè)控制器的優(yōu)化問題可描述為：

對(duì)于式（20）的優(yōu)化問題，可以轉(zhuǎn)換為二次規(guī)劃的問題[13]，由于優(yōu)化問題的約束條件都是不等式約束，因此二次規(guī)劃的問題采用有效集方法便可以得到優(yōu)化問題的最終可行解。

4 仿真驗(yàn)證

為了測(cè)試改進(jìn)模型預(yù)測(cè)控制的智能汽車路徑跟蹤方法的效果，在以CarSim與MATLAB/Simulink構(gòu)建的聯(lián)合仿真平臺(tái)上進(jìn)行了仿真驗(yàn)證。

4.1 仿真平臺(tái)設(shè)計(jì)

該仿真平臺(tái)由CarSim提供車輛動(dòng)力學(xué)模塊，通過MATLAB語言編寫S函數(shù)作為系統(tǒng)控制器，在Simulink中進(jìn)行整體結(jié)構(gòu)的搭建。仿真平臺(tái)系統(tǒng)結(jié)構(gòu)如圖3所示。

圖3 仿真系統(tǒng)結(jié)構(gòu)

本文CarSim車輛動(dòng)力學(xué)模型以北京工業(yè)大學(xué)BJUT-IV無人駕駛車輛平臺(tái)為原型。通過測(cè)量，該車整車質(zhì)量為680 kg；質(zhì)心與前、后軸的距離分別為1 250 mm和1 000 mm；質(zhì)心高度為540 mm；輪距為1 230 mm；滾動(dòng)阻力系數(shù)為0.015；空氣阻力系數(shù)為0.8（電動(dòng)觀光車外形）。

為有效檢驗(yàn)控制算法的動(dòng)力學(xué)性能，跟蹤路徑選擇雙移線曲線，雙移線曲線測(cè)試是國際標(biāo)準(zhǔn)的車輛穩(wěn)定性能的測(cè)試項(xiàng)目，也被稱為動(dòng)態(tài)翻滾測(cè)試，是包括轉(zhuǎn)向角快速逆轉(zhuǎn)的激烈轉(zhuǎn)向路線測(cè)試，以雙移線作為測(cè)試跟蹤路徑，能更好地檢測(cè)控制算法的穩(wěn)定性[14]。雙移線曲線如圖4所示。

圖4 雙移線曲線

4.2 仿真方法及結(jié)果

4.2.1 有、無輪胎測(cè)偏角約束對(duì)比驗(yàn)證

為了驗(yàn)證改進(jìn)模型預(yù)測(cè)控制的智能車路徑跟蹤方法在濕滑道路狀況下的控制效果，對(duì)有輪胎側(cè)偏角約束和無輪胎側(cè)偏角約束的兩種控制方法進(jìn)行了3組對(duì)比仿真。

3組對(duì)比仿真均以道路附著系數(shù)為變量，在車速（15 m/s）、參考路線（雙移線）等條件都相同的情況下，探究有車輪側(cè)偏角約束控制器和無車輪側(cè)偏角約束控制器時(shí)的行駛效果。

由文獻(xiàn)[6]可知，車輛正常行駛的干燥道路附著系數(shù)的取值范圍為0.7～1.0，濕滑道路的附著系數(shù)約為0.4，因此在仿真中，為符合道路實(shí)際情況，設(shè)置道路附著系數(shù)范圍為0.3≤μ≤0.8。

分別設(shè)置路面附著系數(shù)為0.8、0.5、0.3，添加和未添加輪胎側(cè)偏角約束的仿真結(jié)果如圖5所示。

由圖5可知：當(dāng)?shù)缆犯街禂?shù)為0.8和0.5時(shí)，由于道路附著條件相對(duì)較好，車輛均能夠很好地跟隨參考軌跡，車輛的前輪側(cè)偏角小于0.15°，有、無車輪側(cè)偏角約束曲線基本重合。

當(dāng)?shù)缆犯街禂?shù)為0.3時(shí)，道路附著條件已相對(duì)較差。由圖5a中μ=0.3無車輪側(cè)偏角約束的位移曲線可知，試驗(yàn)車輛在x=50 m附近轉(zhuǎn)過第1個(gè)急彎之后，由于道路較為光滑，出現(xiàn)了比較嚴(yán)重的側(cè)滑現(xiàn)象，而有車輪側(cè)偏角約束的位移曲線相對(duì)更好地跟蹤了目標(biāo)軌跡。在x=100 m附近，未進(jìn)行輪胎側(cè)偏角約束的車輛由于路徑偏差過大，逐漸失去了路徑跟蹤能力，發(fā)生車道偏離，而有輪胎側(cè)偏角約束的車輛通過對(duì)車輛的控制，目標(biāo)路徑跟蹤偏差在x=175 m附近減小為零。由圖5b中μ=0.3對(duì)比曲線可知，在約第6 000個(gè)控制周期時(shí)，無輪胎側(cè)偏角約束的車輛的由于轉(zhuǎn)彎不當(dāng)，導(dǎo)致車身在濕滑的道路上發(fā)生旋轉(zhuǎn)，側(cè)偏角偏差不斷增大，車輛失去控制能力，而有輪胎側(cè)偏角約束的車輛在車頭左右擺動(dòng)之后，航向角偏差逐漸減小為零。由圖5c中μ=0.3的對(duì)比曲線可知，在開始階段，無輪胎側(cè)偏角約束的車輛側(cè)滑程度較有輪胎側(cè)偏角約束的車輛大，車輛仍處于可控階段，而在后半段，開始發(fā)生嚴(yán)重的側(cè)滑，處于不可控狀態(tài)。有輪胎側(cè)偏角約束的車輛在急速轉(zhuǎn)向時(shí)發(fā)生了側(cè)滑，但側(cè)偏角度始終未超過0.5°，隨后進(jìn)入平穩(wěn)路段，側(cè)偏角度逐漸減小為零。

圖5 仿真結(jié)果

4.2.2 車輛速度對(duì)比仿真

為了檢驗(yàn)基于改進(jìn)模型預(yù)測(cè)控制的智能汽車路徑跟蹤方法對(duì)于車速的魯棒性以及控制算法的實(shí)時(shí)性，采用接近正常行駛路面的附著系數(shù)為0.8的道路，在其它條件都相同的條件下，使車輛分別以10 m/s、20 m/s、25 m/s的速度在雙移線道路行駛，觀察車輛的跟蹤性能以及控制算法消耗的時(shí)間。

不同車速下車輛的行駛軌跡如圖6所示。由圖6可以看出，車輛在10 m/s、20 m/s車速工況下可以較好地跟隨目標(biāo)路徑，當(dāng)行駛速度為25 m/s時(shí)，雖然出現(xiàn)較大的側(cè)滑導(dǎo)致跟蹤出現(xiàn)偏差，但車輛仍處于可控狀態(tài)。

圖6 車輛軌跡

不同車速下控制算法消耗的時(shí)間如圖7所示。由圖7可知，不同車速條件沒有很大程度影響控制算法耗時(shí)間。除1個(gè)特殊控制周期外，控制算法消耗的時(shí)間大部分在13～25 ms范圍內(nèi)。

圖7 計(jì)算時(shí)間

5 結(jié)束語

本文提出的智能汽車路徑跟蹤方法不僅在道路附著系數(shù)較高時(shí)能夠有效降低輪胎側(cè)偏角，在道路附著系數(shù)較低時(shí)有效減輕車輛側(cè)滑程度，降低智能汽車目標(biāo)路徑跟蹤時(shí)因跟蹤偏差過大導(dǎo)致失控情況發(fā)生的幾率，而且在不同車速下都有較好的跟蹤效果，具有速度的魯棒性，同時(shí)具有良好的實(shí)時(shí)性。

下一步將繼續(xù)改進(jìn)模型預(yù)測(cè)控制的目標(biāo)函數(shù)，簡化算法的復(fù)雜度，并引入對(duì)解算時(shí)間的監(jiān)控，避免出現(xiàn)個(gè)別周期過長的情況；同時(shí)將考慮引入乘客舒適度約束，提高智能車路徑跟蹤的舒適性和實(shí)用性。

參考文獻(xiàn)

1 Jonathan B,Tully F,Jim K,et al.Little Ben:The Ben Franklin Racing Team's Entry in the 2007 DARPA Urban Challenge.Journal of Field Robotics，2008，25（9）：598～614.

2 Urmson C,Ragusa C,Ray D,et al.A Robust Approach to High-Speed Navigation for Unre-Hearsed Desert Terrain.Journal of Field Robotics，2006，23（8）：467～508.

3 Akeb H,Hifi M.Solving the Circular Open Dimension Problem by Using Separate Beams and Look-Ahead Strategies.Computers and Operations Research，2013，40（5）：1243～1255.

4 Kühne F,Fetter W,Jo?o L,et al.Model Predictive Control of a Mobile Robot Using Lineariza-tion.Prodeedings of Mechatronics and Robototic，2004（4）：525～530.

5 孫銀鍵.基于模型預(yù)測(cè)控制的無人駕駛車輛軌跡跟蹤控制算法研究：[學(xué)位論文].北京：北京理工大學(xué)，2015.

6 龔建偉，姜巖，徐威.無人駕駛車輛模型預(yù)測(cè)控制.北京：北京理工大學(xué)出版社，2014.

7 劉洋.基于模型預(yù)測(cè)控制的移動(dòng)機(jī)器人路徑跟蹤控制：[學(xué)位論文].長春：吉林大學(xué)，2016.

8 Gong J,Xu W,Jiang Y.Multi-constrained Model Predictive Control for Autonomous Ground Vehicle Trajectory Tracking.Journal of Bering Institute of Technology，2014，24（4）：441～448.

9 席裕庚，李德偉，林姝.模型預(yù)測(cè)控制現(xiàn)狀與挑戰(zhàn).自動(dòng)化學(xué)報(bào)，2013，39（3）：222～236.

10 李升波，王建強(qiáng)，李克強(qiáng).軟約束線性模型預(yù)測(cè)控制系統(tǒng)的穩(wěn)定性方法.清華大學(xué)學(xué)報(bào)，2010，50（11）：1848～1852.

11 陳虹.模型預(yù)測(cè)控制.北京：科學(xué)出版社，2013.

12 付聰.輪胎側(cè)傾側(cè)偏極限工況力學(xué)特性研究：[學(xué)位論文].長春：吉林大學(xué)，2013.

13 周晶晶，徐友春.基于改進(jìn)的QP算法和MPC理論的智能車縱向速度控制方法.軍事交通學(xué)院學(xué)報(bào).2016，18（10）：49～53.

14 Falcone P.Nonlinear model Predictive Control for Autono?mous Vehicles：[dissertation].Benevento：Universitadel Sannio，2007.

（責(zé)任編輯 斛 畔）

修改稿收到日期為2017年5月9日。

Research on Target Path Tracking Method of Intelligent Vehicle Based on Model Predictive Control

Duan Jianmin,Tian Xiaosheng,Xia Tian,Song Zhixue
（Beijing University of Technology,Beijing 100022）

To avoid the occurrence of vehicles’side-slip and improve the performance of automatic drive during target path tracking,an improved intelligent vehicle target path tracking method was proposed.In this proposed method,the vehicle dynamics model was used as the predictive control model,the objective function was improved and the dynamic constraint of the tire slip angle was added.This proposed method was validated on the co-simulation platform constructed by CarSim and MATLAB/Simulink.The results show that the proposed method can reduce the probability of side-slip during target path tracking,and improve the tracking accuracy and safety of the intelligent vehicle target path tracking.

Intelligent vehicle,Target path tracking,CarSim/Simulink co-simulation,Model predictive control

智能汽車 目標(biāo)路徑跟蹤 CarSim/Simulink聯(lián)合仿真 模型預(yù)測(cè)控制

U467.1 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1000-3703（2017）08-0006-06

北京市屬高等學(xué)校人才強(qiáng)教計(jì)劃資助項(xiàng)目（038000543117004）。