自主駕駛車輛緊急避障的路徑規劃與軌跡跟蹤控制

鄧海鵬，麻斌，趙海光，呂良，劉宇

(1.吉林大學 汽車仿真與控制國家重點試驗室，吉林 長春 130025；2.中國環境科學研究院，北京 100012)

0 引言

緊急避障是車輛實現自主駕駛的關鍵技術，近年來，國內外學者對自主駕駛車輛的路徑規劃問題和軌跡跟蹤問題做了大量研究。在避障路徑規劃問題上，許多解決方案已得到成熟應用，如Hilgert等提出的彈性約束理論，以行駛軌跡局部曲率變化率最小為約束條件，對車輛緊急避障路徑進行了重新規劃[1]；Schouwenaars等考慮車輛行駛過程中的安全約束，提出了非結構環境下的滾動時域自主駕駛車輛路徑規劃[2]；Yu等用不同半徑圓弧表示不同車速下的行駛路徑，并將此觸須狀規劃算法用于未知環境的全局規劃[3]；張純剛等結合預測控制理論提出了動態未知環境下的無人小車滾動路徑規劃[4]。在避障軌跡跟蹤問題上，目前應用較為成熟的控制策略和方法有PID控制、滑模控制、自抗擾控制[5]、模糊自適應控制和神經網絡控制[6]等。Marino等設計了嵌入視覺導航系統的PID轉向控制器，能準確跟蹤未知曲率路徑[7]；Wang等利用魯棒自適應性滑膜控制器，能同時兼顧車輛的橫向控制和對已知軌跡的跟蹤[8]；Antonelli等[9]和Hajjaji等[10]結合PID方法和模糊控制方法的優勢，設計了自適應模糊PID控制器，以改善軌跡跟蹤的控制響應性能。然而上述方法無一例外，控制器的自適應程度較低，控制性能過多依賴于環境和參數的設置，并且不能很好地表達和執行自主駕駛車輛在行駛過程中受到的運動學與動力學約束。

模型預測控制應用范圍廣泛，在車輛電子穩定控制[11]、底盤集成控制[12]、自適應巡航控制[13]等方面都有比較成熟的應用。在自主駕駛車輛的路徑重規劃和軌跡跟蹤方面，模型預測控制(MPC)能根據車輛當前的運動狀態預測其未來軌跡[14-17]，實現對目標函數的在線滾動優化；同時，MPC能處理車輛在不同行駛工況下的多目標約束問題，可以很方便地將道路前方障礙物信息以不等式約束形式或懲罰函數形式添加到目標函數的求解條件中，以實現車輛的障礙物避讓功能[18]。

本文設計了雙層MPC緊急避障控制器。在路徑規劃層引入避障功能函數，綜合考慮避障函數權重和臨時路徑與全局路徑的偏差權重，規劃出緊急避障軌跡。在軌跡跟蹤層，提出以參考軌跡和車輛航向角偏差作為控制參考量的軌跡跟蹤轉向控制算法，采用線性時變MPC計算滿足車輛動力學約束的前輪轉向角最優解。

1 車輛模型

1.1 車輛點質量模型

圖1 車輛點質量模型Fig.1 Point mass model of autonomous vehicle

在參考坐標系下，車輛的點質量運動學模型[15]表示為

(1)

(2)

(3)

(4)

(5)

1.2 車輛動力學模型

地面行駛的車輛通常考慮縱向、橫向和橫擺3個維度的運動。為了使車輛快速且準確地跟蹤臨時路徑，需要建立精確的車輛動力學模型[19](見圖2)，以此做為線性時變模型預測控制(LTV-MPC)的基礎，提高車輛的操縱穩定性。圖2中：δ為車輛前輪轉向角；β為車輛質心側偏角；Flf和Flr為前、后輪胎受到的縱向力；Fcf和Fcr為前、后輪胎受到的側向力；αf和αr為車輛前、后輪側偏角。

圖2 車輛動力學模型Fig.2 Dynamic model of autonomous vehicle

如圖2所示，考慮車輛橫向和橫擺2個維度的運動，假設車輛左右受力完全對稱且δ足夠小，根據牛頓第二定律建立的車輛動力學方程如下：

(6)

(7)

(8)

1.3 輪胎模型

輪胎受到的豎直載荷、縱向力、橫向力以及回正力矩對車輛的安全性和操縱穩定性至關重要。Pacejka用經驗公式擬合輪胎試驗數據后提出的魔術輪胎模型能正確描述輪胎縱向力、側向力等與滑移率和側偏角之間的關系[20]。魔術輪胎模型的一般表達式為

Y(x)=Dsin{Carctan[Bx+Bsh-
E(Bx+Bsh-arctan(Bx+Bsh))]}+sv,

