基于雙五次多項式的智能汽車換道軌跡規(guī)劃*

牛國臣，李文帥，魏洪旭

（中國民航大學(xué)機(jī)器人研究所，天津 300300）

前言

車道變更作為智能汽車自動駕駛時最常見的駕駛行為之一，在很大程度上影響道路的流通性和安全性。安全、快速且平穩(wěn)的換道行為可提高通行效率，減少交通事故的發(fā)生［1］。合理的換道軌跡是智能汽車安全完成換道任務(wù)的前提，同時其性能也決定了換道過程的安全性、高效性和舒適性。

常用的智能汽車軌跡規(guī)劃算法［2］包括基于搜索的算法［3］、基于彈性帶理論［4］和基于參數(shù)化曲線的算法［5］等。前兩類算法所得到的解不一定是全局最優(yōu)解，并且實時性較差，不能滿足智能汽車高實時性的要求。基于參數(shù)化曲線算法中的參數(shù)化曲線包括：圓弧曲線［6］、螺旋曲線、Bezier曲線［7］和多項式曲線［8］等。該類算法具有運(yùn)算量小、直觀、精確等優(yōu)點，也被廣泛應(yīng)用于智能汽車的換道軌跡規(guī)劃研究中。文獻(xiàn)［9］中針對高速道路上的換道行為提出了基于高斯分布的換道模型和最小安全距離模型，用于換道軌跡規(guī)劃，并混合了決策和控制理論，提出了基于自適應(yīng)巡航控制（ACC）的軌跡跟蹤控制方法用于軌跡跟蹤，取得了很好的跟蹤效果，但該方法得到的軌跡的橫向速度和橫向加速度較大。文獻(xiàn)［10］中采用一種基于阻力網(wǎng)絡(luò)的新路徑規(guī)劃方法完成車道保持或變更等行為，其中引入類人因素模擬不同類型的駕駛者，如積極型、溫和型和保守型。但該方法的規(guī)劃時間較長，實時性較低。文獻(xiàn)［11］中提出了加速換道模型，將道路上的交通流進(jìn)行分組，并在V2V的幫助下進(jìn)行多車協(xié)同的換道軌跡規(guī)劃。但該方法對交通環(huán)境和車輛硬件要求比較苛刻，并不能廣泛應(yīng)用。文獻(xiàn)［12］中利用三次多項式得到軌跡簇，利用聚類的思想進(jìn)行多目標(biāo)優(yōu)化，從中選取最優(yōu)軌跡，但該方法計算繁瑣，換道舒適性不高。文獻(xiàn)［13］中結(jié)合人工勢場法（APF）和彈性帶理論，能在多種道路環(huán)境下完成路徑規(guī)劃，并保證軌跡的平滑性，但未考慮車輛在始末時刻航向角的變化過程。文獻(xiàn)［14］中提出一種基于多項式函數(shù)的軌跡規(guī)劃算法，設(shè)置了加速度約束，建立了速度邊界和路徑時間最優(yōu)的多項式參數(shù)化仿真模型，為后續(xù)的動力學(xué)模型研究提供參考。文獻(xiàn)［15］中在車輛運(yùn)動學(xué)方程的基礎(chǔ)上提出了一種基于六次多項式的軌跡規(guī)劃算法來考慮避障，但該算法的階次較高，待定系數(shù)較多，計算復(fù)雜且收斂速度較慢。文獻(xiàn)［16］中對其他學(xué)者所研究的換道軌跡的性能進(jìn)行了對比，最終結(jié)論為使用五次多項式規(guī)劃的換道軌跡綜合性能最優(yōu)。文獻(xiàn)［17］中提出一種基于五次多項式的算法，該算法通過增加階次避開障礙車實現(xiàn)換道，此算法具有較好的實時性，但過程較復(fù)雜且計算量大。文獻(xiàn)［18］中采用3?D Bezier曲線進(jìn)行換道的路徑規(guī)劃，并將換道的路徑規(guī)劃分為兩段，避免換道過程中與障礙物發(fā)生碰撞，再進(jìn)行速度規(guī)劃，使換道過程的速度滿足一系列約束，但將軌跡規(guī)劃分為路徑規(guī)劃和速度規(guī)劃會忽略一部分的運(yùn)動學(xué)和動力學(xué)特性。

