Python 基于A*算法路径规划实现,采用欧氏距离进行计算,起点终点和路径障碍由用户输入,并根据用户输入自动调节地图大小,最后动态显示路径
时间: 2024-03-22 22:38:27 浏览: 60
下面是一个基于A*算法的Python路径规划示例,代码如下:
```python
import heapq
def heuristic(a, b):
# 欧氏距离
return ((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2) ** 0.5
def astar(array, start, goal):
neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]
close_set = set()
came_from = {}
gscore = {start:0}
fscore = {start:heuristic(start, goal)}
oheap = []
heapq.heappush(oheap, (fscore[start], start))
while oheap:
current = heapq.heappop(oheap)[1]
if current == goal:
data = []
while current in came_from:
data.append(current)
current = came_from[current]
return data[::-1]
close_set.add(current)
for i, j in neighbors:
neighbor = current[0] + i, current[1] + j
tentative_g_score = gscore[current] + heuristic(current, neighbor)
if 0 <= neighbor[0] < array.shape[0]:
if 0 <= neighbor[1] < array.shape[1]:
if array[neighbor[0]][neighbor[1]] == 1:
continue
else:
continue
else:
continue
if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0):
continue
if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]:
came_from[neighbor] = current
gscore[neighbor] = tentative_g_score
fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal)
heapq.heappush(oheap, (fscore[neighbor], neighbor))
return False
# 用户输入起点和终点坐标
start_point = tuple(map(int, input("请输入起点坐标,用逗号隔开:").split(",")))
end_point = tuple(map(int, input("请输入终点坐标,用逗号隔开:").split(",")))
# 用户输入障碍物坐标
obstacles = []
while True:
obstacle_str = input("请输入障碍物坐标,用逗号隔开,输入q结束:")
if obstacle_str == "q":
break
obstacles.append(tuple(map(int, obstacle_str.split(","))))
# 自动调节地图大小
max_x = max(start_point[0], end_point[0])
max_y = max(start_point[1], end_point[1])
for obstacle in obstacles:
max_x = max(max_x, obstacle[0])
max_y = max(max_y, obstacle[1])
# 创建地图
map = [[0]*(max_y+1) for i in range(max_x+1)]
for obstacle in obstacles:
map[obstacle[0]][obstacle[1]] = 1
# 进行路径规划
path = astar(array=array.array('d', map), start=start_point, goal=end_point)
# 动态显示路径
for i in range(len(path)):
map[path[i][0]][path[i][1]] = "x"
for j in range(len(map[0])):
if j == len(map[0])-1:
print(map[i][j])
else:
print(map[i][j], end=', ')
```
使用方法:
1. 运行代码,根据提示输入起点、终点和障碍物坐标,输入q结束障碍物输入。
2. 程序会自动调节地图大小,并进行路径规划。
3. 最后会动态显示路径,路径用"x"表示。
阅读全文