無人駕駛車輛解耦混合運(yùn)動(dòng)規(guī)劃方法研究

王志琛，胡廣地，劉建陽

（西南交通大學(xué)機(jī)械工程學(xué)院，成都 610031）

無人駕駛技術(shù)是當(dāng)前汽車行業(yè)重要的技術(shù)發(fā)展方向之一［1-2］。運(yùn)動(dòng)規(guī)劃通常是指在受限空間內(nèi)通過一定方法尋找出一條能夠在保障車輛和人員安全的同時(shí)滿足舒適性和行車效率的最佳軌跡。在高速公路行駛場景中，運(yùn)動(dòng)規(guī)劃面臨的挑戰(zhàn)主要體現(xiàn)在以下方面：首先，車輛高速行駛時(shí)需要更為苛刻地考慮車輛動(dòng)力學(xué)特性；其次，運(yùn)動(dòng)規(guī)劃方法在復(fù)雜約束下應(yīng)規(guī)劃出避免碰撞且高效、操縱穩(wěn)定的軌跡，需要優(yōu)先保證車輛避免碰撞；最后，高速無人駕駛車輛運(yùn)動(dòng)規(guī)劃算法的實(shí)時(shí)性要求相比常速更高，所以算法要充分考慮合理降低計(jì)算的復(fù)雜性。

運(yùn)動(dòng)規(guī)劃的抽象數(shù)學(xué)問題往往是求解一個(gè)以時(shí)間為參數(shù)的泛函優(yōu)化問題［3］。由于一般高維受限優(yōu)化問題的解不局限于某個(gè)函數(shù)，導(dǎo)致問題變得非常復(fù)雜［4］。目前，對于三維非凸優(yōu)化問題的直接解法還比較復(fù)雜，主流方法是將其分解為橫向偏移規(guī)劃和縱向速度規(guī)劃兩個(gè)問題，也就是將運(yùn)動(dòng)規(guī)劃進(jìn)行橫縱向解耦。

基于上述思想，可以在Frenet坐標(biāo)系［5］中將問題簡化表達(dá)，將運(yùn)動(dòng)規(guī)劃問題進(jìn)行橫縱向解耦后可用現(xiàn)有算法進(jìn)行求解，但是解耦降維也帶來了一些信息的損失。進(jìn)行橫向規(guī)劃時(shí)，時(shí)間信息的缺失導(dǎo)致無法完全表現(xiàn)出動(dòng)態(tài)障礙物約束；進(jìn)行縱向規(guī)劃時(shí)，不在指引線上的障礙物很難表現(xiàn)出來，無法實(shí)現(xiàn)道路寬度上的全部障礙物約束。針對上述問題，現(xiàn)行的解決方法主要分為兩種：第1種為并行多條指引線，即對每條車道生成一條指引線，并行使用最優(yōu)化方法求解多條車道中的最優(yōu)解，再進(jìn)行橫向?qū)Ρ冗x擇［6-7］。此方法比較成熟，在城市道路復(fù)雜環(huán)境測試上取得了較好結(jié)果，對中低速復(fù)雜環(huán)境處理表現(xiàn)出色，但并行計(jì)算所帶來的高計(jì)算量對速度較高、道路環(huán)境相對簡單的高速公路行駛場景適用性不佳。第2種方法是根據(jù)交通環(huán)境、交通規(guī)則、自車狀態(tài)（位姿、速度）等來確定與指引線對應(yīng)的可使用空間參數(shù)，在決策階段生成車道級決策，并在此空間上進(jìn)行運(yùn)動(dòng)規(guī)劃［8-10］。此方法能夠避免大量的并行計(jì)算，在高速公路行駛場景中適應(yīng)性較好，更符合無人駕駛規(guī)劃系統(tǒng)層層優(yōu)化的思想。但由于需要在決策階段生成車道級決策，其上層決策算法相較于并行多指引線算法更為復(fù)雜。

