正菜之前，我們先來了解一下圖（包括有向圖和無向圖）的概念。圖是圖論中的基本概念，用于表示物體與物體之間存在某種關(guān)系的結(jié)構(gòu)。在圖中，物體被稱為節(jié)點(diǎn)或頂點(diǎn)，并用一組點(diǎn)或小圓圈表示。節(jié)點(diǎn)間的關(guān)系稱作邊，可以用直線或曲線來表示節(jié)點(diǎn)間的邊。

如果給圖的每條邊規(guī)定一個(gè)方向，那么得到的圖稱為有向圖，其邊也稱為有向邊，如圖10所示。在有向圖中，與一個(gè)節(jié)點(diǎn)相關(guān)聯(lián)的邊有出邊和入邊之分，而與一個(gè)有向邊關(guān)聯(lián)的兩個(gè)點(diǎn)也有始點(diǎn)和終點(diǎn)之分。相反，邊沒有方向的圖稱為無向圖。

圖10有向圖示例

數(shù)學(xué)上，常用二元組G =（V，E）來表示其數(shù)據(jù)結(jié)構(gòu)，其中集合V稱為點(diǎn)集，E稱為邊集。對(duì)于圖6所示的有向圖，V可以表示為{A，B，C，D，E，F(xiàn)，G}，E可以表示為{，，，，，，}。表示從頂點(diǎn)A發(fā)向頂點(diǎn)B的邊，A為始點(diǎn)，B為終點(diǎn)。

在圖的邊中給出相關(guān)的數(shù)，稱為權(quán)。權(quán)可以代表一個(gè)頂點(diǎn)到另一個(gè)頂點(diǎn)的距離、耗費(fèi)等，帶權(quán)圖一般稱為網(wǎng)。

在全局路徑規(guī)劃時(shí)，通常將圖11所示道路和道路之間的連接情況，通行規(guī)則，道路的路寬等各種信息處理成有向圖，其中每一個(gè)有向邊都是帶權(quán)重的，也被稱為路網(wǎng)（Route Network Graph）。

圖11道路連接情況

那么，全局路徑的規(guī)劃問題就變成了在路網(wǎng)中，搜索到一條最優(yōu)的路徑，以便可以盡快見到那個(gè)心心念念的她，這也是全局路徑規(guī)劃算法最樸素的愿望。而為了實(shí)現(xiàn)這個(gè)愿望，誕生了Dijkstra和A*兩種最為廣泛使用的全局路徑搜索算法。

Dijkstra算法

戴克斯特拉算法（Dijkstra’s algorithm）是由荷蘭計(jì)算機(jī)科學(xué)家Edsger W. Dijkstra在1956年提出，解決的是有向圖中起點(diǎn)到其他頂點(diǎn)的最短路徑問題。

假設(shè)有A、B、C、D、E、F五個(gè)城市，用有向圖表示如圖12，邊上的權(quán)重代表兩座城市之間的距離，現(xiàn)在我們要做的就是求出起點(diǎn)A城市到其它城市的最短距離。

圖12 五個(gè)城市構(gòu)建的有向圖

用Dijkstra算法求解步驟如下：

（1）創(chuàng)建一個(gè)二維數(shù)組E來描述頂點(diǎn)之間的距離關(guān)系，如圖13所示。E[B][C]表示頂點(diǎn)B到頂點(diǎn)C的距離。自身之間的距離設(shè)為0，無法到達(dá)的頂點(diǎn)之間設(shè)為無窮大。

圖13 頂點(diǎn)之間的距離關(guān)系

（2）創(chuàng)建一個(gè)一維數(shù)組Dis來存儲(chǔ)起點(diǎn)A到其余頂點(diǎn)的最短距離。一開始我們并不知道起點(diǎn)A到其它頂點(diǎn)的最短距離，一維數(shù)組Dis中所有值均賦值為無窮大。接著我們遍歷起點(diǎn)A的相鄰頂點(diǎn)，并將與相鄰頂點(diǎn)B和C的距離3（E[A][B]）和10（E[A][C]）更新到Dis[B]和Dis[C]中，如圖14所示。這樣我們就可以得出起點(diǎn)A到其余頂點(diǎn)最短距離的一個(gè)估計(jì)值。

圖14 Dis經(jīng)過一次遍歷后得到的值

