基于滾動優化的汽車換道動態軌跡規劃和軌跡跟蹤控制

岳麗姣，時 利，孟建平

（1.安徽江淮汽車集團股份有限公司 智能網聯汽車研究院，安徽 合肥 230601；2.合肥工業大學 汽車與交通工程學院，安徽 合肥 230009）

引言

自動換道控制作為自動駕駛技術的核心功能模塊之一，已成為智能汽車領域的重點[1]。實現自動換道控制的基礎是進行換道軌跡規劃與軌跡跟蹤控制[2]。

為實現換道控制，一些學者使用AI*算法[3]、RRT[4]等基于搜索的方法，雖然這類方法可以在復雜的道路環境下規劃出無碰撞軌跡，但是沒有考慮汽車的非完整約束，可能導致汽車無法跟蹤規劃出的軌跡。為獲得局部最優軌跡，Huang等[5]將人工勢場（APF）與MPC 相結合通過模型預測控制的優化算法來優化選擇人工勢場定義下的規劃軌跡。雖然該方法能實現換道避礙，但是由于道路勢場在兩條車道中心處最低，換道時參考量相當于從當前車道中心突變為目標車道中心，從而導致側向加速度較大，舒適性較差。基于多項式的換道軌跡模型具有形式簡單、曲率連續、可以靈活調整軌跡等優點[6]，能有效地解決上述問題。如LIE 等[7]學者在無障礙物環境下利用多項式規劃一條車道變換軌跡，基于積分反步法設計軌跡跟蹤控制器，實現安全換道。Yang[8]等根據換道起點和終點的邊界條件結合考慮避障設置的約束，求取多項式參數，獲得換道軌跡。大多基于多項式換道軌跡的研究中，軌跡模型雖然考慮了與周圍車輛避撞的問題，但只是在換道初始時刻進行了參數計算，不能避免整個換道過程中因周圍車輛狀態發生變化而造成的碰撞危險[9]。為此，Chebly Alia 等[10]在基于多項式軌跡的基礎上，引入基于回旋曲線的軌跡簇，并從中篩選出一條無碰撞軌跡，但是局部軌跡是否合適取決于軌跡簇的待選軌跡條數，軌跡條數越多，選出的軌跡越接近最優軌跡，但待選軌跡條數的增加會導致計算量增大，換道系統的實時性變差。

為使換道車輛能有效的應對前方車輛的狀態變化，本文提出了一種基于滾動優化的換道動態軌跡規劃和軌跡跟蹤控制方法。所設計方法的結構如圖1 所示，其主要由換道距離模型、換道參考軌跡模塊、動態軌跡生產模塊以及模型預測軌跡跟蹤控制器組成。換道距離模型用于判斷車輛是否需要換道。換道參考軌跡模塊基于車-路信息，利用五次多項式的軌跡方程和由換道軌跡評價指標確定的換道時間共同構建出換道參考軌跡。動態軌跡生產模塊以換道動態軌跡規劃函數得到的動態軌跡和參考軌跡的偏離最小化以及相鄰周期內所規劃軌跡的偏差最小化為目標，設計滾動優化函數，結合包絡橢圓約束、道路邊界約束和穩定性約束，規劃出動態換道軌跡。設計的模型預測軌跡跟蹤控制器以規劃的動態軌跡為目標，計算理想的前輪轉角。最后由電機控制器控制車輛轉向。

圖1 換道控制方法的結構

1 車輛模型

本文采用包含沿y 軸方向側向運動和繞z 軸橫擺運動的二自由度的車輛模型。車身二自由度模型如圖2 所示，其動力學模型可表示為：

圖2 線性二自由度汽車模型

式中，β為車輛質心側偏角；ω為橫擺角速度；m為整車質量；Cf、Cr分別為前、后輪側偏角剛度之和；lf、lr分別為質心到前后軸之間的距離；Iz為整車繞z 軸的轉動慣量；δf為前輪轉角。

在地面坐標系下，汽車運動學微分方程如下：

式中：x、y分別為車輛縱向和側向坐標；φ為汽車航向角。在車輛穩定狀態下，由二自由度汽車穩態轉向模型得到車輛穩態橫擺角速度:

2 換道動態軌跡規劃設計

2.1 換道距離模型

當同車道前方存在低速障礙車輛時，為保證行車安全并提高通行效率，如果目標車道有充足的換道空間，被控車輛與前方障礙車輛之間的距離達到換道請求距離時，被控車輛產生換道需求，并進行換道參考軌跡規劃。換道請求距離為：

