高速公路智能車輛動態(tài)避障研究

馮 櫻，喬寶山，袁顯舉，鄧召文

(湖北汽車工業(yè)學院 汽車工程學院， 湖北 十堰 442002)

智能技術可賦予車輛智能，智能車輛通過車載傳感器獲取周圍環(huán)境信息，決策規(guī)劃出參考軌跡，下達指令，控制車輛按照參考軌跡行駛。其中，軌跡規(guī)劃和跟蹤控制是實現(xiàn)智能駕駛的關鍵技術。由于車輛為強非線性系統(tǒng)，高速公路換道避障是一個復雜和動態(tài)的過程，因此，高速公路的動態(tài)避障成為研究的難點問題。

當前，國內外學者對智能汽車換道軌跡規(guī)劃進行了大量研究，早期研究主要集中在軌跡搜索法，是將車輛周圍實際環(huán)境信息抽象化為一個個點或柵格組成的區(qū)域圖，再對區(qū)域圖中不同的點或柵格根據目標需求進行分類，完成從起點到終點的路徑規(guī)劃。該算法主要包含D*算法(Dijlstra Algorithm)[1]、A*算法(A*Algorithm)以及狀態(tài)柵格算法[2]?；谲壽E搜索的軌跡規(guī)劃方法在低速下可以實現(xiàn)避讓靜態(tài)障礙物，但不適用在高速或具有動態(tài)障礙物的工況下。基于曲線插值法的軌跡規(guī)劃方法是目前應用最廣泛的軌跡規(guī)劃方法。Luo等[3]為應對車輛在向目標車道換道時出現(xiàn)不可預測的突發(fā)狀況，基于5次多項式設計了智能車輛換道過程中可以返回原始車道的軌跡規(guī)劃曲線。曲線插值軌跡規(guī)劃方法靈活，計算速度快，但受曲線形狀的限制，不能保證軌跡的最優(yōu)性和行駛可行性。為解決上述問題，Nilsson等[4]以換道距離和換道時間作為換道軌跡的優(yōu)化變量，用2個凸二次規(guī)劃進行最優(yōu)求解，最后輸出最優(yōu)換道軌跡。Li等[5]把車輛期望位置、期望速度、期望加速度作為優(yōu)化目標，求解出連續(xù)的換道軌跡。在換道場景這種不確定動態(tài)環(huán)境路徑規(guī)劃中，人工勢場法具有很大的優(yōu)勢。Malone等[6]在機器人路徑規(guī)劃下引入虛擬勢力場的概念，建立目標點和障礙物虛擬力場模型，產生隨機的點集，通過建立好的虛擬力場驗證點集中的點是否可行，最終將可行的點連接在一起作為優(yōu)化軌跡。

規(guī)劃出避障軌跡后，是否能及時避開障礙物的關鍵在于車輛是否能按照設計的避障軌跡行駛，因此，需要對軌跡跟蹤進行研究。由于模型預測具備預測模型、滾動優(yōu)化及反饋校正的特點，近年來被廣泛應用于車輛的軌跡跟蹤系統(tǒng)中。龔建偉等[7]在離線軌跡規(guī)劃的基礎上增加了基于車輛點質量模型的MPC(model predictive control)避障二次軌跡規(guī)劃，此方法可有效避開障礙物，但點質量模型無法對車輛施加動力學約束，可能會影響軌跡跟蹤效果。陳虹等[8]基于模型預測提出了面向動態(tài)避障的智能汽車滾動時域軌跡規(guī)劃方法，在避障函數(shù)中引入勢場法，可有效在復雜場景避開靜態(tài)及動態(tài)障礙物，但車輛前輪轉角波動較大。

為避免規(guī)劃出的軌跡不滿足軌跡跟蹤系統(tǒng)的動力學約束，將避障軌跡規(guī)劃和軌跡跟蹤融合在一個集成控制器中，同時進行避障規(guī)劃和軌跡跟蹤。以區(qū)域虛擬力場對動態(tài)換道場景進行模擬，包含本車道和相鄰車道障礙車周圍的虛擬矩形斥力場。建立3自由度車輛動力學模型為預測模型，以主車與障礙車無碰撞、跟隨期望軌跡行駛以及保證車輛穩(wěn)定性作為優(yōu)化目標，綜合系統(tǒng)的控制輸入及狀態(tài)變量等約束條件，以車輛前輪轉角為控制量，設計基于模型預測的滾動優(yōu)化避障規(guī)劃及跟蹤控制器。通過Matlab/Prescan軟件聯(lián)合仿真平臺對動態(tài)避障算法進行仿真驗證。

