胡 杰，張志豪，陳瑞楠，陳銳鵬，劉昊巖，朱 琪，陳 暉

（1.武漢理工大學(xué)，現(xiàn)代汽車零部件技術(shù)湖北省重點實驗室，武漢 430070；2.武漢理工大學(xué)，汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心，武漢 430070；3.武漢理工大學(xué)，新能源與智能網(wǎng)聯(lián)汽車湖北省工程技術(shù)中心，武漢 430070；4.東風(fēng)汽車股份有限公司商品研發(fā)院，武漢 430000）

前言

運動規(guī)劃模塊作為自動駕駛系統(tǒng)的關(guān)鍵組成部分，其主要任務(wù)是基于地圖、感知和定位等信息生成一條安全、高效和舒適的可行駛軌跡［1］。當(dāng)前智能汽車已在低速、簡單道路場景下實現(xiàn)落地，但在中高速的復(fù)雜動態(tài)場景下，由于規(guī)劃方法問題導(dǎo)致智能汽車的行駛表現(xiàn)還有所欠缺。

根據(jù)是否進行時空解耦，軌跡規(guī)劃方法可分為時空解耦規(guī)劃和時空聯(lián)合規(guī)劃。時空解耦規(guī)劃將三維的軌跡規(guī)劃解耦為二維的路徑規(guī)劃和一維的速度規(guī)劃［2-3］，其核心思想是首先考慮靜態(tài)障礙物規(guī)劃一條可行駛路徑，然后在路徑的基礎(chǔ)上考慮動態(tài)障礙物進行速度規(guī)劃。常用時空解耦規(guī)劃方法有采樣法、搜索法和數(shù)值優(yōu)化方法［4］等。這種先求解路徑再求解速度的規(guī)劃方式，可以顯著降低軌跡求解的難度，實現(xiàn)軌跡規(guī)劃在線高效求解。但由于軌跡在路徑與速度層面存在強耦合，時空解耦順序求解的方式極大限制了所生成軌跡的靈活性和適應(yīng)性，只能處理靜態(tài)繞障、路口讓行等縱橫向運動較為解耦的一般場景［5-6］，在換道超車、鄰車切入等多目標(biāo)復(fù)雜動態(tài)場景下容易陷入軌跡次優(yōu)［7］，制約了當(dāng)前智能汽車的實際運行表現(xiàn)。

時空聯(lián)合規(guī)劃直接在三維時空范圍內(nèi)求解可行駛軌跡，相比時空解耦規(guī)劃，盡管由于問題維度增加導(dǎo)致求解效率降低，但這種時空聯(lián)合同時求解的規(guī)劃方法具有更大的求解空間和求解靈活性，在各類場景下具有更接近于熟練駕駛員的表現(xiàn)。目前時空聯(lián)合規(guī)劃方法主要可分為基于采樣［8］、基于搜索［9］和基于數(shù)值優(yōu)化［10］的3 種方法。與時空解耦規(guī)劃方法存在的問題相似，單一基于采樣或搜索的時空聯(lián)合規(guī)劃方法難以平衡軌跡的求解質(zhì)量和求解耗時，單一基于數(shù)值優(yōu)化的時空聯(lián)合規(guī)劃方法在三維時空范圍內(nèi)建模難度大，最優(yōu)解收斂速度慢且易陷入局部最優(yōu)［6，11］。因此有研究者提出將采樣或搜索方法與數(shù)值優(yōu)化方法相結(jié)合的時空聯(lián)合規(guī)劃方法。

文獻［11］中提出了一種基于搜索和優(yōu)化結(jié)合的時空聯(lián)合規(guī)劃方法，首先引入三維時空柵格地圖對車輛的運動環(huán)境進行建模，然后在三維柵格地圖中使用A*搜索自定義的車輛動作空間獲得初始軌跡，最后使用模型預(yù)測控制的方法對初始軌跡進行平滑處理。但該方法對車輛的實際可行駛狀態(tài)空間簡化過多，導(dǎo)致軌跡完備性和軌跡質(zhì)量很差。文獻［6］中采用RRT*在時空范圍內(nèi)進行采樣獲得初始軌跡，然后使用數(shù)值優(yōu)化的方法對初始軌跡進行平滑后處理。但RRT*的搜索魯棒性和一致性較差，導(dǎo)致相鄰周期之間的軌跡不連續(xù)，無法滿足結(jié)構(gòu)化道路上對軌跡求解質(zhì)量和實時性的要求。文獻［7］中首先獲取時空范圍內(nèi)的初始軌跡作為參考軌跡，然后基于參考軌跡構(gòu)建梯形棱柱體時空走廊，在可行駛走廊內(nèi)使用分段貝塞爾曲線進行軌跡優(yōu)化獲得精細(xì)軌跡，但其沒有提出具體的初始軌跡計算方法。