基于以上問題，本文提出一種針對結(jié)構(gòu)化道路高速（80～130 km/h）行駛場景的解耦運(yùn)動(dòng)規(guī)劃指引線模型，在此指引線模型上表述運(yùn)動(dòng)規(guī)劃所依賴的指引路徑、決策信息和道路結(jié)構(gòu)信息。其主要?jiǎng)?chuàng)新點(diǎn)為：提出一種橫縱向解耦增強(qiáng)混合運(yùn)動(dòng)規(guī)劃方法，橫向規(guī)劃中融合分段jerk規(guī)劃算法［10］（Piecewise-Jerk Method，PJM）與多項(xiàng)式規(guī)劃算法生成橫向最優(yōu)解逼近群，使局部路徑滿足2階平滑，并使用代價(jià)函數(shù)和碰撞檢測在最優(yōu)解逼近群中選擇軌跡，以優(yōu)先滿足高速運(yùn)動(dòng)規(guī)劃中車輛避障的要求。在混合規(guī)劃過程中，通過規(guī)劃策略和參數(shù)優(yōu)化降低最優(yōu)解逼近群中的軌跡數(shù)量，進(jìn)而降低算法的計(jì)算復(fù)雜度，以解決高速規(guī)劃中高實(shí)時(shí)性要求的問題。

經(jīng)在聯(lián)合仿真平臺(tái)中驗(yàn)證，證明本文提出的運(yùn)動(dòng)規(guī)劃方法能在單條指引線的基礎(chǔ)上，規(guī)劃出優(yōu)先滿足車輛避免碰撞的可行軌跡，并且有效減少了最優(yōu)解逼近群中的軌跡數(shù)量，相比于多車道分段jerk法混合最優(yōu)軌跡生成法［9］（Optimal Trajectory Generation，OTG），此算法總時(shí)間復(fù)雜度降低了15%。

1 模型構(gòu)建

1.1 解耦規(guī)劃指引線模型

解耦規(guī)劃依賴于Frenet坐標(biāo)系，指引線作為Cartesian和Frenet坐標(biāo)系之間的轉(zhuǎn)換“橋梁”，其平滑性直接影響計(jì)算路徑的質(zhì)量［10］。解耦規(guī)劃指引線模型RE定義為：

式中：re為已滿足二階平滑的實(shí)時(shí)指引路徑，由m個(gè)分化為1 m間距的參考路徑點(diǎn)pr組成；xri、yri、θri、kri、k'ri、sri分別為全局坐標(biāo)系下第i個(gè)參考點(diǎn)的坐標(biāo)位置、航向角、曲率、曲率變化率、里程；lri、rri為指引線左、右可規(guī)劃區(qū)間寬度，其數(shù)值體現(xiàn)了在該段指引道路上是否允許被動(dòng)變向其它車道；rf為參考線道路信息；ta為規(guī)劃目標(biāo)；v為參考速度；d為所在車道寬度；c為在指引線范圍是否包含了可主動(dòng)變道工況；w為車輛在指引線上的參考匹配點(diǎn)索引。

目前普遍認(rèn)為運(yùn)動(dòng)規(guī)劃處理時(shí)間在8 s左右或行駛距離在200 m左右較為合理［11］，綜合工況和求解時(shí)間，本文取指引路徑為220 m，由車輛定位點(diǎn)行駛方向前方的200個(gè)路徑點(diǎn)及后方的20個(gè)路徑點(diǎn)構(gòu)成，即取m為221。

1.2 自車模型

自車模型主要包括車輛的定位模塊所提供的位姿信息和車輛固有的外模型參數(shù)以及動(dòng)力學(xué)參數(shù)。自車抽象模型E定義為：

式中：pe為車輛位姿信息；xe、ye為車輛在全局坐標(biāo)系下在道路平面的投影坐標(biāo)；θe、ke、ve、ae分別為車輛在全局坐標(biāo)系下的航向角、轉(zhuǎn)彎曲率、速度、加速度；m為車輛固有參數(shù)；l、d、r分別為車輛長度、寬度及車輛后邊界到車輛定位中心（后軸中心點(diǎn)）的距離；k、amax、amin分別為車輛標(biāo)定允許最大轉(zhuǎn)向曲率、最大縱向加速度、減速度；jmax、jmin、lamax為縱向jerk最大值、最小值及最大橫向加速度。

1.3 障礙物模型

在運(yùn)動(dòng)規(guī)劃中可將障礙物分為兩類，即靜態(tài)障礙物和動(dòng)態(tài)障礙物，兩者均可抽象表示為道路平面上的矩形。障礙物抽象模型O定義為：

