上次学会了《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*算法寻找最优路径-运行效果
标签：map,人工智能,列表,算法,最优,open,节点,rect,size From： https://www.cnblogs.com/xh2023/p/17909519.html