（3）接著我們尋找一個(gè)離起點(diǎn)A距離最短的頂點(diǎn)，由數(shù)組Dis可知為頂點(diǎn)B。頂點(diǎn)B有兩條出邊，分別連接頂點(diǎn)C和D。因起點(diǎn)A經(jīng)過頂點(diǎn)B到達(dá)頂點(diǎn)C的距離8（E[A][B] + E[B][C] = 3 + 5）小于起點(diǎn)A直接到達(dá)頂點(diǎn)C的距離10，因此Dis[C]的值由10更新為8。同理起點(diǎn)A經(jīng)過B到達(dá)D的距離5（E[A][B] + E[B][D] = 3 + 2）小于初始值無窮大，因此Dis[D]更新為5，如圖15所示。

圖15Dis經(jīng)過第二次遍歷后得到的值

（4）接著在剩下的頂點(diǎn)C、D、E、F中，選出里面離起點(diǎn)A最近的頂點(diǎn)D，繼續(xù)按照上面的方式對(duì)頂點(diǎn)D的所有出邊進(jìn)行計(jì)算，得到Dis[E]和Dis[F]的更新值，如圖16所示。

圖16 Dis經(jīng)過第三次遍歷后得到的值

（5）繼續(xù)在剩下的頂點(diǎn)C、E、F中，選出里面離起點(diǎn)A最近的頂點(diǎn)C，繼續(xù)按照上面的方式對(duì)頂點(diǎn)C的所有出邊進(jìn)行計(jì)算，得到Dis[E]的更新值，如圖17所示。

圖17 Dis經(jīng)過第四次遍歷后得到的值

（6）繼續(xù)在剩下的頂點(diǎn)E、F中，選出里面離起點(diǎn)A最近的頂點(diǎn)E，繼續(xù)按照上面的方式對(duì)頂點(diǎn)E的所有出邊進(jìn)行計(jì)算，得到Dis[F]的更新值，如圖18所示。

圖18 Dis經(jīng)過第五次遍歷后得到的值

（6）最后對(duì)頂點(diǎn)F所有點(diǎn)出邊進(jìn)行計(jì)算，此例中頂點(diǎn)F沒有出邊，因此不用處理。至此，數(shù)組Dis中距離起點(diǎn)A的值都已經(jīng)從“估計(jì)值”變?yōu)榱恕按_定值”。

基于上述形象的過程，Dijkstra算法實(shí)現(xiàn)過程可以歸納為如下步驟：

（1）將有向圖中所有的頂點(diǎn)分成兩個(gè)集合P和Q，P用來存放已知距離起點(diǎn)最短距離的頂點(diǎn)，Q用來存放剩余未知頂點(diǎn)?？梢韵胂?，一開始，P中只有起點(diǎn)A。同時(shí)我們創(chuàng)建一個(gè)數(shù)組Flag[N]來記錄頂點(diǎn)是在P中還是Q中。對(duì)于某個(gè)頂點(diǎn)N，如果Flag[N]為1則表示這個(gè)頂點(diǎn)在集合P中，為1則表示在集合Q中。

（2）起點(diǎn)A到自己的最短距離設(shè)置為0，起點(diǎn)能直接到達(dá)的頂點(diǎn)N，Dis[N]設(shè)為E[A][N]，起點(diǎn)不能直接到達(dá)的頂點(diǎn)的最短路徑為設(shè)為∞。

（3）在集合Q中選擇一個(gè)離起點(diǎn)最近的頂點(diǎn)U（即Dis[U]最小）加入到集合P。并計(jì)算所有以頂點(diǎn)U為起點(diǎn)的邊，到其它頂點(diǎn)的距離。例如存在一條從頂點(diǎn)U到頂點(diǎn)V的邊，那么可以通過將邊U->V添加到尾部來拓展一條從A到V的路徑，這條路徑的長(zhǎng)度是Dis[U]+e[U][V]。如果這個(gè)值比目前已知的Dis[V]的值要小，我們可以用新值來替代當(dāng)前Dis[V]中的值。

（4）重復(fù)第三步，如果最終集合Q結(jié)束，算法結(jié)束。最終Dis數(shù)組中的值就是起點(diǎn)到所有頂點(diǎn)的最短路徑。

A*算法

1968年，斯坦福國際研究院的Peter E. Hart, Nils Nilsson以及Bertram Raphael共同發(fā)明了A*算法。A*算法通過借助一個(gè)啟發(fā)函數(shù)來引導(dǎo)搜索的過程，可以明顯地提高路徑搜索效率。

下文仍以一個(gè)實(shí)例來簡(jiǎn)單介紹A*算法的實(shí)現(xiàn)過程。如圖19所示，假設(shè)小馬要從A點(diǎn)前往B點(diǎn)大榕樹底下去約會(huì)，但是A點(diǎn)和B點(diǎn)之間隔著一個(gè)池塘。為了能盡快提到達(dá)約會(huì)地點(diǎn)，給姑娘留下了一個(gè)守時(shí)踏實(shí)的好印象，我們需要給小馬搜索出一條時(shí)間最短的可行路徑。

