堆優化的A*算法-Python實現

堆優化的A*算法-Python實現

原理參考博客地址
代碼借鑑地址
A*算法解決二維網格地圖中的尋路問題

• 輸入：圖片（白色區域代表可行，深色區域代表不行可行）
• 輸出：路徑（在圖中繪製）

""" 方格地圖中的A*算法 (openList進行了堆優化)
A* 算法：  F = G+H
F: 總移動代價
G: 起點到當前點的移動代價  直:1, 斜:1.4
H: 當前點到終點的預估代價  曼哈頓距離
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1.把起點加入 openList中
2.While True:
    a.遍歷openList，查找F值最小的節點，作爲current
    b.current是終點:
        ========結束========
    c.從openList中彈出，放入closeList中
    d.對八個方位的格點:
        if 越界 or 是障礙物 or 在closeList中:
            continue
        if 不在openList中：
            設置父節點,F,G,H
            加入openList中
        else:
            if 這條路徑更好:
                設置父節點,F,G
                更新openList中的對應節點
3.生成路徑path
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
堆優化:
    openList：作爲最小堆，按F值排序存儲座標 （不更新只增加）
    openDict：座標:點詳細信息 （既更新又增加）
    get_minfNode() 從openList中彈出座標，去openDict中取點 （但由於不更新只增加，座標可能冗餘）
    in_openList() 判斷座標是否在openDict中即可 

"""
import math
from PIL import Image,ImageDraw 
import numpy as np
import heapq # 堆

STAT_OBSTACLE='#'
STAT_NORMAL='.'

class Node():
    """
    節點元素，parent用來在成功的時候回溯路徑
    """
    def __init__(self, x, y,parent=None, g=0, h=0):
        self.parent = parent
        self.x = x
        self.y = y
        self.g = g
        self.h = h
        self.update()
    
    def update(self):
        self.f = self.g+self.h

class RoadMap():
    """ 讀進一張圖片，二值化成爲有障礙物的二維網格化地圖，並提供相關操作
    """
    def __init__(self,img_file):
        """圖片變二維數組"""
        test_map = []
        img = Image.open(img_file)
#        img = img.resize((100,100))  ### resize圖片尺寸
        img_gray = img.convert('L')  # 地圖灰度化
        img_arr = np.array(img_gray)
        img_binary = np.where(img_arr<127,0,255)
        for x in range(img_binary.shape[0]):
            temp_row = []
            for y in range(img_binary.shape[1]):
                status = STAT_OBSTACLE if img_binary[x,y]==0 else STAT_NORMAL 
                temp_row.append(status)
            test_map.append(temp_row)
            
        self.map = test_map
        self.cols = len(self.map[0])
        self.rows = len(self.map)
        
    def is_valid_xy(self, x,y):
        if x < 0 or x >= self.rows or y < 0 or y >= self.cols:
            return False
        return True

    def not_obstacle(self,x,y):
        return self.map[x][y] != STAT_OBSTACLE
    
    def EuclidenDistance(self, xy1, xy2):
        """兩個像素點之間的歐幾里得距離"""
        dis = 0
        for (x1, x2) in zip(xy1, xy2):
            dis += (x1 - x2)**2
        return dis**0.5

    def ManhattanDistance(self,xy1,xy2):
        """兩個像素點之間的曼哈頓距離"""
        dis = 0
        for x1,x2 in zip(xy1,xy2):
            dis+=abs(x1-x2)
        return dis

    def check_path(self, xy1, xy2):
        """碰撞檢測 兩點之間的連線是否經過障礙物"""
        steps = max(abs(xy1[0]-xy2[0]), abs(xy1[1]-xy2[1])) # 取橫向、縱向較大值，確保經過的每個像素都被檢測到
        xs = np.linspace(xy1[0],xy2[0],steps+1)
        ys = np.linspace(xy1[1],xy2[1],steps+1)
        for i in range(1, steps): # 第一個節點和最後一個節點是 xy1，xy2，無需檢查
            if not self.not_obstacle(math.ceil(xs[i]), math.ceil(ys[i])):
                return False
        return True

    def plot(self,path):
        """繪製地圖及路徑"""
        out = []
        for x in range(self.rows):
            temp = []
            for y in range(self.cols):
                if self.map[x][y]==STAT_OBSTACLE:
                    temp.append(0)
                elif self.map[x][y]==STAT_NORMAL:
                    temp.append(255)
                elif self.map[x][y]=='*':
                    temp.append(127)
                else:
                    temp.append(255)
            out.append(temp)
        for x,y in path:
            out[x][y] = 127
        out = np.array(out)
        img = Image.fromarray(out)
        img.show()

