李培新 姜小燕 魏燕定 周曉軍

(1.浙江大學(xué)浙江省先進(jìn)制造技術(shù)重點實驗室，杭州 310027； 2.中國航天科技集團公司第一研究院，北京 100076)

基于跟蹤誤差模型的無人駕駛車輛預(yù)測控制方法

李培新1姜小燕2魏燕定1周曉軍1

(1.浙江大學(xué)浙江省先進(jìn)制造技術(shù)重點實驗室，杭州 310027； 2.中國航天科技集團公司第一研究院，北京 100076)

針對無人駕駛車輛的軌跡跟蹤問題，在分析車輛運動學(xué)模型的基礎(chǔ)上，設(shè)計了一種基于模型預(yù)測控制理論的軌跡跟蹤控制方法。首先，將車輛運動學(xué)模型進(jìn)行線性化處理，得到車輛運動學(xué)線性跟蹤誤差模型，該模型可以用來預(yù)測車輛的未來行為。其次，利用此跟蹤誤差模型作為預(yù)測模型，應(yīng)用線性模型預(yù)測控制方法，通過優(yōu)化得到使性能指標(biāo)最小的控制序列，將控制序列的第一步作用于系統(tǒng)。最后，建立了3種典型的道路試驗曲線，并且在基于實時多體動力學(xué)軟件Vortex搭建的虛擬仿真平臺中對軌跡跟蹤控制器進(jìn)行了仿真。仿真結(jié)果表明，該控制器可以保證無人駕駛車輛快速且穩(wěn)定地跟蹤參考軌跡，距離偏差和方位偏差都在合理的范圍內(nèi)，且實時性可以達(dá)到要求。

無人駕駛車輛； 跟蹤誤差； 預(yù)測控制

引言

車輛的導(dǎo)航控制技術(shù)是實現(xiàn)無人駕駛車輛的基礎(chǔ)，而其中軌跡跟蹤問題是最基本的問題。軌跡跟蹤問題一直是車輛控制中的難點，近年來已經(jīng)有不少學(xué)者進(jìn)行了研究。文獻(xiàn)[1]采用PID控制方法設(shè)計了控制器，它具有較好的跟蹤能力，但是缺乏對復(fù)雜工況的適應(yīng)性。文獻(xiàn)[2]通過車載傳感器實時獲取行駛軌跡相對于最優(yōu)路徑軌跡的偏差信息，采用可變參數(shù)的PID控制，實現(xiàn)鏟運車的無人駕駛。文獻(xiàn)[3]采用了預(yù)瞄方法對車輛軌跡跟蹤問題進(jìn)行了研究，仿真結(jié)果表明它對大的偏差以及不連續(xù)的軌跡有較強的魯棒性，但是預(yù)瞄距離的選取比較麻煩。文獻(xiàn)[4-6]針對車輛軌跡跟蹤問題設(shè)計了二次型最優(yōu)控制器，仿真結(jié)果表明，在曲率變化大的軌跡中表現(xiàn)較差，降低了跟蹤過程中的全局穩(wěn)定性。文獻(xiàn)[7]基于針對車輛換道及超車的工況，利用BackStepping控制算法選取Lyapunov函數(shù)設(shè)計了軌跡跟蹤控制器。近年來，模型預(yù)測控制理論[8-11]引起了人們的關(guān)注。模型預(yù)測控制汲取了優(yōu)化控制的思想，利用滾動的有限時段優(yōu)化取代了一成不變的全局優(yōu)化。文獻(xiàn)[12]設(shè)計了一種應(yīng)用于兩輪移動機器人的非線性模型預(yù)測控制算法，但由于計算量太大，導(dǎo)致其實時性較差，對偏差的收斂速度慢，應(yīng)用范圍有限。為了提高計算實時性，文獻(xiàn)[13-14]利用兩輪機器人的運動學(xué)模型作為預(yù)測模型，文獻(xiàn)[15-17]利用車輛的運動學(xué)或者動力學(xué)模型作為預(yù)測模型，對這些模型進(jìn)行了線性化處理，應(yīng)用線性模型預(yù)測控制方法，通過適當(dāng)?shù)奶幚韺⑵滢D(zhuǎn)換為二次規(guī)劃(QP)問題，可以提高計算速度。

