A*算法python
时间: 2023-08-13 20:08:47 浏览: 58
A*算法是一种启发式搜索算法,常用于解决路径规划问题。下面是一个使用Python实现A*算法的示例代码:
```python
import heapq
# 定义节点类
class Node:
def __init__(self, position, parent=None):
self.position = position
self.parent = parent
self.g = 0 # 起始节点到当前节点的代价
self.h = 0 # 当前节点到目标节点的估计代价
self.f = 0 # f = g + h
def __lt__(self, other):
return self.f < other.f
# 计算两个节点之间的曼哈顿距离
def manhattan_distance(node1, node2):
return abs(node1.position[0] - node2.position[0]) + abs(node1.position[1] - node2.position[1])
# A*算法
def astar(start, goal, grid):
open_list = []
closed_list = []
heapq.heappush(open_list, start)
while open_list:
current_node = heapq.heappop(open_list)
if current_node.position == goal.position:
path = []
while current_node.parent:
path.append(current_node.position)
current_node = current_node.parent
path.append(start.position)
return path[::-1]
closed_list.append(current_node)
neighbors = [(0, 1), (0, -1), (1, 0), (-1, 0)] # 上下左右四个方向
for neighbor in neighbors:
new_position = (current_node.position[0] + neighbor[0], current_node.position[1] + neighbor[1])
if new_position[0] < 0 or new_position[0] >= len(grid) or new_position[1] < 0 or new_position[1] >= len(grid[0]):
continue
if grid[new_position[0]][new_position[1]] == 1:
continue
neighbor_node = Node(new_position, current_node)
if neighbor_node in closed_list:
continue
neighbor_node.g = current_node.g + 1
neighbor_node.h = manhattan_distance(neighbor_node, goal)
neighbor_node.f = neighbor_node.g + neighbor_node.h
if neighbor_node in open_list:
for node in open_list:
if node == neighbor_node and node.f > neighbor_node.f:
open_list.remove(node)
heapq.heappush(open_list, neighbor_node)
break
else:
heapq.heappush(open_list, neighbor_node)
return None
# 示例使用
grid = [
[0, 0, 0, 0, 0],
[0, 1, 1, 0, 0],
[0, 0, 0, 0, 0],
[0, 1, 1, 1, 0],
[0, 0, 0, 0, 0]
]
start = Node((0, 0))
goal = Node((4, 4))
path = astar(start, goal, grid)
if path:
print("Path found:")
for position in path:
print(position)
else:
print("Path not found.")
```
这个例子是在一个5x5的网格中找到从起点(0, 0)到终点(4, 4)的最短路径。网格中的0表示可以通过的路径,1表示障碍物。算法将返回一个路径列表,如果找不到路径则返回None。
相关推荐
![rar](https://img-home.csdnimg.cn/images/20210720083606.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)