基于模型預測的無人駕駛車輛路徑跟隨控制

張 嚴，黃妙華

(武漢理工大學 汽車工程學院，湖北 武漢 430070)

無人駕駛車輛中，路徑跟隨是指車輛在不同的車速、載荷、路況以及風阻等條件下自動跟隨目標行車路線，并保持一定的準確性、平穩性和舒適性[1]。目前，應用較為廣泛的路徑跟隨算法有傳統的預瞄跟隨算法、PID(proportion integral derivative)控制算法、Pure Pursuit算法、自適應控制算法和模糊預測算法[2]等。

模型預測控制(model predictive control,MPC)也稱為滾動時域最優控制。其利用一個已知的模型、系統當前的狀態量和未來控制量去預測系統的未來輸出，即包括預測模型、滾動優化、反饋矯正3個步驟。文獻[3]對線性時變模型預測控制算法的低速直線軌跡跟隨控制效果進行了驗證，但該汽車模型為運動學模型，沒有考慮到汽車在行駛過程中的受力情況和彎道軌跡的跟隨控制效果；文獻[4]雖然在動力學的基礎上對模型預測控制算法進行了驗證，但都只是采用低速工況，沒有對高速工況的跟隨效果進行驗證。此外，在MPC算法中，不同的設計參數可能會影響其跟隨效果，如果按照傳統人工調整參數的方式，不僅費時費力，而且還不能確保MPC路徑跟隨控制器達到最佳的跟隨效果。為了解決上述問題，提高無人駕駛車輛在不同速度工況下對目標路徑跟蹤的準確性，筆者通過建立三自由度汽車動力學模型并加入約束條件，對MPC算法中的重要參數通過遺傳算法(genetic algorithm,GA)進行優化，找到高速、中速和低速3種不同速度工況下的最優控制參數，最后搭建 Carsim/Simulink聯合仿真平臺來驗證其跟隨效果。

1 汽車動力學建模及線性化處理

1.1 動力學模型建立

汽車作為一個復雜的非線性時變系統，在進行無人駕駛車輛路徑跟隨的控制中,只考慮縱向、側向和橫擺運動3個自由度的汽車動力學模型便能很好地滿足所需的控制要求[5]。故建立如圖1所示的三個自由度汽車單軌動力學模型。

圖1 三自由度汽車單軌動力學模型

根據牛頓第二定律建立如下力平衡方程:

mx··=2Fxf+2Fxr+my·φ·

(1)

my··=2Fyf+2Fyr-mx·φ·

(2)

Izφ··=2aFyf- 2bFyr

(3)

式中：a、b分別為質心到前、后軸的距離;m為車輛的整備質量；Fxf、Fxr為車輛前、后車輪受到的x方向的力；Fyf、Fyr為車輛前、后車輪受到的y方向的力;Iz為車輛繞z軸的轉動慣量；φ為車輛航向角，x·為車輛沿x方向的速度；y·為車輛沿y方向的速度；x··，y··分別為車輛沿x、y方向的加速度；φ··為航向角加速度；φ·為航向角速度。

考慮車身坐標系與慣性坐標系之間的轉換關系，可以得到：

Y·=x·sinφ+y·cosφ

(4)

X·=x·cosφ-y·sinφ

(5)

根據式(1)～式(5)，得到車輛的非線性動力學模型。將系統描述為以下狀態空間表達式：

ξ·dyn=fdyn(ξdyn,udyn)

(6)

hdyn=mdyn(ξdyn)

(7)

其中，控制量udyn=δf(僅考慮前輪轉向，故δr=0)，狀態量ξdyn=[y·,x·,φ,φ·,Y,X]T,輸出量hdyn=[φ,Y]T。

1.2 輪胎模型引入后的動力學模型

為了進一步簡化模型，還需引入輪胎模型。主要的輪胎模型包括經驗輪胎模型、物理輪胎模型和理論輪胎模型。筆者采用以魔術公式[6]為基礎的經驗輪胎模型，其表達式為：

Y(x)=Dsin{Carctan[Bx-

E(Bx-arctan(Bx))]}

(8)

式中：x為輸入量，分別表示輪胎側偏角α和滑移率S；B、C、D、E依次為剛度因子、形狀因子、峰值因子和曲率因子；Y(x)為輸出量，可以為縱向力F1、側向力Fc或回正力矩Mz。