本文采用車輛運動學(xué)的跟蹤誤差模型作為預(yù)測模型設(shè)計模型預(yù)測控制器，在基于實時多體動力學(xué)軟件Vortex搭建的無人駕駛車輛虛擬仿真平臺中對所設(shè)計的軌跡跟蹤控制器進(jìn)行仿真。

1 基于運動學(xué)的數(shù)學(xué)模型

1.1 車輛運動學(xué)模型

在笛卡爾世界坐標(biāo)系中的二維平面上定義一個車輛模型，如圖1所示，為了簡化控制器的設(shè)計，本文假設(shè)車輪與地面為點接觸，并且接觸點只有純滾動而不發(fā)生相對滑動，這種理想約束本質(zhì)上是一種非完整約束。

圖1 車輛廣義坐標(biāo)示意圖Fig.1 Generalized coordinates of vehicle

假設(shè)車輛僅在平面上作運動，前后輪的非完整約束方程為

(1)

(2)

式中x——車輛后輪中心橫坐標(biāo)

y——車輛后輪中心縱坐標(biāo)

xf——車輛前輪中心橫坐標(biāo)

yf——車輛前輪中心縱坐標(biāo)

θ——車輛橫擺角

δ——車輛前輪轉(zhuǎn)向角

許多類型的輪式移動機器人運動學(xué)模型都可以轉(zhuǎn)換為Unicycle模型，無人駕駛車輛是一種典型的輪式移動機器人，以車輛后輪中心為參考點，可將車輛運動學(xué)模型寫為

(3)

式中l(wèi)——前輪中心和后輪中心的距離

v——車輛后輪中心的速度

車輛輸入變量定義為u=[vδ]T，車輛當(dāng)前坐標(biāo)定義為x=[xyθ]T。

式(3)也可寫成

(4)

1.2 車輛運動學(xué)誤差模型

一般而言，在軌跡跟蹤問題中，一般采用跟蹤參考車輛的方法，參考軌跡假定由一個虛擬的參考車輛生成，用r表示參考量，因此車輛的參考軌跡可以寫成

(5)

其中

xr=[xryrθr]Tur=[vrδr]T

將式(4)的右邊圍繞點(xr,ur)進(jìn)行泰勒展開，除去高階部分，可得

也可以寫成

(6)

式中fx,r——f相對于x的雅可比矩陣

fu,r——f相對于u的雅可比矩陣

結(jié)合式(6)與式(5)，可得車輛運動學(xué)誤差模型

(7)

為了設(shè)計控制器，應(yīng)用歐拉方法將式(7)進(jìn)行離散化，得到車輛運動學(xué)的離散線性時變模型為

(8)

其中

式中k——采樣時間T——采樣周期

2 車輛軌跡跟蹤控制器設(shè)計

2.1 車輛軌跡跟蹤問題描述

無人駕駛車輛的軌跡跟蹤問題是指在慣性坐標(biāo)系中，根據(jù)某種控制理論，為車輛設(shè)計一個控制器，確定車輛的控制輸入，使得車輛從給定的初始狀態(tài)出發(fā)，到達(dá)并跟隨給定的參考軌跡。車輛軌跡跟蹤示意圖如圖2所示，參考軌跡由一個虛擬的參考車輛生成，實際車輛在跟蹤參考軌跡的時會有相應(yīng)的誤差ex和ey，軌跡跟蹤控制算法設(shè)計的目的就是使得實際車輛能夠精確地跟隨參考軌跡，消除跟蹤誤差。

圖2 車輛軌跡跟蹤示意圖Fig.2 Sketch of virtual simulation platform

模型預(yù)測控制是一種優(yōu)化控制算法，它的基本思想是通過在每一采樣時刻k求取某性能指標(biāo)的最優(yōu)來確定未來的控制輸入，它與最優(yōu)控制的最大區(qū)別在于每一時刻都要在線求取目標(biāo)函數(shù)的最小值，以此得到最優(yōu)的控制輸入，所以它具有良好的全局適應(yīng)性。

