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

# 所有节点的g值并没有初始化为无穷大

# 当两个子节点的f值一样时，程序选择最先搜索到的一个作为父节点加入closed

# 对相同数值的不同对待，导致不同版本的A*算法找到等长的不同路径

# 最后closed表中的节点很多，如何找出最优的一条路径

# 撞墙之后产生较多的节点会加入closed表，此时开始删除closed表中不合理的节点，1.1版本的思路

# 1.2版本思路，建立每一个节点的方向指针，指向f值最小的上个节点

# 参考《无人驾驶概论》、《基于A*算法的移动机器人路径规划》王淼驰，《人工智能及应用》鲁斌

import numpy

from pylab import *

import copy

# 定义一个含有障碍物的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] = 7

map_grid[15, 15] = 5

class AStar(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([[], []]) # 回溯路径表

def h_value_tem(self, son_p):

"""

计算拓展节点和终点的h值

:param son_p:子搜索节点坐标

:return:

"""

h = (son_p[0] - self.goal[0]) ** 2 + (son_p[1] - self.goal[1]) ** 2

h = numpy.sqrt(h) # 计算h

return h

# 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

def g_accumulation(self, son_point, father_point):

&