面向自動(dòng)駕駛的動(dòng)態(tài)路徑規(guī)劃避障算法

周慧子，胡學(xué)敏，陳 龍，田 梅，熊 豆

(1.湖北大學(xué) 計(jì)算機(jī)與信息工程學(xué)院，武漢 430062; 2.中山大學(xué) 數(shù)據(jù)科學(xué)與計(jì)算機(jī)學(xué)院，廣州 510006) (*通信作者電子郵箱huxuemin2003@163.com)

面向自動(dòng)駕駛的動(dòng)態(tài)路徑規(guī)劃避障算法

周慧子1，胡學(xué)敏1*，陳 龍2，田 梅1，熊 豆1

(1.湖北大學(xué) 計(jì)算機(jī)與信息工程學(xué)院，武漢 430062; 2.中山大學(xué) 數(shù)據(jù)科學(xué)與計(jì)算機(jī)學(xué)院，廣州 510006) (*通信作者電子郵箱huxuemin2003@163.com)

針對(duì)自動(dòng)駕駛中避障的動(dòng)態(tài)路徑規(guī)劃問(wèn)題，提出一種在已知車(chē)輛的初始位置、速度、方向和障礙物位置情況下，實(shí)時(shí)避開(kāi)障礙物的動(dòng)態(tài)規(guī)劃算法。首先,利用三次樣條曲線(xiàn)的二階連續(xù)性，結(jié)合已知的車(chē)道信息產(chǎn)生道路基準(zhǔn)線(xiàn)；其次，以車(chē)輛的位置方向和道路的曲率構(gòu)建s-q坐標(biāo)系，并在s-q坐標(biāo)系內(nèi)產(chǎn)生從車(chē)輛當(dāng)前位置到目的位置的一簇平滑曲線(xiàn)，作為候選路徑；最后，綜合考慮車(chē)輛行駛的安全性、平滑性和連貫性準(zhǔn)則，設(shè)計(jì)一種新的代價(jià)函數(shù)，并且通過(guò)使代價(jià)函數(shù)最小化的方法從候選路徑中選擇最佳路徑。在實(shí)驗(yàn)過(guò)程中，通過(guò)設(shè)計(jì)多種不同的模擬道路來(lái)檢驗(yàn)算法的性能。實(shí)驗(yàn)結(jié)果表明，該方法在多種地形的單車(chē)道和多車(chē)道道路上都能夠規(guī)劃出安全、平滑的路徑，有效避開(kāi)障礙物，并且具有較好的實(shí)時(shí)性。

自動(dòng)駕駛；動(dòng)態(tài)路徑規(guī)劃；候選路徑；路徑選擇；代價(jià)函數(shù)

0 引言

自動(dòng)駕駛技術(shù)作為當(dāng)今智能交通科技的重要發(fā)展方向，可以有效地減少因酒駕、疲勞駕駛、超速等人為操作不當(dāng)造成的交通事故，還可以改善交通堵塞、提高交通系統(tǒng)總性能[1-3]。動(dòng)態(tài)路徑規(guī)劃是自動(dòng)駕駛汽車(chē)信息感知和智能控制的橋梁，也是實(shí)現(xiàn)自動(dòng)駕駛的基礎(chǔ)[4]。動(dòng)態(tài)路徑規(guī)劃是按照某一性能指標(biāo)在其行駛區(qū)域內(nèi)搜索一條從起點(diǎn)到終點(diǎn)的無(wú)碰撞的最優(yōu)或近似最優(yōu)路徑，其本質(zhì)是一個(gè)具有約束的復(fù)雜系統(tǒng)優(yōu)化問(wèn)題，在多智能體集群、避障和目標(biāo)跟蹤控制中都會(huì)涉及到，是一個(gè)關(guān)鍵的基礎(chǔ)共性問(wèn)題[5-7]，因此，動(dòng)態(tài)路徑規(guī)劃和避障對(duì)于設(shè)計(jì)合理的駕駛路徑具有重要意義，不僅規(guī)劃結(jié)果的優(yōu)劣影響著自動(dòng)駕駛汽車(chē)的準(zhǔn)確性和安全性，規(guī)劃算法的復(fù)雜度、穩(wěn)定性也間接影響著汽車(chē)的工作效率。

動(dòng)態(tài)路徑規(guī)劃技術(shù)主要分為兩大類(lèi)：第一類(lèi)是全局路徑規(guī)劃[6]，它是根據(jù)先驗(yàn)環(huán)境模型找出從起點(diǎn)到終點(diǎn)中符合條件的最優(yōu)或次優(yōu)路徑，涉及的根本問(wèn)題是環(huán)境模型的表達(dá)和路徑搜尋策略；第二類(lèi)是局部路徑規(guī)劃[8-9]，是指在未知或部分未知的環(huán)境下通過(guò)傳感器獲取周?chē)h(huán)境信息，并使自動(dòng)駕駛汽車(chē)自主獲得一條無(wú)碰撞最優(yōu)規(guī)劃的路徑，它側(cè)重于考慮車(chē)輛當(dāng)前局部環(huán)境信息。本文研究的是局部路徑規(guī)劃問(wèn)題。

