185 lines
6.3 KiB
Python
185 lines
6.3 KiB
Python
"""
|
||
A*算法实现
|
||
A*是一种启发式搜索算法，广泛用于路径规划
|
||
"""
|
||
|
||
import heapq
|
||
import math
|
||
from typing import List, Tuple, Optional, Dict, Set
|
||
|
||
class AStar:
|
||
"""
|
||
A*算法实现
|
||
A*是一种启发式搜索算法，广泛用于路径规划
|
||
"""
|
||
|
||
def __init__(self):
|
||
"""初始化A*算法"""
|
||
self.heuristic = 'manhattan' # 默认启发式函数
|
||
self.stats = {
|
||
'nodes_visited': 0,
|
||
'max_queue_size': 0
|
||
}
|
||
|
||
def find_path(self, grid: List[List[int]], start: Tuple[int, int], goal: Tuple[int, int]) -> Optional[List[Tuple[int, int]]]:
|
||
"""
|
||
使用A*算法查找路径
|
||
|
||
Args:
|
||
grid: 网格地图，0表示可通行，1表示障碍物
|
||
start: 起点坐标 (row, col)
|
||
goal: 终点坐标 (row, col)
|
||
|
||
Returns:
|
||
路径坐标列表，如果找不到路径则返回None
|
||
"""
|
||
# 重置统计信息
|
||
self.stats = {
|
||
'nodes_visited': 0,
|
||
'max_queue_size': 0
|
||
}
|
||
|
||
# 获取网格尺寸
|
||
rows = len(grid)
|
||
cols = len(grid[0]) if rows > 0 else 0
|
||
|
||
# 检查起点和终点是否有效
|
||
if not (0 <= start[0] < rows and 0 <= start[1] < cols):
|
||
return None
|
||
if not (0 <= goal[0] < rows and 0 <= goal[1] < cols):
|
||
return None
|
||
if grid[start[0]][start[1]] == 1 or grid[goal[0]][goal[1]] == 1:
|
||
return None
|
||
|
||
# 初始化开放列表（优先队列）
|
||
open_list = []
|
||
heapq.heappush(open_list, (0, start))
|
||
|
||
# 初始化关闭列表
|
||
closed_list: Set[Tuple[int, int]] = set()
|
||
|
||
# 初始化距离和父节点字典
|
||
g_score: Dict[Tuple[int, int], float] = {start: 0}
|
||
f_score: Dict[Tuple[int, int], float] = {start: self._calculate_heuristic(start, goal)}
|
||
parent: Dict[Tuple[int, int], Tuple[int, int]] = {}
|
||
|
||
# 8方向移动（包括对角线）
|
||
directions = [
|
||
(-1, 0), (1, 0), (0, -1), (0, 1), # 上下左右
|
||
(-1, -1), (-1, 1), (1, -1), (1, 1) # 对角线
|
||
]
|
||
|
||
while open_list:
|
||
# 更新统计信息
|
||
self.stats['nodes_visited'] += 1
|
||
self.stats['max_queue_size'] = max(self.stats['max_queue_size'], len(open_list))
|
||
|
||
# 取出f_score最小的节点
|
||
current = heapq.heappop(open_list)[1]
|
||
|
||
# 如果到达目标节点
|
||
if current == goal:
|
||
# 重构路径
|
||
path = self._reconstruct_path(parent, current)
|
||
return path
|
||
|
||
# 将当前节点加入关闭列表
|
||
closed_list.add(current)
|
||
|
||
# 检查所有邻居节点
|
||
for dr, dc in directions:
|
||
neighbor = (current[0] + dr, current[1] + dc)
|
||
|
||
# 检查邻居节点是否在网格范围内
|
||
if not (0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols):
|
||
continue
|
||
|
||
# 检查邻居节点是否为障碍物
|
||
if grid[neighbor[0]][neighbor[1]] == 1:
|
||
continue
|
||
|
||
# 检查邻居节点是否已在关闭列表中
|
||
if neighbor in closed_list:
|
||
continue
|
||
|
||
# 计算移动代价
|
||
if abs(dr) + abs(dc) == 2: # 对角线移动
|
||
move_cost = 1.414 # √2
|
||
else: # 直线移动
|
||
move_cost = 1.0
|
||
|
||
# 计算到邻居节点的 tentative_g_score
|
||
tentative_g_score = g_score[current] + move_cost
|
||
|
||
# 如果找到更优路径或首次访问该节点
|
||
if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
|
||
# 更新父节点和得分
|
||
parent[neighbor] = current
|
||
g_score[neighbor] = tentative_g_score
|
||
f_score[neighbor] = g_score[neighbor] + self._calculate_heuristic(neighbor, goal)
|
||
|
||
# 将邻居节点加入开放列表
|
||
heapq.heappush(open_list, (f_score[neighbor], neighbor))
|
||
|
||
# 未找到路径
|
||
return None
|
||
|
||
def _calculate_heuristic(self, point1: Tuple[int, int], point2: Tuple[int, int]) -> float:
|
||
"""
|
||
计算启发式函数值
|
||
|
||
Args:
|
||
point1: 第一个点
|
||
point2: 第二个点
|
||
|
||
Returns:
|
||
启发式函数值
|
||
"""
|
||
if self.heuristic == 'manhattan':
|
||
# 曼哈顿距离
|
||
return abs(point1[0] - point2[0]) + abs(point1[1] - point2[1])
|
||
elif self.heuristic == 'euclidean':
|
||
# 欧几里得距离
|
||
return math.sqrt((point1[0] - point2[0])**2 + (point1[1] - point2[1])**2)
|
||
elif self.heuristic == 'chebyshev':
|
||
# 切比雪夫距离
|
||
return max(abs(point1[0] - point2[0]), abs(point1[1] - point2[1]))
|
||
else:
|
||
# 默认使用曼哈顿距离
|
||
return abs(point1[0] - point2[0]) + abs(point1[1] - point2[1])
|
||
|
||
def _reconstruct_path(self, parent: Dict[Tuple[int, int], Tuple[int, int]], current: Tuple[int, int]) -> List[Tuple[int, int]]:
|
||
"""
|
||
重构路径
|
||
|
||
Args:
|
||
parent: 父节点字典
|
||
current: 当前节点
|
||
|
||
Returns:
|
||
路径坐标列表
|
||
"""
|
||
path = [current]
|
||
while current in parent:
|
||
current = parent[current]
|
||
path.append(current)
|
||
path.reverse()
|
||
return path
|
||
|
||
def get_stats(self) -> Dict[str, int]:
|
||
"""
|
||
获取算法统计信息
|
||
|
||
Returns:
|
||
统计信息字典
|
||
"""
|
||
return self.stats.copy()
|
||
|
||
def set_heuristic(self, heuristic: str):
|
||
"""
|
||
设置启发式函数
|
||
|
||
Args:
|
||
heuristic: 启发式函数名称 ('manhattan', 'euclidean', 'chebyshev')
|
||
"""
|
||
self.heuristic = heuristic |