彭曉燕，謝 浩，黃 晶

（湖南大學(xué)機(jī)械與運(yùn)載工程學(xué)院，長(zhǎng)沙 410082）

前言

隨著汽車數(shù)量的不斷增長(zhǎng)，交通事故也隨之增多，汽車安全早已成為全社會(huì)關(guān)注的焦點(diǎn)問題［1］。無人駕駛技術(shù)在降低道路交通事故發(fā)生率方面有著重要的研究意義和巨大的應(yīng)用價(jià)值。隨著人工智能的應(yīng)用和發(fā)展，無人駕駛汽車也越來越受到關(guān)注，其中的避障問題已經(jīng)成為了研究的熱點(diǎn)［2］。局部路徑規(guī)劃，也稱為避障路徑規(guī)劃，即考慮本車和障礙物之間的幾何關(guān)系尋找出一條避免與障礙物發(fā)生碰撞的路徑，是無人駕駛汽車的重要功能模塊之一。

目前常用的局部路徑規(guī)劃算法主要分為4大類：人工勢(shì)場(chǎng)法、基于圖搜索的方法、基于采樣的方法和基于離散優(yōu)化的方法。人工勢(shì)場(chǎng)法是Khatib提出的虛擬力法，此方法算法簡(jiǎn)明，實(shí)時(shí)性良好，但存在容易陷入局部最小點(diǎn)的問題，且因未考慮車輛動(dòng)力學(xué)約束，導(dǎo)致無法得到合理的路徑甚至規(guī)劃失?。?］。針對(duì)此問題，安林芳等［4］提出了一種新的障礙點(diǎn)構(gòu)建方式，解決了局部最小值問題；修彩靖等［5］利用高斯組合隸屬函數(shù)建立引力的目標(biāo)點(diǎn)函數(shù)，并在引力點(diǎn)函數(shù)中考慮障礙物約束和車輛約束，提高了規(guī)劃路徑的平滑性；唐志榮等［6］利用橢圓化距離代替?zhèn)鹘y(tǒng)斥力勢(shì)場(chǎng)中的實(shí)際距離，結(jié)合模型預(yù)測(cè)控制的方法，使規(guī)劃路徑滿足多約束條件。但是以上方法只考慮了障礙物靜止的工況，因此在動(dòng)態(tài)環(huán)境無法取得良好效果?；趫D搜索的方法中常用的有A*［7-12］和 D*算法［13］，此類算法雖然在機(jī)器人領(lǐng)域得到廣泛應(yīng)用，但其規(guī)劃的路徑未能滿足車輛的非完整性約束，且路徑平滑性較差。辛煜等［7］提出了一種改進(jìn)的A*算法，對(duì)柵格地圖中的柵格進(jìn)行線性插值，增加了A*算法的可搜索鄰域和搜索的方向，改善了傳統(tǒng)A*算法平滑性較差的問題；齊堯等［8］提出了新式的啟發(fā)式函數(shù)并結(jié)合了車輛非完整性約束進(jìn)行三維規(guī)劃，提高了路徑的安全性和舒適性；Boroujeni等［9］基于 A*算法提出了 FU-A*算法，該算法根據(jù)車輛的速度調(diào)節(jié)柵格的大小，提高了規(guī)劃路徑的平滑性；城市交通環(huán)境中的最優(yōu)路徑受諸多因素影響［10］，胡林等［11-12］在城市道路網(wǎng)的最短路徑規(guī)劃中，在傳統(tǒng)的A*算法的估價(jià)函數(shù)中分別考慮了信號(hào)交叉口的等待時(shí)間和能源消耗，有效降低了等待時(shí)間和能耗?；诓蓸拥姆椒ㄓ懈怕事穲D法（probabilistic road map，PRM）［14］和快速隨機(jī)擴(kuò)展樹法（rapidly random tree，RRT）［15-17］，該類算法具備搜索速度快、無須對(duì)環(huán)境進(jìn)行建模等優(yōu)點(diǎn)，但其隨機(jī)采樣的特性導(dǎo)致路徑不平滑。Taheri等［15］基于RRT算法提出了模糊貪婪快速探索隨機(jī)樹算法，減少了節(jié)點(diǎn)的數(shù)量，降低了算法的計(jì)算復(fù)雜度；宋曉琳等［16］利用高斯分布函數(shù)結(jié)合期望路徑模型，在RRT算法中引入啟發(fā)式搜索機(jī)制尋找合適的避障路徑，最后用B樣條曲線插值的方法進(jìn)行光滑化處理，解決了RRT算法規(guī)劃的路徑不平滑的問題；杜明博等［17］在RRT算法基礎(chǔ)上結(jié)合了環(huán)境約束和車輛自身的約束，實(shí)現(xiàn)了復(fù)雜的低速環(huán)境的路徑規(guī)劃?；陔x散優(yōu)化的路徑規(guī)劃方法是用數(shù)值積分和微分等方程來描述車輛的運(yùn)動(dòng)，從而產(chǎn)生數(shù)量有限的候選路徑，并通過設(shè)計(jì)代價(jià)函數(shù)，從候選路徑中選擇最優(yōu)路徑［18-19］。該方法計(jì)算量小，實(shí)時(shí)性好，在近年來得到了廣泛應(yīng)用［20］。Chu等［18］提出了一種基于此方法的算法，但未考慮城市中多車道的情況。周惠子等［21］提出了一種加權(quán)碰撞檢測(cè)的方法，在路徑規(guī)劃中考慮了多車道的工況，該方法可滿足車輛對(duì)靜止障礙的避障要求，但未考慮車輛對(duì)移動(dòng)障礙的避障要求。Hu等［20］在此基礎(chǔ)上考慮了移動(dòng)障礙，并提出了一種新的移動(dòng)障礙安全性代價(jià)函數(shù)，但是該方法僅考慮了同向行駛的動(dòng)態(tài)車輛，未考慮行人等移動(dòng)隨機(jī)性較大的障礙，具有一定的局限性。陳成等［22］提出了一種基于4階貝塞爾曲線的軌跡規(guī)劃方法，采用優(yōu)化方法求解最優(yōu)軌跡。張琳等［23］引入了滾動(dòng)窗口優(yōu)化策略，根據(jù)最優(yōu)指標(biāo)決策出當(dāng)前規(guī)劃窗口的局部最優(yōu)路徑。Li等［24］基于離散優(yōu)化的方法提出了一種實(shí)時(shí)的軌跡規(guī)劃框架，并在實(shí)際道路環(huán)境中驗(yàn)證了算法的可行性。Lim等［25］提出了一種基于采樣和優(yōu)化方法相結(jié)合的分層軌跡規(guī)劃方法，仿真表明該算法適用于多種城市場(chǎng)景。Zhang等［26］結(jié)合多種方法提出了一種混合軌跡規(guī)劃算法，該方法考慮了車輛的幾何約束及非完整約束，規(guī)劃路徑較為平滑。以上基于離散優(yōu)化的局部路徑規(guī)劃算法在規(guī)劃時(shí)需要進(jìn)行坐標(biāo)轉(zhuǎn)換，常規(guī)轉(zhuǎn)換方法存在積分等復(fù)雜運(yùn)算，實(shí)時(shí)性有待進(jìn)一步提高。

