自動駕駛微客模型預測的多點預瞄軌跡跟蹤控制算法研究

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

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

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

1 原純跟蹤控制算法

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

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

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

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

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

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

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

圖2 微客車輛預瞄跟蹤模型

微客轉向角δ1可以用軸距L和轉彎半徑R來計算：

δ1=tan-1(L/R)

(1)

以在路徑上的目標點T為預瞄點，由正弦定理得：

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

(2)

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

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

(3)

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

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

1.2.2 原純追蹤控制算法效果

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

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

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

2 原純追蹤算法的改進及效果

2.1 在彎道路段和直線路段的改進及效果

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

2.1.1 彎道路段

彎道的車身坐標系控制偏差示意圖如圖4所示。在車身坐標系XOY下，對目標軌跡點進行3次多項式擬合，得到以下軌跡曲線方程：

y=A3x3+A2x2+A1x+A0

(4)

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

圖4 彎道的車身坐標系控制偏差示意圖

在預瞄點處，自車的斜率偏差為Δk：

Δk=3A3x2+2A2x+A1

(5)

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

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

(6)

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

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

(7)

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

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

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

(8)

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

因此改進控制后的輸出車輪轉角δ為

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

(9)

式中λ1、λ2、λ3是對應轉角的權重系數(shù)，三者的和為1。

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

圖5 改進純追蹤控制算法橫向位置跟蹤偏差

2.1.2 直線路段

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

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

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

2.2 直線入彎時的抖動問題解決及效果

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

ld=qld1+(1-q)ld2

(10)

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

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

為保證預瞄點實時向前移動，需限制預瞄值下降率，改進后的預瞄距離ld如式(11)：

(11)

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

圖6 改進后車輛直線入彎的方向盤轉角

3 應用MPC對控制算法在彎道上的改進及效果

為進一步減小改進純追蹤算法在彎道的軌跡跟蹤偏差，對多點預瞄控制算法和MPC算法進行研究。

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

應用MPC改進多點預瞄算法在目標跟蹤軌跡上，依據(jù)不同的預瞄距離選取3個預瞄點，由改進純追蹤算法解算出車輪轉角，結合實時車速，輸入到二自由度車輛運動學模型，由模型預測出自車未來軌跡，對比目標軌跡，得到預測的橫向位置偏差，再依據(jù)偏差值進行前饋補償，以提高前饋控制的精度。

3.1 二自由度車輛運動學模型

如圖2所示，在慣性坐標系XOY下，R是車輛后輪轉向半徑，C是瞬時轉動中心，ψ是車輛的航向角。假設轉向過程中車輛質心側偏角保持不變，即車輛瞬時轉向半徑與道路曲率半徑相同。車輛橫擺角速度ω為

ω=Vxtanδ/L

(12)

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

然后可得：

R=Vx/ω

(13)

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

(14)

3.2 多點預瞄算法的MPC改進

改進多點預瞄控制算法中，3個預瞄點由實際車速和預測時長確定，此次時長分別確定為0.5 s、1.0 s、1.5 s，對應的橫向位移偏差為Δy1、Δy2、Δy3。

Δy1=y1-ym1

(15)

式中：y1是在目標路徑第一個預瞄點處的車輛橫向位移；ym1是在此預瞄點處模型預測的車輛橫向位移，可通過式(14)求得。Δy2、Δy3的計算類似于Δy1。

將3處橫向位移偏差結合式(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是對應轉角的權重系數(shù)，四者的和為1。

在彎道工況下，結合式(9)和式(16)可得到改進多點預瞄控制輸出的車輪轉角δ：

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

(17)

3.3 MPC改進多點預瞄算法在彎道工況的效果

在該自動駕駛微客上，采用改進多點預瞄算法進行變曲率彎道測試，車速是10 km/h，其測試結果如圖7所示。

圖7 改進多點預瞄算法橫向位置跟蹤偏差

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

4 結束語

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