自動(dòng)駕駛微客模型預(yù)測(cè)的多點(diǎn)預(yù)瞄軌跡跟蹤控制算法研究

張 勇, 彭之川， 王文明， 朱 田， 周艷輝

(長沙中車智馭新能源科技有限公司， 長沙 410083)

自動(dòng)駕駛車輛軌跡跟蹤算法中，常用的有純追蹤算法、多點(diǎn)預(yù)瞄控制算法、模型預(yù)測(cè)控制(MPC)算法等。某自動(dòng)駕駛微客采用了純追蹤算法[1-2]進(jìn)行軌跡跟蹤控制，隨著自動(dòng)駕駛里程和控制信息數(shù)據(jù)的積累，發(fā)現(xiàn)該自動(dòng)駕駛微客在道路曲率產(chǎn)生較大變化的情況下(如進(jìn)入直角轉(zhuǎn)彎、自動(dòng)掉頭等場(chǎng)景)，其軌跡跟蹤偏差大。基于當(dāng)時(shí)車速和控制數(shù)據(jù)分析，車輛線控方向盤反應(yīng)滯后是次要原因；主要原因是在車輛入彎時(shí)，由于控制算法的不足導(dǎo)致補(bǔ)償輸出的最終轉(zhuǎn)角偏小。因此本文首先結(jié)合考慮車速偏差的反饋控制，對(duì)原有的純追蹤算法進(jìn)行優(yōu)化改進(jìn)，雖然提升了直線路段的軌跡跟蹤效果，但在道路曲率突變的情況下，軌跡跟蹤效果依然較差；因而進(jìn)一步基于MPC方法[3]，提出軌跡跟蹤的多點(diǎn)預(yù)瞄控制算法；最后在該智能駕駛微客上進(jìn)行實(shí)車驗(yàn)證。

1 原純跟蹤控制算法

1.1 微客自動(dòng)駕駛系統(tǒng)架構(gòu)

某微客自動(dòng)駕駛系統(tǒng)架構(gòu)如圖1所示，主要包括四大系統(tǒng)。其中環(huán)境感知系統(tǒng)主要由激光雷達(dá)、毫米波雷達(dá)、超聲波雷達(dá)以及多種智能攝像頭等組成，實(shí)時(shí)獲取車輛周圍環(huán)境信息；高精度定位系統(tǒng)主要依靠組合慣導(dǎo)提供高精度定位信息。決策控制系統(tǒng)收到上述兩個(gè)系統(tǒng)的信息后，進(jìn)行全局和局部路徑規(guī)劃，然后解算出軌跡跟蹤控制的方向盤轉(zhuǎn)角、車速等控制信息。最終由線控底盤系統(tǒng)執(zhí)行決策系統(tǒng)指令，使車輛進(jìn)行自動(dòng)駕駛。

圖1 微客自動(dòng)駕駛系統(tǒng)架構(gòu)圖

1.2 原純追蹤控制算法及效果

1.2.1 原純追蹤控制算法及理論分析

該自動(dòng)駕駛微客一開始采用的軌跡跟蹤控制算法是純追蹤控制算法，其對(duì)期望軌跡跟蹤效果主要取決于預(yù)瞄距離選取。偏大的預(yù)瞄距離，適用于直線和小曲率軌跡，對(duì)車輛航向角控制敏感，對(duì)位移偏差控制較弱，軌跡的跟蹤效果較差；偏小的預(yù)瞄距離，適用于大曲率彎道，對(duì)橫向位移偏差敏感，但易產(chǎn)生控制超調(diào)，方向盤抖動(dòng)，其收斂速度慢。因此自適應(yīng)的預(yù)瞄距離調(diào)節(jié)就尤為重要，將其轉(zhuǎn)換成車速的函數(shù)，跟蹤效果較好。但因車速變化引起預(yù)瞄距離的跳變，難以調(diào)節(jié)控制，因而導(dǎo)致方向盤抖動(dòng)并產(chǎn)生較大的軌跡跟蹤偏差。