由于本文將車輛運動學(xué)的跟蹤誤差模型作為預(yù)測模型，因此在車輛軌跡跟蹤控制中，將k時刻的優(yōu)化性能指標(biāo)定義為

(9)

Q——誤差權(quán)矩陣，Q∈Rn×Rn

R——控制權(quán)矩陣，R∈Rm×Rm

h——預(yù)測時域

圖3 車輛軌跡跟蹤控制原理圖Fig.3 Schematic of vehicle trajectory tracking control

2.2 軌跡跟蹤控制算法設(shè)計

定義整個預(yù)測時域h內(nèi)的車輛的軌跡跟蹤誤差和預(yù)測時域h內(nèi)的控制變量誤差為

根據(jù)1.2小節(jié)的離散線性時變車輛運動學(xué)模型，可以推導(dǎo)出h時刻的預(yù)測輸出為

(10)

(11)

其中

Λ(k,i)定義為

由于模型預(yù)測控制的目標(biāo)是使得無人駕駛車輛與參考車輛的軌跡跟蹤誤差越來越小，因此可以定義車輛的參考軌跡跟蹤誤差模型為

(12)

可以定義矩陣Ar的值使得在預(yù)測時域h內(nèi)，車輛的參考軌跡跟蹤誤差越來越小。定義整個預(yù)測時域h內(nèi)的車輛參考軌跡跟蹤誤差為

(13)

根據(jù)推導(dǎo)，性能指標(biāo)式(9)也可寫成

(14)

通過求取該性能指標(biāo)的最小值可得輸入變量為

(15)

其中

(Q∈Rnh×Rnh，R∈Rmh×Rmh)

3 仿真分析

3.1 虛擬仿真平臺介紹

為了有效地驗證本文提出的控制算法，本文的仿真在實驗室搭建的虛擬仿真平臺中進(jìn)行，虛擬仿真平臺示意圖如圖4所示。其中動力學(xué)解算模塊采用實時多體動力學(xué)軟件Vortex。Vortex是用于車輛、機械、機器人等多體系統(tǒng)動力學(xué)建模的實時交互式動力學(xué)仿真引擎，其采用了碰撞檢測算法和動力學(xué)計算方法，強調(diào)準(zhǔn)確性與實時性并重。

為了保證仿真過程的實時性，采用2臺計算機分別進(jìn)行動力學(xué)解算和視景渲染，Vortex只負(fù)責(zé)動力學(xué)解算，Vega Prime進(jìn)行視景圖像渲染，利用高層體系結(jié)構(gòu)HLA實現(xiàn)Vortex和Vega Prime仿真例程的同步和數(shù)據(jù)交互。整個仿真過程通過設(shè)定管理者聯(lián)邦成員，并通過高精度的軟件定時器實現(xiàn)仿真步長的推進(jìn)控制，這樣可以保證仿真過程的同步性[18]。算法中包含了矩陣的相關(guān)計算，由于整個仿真環(huán)境是建立在Visual C++基礎(chǔ)上，故需要采用實時矩陣計算工具，本文采用了開源矩陣計算工具包Eigen進(jìn)行矩陣的實時計算。

圖4 虛擬仿真平臺示意圖Fig.4 Sketches of virtual simulation platform

3.2 參考軌跡生成

軌跡生成的方法有很多，由于車輛是非完整系統(tǒng)，需要生成曲率連續(xù)的軌跡。

KANAYAMA等[19]采用回旋曲線的方法生成曲率連續(xù)的軌跡，其缺點是缺少車輛坐標(biāo)的閉式表達(dá)式。本文采用PINCHARD等[20]推導(dǎo)的車輛軌跡生成方法，示意圖如圖5所示，該方法根據(jù)車輛的位置、速度及曲率等約束條件，推導(dǎo)軌跡的五次多項式，它的一般形式是

圖5 軌跡生成示意圖Fig.5 Path generation

(16)

(17)

其中

式中k——曲率r——極半徑

φ——極角ai——系數(shù)，i=0,1,2,…,5

該軌跡起點約束條件為φ=0，r=R1，r′=0，k=0，終點約束條件為φ=Φ，r=R2，r′=0，k=0。Φ為車輛轉(zhuǎn)過的角度，R1為起點半徑，R2為終點半徑，v1為起點速度，v2為終點速度。

