基于分段優(yōu)化的車(chē)輛換道避障軌跡規(guī)劃*

唐 斌，許占祥，江浩斌，蔡英鳳，胡子添，楊錚奕

（1.江蘇大學(xué)汽車(chē)工程研究院，鎮(zhèn)江 212013；2.江蘇大學(xué)汽車(chē)與交通工程學(xué)院，鎮(zhèn)江 212013）

前言

換道避障作為智能駕駛的基本功能，對(duì)提高車(chē)輛的行駛安全性、通行效率和智能化水平具有重要作用。換道避障軌跡規(guī)劃是連接智能車(chē)輛感知決策與跟蹤控制的重要環(huán)節(jié)，如何有效地規(guī)劃一條能夠確保車(chē)輛平穩(wěn)、舒適和安全地完成避障的軌跡，成為當(dāng)前智能駕駛領(lǐng)域的研究熱點(diǎn)。

車(chē)輛軌跡是一條帶有時(shí)間戳的路徑曲線(xiàn)，當(dāng)前對(duì)車(chē)輛的軌跡規(guī)劃方法研究大致分為以下4 類(lèi)：基于搜索的規(guī)劃方法、人工勢(shì)場(chǎng)法、基于隨機(jī)采樣的規(guī)劃方法和基于離散優(yōu)化的規(guī)劃方法等?；谒阉鞯囊?guī)劃方法是通過(guò)狀態(tài)格子將道路空間離散化，然后通過(guò)搜索算法來(lái)搜索最優(yōu)軌跡，常見(jiàn)的搜索算法有Dijkstr、A*、D*。A*通過(guò)啟發(fā)式函數(shù)使靠近目標(biāo)的節(jié)點(diǎn)優(yōu)先擴(kuò)展，但狀態(tài)格子較多時(shí)存在效率較低的問(wèn)題。Aine 等在A(yíng)*的基礎(chǔ)上提出了MHA*算法，引入多個(gè)啟發(fā)式函數(shù)協(xié)調(diào)路徑代價(jià)，可以兼顧路徑的最優(yōu)性與搜索效率，但規(guī)劃路徑并不能滿(mǎn)足車(chē)輛運(yùn)動(dòng)學(xué)約束。人工勢(shì)場(chǎng)法由Khatib 提出，算法簡(jiǎn)單效率高，但容易陷入局部最優(yōu)，且未考慮車(chē)輛的運(yùn)動(dòng)學(xué)約束，規(guī)劃出的路徑存在不可行的概率。安林芳等提出了一種新的障礙點(diǎn)構(gòu)建方式以防止目標(biāo)不可達(dá)和陷入局部最優(yōu)的缺點(diǎn)。Rasekhipour 等將人工勢(shì)場(chǎng)法與最優(yōu)控制相結(jié)合進(jìn)行路徑規(guī)劃，可保證路徑的可行性和最優(yōu)性。但人工勢(shì)場(chǎng)法對(duì)復(fù)雜場(chǎng)景的勢(shì)場(chǎng)構(gòu)建難度較大?；陔S機(jī)采樣的規(guī)劃方法有快速隨機(jī)搜索樹(shù)（rapidlyexploring random tree，RRT）和概率路圖法（probabilistic road map，PRM），該類(lèi)算法采樣搜索能力強(qiáng)，但由于采樣盲目性較大，浪費(fèi)過(guò)多的計(jì)算資源，且由于采樣點(diǎn)是隨機(jī)的，導(dǎo)致路徑不夠平滑?；陔x散優(yōu)化的規(guī)劃方法是目前應(yīng)用最為廣泛的，該方法是對(duì)結(jié)構(gòu)化的道路進(jìn)行有序采樣，避免了隨機(jī)采樣的盲目性，可以大大減少計(jì)算難度，但對(duì)于復(fù)雜場(chǎng)景的適應(yīng)能力仍有待探索。郭蓬等針對(duì)結(jié)構(gòu)化道路，有序地進(jìn)行采樣生成多條路徑，提出一種考慮到車(chē)道線(xiàn)曲率和障礙物模型信息的代價(jià)函數(shù)，通過(guò)代價(jià)函數(shù)進(jìn)行優(yōu)化選取局部路徑。鄧海鵬等在車(chē)輛恒速行駛時(shí)通過(guò)MPC（model predictive control）對(duì)車(chē)輛緊急避障路徑進(jìn)行優(yōu)化處理，但并未對(duì)車(chē)輛速度進(jìn)行規(guī)劃。