綜上，針對傳統(tǒng)時空解耦規(guī)劃方法在多目標(biāo)復(fù)雜動態(tài)場景下易陷入軌跡次優(yōu)，而現(xiàn)有時空聯(lián)合規(guī)劃方法難以平衡軌跡求解質(zhì)量和求解耗時的問題。為此，本文提出了一種采用分層架構(gòu)的智能汽車時空聯(lián)合規(guī)劃方法，首先使用改進混合A*在三維時空范圍內(nèi)進行軌跡粗搜索獲得初始軌跡，通過合理設(shè)計混合A*的節(jié)點擴展方式以及啟發(fā)函數(shù)和代價函數(shù)，提升軌跡的直接搜索效率和行為的合理性，然后基于初始軌跡構(gòu)建安全可行駛時空走廊，并綜合考慮車輛動力學(xué)約束、路徑及速度連續(xù)性約束等條件，構(gòu)建以安全、舒適和高效為優(yōu)化目標(biāo)的數(shù)值優(yōu)化模型對初始軌跡進行平滑后處理獲得最終軌跡。

本文中首先介紹時空軌跡粗搜索過程，然后介紹時空軌跡后處理過程，最后進行仿真分析，驗證所提規(guī)劃方法相較于傳統(tǒng)時空解耦規(guī)劃方法具有更高的靈活性和規(guī)劃結(jié)果合理性，同時驗證所提方法計算的實時性。

1 時空軌跡粗搜索

時空軌跡粗搜索包括兩個步驟。第一步，構(gòu)建時空柵格地圖，以描述未來一段時間內(nèi)自車與障礙物的位置關(guān)系及相對運動關(guān)系。第二步，基于時空柵格地圖使用改進的混合A*直接搜索初始粗軌跡。為提高軌跡搜索效率，本文基于軌跡的時空屬性提出了一種新的子節(jié)點擴展方法，并通過合理設(shè)計啟發(fā)函數(shù)和代價函數(shù)對可行駛節(jié)點進行代價排序，避免了節(jié)點沿著不必要的方向進行無效拓展。

1.1 時空柵格地圖構(gòu)建

二維x-y柵格地圖只具有幾何屬性，因此傳統(tǒng)混合A*［12］無法直接搜索帶有時間屬性的可行駛軌跡。軌跡的直接搜索需要構(gòu)建三維x-y-t柵格地圖，考慮到本文研究場景為具有車道線指引的結(jié)構(gòu)化道路，為降低運動建模的難度，本文使用Frenet 坐標(biāo)系進行統(tǒng)一的場景描述［13］，構(gòu)建三維s-l-t時空柵格地圖。其中s表示沿道路指引線的縱向位置，l表示相對于道路指引線的橫向位置，t表示時間。首先構(gòu)建二維s-l柵格地圖，然后將二維柵格地圖沿離散化的時間軸t進行擴展，離散時間間隔為Δt，得到離散形式的三維s-l-t時空柵格地圖，如圖1所示。

圖1 時空柵格地圖

在三維s-l-t時空柵格地圖中，靜止物體表現(xiàn)為沿時間軸t占據(jù)相同的s-l區(qū)域，如圖1綠色部分所示；移動物體在不同時間點的s-l位置狀態(tài)如圖1 藍色和黃色部分所示，表現(xiàn)為傾斜向上。其中動態(tài)障礙物在未來一段時間內(nèi)的位置應(yīng)由預(yù)測模塊［14］給出。因此相較于二維柵格地圖，使用時空柵格地圖可以清晰地描述未來一段時間內(nèi)自車與障礙物的位置關(guān)系及相對運動關(guān)系。

1.2 時空節(jié)點擴展

1.2.1 時空節(jié)點狀態(tài)空間表示

傳統(tǒng)混合A*的節(jié)點狀態(tài)空間為（s，l，θ，d），其中(s，l)表示車輛的縱向位置和橫向位置，θ表示航向角，d表示車輛前進或后退行駛方向，相鄰節(jié)點之間保持空間狀態(tài)連續(xù)，節(jié)點擴展方式如圖2（a）所示。

