python 实现 A* 算法
A*算法是一种广泛使用的路径搜索算法,结合了启发式搜索和Dijkstra算法的优点。它通过评估每个节点的代价函数 ( f(n) = g(n) + h(n) ) 来选择最优路径,其中:
- ( g(n) ) 是从起点到当前节点的实际代价。
- ( h(n) ) 是从当前节点到目标节点的启发式估计代价(如曼哈顿距离或欧几里得距离)。
以下是一个Python实现的A*算法示例:
Python实现A*算法
import heapq
from math import sqrt
class Node:
def __init__(self, position, parent=None):
self.position = position # 节点的坐标 (x, y)
self.parent = parent # 父节点
self.g = 0 # 从起点到当前节点的实际代价
self.h = 0 # 启发式估计代价
self.f = 0 # 总代价 f = g + h
def __eq__(self, other):
return self.position == other.position
def __lt__(self, other):
return self.f < other.f
def heuristic(a, b):
"""启发式函数:计算曼哈顿距离"""
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def a_star(grid, start, end):
"""A*算法实现"""
open_list = [] # 优先队列,存储待探索的节点
closed_list = set() # 存储已探索的节点
start_node = Node(start)
end_node = Node(end)
heapq.heappush(open_list, start_node)
while open_list:
current_node = heapq.heappop(open_list)
closed_list.add(current_node.position)
# 如果找到目标节点,返回路径
if current_node == end_node:
path = []
while current_node:
path.append(current_node.position)
current_node = current_node.parent
return path[::-1] # 反转路径,从起点到终点
# 遍历当前节点的邻居
for neighbor in [(0, -1), (0, 1), (-1, 0), (1, 0)]: # 上下左右四个方向
neighbor_position = (current_node.position[0] + neighbor[0], current_node.position[1] + neighbor[1])
# 检查邻居是否在地图范围内且不是障碍物
if (neighbor_position[0] < 0 or neighbor_position[0] >= len(grid) or
neighbor_position[1] < 0 or neighbor_position[1] >= len(grid[0]) or
grid[neighbor_position[0]][neighbor_position[1]] == 1):
continue
# 创建邻居节点
neighbor_node = Node(neighbor_position, current_node)
# 如果邻居节点已经探索过,跳过
if neighbor_node.position in closed_list:
continue
# 计算 g, h, f 值
neighbor_node.g = current_node.g + 1
neighbor_node.h = heuristic(neighbor_node.position, end_node.position)
neighbor_node.f = neighbor_node.g + neighbor_node.h
# 如果邻居节点已经在 open_list 中且新代价更高,跳过
if any(neighbor_node == open_node and neighbor_node.g > open_node.g for open_node in open_list):
continue
# 将邻居节点加入 open_list
heapq.heappush(open_list, neighbor_node)
return None # 如果没有找到路径,返回 None
# 示例地图 (0 表示可通行,1 表示障碍物)
grid = [
[0, 0, 0, 0, 1, 0],
[0, 1, 1, 0, 1, 0],
[0, 0, 0, 0, 1, 0],
[0, 1, 0, 1, 1, 0],
[0, 0, 0, 0, 0, 0]
]
# 起点和终点
start = (0, 0)
end = (4, 5)
# 运行A*算法
path = a_star(grid, start, end)
if path:
print("找到路径:", path)
else:
print("未找到路径")
代码说明:
Node
类:表示搜索中的每个节点,包含位置、父节点、代价等信息。heuristic
函数:计算启发式估计代价(这里使用曼哈顿距离)。a_star
函数:实现A*算法的核心逻辑,使用优先队列(堆)来管理待探索的节点。- 地图表示:
grid
是一个二维列表,0
表示可通行的路径,1
表示障碍物。 - 路径返回:如果找到路径,返回从起点到终点的坐标列表;否则返回
None
。
示例输出:
对于上面的地图,输出可能是:
找到路径: [(0, 0), (0, 1), (0, 2), (1, 2), (2, 2), (2, 3), (3, 3), (4, 3), (4, 4), (4, 5)]
扩展:
- 启发式函数:可以替换为欧几里得距离或其他更适合的启发式函数。
- 地图生成:可以从文件中读取地图,或动态生成随机地图。
- 可视化:可以使用
matplotlib
或pygame
可视化路径搜索过程。