針對(duì)上述算法存在的問(wèn)題，為進(jìn)一步提高算法的實(shí)時(shí)性和最優(yōu)性，對(duì)道路空間進(jìn)行規(guī)則采樣以減少計(jì)算資源的浪費(fèi)，通過(guò)單次遍歷軌跡簇優(yōu)選出一條參考軌跡以提升車(chē)輛的行駛平順性，且將后續(xù)軌跡優(yōu)化由非凸空間轉(zhuǎn)變?yōu)橥箍臻g，通過(guò)分段曲線(xiàn)對(duì)參考軌跡進(jìn)行實(shí)時(shí)優(yōu)化求解得出最優(yōu)的避障軌跡以提高避障安全性和效率。為此基于分層式架構(gòu)對(duì)換道避障軌跡規(guī)劃進(jìn)行研究，首先將軌跡降維分解成-曲線(xiàn)和-曲線(xiàn)，根據(jù)本車(chē)當(dāng)前狀態(tài)基于五次多項(xiàng)式擬合候選軌跡簇；考慮到路徑的平滑性、行駛平順性、行駛效率以及碰撞風(fēng)險(xiǎn)等因素設(shè)計(jì)評(píng)價(jià)函數(shù)，選取出最優(yōu)的一對(duì)曲線(xiàn)的組合作為參考軌跡；以參考軌跡為引導(dǎo)構(gòu)建優(yōu)化目標(biāo)函數(shù)，基于分段樣條曲線(xiàn)對(duì)-曲線(xiàn)和-曲線(xiàn)進(jìn)行優(yōu)化求解，通過(guò)外點(diǎn)法求解得出最優(yōu)軌跡。該方法更加類(lèi)似人類(lèi)的駕駛員思考過(guò)程，在參考軌跡的引導(dǎo)下會(huì)保證軌跡的平順性且避免軌跡的優(yōu)化陷入局部最優(yōu)，大大減少非線(xiàn)性規(guī)劃目標(biāo)函數(shù)的復(fù)雜度，因此降低了軌跡規(guī)劃對(duì)處理芯片的算力要求，且提高軌跡的避障調(diào)整能力與舒適性，為自動(dòng)駕駛換道的工程實(shí)踐提供一種有效的解決方案。

1 確定候選軌跡

針對(duì)換道避障軌跡規(guī)劃研究，定義如圖1 所示的換道場(chǎng)景。圖中，假定當(dāng)前車(chē)輛所處直道行駛，本車(chē)道中心線(xiàn)cl和目標(biāo)車(chē)道中心線(xiàn)cl上對(duì)應(yīng)存在障礙車(chē)輛V和V，bound與bound分別為道路的上下邊界，div為車(chē)道分界線(xiàn)，本車(chē)車(chē)輛記為Auto。

當(dāng)本車(chē)接收上層換道指令時(shí)，此時(shí)刻作為換道周期的起始時(shí)刻，以該時(shí)刻車(chē)輛質(zhì)心在cl上的投影點(diǎn)作為坐標(biāo)原點(diǎn)，以cl作為軸，過(guò)原點(diǎn)做軸的垂線(xiàn)作為軸，建立如圖1 所示的換道坐標(biāo)系。圖中與分別為車(chē)輛在換道起始時(shí)刻在坐標(biāo)系上的橫向位置和航向角。設(shè)定換道初始時(shí)刻，本車(chē)以=0、=20 m/s、=0 行 駛，車(chē) 輛V在(-20 m，3.5 m)的位置以30 m/s 的速度行駛，本車(chē)道車(chē)輛V在(30 m，0)的位置以車(chē)速15 m/s行駛。

圖1 換道場(chǎng)景示意圖

1.1 候選軌跡生成

駕駛員在進(jìn)行換道時(shí)，期望車(chē)輛會(huì)最終行駛在目標(biāo)車(chē)道中心線(xiàn)上，且車(chē)輛完成一個(gè)換道周期一般所行駛縱向位移20 m ≤≤200 m 和所需時(shí)間4 s ≤≤9 s；分別對(duì)與采取和個(gè)采樣點(diǎn)，記為x和t，這里為減少計(jì)算量，且確保參考軌跡的可靠性，縱向位移每間隔10 m 取一個(gè)采樣點(diǎn)，所需時(shí)間每間隔1 s取一個(gè)采樣點(diǎn)。