根據(jù)以上約束條件，可以得到多項式系數(shù)的一般形式為

(18)

3.3 仿真結(jié)果及分析

為了檢驗控制器對初始偏差的響應(yīng)能力，在仿真過程中取初始橫向偏差為-0.5 m，車速為20 km/h，虛擬仿真平臺聯(lián)合仿真的刷新頻率為30 Hz，前輪轉(zhuǎn)向角為-36°≤δ≤36°，預(yù)測時域為Hp=5，控制時域為Hc=5，加權(quán)矩陣為R=0.05I2×2，Ar=0.5I3×3。

3.3.1換道曲線試驗

車輛換道曲線試驗通常用來測試車輛的操縱性能，它能檢驗車輛的直線跟蹤性能以及對位置及曲率變化的快速響應(yīng)能力。車輛換道試驗曲線如圖6所示。

圖6 車輛換道試驗曲線Fig.6 Test curve of vehicle lane change

車輛在換道曲線的軌跡跟蹤仿真結(jié)果如圖7所示，由圖7可以看出，車輛在跟蹤換道曲線時，該控制器克服了初始偏差的影響，具有較快的收斂速度，車輛運行穩(wěn)定后距離偏差在-0.1～0.1 m的范圍內(nèi)，方位偏差在-1°～1°的范圍內(nèi)。曲線試驗的誤差都在合理的范圍內(nèi)，表明該控制器可以準(zhǔn)確穩(wěn)定地跟蹤直線和曲線，且對曲率變化有較快的響應(yīng)能力。

3.3.2“8”字形曲線試驗

“8”字形曲線在日常駕駛中并不常見，但它可以提供對車輛操控情況的直觀感受，其在2個圓弧連接處的曲率是不連續(xù)的，所以它還可以用來檢驗軌跡跟蹤控制算法的穩(wěn)定性?！?”字形試驗曲線如圖8所示。

圖7 換道曲線試驗仿真結(jié)果Fig.7 Simulation results of lane change curve

圖8 “8”字形試驗曲線Fig.8 Eight-shaped test curve

車輛在“8”字形曲線軌跡跟蹤仿真結(jié)果如圖9所示，由圖9可以看出，車輛在跟蹤“8”字形曲線時，控制器克服了初始橫向偏差的影響，具有較快的收斂速度，車輛運行穩(wěn)定后距離偏差雖然有一定的穩(wěn)態(tài)誤差，但是距離偏差控制在-0.1～0.1 m的范圍內(nèi)，且距離偏差曲線變化比較柔和，穩(wěn)態(tài)誤差越來越小，方位偏差控制在-1°～1°的范圍內(nèi)。曲線試驗的誤差都在合理的范圍內(nèi)，表明該控制器能夠快速準(zhǔn)確地跟蹤預(yù)定曲線，而且可以保證運行過程中的平穩(wěn)性。

3.3.3道路曲線試驗

為了更好地檢驗所設(shè)計的車輛軌跡跟蹤控制器在復(fù)雜路況下的跟蹤效果，設(shè)計了道路試驗曲線，如圖10所示，道路試驗曲線由一系列真實的駕駛場景組合而成，它可以用來測試控制算法在真實路面的性能。

圖9 “8”字形曲線試驗仿真結(jié)果Fig.9 Simulation results of eight-shaped curves test

圖10 道路試驗曲線Fig.10 Road test curve

相比于前面的2種參考曲線，道路曲線的曲率變化更快，車輛跟蹤曲線所處的工況環(huán)境更加惡劣。車輛在道路曲線的軌跡跟蹤仿真結(jié)果如圖11所示，由圖11可以看出，車輛在跟蹤預(yù)定軌跡時，控制器克服了初始偏差的影響，可以快速收斂，穩(wěn)定后道路曲線試驗的距離偏差控制在-0.1～0.1 m的范圍內(nèi)，方位偏差控制在-5°～5°的范圍內(nèi)。曲線試驗的誤差都在合理的范圍內(nèi)，表明該控制器可以快速且穩(wěn)定地跟蹤復(fù)雜的參考軌跡，可以將其應(yīng)用于真實的駕駛場景中。