圖2 傳統(tǒng)混合A*與時空混合A*節(jié)點擴展方式對比

本文提出的三維時空混合A*節(jié)點狀態(tài)空間增加了時間維度信息，同時考慮到結(jié)構(gòu)化道路行車過程中不允許倒車，因此時空混合A*對應(yīng)的車輛狀態(tài)空間為（s，l，θ，v，t）。其中前3 項與傳統(tǒng)混合A*相同，第4 項v為速度狀態(tài)量，第5 項t為節(jié)點相對于當(dāng)前自車位置所在節(jié)點的時間，定義規(guī)劃時域T表示一次規(guī)劃考慮的未來時間范圍，則節(jié)點的相對時間t取值范圍為[0，T]。

1.2.2 時空子節(jié)點擴展過程

基于時空節(jié)點的狀態(tài)空間特征，本文采用前輪轉(zhuǎn)角δ和加速度a作為具有非完整性約束車輛的控制變量，基于如圖3 所示的車輛運動學(xué)模型進行子節(jié)點擴展，節(jié)點擴展方式如圖2（b）所示，時空子節(jié)點擴展過程如下。

圖3 車輛運動學(xué)模型

（1）離散前輪轉(zhuǎn)角δ和加速度a

基于車輛運動學(xué)約束將前輪轉(zhuǎn)角δ在[-δmax，δmax]進行均勻離散化，為保證子節(jié)點的擴展能夠沿著車輛航向角方向進行，前輪轉(zhuǎn)角離散數(shù)量N需要選取為奇數(shù)，得到離散前輪轉(zhuǎn)角集Δ，表達式如下：

式中i=1，2，…，N。

為平衡節(jié)點的擴展效率和擴展質(zhì)量，采用變間隔離散化車輛加速度a，離散加速度分布以0 為基準(zhǔn)，基于車輛動力學(xué)參數(shù)確定加速度上下限[-amax，amax]，在[-amax，amax]區(qū)間內(nèi)向左右兩側(cè)以逐漸加大的間隔采樣加速度，得到離散加速度集A，表達式如下：

（2）基于運動學(xué)模型的時空子節(jié)點擴展

傳統(tǒng)混合A*采用固定步長的方式擴展節(jié)點，如圖2（a）所示，軌跡搜索效果受搜索步長的影響很大，搜索步長過小將降低搜索效率，步長過大將降低搜索結(jié)果質(zhì)量。本文所提出的時空混合A*基于加速度采樣獲得空間變步長，如圖2（b）所示，從每個父節(jié)點NP(sp，lp，θp，vp，tp)開始，基于前述離散前輪轉(zhuǎn)角δ和加速度a和進行子節(jié)點擴展，子節(jié)點Ni(si，li，θi，vi，ti)的時空擴展過程表示如下：

式中：L為車輛軸距；dt為相鄰節(jié)點的時間步長；ai∈A；δi∈Δ。

為保證子節(jié)點與障礙物安全無碰撞，將上述時間步長dt按照更小的時間步長tstep均勻劃分為M段，則基于式（3）可獲得由父節(jié)向子節(jié)點擴展過程中的一組中間過渡節(jié)點[Ni1，Ni2...NiM]。若子節(jié)點及中間過渡節(jié)點與障礙物均無碰撞發(fā)生，則認(rèn)為該子節(jié)點安全。此外，對子節(jié)點還應(yīng)進行道路邊界約束檢查，以確保子節(jié)點處于道路邊界內(nèi)。若子節(jié)點同時通過碰撞檢查和道路邊界約束檢查，則進行下一步的節(jié)點代價評估，否則丟棄該擴展節(jié)點。

遍歷離散前輪轉(zhuǎn)角集Δ 和離散加速度集A，即可基于父節(jié)點擴展得到一系列子節(jié)點。

1.3 時空節(jié)點啟發(fā)函數(shù)設(shè)計

合適的啟發(fā)函數(shù)可以引導(dǎo)混合A*的搜索沿著最接近目標(biāo)點的方向進行。傳統(tǒng)混合A*多采用A*搜索結(jié)果和Reeds-Sheep 曲線作為啟發(fā)式函數(shù)，選擇兩者的最大值作為最終啟發(fā)信息。但使用A*和Reeds-Sheep 曲線作為啟發(fā)函數(shù)存在耗時嚴(yán)重的問題［15］，因此傳統(tǒng)混合A*一般應(yīng)用于對于算法計算實時性要求不高的低速非結(jié)構(gòu)化場景，無法滿足實時性要求較高的復(fù)雜動態(tài)場景。