針對(duì)以上不足，本文中提出了一種基于離散優(yōu)化的無人駕駛汽車局部路徑規(guī)劃算法，設(shè)計(jì)了一種基于運(yùn)動(dòng)估計(jì)結(jié)合高斯卷積的移動(dòng)障礙安全性代價(jià)函數(shù)，并結(jié)合靜止障礙安全性代價(jià)函數(shù)，使無人駕駛車輛可以完成對(duì)靜止和移動(dòng)障礙的規(guī)避；使用了一種新的坐標(biāo)轉(zhuǎn)換計(jì)算方法，將路徑從規(guī)劃時(shí)使用的s-ρ坐標(biāo)系轉(zhuǎn)換到大地笛卡爾坐標(biāo)系，提高了實(shí)時(shí)性。最后仿真和實(shí)驗(yàn)結(jié)果表明，提出的規(guī)劃算法規(guī)劃的路徑不僅能滿足避障要求，且運(yùn)動(dòng)軌跡平滑，車輛跟蹤路徑時(shí)側(cè)向加速度在合理范圍內(nèi)，穩(wěn)定性良好，實(shí)時(shí)性滿足無人車對(duì)局部路徑規(guī)劃算法的要求。

1 局部路徑規(guī)劃算法

局部路徑規(guī)劃是在已知全局路徑的基礎(chǔ)上進(jìn)行的，全局路徑由高精度地圖提供。算法的流程如下：首先，使用三次樣條曲線對(duì)全局路徑進(jìn)行弧長(zhǎng)參數(shù)化擬合；然后，利用全局路徑上的弧長(zhǎng)s和距離全局路徑的橫向偏移量ρ建立s-ρ坐標(biāo)系，并規(guī)劃出一系列的平滑曲線，即候選路徑，再將其從s-ρ坐標(biāo)系轉(zhuǎn)換到大地笛卡爾坐標(biāo)系中以便于后續(xù)的路徑跟隨控制；最后，采用多目標(biāo)代價(jià)函數(shù)從候選路徑中選擇出最優(yōu)路徑。

1.1 全局路徑基準(zhǔn)線的擬合

全局路徑由一系列離散的點(diǎn)序列組成，作為局部路徑規(guī)劃的基準(zhǔn)線。考慮到三次樣條曲線的1階和2階導(dǎo)數(shù)具有連續(xù)性的優(yōu)點(diǎn)，本研究使用三次樣條曲線來擬合全局路徑，即