式中：Os為靜態(tài)障礙物的集合；Obsi為第i個(gè)靜態(tài)障礙物；Od為動(dòng)態(tài)障礙物的集合；Obdi為第i個(gè)動(dòng)態(tài)障礙物；Obditj為第i個(gè)動(dòng)態(tài)障礙物在tj時(shí)刻占據(jù)矩形的大小和位姿；ver1，ver2，ver3，ver4為障礙物矩形的4個(gè)頂點(diǎn)在道路平面上正投影點(diǎn)；cen為矩形中心點(diǎn)在道路平面的整投影點(diǎn)；θ為矩形的橫擺角；l為矩形長度；d為矩形寬度；xveri，yveri為矩形頂點(diǎn)在全局坐標(biāo)系下的坐標(biāo)位置；xcen、ycen為矩形中心點(diǎn)在全局坐標(biāo)系下的坐標(biāo)位置。

2 運(yùn)動(dòng)規(guī)劃解空間構(gòu)造

運(yùn)動(dòng)規(guī)劃中的解空間構(gòu)造即道路與障礙物約束構(gòu)造，將解空間分為橫向規(guī)劃L(s)的解空間構(gòu)造和縱向規(guī)劃S(t)的解空間構(gòu)造，橫向的解空間構(gòu)造以參考線為基準(zhǔn)進(jìn)行Frenet坐標(biāo)系的道路模型和障礙物轉(zhuǎn)換，縱向的解空間構(gòu)造以參考線為基準(zhǔn)進(jìn)行侵占參考線障礙物的Frenet坐標(biāo)轉(zhuǎn)換。

2.1 規(guī)劃起點(diǎn)處理

對于規(guī)劃起點(diǎn)，以定位點(diǎn)時(shí)間戳查詢運(yùn)動(dòng)規(guī)劃上一幀的輸出軌跡，如果與定位數(shù)據(jù)位置誤差小于閾值，則查詢算法輸出時(shí)間在上一幀輸出軌跡所對應(yīng)位姿信息作為規(guī)劃起點(diǎn)，否則，以運(yùn)動(dòng)學(xué)模型預(yù)測規(guī)劃為起點(diǎn)，規(guī)劃起點(diǎn)ps為：

式中：xs、ys為車輛在規(guī)劃起點(diǎn)位置的坐標(biāo)；θs、ks、vs、as分別為車輛在規(guī)劃起點(diǎn)的航向角、轉(zhuǎn)彎曲率、速度、加速度。

利用式（5）［9］將規(guī)劃起點(diǎn)轉(zhuǎn)換到以指引線路徑RE.re為基礎(chǔ)的Frenet坐標(biāo)系，得Frenet坐標(biāo)系下規(guī)劃起點(diǎn)psf如式（7）所示。

式中：pp為規(guī)劃起點(diǎn)在指引線上的投影點(diǎn)，考慮路徑在計(jì)算機(jī)表達(dá)中的離散性，pp由式（6）解得。

式中：pm為規(guī)劃起點(diǎn)在指引線上距離最近的點(diǎn)，定義為匹配點(diǎn)，r=(xs-xm)cosθm+(ys-ym)sinθm。

式中：t0，s0，s?0，s?0，l0，l'0，l''0分別為規(guī)劃起點(diǎn)的時(shí)間、行駛里程、縱向速度、縱向加速度、橫向偏移、橫向偏移速度、橫向偏移加速度；w為規(guī)劃起點(diǎn)是否在指引線所在車道的標(biāo)志位，規(guī)劃起點(diǎn)在車道空間為0，否則為與指引路徑的距離值。

2.2 橫向解空間構(gòu)造

橫向解空間SLM的構(gòu)造，首先在規(guī)劃起點(diǎn)psf至最大規(guī)劃距離ras中利用道路可規(guī)劃橫向區(qū)間RE.re.l、RE.re.r建立基本約束；其次將障礙物模型O中靜態(tài)障礙物及速度小于閾值的動(dòng)態(tài)障礙物轉(zhuǎn)換到Frenet坐標(biāo)系中收縮橫向區(qū)間；最后使用自身車輛寬度E.m.d再次收縮橫向空間并校驗(yàn)橫向空間是否為連通空間，非連通空間則表示無橫向規(guī)劃條件，所以開辟指引線作為連通區(qū)域，使橫向規(guī)劃有解。橫向解空間構(gòu)造完成示意圖，如圖1所示。

