面向自動(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ī)劃問題，提出一種在已知車輛的初始位置、速度、方向和障礙物位置情況下，實(shí)時(shí)避開障礙物的動(dòng)態(tài)規(guī)劃算法。首先,利用三次樣條曲線的二階連續(xù)性，結(jié)合已知的車道信息產(chǎn)生道路基準(zhǔn)線；其次，以車輛的位置方向和道路的曲率構(gòu)建s-q坐標(biāo)系，并在s-q坐標(biāo)系內(nèi)產(chǎn)生從車輛當(dāng)前位置到目的位置的一簇平滑曲線，作為候選路徑；最后，綜合考慮車輛行駛的安全性、平滑性和連貫性準(zhǔn)則，設(shè)計(jì)一種新的代價(jià)函數(shù)，并且通過使代價(jià)函數(shù)最小化的方法從候選路徑中選擇最佳路徑。在實(shí)驗(yàn)過程中，通過設(shè)計(jì)多種不同的模擬道路來檢驗(yàn)算法的性能。實(shí)驗(yàn)結(jié)果表明，該方法在多種地形的單車道和多車道道路上都能夠規(guī)劃出安全、平滑的路徑，有效避開障礙物，并且具有較好的實(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)駕駛汽車信息感知和智能控制的橋梁，也是實(shí)現(xiàn)自動(dòng)駕駛的基礎(chǔ)[4]。動(dòng)態(tài)路徑規(guī)劃是按照某一性能指標(biāo)在其行駛區(qū)域內(nèi)搜索一條從起點(diǎn)到終點(diǎn)的無碰撞的最優(yōu)或近似最優(yōu)路徑，其本質(zhì)是一個(gè)具有約束的復(fù)雜系統(tǒng)優(yōu)化問題，在多智能體集群、避障和目標(biāo)跟蹤控制中都會(huì)涉及到，是一個(gè)關(guān)鍵的基礎(chǔ)共性問題[5-7]，因此，動(dòng)態(tài)路徑規(guī)劃和避障對(duì)于設(shè)計(jì)合理的駕駛路徑具有重要意義，不僅規(guī)劃結(jié)果的優(yōu)劣影響著自動(dòng)駕駛汽車的準(zhǔn)確性和安全性，規(guī)劃算法的復(fù)雜度、穩(wěn)定性也間接影響著汽車的工作效率。

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

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

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

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

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

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

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

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

(1)

如圖2(a)所示，s為車輛當(dāng)前位置映射在基準(zhǔn)線上的弧長(zhǎng)，該弧長(zhǎng)計(jì)算的起點(diǎn)為第i個(gè)道路片段的起點(diǎn)si。(xbf,ybf)是曲線上的點(diǎn)在笛卡爾坐標(biāo)系中的坐標(biāo)。ax,i、bx,i、cx,i、dx,i、ay,i、by,i、cy,i、dy,i是參數(shù)樣條曲線的參數(shù)。假設(shè)車輛當(dāng)前位置到基準(zhǔn)線上最近的點(diǎn)的距離，即橫向偏移量為q，則車輛當(dāng)前坐標(biāo)可以用車輛行駛的弧長(zhǎng)s和橫向偏移量q來表示。本文將s和q表示車輛位置的坐標(biāo)稱為“s-q坐標(biāo)”。在s-q坐標(biāo)中，基準(zhǔ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)線上點(diǎn)的方向角和曲率，有必要找到車輛在基準(zhǔn)線上的位置，如圖2(a)所示的p0點(diǎn)。本文利用牛頓拉夫森二次極小化方法找到曲線上最接近車輛位置的點(diǎn)坐標(biāo)[16]。

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

(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)的方程式表示一條候選路徑，則通過不同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ī)劃車輛能夠安全、平滑地繞過障礙物。本文提出一種新的代價(jià)函數(shù)法，通過代價(jià)函數(shù)的最小化來實(shí)現(xiàn)最優(yōu)路徑的選擇。考慮到駕駛時(shí)的安全性和舒服性，本文從安全、平滑和連續(xù)這三個(gè)方面來設(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)重決定車輛的駕駛風(fēng)格。本文中這3個(gè)權(quán)值依據(jù)經(jīng)驗(yàn)分別取0.6、0.2和0.2。

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

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

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

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

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

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

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

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

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

行駛過程中，不平滑的路徑可能會(huì)引起車輪打滑，影響駕駛的安全性和舒適性，因此平滑性也是路徑選擇中必須要考慮的一個(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ù)曲線圖。圖5(a)是只考慮安全性的代價(jià)函數(shù)曲線圖，其代價(jià)最小值出現(xiàn)在i=10的地方，該路徑的彎度較大，平滑性較差，如圖5(d)中r10所示。圖5(b)是只考慮平滑性的代價(jià)函數(shù)曲線圖，其代價(jià)最小值出現(xiàn)在i=13的地方，該路徑較r10平滑，但是相對(duì)遠(yuǎn)離車道中心線，如圖5(d)中r13所示。圖5(c)為綜合考慮安全性和平滑性的代價(jià)函數(shù)曲線圖，其代價(jià)最小值為候選路徑r11，如圖5(d)中所示。很明顯，r11相比r10和r13，權(quán)衡了安全性和平滑性因素，更合適作為最優(yōu)路徑。

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

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

(8)

其中：s1和s2分別是當(dāng)前候選路徑與上次選擇路徑在基準(zhǔ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，因此考慮連貫性因素后，車輛行駛過程中沒有了路徑的突然改變，加強(qiáng)了行駛的安全性和舒適性。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖8 復(fù)雜地形的多車道避障規(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)，道路信息和車輛行駛控制通過Matlab2009b模擬仿真進(jìn)行。由實(shí)驗(yàn)可知，本文算法在基準(zhǔ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è)定車輛每行駛1m重新規(guī)劃一次，因此本文方法能滿足約20m/s(72km/h)的車輛行駛速度。鑒于車輛在避障時(shí)的速度一般低于50km/h，因此本文的方法完全能滿足動(dòng)態(tài)路徑規(guī)劃的實(shí)時(shí)性要求。

3 結(jié)語

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

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

References)

[1] 劉小洋,伍民友.車聯(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的列車自動(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] 于建志,陳永生.磁浮列車自動(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ī)視覺、動(dòng)態(tài)路徑規(guī)劃； 陳龍(1985—)，男，湖北襄陽(yáng)人，講師，博士，主要研究方向：立體視覺、無人駕駛； 田梅(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