A星寻路算法

A星寻路算法

from queue import PriorityQueue

# 定义二维网格
grid = [
    [0, 1, 0, 0, 0],
    [0, 1, 0, 1, 0],
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 1, 0, 0, 0]
]

# 定义起点和终点
start = (1, 1)
goal = (5, 5)


# 定义A*算法的启发式函数
def heuristic(a, b):
    # 曼哈顿距离作为启发式函数
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


# 获取邻居节点
# 修改邻居节点函数
# 定义起点和终点

# 修改邻居节点函数
def neighbors(grid, current):
    x, y = current
    results = []
    for dx, dy in [(1, 0), (-1, 0), (0, 1), (0, -1)]:  # 右、左、上、下四个方向
        nx, ny = x + dx, y + dy
        if 1 <= nx <= 5 and 1 <= ny <= 5 and grid[ny-1][nx-1] == 0:  # 注意坐标转换
            results.append((nx, ny))
    return results



# 计算移动成本
def cost(current, next):
    return 1  # 假设所有移动的成本都是1


# 实现A*搜索算法
def a_star_search(grid, start, goal):  # 图，开始，目标
    frontier = PriorityQueue()  # 存在这一轮探测过的所有边界方块，它是第一个优先队列
    frontier.put((0, start)) # 存放一个起点
    came_from = {} # 路径来向
    cost_so_far = {} # 当前代价
    came_from[start] = None   # 起点置空
    cost_so_far[start] = 0  # 起点代价设置为0

    # 当优先队列不为空时，循环执行搜索过程
    while not frontier.empty():
        _, current = frontier.get() # 从优先队列中获取当前代价最小的节点

        if current == goal:   # 如果当前节点就是目标节点，则搜索结束
            break

        # 遍历当前节点的所有邻居节点
        for next in neighbors(grid, current):

            # 计算从起点经当前节点到邻居节点的新代价
            new_cost = cost_so_far[current] + cost(current, next)

            # 如果邻居节点不在代价表中，或者新代价更小，则更新代价表
            if next not in cost_so_far or new_cost < cost_so_far[next]:
                cost_so_far[next] = new_cost

                # 计算邻居节点的优先级，以便将其加入优先队列
                priority = new_cost + heuristic(goal, next)
                frontier.put((priority, next))

                # 更新邻居节点的父节点为当前节点
                came_from[next] = current

    # 返回最终的父节点表和代价表，用于重构最短路径
    return came_from, cost_so_far


# 调用A*算法并获取路径
came_from, cost_so_far = a_star_search(grid, start, goal)

# 从终点回溯找到路径
current = goal
path = []
while current != start:
    path.append(current)
    current = came_from[current]
path.append(start)
path.reverse()

# 打印路径
print("找到的路径：", path)