齊堯，朱彥齊，李永樂(lè)，徐友春

（陸軍軍事交通學(xué)院，天津 300161）

0 引言

運(yùn)動(dòng)規(guī)劃以獲取安全無(wú)碰的可行駛軌跡為目標(biāo)，在智能駕駛汽車(chē)、無(wú)人作業(yè)機(jī)器人和服務(wù)機(jī)器人的智能軟件技術(shù)中擔(dān)任著舉足輕重的作用。對(duì)于動(dòng)靜態(tài)混合環(huán)境中（如狹窄街道和越野動(dòng)態(tài)場(chǎng)景）的智能車(chē)，其運(yùn)動(dòng)規(guī)劃受到動(dòng)態(tài)障礙物的速度維度、靜態(tài)障礙物形狀的非凸性和自身運(yùn)動(dòng)學(xué)模型的共同約束。

傳統(tǒng)的規(guī)劃方法如仿生學(xué)方法、A*和RRT等，未考慮車(chē)輛的非完整性約束，所規(guī)劃路徑難以被智能車(chē)跟蹤。近年來(lái)靜態(tài)環(huán)境中的智能車(chē)路徑規(guī)劃問(wèn)題已得到較好地解決[1]，動(dòng)靜態(tài)混合環(huán)境中的運(yùn)動(dòng)規(guī)劃主要有如下挑戰(zhàn)：車(chē)輛的非完整性約束增加了軌跡求解的復(fù)雜度，靜態(tài)障礙物的不規(guī)則形狀降低了求解速度，感知、預(yù)測(cè)和控制結(jié)果存在的不確定性要求高頻進(jìn)行重規(guī)劃，無(wú)時(shí)間維度的規(guī)劃結(jié)果極大地喪失求解的最優(yōu)性，增加時(shí)間維度后的求解時(shí)效性大幅降低，復(fù)雜的決策內(nèi)容（超車(chē)、會(huì)車(chē)和停車(chē)避讓等）需要融合在運(yùn)動(dòng)規(guī)劃中。

本文將針對(duì)動(dòng)態(tài)環(huán)境的運(yùn)動(dòng)規(guī)劃方法總結(jié)為以下4類(lèi)：第1類(lèi)方法將障礙物視為靜止，通過(guò)快速重規(guī)劃完成避障[2]，即將動(dòng)態(tài)障礙物或其短期預(yù)測(cè)軌跡視為靜態(tài)障礙物，確保短期內(nèi)與其不發(fā)生碰撞。該類(lèi)方法的求解結(jié)果缺乏完備性和最優(yōu)性，無(wú)法適應(yīng)快速變化的場(chǎng)景和需要“停止-等待”的場(chǎng)景[3]。第2類(lèi)是速度障礙（Velocity Obstacles,VO）方法，F(xiàn)iorini 等[4]提出以機(jī)器人速度矢量為原點(diǎn)建立速度空間，將障礙物和機(jī)器人擬合為帶有速度矢量的圓，并投影到該相對(duì)速度空間內(nèi)，通過(guò)選擇安全速度區(qū)間的方式進(jìn)行避障，能夠以毫米級(jí)的運(yùn)算速度得到躲避動(dòng)態(tài)障礙物的動(dòng)作序列。該類(lèi)方法易出現(xiàn)陷入“死胡同”和頻繁抖動(dòng)的局部最優(yōu)問(wèn)題。第3 類(lèi)是路徑-速度解耦規(guī)劃的方法，首先使用采樣、擬合和優(yōu)化等方式生成靜態(tài)環(huán)境中的安全路徑，其次基于可行駛路徑建立ST（路徑長(zhǎng)度-時(shí)間）空間對(duì)每一動(dòng)態(tài)障礙物做出決策（避讓、忽略、超車(chē)和停止）[5]，以決策為約束條件規(guī)劃得到速度曲線。這類(lèi)方法在結(jié)構(gòu)化環(huán)境中的適應(yīng)能力較強(qiáng)，在非結(jié)構(gòu)化場(chǎng)景中需輔以大量的前期決策。第4 類(lèi)是在三維時(shí)空內(nèi)進(jìn)行運(yùn)動(dòng)規(guī)劃的方法，以基于搜索和基于采樣的方法為主，Ajanovic等[6]在SL空間內(nèi)加入時(shí)間維度，對(duì)環(huán)境進(jìn)行大粒度離散，基于搜索的方法在結(jié)構(gòu)化動(dòng)態(tài)環(huán)境中得到三維軌跡；Lin 等[7]在XYT空間內(nèi)通過(guò)簡(jiǎn)化搜索空間和障礙物碰撞檢測(cè)，為擁擠行人場(chǎng)景內(nèi)的低速運(yùn)動(dòng)（速度不大于1.5 m·s-1）類(lèi)車(chē)機(jī)器人提供安全軌跡；Yang等[8]提出RRT（Rapidly-exploring Random Trees）與CBF（Control Barrier Function）結(jié)合的CBF-RRT 方法，在凸空間快速采樣得到可行軌跡。

