動態(tài)交通場景下基于時空導(dǎo)航地圖的行駛軌跡規(guī)劃方法

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

（北京理工大學(xué) 自動化學(xué)院，北京 100081）

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

無人駕駛汽車在復(fù)雜動態(tài)的城市結(jié)構(gòu)化道路條件下實現(xiàn)安全（Safe）、平穩(wěn)（Smooth）、智能（Smart）、快速（Speed）的4S自動駕駛，目前主要面臨兩大挑戰(zhàn)：1. 大范圍場景的高精度地圖構(gòu)建及精確定位定向；2. 高密度動態(tài)目標(biāo)干擾下的智能決策與規(guī)劃。

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

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

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

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

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

1 基于時空導(dǎo)航地圖的軌跡優(yōu)化方法系統(tǒng)

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

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

2 基于時空導(dǎo)航地圖的軌跡生成與優(yōu)化

2.1 時空導(dǎo)航地圖構(gòu)建

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

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

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

圖2 高速場景仿真環(huán)境Fig.2 The high-speed scene simulation environment

圖3 高速場景環(huán)境構(gòu)建時空域地圖Fig.3 Constructing spatio-temporal maps in high-speed scenarios

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

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

圖4 構(gòu)建歐式距離場ESDFFig.4 Build ESDF map

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

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

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

2.3 基于B-樣條曲線的軌跡優(yōu)化

2.3.1 B-樣條曲線

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

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

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

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

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

B-樣條曲線具有凸包性質(zhì)：樣條曲線會被包含于控制點構(gòu)成的凸包內(nèi)（圖6）；并且，樣條曲線一階微分仍為B樣條曲線（圖7）。在軌跡優(yōu)化過程中這些性質(zhì)被廣泛應(yīng)用[15,16]，以確保軌跡的動態(tài)可行性和行駛安全性。B-樣條曲線的階數(shù)越高，曲線的導(dǎo)數(shù)次數(shù)也越高，產(chǎn)生更多零點，導(dǎo)致原曲線存在較多極值；相反，階數(shù)越低，樣條曲線逼近控制點的效果越好。四階（三次）B-樣條曲線能夠?qū)崿F(xiàn)二階導(dǎo)數(shù)連續(xù)，因此，本文選擇四階B-樣條曲線用于軌跡優(yōu)化。針對動態(tài)可行性，對曲線進行一階和二階微分即可得到速度與加速度曲線，二者同樣具備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 控制點非線性優(yōu)化

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

設(shè)計如下的代價函數(shù)：

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

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

距離代價函數(shù)定義如下：

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

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

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

曲率代價函數(shù)定義如下：

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

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

圖8 B-樣條曲線優(yōu)化結(jié)果Fig.8 B-spline curve optimization results

3 仿真試驗與分析

本部分主要展示并分析時空域地圖中的軌跡優(yōu)化結(jié)果，驗證所提出算法的有效性。在進行局部軌跡優(yōu)化過程中，構(gòu)建局部時空域?qū)Ш降貓D，并先后進行前端路徑搜索與后端軌跡優(yōu)化。

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

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

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

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

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

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

圖12 換道場景軌跡優(yōu)化結(jié)果分析Fig.12 Analysis of the trajectory optimization results of the lane-changing scene

4 結(jié) 論

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