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

Dijistra和A star算法

作者：weixin_40725706 | 2024-06-06 23:41:13

赞

踩

dijistra

Dijistra和A star算法在自动驾驶领域可用于路径的搜索，甚至在一定的场景下能用作轨迹的规划。

A star算法相当于是Dijistra算法的改进，在其基础上加上了绝对距离等判断条件，从而大大降低了路径搜索的范围，节约了算力。理解两个算法，我们要先从Dijistra算法开始。

1.Dijistra算法的思路

Dijistra算法的思路很简单，采用的是一种贪心的策略，声明一个数组dis来保存源点到各个顶点的最短距离和一个保存已经找到了最短路径的顶点的集合：T，初始时，原点 s 的路径权重被赋为 0 （dis[s] = 0）。若对于顶点 s 存在能直接到达的边（s,m），则把dis[m]设为w（s, m）,同时把所有其他（s不能直接到达的）顶点的路径长度设为无穷大。初始时，集合T只有顶点s。
然后，从dis数组选择最小值，则该值就是源点s到该值对应的顶点的最短路径，并且把该点加入到T中，OK，此时完成一个顶点，然后，我们需要看看新加入的顶点是否可以到达其他顶点并且看看通过该顶点到达其他点的路径长度是否比源点直接到达短，如果是，那么就替换这些顶点在dis中的值。然后，又从dis中找出最小值，重复上述动作，直到T中包含了图的所有顶点。

算法的伪代码如下：

 其中：

最小堆open：存储所有需要处理的节点

变量closed: 表示已经处理完的节点集

变量predecessors: 回溯最短路径上的已经访问过的节点

uCost:源节点到该点的距离

从下图可以大致理解一下Djistra的搜索过程：

假设一个从s到t的场景：

首先s可以到b和a，这是我们将其作为存储起来，这个时候从s到a的距离是大于从s到b的；因此再搜索b的下一步路径，将a保留存储。s到b再到c的距离是1.5还是小于s到a，因此继续从c向后搜索这时从s到b再到c再到d的距离大于从s到a的距离，因此从a再开始搜索，搜索一步后就到了目的地，整个算法的进程结束。

能力有限可能讲的不是很到位，大伙可以自行查阅资料，Dijistra算法还是比较好理解的。

2.A star算法的改进

 A*算法其实就是在Dijistra算法加了一个欧氏距离或者曼哈顿距离的判断。

欧式距离：

欧式距离计算公式_Panpan Wei的博客-CSDN博客_欧式距离计算公式

曼哈顿距离：

 

 其实现伪代码如图：

可以看出A*算法相对于Dijistra主要加上了绝对距离h，这个h代表的是当前点到终点的欧式距离或者曼哈顿距离。可以理解为如果我在搜索时，这个距离过大，那么这个点之后的路径将不在进行搜索。我们可以对这个欧式距离设定一定的权重，来使得A*算法探索路径的能力和得到最优路径的能力有一个很好的平衡。

3.两种算法的实现和结果对比

