算法介紹

A*（念做：A Star）算法是一種很常用的路徑查找和圖形遍歷算法。它有較好的性能和準確度。本文在講解算法的同時也會提供Python語言的代碼實現，並會藉助matplotlib庫動態的展示算法的運算過程。

A*算法最初發表於1968年，由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael發表。它可以被認為是Dijkstra算法的擴展。

由於藉助啟發函數的引導，A*算法通常擁有更好的性能。

廣度優先搜索

為了更好的理解A*算法，我們首先從廣度優先（Breadth First）算法講起。

正如其名稱所示，廣度優先搜索以廣度做為優先級進行搜索。

從起點開始，首先遍歷起點周圍鄰近的點，然後再遍歷已經遍歷過的點鄰近的點，逐步的向外擴散，直到找到終點。

這種算法就像洪水（Flood fill）一樣向外擴張，算法的過程如下圖所示：

在上面這幅動態圖中，算法遍歷了圖中所有的點，這通常沒有必要。對於有明確終點的問題來說，一旦到達終點便可以提前終止算法，下面這幅圖對比了這種情況：

在執行算法的過程中，每個點需要記錄達到該點的前一個點的位置 -- 可以稱之為父節點。這樣做之後，一旦到達終點，便可以從終點開始，反過來順著父節點的順序找到起點，由此就構成了一條路徑。

Dijkstra算法

Dijkstra算法是由計算機科學家Edsger W. Dijkstra在1956年提出的。

Dijkstra算法用來尋找圖形中節點之間的最短路徑。

考慮這樣一種場景，在一些情況下，圖形中相鄰節點之間的移動代價並不相等。例如，遊戲中的一幅圖，既有平地也有山脈，那麼遊戲中的角色在平地和山脈中移動的速度通常是不相等的。

在Dijkstra算法中，需要計算每一個節點距離起點的總移動代價。同時，還需要一個優先隊列結構。對於所有待遍歷的節點，放入優先隊列中會按照代價進行排序。

在算法運行的過程中，每次都從優先隊列中選出代價最小的作為下一個遍歷的節點。直到到達終點為止。

下面對比了不考慮節點移動代價差異的廣度優先搜索與考慮移動代價的Dijkstra算法的運算結果：

當圖形為網格圖，並且每個節點之間的移動代價是相等的，那麼Dijkstra算法將和廣度優先算法變得一樣。

最佳優先搜索

在一些情況下，如果我們可以預先計算出每個節點到終點的距離，則我們可以利用這個信息更快的到達終點。

其原理也很簡單。與Dijkstra算法類似，我們也使用一個優先隊列，但此時以每個節點到達終點的距離作為優先級，每次始終選取到終點移動代價最小（離終點最近）的節點作為下一個遍歷的節點。這種算法稱之為最佳優先（Best First）算法。

這樣做可以大大加快路徑的搜索速度，如下圖所示：

但這種算法會不會有什麼缺點呢？答案是肯定的。

因為，如果起點和終點之間存在障礙物，則最佳優先算法找到的很可能不是最短路徑，下圖描述了這種情況。

A*算法

對比了上面幾種算法，最後終於可以講解本文的重點：A*算法了。

下面的描述我們將看到，A*算法實際上是綜合上面這些算法的特點於一身的。

A*算法通過下面這個函數來計算每個節點的優先級。

• 其中：
• f(n)是節點n的綜合優先級。當我們選擇下一個要遍歷的節點時，我們總會選取綜合優先級最高（值最小）的節點。
• g(n)是節點n距離起點的代價。
• h(n)是節點n距離終點的預計代價，這也就是A*算法的啟發函數。關於啟發函數我們在下面詳細講解。

A*算法在運算過程中，每次從優先隊列中選取f(n)值最小（優先級最高）的節點作為下一個待遍歷的節點。

另外，A*算法使用兩個集合來表示待遍歷的節點，與已經遍歷過的節點，這通常稱之為open_set和close_set。

完整的A*算法描述如下：

* 初始化open_set和close_set；
* 將起點加入open_set中，並設置優先級為0（優先級最高）；
* 如果open_set不為空，則從open_set中選取優先級最高的節點n：
 * 如果節點n為終點，則：
 * 從終點開始逐步追蹤parent節點，一直達到起點；
 * 返回找到的結果路徑，算法結束；
 * 如果節點n不是終點，則： 

 * 將節點n從open_set中刪除，並加入close_set中；
 * 遍歷節點n所有的鄰近節點：
 * 如果鄰近節點m在close_set中，則：
 * 跳過，選取下一個鄰近節點
 * 如果鄰近節點m也不在open_set中，則：
 * 設置節點m的parent為節點n
 * 計算節點m的優先級
 * 將節點m加入open_set中