式中：s為車輛當(dāng)前位置距離基準(zhǔn)線的最近點(diǎn)所在基準(zhǔn)線上的弧長(zhǎng)；si為該弧長(zhǎng)所在的第i個(gè)路徑片段的起點(diǎn)；x0和y0為基準(zhǔn)線的點(diǎn)在大地笛卡爾坐標(biāo)系的x，y坐標(biāo)；axi、bxi、cxi、dxi、ayi、byi、cyi和 dyi為基準(zhǔn)線擬合的樣條曲線的參數(shù)。

如圖1所示，基準(zhǔn)線在擬合時(shí)會(huì)被分成很多小段，其中每一段路徑片段都可以由式（1）表示。

圖1 車輛在全局路徑的定位

因此，基準(zhǔn)線上的點(diǎn)的航向角和曲率可以用x0（s）和y0（s）對(duì)s的1階導(dǎo)數(shù)和2階導(dǎo)數(shù)計(jì)算求出，即

式中：x′0、y′0和 x″0、y″0分別為 x0（s）和 y0（s）對(duì) s的1階導(dǎo)數(shù)和2階導(dǎo)數(shù)。

1.2 候選路徑的生成

候選路徑為以汽車當(dāng)前位置為起點(diǎn)，路徑的末端與基準(zhǔn)線切向角相同的一系列路徑。為了確定候選路徑的起點(diǎn)和終點(diǎn)，需快速準(zhǔn)確地找到車輛距離基準(zhǔn)線的最近點(diǎn)，本研究使用Wang等［27］提出的二次規(guī)劃與牛頓法相結(jié)合的方法尋找車輛距離基準(zhǔn)線的最近點(diǎn)。假設(shè)車輛距離基準(zhǔn)線的最近點(diǎn)所在基準(zhǔn)線上的弧長(zhǎng)為s，車輛與基準(zhǔn)線之間的距離為橫向偏移量ρ，則車輛當(dāng)前的坐標(biāo)即可用弧長(zhǎng)s和橫向偏移量ρ表示，本研究將這樣描述車輛位置的坐標(biāo)系稱為s-ρ坐標(biāo)系。在s-ρ坐標(biāo)系中，每條候選路徑由沿基準(zhǔn)線的長(zhǎng)度Δs、當(dāng)前車輛位置的偏移量ρsi以及最終的橫向偏移量ρfi確定。假設(shè)候選路徑的橫向偏移量也滿足三次樣條曲線的方程，則第i條候選路徑可以表示為

式中：Δs＝s-sstart，sstart為車輛距離基準(zhǔn)線的最近點(diǎn)所在基準(zhǔn)線上的弧長(zhǎng)；send為基準(zhǔn)線上的候選路徑的末端對(duì)應(yīng)的弧長(zhǎng)。

為求解式（4）中的系數(shù) a、b、c，候選路徑的生成需要考慮車輛當(dāng)前的航向，同時(shí)希望路徑的末端與基準(zhǔn)線的前進(jìn)方向相同，以確保規(guī)劃出可行的路徑，如圖2所示，由此得出式（5）中的4個(gè)邊界條件。

式中θ為基準(zhǔn)線最近點(diǎn)切向角θstart與車輛當(dāng)前航向θ0之差。

圖2 邊界條件示意圖

如圖3所示，每條候選路徑都由不同的末端橫向偏移量ρfi確定，設(shè)定合適的橫向偏移量的變化量Δρ，根據(jù)不同的橫向偏移量ρfi的值，可以計(jì)算得到多組不同的系數(shù) a、b、c，從而得到多個(gè)式（4）的方程，即可生成多條候選路徑。

圖3 候選路徑示意圖

1.3 坐標(biāo)轉(zhuǎn)換

候選路徑基于s-ρ坐標(biāo)系計(jì)算，而路徑跟隨控制基于大地笛卡爾坐標(biāo)系，故必須將候選路徑從s-ρ坐標(biāo)系轉(zhuǎn)換到大地笛卡爾坐標(biāo)系。常規(guī)的基于離散優(yōu)化的路徑規(guī)劃方法［18，20-21］的坐標(biāo)轉(zhuǎn)換需要進(jìn)行復(fù)雜的積分計(jì)算。為提高算法的實(shí)時(shí)性，本研究使用了一種新的坐標(biāo)轉(zhuǎn)換計(jì)算方法。候選路徑可以用一系列離散點(diǎn)來表示，如圖4所示，根據(jù)幾何關(guān)系，第i條候選路徑的離散點(diǎn)可表示為

