帮我用python写一个A*算法
时间: 2024-05-10 15:02:30 浏览: 12
以下是一个基本的 A* 算法的 Python 实现,该算法通过一个二维网格搜索来寻找从起点到终点的最短路径:
```python
import heapq
class Node:
def __init__(self, x, y, cost, parent=None):
self.x = x
self.y = y
self.cost = cost
self.parent = parent
def __lt__(self, other):
return self.cost < other.cost
def astar(start, goal, grid):
# 初始化起点和终点节点
start_node = Node(*start, 0.0)
goal_node = Node(*goal, 0.0)
# 初始化开启列表和关闭列表
open_list = []
closed_list = set()
# 将起点加入开启列表
heapq.heappush(open_list, start_node)
# 开始搜索
while open_list:
# 从开启列表中取出最小代价节点
current_node = heapq.heappop(open_list)
# 如果当前节点是终点,则返回路径
if current_node.x == goal_node.x and current_node.y == goal_node.y:
path = []
while current_node.parent:
path.append((current_node.x, current_node.y))
current_node = current_node.parent
path.append((start_node.x, start_node.y))
return path[::-1]
# 将当前节点加入关闭列表
closed_list.add((current_node.x, current_node.y))
# 扩展当前节点的邻居节点
for dx, dy, cost in [(-1, 0, 1.0), (0, -1, 1.0), (1, 0, 1.0), (0, 1, 1.0)]:
x, y = current_node.x + dx, current_node.y + dy
if not (0 <= x < len(grid) and 0 <= y < len(grid[0])):
continue
if grid[x][y] == 1:
continue
if (x, y) in closed_list:
continue
neighbor_node = Node(x, y, current_node.cost + cost, current_node)
heapq.heappush(open_list, neighbor_node)
# 如果没有找到路径,则返回空列表
return []
# 测试
start = (0, 0)
goal = (4, 4)
grid = [[0, 0, 0, 0, 0],
[0, 1, 1, 0, 0],
[0, 0, 0, 0, 0],
[0, 1, 0, 1, 0],
[0, 0, 0, 0, 0]]
path = astar(start, goal, grid)
print(path)
```
在这段代码中,`Node` 类表示一个节点,包含了节点的坐标、代价和父节点。`astar` 函数接受起点、终点和网格作为参数,使用堆来实现开启列表,并使用集合来实现关闭列表。在搜索过程中,首先从开启列表中取出代价最小的节点,然后将其加入关闭列表,并扩展其邻居节点。如果邻居节点已经在关闭列表中,则跳过;否则,计算邻居节点的代价,并将其加入开启列表。如果在搜索过程中找到了终点,则按照父节点指针从终点遍历到起点,构造出最短路径。最后,将路径反转后返回。如果没有找到路径,则返回空列表。