秦雪帥,王旭東

重慶工商大學(xué) 機(jī)械工程學(xué)院,重慶 400067

1 引 言

軌跡跟蹤控制作為無人駕駛中的關(guān)鍵技術(shù),其目的是使車輛在保證穩(wěn)定性與舒適性的前提下可以準(zhǔn)確地按照給定的規(guī)劃軌跡運(yùn)行[1]。目前,國內(nèi)外學(xué)者在軌跡跟蹤控制上已經(jīng)做出了大量研究,提出了一系列跟蹤控制方法,主要可以分為兩大類:基于模型預(yù)測的方法和基于幾何追蹤的方法。Wallace在1985年提出了純跟蹤控制算法(Pure Pursuit),純跟蹤控制算法是車輛橫向控制算法中的一個代表,對于外界的環(huán)境變化具有較強(qiáng)的魯棒性。在純跟蹤法中,由于預(yù)瞄點(diǎn)的確定與車輛速度緊密相關(guān),所以在車速有顯著變化時(shí)跟蹤效果會變得很差。模型預(yù)測控制(Model Predictive Control)作為基于模型控制器的代表,由于具備便于添加各種空間狀態(tài)變量的相關(guān)約束,控制精確度高等特點(diǎn)被廣泛應(yīng)用到無人駕駛車輛的跟蹤控制當(dāng)中。Falcone等[2]設(shè)計(jì)了一種基于線性模型的預(yù)測控制算法,使得模型預(yù)測控制的實(shí)時(shí)性得以提升,從而使得軌跡跟蹤控制的精度和穩(wěn)定性得以提高;陳龍等[3]提出一種基于非線性模型預(yù)測控制(NMPC)的縱橫向綜合軌跡跟蹤控制方法,通過NMPC和障礙函數(shù)法(BM)的有效結(jié)合,提高了跟蹤精度,改善了行駛穩(wěn)定性。但由于MPC控制器在計(jì)算上比較復(fù)雜,隨著運(yùn)行狀態(tài)約束、復(fù)雜度的增加,會使得MPC控制器的響應(yīng)速度下降,無法滿足無人駕駛車輛在實(shí)時(shí)性上的要求,這也是導(dǎo)致MPC控制器停留在學(xué)術(shù)研究而沒有廣泛應(yīng)用到實(shí)際工程項(xiàng)目當(dāng)中的主要原因。郭應(yīng)時(shí)等[4]在神經(jīng)工效學(xué)認(rèn)知體系架構(gòu)的基礎(chǔ)上,融合模型預(yù)測控制算法(MPC)以及手臂肌肉模型,建立了一種基于神經(jīng)工效學(xué)的車輛橫向控制模型;張志勇等[5]采用模型預(yù)測原理設(shè)計(jì)了智能駕駛路徑跟蹤控制器,通過數(shù)據(jù)聚類結(jié)果來設(shè)計(jì)控制器的代價(jià)函數(shù)與約束條件;Chen等[6]提出了約束迭代LQR(CILQR),以有效解決具有一般約束形式的非線性系統(tǒng)動力學(xué)最優(yōu)控制問題;Bae等[7]給出了一種可行的基于五階貝塞爾曲線的自動駕駛車輛實(shí)時(shí)路徑規(guī)劃與跟蹤方法,但是整體實(shí)時(shí)性較差;Feng等[8]提出了一種Bezier曲線與矢量場直方圖(VFH)算法相結(jié)合的算法,利用VFH搜索方法搜索無碰撞狀態(tài),以Bezier曲線為參考線選取最優(yōu)軌跡點(diǎn)。此外,常用的跟蹤控制方法還包括線性二次調(diào)節(jié)器法(LQR)、比例-積分-微分控制(PID)法、前輪反饋控制(Stanley)法、滑模控制方法、自適應(yīng)控制方法和模糊控制方法等。

