A-star算法

算法简介

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

代码说明

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

代码

import heapqclass 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.parentreturn 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  # 假设每步的代价为1h_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  # 如果没有路径，返回 Nonedef heuristic(node, goal):# 计算启发式代价（这里使用曼哈顿距离）return abs(node[0] - goal[0]) + abs(node[1] - goal[1])def get_neighbors(position):# 获取当前节点的相邻节点（上下左右）x, y = positionreturn [(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)