車(chē)輛軌跡規(guī)劃是規(guī)劃出車(chē)輛的橫向位置、縱向位置和行駛時(shí)間之間的復(fù)雜三維問(wèn)題。為簡(jiǎn)化問(wèn)題難度，將軌跡轉(zhuǎn)化成車(chē)輛橫向位置隨縱向位置變化的-曲線(xiàn)和車(chē)輛縱向位置隨行駛時(shí)間變化的-曲線(xiàn)兩個(gè)二維問(wèn)題。為保證各曲線(xiàn)的平滑性，且由于各曲線(xiàn)的起始點(diǎn)和終止點(diǎn)存在3 個(gè)約束等式，故使用五次多項(xiàng)式擬合各候選軌跡。根據(jù)上述離散采樣點(diǎn)x和t，通過(guò)五次多項(xiàng)式分別擬合候選-曲線(xiàn)y= f()和候選-曲線(xiàn)x= g()計(jì)算如下：

式中：，，...，為第條-曲線(xiàn)系數(shù)；、和分別為起始時(shí)刻車(chē)輛車(chē)速、前輪轉(zhuǎn)角和加速度；為車(chē)輛軸距；為cl對(duì)應(yīng)的橫向坐標(biāo)值；，，...，為第條-曲線(xiàn)系數(shù)；表示車(chē)輛完成換道后的期望車(chē)速，考慮到換道的舒適性，這里的取值是根據(jù)本車(chē)的初始車(chē)速與換道完成時(shí)間所確定；=×。

1.2 候選軌跡篩選

為確保候選軌跡的可行性并減少后續(xù)軌跡評(píng)價(jià)的計(jì)算量，將滿(mǎn)足車(chē)輛行駛條件的軌跡保留作為下一階段的候選軌跡，反之則被剔除；車(chē)輛的行駛條件包括道路邊界約束、車(chē)速約束、加速度約束、轉(zhuǎn)彎半徑約束和路面附著力約束，即

式中：為車(chē)寬；與分別為bound和bound對(duì)應(yīng)的橫向坐標(biāo)值；、、、與分別為道路允許最低車(chē)速、最高車(chē)速、本車(chē)的最小轉(zhuǎn)彎半徑、最小縱向加速度和最大縱向加速度；為路面附著系數(shù)；為重力加速度；y、、、與分別為候選軌跡的橫向位置、車(chē)速、轉(zhuǎn)彎半徑、車(chē)輛縱向加速度和總加速度；這里設(shè)定該道路所允許的車(chē)速范圍為0～35 m/s，在該路上行駛的車(chē)輛為-6 m/s，為4 m/s，為2.6 m，最大值為35。符合車(chē)輛行駛條件的候選軌跡如圖2所示。

圖2 候選軌跡圖

2 選取參考軌跡

考慮到各候選參考軌跡的舒適性、碰撞風(fēng)險(xiǎn)性和利他性等因素，需要對(duì)這些候選軌跡進(jìn)一步評(píng)價(jià)以選取出最優(yōu)參考軌跡，為車(chē)輛當(dāng)前時(shí)刻的換道避障提供方向以及速度的參考值。

2.1 評(píng)價(jià)函數(shù)建立

構(gòu)建評(píng)價(jià)函數(shù)見(jiàn)式（6），選出值最小的軌跡作為最優(yōu)換道參考軌跡。評(píng)價(jià)函數(shù)的建立主要包括參考-曲線(xiàn)評(píng)價(jià)函數(shù)、參考-曲線(xiàn)評(píng)價(jià)函數(shù)和障礙風(fēng)險(xiǎn)評(píng)價(jià)函數(shù)。

式中、和分別為、和的影響系數(shù)。根據(jù)式（7）、式（8）和式（11）計(jì)算可得：取值范圍為0～4.3；的取值范圍為0～5.5；的取值范圍為1～3；本文在軌跡選取時(shí)，綜合舒適性和安全性等指標(biāo)最終取=1.2、=1、=1.3 以獲取最優(yōu)參考軌跡。、和函數(shù)的構(gòu)建具體內(nèi)容如下：

