import numpy as npimport matplotlib.pyplot as pltimport heapqfrom typing import List, Tuple, Optional

定义节点类class Node:

def __init__(self, x: int, y: int, cost: float, parent: Optional['Node']):
    self.x = x
    self.y = y
    self.cost = cost
    self.parent = parent

def __lt__(self, other: 'Node') -> bool:
    return self.cost < other.cost

检查点是否在障碍物中def is_in_obstacle(point: Tuple[int, int], obstacles: List[Tuple[int, int, int, int]]) -> bool:

x, y = point
for (x1, y1, x2, y2) in obstacles:
    if x1 <= x < x2 and y1 <= y < y2:
        return True
return False

A* 算法def a_star(start: Tuple[int, int], goal: Tuple[int, int], obstacles: List[Tuple[int, int, int, int]], grid_size: int) -> Optional[List[Tuple[int, int]]]:

open_list = []
closed_set = set()
start_node = Node(start[0], start[1], 0, None)
heapq.heappush(open_list, (0, start_node))

# 允许对角线移动
directions = [(-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (-1, 1), (1, -1), (1, 1)]

while open_list:
    _, current = heapq.heappop(open_list)
    
    if (current.x, current.y) == goal:
        path = []
        while current:
            path.append((current.x, current.y))
            current = current.parent
        return path[::-1]
    
    closed_set.add((current.x, current.y))
    
    for dx, dy in directions:
        new_x, new_y = current.x + dx, current.y + dy
        
        # 检查是否超出边界
        if not (0 <= new_x < grid_size and 0 <= new_y < grid_size):
            continue
            
        # 检查是否在障碍物中
        if is_in_obstacle((new_x, new_y), obstacles):
            continue
            
        # 检查是否已访问
        if (new_x, new_y) in closed_set:
            continue
            
        # 计算移动成本（对角线移动成本更高）
        move_cost = 1.0 if dx == 0 or dy == 0 else 1.414  # 对角线移动成本为√2
        new_cost = current.cost + move_cost
        
        # 使用曼哈顿距离作为启发函数
        heuristic = abs(new_x - goal[0]) + abs(new_y - goal[1])
        
        new_node = Node(new_x, new_y, new_cost, current)
        heapq.heappush(open_list, (new_cost + heuristic, new_node))

return None

能耗计算def calculate_energy(path: List[Tuple[int, int]]) -> float:

if not path:
    return 0

energy = 0.0
for i in range(1, len(path)):
    dx = path[i][0] - path[i-1][0]
    dy = path[i][1] - path[i-1][1]
    energy += 1.0 if dx == 0 or dy == 0 else 1.414  # 对角线移动能耗更高
return energy

可视化def visualize_path(start: Tuple[int, int], goal: Tuple[int, int], obstacles: List[Tuple[int, int, int, int]], path: Optional[List[Tuple[int, int]]]):

fig, ax = plt.subplots(figsize=(8, 8))
ax.set_xlim(0, 10)
ax.set_ylim(0, 10)
ax.set_aspect('equal')
ax.grid(True)

# 绘制障碍物
for obs in obstacles:
    x1, y1, x2, y2 = obs
    rect = plt.Rectangle((x1, y1), x2 - x1, y2 - y1, color='red', alpha=0.7)
    ax.add_patch(rect)

# 绘制起点和终点
ax.plot(start[0] + 0.5, start[1] + 0.5, 'go', markersize=10)  # 绿色点表示起点
ax.plot(goal[0] + 0.5, goal[1] + 0.5, 'bo', markersize=10)   # 蓝色点表示终点

# 绘制路径
if path:
    path_x = [p[0] + 0.5 for p in path]
    path_y = [p[1] + 0.5 for p in path]
    ax.plot(path_x, path_y, 'k--', linewidth=2)

plt.title('A* Pathfinding Visualization')
plt.xlabel('X Coordinate')
plt.ylabel('Y Coordinate')
plt.tight_layout()
plt.show()

主函数def main():

start = (1, 1)
goal = (8, 8)
obstacles = [(2, 2, 4, 4), (6, 6, 8, 8)]  # 障碍物列表，每个障碍物用矩形区域表示
grid_size = 10

path = a_star(start, goal, obstacles, grid_size)
if path:
    energy = calculate_energy(path)
    print(f"Path: {path}")
    print(f"Energy Consumption: {energy:.2f} units")
    visualize_path(start, goal, obstacles, path)
else:
    print("No path found.")

if name == "main": main()