式中：（xi，yi）為第 i條候選路徑在大地笛卡爾坐標(biāo)系的坐標(biāo)；（x0，y0）為基準(zhǔn)線上弧長(zhǎng)s對(duì)應(yīng)的點(diǎn)在大地笛卡爾坐標(biāo)系的坐標(biāo)，可由式（1）求得；ρi（s）為橫向偏移量，可由式（4）求得；θ為車輛當(dāng)前航向θn與基準(zhǔn)線最近點(diǎn)切向角θ0之差。

圖4 坐標(biāo)轉(zhuǎn)換示意圖

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

最優(yōu)路徑為候選路徑中在滿足安全性的前提下平滑性較高的路徑。本研究設(shè)計(jì)了一種加權(quán)多目標(biāo)代價(jià)函數(shù)去評(píng)價(jià)候選路徑，歸一化處理各代價(jià)函數(shù)后進(jìn)行加權(quán)求和計(jì)算，通過選擇代價(jià)函數(shù)最小的路徑來選取最優(yōu)路徑。代價(jià)函數(shù)考慮路徑的安全性、平滑性及跟隨全局路徑的能力，設(shè)計(jì)的代價(jià)函數(shù)包含3個(gè)部分：路徑安全性代價(jià)函數(shù)fs、路徑偏移代價(jià)函數(shù)fo和路徑平滑性代價(jià)函數(shù)fsm，即

式中：f（i）為路徑總代價(jià)函數(shù)；i為候選路徑的序列號(hào)；ws、wo和wsm分別為各代價(jià)函數(shù)的權(quán)重系數(shù)；select為選擇的路徑。

本研究的最優(yōu)路徑的選擇優(yōu)先考慮安全性，所以根據(jù)安全性代價(jià)函數(shù)閾值設(shè)計(jì)了各代價(jià)函數(shù)權(quán)重值的函數(shù)，當(dāng)安全性代價(jià)函數(shù)值在閾值以上時(shí)，說明安全性較低，故安全性代價(jià)函數(shù)取較大的權(quán)重，降低選擇此類路徑的概率，反之則取較小的權(quán)重，提高選擇此類路徑的概率。經(jīng)過大量仿真實(shí)驗(yàn)總結(jié)，本研究中ws、wo和wsm3個(gè)權(quán)重系數(shù)的選擇為

1.4.1 路徑安全性代價(jià)函數(shù)的設(shè)計(jì)

為解決無人駕駛車輛對(duì)靜止和移動(dòng)障礙的規(guī)避問題，本研究設(shè)計(jì)了一種綜合考慮靜止和移動(dòng)障礙因素的加權(quán)路徑安全性代價(jià)函數(shù)，其中，靜止障礙安全性代價(jià)函數(shù)使用離散高斯卷積結(jié)合碰撞風(fēng)險(xiǎn)的方法來評(píng)估每條候選路徑的安全性；移動(dòng)障礙安全性代價(jià)函數(shù)則是基于移動(dòng)障礙運(yùn)動(dòng)狀態(tài)估計(jì)結(jié)合高斯卷積。

（1）靜止障礙安全性代價(jià)函數(shù)

無人駕駛汽車在道路行駛時(shí)，道路上的靜止障礙如?？柯愤叺能囕v等，都是潛在的危險(xiǎn)源。路徑規(guī)劃算法必須確保車輛行駛的安全，避免車輛與障礙物發(fā)生碰撞。

本研究采用外接圓對(duì)障礙物進(jìn)行包絡(luò)，障礙物的信息可以由車上的雷達(dá)傳感器獲得。本研究通過判斷候選路徑與障礙物外接圓是否相交來進(jìn)行碰撞檢測(cè)，若相交，則該路徑不可行，如圖5所示，第i條候選路徑的碰撞檢測(cè)結(jié)果為Ci，若相交，Ci＝1；否則Ci＝0。

候選路徑的碰撞檢測(cè)結(jié)果是選擇安全路徑的重要信息，但是只考慮碰撞檢測(cè)結(jié)果不能完全保證無人車行駛的安全性。如圖5（a）所示，R5和R10都是非碰撞的路徑，但是這兩條路徑距離障礙物很近，選擇其作為避障路徑是不安全的，因?yàn)槿绻窂礁櫟木炔桓呋蛘系K檢測(cè)誤差過大，將導(dǎo)致車輛與障礙物發(fā)生碰撞，所以對(duì)于這種不會(huì)與障礙物發(fā)生碰撞但是接近障礙物的路徑的安全性，必須引入相鄰路徑的碰撞風(fēng)險(xiǎn)并作進(jìn)一步評(píng)估。

圖5 候選路徑碰撞風(fēng)險(xiǎn)示意圖