上述的跟蹤控制方法常運(yùn)用在車速固定的場景下,無人駕駛車輛的行駛工況多較為復(fù)雜,車輛的縱向行駛速度處于一個動態(tài)變化的狀態(tài)當(dāng)中,固定車速下的跟蹤會導(dǎo)致應(yīng)用場景受限。同時(shí),無人駕駛汽車屬于常見的非完整性約束系統(tǒng),車輛的橫向控制與縱向行駛速度之間有密不可分的關(guān)系。在進(jìn)行軌跡跟蹤控制過程中,車輛的縱向速度控制效果將直接影響車輛對整個路徑的跟蹤效果。為了豐富無人駕駛車輛的應(yīng)用場景,實(shí)現(xiàn)車輛橫縱向聯(lián)合控制以提升控制精度,本文針對無人駕駛車輛在結(jié)構(gòu)化環(huán)境下的各種工況,設(shè)計(jì)了一種LQR與PID相結(jié)合的橫縱向耦合控制器。LQR法相比于模型預(yù)測控制法(MPC),控制過程中沒有設(shè)置各種空間狀態(tài)變量約束,控制器的計(jì)算量大大降低,使用LQR法對車輛進(jìn)行橫向跟蹤控制可以有效提升無人駕駛系統(tǒng)的實(shí)時(shí)性。另一方面,PID法雖然不能很好地處理多變量問題,但由于其計(jì)算量小,不用得到系統(tǒng)模型,因此在縱向控制過程中可應(yīng)用其對車輛速度及位置誤差進(jìn)行控制調(diào)節(jié),可以在保證跟蹤控制精度的同時(shí),提升系統(tǒng)的實(shí)時(shí)性。另外,本文在規(guī)劃層面采用多項(xiàng)式曲線進(jìn)行規(guī)劃器的設(shè)計(jì),以保證所規(guī)劃軌跡滿足車輛位置、速度及加速度的連續(xù)。

2 車輛跟蹤誤差模型的建立

圖1 跟蹤誤差模型

為了得到跟蹤誤差模型,首先需要獲得基于車身坐標(biāo)系下的二自由度車輛動力學(xué)模型,即模型重點(diǎn)關(guān)注車輛沿Y軸的運(yùn)動以及繞Z軸的轉(zhuǎn)動;然后通過向量之間存在的關(guān)系,將其轉(zhuǎn)化成自然坐標(biāo)系下的誤差模型[10],具體如圖2所示。

圖2 二自由度車輛動力學(xué)模型

對車輛進(jìn)行受力分析,根據(jù)系統(tǒng)所受到的力和力矩平衡可以得到:

∑Fy?may=Fyf·cosδf+Fyr

(1)

(2)

當(dāng)輪胎側(cè)偏角α較小時(shí),輪胎的側(cè)向力與輪胎的側(cè)偏角近似呈線性函數(shù)關(guān)系[11],即Fy=Caα。故前后輪胎的側(cè)向力分別為

Fyf=Caf·αf,Fyr=Car·αr

(3)

式(3)中,Caf和Car分別為前后輪胎的側(cè)偏剛度。

由小角度假設(shè)可以近似得到[12]:

(4)

由式(1)—式(4)可以得到:

(5)

(6)

(7)

圖3 車輛動力學(xué)模型轉(zhuǎn)換

其中,k為道路曲率。

(8)

(9)

(10)

(11)

將式(8)—式(11)代入式(5)、式(6)中,經(jīng)過化簡后便可以得到基于自然坐標(biāo)系下的誤差微分方程:

3 規(guī)劃及跟蹤控制模塊設(shè)計(jì)

3.1 多項(xiàng)式曲線規(guī)劃

作為控制的上層模塊,軌跡規(guī)劃是整個無人駕駛車輛系統(tǒng)當(dāng)中不可或缺的重要組成部分,其目的是生成一條可供無人車輛行駛的軌跡,并將軌跡輸送至控制模塊。

多項(xiàng)式曲線多應(yīng)用于要求車輛在起始點(diǎn)以及目標(biāo)點(diǎn)達(dá)到某種期望狀態(tài)的場景當(dāng)中。多項(xiàng)式曲線法是路徑與速度耦合求解算法的一種,即在獲得車輛行駛路徑的同時(shí),速度和加速度也隨之獲得[15]。本文中采用五次多項(xiàng)式曲線進(jìn)行軌跡規(guī)劃以保證生成的軌跡具備位置、速度、加速度連續(xù)的特性。

