無人駕駛汽車系統入門（十六）——最短路徑搜索之A*算法

路線規劃中一個很核心的問題即最短路徑的搜索，說到最短路徑的搜索我們就不得不提A*算法，雖然原始的A*算法屬于離散路徑搜索算法（我們的世界是連續的），但是其使用啟發式搜索函數的理念卻影響著我們后面會介紹的連續路徑搜索算法，所以在介紹連續路徑搜索算法之前，理解基本的A*算法是很有必要的，本節我們從廣度優先算法出發，一步步改良算法直到引出A*算法。

最短路徑搜索是通過算法找到一張圖從起點（start）到終點（goal）之間的最短路徑（path），為了簡化，我們這里使用方格圖（該圖可以簡單地用二維數組來表示），如下動圖所示，其中代表起點，代表終點。

廣度優先算法（Breadth-First-Search, BFS）

廣度優先算法實際上已經能夠找到最短路徑，BFS通過一種從起點開始不斷擴散的方式來遍歷整個圖。可以證明，只要從起點開始的擴散過程能夠遍歷到終點，那么起點和終點之間一定是連通的，因此他們之間至少存在一條路徑，而由于BFS從中心開始呈放射狀擴散的特點，它所找到的這一條路徑就是最短路徑，下圖演示了BFS的擴散過程：

其中由全部藍色方塊組成的隊列叫做frontier （參考下面的BFS代碼）

然而，BFS搜索最短路徑實在太慢了，為了提高BFS的搜索效率，接下來我們從BFS一步步改良到A*算法（其中的代碼主要用于表達思路，距離實際運行還缺部分support code)

BFS代碼：

frontier = Queue() frontier.put(start) visited = {} visited[start] = Truewhile not frontier.empty():current = frontier.get()for next in graph.neighbors(current):if next not in visited:frontier.put(next)visited[next] = True

所涉及到的主要數據結構:

graph，要找到一張圖中兩點之間的path，我們需要一個最基本的graph數據結構。在本文中，我們只需要得到某一點的鄰近點，在這里我們的代碼調用graph.neighbors(current)，該函數返回點current周圍的所有鄰近點構成的一個列表，由for循環可以遍歷這個列表。

queue 為了解釋用隊列的原因，請看下圖，假設此時frontier為空，current當前是A點，它的neighbors將返回B、C、D、E四個點，在將這4個點都添加到frontier當中以后，下一輪while循環，frontier.get()將返回B點（根據FIFO原則，B點最早入隊，應當最早出隊），此時調用neighbors，返回A、f、g、h四個點，除了A點，其他3個點又被添加到frontier當中去。再到下一輪循環，此時frontier當中有C、D、f、g、h這幾個點，由于隊列的FIFO原則，frontier.get()將返回C點。這樣就保證了整個擴散過程是由近到遠，由內而外的，這也是廣度優先搜索的原則。可以看到，frontier.get()從隊列中取出一個元素（該元素將從隊列中被刪除）。而frontier.put()將current的鄰近點又添加進去，整個過程不斷重復，直到圖中的所有點都被遍歷一遍。

visited列表：接著上面的討論，graph.neighbors(A)將返回B、C、D、E 4個點，隨后這4個點被添加到frontier當中，下一輪graph.neighbors(B)將返回A、h、f、g四個點，而加入此時A再被添加到frontier當中就導致遍歷陷入死循環，為了避免這種狀況出現，我們需要將已經遍歷過了的點添加到visited列表當中，之后在將點放入frontier之前，首先判斷該點是否已經在visited列表當中。

下面的動圖可以看到整個廣度優先算法運行的詳細過程：

其中綠色方框代表neighbors所返回的current點的鄰近點，藍色方塊代表當前frontier隊列中的點，由于隊列先進先出(FIFO)的特點，越早加入隊列的點的序號（圖中方塊的數字）越小，因此越早被while not frontier.empty()循環中的 current = frontier.get()遍歷到。

找到路線

現在我們的算法能夠對所有的點進行遍歷，也就意味著一定能夠擴散到目標點，因此從開始到終止點的路徑是存在的。為了生成這一路徑，我們需要對擴散的過程進行記錄，保存每一個點的來源（該點由哪一個點擴散而來），最后通過這些記錄進行回溯即可得出完整的路徑。將visited數組改為came_from。比如從A擴散到B，則came_from[B]=A。有了這樣的線索，我們就能夠從終點回溯到起點。

frontier = Queue() frontier.put() came_from = {} came_from[start] = Nonewhile not frontier.empty():current = frontier.get()for next in graph.neighbors(current):if next not in came_from:frontier.put(next)came_from[next] = current

下面的算法通過came_from數組來生成path的算法，其中goal表示終點。

current = goal path = [] while current != startpath.append(current)current = came_from[current] path.append(start) path.reverse()

效果如下圖，其中每個方塊上的箭頭指向它的來源點，注意觀察隨著終點的變化，如何通過箭頭的回溯得到完整路徑。

提前結束

目前我們的算法會遍歷整個圖的每一個點，回想我們最初的目標：找到從起點到終點之間的路徑，只需要遍歷到終點即可。為了減少無用功，我們設置終止條件：一旦遍歷到goal以后，通過break讓整個算法停止。