本文研究對象為具有明確指引線的結(jié)構(gòu)化道路，并基于Frenet 坐標(biāo)系進行環(huán)境和運動建模，因此道路的幾何形狀對混合A*搜索過程的影響很小，同時考慮到在線求解的實時性，因此可以使用當(dāng)前點到目標(biāo)點的距離作為啟發(fā)項。本文采用時空歐式距離作為混合A*粗搜索過程的啟發(fā)函數(shù)，滿足條件的子節(jié)點Ni(si，li，θi，vi，ti)啟發(fā)函數(shù)Hi計算如下：

式中：第1 項為平面歐式距離啟發(fā)項；sgoal、lgoal分別為目標(biāo)節(jié)點的縱向位置和橫向位置；第2 項為時間距離啟發(fā)項；tgoal為目標(biāo)節(jié)點對應(yīng)的相對時間，作用于避免混合A*陷入不必要的時間停滯，加快搜索過程；wH1和wH2分別為對應(yīng)啟發(fā)項的權(quán)重。

1.4 時空節(jié)點代價函數(shù)設(shè)計

子節(jié)點代價指從搜索起始節(jié)點到當(dāng)前節(jié)點Ni(si，li，θi，vi，ti)的累積代價，由父節(jié)點NP的代 價Gp、父節(jié)點Np與子節(jié)點Ni之間的過渡代價兩部分組成，可表示為

式中：Gi為子節(jié)點舒適性代價項，通過子節(jié)點與車道中心線(scenterline，lcenterline)的橫向偏移衡量（式（6））；Gp為父節(jié)點代價；wg1、wg2、wg3為權(quán)重系數(shù)；Ei為子節(jié)點目標(biāo)代價項，通過子節(jié)點的速度與期望車速vexpected的偏差衡量（式（7））；Si為子節(jié)點安全性代價項，節(jié)點與障礙物的相對距離可以衡量碰撞風(fēng)險。

為提高混合A*節(jié)點搜索的高效性和軌跡行為的擬人性，本文參照文獻［16］中在時空柵格地圖中建立風(fēng)險場，該理論將障礙物視為風(fēng)險源中心，給予障礙物周圍的節(jié)點賦予不同的碰撞風(fēng)險懲罰值，風(fēng)險場方向由風(fēng)險源中心向四周輻射，自車與風(fēng)險源中心距離越近則碰撞風(fēng)險越大，反之，碰撞風(fēng)險越小。風(fēng)險場分布模型表達式為

式中：(s，l)和(us，ul)分別表示節(jié)點和障礙物風(fēng)險源中心的位置坐標(biāo)；σsg和σlg分別表示障礙物在s向和l向的分布因子，分布因子與障礙物的輪廓以及障礙物在s向和l向的速度和加速度相關(guān)。

結(jié)構(gòu)化道路行車過程中，靜態(tài)障礙物的風(fēng)險場分布具有各向同性，動態(tài)障礙物的風(fēng)險場分布具有各向異性，相應(yīng)的基于風(fēng)險場的子節(jié)點安全代價值可表示為

式中：r為節(jié)點相對風(fēng)險源中心的位置向量；θr為向量r與障礙物速度v之間的夾角，由于靜態(tài)障礙物的風(fēng)險場具有各向同性，因此其θr始終為0。

1.5 時空混合A*算法流程

以當(dāng)前自車所在位置（s1，l1，θ1，v1，t1）為搜索起始節(jié)點Nstart，基于式（3）進行時空節(jié)點擴展，擴展過程中綜合考慮節(jié)點無碰撞和道路邊界約束進行節(jié)點合理性檢查，并基于式（4）和式（5）進行節(jié)點代價評估以尋找最優(yōu)擴展節(jié)點，直至搜索至目標(biāo)節(jié)點Nend，完成時空混合A*搜索過程，得到由離散節(jié)點表示的初始粗軌跡。算法流程如表1所示。

表1 時空混合A*算法偽代碼

2 時空軌跡后處理