（1）參考-曲線(xiàn)評(píng)價(jià)函數(shù)，為保證參考-曲線(xiàn)的平滑性、行駛效率，要求參考-曲線(xiàn)的弧長(zhǎng)、曲率和曲率的變化率要盡量較小。為方便計(jì)算分別對(duì)上述指標(biāo)進(jìn)行簡(jiǎn)化處理，構(gòu)建表達(dá)式為

（2）參考-曲線(xiàn)評(píng)價(jià)函數(shù)，為保證參考-曲線(xiàn)的利他性和舒適性，要求參考速度應(yīng)與該車(chē)道的期望車(chē)速的差值、參考加速度和參考加加速度盡量較小。經(jīng)過(guò)簡(jiǎn)化處理后，構(gòu)建的表達(dá)式為

（3）障礙風(fēng)險(xiǎn)評(píng)價(jià)函數(shù)，為保證本車(chē)與其他車(chē)輛的安全性，在換道避障過(guò)程中應(yīng)讓本車(chē)與障礙之間的風(fēng)險(xiǎn)距離盡可能大以減小碰撞風(fēng)險(xiǎn)?？紤]到目標(biāo)車(chē)道障礙車(chē)輛與本車(chē)道障礙車(chē)輛對(duì)于本車(chē)影響不同，將又分為本車(chē)與本車(chē)道障礙物之間的風(fēng)險(xiǎn)距離和本車(chē)與目標(biāo)車(chē)道障礙物的風(fēng)險(xiǎn)距離，如圖3 所示。表示當(dāng)車(chē)輛行駛到車(chē)道分界線(xiàn)上時(shí)與目標(biāo)車(chē)道障礙物之間的風(fēng)險(xiǎn)距離，計(jì)算公式如式（9）；表示車(chē)輛在參考軌跡上行駛與本車(chē)道障礙物的最小風(fēng)險(xiǎn)距離，計(jì)算公式如式（10）。

圖3 風(fēng)險(xiǎn)距離示意圖

式中：與分別為橫向距離影響系數(shù)和縱向距離影響系數(shù)，其取值可根據(jù)高速場(chǎng)景下障礙風(fēng)險(xiǎn)影響范圍來(lái)確定，本文中以縱向距離為20 m、橫向距離為3.5 m 構(gòu)造風(fēng)險(xiǎn)距離等勢(shì)面，則可得=0.08，=0.0025；()、()分別表示車(chē)輛V橫向位置和縱向位置隨時(shí)間的變化關(guān)系；()、()分別為車(chē)輛V在時(shí)刻的橫向位置和縱向位置；為候選軌跡在本車(chē)道與目標(biāo)車(chē)道分界線(xiàn)上所對(duì)應(yīng)的時(shí)刻。

為保證車(chē)輛的安全性，當(dāng)軌跡點(diǎn)距離障礙車(chē)輛較近時(shí)，風(fēng)險(xiǎn)值應(yīng)快速增長(zhǎng)，為此利用指數(shù)函數(shù)的增減特性對(duì)風(fēng)險(xiǎn)函數(shù)進(jìn)行設(shè)計(jì)，根據(jù)障礙車(chē)輛的風(fēng)險(xiǎn)距離，構(gòu)建障礙車(chē)輛風(fēng)險(xiǎn)評(píng)價(jià)函數(shù)如式（11），得出障礙車(chē)輛風(fēng)險(xiǎn)示意圖如圖4 所示。需要指出，本文設(shè)定本車(chē)外圍尺寸長(zhǎng)為4.3 m，寬為1.8 m，當(dāng)本車(chē)與障礙車(chē)輛距離較近時(shí)碰撞風(fēng)險(xiǎn)值達(dá)到一定值則說(shuō)明碰撞風(fēng)險(xiǎn)較大，即當(dāng)≥或≥時(shí)，結(jié)合圖像可以看出，當(dāng)風(fēng)險(xiǎn)值達(dá)到3 以上時(shí)，便與障礙有發(fā)生碰撞的風(fēng)險(xiǎn)，此時(shí)該候選軌跡不應(yīng)被選取作為車(chē)輛的參考軌跡，因此本文取=3，若車(chē)輛外圍尺寸更大，則的取值應(yīng)更小，使避障時(shí)的碰撞風(fēng)險(xiǎn)較小，反之可以適當(dāng)增加的取值。