在三維時(shí)空內(nèi)進(jìn)行運(yùn)動(dòng)規(guī)劃能夠完備地解決動(dòng)靜態(tài)混合環(huán)境中的運(yùn)動(dòng)規(guī)劃問(wèn)題，現(xiàn)有研究大多針對(duì)某一特定工況，在動(dòng)靜態(tài)混合環(huán)境中應(yīng)用時(shí)存在如下問(wèn)題：針對(duì)動(dòng)態(tài)障礙物的方法，難以同時(shí)解決任意形狀（非凸）靜態(tài)障礙物的避讓問(wèn)題；多以低速運(yùn)動(dòng)的小型機(jī)器人（如圓盤(pán)機(jī)器人和小型輪式機(jī)器人）為研究對(duì)象，在小范圍場(chǎng)景內(nèi)展開(kāi)研究。本文面向存在動(dòng)態(tài)障礙物和任意形狀靜態(tài)障礙物的環(huán)境，針對(duì)以上問(wèn)題提出一種基于搜索的三維運(yùn)動(dòng)規(guī)劃方法，以解決智能車(chē)長(zhǎng)期運(yùn)動(dòng)規(guī)劃問(wèn)題。本文的主要貢獻(xiàn)如下：

（1）提出一種基于混合A*的時(shí)空規(guī)劃方法，該方法基于時(shí)空柵格同步規(guī)劃速度和路徑，基于舒適性模型設(shè)置不同車(chē)速下運(yùn)動(dòng)基元的轉(zhuǎn)彎半徑，采用PMP（Partial Motion Planning）解決搜索周期長(zhǎng)的問(wèn)題，以高頻求解得到滿(mǎn)足車(chē)輛運(yùn)動(dòng)學(xué)約束的時(shí)間-空間軌跡。

（2）使用多圓擬合的方法將車(chē)輛轉(zhuǎn)化為線段，在膨脹空間內(nèi)快速完成靜態(tài)障礙物碰撞檢測(cè)；提出時(shí)間間隔法對(duì)動(dòng)態(tài)障礙物進(jìn)行時(shí)空安全檢測(cè)，提升求解過(guò)程的碰撞檢測(cè)效率。

（3）構(gòu)建速度啟發(fā)式函數(shù)，與距離啟發(fā)式函數(shù)共同引導(dǎo)搜索過(guò)程，提升在時(shí)空柵格中的搜索效率，并將速度啟發(fā)式的計(jì)算方法應(yīng)用于分析擴(kuò)展。

1 基于時(shí)空柵格的運(yùn)動(dòng)基元構(gòu)建

1.1 搜索空間

本文選擇笛卡爾直角坐標(biāo)系作為搜索空間以適應(yīng)復(fù)雜非結(jié)構(gòu)化環(huán)境。在包含平面和時(shí)間的空間中，運(yùn)動(dòng)規(guī)劃包含路徑和速度兩個(gè)維度，常用的方式有解耦式和耦合式兩類(lèi)運(yùn)動(dòng)規(guī)劃方法。解耦式方法是在二維平面獲取路徑，基于路徑在ST 空間內(nèi)進(jìn)行速度規(guī)劃，這類(lèi)方法輔以離散型決策能夠保守地應(yīng)對(duì)大多數(shù)場(chǎng)景。但如圖1 所示的場(chǎng)景卻難以有效處理，解耦式的運(yùn)動(dòng)規(guī)劃方法將會(huì)導(dǎo)致智能車(chē)停在路口，并不能有效規(guī)劃出圖1所示“停車(chē)-等待”的運(yùn)動(dòng)軌跡。

圖1 一種極限場(chǎng)景Fig.1 An extreme scene

本文使用三維坐標(biāo)系為搜索空間，即在二維直角坐標(biāo)中增加時(shí)間維度作為搜索空間Ω。

式中：q為搜索空間Ω內(nèi)的點(diǎn)，t為時(shí)間，以車(chē)輛初始位置為時(shí)間原點(diǎn)，單次規(guī)劃的最長(zhǎng)時(shí)間為Nt；x、y為笛卡爾坐標(biāo)系坐標(biāo)。