將該5.3 m自動(dòng)駕駛微客簡(jiǎn)化為二自由度預(yù)瞄跟蹤模型，如圖2所示。其最高車速不超過40 km/h,屬于低速運(yùn)行。假設(shè)車輛在平面運(yùn)行，無滑移，只有前輪可以轉(zhuǎn)向。

圖2 微客車輛預(yù)瞄跟蹤模型

微客轉(zhuǎn)向角δ1可以用軸距L和轉(zhuǎn)彎半徑R來計(jì)算：

δ1=tan-1(L/R)

(1)

以在路徑上的目標(biāo)點(diǎn)T為預(yù)瞄點(diǎn)，由正弦定理得：

ld/sin(2α)=R/sin(π/2-α)

(2)

將式(2)代入式(1)，可推導(dǎo)出式(3)：

δ1=tan-1[2Lsin(α)/ld]

(3)

其中轉(zhuǎn)向角δ1、車身軸線和目標(biāo)點(diǎn)T的夾角α，都是關(guān)于時(shí)間t的函數(shù)，ld是從車輛后軸中心至預(yù)瞄點(diǎn)的距離，2α是預(yù)瞄距離ld在△TBC中的對(duì)角。

從式(3)可以看出，預(yù)瞄距離值ld對(duì)轉(zhuǎn)向角值δ1影響很大。通常在直線路段選取的ld1值大于彎道選取的ld2值。在車速較低(如小于20 km/h)時(shí)，ld1的跟蹤效果較好，當(dāng)車速較高(如提高至30 km/h)時(shí)，車輛會(huì)出現(xiàn)左右晃動(dòng)，這是ld1的選取偏小導(dǎo)致的。還有在車輛由直線轉(zhuǎn)入彎道時(shí)，ld1突變至ld2，預(yù)瞄值的突變會(huì)導(dǎo)致轉(zhuǎn)角值突變，從而導(dǎo)致方向盤突然抖動(dòng)。

1.2.2 原純追蹤控制算法效果

對(duì)采用純追蹤控制算法的自動(dòng)駕駛微客進(jìn)行直線和彎道行駛軌跡跟蹤測(cè)試，ld1為12 m，ld2為7 m，車速分別為30 km/h和10 km/h，其測(cè)試結(jié)果如圖3所示。直線路段的純追蹤算法橫向位置偏差絕對(duì)平均值是0.13 m，最大值是0.32 m；彎道路段的純追蹤算法橫向位置偏差絕對(duì)平均值是0.19 m，最大值是0.51 m。

圖3 純追蹤控制算法橫向位置跟蹤偏差

實(shí)際結(jié)果也表明，微客在直線路段的軌跡跟蹤偏差較大，在彎道路段的軌跡跟蹤偏差更大。其主要原因是純追蹤軌跡跟蹤控制對(duì)預(yù)瞄距離選取要求高，車速變化和道路曲率變化需要適配不同的預(yù)瞄距離。

2 原純追蹤算法的改進(jìn)及效果

2.1 在彎道路段和直線路段的改進(jìn)及效果

為解決上述彎道路段和直線路段的軌跡跟蹤偏差大的問題，均考慮引入偏差反饋控制來改善其收斂效果。引入的偏差反饋控制中的偏差是指橫向位移偏差和航向角偏差。下面結(jié)合實(shí)時(shí)車速對(duì)這兩種偏差進(jìn)行反饋控制改進(jìn)。

2.1.1 彎道路段

彎道的車身坐標(biāo)系控制偏差示意圖如圖4所示。在車身坐標(biāo)系XOY下，對(duì)目標(biāo)軌跡點(diǎn)進(jìn)行3次多項(xiàng)式擬合，得到以下軌跡曲線方程：

y=A3x3+A2x2+A1x+A0

(4)

式中：y是車身橫向位移，向左為正；x是車身縱向位移，向前為正；A0是自車慣導(dǎo)位置與擬合軌跡的位移偏差；A1是自車航向與擬合軌跡的切線方向的偏差斜率；A2是擬合軌跡的曲率；A3是曲率的變化率[4]。

圖4 彎道的車身坐標(biāo)系控制偏差示意圖