圖4 障礙車(chē)輛風(fēng)險(xiǎn)示意圖

2.2 參考軌跡選取與分析

通過(guò)2.1 節(jié)中所建立的軌跡評(píng)價(jià)函數(shù)，求出各候選參考軌跡的值，選取值最小的軌跡作為換道參考軌跡。記參考-曲線(xiàn)為=()，記參考-曲線(xiàn)為=()。對(duì)圖3中候選軌跡進(jìn)行選取，結(jié)果如圖5所示。

圖5 參考軌跡

將=()與=()組合起來(lái)得到最優(yōu)參考軌跡示意圖，如圖5（c）所示，圖中橢圓表示車(chē)輛在軌跡點(diǎn)上的粗略幾何形狀，，，...，表示各個(gè)時(shí)刻車(chē)輛所在的位置坐標(biāo)。由圖5（b）可以看出，參考軌跡在0～3 s時(shí)段之間，曲線(xiàn)斜率幾乎不變，說(shuō)明參考軌跡先保持當(dāng)前車(chē)速，結(jié)合圖5（c），此時(shí)可以看到規(guī)劃軌跡在保持與本車(chē)道障礙車(chē)輛一定的安全距離的同時(shí)，維持當(dāng)前車(chē)速等待目標(biāo)車(chē)道的車(chē)輛先行，時(shí)刻后參考-曲線(xiàn)斜率稍有增加，為使車(chē)輛行駛到左側(cè)快車(chē)道不影響其他車(chē)輛的行駛，這樣不僅減少了與障礙車(chē)輛的碰撞風(fēng)險(xiǎn)且使本車(chē)的行駛也更加平順。

本文僅定性地分析了參考軌跡的選取是否合理，因?yàn)閰⒖架壽E并不作為車(chē)輛最終跟蹤的軌跡，僅為整個(gè)換道避障過(guò)程提供引導(dǎo)。由于存在參考軌跡的引導(dǎo)，可以確定優(yōu)化空間，將原本非凸空間轉(zhuǎn)變成凸空間，且可以大大簡(jiǎn)化目標(biāo)函數(shù)的建立。

3 軌跡優(yōu)化修正

車(chē)輛在行駛過(guò)程中，障礙物的運(yùn)動(dòng)狀態(tài)是實(shí)時(shí)變化的，為保證車(chē)輛行駛的安全性，車(chē)輛實(shí)際行駛軌跡也必須根據(jù)障礙物的運(yùn)動(dòng)狀態(tài)進(jìn)行實(shí)時(shí)修正。因此，根據(jù)障礙物實(shí)時(shí)的狀態(tài)信息包括障礙物的位置和速度，通過(guò)分段五次多項(xiàng)式曲線(xiàn)對(duì)參考軌跡進(jìn)行實(shí)時(shí)優(yōu)化修正得出最優(yōu)換道軌跡。

3.1 構(gòu)建分段多項(xiàng)式曲線(xiàn)

分別構(gòu)建段-曲線(xiàn)y= f()和-曲線(xiàn)x=g()，如式（12）和式（13）；這里取值較大時(shí)會(huì)增加優(yōu)化算法的計(jì)算負(fù)擔(dān)，而取值較小時(shí)又會(huì)造成避障軌跡優(yōu)化效果不佳；為綜合考慮計(jì)算的實(shí)時(shí)性與避障軌跡的效果，經(jīng)多次測(cè)試最終取=10。

式中：，…，與，…，分別為y= f()和x=g()各項(xiàng)系數(shù)；dl與du分別為y= f()自變量取值的下限和上限；tl與tu分別為x= g()自變量的下限和上限；dl= g(tl)，du= g(tu)。

3.2 優(yōu)化目標(biāo)函數(shù)