雖然候選路徑與障礙物的距離可以很好地評(píng)估候選路徑的安全性，但是候選路徑或障礙物過多時(shí)需要大量的計(jì)算，導(dǎo)致實(shí)時(shí)性較差，而離散高斯卷積計(jì)算僅需候選路徑的碰撞檢測(cè)結(jié)果即可通過較小的計(jì)算量完成對(duì)所有候選路徑的碰撞風(fēng)險(xiǎn)評(píng)估，具有良好的實(shí)時(shí)性，但離散高斯卷積需要合理設(shè)置碰撞風(fēng)險(xiǎn)的標(biāo)準(zhǔn)差，該值決定了碰撞檢測(cè)的有效范圍，若過大會(huì)導(dǎo)致路徑選擇過于保守，過小則無法保證路徑的安全性。離散高斯卷積的過程是對(duì)每條候選路徑進(jìn)行碰撞風(fēng)險(xiǎn)評(píng)估時(shí)，考慮了其相鄰的一系列候選路徑的碰撞風(fēng)險(xiǎn)，當(dāng)其相鄰一系列候選路徑有碰撞風(fēng)險(xiǎn)的路徑則說明該路徑危險(xiǎn)程度較高，反之說明危險(xiǎn)程度較低。為了保證安全性和實(shí)時(shí)性，本研究使用離散高斯卷積結(jié)合碰撞風(fēng)險(xiǎn)的方法來評(píng)估每條候選路徑的安全性，可表示為

式中：fsta（i）為候選路徑Ri的靜止障礙安全性代價(jià)函數(shù)；gsta（j）為靜止障礙離散高斯卷積函數(shù)；σs為靜止障礙碰撞風(fēng)險(xiǎn)的標(biāo)準(zhǔn)差，決定了碰撞檢測(cè)的有效范圍，經(jīng)過大量的仿真分析，取值為2。

各路徑的靜態(tài)安全性代價(jià)函數(shù)見圖5（b），結(jié)果說明，路徑 R1、R2、R3、R12和 R13的安全性代價(jià)函數(shù)均接近0，而有可能引導(dǎo)無人車與移動(dòng)障礙車碰撞的R6～R9的安全性代價(jià)函數(shù)均在0.6以上，這說明安全性代價(jià)函數(shù)可以顯式地說明候選路徑的安全性，可以作為衡量本研究候選路徑安全性的指標(biāo)。

（2）移動(dòng)障礙安全性代價(jià)函數(shù)

無人駕駛汽車在實(shí)際的道路中行駛，還存在如行人、行駛的車輛等移動(dòng)障礙物，因此除考慮靜止障礙因素外，還須考慮移動(dòng)障礙對(duì)路徑選擇的影響。如圖6所示，不考慮移動(dòng)障礙物時(shí)，根據(jù)其余的代價(jià)函數(shù)可知R4為選擇的最優(yōu)路徑，但若存在如行駛在相鄰車道速度為vobs的汽車等移動(dòng)障礙物，則選擇路徑R4將會(huì)導(dǎo)致無人車與之發(fā)生碰撞。因此，在選擇最優(yōu)路徑時(shí)，必須考慮移動(dòng)障礙物的影響。

圖6 移動(dòng)障礙示意圖

移動(dòng)障礙安全性代價(jià)函數(shù)是基于移動(dòng)障礙運(yùn)動(dòng)狀態(tài)估計(jì)并結(jié)合高斯卷積得到。車載雷達(dá)測(cè)量值帶有測(cè)量噪聲，因此需要設(shè)計(jì)運(yùn)動(dòng)狀態(tài)估計(jì)算法以更為準(zhǔn)確辨識(shí)障礙目標(biāo)的狀態(tài)［28］。首先，采用謝德勝等［29］提出的移動(dòng)障礙物運(yùn)動(dòng)狀態(tài)估計(jì)方法對(duì)下一時(shí)刻障礙物的位置坐標(biāo)進(jìn)行估計(jì)。移動(dòng)障礙物的運(yùn)動(dòng)狀態(tài)可以表示為

式中：（xk，yk）為第k個(gè)周期時(shí)移動(dòng)障礙物在大地平面直角坐標(biāo)系XOY的坐標(biāo)；vksinβk為移動(dòng)障礙物在正東方向上的分速度；vkcosβk為移動(dòng)障礙物在正北方向上的分速度；vk為移動(dòng)障礙物的速度；βk為移動(dòng)障礙物的航向角。

將式（12）的狀態(tài)量代入基于勻速模型的標(biāo)準(zhǔn)卡爾曼濾波器，即可預(yù)測(cè)到下一時(shí)刻的移動(dòng)障礙物位置坐標(biāo)。然后，使用高斯卷積結(jié)合碰撞風(fēng)險(xiǎn)的方法即可得到移動(dòng)障礙物的安全性代價(jià)函數(shù)，即

式中：Cdk為第 i條候選路徑的碰撞檢測(cè)結(jié)果；gd（j）為移動(dòng)障礙高斯卷積函數(shù)；σd為移動(dòng)障礙碰撞風(fēng)險(xiǎn)的標(biāo)準(zhǔn)差，考慮到移動(dòng)障礙的碰撞風(fēng)險(xiǎn)較大，經(jīng)過大量的仿真分析，設(shè)定σd為3。