Dijistra算法：

  1. """
  2. Grid based Dijkstra planning
  3. """
  4.  
  5. import matplotlib.pyplot as plt
  6. import math
  7.  
  8. show_animation = True
  9.  
  10.  
  11. class Dijkstra:
  12.  
  13.     def __init__(self, ox, oy, resolution, robot_radius):
  14.         """
  15.         Initialize map for a star planning
  16.         ox: x position list of Obstacles [m]
  17.         oy: y position list of Obstacles [m]
  18.         resolution: grid resolution [m]
  19.         rr: robot radius[m]
  20.         """
  21.  
  22.         self.min_x = None
  23.         self.min_y = None
  24.         self.max_x = None
  25.         self.max_y = None
  26.         self.x_width = None
  27.         self.y_width = None
  28.         self.obstacle_map = None
  29.  
  30.         self.resolution = resolution
  31.         self.robot_radius = robot_radius
  32.         self.calc_obstacle_map(ox, oy)
  33.         self.motion = self.get_motion_model()
  34.  
  35.     class Node:
  36.         def __init__(self, x, y, cost, parent_index):
  37.             self.x = x  # index of grid
  38.             self.y = y  # index of grid
  39.             self.cost = cost
  40.             self.parent_index = parent_index  # index of previous Node
  41.  
  42.         def __str__(self):
  43.             return str(self.x) + "," + str(self.y) + "," + str(
  44.                 self.cost) + "," + str(self.parent_index)
  45.  
  46.     def planning(self, sx, sy, gx, gy):
  47.         """
  48.         dijkstra path search
  49.         input:
  50.             s_x: start x position [m]
  51.             s_y: start y position [m]
  52.             gx: goal x position [m]
  53.             gx: goal x position [m]
  54.         output:
  55.             rx: x position list of the final path
  56.             ry: y position list of the final path
  57.         """
  58. #设置起点
  59.         start_node = self.Node(self.calc_xy_index(sx, self.min_x),
  60.                                self.calc_xy_index(sy, self.min_y), 0.0, -1)
  61. #设置目标终点
  62.         goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
  63.                               self.calc_xy_index(gy, self.min_y), 0.0, -1)
  64. #初始化两个字典
  65.         open_set, closed_set = dict(), dict()
  66. #给字典放元素，先放进去起始点
  67.         open_set[self.calc_index(start_node)] = start_node
  68.  
  69.         while 1:
  70. #搜索最小的键值对
  71.             c_id = min(open_set, key=lambda o: open_set[o].cost)
  72. #得到当前可以保存的路径
  73.             current = open_set[c_id]
  74. #在图像中得到展示
  75.             # show graph
  76.             if show_animation:  # pragma: no cover
  77.                 plt.plot(self.calc_position(current.x, self.min_x),
  78.                          self.calc_position(current.y, self.min_y), "xc")
  79.                 # for stopping simulation with the esc key.
  80.                 plt.gcf().canvas.mpl_connect(
  81.                     'key_release_event',
  82.                     lambda event: [exit(0) if event.key == 'escape' else None])
  83.                 if len(closed_set.keys()) % 10 == 0:
  84.                     plt.pause(0.001)
  85. #如果x，y已经等于目标点
  86.             if current.x == goal_node.x and current.y == goal_node.y:
  87.                 print("Find goal")
  88.                 goal_node.parent_index = current.parent_index
  89.                 goal_node.cost = current.cost
  90.                 break
  91.  
  92.             # Remove the item from the open set
  93.             del open_set[c_id]
  94.  
  95.             # Add it to the closed set
  96.             closed_set[c_id] = current
  97.  
  98.             # expand search grid based on motion model
  99.             for move_x, move_y, move_cost in self.motion:
  100.                 node = self.Node(current.x + move_x,
  101.                                  current.y + move_y,
  102.                                  current.cost + move_cost, c_id)
  103.                 n_id = self.calc_index(node)
  104.  
  105.                 if n_id in closed_set:
  106.                     continue
  107.  
  108.                 if not self.verify_node(node):
  109.                     continue
  110.  
  111.                 if n_id not in open_set:
  112.                     open_set[n_id] = node  # Discover a new node
  113.                 else:
  114.                     if open_set[n_id].cost >= node.cost:
  115.                         # This path is the best until now. record it!
  116.                         open_set[n_id] = node
  117.  
  118.         rx, ry = self.calc_final_path(goal_node, closed_set)
  119.  
  120.         return rx, ry
  121.  
  122.     def calc_final_path(self, goal_node, closed_set):
  123.         # generate final path
  124.         rx, ry = [self.calc_position(goal_node.x, self.min_x)], [
  125.             self.calc_position(goal_node.y, self.min_y)]
  126.         parent_index = goal_node.parent_index
  127.         while parent_index != -1:
  128.             n = closed_set[parent_index]
  129.             rx.append(self.calc_position(n.x, self.min_x))
  130.             ry.append(self.calc_position(n.y, self.min_y))
  131.             parent_index = n.parent_index
  132.  
  133.         return rx, ry
  134.  
  135.     def calc_position(self, index, minp):
  136.         pos = index * self.resolution + minp
  137.         return pos
  138.  
  139.     def calc_xy_index(self, position, minp):
  140.         return round((position - minp) / self.resolution)
  141.  
  142.     def calc_index(self, node):
  143.         return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
  144.  
  145.     def verify_node(self, node):
  146.         px = self.calc_position(node.x, self.min_x)
  147.         py = self.calc_position(node.y, self.min_y)
  148.  
  149.         if px < self.min_x:
  150.             return False
  151.         if py < self.min_y:
  152.             return False
  153.         if px >= self.max_x:
  154.             return False
  155.         if py >= self.max_y:
  156.             return False
  157.  
  158.         if self.obstacle_map[node.x][node.y]:
  159.             return False
  160.  
  161.         return True
  162.  
  163.     def calc_obstacle_map(self, ox, oy):
  164.  
  165.         self.min_x = round(min(ox))
  166.         self.min_y = round(min(oy))
  167.         self.max_x = round(max(ox))
  168.         self.max_y = round(max(oy))
  169.         print("min_x:", self.min_x)
  170.         print("min_y:", self.min_y)
  171.         print("max_x:", self.max_x)
  172.         print("max_y:", self.max_y)
  173.  
  174.         self.x_width = round((self.max_x - self.min_x) / self.resolution)
  175.         self.y_width = round((self.max_y - self.min_y) / self.resolution)
  176.         print("x_width:", self.x_width)
  177.         print("y_width:", self.y_width)
  178.  
  179.         # obstacle map generation
  180.         self.obstacle_map = [[False for _ in range(self.y_width)]
  181.                              for _ in range(self.x_width)]
  182.         for ix in range(self.x_width):
  183.             x = self.calc_position(ix, self.min_x)
  184.             for iy in range(self.y_width):
  185.                 y = self.calc_position(iy, self.min_y)
  186.                 for iox, ioy in zip(ox, oy):
  187.                     d = math.hypot(iox - x, ioy - y)
  188.                     if d <= self.robot_radius:
  189.                         self.obstacle_map[ix][iy] = True
  190.                         break
  191.  
  192.     @staticmethod
  193.     def get_motion_model():
  194.         # dx, dy, cost
  195.         motion = [[1, 0, 1],
  196.                   [0, 1, 1],
  197.                   [-1, 0, 1],
  198.                   [0, -1, 1],
  199.                   [-1, -1, math.sqrt(2)],
  200.                   [-1, 1, math.sqrt(2)],
  201.                   [1, -1, math.sqrt(2)],
  202.                   [1, 1, math.sqrt(2)]]
  203.  
  204.         return motion
  205.  
  206.  
  207. def main():
  208.     print(__file__ + " start!!")
  209.  
  210.     # start and goal position
  211.     sx =10.0  # [m]
  212.     sy = 10.0  # [m]
  213.     gx = 50.0  # [m]
  214.     gy = 50.0  # [m]
  215.     grid_size = 2.0  # [m]
  216.     robot_radius = 1.0  # [m]
  217.  
  218.     # set obstacle positions
  219.     ox, oy = [], []
  220.     for i in range(-10, 60):
  221.         ox.append(i)
  222.         oy.append(-10.0)
  223.     for i in range(-10, 60):
  224.         ox.append(60.0)
  225.         oy.append(i)
  226.     for i in range(-10, 61):
  227.         ox.append(i)
  228.         oy.append(60.0)
  229.     for i in range(-10, 61):
  230.         ox.append(-10.0)
  231.         oy.append(i)
  232.     for i in range(-10, 40):
  233.         ox.append(20.0)
  234.         oy.append(i)
  235.     for i in range(0, 40):
  236.         ox.append(40.0)
  237.         oy.append(60.0 - i)
  238.     for i in range(40, 55):
  239.         ox.append(i)
  240.         oy.append(30)
  241.  
  242.     if show_animation:  # pragma: no cover
  243.         plt.plot(ox, oy, ".k")
  244.         plt.plot(sx, sy, "og")
  245.         plt.plot(gx, gy, "xb")
  246.         plt.grid(True)
  247.         plt.axis("equal")
  248.  
  249.     dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
  250.     rx, ry = dijkstra.planning(sx, sy, gx, gy)
  251.  
  252.     if show_animation:  # pragma: no cover
  253.         plt.plot(rx, ry, "-r")
  254.         plt.pause(0.01)
  255.         plt.show()
  256.  
  257.  
  258. if __name__ == '__main__':
  259.     main()

