python路径规划仿真实验_[python] A*算法基于栅格地图的全局路径规划

#所有节点的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):"