A-star算法

算法简介

A*（A-star）算法是一种用于图形搜索和路径规划的启发式搜索算法，它结合了最佳优先搜索（Best-First Search）和Dijkstra算法的思想，能够有效地寻找从起点到目标点的最短路径。A*算法广泛应用于导航、游戏AI、机器人路径规划等领域。

代码说明

Node类：表示搜索过程中的一个节点，包含位置、从起点到当前节点的代价 (g)、从当前节点到目标节点的启发式代价 (h)，以及父节点用于回溯路径。
A算法：astar函数实现了A算法的核心逻辑。通过开放列表优先队列不断从代价最小的节点扩展，直到找到目标节点。
启发式函数：heuristic使用曼哈顿距离作为启发式代价，适用于网格布局。
邻居节点：get_neighbors返回当前节点的四个邻居（上下左右）。

代码

import heapq

class Node:
    def __init__(self, position, g=0, h=0):
        self.position = position  # 坐标 (x, y)
        self.g = g  # 从起点到当前节点的代价
        self.h = h  # 从当前节点到目标节点的预估代价（启发式估计）
        self.f = g + h  # 总代价
        self.parent = None  # 记录父节点
    
    def __lt__(self, other):
        return self.f < other.f  # 优先队列按 f 值排序

def astar(start, goal, grid):
    # 创建开放列表（优先队列）和闭合列表
    open_list = []
    closed_list = set()
    
    # 将起点添加到开放列表
    start_node = Node(start, 0, heuristic(start, goal))
    heapq.heappush(open_list, start_node)

    while open_list:
        # 从开放列表中取出代价最小的节点
        current_node = heapq.heappop(open_list)
        
        # 如果目标已经找到，返回路径
        if current_node.position == goal:
            path = []
            while current_node:
                path.append(current_node.position)
                current_node = current_node.parent
            return path[::-1]  # 返回反转后的路径
        
        # 将当前节点添加到闭合列表
        closed_list.add(current_node.position)
        
        # 获取相邻节点
        neighbors = get_neighbors(current_node.position)
        
        for neighbor in neighbors:
            if neighbor in closed_list:
                continue  # 如果相邻节点已经被处理过，跳过
            
            g_cost = current_node.g + 1  # 假设每步的代价为1
            h_cost = heuristic(neighbor, goal)
            neighbor_node = Node(neighbor, g_cost, h_cost)
            neighbor_node.parent = current_node

            # 如果相邻节点不在开放列表中，加入开放列表
            heapq.heappush(open_list, neighbor_node)
    
    return None  # 如果没有路径，返回 None

def heuristic(node, goal):
    # 计算启发式代价（这里使用曼哈顿距离）
    return abs(node[0] - goal[0]) + abs(node[1] - goal[1])

def get_neighbors(position):
    # 获取当前节点的相邻节点（上下左右）
    x, y = position
    return [(x + 1, y), (x - 1, y), (x, y + 1), (x, y - 1)]

if __name__ == "__main__":
    start = (0, 0)  # 起点
    goal = (4, 4)  # 目标点
    grid = [[0 for _ in range(5)] for _ in range(5)]  # 假设网格，0表示可行走区域

    path = astar(start, goal, grid)
    print("找到的路径：", path)