A*三维路径规划python

以下是一个简单的A*三维路径规划python实现的示例代码：

import heapq
import math

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

    def f(self):
        return self.g + self.h

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

def get_neighbors(node, step_size):
    neighbors = []
    for dx in [-step_size, 0, step_size]:
        for dy in [-step_size, 0, step_size]:
            for dz in [-step_size, 0, step_size]:
                if dx == dy == dz == 0:
                    continue
                neighbor = Node(node.x + dx, node.y + dy, node.z + dz)
                neighbors.append(neighbor)
    return neighbors

def heuristic(node, goal):
    return math.sqrt((node.x - goal.x) ** 2 + (node.y - goal.y) ** 2 + (node.z - goal.z) ** 2)

def a_star(start, goal, step_size, obstacles):
    open_set = []
    closed_set = set()
    heapq.heappush(open_set, start)

    while open_set:
        current = heapq.heappop(open_set)
        if current == goal:
            path = []
            while current:
                path.append(current)
                current = current.parent
            return path[::-1]

        closed_set.add(current)
        for neighbor in get_neighbors(current, step_size):
            if not (0 <= neighbor.x < 100 and 0 <= neighbor.y < 100 and 0 <= neighbor.z < 100):
                continue
            if neighbor in obstacles:
                continue
            if neighbor in closed_set:
                continue
            tentative_g = current.g + heuristic(current, neighbor)
            if neighbor not in open_set or tentative_g < neighbor.g:
                neighbor.g = tentative_g
                neighbor.h = heuristic(neighbor, goal)
                neighbor.parent = current
                if neighbor not in open_set:
                    heapq.heappush(open_set, neighbor)

    return None

start = Node(0, 0, 0)
goal = Node(99, 99, 99)
step_size = 1
obstacles = [Node(50, 50, 50), Node(51, 50, 50), Node(50, 51, 50), Node(50, 50, 51)]

path = a_star(start, goal, step_size, obstacles)
if path is None:
    print("No path found")
else:
    for node in path:
        print(node.x, node.y, node.z)

该示例代码实现了一个A*算法的函数，该函数接受起始点、目标点、步长和障碍物列表作为输入，并返回一条从起始点到目标点的路径。在示例中，障碍物被硬编码为一个节点列表。函数中的get_neighbors函数计算当前节点的所有可行邻居节点。heuristic函数计算两个节点之间的启发式距离。最后，函数使用堆来实现open set的排序，以便更高效地搜索路径。