在預(yù)瞄點(diǎn)處，自車的斜率偏差為Δk：

Δk=3A3x2+2A2x+A1

(5)

車輛航向角ψ=tan-1(k)，基于航向角偏差角Δψ的補(bǔ)償角δ2(t1,Δψ)為

δ2(t1,Δψ)=[tan-1(Δk)]/(vt1)

(6)

式中t1是航向偏差補(bǔ)償?shù)脑O(shè)定時(shí)間，設(shè)定時(shí)間越短，補(bǔ)償值越大，偏差可快速趨于零；反之，補(bǔ)償值越小，偏差較慢趨于零。

R=s2/(2Δy)=(vtd)2/(2Δy)

(7)

式中：R是轉(zhuǎn)彎半徑；s是縱向(X向)位移距離，等于自車車速v與行駛時(shí)間td的乘積。

由式(7)結(jié)合式(1)可得，基于最近點(diǎn)的橫向位移偏差Δy進(jìn)行補(bǔ)償?shù)摩?(t2,Δy)為

δ3(t2,Δy)=tan-1[2ΔyL/(vt2)2]

(8)

式中t2是橫向位移偏差補(bǔ)償?shù)脑O(shè)定時(shí)間，與t1的含義相同。

因此改進(jìn)控制后的輸出車輪轉(zhuǎn)角δ為

δ=λ1δ1(t)+λ2δ2(t1,Δψ)+λ3δ3(t2,Δy)

(9)

式中λ1、λ2、λ3是對(duì)應(yīng)轉(zhuǎn)角的權(quán)重系數(shù)，三者的和為1。

在該自動(dòng)駕駛微客上，采用改進(jìn)純追蹤算法進(jìn)行彎道跟蹤測(cè)試，彎道路段車速10 km/h，其測(cè)試結(jié)果如圖5中虛線所示，跟蹤偏差的絕對(duì)平均值是0.11 m，最大值是0.30 m，軌跡跟蹤效果有明顯提升，但跟蹤偏差仍然偏大，偏差變化趨勢(shì)也不理想，仍需進(jìn)一步優(yōu)化。

圖5 改進(jìn)純追蹤控制算法橫向位置跟蹤偏差

2.1.2 直線路段

直線路段和彎道路段改進(jìn)方法相同，區(qū)別僅是式(4)、式(5)中A2、A3系數(shù)趨近于零。直線路段最終輸出轉(zhuǎn)角δ同式(9)。式中兩項(xiàng)偏差補(bǔ)償δ2(t1,Δψ)、δ3(t2,Δy)都基于當(dāng)前自車車速，自車車速增大時(shí)，兩個(gè)補(bǔ)償值都會(huì)減小，因此不易超調(diào)。

試驗(yàn)測(cè)試的直線路段車速為30 km/h，測(cè)試結(jié)果如圖5中實(shí)線所示。直線路段改進(jìn)純追蹤控制算法的橫向跟蹤位置偏差絕對(duì)平均值是0.08 m，最大值是0.21 m。直線路段中車速較高情況下，改進(jìn)純追蹤算法的跟蹤偏差小于原純追蹤算法的偏差，軌跡跟蹤效果較好。

改進(jìn)純追蹤控制算法在直線路段對(duì)目標(biāo)軌跡的跟蹤效果好，對(duì)軌跡跟蹤偏差敏感，可及時(shí)修正車輛位置。且當(dāng)車輛速度提高時(shí)，由于考慮了自車車速因素，其偏差反饋補(bǔ)償值適中，車輛橫向控制穩(wěn)定，抑制車輛出現(xiàn)走蛇形現(xiàn)象。

2.2 直線入彎時(shí)的抖動(dòng)問題解決及效果

針對(duì)直線入彎時(shí)方向盤突然抖動(dòng)問題，通過采用控制預(yù)瞄距離變化的方法，進(jìn)行初步改進(jìn)。一般直線路段的預(yù)瞄距離ld1要大于彎道路段的預(yù)瞄距離ld2，通過增加預(yù)瞄距離ld的單調(diào)遞減控制函數(shù)來改進(jìn):

