路徑規(guī)劃是非常常見的一類問題，例如移動機器人從A點移動到B點，游戲中的人物從A點移動到B點，以及自動駕駛中，汽車從A點到B點。這類問題中，都有兩個關(guān)鍵問題需要解決：

一是找到最短路徑

二是避開障礙物

解決這類問題，不得不提的一個經(jīng)典的算法就是A*算法。

我們盡量以淺顯易懂的語言講解清楚A*算法的原理及實現(xiàn)過程。

首先，A*算法是什么？

A*算法是一種基于采樣搜索的粗略路徑規(guī)劃算法，由stanford研究院的Peter Hart，Nils Nilsson以及Bertram Raphael發(fā)表于1968年。

A*算法的提出是想要解決移動機器人路徑規(guī)劃問題，也就是要在地圖上找到一條從起點到終點的最短路徑。

其次，如何搜索？

那么A*算法是如何去找到一條既短又無障的路徑的呢?

?

簡化搜索區(qū)域

這張圖是不是很難很快的給出答案。那么可以先將問題簡化一下：先將圖網(wǎng)格化，如圖2所示。

可以這么理解，網(wǎng)格化就是將連續(xù)的問題離散化，離散的數(shù)據(jù)更便于計算機處理，同時也便于理解。

如圖2所示，我們將要搜尋的區(qū)域劃分成了正方形的格子。

這是搜索路徑的第一步：簡化搜索區(qū)域。

將搜索區(qū)域簡化為2維數(shù)組。數(shù)組的每一項代表一個格子，它的狀態(tài)就是可走和不可走。通過計算出從S到D需要走過哪些格子，就找到了路徑。

如圖2所示，藍(lán)色格子是障礙物，灰色格子是可通過的區(qū)域，綠色格子是起點，標(biāo)記為S，紅色各種是終點，標(biāo)記為D。

我們要做的就是找到一條從起點（S）到終點（D）的最優(yōu)路徑。

約束條件?

為了順利地解決問題，我們還需要設(shè)定一些約束條件：

1、從一個格子可以朝周圍8個方向移動，其中朝上、下、左、右移動的成本為1，朝左上、右上、左下、右下移動的成本為1.4，也就是的（根號2）近似值；

2、不能朝障礙物所在格子移動；

3、如果右邊和上邊兩個格子都是障礙物，那么不能朝右上方的格子移動。

開始搜索

設(shè)定好約束條件后，就可以開始搜索路徑了。

創(chuàng)建兩個列表，分別命名為Open List，Closed List。

列表Open List將放入待檢查的節(jié)點，openlist中的格子是路徑可能會經(jīng)過的，也有可能不經(jīng)過。

列表closed list將加入已檢查節(jié)點，也就是不需要再關(guān)注的節(jié)點。

首先將起點S加入open list；然后找出S周圍所有可移動的格子，這些格子也可以被稱為鄰居；接下來算出從S移動到該格子的代價（cost），我們把這個代價記為G；將S設(shè)為父節(jié)點。每一個鄰居格子中都有一個箭頭，指向的是它的父親節(jié)點，在這一步中，他們的父親節(jié)點都是起點S。

這樣，我們就完成了對S的檢查。?

將上一步找到的鄰居節(jié)點都加入open list。

將S從openlist中移除，并將其加入cloed list。

我們可以用圖4表示，橙色邊框代表待檢查節(jié)點，黑色邊框表示已檢查節(jié)點。

?

現(xiàn)在可以看到openlist中一下有了8個待檢查節(jié)點，先檢查哪一個呢？

先給出結(jié)論：選擇具有最小F值的那個格子。

路徑排序

計算出組成路徑的方格的關(guān)鍵是下面這個等式：

F=G+H

G代表從起點S移動到這個節(jié)點的代價，沿著到達(dá)該節(jié)點而生成的路徑。

H是從指定的節(jié)點移動到終點D的估算成本。因為在這個時候我們還不知道到終點的真正距離，所以H只是對剩余距離的估算值，在這里我們采用曼哈頓方法對其進行估算。

曼哈頓距離：計算從當(dāng)前節(jié)點橫向或縱向移動到達(dá)目標(biāo)所經(jīng)過的方格數(shù)，忽略對角移動。也就是說只通過朝上、下、左、右四個方向的移動，抵達(dá)終點D的最短距離。

例如，在平面上，坐標(biāo)為（x1，y1）的i點與坐標(biāo)為（x2，y2）的j點的曼哈頓距離為d（i，j）=|x1-x2|+|y1-y2|。

要注意的是，這里用曼哈頓方法計算H時要忽略路徑中的障礙物。

最后，把G和H相加，就可以得到F。我們在每個方格上都標(biāo)注了G,H,F值。

?

如圖5所示，與起點直接相鄰的上方、下方、左方、右方的方格的G值都是1，對角線方格的G值為1.4.