優(yōu)化目標(biāo)函數(shù)使優(yōu)化軌跡能夠遠(yuǎn)離障礙的同時(shí)又會(huì)按照參考軌跡的引導(dǎo)去行駛以保證安全性和舒適性。因此，構(gòu)建-曲線(xiàn)優(yōu)化目標(biāo)函數(shù)如式（14），包括參考線(xiàn)引導(dǎo)函數(shù)Gp和障礙物排斥函數(shù)Rp，其中Gp表征分段曲線(xiàn)與參考路徑的貼合程度，Rp表征各分段樣條曲線(xiàn)連接點(diǎn)與障礙物的排斥程度，這里取值越小，說(shuō)明優(yōu)化效果越好。同理，構(gòu)建-曲線(xiàn)優(yōu)化目標(biāo)函數(shù)如式（17），Rp、Rs的構(gòu)建與2.1 節(jié)中的障礙車(chē)輛風(fēng)險(xiǎn)評(píng)價(jià)函數(shù)的原理類(lèi)似，不做過(guò)多介紹。

式中：、、、分別為Gp、Rp、Gs、Rs影響系數(shù)；為障礙物縱向位置；為障礙物橫向位置；與這一對(duì)參數(shù)組合選取時(shí)需要相互協(xié)調(diào)，如圖6 所示。當(dāng)相對(duì)取值較大時(shí)，優(yōu)化后的-曲線(xiàn)與參考-曲線(xiàn)貼合程度較好，而遠(yuǎn)離障礙物的趨勢(shì)有所減小。最終取=1、=4 較為合理；同理，對(duì)-曲線(xiàn)進(jìn)行優(yōu)化時(shí)，取=1、=6較為合理。

圖6 優(yōu)化參數(shù)圖

3.3 軌跡約束條件

為使優(yōu)化求解出的最優(yōu)軌跡始終是可行的，必須對(duì)分段多項(xiàng)式曲線(xiàn)加以約束來(lái)滿(mǎn)足車(chē)輛的行駛條件。-曲線(xiàn)約束包括道路邊界約束、最小安全車(chē)距約束、最小轉(zhuǎn)彎半徑約束和側(cè)向加速度約束，如式（20）。-曲線(xiàn)約束包括最小安全車(chē)距約束、路面附著約束、車(chē)速約束和車(chē)輛加速度約束，如式（21）。取=0.5，=0.8。

以當(dāng)前車(chē)輛狀態(tài)為優(yōu)化初始約束條件如式（2）中=0 時(shí)和式（4）中=0 時(shí)對(duì)應(yīng)的等式約束，軌跡優(yōu)化的末端約束與參考軌跡不同，為使軌跡優(yōu)化效果更好，優(yōu)化修正能力更強(qiáng)，對(duì)優(yōu)化軌跡的末端位置不加以固定約束，只需滿(mǎn)足以下條件即可：

同時(shí)，根據(jù)車(chē)輛的運(yùn)動(dòng)學(xué)特性，相鄰兩段多項(xiàng)式曲線(xiàn)必須保證平滑連接，平滑連接條件為

3.4 軌跡優(yōu)化求解

前述的準(zhǔn)備條件完成后，也就將軌跡優(yōu)化問(wèn)題轉(zhuǎn)換成帶約束的非線(xiàn)性規(guī)劃求解問(wèn)題。下一步對(duì)軌跡進(jìn)行實(shí)時(shí)優(yōu)化求解，每個(gè)采樣時(shí)刻分別對(duì)-曲線(xiàn)和-曲線(xiàn)進(jìn)行更新一次以滿(mǎn)足障礙車(chē)輛運(yùn)動(dòng)狀態(tài)實(shí)時(shí)變化的要求，優(yōu)化流程見(jiàn)表1。表中，為車(chē)輛換道起始時(shí)刻，為采樣周期，為換道結(jié)束時(shí)刻，()和()分別為優(yōu)化后的車(chē)輛行駛軌跡。

表1 軌跡優(yōu)化流程

對(duì)于帶約束的非線(xiàn)性規(guī)劃問(wèn)題簡(jiǎn)記為式（24）和式（25）的形式，通過(guò)罰函數(shù)法（外點(diǎn)法）對(duì)目標(biāo)函數(shù)與在約束條件下取得最小值時(shí)，求解出向量和。向量和分別代表分段-曲線(xiàn)系數(shù)，…，和分段-曲線(xiàn)的系數(shù)，…，。

4 結(jié)果分析

4.1 場(chǎng)景設(shè)定