其中，εv為道路法規規定的最小行車間距。如圖3 所示，為了保證換道過程中的安全性，換道結束時在縱向上與原車道前方障礙車輛仍留有一定的安全裕度值D3，則：

圖3 換道距離

式中，v為自車車速，vobs為前方障礙車輛車速，T1為換道時間。

2.2 換道參考軌跡規劃

以換道起始時汽車質心為坐標原點，沿車身縱軸方向為x軸，沿橫軸方向為y軸。要求規劃出的軌跡能保證車輛側向速度和側向加速度連續，由換道起止點的6 個邊界條件，可以確定多項式軌跡的6 個系數，所以本文采用5 次多項式來描述側向位移軌跡：

換道的起始和終止時刻汽車側向位移、側向速度、側向加速度分別為：

其中，H為車道寬度。結合公式（7）中邊界條件，可求得換道參考軌跡的系數：

將所求系數代入到公式（9）中并對其進行二階求導，可得到換道過程中的側向加速度，即：

則側向加速度最大值和最小值對應的時刻為：

為保證汽車在跟蹤換道參考軌跡過程中的穩定性，對側向加速度進行約束，側向加速度滿足約束條件：

聯合公式（13）-（15），可求得換道時間T1應滿足：

本文中取ay,max=－ay,min，=－ay,min=0.4g，即可求得最小換道時間T1min=2.35s。將T1min代入公式（8），求得滿足側向加速度約束的最小換道安全距離Smin。

換道時間越短，最大側向加速度越大，換道時的乘坐舒適性變差；但換道時間越短，換道效率越高。為了綜合行車效率和換道乘坐舒適性，構建換道軌跡評價指標：

換道軌跡評價指標隨換道時間的變化曲線如圖4 所示，本文選擇換道時間為T1=3.6s。

圖4 換道軌跡評價指標

2.3 換道參考軌跡規劃

2.3.1 換道動態軌跡規劃函數

為了保證換道過程中側向加速度的連續性，較好的擬合出換道軌跡，結合當前采樣時刻三個狀態，要求動態軌跡多項式的階數n≥3。階數越高，擬合精度越好，但隨階數的增加，待優化參數數量增加，實時性變差。綜合考慮擬合精度和實時性要求，本文采用四次多項式來表示動態換道軌跡：

當前采樣時刻t0，結合汽車運動方程（5），則當前被控車輛橫向行駛狀態為：

即可得到：

其中：

則動態換道軌跡為：

式中，f（t）=[1t t2]，g（t）=[t3t4]。即可將u作為優化變量，實現對軌跡的優化。

2.3.2 滾動優化目標函數設計

為使動態規劃的軌跡更接近換道參考軌跡，以保證汽車在換道過程中的舒適性和效率，對規劃時域T內動態軌跡與參考軌跡差值的平方進行積分，從而獲得與參考軌跡偏差的代價函數：

動態換道軌跡與參考軌跡偏差越大，其代價值越高。但僅以上述代價函數作為優化目標函數，則可能出現相鄰兩次規劃周期內所規劃的軌跡出現較大偏差，從而造成軌跡跟蹤時方向盤出現較大抖動，為避免上述問題，對本次規劃周期內規劃的軌跡與上個規劃周期內規劃的軌跡偏差的平方進行積分，從而減小相鄰規劃周期內軌跡之間的偏差：

其中：ypref（t）為上個規劃周期內規劃出的軌跡。最終得到優化目標函數：

并可以離線求出滾動優化函數：

其中，ξ為與u無關的參數。

2.3.3 障礙車隨動危險區域包絡橢圓

實際情況下汽車行駛狀態是連續變化的，短時間內不存在突變。基于此預測未來一段時間T內障礙車位置：

被控汽車在時間T內的縱向坐標為：

式中，x0為當前時刻被控車輛縱向坐標。以障礙車位置為橢圓圓心，以最小換道安全距離Smin為長半軸，對障礙車構建跟隨其運動的危險區域包絡橢圓，其邊界為：

式中，Sy為橫向安全距離。

2.3.4 約束條件

為防止與前方障礙車發生碰撞，將動態規劃出的軌跡約束在前車隨動危險包絡橢圓范圍之外，即當｜x（t）－xobs（t）｜＜Smin時有：

假設被控車輛可以理想的跟蹤動態規劃的軌跡，則換道過程中的側向加速度為：

