【算法 python A*算法的实现】

 - 算法实现：

import heapq


class Node:
    def __init__(self, position, g=0, h=0):
        self.position = position  # 节点的位置
        self.g = g  # 从起点到当前节点的成本
        self.h = h  # 从当前节点到终点的启发式估计成本
        self.f = g + h  # 总成本
        self.parent = None  # 父节点

    def __lt__(self, other):
        return self.f < other.f


def heuristic(a, b):
    # 使用曼哈顿距离作为启发式函数
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


def a_star(start, goal, grid):
    open_list = []
    closed_list = set()

    start_node = Node(start, 0, heuristic(start, goal))
    goal_node = Node(goal)

    heapq.heappush(open_list, start_node)

    while open_list:
        current_node = heapq.heappop(open_list)

        if current_node.position == goal_node.position:
            path = []
            while current_node:
                path.append(current_node.position)
                current_node = current_node.parent
            return path[::-1]  # 返回反转后的路径

        closed_list.add(current_node.position)

        # 获取相邻节点
        neighbors = [(0, 1), (1, 0), (0, -1), (-1, 0)]  # 右，下，左，上
        for new_position in neighbors:
            node_position = (
                current_node.position[0] + new_position[0],
                current_node.position[1] + new_position[1],
            )

            # 检查节点是否在网格内
            if (0 <= node_position[0] < len(grid)) and (
                0 <= node_position[1] < len(grid[0])
            ):
                # 检查节点是否是障碍物
                if grid[node_position[0]][node_position[1]] == 1:
                    continue

                # 创建新的节点
                neighbor_node = Node(node_position)
                if neighbor_node.position in closed_list:
                    continue

                # 计算 g, h, f 值
                neighbor_node.g = current_node.g + 1
                neighbor_node.h = heuristic(neighbor_node.position, goal_node.position)
                neighbor_node.f = neighbor_node.g + neighbor_node.h
                neighbor_node.parent = current_node

                # 检查节点是否在开放列表中
                if add_to_open(open_list, neighbor_node):
                    heapq.heappush(open_list, neighbor_node)

    return None  # 如果没有找到路径


def add_to_open(open_list, neighbor):
    for node in open_list:
        if neighbor.position == node.position and neighbor.g >= node.g:
            return False
    return True

 - 测试：

# 示例使用

import numpy as np
grid = np.array(
    [
        [0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0],
        [0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1],
        [0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0],
        [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0],
        [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1],
        [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1],
        [0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0],
        [0, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1],
        [0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1],
        [0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1],
        [0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1],
        [0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0],
        [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0],
        [0, 1, 1, 0, 1, 0, 1, 1, 0, 1, 0, 1, 0, 0, 1, 0],
    ]
)

start = (0, 0)  # 起点
goal = (15, 15)  # 终点

path = a_star(start, goal, grid)
print("找到的路径:", path)

找到的路径: [(0, 0), (1, 0), (2, 0), (3, 0), (4, 0), (4, 1), (4, 2), (3, 2), (2, 2), (1, 2), (0, 2), (0, 3), (0, 4), (1, 4), (2, 4), (2, 5), (2, 6), (2, 7), (2, 8), (1, 8), (0, 8), (0, 9), (0, 10), (0, 11), (0, 12), (0, 13), (0, 14), (1, 14), (2, 14), (3, 14), (4, 14), (4, 13), (4, 12), (4, 11), (4, 10), (4, 9), (4, 8), (4, 7), (4, 6), (5, 6), (6, 6), (6, 5), (6, 4), (7, 4), (8, 4), (8, 3), (8, 2), (9, 2), (10, 2), (11, 2), (12, 2), (12, 3), (12, 4), (12, 5), (12, 6), (11, 6), (10, 6), (9, 6), (8, 6), (8, 7), (8, 8), (7, 8), (6, 8), (6, 9), (6, 10), (6, 11), (6, 12), (7, 12), (8, 12), (8, 11), (8, 10), (9, 10), (10, 10), (10, 9), (10, 8), (11, 8), (12, 8), (12, 9), (12, 10), (13, 10), (14, 10), (14, 11), (14, 12), (13, 12), (12, 12), (11, 12), (10, 12), (10, 13), (10, 14), (11, 14), (12, 14), (13, 14), (13, 15), (14, 15), (15, 15)]

 - 可视化

def visualize_path(maze, path):
    fig, axis = plt.subplots(1, 2)
    mask = maze == 1
    maze[mask] = 0
    maze[~mask] = 1
    axis[0].imshow(maze, cmap="gray")
    axis[0].set_xticks([])
    axis[0].set_yticks([])
    axis[0].set_title("map")

    for position in path:
        maze[position[0]][position[1]] = 2
    axis[1].imshow(maze, cmap="gray")
    axis[1].set_xticks([])
    axis[1].set_yticks([])
    axis[1].set_title("shortest path")
    plt.show()


visualize_path(grid.copy(), path)

 

原文地址：https://blog.csdn.net/qq_39749966/article/details/144087224

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！