圖1 橫向解空間構(gòu)造完成示意圖

2.3 縱向解空間構(gòu)造

縱向解空間STM的構(gòu)造即是描繪侵占指引線的障礙物，對每一個(gè)障礙物在規(guī)劃時(shí)長內(nèi)以時(shí)間為基準(zhǔn)進(jìn)行遍歷，并將障礙物轉(zhuǎn)換到Frenet坐標(biāo)系。在障礙物校驗(yàn)時(shí)需要判斷障礙物是否處于可規(guī)劃橫向空間，且該障礙物是否超出規(guī)劃距離。校驗(yàn)通過后以障礙物在Frenet坐標(biāo)系下所占據(jù)橫縱向空間的形式輸出障礙物數(shù)據(jù)?？v向解空間構(gòu)造完成示意圖，如圖2所示。

圖2 縱向解空間構(gòu)造完成示意圖

由于解耦的固有特性，丟失了一些維度信息，在橫、縱向兩個(gè)解空間中不能完全排除解空間中障礙物所占空間，但此空間已經(jīng)完全包含可行空間，此問題產(chǎn)生的偽解可以通過碰撞檢測排除。

3 橫縱向解耦增強(qiáng)混合運(yùn)動(dòng)規(guī)劃方法

3.1 橫向偏移混合規(guī)劃方法

為提高規(guī)劃效率，在同時(shí)滿足STM=?、psf.w=0時(shí)，即在解空間不存在障礙物且規(guī)劃起點(diǎn)在指引線所在車道時(shí)，只使用多項(xiàng)式橫向偏移規(guī)劃算法。在不滿足上述條件時(shí)使用分段jerk橫向偏移規(guī)劃算法進(jìn)行計(jì)算，然后再使用多項(xiàng)式規(guī)劃算法進(jìn)行補(bǔ)充。

3.1.1 多項(xiàng)式橫向偏移規(guī)劃算法

首先，以數(shù)值方法得到規(guī)劃終點(diǎn)；其次，對終點(diǎn)與規(guī)劃起點(diǎn)使用五次多項(xiàng)式規(guī)劃橫向軌跡L(s)。橫向規(guī)劃終點(diǎn)群LP1的定義如式（8）所示。

式中：psf.s0為規(guī)劃起點(diǎn)對應(yīng)的里程；qi、ri為終點(diǎn)選取參數(shù)。

橫向偏移規(guī)劃在規(guī)劃終點(diǎn)完成橫向動(dòng)作，隨后維持l不變，即規(guī)劃終點(diǎn)l'i=0，l''i=0。車輛處于本車道且前方無任何阻擋，只使用基于多項(xiàng)式規(guī)劃追蹤指引線行駛。本文提出考慮偏差距離取值方法，取0，以確定規(guī)劃終點(diǎn)。

最后，由規(guī)劃起點(diǎn)psf和唯一規(guī)劃終點(diǎn)l p1組成6個(gè)橫向約束，求解式（9）中多項(xiàng)式表達(dá)式系數(shù)。解出唯一橫向規(guī)劃解L1(s)，文獻(xiàn)［12］中對此方法的完備性進(jìn)行了詳細(xì)證明。

3.1.2 分段jerk橫向偏移規(guī)劃算法

在已經(jīng)構(gòu)造的L(s)的解空間SLM中，使用分段jerk法進(jìn)行橫向偏移規(guī)劃的主要思想，是將車輛橫向運(yùn)動(dòng)L(s)作為一個(gè)3階系統(tǒng)，構(gòu)造二次規(guī)劃問題。

首先建立狀態(tài)量，從規(guī)劃起點(diǎn)開始在整個(gè)解空間進(jìn)行規(guī)劃，離散狀態(tài)量x為：

式中：lsi、l'si、l''si分別為指引線離散路徑點(diǎn)對應(yīng)的橫向偏移距離、偏移速度、偏移加速度；ras為橫向規(guī)劃距離，綜合高速工況要求和計(jì)算復(fù)雜度，取ras=180。

其次建立約束，具體定義見表1。

表1 橫向規(guī)劃約束定義

最后構(gòu)建代價(jià)函數(shù)，如式（11）所示。

式中：第1項(xiàng)為規(guī)劃產(chǎn)生的橫向偏移代價(jià)；第2項(xiàng)、第3項(xiàng)為橫向偏移帶來的舒適性代價(jià)；第4項(xiàng)為結(jié)果路徑與橫向邊界的側(cè)向間距所反映的安全冗余。