目前，局部路徑規(guī)劃方法主要有：人工勢(shì)場(chǎng)法、啟發(fā)式搜索算法和基于離散優(yōu)化的方法。人工勢(shì)場(chǎng)法是將車(chē)輛在周?chē)h(huán)境中的運(yùn)動(dòng)設(shè)計(jì)成一種抽象的人造引力場(chǎng)中的運(yùn)動(dòng)，目標(biāo)點(diǎn)對(duì)車(chē)輛產(chǎn)生“引力”，障礙物對(duì)車(chē)輛產(chǎn)生“斥力”，最后通過(guò)求合力來(lái)控制車(chē)輛的運(yùn)動(dòng)[10]。該方法在數(shù)學(xué)描述上簡(jiǎn)潔、結(jié)構(gòu)簡(jiǎn)單、計(jì)算量小，但是容易產(chǎn)生局部最優(yōu)解。啟發(fā)式搜索算法主要是A*算法和D*算法[11-12]。A*算法是一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法，需要建立環(huán)境模型地圖，地圖本身充當(dāng)了人與車(chē)輛互相交流的媒介，這使得操作方便可靠，但是計(jì)算量大、耗時(shí)長(zhǎng)；D*算法是對(duì)A*算法的擴(kuò)充，適合動(dòng)態(tài)環(huán)境下的路徑規(guī)劃，在動(dòng)態(tài)環(huán)境中尋路非常有效，但是它比較復(fù)雜，應(yīng)用范圍有限。基于離散優(yōu)化的路徑規(guī)劃方法是用數(shù)值積分和微分等方程來(lái)描述車(chē)輛的運(yùn)動(dòng)，從而產(chǎn)生數(shù)量有限的候選路徑，并通過(guò)設(shè)計(jì)代價(jià)函數(shù)，從候選路徑中選擇最優(yōu)路徑[13-14]。該方法計(jì)算量小，實(shí)時(shí)性好。文獻(xiàn)[14]提出了一種基于此方法的自動(dòng)駕駛車(chē)輛路徑規(guī)劃避障算法，可以為車(chē)輛提供最優(yōu)路徑，但只針對(duì)單車(chē)道公路或鄉(xiāng)間小路，未考慮城市中的多車(chē)道情況。

針對(duì)現(xiàn)有局部路徑規(guī)劃算法中的容易陷入局部最優(yōu)、計(jì)算量大、未考慮多車(chē)道等問(wèn)題，本文基于離散優(yōu)化方法，提出了一種新的面向自動(dòng)駕駛的動(dòng)態(tài)路徑規(guī)劃避障算法。該算法能夠在不進(jìn)行迭代的前提下產(chǎn)生多條候選路徑，并且綜合考慮駕駛的安全性、平滑性和連貫性來(lái)設(shè)計(jì)代價(jià)函數(shù)，選擇一條最優(yōu)路徑。實(shí)驗(yàn)結(jié)果表明，本文提出的算法不管是對(duì)于單車(chē)道還是多車(chē)道道路，都能夠產(chǎn)生一條最優(yōu)的路徑，使規(guī)劃車(chē)輛能夠安全、舒適地繞過(guò)障礙物，從起點(diǎn)到達(dá)終點(diǎn)，完成路徑的規(guī)劃。

1 動(dòng)態(tài)路徑規(guī)劃避障算法

本文提出的動(dòng)態(tài)路徑規(guī)劃方法是在已知車(chē)輛初始狀態(tài)信息(包括車(chē)輛的位置、車(chē)頭方向等)的基礎(chǔ)上，產(chǎn)生一條安全又舒適的行駛路線(xiàn)。本文是在已知全局路線(xiàn)的情況下進(jìn)行局部規(guī)劃，全局路線(xiàn)通過(guò)車(chē)道級(jí)的高精度導(dǎo)航系統(tǒng)獲取。本文中，全局路線(xiàn)由一組道路邊緣的有序點(diǎn)組成。如圖1所示，本文的方法包括三個(gè)部分：基準(zhǔn)線(xiàn)生成、候選路徑生成和最優(yōu)路徑選擇。

圖1 動(dòng)態(tài)路徑規(guī)劃算法流程

1.1 基準(zhǔn)線(xiàn)的生成

路徑規(guī)劃算法中常用參數(shù)曲線(xiàn)來(lái)建立道路的集合模型。由于全局路線(xiàn)是一組道路邊緣的有序點(diǎn)，并且考慮到計(jì)算曲率時(shí)曲線(xiàn)二階導(dǎo)數(shù)的連續(xù)性，本文采用三次樣條曲線(xiàn)來(lái)擬合道路基準(zhǔn)線(xiàn)。弧長(zhǎng)是最常用的曲線(xiàn)參數(shù)，因此采用正交數(shù)值法[15]對(duì)由道路點(diǎn)擬合成的參數(shù)樣條曲線(xiàn)弧長(zhǎng)作參數(shù)化計(jì)算，如式(1)所示：

(1)

如圖2(a)所示，s為車(chē)輛當(dāng)前位置映射在基準(zhǔn)線(xiàn)上的弧長(zhǎng)，該弧長(zhǎng)計(jì)算的起點(diǎn)為第i個(gè)道路片段的起點(diǎn)si。(xbf,ybf)是曲線(xiàn)上的點(diǎn)在笛卡爾坐標(biāo)系中的坐標(biāo)。ax,i、bx,i、cx,i、dx,i、ay,i、by,i、cy,i、dy,i是參數(shù)樣條曲線(xiàn)的參數(shù)。假設(shè)車(chē)輛當(dāng)前位置到基準(zhǔn)線(xiàn)上最近的點(diǎn)的距離，即橫向偏移量為q，則車(chē)輛當(dāng)前坐標(biāo)可以用車(chē)輛行駛的弧長(zhǎng)s和橫向偏移量q來(lái)表示。本文將s和q表示車(chē)輛位置的坐標(biāo)稱(chēng)為“s-q坐標(biāo)”。在s-q坐標(biāo)中，基準(zhǔn)線(xiàn)上每個(gè)點(diǎn)的切向角θbf和曲率κbf可以用xbf(s)和ybf(s)對(duì)s的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)計(jì)算求得，如式(2)所示[14]：

