python 实现简单A*

B站A星算法讲解

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()