為規避跟隨軌跡過程中因側向加速度過大而造成的失穩可能，對汽車理想跟隨軌跡過程中的側向加速度進行約束，約束范圍為：

為避免汽車駛出道路，將動態規劃軌跡約束在道路安全區域范圍內，防止被控車輛駛出車道邊界或進入對向車道。

式中w為被控車輛寬度。將公式（18）分別代入以上約束公式中，即可求得對u的不等式約束，通過求解帶約束二次規劃問題，即可求得優化變量u，并將其代入公式（21），得到優化后的未來一段時間T內最優軌跡。

3 基于模型預測的軌跡跟蹤控制器

為準確的跟蹤規劃出的動態軌跡，設計基于模型預測的軌跡跟蹤控制器來獲得理想的前輪轉角。結合公式（4）、（5），以X=[y φ ω β]T為系統狀態量，以前輪轉角δf為控制量uc，可得離散化后的線性時變系統狀態方程：

為保證控制量的連續性，防止控制量突變，需要對控制量增量進行約束，為方便處理，采用控制增量代替原控制量作為系統控制輸入。并對系統狀態量進行如下轉換：

轉換后新的系統控制輸入為：

得到新的離散線性化狀態方程：

其中，η（k｜t）=Y（k｜t）。設置控制器預測時域Np=8，控制時域Nc=5。

系統控制增量是未知的，通過設定適當的優化目標函數，對其進行優化求解，才能得到控制時域內的控制序列，并將控制序列中第一個元素作為實際控制增量作用于系統，實現車輛軌跡跟蹤控制。根據參考文獻[5]，構建以下形式的優化目標函數：

其中，Q 和R分別為權重矩陣。第一項反應軌跡跟蹤能力；第二項反應對控制量平穩變化的要求；第三項為在目標函數中加入的松弛因子，防止無可行解的情況。考慮對控制量和控制增量的約束，控制量即前輪轉角應被約束在機械結構所允許的最大范圍之內，則有：

根據參考文獻[10]并結合本文離散化周期，取△ucmax=－△ucmin=1.7deg。

4 仿真與試驗

為準確地評估所設計的換道控制方法的有效性，進行CarSim/Simulink 硬件在環試驗。根據設計的換道控制方法，進行前車緊急制動三種情況下的車輛換道硬件試驗。采用基于LabVIEW/CarSim 聯合的自動駕駛試驗平臺進行車輛換道的硬件在環試驗研究。硬件在環試驗臺如圖5 所示。

圖5 硬件在環試驗平臺（換成LAC 的試驗臺）

試驗中使用轉向臺架的實際轉角代替計算到的期望方向盤轉角，設置被控車輛車速為80 km/h、路面附著系數為0.85、仿真時間為6 s。在前方障礙車輛緊急制動情況下進行換道試驗。

從圖6（a）、（b）和（d）中可以看出，有動態軌跡規劃的換道過程相較于無動態軌跡規劃的換道過程，由于受到前方障礙車輛的橢圓危險包絡橢圓約束，方向盤轉角明顯增大，車輛實際軌跡較大的偏離了換道參考軌跡，從而避免換道過程中自車與前車發生碰撞。從圖6（c）中可以看出，在所設計的軌跡跟蹤控制器的作用下，被控車輛的軌跡誤差在換道過程中均保持在較小的范圍內。另外，由圖6（e）可知，換道過程中車輛的側向加速度基本保持在約束的范圍內，且變化平穩。

圖6 硬件在環試驗結果

從硬件在環試驗的結果分析可知，在基于滾動優化的汽車換道動態軌跡規劃和軌跡跟蹤控制方法的作用下車輛具有良好的應對不同換道環境的能力。

5 結論

（1）針對換道過程中前車狀態變化問題，本文在規劃出換道參考軌跡的基礎上，基于滾動優化思想，設計了以規劃軌跡和參考軌跡偏差最小化為目標的動態軌跡規劃方法，并基于模型預測控制設計了軌跡跟蹤控制器。

（2）對所設計的換道控制方法在前車緊急制動工況下在基于CarSim/labview 的自動駕駛試驗平臺進行了硬件在環試驗。硬件在環試驗結果表明，本文設計的基于滾動優化的汽車換道動態軌跡規劃和軌跡跟蹤控制方法能使車輛有效的應對不同換道環境，在前車狀態發生改變的情況下，能夠保證車輛的換道安全性和舒適性。