(2)

其中:xbf′、ybf′、xbf″和ybf″分別為xbf(s)和ybf(s)對(duì)s的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)。

圖2 s-q坐標(biāo)系與候選路徑示意圖

Fig. 2 Schematic diagram ofs-qcoordinate system and candidate path

1.2 候選路徑的產(chǎn)生

為使用基準(zhǔn)線(xiàn)上點(diǎn)的方向角和曲率，有必要找到車(chē)輛在基準(zhǔn)線(xiàn)上的位置，如圖2(a)所示的p0點(diǎn)。本文利用牛頓拉夫森二次極小化方法找到曲線(xiàn)上最接近車(chē)輛位置的點(diǎn)坐標(biāo)[16]。

如圖2(b)所示，車(chē)輛在繞過(guò)障礙物時(shí)，是否與障礙物碰撞可以用偏移量q來(lái)表示。由于車(chē)輛行駛的距離是用弧長(zhǎng)s表示，因此車(chē)輛繞過(guò)障礙物的候選路徑可以用橫向偏移量q和弧長(zhǎng)s來(lái)表示。在s-q坐標(biāo)系中，假設(shè)候選路徑也滿(mǎn)足三次樣條曲線(xiàn)方程，則候選路徑可以用式(3)來(lái)表示：

(3)

其中:qstart、qend、sstart和send分別代表本次規(guī)劃中候選路徑起點(diǎn)的橫向偏移量、終點(diǎn)的橫向偏移量、起點(diǎn)對(duì)應(yīng)的弧長(zhǎng)和終點(diǎn)對(duì)應(yīng)的弧長(zhǎng)。為了求解式(3)中的系數(shù)a、b、c，本文采用文獻(xiàn)[14]中的邊界條件的方法進(jìn)行計(jì)算。如圖2(b)所示，不同的候選路徑，對(duì)應(yīng)的qend不同，因此，為了獲取多條候選路徑，本文設(shè)置N個(gè)不同的qend值，分別計(jì)算得到N組a、b、c的系數(shù)，以此構(gòu)造出N個(gè)式(3)的方程式。由于一個(gè)式(3)的方程式表示一條候選路徑，則通過(guò)不同qend值可以計(jì)算出多條候選路徑。因?yàn)楹蜻x路徑是在s-q坐標(biāo)系中計(jì)算得到的，而道路和障礙物信息一般都是基于笛卡爾坐標(biāo)，因此本文采用文獻(xiàn)[14]和[17]描述的坐標(biāo)轉(zhuǎn)換方法，并結(jié)合四階龍格庫(kù)塔法求解微分方程[18]，實(shí)現(xiàn)將候選路徑上的點(diǎn)從s-q坐標(biāo)系到笛卡爾坐標(biāo)系的轉(zhuǎn)換。

1.3 最優(yōu)路徑的選擇

在獲取多條候選路徑之后，需要從多條路徑中選出一條最優(yōu)路徑，使規(guī)劃車(chē)輛能夠安全、平滑地繞過(guò)障礙物。本文提出一種新的代價(jià)函數(shù)法，通過(guò)代價(jià)函數(shù)的最小化來(lái)實(shí)現(xiàn)最優(yōu)路徑的選擇。考慮到駕駛時(shí)的安全性和舒服性，本文從安全、平滑和連續(xù)這三個(gè)方面來(lái)設(shè)計(jì)代價(jià)函數(shù)，因此，本文提出的代價(jià)函數(shù)f(i)包含三個(gè)部分，如式(4)所示：

f(i)=wsaffsaf(i)+wsmofsmo(i)+wcohfcoh(i)

(4)

其中：i是候選路徑標(biāo)號(hào),fsaf、fsmo和fcoh分別是安全性代價(jià)函數(shù)、平滑性代價(jià)函數(shù)和連貫性代價(jià)函數(shù)；wsaf、wsmo、wcoh分別是這三個(gè)代價(jià)函數(shù)所占的權(quán)重，這三個(gè)權(quán)重決定車(chē)輛的駕駛風(fēng)格。本文中這3個(gè)權(quán)值依據(jù)經(jīng)驗(yàn)分別取0.6、0.2和0.2。

1.3.1 路徑安全性代價(jià)函數(shù)

安全性是自動(dòng)駕駛的最重要的因素。障礙物、車(chē)道線(xiàn)、道路邊緣是安全性的潛在影響因素。由于任何平面幾何形狀都能被其外接圓包圍，因此本文用外接圓作為描述障礙物的數(shù)學(xué)模型。

為了避開(kāi)障礙物和道路邊緣，必須找到并舍棄與障礙物或車(chē)道邊緣交叉的候選路徑。本文將圓心到候選路徑的最小距離與圓半徑進(jìn)行比較來(lái)確定該候選路徑是否與障礙物碰撞。圖3(a)給出了一種在單車(chē)道上碰撞檢測(cè)的示例圖，其碰撞檢測(cè)結(jié)果用R來(lái)表示，候選路徑用ri表示，其中i為序號(hào)。如果路徑跨越障礙物或車(chē)道邊緣，R=1；否則R=0。

圖3 碰撞檢測(cè)結(jié)果示意圖

