尋路演算法之A*演算法詳解

丿風色幻想發表於2022-03-24
演算法

前言

在實際開發中我們會經常用到尋路演算法，例如MMOARPG遊戲魔獸中，裡面的人物行走為了模模擬實人物行走的體驗，會選擇最近路線達到目的地，期間會避開高山或者湖水，繞過箱子或者樹林，直到走到你所選定的目的地。這種人類理所當然的行為，在計算機中卻需要特殊的演算法去實現，常用的尋路演算法主要有寬度最優搜尋[1]、Dijkstra演算法、貪心演算法、A*搜尋演算法、B*搜尋演算法[2]、導航網格演算法、JPS演算法[3]等，學習這些演算法的過程就是不斷抽象人類尋路決策的過程。本文主要以一個簡單空間尋路為例，對A*演算法進行分析實現。

介紹

A*(A-Star)演算法是一種靜態路網中求解最短路徑最有效的直接搜尋方法，也是解決許多搜尋問題的常用啟發式演算法，演算法中的距離估算值與實際值越接近，最終搜尋速度越快。之後湧現了很多預處理演算法（如ALT，CH，HL等等），線上查詢效率是A*演算法的數千甚至上萬倍。

問題

在包含很多凸多邊形障礙的空間裡，解決從起始點到終點的機器人導航問題。
問題

步驟

地圖預處理

題中尋路地圖包含在很多文字之中，且圖中還包含logo，這都直接影響了尋路演算法的使用，因此需要將圖片轉程式易處理的資料結構。預處理後地圖如下：
預處理

演算法思想

A*演算法為了在獲得最短路徑的前提下搜尋最少節點，通過不斷計算當前節點的附近節點F(N)值來判斷下次探索的方向，每個節點的值計算方法為：F(N)=G(N)+H(N)。

其中G(N)是從起點到當前節點N的移動消耗（比如低消耗代表平地、高消耗代表沙漠）；H(N)代表當前節點到目標節點的預期距離，可以用使用曼哈頓距離、歐氏距離等。當節點間移動消耗G(N)非常小時，G(N)對F(N)的影響也會微乎其微，A*演算法就退化為最好優先貪婪演算法；當節點間移動消耗G(N)非常大以至於H(N)對F(N)的影響微乎其微時，A*演算法就退化為Dijkstra演算法。

演算法步驟

整個演算法流程為[4]：

  1. 設定兩個集合：open集、close集
  2. 將起始點加入open集，其F值為0（設定父親節點為空）
  3. 當open集合非空，則執行以下迴圈
    3.1 在open集中移出一個F值最小的節點作為當前節點，並將其加入close集
    3.2 如果當前節點為終點，則退出迴圈完成任務
    3.3 處理當前節點的所有鄰居節點，對於每個節點進行以下操作：
    - 如果該節點不可達或在close集中則忽略該節點
    - 計算該節點的F(N)值，並：如果該節點在open集中且F(N)大於當前F(N)，則選擇較小F(N)替換；否則將該節點加入open集
    - 將該節點的父節點設定為當前節點
    - 將該節點加入open集
  4. 搜尋結束如果open集為空，則可能搜尋到一條路徑；如果open集非空，則必然搜尋到一條路徑，從終點不斷遍歷其父節點便是搜尋路徑。

程式碼實現

使用Python編寫A*演算法的核心程式碼為：

class AStar(object):
    '''
    @param      {*} graph   地圖
    @param      {*} start   開始節點
    @param      {*} goal    終點
    '''
    def __init__(self, graph, start, goal):
        self.start = start
        self.goal = goal
        self.graph = graph
        # 優先佇列儲存open集
        self.frontier = PriorityQueue()
        # 初始化起點
        self.frontier.put(start)

    '''
    @description: 繪出最終路徑
    '''

    def draw_path(self):
        path = self.goal
        matrix = self.graph.matrix
        while path:
            matrix[path.x][path.y] = 0
            path = path.father

    def run(self):
        plt.ion()
        n = 0
        while not self.frontier.empty():
            n = n + 1
            current = self.frontier.get()
            # 是否為終點
            if current.equal(self.goal):
                self.goal.father = current
                self.draw_path()
                return True
            # 遍歷鄰居節點
            for next in self.graph.neighbors(current):
                # 計算移動消耗G
                next.g = current.g + self.graph.cost(current, next)
                # 計算曼哈頓距離H
                next.manhattan(self.goal)
                # 如果當前節點未在open集中
                if not next.used:
                    next.used = True
                    # 將探索過的節點設為陰影，便於觀察
                    self.graph.matrix[next.x][next.y] = 99
                    # 將當前節點加入open集
                    self.frontier.put(next)
                    # 設定該節點的父節點為當前節點
                    next.father = current
            # 沒100次更新一次影像
            if n % 100 == 0:
                plt.clf()
                plt.imshow(self.graph.matrix)
                plt.pause(0.01)
                plt.show()
        return False

