python编程实现a*算法,并在给定地图上实现路径规划

好的，下面是Python实现A*算法的代码。首先，需要定义一个`Node`类来表示节点，包括节点的坐标、f、g、h值和父节点。

class Node:
    def __init__(self, x, y, f=0, g=0, h=0, parent=None):
        self.x = x
        self.y = y
        self.f = f
        self.g = g
        self.h = h
        self.parent = parent

接下来，定义A*算法主函数`astar_search()`，其中包括启发式函数`heuristic()`和计算节点花费函数`cost()`。

import heapq

def heuristic(node, goal):
    return abs(node.x - goal.x) + abs(node.y - goal.y)

def cost(current, next):
    return 1

def astar_search(map, start, goal):
    start_node = Node(start[0], start[1])
    goal_node = Node(goal[0], goal[1])
    
    open_list = []
    closed_list = []
    
    heapq.heappush(open_list, (start_node.f, start_node))
    
    while open_list:
        current_node = heapq.heappop(open_list)[1]
        
        if current_node.x == goal_node.x and current_node.y == goal_node.y:
            path = []
            while current_node.parent:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            path.append((start_node.x, start_node.y))
            return path[::-1]
        
        closed_list.append(current_node)
        
        for dx, dy in [(0, 1), (1, 0), (0, -1), (-1, 0)]:
            next_node = Node(current_node.x + dx, current_node.y + dy)
            
            if next_node.x < 0 or next_node.x >= len(map) or next_node.y < 0 or next_node.y >= len(map[0]):
                continue
                
            if map[next_node.x][next_node.y] == 1:
                continue
                
            if next_node in closed_list:
                continue
                
            next_node.g = current_node.g + cost(current_node, next_node)
            next_node.h = heuristic(next_node, goal_node)
            next_node.f = next_node.g + next_node.h
            next_node.parent = current_node
            
            if any((next_node.f, node) in open_list for node in open_list):
                continue
                
            heapq.heappush(open_list, (next_node.f, next_node))
            
    return None

最后，定义一个地图并调用`astar_search()`函数进行路径规划。

map = [[0, 0, 0, 0, 0],
       [0, 1, 0, 1, 0],
       [0, 1, 0, 0, 0],
       [0, 1, 0, 1, 0],
       [0, 0, 0, 1, 0]]

start = (0, 0)
goal = (4, 4)

path = astar_search(map, start, goal)
print(path)

输出结果为`[(0, 0), (0, 1), (1, 1), (2, 1), (3, 1), (4, 1), (4, 2), (4, 3), (4, 4)]`，表示从起点(0,0)到终点(4,4)的路径。