啟發函數

上面已經提到，啟發函數會影響A*算法的行為。

• 在極端情況下，當啟發函數h(n)始終為0，則將由g(n)決定節點的優先級，此時算法就退化成了Dijkstra算法。
• 如果h(n)始終小於等於節點n到終點的代價，則A*算法保證一定能夠找到最短路徑。但是當h(n)的值越小，算法將遍歷越多的節點，也就導致算法越慢。
• 如果h(n)完全等於節點n到終點的代價，則A*算法將找到最佳路徑，並且速度很快。可惜的是，並非所有場景下都能做到這一點。因為在沒有達到終點之前，我們很難確切算出距離終點還有多遠。
• 如果h(n)的值比節點n到終點的代價要大，則A*算法不能保證找到最短路徑，不過此時會很快。
• 在另外一個極端情況下，如果h(n)相較於g(n)大很多，則此時只有h(n)產生效果，這也就變成了最佳優先搜索。

由上面這些信息我們可以知道，通過調節啟發函數我們可以控制算法的速度和精確度。因為在一些情況，我們可能未必需要最短路徑，而是希望能夠儘快找到一個路徑即可。這也是A*算法比較靈活的地方。

對於網格形式的圖，有以下這些啟發函數可以使用：

• 如果圖形中只允許朝上下左右四個方向移動，則可以使用曼哈頓距離（Manhattan distance）。
• 如果圖形中允許朝八個方向移動，則可以使用對角距離。
• 如果圖形中允許朝任何方向移動，則可以使用歐幾里得距離（Euclidean distance）。

關於距離

曼哈頓距離

如果圖形中只允許朝上下左右四個方向移動，則啟發函數可以使用曼哈頓距離，它的計算方法如下圖所示：

計算曼哈頓距離的函數如下，這裡的D是指兩個相鄰節點之間的移動代價，通常是一個固定的常數。

function heuristic(node) =
 dx = abs(node.x - goal.x)
 dy = abs(node.y - goal.y)
 return D * (dx + dy)

如果圖形中允許斜著朝鄰近的節點移動，則啟發函數可以使用對角距離。它的計算方法如下：

計算對角距離的函數如下，這裡的D2指的是兩個斜著相鄰節點之間的移動代價。如果所有節點都正方形，則其值就是:

function heuristic(node) =
 dx = abs(node.x - goal.x)
 dy = abs(node.y - goal.y)
 return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

歐幾里得距離

如果圖形中允許朝任意方向移動，則可以使用歐幾里得距離。

歐幾里得距離是指兩個節點之間的直線距離，因此其計算方法也是我們比較熟悉的:

其函數表示如下：

function heuristic(node) =
 dx = abs(node.x - goal.x)
 dy = abs(node.y - goal.y)
 return D * sqrt(dx * dx + dy * dy)

算法實現

雖然前面介紹了很多內容，但實際上A*算法並不複雜，實現起來也比較簡單。

下面我們給出一個Python語言的代碼示例。

之所以使用Python語言是因為我們可以藉助matplotlib庫很方便的將結果展示出來。在理解了算法之後，通過其他語言實現也並非難事。

算法的源碼可以到我的github上下載：paulQuei/a-star-algorithm。

我們的算法演示的是在一個二維的網格圖形上從起點找尋終點的求解過程。

座標點與地圖

首先，我們創建一個非常簡單的類來描述圖中的點，相關代碼如下：

# point.py
import sys
class Point: 

 def __init__(self, x, y):
 self.x = x
 self.y = y
 self.cost = sys.maxsize

接著，我們實現一個描述地圖結構的類。為了簡化算法的描述:

我們選定左下角座標[0, 0]的點是算法起點，右上角座標[size - 1, size - 1]的點為要找的終點。

為了讓算法更有趣，我們在地圖的中間設置了一個障礙，並且地圖中還會包含一些隨機的障礙。該類的代碼如下：