將搜索空間離散為時(shí)空柵格，處于同一時(shí)空柵格內(nèi)的搜索節(jié)點(diǎn)具有相同的索引值。圖2（a）為時(shí)空柵格中的靜態(tài)障礙物示意圖，圖2（b）為動(dòng)態(tài)障礙物在時(shí)空柵格中的示意圖，不同于靜態(tài)障礙物，隨著時(shí)間增加，其在XY平面的位置逐漸發(fā)生變化。

圖2 障礙物時(shí)空柵格Fig.2 Spatiotemporal grid of obstacles

1.2 車(chē)輛運(yùn)動(dòng)基元

以u(píng)=（a,φ）（a∈[amin,amax],φ∈[-φmax,φmax]）表示車(chē)輛的縱向控制量a和橫向控制量φ，其中，amin、amax為最小、最大加速度，φmax為最大等效車(chē)輪轉(zhuǎn)角。以（v,ω）表示行進(jìn)切線方向的速度v和角速度ω，行駛長(zhǎng)度為d，車(chē)輛軸距為L(zhǎng)，朝向?yàn)棣?，初速度為v0。假設(shè)車(chē)輪在平面內(nèi)做無(wú)滑動(dòng)的純滾動(dòng)運(yùn)動(dòng)，可得控制量、速度和角速度的運(yùn)動(dòng)學(xué)關(guān)系為

為有效在搜索空間展開(kāi)運(yùn)動(dòng)規(guī)劃，選擇后軸中心點(diǎn)作為車(chē)輛的定位點(diǎn)。x=（x,y,θ,v）為車(chē)輛狀態(tài)空間，初始狀態(tài)x0=（x0,y0,θ0,v0），終點(diǎn)狀態(tài)xg=（xg,yg,θg,vg）。在搜索時(shí)按照固定時(shí)間間隔Δt0和最長(zhǎng)擴(kuò)展步長(zhǎng)dmax進(jìn)行單步節(jié)點(diǎn)擴(kuò)展，即

當(dāng)φ≠0 時(shí)，行駛時(shí)兩個(gè)節(jié)點(diǎn)之間的狀態(tài)量差值為

當(dāng)φ=0 時(shí)，車(chē)輛保持直行狀態(tài)，轉(zhuǎn)彎半徑為無(wú)窮，此時(shí)運(yùn)動(dòng)學(xué)模型與阿克曼轉(zhuǎn)向模型無(wú)關(guān)，節(jié)點(diǎn)間的差值為

本文通過(guò)對(duì)控制量u離散的方式生成運(yùn)動(dòng)學(xué)基元。離散取各個(gè)控制量作為輸入量，得到XY平面和XT平面的節(jié)點(diǎn)ε，如圖3所示，圖3（a）括號(hào)中數(shù)值為該位置節(jié)點(diǎn)對(duì)應(yīng)的輸入控制量。

圖3 節(jié)點(diǎn)擴(kuò)展方式Fig.3 Node expansion mode

智能車(chē)的速度可達(dá)每小時(shí)數(shù)十公里，在不發(fā)生側(cè)滑的約束條件下，不同車(chē)速對(duì)應(yīng)的最小轉(zhuǎn)彎半徑不盡相同，本文提出由“舒適性模型”確定不同速度的最小轉(zhuǎn)彎半徑。根據(jù)公路線形舒適性評(píng)價(jià)方法對(duì)舒適性模型進(jìn)行定義：車(chē)輛行駛過(guò)程中橫向加速度低于0.36g時(shí)為舒適，其中，g為重力加速度。橫向加速度定義為垂直于車(chē)輛行駛方向的加速度，是車(chē)輛在轉(zhuǎn)彎過(guò)程中離心力帶來(lái)的加速度。其計(jì)算方法可用aL=ρv2表示，其中，ρ為車(chē)輛轉(zhuǎn)彎半徑為r時(shí)的曲率，以此確定搜索過(guò)程中的節(jié)點(diǎn)半徑。

2 智能車(chē)時(shí)空運(yùn)動(dòng)規(guī)劃方法

2.1 實(shí)時(shí)搜索算法