圖19 約會(huì)場(chǎng)景示意圖

A*算法的第一步就是簡(jiǎn)化搜索區(qū)域，將搜索區(qū)域劃分為若干柵格。并有選擇地標(biāo)識(shí)出障礙物不可通行與空白可通行區(qū)域。一般地，柵格劃分越細(xì)密，搜索點(diǎn)數(shù)越多，搜索過程越慢，計(jì)算量也越大；柵格劃分越稀疏，搜索點(diǎn)數(shù)越少，相應(yīng)的搜索精確性就越低。

如圖20所示，我們?cè)谶@里將要搜索的區(qū)域劃分成了正方形（當(dāng)然也可以劃分為矩形、六邊形等）的格子，圖中藍(lán)色格子代表A點(diǎn)（小馬當(dāng)前的位置），紫色格子代表B點(diǎn)（大榕樹的位置），灰色格子代表池塘。同時(shí)我們可以用一個(gè)二維數(shù)組S來表示搜素區(qū)域，數(shù)組中的每一項(xiàng)代表一個(gè)格子，狀態(tài)代表可通行和不可通行。

圖20 經(jīng)過簡(jiǎn)化后的搜索區(qū)域

接著我們引入兩個(gè)集合OpenList和CloseList，以及一個(gè)估價(jià)函數(shù)F = G + H。OpenList用來存儲(chǔ)可到達(dá)的格子，CloseList用來存儲(chǔ)已到達(dá)的格子。G代表從起點(diǎn)到當(dāng)前格子的距離，H表示在不考慮障礙物的情況下，從當(dāng)前格子到目標(biāo)格子的距離。F是起點(diǎn)經(jīng)由當(dāng)前格子到達(dá)目標(biāo)格子的總代價(jià)，值越小，綜合優(yōu)先級(jí)越高。

G和H也是A*算法的精髓所在，通過考慮當(dāng)前格子與起始點(diǎn)的距離，以及當(dāng)前格子與目標(biāo)格子的距離來實(shí)現(xiàn)啟發(fā)式搜索。對(duì)于H的計(jì)算，又有兩種方式，一種是歐式距離，一種是曼哈頓距離。

歐式距離用公式表示如下，物理上表示從當(dāng)前格子出發(fā)，支持以8個(gè)方向向四周格子移動(dòng)（橫縱向移動(dòng)+對(duì)角移動(dòng)）。

曼哈頓距離用公式表示如下，物理上表示從當(dāng)前格子出發(fā)，支持以4個(gè)方向向四周格子移動(dòng)（橫縱向移動(dòng)）。這是A*算法最常用的計(jì)算H值方法，本文H值的計(jì)算也采用這種方法。

現(xiàn)在我們開始搜索，查找最短路徑。首先將起點(diǎn)A放入到OpenList中，并計(jì)算出此時(shí)OpenList中F值最小的格子作為當(dāng)前方格移入到CloseList中。由于當(dāng)前OpenList中只有起點(diǎn)A這個(gè)格子，所以將起點(diǎn)A移入CloseList，代表這個(gè)格子已經(jīng)檢查過了。

接著我們找出當(dāng)前格子A上下左右所有可通行的格子，看它們是否在OpenList當(dāng)中。如果不在，加入到OpenList中計(jì)算出相應(yīng)的G、H、F值，并把當(dāng)前格子A作為它們的父節(jié)點(diǎn)。本例子，我們假設(shè)橫縱向移動(dòng)代價(jià)為10，對(duì)角線移動(dòng)代價(jià)為14。

我們?cè)诿總€(gè)格子上標(biāo)出計(jì)算出來的F、G、H值，如圖21所示，左上角是F，左下角是G，右下角是H。通過計(jì)算可知S[3][2]格子的F值最小，我們把它從OpenList中取出，放到CloseList中。

圖21 第一輪計(jì)算后的結(jié)果

接著將S[3][2]作為當(dāng)前格子，檢查所有與它相鄰的格子，忽略已經(jīng)在CloseList或是不可通行的格子。如果相鄰的格子不在OpenList中，則加入到OpenList，并將當(dāng)前方格子S[3][2]作為父節(jié)點(diǎn)。

