• 使用 Python 的自主机器人算法 Dijkstra 路径规划

    迪杰斯特拉算法

    • Dijkstra 算法是一种广泛使用的算法，用于查找图中的最短路径。
    • 它通过从起始节点迭代选择成本最小的节点来探索搜索空间。
    • Dijkstra 算法计算到达每个节点的成本，并在找到更短路径时更新成本。
    • 它会持续下去，直到到达目标节点或探索完所有可到达的节点。

    公式

    • math.hypot()

    • math.sqrt()

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

    Djikstra算法结果

    Dijkstra算法优点

    • 最佳解决方案：Dijkstra 算法保证找到起始节点和图中所有其他节点之间的最短路径，前提是该图具有非负边权重。这种最优性确保机器人采用最有效的路线到达目的地。
    • 完整性：Dijkstra 算法是完整的，这意味着如果存在解决方案，它总是会找到解决方案。如果从起始节点到目标节点存在有效路径，Dijkstra 算法就会发现它。
    • 可扩展性：Dijkstra 算法在具有适度数量的节点和边的图上表现良好。其时间复杂度为O((V + E) log V)，其中V是节点数，E是边数。这使得它适用于许多现实世界的路径规划场景。
    • 灵活性：Dijkstra 算法可以应用于各种图表示，包括有向图和无向图。它还可以处理加权图，其中不同的边具有不同的成本或距离
    • 增量更新：Dijkstra 算法允许对图进行增量更新。如果环境发生变化或出现新的障碍物，算法可以重新运行以找到更新的最短路径，而无需从头开始。
    • 广泛的适用性：Dijkstra 的算法广泛应用于各种应用，包括机器人、导航系统、网络路由和物流。其多功能性和经过验证的有效性使其成为自主机器人路径规划的热门选择
  • 相关阅读:
    vulnhub Photographer: 1
    基于Apache Hudi 构建Serverless实时分析平台
    【Vue】params和query的区别？实战两种路由传参方式
    【IC设计】ZC706板卡点灯入门（含Verilog代码，xdc约束，实验截图）
    Git 标签（Tag）实战：打标签和删除标签的步骤指南
    微服务组件之Feign
    【软考】-- 操作系统（上）
    Spring 中BeanFactory和FactoryBean有什么不同之处呢?
    远程仓库地址改变后更换url
    el form 表单validate成功后没有执行逻辑
  • 原文地址：https://blog.csdn.net/qq_41929396/article/details/133378006