本文基于混合A*算法[1]，采用所提的時(shí)空運(yùn)動(dòng)基元、啟發(fā)式函數(shù)和碰撞檢測(cè)方法，按照?qǐng)D4 的算法進(jìn)行時(shí)空搜索。采用哈希表M和優(yōu)先隊(duì)列Q對(duì)搜索算法進(jìn)行實(shí)現(xiàn)：通過(guò)維護(hù)高效的哈希表M存儲(chǔ)節(jié)點(diǎn)及其對(duì)應(yīng)的索引值，可使用節(jié)點(diǎn)的索引值在M中快速查找對(duì)應(yīng)的節(jié)點(diǎn)；通過(guò)優(yōu)先隊(duì)列Q存儲(chǔ)節(jié)點(diǎn)索引值及其f數(shù)值，f=g+h，其中，f、g、h分別為總代價(jià)、實(shí)際代價(jià)和啟發(fā)式代價(jià)，優(yōu)先隊(duì)列Q中的節(jié)點(diǎn)按照f(shuō)進(jìn)行升序排列，獲取最優(yōu)節(jié)點(diǎn)的索引值，通過(guò)索引值在M中快速查找對(duì)應(yīng)的優(yōu)先節(jié)點(diǎn)。智能車(chē)在動(dòng)態(tài)行進(jìn)過(guò)程中的目標(biāo)狀態(tài)xg難以準(zhǔn)確設(shè)置，Petti[9]提出PMP：考慮觀測(cè)范圍內(nèi)的障礙物，獲得有效時(shí)間內(nèi)的可行軌跡，不將到達(dá)目標(biāo)點(diǎn)作為唯一搜索終止條件。據(jù)此本文使用兩類(lèi)搜索終止條件：第1種為到達(dá)目標(biāo)狀態(tài)，第2種為達(dá)到最長(zhǎng)時(shí)間后，尋找不屬于ICS（Inevitable Collision States）狀態(tài)XICS的子節(jié)點(diǎn)并返回，即最優(yōu)節(jié)點(diǎn)ni?XICS。同時(shí)使用分析擴(kuò)展（Analytic Expansions）進(jìn)行抽樣擴(kuò)展，生成節(jié)點(diǎn)與終點(diǎn)直連的路徑和速度曲線，無(wú)碰撞后的路徑及其速度曲線將直接用于最終路徑，搜索過(guò)程及結(jié)果如圖5所示。使用分析擴(kuò)展的方式，解決了離散空間內(nèi)搜索算法不能夠準(zhǔn)確到達(dá)目標(biāo)狀態(tài)的問(wèn)題。除此之外，通過(guò)離散控制量的方式在連續(xù)空間搜索得到的結(jié)果并不是全局最優(yōu)的和完備的，且隨著離散精度的降低，到達(dá)目標(biāo)狀態(tài)的精度、最優(yōu)性和完備性逐漸下降，文獻(xiàn)[1]通過(guò)實(shí)驗(yàn)證明，選擇合理的離散精度能夠獲得接近全局最優(yōu)和完備的結(jié)果。

圖4 算法流程圖Fig.4 Flowchart of algorithm

圖5 時(shí)空搜索結(jié)果Fig.5 Spatiotemporal search result

節(jié)點(diǎn)的索引值由（x,y,θ,t）組成，同一柵格內(nèi)的節(jié)點(diǎn)具備相同的索引值，但不會(huì)將節(jié)點(diǎn)坐標(biāo)值映射到柵格中心。生成有效子節(jié)點(diǎn)的方式為遍歷離散的控制量，獲取不處于關(guān)閉列表、靜態(tài)障礙物碰撞檢測(cè)和動(dòng)態(tài)障礙物時(shí)空檢測(cè)均為安全的節(jié)點(diǎn)。得到有效子節(jié)點(diǎn)集合后，對(duì)其進(jìn)行遍歷，把不處于開(kāi)啟列表的子節(jié)點(diǎn)插入到隊(duì)列，并將子節(jié)點(diǎn)與歷史節(jié)點(diǎn)進(jìn)行對(duì)比，選擇較優(yōu)節(jié)點(diǎn)替換隊(duì)列中的原有節(jié)點(diǎn)。

2.2 碰撞檢測(cè)

按照速度將環(huán)境E中的障礙物O分為靜態(tài)障礙物OS和動(dòng)態(tài)障礙物OD。假設(shè)在上層系統(tǒng)中已對(duì)動(dòng)態(tài)障礙物OD進(jìn)行跟蹤預(yù)測(cè)，任意動(dòng)態(tài)障礙物OD,i,i∈{0,1,…,N}，N為動(dòng)態(tài)障礙物總數(shù)量，均可使用其凸包絡(luò)擬合的矩形框表示，包含一條或多條行駛預(yù)測(cè)軌跡Pi。每條預(yù)測(cè)軌跡由一系列按照時(shí)間排序的點(diǎn)組成，每個(gè)點(diǎn)有位置、時(shí)間、速度和加速度信息，本文暫未考慮預(yù)測(cè)軌跡的置信度。

