From ec370b0585f2239a678d83850bb5111ec2ecbc74 Mon Sep 17 00:00:00 2001 From: pzc4urgo8 <1610339677@qq.com> Date: Fri, 28 Nov 2025 20:39:58 +0800 Subject: [PATCH] ADD file via upload --- aa.py | 168 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 168 insertions(+) create mode 100644 aa.py diff --git a/aa.py b/aa.py new file mode 100644 index 0000000..e09e475 --- /dev/null +++ b/aa.py @@ -0,0 +1,168 @@ +import numpy as np +import matplotlib.pyplot as plt +import heapq +import random +import time + +# 计算曼哈顿距离 +def heuristic(a, b): + return abs(a[0] - b[0]) + abs(a[1] - b[1]) + +# A* 算法 +def a_star(start, goal, grid): + open_list = [] + closed_list = set() + + heapq.heappush(open_list, (0 + heuristic(start, goal), 0, start, None)) + + came_from = {} + g_score = {start: 0} + + while open_list: + _, current_g, current, parent = heapq.heappop(open_list) + + if current == goal: + path = [] + while current: + path.append(current) + current = came_from.get(current) + return path[::-1] + + closed_list.add(current) + + for neighbor in [(0, 1), (1, 0), (0, -1), (-1, 0)]: + neighbor_pos = (current[0] + neighbor[0], current[1] + neighbor[1]) + + if (0 <= neighbor_pos[0] < len(grid)) and (0 <= neighbor_pos[1] < len(grid[0])) and grid[neighbor_pos[0]][neighbor_pos[1]] == 0: + if neighbor_pos in closed_list: + continue + + tentative_g_score = current_g + 1 + + if neighbor_pos not in g_score or tentative_g_score < g_score[neighbor_pos]: + g_score[neighbor_pos] = tentative_g_score + f_score = tentative_g_score + heuristic(neighbor_pos, goal) + heapq.heappush(open_list, (f_score, tentative_g_score, neighbor_pos, current)) + came_from[neighbor_pos] = current + + return None + +# 绘制网格 +def draw_grid(ax,grid, path=None, open_list=None, closed_list=None, start=None, goal=None): + grid_size = len(grid) + # fig, ax = plt.subplots(figsize=(grid_size, grid_size)) + ax.clear() + ax.set_xticks(np.arange(grid_size + 2) - 0.5, minor=True) + ax.set_yticks(np.arange(grid_size + 2) - 0.5, minor=True) + + + ax.grid(which='minor', color='gray', linestyle='--', linewidth=1) + + # 绘制每个单元格 + for i in range(grid_size): + for j in range(grid_size): + if grid[i][j] == 1: + ax.add_patch(plt.Rectangle((j, i), 1, 1, color='black')) # 障碍物 + elif (i, j) == start: + ax.add_patch(plt.Rectangle((j, i), 1, 1, color='red')) # 起点 + elif (i, j) == goal: + ax.add_patch(plt.Rectangle((j, i), 1, 1, color='blue')) # 终点 + elif open_list and (i, j) in open_list: + ax.add_patch(plt.Rectangle((j, i), 1, 1, color='purple')) # 开放列表 + elif path and (i, j) in path: + ax.add_patch(plt.Rectangle((j, i), 1, 1, color='green')) # 路径 + elif closed_list and (i, j) in closed_list: + ax.add_patch(plt.Rectangle((j, i), 1, 1, color='lightblue')) # 闭合列表 + # ax.add_patch(plt.plot(path(1),path(2), label='Sine Wave', color='blue', linestyle='-', linewidth=1)) + # ax.set_xticks([]) + # ax.set_yticks([]) + # plt.show() + plt.draw() # 更新图形 + plt.pause(0.1) # 暂停0.1秒，以便视觉上可以观察到变化 + +def generate_random_grid(size, obstacle_probability=0.45): + """ + 生成一个随机的网格，指定障碍物的概率。 + size: 网格的大小，大小为 size x size + obstacle_probability: 每个单元格成为障碍物的概率 + """ + grid = np.random.choice([0, 1], size=(size, size), p=[1 - obstacle_probability, obstacle_probability]) + return grid + +def save_grid_to_txt(grid, filename="grid_data.txt"): + """将二维网格保存到txt文件""" + with open(filename, "w", encoding="utf-8") as f: + for row in grid: + # 将每行元素转为字符串，用空格分隔，末尾加换行 + row_str = " ".join(map(str, row)) + "\n" + f.write(row_str) + print(f"网格数据已保存到 {filename}") +def read_grid_from_txt(filename="grid_data.txt"): + """从txt文件读取网格数据""" + with open(filename, "r", encoding="utf-8") as f: + grid = [] + for line in f: + # 去除换行符，按空格分割为整数列表 + row = list(map(int, line.strip().split())) + grid.append(row) + return np.array(grid) + +# 验证读取 +# saved_grid = read_grid_from_txt("correlated_01_grid.txt") +# print(saved_grid) # 输出与保存的网格一致 + +def main(): + # 网格大小 + grid_size = 10 # 你可以修改为任意大小 + while True: + #grid = generate_random_grid(grid_size, obstacle_probability=0.45) + grid = read_grid_from_txt(filename="./correlated_01_grid.txt") + # 随机选择起点和终点 + start = (0, 0) + goal = (grid_size - 1, grid_size - 1) + + # 确保起点和终点不是障碍物 + grid[start] = 0 + grid[goal] = 0 + + # 执行A*算法 + path = a_star(start, goal, grid) + if path: + break + else: + print('try again') + save_grid_to_txt(grid, filename="./correlated_01_grid.txt") + # # 可视化A*算法的搜索过程 + # if path: + # for step in path: + # draw_grid(grid, path=path, open_list=[step], closed_list=[], start=start, goal=goal) + # time.sleep(0.1) # 暂停一段时间以便观察过程 + # print("路径找到:", path) + # else: + # print("没有找到路径") + if path: + print(path) + else: + return 1 + # 拆分x和y坐标 + x_vals, y_vals = zip(*path) + x_vals = [x + 0.5 for x in x_vals] + y_vals = [y + 0.5 for y in y_vals] + + # 初始化绘图 + fig, ax = plt.subplots(figsize=(grid_size+1, grid_size+1)) + # 循环更新路径 + for i in range(len(path)): + # plt.clf() # 清除图形内容 + draw_grid(ax,grid, path=path[:i+1], open_list=[path[i]], closed_list=[], start=start, goal=goal) + # time.sleep(0.1) # 暂停0.5秒以便观察过程 + # 绘制路径 + + plt.plot(y_vals,x_vals, marker='o', color='b', linestyle='-', markersize=5, label='Path') + plt.draw() # 更新图形 + plt.show() + # 绘制路径 + + +if __name__ == "__main__": + main()