綜上所述，五次多項式被廣泛用于換道軌跡規(guī)劃，在避障換道時普遍采用增加階次的方法避免發(fā)生碰撞，但該方法計算繁瑣，規(guī)劃時間增加，安全性降低。為解決以上問題，本文中提出了基于雙五次多項式的智能汽車換道軌跡規(guī)劃算法，通過提出的舒適性約束改進(jìn)五次多項式規(guī)劃算法，增加換道過程的舒適性，采用雙五次規(guī)劃的換道策略，保障換道過程安全性的同時又提高了規(guī)劃算法的實時性。

1 無障礙換道軌跡規(guī)劃

進(jìn)行換道軌跡規(guī)劃時，首先分析無障礙換道軌跡規(guī)劃，此時換道車周圍無其他車輛，如圖1所示。

圖1 無障礙換道軌跡規(guī)劃示意圖

圖中C0是換道車，換道過程為C0車從本車道的中心線行駛到相鄰車道的中心線上。僅根據(jù)換道車的起始狀態(tài)Sin和Sfin目標(biāo)狀態(tài)，經(jīng)計算便可得到一條光滑的換道軌跡［19］：

基于五次多項式的特性，分別在x方向和y方向選取五次多項式表示換道軌跡函數(shù)：

定義時間參數(shù)矩陣：

式中：tin為換道初始時刻；tfin為換道完成時刻。

定義系數(shù)矩陣分別為

為保證換道車的平穩(wěn)運(yùn)動，須為軌跡函數(shù)提供以下邊界條件：

式中T為換道過程持續(xù)的時間。

結(jié)合邊界條件和約束條件，再聯(lián)合式（2）、式（3）、式（6）和式（7）可求出系數(shù)矩陣A、B：

利用求得的系數(shù)矩陣，求式（7）解方程組，即可得到無障礙換道時智能汽車的換道軌跡。

為提高換道過程的舒適感，本文中結(jié)合實際運(yùn)動過程中車輛動力學(xué)特性的約束［20］、GB/T13441.1—2007［21］中關(guān)于舒適性感受的近似描述和加加速度對乘車舒適性的影響［22］，提出了如下?lián)Q道過程的舒適性約束。

（1）設(shè)置換道軌跡的加速度(m/s2)約束：

（2）對換道軌跡的加加速度j(m/s3)的最值進(jìn)行約束，保證換道過程中速度變化平滑和較好的舒適感。例如無障礙換道時：

在舒適性約束的基礎(chǔ)上重新調(diào)整換道時間，改進(jìn)五次多項式規(guī)劃，優(yōu)化軌跡性能直至滿足約束，如圖2所示。

圖2 改進(jìn)五次多項式

具體的無障礙換道軌跡規(guī)劃步驟如圖3所示。

圖3 無障礙換道軌跡規(guī)劃步驟

2 避障換道軌跡規(guī)劃

智能汽車行駛在結(jié)構(gòu)化道路上時，主要的障礙物為交通車輛，如圖4所示，則避障換道規(guī)劃主要考慮如何避免換道車C0在換道過程中與交通車C1發(fā)生碰撞。

圖4 避障換道狀況示意圖

針對避障換道的情況，本文中提出了雙五次多項式換道軌跡規(guī)劃算法，將換道過程分為兩個改進(jìn)五次多項式規(guī)劃過程，完成首次規(guī)劃時，換道車到達(dá)預(yù)先設(shè)計的換道中轉(zhuǎn)位置，該次規(guī)劃的橫向位移大于交通車車身寬度，此后換道車進(jìn)行第二次規(guī)劃時與交通車在縱向上再無交集，避免換道過程中與交通車輛發(fā)生碰撞，簡化計算。該算法的思路如下。

（1）根據(jù)智能汽車的換道初始狀態(tài)Sin和交通車的當(dāng)前狀態(tài)Stf，計算出合適的中轉(zhuǎn)位置。

（2）根據(jù)換道車的換道初始狀態(tài)Sin和中轉(zhuǎn)位置的狀態(tài)Scin，進(jìn)行首次改進(jìn)五次多項式規(guī)劃，如圖5所示。

圖5 首次五次多項式軌跡規(guī)劃

（3）到達(dá)中轉(zhuǎn)位置后，根據(jù)換道車的中轉(zhuǎn)狀態(tài)Scin和換道目標(biāo)狀態(tài)Sfin進(jìn)行第二次改進(jìn)五次多項式規(guī)劃，得到換道軌跡，如圖6所示。