由于車輛不同于傳統(tǒng)的自由移動機(jī)器人,不能單獨(dú)做橫向運(yùn)動,其橫向運(yùn)動需要由縱向運(yùn)動誘發(fā),所以所規(guī)劃的軌跡需要有切線斜率、曲率、速度以及加速度的要求[16]。因此在軌跡規(guī)劃過程當(dāng)中,需要做相應(yīng)的修改,具體如下:

x(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

y(x)=b0+b1x+b2x2+b3x3+b4x4+b5x5

關(guān)于軌跡的航向角及曲率通過下式可得:

θr(t)=arctan{y′[x(t)]}

至此軌跡規(guī)劃模塊便完成,生成的參考軌跡信息為

3.2 橫向控制

線性二次調(diào)節(jié)器法(LQR)是多目標(biāo)優(yōu)化最優(yōu)控制問題的代表性算法之一,目標(biāo)函數(shù)可以表示為跟蹤過程所產(chǎn)生的跟蹤偏差與控制輸入的累計(jì)加權(quán)之和[17],具體如式(12):

(12)

其中,err為狀態(tài)變量,Q為半正定的狀態(tài)加權(quán)矩陣,R為正定控制加權(quán)矩陣,為簡化計(jì)算兩者通常取為對角陣;Q矩陣元素影響著系統(tǒng)趨向穩(wěn)定的速度,數(shù)值越大,系統(tǒng)相對快速趨向穩(wěn)定;R矩陣元素影響著控制輸入的大小,數(shù)值越大控制輸入會相對減小,但加權(quán)矩陣并非越大越好。因此,第一項(xiàng)優(yōu)化目標(biāo)表示跟蹤過程中累積路徑偏差的大小,第二項(xiàng)優(yōu)化目標(biāo)表示跟蹤控制過程中能量的損耗,這樣就將軌跡跟蹤控制問題轉(zhuǎn)化為一個最優(yōu)控制問題[18],本文中選取加權(quán)矩陣如下:

對于式(12)目標(biāo)函數(shù)的優(yōu)化求解,解出的最優(yōu)控制規(guī)律u是關(guān)于狀態(tài)變量err的線性函數(shù):

uk=-[(R+BTPk+1B)-1BTPk+1A]err(k)=-Kerr(k)

其中,P是式(13)黎卡提方程的解[19]:

Pk-1=Q+ATPk(I+BR-1BPk)-1A

(13)

引入前饋控制之后:

其中,K3是通過LQR反饋控制計(jì)算出的矩陣K=[K1,K2,K3,K4]中的K3。

通過車輛實(shí)際位置與規(guī)劃模塊所得到的軌跡計(jì)算出誤差量err;然后通過迭代的方式求解出黎卡提方程的解P進(jìn)而求解出K,該步驟為求解LQR的關(guān)鍵所在;最后結(jié)合前饋控制求解出系統(tǒng)的控制量u=-K·err+δf,實(shí)現(xiàn)車輛的橫向控制。

3.3 縱向控制

無人駕駛車輛縱向控制是通過控制車輛油門和剎車實(shí)現(xiàn)對車輛加速度的控制,實(shí)現(xiàn)車輛的加速和減速,使得車輛的實(shí)際位置、速度和加速度與規(guī)劃模塊所生成的軌跡一致。在整個控制過程當(dāng)中,本文重點(diǎn)討論車輛的跟蹤效果而不對車輛行駛時(shí)的換擋過程以及發(fā)動機(jī)的能量損耗進(jìn)行分析。因此,本文構(gòu)建了理想的電機(jī)模型,即電機(jī)在達(dá)到最大轉(zhuǎn)速之前以恒定扭矩運(yùn)行,在達(dá)到電機(jī)最大轉(zhuǎn)速之后以恒定功率運(yùn)行。其中,功率P、扭矩M和轉(zhuǎn)速n之間的關(guān)系滿足P=2πn·M。整車的具體參數(shù)配置如表1所示。

表1 車輛參數(shù)表

(14)

對式(7)做進(jìn)一步化簡可以得到,車輛在自然坐標(biāo)系下的縱向速度為

(15)

由于多項(xiàng)式曲線法是路徑與速度耦合求解算法的一種,即在獲得車輛行駛路徑的同時(shí),速度和加速度也隨之獲得。因此所規(guī)劃的速度及加速度信息為

(16)

(17)

通過應(yīng)用式(14)—式(17),本文設(shè)計(jì)了基于位置與速度誤差的PID縱向控制模型,通過PID控制器參數(shù)的設(shè)置實(shí)現(xiàn)車輛在縱向上準(zhǔn)確跟蹤規(guī)劃模塊所生成的參考軌跡[20]。

3.4 橫縱向聯(lián)合控制

對于像無人駕駛車輛這樣的非完整性約束系統(tǒng),其行駛過程中的橫向移動需要依靠車輛縱向移動的耦合觸發(fā)。傳統(tǒng)的控制方法在跟蹤控制過程中,多設(shè)置車輛的縱向行駛速度為定值,然后進(jìn)行軌跡跟蹤,導(dǎo)致應(yīng)用場景較為局限,不符合實(shí)際情況。為了實(shí)現(xiàn)無人駕駛汽車在變速行駛工況下的軌跡跟蹤控制,由軌跡規(guī)劃模塊確定出車輛行駛的速度、路徑參考曲線?？v向控制器根據(jù)車輛實(shí)際速度和位置信息與規(guī)劃模塊所計(jì)算出的軌跡信息之間的誤差,運(yùn)用PID控制策略計(jì)算并輸出期望的速度與位置曲線,縱向控制器所輸出的速度信息將作為橫向控制器的輸入量,從而形成橫縱向協(xié)同控制系統(tǒng),以實(shí)現(xiàn)無人駕駛汽車對整條規(guī)劃軌跡的準(zhǔn)確跟蹤。圖4展示了無人駕駛車輛橫縱向控制流程圖。

(a) 縱向控制流程圖

由于車輛的橫向移動需要依靠其縱向移動才可實(shí)現(xiàn),所以無人駕駛車輛橫縱向控制之間必然存在著耦合關(guān)系。為了實(shí)現(xiàn)對軌跡的準(zhǔn)確跟蹤,需要將橫向控制和縱向控制二者之間進(jìn)行有效融合。結(jié)合圖4內(nèi)容,實(shí)現(xiàn)了無人駕駛車輛的橫縱向協(xié)同控制,具體如圖5所示。整個系統(tǒng)由橫向控制系統(tǒng)、縱向車速控制系統(tǒng)以及車輛平臺三部分構(gòu)成,通過該系統(tǒng)可以實(shí)現(xiàn)車輛在變速行駛過程中的軌跡跟蹤。

圖5 橫縱向協(xié)同控制結(jié)構(gòu)圖

4 仿真分析

本文所建立的仿真工況均在結(jié)構(gòu)化環(huán)境之下,重點(diǎn)對無人駕駛車輛的軌跡規(guī)劃及跟蹤效果進(jìn)行分析討論。相較于已有的控制方法對固定車速的跟蹤控制,本文所采用的橫縱向聯(lián)合控制方式最大的優(yōu)勢在于可以有效解決無人駕駛汽車在變速行駛工況下的軌跡跟蹤控制。為驗(yàn)證所建立規(guī)劃器和控制器的性能,設(shè)置了泊車、駛?cè)胫髀芬约俺嚾N典型工況進(jìn)行分析。