OS形狀迥異，凹凸形狀不定，無(wú)論是轉(zhuǎn)化為圓形還是多邊形，都存在轉(zhuǎn)化誤差和較大計(jì)算量。車(chē)輛并非標(biāo)準(zhǔn)的圓形，采用圖6所示的多個(gè)圓近似表示，將車(chē)輛轉(zhuǎn)化為多個(gè)擬合圓圓心連接成的線段L。本文使用柵格法表示OS，對(duì)所有OS進(jìn)行膨脹。使用L在經(jīng)過(guò)膨脹后的位形空間內(nèi)進(jìn)行安全檢測(cè)，以此在搜索過(guò)程中完成高效的靜態(tài)障礙物碰撞檢測(cè)。

圖6 碰撞檢測(cè)方法Fig.6 Collision detection method

OD具備較規(guī)則的形狀，為達(dá)到快速準(zhǔn)確碰撞檢測(cè)的目的，本文將所有動(dòng)態(tài)障礙物轉(zhuǎn)化為單一或多個(gè)圓形。使用時(shí)間間隔（Time Interval, TI）的概念，將OD,i的預(yù)測(cè)軌跡Pi按照等時(shí)間間隔離散化為M段，如圖7（a）所示。圖7（b）中動(dòng)態(tài)障礙a、b、c分別按照加速、減速、勻速運(yùn)動(dòng)，其預(yù)測(cè)軌跡在相同時(shí)間間隔內(nèi)的長(zhǎng)度不盡相同。本文對(duì)每一段時(shí)間間隔內(nèi)的軌跡進(jìn)行線性化處理，按照當(dāng)前擴(kuò)展子節(jié)點(diǎn)εk的時(shí)間，選擇對(duì)應(yīng)時(shí)間間隔內(nèi)所有軌跡段進(jìn)行快速交叉性檢測(cè)：將εk∩Pi,ti-ti+1=?（i=1,…,N）確定為安全狀態(tài)，即圖3中運(yùn)動(dòng)基元εk與該時(shí)間間隔Ii內(nèi)的所有預(yù)測(cè)軌跡線段均無(wú)交集。

圖7 預(yù)測(cè)軌跡時(shí)間間隔Fig.7 Time interval of prediction trajectory

2.3 啟發(fā)式函數(shù)

增加時(shí)間維度將大幅降低搜索算法的時(shí)效性，本文提出一種聯(lián)合啟發(fā)式算法以提高搜索效率。在保留距離啟發(fā)式函數(shù)hd的基礎(chǔ)上，增加包含時(shí)間維度的速度啟發(fā)式函數(shù)ht，ωd、ωt為距離啟發(fā)式和速度啟發(fā)式的權(quán)重系數(shù)，以h=ωdhd+ωtht加權(quán)組合距離啟發(fā)式和速度啟發(fā)式，在平面域和時(shí)間域內(nèi)指引搜索樹(shù)快速到達(dá)目標(biāo)狀態(tài)。

選擇Dubins曲線長(zhǎng)度作為距離啟發(fā)式函數(shù)，并根據(jù)上文“舒適性模型”確定轉(zhuǎn)彎半徑。Dubins 曲線由最小轉(zhuǎn)彎半徑形成的弧線線段和直線線段組成，滿(mǎn)足可接受性；節(jié)點(diǎn)擴(kuò)展時(shí)使用相同轉(zhuǎn)彎半徑，滿(mǎn)足一致性。本文采用梯形速度規(guī)劃法計(jì)算到達(dá)目標(biāo)狀態(tài)的時(shí)間，以此作為速度啟發(fā)式函數(shù)。根據(jù)距離啟發(fā)式的長(zhǎng)度sh，在最大速度vmax的約束下，使用梯形速度規(guī)劃法計(jì)算當(dāng)前狀態(tài)（s0,v0）到目標(biāo)狀態(tài)（sg,vg）的時(shí)間，以到達(dá)目標(biāo)狀態(tài)的時(shí)間作為啟發(fā)式數(shù)值，不能到達(dá)目標(biāo)狀態(tài)則時(shí)間為無(wú)窮大。圖8為兩種典型場(chǎng)景中的梯形速度規(guī)劃結(jié)果。

圖8 梯形速度規(guī)劃結(jié)果Fig.8 Results of trapezoidal velocity planning

按照梯形特征將行駛長(zhǎng)度分為3段，各段長(zhǎng)度分別為s1、s2、s3，sh=s1+s2+s3。得到圖8（a）所示場(chǎng)景的計(jì)算方法為

圖8（b）所示場(chǎng)景的計(jì)算方法為

根據(jù)式（6）和式（7）計(jì)算得到s1、s2、s3，進(jìn)而得到總耗時(shí)。使用距離和速度共同作為本文方法的啟發(fā)式函數(shù)，可達(dá)到降低搜索節(jié)點(diǎn)數(shù)量和加快搜索速度的目的，是在時(shí)空柵格內(nèi)快速搜索的重要前提。