在多車(chē)道的碰撞檢測(cè)中，本文提出了一種加權(quán)碰撞檢測(cè)的方法，如圖3(b)所示。如果某候選路徑穿過(guò)障礙物或道路邊緣，則R=1；越過(guò)對(duì)向車(chē)道線(xiàn)，則R=0.5；穿過(guò)同向車(chē)道線(xiàn)，則R=0.2；在本車(chē)道上不穿越任何障礙物，則R=0；如果某候選路徑穿過(guò)多個(gè)種類(lèi)的車(chē)道、道路邊緣或障礙物，則R為它所涉及到的碰撞檢測(cè)中的最高值。

如果只考慮碰撞檢測(cè)，圖3(a)中r6和圖3(b)中r5都是非碰撞路徑，然而這兩條候選路徑離障礙物的距離太近，如果選擇這兩條路徑，駕駛時(shí)稍有不慎車(chē)輛就會(huì)與障礙物相撞。所以只依靠碰撞檢測(cè)來(lái)選擇最優(yōu)路徑無(wú)法保證行車(chē)的安全性。候選路徑和障礙物之間的距離是評(píng)估一條路徑是否安全的有效方法，但是當(dāng)障礙物或者候選路徑很多時(shí)計(jì)算量會(huì)很大，實(shí)時(shí)性不高。為了保證安全性和實(shí)時(shí)性，本文用離散的高斯卷積結(jié)合碰撞檢測(cè)的方法來(lái)定義每條候選路徑的碰撞風(fēng)險(xiǎn)，如式(5)所示：

(5)

其中：fsaf(i)定義為候選路徑ri的安全性代價(jià)函數(shù)；gi[k]是離散高斯函數(shù)，如式(6)所示。

(6)

其中：σ是碰撞風(fēng)險(xiǎn)標(biāo)準(zhǔn)差，決定碰撞檢測(cè)的有效范圍，本文實(shí)驗(yàn)中σ=2。

候選路徑的離散高斯卷積過(guò)程如圖4(a)、(c)所示。如果i超出候選路徑索引標(biāo)號(hào)的范圍則定義碰撞檢測(cè)結(jié)果R[i]=1。圖4(b)、(d)分別代表單車(chē)道和多車(chē)道的碰撞風(fēng)險(xiǎn)值結(jié)果。圖4(a)表明r6是一條碰撞檢測(cè)最小的路徑，但碰撞風(fēng)險(xiǎn)分布即圖4(b)卻說(shuō)明r6的風(fēng)險(xiǎn)值不是最低的，即r6不是最安全的路徑，因?yàn)樗^(guò)于接近障礙物。同理，對(duì)于多車(chē)道，路徑的碰撞風(fēng)險(xiǎn)分布如圖4(d)所示，它說(shuō)明雖然r5是碰撞檢測(cè)最小的路徑，但同時(shí)它離障礙物距離過(guò)近，最優(yōu)路徑是r8。

圖4 單車(chē)道與多車(chē)道碰撞風(fēng)險(xiǎn)示意圖

1.3.2 路徑平滑性代價(jià)函數(shù)

行駛過(guò)程中，不平滑的路徑可能會(huì)引起車(chē)輪打滑，影響駕駛的安全性和舒適性，因此平滑性也是路徑選擇中必須要考慮的一個(gè)因素。由于路徑的平滑性與路徑的曲率直接相關(guān)，所以本文利用曲率的平方在路徑上的積分作為平滑性代價(jià)函數(shù)，如式(7)所示。其中fsmo(i)代表第i條路的平滑性代價(jià)函數(shù)，Ki(s)是第i條路徑上弧長(zhǎng)為s位置的點(diǎn)的曲率，積分的上下限為該候選路徑的弧長(zhǎng)s的起點(diǎn)和終點(diǎn)。

(7)

圖5為考慮安全性和平滑性的代價(jià)函數(shù)曲線(xiàn)圖。圖5(a)是只考慮安全性的代價(jià)函數(shù)曲線(xiàn)圖，其代價(jià)最小值出現(xiàn)在i=10的地方，該路徑的彎度較大，平滑性較差，如圖5(d)中r10所示。圖5(b)是只考慮平滑性的代價(jià)函數(shù)曲線(xiàn)圖，其代價(jià)最小值出現(xiàn)在i=13的地方，該路徑較r10平滑，但是相對(duì)遠(yuǎn)離車(chē)道中心線(xiàn)，如圖5(d)中r13所示。圖5(c)為綜合考慮安全性和平滑性的代價(jià)函數(shù)曲線(xiàn)圖，其代價(jià)最小值為候選路徑r11，如圖5(d)中所示。很明顯，r11相比r10和r13，權(quán)衡了安全性和平滑性因素，更合適作為最優(yōu)路徑。

1.3.3 路徑連貫性代價(jià)函數(shù)

安全性和平滑性只考慮了本次規(guī)劃所涉及到的信息，但是沒(méi)有考慮多次規(guī)劃的連續(xù)性問(wèn)題，無(wú)法保證本次規(guī)劃的路徑與上次規(guī)劃的路徑?jīng)]有出現(xiàn)突變。如果本次規(guī)劃路徑與上次規(guī)劃路徑的距離太遠(yuǎn)，則會(huì)出現(xiàn)路徑的突然轉(zhuǎn)向，不僅影響駕駛的舒適度，嚴(yán)重情況下甚至?xí)管?chē)身出現(xiàn)較大的側(cè)傾，存在安全隱患。為了解決這個(gè)問(wèn)題，必須考慮上次選擇的最優(yōu)路徑對(duì)本次路徑選擇的影響，因此，本文利用當(dāng)前候選路徑與上次選擇路徑之間的距離的積分來(lái)計(jì)算路徑的連貫性代價(jià)函數(shù)，如式(8)所示：

