A*算法三维路径规划

### A* 算法在三维空间中的路径规划

A* (A-star) 是一种广泛应用于二维网格环境下的最短路径搜索算法。当扩展到三维空间时，该算法同样适用并能有效解决复杂地形上的导航问题。

#### 1. 基本原理

为了适应三维场景，节点表示由(x,y)坐标变为(x,y,z)，其中z轴代表高度方向的变化。代价函数f(n)=g(n)+h(n)保持不变：

- g(n): 起始点至当前节点的实际移动成本；
- h(n): 当前节点到达目标位置的最佳估计成本(通常采用欧几里得距离或曼哈顿距离)[^3]；

import numpy as np
from queue import PriorityQueue

def heuristic(a, b):
    """Calculate Euclidean distance between points."""
    return np.sqrt((a[0]-b[2])**2)

class Node:
    def __init__(self, position=None, parent=None):
        self.position = position
        self.parent = parent
        self.g = 0
        self.h = 0
        self.f = 0
    
    def __eq__(self, other):
        return self.position == other.position
        
def astar_3d(maze, start, end):
    """
    Returns a list of tuples as a path from the given start to the given end in the given maze.
    
    :param maze: List[List[List[int]]], 3D matrix representing obstacles with value '1' and free space with '0'.
    :param start: Tuple(int,int,int), starting point coordinates.
    :param end: Tuple(int,int,int), ending point coordinates.
    """

    # Create start and end node
    start_node = Node(position=start)
    end_node = Node(position=end)

    open_list = PriorityQueue()
    closed_set = set()

    open_list.put((start_node.f, id(start_node), start_node))
    
    while not open_list.empty():
        
        current_f, _, current_node = open_list.get()

        if current_node == end_node:
            path = []
            temp = current_node
            while temp is not None:
                path.append(temp.position)
                temp = temp.parent
                
            return path[::-1]

        children = []
        for new_position in [(0,-1,0),(0,1,0),(-1,0,0),(1,0,0),(0,0,-1),(0,0,1)]:

            node_position = (
                current_node.position[0]+new_position[0],
                current_node.position[1]+new_position[1],
                current_node.position[2]+new_position[2])

            within_range_cond = [
                node_position[0]>len(maze)-1,
                node_position[0]<0,
                node_position[1]>len(maze[len(maze)-1])-1,
                node_position[1]<0,
                node_position[2]>len(maze[len(maze)-1][len(maze[len(maze)-1])-1])-1,
                node_position[2]<0]

            if any(within_range_cond):
                continue
            
            if maze[node_position[0]][node_position[1]][node_position[2]] != 0:
                continue

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

        for child in children:

            if hash(child.position) in closed_set:
                continue

            child.g = current_node.g + 1
            child.h = heuristic(end_node.position, child.position)
            child.f = child.g + child.h

            open_list.put((child.f, id(child), child))

        closed_set.add(hash(current_node.position))

    raise ValueError('No Path Found')

此代码实现了基本的三维A*寻径功能，适用于简单的立方体障碍物地图。对于更复杂的环境，则需考虑更多因素如倾斜角度限制、动态避障等。