基于改進混合A*的智能汽車時空聯合規劃方法*

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

（1.武漢理工大學，現代汽車零部件技術湖北省重點實驗室，武漢 430070；2.武漢理工大學，汽車零部件技術湖北省協同創新中心，武漢 430070；3.武漢理工大學，新能源與智能網聯汽車湖北省工程技術中心，武漢 430070；4.東風汽車股份有限公司商品研發院，武漢 430000）

前言

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

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

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

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

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

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

1 時空軌跡粗搜索

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

1.1 時空柵格地圖構建

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

圖1 時空柵格地圖

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

1.2 時空節點擴展

1.2.1 時空節點狀態空間表示

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

圖2 傳統混合A*與時空混合A*節點擴展方式對比

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

1.2.2 時空子節點擴展過程

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

圖3 車輛運動學模型

（1）離散前輪轉角δ和加速度a

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

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

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

（2）基于運動學模型的時空子節點擴展

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

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

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

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

1.3 時空節點啟發函數設計

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

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

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

1.4 時空節點代價函數設計

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

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

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

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

結構化道路行車過程中，靜態障礙物的風險場分布具有各向同性，動態障礙物的風險場分布具有各向異性，相應的基于風險場的子節點安全代價值可表示為

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

1.5 時空混合A*算法流程

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

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

2 時空軌跡后處理

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

2.1 可行駛時空走廊創建

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

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

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

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

如圖5 所示，以連續的擴展量 Δs，沿d方向順序對上述最小包絡框進行增量式膨脹，其中增量式膨脹過程如圖5 序號1，2，…，10 所示。當某一方向的膨脹與障礙物發生碰撞、超出道路邊界或累計膨脹量達到設定上限時，終止該方向膨脹并記錄此時該方向上的累計膨脹量Id，直至4 個方向全部擴展結束。基于累計膨脹量Id，可以確定t1時刻節點在s和l方向的上下邊界限值，如式（11）所示。

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

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

2.2 軌跡優化模型構建

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

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

考慮車輛動力學約束，構造軌跡點橫向和縱向速度、加速度約束為

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

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

上述目標函數是二次函數，約束條件為線性約束，因此可將上述數值優化模型轉化為經典的二次規劃問題，求解二次規劃問題獲得最終軌跡。

3 仿真分析

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

圖6 聯合仿真平臺

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

表2 仿真實驗相關參數

3.1 換道超車場景

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

圖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距離自車較遠，綜合考慮通行效率和碰撞風險，自車選擇進行換道并適當加速以超越前車V1；t=3.71 s時，自車超越前車V1，此時由于車輛V2 駛近碰撞風險增大，自車選擇并回原車道；t=4.96 s 時，完成并道；此后車速逐漸降低至期望車速15 m/s，完成換道超車。圖11 展示了本方法在其中一個規劃周期的軌跡規劃結果對比。可以看出，本方法在軌跡粗搜索階段得到的路徑、速度和加速度不夠平滑，經過時空軌跡后處理過程可以有效提升規劃路徑和速度、加速度的質量，同時嚴格符合車輛動力學約束，有利于控制模塊進行軌跡跟蹤。

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

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

圖11 換道超車路徑及速度、加速度規劃結果（場景一）

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

（2）換道超車場景二

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

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

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

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

圖14 換道超車路徑及速度、加速度規劃結果（場景二）

3.2 旁車切入場景

如圖15和圖16所示，初始狀態自車位置為（5 m，1.75 m），縱向速度vs(0)=15.0 m/s，縱向加速度as(0)=0，橫向速度和加速度均為0。自車相鄰車道上有兩輛同向行駛的障礙物車輛V1 和V2，由于車輛V2 行駛車速較低，車輛V1 選擇切入自車所在車道以尋求超越車輛V2。根據障礙物不同的分布情況和運動狀態，旁車切入場景主要可分為以下兩類，如圖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 切入車速較低，綜合考慮通行效率和碰撞風險，t=2.23 s 時，自車選擇縱向加速以超越切入車輛V1，同時在車輛V1 切入點附近向右進行輕微避讓以降低碰撞風險。值得說明的是，這種橫縱向聯合處理橫向方向上突然出現障礙物的行為與人類駕駛員的駕駛習慣是一致的。t=5.47 s 時，超越車輛V1 完成，自車返回車道線附近居中行駛，同時車速逐漸降低到期望車速。

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

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

圖19 旁車切入路徑及速度、加速度規劃結果（場景一）

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

（2）旁車切入場景二

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

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

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

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

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

圖22 旁車切入路徑及速度、加速度規劃結果（場景二）

3.3 算法實時性分析

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

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

表3 算法實時性分析

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

4 結論

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