(9)

式中：Y(x)為輪胎受力模型；B為剛度因子；C為形狀因子；D為峰值因子；E為曲率因子；sh為輪胎水平偏移；sv為輪胎垂直偏移。

當自變量x表示輪胎縱向滑移率時，輸出量Y為輪胎的縱向力；當x表示輪胎側偏角時，Y為輪胎側向力。根據(9)式，可得輪胎縱向力隨滑移率s的變化曲線如圖3所示，輪胎側向力隨輪胎側偏角的變化曲線如圖4所示。

圖3 輪胎縱向力與滑移率關系曲線Fig.3 Longitudinal tire force vs.slip rate

圖4 輪胎側向力與側偏角關系曲線Fig.4 Lateral tire force vs.slip angle

縱向滑移率和輪胎側偏角的變化范圍通常較小，因此可以將輪胎魔術公式用線性表達式近似[19]，線性化處理的輪胎縱向力和側向力可以表示為

F1=C1s，

(10)

Fc=Ccα,

(11)

式中：C1為輪胎縱向剛度；Cc為輪胎側向剛度。當車輛側向加速度ay≤0.4g時，可認為(10)式和(11)式線性化處理的輪胎縱向力和側向力具有較高的擬合精度[21-22]。

車輛前、后輪側偏角的近似表達式為

(12)

(13)

(12)式和(13)式代入(10)式和(11)式，可以得到前、后輪縱向力和側向力為

Flf=Clfsf，

(14)

Flr=Clrsr，

(15)

(16)

(17)

式中：Clf為前輪縱向剛度；Clr為后輪縱向剛度；sf為前輪滑移率；sr為后輪滑移率；Ccf為前輪側向剛度；Ccr為后輪側向剛度。

上述簡化處理的輪胎模型代入(6)式～(8)式中，結合車輛在參考坐標系下的位置信息(4)式和(5)式，即可得到基于前輪小角度轉向和線性輪胎模型的車輛動力學模型如下：

(18)

(19)

(20)

(21)

(22)

2 路徑規劃與跟蹤控制器設計

2.1 路徑規劃器設計

為使路徑二次規劃能適應不同的避障函數形式，上層控制器采用非線性形式的模型預測控制。使用歐拉公式將(1)式～(5)式離散化，得到車輛狀態量的差分方程如下：

(23)

(24)

(25)

(26)

(27)

η(k+i|t)=Fi[η(k|k),ay(j)],
i=1,2,…,Np，j=1,2,…,Nc,

(28)