H值是通過估算該節(jié)點與終點的曼哈頓距離得到，僅作橫向和縱向移動，并且忽略沿途的障礙物。采用這種方式，我們可以看到起點S左邊的方格到終點D有5個方格的距離，因此H=5。這個方格上方的方格到終點D有6個方格的距離，所以H=6。

用同樣的方法可以計算出其他的方格的H值。

繼續(xù)搜索

為了繼續(xù)搜索，我們從open list中選擇F值最小的節(jié)點，這里對應(yīng)的應(yīng)該是起點S右邊F值為4的格子。將所選節(jié)點從open list中取出，加入close list中，然后對它執(zhí)行前面的檢查。

這次檢查時要注意以下幾點：

1、如果鄰居節(jié)點已經(jīng)在closed list中，或者是不可走的方格，直接忽略；

2、如果鄰居節(jié)點不在open list中，計算該鄰居的G、H、F，并將當(dāng)前選定的節(jié)點設(shè)置為該鄰居的父節(jié)點。另外記得將該鄰居加入open list中。

3、如果鄰居節(jié)點已經(jīng)在open list中，也就是說，這個鄰居已有父節(jié)點，計算從起點經(jīng)由當(dāng)前所選節(jié)點到達(dá)該鄰居的G值，檢查G值是否更小。如果沒有，那么不做任何操作。

如果新的G值更小，那么將這個鄰居的父節(jié)點重設(shè)為當(dāng)前選定的節(jié)點，并更新該鄰居的G值和F值。

當(dāng)將相鄰的4個鄰居都檢查之后，沒有發(fā)現(xiàn)經(jīng)由當(dāng)前方格的更優(yōu)的路徑，因此我們不做任何改變。

現(xiàn)在我們已經(jīng)檢查了當(dāng)前方格的所有相鄰的方格，也對他們進行了處理，接下來該選擇下一個待檢查的方格了。

再次遍歷open list，現(xiàn)在只有7個方格了，依然選擇F值最小的那個。

這次我們會發(fā)現(xiàn)有兩個方格的F值都是54，選哪個都可以。通常從速度上考慮，選擇最后加入open list的方格更快。

我們選擇起點右下方的方格。

將不在open list中的鄰居加入open list，同時將當(dāng)前選定的方格設(shè)為他們的父親節(jié)點；當(dāng)前選定方格剩下的3個鄰居中，有2個已在close list中，可以忽略。

對已在openlist中的鄰居方格進行檢查，即檢查從起點經(jīng)過當(dāng)前方格到達(dá)那里是否具有更小的G值。沒有，那么不做任何的操作。

?

不斷重復(fù)這個過程，直到將終點D也加入到了open list中,并且是其中F值最小的節(jié)點。按照之前的步驟，取出F值最小的節(jié)點，發(fā)現(xiàn)它的H值為0，這意味著這個節(jié)點就是終點D。到此搜索也就可以告一段落了。

這里要注意的是，在起點下面2格的方格（也就是標(biāo)記了星標(biāo)的方案）已與前面不同了。之前它的G值是2.8，并且指向它右上方的方格?，F(xiàn)在它的G值為2，指向它正上方的方格。

這是在尋路過程中的某處發(fā)生的，經(jīng)過檢查發(fā)現(xiàn)使用新路徑時G值變得更低，因此父親節(jié)點被重置,G值和F值被重新計算。

確定最短路徑

最后，我們怎么去確定最短路徑呢？

從終點開始，按著箭頭依次向父親節(jié)點移動，直到回到起點S，這個路徑就是最佳路徑。

要注意的是，最佳路徑可能有多條；例如在這個案例中，下圖也是一條F=5.6的路徑，這取決于當(dāng)openlist中存在多個F值最小的節(jié)點時，先選取哪一個進行搜索。

?

A*算法總結(jié)

1.將地圖網(wǎng)格化

2.創(chuàng)建openlist列表與close list列表

3.將起點加入openlist

4.遍歷openlist，查找F值最小的節(jié)點，將它作為當(dāng)前要檢查的節(jié)點。

5.將這個節(jié)點移到close list

6.對與當(dāng)前節(jié)點方格相鄰的8個方格進行進行以下檢查：

如果它在close list中，或者是不可走，忽略它；

如果它不在open list中，將它加入open list，并且將當(dāng)前方格設(shè)置為它的父親節(jié)點，記錄這個方格的G、H和F值；

如果它已經(jīng)在openlist中，檢查經(jīng)由當(dāng)前方格到達(dá)它是否是更優(yōu)路徑，用G值作參考，更小的G值表示這是更優(yōu)的路徑。如果新G值比原G值小，將它的父親節(jié)點重新設(shè)置為當(dāng)前方格節(jié)點，并重新計算它的G值和F值。

7.重復(fù)4-6步，直到遇到以下情況，即可停止搜索。

將終點加入到了open list中，此時路徑已經(jīng)找到了；

查找終點失敗，并且openlist是空的，此時意味著沒有路徑。

8.保存路徑。從終點開始，依次向父親節(jié)點移動直到起點，這就是搜索到的最優(yōu)路徑。