3 實(shí)驗(yàn)與分析

3.1 定性分析

為驗(yàn)證本文時(shí)空搜索方法在非結(jié)構(gòu)化動(dòng)靜態(tài)混合環(huán)境中的有效性，本文在仿真環(huán)境中設(shè)置大量典型場(chǎng)景，并與在靜態(tài)和動(dòng)態(tài)環(huán)境中常用的混合A*算法（Hybrid State A*）和速度障礙法（Velocity Obstacles,VO）進(jìn)行比較?；诼窂剿俣冉怦钜?guī)劃的思想設(shè)計(jì)一種HAWG（Hybrid State A*&Wait and Go）方法：該方法采用混合A*算法搜索得到與靜態(tài)障礙物無(wú)碰撞的路徑，基于路徑不變、改變速度的方式規(guī)避動(dòng)態(tài)障礙物，實(shí)施時(shí)考慮按照預(yù)設(shè)速度行駛會(huì)發(fā)生碰撞的第一個(gè)動(dòng)態(tài)障礙物，通過(guò)減速甚至停止等待的方法避讓該動(dòng)態(tài)障礙物。分別使用本文方法、HAWG方法和VO方法在多個(gè)場(chǎng)景中進(jìn)行實(shí)驗(yàn)，感知范圍為20 m，運(yùn)動(dòng)過(guò)程中按照10 Hz 進(jìn)行運(yùn)動(dòng)規(guī)劃，在仿真行駛中暫未考慮控制誤差和感知的不確定性。圖9 中運(yùn)動(dòng)規(guī)劃的起點(diǎn)為100 ms后智能車(chē)預(yù)期所在位置Ls，并非其當(dāng)前位置Le，以此達(dá)到連續(xù)行駛時(shí)軌跡間的時(shí)間連續(xù)性。但在接近目標(biāo)點(diǎn)時(shí)將不斷校驗(yàn)軌跡是否可行，可行的軌跡將直接返回。

圖9 重規(guī)劃起點(diǎn)選擇方式Fig.9 Start point selected method of replanning

圖10 所示仿真環(huán)境中，下側(cè)方框?yàn)檫\(yùn)動(dòng)規(guī)劃起點(diǎn)，上側(cè)方框?yàn)榻K點(diǎn)，起點(diǎn)和終點(diǎn)的車(chē)速均為0，箭頭方向?yàn)閯?dòng)態(tài)障礙物的運(yùn)動(dòng)方向，動(dòng)態(tài)障礙物的運(yùn)動(dòng)暫未考慮相互干涉和碰撞。本文方法在實(shí)驗(yàn)中設(shè)置的關(guān)鍵參數(shù)如下：位置精度為0.2 m，朝向精度為5°，加速度為1 m·s-1，時(shí)間間隔為0.2 s。本文方法和HAWG 方法的仿真平臺(tái)長(zhǎng)寬分別設(shè)置為4.24 m和1.84 m，碰撞檢測(cè)時(shí)擬合為3個(gè)圓，各方法均設(shè)定最高車(chē)速為6 m·s-1。為便于在各場(chǎng)景內(nèi)使用VO 方法，將所有靜態(tài)和動(dòng)態(tài)障礙物設(shè)置為圓，本文方法和HAWG方法按照前文方法將圓投影和膨脹后進(jìn)行碰撞檢測(cè)。3種典型仿真場(chǎng)景中障礙物類(lèi)型分別設(shè)置為僅靜態(tài)障礙物（場(chǎng)景1）、僅動(dòng)態(tài)障礙物（場(chǎng)景2）和動(dòng)靜態(tài)混合（場(chǎng)景3）。靜態(tài)場(chǎng)景為模擬S形障礙；動(dòng)態(tài)場(chǎng)景是從數(shù)據(jù)集ETH中抽取得到的行人運(yùn)動(dòng)場(chǎng)景[10]，并將行人運(yùn)動(dòng)數(shù)據(jù)映射到仿真地圖中；動(dòng)靜態(tài)混合場(chǎng)景中包含多個(gè)相向和同向行駛的動(dòng)態(tài)障礙物和靜態(tài)障礙物，構(gòu)建時(shí)在場(chǎng)景2的基礎(chǔ)上增加了靜態(tài)障礙物。3種場(chǎng)景中起點(diǎn)與終點(diǎn)距離分別為35.9，30.5，30.5 m，設(shè)定單次實(shí)驗(yàn)成功的條件為不發(fā)生任何形式的碰撞，且在60 s內(nèi)到達(dá)目標(biāo)位置和速度。