基于改進混合A*直接搜索出的軌跡不夠平滑，無法滿足控制模塊跟蹤執(zhí)行的需要，故本文采用數(shù)值優(yōu)化的方法對初始軌跡進行平滑后處理。所提時空軌跡后處理方案分為兩步：第一步，創(chuàng)建可行駛時空走廊，三維時空范圍的移動障礙物將導(dǎo)致自車可行駛區(qū)域嚴(yán)重非凸，因此本文引入可行駛時空走廊［13］將自車與周圍障礙物分離開，以構(gòu)建優(yōu)化問題的凸可行集，保證軌跡優(yōu)化后處理過程的實時性；第二步，創(chuàng)建軌跡優(yōu)化模型，在可行駛時空走廊內(nèi)，綜合考慮車輛動力學(xué)約束、路徑及速度連續(xù)性約束等條件，構(gòu)造以安全、舒適和高效為優(yōu)化目標(biāo)的軌跡優(yōu)化模型，求解優(yōu)化模型獲得最終軌跡。

2.1 可行駛時空走廊創(chuàng)建

上述軌跡粗搜索過程獲得的離散軌跡點時空分布較為稀疏，需要減小相鄰軌跡點之間的間隔以便于控制執(zhí)行。本文利用前述節(jié)點擴展過程中獲得的一系列嚴(yán)格無碰撞且符合車輛運動學(xué)約束的中間過渡節(jié)點進行軌跡等價重構(gòu)，獲得一組由擴展節(jié)點Ni及其對應(yīng)的中間過渡節(jié)點[Ni1，Ni2，...，NiM]構(gòu)成的時空節(jié)點集Λ，節(jié)點個數(shù)為Nc，Λ表達式為

沿時空節(jié)點集Λ表示的軌跡鋪設(shè)可行駛時空走廊，具體過程如下。

從當(dāng)前自車所在位置對應(yīng)的節(jié)點P1(s1，l1，θ1，v1，t1)開始，獲取相對時間t1時刻障礙物及道路邊界的空間占據(jù)狀態(tài)。使用有向矩形框描述t1時刻的自車及障礙物實際外輪廓，如圖4 所示，并沿坐標(biāo)系的s向和l向構(gòu)建自車有向矩形框的最小包絡(luò)框。

圖4 t時刻自車及障礙物的矩形框表示

如圖5 所示，以連續(xù)的擴展量 Δs，沿d方向順序?qū)ι鲜鲎钚“j(luò)框進行增量式膨脹，其中增量式膨脹過程如圖5 序號1，2，…，10 所示。當(dāng)某一方向的膨脹與障礙物發(fā)生碰撞、超出道路邊界或累計膨脹量達到設(shè)定上限時，終止該方向膨脹并記錄此時該方向上的累計膨脹量Id，直至4 個方向全部擴展結(jié)束?；诶塾嬇蛎浟縄d，可以確定t1時刻節(jié)點在s和l方向的上下邊界限值，如式（11）所示。

圖5 t時刻節(jié)點可行駛走廊開辟過程

由上述最小包絡(luò)框的擴展過程可知，節(jié)點在上述邊界限值內(nèi)可以確保嚴(yán)格無碰撞，因此基于邊界限值可構(gòu)造該節(jié)點的可行駛走廊。以相同的方式對時空節(jié)點集Λ中剩余節(jié)點Pj(sj，lj，θj，vj，tj) 依次執(zhí)行上述過程，完成可行駛時空走廊的開辟過程。

2.2 軌跡優(yōu)化模型構(gòu)建

優(yōu)化模型的目標(biāo)函數(shù)定義如式（12）所示，由Jl和Js兩部分構(gòu)成。目標(biāo)函數(shù)第1 項為軌跡點相對于初始粗軌跡的偏離懲罰項，第2 項為軌跡點的橫縱向速度與期望速度的偏差懲罰項，第3項和第4項分別為軌跡點的橫縱向加速度和加速度變化率懲罰項，目標(biāo)函數(shù)Jl的第5 項為軌跡點相對于車道中心線的偏離懲罰項。

基于前述可行駛時空走廊計算出的節(jié)點邊界限值，構(gòu)造軌跡點邊界安全性約束為

考慮車輛動力學(xué)約束，構(gòu)造軌跡點橫向和縱向速度、加速度約束為

為保證軌跡的高階連續(xù)性，本文參照文獻［17］中所提出的分段加速度變化率優(yōu)化理論，使用常量加速度變化率連接相鄰兩點，則軌跡點橫向的連續(xù)性約束可表示為

