Python3 A*寻路算法实现

import math import random import copy import time import sys import tkinter import threading # 地图 tm = [ '############################################################', '#S............................#............#.....#.........#', '#..........#..................#......#.....#.....#.........#', '#..........#..................#......#.....#.....#.........#', '#..........#..................#......#.....#.....#.........#', '#..........#.........................#.....#.....#.........#', '#..........#..................#......#.....#...............#', '#..#########..................#......#.....#.....#.........#', '#..#..........................#......#.....#.....#.........#', '#..#..........................#......#.....#.....#.........#', '#..############################......#.....#.....#.........#', '#.............................#......#.....#.....#.........#', '#.............................#......#...........#.........#', '#######.##################################################.#', '#....#........#.................#.............#............#', '#....#........#........#........#.............#............#', '#....####.#####........#........#.............#............#', '#.........#............#........#.............#............#', '#.........#............#........#.............#............#', '#.........#............#........#.............#............#', '#.........#............#........#.............#............#', '#.........#............#........#.............#............#', '#.........#............#........####.#######.##............#', '#.........#............#........#....#.......#.............#', '#.........#............#........#....#.......#.............#', '#......................#........#....#.......#.............#', '#.........#............#........##.########..#.............#', '#.........#............#..................#..########.######', '#.........#............#..................#...............E#', '############################################################'] # 存储搜索时的地图 test_map = [] #----------- 开放列表和关闭列表的元素类型，parent用来在成功的时候回溯路径 ----------- class Node_Elem: def __init__(self, parent, x, y, dist): self.parent = parent # 回溯父节点 self.x = x # x坐标 self.y = y # y坐标 self.dist = dist # 从起点到此位置的实际距离 #----------- A*算法 ----------- class A_Star: def __init__(self, root, s_x, s_y, e_x, e_y, w=60, h=30): self.s_x = s_x # 起点x self.s_y = s_y # 起点y self.e_x = e_x # 终点x self.e_y = e_y # 终点y self.open = [] # open表 self.close = [] # close表 self.path = [] # path表 # 创建画布 self.root = root # 画布根节点 self.width = w # 地图w，默认60 self.height = h # 地图h，默认30 self.__r = 3 # 半径 # Tkinter.Canvas self.canvas = tkinter.Canvas( root, width=self.width * 10 + 100, height=self.height * 10 + 100, bg="#EBEBEB", # 背景白色 xscrollincrement=1, yscrollincrement=1 ) self.canvas.pack(expand=tkinter.YES, fill=tkinter.BOTH) self.title("A*迷宫算法(e:开始搜索或退出)") self.__bindEvents() self.new() # 按键响应程序 def __bindEvents(self): self.root.bind("e", self.quite) # 退出程序 # 退出程序 def quite(self, evt): self.root.destroy() # 更改标题 def title(self, s): self.root.title(s) # 初始化 def new(self): node = self.canvas.create_oval(100 - self.__r, 20 - self.__r, 100 + self.__r, 20 + self.__r, fill="#ff0000", outline="#ffffff", tags="node", ) self.canvas.create_text(130, 20, text=u'Wall', fill='black' ) node = self.canvas.create_oval(200 - self.__r, 20 - self.__r, 200 + self.__r, 20 + self.__r, fill="#00ff00", outline="#ffffff", tags="node", ) self.canvas.create_text(230, 20, text=u'Path', fill='black' ) node = self.canvas.create_oval(300 - self.__r, 20 - self.__r, 300 + self.__r, 20 + self.__r, fill="#AAAAAA", outline="#ffffff", tags="node", ) self.canvas.create_text(330, 20, text=u'Searched', fill='black' ) for i in range(self.width): for j in range(self.height): # 生成障碍节点，半径为self.__r if test_map[j][i] == '#': node = self.canvas.create_oval(i * 10 + 50 - self.__r, j * 10 + 50 - self.__r, i * 10 + 50 + self.__r, j * 10 + 50 + self.__r, fill="#ff0000", # 填充红色 outline="#ffffff", # 轮廓白色 tags="node", ) # 显示起点 if test_map[j][i] == 'S': node = self.canvas.create_oval(i * 10 + 50 - self.__r, j * 10 + 50 - self.__r, i * 10 + 50 + self.__r, j * 10 + 50 + self.__r, fill="#00ff00", # 填充绿色 outline="#ffffff", # 轮廓白色 tags="node", ) self.canvas.create_text(i * 10 + 50, j * 10 + 50 - 20, # 使用create_text方法在坐标处绘制文字 text=u'Start', # 所绘制文字的内容 fill='black' # 所绘制文字的颜色为灰色 ) # 显示终点 if test_map[j][i] == 'E': node = self.canvas.create_oval(i * 10 + 50 - self.__r, j * 10 + 50 - self.__r, i * 10 + 50 + self.__r, j * 10 + 50 + self.__r, fill="#00ff00", # 填充绿色 outline="#ffffff", # 轮廓白色 tags="node", ) self.canvas.create_text(i * 10 + 50, j * 10 + 50 + 20, # 使用create_text方法在坐标处绘制文字 text=u'End', # 所绘制文字的内容 fill='black' # 所绘制文字的颜色为灰色 ) # 生成路径节点，半径为self.__r if test_map[j][i] == '*': node = self.canvas.create_oval(i * 10 + 50 - self.__r, j * 10 + 50 - self.__r, i * 10 + 50 + self.__r, j * 10 + 50 + self.__r, fill="#0000ff", # 填充蓝色 outline="#ffffff", # 轮廓白色 tags="node", ) # 生成搜索区域，半径为self.__r if test_map[j][i] == ' ': node = self.canvas.create_oval(i * 10 + 50 - self.__r, j * 10 + 50 - self.__r, i * 10 + 50 + self.__r, j * 10 + 50 + self.__r, fill="#AAAAAA", # 填充白色 outline="#ffffff", # 轮廓白色 tags="node", ) # 查找路径的入口函数 def find_path(self): # 构建开始节点 p = Node_Elem(None, self.s_x, self.s_y, 0.0) while True: # 扩展节点 self.extend_round(p) # 如果open表为空，则不存在路径，返回 if not self.open: return # 取F值最小的节点 idx, p = self.get_best() # 到达终点，生成路径，返回 if self.is_target(p): self.make_path(p) return # 把此节点加入close表，并从open表里删除 self.close.append(p) del self.open[idx] # 生成路径 def make_path(self, p): # 从结束点回溯到开始点，开始点的parent == None while p: self.path.append((p.x, p.y)) p = p.parent # 判断是否为终点 def is_target(self, i): return i.x == self.e_x and i.y == self.e_y # 取F值最小的节点 def get_best(self): best = None bv = 10000000 # MAX值 bi = -1 for idx, i in enumerate(self.open): value = self.get_dist(i) if value < bv: best = i bv = value bi = idx return bi, best # 求距离 def get_dist(self, i): # F = G + H # G 为当前路径长度，H为估计长度 return i.dist + math.sqrt((self.e_x - i.x) * (self.e_x - i.x)) + math.sqrt((self.e_y - i.y) * (self.e_y - i.y)) # 扩展节点 def extend_round(self, p): # 八个方向移动 xs = (-1, 0, 1, -1, 1, -1, 0, 1) ys = (-1, -1, -1, 0, 0, 1, 1, 1) # 上下左右四个方向移动 xs = (0, -1, 1, 0) ys = (-1, 0, 0, 1) for x, y in zip(xs, ys): new_x, new_y = x + p.x, y + p.y # 检查位置是否合法 if not self.is_valid_coord(new_x, new_y): continue # 构造新的节点，计算距离 node = Node_Elem(p, new_x, new_y, p.dist + self.get_cost( p.x, p.y, new_x, new_y)) # 新节点在关闭列表，则忽略 if self.node_in_close(node): continue i = self.node_in_open(node) # 新节点在open表 if i != -1: # 当前路径距离更短 if self.open[i].dist > node.dist: # 更新距离 self.open[i].parent = p self.open[i].dist = node.dist continue # 否则加入open表 self.open.append(node) # 移动距离，直走1.0，斜走1.4 def get_cost(self, x1, y1, x2, y2): if x1 == x2 or y1 == y2: return 1.0 return 1.4 # 检查节点是否在close表 def node_in_close(self, node): for i in self.close: if node.x == i.x and node.y == i.y: return True return False # 检查节点是否在open表，返回序号 def node_in_open(self, node): for i, n in enumerate(self.open): if node.x == n.x and node.y == n.y: return i return -1 # 判断位置是否合法，超出边界或者为阻碍 def is_valid_coord(self, x, y): if x < 0 or x >= self.width or y < 0 or y >= self.height: return False return test_map[y][x] != '#' # 搜寻过的位置 def get_searched(self): l = [] for i in self.open: l.append((i.x, i.y)) for i in self.close: l.append((i.x, i.y)) return l # 获取起点坐标 def get_start_XY(): return get_symbol_XY('S') # 获取终点坐标 def get_end_XY(): return get_symbol_XY('E') # 查找特定元素 def get_symbol_XY(s): for y, line in enumerate(test_map): try: x = line.index(s) except: continue else: break return x, y # 标记路径位置 def mark_path(l): mark_symbol(l, '*') # 标记已搜索过的位置 def mark_searched(l): mark_symbol(l, ' ') # 标记函数 def mark_symbol(l, s): for x, y in l: test_map[y][x] = s # 标记起点和终点 def mark_start_end(s_x, s_y, e_x, e_y): test_map[s_y][s_x] = 'S' test_map[e_y][e_x] = 'E' # 将地图字符串转化为表 def tm_to_test_map(): for line in tm: test_map.append(list(line)) # 寻找路径 def find_path(): s_x, s_y = get_start_XY() e_x, e_y = get_end_XY() # A*算法 a_star = A_Star(tkinter.Tk(), s_x, s_y, e_x, e_y) a_star.root.mainloop() a_star.find_path() searched = a_star.get_searched() path = a_star.path # 标记已搜索过的位置 mark_searched(searched) # 标记路径位置 mark_path(path) # 标记起点和终点 mark_start_end(s_x, s_y, e_x, e_y) print(u"路径长度:%d" % (len(path))) print(u"搜索过的区域:%d" % (len(searched))) a_star = A_Star(tkinter.Tk(), s_x, s_y, e_x, e_y) a_star.root.mainloop() #----------- 程序的入口处 ----------- if __name__ == '__main__': print (u""" -------------------------------------------------------- 程序：A*迷宫问题程序 作者：Gm 日期：2019-7-08 语言：Python 3.7 -------------------------------------------------------- """) # 载入地图 tm_to_test_map() # 寻找路径 find_path()