圖10 實(shí)驗(yàn)場(chǎng)景及行駛軌跡Fig.10 Experimental scenes and drive paths

3 種方法得到的全局行駛軌跡如圖10 中曲線所示。VO 方法在場(chǎng)景1（靜態(tài)障礙物數(shù)量為72）中無(wú)法規(guī)劃出到達(dá)終點(diǎn)的軌跡，在場(chǎng)景2（動(dòng)態(tài)障礙物數(shù)量為9）和場(chǎng)景3（靜態(tài)障礙物數(shù)量為23，動(dòng)態(tài)障礙物數(shù)量為9）中的行駛軌跡均較長(zhǎng)；本文方法和HAWG 方法能夠在所有場(chǎng)景中規(guī)劃出直達(dá)終點(diǎn)的有效軌跡。圖11 為行駛過(guò)程中某一幀規(guī)劃結(jié)果，HAWG方法不具備主動(dòng)避讓動(dòng)態(tài)障礙物的能力，屬于被動(dòng)避讓?zhuān)趫D11（a）所示的相向行駛場(chǎng)景中出現(xiàn)碰撞，在圖11（b）所示的相同場(chǎng)景中，本文方法規(guī)劃出主動(dòng)躲避動(dòng)靜態(tài)障礙物的軌跡。結(jié)果表明，本文所提的在時(shí)空柵格中進(jìn)行搜索的方法，在僅靜態(tài)環(huán)境、僅動(dòng)態(tài)環(huán)境和動(dòng)靜態(tài)混合場(chǎng)景中均能夠快速實(shí)現(xiàn)主動(dòng)躲避靜態(tài)和動(dòng)態(tài)障礙物，具備較強(qiáng)的環(huán)境適應(yīng)性，能夠安全引導(dǎo)智能車(chē)到達(dá)終點(diǎn)。

圖11 單幀規(guī)劃結(jié)果Fig.11 Single frame planning results

根據(jù)圖12（a）行駛總耗時(shí)的統(tǒng)計(jì)結(jié)果可知：HAWG 方法在靜態(tài)場(chǎng)景中的行駛耗時(shí)比本文方法短1.4 s，這是由于本文方法在生成運(yùn)動(dòng)基元的過(guò)程中，考慮了車(chē)輛速度和轉(zhuǎn)彎半徑的相互限制；在存在動(dòng)態(tài)障礙物的場(chǎng)景2 和場(chǎng)景3 中，HAWG 方法行駛時(shí)間遠(yuǎn)高于本文方法，且在圖11（a）所示的兩類(lèi)場(chǎng)景中均發(fā)生碰撞，認(rèn)定為行駛失敗。VO 方法在有可行解時(shí)（場(chǎng)景2 和場(chǎng)景3）行駛時(shí)間與本文方法相似，但在包含大量靜態(tài)障礙物的環(huán)境中處于無(wú)解狀態(tài)（場(chǎng)景1）。圖12（b）為本文方法在圖11（b）場(chǎng)景3的行駛速度曲線，該曲線在第8 s 規(guī)劃出“停止-等待”的縱向速度信息，表明本文橫縱向耦合避讓的方式能夠自動(dòng)得到“停止-等待”的縱向決策，無(wú)需單獨(dú)進(jìn)行離散型決策。以上場(chǎng)景內(nèi)的仿真實(shí)驗(yàn)結(jié)果驗(yàn)證了本文方法在動(dòng)靜態(tài)混合場(chǎng)景中的有效性。

圖12 實(shí)驗(yàn)結(jié)果Fig.12 Experimental results

3.2 定量分析

為進(jìn)一步驗(yàn)證環(huán)境的密集程度和感知范圍對(duì)各方法的影響，本文在不同靜態(tài)障礙物數(shù)量、動(dòng)態(tài)障礙物數(shù)量和感知范圍下，統(tǒng)計(jì)各方法的成功率和行駛總耗時(shí)。VO方法因不考慮加速和轉(zhuǎn)向等運(yùn)動(dòng)學(xué)約束，故不考慮其行駛總耗時(shí)。默認(rèn)實(shí)驗(yàn)環(huán)境參數(shù)為：10 個(gè)靜態(tài)障礙物，10 個(gè)動(dòng)態(tài)障礙物，感知范圍為20 m，運(yùn)動(dòng)規(guī)劃起點(diǎn)和終點(diǎn)分別位于相距50 m 的左右邊界處。靜態(tài)障礙物和動(dòng)態(tài)障礙物的位置和運(yùn)動(dòng)方向在50 m×50 m 范圍內(nèi)隨機(jī)生成，障礙物半徑在1～2 m，速度在1～2 m·s-1之間隨機(jī)產(chǎn)生。其中兩組實(shí)驗(yàn)場(chǎng)景如圖13 所示，分別為設(shè)置感知范圍為40 m、靜態(tài)障礙物數(shù)量為40 個(gè)時(shí)隨機(jī)產(chǎn)生的場(chǎng)景。在每組實(shí)驗(yàn)中僅有一個(gè)參數(shù)被改變，每組參數(shù)下各進(jìn)行50 次實(shí)驗(yàn)，各組參數(shù)下的實(shí)驗(yàn)結(jié)果如圖14所示。