該輪胎模型認為，輪胎在垂直、側面方向上是線性的，阻尼為常量。在輪胎側偏角α≤5°，側向加速度ay≤0.4g時，對輪胎的擬合精度較高。在這個范圍內，當縱向滑移率及側偏角較小時，可用式(9)和式(10)得到輪胎的側向力和縱向力：

Fc=Ccα

(9)

F1=C1s

(10)

式中：Cc為輪胎的側偏剛度；C1為輪胎縱向剛度；α為輪胎側偏角；S為縱向滑移率。

在輪胎力的計算過程中，對模型進一步簡化，故提出小角速度假設，即：

sinθ≈θ，cosθ≈1，tanθ≈θ

通過簡化，可以得到車輪側偏角為：

αf=y·+aφ·x·-δf

αr=y·-bφ·x

(11)

式中：δf為前輪偏轉角。

車輪側偏力為：

Fcf=Ccfδf-y·+aφ·x·

(12)

Fcr=Ccrbφ·-y·x·

(13)

式中：Ccf、Ccr為前、后輪胎側偏剛度。

車輪縱向力為：

F1f=C1fSfF1r=C1rSr

(14)

式中：C1f、C1r為前、后輪胎縱向剛度；Sf、Sr分別為前、后輪胎縱向滑移率。

將式(14)帶入式(6)和式(7)，得到基于線性輪胎模型和前輪較小偏角假設的車輛動力學模型：

x··=1m2C1fSf+Ccfδf-

(y·+aφ·)/x·δf+C1rSr+my·φ·

y··=1m(2[Ccf(δf- (y·+aφ·)/x·)+

Ccr(bφ·-y·)/x·]-mx·φ·)

Izφ··=2[aCf(δf-(y·+aφ·)/x·)-

bCcr(bφ·-y·)/x·]

(15)

式中：控制量udyn=δf，狀態量ξdyn=[y·,x·,φ,φ·,Y,X]T；X、Y分別為汽車的行駛軸心位置。

1.3 動力學模型線性化

以上建立的動力學模型為非線性系統，不能直接作用于線性時變模型預測控制，故需要對其進行線性化處理，得到線性時變方程如下：

ξ·dyn=Adyn(t)ξdyn(t)+Bdyn(t)udyn(t)

(16)

其中

Adyn(t)=?fdyn?udynξt,ut=-2(Ccf+Ccr)mx·t?fy··?x·02(bCcr-aCcf)mx·t-x·00

φ·-2Ccfδf,t-1mx·t?fx··?x·0y·t-2aCcfδf,t-1mx·t00

000100

2(bCcr-aCcf)Izx·t?fφ··?x·0-2(b2Ccr+a2Ccf)Izx·t00

cosφt-sinφtx·tcosφt-y·tsinφt000

-sinφtcosφt-y·tcosφt-x·tsinφt000

Bdyn(t)=?fdyn?udynξt,ut=

2Ccfm,2Ccf2δf,t-1-y·+aφ·x·tm,0,2aCcfIz,0,0

?fy··?x·=2Ccf(y·r+aφ·r)+2Ccr(y·t-bφ·t)mx·2t-φ·t

?fx··?x·= 2Ccfδf,t-1(y·t+aφ·t)mx·2t

?fφ··?x·=2(aCcf(y·t+aφ·t)-bCcr(y·t-bφ·t))Izx·2t

2 模型預測控制器

2.1 預測方程

對式(16)采用一階差商方法進行離散化處理，得到離散的狀態空間表達式為：

ξdyn(k+1)=Adyn(k)ξdyn(k)+Bdyn(k)udyn(k)

(17)

設定

ξdyn(k|t)=x(k|t)

udyn(k-1|t)

可以得到一個新的狀態空間表達式：

ξdyn(k+1|t)=

Ak|tξdyn(k|t)+Bk|tΔudyn(k|t)

hdyn(k|t)=Ck|t(k|t)

(18)

其中，

Ak|t=Adyn(k|t)0

0m×nIm,Bk|t=Bdyn(k|t)

Im，

Ck|t=[Cdyn(k|t) 0]，Im為m階單位矩陣， Δudyn為控制增量，ξdyn為當前狀態量，m，n分別為系統的狀態量和控制量個數。