式中j=1，…，Nc-1。

上述目標(biāo)函數(shù)是二次函數(shù)，約束條件為線性約束，因此可將上述數(shù)值優(yōu)化模型轉(zhuǎn)化為經(jīng)典的二次規(guī)劃問題，求解二次規(guī)劃問題獲得最終軌跡。

3 仿真分析

為充分驗證本文所提時空聯(lián)合規(guī)劃方法的有效性和優(yōu)越性，搭建MALTAB/SimuLink 和PreScan、CarSim 聯(lián)合仿真平臺，在換道超車和旁車切入兩類典型復(fù)雜動態(tài)場景下進行實驗驗證，如圖6所示。

圖6 聯(lián)合仿真平臺

規(guī)劃時域T選取為8 s；障礙物軌跡預(yù)測時長Tp應(yīng)與規(guī)劃時域T一致［14］，故Tp=8 s。本文假設(shè)障礙物軌跡預(yù)測結(jié)果為已知；本文規(guī)劃軌跡的跟蹤控制采用常用的LQR 算法和PID 算法。實驗其它相關(guān)參數(shù)見表2。

表2 仿真實驗相關(guān)參數(shù)

3.1 換道超車場景

如圖7 和圖8 所示，初始狀態(tài)自車位置為（5 m，1.75 m），縱向速度vs(0)=15.0 m/s，縱向加速度as(0)=0 m/s2，橫向速度和加速度均為0。自車行駛方向前方有一輛低速移動的同向行駛障礙物車輛V1 影響自車正常通行，相鄰車道有一輛對向行駛障礙物車輛V2，為獲得滿意的駕駛空間，自車需要進行換道以超越前方障礙物車輛。根據(jù)障礙物不同的分布情況和運動狀態(tài)，換道超車場景可分為以下兩類。

圖7 換道超車場景一

（1）換道超車場景一

車輛V1 從初始位置（30 m，1.75 m）處以5 m/s的車速做勻速直線運動，車輛V2從初始位置（110 m，5.25 m）處以-10 m/s的車速做勻速直線運動。

圖9和圖10展示了本文所提方法在換道超車過程中的完整行駛軌跡。t=1.13 s 時，由于車輛V2距離自車較遠(yuǎn)，綜合考慮通行效率和碰撞風(fēng)險，自車選擇進行換道并適當(dāng)加速以超越前車V1；t=3.71 s時，自車超越前車V1，此時由于車輛V2 駛近碰撞風(fēng)險增大，自車選擇并回原車道；t=4.96 s 時，完成并道；此后車速逐漸降低至期望車速15 m/s，完成換道超車。圖11 展示了本方法在其中一個規(guī)劃周期的軌跡規(guī)劃結(jié)果對比。可以看出，本方法在軌跡粗搜索階段得到的路徑、速度和加速度不夠平滑，經(jīng)過時空軌跡后處理過程可以有效提升規(guī)劃路徑和速度、加速度的質(zhì)量，同時嚴(yán)格符合車輛動力學(xué)約束，有利于控制模塊進行軌跡跟蹤。

圖9 換道超車過程三維行駛軌跡（場景一）

圖10 換道超車過程不同規(guī)劃方法對比（場景一）

圖11 換道超車路徑及速度、加速度規(guī)劃結(jié)果（場景一）

圖10 還展示了另外兩種對比方法在換道超車過程中的車輛行駛軌跡。藍色曲線為傳統(tǒng)解耦規(guī)劃方法Lattice［18］，自車選擇減速跟隨低速運動的前車V1 行駛，在相鄰車道車輛V2 完全駛離后再進行換道超車。相比本文所提方法，Lattice 方法的行為過于保守，降低了自車通行效率。這是因為解耦的方法分別由路徑規(guī)劃和速度規(guī)劃處理靜態(tài)和動態(tài)障礙物，然后合并路徑和速度的求解結(jié)果生成軌跡。這種解耦處理的方式極大地限制了軌跡的求解空間和求解靈活性，導(dǎo)致復(fù)雜動態(tài)場景下容易出現(xiàn)軌跡次優(yōu)。綠色曲線為前言部分所述時空聯(lián)合規(guī)劃方法［6］時空RRT*，軌跡行為表現(xiàn)與本方法相近，靈活性相較Lattice均有明顯提升，這是由于時空RRT*與本方法的可行駛軌跡求解空間近似。但其在相鄰規(guī)劃周期之間的軌跡連續(xù)性很差，車輛行駛路徑和速度頻繁抖動，原因在于時空RRT*本質(zhì)上仍是一種隨機采樣方法，導(dǎo)致軌跡規(guī)劃結(jié)果的不確定度很高。本文所提方法在三維時空范圍內(nèi)統(tǒng)一處理障礙物，具有更加完備的軌跡求解空間。同時采用基于搜索的時空混合A*進行軌跡直接求解，在提升求解靈活性的同時，有效兼顧了相鄰周期之間軌跡的連續(xù)性，因此行為表現(xiàn)更加靈活，結(jié)果更加合理。

