当前位置 主页 > 网站技术 > 代码类 >

    Python3 A*寻路算法实现方式

    栏目:代码类 时间:2019-12-25 06:08

    我就废话不多说了,直接上代码吧!

    # -*- coding: utf-8 -*-
    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()