ld=qld1+(1-q)ld2

(10)

其中q是隨時(shí)間變化，由1逐漸衰減至0。因而將直線預(yù)瞄值緩慢漸變至彎道預(yù)瞄值，消除預(yù)瞄值的突變。

經(jīng)過試驗(yàn)發(fā)現(xiàn)方向盤仍然會(huì)有抖動(dòng)，原因分析如下：實(shí)際上預(yù)瞄點(diǎn)是由車輛所處位置和預(yù)瞄距離值共同決定，只控制預(yù)瞄距離變化是不完整的。當(dāng)預(yù)瞄距離變小時(shí)，預(yù)瞄點(diǎn)后退；車輛向前行駛，預(yù)瞄點(diǎn)會(huì)隨之前移。因此當(dāng)車輛前進(jìn)距離大于預(yù)瞄距離時(shí)，預(yù)瞄點(diǎn)前移；反之，預(yù)瞄點(diǎn)后移。通常車輛在入彎前會(huì)進(jìn)行適當(dāng)?shù)販p速，車輛前進(jìn)距離在變小，當(dāng)預(yù)瞄距離以固定速率減小，就會(huì)出現(xiàn)預(yù)瞄點(diǎn)前移或后退反復(fù)切換，計(jì)算出的轉(zhuǎn)角值反復(fù)跳動(dòng)。

為保證預(yù)瞄點(diǎn)實(shí)時(shí)向前移動(dòng)，需限制預(yù)瞄值下降率，改進(jìn)后的預(yù)瞄距離ld如式(11)：

(11)

其中q同式(10)，v是自車車速，預(yù)瞄點(diǎn)位移控制系數(shù)k應(yīng)小于1，建議取0.9左右，可確保ld減小值小于車輛前進(jìn)距離，實(shí)現(xiàn)預(yù)瞄點(diǎn)實(shí)時(shí)前移。最終，通過式(11)，ld可實(shí)現(xiàn)由直線預(yù)瞄距離ld1平穩(wěn)漸變至彎道預(yù)瞄距離ld2，大幅抑制方向盤的抖動(dòng)。實(shí)車測(cè)試效果如圖6所示，直線入彎時(shí)，方向盤轉(zhuǎn)角連續(xù)變化，無抖動(dòng)現(xiàn)象。

圖6 改進(jìn)后車輛直線入彎的方向盤轉(zhuǎn)角

3 應(yīng)用MPC對(duì)控制算法在彎道上的改進(jìn)及效果

為進(jìn)一步減小改進(jìn)純追蹤算法在彎道的軌跡跟蹤偏差，對(duì)多點(diǎn)預(yù)瞄控制算法和MPC算法進(jìn)行研究。

現(xiàn)有的多點(diǎn)預(yù)瞄控制研究，一部分預(yù)瞄點(diǎn)用于檢測(cè)車輛軌跡偏差，進(jìn)行反饋控制；另一部分獲取目標(biāo)軌跡的曲率信息，以確定前饋控制中方向盤轉(zhuǎn)角[5-6]，能較好地跟蹤目標(biāo)軌跡。不同研究表明[7-12]，MPC的自動(dòng)駕駛車輛橫向路徑跟蹤控制器，在車速較高或低附著且變速條件等不同工況下，該控制器在保證車輛跟蹤精度基礎(chǔ)上可有效提高行駛穩(wěn)定性，具有較強(qiáng)的魯棒性。因此應(yīng)用MPC算法對(duì)多點(diǎn)預(yù)瞄算法進(jìn)行改進(jìn)。

應(yīng)用MPC改進(jìn)多點(diǎn)預(yù)瞄算法在目標(biāo)跟蹤軌跡上，依據(jù)不同的預(yù)瞄距離選取3個(gè)預(yù)瞄點(diǎn)，由改進(jìn)純追蹤算法解算出車輪轉(zhuǎn)角，結(jié)合實(shí)時(shí)車速，輸入到二自由度車輛運(yùn)動(dòng)學(xué)模型，由模型預(yù)測(cè)出自車未來軌跡，對(duì)比目標(biāo)軌跡，得到預(yù)測(cè)的橫向位置偏差，再依據(jù)偏差值進(jìn)行前饋補(bǔ)償，以提高前饋控制的精度。