路徑安全性代價(jià)函數(shù)定義為靜止障礙安全性代價(jià)函數(shù)和移動(dòng)障礙安全性代價(jià)函數(shù)的加權(quán)和，即

式中wsta和wd分別為靜止和移動(dòng)障礙安全性代價(jià)函數(shù)的權(quán)重值。當(dāng)只檢測(cè)到靜止障礙物時(shí)，wsta和wd分別設(shè)定為1和0；同理，只檢測(cè)到移動(dòng)障礙時(shí)，wsta和wd分別設(shè)定為0和1；同時(shí)檢測(cè)到移動(dòng)障礙與靜止障礙時(shí)，經(jīng)過大量的仿真實(shí)驗(yàn)發(fā)現(xiàn)wsta和wd同時(shí)設(shè)定為0.5效果較好。

1.4.2 路徑偏移代價(jià)函數(shù)的設(shè)計(jì)

引入路徑偏移代價(jià)函數(shù)的目的是使行駛車輛盡可能靠近全局路徑，當(dāng)其它代價(jià)函數(shù)為零時(shí)，路徑偏移代價(jià)函數(shù)則是決定路徑選擇的唯一因素。路徑偏移代價(jià)函數(shù)設(shè)計(jì)為每條候選路徑的最終的橫向偏移量 ρfi的絕對(duì)值，即

圖7（a）為候選路徑示意圖，圖7（b）為路徑偏移代價(jià)函數(shù)圖，當(dāng)i＝7時(shí)，fo最小，說明R7為最接近全局路徑的路徑。

圖7 路徑偏移代價(jià)函數(shù)示意圖

1.4.3 路徑平滑性代價(jià)函數(shù)的設(shè)計(jì)

汽車在行駛時(shí)，不平滑的路徑會(huì)導(dǎo)致駕駛的舒適性降低，甚至?xí)疖囕喆蚧?，從而降低了車輛行駛的穩(wěn)定性和安全性［21］。只考慮一個(gè)規(guī)劃周期時(shí)，候選路徑的平滑性與路徑的曲率有關(guān)，路徑的曲率變化越小說明路徑變化緩慢，路徑的平滑性越好，反之說明平滑性較差。因此本研究利用候選路徑在離散采樣點(diǎn)上的曲率絕對(duì)值之和作為路徑平滑性的代價(jià)函數(shù)：

路徑的連續(xù)性也會(huì)影響無人駕駛汽車行駛的平順性。上面只考慮了本周期規(guī)劃的路徑信息，無法避免路徑發(fā)生突變。如圖8所示，R1和R3都是安全性代價(jià)函數(shù)較小的路徑，但是如果選擇R1作為最優(yōu)路徑，則會(huì)導(dǎo)致路徑發(fā)生突變，從而導(dǎo)致控制時(shí)前輪轉(zhuǎn)向角突變，降低駕駛的舒適性與穩(wěn)定性。為解決這個(gè)問題，路徑的連續(xù)性代價(jià)函數(shù)在選擇最優(yōu)路徑時(shí)考慮了上一周期規(guī)劃的路徑對(duì)本次規(guī)劃的影響，在滿足安全性的前提下，選擇與之前規(guī)劃的路徑變化最小的路徑，即圖8中與上一時(shí)刻規(guī)劃的路徑Rpre偏移量最小的路徑為R4。本研究使用離散的方法計(jì)算前一路徑與新路徑之間的公共弧長(zhǎng)部分的偏移量之差的總和，即

式中：N為兩段路徑公共弧長(zhǎng)之間離散采樣點(diǎn)的個(gè)數(shù)；sfi為路徑末端點(diǎn)對(duì)應(yīng)的弧長(zhǎng)；ssi為路徑起始點(diǎn)對(duì)應(yīng)的弧長(zhǎng)。

圖8 路徑連續(xù)性代價(jià)函數(shù)示意圖

路徑平滑性代價(jià)函數(shù)定義為曲率變化代價(jià)函數(shù)和連續(xù)性代價(jià)函數(shù)的加權(quán)求和值，即

式中wc和wco分別為曲率變化代價(jià)函數(shù)和連續(xù)性代價(jià)函數(shù)的權(quán)重值，經(jīng)過大量仿真實(shí)驗(yàn)，取值分別為0.3和0.7時(shí)效果較好。

2 仿真結(jié)果與分析