圖6 雙五次多項式軌跡規(guī)劃

避障換道軌跡規(guī)劃的具體流程圖如圖7所示。

圖7 避障軌跡規(guī)劃流程圖

首次改進(jìn)五次多項式規(guī)劃的橫向位移為1.8-2 m，略大于交通車的寬度，縱向位移和換道時間為待定系數(shù)，規(guī)劃軌跡滿足約束后會到達(dá)中轉(zhuǎn)位置，此后兩車不會再有碰撞的危險，第二次改進(jìn)五次多項式規(guī)劃等效為無障礙換道軌跡規(guī)劃，兩次規(guī)劃的結(jié)果即為規(guī)劃的軌跡。

雙五次多項式規(guī)劃的難點在于中轉(zhuǎn)位置的選取。中轉(zhuǎn)位置的選取須結(jié)合換道車的車速、交通車的車速、智能汽車與交通車輛之間的距離和智能汽車最終的位移等因素。綜上可得，軌跡規(guī)劃狀態(tài)如式（11）所示，交通車的狀態(tài)Stf如式（12）所示。

式中：vs為換道初始狀態(tài)的車速；xc、yc和vc為換道中轉(zhuǎn)狀態(tài)的橫向位移、縱向位移和速度；xf、yf和vf為換道目標(biāo)狀態(tài)的橫向位移、縱向位移和速度。

式中：vtf為交通車C1的車速（小于vs）；xtf為換道車與交通車之間距離；atf為交通車的加速度。

當(dāng)vtf=0時，即為障礙物靜止時的避障換道。此時障礙物不再移動，為了避免發(fā)生碰撞，須xc＜xtf，所以當(dāng)vtf=0時，完成換道需要的xtf，應(yīng)大于vtf＞0時的xtf。

為盡快地完成換道過程，取vc略大于vs：

式中λ為換道加速因子，取值與車速成反比。

假設(shè)C0車和C1車經(jīng)過t時間后兩車位置相同，可得式（14）和式（15）：

式中vt為t時間后的交通車車速。

對式（15）求解得到Tmax：

式中：a為0.5倍的交通車的加速度；b為交通車的車速vtf與換道初始狀態(tài)的車速vs之差；c為換道車與交通車之間距離xtf。

當(dāng)vtf＞0時，使用微元法的思想進(jìn)行分析，可得到如下等式：

式中T1為動態(tài)換道時間T的首項。

當(dāng)vtf=0時，為避免發(fā)生碰撞，取T1=0.9Tmax，則

智能汽車進(jìn)行第一階段規(guī)劃時，對于待定的中轉(zhuǎn)狀態(tài)的縱向位移xc、速度vc和換道時間T，取縱向位移xc如式（22）所示，速度vc如式（13）所示，換道時間T如式（24）所示。

式中Δt=0.1 s。

3 試驗仿真

設(shè)定不同的工況進(jìn)行仿真和實車試驗，驗證本文中所提算法的合理性。使用雙五次多項式算法規(guī)劃換道軌跡，并利用汽車動力學(xué)仿真軟件CarSim對換道軌跡進(jìn)行軌跡跟蹤，對規(guī)劃得到的規(guī)劃軌跡和軌跡跟蹤得到的實際軌跡性能進(jìn)行對比，驗證規(guī)劃軌跡的性能，換道相關(guān)參數(shù)如表1所示。

表1 換道相關(guān)參數(shù)

3.1 無障礙換道仿真

對于無障礙換道，根據(jù)車輛的運(yùn)動學(xué)特性，在換道起始時刻和結(jié)束時刻換道車的橫向、縱向加速度均為0，取起始狀態(tài)為（0，14，0，0，0，0），最終狀態(tài)為（100，14，0，3.75，0，0）。

使用改進(jìn)的五次多項式規(guī)劃算法進(jìn)行仿真試驗，求解得到系數(shù)矩陣：

得到的無障礙換道軌跡參數(shù)曲線如圖8所示。

從圖8中可以看出，換道軌跡平滑，橫向速度峰值小，換道平穩(wěn)性較好。該算法在橫向、縱向加速度方面的極值都較低，滿足約束，曲線也更加平滑。在加速度變化率和曲率方面，曲線波動范圍較小，證明該換道軌跡的舒適性較好。除此之外，整個換道規(guī)劃的時間約為8 ms，也滿足了高實時性的要求。