3.1 二自由度車輛運(yùn)動(dòng)學(xué)模型

如圖2所示，在慣性坐標(biāo)系XOY下，R是車輛后輪轉(zhuǎn)向半徑，C是瞬時(shí)轉(zhuǎn)動(dòng)中心，ψ是車輛的航向角。假設(shè)轉(zhuǎn)向過程中車輛質(zhì)心側(cè)偏角保持不變，即車輛瞬時(shí)轉(zhuǎn)向半徑與道路曲率半徑相同。車輛橫擺角速度ω為

ω=Vxtanδ/L

(12)

式中：Vx是車輛后軸中心朝正前方速度；δ是前輪轉(zhuǎn)角；L是自車軸距。

然后可得：

R=Vx/ω

(13)

整理式(1)、(12)、(13)得到以下車輛模型[13]：

(14)

3.2 多點(diǎn)預(yù)瞄算法的MPC改進(jìn)

改進(jìn)多點(diǎn)預(yù)瞄控制算法中，3個(gè)預(yù)瞄點(diǎn)由實(shí)際車速和預(yù)測(cè)時(shí)長確定，此次時(shí)長分別確定為0.5 s、1.0 s、1.5 s，對(duì)應(yīng)的橫向位移偏差為Δy1、Δy2、Δy3。

Δy1=y1-ym1

(15)

式中：y1是在目標(biāo)路徑第一個(gè)預(yù)瞄點(diǎn)處的車輛橫向位移；ym1是在此預(yù)瞄點(diǎn)處模型預(yù)測(cè)的車輛橫向位移，可通過式(14)求得。Δy2、Δy3的計(jì)算類似于Δy1。

將3處橫向位移偏差結(jié)合式(8)得到：

δ4(t2,Δy)=λ4δ3(t2,Δy1)+λ5δ3(t2,Δy2)+
λ6δ3(t2,Δy3)+λ7δ3(t2,Δy)

(16)

式中，λ4、λ5、λ6、λ7是對(duì)應(yīng)轉(zhuǎn)角的權(quán)重系數(shù)，四者的和為1。

在彎道工況下，結(jié)合式(9)和式(16)可得到改進(jìn)多點(diǎn)預(yù)瞄控制輸出的車輪轉(zhuǎn)角δ：

δ=λ1δ1(t)+λ2δ2(t1,Δψ)+λ3δ4(t2,Δy)

(17)

3.3 MPC改進(jìn)多點(diǎn)預(yù)瞄算法在彎道工況的效果

在該自動(dòng)駕駛微客上，采用改進(jìn)多點(diǎn)預(yù)瞄算法進(jìn)行變曲率彎道測(cè)試，車速是10 km/h，其測(cè)試結(jié)果如圖7所示。

圖7 改進(jìn)多點(diǎn)預(yù)瞄算法橫向位置跟蹤偏差

模型預(yù)測(cè)多點(diǎn)預(yù)瞄算法在彎道橫向位置跟蹤偏差的絕對(duì)平均值是0.10 m，最大值是0.23 m。與改進(jìn)純追蹤算法對(duì)比，跟蹤偏差最大值減小了0.07 m。試驗(yàn)結(jié)果表明，改進(jìn)多點(diǎn)預(yù)瞄算法的彎道跟蹤偏差小于改進(jìn)純追蹤算法的偏差，且控制穩(wěn)定性好。

4 結(jié)束語

本文基于自動(dòng)駕駛微客跟蹤軌跡偏差較大的問題進(jìn)行歸納分析，考慮車速因素，對(duì)純追蹤算法進(jìn)行優(yōu)化改進(jìn)。然后參考MPC方法提出了模型預(yù)測(cè)多點(diǎn)預(yù)瞄控制算法，預(yù)測(cè)自車橫向位移誤差進(jìn)行軌跡跟蹤研究。試驗(yàn)結(jié)果表明，該控制算法有良好的目標(biāo)軌跡跟蹤效果。