結(jié)合文獻(xiàn)和工程實(shí)際測試，代價(jià)函數(shù)權(quán)重取值見表2。

表2 代價(jià)函數(shù)權(quán)重取值

表2中：RE.rf.c為解耦運(yùn)動(dòng)規(guī)劃模型中在指引線范圍是否包含了可主動(dòng)變道工況的標(biāo)志位。

通過以上步驟完成問題構(gòu)造，使用osqp求解器［13］解得此解空間內(nèi)的橫向最優(yōu)解L1(s)，并將其存入橫向最優(yōu)解逼近群LA。

3.1.3 橫向偏移規(guī)劃混合策略

在完成分段jerk橫向偏移規(guī)劃后，使用多項(xiàng)式規(guī)劃對最優(yōu)解的合理逼近進(jìn)行補(bǔ)充，稱其為橫向偏移規(guī)劃混合策略，其偽代碼見表3。

表3 橫向偏移規(guī)劃混合策略偽代碼

續(xù)表

表3中：LP(a，b)、POLY(a，b)函數(shù)分別為構(gòu)造多項(xiàng)式規(guī)劃約束和使用多項(xiàng)式規(guī)劃軌跡?；旌喜呗砸詸M向軌跡L1(s)（由分段jerk橫向偏移規(guī)劃算法所生成）中的最大橫向變化量?lmax作為判別依據(jù)，使用五次多項(xiàng)式橫向偏移規(guī)劃算法補(bǔ)充一個(gè)保持當(dāng)前車道，依靠縱向速度規(guī)劃避障的可行橫向解L2(s)，將其存入橫向最優(yōu)解逼近群LA。

3.2 圍繞不同規(guī)劃目標(biāo)的混合縱向規(guī)劃方法

縱向規(guī)劃使用多項(xiàng)式規(guī)劃算法，將圍繞速度規(guī)劃和圍繞避障規(guī)劃兩種不同規(guī)劃目標(biāo)的規(guī)劃器進(jìn)行混合使用，共同解得縱向最優(yōu)解的逼近群。兩種規(guī)劃器都需要先確定規(guī)劃終點(diǎn)狀態(tài)，然后以起始點(diǎn)狀態(tài)與規(guī)劃終點(diǎn)狀態(tài)為約束，使用多項(xiàng)式規(guī)劃算法解得縱向軌跡S(t)。

3.2.1 圍繞速度進(jìn)行縱向規(guī)劃

速度規(guī)劃是縱向規(guī)劃的本質(zhì)，在規(guī)劃終點(diǎn)使車輛達(dá)到目標(biāo)速度，同時(shí)在規(guī)劃終點(diǎn)s''i為0時(shí)，才有利于控制縱向規(guī)劃終點(diǎn)之后的軌跡速度，符合縱向規(guī)劃系統(tǒng)的物理背景。規(guī)劃終點(diǎn)群S’P1為：

規(guī)劃終點(diǎn)狀態(tài)確定完成之后，利用多項(xiàng)式規(guī)劃算法求解縱向軌跡，即以規(guī)劃起點(diǎn)psf和規(guī)劃終點(diǎn)群S'P1為約束，求解式（13）中多項(xiàng)式表達(dá)式系數(shù)，可得c?g個(gè)縱向規(guī)劃結(jié)果S(t)。

上述方法的完備性、可達(dá)性及連續(xù)性在文獻(xiàn)［9］中進(jìn)行了詳細(xì)證明。隨后增加一條保持車輛當(dāng)前速度的縱向規(guī)劃結(jié)果Sn+1(t)=psf.s?0，將其作為對圍繞速度進(jìn)行縱向規(guī)劃的補(bǔ)充，最后將以圍繞速度進(jìn)行規(guī)劃得到的縱向解存入縱向最優(yōu)解逼近群LO。

3.2.2 圍繞避障進(jìn)行縱向規(guī)劃

圍繞避障進(jìn)行縱向規(guī)劃的目的是跟馳或超越前方車輛。圍繞避障的縱向規(guī)劃方法對縱向解空間STM中的每一個(gè)障礙物stm進(jìn)行處理，規(guī)劃終點(diǎn)群SP1為：