2.2 目標函數

為了得到控制時域內的控制序列，需要進行目標函數的設計[7]?？砂芽刂圃隽孔鳛槟繕撕瘮档臓顟B量，優化目標函數可設為如下形式：

J(ξ(t),u(t-1),ΔU(t))=

∑Npi=1‖Δηdyn(t+i|t)‖Q2+

∑Nc-1i=1‖Δudyn(t+i|t)‖2R+ρε2

(19)

式中:Δηdyn(t+i|t)=ηdyn(t+i|t)-ηdyn,ref(t+i|t)為輸出量與參考量之差；Q、R為權重矩陣；Δudyn(t+i|t)為控制增量；ρ為權重系數；ε為松弛因子；Np為預測時域參數；Nc為控制時域參數。

2.3 約束條件

為了保證無人駕駛汽車快速平穩地跟蹤目標路徑，需對輪胎側偏角、前輪轉角及其增量以及質心側偏角進行動力學的條件約束[8]。

2.3.1 輪胎側偏角

由式(11)可推出在某一任意時刻，當系統各狀態量已知時，前、后輪胎側偏角的表達式為：

αf,t=y·t+aφ·tx·t-δf,t-1αr,t=y·t-bφ·tx·t

(20)

其中，αf,t為某一任意時刻的前輪側偏角；αr,t為某一任意時刻的后輪側偏角，δf,t-1為上一時刻的前輪轉角。此處對前輪側偏角做出限定即：

-3°≤αf,t≤3°

2.3.2 前輪轉角及其增量

前輪轉角作為所設計的MPC控制器中的控制量，需要對它及它的增量進行約束。此處將前輪轉角及其增量約束設置成：

-10°≤u≤10°，-0.85°≤Δu≤0.85°

2.3.3 質心側偏角

汽車在附著條件較差的冰雪路面上，極限值為±2°， 而在附著條件良好的路面上行駛時，質心側偏角的極限可以達到±12°。因此質心側偏角的約束條件為：

-12°≤β≤12° (良好路面)

-2°≤β≤2° (冰雪路面)

綜上所述，MPC路徑跟隨控制器可歸納為如以下優化問題：

minΔudyn,∈‖Δηdyn(t+i|t)‖2Q+∑Nc-1i=1

‖Δudyn(t+i|t)‖2R+ρε2Δηdyn(t+i|t)=

ηdyn(t+i|t)-ηdyn,ref(t+i|t)

(21)

s.t.ΔUdyn,min≤ΔUdyn≤ΔUdyn,max

Udyn,min≤AΔUdyn+Udyn,t≤Udyn,max

yhc,min≤yhc≤yhc,max

ysc,min-ε≤ysc≤yyc,max+ε

ε>0

(22)

式中：yhc為硬約束輸出；ySC為軟約束輸出；yhc,max、yhc,max為硬約束極限值；ysc,min、ysc,max為軟約束極限值；ΔUdyn為運動學中控制增量；Udyn為運動學控制量；ΔUdyn,min、ΔUdyn,max為運動學控制增量的極限值；Udyn,min、Udyn,max為運動學控制值的極限值。當每個控制周期內完成對上式的求解后，將控制序列中的第一個元素作為實際的控制輸入增量作用于系統：

udyn(t)=udyn(t-1)+Δu*dyn

如此循環，即可實現對目標路徑的跟蹤。

3 仿真實驗

假設已有規劃好的目標路徑，故只針對其跟隨效果進行仿真實驗。由于雙移線路徑[9-10]可以很好地測試汽車的行駛穩定性[11-13]，故仿真實驗以雙移線路徑為無人駕駛汽車的跟隨目標路徑，雙移線路徑如圖2所示。

圖2 雙移線參考軌跡

3.1 遺傳算法對MPC優化

遺傳算法是一類借鑒生物界自然選擇和自然遺傳機制的隨機搜索算法。其根據適應度的好壞從上一代中選擇一定數量的優秀個體，通過交叉、變異形成下一代群體。經過若干代的進化，算法收斂于最好的染色體，它即是問題最優解[14]。