圖13 隨機(jī)場(chǎng)景Fig.13 Random scenes

圖14 實(shí)驗(yàn)結(jié)果Fig.14 Experimental results

從圖14（a）可以看出：靜態(tài)障礙物數(shù)量增加對(duì)VO方法的行駛成功率影響最大，在40個(gè)靜態(tài)障礙物時(shí)成功率下降至38%；HAWG方法由于不能夠主動(dòng)避讓動(dòng)態(tài)障礙物，隨著靜態(tài)障礙物的增加，其行駛耗時(shí)的標(biāo)準(zhǔn)偏差較大，穩(wěn)定性較本文方法差；本文方法的行駛成功率受靜態(tài)障礙物稀疏程度影響較小，行駛耗時(shí)比HAWG 方法略短。隨著動(dòng)態(tài)障礙物數(shù)量的增加，如圖14（b）所示，本文方法、HAWG 方法和VO 方法成功率分別降低至42%、30%和45%，說(shuō)明各方法對(duì)存在密集動(dòng)態(tài)障礙物環(huán)境的適應(yīng)能力均有所欠缺，其中HAWG 方法行駛成功率下降最為明顯，行駛耗時(shí)增長(zhǎng)最快；在動(dòng)態(tài)障礙物數(shù)量增加到40 時(shí)，VO 方法成功率成為最高，說(shuō)明該方法對(duì)密集動(dòng)態(tài)場(chǎng)景具備一定的適應(yīng)性；本文方法在障礙物數(shù)量達(dá)到40 之前的成功率均高于其他方法，行駛耗時(shí)增幅遠(yuǎn)小于HAWG 方法。如圖14（c）所示：在默認(rèn)環(huán)境中隨著感知范圍的增加，本文方法具備最高的成功率；各方法成功率和行駛耗時(shí)受影響不大，這是由于各方法都具備較高的實(shí)時(shí)性，在較為稀疏的環(huán)境中，不易陷入“死胡同”等局部最優(yōu)場(chǎng)景。

圖14 和表1 所示的平均成功率表明：VO 方法在密集靜態(tài)場(chǎng)景內(nèi)（40 個(gè)靜態(tài)障礙物）適應(yīng)能力較差，成功率僅有38%，平均成功率為各方法最低（58.5%）；HAWG方法在密集動(dòng)態(tài)場(chǎng)景（40個(gè)動(dòng)態(tài)障礙物）內(nèi)的成功率僅有30%；本文方法在各類(lèi)環(huán)境中行駛成功率均高于其他方法，在所有環(huán)境中的平均成功率為82.8%，比HAWG 方法和VO 方法分別高出19%和23.4%，在稀疏的默認(rèn)環(huán)境內(nèi)平均成功率達(dá)到最高的91%。

表1 平均成功率Table 1 Average success rate（%）

圖14和表2所示的行駛耗時(shí)表明，本文方法與HAWG方法相比，在不同動(dòng)態(tài)障礙物數(shù)量的實(shí)驗(yàn)中平均耗時(shí)減少30%，在動(dòng)態(tài)障礙物數(shù)量增加到40時(shí)，行駛耗時(shí)減少41%，在所有場(chǎng)景內(nèi)的平均行駛耗時(shí)縮短21%，表明本文方法在考慮車(chē)輛運(yùn)動(dòng)學(xué)約束下的行駛效率高于HAWG方法。

表2 平均行駛耗時(shí)Table 2 Average time cost（s）

4 結(jié)論

本文所提基于時(shí)空柵格的運(yùn)動(dòng)規(guī)劃方法能夠融合離散的橫縱向決策信息，簡(jiǎn)化智能車(chē)決策層的數(shù)量，提高智能車(chē)在非結(jié)構(gòu)化動(dòng)態(tài)環(huán)境中的適應(yīng)性；面對(duì)不同稀疏程度的環(huán)境和不同的感知范圍都具備一定的適應(yīng)性，平均成功率達(dá)到82.8%；500次隨機(jī)場(chǎng)景中的實(shí)驗(yàn)結(jié)果表明，在行駛總耗時(shí)和平均成功率方面均有所提升。