自動駕駛汽車四種常用的路徑規(guī)劃算法解析

自動駕駛汽車的路徑規(guī)劃算法最早源于機(jī)器人的路徑規(guī)劃研究，但是就工況而言卻比機(jī)器人的路徑規(guī)劃復(fù)雜得多，自動駕駛車輛需要考慮車速、道路的附著情況、車輛最小轉(zhuǎn)彎半徑、外界天氣環(huán)境等因素。

本文將為大家介紹四種常用的路徑規(guī)劃算法，分別是搜索算法、隨機(jī)采樣、曲線插值和人工勢場法。

1．搜索算法

搜索算法主要包括遍歷式和啟發(fā)式兩種，其中Dijkstra算法屬于傳統(tǒng)的遍歷式，A＊算法屬于啟發(fā)式，在A＊算法的基礎(chǔ)上，還衍生出了D＊Lite算法、Weighted A＊算法等其他類型。Dijkstra算法最早由荷蘭計算機(jī)學(xué)家狄克斯特拉于1959年提出，算法核心是計算從一個起始點到終點的最短路徑，其算法特點是以起始點開始向周圍層層擴(kuò)展，直到擴(kuò)展到終點為止，再從中找到最短路徑，算法搜索方式如圖（1－1）所示。A＊算法在Dijkstra算法上結(jié)合了最佳優(yōu)先算法，在空間的每個節(jié)點定義了一個啟發(fā)函數(shù)（估價函數(shù)），啟發(fā)函數(shù)為當(dāng)前節(jié)點到目標(biāo)節(jié)點的估計值，從而減少搜索節(jié)點的數(shù)量從而提高效率。A＊算法中的啟發(fā)函數(shù)

包括兩部分，表示從初始點到任意節(jié)點n的代價，表示節(jié)點n到目標(biāo)點的啟發(fā)式評估代價，在對象從初始點開始向目標(biāo)點移動時，不斷計算的值，從而選擇代價最小的節(jié)點。一般來說遍歷式算法可以取得全局最優(yōu)解，但是計算量大，實時性不好；啟發(fā)式算法結(jié)合了遍歷式算法以及最佳優(yōu)先算法的優(yōu)點，具有計算小、收斂快的特點。圖（1－2）是最佳優(yōu)先算法示意圖，可以看出該算法有一定的選擇性，但是面對圖中的u型障礙物會出現(xiàn)計算效率低的情況。而A＊算法完美的結(jié)合了Dijkstra算法和最佳優(yōu)先算法，不僅有一定的選擇性，并且計算量相對也是最少的，更快得找到了最短路徑。

圖1－1 Dijkstra算法示意圖

圖1－2 最佳優(yōu)先算法示意圖

圖1－3 A＊算法示意圖

2．隨機(jī)采樣

隨機(jī)采樣主要包括蟻群算法以及RRT（快速擴(kuò)展隨機(jī)樹）算法。蟻群算法是由Dorigo M等人于1991年首先提出，并首先使用在解決TSP（旅行商問題）上。其算法基本原理如下：1．螞蟻在路徑上釋放信息素。2．碰到還沒走過的路口，隨機(jī)選一條走，同時釋放與路徑長度有關(guān)的信息素。3．信息素濃度與路徑長度成反比。后來螞蟻再次碰到該路口時，就選擇信息濃度較高的路徑。4．最優(yōu)路徑上的信息素濃度越來越大。5．信息素濃度最大的路徑為最優(yōu)路徑。其在小規(guī)模TSP中性能尚可，再大規(guī)模TSP問題中性能下降，容易停滯。實際道路環(huán)境是比較復(fù)雜的，不光有道路、障礙物等的限制，也有其自身動力學(xué)的約束，所以該算法更適合做全局路徑規(guī)劃，不太適合局部路徑規(guī)劃。

圖2－1 蟻群算法示意圖

3．曲線插值

曲線插值的方法是按照車輛在某些特定條件（安全、快速、高效）下，進(jìn)行路線的曲線擬合，常見的有貝塞爾曲線、多項式曲線、B樣條曲線等。一般就多項式算法而言，主要考慮以下幾個幾何約束，從而確定曲線的參數(shù)。幾何約束：1．起始點的位置與姿態(tài)。2．最小轉(zhuǎn)彎半徑。3．障礙物約束。4．目標(biāo)點的位置與姿態(tài)。根據(jù)考慮的幾何約束不同，多項式算法的階數(shù)從三階到六階甚至更高階，階數(shù)越高的算法復(fù)雜度越高，收斂速度越慢。四次多項式的形式如式（3－1）所示，參數(shù)由幾何約束條件確定。基于參數(shù)化曲線來描述軌跡，這種類型的算法比較直觀，也可以更加準(zhǔn)確的描述車輛所需滿足的道路條件，規(guī)劃出的軌跡也十分平坦、曲率變化連續(xù)并可進(jìn)行約束。缺點是計算量較大，實時性不太好，并且其評價函數(shù)也比較難以找到最優(yōu)的，未來的研究方向主要集中于簡化算法以及更加完善的評價函數(shù)。目前，曲線擬合算法是采用比較廣泛的規(guī)劃方法。

（3－1）

4．人工勢場法

人工勢場法（Artificial PotentialField，APF）是由KhaTIb于1986年提出的。該算法是假設(shè)目標(biāo)點會對自動駕駛車輛產(chǎn)生引力，障礙物對自動駕駛車輛產(chǎn)生斥力，從而使自動駕駛車輛沿“勢峰”間的“勢谷”前進(jìn)。這種算法的優(yōu)點就是結(jié)構(gòu)簡單，有利于底層控制的實時性，可大大減少計算量和計算時間，并且生成相對光滑的路徑，利于保持自動駕駛車輛的穩(wěn)定性。算法的缺點是有可能陷入局部最優(yōu)解，難以對規(guī)劃出的路徑進(jìn)行車輛動力學(xué)約束，復(fù)雜環(huán)境下的勢場搭建也比較棘手。勢場的基本步驟如下：首先搭建勢場，包括障礙物勢場以及目標(biāo)點勢場，然后通過求勢場負(fù)梯度，可以得到車輛在勢場中所受的障礙物斥力以及目標(biāo)點引力。將所受的所有障礙物斥力與目標(biāo)點引力疊加，就可以得到車輛在勢場中任意位置的受力情況，最后根據(jù)合力情況不斷迭代更新位置，就可以得到從起始點到終點的完整路徑。

圖4－1 基于人工勢場法搭建的勢能場

圖4－2 基于人工勢場法規(guī)劃的路徑點

最后以下表對本文介紹的四種算法的優(yōu)缺點、計算效率進(jìn)行一個簡要的對比總結(jié)。不難發(fā)現(xiàn)，其中人工勢場法的計算速度最快，實時性也最好，但是存在局部最優(yōu)解、復(fù)雜勢場難以搭建的情況，這也是未來該算法的研究熱點、難點；其中，曲線插值是目前較常見的一種算法，雖然該算法的計算效率不高，但是相信在未來車載計算機(jī)的計算能力大幅度提升之后，該算法可以被更廣泛得使用。