1 車輛模型

采用四輪車輛模型分析車輛在運動過程中的受力情況，為簡化計算，假設：① 車輛在水平路面行駛，不考慮橫向和縱向空氣阻力以及道路阻力；② 不考慮車輛沿z軸的垂向運動和繞y軸的俯仰運動；③ 車輛輪胎完全一致，且輪胎側偏特性為線性。滿足以上條件的車輛動力學模型如圖1所示，其中，XOY為大地坐標系，xoy為車體坐標系，規(guī)定x軸正向為車頭方向，y軸為車體橫向。該模型為包含整車縱向、橫向和橫擺運動的3自由度模型。

圖1 3自由度車輛動力學模型示意圖

根據牛頓第二定律，分別得到，

縱向運動方程：

(1)

橫向運動方程：

(2)

橫擺運動方程：

(3)

(4)

車輛在全局坐標系下的運動可以表示為：

(5)

2 障礙物區(qū)域虛擬力場

車輛換道行為是一個復雜的動態(tài)過程，為安全有效地完成換道任務，需要根據時刻變化的交通場景做出自適應的操作。當智能車輛行駛在高速公路上，發(fā)現(xiàn)車輛前方有低速障礙車輛，為獲取更好的駕駛體驗，主車向左轉換到高速車道行駛。本文在“車-路”系統(tǒng)上建立障礙物區(qū)域虛擬斥力場，車輛在換道過程中要同時考慮當前車道和相鄰的障礙車輛，因此，針對當前車道和相鄰車道分別設計了對應的虛擬斥力場，其作用域如圖2所示。

圖2 區(qū)域虛擬力場作用域示意圖

圖2中，紅色區(qū)域為障礙車產生的虛擬斥力場矩形作用域，對進入作用域內的車輛產生虛擬斥力，其虛擬作用域的大小由Ds1、Ds2、Ds3、Ds4、Ds5決定。其中Ds1、Ds2為障礙車輛質心為起點沿車道行駛反方向和正方向作用域長度；Ds3為與主車S在同一車道線的障礙車輛O1i作用域寬度；Ds4、Ds5分別為主車S相鄰車道障礙車O2i以質心為起點遠離主車方向的作用域寬度和靠近主車方向的作用域寬度。

為保證車輛在進入虛擬力場內有充足的避障操作時間，基于車輛最小安全距離模型[10]，進行障礙車虛擬力場區(qū)域長度設計，Ds1、Ds2表達式為：

(6)

(7)

式中：d0為兩車靜止時需要保持的縱向最小距離，根據經驗選取為3 m；aoi為障礙車輛最大制動減速度；as為主車最大制動減速度；voi為障礙車輛行駛速度；vs為主車行駛速度；Ts1、Ts2為最小安全距離調節(jié)因子，取1～1.2。

當前車道虛擬場寬度Ds3與相鄰車道虛擬場寬度Ds4、Ds5，具體計算公式為：

(8)

式中：l為車輛寬度；l0為兩車橫向需要保持的最小距離，根據經驗選取為0.5 m；droad為車道寬度。

主車距離障礙車越近，越危險，斥力的大小與相對距離成反比；相對速度越大，發(fā)生碰撞時間越短，危險程度越高，斥力的大小與相對速度成正比。據此建立的虛擬斥力為：

(9)

由式(9)可知，當主車行駛至障礙車虛擬斥力場區(qū)域內，f≠0，此時斥力場開始發(fā)揮作用；當主車未駛進斥力場區(qū)域時，f=0，斥力場不起作用。

3 模型預測控制器設計

模型預測包含預測模型、目標函數(shù)及約束條件3個部分。通過建立的車輛3自由度動力學方程推導出預測方程，將上述虛擬斥力場方程導入到目標函數(shù)，以盡可能小地偏離原始期望軌跡、控制量方向盤轉角輸入及其增量不至過大。作為控制目標，通過對不同目標添加權重因子，將多目標問題轉換成單目標問題。考慮車輛的行駛安全性及穩(wěn)定性問題，對車輛前輪轉角、橫擺角及側向加速度施加動力學約束。

3.1 預測模型推導

將上述車輛3自由度動力學方程整合如式(10)：

(10)

(11)

根據參考文獻[11]，將式(11)系統(tǒng)轉換為線性時變系統(tǒng)方程：

(12)

對式(12)進行近似離散化處理為：

(13)

式中：T為采樣時間。

對式(13)進行綜合可得：