尋路結果如下（其中黑實線是演算法得出的最優路徑，路徑旁邊的黑色地帶是探索過的節點）：
結果

思考

本例中影像共有406×220畫素，即有89320個畫素點，也就是狀態空間共有89320，可選線路最高有893202約為80億種，雖然經過了簡單的地圖優化處理，但直接使用A*演算法的效率還是很低。要想進一步提高搜尋效率，可以引出另外一條定理：給定平面上一個起始點s和一個目標點g，以及若干多邊形障礙物P1, P2, P3 ... Pk，由於兩點間直線最短，故在所有從s到g的路徑中，距離最短的路徑的拐點一定在多邊形頂點上。基於以上定理，我們可以人工將地圖中的多邊形頂點進行提取，再用A*演算法對提取的頂點進行計算，即可在獲得最短路徑的同時大大增加了演算法的效率。

完整程式碼

  1. 資料結構
from queue import PriorityQueue
import cv2
import math
import matplotlib.pyplot as plt
import fire


class Node(object):
    def __init__(self, x=0, y=0, v=0, g=0, h=0):
        self.x = x
        self.y = y
        self.v = v
        self.g = g  #g值
        self.h = h  #h值
        self.used = False
        self.father = None  #父節點

    '''
    @description: 曼哈頓距離
    @param      {*} endNode 目標節點
    '''

    def manhattan(self, endNode):
        self.h = (abs(endNode.x - self.x) + abs(endNode.y - self.y)) * 10

    '''
    @description: 尤拉距離
    @param      {*} self
    @param      {*} endNode
    @return     {*}
    '''

    def euclidean(self, endNode):
        self.h = int(math.sqrt(abs(endNode.x - self.x)**2 + abs(endNode.y - self.y)**2)) * 30

    '''
    @description: 判斷other節點與當前節點是否相等
    @param      {*} other
    '''

    def equal(self, other):
        if self.x == other.x and self.y == other.y:
            return True
        else:
            return False

    '''
    @description: 函式過載，為了滿足PriorityQueue進行排序
    @param      {*} other
    '''

    def __lt__(self, other):
        if self.h + self.g <= other.h + other.g:
            return True
        else:
            return False


class Graph(object):
    '''
    @description: 類初始化
    @param      {*} matrix  地圖矩陣
    @param      {*} maxW    地圖寬
    @param      {*} maxH    地圖高
    '''
    def __init__(self, matrix, maxW, maxH):
        self.matrix = matrix
        self.maxW = maxW
        self.maxH = maxH
        self.nodes = []
        # 普通二維矩陣轉一維座標矩陣
        for i in range(maxH):
            for j in range(maxW):
                self.nodes.append(Node(i, j, self.matrix[i][j]))

    '''
    @description: 檢查座標是否合法
    @param      {*} x
    @param      {*} y
    '''

    def checkPosition(self, x, y):
        return x > 0 and x < self.maxH and y > 0 and y < self.maxW and self.nodes[x * self.maxW + y].v > 200

    '''
    @description: 尋找當前節點的鄰居節點
    @param      {Node} node 當前節點
    @return     {*}
    '''

    def neighbors(self, node: Node):
        ng = []
        if self.checkPosition(node.x - 1, node.y):
            ng.append(self.nodes[(node.x - 1) * self.maxW + node.y])
        if self.checkPosition(node.x + 1, node.y):
            ng.append(self.nodes[(node.x + 1) * self.maxW + node.y])
        if self.checkPosition(node.x, node.y - 1):
            ng.append(self.nodes[node.x * self.maxW + node.y - 1])
        if self.checkPosition(node.x, node.y + 1):
            ng.append(self.nodes[node.x * self.maxW + node.y + 1])

        if self.checkPosition(node.x + 1, node.y + 1):
            ng.append(self.nodes[(node.x + 1) * self.maxW + node.y + 1])
        if self.checkPosition(node.x + 1, node.y - 1):
            ng.append(self.nodes[(node.x + 1) * self.maxW + node.y - 1])
        if self.checkPosition(node.x - 1, node.y + 1):
            ng.append(self.nodes[(node.x - 1) * self.maxW + node.y + 1])
        if self.checkPosition(node.x - 1, node.y - 1):
            ng.append(self.nodes[(node.x - 1) * self.maxW + node.y - 1])
        return ng

    '''
    @description: 畫出結果路徑
    '''

    def draw(self):
        cv2.imshow('result', self.matrix)
        cv2.waitKey(0)
        cv2.destroyAllWindows()

    '''
    @description: 計算節點間移動消耗
    @param      {Node} current
    @param      {Node} next
    @return     {*}
    '''

    def cost(self, current: Node, next: Node):
        return 11 if abs(current.x - next.x) + abs(current.y - next.y) > 1 else 10


