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

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

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

1．搜索算法

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

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

圖1－1 Dijkstra算法示意圖

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

圖1－3 A＊算法示意圖

2．隨機(jī)采樣

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

圖2－1 蟻群算法示意圖

3．曲線插值

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

（3－1）

4．人工勢(shì)場(chǎng)法

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

圖4－1 基于人工勢(shì)場(chǎng)法搭建的勢(shì)能場(chǎng)

圖4－2 基于人工勢(shì)場(chǎng)法規(guī)劃的路徑點(diǎn)

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