python - python中A*规划的运动
问题描述
我很难让代码中的星星从它的开始位置移动到它在 python 中的结束位置。我不是在寻找他们的确切答案,而只是一些帮助让我朝着正确的方向前进。我还想知道如何设置代码以从五个设置网格中选择一个,并允许您选择开始和结束路径。如果我让它随机生成一个数字,我是否可以使用网格的 if 语句。然后使用与 if 语句对应的网格。
import numpy as np
import heapq
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.pyplot import figure
# creating a square grid
grid = np.array([
[0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0],
[0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
# start point and goal
start = (0, 0)
goal = (10, 15)
# plot map and path
fig, ax = plt.subplots(figsize=(12, 12))
ax.imshow(grid, cmap=plt.cm.Dark2)
ax.scatter(start[1], start[0], marker="*", color="yellow", s=200)
ax.scatter(goal[1], goal[0], marker="*", color="red", s=200)
plt.show()
# define Heuristic function
def heuristic(a, b):
return np.sqrt((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2)
# define A-star function
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
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:
# array bound y walls
continue
else:
# array bound x walls
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))
# creating route
route = astar(grid, start, goal)
# add the start position
route = route + [start]
# reverse the backward sequence
route = route[::-1]
# print route
print(route)
# extract x and y coordinates from route list
x_coords = []
y_coords = []
for i in(range(0,len(route))):
x = route[i][0]
y = route[i][1]
x_coords.append(x)
y_coords.append(y)
#plot map and path
fig, ax = plt.subplots(figsize=(20,20))
ax.imshow(grid, cmap=plt.cm.Dark2)
ax.scatter(start[1],start[0], marker ="*", color ="yellow", s = 200)
ax.scatter(goal[1],goal[0], marker ="*", color ="red", s = 200)
ax.plot(y_coords,x_coords, color ="black")
plt.show()
解决方案
推荐阅读
- java - 如何处理基于事件的异步服务通信的超时?
- python - fast_executemany 底层 SQL 操作
- oauth - 通过 MSAL/OAuth 访问 Azure 存储帐户的 Blob
- python - 在Python中解析字典键连接列表的两项
- makefile - make [1]: *** 读取作业管道:资源暂时不可用。停止
- python - Python socket.accept() 在第二次调用时不起作用
- r - R 正则表达式中的负前瞻问题
- reactjs - 使用翻译 react-i18next 和 react-moment
- r - 使用另一个函数重现 hist() 的 $count 方面
- kotlin - 如何仅使用元素上的特定序列来减少列表