演算法介紹

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) = g(n) + h(n)

$$

其中：

• $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指的是兩個斜著相鄰節點之間的移動代價。如果所有節點都正方形，則其值就是$sqrt{2} * D$。

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)

歐幾里得距離

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

歐幾里得距離是指兩個節點之間的直線距離，因此其計算方法也是我們比較熟悉的：$sqrt{(p2.x-p1.x)^2 + (p2.y-p1.y)^2}$。其函式表示如下：

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. 建構函式，地圖的預設大小是50×50；
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

原文地址：《路徑規劃之 A* 演算法》 by 保羅的酒吧