用python实现Astar算法
时间: 2024-03-15 21:10:39 浏览: 17
当然可以!以下是一个用Python实现A*算法的示例代码:
```python
import heapq
class Node:
def __init__(self, position, parent=None):
self.position = position
self.parent = parent
self.g = 0 # Cost from start node to current node
self.h = 0 # Heuristic cost from current node to goal node
self.f = 0 # Total cost
def __lt__(self, other):
return self.f < other.f
def astar(grid, start, goal):
open_list = []
closed_list = set()
start_node = Node(start)
goal_node = Node(goal)
heapq.heappush(open_list, start_node)
while open_list:
current_node = heapq.heappop(open_list)
closed_list.add(current_node.position)
if current_node.position == goal_node.position:
path = []
while current_node.parent:
path.append(current_node.position)
current_node = current_node.parent
path.append(start_node.position)
return path[::-1]
neighbors = get_neighbors(grid, current_node)
for neighbor in neighbors:
if neighbor.position in closed_list:
continue
cost = current_node.g + 1
if neighbor not in open_list or cost < neighbor.g:
neighbor.g = cost
neighbor.h = heuristic(neighbor, goal_node)
neighbor.f = neighbor.g + neighbor.h
neighbor.parent = current_node
if neighbor not in open_list:
heapq.heappush(open_list, neighbor)
return None
def get_neighbors(grid, node):
neighbors = []
directions = [(0, 1), (0, -1), (1, 0), (-1, 0)] # Up, down, left, right
for dir in directions:
new_position = (node.position[0] + dir[0], node.position[1] + dir[1])
if is_valid_position(grid, new_position):
neighbors.append(Node(new_position, node))
return neighbors
def is_valid_position(grid, position):
rows = len(grid)
cols = len(grid[0])
return 0 <= position[0] < rows and 0 <= position[1] < cols and grid[position[0]][position[1]] == 0
def heuristic(node, goal_node):
return abs(node.position[0] - goal_node.position[0]) + abs(node.position[1] - goal_node.position[1])
# 测试示例
grid = [
[0, 1, 0, 0, 0],
[0, 1, 0, 1, 0],
[0, 0, 0, 1, 0],
[0, 1, 0, 0, 0],
[0, 0, 0, 0, 0]
]
start = (0, 0)
goal = (4, 4)
path = astar(grid, start, goal)
if path:
print("Path found:")
for position in path:
print(position)
else:
print("No path found.")
```
这是一个简单的实现,用于在给定的网格中找到起点和终点之间的最短路径。你可以根据自己的需求对代码进行修改和扩展。