為驗(yàn)證本研究局部路徑規(guī)劃算法的可行性和有效性，在PreScan軟件中搭建了車道寬為3.5 m的三車道作為車輛行駛道路，并在道路上設(shè)置了若干靜止與移動(dòng)障礙車和行人，道路包括直道路段和彎道路段，如圖9所示，使用Matlab／Simulink軟件對(duì)本研究算法進(jìn)行編程實(shí)現(xiàn)，對(duì)無人車在存在靜止及移動(dòng)障礙的道路行駛過程進(jìn)行了聯(lián)合仿真。當(dāng)無人車接近移動(dòng)障礙車時(shí)，移動(dòng)障礙車將會(huì)從靜止加速到恒定速度，移動(dòng)障礙車1和2的速度分別為36和25 km／h，行人橫穿馬路的速度為3.6 km／h，以檢驗(yàn)本研究算法在直道和彎道中對(duì)靜止障礙連續(xù)避障及對(duì)行人和移動(dòng)障礙車進(jìn)行避障的有效性。

圖9 道路場(chǎng)景全局視圖

為使無人車可以快速、穩(wěn)定地對(duì)規(guī)劃的路徑進(jìn)行跟蹤，本研究基于Pure Pursuit算法設(shè)計(jì)了路徑跟蹤控制器。Pure Pursuit算法是一種基于幾何關(guān)系的路徑跟蹤算法，最早應(yīng)用于移動(dòng)機(jī)器人路徑跟蹤上，卡耐基梅隆大學(xué)將其應(yīng)用在無人駕駛汽車的路徑跟蹤控制，由于其具有模型簡(jiǎn)單直觀、參數(shù)易調(diào)、計(jì)算量小等優(yōu)點(diǎn)而得到了廣泛應(yīng)用［30］。

以PreScan軟件中的Audi A8為被控的無人車，設(shè)定其速度為50 km／h。無人車運(yùn)動(dòng)軌跡如圖10所示，圖中圓形為靜止障礙車，無人車的運(yùn)動(dòng)軌跡如虛線所示，車輛順利地避開了所有靜止和移動(dòng)障礙車以及行人，并在滿足避障要求的同時(shí)，很好地對(duì)全局路徑進(jìn)行跟蹤。圖11為無人車運(yùn)動(dòng)軌跡曲率，車輛行駛軌跡的曲率在-0.026 1～0.041 m-1之間，曲率的最大值為0.040 8 m-1，曲率連續(xù)且始終保持在較小值，說明本研究算法規(guī)劃的路徑平滑性較好。圖12為無人車在行駛過程中的側(cè)向加速度變化。從圖中可知，無人車在整個(gè)行駛過程中，側(cè)向加速度均低于0.5g，峰值為0.49g，說明車輛行駛穩(wěn)定。

圖10 仿真工況無人車運(yùn)動(dòng)軌跡

圖11 仿真工況無人車運(yùn)動(dòng)軌跡曲率

圖12 仿真工況側(cè)向加速度

圖13 為無人車對(duì)移動(dòng)障礙車2的避障過程的路徑生成及選擇的示意圖。圖13（a）描述的是在無人車前方檢測(cè)到移動(dòng)障礙車時(shí)，選擇了R6，該路徑的平滑性較好且滿足避障要求；圖13（b）描述的是無人車即將對(duì)移動(dòng)障礙車2進(jìn)行超車避障，依然選擇了R6，該路徑與全局路徑距離很近，可以引導(dǎo)無人車很好地跟蹤全局路徑；圖13（c）和圖13（d）描述的是無人車順利換道避障并最終行駛在全局路徑上。

圖13 無人車避障示意圖

為更為顯式地說明路徑選擇的過程，提供了圖13對(duì)應(yīng)的避障時(shí)刻的各代價(jià)函數(shù)圖，如圖14所示。在 t＝30.7 s、t＝31.3 s和 t＝31.9 s這 3個(gè)避障時(shí)刻，有可能引導(dǎo)無人車與移動(dòng)障礙車碰撞的R9～R13的安全性代價(jià)函數(shù)均在0.6以上，而路徑R6的安全性代價(jià)函數(shù)分別為0.17、0.18和0.04，均在較低水平，這說明R6的安全性較高，且路徑R6的平滑性及偏移代價(jià)函數(shù)均為較小值，這說明該路徑平滑性較好且能很好地跟隨全局路徑，故選為最優(yōu)路徑；t＝32.5 s時(shí)，候選路徑均不會(huì)與移動(dòng)障礙車發(fā)生碰撞，所以各候選路徑的安全性代價(jià)函數(shù)均為0，此時(shí)路徑選擇主要由平滑性與偏移代價(jià)函數(shù)共同決定，在上述兩種代價(jià)函數(shù)的作用下，最終選擇了與全局路徑重疊的R7，引導(dǎo)車輛最終行駛在全局路徑上。

圖14 候選路徑各類代價(jià)函數(shù)

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

