devbox
秋刀鱼在做梦
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

自动驾驶算法（二）：A*算法讲解与代码实现_a*算法代码实现

作者：秋刀鱼在做梦 | 2024-07-19 01:57:51

赞

踩

a*算法代码实现

目录

1 A* 算法提出的动机

2 A*算法代码详解

3 A*算法完整代码

1 A* 算法提出的动机

        减少收录的珊格树目，增加搜索速度。在Dijkstra算法中，我们考虑收录栅格时我们考虑的是到起点的距离，我们会考虑收录距离起点较近的珊格进行收录。在A*算法，我们增加启发式函数，加快其导向终点的速度。

        举个例子：

        图中两个红色光晕节点，下面的点距离起点较近（蓝色的），所以Dijkstra会选择下面的节点进行收录。我们在这基础上增加这两个节点到终点的距离。我们发现上面节点到终点的距离会更加小，我们就会收录上面的节点。从而在收录节点的时候就会更加快速的导向终点。

        看一下算法对比：

A*
Dijkstra

        算法和Dijkstra相比仅多了一个启发项，因此代码架构只需要更改一点即可：

        当然，启发项不是随便加的，我们需要保证算法还是找到的最优路径。我们需要保证h(n) <= *h(n)，这里*h(n)是最优解。

        比如我们从1-->3这个节点要寻找一条路径：

        我们回顾一下open list和close list：上方的h表示预计到终点的距离。（假设的...）

  1. openlist:      2(1+6)      3(5+0)
  2. closelist:     1(0)

        那么我会选择收录三号点。我们认为最优路径是1-->3而不是1-->2-->3我们就不能保证最优性了。但是1-->3的扩展点更加少，一部分程度上来说增加了速度。（有好有坏）

2 A*算法代码详解

        相比较于Dijkstra算法就增加了一个启发函数：

  1.             # 选择扩展点 f(n) = g(n) + h(n)
  2.             c_id = min(
  3.                 open_set,
  4.                 key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,
  5.                                                                      open_set[
  6.                                                                          o]))

        我们看一下这个启发函数：

  1.     def calc_heuristic(n1, n2):
  2.         w = 1.0  # weight of heuristic
  3.         d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
  4.         return d

        先来看函数调用，goal_node是目标点。

        d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)：使用欧几里得距离公式计算了两个节点在二维平面上的距离，其中n1.x和n1.y分别表示节点n1的x坐标和y坐标，n2.x和n2.y分别表示节点n2的x坐标和y坐标。将得到的距离乘以权重w得到启发式值d。也就是计算出了该节点到终点的距离作为启发项。其余与Dijkstra算法一致，不再赘述。