場景1:泊車是日常駕駛中常見的工作場景之一,這個過程駕駛員需要關(guān)注車輛四周的環(huán)境,同時(shí)泊車過程中需要兼顧車輛的油門和制動,這也在一定程度上加大了泊車的難度。高效的自動泊車系統(tǒng)可以提升泊車的效率。本文采用五次多項(xiàng)式曲線進(jìn)行軌跡規(guī)劃以保證生成的軌跡具備位置、速度、加速度連續(xù)的特性,同時(shí)生成的軌跡曲率連續(xù)可以保證整個過程的舒適性。在該場景中,設(shè)置車輛初始位置、速度和加速度均為零,即(x0,y0)=(0,0),v0=0 m/s,a0=0 m/s2;終點(diǎn)位置為停車位位置,終點(diǎn)速度和加速度為零,即vt=0 m/s,at=0 m/s2。整個泊車規(guī)劃過程的誤差、速度及位置信息如圖6所示。

(a) 泊車誤差

場景2:在城市道路駕駛的過程中,常遇到車輛由靜止?fàn)顟B(tài)駛?cè)胫髀返鸟{駛工況。在駛?cè)胫髀返倪^程中,需要仔細(xì)觀察主路后方的交通情況,在確保后方?jīng)]車的情況下才緩慢匯入主路。同時(shí)在匯入主路的過程中,方向盤的幅度不可以打得太大,即車輛的航向角不可以過大。當(dāng)車輛航向角θ較小時(shí),車身與道路之間的夾角就小,這樣可以保證車輛在匯入主路過程中即使有后方來車,后車也可以有一定空間進(jìn)行緩沖,從而避免事故的發(fā)生。這就要求軌跡不僅要平滑,而且航向角要小。由于本文不涉及無人車的感知模塊,所以設(shè)定匯入主車道時(shí)的車速vt=5 m/s,加速度at=0 m/s2。整個駛?cè)胫鬈嚨酪?guī)劃過程中的誤差、速度、航向角以及橫向位置信息如圖7所示。