圖8 無障礙換道軌跡參數(shù)

3.2 避障換道仿真

對于避障換道，交通情況如圖4所示。智能汽車的換道場景如圖9所示，圖中的紅色虛線為CarSim換道場景中規(guī)劃的換道軌跡，紅色車輛為換道車，白色車輛為交通車。

圖9 雙五次換道軌跡跟蹤場景圖

（1）前車車速為非勻速行駛時的軌跡規(guī)劃

設(shè)定換道車起始時刻的速度為51 km/h，兩車之間的距離為30 m，前方交通車車速為43 km/h，交通車的加速度為-2 m/s2。首次五次多項式規(guī)劃求解得到系數(shù)矩陣分別為

第二次求解得到系數(shù)矩陣分別為

得到的仿真結(jié)果如圖10所示。

圖10 交通車變速時的換道軌跡參數(shù)

從圖10（a）和圖10（b）中可以看出，當(dāng)前方交通車的速度發(fā)生變化時，中轉(zhuǎn)位置在換道軌跡的后半段，且該條軌跡的跟蹤精度高。在圖10（c）中，軌跡的橫向速度曲線峰值約為1.0 m/s。由圖10（d）和圖10（e）可知在軌跡的加速度方面，橫向加速度和縱向加速度滿足加速度約束，波動范圍較低，故駕駛穩(wěn)定性較高。由圖10（f）可知在3.4 s時完成首次五次多項式規(guī)劃，此時兩車間距約為8.5 m，不存在碰撞風(fēng)險。此后第二次五次多項式規(guī)劃時，兩車在縱向方面沒有交集，故換道過程不存在碰撞風(fēng)險。

（2）換道車高速行駛時的軌跡規(guī)劃

設(shè)定換道車起始時刻的速度約為70 km/h，兩車之間的距離為20 m，前方交通車車速約為50 km/h，另增加了與文獻(xiàn)［18］中Chen Long等人的3?D Bezier規(guī)劃軌跡在橫向、縱向加速度方面的對比，得到的仿真結(jié)果如圖11所示。

圖11 高速情況的軌跡規(guī)劃

從圖11（a）中可以看出，軌跡跟蹤效果好，精度高。圖11（b）表明車輛能及時跟蹤到目標(biāo)速度并保持穩(wěn)定。在圖11（c）中，換道車輛的橫向速度變化曲線的峰值小于1.5 m/s。由圖11（d）可知，雙五次多項式的換道橫向加速度和軌跡跟蹤得到的橫向加速度的峰值均明顯低于3?DBezier的峰值，相比之下駕駛穩(wěn)定性較高。由圖11（e）可知，換道前段的軌跡跟蹤的換道縱向加速度較低，在換道后段縱向加速度波動較大，可以調(diào)節(jié)軌跡跟蹤的相關(guān)參數(shù)避免波動發(fā)生，且此時兩車在縱向已無交集，故不存在危險。由圖11（f）可知，在2.73 s時完成首次五次多項式規(guī)劃，此時兩車間距約為12 m，不存在碰撞風(fēng)險。此后第二次五次多項式規(guī)劃時，兩車在縱向方面沒有交集，故換道過程不存在碰撞風(fēng)險。

（3）換道車低速行駛時的軌跡規(guī)劃

設(shè)定換道車起始時刻的速度為36 km/h，兩車之間的距離為20 m，前方交通車車速為30 km/h，另增加了與文獻(xiàn)［18］中Chen Long等人的3?DBezier規(guī)劃軌跡在橫向、縱向加速度方面的對比，得到的仿真結(jié)果如圖12所示。

圖12 低速情況下的軌跡規(guī)劃

從圖12（a）中可以看出，跟蹤軌跡在整體趨勢下遵循參考的規(guī)劃軌跡，精度高。圖12（b）表明，車輛能及時跟蹤到目標(biāo)速度并保持穩(wěn)定。在圖12（c）中，軌跡的橫向速度曲線峰值小于1.0 m/s。由圖12（d）和圖12（e）可知，在軌跡的加速度方面，雙五次多項式規(guī)劃的加速度和軌跡跟蹤得到的加速度的峰值均明顯低于3?D Bezier規(guī)劃得到的峰值，故駕駛穩(wěn)定性較高。雖然延長了換道時間，但提高了換道舒適性。由圖12（f）可知，在4 s時完成首次五次多項式規(guī)劃，此時兩車間距約為7 m，不存在碰撞風(fēng)險。此后第二次五次多項式規(guī)劃時，兩車在縱向方面沒有交集，故換道過程不存在碰撞風(fēng)險。

