Python 基于A*算法路径规划实现,采用欧氏距离进行计算,起点终点和路径障碍由用户输入,并根据用户输入自动调节地图大小,最后动态显示路径
时间: 2024-03-21 21:44:17 浏览: 37
好的,下面是一个基于A*算法的路径规划的Python实现,其中采用欧氏距离进行计算,起点、终点和路径障碍由用户输入,程序会自动调节地图大小,并最终动态显示路径。代码如下:
```python
import heapq
import math
class Node:
def __init__(self, x, y):
self.x = x
self.y = y
self.g = 0
self.h = 0
self.f = 0
self.parent = None
def __eq__(self, other):
return self.x == other.x and self.y == other.y
def __lt__(self, other):
return self.f < other.f
def astar(start, end, obstacles):
open_list = []
closed_list = []
heapq.heappush(open_list, start)
while len(open_list) > 0:
current = heapq.heappop(open_list)
if current == end:
path = []
while current.parent is not None:
path.insert(0, (current.x, current.y))
current = current.parent
path.insert(0, (current.x, current.y))
return path
closed_list.append(current)
neighbors = [(current.x, current.y-1), (current.x, current.y+1), (current.x-1, current.y), (current.x+1, current.y), (current.x-1, current.y-1), (current.x+1, current.y-1), (current.x-1, current.y+1), (current.x+1, current.y+1)]
for neighbor in neighbors:
if neighbor[0] < 0 or neighbor[0] >= len(grid) or neighbor[1] < 0 or neighbor[1] >= len(grid[0]):
continue
if grid[neighbor[0]][neighbor[1]] == 1:
continue
neighbor_node = Node(neighbor[0], neighbor[1])
if neighbor_node in closed_list:
continue
neighbor_node.g = current.g + math.sqrt((current.x - neighbor_node.x)**2 + (current.y - neighbor_node.y)**2)
neighbor_node.h = math.sqrt((end.x - neighbor_node.x)**2 + (end.y - neighbor_node.y)**2)
neighbor_node.f = neighbor_node.g + neighbor_node.h
neighbor_node.parent = current
if neighbor_node in open_list:
index = open_list.index(neighbor_node)
if open_list[index].f > neighbor_node.f:
open_list[index] = neighbor_node
heapq.heapify(open_list)
else:
heapq.heappush(open_list, neighbor_node)
return None
def display_path(path):
for i in range(len(grid)):
for j in range(len(grid[0])):
if (i, j) == start:
print("S ", end="")
elif (i, j) == end:
print("E ", end="")
elif (i, j) in path:
print("* ", end="")
elif grid[i][j] == 1:
print("X ", end="")
else:
print("- ", end="")
print()
if __name__ == "__main__":
start = tuple(map(int, input("Enter start point (x, y): ").split()))
end = tuple(map(int, input("Enter end point (x, y): ").split()))
obstacles = []
while True:
obstacle = input("Enter obstacle (x, y) or type 'done' if there are no more obstacles: ")
if obstacle == "done":
break
obstacles.append(tuple(map(int, obstacle.split())))
max_x = max(start[0], end[0], max(obstacles, key=lambda x: x[0])[0])
max_y = max(start[1], end[1], max(obstacles, key=lambda x: x[1])[1])
grid = [[0 for j in range(max_y+1)] for i in range(max_x+1)]
for obstacle in obstacles:
grid[obstacle[0]][obstacle[1]] = 1
path = astar(Node(start[0], start[1]), Node(end[0], end[1]), obstacles)
if path is None:
print("No path found!")
else:
display_path(path)
```
在这个程序中,我们定义了一个`Node`类来表示节点,包括节点的坐标,G、H和F值,以及父节点。`astar`函数使用A*算法来搜索路径,包括一个开放列表和一个关闭列表,使用堆来实现开放列表,并使用欧氏距离计算节点的H值。`display_path`函数用于动态显示路径。
在主程序中,用户输入起点、终点和路径障碍,程序会自动计算地图大小并生成网格,然后调用`astar`函数搜索路径,并根据搜索结果调用`display_path`函数动态显示路径。