動態交通場景下基于時空導航地圖的行駛軌跡規劃方法

宋文杰，馮思源，封志奇，付夢印

（北京理工大學 自動化學院，北京 100081）

智能無人系統，是一個集自主感知理解、智能決策規劃、集群協同控制等核心技術于一體的綜合體，具有極高的社會和經濟價值，是國家重點布局的科技方向，已成為引領新一輪科技革命和產業變革的重要戰略支點。無人駕駛汽車作為智能無人系統技術最典型的市場化/社會化應用，不僅可以極大地推動車輛產品革新和產業生態變革，更是解決當前社會交通通行效率低下、道路安全問題突出、資源環境壓力巨大等問題的有效途徑。

無人駕駛汽車在復雜動態的城市結構化道路條件下實現安全（Safe）、平穩（Smooth）、智能（Smart）、快速（Speed）的4S自動駕駛，目前主要面臨兩大挑戰：1. 大范圍場景的高精度地圖構建及精確定位定向；2. 高密度動態目標干擾下的智能決策與規劃。

針對挑戰1，百度、谷歌、高德等公司均已投入大量人力、物力、財力開展高精度地圖構建、組合定位、地圖匹配等工作，目前無人駕駛汽車在絕大多數城市環境下可實現較為穩定可靠的定位效果，具備一定商業落地條件；針對挑戰2，由于環境極為多樣、多類型目標運動特點極為復雜，盡管世界范圍內大量研究人員已開展廣泛研究，目前無人駕駛汽車在動態環境下的安全、高效、智能決策與規劃仍是一項重要難題，其自動駕駛水平與人類仍不具備可比性（或可替代性）。因此，高密度動態交通場景下實現4S標準的智能決策與規劃是自動駕駛功能的瓶頸技術，也是無人駕駛汽車體現其智力水平的一項重要指標。

為克服上述挑戰2，現有方法通常在二維行駛空間中搜索出一條無碰撞參考路徑，再將時間約束信息加入到參考路徑中，經過反復迭代，尋找出一條符合要求的軌跡。其中，最優化方法[1]通過在多組約束（如車輛橫向和縱向限制、環境限制、曲率限制等）下最小化性能指標（動能變化、加加速度、橫向加速度），以獲得平滑的駕駛軌跡[2]。Dieter Fox[3]提出的動態窗口法（Dynamic Window Approach）可以有效解決移動機器人的避障問題；斯坦福大學開發的自動駕駛汽車Junior在DARPA城市挑戰賽中成功展示了基于最優控制的軌跡規劃技術的有效性[4]。但此類方法往往會增加算法的時間復雜度且難以應用于速度和路徑高度耦合的高密度動態交通場景。

在Frenet[5]框架中，路徑-速度解耦法是目前常用的最優化方法之一，主要是分別優化路徑和速度：路徑優化主要考慮靜態障礙物；然后基于生成的路徑，結合動態目標以優化速度[6]。Zhan[7]等人通過沿著粗略參考路徑的縱向和橫向對運動規劃進行空間劃分，結合A*搜索和二次規劃進行粗略長期縱向運動和短期軌跡規劃；但該方法依然是對空間橫縱向的分解規劃，雖然降低了規劃問題的計算復雜度，但由于缺乏時間維度信息，存在一定局限性。百度Apollo EM planner[5]將高維的軌跡規劃解耦：路徑優化框架利用ST圖[8]進行無碰撞軌跡規劃；速度優化框架用來評估路徑優化器中車輛與低速障礙物的交互性，然后速度優化器接到生成的軌跡并求解最優速度。該類方法通過將時間與空間解耦，分別求解軌跡與速度，在一定程度上可以有效解決復雜交通場景下動態車輛擾動問題。

與上述思路不同，一些研究人員將時間和空間維度統一到同一坐標系進行軌跡規劃，以解決動態場景時空耦合問題。Ding等人[9]提出了一種新的統一時空語義走廊結構，為不同類型的語義提供一個抽象層次元素；該系統通過正向推演的模擬模型從預測行為中采樣得到種子點，依次膨脹出一系列相互連接的無碰撞立方體，并在時空域內由語義元素構成動態約束，利用分段Bezier曲線參數化的凸包和速度圖特性進行二次規劃，但該方法在時空走廊的種子點選取和時間分配等方面并未進行明確闡述。

