算法介紹

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

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

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

廣度優先搜尋

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

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

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

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

路徑規劃之 A* 算法算法介紹廣度優先搜尋Dijkstra算法最佳優先搜尋A*算法算法實作算法變種參考資料與推薦讀物

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

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

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

這段代碼說明如下：

構造函數，地圖的預設大小是50x50；
設定障礙物的數量為地圖大小除以8；
調用 GenerateObstacle 生成随機障礙物；
在地圖的中間生成一個斜着的障礙物；
随機生成其他幾個障礙物；
障礙物的方向也是随機的；
定義一個方法來判斷某個節點是否是障礙物；

算法主體

有了基本的資料結構之後，我們就可以開始實作算法主體了。

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

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

# 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) ⑦

建立一個随機地圖；
設定圖像的内容與地圖大小一緻；
繪制地圖：對于障礙物繪制一個灰色的方塊，其他區域繪制一個白色的的方塊；
繪制起點為藍色方塊；
繪制終點為紅色方塊；
設定圖像的坐标軸比例相等并且隐藏坐标軸；
調用算法來查找路徑；

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

如果感興趣這篇文章中的動圖是如何制作的，請看我的另外一篇文章：使用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*的更多内容可以閱讀這兩篇文章：

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

參考資料與推薦讀物

原文位址：

《路徑規劃之 A* 算法》

by 保羅的酒吧

路徑規劃之 A* 算法算法介紹廣度優先搜尋Dijkstra算法最佳優先搜尋A*算法算法實作算法變種參考資料與推薦讀物

算法介紹

廣度優先搜尋

Dijkstra算法

最佳優先搜尋

A*算法

啟發函數

關于距離

曼哈頓距離

對角距離

歐幾裡得距離

算法實作

坐标點與地圖

算法主體

測試入口

算法變種

ARA*

D*

Field D*

Block A*

參考資料與推薦讀物

繼續閱讀

無元件上傳圖檔到資料庫中，最完整解決方案

【MySQL資料庫】資料庫索引事務1.索引2.事務

neo4j之cypher使用文檔

Dijkstra--簡易版（最短路徑）

GitHub連夜封殺！這份阿裡 10W 字内部 Java 字面試手冊到底有多強？

Cloud Studio初體驗

使用 ctypes 進行 Python 和 C 的混合程式設計

【python】【資料處理】畫多元資料分布圖

NOSQL安全攻擊

mybatis_入門程式Mybatis入門

登入plsql 報錯 the account is locked --使用者被鎖

SequoiaDB巨杉資料庫C++驅動概述

【python】netconf協定對接管理裝置

「Python 網絡自動化」NETCONF —— Python 使用 NETCONF 管理配置 H3C 網絡裝置

在python中建立excel并寫入

hdu7108哈希