本文來(lái)源：智車科技

／ 導(dǎo)讀 ／

自動(dòng)駕駛汽車從A點(diǎn)行駛到B點(diǎn)，需要軌跡規(guī)劃算法來(lái)進(jìn)行全局規(guī)劃，而具體都有哪些算法呢？這篇文章想和大家分享一下一類最常用的軌跡規(guī)劃算法，基于圖搜索的規(guī)劃算法。

在開(kāi)始介紹圖搜索算法之前，先簡(jiǎn)單介紹一下自動(dòng)駕駛中的規(guī)劃問(wèn)題：規(guī)劃模塊處于自動(dòng)駕駛軟件框架中的中間位置，其接收感知、定位、地圖發(fā)來(lái)的上游信息，輸出一條安全、平穩(wěn)、舒適的軌跡給到控制模塊，因此起到了一個(gè)承上啟下的作用，可以說(shuō)是影響自動(dòng)駕駛中舒適性及安全性最重要的一環(huán)。而傳統(tǒng)意義上的規(guī)劃問(wèn)題可以分為兩個(gè)步驟：

前端負(fù)責(zé)粗粒度的路徑查找，搜索出一條可行路徑；后端負(fù)責(zé)細(xì)粒度的軌跡生成，生成出一條控制模塊可以很好執(zhí)行的平滑軌跡。而這篇文章想要探討的，就是前端路徑搜索中一種最常用的方法，基于圖搜索的路徑規(guī)劃算法。

圖搜索基礎(chǔ)

圖是數(shù)據(jù)結(jié)構(gòu)中非常重要的一個(gè)概念，包含了節(jié)點(diǎn)和邊。在自動(dòng)駕駛中，通�？梢詫⒌貓D存儲(chǔ)為柵格地圖，每一格就代表了圖的節(jié)點(diǎn)，格與格之間的連線就代表了邊。

上圖展示了一種無(wú)向圖，即節(jié)點(diǎn)之間的連線是沒(méi)有指向的。而在實(shí)際場(chǎng)景中，往往每條邊（道路）不僅僅需要考慮距離信息，還需要考慮方向信息、路口擁堵情況、車流量等等，因此自動(dòng)駕駛中往往構(gòu)建的為有向圖、權(quán)重圖等等。除此之外，合理地對(duì)自動(dòng)駕駛場(chǎng)景下的地圖進(jìn)行分割也是保證規(guī)劃效果的一個(gè)很重要的基礎(chǔ)，不能分割太密集導(dǎo)致規(guī)劃搜索的效率太低，也不能太粗略從而導(dǎo)致某些場(chǎng)景下明明存在可行解卻無(wú)法搜索到。 構(gòu)建完圖之后，具體的規(guī)劃過(guò)程其實(shí)就是一個(gè)搜索的過(guò)程，即如何在給定起點(diǎn)及終點(diǎn)的條件下快速搜索出一條滿足期望的最優(yōu)路徑。在代碼實(shí)現(xiàn)上，整個(gè)過(guò)程需要維護(hù)一個(gè)容器（container），具體的操作分為三個(gè)步驟：移除、擴(kuò)展、塞入，以此不停循環(huán)，直至搜索到終點(diǎn)。下面介紹幾種最常用的搜索算法。

        搜索算法DFS ＆ BFS

了解了圖搜索的基礎(chǔ)之后，接下來(lái)介紹兩種最基礎(chǔ)的搜索算法：深度優(yōu)先搜索（DFS）和廣度優(yōu)先搜索（BFS）。深度優(yōu)先顧名思義，從起點(diǎn)開(kāi)始，按照某個(gè)順序一條路走下去，直至不能再繼續(xù)為止，然后回到上一節(jié)點(diǎn)，再換另一條路走下去；而廣度優(yōu)先則是每一步都擴(kuò)展同一層的所有可能節(jié)點(diǎn)，一層一層擴(kuò)展下去，直到某一層搜索到終點(diǎn)為止。可以看到深度優(yōu)先搜索的過(guò)程是一條路走到底后，最后訪問(wèn)的節(jié)點(diǎn)最先拿來(lái)處理，整個(gè)過(guò)程可以用棧（stack）來(lái)表示，符合“后進(jìn)先出”的原則；而廣度優(yōu)先搜索的過(guò)程是一層中先訪問(wèn)的節(jié)點(diǎn)拿來(lái)處理，可以用隊(duì)列（queue）來(lái)表示，符合“先進(jìn)先出”的原則。

那對(duì)于搜索算法來(lái)說(shuō)，哪一種算法好一些呢？可以看下下面這張圖，相同的場(chǎng)景下，BFS可以給出一條最短路徑，而DFS雖然速度很快，但隨機(jī)性很大，無(wú)法給出一條最優(yōu)路徑，這一缺陷使得我們不得不拋棄DFS，目前的主流基于圖搜索的規(guī)劃算法，原理其實(shí)都是基于BFS延伸出來(lái)的。