已經(jīng)在OpenList中的格子，則檢查這條路徑是否最優(yōu)，如果非最優(yōu)，不做任何操作。如果G值更小，則意味著經(jīng)由當(dāng)前格子到達(dá)OpenList中這個(gè)格子距離更短，此時(shí)我們將OpenList中這個(gè)格子的父節(jié)點(diǎn)更新為當(dāng)前節(jié)點(diǎn)。

對(duì)于當(dāng)前格子S[3][2]來說，它的相鄰5個(gè)格子中有4個(gè)已經(jīng)在OpenList，一個(gè)未在。對(duì)于已經(jīng)在OpenList中的4個(gè)格子，我們以它上面的格子S[2][2]舉例，從起點(diǎn)A經(jīng)由格子S[3][2]到達(dá)格子S[2][2]的G值為20（10+10）大于從起點(diǎn)A直接沿對(duì)角線到達(dá)格子S[2][2]的G值14。顯然A經(jīng)由格子S[3][2]到達(dá)格子S[2][2]不是最優(yōu)的路徑。當(dāng)把4個(gè)已經(jīng)在OpenList 中的相鄰格子都檢查后，沒有發(fā)現(xiàn)經(jīng)由當(dāng)前方格的更好路徑，因此我們不做任何改變。

對(duì)于未在OpenList的格子S[2][3]（假設(shè)小馬可以斜穿墻腳），加入OpenList中，并計(jì)算它的F、G、H值，并將當(dāng)前格子S[3][2]設(shè)置為其父節(jié)點(diǎn)。經(jīng)歷這一波騷操作后，OpenList中有5個(gè)格子，我們需要從中選擇F值最小的那個(gè)格子S[2][3]，放入CloseList中，并設(shè)置為當(dāng)前格子，如圖22所示。

圖22第二輪計(jì)算后的結(jié)果

重復(fù)上面的故事，直到終點(diǎn)也加入到OpenList中。此時(shí)我們以當(dāng)前格子倒推，找到其父節(jié)點(diǎn)，父節(jié)點(diǎn)的父節(jié)點(diǎn)……，如此便可搜索出一條最優(yōu)的路徑，如圖23中紅色圓圈標(biāo)識(shí)。

圖23 最后計(jì)算得到的結(jié)果

基于上述形象的過程，A*算法實(shí)現(xiàn)過程可以歸納為如下步驟：

（1）將搜索區(qū)域按一定規(guī)則劃分，把起點(diǎn)加入OpenList。

（2）在OpenList中查找F值最小的格子，將其移入CloseList，并設(shè)置為當(dāng)前格子。

（3）查找當(dāng)前格子相鄰的可通行的格子，如果它已經(jīng)在OpenList中，用G值衡量這條路徑是否更好。如果更好，將該格子的父節(jié)點(diǎn)設(shè)置為當(dāng)前格子，重新計(jì)算F、G值，如果非更好，不做任何處理；如果不在OpenList中，將它加入OpenList中，并以當(dāng)前格子為父節(jié)點(diǎn)計(jì)算F、G、H值。

（4）重復(fù)步驟（2）和步驟（3），直到終點(diǎn)加入到OpenList中。

兩種算法比較

Dijkstra算法的基本思想是“貪心”，主要特點(diǎn)是以起點(diǎn)為中心向周圍層層擴(kuò)展，直至擴(kuò)展到終點(diǎn)為止。通過Dijkstra算法得出的最短路徑是最優(yōu)的，但是由于遍歷沒有明確的方向，計(jì)算的復(fù)雜度比較高，路徑搜索的效率比較低。且無法處理有向圖中權(quán)值為負(fù)的路徑最優(yōu)問題。

A*算法將Dijkstra算法與廣度優(yōu)先搜索（Breadth-First-Search，BFS）算法相結(jié)合，并引入啟發(fā)函數(shù)（估價(jià)函數(shù)），大大減少了搜索節(jié)點(diǎn)的數(shù)量，提高了搜索效率。但是A*先入為主的將最早遍歷路徑當(dāng)成最短路徑，不適用于動(dòng)態(tài)環(huán)境且不太適合高維空間，且在終點(diǎn)不可達(dá)時(shí)會(huì)造成大量性能消耗。

圖24是兩種算法路徑搜索效率示意圖，左圖為Dijkstra算法示意圖，右圖為A*算法示意圖，帶顏色的格子表示算法搜索過的格子。由圖24可以看出，A*算法更有效率，手術(shù)的格子更少。

圖24 Dijkstra算法和A*算法搜索效率對(duì)比圖（圖片來源：https://mp.weixin.qq.com/s/myU204Uq3tfuIKHGD3oEfw）

審核編輯 ：李倩