由此可見，在高動態結構化道路高速自動駕駛場景中，無人駕駛汽車周圍運動的障礙物會在其規劃空間中引入時空約束。因此實現安全、平穩的自主規劃必須解決時空解耦問題，以在時空約束條件下搜索一條最優平滑時空軌跡。本文基于實時車路狀態反饋信息（從高精度地圖獲得道路狀態，從感知模塊獲得周圍實時動態目標狀態），以無人駕駛汽車為第一視角，通過多目標行為預測等手段，構建三維時空導航地圖；通過增加時間維度，將靜態障礙物和動態障礙物統一到同一參數空間中。基于該參數空間，通過前端A*路徑搜索初始化均勻B-樣條曲線控制點，設計軌跡代價函數，進行非線性優化，生成一條符合安全無碰撞和車輛運動約束（速度、加速度限制）的時空軌跡，從而將二維Frenet動態物理空間下決策與規劃問題轉化為三維時空下靜態場景決策與規劃問題，有效降低多目標運動對無人駕駛汽車決策與規劃引入的擾動。

1 基于時空導航地圖的軌跡優化方法系統

本文所述算法系統如圖1所示，主要包含時空導航地圖構建、前端路徑搜索和基于B-樣條曲線的軌跡優化三個模塊。首先，根據感知模塊的主車狀態信息（其中包括位置、速度、方向等）、周圍動態車輛的運動信息以及道路結構信息，通過對周圍動態車輛進行軌跡預測，構建Frenet坐標系下（s,d,t）局部三維時空占據柵格地圖，并構建歐式符號距離場（Euclidean Signed Distance Fields, ESDF）[10]。在基于本團隊已完成的車道時空間隙決策[11]下（綜合分析道路交通情況，構建車道時空間隙決策拓撲，選擇最優決策拓撲路徑作為無人駕駛汽車的實時決策結果，主要用于指導無人駕駛汽車是否變道、以及如何變道（依次通過哪些間隙），由于本部分非本文重點，此處不展開詳細介紹），對決策間隙進行局部目標點選取和A*路徑搜索，得到柵格路徑。將柵格路徑點進行采樣作為B-樣條曲線的初始控制點，并且構建均勻B-樣條曲線，基于歐式符號距離場對控制點進行非線性優化，得到符合車輛運動學約束的行駛軌跡。

圖1 基于時空域地圖的軌跡優化方法系統框圖Fig.1 System block diagram of trajectory optimization method based on spatio-temporal map

2 基于時空導航地圖的軌跡生成與優化

2.1 時空導航地圖構建

在機器人領域，常用地圖種類主要包含尺度地圖（Metric Map）、拓撲地圖（Topological Map）以及語義地圖（Semantic Map）。在軌跡規劃方面，常用的為尺度地圖，即地圖中每一點對應于一個坐標。占據柵格地圖（Occupancy Grid Map）[12]是將場景按照一定距離進行劃分，得到的一種結構化地圖，在占據柵格地圖中，對于一個點，存儲的是障礙物占據概率。對每個柵格點直接的存儲方式帶來的好處是坐標索引查詢復雜度低至O(1)（無論數據規模多大，都可以在一次計算后找到目標），但較高的地圖分辨率也將導致內存占用較大和更新延遲。而八叉樹地圖（Octo-map）[13]是對占據柵格地圖存儲方式的一種改進，環境中沒有障礙物的空間，用大方塊表示，當大方塊里面有障礙物時，便進行遞歸切分，直至尋到完整包含障礙物的最小方塊為止。該方法所構建的地圖為稀疏地圖，并且在查詢過程中是非直接查詢。

在高速道路環境下，車輛不進行任何倒車操作，因此本文方法所采用的局部地圖在Frenet坐標系下，僅考慮了無人駕駛汽車前方50米范圍區域，所包含時間維度范圍為0～5 s。

如圖2-3所示，將車輛感知結果映射到局部坐標系，主車位置始終保持固定（圖3中黑色標記）。

圖2 高速場景仿真環境Fig.2 The high-speed scene simulation environment

圖3 高速場景環境構建時空域地圖Fig.3 Constructing spatio-temporal maps in high-speed scenarios

由于本文重點不包括車輛軌跡預測，因此對于周圍動態車輛，我們研究采用線性預測的方法對周圍車輛進行運動狀態估計，從而得到如圖3所示的三維空間占據柵格地圖，地圖分辨率為0.02 m。時間維度從當前時刻至未來5 s，以推測5 s內周圍車輛的運動分布，車輛速度的不同呈現為占據柵格點在時間軸正方向上斜率的不同。如圖2所示，車輛1相比于車輛4的速度較大，則在圖3中車輛1的柵格點序列沿時間軸斜率更大。

