A算法python实现

A* 算法是一种常用的寻路算法，它基于启发式函数，能够在地图上高效地找到两点之间的最短路径。下面是一个 Python 实现：

from queue import PriorityQueue

# 定义启发式函数
def heuristic(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

# 定义 A* 算法
def astar(array, start, goal):
    neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]

    close_set = set()
    came_from = {}
    gscore = {start:0}
    fscore = {start:heuristic(start, goal)}
    oheap = []

    # 将起点加入堆中
    PriorityQueue.heappush(oheap, (fscore[start], start))
    
    while oheap:

        current = PriorityQueue.heappop(oheap)[1]

        if current == goal:
            data = []
            while current in came_from:
                data.append(current)
                current = came_from[current]
            return data[::-1]

        close_set.add(current)

        for i, j in neighbors:
            neighbor = current[0] + i, current[1] + j
            tentative_g_score = gscore[current] + heuristic(current, neighbor)

            # 如果邻居节点已经在 close set 中并且 tentative_g_score 大于等于 gscore[neighbor]，则跳过
            if 0 <= neighbor[0] < array.shape[0]:
                if 0 <= neighbor[1] < array.shape[1]:                
                    if array[neighbor[0]][neighbor[1]] == 1:
                        continue
                else:
                    # 该邻居节点在地图的边界之外，跳过
                    continue
            else:
                # 该邻居节点在地图的边界之外，跳过
                continue 

            if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0):
                continue

            # 如果 tentative_g_score 小于 fscore[neighbor] 或者 neighbor 不在 open set 中，则更新 fscore 和 gscore
            if  tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]:
                came_from[neighbor] = current
                gscore[neighbor] = tentative_g_score
                fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal)
                PriorityQueue.heappush(oheap, (fscore[neighbor], neighbor))
                
    return False

其中，`array` 是一个二维数组，1 表示障碍物，0 表示可通行；`start` 和 `goal` 分别为起点和终点。

该函数返回的是从起点到终点的最短路径，如果不存在则返回 False。