(a) 駛?cè)脒^程誤差

場景3:車輛在高速駕駛的過程中,常常需要進(jìn)行超車,在超車過程中需要根據(jù)前車的行駛狀況提升本車的車速進(jìn)行變道超車。同匯入主路一樣,在超車過程中,方向盤轉(zhuǎn)動的幅度不可以太大,以免車輛產(chǎn)生較大的側(cè)向加速度而發(fā)生橫擺,因此所規(guī)劃的軌跡航向角θ應(yīng)該較小,同時(shí)整個超車過程應(yīng)該在10 s內(nèi)完成,以減小可能造成的安全隱患。本文設(shè)置超車完成時(shí)間為6 s,即t=6 s。由于本文不涉及無人車感知模塊,對前車的行駛狀態(tài)無法實(shí)時(shí)獲取,故在此設(shè)置整個超車過程中車速由v0=20 m/s提升至vt=30 m/s,超車距離s=150 m。整個超車規(guī)劃過程中的誤差、速度、航向角以及位置信息如圖8所示。

(a) 超車過程誤差

5 結(jié)束語

本文針對智能車輛在結(jié)構(gòu)化場景下的多種運(yùn)行工況,設(shè)計(jì)了基于LQR與PID相結(jié)合的車輛橫縱向耦合控制器。在橫向控制層面,通過引入前饋控制量以消除系統(tǒng)的穩(wěn)態(tài)誤差,減小了車輛在實(shí)際運(yùn)行過程中產(chǎn)生的橫向誤差,提升了控制過程的穩(wěn)定性;在縱向控制層面,PID控制使得車輛的運(yùn)行速度可以很好地與規(guī)劃速度相匹配,且系統(tǒng)的穩(wěn)態(tài)誤差控制在一定的范圍之內(nèi)。

從具體的仿真實(shí)例中可以得出:無論是在具有較大轉(zhuǎn)角的泊車工況下,還是低速駛?cè)胫髀饭r下或者是具有高速小轉(zhuǎn)角的超車工況下,本文所設(shè)計(jì)的橫縱向耦合跟蹤控制器都具有較好的跟蹤效果,系統(tǒng)誤差都控制在可接受的范圍之內(nèi)。因此,本文所設(shè)計(jì)的智能車輛軌跡跟蹤控制器可以很好地應(yīng)用于機(jī)場、港口等結(jié)構(gòu)比較固定的場景當(dāng)中。

智能車輛是集感知、決策和控制于一體的集成系統(tǒng)。因此在今后的研究當(dāng)中,需要考慮將感知與規(guī)劃進(jìn)行有效融合以實(shí)現(xiàn)更為智能的軌跡規(guī)劃。同時(shí),非結(jié)構(gòu)化場景路況復(fù)雜,環(huán)境多變,本文所設(shè)計(jì)的跟蹤控制器是否可以適用仍需要深入探討。