式中：Fi(·)為差分方程的函數表達形式；η(k|k)為5個車輛初始狀態量。為減少計算量、增強系統的實時性，本文取控制步長Nc=3.在控制步長外(Nc

(29)

綜上所述，可得非線性模型預測控制(NMPC)預測時域Np內的預測表達式為

(30)

式中：η(k+i|k)表示用k時刻車輛狀態預測k+i時刻車輛狀態。

考慮到自主駕駛車輛的避障路徑要求控制器盡可能縮小臨時路徑與全局參考路徑偏差，同時實現自主駕駛車輛對障礙物的避讓，因此本文設計的非線性模型預測控制的目標函數形式為

(31)

s.t.amin≤ay(j)≤amax,j=1,2,…,Nc，

(32)

Δamin≤Δay(j)≤Δamax,j=1,2,…,Nc.

(33)

式中：r(k+i|k),i=1,2,…,Np為給定的全局參考路徑；Bobst為避障功能函數；Qi為預測時域內(k+i,i=1,2,…,Np)控制輸出的權重矩陣；Rj為控制時域內(j=1,2,…,Nc)控制輸入的權重矩陣；S為避障功能函數的權重系數。

避障功能函數通過車輛與障礙物之間的距離和當前車速計算函數值的大小，如(34)式所示，自主駕駛車輛到障礙物的距離越近，車速越大，則函數懲罰值越大。

(34)

式中：xi、yi為障礙物在慣性坐標系下的位置坐標；X0、Y0為車輛當前位置坐標；ε為非0正數(本文取0.001以避免分母為0)。從避障功能函數懲罰值示意圖(見圖5)可以看出，當車輛接近障礙物時，懲罰數值急劇增大，使得目標函數minJ(η(k),ay(k))的求解遠離障礙物的車輛坐標X、Y，減小避障懲罰函數的罰值，規劃出避開障礙物的行駛路徑。當車輛與障礙物處于安全距離時，避障功能函數懲罰值極小，此時目標函數正常跟蹤全局參考路徑。

圖5 避障功能函數懲罰值示意圖Fig.5 Penalty value of obstacle avoidance function

求解得出滿足約束條件的控制輸入量ay(1)、ay(2)和ay(3)，根據(25)式和(27)式，即可得到避障路徑的航向角和車輛縱向位置坐標。

由于求解得出的新軌跡是預測時域內的離散點，控制層很難根據離散點參考軌跡完成跟蹤任務，需要對規劃軌跡的離散數據點進行擬合。為得到較高的曲線擬合精度、減小路徑跟蹤誤差，本文選用四次多項式對預測軌跡離散點擬合。將規劃路徑的縱坐標yr和車輛航向角ψr隨車輛縱向位移X的變化曲線傳遞至軌跡跟蹤層，作為下層軌跡跟蹤控制器的參考值，yr和ψr的傳遞形式為

yr=p0+p1X1+p2X2+p3X3+p4X4，

(35)

ψr=q0+q1X1+q2X2+q3X3+q4X4，

(36)

式中：p0、p1、p2、p3和p4分別為參考軌跡擬合系數；q0、q1、q2、q3和q4分別為參考航向角ψr擬合系數。

2.2 軌跡跟蹤器設計

下層軌跡跟蹤控制器根據車輛動力學模型實時調整車輛和參考軌跡之間的位姿偏差。本文跟蹤器以參考軌跡yr和車輛的航向角偏差Δψ作為軌跡跟隨的控制參考量，以前輪偏轉角δ作為轉向控制輸入量。由于NMPC對于實時系統存在局限性，故設計線性時變預測模型求解滿足車輛動力學約束的最優前輪偏轉角。如圖6所示，車輛航向角偏差為參考軌跡航向ψr和車輛實際航向之差，計算公式如(37)式所示：

Δψ=ψr-ψ.

(37)

圖6 車輛航向角偏差示意圖Fig.6 Deviation of vehicle yaw angle

結合車輛動力學模型(18)式～(22)式和航向角偏差(37)式，可將軌跡跟蹤器的設計表示為如下狀態空間形式：

(38)

yc=Cχ，

(39)

(38)式中狀態變量的非線性模塊難以滿足軌跡跟蹤控制器實時性的要求，且在計算過程中容易引入非線性約束增加計算復雜程度，因此需要對(38)式進行線性化處理，以獲得線性時變預測模型。將(38)式在工作點(χ0,u0)處以泰勒公式展開，忽略高階項，得

(40)

式中：u0為工作點的控制輸入量；χ0為工作點的狀態變量，

(41)

(41)式與(40)式相減，得到

(42)

(42)式離散化，可得狀態變量表達式為

χ(k+1)=Aχ(k)+Bu(k)+d(k)，

(43)

為減小靜態誤差，將(43)式中的控制量改寫成控制增量[17]的形式：

ξ(k+1|k)=Akξ(k|k)+BkΔu(k|k)+dk(k),

(44)

γ(k|k)=Ckξ(k|k),

(45)

迭代(45)式，可得預測時域內Np輸出量的表達形式為

(46)

結合車輛動力學約束條件，得到線性模型預測控制目標函數如下：

(47)

s.t.umin≤u(j)≤umax,j=1,2,…,Nc，

(48)

Δumin≤Δu(j)≤Δumax,j=1,2,…,Nc，

(49)

式中：Δumax(k)為控制增量上邊界；Δumin(k)為控制增量下邊界。

2.3 緊急避障控制器結構

綜合路徑規劃器和軌跡跟蹤器的設計思路得到圖7所示的雙層MPC緊急避障控制器邏輯結構，其中，上層控制器根據前方道路障礙物信息和車輛當前運動狀態進行路徑二次規劃，下層控制器接收臨時規劃的軌跡信息和車輛當前運動狀態進行軌跡跟蹤。

圖7 路徑規劃與軌跡跟蹤控制邏輯圖Fig.7 Control model of path planning and tracking

3 仿真結果與分析

為了驗證雙層MPC緊急避障控制器的有效性，利用汽車動力學仿真軟件Carsim和MATLAB/Simulink聯合仿真平臺測試自主駕駛車輛的避障效果。仿真車輛模型參數如表1所示。

表1 車輛基本參數Tab.1 Essential parameters of autonomous vehicle

3.1 平直路面避障效果分析

全局參考路徑為一段長240 m的直行路段，在自主駕駛車輛正前方有一臺故障車輛，分析不同車速下控制器對該靜止故障車的避障性能。

根據道路前方故障車輛位置信息，雙層MPC緊急避障控制器綜合考慮車輛的行駛操縱穩定性約束條件自動規劃出最適合車輛當前運動狀態的軌跡。在平直城市路面，自主駕駛車輛的避障效果如圖8所示。

圖8 避障軌跡車輛移動幻影示意圖Fig.8 Moving trajectory during obstacle avoidance

圖9所示為車輛速度分別為60 km/h、80 km/h和100 km/h時車輛的行駛軌跡。從圖9中可以看到，車速為60 km/h時，自主駕駛車輛在85 m處開始響應路徑規劃層的避障功能，車速越大，避障功能響應時刻提前，車速為80 km/h和100 km/h時控制器分別在80 m和75 m處做出避障決策。這是因為本文設計的避障功能函數考慮了本車速度對二次路徑規劃的影響，車速越大、避障功能函數的懲罰值越大，規劃控制器以犧牲規劃路徑與全局參考路徑(Y=2 m)的跟蹤精度為代價增大車輛與障礙物之間的距離，避障控制策略符合高速行駛車輛提前避讓的駕駛行為。

圖9 不同行駛速度下車輛軌跡Fig.9 Moving trajectories of vehicle at various velocities

從圖9中也可以看出，不同車速下車輛的側向位移最大偏差相同。圖10和圖11所示分別為不同車速下車輛的航向角偏差和質心側偏角，仿真結果顯示，在平直路段車輛的質心側偏角和航向角偏差都控制在一個較小的范圍內(|Δψ|<2.5°，|β|<1°)，表明車輛在緊急避障過程中行駛穩定，設計的避障控制器對車速變化具有很強的適應性。

圖10 不同行駛速度下車輛航向角偏差Fig.10 Deviation of vehicle yaw angles at various velocities

圖11 不同行駛速度下車輛質心側偏角Fig.11 Side-slip angles of vehicle at various velocities

3.2 多目標連續避障效果分析

針對行車過程中道路障礙物出現的隨機突發性，本文建立貼近實際情況的自主駕駛車輛多車連續避障實驗場景(在道路前方50 m處存在多輛需避讓的目標車輛，在道路前方190 m處存在逆向行駛的越界車輛)。

根據前方目標車輛和逆向行駛車輛的位置信息，本文設計的雙層MPC緊急避障控制器的多車避障效果和會車避讓效果如圖12和圖13所示。

圖12 多目標避障效果示意圖Fig.12 Moving trajectory of vehicle during multi-obstacles avoidance

圖13 逆向越界車輛避障效果示意圖Fig.13 Moving trajectory of vehicle during multi-obstacles avoidance

圖14所示為60 km/h、80 km/h和100 km/h不同車速下自主駕駛車輛的多車連續避障行駛軌跡。對于正前方的目標車輛，隨著行車速度的增大，自主駕駛車輛分別在25 m、20 m和15 m處響應控制器的避障功能，避障最大側向位移約為3 m；對于逆向駛來的越界車輛，隨著車速增加，自主駕駛車輛分別在175 m、170 m和165 m處響應控制器的避障效果，避障最大側向位移約為1.2 m.從避障軌跡來看，車速越大，車輛的避障響應時刻越早。

圖14 不同行駛速度下自主駕駛車輛連續避障軌跡Fig.14 Moving trajectories of vehicle at various velocities during multi-obstacles avoidance

圖15和圖16所示分別為60 km/h、80 km/h和100 km/h不同車速下多車連續避障時車輛質心側偏角和航向角偏差的變化情況。實驗結果顯示，在多目標連續避障過程中，車輛最大質心側偏角|β|均不超過1°，表明MPC路徑規劃控制策略能保證車輛較高的行駛穩定性；車輛的最大航向角偏差|Δψ|均不超過2.5°，表明對于多目標連續避障情況，雙層MPC緊急避障控制器同樣具有很好的控制效果。

圖15 車輛連續避障質心側偏角Fig.15 Side-slip angles of vehicle during multi-obstacles avoidance

圖16 車輛連續避障航向角偏差Fig.16 Deviation of vehicle yaw angles during multi-obstacles avoidance

4 結論

本文根據車輛點質量模型、車輛運動學模型和魔術輪胎模型設計了雙層MPC緊急避障控制器，通過Carsim和MATLAB/Simulink聯合仿真平臺驗證了緊急避障控制器對車輛速度變化以及對多目標連續避障均具有很好的控制效果。得到主要結論如下：

1)緊急避障控制策略對低速(60 km/h)、中速(80 km/h)、高速(100 km/h)行駛車輛有很強的適應性，高速行駛時車輛的最大質心側偏角不超過1°，最大航向角偏差不超過2.5°.

2)在由道路前方目標車輛和逆向駛來的越界車輛組成的多車連續避障實驗場景中，自主駕駛車輛的質心側偏角β和航向角偏差Δψ均控制在合理的范圍內。