写一个无人机轨迹规划A*算法Python代码

以下是一个简单的无人机轨迹规划A*算法Python代码实现，其中包含起点、终点、地图、障碍物等相关信息。该代码使用了Python的heapq模块来实现优先队列。

import heapq

# 定义节点类
class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.g = 0
        self.h = 0
        self.f = 0
        self.parent = None

    # 重写比较运算符
    def __lt__(self, other):
        return self.f < other.f

# 定义A*算法函数
def astar(start, end, obstacle_map):
    # 定义开放列表、关闭列表和起点、终点节点
    open_list = []
    closed_list = []
    start_node = Node(start[0], start[1])
    end_node = Node(end[0], end[1])

    # 将起点节点加入开放列表
    heapq.heappush(open_list, start_node)

    # 循环直到找到终点或开放列表为空
    while open_list:
        # 从开放列表中取出f值最小的节点
        current_node = heapq.heappop(open_list)

        # 如果找到了终点，则返回路径
        if current_node.x == end_node.x and current_node.y == end_node.y:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return path[::-1]

        # 将当前节点加入关闭列表
        closed_list.append(current_node)

        # 获取当前节点周围的可行节点
        for x_offset in range(-1, 2):
            for y_offset in range(-1, 2):
                if x_offset == 0 and y_offset == 0:
                    continue
                x = current_node.x + x_offset
                y = current_node.y + y_offset
                if x < 0 or x >= len(obstacle_map) or y < 0 or y >= len(obstacle_map[0]) or obstacle_map[x][y] == 1:
                    continue
                neighbor = Node(x, y)
                neighbor.g = current_node.g + 1
                neighbor.h = abs(x - end_node.x) + abs(y - end_node.y)
                neighbor.f = neighbor.g + neighbor.h

                # 如果新节点已经在关闭列表中，则跳过
                if neighbor in closed_list:
                    continue

                # 如果新节点不在开放列表中，则将其加入开放列表
                if neighbor not in open_list:
                    neighbor.parent = current_node
                    heapq.heappush(open_list, neighbor)

                # 如果新节点已经在开放列表中，则更新其g值和父节点
                else:
                    if neighbor.g < current_node.g:
                        neighbor.parent = current_node
                        current_node = neighbor

    # 如果开放列表为空，则表示没有找到路径
    return None

# 测试代码
if __name__ == '__main__':
    obstacle_map = [[0, 0, 0, 0],
                    [0, 1, 1, 0],
                    [0, 0, 0, 0],
                    [0, 1, 0, 0]]
    start = (0, 0)
    end = (3, 3)
    path = astar(start, end, obstacle_map)
    print(path)

在上面的代码中，我们首先定义了一个节点类Node，其中包含了节点的坐标、g值、h值、f值和父节点信息。然后定义了A*算法函数astar，其中包含了开放列表、关闭列表、起点节点、终点节点等相关信息。在函数中，我们首先将起点节点加入开放列表，然后循环直到找到终点或开放列表为空。在循环中，我们使用heapq模块来实现优先队列，每次取出f值最小的节点进行扩展。对于当前节点周围的可行节点，我们计算其g值、h值、f值和父节点，并将其加入开放列表。如果新节点已经在开放列表中，则更新其g值和父节点。如果新节点已经在关闭列表中，则跳过。如果开放列表为空，则表示没有找到路径，返回None。最后，我们通过测试代码来验证astar函数的正确性。