但是BFS其實(shí)也有一個(gè)很嚴(yán)重的問(wèn)題，就是其遍歷的無(wú)效節(jié)點(diǎn)過(guò)多，從而導(dǎo)致搜索效率太慢，上面左圖中的深灰色格點(diǎn)就展示了在搜索過(guò)程中，所需要訪問(wèn)的節(jié)點(diǎn)，可以看出大多數(shù)的訪問(wèn)其實(shí)都是無(wú)用的，不能給最終的搜索提供任何幫助。針對(duì)這一缺陷，就引入了Heuristic Search（啟發(fā)式搜索），即加入終點(diǎn)信息，從而使得搜索的目標(biāo)更明確，避免過(guò)多的無(wú)效搜索。而基于這一改進(jìn)提出的算法就是GBFS（Greedy Breath－First Search）。

BFS和DPS是根據(jù)先入或者后入的順序來(lái)選擇要處理的節(jié)點(diǎn)，之中不考慮任何終點(diǎn)相關(guān)的信息，而GBFS則是將與終點(diǎn)的距離考慮進(jìn)來(lái)，構(gòu)造一個(gè)規(guī)則來(lái)挑選依次要訪問(wèn)的節(jié)點(diǎn)。與終點(diǎn)的距離有多種形式，最常用的三種為Euclidean Distance、Manhattan Distance以及Diagonal Distance。

舉個(gè)例子，在實(shí)現(xiàn)BFS算法時(shí)，上圖中起點(diǎn)周圍的8個(gè)鄰居節(jié)點(diǎn)會(huì)一起存儲(chǔ)進(jìn)容器中，由于右上角的節(jié)點(diǎn)距離終點(diǎn)更近，因此再?gòu)棾鰰r(shí)首先彈出該節(jié)點(diǎn)，基于該節(jié)點(diǎn)再進(jìn)行擴(kuò)展，從而加快了搜索效率。從下圖中可以看出，算法過(guò)程中所訪問(wèn)的節(jié)點(diǎn)減少了很多，搜索的目標(biāo)性更加明確，從而極大提升了搜索效率。

Dijkstra算法和A＊算法

有了上面的基礎(chǔ)，理解路徑規(guī)劃中的Dijkstra和A＊算法就很容易了。Dijkstra算法其實(shí)BFS的進(jìn)階版，其可以用于處理帶權(quán)重邊的地圖，因此更適合在實(shí)際場(chǎng)景中使用。在該算法中，通常采用優(yōu)先隊(duì)列（priority queue）來(lái)作為訪問(wèn)容器，這是由于優(yōu)先隊(duì)列（＜key， value＞這種形式）可以根據(jù)設(shè)定的key值自動(dòng)進(jìn)行排序，在Dijkstra中key值可以設(shè)定為和起點(diǎn)的距離，由于沒(méi)考慮和終點(diǎn)的距離信息，因此還不能顯示出優(yōu)先隊(duì)列的優(yōu)勢(shì)，但之后的A＊算法里可以看出利用這種結(jié)構(gòu)的方便性。Dijkstra算法的偽代碼如下圖所示：

A＊算法和Dijkstra算法的唯一區(qū)別就在于優(yōu)先隊(duì)列中排序的依據(jù)不同，即key值不同。不同于Dijkstra，A＊在存儲(chǔ)節(jié)點(diǎn)時(shí)，還會(huì)考慮和終點(diǎn)的距離（可以類比GBFS之于BFS），其key值計(jì)算可以表示為：

其中即為Heuristic Function，有了這個(gè)指向信息，A＊算法可以更快地找到終點(diǎn)，避免了許多的無(wú)效搜索。其偽代碼如下圖所示：

這里我們可以看出優(yōu)先隊(duì)列的優(yōu)勢(shì)了，我們只需要每次計(jì)算的值并將其存儲(chǔ)進(jìn)優(yōu)先隊(duì)列，它就會(huì)自動(dòng)根據(jù)其值進(jìn)行排序，因此每次就可以取出容器的頂部值即為的最小值。在同一場(chǎng)景下，它們的實(shí)際效果如下圖所示，可以看出由于A＊避免了許多無(wú)效節(jié)點(diǎn)的訪問(wèn)，效率提升很多。 而這又引出了另一個(gè)問(wèn)題，Dijkstra由于無(wú)差別的搜索可以保證最短路徑，A＊帶有強(qiáng)指向型的搜索方式，能保證結(jié)果最優(yōu)嗎？這其實(shí)取決于A＊的啟發(fā)函數(shù)設(shè)定，為了保證最優(yōu)性，需要保證啟發(fā)函數(shù)是admissible的，即啟發(fā)函數(shù)的值需要小于等于實(shí)際上該點(diǎn)到終點(diǎn)的距離。

如果啟發(fā)式函數(shù)是admissible的，那么A＊的最終搜索結(jié)果就是最優(yōu)的。其實(shí)這也很好理解，因?yàn)槿绻麊�發(fā)函數(shù)的選擇實(shí)際上大于到終點(diǎn)的實(shí)際距離，那么依據(jù)該規(guī)則進(jìn)行的排序搜索，必然會(huì)漏掉距離最短的那條路。因此如果我們需要A＊給出最短路徑的話，我們可以將啟發(fā)函數(shù)設(shè)定為歐式距離或者對(duì)角距離，而不是曼哈頓距離。
以上就是基于圖搜索的常用路徑規(guī)劃算法介紹，歡迎大家交流指正～

－ End －