A*算法python实现

import numpy as np

from numpy.linalg import norm
from heapq import heappush,heappop
from PIL import Image
import matplotlib.pyplot as plt

def MyNorm(a, b, order):

a = np.array(a)
b = np.array(b)
return norm(a - b, order)

def ReconstructPath(OldDict, BestNode):

path = [BestNode]
while BestNode in OldDict:
    BestNode = OldDict[BestNode]
    path.append(BestNode)
return path

def astar(array, start, goal):

directions = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)] # 8个方向

ClosedList = set()
OldDict = {}
gscore = {start:0}                 # 定义一个字典，start是key
fscore = {start:MyNorm(start, goal, 1)} # 定义f字典

OpenList = []
heappush(OpenList, (fscore[start], start))   # 将(fscore[start], start)放入最小优先级队列，按fscore[start]排序，start是真正存储元素

# while OpenList is not empty
while OpenList:  
    # 选择一个 OPEN 表没有设置的 f 值最小的节点 BESTNODE，移到 CLOSED 表
    BestNode = heappop(OpenList)[1]    

    if BestNode == goal:
        return ReconstructPath(OldDict, BestNode)

    ClosedList.add(BestNode)

    for i, j in directions:      # 对当前节点的 8 个后继节点一一进行检查
        Successor = BestNode[0] + i, BestNode[1] + j     

        ## 判断节点是否在地图范围内，并判断是否为障碍物
        if 0 <= Successor[0] < array.shape[0]:
            if 0 <= Successor[1] < array.shape[1]:                
                if array[Successor[0]][Successor[1]] == 1:   # 1为障碍物
                    continue
            else:
                # array bound y walls
                continue
        else:
            # array bound x walls
            continue

        # Ignore the Successor which is already evaluated.
        if Successor in ClosedList: 
            continue

        #  g(SUC)=g(BES)+h(BES,SUC)，将两个节点间的欧氏距离作为启发函数
        tentative_gScore = gscore[BestNode] + MyNorm(BestNode, Successor, 2)

        if  Successor not in [i[1] for i in OpenList]:                # 若SUCCESSOR不在OPEN表或CLOSED表中，则将其填入 OPEN 表
            heappush(OpenList, (fscore.get(Successor, np.inf), Successor)) 
        elif tentative_gScore >= gscore.get(Successor, np.inf):   # This is not a better path.
            continue        

        # This path is the BEST until now.
        OldDict[Successor] = BestNode
        gscore[Successor] = tentative_gScore
        # 修正父节点的 g 值和 f 值，记录 g(OLD)，将successor到目标点的曼哈顿距离作为启发函数
        fscore[Successor] = tentative_gScore + MyNorm(Successor, goal, 1)

return False

if name == “main”:
nmap = np.array([
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,1,1,1,1,1,1,1,1,0,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,0,1,1,1,1,1,1,1,1,1,1,1,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,1,1,1,1,1,1,1,1,0,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,0,1,1,1,1,1,1,1,1,1,1,1,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,1,1,1,1,1,1,1,1,0,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0]])
path = astar(nmap, (0,0), (10,13)) # 设置起点和终点，测试图像尺寸为50x50

for i in range(len(path)):
    nmap[path[i]] = 88
print(nmap)