A*算法实现最优路径规划

1.  用A*算法实现最优路径规划，绿色五角星为起点，红色四角星为终点，黑色方块为障碍物，如下图所示。简要介绍问题的估价函数、算法步骤、搜索路径、代码实现和代码运行结果。

   

2. import math
   from random import randint
   import pygame
   from enum import Enum
   # 定义全局变量：地图中节点的像素大小
   CELL_WIDTH = 50  # 单元格宽度
   CELL_HEIGHT = 50  # 单元格长度
   BORDER_WIDTH = 1  # 边框宽度
   BLOCK_NUM = 5  # 地图中的障碍物数量
   class Color(Enum):
       ''' 颜色 '''
       RED = (255, 0, 0)
       GREEN = (0, 255, 0)
       BLUE = (0, 0, 255)
       WHITE = (255, 255, 255)
       BLACK = (0, 0, 0)
       GREY = (128, 128, 128)
       @staticmethod
       def random_color():
           '''设置随机颜色'''
           r = randint(0, 255)
           g = randint(0, 255)
           b = randint(0, 255)
           return (r, g, b)
   class Map(object):
       def __init__(self, mapsize):
           self.mapsize = mapsize
       def generate_cell(self, cell_width, cell_height):
           '''
           定义一个生成器，用来生成地图中的所有节点坐标
           :param cell_width: 节点宽度
           :param cell_height: 节点长度
           :return: 返回地图中的节点
           '''
           x_cell = -cell_width
           for num_x in range(self.mapsize[0] // cell_width):
               y_cell = -cell_height
               x_cell += cell_width
               for num_y in range(self.mapsize[1] // cell_height):
                   y_cell += cell_height
                   yield (x_cell, y_cell)
   class Node(object):
       def __init__(self, pos):
           self.pos = pos
           self.father = None
           self.gvalue = 0
           self.fvalue = 0
       def compute_fx(self, enode, father):
           if father == None:
               print('未设置当前节点的父节点！')
           gx_father = father.gvalue
   
           # 采用欧氏距离计算父节点到当前节点的距离
           # gx_f2n = math.sqrt((father.pos[0] - self.pos[0])**2 + (father.pos[1] - self.pos[1])**2)
           gx_f2n = abs(father.pos[0] - self.pos[0]) + abs(father.pos[1] - self.pos[1])
           gvalue = gx_f2n + gx_father
   
           # hx_n2enode = math.sqrt((self.pos[0] - enode.pos[0])**2 + (self.pos[1] - enode.pos[1])**2)
           hx_n2enode = abs(self.pos[0] - enode.pos[0]) + abs(self.pos[1] - enode.pos[1])
           fvalue = gvalue + hx_n2enode
           return gvalue, fvalue
   
       def set_fx(self, enode, father):
           self.gvalue, self.fvalue = self.compute_fx(enode, father)
           self.father = father
   
       def update_fx(self, enode, father):
           gvalue, fvalue = self.compute_fx(enode, father)
           if fvalue < self.fvalue:
               self.gvalue, self.fvalue = gvalue, fvalue
               self.father = father
   
   class AStar(object):
       def __init__(self, mapsize, pos_sn, pos_en):
           self.mapsize = mapsize  # 表示地图的投影大小，并非屏幕上的地图像素大小
           self.openlist, self.closelist, self.blocklist = [], [], []
           self.snode = Node(pos_sn)  # 用于存储路径规划的起始节点
           self.enode = Node(pos_en)  # 用于存储路径规划的目标节点
           self.cnode = self.snode  # 用于存储当前搜索到的节点
       def run(self):
           self.openlist.append(self.snode)
           while (len(self.openlist) > 0):
               # 查找openlist中fx最小的节点
               fxlist = list(map(lambda x: x.fvalue, self.openlist))
               index_min = fxlist.index(min(fxlist))
               self.cnode = self.openlist[index_min]
               del self.openlist[index_min]
               self.closelist.append(self.cnode)
               # 扩展当前fx最小的节点，并进入下一次循环搜索
               self.extend(self.cnode)
               # 如果openlist列表为空，或者当前搜索节点为目标节点，则跳出循环
               if len(self.openlist) == 0 or self.cnode.pos == self.enode.pos:
                   break
   
           if self.cnode.pos == self.enode.pos:
               self.enode.father = self.cnode.father
               return 1
           else:
               return -1
       def get_minroute(self):
           minroute = []
           current_node = self.enode
           while (True):
               minroute.append(current_node.pos)
               current_node = current_node.father
               if current_node.pos == self.snode.pos:
                   break
           minroute.append(self.snode.pos)
           minroute.reverse()
           return minroute
       def extend(self, cnode):
           nodes_neighbor = self.get_neighbor(cnode)
           for node in nodes_neighbor:
               # 判断节点node是否在closelist和blocklist中，因为closelist和blocklist中元素均为Node类，所以要用map函数转换为坐标集合
               if node.pos in list(map(lambda x: x.pos, self.closelist)) or node.pos in self.blocklist:
                   continue
               else:
                   if node.pos in list(map(lambda x: x.pos, self.openlist)):
                       node.update_fx(self.enode, cnode)
                   else:
                       node.set_fx(self.enode, cnode)
                       self.openlist.append(node)
       def setBlock(self, blocklist):
           '''
           获取地图中的障碍物节点，并存入self.blocklist列表中
           注意：self.blocklist列表中存储的是障碍物坐标，不是Node类
           :param blocklist:
           :return:
           '''
           self.blocklist.extend(blocklist)
       def get_neighbor(self, cnode):
           # offsets = [(-1,1),(0,1),(1,1),(-1,0),(1,0),(-1,-1),(0,-1),(1,-1)]
           offsets = [(-1, 0), (1, 0), (0, 1), (0, -1)]
           nodes_neighbor = []
           x, y = cnode.pos[0], cnode.pos[1]
           for os in offsets:
               x_new, y_new = x + os[0], y + os[1]
               pos_new = (x_new, y_new)
               # 判断是否在地图范围内,超出范围跳过
               if x_new < 0 or x_new > self.mapsize[0] - 1 or y_new < 0 or y_new > self.mapsize[1]:
                   continue
               nodes_neighbor.append(Node(pos_new))
           return nodes_neighbor
   def main():
       mapsize = tuple(map(int, input('请输入地图大小，以逗号隔开：').split(',')))
       pos_snode = tuple(map(int, input('请输入起点坐标，以逗号隔开：').split(',')))
       pos_enode = tuple(map(int, input('请输入终点坐标，以逗号隔开：').split(',')))
       myAstar = AStar(mapsize, pos_snode, pos_enode)
       blocklist = gen_blocks(mapsize[0], mapsize[1])
       myAstar.setBlock(blocklist)
       routelist = []  # 记录搜索到的最优路径
       if myAstar.run() == 1:
           routelist = myAstar.get_minroute()
           print(routelist)
           showresult(mapsize, pos_snode, pos_enode, blocklist, routelist)
       else:
           print('路径规划失败！')
   def gen_blocks(width, height):
       '''
       随机生成障碍物
       :param width: 地图宽度
       :param height: 地图高度
       :return:返回障碍物坐标集合
       '''
       i, blocklist = 0, []
   
       block = (1, 0)
       if block not in blocklist:
                   blocklist.append(block)
       block = (3, 1)
       if block not in blocklist:
           blocklist.append(block)
       block = (2, 3)
       if block not in blocklist:
           blocklist.append(block)
       block = (4, 3)
       if block not in blocklist:
           blocklist.append(block)
       block = (1, 5)
       if block not in blocklist:
           blocklist.append(block)
       return blocklist
   def showresult(mapsize, pos_sn, pos_en, blocklist, routelist):
       # 初始化导入的Pygame模块
       pygame.init()
       # 此处要将地图投影大小转换为像素大小，此处设地图中每个单元格的大小为CELL_WIDTH*CELL_HEIGHT像素
       mymap = Map((mapsize[0] * CELL_WIDTH, mapsize[1] * CELL_HEIGHT))
       pix_sn = (pos_sn[0] * CELL_WIDTH, pos_sn[1] * CELL_HEIGHT)
       pix_en = (pos_en[0] * CELL_WIDTH, pos_en[1] * CELL_HEIGHT)
       # 对blocklist和routelist中的坐标同样要转换为像素值
       bl_pix = list(map(transform, blocklist))
       rl_pix = list(map(transform, routelist))
       # 初始化显示的窗口并设置尺寸
       screen = pygame.display.set_mode(mymap.mapsize)
       # 设置窗口标题
       pygame.display.set_caption('A*算法路径搜索演示：')
       # 用白色填充屏幕3
       screen.fill(Color.WHITE.value)
       # 绘制屏幕中的所有单元格
       for (x, y) in mymap.generate_cell(CELL_WIDTH, CELL_HEIGHT):
           if (x, y) in bl_pix:
               # 绘制黑色的障碍物单元格，并留出2个像素的边框
               pygame.draw.rect(screen, Color.BLACK.value, (
               (x + BORDER_WIDTH, y + BORDER_WIDTH), (CELL_WIDTH - 2 * BORDER_WIDTH, CELL_HEIGHT - 2 * BORDER_WIDTH)))
           else:
               # 绘制绿色的可通行单元格，并留出2个像素的边框
               pygame.draw.rect(screen, Color.GREEN.value, (
               (x + BORDER_WIDTH, y + BORDER_WIDTH), (CELL_WIDTH - 2 * BORDER_WIDTH, CELL_HEIGHT - 2 * BORDER_WIDTH)))
       # 绘制起点和终点
       pygame.draw.circle(screen, Color.BLUE.value, (pix_sn[0] + CELL_WIDTH // 2, pix_sn[1] + CELL_HEIGHT // 2),
                          CELL_WIDTH // 2 - 1)
       pygame.draw.circle(screen, Color.RED.value, (pix_en[0] + CELL_WIDTH // 2, pix_en[1] + CELL_HEIGHT // 2),
                          CELL_WIDTH // 2 - 1)
   
       # 绘制搜索得到的最优路径
       for (x, y) in mymap.generate_cell(CELL_WIDTH, CELL_HEIGHT):
           if (x, y) in rl_pix and (x, y) != (pix_sn[0], pix_sn[1]) and (x, y) != (pix_en[0], pix_en[1]):
               pygame.draw.rect(screen, Color.GREY.value, (
               (x + BORDER_WIDTH, y + BORDER_WIDTH), (CELL_WIDTH - 2 * BORDER_WIDTH, CELL_HEIGHT - 2 * BORDER_WIDTH)))
       # pygame.draw.aalines(screen, Color.RED.value, False, rl_pix)
       keepGoing = True
       while keepGoing:
           pygame.time.delay(100)
           for event in pygame.event.get():
               if event.type == pygame.QUIT:
                   keepGoing = False
           pygame.display.flip()
   def transform(pos):
       xnew, ynew = pos[0] * CELL_WIDTH, pos[1] * CELL_HEIGHT
       return (xnew, ynew)
   if __name__ == '__main__':
       main()