式中：spij為規(guī)劃終點(diǎn)群中第i+(c-1)j個(gè)規(guī)劃終點(diǎn)；qi、rij、uij為規(guī)劃終點(diǎn)選取參數(shù)，取值見表4。

表4 圍繞避障規(guī)劃終點(diǎn)取值

表4中：stm.ru.t、stm.lu.t分別為障礙物stm在縱向解空間中所占據(jù)的時(shí)間最大值、最小值；stm(qi).u、astm(qi).d分別為障礙物stm在qi時(shí)刻所占據(jù)縱向空間最大、最小值；stm.v為障礙物stm速度。以上數(shù)據(jù)均為縱向解空間構(gòu)造時(shí)定義并求解完成的數(shù)據(jù)。d pt為時(shí)間規(guī)劃密度，dpt=2；k、sfi為車距參數(shù)，k=2、sf1=5、s f2=10。

規(guī)劃終點(diǎn)狀態(tài)確定完成之后，利用多項(xiàng)式規(guī)劃算法求解縱向軌跡。以規(guī)劃起點(diǎn)psf和規(guī)劃終點(diǎn)群SP1為約束，求解式（15）中多項(xiàng)式的表達(dá)式系數(shù)，可得c?g個(gè)縱向規(guī)劃結(jié)果S(t)。

將得到的規(guī)劃結(jié)果存入縱向軌跡最優(yōu)解逼近群LO，共同構(gòu)成縱向最優(yōu)解的合理逼近，其規(guī)劃如圖3所示，黃色標(biāo)識(shí)為障礙物，藍(lán)色線條為縱向規(guī)劃結(jié)果。

圖3 圍繞避障進(jìn)行規(guī)劃

3.3 軌跡選擇

上述過程生成橫縱向最優(yōu)解逼近群之后，需要對其進(jìn)行組合構(gòu)成完整軌跡，再在其中選取最優(yōu)解。

首先，對縱向最優(yōu)解逼近群LO進(jìn)行約束校驗(yàn)，將校驗(yàn)所得縱向規(guī)劃結(jié)果和橫向最優(yōu)解逼近群LA中的橫向規(guī)劃結(jié)果進(jìn)行組合，構(gòu)成完整軌跡群。隨后，利用代價(jià)函數(shù)對所構(gòu)成的完整軌跡群進(jìn)行評估，見表5。

表5 軌跡評估代價(jià)函數(shù)

結(jié)合文獻(xiàn)和工程實(shí)際測試，代價(jià)函數(shù)參數(shù)即權(quán)重取值，見表6。表中，RE.rf.d、RE.rf.c為式（1）中的指引線數(shù)據(jù)。

表6 代價(jià)函數(shù)參數(shù)取值

最后，順序抽取代價(jià)評估后的軌跡，將Frenet坐標(biāo)系下的軌跡轉(zhuǎn)換到Cartesian坐標(biāo)系下，并使用最大速度、最大加速度、最大側(cè)向加速度再次進(jìn)行約束校驗(yàn)。校驗(yàn)通過之后，與道路上的障礙物O做碰撞檢測的零一決策，如果軌跡通過校驗(yàn)且無碰撞，則輸出軌跡，否則抽取下一軌跡進(jìn)行校驗(yàn)、檢測。

4 仿真驗(yàn)證

4.1 仿真平臺(tái)搭建

仿真模擬器在無人駕駛算法開發(fā)和測試中占據(jù)重要地位［14］。本文利用Apollo-LGsvl搭建聯(lián)合仿真測試平臺(tái)，以LGsvl模擬器中高速公路高精地圖作為道路環(huán)境，注入不同靜態(tài)、動(dòng)態(tài)障礙物，模擬巡航、跟馳、規(guī)避靜態(tài)障礙物、規(guī)避動(dòng)態(tài)障礙物等真實(shí)交通場景駕駛工況。取巡航車速在80～120 km/h之間，匝道車速為40 km/h。

4.2 規(guī)劃結(jié)果及分析

4.2.1 完成性分析

仿真驗(yàn)證過程中算法執(zhí)行頻率設(shè)置為10 Hz，測試巡航、跟馳、避障等工況的最終輸出軌跡如圖4～6所示。

圖4 起步、巡航、停車工況仿真結(jié)果

