彭曉燕，謝 浩，黃 晶

（湖南大學(xué)機械與運載工程學(xué)院，長沙 410082）

［摘要］ 無人駕駛汽車局部路徑規(guī)劃算法應(yīng)確保避障的安全性、實時性和路徑的平滑性等。本文提出了一種基于離散優(yōu)化的局部路徑規(guī)劃算法，即采用代價函數(shù)分別評估離散生成的候選路徑的安全性、平滑性等，再根據(jù)各代價函數(shù)加權(quán)計算獲得局部最佳路徑。針對障礙物移動隨機性，設(shè)計了一種基于運動估計結(jié)合高斯卷積的移動障礙安全性代價函數(shù)；考慮候選路徑曲率的變化及其連續(xù)性，設(shè)計了路徑平滑性代價函數(shù)。使用了一種新的坐標轉(zhuǎn)換計算方法將路徑從s-ρ坐標系轉(zhuǎn)換到大地笛卡爾坐標系，提高了實時性。最后，利用PreScan和Matlab軟件進行聯(lián)合仿真，并在“遠飛”無人車實驗平臺上進行了真實道路場景的實車實驗。實驗結(jié)果表明：提出的路徑規(guī)劃算法不僅能使無人車安全、合理地規(guī)避靜止和移動障礙，且完全滿足局部路徑規(guī)劃算法對實時性的要求。

關(guān)鍵詞：無人駕駛汽車；避障；路徑規(guī)劃；代價函數(shù)；實車實驗

前言

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

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

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

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

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

1.1 全局路徑基準線的擬合

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

 

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

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

   

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

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

 

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

1.2 候選路徑的生成

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

 

式中：Δs＝s-s start，s start為車輛距離基準線的最近點所在基準線上的弧長；s end為基準線上的候選路徑的末端對應(yīng)的弧長。

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

 

式中θ為基準線最近點切向角θstart與車輛當前航向θ0之差。

   

圖2 邊界條件示意圖

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

   

圖3 候選路徑示意圖

1.3 坐標轉(zhuǎn)換

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

 

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

   

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

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

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

 

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

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

 

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

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

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

無人駕駛汽車在道路行駛時，道路上的靜止障礙如停靠路邊的車輛等，都是潛在的危險源。路徑規(guī)劃算法必須確保車輛行駛的安全，避免車輛與障礙物發(fā)生碰撞。

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

候選路徑的碰撞檢測結(jié)果是選擇安全路徑的重要信息，但是只考慮碰撞檢測結(jié)果不能完全保證無人車行駛的安全性。如圖5（a）所示，R5和R10都是非碰撞的路徑，但是這兩條路徑距離障礙物很近，選擇其作為避障路徑是不安全的，因為如果路徑跟蹤的精度不高或障礙檢測誤差過大，將導(dǎo)致車輛與障礙物發(fā)生碰撞，所以對于這種不會與障礙物發(fā)生碰撞但是接近障礙物的路徑的安全性，必須引入相鄰路徑的碰撞風險并作進一步評估。

   

圖5 候選路徑碰撞風險示意圖

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

 

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

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

（2）移動障礙安全性代價函數(shù)

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

   

圖6 移動障礙示意圖

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

 

式中：（xk，yk）為第k個周期時移動障礙物在大地平面直角坐標系XOY的坐標；vk sinβk為移動障礙物在正東方向上的分速度；vk cosβk為移動障礙物在正北方向上的分速度；vk為移動障礙物的速度；βk為移動障礙物的航向角。

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

 

式中：C d k為第 i條候選路徑的碰撞檢測結(jié)果；g d（j）為移動障礙高斯卷積函數(shù)；σd為移動障礙碰撞風險的標準差，考慮到移動障礙的碰撞風險較大，經(jīng)過大量的仿真分析，設(shè)定σd為3。

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

 

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

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

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

 

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

   

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

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

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

 

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

 

式中：N為兩段路徑公共弧長之間離散采樣點的個數(shù)；s f i為路徑末端點對應(yīng)的弧長；s s i為路徑起始點對應(yīng)的弧長。

   

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

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

 

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

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

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

   

圖9 道路場景全局視圖

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

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

   

圖10 仿真工況無人車運動軌跡

   

圖11 仿真工況無人車運動軌跡曲率

   

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

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

   