(8)

其中：s1和s2分別是當(dāng)前候選路徑與上次選擇路徑在基準(zhǔn)線(xiàn)重疊部分的起點(diǎn)和終點(diǎn)；di是本次規(guī)劃的第i條候選路徑上的點(diǎn)到上次選擇路徑上相同弧長(zhǎng)的點(diǎn)的歐氏距離[19]。

圖6為連貫性代價(jià)函數(shù)對(duì)路徑選擇的影響。圖6(a)和圖6(d)中r6顯示了不考慮連貫性的代價(jià)函數(shù)和路徑選擇結(jié)果，其選擇的路徑偏離了上次規(guī)劃的最優(yōu)路徑rb；圖6(b)是僅考慮連貫性的代價(jià)函數(shù)；圖6(c)為綜合考慮安全性、平滑性和連貫性的代價(jià)函數(shù)，其選擇結(jié)果如圖6(d)中的r7所示。可以看出，考慮了連貫性的本次規(guī)劃的選擇路徑r7相對(duì)未考慮連貫性的選擇路徑r6更接近上次規(guī)劃的最優(yōu)路徑rb，因此考慮連貫性因素后，車(chē)輛行駛過(guò)程中沒(méi)有了路徑的突然改變，加強(qiáng)了行駛的安全性和舒適性。

圖5 考慮安全性和平滑性的代價(jià)函數(shù)和路徑選擇示意圖

圖6 考慮連貫性的代價(jià)函數(shù)和路徑選擇示意圖

2 實(shí)驗(yàn)結(jié)果和分析

為了驗(yàn)證本文算法的有效性，本文分別在單車(chē)道和多車(chē)道上進(jìn)行避障實(shí)驗(yàn)。本文實(shí)驗(yàn)分為兩部分：第一部分采用多種復(fù)雜地形的單車(chē)道來(lái)驗(yàn)證算法的性能；第二部分通過(guò)模擬與單車(chē)道相同路段的多車(chē)道，對(duì)比測(cè)試算法在多車(chē)道上的性能。由于實(shí)際復(fù)雜地形場(chǎng)景地圖較難獲取，因此本文通過(guò)Matlab軟件仿真進(jìn)行實(shí)驗(yàn)，其中地圖數(shù)據(jù)人工預(yù)先定義。圖7和圖8為實(shí)驗(yàn)結(jié)果，兩邊黑色實(shí)線(xiàn)、黑色長(zhǎng)虛線(xiàn)、黑色點(diǎn)虛線(xiàn)和黑色均勻曲線(xiàn)分別表示車(chē)道邊界、同方向車(chē)道線(xiàn)、基準(zhǔn)線(xiàn)以及候選路徑，路中黑色粗實(shí)線(xiàn)表示車(chē)輛行駛軌跡，空心圓表示障礙物，黑色實(shí)心圓表示車(chē)輛當(dāng)前位置。

2.1 復(fù)雜地形的單車(chē)道避障

有連續(xù)多個(gè)障礙物的直道、“S”彎道和環(huán)島是生活中常見(jiàn)且具有挑戰(zhàn)性的幾種車(chē)道類(lèi)型，即使是人類(lèi)駕駛員也不能大意，因此本文將這幾類(lèi)車(chē)道作為實(shí)驗(yàn)對(duì)象進(jìn)行測(cè)試。

2.1.1 有連續(xù)多個(gè)障礙物的單車(chē)道直道

圖7(a)(d)為本文方法在有連續(xù)多個(gè)障礙物的直道上測(cè)試的結(jié)果：圖7(a)為車(chē)輛在繞過(guò)第一個(gè)障礙物的時(shí)刻，從圖7(a)中可以看出，車(chē)輛選擇了障礙物上邊的一條安全且又平滑的路徑；圖7(d)為車(chē)輛通過(guò)整段模擬道路的軌跡。從軌跡中可以看出，本文算法能夠規(guī)劃出一條有效的路徑，安全地繞過(guò)連續(xù)多個(gè)障礙物并回到基準(zhǔn)線(xiàn)上，而且路徑平滑連續(xù)，車(chē)輛不需要急轉(zhuǎn)向，滿(mǎn)足駕駛安全和舒適的要求。

2.1.2 有多個(gè)障礙物的單車(chē)道“S”彎道

圖7(b)(e)為本文方法在“S”彎道路段的測(cè)試結(jié)果：其中:圖7(b)為規(guī)劃車(chē)輛在避開(kāi)第一個(gè)彎道上兩個(gè)連續(xù)障礙物的時(shí)刻，車(chē)輛選擇了一條居中的較平滑的路徑;圖7(e)為通過(guò)包含一個(gè)障礙物的第二個(gè)彎道的時(shí)刻。障礙物在道路正中間，車(chē)輛可以選擇從障礙物兩側(cè)通過(guò)，但是由于平滑性的限制，選擇下方的路徑需要車(chē)輛更多的轉(zhuǎn)向控制，因此本文方法選擇了靠上的路徑作為最優(yōu)路徑，此路徑安全且平滑。

2.1.3 有多個(gè)障礙物的單車(chē)道環(huán)島路

