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

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

前言

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

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

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

1 確定候選軌跡

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

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

圖1 換道場景示意圖

1.1 候選軌跡生成

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

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

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

1.2 候選軌跡篩選

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

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

圖2 候選軌跡圖

2 選取參考軌跡

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

2.1 評價函數(shù)建立

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

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

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

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

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

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

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

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

圖4 障礙車輛風(fēng)險示意圖

2.2 參考軌跡選取與分析

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

圖5 參考軌跡

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

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

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

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

3.1 構(gòu)建分段多項式曲線

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

式中：，…，與，…，分別為y= f()和x=g()各項系數(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)離障礙的同時又會按照參考軌跡的引導(dǎo)去行駛以保證安全性和舒適性。因此，構(gòu)建-曲線優(yōu)化目標(biāo)函數(shù)如式（14），包括參考線引導(dǎo)函數(shù)Gp和障礙物排斥函數(shù)Rp，其中Gp表征分段曲線與參考路徑的貼合程度，Rp表征各分段樣條曲線連接點與障礙物的排斥程度，這里取值越小，說明優(yōu)化效果越好。同理，構(gòu)建-曲線優(yōu)化目標(biāo)函數(shù)如式（17），Rp、Rs的構(gòu)建與2.1 節(jié)中的障礙車輛風(fēng)險評價函數(shù)的原理類似，不做過多介紹。

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

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

3.3 軌跡約束條件

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

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

同時，根據(jù)車輛的運動學(xué)特性，相鄰兩段多項式曲線必須保證平滑連接，平滑連接條件為

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

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

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

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

4 結(jié)果分析

4.1 場景設(shè)定

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

A.車輛V與V按照換道初始時刻的運動狀態(tài)勻速行駛；

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

4.2 仿真結(jié)果

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

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

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

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

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

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

5 結(jié)論

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

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

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

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