#所有节点的g值并没有初始化为无穷大#当两个子节点的f值一样时,程序选择最先搜索到的一个作为父节点加入closed#对相同数值的不同对待,导致不同版本的A*算法找到等长的不同路径#最后closed表中的节点很多,如何找出最优的一条路径#撞墙之后产生较多的节点会加入closed表,此时开始删除closed表中不合理的节点,1.1版本的思路#1.2版本思路,建立每一个节点的方向指针,指向f值最小的上个节点#参考《无人驾驶概论》、《基于A*算法的移动机器人路径规划》王淼驰,《人工智能及应用》鲁斌
importnumpyfrom pylab import *
importcopy#定义一个含有障碍物的20×20的栅格地图#10表示可通行点#0表示障碍物#7表示起点#5表示终点
map_grid = numpy.full((20, 20), int(10), dtype=numpy.int8)
map_grid[3, 3:8] =0
map_grid[3:10, 7] =0
map_grid[10, 3:8] =0
map_grid[17, 13:17] =0
map_grid[10:17, 13] =0
map_grid[10, 13:17] =0
map_grid[5, 2] = 7map_grid[15, 15] = 5
classAStar(object):"""创建一个A*算法类"""
def __init__(self):"""初始化"""
#self.g = 0 # g初始化为0
self.start = numpy.array([5, 2]) #起点坐标
self.goal = numpy.array([15, 15]) #终点坐标
self.open = numpy.array([[], [], [], [], [], []]) #先创建一个空的open表, 记录坐标,方向,g值,f值
self.closed = numpy.array([[], [], [], [], [], []]) #先创建一个空的closed表
self.best_path_array = numpy.array([[], []]) #回溯路径表
defh_value_tem(self, son_p):"""计算拓展节点和终点的h值
:param son_p:子搜索节点坐标
:return:"""h= (son_p[0] - self.goal[0]) ** 2 + (son_p[1] - self.goal[1]) ** 2h= numpy.sqrt(h) #计算h
returnh#def g_value_tem(self, son_p, father_p):
#"""
#计算拓展节点和父节点的g值
#其实也可以直接用1或者1.414代替
#:param son_p:子节点坐标
#:param father_p:父节点坐标,也就是self.current_point
#:return:返回子节点到父节点的g值,但不是全局g值
#"""
#g1 = father_p[0] - son_p[0]
#g2 = father_p[1] - son_p[1]
#g = g1 ** 2 + g2 ** 2
#g = numpy.sqrt(g)
#return g
defg_accumulation(self, son_point, father_point):"