當(dāng)上層決策車(chē)輛執(zhí)行換道避障指令時(shí)，如車(chē)輛狀態(tài)如第1 節(jié)中所述，隨后障礙車(chē)輛行駛狀態(tài)發(fā)生變化，按照以下A、B 兩種情況行駛，通過(guò)仿真分析，以驗(yàn)證算法的可行性。

A.車(chē)輛V與V按照換道初始時(shí)刻的運(yùn)動(dòng)狀態(tài)勻速行駛；

B.車(chē)輛V以加速度-1 m/s減速行駛，車(chē)輛V以加速度-1 m/s減速至28 m/s，再以加速度0.4 m/s加速至30 m/s。

4.2 仿真結(jié)果

應(yīng)用MATLAB 對(duì)文中所設(shè)計(jì)的算法進(jìn)行編程，優(yōu)化采樣周期，取為0.1 s。當(dāng)障礙物的行駛狀態(tài)按4.1 節(jié)中所述的A、B 兩種場(chǎng)景變化時(shí)，得出優(yōu)化軌跡分別如圖7和圖8所示。

圖7 A場(chǎng)景下優(yōu)化結(jié)果

圖8 B場(chǎng)景下優(yōu)化結(jié)果

考慮到軌跡的最優(yōu)性，本車(chē)最終所跟蹤行駛的軌跡會(huì)隨障礙車(chē)輛的運(yùn)動(dòng)變化進(jìn)行實(shí)時(shí)的優(yōu)化調(diào)整，從圖5（c）中可以看出本車(chē)優(yōu)化后的軌跡會(huì)隨換道參考軌跡引導(dǎo)換入目標(biāo)車(chē)道障礙車(chē)輛V的后方，從而避免優(yōu)化求解陷入其他非最優(yōu)的局部空間。

當(dāng)障礙車(chē)輛按A 場(chǎng)景運(yùn)動(dòng)時(shí)，結(jié)合圖7（a）和圖7（b）可以看出，優(yōu)化后軌跡相比于換道參考軌跡做了微弱調(diào)整，主要調(diào)整發(fā)生在縱向位置20～80 m 之間，相比于參考-曲線(xiàn)橫向調(diào)整范圍約在±0.1 m之間，相比于參考-曲線(xiàn)縱向調(diào)整范圍0～0.5 m 以?xún)?nèi)；同時(shí)由圖7（c）可以看出，優(yōu)化后的-曲線(xiàn)的曲率范圍在0～2.5 × 10m之間，曲率相對(duì)較小說(shuō)明優(yōu)化后的-曲線(xiàn)平滑性沒(méi)有受到太大影響，由曲率和車(chē)速可計(jì)算得到車(chē)輛的側(cè)向加速度不超過(guò)1.3 m/s，可以滿(mǎn)足車(chē)輛行駛的側(cè)向穩(wěn)定性要求；由圖7（d）和圖7（e）可以看出，優(yōu)化后的縱向車(chē)速和縱向加速度所在的范圍分別為19～22 m/s 和-0.6～0.8 m/s，均可以確保車(chē)輛平順、穩(wěn)定地行駛；由圖7（f）可以看出，優(yōu)化后的軌跡相比于參考軌跡的風(fēng)險(xiǎn)值有所下降，下降范圍在0～0.4。綜上可以看出，優(yōu)化后的軌跡避障靈活度更高，相比參考軌跡舒適度略有下降，但避障風(fēng)險(xiǎn)更低，安全性大大提高。結(jié)合圖7（g）可以看出，車(chē)輛在換道開(kāi)始階段先進(jìn)行輕微減速，以保證與本車(chē)到障礙車(chē)輛的跟車(chē)距離，同時(shí)由于目標(biāo)車(chē)道障礙車(chē)輛的臨近，優(yōu)化后的-曲線(xiàn)也進(jìn)行微弱的下調(diào)以更小地影響它車(chē)的行駛；待到時(shí)刻后，車(chē)輛所受到的碰撞風(fēng)險(xiǎn)較小，這時(shí)車(chē)輛開(kāi)始逐漸加速以跟隨參考軌跡的引導(dǎo)換入目標(biāo)車(chē)道。