為驗(yàn)證本研究算法在真實(shí)環(huán)境中避障的有效性和實(shí)時(shí)性，開展了實(shí)車實(shí)驗(yàn)。采用湖南大學(xué)“遠(yuǎn)飛”無人車實(shí)驗(yàn)平臺(tái)，硬件為Intel Core i5-3337U（四核，1.8 GHz），8 GB內(nèi)存。由激光雷達(dá)、攝像頭等環(huán)境感知傳感器獲取車輛周邊的環(huán)境信息，決策規(guī)劃模塊根據(jù)這些信息實(shí)時(shí)規(guī)劃出最優(yōu)路徑并輸出給路徑跟蹤控制部分。圖15為無人駕駛汽車系統(tǒng)硬件實(shí)驗(yàn)平臺(tái)。實(shí)驗(yàn)使用Pure Pursuit算法［30］進(jìn)行路徑跟隨控制。實(shí)驗(yàn)場(chǎng)地包括了直道和彎道路段，并布置了靜止障礙物與移動(dòng)障礙車。利用無人車導(dǎo)航系統(tǒng)采集的RTK-GPS信息對(duì)行駛軌跡進(jìn)行擬合得到全局路徑。實(shí)驗(yàn)時(shí)無人車的車速為30 km／h。圖16示出無人車對(duì)移動(dòng)障礙車的避障過程?？梢钥闯鰺o人車從道路左側(cè)避開移動(dòng)障礙車后，回到全局路徑。

圖15 湖南大學(xué)“遠(yuǎn)飛”無人車實(shí)驗(yàn)平臺(tái)

圖16 無人車避開移動(dòng)障礙車的過程

圖17 為車輛在實(shí)驗(yàn)場(chǎng)地的避障路徑規(guī)劃軌跡圖。由圖中可見，車輛順利避障并很好地跟蹤上全局路徑。圖18為實(shí)驗(yàn)工況無人車速動(dòng)軌跡曲率。由圖可知，車輛軌跡的曲率在-0.08～0.06 m-1之間，曲率的最大值為-0.070 25 m-1，曲率連續(xù)且始終保持在較小值，滿足車輛對(duì)路徑的平滑性要求。圖19為無人車在行駛過程中的側(cè)向加速度變化。從圖中可知，無人車在整個(gè)行駛過程中，側(cè)向加速度均低于0.5g，峰值為-0.23g，滿足車輛對(duì)穩(wěn)定性的要求。圖20為無人車避障過程的顯示界面截圖，從中可以看出，本研究的規(guī)劃算法能夠根據(jù)自身狀態(tài)實(shí)時(shí)選擇最優(yōu)的避障路徑。

圖17 實(shí)驗(yàn)工況無人車運(yùn)動(dòng)軌跡

圖18 實(shí)驗(yàn)工況無人車運(yùn)動(dòng)軌跡曲率

圖19 實(shí)驗(yàn)工況側(cè)向加速度

為進(jìn)一步了解本研究算法在提高實(shí)時(shí)性方面的效果，對(duì)采用常規(guī)坐標(biāo)轉(zhuǎn)換方法的離散優(yōu)化路徑算法（稱為方法1）在相同的實(shí)驗(yàn)條件下進(jìn)行了規(guī)劃實(shí)驗(yàn)，與本研究算法進(jìn)行比較。圖21為本研究算法與方法1在相同實(shí)驗(yàn)條件下的耗時(shí)對(duì)比。方法1算法耗時(shí)在18～55 ms之間，平均耗時(shí)29.9 ms，本研究算法耗時(shí)在10～25 ms之間，平均耗時(shí)18.3 ms，實(shí)時(shí)性相比方法1有明顯提高。

圖20 無人車避障示意圖

圖21 算法耗時(shí)

4 結(jié)論

為滿足無人駕駛汽車局部路徑規(guī)劃算法對(duì)安全性、實(shí)時(shí)性以及規(guī)劃路徑的平滑性等要求，設(shè)計(jì)了一種基于運(yùn)動(dòng)估計(jì)結(jié)合高斯卷積的移動(dòng)障礙安全性代價(jià)函數(shù)，并結(jié)合路徑平滑性和路徑偏移代價(jià)函數(shù)，使規(guī)劃的路徑可以引導(dǎo)車輛安全合理地避開靜止和移動(dòng)障礙；使用了一種新的坐標(biāo)轉(zhuǎn)換計(jì)算方法，提高了算法的實(shí)時(shí)性。仿真和實(shí)車實(shí)驗(yàn)結(jié)果表明，本研究算法可以實(shí)時(shí)規(guī)劃得到安全平滑的避障路徑，可應(yīng)用于實(shí)時(shí)的無人車局部路徑規(guī)劃。

今后將研究各代價(jià)函數(shù)的權(quán)重系數(shù)的取值對(duì)路徑規(guī)劃效果的影響，及障礙的危險(xiǎn)性與其碰撞風(fēng)險(xiǎn)的標(biāo)準(zhǔn)差的關(guān)系，使路徑規(guī)劃算法得到更為擬人化的效果，并將該算法應(yīng)用于更為復(fù)雜的場(chǎng)景中。