圖13 無人車避障示意圖

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

   

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

3 實驗結(jié)果分析

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

   

圖15 湖南大學(xué)“遠飛”無人車實驗平臺

   

圖16 無人車避開移動障礙車的過程

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

   

圖17 實驗工況無人車運動軌跡

   

圖18 實驗工況無人車運動軌跡曲率

   

圖19 實驗工況側(cè)向加速度

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

   

圖20 無人車避障示意圖

   

圖21 算法耗時

4 結(jié)論

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

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

參考文獻

［1］ 胡林，易平，黃晶，等.基于真實事故案例的自動緊急制動系統(tǒng)兩輪車測試場景研究［J］.汽車工程，2018，40（12）：1435-1446.

［2］ GUO H，SHEN C，ZHANG H，et al.Simultaneous trajectory planning and tracking using an MPCmethod for cyber-physical systems：a case study of obstacle avoidance for an intelligent vehicle［J］.IEEE Transactions on Industrial Informatics，2018，14（9）：4273-4283.

［3］ LEE J，NAM Y Y，HONG SJ.Random force based algorithm for local minima escape of potential filed method［C］.11th International Conference on Control Automation Robotics＆Vision.Singapore：Nanyang Technological University，2010：827-832.

［4］ 安林芳，陳濤，成艾國，等.基于人工勢場算法的智能車輛路徑規(guī)劃仿真［J］.汽車工程，2017，39（12）：1451-1456.

［5］ 修彩靖，陳慧.基于改進人工勢場法的無人駕駛車輛局部路徑規(guī)劃的研究［J］.汽車工程，2013，35（9）：808-811.

［6］ 唐志榮，冀杰，吳明陽，等.基于改進人工勢場法的車輛路徑規(guī)劃與跟蹤［J］.西南大學(xué)學(xué)報（自然科學(xué)版），2018，40（6）：174-182.

［7］ 辛煜，梁華為，杜名博，等.一種可搜索無限個鄰域的改進A*算法［J］.機器人，2014，36（5）：627-633.

［8］ 齊堯，徐友春，李華，等.一種基于改進混合A*的智能車路徑規(guī)劃算法［J］.軍事交通學(xué)院學(xué)報，2018，20（8）：85-90.

［9］ BOROUJENI Z，GOEHRINGD，ULBRICH F，et al.Flexible unit A-star trajectory planning for autonomous vehicles on structured road maps［C］.IEEE International Conference on Vehicular Electronics＆Safety，IEEE，2017.

［10］ HU L，YANG J，HUANG J.The real-time shortest path algorithm with a consideration of traffic-light［J］.Journal of Intelligent＆Fuzzy Systems，2016，31（4）：2403-2410.

［11］ 胡林，鐘遠興，黃晶，等.考慮信號交叉口延時的最優(yōu)車輛路徑規(guī)劃算法［J］.汽車工程，40（10）：1223-1229.

［12］ HU L，ZHONG Y，HAO W，et al.Optimal route algorithm considering traffic light and energy consumption［J］.IEEE ACCESS，2018，6：59695-59704.

［13］ MAJUMDER S，PRASAD M S.Three dimensional D*algorithm for incremental path planning in uncooperative environment［C］.2016 3rd International Conference on Signal Processing＆Integrated Networks，IEEE，2016.

［14］ AKBARIPOUR H，MASEHIAN E.Semi-lazy probabilistic roadmap：a parameter-tuned，resilient and robust path planning method for manipulator robots［J］.International Journal of Advanced Manufacturing Technology，2016，89（5-8）：1-30.

［15］ TAHERI E，F(xiàn)ERDOWSI M H，DANESH M.Fuzzy greedy RRT path planning algorithm in a complex configuration space［J］.International Journal of Control，Automation and Systems，2018，16（6）：3026-3035.

［16］ 宋曉琳，周南，黃正瑜，等.改進RRT在汽車避障局部路徑規(guī)劃中的應(yīng)用［J］.湖南大學(xué)學(xué)報（自然科學(xué)版），2017，44（4）：30-37.

［17］ 杜明博，梅濤，陳佳佳，等.復(fù)雜環(huán)境下基于RRT的智能車輛運動規(guī)劃算法［J］.機器人，2015，37（4）：443-450.

［18］ 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.

