import time
import queue
import math
import numpy as np
import random
class Graph():
def __init__(self, graph):
self.graph = graph
self.y_, self.x_ = self.graph.shape
print(self.graph)
def __del__(self):
pass
#获取近邻上下左右的格子坐标
def neighbors(self, current):
x, y = current
left = (x - 1, y)
up = (x, y - 1)
right = (x + 1, y)
down = (x, y + 1)
neigh_list = [left, up, right, down]
neigh_list_ = neigh_list.copy()
for neigh in neigh_list_:
if neigh[0] < 0 or neigh[1] < 0 or neigh[0] >= self.x_ or neigh[1] >= self.y_:
neigh_list.remove(neigh)
continue
if self.graph[neigh[1], neigh[0]] == 1:
neigh_list.remove(neigh)
return neigh_list
#下一个格子和当前格子的距离,因为是近邻格子,所以为1
def cost(self, current, next):
return 1
#启发函数
def heuristic(goal, next):
return math.fabs(goal[0] - next[0]) + math.fabs(goal[1] - next[1]) # 曼哈顿距离
#return math.sqrt(math.fabs(goal[0] - next[0])**2 + math.fabs(goal[1] - next[1])**2) #欧拉距离
def a_start_search(graph, start, goal):
frontier = queue.PriorityQueue()
frontier.put((0, start))
came_from = {} #保存路径,可以从终点一直回溯到起点就是完整的最短路径了
cost_so_far = {} #当前代价(当前位置距离起点的步数)
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
current = frontier.get()[1]
if current == goal:
break
for next in graph.neighbors(current):
new_cost = cost_so_far[current] + graph.cost(current, next)
if next not in cost_so_far or new_cost < cost_so_far[next]: #如果next的代价还没计算或者已经计算了但不同current的同一个next的cost更小
cost_so_far[next] = new_cost
priority = new_cost + heuristic(goal, next)
frontier.put((priority, next))
came_from[next] = current
return came_from, cost_so_far
if __name__=="__main__":
start = (0, 0)#起点
goal = (33, 33)#终点
graph = np.mat(np.zeros((50, 50)), dtype=int) #地图大小
y_, x_ = graph.shape
#随机设置障碍物
for y in range(y_):
for x in range(x_):
if (x, y) != start and (x, y) != goal and random.randint(1, 10) < 2:
graph[y, x] = 1
graph_ = Graph(graph)
came_from, cost_so_far = a_start_search(graph_, start, goal)
path = [] #存放起点到终点的中间路径
goal_ = goal
#path.append(goal_)
while True:
try:
if start == came_from[goal_]:
break
path.append(came_from[goal_])
goal_ = came_from[goal_]
except Exception:
print("因障碍物阻挡,无法规划当前路径")
exit(1)
#path.append(start)
# ^为起点,#为终点,|为障碍物,*为规划出的最短路径,.为可通行区域
for y in range(y_):
for x in range(x_):
if (x, y) == start:
print("^", end='')
elif (x, y) == goal:
print("#", end='')
elif graph[y, x] == 1:
print("|", end='')
elif (x, y) in path:
print("*", end='')
else:
print(".", end='')
if x < x_ - 1:
print(" ", end='')
else:
print()
python 实现简单A*
最新推荐文章于 2024-02-21 14:08:36 发布