MPC 控制器中，對預測時域Np、控制時域Nc這兩個參數進行優化具有重要意義。按照傳統人工調整參數的方式，不僅費時費力，而且還不能確保MPC路徑跟隨控制器的效果達到最佳。因此筆者利用遺傳優化算法對MPC的控制參數進行了優化設計，從而尋找出不同車速下的最優參數。系統的輸出性能指標采用絕對誤差積分準則(integrated absolute error，IAE)，其表達式為：

JIAE=∫∞0|e(t)|dt

遺傳算法優化流程如圖3所示。

圖3 遺傳算法優化流程圖

3.2 仿真實驗結果分析

采用遺傳算法分別對30 km/h、60 km/h、90 km/h、120 km/h 4種速度工況進行仿真試驗，找到不同速度工況下的最優控制參數。遺傳算法中的優化代數設為40，種群數量為40，交叉概率為0.6，變異概率為0.1。其優化結果如圖4所示。

圖4 遺傳算法優化結果

從圖4(a)的各代最佳適應度隨迭代次數變化曲線可看出，30 km/h車速下最優適應值(即IAE 性能指標值)在第24次迭代時趨于穩定，并達到最小值0.711 1；60 km/h車速下最優適應值在第2次迭代時趨于穩定，并達到最小值0.633 6；90 km/h車速下最優適應值在第5次迭代時趨于穩定，并達到最小值1.160 4；120 km/h車速下最優適應值在第3次迭代時趨于穩定，并達到最小值1.954 3。

從圖4(b)和圖4(c)的Np、Nc參數值隨迭代次數變化曲線可看出，30 km/h車速下參數最優值分別為15.547 0和19.660 7；60 km/h車速下參數最優值分別為18.973 9和14.145 0；90 km/h車速下參數最優值分別為30.648 9和6.368 7；120 km/h車速下參數最優值分別為34.508 7和7.195 7。

將優化后的參數分別帶入對應的速度工況，通過搭建Carsim/Simulink仿真平臺，對雙移線目標路徑進行仿真試驗,結果如圖5所示。

圖5 優化前后路徑跟隨效果對比

由圖5可知，經過GA優化過后的MPC控制器，在低速、中速和高速時，均可以較好地跟隨目標路徑。在汽車處于低速工況30 km/h時，最大的橫向距離為0.1 m，可以很精準地跟隨目標路徑。在汽車處于中速工況60 km/h時，最大的橫向距離為0.2 m，對目標路徑也有很好的跟隨效果。在汽車處于高速工況90 km/h和120 km/h時，在有較大道路彎曲曲率的試驗目標路徑上出現了一個較大的偏移量，但最大的橫向距離僅為0.88 m，相當于一半的車寬，且后期很快可以跟隨上目標路徑，而且考慮到正常情況下，汽車高速工況行駛路況一般應較好，很少出現較大的道路彎曲曲率，故其誤差距離可以接受。

汽車的其他性能指標如圖6所示。

圖6 優化后的汽車主要性能指標

由圖6可知，30 km/h車速下橫擺角速度最大值在11.9 deg/s左右，側向加速度最大值在0.17 m/s2左右，質心側偏角為1.57°；60 km/h車速下橫擺角速度最大值在24.7 deg/s左右，側向加速度最大值在0.67 m/s2左右，質心側偏角為0.94°；90 km/h車速下橫擺角速度最大值在26.5 deg/s左右，側向加速度最大值在0.89m/s2左右，質心側偏角為3.6°；120 km/h車速下橫擺角速度最大值在33.2 deg/s左右，側向加速度最大值在0.97 m/s2左右，質心側偏角為5.9°。

以上實驗結果分析表明，經過GA優化過的MPC控制器，雖然可能部分犧牲了駕駛員及乘客的乘坐舒適性，但是在對目標路徑進行跟隨時，具有較好的魯棒性，且汽車響應也符合穩定地要求，可以準確穩定的對目標路徑進行跟隨。

4 結論

針對無人駕駛車輛的路徑跟隨問題，提出了一種基于模型預測的路徑跟隨控制，并通過遺傳算法，對MPC控制器中的控制參數，預測時域Np和控制時域Nc進行優化，找到了不同速度工況下的最優控制參數，從而大大提高汽車對目標路徑跟隨的穩定性和準確性。故將優化后MPC控制器應用到無人駕駛汽車路徑跟隨控制領域具有一定的可行性。