ξ(k+1)=Ak，tξ(k)+Bk，tu(k)

(14)

對式(14)做如下轉換：

(15)

得到新的狀態(tài)空間表達式：

(16)

式中：

Nu為輸入量個數(shù)；Ny為輸出量個數(shù)。

為了簡化計算，令Ak，t=At，t，Bk，t=Bt，t，通過推導可以得到系統(tǒng)的預測輸出表達式

Y(t)=ψtξ(t|t)+ΘtΔU(t)+ΓtΦ

(17)

3.2 目標函數(shù)設計

設計3個優(yōu)化目標，為軌跡無障礙時車輛能跟蹤上期望軌跡、車輛在軌跡上有障礙時能及時避開障礙物(車)和行駛過程中保證車輛穩(wěn)定性。根據以上優(yōu)化目標分別建立優(yōu)化函數(shù)。

為保證車輛在前方無障礙時能按照期望軌跡行駛，參照文獻[12]所使用的軟約束方法，其優(yōu)化函數(shù)表達式為：

(18)

為保證車輛不與障礙物發(fā)生碰撞，以主車所受勢場力最小為優(yōu)化目標，其優(yōu)化函數(shù)為：

(19)

式中：Nobs為主車進入障礙車斥力場區(qū)域的數(shù)量；其他符號含義與式(9)相同。

在滿足前輪轉角動力學約束的情況下，為了防止出現(xiàn)急轉現(xiàn)象，以前輪轉角增量最小為優(yōu)化目標建立優(yōu)化函數(shù)表達式：

(20)

式中：Nc為控制時域。

綜合式(18)-(20)，得到最終目標函數(shù)：

J=J1+J2+J3+ρε2

(21)

式中：ρ為松弛因子權重系數(shù)；ε為松弛因子。

3.3 約束條件

為保證在高速公路上路徑跟蹤時的行駛安全性及避障的可靠性，設置以下約束：

前輪轉角為：-δfmin≤δf≤δfmax；

轉角增量為：-Δδfmin≤Δδf≤Δδfmax；

側向加速度為：-aymin≤ay≤aymax；

橫擺角為：-φmin≤φ≤φmax；

3.4 二次規(guī)劃求解

綜上所述，MPC避障跟隨控制器可歸納為如下優(yōu)化問題：

(22)

(23)

式中：ΔU為控制量增量；ΔUmin、ΔUmax為增量約束極限值；U為控制量；Umin、Umax為控制量約束極限值；Y為系統(tǒng)輸出量；Ymin、Ymax為輸出量約束極限值。當每個控制周期內完成對上式的求解后，將控制序列的第一個元素作為實際的控制輸入增量作用于系統(tǒng)：

U(k)=U(k-1)+ΔU

(24)

4 仿真分析

為驗證所設計的避障跟蹤算法的有效性，在Matlab/Simulink建立智能車輛避障跟蹤控制模型，在Prescan中搭建高速公路動態(tài)換道場景，以Prescan中AudiA8作為被控對象，針對高速公路避障任務進行動態(tài)障礙避障仿真實驗。車輛整車主要參數(shù)及MPC控制參數(shù)，如表1所示。

表1 車輛整車主要參數(shù)及道路MPC控制參數(shù)

表2中，Np為模型預測控制預測時域，Nc為控制時域，Ts為離散步長。通過對避障效果、軌跡跟蹤能力及模型運算時間3個方面的綜合考慮，最終確定2種控制器參數(shù)均為Np=50，Nc=5，Ts=0.02。

表2 控制器參數(shù)

4.1 相鄰車道無車輛工況

工況1為主車S行駛在高速公路第三車道，其前方有車輛PB1和PB2，而相鄰車道無行駛車輛，如圖3所示。主車S所帶傳感器能探測到前方車輛PB1，主車S無法探測到被PB1所阻擋車輛PB2。行駛工況為：主車S行駛速度高于車輛PB1，為追求行駛效率，在行駛過程中會向左換道；換道過程中，前方車速較高車輛PB2也向第二車道換道，主車S和PB2在換道過程中可能發(fā)生碰撞，驗證此時控制器的避障性能。該工況下各車輛具體位置數(shù)據見表3。

圖3 相鄰車道無車輛

表3 換道避障工況參數(shù)