（2）換道超車場景二

車輛V1 從初始位置（25 m，1.75 m）處以5 m/s的車速做勻速直線運動；車輛V2從初始位置（40 m，5.25 m）處以-10 m/s 的初始車速做變加速直線運動。

仿真結(jié)果如圖12 和圖13 所示，初始時刻由于相鄰車道的車輛V2距離自車較近且車速較高，綜合考慮碰撞風(fēng)險和通行效率，自車適當(dāng)減速避讓前車V1；t=1.45 s 時，旁車V2 開始逐漸駛離，自車選擇換道并適當(dāng)加速以超越前車V1；t=2.93 s 時，超越前車V1，自車選擇并回原車道，同時車速逐漸降低，在t=4.62 s 時完成并道，換道超車完成。圖13 藍色曲線Lattice 與本文所提方法的行為表現(xiàn)相近，但本文所提方法的換道開始時間更早，速度波動更小，具有更高的通行效率和乘坐舒適性。圖13 綠色曲線所示為時空RRT* 行駛軌跡，同樣存在由于周期之間軌跡連續(xù)性差導(dǎo)致的行駛抖動問題。圖14 展示了本文方法在軌跡平滑后處理前后的路徑及速度、加速度的規(guī)劃結(jié)果對比。

圖12 換道超車過程三維行駛軌跡（場景二）

圖13 換道超車過程不同規(guī)劃方法對比（場景二）

圖14 換道超車路徑及速度、加速度規(guī)劃結(jié)果（場景二）

3.2 旁車切入場景

如圖15和圖16所示，初始狀態(tài)自車位置為（5 m，1.75 m），縱向速度vs(0)=15.0 m/s，縱向加速度as(0)=0，橫向速度和加速度均為0。自車相鄰車道上有兩輛同向行駛的障礙物車輛V1 和V2，由于車輛V2 行駛車速較低，車輛V1 選擇切入自車所在車道以尋求超越車輛V2。根據(jù)障礙物不同的分布情況和運動狀態(tài)，旁車切入場景主要可分為以下兩類，如圖15和圖16所示。

圖15 旁車切入場景一

圖16 旁車切入場景二

（1）旁車切入場景一

車輛V1 從初始位置（30 m，5.25 m）處以5 m/s的車速勻速切入自車車道；障礙物車輛V2從初始位置（90 m，5.25 m）處以15 m/s的車速勻速運動。

圖17 和圖18 展示了本文所提方法在旁車切入自車車道過程中的完整行駛軌跡。由于旁車V1 切入車速較低，綜合考慮通行效率和碰撞風(fēng)險，t=2.23 s 時，自車選擇縱向加速以超越切入車輛V1，同時在車輛V1 切入點附近向右進行輕微避讓以降低碰撞風(fēng)險。值得說明的是，這種橫縱向聯(lián)合處理橫向方向上突然出現(xiàn)障礙物的行為與人類駕駛員的駕駛習(xí)慣是一致的。t=5.47 s 時，超越車輛V1 完成，自車返回車道線附近居中行駛，同時車速逐漸降低到期望車速。

圖17 旁車切入過程三維行駛軌跡（場景一）

圖18 旁車切入過程不同規(guī)劃方法對比（場景一）

圖19 旁車切入路徑及速度、加速度規(guī)劃結(jié)果（場景一）