class A_Star(RoadMap):
    """ x是行索引，y是列索引
    """
    def __init__(self, img_file, start=None, end=None):
        """地圖文件，起點，終點"""
        RoadMap.__init__(self,img_file)
        self.startXY = tuple(start) if start else (0,0)
        self.endXY = tuple(end) if end else (self.rows-1, self.cols-1)
        self.closeList = set()
        self.path = []
        self.openList = []  # 堆，只添加，和彈出最小值點，
        self.openDict = dict() # openList中的 座標:詳細信息 -->不冗餘的
        
    def find_path(self):
        """A*算法尋路主程序"""
        p = Node(self.startXY[0], self.startXY[1], 
                 h=self.ManhattanDistance(self.startXY, self.endXY)) # 構建開始節點
        heapq.heappush(self.openList, (p.f,(p.x,p.y)))
        
        self.openDict[(p.x,p.y)] = p  # 加進dict目錄
        while True:
            current = self.get_minfNode()
            if (current.x,current.y)==self.endXY:
                print('found path successfully..')
                self.make_path(current)
                return 
            
            self.closeList.add((current.x,current.y))  ## 加入closeList
            del self.openDict[(current.x,current.y)]
            self.extend_surrounds(current) # 會更新close list

    def make_path(self,p):
        """從結束點回溯到開始點，開始點的parent==None"""
        while p:
            self.path.append((p.x, p.y))
            p = p.parent
    
    def extend_surrounds(self, node):
        """ 將當前點周圍可走的點加到openList中，
            其中 不在openList中的點 設置parent、F,G,H 加進去，
                 在openList中的點  更新parent、F,G,H
            (斜向時，正向存在障礙物時不允許穿越)
        """
        motion_direction = [[1, 0], [0,  1], [-1, 0], [0,  -1], 
                            [1, 1], [1, -1], [-1, 1], [-1, -1]]  
        for dx, dy in motion_direction:
            x,y = node.x+dx, node.y+dy
            new_node = Node(x,y)
            # 位置無效，或者是障礙物, 或者已經在closeList中 
            if not self.is_valid_xy(x,y) or not self.not_obstacle(x,y) or self.in_closeList(new_node): 
                continue
            if abs(dx)+abs(dy)==2:  ## 斜向 需檢查正向有無障礙物
                h_x,h_y = node.x+dx,node.y # 水平向
                v_x,v_y = node.x,node.y+dy # 垂直向
                if not self.is_valid_xy(h_x,h_y) or not self.not_obstacle(h_x,h_y) or self.in_closeList(Node(h_x,h_y)): 
                    continue
                if not self.is_valid_xy(v_x,v_y) or not self.not_obstacle(v_x,v_y) or self.in_closeList(Node(v_x,v_y)): 
                    continue
            #============ ** 關鍵 **             ========================
            #============ 不在openList中，加進去； ========================
            #============ 在openList中，更新      ========================
            #============對於openList和openDict來說，操作都一樣 ===========
            new_g = node.g + self.cal_deltaG(node.x,node.y, x,y)
            sign=False # 是否執行操作的標誌 
            if not self.in_openList(new_node): # 不在openList中
                # 加進來，設置 父節點, F, G, H
                new_node.h = self.cal_H(new_node)
                sign=True
            elif self.openDict[(new_node.x,new_node.y)].g > new_g: # 已在openList中，但現在的路徑更好
                sign=True
            if sign:
                new_node.parent = node
                new_node.g = new_g
                new_node.f = self.cal_F(new_node)
                self.openDict[(new_node.x,new_node.y)]=new_node # 更新dict目錄
                heapq.heappush(self.openList, (new_node.f,(new_node.x,new_node.y)))
        
    def get_minfNode(self):
        """從openList中取F=G+H值最小的 (堆-O(1))"""
        while True:
            f, best_xy=heapq.heappop(self.openList)
            if best_xy in self.openDict:
                return self.openDict[best_xy]

    def in_closeList(self, node):
        """判斷是否在closeList中 (集合-O(1)) """
        return True if (node.x,node.y) in self.closeList else False
     
    def in_openList(self, node):
        """判斷是否在openList中 (字典-O(1))"""
        if not (node.x,node.y) in self.openDict:
            return False
        else:
            return True

    def cal_deltaG(self,x1,y1,x2,y2):
        """ 計算兩點之間行走的代價
            （爲簡化計算）上下左右直走，代價爲1.0，斜走，代價爲1.4  G值
        """
        if x1 == x2 or y1 == y2:
            return 1.0
        return 1.4
    
    def cal_H(self, node):
        """ 曼哈頓距離 估計距離目標點的距離"""
        return abs(node.x-self.endXY[0])+abs(node.y-self.endXY[1]) # 剩餘路徑的估計長度
    
    def cal_F(self, node):
        """ 計算F值 F = G+H 
            A*算法的精髓：已經消耗的代價G，和預估將要消耗的代價H
        """
        return node.g + node.h


def path_length(path):
    """計算路徑長度"""
    l = 0
    for i in range(len(path)-1):
        x1,y1 = path[i]
        x2,y2 = path[i+1]
        if x1 == x2 or y1 == y2:
            l+=1.0
        else:
            l+=1.4
    return l


# ===== test case ===============
a = A_Star('map_1.bmp')
a.find_path()
a.plot(a.path)
print('path length:',path_length(a.path))

測試用例及結果

存在的問題

不確定是否是最優路徑
原文描述：
“ If we overestimate this distance, however, it is not guaranteed to give us the shortest path. In such cases, we have what is called an “inadmissible heuristic.”.

Technically, in this example, the Manhattan method is inadmissible because it slightly overestimates the remaining distance.”
即如果我們高估了H，則不能保證最短路徑。而曼哈頓距離略微高估了。

另外，筆者不確定程序是不是正確，以及是不是真正的A*算法，請大神們指正。