（4）不同的前車距離和前車車速時的軌跡規(guī)劃

設(shè)定換道車以約70 km/h的速度行駛，設(shè)定不同的前車距離xtf和車速vtf，并對規(guī)劃得到的軌跡進(jìn)行跟蹤，如圖13和圖14所示。

在圖13中，設(shè)定兩車距離分別為15、20和25 m，前車的速度為50 km/h。圖13（a）表明，使用該算法規(guī)劃的軌跡合理。隨著兩車之間的距離改變，規(guī)劃的不同軌跡也都可以被跟蹤，偏差也都較小。由圖13（b）可知，完成首次五次多項式規(guī)劃時兩車間距均在4 m以上，可以避免發(fā)生碰撞。

圖13 不同前車距離時的換道軌跡跟蹤

在圖14中，前車的車速分別設(shè)定為30、40、50和0 km/h，初始時刻兩車的距離分別為20、20、20和50 m。圖14（a）表明，利用該算法規(guī)劃的軌跡是可行的。規(guī)劃得到的每條參考軌跡都能被高精度跟蹤。由圖14（b）可知，完成首次五次多項式規(guī)劃時兩車間距均在5 m以上，可以避免發(fā)生碰撞。

圖14 不同前車車速時的換道軌跡跟蹤

3.3 實車換道試驗

將該算法應(yīng)用于實際車輛的換道，交通情況如圖15所示。

圖15 換道交通情況

設(shè)定交通車靜止，換道車也由靜止開始換道，兩車之間的距離為30 m，進(jìn)行換道軌跡規(guī)劃并對規(guī)劃得到的軌跡進(jìn)行跟蹤，如圖16所示。

圖16 換道軌跡跟蹤

從圖16（a）中可以看出，跟蹤軌跡在整體趨勢下遵循參考的規(guī)劃軌跡，規(guī)劃的軌跡可跟蹤性高。圖16（b）表明，實際車輛的速度整體上可以跟蹤上規(guī)劃速度的變化趨勢。在圖16（c）中，軌跡的橫向速度曲線峰值小于1.0 m/s。由圖16（d）和圖16（e）可知在軌跡的加速度方面，雙五次多項式規(guī)劃的橫向、縱向加速度均滿足舒適性約束。軌跡跟蹤時除縱向加速度在從靜止開始運(yùn)動時瞬時值偏大外，其他時刻和平均值均符合約束。此外車輛在運(yùn)動過程中與交通車的距離均在3 m以上。

4 結(jié)論

本文中以智能汽車換道軌跡規(guī)劃研究為前提，對五次多項式規(guī)劃算法進(jìn)行了改進(jìn)，最后提出了基于雙五次多項式理論的智能汽車換道軌跡規(guī)劃算法。該算法的特點如下。

（1）動態(tài)規(guī)劃換道時間，使換道軌跡滿足舒適性約束，提高了換道過程中的舒適性。

（2）引入中轉(zhuǎn)位置并進(jìn)行兩次改進(jìn)的五次多項式規(guī)劃，避免有交通車輛時存在的碰撞危險，提高安全性。將中轉(zhuǎn)點的狀態(tài)選擇與換道車和交通車的狀態(tài)相結(jié)合，確保了中轉(zhuǎn)位置的合理性。

（3）對算法程序進(jìn)行優(yōu)化，增強(qiáng)算法的實時性，可以快速地得到一條安全并且舒適度較高的換道軌跡曲線。

試驗部分設(shè)計了包括前車變速行駛、換道車高速行駛、換道車低速行駛等多種工況的仿真試驗和交通車靜止時的實車換道試驗，其中規(guī)劃仿真與實車試驗的結(jié)果表明了雙五次多項式軌跡規(guī)劃算法在各種工況下都可以成功規(guī)劃出舒適的換道軌跡，并且在軌跡性能和運(yùn)行時間方面都進(jìn)行了優(yōu)化，提高了換道過程中的安全性并增加了換道的實時性。軌跡跟蹤的試驗結(jié)果驗證了該算法規(guī)劃得到的軌跡的合理性，也證明該軌跡的可追蹤性較高。綜上，本文中提出的軌跡規(guī)劃算法具有一定的實際應(yīng)用價值。