圖4為車輛換道避障過程中的狀態(tài)參數(shù)曲線，圖4(a)為主車S及周圍車輛行駛軌跡，由圖中可以看出，主車S沿第三車道行駛至2.64 s時，主車進入前車PB1的虛擬勢場作用域中，根據判斷，主車S開始向第二車道進行換道，換道至4.5 s時，前車PB2也開始向第二車道進行換道，此時主車S未進入車輛PB2的虛擬勢場作用域中，主車S會繼續(xù)換道動作，至6.06 s主車S進入車輛PB2的虛擬勢場作用域中，為防止與車輛PB2發(fā)生碰撞，主車駛向第一車道進行躲避，當車輛PB2完全換道至第二車道，主車S判斷安全后，會返回至第二車道行駛。圖中各車換道軌跡曲線上的黑色圓圈O圈出的點為主車開始避障時刻(6.06 s)各車的位置，可見在此換道過程中主車完成了安全避障。

圖4(b)(c)(d)(e)分別為車輛行駛軌跡、前輪轉角、橫向加速度、橫擺角及橫擺角速度曲線。從圖中可以看出，車輛在換道避障過程中，前輪轉角、橫向加速度及橫擺角在6.06 s由于主車躲避車輛OBS發(fā)生較大波動，但整個過程都處于設定的約束范圍內，橫擺角速度曲線也較為光滑，表明車輛在換道避障處于一個安全穩(wěn)定的過程。

圖4 動態(tài)避障車輛狀態(tài)參數(shù)

4.2 相鄰車道有車輛工況

工況2為主車S行駛在第三車道，其前方有相對速度較低的車輛PB，第二車道前、后車輛PA和FA，在第一車道有行駛車輛FC。如圖5所示，其行駛工況為：主車S在行駛過程中，由于前車PB速度較低，主車S有向左換道意愿，將插入到車輛PA和FA之間，在第一車道車輛FC在此過程中也向第二車道換道可能會出現(xiàn)主車S與車輛FC碰撞情況。該工況下各車輛行駛參數(shù)如表4所示。

表4 換道避障工況參數(shù)

圖5 相鄰車道有車輛

圖6(a)為主車S和周圍車輛的行駛軌跡曲線，從圖中可以看出，起始主車S沿第三車道行駛，行駛至4 s時刻，主車S進入前車PB的虛擬勢場作用域，開始向第二車道進行換道操作，換道至4.5 s時車輛FC也向第二車道進行換道，由于其被車輛FA遮擋，導致無法被車輛S探測。主車S繼續(xù)行駛至6.44 s時，主車S發(fā)現(xiàn)并進入車輛FC的虛擬勢場作用域中，且車輛FC速度較快，為躲避車輛FC，主車S返回至第三車道進行避障操作，當車輛FC完全換道至第二車道時，主車繼續(xù)向第二車道行駛進行換道操作。圖中各車換道軌跡曲線上的黑色圓圈O圈出的點為主車開始避障時刻(6.44 s)各車的位置，可見在此換道過程中主車完成了安全避障。

圖6(b)(c)(d)(e)分別為前輪轉角、橫向加速度、橫擺角和橫擺角速度。從圖中可以看出，車輛前輪轉角、橫向加速度和橫擺角都在6.44 s時產生較大波動，但都處于約束范圍內，表明換道躲避障礙過程中主車處于平順、安全、穩(wěn)定的狀態(tài)。相對于工況1，此工況車輛狀態(tài)參數(shù)存在頻繁波動，是由于此工況包含車輛較多，障礙車車速較高，避障過程相對復雜，而模型預測避障算法在每一時刻都會進行滾動優(yōu)化求解最優(yōu)前輪轉角，因此，隨著周圍環(huán)境的變化，前輪轉角變化相對頻繁。

圖6 動態(tài)避障車輛狀態(tài)參數(shù)曲線

5 結論

1) 基于虛擬勢場法和模型預測理論設計了包含避障功能和軌跡跟蹤功能的滾動優(yōu)化控制器。

2) 將避障功能和軌跡跟蹤功能融合在一個控制系統(tǒng)內，避免避障軌跡規(guī)劃不滿足車輛動力學約束問題，減少系統(tǒng)運算時間，保證車輛及時有效地避開障礙物。

3) 避障跟蹤控制器在高速換道過程中可快速有效地避開動態(tài)障礙車，滿足車輛動力學約束，保證車輛的行駛安全性和穩(wěn)定性。

在設計避障及軌跡跟蹤控制器時采用包含車輛縱向動力學的3自由度模型作為預測模型，其控制器輸出量為前輪轉角和車速。為體現(xiàn)所設計的動態(tài)避障效果，把車速作為常值，后續(xù)研究將對主車的縱向速度進行控制。