圖4分別為路邊起步、巡航和本車道減速避障工況仿真結(jié)果。車輛橫向跟蹤指引路徑，縱向進(jìn)行速度規(guī)劃，并由代價(jià)函數(shù)選擇最佳規(guī)劃結(jié)果。

圖5分別為自車跟馳所在車道前方不同車輛的工況，車輛橫向跟蹤指引路徑，縱向圍繞速度和障礙物進(jìn)行混合規(guī)劃以應(yīng)對具體環(huán)境。本文使用的混合縱向規(guī)劃算法在圍繞避障進(jìn)行規(guī)劃時(shí)會(huì)根據(jù)他車速度進(jìn)行速度和距離規(guī)劃，由代價(jià)函數(shù)選擇最優(yōu)跟蹤軌跡。上述過程仿真中則體現(xiàn)為對他車產(chǎn)生不同速度、不同跟車距離的軌跡。

圖5 跟馳工況仿真結(jié)果

橫向規(guī)劃空間限制為本車道且存在不完全占據(jù)本車道的靜態(tài)障礙物場景下，橫向規(guī)劃在指引線所提供的可規(guī)劃空間中生成避障最優(yōu)路徑，如圖6所示。在可跨車道避障工況下，用橫縱向解耦增強(qiáng)混合運(yùn)動(dòng)規(guī)劃方法進(jìn)行橫向規(guī)劃時(shí)，在變道軌跡基礎(chǔ)上補(bǔ)充了一條跟蹤指引路徑的解，并使用碰撞檢測的方式保證最終軌跡的安全性；借道車道存在高速車輛，變道存在碰撞風(fēng)險(xiǎn)時(shí)，代價(jià)函數(shù)與碰撞檢測算法選擇的依靠速度規(guī)劃在本車道避障的路徑，圖6b所示；在無碰撞風(fēng)險(xiǎn)后的借道超車軌跡，如圖6c所示。

圖6 避障工況仿真結(jié)果

提取仿真結(jié)果中的相關(guān)規(guī)劃結(jié)果指標(biāo)，見表7。各工況中，碰撞次數(shù)為0，相關(guān)指標(biāo)均符合動(dòng)力學(xué)約束。綜上，本文提出的運(yùn)動(dòng)規(guī)劃方法能夠返回一個(gè)安全的有效軌跡。

表7 不同工況規(guī)劃結(jié)果指標(biāo)

4.2.2 結(jié)果質(zhì)量分析

各工況下，規(guī)劃時(shí)間內(nèi)不同軌跡的航向角θ、轉(zhuǎn)向曲率k和速度v，如圖7～12所示。

圖7 起步、巡航、停車工況軌跡航向角θ、轉(zhuǎn)向曲率k

軌跡π1、π2、π3分別對應(yīng)圖4中的起步、巡航、停車軌跡。起步軌跡π1，航向角θ、參考轉(zhuǎn)向曲率k變化連續(xù)平滑，最終與指引線一致，并持續(xù)加速。經(jīng)過渡后生成巡航軌跡π2。避障軌跡π3，在障礙物完全占據(jù)車道，不存在通過空間時(shí)，車輛橫向跟蹤指引路徑，縱向持續(xù)減速，最終停車，速度v變化連續(xù)平滑。

軌跡π4、π5、π6分別對應(yīng)圖5中的3種跟馳軌跡，車輛追蹤指引路徑，縱向速度變化連續(xù)平滑，通過規(guī)劃車速與障礙物保持安全車距。

軌跡π7、π8、π9分別對應(yīng)圖6中的各避障軌跡。軌跡π7對應(yīng)圖6a軌跡，運(yùn)動(dòng)規(guī)劃算法生成航向角和參考曲率變化連續(xù)的路徑，由于代價(jià)函數(shù)中的碰撞代價(jià)，縱向規(guī)劃在接近障礙物前將速度降低在安全值內(nèi)，并且在通過障礙物后重新加速達(dá)到巡航速度。軌跡π8、π9分別對應(yīng)圖6b和圖6c中的軌跡，軌跡的航向角θ變化連續(xù)平滑，參考轉(zhuǎn)向曲率k連續(xù)平滑且波動(dòng)較小，速度變化連續(xù)平滑。