3 A*算法完整代码

  1. """
  3. A* grid planning
  5. author: Atsushi Sakai(@Atsushi_twi)
  6.         Nikos Kanargias (nkana@tee.gr)
  8. See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
  10. """
  11.  
  12. import math
  13.  
  14. import matplotlib.pyplot as plt
  15.  
  16. show_animation = True
  17.  
  18.  
  19. class AStarPlanner:
  20.  
  21.     def __init__(self, ox, oy, resolution, rr):
  22.         """
  23.         Initialize grid map for a star planning
  25.         ox: x position list of Obstacles [m]
  26.         oy: y position list of Obstacles [m]
  27.         resolution: grid resolution [m]
  28.         rr: robot radius[m]
  29.         """
  30.  
  31.         self.resolution = resolution
  32.         self.rr = rr
  33.         self.min_x, self.min_y = 0, 0
  34.         self.max_x, self.max_y = 0, 0
  35.         self.obstacle_map = None
  36.         self.x_width, self.y_width = 0, 0
  37.         self.motion = self.get_motion_model()
  38.         self.calc_obstacle_map(ox, oy)
  39.  
  40.     class Node:
  41.         def __init__(self, x, y, cost, parent_index):
  42.             self.x = x  # index of grid
  43.             self.y = y  # index of grid
  44.             self.cost = cost
  45.             self.parent_index = parent_index
  46.  
  47.         def __str__(self):
  48.             return str(self.x) + "," + str(self.y) + "," + str(
  49.                 self.cost) + "," + str(self.parent_index)
  50.  
  51.     def planning(self, sx, sy, gx, gy):
  52.         """
  53.         A star path search
  55.         input:
  56.             s_x: start x position [m]
  57.             s_y: start y position [m]
  58.             gx: goal x position [m]
  59.             gy: goal y position [m]
  61.         output:
  62.             rx: x position list of the final path
  63.             ry: y position list of the final path
  64.         """
  65.  
  66.         start_node = self.Node(self.calc_xy_index(sx, self.min_x),
  67.                                self.calc_xy_index(sy, self.min_y), 0.0, -1)
  68.         goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
  69.                               self.calc_xy_index(gy, self.min_y), 0.0, -1)
  70.  
  71.         open_set, closed_set = dict(), dict()
  72.         open_set[self.calc_grid_index(start_node)] = start_node
  73.  
  74.         while 1:
  75.             if len(open_set) == 0:
  76.                 print("Open set is empty..")
  77.                 break
  78.  
  79.             # 选择扩展点 f(n) = g(n) + h(n)
  80.             c_id = min(
  81.                 open_set,
  82.                 key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,
  83.                                                                      open_set[
  84.                                                                          o]))
  85.             current = open_set[c_id]
  86.  
  87.             # show graph
  88.             if show_animation:  # pragma: no cover
  89.                 plt.plot(self.calc_grid_position(current.x, self.min_x),
  90.                          self.calc_grid_position(current.y, self.min_y), "xc")
  91.                 # for stopping simulation with the esc key.
  92.                 plt.gcf().canvas.mpl_connect('key_release_event',
  93.                                              lambda event: [exit(
  94.                                                  0) if event.key == 'escape' else None])
  95.                 if len(closed_set.keys()) % 10 == 0:
  96.                     plt.pause(0.001)
  97.  
  98.             if current.x == goal_node.x and current.y == goal_node.y:
  99.                 print("Find goal")
  100.                 goal_node.parent_index = current.parent_index
  101.                 goal_node.cost = current.cost
  102.                 break
  103.  
  104.             # Remove the item from the open set
  105.             del open_set[c_id]
  106.  
  107.             # Add it to the closed set
  108.             closed_set[c_id] = current
  109.  
  110.             # expand_grid search grid based on motion model
  111.             for i, _ in enumerate(self.motion):
  112.                 node = self.Node(current.x + self.motion[i][0],
  113.                                  current.y + self.motion[i][1],
  114.                                  current.cost + self.motion[i][2], c_id)
  115.                 n_id = self.calc_grid_index(node)
  116.  
  117.                 # If the node is not safe, do nothing
  118.                 if not self.verify_node(node):
  119.                     continue
  120.  
  121.                 if n_id in closed_set:
  122.                     continue
  123.  
  124.                 if n_id not in open_set:
  125.                     open_set[n_id] = node  # discovered a new node
  126.                 else:
  127.                     if open_set[n_id].cost > node.cost:
  128.                         # This path is the best until now. record it
  129.                         open_set[n_id] = node
  130.  
  131.         rx, ry = self.calc_final_path(goal_node, closed_set)
  132.  
  133.         return rx, ry
  134.  
  135.     def calc_final_path(self, goal_node, closed_set):
  136.         # generate final course
  137.         rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
  138.             self.calc_grid_position(goal_node.y, self.min_y)]
  139.         parent_index = goal_node.parent_index
  140.         while parent_index != -1:
  141.             n = closed_set[parent_index]
  142.             rx.append(self.calc_grid_position(n.x, self.min_x))
  143.             ry.append(self.calc_grid_position(n.y, self.min_y))
  144.             parent_index = n.parent_index
  145.  
  146.         return rx, ry
  147.  
  148.     @staticmethod
  149.     def calc_heuristic(n1, n2):
  150.         w = 1.0  # weight of heuristic
  151.         d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
  152.         return d
  153.  
  154.     def calc_grid_position(self, index, min_position):
  155.         """
  156.         calc grid position
  158.         :param index:
  159.         :param min_position:
  160.         :return:
  161.         """
  162.         pos = index * self.resolution + min_position
  163.         return pos
  164.  
  165.     def calc_xy_index(self, position, min_pos):
  166.         return round((position - min_pos) / self.resolution)
  167.  
  168.     def calc_grid_index(self, node):
  169.         return node.y * self.x_width + node.x
  170.  
  171.     def verify_node(self, node):
  172.         px = self.calc_grid_position(node.x, self.min_x)
  173.         py = self.calc_grid_position(node.y, self.min_y)
  174.  
  175.         if px < self.min_x:
  176.             return False
  177.         elif py < self.min_y:
  178.             return False
  179.         elif px >= self.max_x:
  180.             return False
  181.         elif py >= self.max_y:
  182.             return False
  183.  
  184.         # collision check
  185.         if self.obstacle_map[node.x][node.y]:
  186.             return False
  187.  
  188.         return True
  189.  
  190.     def calc_obstacle_map(self, ox, oy):
  191.  
  192.         self.min_x = round(min(ox))
  193.         self.min_y = round(min(oy))
  194.         self.max_x = round(max(ox))
  195.         self.max_y = round(max(oy))
  196.         print("min_x:", self.min_x)
  197.         print("min_y:", self.min_y)
  198.         print("max_x:", self.max_x)
  199.         print("max_y:", self.max_y)
  200.  
  201.         self.x_width = round((self.max_x - self.min_x) / self.resolution)
  202.         self.y_width = round((self.max_y - self.min_y) / self.resolution)
  203.         print("x_width:", self.x_width)
  204.         print("y_width:", self.y_width)
  205.  
  206.         # obstacle map generation
  207.         self.obstacle_map = [[False for _ in range(self.y_width)]
  208.                              for _ in range(self.x_width)]
  209.         for ix in range(self.x_width):
  210.             x = self.calc_grid_position(ix, self.min_x)
  211.             for iy in range(self.y_width):
  212.                 y = self.calc_grid_position(iy, self.min_y)
  213.                 for iox, ioy in zip(ox, oy):
  214.                     d = math.hypot(iox - x, ioy - y)
  215.                     if d <= self.rr:
  216.                         self.obstacle_map[ix][iy] = True
  217.                         break
  218.  
  219.     @staticmethod
  220.     def get_motion_model():
  221.         # dx, dy, cost
  222.         motion = [[1, 0, 1],
  223.                   [0, 1, 1],
  224.                   [-1, 0, 1],
  225.                   [0, -1, 1],
  226.                   [-1, -1, math.sqrt(2)],
  227.                   [-1, 1, math.sqrt(2)],
  228.                   [1, -1, math.sqrt(2)],
  229.                   [1, 1, math.sqrt(2)]]
  230.  
  231.         return motion
  232.  
  233.  
  234. def main():
  235.     print(__file__ + " start!!")
  236.  
  237.     # start and goal position
  238.     sx = -5.0  # [m]
  239.     sy = -5.0  # [m]
  240.     gx = 50.0  # [m]
  241.     gy = 50.0  # [m]
  242.     grid_size = 2.0  # [m]
  243.     robot_radius = 1.0  # [m]
  244.  
  245.     # set obstacle positions
  246.     ox, oy = [], []
  247.     for i in range(-10, 60):
  248.         ox.append(i)
  249.         oy.append(-10.0)
  250.     for i in range(-10, 60):
  251.         ox.append(60.0)
  252.         oy.append(i)
  253.     for i in range(-10, 61):
  254.         ox.append(i)
  255.         oy.append(60.0)
  256.     for i in range(-10, 61):
  257.         ox.append(-10.0)
  258.         oy.append(i)
  259.     for i in range(-10, 40):
  260.         ox.append(20.0)
  261.         oy.append(i)
  262.     for i in range(0, 40):
  263.         ox.append(40.0)
  264.         oy.append(60.0 - i)
  265.  
  266.     if show_animation:  # pragma: no cover
  267.         plt.plot(ox, oy, ".k")
  268.         plt.plot(sx, sy, "og")
  269.         plt.plot(gx, gy, "xb")
  270.         plt.grid(True)
  271.         plt.axis("equal")
  272.  
  273.     a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
  274.     rx, ry = a_star.planning(sx, sy, gx, gy)
  275.  
  276.     if show_animation:  # pragma: no cover
  277.         plt.plot(rx, ry, "-r")
  278.         plt.pause(0.001)
  279.         plt.show()
  280.  
  281.  
  282. if __name__ == '__main__':
  283.     main()

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/秋刀鱼在做梦/article/detail/848846
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号