python实现路径规划算法实战

import heapq

class Node:
    def __init__(self, position, parent=None):
        self.position = position
        self.parent = parent
        self.g = 0  # 起点到当前节点的实际代价
        self.h = 0  # 当前节点到目标节点的预估代价
        self.f = 0  # f = g + h

    def __eq__(self, other):
        return self.position == other.position

def astar_search(start, end, grid):
    open_list = []  # 待探索的节点
    closed_list = set()  # 已探索的节点

    start_node = Node(start)
    end_node = Node(end)

    heapq.heappush(open_list, (0, start_node))

    while open_list:
        current_node = heapq.heappop(open_list)[1]
        closed_list.add(current_node)

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

        children = []
        for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0)]:  # 遍历当前节点的相邻节点
            node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1])

            # 确保节点在地图内
            if node_position[0] > (len(grid) - 1) or node_position[0] < 0 or \
               node_position[1] > (len(grid[len(grid)-1]) -1) or node_position[1] < 0:
                continue

            # 确保节点不是障碍物
            if grid[node_position[0]][node_position[1]] == 1:
                continue

            new_node = Node(node_position, current_node)
            children.append(new_node)

        for child in children:
            if child in closed_list:
                continue

            # 计算g、h、f值
            child.g = current_node.g + 1
            child.h = ((child.position[0] - end_node.position[0]) ** 2) + \
                      ((child.position[1] - end_node.position[1]) ** 2)
            child.f = child.g + child.h

            # 如果该节点已经在open_list中，且新的g值更小，则更新该节点的信息
            for open_node in open_list:
                if child == open_node[1] and child.g > open_node[1].g:
                    continue

            heapq.heappush(open_list, (child.f, child))

    return None

# 示例地图
grid = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 0, 0]
]

start = (0, 0)  # 起点
end = (4, 4)  # 终点

path = astar_search(start, end, grid)
if path:
    print("路径：", path)
else:
    print("未找到路径")