frontier = Queue() frontier.put(start) came_from = {} came_from[start] = Nonewhile not frontier.empty():current = frontier.get()if current == goalbreakfor next in graph.neighbors(current):if next not in came_from:frontier.put(next)came_from[next] = current

擴散的方向性

到了這一步，整個BFS的思路已經完整了，但目前它的遍歷方法仍然沒有明確的目標，擴散朝著所有方向前進，十分蠢笨的遍歷了以起點為中心的周圍每一個方塊，這不就是窮舉嗎？

在上面的算法運行過程中，frontier隊列內部一般都會保持幾個點（每次frontier.get()拿出來一個點，frontier.put()又放回去一個點）。而frontier.get()返回這些點中的哪一個決定了我們的擴散向著哪一個方向進行。之前這個函數只是根據queue默認的FIFO原則來進行，因此產生了輻射狀的擴散方式，上文在介紹frontier的時候已經解釋過這一點。

我們想到，能否讓我們的擴散過程有側重方向地進行呢？ 注意，其實我們始終清楚地知道起始點和終止點的坐標，卻浪費了這條有價值的信息。在frontier.get()返回了frontier幾個點當中的一個，為了“有方向”地進行擴散，我們讓frontier返回那個看似距離終點最近的點。由于我們使用的是方格圖，每個點都有(x,y)坐標，通過兩點的(x,y)坐標就可以計算它們之間的距離，這里采用曼哈頓距離算法：

def heuristic(a, b):# 這種距離叫做曼哈頓距離（Manhattan）return abs(a.x - b.x) + abs(a.y - b.y)

啟發式的搜索

接下來我們改變原來隊列的FIFO模式，給不同的點加入優先級，使用PriorityQueue，其中frontier.put(next,priority)的第二個參數越小，該點的優先級越高，可以知道，距離終點的曼哈頓距離越小的點，會越早從frontier.get()當中返回。

frontier = PriorityQueue() frontier.put(start, 0) came_from = {} came_from[start] = Nonewhile not frontier.empty():current = frontier.get()if current == goal:breakfor next in graph.neighbors(current):if next not in came_from:priority = heuristic(goal, next)frontier.put(next, priority)came_from[next] = current

下面就是啟發式搜索的效果，unbelievable!

到這里是不是游戲就結束了？ 這不就搞定啦，還要A*做什么？ 且慢，請看下圖中出現的新問題：

可以看到，雖然啟發式搜索比BFS更快得出結果，但它所生成的路徑并不是最優的，其中出現了一些繞彎路的狀況。

從起點到終點總會存在多條路徑，之前我們通過visited（后來用came_from） 數組來避免重復遍歷同一個點，然而這導致了先入為主地將最早遍歷路徑當成最短路徑。為了兼顧效率和最短路徑，我們來看Dijkstra算法，這種算法的主要思想是從多條路徑中選擇最短的那一條：我們記錄每個點從起點遍歷到它所花費的當前最少長度，當我們通過另外一條路徑再次遍歷到這個點的時候，由于該點已經被遍歷過了（已經加入了came_from數組），我們此時不再直接跳過該點，而是比較一下目前的路徑是否比該點最初遍歷的路徑花費更少，如果是這樣，那就將該點納入到新的路徑當中去（修改該點在came_from中的值）。下面的代碼可以看到這種變化，我們通過維護cost_so_far記錄每個點到起點的當前最短路徑花費（長度），并將這里的cost作為該點在PriorityQueue中的優先級。

Dijkstra:

frontier = PriorityQueue() frontier.put(start, 0) came_from = {} cost_so_far = {} came_from[start] = None cost_so_far[start] = 0while not frontier.empty():current = frontier.get()if current == goal:breakfor next in graph.neighbors(current):new_cost = cost_so_far[current] + 1if next not in cost_so_far or new_cost < cost_so_far[next]:cost_so_far[next] = new_costpriority = new_costfrontier.put(next, priority)came_from[next] = current

一方面，我們需要算法有方向地進行擴散（啟發式），另一方面我們需要得到盡可能最短的路徑，因此A*就誕生了， 它結合了Dijkstra和啟發式算法的優點，以從起點到該點的距離加上該點到終點的估計距離之和作為該點在Queue中的優先級，下面是A*算法的代碼：

A*算法

frontier = PriorityQueue() frontier.put(start, 0) came_from = {} cost_so_far = {} came_from[start] = None cost_so_far[start] = 0while not frontier.empty():current = frontier.get()if current == goal:breakfor next in graph.neighbors(current):new_cost = cost_so_far[current] + 1if next not in cost_so_far or new_cost < cost_so_far[next]:cost_so_far[next] = new_costpriority = new_cost + heuristic(goal, next)frontier.put(next, priority)came_from[next] = current

下面的圖展現了A*算法如何克服了啟發式搜索遇到的問題：

這種A*算法用公式表示為： f(n)=g(n)+h(n),

也就指代這句代碼：

priority = new_cost + heuristic(goal, next)

其中， f(n) 指當前n點的總代價(也就是priority，總代價越低，priority越小，優先級越高，越早被frontier.get()遍歷到)，g(n) 指new_cost，從起點到n點已知的代價，h(n) 是從n點到終點所需代價的估算。

總結

以上是生活随笔為你收集整理的无人驾驶汽车系统入门（十六）——最短路径搜索之A*算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。