A Star 算法求解数值方阵最短路径（A*，python）

作业题干

随机地图搜索，在[16,16]矩阵中生成[0,387]米的随机高度值，采用A算法和A*算法分别搜索从[0,0]到[15,15]的 最短爬山路径。

题目解析

题目里面有一些细节，可以进行多义解析，为了顺利编代码，做一些自主的定义如下。

1. 每个格子可以朝上下左右四个方向移动，而不是朝八个方向，也不是只能向下或向右。
2. 所谓的长短，我们这里仅仅定义为路径走过格子上数值的和。不考虑所谓高差、也不考虑走了多少格子（步数）

话不多说，直接上代码

是用python写的，主干部分借鉴了CSDN一篇博文的代码，在g_cost更新、可视化绘图等方面是自己写的。

"""
==========
A* Shortest Route on a Map
By WWW, SYSU_SAA_19_Doc
finished in 20200701
==========

"""
# 加载需要的各种包
import matplotlib.pyplot as plt
import numpy as np
from matplotlib import colors
from matplotlib.ticker import PercentFormatter
from matplotlib import cm
from matplotlib.colors import ListedColormap, LinearSegmentedColormap

# 定义参数
width=16
height=16
altitude=387

# 生成16*16的随机数矩阵,用来表示"高程"（并不是真的高程）ht
ht = np.random.randint(0,altitude,size=(width,height))


# 定义搜索到的节点类

'''每一个搜索到将到添加到OPEN集的节点，都会创建一个下面的节点类，保存有entry的位置信息（x，y），
计算得到的G值和F值，和该节点的父节点（pre_entry）。
在整个程序中，我们把节点叫做Entry
'''

class SearchEntry():
	def __init__(self, x, y, g_cost, f_cost=0, pre_entry=None):
		self.x = x
		self.y = y
		self.g_cost = g_cost  # 函数g
		self.f_cost = f_cost  # 函数f
		self.pre_entry = pre_entry #父节点
	# 确定当前节点的位置。
	def getPos(self):
		return (self.x, self.y)
    
# 定义地图类

class Map():
	def __init__(self, width, height):
		self.width = width
		self.height = height
		self.ht = np.random.randint(0, altitude, (width,height))
	
		

def AStarSearch(map, source, dest):# source是开始，dest是目标
	def getNewPosition(map, locatioin, offset):#向下一个节点移动
		x,y = (location.x + offset[0], location.y + offset[1])
		if x < 0 or x >= map.width or y < 0 or y >= map.height:
			return None
		return (x, y) #这就是这个函数输出的东西，一个xy值对，就是pos，就是节点
		
	def getPositions(map, location):
		offsets = [(-1,0), (0, -1), (1, 0), (0, 1)]
		poslist = []
		for offset in offsets:
			pos = getNewPosition(map, location, offset)
			if pos is not None:			
				poslist.append(pos)#往位置序列里面再加位置点
		return poslist
	
	# h函数，就用当前节点到目标点的斜线距离，简单粗暴
	def calHeuristic(pos, dest):
		return abs(dest.x - pos[0]) + abs(dest.y - pos[1])
		

	# check if the position is in list
	def isInList(list, pos):
		if pos in list:
			return list[pos]
			# print(list)
		return None
	
	# add available adjacent positions 这一段是抄的，就是把毗邻点在Openlist和ClosedList里腾挪
	def addAdjacentPositions(map, location, dest, openlist, closedlist):
		poslist = getPositions(map, location)
		for pos in poslist:
			# if position is already in closedlist, do nothing
			if isInList(closedlist, pos) is None:
				findEntry = isInList(openlist, pos)
				h_cost = calHeuristic(pos, dest)
				g_cost = location.g_cost + ht[pos[0],pos[1]]
				# print(pos)
				print(g_cost)                
				if findEntry is None :
					# if position is not in openlist, add it to openlist
					openlist[pos] = SearchEntry(pos[0], pos[1], g_cost, g_cost+h_cost, location)
				elif findEntry.g_cost > g_cost:
					# if position is in openlist and cost is larger than current one,
					# then update cost and previous position
					findEntry.g_cost = g_cost
					findEntry.f_cost = g_cost + h_cost
					findEntry.pre_entry = location

					
	
	# find a least cost position in openlist, return None if openlist is empty
	def getFastPosition(openlist):
		fast = None
		for entry in openlist.values():
			if fast is None:
				fast = entry
			elif fast.f_cost > entry.f_cost:
				fast = entry
		return fast

	openlist = {}
	closedlist = {}
	location = SearchEntry(source[0], source[1], 0.0)
	dest = SearchEntry(dest[0], dest[1], 0.0)
	openlist[source] = location
	route = [[dest.x,dest.y]] #用来记录路径上的Entry，倒着记，从目标开始逐个父节点记录
	while True:
		location = getFastPosition(openlist)
		if location is None: # not found valid path			
			print("can't find valid path")
			break;
		if location.x == dest.x and location.y == dest.y:
			break
		
		closedlist[location.getPos()] = location
		openlist.pop(location.getPos())
		addAdjacentPositions(map, location, dest, openlist, closedlist)
		
	while location.pre_entry is not None:   #把路径上的点都记录进route
		location = location.pre_entry
		# print(location.x,location.y)
		route.append([location.x,location.y])
	print(np.array(route)) #把路径上的点打印出来
	# plt.scatter(np.array(route).T[0],np.array(route).T[1],s=100) #用散点图的形式画出来
	# plt.show()
        
    # 把这个矩阵用灰度云图和散点阵画出来
	fig, ax = plt.subplots(figsize=(10, 10))
	ax.imshow(altitude-ht, cmap='gray')
	for i in range(0,width):
		for j in range(0,height):
			text = ax.text(j, i, ht[i, j])
	fig.tight_layout()
	ax.scatter(np.array(route).T[1],np.array(route).T[0],s=200)
	plt.show()  
    

# 接下来要正式运行喽
map = Map(width, height)
source = (0,0)
dest = (15,15)
print("source:", source)
print("dest:", dest)
AStarSearch(map, source, dest)

运行结果

地图是随机的，那么就放几个运行的结果

片