通過以上3種不同曲線的仿真結(jié)果表明，該控制器克服了初始軌跡跟蹤誤差的影響，能夠準(zhǔn)確快速地跟蹤不同場景下的參考軌跡，跟蹤效果理想，且具有良好的全局穩(wěn)定性，能夠滿足無人駕駛車輛軌跡跟蹤控制的要求。

圖11 道路曲線試驗仿真結(jié)果Fig.11 Simulation results of road curves test

通過分析對比動力學(xué)軟件Vortex和視景顯示軟件Vega Prime的刷新頻率，穩(wěn)定在30 Hz左右，誤差在±0.05 Hz以內(nèi)，控制器達(dá)到了實時性的要求。

4 結(jié)論

(1)將車輛非線性運動學(xué)模型進(jìn)行線性化處理，得到車輛運動學(xué)的線性跟蹤誤差模型，以此模型作為預(yù)測模型，可以將線性模型預(yù)測控制方法應(yīng)用于無人駕駛車輛軌跡跟蹤問題中，大幅減少了運算量，滿足了系統(tǒng)實時性的要求。

(2)在3種典型道路曲線中的仿真結(jié)果表明，該控制器可以使無人駕駛車輛快速且穩(wěn)定地跟蹤參考軌跡，距離與方位偏差都在合理的范圍之內(nèi)，跟蹤效果理想，可滿足無人駕駛車輛的軌跡跟蹤要求。

(3)在基于實時多體動力學(xué)Vortex的虛擬仿真平臺中進(jìn)行了聯(lián)合仿真實驗，仿真結(jié)果表明該控制器的實時性可以得到保證。

1 NETTO M, BLOSSEVILLE J M, LUSETTI B, et al.A new robust control system with optimized use of the lane detection data for vehicle full lateral control under strong curvatures[C]∥IEEE Intelligent Transportation Systems Conference, ITSC’06, IEEE, 2006: 1382-1387.

2 李建國, 戰(zhàn)凱, 石峰, 等.基于最優(yōu)軌跡跟蹤的地下鏟運機無人駕駛技術(shù)[J/OL].農(nóng)業(yè)機械學(xué)報, 2015, 46(12): 323-328.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20151244&journal_id=jcsam.DOI：10.6041/j.issn.1000-1298.2015.12.044.