［19］ WERLINGM，ZIEGLER J，KAMMEL S，et al.Optimal trajectory generation for dynamic street scenarios in a Frenét frame［C］.IEEE International Conference on Robotics and Automation，IEEE，2010：987-993.

［20］ HU X，CHEN L，TANG B，et al.Dynamic path planning for autonomous driving on various roads with avoidance of static and moving obstacles［J］.Mechanical Systems and Signal Processing，2018，100：482-500.

［21］ 周慧子，胡學(xué)敏，陳龍，等.面向自動駕駛的動態(tài)路徑規(guī)劃避障算法［J］.計算機應(yīng)用，2017，37（3）：883-888.

［22］ 陳成，何玉慶，卜春光，等.基于四階貝塞爾曲線的無人車可行軌跡規(guī)劃［J］.自動化學(xué)報，2015，41（3）：486-496.

［23］ 張琳，章新杰，郭孔輝，等.未知環(huán)境下智能汽車軌跡規(guī)劃滾動窗口優(yōu)化［J］.吉林大學(xué)學(xué)報（工學(xué)版），2018，48（3）：652-660.

［24］ LI X，SUN Z，CAO D，et al.Real-time trajectory planning for autonomous urban driving：framework，algorithms，and verifications［J］.IEEE／ASME Transactions on Mechatronics，2016，21（2）：740-753.

［25］ LIM W，LEE S，SUNWOO M，et al.Hierarchical trajectory planning of an autonomous car based on the integration of a sampling and an optimization method［J］.IEEE Transactions on Intelligent Transportation Systems，2018，19（2）：613-626.

［26］ ZHANG Y，CHEN H，WASLANDER SL，et al.Hybrid trajectory planning for autonomous driving in highly constrained environments［J］.IEEE Access，2018，6：32800-32819.

［27］ WANG H，KEARNEY J，ATKINSON K.Arc-length parameterized spline curves for real-time simulation［C］.Proc.5th International Conference on Curves and Surfaces，2002：387-396.

［28］ 黃晶，李勇，胡林.基于數(shù)據(jù)關(guān)聯(lián)和改進統(tǒng)計模型的激光雷達目標跟蹤研究［J］.汽車工程，2018，40（3）：356-362.

［29］ 謝德勝，徐友春，王任棟，等.基于三維激光雷達的無人車障礙物檢測與跟蹤［J］.汽車工程，2018，40（8）：952-959.

［30］ 段建民，楊晨，石慧.基于Pure Pursuit算法的智能車路徑跟蹤［J］.北京工業(yè)大學(xué)學(xué)報，2016，42（9）：1301-1306.

Research on Local Path Planning Algorithm for Unmanned Vehicles

Peng Xiaoyan，Xie Hao＆Huang Jing
College of Mechanical and Vehicle Engineering，Hunan University，Changsha 410082

［Abstract］ The local path planning algorithm of unmanned vehicle has certain requirements for the safety and real-time performance of obstacle avoidance，and the smoothness of obstacle avoidance path.In this paper，a local path planning algorithm based on discrete optimization is proposed，which uses cost function to evaluate the safety and smoothness of discretely generated candidate paths，and then obtains the local optimal path through the weighted calculation of each cost function.Aiming at the randomness of obstacles movement，a moving obstacles safety cost function is designed based on motion estimation combined with Gaussian convolution.Considering the curvature and its continuity of path，a path smoothness cost function is designed.A new coordinate transformation calculation method is adopted to convert the path from the s-ρcoordinate system to the earth Cartesian coordinate system，enhancing real-time performance.Finally，a PreScan／Matlab co-simulation and a real vehicle experiment on“Yuan Fei”unmanned vehicle experimental platform are both carried out.The results show that the path planning algorithm proposed not only enables the unmanned vehicle to safely and reasonably avoid the static and moving obstacles，but also fully meets the real-time requirements of local path planning algorithm.

Keywords：unmanned vehicle；obstacle avoidance；path planning；cost function；real vehicle experiment

doi：10.19562／j.chinasae.qcgc.2020.01.001

*國家自然科學(xué)基金（51575167、51775178）和湖南省自然科學(xué)基金（2017JJ2034）資助。

原稿收到日期為2019年1月24日，修改稿收到日期為2019年3月5日。

通信作者：黃晶，副教授，博士，E-mail：huangjing926＠hnu.edu.cn。