三維占據柵格的作用在于為前端路徑搜索模塊提供可行的搜索空間，而針對后端軌跡優化模塊，為確保軌跡的安全可行，需要對軌跡的控制點進行位置約束，其中包括距離障礙物的距離。由于三維占據柵格地圖無法直接提供距離信息，本文通過構建三維空間中的歐式符號距離場，描述三維空間中任意點距離障礙物的距離信息，并對障礙物進行距離和梯度信息查詢。歐式符號距離場的應用對機器人的在線運動規劃具有重要意義。Han等人[14]于2019年提出FIESTA地圖系統，增量式地建立ESDF地圖。FIESTA系統通過引入兩個用以插入和刪除障礙物的獨立更新隊列，使用索引數據結構和雙向鏈表進行ESDF地圖維護，實現在廣度優先搜索算法框架下，更新盡可能少的地圖節點。如圖4所示，右側為三維空間中t= 0, 1, 2, 3, 4 s時的ESDF地圖。其中主車位于第三車道，根據構建的ESDF地圖可快速獲取任意一點處距離障礙物的距離信息，以用于后端軌跡優化。

圖4 構建歐式距離場ESDFFig.4 Build ESDF map

2.2 時空地圖中方向限制的A*路徑搜索

基于三維時空地圖構建和本團隊已完成的時空間隙決策算法[11]，可以獲得三維空間中局部目標點g（s,d,t）和三維占據柵格地圖。然后，采用A*搜索算法對主車節點到目標點的路徑進行搜索：其中采用的啟發函數為歐式距離函數，描述為當前節點距目標點的歐式距離d。如圖5所示（針對圖4所示場景），在高速道路行駛過程中，車輛行駛不能掉頭、倒車，因此在進行路徑搜索中，限制搜索方向為s和t維度的正方向，減少搜索過程中的訪問節點數，縮短A*搜索時間。

圖5 三維時空導航地圖下前端A*路徑搜索結果Fig.5 Front-end A* path search results under the three-dimensional spatio-temporal navigation map

2.3 基于B-樣條曲線的軌跡優化

2.3.1 B-樣條曲線

B-樣條曲線是B-樣條基函數（給定區間上的所有樣條函數組成一個線性空間）的線性組合，如圖6-7所示。

圖6 四階B-樣條曲線Fig.6 Fourth-order B-spline curve

設有{Q0,Q1…QN}共有N+1個控制點，一個節點向量U= {u0,u1…um}，用于定義k階（k-1次）樣條曲線，其中必須滿足k≥1、m=N+k，則k階樣條曲線的定義如下：

式中，Bi,k(u)為第i個k階B-樣條基函數，與控制點Qi相對應，定義如下：

B-樣條軌跡可由時間t參數化，其中t∈ [tk,tm-k]。對于均勻B-樣條曲線，每個節點跨度 Δtj=tj+1-tj的值Δt是相同的。因此，對于局部起始點（s0,d0,t0）和局部目標點（sg,dg,tg），定義節點跨度為Δt= (tg-t0)/(m+1)。

B-樣條曲線具有凸包性質：樣條曲線會被包含于控制點構成的凸包內（圖6）；并且，樣條曲線一階微分仍為B樣條曲線（圖7）。在軌跡優化過程中這些性質被廣泛應用[15,16]，以確保軌跡的動態可行性和行駛安全性。B-樣條曲線的階數越高，曲線的導數次數也越高，產生更多零點，導致原曲線存在較多極值；相反，階數越低，樣條曲線逼近控制點的效果越好。四階（三次）B-樣條曲線能夠實現二階導數連續，因此，本文選擇四階B-樣條曲線用于軌跡優化。針對動態可行性，對曲線進行一階和二階微分即可得到速度與加速度曲線，二者同樣具備B-樣條曲線特性，其控制點分別為{V0,V1…VN-1}和{A0,A1…AN-2}，限制速度與加速度為Vi∈[ -vmax,vmax]，Ai∈[ -amax,amax]。其中Vi和Ai計算如下：

圖7 B-樣條曲線的一階微分Fig.7 The first derivative of the fourth-order B-spline curve

2.3.2 控制點非線性優化

針對由N+1個控制點{Q0,Q1,… ,QN}定義的四階B-樣條曲線，由于軌跡初始狀態與末端狀態固定，需對初始和末端的位置、速度、加速度進行限制，故設置{Q0,Q1,Q2,QN-2,QN-1,QN}為固定值，優化的控制點為{Q3,Q4,… ,QN-3}共N-5個。