當(dāng)障礙車(chē)輛按B場(chǎng)景運(yùn)動(dòng)時(shí)，結(jié)合圖8（a）和圖8（b）可以看出，優(yōu)化后軌跡相比于換道參考軌跡做了較大調(diào)整，主要調(diào)整發(fā)生在縱向位置20～100 m 之間，相比于參考-曲線(xiàn)調(diào)整范圍約在-0.1～0.5 m之間，相比于參考-曲線(xiàn)調(diào)整范圍在0～2.1 m之間；同時(shí)由圖8（c）可以看出，優(yōu)化后的-曲線(xiàn)的曲率范圍在0～3 × 10m之間，曲率也相對(duì)較小，說(shuō)明優(yōu)化后的-曲線(xiàn)平滑性沒(méi)有太大變化，且由車(chē)輛車(chē)速和曲率峰值可以計(jì)算得到車(chē)輛的側(cè)向加速度不超過(guò)1.6 m/s，可滿(mǎn)足車(chē)輛的側(cè)向穩(wěn)定性要求；由圖8（d）、圖8（e）可以看出，優(yōu)化后的縱向車(chē)速和縱向加速度所在的范圍分別為18～22 m/s和-1～2 m/s，由于障礙車(chē)輛的運(yùn)動(dòng)狀態(tài)發(fā)生了較大變化，這里優(yōu)化后的軌跡加速度變化也相對(duì)較大，但側(cè)向加速度和縱向加速度仍在合理范圍內(nèi)，可以保證車(chē)輛的穩(wěn)定行駛，優(yōu)化后軌跡相比于參考軌跡舒適性有所降低，但避障風(fēng)險(xiǎn)由參考軌跡的4.5下降至3.7，避障風(fēng)險(xiǎn)大大降低，以使車(chē)輛具備較高的行駛安全性；結(jié)合圖8（f）可以看出，整個(gè)換道過(guò)程中，車(chē)輛在實(shí)時(shí)調(diào)整相對(duì)于參考軌跡、本車(chē)道障礙和目標(biāo)車(chē)道障礙三者之間的相對(duì)距離，以計(jì)算出最優(yōu)的換道軌跡，使車(chē)輛相對(duì)于障礙車(chē)輛的碰撞風(fēng)險(xiǎn)最小且盡可能跟隨參考軌跡以保證整個(gè)換道過(guò)程的平順性。

5 結(jié)論

（1）所提出的換道避障軌跡規(guī)劃方法，將軌跡規(guī)劃降維分解成路徑規(guī)劃和速度規(guī)劃，根據(jù)本車(chē)狀態(tài)和多個(gè)采樣點(diǎn)生成候選-曲線(xiàn)簇和候選-曲線(xiàn)簇，簡(jiǎn)化了規(guī)劃難度，且考慮了多種換道可能。

（2）基于指數(shù)函數(shù)設(shè)計(jì)了一種障礙風(fēng)險(xiǎn)評(píng)價(jià)函數(shù)，并結(jié)合軌跡平順性、利他性和行駛效率等構(gòu)建了綜合評(píng)價(jià)體系，選取出一對(duì)最優(yōu)的-曲線(xiàn)和-曲線(xiàn)的組合作為參考軌跡。

（3）以參考軌跡為引導(dǎo)，利用障礙車(chē)輛與本車(chē)之間的風(fēng)險(xiǎn)關(guān)系設(shè)計(jì)了優(yōu)化目標(biāo)函數(shù)，并基于分段樣條曲線(xiàn)對(duì)-曲線(xiàn)和-曲線(xiàn)進(jìn)行實(shí)時(shí)優(yōu)化，通過(guò)外點(diǎn)法求解得出最優(yōu)軌跡，使每個(gè)優(yōu)化周期都會(huì)得出一對(duì)最優(yōu)的-曲線(xiàn)和-曲線(xiàn)的最優(yōu)組合。

（4）結(jié)果表明，該算法同時(shí)對(duì)速度和路徑進(jìn)行規(guī)劃在滿(mǎn)足車(chē)輛穩(wěn)定行駛的前提下，具有更好的平順性和舒適性，且應(yīng)對(duì)不同障礙車(chē)輛運(yùn)動(dòng)狀態(tài)變化時(shí)具有較好的適應(yīng)性和避障調(diào)整能力，且該算法中所涉及的障礙風(fēng)險(xiǎn)指標(biāo)可與決策層進(jìn)一步交互。