LI Jianguo, ZHAN Kai, SHI Feng, et al.Auto-driving technology for underground scraper based on optimal trajectory tracking[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015, 46(12): 323-328.(in Chinese)

3 CAMPBELLl S F.Steering control of an autonomous ground vehicle with application to the DARPA urban challenge[D].Cambridge:Massachusetts Institute of Technology, 2007.

4 SNIDER J M.Automatic steering methods for autonomous automobile path tracking[R].Robotics Institute, Pittsburgh, PA, Tech.Rep.CMU-RITR-09-08, 2009.

5 PENG H, TOMIZUKA M.Optimal preview control for vehicle lateral guidance[R].California Partners for Advanced Transit and Highways (PATH), 1991.

6 SHARP R S.Driver steering control and a new perspective on car handling qualities[J].Proc.IMech E,Part C: Journal of Mechanical Engineering Science, 2005, 219(10): 1041-1051.

7 游峰，王榮本,，張榮輝, 等.智能車輛換道與超車軌跡跟蹤控制[J].農(nóng)業(yè)機械學(xué)報, 2008, 39(6): 42-45.

YOU Feng, WANG Rongben, ZHANG Ronghui, et al.Lane changing and overtaking control method for intelligent vehicle based on backstepping algorithm[J].Transactions of the Chinese Society for Agricultural Machinery, 2008, 39(6): 42-45.(in Chinese)

8 RAWLINGS J B.Tutorial overview of model predictive control[J].Control Systems, IEEE, 2000, 20(3): 38-52.

9 席裕庚.預(yù)測控制[M].北京：國防工業(yè)出版社, 1993.

10 MACIEJOWSKI J M.Predictive control: with constraints[M].New York:Pearson Education, 2002.

11 KüNHE F, GOMES J, FETTER W.Mobile robot trajectory tracking using model predictive control[C]∥IEEE Latin-American Robotics Symposium, 2005.

12 LAGES W F, ALVES J A V.Real-time control of a mobile robot using linearized model predictive control[J].IFAC Proceedings Volumes, 2006, 39(16): 968-973.

13 KüNHE F, LAGES W F, DA SILVA JR J M G.Model predictive control of a mobile robot using linearization[C]∥Proceedings of Mechatronics and Robotics, 2004: 525-530.

14 FALCONE P, BORRELLI F, ASGARI J, et al.Predictive active steering control for autonomous vehicle systems[J].IEEE Transactions on Control Systems Technology, 2007, 15(3): 566-580.

15 CARVALHO A, LEFéVRE S, SCHILDBACH G, et al.Automated driving: the role of forecasts and uncertainty—a control perspective[J].European Journal of Control, 2015, 24: 14-32.

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

17 孫銀健.基于模型預(yù)測控制的無人駕駛車輛軌跡跟蹤控制算法研究[D].北京：北京理工大學(xué), 2015.

18 余小勇, 魏燕定, 黃茫茫,等.基于HLA的車輛半實物仿真中精確定時方法[J].浙江大學(xué)學(xué)報:工學(xué)版, 2012, 46(7):1195-1200.

YU Xiaoyong, WEI Yanding, HUANG Mangmang, et al.Precise timing of HLA-based hardware-in-loop simulation for vehicles[J].Journal of Zhejiang University:Engineering Science, 2012, 46(7):1195-1200.(in Chinese)

19 KANAYAMA Y, MIYAKE N.Trajectory generation for mobile robots[C]∥Proceedings of the International Symposium on Robotics Research,1985: 16-23.

20 PINCHARD O, LIEGEOIS A, POUGNET F.Generalized polar polynomials for vehicle path generation with dynamic constraints[C]∥1996 IEEE International Conference on Robotics and Automation, 1996, 1: 915-920.

PredictiveControlMethodofAutonomousVehicleBasedonTracking-errorModel

LI Peixin1JIANG Xiaoyan2WEI Yanding1ZHOU Xiaojun1

(1.KeyLaboratoryofAdvancedManufacturingTechnologyofZhejiangProvince,ZhejiangUniversity,Hangzhou310027,China2.TheFirstInstituteofChinaAerospaceScienceandTechnologyCorporation,Beijing100076,China)

For the trajectory tracking problem of autonomous vehicle, on the basis of analysis of kinematic model of vehicle, a model based predictive control method for autonomous vehicle trajectory tracking was designed.Firstly, a linear error model of vehicle kinematics was obtained by using a successive linearization approach, and it was used to predict the future behavior of the vehicle.Secondly, based on this model, it was possible to get a sequence of optimal control by using the linear MPC method and minimizing the objective function, and the first element of this sequence was applied to the system.Lastly, three typical test trajectories (lane change course, figure eight course and road course) were designed and the tracking controller was tested in the virtual simulation platform.The platform was set up on real-time multi-body dynamics software Vortex and visual rendering software Vega Prime.In order to meet the real-time requirements of the platform, two computers were used for dynamic resolving and visual rendering respectively, and the high level architecture (HLA) was adopted to realize the synchronization and data interaction between Vortex and Vega Prime.Simulation results showed that this controller can track the reference trajectory quickly and stably, the distance error and heading error were in a reasonable range.The refresh rate of Vortex and Vega Prime was stabilized at about 30 Hz, the error was within ±0.05 Hz, indicating that the controller can meet the real-time requirements of the system.

autonomous vehicle; tracking error; predictive control

10.6041/j.issn.1000-1298.2017.10.045

TP242.6； U461.1

A

1000-1298(2017)10-0351-07

2017-01-17

2017-03-06

航天支撐技術(shù)基金項目(E20130116)

李培新(1986—)，男，博士生，主要從事車輛半實物虛擬仿真和無人駕駛車輛仿真研究，E-mail：beyondlpx@163.com

魏燕定(1970—)，男，教授，博士生導(dǎo)師，主要從事機械動力學(xué)和車輛半實物虛擬仿真研究，E-mail：weiyd@zju.edu.cn