A*算法实现（定义一个欧式距离或曼哈顿距离函数）

  1. """
  2. A* grid planning
  3. """
  4. import numpy as np
  5. import math
  6.  
  7. import matplotlib.pyplot as plt
  8.  
  9. show_animation = True
  10.  
  11.  
  12. class AStarPlanner:
  13.  
  14.     def __init__(self, ox, oy, resolution, rr):
  15.         """
  16.         Initialize grid map for a star planning
  17.         ox: x position list of Obstacles [m]
  18.         oy: y position list of Obstacles [m]
  19.         resolution: grid resolution [m]
  20.         rr: robot radius[m]
  21.         """
  22.  
  23.         self.resolution = resolution
  24.         self.rr = rr
  25.         self.min_x, self.min_y = 0, 0
  26.         self.max_x, self.max_y = 0, 0
  27.         self.obstacle_map = None
  28.         self.x_width, self.y_width = 0, 0
  29.         self.motion = self.get_motion_model()
  30.         self.calc_obstacle_map(ox, oy)
  31.  
  32.     class Node:
  33.         def __init__(self, x, y, cost, parent_index):
  34.             self.x = x  # index of grid
  35.             self.y = y  # index of grid
  36.             self.cost = cost
  37.             self.parent_index = parent_index
  38.  
  39.         def __str__(self):
  40.             return str(self.x) + "," + str(self.y) + "," + str(
  41.                 self.cost) + "," + str(self.parent_index)
  42.  
  43.     def planning(self, sx, sy, gx, gy):
  44.         """
  45.         A star path search
  46.         input:
  47.             s_x: start x position [m]
  48.             s_y: start y position [m]
  49.             gx: goal x position [m]
  50.             gy: goal y position [m]
  51.         output:
  52.             rx: x position list of the final path
  53.             ry: y position list of the final path
  54.         """
  55.         # 设置起点
  56.         start_node = self.Node(self.calc_xy_index(sx, self.min_x),
  57.                                self.calc_xy_index(sy, self.min_y), 0.0, -1)
  58.         # 设置目标终点
  59.         goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
  60.                               self.calc_xy_index(gy, self.min_y), 0.0, -1)
  61.         # 初始化两个字典
  62.         open_set, closed_set = dict(), dict()
  63.         # 给字典放元素，先放进去起始点
  64.         open_set[self.calc_grid_index(start_node)] = start_node
  65. #添加欧式距离或者曼哈顿距离
  66.         while 1:
  67.             # 搜索最小的键值对，前一项和Dijstra一样，后面一项是添加的距离
  68.             #c_id = min(open_set, key=lambda o: open_set[o].cost)
  69.             #c_id = min(open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(open_set[o], goal_node))
  70.             #c_id = min(open_set, key=lambda o: open_set[o].cost + self.calc_euclidean(open_set[o], goal_node))
  71.             c_id = min(open_set, key=lambda o: open_set[o].cost + self.calc_manhattan(open_set[o], goal_node))
  72.  
  73.             # 得到当前可以保存的路径
  74.             current = open_set[c_id]
  75.             # 在图像中得到展示
  76.             # show graph
  77.             if show_animation:  # pragma: no cover
  78.                 plt.plot(self.calc_grid_position(current.x, self.min_x),
  79.                          self.calc_grid_position(current.y, self.min_y), "xc")
  80.                 # for stopping simulation with the esc key.
  81.                 plt.gcf().canvas.mpl_connect(
  82.                     'key_release_event',
  83.                     lambda event: [exit(0) if event.key == 'escape' else None])
  84.                 if len(closed_set.keys()) % 10 == 0:
  85.                     plt.pause(0.001)
  86.             # 如果x，y已经等于目标点
  87.             if current.x == goal_node.x and current.y == goal_node.y:
  88.                 print("Find goal")
  89.                 goal_node.parent_index = current.parent_index
  90.                 goal_node.cost = current.cost
  91.                 break
  92.  
  93.             # Remove the item from the open set
  94.             del open_set[c_id]
  95.  
  96.             # Add it to the closed set
  97.             closed_set[c_id] = current
  98.  
  99.             # expand search grid based on motion model
  100.             for move_x, move_y, move_cost in self.motion:
  101.                 node = self.Node(current.x + move_x,
  102.                                  current.y + move_y,
  103.                                  current.cost + move_cost, c_id)
  104.                 n_id = self.calc_grid_index(node)
  105.  
  106.                 if n_id in closed_set:
  107.                     continue
  108.  
  109.                 if not self.verify_node(node):
  110.                     continue
  111.  
  112.                 if n_id not in open_set:
  113.                     open_set[n_id] = node  # Discover a new node
  114.                 else:
  115.                     if open_set[n_id].cost >= node.cost:
  116.                         # This path is the best until now. record it!
  117.                         open_set[n_id] = node
  118.  
  119.         rx, ry = self.calc_final_path(goal_node, closed_set)
  120.  
  121.  
  122.         return rx, ry
  123.  
  124.     def calc_final_path(self, goal_node, closed_set):
  125.         # generate final path，用于更新最后的环境
  126.         rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
  127.             self.calc_grid_position(goal_node.y, self.min_y)]
  128.         parent_index = goal_node.parent_index
  129.         while parent_index != -1:
  130.             n = closed_set[parent_index]
  131.             rx.append(self.calc_grid_position(n.x, self.min_x))
  132.             ry.append(self.calc_grid_position(n.y, self.min_y))
  133.             parent_index = n.parent_index
  134.  
  135.         return rx, ry
  136.  
  137.     @staticmethod
  138.     #三角形距离计算
  139.     # def calc_heuristic(n1, n2):
  140.     #     w = 5.0  # weight of heuristic
  141.     #     d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
  142.     #     return d
  143.     #欧氏距离计算
  144.     # def calc_euclidean(n1,n2):
  145.     #     w=1.0
  146.     #     d=w*pow(pow(n1.x-n2.x,2)+pow(n1.y-n2.y,2),0.5)
  147.     #     return d
  148.     #曼哈顿距离计算
  149.  
  150.     def calc_manhattan(n1,n2):
  151.         """
  152.         添加曼哈顿距离
  153.         w:权重系数
  154.         n1:x方向node
  155.         n2:y方向node
  156.         d:距离
  157.         """
  158.         w = 5.0
  159.         d = w*(abs(n1.x-n2.x)+abs(n1.y - n2.y))
  160.         return d
  161.  
  162.     def calc_grid_position(self, index, min_position):
  163.         """
  164.         calc grid position
  165.         :param index:
  166.         :param min_position:
  167.         :return:
  168.         """
  169.         pos = index * self.resolution + min_position
  170.         return pos
  171. #获取下标的函数
  172.     def calc_xy_index(self, position, min_pos):
  173.         return round((position - min_pos) / self.resolution)
  174. #获取网格下标
  175.     def calc_grid_index(self, node):
  176.         return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
  177. #判断存入节点选择
  178.     def verify_node(self, node):
  179.         px = self.calc_grid_position(node.x, self.min_x)
  180.         py = self.calc_grid_position(node.y, self.min_y)
  181.  
  182.         if px < self.min_x:
  183.             return False
  184.         elif py < self.min_y:
  185.             return False
  186.         elif px >= self.max_x:
  187.             return False
  188.         elif py >= self.max_y:
  189.             return False
  190.  
  191.         # collision check
  192.         if self.obstacle_map[node.x][node.y]:
  193.             return False
  194.  
  195.         return True
  196. #生成地图
  197.     def calc_obstacle_map(self, ox, oy):
  198.  
  199.         self.min_x = round(min(ox))
  200.         self.min_y = round(min(oy))
  201.         self.max_x = round(max(ox))
  202.         self.max_y = round(max(oy))
  203.         print("min_x:", self.min_x)
  204.         print("min_y:", self.min_y)
  205.         print("max_x:", self.max_x)
  206.         print("max_y:", self.max_y)
  207.  
  208.         self.x_width = round((self.max_x - self.min_x) / self.resolution)
  209.         self.y_width = round((self.max_y - self.min_y) / self.resolution)
  210.         print("x_width:", self.x_width)
  211.         print("y_width:", self.y_width)
  212.  
  213.         # obstacle map generation
  214.         self.obstacle_map = [[False for _ in range(self.y_width)]
  215.                              for _ in range(self.x_width)]
  216.         for ix in range(self.x_width):
  217.             x = self.calc_grid_position(ix, self.min_x)
  218.             for iy in range(self.y_width):
  219.                 y = self.calc_grid_position(iy, self.min_y)
  220.                 for iox, ioy in zip(ox, oy):
  221.                     d = math.hypot(iox - x, ioy - y)
  222.                     if d <= self.rr:
  223.                         self.obstacle_map[ix][iy] = True
  224.                         break
  225. #规定可执行动作行为
  226.     @staticmethod
  227.     def get_motion_model():
  228.         # dx, dy, cost
  229.         motion = [[1, 0, 1],
  230.                   [0, 1, 1],
  231.                   [-1, 0, 1],
  232.                   [0, -1, 1],
  233.                   [-1, -1, math.sqrt(2)],
  234.                   [-1, 1, math.sqrt(2)],
  235.                   [1, -1, math.sqrt(2)],
  236.                   [1, 1, math.sqrt(2)]]
  237.  
  238.         return motion
  239.  
  240.  
  241. def main():
  242.     print(__file__ + " start!!")
  243.  
  244.     # start and goal position
  245.     sx = 10.0  # [m]
  246.     sy = 10.0  # [m]
  247.     gx = 50.0  # [m]
  248.     gy = 50.0  # [m]
  249.     grid_size = 2.0  # [m]
  250.     robot_radius = 1.0  # [m]
  251.  
  252.     # set obstacle positions
  253.     ox, oy = [], []
  254.     for i in range(-10, 60):
  255.         ox.append(i)
  256.         oy.append(-10.0)
  257.     for i in range(-10, 60):
  258.         ox.append(60.0)
  259.         oy.append(i)
  260.     for i in range(-10, 61):
  261.         ox.append(i)
  262.         oy.append(60.0)
  263.     for i in range(-10, 61):
  264.         ox.append(-10.0)
  265.         oy.append(i)
  266.     for i in range(-10, 40):
  267.         ox.append(20.0)
  268.         oy.append(i)
  269.     for i in range(0, 40):
  270.         ox.append(40.0)
  271.         oy.append(60.0 - i)
  272. # #添加一个障碍
  273.     for i in range(40, 55):
  274.         ox.append(i)
  275.         oy.append(30)
  276.  
  277.  
  278.  
  279.     if show_animation:  # pragma: no cover
  280. #显示中文标题
  281.         plt.rcParams['font.sans-serif'] = ['SimHei']  # 显示中文标签
  282.         plt.rcParams['axes.unicode_minus'] = False
  283.         plt.title("路径规划实验结果")
  284. #设置坐标轴字体字号
  285.         plt.yticks(fontproperties='Times New Roman', size=16)
  286.         plt.xticks(fontproperties='Times New Roman', size=16)
  287. #更改作图颜色
  288.         plt.plot(ox, oy, ".r")
  289.         plt.plot(sx, sy, "og")
  290.         plt.plot(gx, gy, "xb")
  291.         plt.grid(True)
  292.         plt.axis("equal")
  293.  
  294.     a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
  295.     rx, ry = a_star.planning(sx, sy, gx, gy)
  296.  
  297.     if show_animation:  # pragma: no cover
  298.         plt.plot(rx, ry, "-b")
  299.         plt.pause(0.001)
  300.         plt.show()
  301.     plt.show()
  302.  
  303.  
  304. if __name__ == '__main__':
  305.     main()

4.实现效果

Dijistra算法：

 A*算法：

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

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

  

闽ICP备14008679号