class AStar(object):
    '''
    @param      {*} graph   地圖\n
    @param      {*} start   開始節點
    @param      {*} goal    終點
    '''
    def __init__(self, graph, start, goal):
        self.start = start
        self.goal = goal
        self.graph = graph
        # 優先佇列儲存open集
        self.frontier = PriorityQueue()
        # 初始化起點
        self.frontier.put(start)

    '''
    @description: 繪出最終路徑
    '''

    def draw_path(self):
        path = self.goal
        matrix = self.graph.matrix
        while path:
            matrix[path.x][path.y] = 0
            path = path.father

    def run(self):
        plt.ion()
        n = 0
        while not self.frontier.empty():
            n = n + 1
            current = self.frontier.get()
            # 是否為終點
            if current.equal(self.goal):
                self.goal.father = current
                self.draw_path()
                return True
            # 遍歷鄰居節點
            for next in self.graph.neighbors(current):
                # 計算移動消耗G
                next.g = current.g + self.graph.cost(current, next)
                # 計算曼哈頓距離H
                next.manhattan(self.goal)
                # 如果當前節點未在open集中
                if not next.used:
                    next.used = True
                    # 將探索過的節點設為陰影，便於觀察
                    self.graph.matrix[next.x][next.y] = 99
                    # 將當前節點加入open集
                    self.frontier.put(next)
                    # 設定該節點的父節點為當前節點
                    next.father = current
            # 沒100次更新一次影像
            if n % 100 == 0:
                plt.clf()
                plt.imshow(self.graph.matrix)
                plt.pause(0.01)
                plt.show()
        return False
  1. 主程式

import cv2
from AStar import Node, AStar, Graph

src_path = "./map.png"
# 讀取圖片
img_grey = cv2.imread(src_path, cv2.IMREAD_GRAYSCALE)
# 去除水印
img_grey = cv2.threshold(img_grey, 200, 255, cv2.THRESH_BINARY)[1]
# 二值化
img_binary = cv2.threshold(img_grey, 128, 255, cv2.THRESH_BINARY)[1]
img_binary[:][0] = 0
img_binary[:][-1] = 0
img_binary[0][:] = 0
img_binary[-1][:] = 0

start = Node(180, 30)
goal = Node(20, 370)
maxH, maxW = img_binary.shape
graph = Graph(img_binary, maxW, maxH) 
astar = AStar(graph, start, goal)
astar.run()

cv2.imshow('result', graph.matrix)
cv2.waitKey(0)
cv2.destroyAllWindows()

參考文獻

  1. 好好學習天天引體向上. 尋路演算法小結. 簡書. [2017-01-09] ↩︎

  2. wier. 深入理解遊戲中尋路演算法. OSChina. [2017-07-25] ↩︎

  3. 雲加社群. 最快速的尋路演算法 Jump Point Search. InfoQ. [2020-11-29] ↩︎

  4. Amit Patel. Introduction to the A* Algorithm. redblobgames.com. [2014-05-26] ↩︎

相關文章