綜上，不同工況中運(yùn)動(dòng)規(guī)劃產(chǎn)生的軌跡能夠保證車輛在進(jìn)行橫向動(dòng)作時(shí)，其航向角θ、參考轉(zhuǎn)向曲率k的變化連續(xù)平滑，指引路徑本身所具有的曲率變化亦能夠在運(yùn)動(dòng)規(guī)劃后保證連續(xù)。由于圍繞避障進(jìn)行規(guī)劃的主要目標(biāo)是對跟車距離進(jìn)行約束，所以在部分工況下存在速度有微小超調(diào)情況，又由于本文使用多項(xiàng)式進(jìn)行縱向規(guī)劃，速度曲線滿足高階平滑，加之軌跡評估選擇階段存在約束校驗(yàn)，所以此項(xiàng)超調(diào)可以接受，整體認(rèn)為速度變化能夠保持連續(xù)平滑。

圖8 起步、巡航、停車工況軌跡速度v

圖9 跟馳工況軌跡航向角θ、轉(zhuǎn)向曲率k

圖10 跟馳工況軌跡速度v

圖11 避障工況跡航向角θ、轉(zhuǎn)向曲率k

圖12 避障工況軌跡速度v

由以上驗(yàn)證結(jié)果可知，本文基于OTG所提出的橫縱向解耦增強(qiáng)混合運(yùn)動(dòng)規(guī)劃方法實(shí)現(xiàn)了在指引線基礎(chǔ)上完成運(yùn)動(dòng)規(guī)劃。這種運(yùn)動(dòng)規(guī)劃方法綜合了多項(xiàng)式規(guī)劃算法和分段jerk法的平滑性優(yōu)勢，與其他方法在Apollo-LGsvl平臺(tái)的對比，見表8，數(shù)據(jù)表明，算法顯著降低了最優(yōu)解軌跡逼近群中的軌跡數(shù)量，使綜合工況平均軌跡評估時(shí)間縮短為8.625 ms。

表8 不同運(yùn)動(dòng)規(guī)劃方法在Apollo-LGsvl平臺(tái)結(jié)果對比

5 結(jié)論

本文提出了一種橫縱向解耦增強(qiáng)混合規(guī)劃方法，該方法在橫向規(guī)劃中融合了數(shù)值優(yōu)化橫向偏移規(guī)劃算法和多項(xiàng)式橫向偏移規(guī)劃算法的優(yōu)勢，在縱向規(guī)劃中融合圍繞速度和避障進(jìn)行規(guī)劃。通過搭建Apollo-LGsvl對所提出的運(yùn)動(dòng)規(guī)劃方法進(jìn)行仿真驗(yàn)證，主要結(jié)果如下：

（1）提出的運(yùn)動(dòng)規(guī)劃方法所得運(yùn)動(dòng)軌跡的橫向偏移和縱向速度都能滿足2階平滑。

（2）在高速公路各工況中生成的運(yùn)動(dòng)軌跡能夠有效避免車輛發(fā)生碰撞。

（3）所得運(yùn)動(dòng)軌跡的最大橫向加速度、最大縱向加速度、最大縱向jerk等均滿足動(dòng)力學(xué)約束。

（4）有效降低了運(yùn)動(dòng)規(guī)劃算法時(shí)間復(fù)雜度。綜合工況下，相比與分段jerk法混合最優(yōu)軌跡生成法，本文提出的運(yùn)動(dòng)規(guī)劃方法有效減少了最優(yōu)解逼近群中的軌跡數(shù)量，軌跡評估計(jì)算復(fù)雜度降低了46%，運(yùn)動(dòng)規(guī)劃算法總時(shí)間復(fù)雜度降低了15%。

綜上所述，本文提出的運(yùn)動(dòng)規(guī)劃方法能夠找到一條避免碰撞且滿足動(dòng)力學(xué)約束和行駛效率的可行軌跡，同時(shí)降低了算法時(shí)間復(fù)雜度。

橫縱向解耦增強(qiáng)混合運(yùn)動(dòng)規(guī)劃算法有以下值得繼續(xù)優(yōu)化的方向：針對更復(fù)雜、更多交通參與者的環(huán)境，對算法中的數(shù)據(jù)擴(kuò)展方式做進(jìn)一步優(yōu)化；應(yīng)進(jìn)一步將障礙物在感知、預(yù)測層獲取時(shí)的延遲及可信度納入考慮范圍，以增強(qiáng)規(guī)劃安全性和魯棒性。