圖7(c)(f)是車(chē)輛在單車(chē)道環(huán)島路上行駛的實(shí)驗(yàn)結(jié)果。圖7(c)表明，當(dāng)車(chē)輛開(kāi)始進(jìn)入環(huán)狀路時(shí)(此時(shí)無(wú)障礙物)，它沒(méi)有選擇車(chē)道基準(zhǔn)線(xiàn)而是選擇距基準(zhǔn)線(xiàn)不遠(yuǎn)的偏環(huán)狀路內(nèi)側(cè)的路徑作為最優(yōu)路徑，這是因?yàn)榭績(jī)?nèi)側(cè)的路徑相比基準(zhǔn)線(xiàn)更平滑且足夠安全供車(chē)輛行駛；圖7(f)顯示了車(chē)輛在環(huán)狀路內(nèi)避開(kāi)兩個(gè)連續(xù)障礙物的情況，如圖所示，雖然兩個(gè)障礙物相距很近，但是本文的算法可以產(chǎn)生一條平滑且安全的路線(xiàn)使車(chē)輛能成功繞過(guò)障礙物。

2.2 復(fù)雜地形的多車(chē)道避障

鄉(xiāng)村和郊區(qū)的道路以單車(chē)道道路為主，但是城市里和高速上道路主要是多車(chē)道道路，因此，本文采用具有與單車(chē)道相同路況(障礙物位置相同)的多車(chē)道(兩條同向車(chē)道和兩條對(duì)向車(chē)道)來(lái)驗(yàn)證本文方法的有效性。

2.2.1 有連續(xù)多個(gè)障礙物的多車(chē)道直道

圖8(a)(d)為在有多個(gè)連續(xù)障礙物的多車(chē)道直道上的規(guī)劃結(jié)果。從圖8(a)可以看出，當(dāng)車(chē)輛遇到障礙物時(shí)，基于多車(chē)道安全性的代價(jià)函數(shù)規(guī)則，會(huì)優(yōu)先選擇當(dāng)前車(chē)道內(nèi)的最優(yōu)路徑，但是當(dāng)前車(chē)道有危險(xiǎn)時(shí)，算法優(yōu)先選擇了同向車(chē)道的一條最優(yōu)路徑，這與圖7(a)選擇的最優(yōu)路徑有區(qū)別，這是因?yàn)閼?yīng)用于多車(chē)道的碰撞檢測(cè)規(guī)則不同；圖8(d)為車(chē)輛繞過(guò)最后障礙物時(shí)刻結(jié)果圖，由于障礙物占據(jù)了同向兩條車(chē)道，所以算法選擇一條跨越對(duì)向車(chē)道路徑作為最優(yōu)路徑。當(dāng)遇到障礙物時(shí)，車(chē)輛能夠成功避開(kāi)障礙物。基于平滑性規(guī)則，車(chē)輛會(huì)逐漸回到原車(chē)道中，沒(méi)有急轉(zhuǎn)向的突變軌跡。

2.2.2 有多個(gè)障礙物的多車(chē)道“S”彎道

如圖8(b)(e)為多車(chē)道“S”彎道的測(cè)試結(jié)果，其中圖8(b)與圖7(b)類(lèi)似，車(chē)輛為避開(kāi)兩個(gè)相距很近的障礙物選擇了一條居中的平滑路徑。圖8(e)與圖7(e)顯示了兩種不同的選擇結(jié)果，雖然圖8(e)中障礙物上邊的候選路徑有較小的平滑性代價(jià)函數(shù)，但是如果從上邊避過(guò)障礙物，車(chē)輛將越過(guò)對(duì)向車(chē)道線(xiàn)。由于對(duì)向車(chē)道線(xiàn)的代價(jià)比同向車(chē)道線(xiàn)高，此時(shí)總的代價(jià)函數(shù)是由安全性代價(jià)函數(shù)主導(dǎo)，所以最終選擇了障礙物下邊的一條安全且平滑的路徑。雖然多車(chē)道與單車(chē)道規(guī)劃結(jié)果不同，但是本文的算法都能根據(jù)實(shí)際情況很好地應(yīng)用在各種不同車(chē)道上。

2.2.3 有多個(gè)障礙物的多車(chē)道環(huán)島路

圖8(c)(f)是本文算法在多車(chē)道環(huán)島路上的實(shí)驗(yàn)結(jié)果。其中圖8(c)結(jié)果與7(c)類(lèi)似，當(dāng)車(chē)輛開(kāi)始進(jìn)入環(huán)島路時(shí)，它選擇了偏環(huán)島路內(nèi)側(cè)更平滑的、且屬于當(dāng)前車(chē)道的路徑作為最優(yōu)路徑；圖8(f)為車(chē)輛在多車(chē)道環(huán)狀路內(nèi)避開(kāi)兩個(gè)連續(xù)障礙物的時(shí)刻，可以看出，此次規(guī)劃與圖7(f)不同，這是因?yàn)樵诙鄺l候選路徑中，考慮到不同車(chē)道線(xiàn)的代價(jià)因素，算法優(yōu)先選擇了同向安全的車(chē)道。

由以上對(duì)比實(shí)驗(yàn)結(jié)果可見(jiàn)，本文方法不管是對(duì)于鄉(xiāng)村中單車(chē)道道路還是城市中多車(chē)道道路，以及各種復(fù)雜地形的道路，都能規(guī)劃出一條安全、平滑的路徑，有效地避開(kāi)障礙物。

圖7 復(fù)雜地形的單車(chē)道避障規(guī)劃結(jié)果