圖18 綠色曲線所示的時空RRT*行駛軌跡與本方法相近，但其同樣存在路徑和速度頻繁抖動，且規(guī)劃結(jié)果沒有本文方法合理。圖18 藍色曲線所示的Lattice 方法選擇縱向加速超越切入車輛V1，但與本文方法不同的是，其行駛路徑在橫向上無任何避讓行為。這是由于解耦規(guī)劃方法無法在路徑規(guī)劃中考慮具有時間屬性的動態(tài)障礙物，只能由速度規(guī)劃進行處理，因此以Lattice 為代表的解耦方法很難靈活應(yīng)對這類需要進行橫縱向聯(lián)合處理的動態(tài)場景，行為表現(xiàn)過于機械，規(guī)劃結(jié)果不夠合理。

（2）旁車切入場景二

車輛V1 從初始位置（15 m，5.25 m）處以10 m/s的初始車速加速切入自車車道（切入完成后車速達到15 m/s）；車輛V2 從初始位置（75 m，5.25 m）處以10 m/s的初始車速做勻速直線運動。

仿真實驗結(jié)果如圖20 和圖21 所示，由于旁車V1 切入車速較高，綜合考慮碰撞風(fēng)險和通行效率，t=1.57 s 時，自車選擇減速避讓切入車輛V1，同時在車輛V1 切入點附近向右進行輕微避讓以降低碰撞風(fēng)險。在t=4.21 s 時，車輛V1 切入過程完成，此時由于相鄰車道仍被車輛V2占據(jù)，自車選擇跟隨前車V1 行駛。圖21 中 Lattice 方法的結(jié)果與本文相似，自車選擇縱向減速避讓切入車輛V1，然后跟隨前車V1行駛，但其行駛路徑仍舊沒有類似本文所提方法的橫向輕微避讓行為。

圖20 旁車切入過程三維行駛軌跡（場景二）

圖21 旁車切入過程不同規(guī)劃方法對比（場景二）

圖19 和圖22 分別展示了本方法在其中一個規(guī)劃周期的軌跡規(guī)劃結(jié)果對比，同樣可以看出，經(jīng)過上述時空軌跡后處理的平滑過程，可使規(guī)劃出的最終路徑、速度及加速度更加平滑，軌跡舒適性更好，且有利于控制模塊進行軌跡跟蹤。

圖22 旁車切入路徑及速度、加速度規(guī)劃結(jié)果（場景二）

3.3 算法實時性分析

仿真電腦配置為Intel i7-11800H/16GB RAM，MATLAB 版本為2020a。使用MATLAB 優(yōu)化工具箱中的quadprog 求解器進行二次規(guī)劃求解，并將MATLAB程序轉(zhuǎn)成C代碼執(zhí)行以提升運行速度。

基于前述設(shè)計的換道超車和旁車切入兩類典型場景，隨機設(shè)置障礙物的初始位置及運動狀態(tài)，每類場景重復(fù)實驗100 次，統(tǒng)計本文所提方法及另外兩種規(guī)劃方法的平均耗時情況，結(jié)果如表3所示。

表3 算法實時性分析

本文所提方法的計算耗時相較于傳統(tǒng)解耦規(guī)劃方法Lattice 有所增加，但顯著低于時空聯(lián)合規(guī)劃方法時空RRT*，同時本文方法的靈活性和規(guī)劃結(jié)果合理性顯著提升，因此較好地平衡了軌跡求解效率和求解質(zhì)量。此外，值得說明的是，若將本文所提方法直接用C++進行實現(xiàn)，同時使用OSQP等求解效率更高的優(yōu)化求解器，可進一步提升本文方法的計算實時性表現(xiàn)。

4 結(jié)論

本文提出了一種基于分層架構(gòu)的智能汽車時空聯(lián)合規(guī)劃方法，在三維時空范圍內(nèi)統(tǒng)一處理靜態(tài)和動態(tài)障礙物并進行軌跡直接求解，有效提升了軌跡規(guī)劃質(zhì)量。首先使用改進混合A*進行軌跡粗搜索獲得初始軌跡，在保證軌跡搜索效率的同時，顯著提升了軌跡規(guī)劃的靈活性和結(jié)果的合理性。然后使用數(shù)值優(yōu)化的方法對初始軌跡進行平滑后處理，通過引入安全可行駛時空走廊簡化時空避碰約束，保證了軌跡優(yōu)化過程的實時性。仿真結(jié)果表明，本文所提方法相較于傳統(tǒng)時空解耦規(guī)劃方法，靈活性更高、規(guī)劃結(jié)果更加合理，同時具有較好的計算實時性。未來將對障礙物軌跡預(yù)測的不確定性進行建模，以適應(yīng)實際復(fù)雜駕駛場景的需要。