設計如下的代價函數：

其中，fs為平滑度代價函數，fd為距離代價函數，fv和fa分別表示速度和加速度代價函數，fκ表示曲率代價函數，1λ、2λ、3λ、4λ為相應的權重參數。

通過軌跡的幾何信息來定義平滑度代價函數fs，代價定義如下[17]：

距離代價函數定義如下：

其中，d（Qi）表示控制點Qi到最近障礙物的距離值（該值通過歐式距離場ESDF獲取），dsafe表示安全距離閾值，設置dsafe= 3m。

為將速度與加速度約束于規定范圍內，對超出最大允許值vmax、amax的速度和加速度進行懲罰，超速評估函數設計如下：

其中p∈{s,d}，超加速度評估函數形式與之相同。基于二者，定義速度和加速度代價函數fv和fa以懲罰速度和加速度不可行的控制點，形式如下:

曲率代價函數定義如下：

其中κ(Qi)表示Qi處曲率，進行軌跡曲率代價計算時需轉換為笛卡爾坐標系。

圖8展示了樣條曲線優化結果。首先對A*搜索得到的路徑點進行采樣得到初始控制點，并進行時間均勻分配得到均勻樣條曲線（圖8中黃色軌跡）。利用Nlopt開源非線性優化庫中的NLOPT_LD_TNE WTON算法[18]對式(4)的優化問題進行求解，得到優化后的軌跡（圖8中綠色軌跡）。

圖8 B-樣條曲線優化結果Fig.8 B-spline curve optimization results

3 仿真試驗與分析

本部分主要展示并分析時空域地圖中的軌跡優化結果，驗證所提出算法的有效性。在進行局部軌跡優化過程中，構建局部時空域導航地圖，并先后進行前端路徑搜索與后端軌跡優化。

首先分析系統框架中各個模塊耗時情況，如表1所示。對無人駕駛汽車駕駛過程中所有車輛的行駛方向進行假設，即假設車輛只能沿道路前向行駛，對A*搜索方向進行限制，縮小了搜索空間，使搜索平均耗時由12.20 ms減少至8.80 ms，效率得到提升。整個軌跡優化過程平均耗時為51.27 ms，滿足無人駕駛汽車行駛要求。

表1 時空域軌跡規劃仿真實驗耗時Tab.1 Time-consuming simulation experiment of trajectory planning under spatio-temporal map

圖9-12分別展示了在簡單車道保持場景和復雜變道場景下時空域軌跡規劃的仿真試驗界面和數據分析結果。如圖9-10所示，在保持車道場景下，主車（無人駕駛汽車）的速度、加速度以及曲率都在車輛運動約束范圍之內。由于主車前方有其他車輛，因此在保持車道的過程中，可以看到主車規劃結果有適當減速，以保持安全距離。圖11中換道場景隨即發生在圖9場景之后，相比之下，主車速度與加速度發生較大變化，在換道過程中主車稍微加速進行換道，換道完成后重新進行車速調整。并且在換道過程中勢必引起車輛曲率變化，如圖12所示，換道成功后軌跡曲率減小。

圖9 車道保持場景仿真界面Fig.9 Lane-keeping scene simulation interface

圖10 車道保持場景軌跡優化結果分析Fig.10 Analysis of the trajectory optimization results of the lane-keeping scene

圖11 換道場景仿真界面Fig.11 Lane-changing scene simulation interface

圖12 換道場景軌跡優化結果分析Fig.12 Analysis of the trajectory optimization results of the lane-changing scene

4 結 論

針對高動態結構化道路自動駕駛需求，本文提出了一種基于時空導航地圖的軌跡優化方法，在時空約束條件下提高車輛行駛軌跡質量，解決了高速場景下靜動態障礙物統一表征問題，提高了軌跡平滑度。該方法引入時間維度作為參考，將感知結果投影至三維時空地圖，構建歐氏距離場，針對決策目標先后進行路徑搜索與軌跡優化。同時，通過限制A*路徑搜索方向，縮小搜索空間，加快搜索效率，用A*路徑初始化均勻B-樣條曲線控制點，設計軌跡代價函數，進行非線性優化，生成最優軌跡控制點。仿真試驗結果表明，所提出的方法在車道保持場景以及換道場景下均能有效生成符合車輛運動約束的平滑軌跡，在4S駕駛標準上符合人類駕駛特性。