人工智能-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 = 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*算法寻找最优路径-运行效果