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

     启发式探索是利用问题拥有的启发信息来引导搜索，达到减少探索范围、降低问题复杂度的目的。A*寻路算法是启发式探索的一个典型实践，在寻路搜索的过程中，给每个节点绑定了一个估计值(即启发式)，在对节点的遍历过程中采取估计值优先原则，估计值更优的节点会被优先遍历。

    1、A*算法基本原理

    A*算法是一种有序搜索算法，其特点在于对估价函数的定义上。公式表示为：  f(n)=g(n)+h(n)，其中， f(n)是从初始状态经由状态n到目标状态的代价估计，g(n) 是在状态空间中从初始状态到状态n的实际代价，h(n)是从状态n到目标状态的最佳路径的估计代价。对于路径搜索问题，状态就是图中的节点，代价就是距离。

    2、A*算法搜索步骤

    1. 算法步骤

    (1) 设置地图大小，起点S，终点E，障碍集合Blocklist。

    (2) 添加起点S到Openlist(待搜索集合)。

    (3) 将S取出，添加到Closelist(已搜索集合)。

    (4) 查找S所有相邻节点，添加到Openlist，并设置S为它们的父节点；以绿色初始节点右侧的灰色节点为例：f(n)=g(n)+h(n)。g(n)=1,绿色初始节点到该节点的移动步数；h(n)=3,灰色节点移动到红色终点的步数(曼哈顿距离)，也可以使用欧氏距离。f(n)=g(n)+h(n)=1+3=4,其他相邻节点计算相同。如图1所示，曼哈顿距离向4个方向移动，距离公式为：

    欧氏距离向8个方向移动，距离公式为：

    ■ 图 1步骤(4)示意图

    (5) 选择Openlist中f值最小点，有两个分别为绿色右侧节点和绿色下方节点，将右侧节点添加至Closelist，并设置绿色节点为其父节点，选择下方节点也可以，本例节点顺序右下左上，根据启发式规则相同，结果和搜索效率与选取顺序无关,如图2所示。

    ■ 图2 步骤(5)示意图

    (6) 此时Openlist中f的最小值是4，黄色相邻节点中只有上下节点可达，根据计算得到上下节点f值，此处f值为最小值，采用其他路径计算值均不小于4,如图3所示。

    ■ 图3 黄色相邻上下节点

    因此，选取黄色节点下方f值小的节点添加至Closelist，此时Openlist中f的最小值为4，继续选取节点添加至Openlist,如图4所示。

    ■ 图4 步骤(6)示意图

    接下来算法进入相同方式迭代过程。

    A*算法最终执行结果如图5所示。

    ■ 图5 结果示意图

    2. 路径搜索

    开始从红色节点逆推，红色节点的父节点为⑦号节点，⑦号节点的父节点为⑥号节点，⑥号节点的父节点为⑤号节点，⑤号节点的父节点为②号节点，⑤号节点是搜索到②号节点时添加到Openlist中的，并且一直未被更新，②号节点的父节点为①号节点，最终的搜索路径为：起点-①-②-⑤-⑥-⑦-终点。

    这里的搜索路径并不是最佳路径的唯一解，其中路径：起点-③-④-⑤-⑥-⑦和路径：起点-③-②-⑤-⑥-⑦都可以通过相应的算法求出，作为搜索的最佳路径，因为这些路径理论上是等同的，这里只以一种最佳路径作为演示。

    对于上例演示的情况中存在无论如何重新计算Openlist中节点的f值都不会更小，也就是无法进行更新操作，因此再举一个例子，演示更新搜索。

    现在考虑可以8个方向搜索，但是斜向搜索需要步数为4。

    选择Openlist中f值最小的节点，选择了右侧f值为4的节点，此时计算右侧f值为4的节点的相邻节点的f值，如图6所示。其中左侧绿色起始点已经添加进Closelist中，右侧三个为黑色节点，因此不考虑这4个节点，其他节点均已存在于Openlist中，现对其f值进行更新。

    (1) 先看左上角的相邻节点，通过黄色节点到达该节点，g(n) = 5，h(n)不变，f(n)反而更大了，因此不更新。左下角节点同理。

    基于蚁族算法的二维路径规划算法
    rar 5星 超过95%的资源 9KB
    下载

    (2) 上方节点，通过黄色节点计算g(n) = 2, h(n)不变，f(n) = 6 < 8。所以，更新这个节点的f值，并将其父节点修改为黄色节点。下方居中节点同理,如图7所示。

     

    3、使用Python实现上述流程

    (1) 绘制地图全貌：起点、终点、障碍和可通行节点。

    (2) 获得相邻节点。

    (3) 采用曼哈顿或欧氏距离计算h(n)。

    (4) 更新f值。

    (5) 绘制搜索最优路径

     

    4、最优路径规划

    尝试采用A*算法对图8所示的地图进行最优路径规划。

    战术导弹发射车最优路径规划算法研究.pdf
    pdf 0星 超过10%的资源 92KB
    下载

    ■ 图8 路径规划图

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

    (1) 已搜索的③号节点邻居节点添加至Openlist，Openlist中f最小值为6，根据右下左上顺序原则优先选取下方节点，如图B.1所示。

    附B.1 Step7示意图

    (2) 此时④号节点邻居节点添加至Openlist，Openlist中f最小值为6，根据右下左上顺序原则，优先选取右侧节点，如图B.2所示。

    附B.2  Step8示意图

    (3) 此时Openlist中f最小值为6，选择⑤号节点的右侧节点添加至Closelist，如图B.3所示。

    附B.3  Step9示意图

    (4) 选取⑥号节点右侧节点添加到Closelist，此时Openlist中已包含了红色终止节点，算法结束，如图B.4所示。

    附B.4  Step10示意图

    算法示例程序：

    1. import math
    2. from random import randint
    3. import pygame
    4. from enum import Enum
    5.  
    6. # 定义全局变量：地图中节点的像素大小
    7. CELL_WIDTH = 30 #单元格宽度
    8. CELL_HEIGHT = 30 #单元格长度
    9. BORDER_WIDTH = 1 #边框宽度
    10. BLOCK_NUM = 3 #地图中的障碍物数量
    11.  
    12. class Color(Enum):
    13.     ''' 颜色 '''
    14.     RED = (255, 0, 0)
    15.     GREEN = (0, 255, 0)
    16.     BLUE = (0, 0, 255)
    17.     WHITE = (255, 255, 255)
    18.     BLACK = (0, 0, 0)
    19.     GREY=(128,128,128)
    20.  
    21.     @staticmethod
    22.     def random_color():
    23.         '''设置随机颜色'''
    24.         r = randint(0, 255)
    25.         g = randint(0, 255)
    26.         b = randint(0, 255)
    27.         return (r, g, b)
    28.  
    29. class Map(object):
    30.     def __init__(self, mapsize):
    31.         self.mapsize = mapsize
    32.  
    33.     def generate_cell(self, cell_width, cell_height):
    34.         '''
    35.         定义一个生成器，用来生成地图中的所有节点坐标
    36.         :param cell_width: 节点宽度
    37.         :param cell_height: 节点长度
    38.         :return: 返回地图中的节点
    39.         '''
    40.         x_cell = -cell_width
    41.         for num_x in range(self.mapsize[0] // cell_width):
    42.             y_cell = -cell_height
    43.             x_cell += cell_width
    44.             for num_y in range(self.mapsize[1] // cell_height):
    45.                 y_cell += cell_height
    46.                 yield (x_cell, y_cell)
    47.  
    48. class Node(object):
    49.     def __init__(self, pos):
    50.         self.pos = pos
    51.         self.father = None
    52.         self.gvalue = 0
    53.         self.fvalue = 0
    54.  
    55.     def compute_fx(self, enode, father):
    56.         if father == None:
    57.             print('未设置当前节点的父节点！')
    58.  
    59.         gx_father = father.gvalue
    60.         #采用欧氏距离计算父节点到当前节点的距离
    61.         #gx_f2n = math.sqrt((father.pos[0] - self.pos[0])**2 + (father.pos[1] - self.pos[1])**2)
    62.         gx_f2n = abs(father.pos[0] - self.pos[0]) + abs(father.pos[1] - self.pos[1])
    63.         gvalue = gx_f2n + gx_father
    64.  
    65.         #hx_n2enode = math.sqrt((self.pos[0] - enode.pos[0])**2 + (self.pos[1] - enode.pos[1])**2)
    66.         hx_n2enode = abs(self.pos[0] - enode.pos[0])+ abs(self.pos[1] - enode.pos[1])
    67.         fvalue = gvalue + hx_n2enode
    68.         return gvalue, fvalue
    69.  
    70.     def set_fx(self, enode, father):
    71.         self.gvalue, self.fvalue = self.compute_fx(enode, father)
    72.         self.father = father
    73.  
    74.     def update_fx(self, enode, father):
    75.         gvalue, fvalue = self.compute_fx(enode, father)
    76.         if fvalue < self.fvalue:
    77.             self.gvalue, self.fvalue = gvalue, fvalue
    78.             self.father = father
    79.  
    80. class AStar(object):
    81.     def __init__(self, mapsize, pos_sn, pos_en):
    82.         self.mapsize = mapsize #表示地图的投影大小，并非屏幕上的地图像素大小
    83.         self.openlist, self.closelist, self.blocklist = [], [], []
    84.         self.snode = Node(pos_sn) #用于存储路径规划的起始节点
    85.         self.enode = Node(pos_en) #用于存储路径规划的目标节点
    86.         self.cnode = self.snode #用于存储当前搜索到的节点
    87.  
    88.     def run(self):
    89.         self.openlist.append(self.snode)
    90.         while(len(self.openlist) > 0):
    91.             #查找openlist中fx最小的节点
    92.             fxlist = list(map(lambda x: x.fvalue, self.openlist))
    93.             index_min = fxlist.index(min(fxlist))
    94.             self.cnode = self.openlist[index_min]
    95.             del self.openlist[index_min]
    96.             self.closelist.append(self.cnode)
    97.  
    98.             # 扩展当前fx最小的节点，并进入下一次循环搜索
    99.             self.extend(self.cnode)
    100.             # 如果openlist列表为空，或者当前搜索节点为目标节点，则跳出循环
    101.             if len(self.openlist) == 0 or self.cnode.pos == self.enode.pos:
    102.                 break
    103.  
    104.         if self.cnode.pos == self.enode.pos:
    105.             self.enode.father = self.cnode.father
    106.             return 1
    107.         else:
    108.             return -1
    109.  
    110.     def get_minroute(self):
    111.         minroute = []
    112.         current_node = self.enode
    113.  
    114.         while(True):
    115.             minroute.append(current_node.pos)
    116.             current_node = current_node.father
    117.             if current_node.pos == self.snode.pos:
    118.                 break
    119.  
    120.         minroute.append(self.snode.pos)
    121.         minroute.reverse()
    122.         return minroute
    123.  
    124.     def extend(self, cnode):
    125.         nodes_neighbor = self.get_neighbor(cnode)
    126.         for node in nodes_neighbor:
    127.             #判断节点node是否在closelist和blocklist中，因为closelist和blocklist中元素均为Node类，所以要用map函数转换为坐标集合
    128.             if node.pos in list(map(lambda x:x.pos, self.closelist)) or node.pos in self.blocklist:
    129.                 continue
    130.             else:
    131.                 if node.pos in list(map(lambda x:x.pos, self.openlist)):
    132.                     node.update_fx(self.enode, cnode)
    133.                 else:
    134.                     node.set_fx(self.enode, cnode)
    135.                     self.openlist.append(node)
    136.  
    137.     def setBlock(self, blocklist):
    138.         '''
    139.         获取地图中的障碍物节点，并存入self.blocklist列表中
    140.         注意：self.blocklist列表中存储的是障碍物坐标，不是Node类
    141.         :param blocklist:
    142.         :return:
    143.         '''
    144.         self.blocklist.extend(blocklist)
    145.  
    146.     def get_neighbor(self, cnode):
    147.         #offsets = [(-1,1),(0,1),(1,1),(-1,0),(1,0),(-1,-1),(0,-1),(1,-1)]
    148.         offsets = [(-1, 0), (1, 0), (0, 1), (0, -1)]
    149.         nodes_neighbor = []
    150.         x, y = cnode.pos[0], cnode.pos[1]
    151.         for os in offsets:
    152.             x_new, y_new = x + os[0], y + os[1]
    153.             pos_new = (x_new, y_new)
    154.             #判断是否在地图范围内,超出范围跳过
    155.             if x_new < 0 or x_new > self.mapsize[0] - 1 or y_new < 0 or y_new > self.mapsize[1]:
    156.                 continue
    157.             nodes_neighbor.append(Node(pos_new))
    158.  
    159.         return nodes_neighbor
    160.  
    161.  
    162. def main():
    163.     mapsize = tuple(map(int, input('请输入地图大小，以逗号隔开：').split(',')))
    164.     pos_snode = tuple(map(int, input('请输入起点坐标，以逗号隔开：').split(',')))
    165.     pos_enode = tuple(map(int, input('请输入终点坐标，以逗号隔开：').split(',')))
    166.     myAstar = AStar(mapsize, pos_snode, pos_enode)
    167.     blocklist = gen_blocks(mapsize[0], mapsize[1])
    168.     myAstar.setBlock(blocklist)
    169.     routelist = [] #记录搜索到的最优路径
    170.     if myAstar.run() == 1:
    171.         routelist = myAstar.get_minroute()
    172.         print(routelist)
    173.         showresult(mapsize, pos_snode, pos_enode, blocklist, routelist)
    174.     else:
    175.         print('路径规划失败！')
    176.  
    177. def gen_blocks(width, height):
    178.     '''
    179.     随机生成障碍物
    180.     :param width: 地图宽度
    181.     :param height: 地图高度
    182.     :return:返回障碍物坐标集合
    183.     '''
    184.     i, blocklist = 0, []
    185.     while(i < BLOCK_NUM):
    186.         for j in range(3):
    187.             block = (3, j+1)
    188.             if block not in blocklist:
    189.                 blocklist.append(block)
    190.                 i+=1
    191.     return blocklist
    192.  
    193. def showresult(mapsize, pos_sn, pos_en, blocklist, routelist):
    194.     # 初始化导入的Pygame模块
    195.     pygame.init()
    196.     # 此处要将地图投影大小转换为像素大小，此处设地图中每个单元格的大小为CELL_WIDTH*CELL_HEIGHT像素
    197.     mymap = Map((mapsize[0]*CELL_WIDTH, mapsize[1]*CELL_HEIGHT))
    198.     pix_sn = (pos_sn[0]*CELL_WIDTH, pos_sn[1]*CELL_HEIGHT)
    199.     pix_en = (pos_en[0]*CELL_WIDTH, pos_en[1]*CELL_HEIGHT)
    200.     #对blocklist和routelist中的坐标同样要转换为像素值
    201.     bl_pix = list(map(transform, blocklist))
    202.     rl_pix = list(map(transform, routelist))
    203.     # 初始化显示的窗口并设置尺寸
    204.     screen = pygame.display.set_mode(mymap.mapsize)
    205.     # 设置窗口标题
    206.     pygame.display.set_caption('A*算法路径搜索演示：')
    207.     #用白色填充屏幕
    208.     screen.fill(Color.WHITE.value)
    209.  
    210.     #绘制屏幕中的所有单元格
    211.     for (x, y) in mymap.generate_cell(CELL_WIDTH, CELL_HEIGHT):
    212.         if (x,y) in bl_pix:
    213.             #绘制黑色的障碍物单元格，并留出2个像素的边框
    214.             pygame.draw.rect(screen, Color.BLACK.value, ((x+BORDER_WIDTH,y+BORDER_WIDTH), (CELL_WIDTH-2*BORDER_WIDTH, CELL_HEIGHT-2*BORDER_WIDTH)))
    215.         else:
    216.             # 绘制绿色的可通行单元格，并留出2个像素的边框
    217.             pygame.draw.rect(screen, Color.GREEN.value, ((x+BORDER_WIDTH,y+BORDER_WIDTH), (CELL_WIDTH-2*BORDER_WIDTH, CELL_HEIGHT-2*BORDER_WIDTH)))
    218.     #绘制起点和终点
    219.     pygame.draw.circle(screen, Color.BLUE.value, (pix_sn[0]+CELL_WIDTH//2, pix_sn[1]+CELL_HEIGHT//2), CELL_WIDTH//2 - 1)
    220.     pygame.draw.circle(screen, Color.RED.value, (pix_en[0]+CELL_WIDTH//2, pix_en[1]+CELL_HEIGHT//2), CELL_WIDTH//2 - 1)
    221.  
    222.     #绘制搜索得到的最优路径
    223.     for (x, y) in mymap.generate_cell(CELL_WIDTH, CELL_HEIGHT):
    224.         if (x,y) in rl_pix and (x,y)!=(pix_sn[0],pix_sn[1]) and (x,y)!=(pix_en[0],pix_en[1]):
    225.             pygame.draw.rect(screen, Color.GREY.value, ((x+BORDER_WIDTH,y+BORDER_WIDTH), (CELL_WIDTH-2*BORDER_WIDTH, CELL_HEIGHT-2*BORDER_WIDTH)))
    226.     #pygame.draw.aalines(screen, Color.RED.value, False, rl_pix)
    227.     keepGoing = True
    228.     while keepGoing:
    229.         pygame.time.delay(100)
    230.         for event in pygame.event.get():
    231.             if event.type == pygame.QUIT:
    232.                 keepGoing = False
    233.         pygame.display.flip()
    234.  
    235. def transform(pos):
    236.     xnew, ynew = pos[0]*CELL_WIDTH, pos[1]*CELL_HEIGHT
    237.     return (xnew, ynew)
    238.  
    239. if __name__ == '__main__':
    240.     main()
  • 相关阅读:
    nasm寄存器xmm[0~8使用]
    Vue3+ECharts
    带着upp闯关----性能考验
    虹科示波器 | 汽车免拆检修 | 2012 款上汽大众帕萨特车 发动机偶尔无法起动
    如何成为一名优秀的老师？
    尚医通_第11章_医院排班管理和搭建用户系统环境
    好用的js快速排序的方法
    多核处理器AMP、SMP、BMP区分
    【企业安全】企业安全系列第 1 部分 — 数据治理
    2022年python国际化课程上机考试二题解
  • 原文地址：https://blog.csdn.net/qq_41640218/article/details/126000961