人工智能-A*算法-最优路径搜索实验-编程知识

上次学会了《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 = 0h0 = 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:continueelse:continueelse: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_scoreh_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] = 1return 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*算法找到的路径地图：")# 初始化 Pygamepygame.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)# 退出 Pygamepygame.quit()if __name__ == "__main__":draw_a_star_path()

五、A*算法寻找最优路径-运行效果