圖8 復(fù)雜地形的多車(chē)道避障規(guī)劃結(jié)果

2.3 運(yùn)行時(shí)間

本文實(shí)驗(yàn)硬件平臺(tái)為IntelCorei5- 450MCPU(雙核，2.4GHz)，4GB內(nèi)存；規(guī)劃路徑的產(chǎn)生基于VisualStudio2010平臺(tái)，道路信息和車(chē)輛行駛控制通過(guò)Matlab2009b模擬仿真進(jìn)行。由實(shí)驗(yàn)可知，本文算法在基準(zhǔn)線(xiàn)產(chǎn)生和路徑選擇的平均時(shí)間分別為3ms和4ms;而路徑產(chǎn)生階段由于要產(chǎn)生多條候選路徑，因此耗時(shí)較長(zhǎng)，平均時(shí)間47ms。所以每次規(guī)劃總的平均時(shí)間為54ms，即相當(dāng)于1s內(nèi)能進(jìn)行約20次規(guī)劃。由于本文設(shè)定車(chē)輛每行駛1m重新規(guī)劃一次，因此本文方法能滿(mǎn)足約20m/s(72km/h)的車(chē)輛行駛速度。鑒于車(chē)輛在避障時(shí)的速度一般低于50km/h，因此本文的方法完全能滿(mǎn)足動(dòng)態(tài)路徑規(guī)劃的實(shí)時(shí)性要求。

3 結(jié)語(yǔ)

本文提出了一種新的面向自動(dòng)駕駛的動(dòng)態(tài)路徑規(guī)劃的方法。該方法首先利用地圖信息產(chǎn)生道路基準(zhǔn)線(xiàn)，然后依據(jù)障礙物的位置和車(chē)輛的橫向偏移等信息，在s-q坐標(biāo)系下產(chǎn)生一簇候選路徑；在路徑選擇階段，本文綜合考慮安全性、平滑性和連續(xù)性因素，設(shè)計(jì)了一種新的代價(jià)函數(shù)，利用代價(jià)函數(shù)的最小化來(lái)選擇最優(yōu)路徑。本文在多種復(fù)雜地形道路中進(jìn)行測(cè)試，實(shí)驗(yàn)結(jié)果表明本文方法不管是對(duì)于單車(chē)道道路還是多車(chē)道道路，都能夠規(guī)劃出一條安全、平滑的路徑，引導(dǎo)規(guī)劃車(chē)輛有效、實(shí)時(shí)地避開(kāi)道路中的各個(gè)障礙物。

由于本文方法中只考慮了路徑規(guī)劃中的靜態(tài)障礙物避障問(wèn)題，并沒(méi)有涉及到動(dòng)態(tài)障礙物，因此，未來(lái)的工作將集中在動(dòng)態(tài)障礙物的避障問(wèn)題。

References)

[1] 劉小洋,伍民友.車(chē)聯(lián)網(wǎng):物聯(lián)網(wǎng)在城市交通網(wǎng)絡(luò)中的應(yīng)用[J].計(jì)算機(jī)應(yīng)用,2012,32(4):900-904. (LIU X Y, WU M Y. A vehicular CPS: an application of IoT in vehicular networks [J].Journal of Computer Applications, 2012, 32(4): 900-904.)

[2] 陳榮武,劉莉,諸昌鈐.基于CBTC的列車(chē)自動(dòng)駕駛控制算法[J].計(jì)算機(jī)應(yīng)用,2007,27(11):2649-2651. (CHEN R W, LIU L, ZHU C Q. Automatic train operation and its control algorithm based on CBTC [J]. Journal of Computer Applications, 2007, 27(11): 2649-2651.)

[3] 于建志,陳永生.磁浮列車(chē)自動(dòng)駕駛系統(tǒng)控制策略及可靠性研究[J].計(jì)算機(jī)應(yīng)用,2010,30(12):3419-3422. (YU J Z, CHEN Y S. Control strategy and reliability study of maglev automatic train operation system [J]. Journal of Computer Applications, 2010, 30(12): 3419-3422.)

[4] GARROTE L, PREMEBIDA C, SILVA M, et al. An RRT-based navigation approach for mobile robots and automated vehicles [C]// INDIN 2014: Proceedings of the 12th IEEE International Conference on Industrial Informatics. Piscataway, NJ: IEEE, 2014:326-331.

[5] 周明秀,程科,汪正霞.動(dòng)態(tài)路徑規(guī)劃中的改進(jìn)蟻群算法[J].計(jì)算機(jī)科學(xué),2013,40(1):314-316. (ZHOU M X, CHENG K, WANG Z X. Improved ant colony algorithm with planning of dynamic path [J]. Computer Science, 2013, 40(1):314-316.)

[6] 羅元,邵帥,張毅.基于信息融合的移動(dòng)機(jī)器人定位與路徑規(guī)劃[J]. 計(jì)算機(jī)應(yīng)用,2010,30(11):3091-3093. (LUO Y, SHAO S, ZHANG Y. Location and path planning of mobile robots based on data fusion [J]. Journal of Computer Applications, 2010, 30(11): 3091-3093.)

[7] ZHANG S, DENG W, ZHAO Q, et al. Dynamic trajectory planning for vehicle autonomous driving [C]// SMC ’13: Proceedings of the 2013 IEEE International Conference on Systems, Man, and Cybernetics. Washington, DC: IEEE Computer Society, 2013: 4161-4166.

