上次学会了《A*算法-八数码问题》，初步了解了A*算法的原理，本次再用A*算法完成一个最优路径搜索实验。
一、实验内容
1. 设计自己的启发式函数。
2. 在网格地图中，设计部分障碍物。
3. 实现A*算法，搜索一条最优路径。
二、A*算法实现步骤
1. 初始化：设置起始节点和目标节点，并创建一个open列表和一个closed列表。open列表用于存储待检查的节点，closed列表用于存储已经检查过的节点。
2. 开始循环，直到open列表为空：
2.1. 从open列表中取出f值最小的节点（这个值是由g值（从起始节点到当前节点的实际距离）和h值（从当前节点到目标节点的启发式估计距离）之和组成的）。
2.2. 如果这个节点是目标节点，那么就找到了最优路径，结束循环。
2.3. 否则，将这个节点从open列表移动到closed列表，并检查它的所有邻居节点。
2.4. 对于每一个邻居节点：
i. 如果它已经在closed列表中，那么跳过它。
ii. 如果它不在open列表中，那么添加它，并计算它的g值和f值。
iii. 如果它已经在open列表中，那么比较g值：如果当前节点有更小的g值，那么更新它的g值和f值。
3. 如果open列表为空（没有找到路径），那么算法失败。
4. 返回从起始节点到目标节点的最短路径。
三、启发式估算距离方法
1, 曼哈顿距离计算公式
2, 欧几里得距离计算公式
3, 定义启发式函数
# 曼哈顿距离计算函数（上下左右4个方向移动） def manhattan_distance(pos1, pos2): # 曼哈顿距离计算; D = |X1-X2|+|Y1-Y2| ;两点X,Y值差的绝对值和 return abs(pos1[0] - pos2[0]) + abs(pos1[1] - pos2[1]) # 欧几里得距离的计算函数（上下左右及其对角线8个方向移动） # 相对来说，欧几里得距离计算函数更准确点 def euclidean_distance(pos1, pos2): # 欧几里得距离计算; D = 开平方根( (X1-X2)^2 + (Y1-Y2)^2 ) ;两点X,Y值差的平方和，再开平方根，即2点间的绝对距离。 return math.sqrt((pos1[0] - pos2[0]) ** 2 + (pos1[1] - pos2[1]) ** 2) # 定义启发式函数，这里只使用欧几里得距离作为估价值 def heuristic(pos1, pos2): return euclidean_distance(pos1, pos2)
四、A*算法寻找最优路径
1，根据欧几里得距离计算寻找最优路径示例图
2，A*算法寻找最优路径方法
# 定义A*算法 def a_star_path(array, start, goal): # 所有可能的移动方向，包括对角线方向 neighbors = [(0, 1), (0, -1), (1, 0), (-1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)] # 只考虑上下左右移动方向，不包括对角线方向 # neighbors = [(0, 1), (0, -1), (1, 0), (-1, 0)] ''' 计算每个节点的总评分f值（f(n) = g(n) + h(n)）来寻找最优路径 g值（从起始节点到下一个移动方向节点的实际距离） h值（从下一个移动方向到目标节点的估计距离，使用启发式heuristic函数计算） ''' # 起点的g,f初始值 g0 = 0 h0 = heuristic(start, goal) gscore = {start: g0} fscore = {start: g0 + h0} # 定义一个优先队列，open列表，用于存储待检查的节点，open列表的优先级由f值决定 open_list = [] # 关闭列表（表示已经访问过），用于存储已经检查过的节点，以避免重复检查 close_set = set() # 从哪里来字典，记录节点来源，当成父节点 come_from = {} ''' heapq实现了一个适合与Python的列表一起使用的最小堆排序算法。 heapq.heappush()，创建一个堆；当数据源添加新项时，将维护元素的堆排序顺序。 heapq.heappop()，弹出并返回堆中的最小项。 将起点加入open列表，fscore[start]为起点的优先级f值，即启发式函数估算值 ''' # 将起点加入open列表，fscore[start]为起点的优先级f值，即启发式函数估算值 heapq.heappush(open_list, (fscore[start], start)) # 遍历open列表 while open_list: # 取出f值最小的节点作为当前节点 current = heapq.heappop(open_list)[1] # 如果到达目标点，返回路径 if current == goal: data = [] # 从目标点向起点遍历路径 while current in come_from: # 将当前点的位置加入路径 data.append(current) # 将当前点设为从哪里来的节点，继续向上遍历 current = come_from[current] # 将起始点的位置也加入路径 data.append(start) # 将路径反转，因为我们是从目标向起点遍历的，所以需要反转得到真正的路径 return data[::-1] # 将当前节点加入closed列表，表示已经检查过该节点了 close_set.add(current) # 对当前节点的所有移动方向进行遍历 for i, j in neighbors: # 根据当前节点的位置计算邻居节点的位置 neighbor = current[0] + i, current[1] + j # 从当前节点到邻居节点的距离，作为 tentative_g_score 值（暂时的g值） tentative_g_score = gscore[current] + heuristic(current, neighbor) # 如果邻居节点在地图之外，跳过该邻居节点 if 0 <= neighbor[0] < array.shape[0]: if 0 <= neighbor[1] < array.shape[1]: # 如果邻居节点是障碍物，跳过该邻居节点 if array[neighbor[0]][neighbor[1]] == 1: continue else: continue else: continue ''' 如果邻居节点在closed列表中，并且从当前节点到邻居节点的距离比之前计算的距离长， 则跳过该邻居节点 ''' if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0): continue ''' 如果从当前节点到邻居节点的距离比之前计算的更短，或者邻居节点不在open列表中的话 更新邻居节点的g值和f值，并将邻居节点重新加入open列表中（因为g值发生了变化） ''' if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1] for i in open_list]: come_from[neighbor] = current # ''' 更新邻居节点的g值和f值 计算每个节点的总评分f值（f(n) = g(n) + h(n)）来寻找最优路径 g值（从起始节点到下一个移动方向节点的实际距离） h值（从下一个移动方向到目标节点的估计距离，使用启发式heuristic函数计算） ''' gscore[neighbor] = tentative_g_score h_score = heuristic(neighbor, goal) fscore[neighbor] = tentative_g_score + h_score # 将邻居节点重新加入open列表中 heapq.heappush(open_list, (fscore[neighbor], neighbor)) return False
3，绘制网格地图和障碍物
# 定义网格矩阵长宽 map_size = (20, 20) # 设置起点和终点 start = (0, 0) end = (map_size[0] - 1, map_size[1] - 1) # 随机生成网格矩阵障碍物位置 def generate_obstacles(map_size): # 生成障碍物位置个数在10到20之间的随机数 num_obstacles = random.randint(map_size[0], (map_size[0] / 4) * (map_size[1] / 4)) # 生成所有可能的位置 all_positions = [(i, j) for i in range(map_size[0]) for j in range(map_size[1])] # print(all_positions) # 去除起始点 all_positions.remove(start) all_positions.remove(end) # 从所有可能的位置中随机选择一些位置作为障碍物的位置 obstacles = random.sample(all_positions, num_obstacles) # print(obstacles) return obstacles # 随机生成障碍物位置列表 obstacles = generate_obstacles(map_size) # 定义屏幕一个格子大小 CELL_SIZE = 20 # 定义屏幕宽高大小 WIDTH, HEIGHT = map_size[0] * CELL_SIZE, map_size[1] * CELL_SIZE # 定义颜色 WHITE = (255, 255, 255) RED = (255, 0, 0) BLUE = (0, 0, 255) YELLOW = (255, 255, 0) BLACK = (0, 0, 0) # 初始化网格地图 def grid_init(map_size, obstacles): # 创建网格地图 grid = [[0 for x in range(map_size[0])] for y in range(map_size[1])] for x, y in obstacles: # 设置障碍物 grid[x][y] = 1 return grid # 打印网格地图 def grid_print(grid): for line in grid: print(line) # 绘制主地图 def draw_grid(pygame, screen): for i in range(0, WIDTH, CELL_SIZE): pygame.draw.line(screen, BLACK, (i, 0), (i, HEIGHT)) for i in range(0, HEIGHT, CELL_SIZE): pygame.draw.line(screen, BLACK, (0, i), (WIDTH, i)) for obs in obstacles: # 设置矩形的位置和大小 rect_pos = (obs[1] * CELL_SIZE, obs[0] * CELL_SIZE) rect_size = (CELL_SIZE, CELL_SIZE) # 使用pygame.draw.rect绘制矩形 pygame.draw.rect(screen, BLUE, (rect_pos[0], rect_pos[1], rect_size[0], rect_size[1])) # 绘制A*算法找到的路径 def draw_a_star_path(): print("绘制A*算法找到的路径地图：") # 初始化 Pygame pygame.init() # 创建一个窗口（屏幕）对象 screen = pygame.display.set_mode((WIDTH, HEIGHT)) # 窗口描述 pygame.display.set_caption("A星算法动画演示") # 定义字体 font = pygame.font.Font(None, 30) # 填充屏幕背景为白色 screen.fill(WHITE) # 绘制地图 draw_grid(pygame, screen) # 更新显示屏幕 pygame.display.flip() # 初始化网格地图 grid = grid_init(map_size, obstacles) print("初始化网格地图：") # 打印网格地图 grid_print(grid) # 设置起点和终点 # start = (0, 0) # end = (map_size[0] - 1, map_size[1] - 1) # 执行A*算法 path = a_star_path(np.array(grid), start, end) print("最优路径为：") print(path) # 循环刷新地图，显示最优路径 for node in path: # 设置矩形的位置和大小 rect_pos = (node[1] * CELL_SIZE, node[0] * CELL_SIZE) rect_size = (CELL_SIZE, CELL_SIZE) # 使用pygame.draw.rect绘制矩形 pygame.draw.rect(screen, RED, (rect_pos[0], rect_pos[1], rect_size[0], rect_size[1])) # 更新显示屏幕 pygame.display.flip() # 间隔0.5秒刷新地图 time.sleep(0.5) # 退出 Pygame pygame.quit() if __name__ == "__main__": draw_a_star_path()
五、A*算法寻找最优路径-运行效果