# random_map.py
import numpy as np
import point
class RandomMap:
 def __init__(self, size=50): ①
 self.size = size
 self.obstacle = size//8 ②
 self.GenerateObstacle() ③
 def GenerateObstacle(self):
 self.obstacle_point = []
 self.obstacle_point.append(point.Point(self.size//2, self.size//2))
 self.obstacle_point.append(point.Point(self.size//2, self.size//2-1))
 # Generate an obstacle in the middle
 for i in range(self.size//2-4, self.size//2): ④
 self.obstacle_point.append(point.Point(i, self.size-i))
 self.obstacle_point.append(point.Point(i, self.size-i-1))
 self.obstacle_point.append(point.Point(self.size-i, i))
 self.obstacle_point.append(point.Point(self.size-i, i-1))
 for i in range(self.obstacle-1): ⑤
 x = np.random.randint(0, self.size)
 y = np.random.randint(0, self.size)
 self.obstacle_point.append(point.Point(x, y))
 if (np.random.rand() > 0.5): # Random boolean ⑥
 for l in range(self.size//4):
 self.obstacle_point.append(point.Point(x, y+l))
 pass 

 else:
 for l in range(self.size//4):
 self.obstacle_point.append(point.Point(x+l, y))
 pass
 def IsObstacle(self, i ,j): ⑦
 for p in self.obstacle_point:
 if i==p.x and j==p.y:
 return True
 return False

這段代碼說明如下：

1. 構造函數，地圖的默認大小是50x50；
2. 設置障礙物的數量為地圖大小除以8；
3. 調用GenerateObstacle生成隨機障礙物；
4. 在地圖的中間生成一個斜著的障礙物；
5. 隨機生成其他幾個障礙物；
6. 障礙物的方向也是隨機的；
7. 定義一個方法來判斷某個節點是否是障礙物；

算法主體

有了基本的數據結構之後，我們就可以開始實現算法主體了。

這裡我們通過一個類來封裝我們的算法。

首先實現一些算法需要的基本函數，它們如下：

# a_star.py
import sys
import time
import numpy as np
from matplotlib.patches import Rectangle
import point
import random_map
class AStar:
 def __init__(self, map):
 self.map=map
 self.open_set = []
 self.close_set = []
 def BaseCost(self, p):
 x_dis = p.x
 y_dis = p.y
 # Distance to start point
 return x_dis + y_dis + (np.sqrt(2) - 2) * min(x_dis, y_dis)
 def HeuristicCost(self, p):
 x_dis = self.map.size - 1 - p.x
 y_dis = self.map.size - 1 - p.y
 # Distance to end point
 return x_dis + y_dis + (np.sqrt(2) - 2) * min(x_dis, y_dis)
 def TotalCost(self, p):
 return self.BaseCost(p) + self.HeuristicCost(p)
 def IsValidPoint(self, x, y):
 if x < 0 or y < 0:
 return False
 if x >= self.map.size or y >= self.map.size:
 return False
 return not self.map.IsObstacle(x, y)
 def IsInPointList(self, p, point_list):
 for point in point_list:
 if point.x == p.x and point.y == p.y:
 return True
 return False
 def IsInOpenList(self, p):
 return self.IsInPointList(p, self.open_set)
 def IsInCloseList(self, p):
 return self.IsInPointList(p, self.close_set)
 def IsStartPoint(self, p):
 return p.x == 0 and p.y ==0
 def IsEndPoint(self, p):
 return p.x == self.map.size-1 and p.y == self.map.size-1

這裡的函數說明如下：

• __init__：類的構造函數。
• BaseCost：節點到起點的移動代價，對應了上文的g(n)。

HeuristicCost：節點到終點的啟發函數，對應上文的h(n)

。由於我們是基於網格的圖形，所以這個函數和上一個函數用的是對角距離。

TotalCost：代價總和，即對應上面提到的f(n)

• IsValidPoint：判斷點是否有效，不在地圖內部或者障礙物所在點都是無效的。
• IsInPointList：判斷點是否在某個集合中。
• IsInOpenList：判斷點是否在open_set中。
• IsInCloseList：判斷點是否在close_set中。
• IsStartPoint：判斷點是否是起點。
• IsEndPoint：判斷點是否是終點。

有了上面這些輔助函數，就可以開始實現算法主邏輯了，相關代碼如下：

# a_star.py
def RunAndSaveImage(self, ax, plt):
 start_time = time.time()
 start_point = point.Point(0, 0)
 start_point.cost = 0
 self.open_set.append(start_point)
 while True:
 index = self.SelectPointInOpenList()
 if index < 0:
 print('No path found, algorithm failed!!!')
 return
 p = self.open_set[index]
 rec = Rectangle((p.x, p.y), 1, 1, color='c')
 ax.add_patch(rec)
 self.SaveImage(plt)
 if self.IsEndPoint(p):
 return self.BuildPath(p, ax, plt, start_time)
 del self.open_set[index]
 self.close_set.append(p)
 # Process all neighbors
 x = p.x
 y = p.y
 self.ProcessPoint(x-1, y+1, p)
 self.ProcessPoint(x-1, y, p)
 self.ProcessPoint(x-1, y-1, p)
 self.ProcessPoint(x, y-1, p)
 self.ProcessPoint(x+1, y-1, p)
 self.ProcessPoint(x+1, y, p)
 self.ProcessPoint(x+1, y+1, p)
 self.ProcessPoint(x, y+1, p)

這段代碼應該不需要太多解釋了，它就是根據前面的算法邏輯進行實現。為了將結果展示出來，我們在算法進行的每一步，都會藉助於matplotlib庫將狀態保存成圖片。

上面這個函數調用了其他幾個函數代碼如下：

# a_star.py
def SaveImage(self, plt):
 millis = int(round(time.time() * 1000))
 filename = './' + str(millis) + '.png'
 plt.savefig(filename) 

def ProcessPoint(self, x, y, parent):
 if not self.IsValidPoint(x, y):
 return # Do nothing for invalid point
 p = point.Point(x, y)
 if self.IsInCloseList(p):
 return # Do nothing for visited point
 print('Process Point [', p.x, ',', p.y, ']', ', cost: ', p.cost)
 if not self.IsInOpenList(p):
 p.parent = parent
 p.cost = self.TotalCost(p)
 self.open_set.append(p)
def SelectPointInOpenList(self):
 index = 0
 selected_index = -1
 min_cost = sys.maxsize
 for p in self.open_set:
 cost = self.TotalCost(p)
 if cost < min_cost:
 min_cost = cost
 selected_index = index
 index += 1
 return selected_index
def BuildPath(self, p, ax, plt, start_time):
 path = []
 while True:
 path.insert(0, p) # Insert first
 if self.IsStartPoint(p):
 break
 else:
 p = p.parent
 for p in path:
 rec = Rectangle((p.x, p.y), 1, 1, color='g')
 ax.add_patch(rec)
 plt.draw()
 self.SaveImage(plt)
 end_time = time.time()
 print('===== Algorithm finish in', int(end_time-start_time), ' seconds')

這三個函數應該是比較容易理解的：

• SaveImage：將當前狀態保存到圖片中，圖片以當前時間命名。
• ProcessPoint：針對每一個節點進行處理：如果是沒有處理過的節點，則計算優先級設置父節點，並且添加到open_set中。
• SelectPointInOpenList：從open_set中找到優先級最高的節點，返回其索引。
• BuildPath：從終點往回沿著parent構造結果路徑。然後從起點開始繪製結果，結果使用綠色方塊，每次繪製一步便保存一個圖片。

測試入口

最後是程序的入口邏輯，使用上面寫的類來查找路徑：

# main.py
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
import random_map
import a_star
plt.figure(figsize=(5, 5))
map = random_map.RandomMap() ①
ax = plt.gca()
ax.set_xlim([0, map.size]) ②
ax.set_ylim([0, map.size])
for i in range(map.size): ③
 for j in range(map.size):
 if map.IsObstacle(i,j):
 rec = Rectangle((i, j), width=1, height=1, color='gray')
 ax.add_patch(rec)
 else:
 rec = Rectangle((i, j), width=1, height=1, edgecolor='gray', facecolor='w')
 ax.add_patch(rec)
rec = Rectangle((0, 0), width = 1, height = 1, facecolor='b')
ax.add_patch(rec) ④
rec = Rectangle((map.size-1, map.size-1), width = 1, height = 1, facecolor='r')
ax.add_patch(rec) ⑤
plt.axis('equal') ⑥
plt.axis('off')
plt.tight_layout()
#plt.show()
a_star = a_star.AStar(map)
a_star.RunAndSaveImage(ax, plt) ⑦ 

這段代碼說明如下：

1. 創建一個隨機地圖；
2. 設置圖像的內容與地圖大小一致；
3. 繪製地圖：對於障礙物繪製一個灰色的方塊，其他區域繪製一個白色的的方塊；
4. 繪製起點為藍色方塊；
5. 繪製終點為紅色方塊；
6. 設置圖像的座標軸比例相等並且隱藏座標軸；
7. 調用算法來查找路徑；

由於我們的地圖是隨機的，所以每次運行的結果可能會不一樣，下面是我的電腦上某次運行的結果：

如果感興趣這篇文章中的動圖是如何製作的，請看我的另外一篇文章：使用Matplotlib繪製3D圖形 - 製作動圖。

算法變種

A*算法有不少的變種，這裡我們介紹最主要的幾個。

更多的內容請以訪問維基百科：A* Variants。

ARA*

ARA 全稱是Anytime Repairing A*，也稱為Anytime A。

與其他Anytime算法一樣，它具有靈活的時間成本，即使在它結束之前被中斷，也可以返回路徑查找或圖形遍歷問題的有效解決方案。方法是在逐步優化之前生成快速，非最優的結果。

在現實世界的規劃問題中，問題的解決時間往往是有限的。與時間相關的規劃者對這種情況都會比較熟悉：他們能夠快速找到可行的解決方案，然後不斷努力改進，直到時間用完為止。

啟發式搜索ARA*算法，它根據可用的搜索時間調整其性能邊界。它首先使用鬆散邊界快速找到次優解，然後在時間允許的情況下逐漸收緊邊界。如果有足夠的時間，它會找到可證明的最佳解決方方案。在改進其約束的同時，ARA*重複使用以前的搜索工作，因此，比其他隨時搜索方法更有效。

與A*算法不同，Anytime A*算法最重要的功能是，它們可以被停止，然後可以隨時重啟。該方法使用控制管理器類來處理時間限制以及停止和重新啟動A*算法以找到初始的，可能是次優的解決方案，然後繼續搜索改進的解決方案，直到達到可證明的最佳解決方案。

關於ARA*的更多內容可以閱讀這篇論文：

• ARA - Anytime A with Provable Bounds on Sub-Optimality。

D*

D*是Dynamic A*的簡寫，其算法和A*類似，不同的是，其代價的計算在算法運行過程中可能會發生變化。

D*包含了下面三種增量搜索算法：

• 原始的D*由Anthony Stentz發表。
• Focussed D*由Anthony Stentz發表，是一個增量啟發式搜索算法，結合了A*和原始D*的思想。
• D Lite是由Sven Koenig和Maxim Likhachev基於LPA構建的算法。

所有三種搜索算法都解決了相同的基於假設的路徑規劃問題，包括使用自由空間假設進行規劃。在這些環境中，機器人必須導航到未知地形中的給定目標座標。它假設地形的未知部分（例如：它不包含障礙物），並在這些假設下找到從當前座標到目標座標的最短路徑。

然後機器人沿著路徑行進。當它觀察到新的地圖信息（例如以前未知的障礙物）時，它會將信息添加到其地圖中，並在必要時將新的最短路徑從其當前座標重新添加到給定的目標座標。它會重複該過程，直到達到目標座標或確定無法達到目標座標。在穿越未知地形時，可能經常發現新的障礙，因此重新計劃需要很快。增量（啟發式）搜索算法通過使用先前問題的經驗來加速搜索當前問題，從而加速搜索類似搜索問題的序列。假設目標座標沒有改變，則所有三種搜索算法都比重複的A*搜索更有效。

D*及其變體已廣泛用於移動機器人和自動車輛導航。當前系統通常基於D* Lite而不是原始D*或Focussed D*。

關於D*的更多內容可以閱讀這兩篇文章：

• Project "Fast Replanning （Incremental Heuristic Search）"
• Real-Time Replanning in Dynamic and Unknown Environments

Field D*

Field D*擴展了D*和D* Lite，是一種基於插值（ interpolation-based ）的規劃算法，它使用線性插值來有效地生成低成本路徑，從而消除不必要的轉向。

在給定線性插值假設的情況下，路徑是最優的，並且在實踐中非常有效。該算法目前被各種現場機器人系統使用。

關於Field D*的詳細內容可以看下面這篇論文：

• Field D*: An Interpolation-based Path Planner and Replanner

Block A*

Block A*擴展自A*，但它操作是一塊（block）單元而不是單個單元。

其open_set中的每個條目都是已到達但尚未擴展的塊，或者需要重新擴展的塊。

open_set中塊的優先級稱為其堆值（heap value）。與A*類似，Block A*中的基本循環是刪除具有最低堆值的條目並將其展開。在擴展期間使用LDDB來計算正在擴展的塊中的邊界單元的g值。

LDDB是一種新型數據庫，它包含了本地鄰域邊界點之間的距離。

關於Block A*的更多內容可以看下面這篇論文：

• Block A*: Database-Driven Search with Applications in Any-angle Path-Planning

• Stanford: Introduction to A*
• Wikipedia: A* search algorithm
• PythonRobotics: A* algorithm
• ARA - Anytime A with Provable Bounds on Sub-Optimality