[8] 王永興,丁睿,姚林泉.移動(dòng)機(jī)器人全局路徑規(guī)劃的盲人摸路算法[J].計(jì)算機(jī)仿真,2010,27(5):157-161. (WANG Y X, DING R, YAO L Q. The blind groping algorithm: global path planning for mobile robot [J]. Computer Simulation, 2010, 27(5): 157-161.)

[9] XU W, PAN J, WEI J, et al. Motion planning under uncertainty for on-road autonomous driving [C]// ICRA 2014: Proceedings of the 2014 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2014: 2507-2512.

[10] KOREN Y, BORENSTEIN J. Potential field methods and their inherent limitations for mobile robot navigation [C]// Proceedings of the 1991 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 1991, 2: 1398-1404.

[11] HART P E, NILSSON N J, RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths [J]. IEEE Transactions on Systems Science and Cybernetics, 1968, 4(2):100-107.

[12] STENTZ A. Optimal and efficient path planning for partially known environments [C]// Proceedings of the 1994 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 1994: 3310-3317.

[13] WERLING M, ZIEGLER J, KAMMEL S, et al. Optimal trajectory generation for dynamic street scenarios in a Frenét frame [C]// ICRA 2010: Proceedings of the IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2010:987-993.

[14] CHU K, LEE M, SUNWOO M. Local path planning for off-road autonomous driving with avoidance of static obstacles [J]. IEEE Transactions on Intelligent Transportation Systems, 2012, 13(4): 1599-1616.

[15] BAGIROV A M. Numerical methods for minimizing quasidifferentiable functions: a survey and comparison [M]// Quasidifferentiability and Related Topics. Berlin: Springer-Verlag, 2000: 33-71.

[16] SANDE H V, HENROTTE F, HAMEYER K. The Newton-Raphson method for solving non-linear and anisotropic time-harmonic problems [J]. COMPEL: The International Journal for Computation and Mathematics in Electrical and Electronic Engineering, 2004, 23(4): 950-958.

[18] FAMELIS I Th, TSITOURAS Ch. On modifications of Runge-Kutta-Nystr?m methods for solvingy(4)=f(x,y) [J]. Applied Mathematics and Computation, 2016, 273: 726-734.

[19] HUANG H-X, LIANG Z-A, PARDALOS P M. Some properties for the Euclidean distance matrix and positive semidefinite matrix completion problems [J]. Journal of Global Optimization, 2002, 25(1): 3-21.

This work is partially supported by the National Natural Science Foundation of China (41401525), the Natural Science Foundation of Guangdong Province (2014A030313209), the Student’s Platform for Innovation and Entrepreneurship Training Program of Hubei Province (201410512030).

ZHOU Huizi, born in 1995. Her research interests include dynamic path planning.

HU Xuemin, born in 1985, Ph. D., lecturer. His research interests include computer vision, dynamic path planning.

CHEN Long, born in 1985, Ph. D., lecturer. His research interests include stereo vision, autonomous driving.

TIAN Mei, born in 1995. Her research interests include dynamic path planning.

XIONG Dou, born in 1995. Her research interests include automatic control.

Dynamic path planning for autonomous driving with avoidance of obstacles

ZHOU Huizi1, HU Xuemin1*, CHEN Long2, TIAN Mei1, XIONG Dou1

(1.SchoolofComputerScienceandInformationEngineering,HubeiUniversity,WuhanHubei430062,China; 2.SchoolofDataandComputerScience,SunYat-senUniversity,GuangzhouGuangdong510006,China)

To deal with the problem of dynamic path planning for autonomous driving with avoidance of obstacles, a real-time dynamic path planning approach was proposed to avoid obstacles in real-time under the condition of knowing initial vehicle position, speed, orientation and the obstacle positions. Firstly, a base frame of the road was constructed using the continuity of the second derivative for cubic spline curves combined with the information of the road edges and lanes. Secondly, thes-qcoordinate system was established using the position and orientation of the vehicle and the curvature of the road. Then a set of smooth curves from the current position to the destination were generated as the path candidates in thes-qcoordinate system. Finally, considering the factors of safety, smoothness and continuity, a novel cost function was designed, and the optimal path was selected by minimizing the cost function. Various simulative roads were designed to test the proposed method in the experiments. The experimental results show that the proposed approach has the ability of planning a safe and smooth path for avoiding the obstacles on both single-lane roads and multi-lane roads with good real-time performance.

autonomous driving; dynamic path planning; path candidate; path selection; cost function

2016- 07- 18;

2016- 08- 22。

國(guó)家自然科學(xué)基金青年基金資助項(xiàng)目(41401525)；廣東省自然科學(xué)基金資助項(xiàng)目(2014A030313209)；湖北省大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計(jì)劃基金資助項(xiàng)目(201410512030)。

周慧子(1995—)，女，遼寧沈陽(yáng)人，主要研究方向：動(dòng)態(tài)路徑規(guī)劃； 胡學(xué)敏(1985—)，男，湖南岳陽(yáng)人，講師，博士，主要研究方向：計(jì)算機(jī)視覺(jué)、動(dòng)態(tài)路徑規(guī)劃； 陳龍(1985—)，男，湖北襄陽(yáng)人，講師，博士，主要研究方向：立體視覺(jué)、無(wú)人駕駛； 田梅(1995—)，女，湖北武漢人，主要研究方向：動(dòng)態(tài)路徑規(guī)劃； 熊豆(1995—)，女，湖北武漢人，主要研究方向：自動(dòng)控制。

1001- 9081(2017)03- 0883- 06

10.11772/j.issn.1001- 9081.2017.03.883

TP391.7

A