SUTD
作者在读学校Singapore University of Technology and Design

Established under strong support from MIT, Singapore University of Technology and Design (SUTD) plans to do for Singapore what MIT has done for Massachusetts and Silicon Valley, as well as for the world. SUTD is conducting top-tier research and education. In 2017, SUTD was ranked by Clarivate Analytics as the 5th most influential scientific research university in telecommunications in the world. SUTD was also ranked the 6th in electrical engineering research in the world.

代码分享

PythonRobotics是由Atsushi Sakai, Daniel Ingram等人建立的开源代码软件平台，收集了机器人学当下主流算法的python代码（基于python3），为了帮助初学者明白各个算法的基本原理，详细介绍见PythonRobotics: a Python code collection of robotics algorithms
本系列已发布文章：
Python Robotics代码详解（一）A* 搜索算法
并且将会持续发布更全面、更深层次的讲解，觉得使用方便还请星标和关注笔者的git！

Dijkstra 算法代码讲解

迪杰斯特拉（Dijkstra）算法是典型的最短路径的算法，由荷兰计算机科学家迪杰斯特拉于1959年提出，用来求得从起始点到其他所有点最短路径。该算法采用了贪心的思想，每次都查找与该点距离最近的点，也因为这样，它不能用来解决存在负权边的图。解决的问题可描述为：在无向图 G=(V,E) 中，假设每条边 E[i] 的长度为 w[i]，找到由顶点vs到其余各点的最短路径。 关于Dijkstra算法的讲解，详细可参考Dijkstra算法详解。该算法存在很多变体：戴克斯特拉的原始版本仅适用于找到两个顶点之间的最短路径[9]，后来更常见的变体固定了一个顶点作为源结点然后找到该顶点到图中所有其它结点的最短路径，产生一个最短路径树。

变量定义

在介绍代码函数之前，我们需要定义算法所需要的变量

# start and goal position
    sx = 10.0  # [m] 起始位置x坐标
    sy = 10.0  # [m] 起始位置y坐标
    gx = 50.0  # [m] 目标点x坐标
    gy = 50.0  # [m] 目标点y坐标
    grid_size = 2.0  # [m] 网格的尺寸
    robot_radius = 1.0  # [m] 机器人尺寸

我们将地图（假设是方形）均匀地网格化，每一个网格代表一个点，同时每个点有一个序列号index变量来记录点的位置，另外我们需要定义地图的边界min_x, min_y和max_x, max_y，用来计算遍历的点的具体位置：

def calc_position(self, index, minp):
        pos = index * self.resolution + minp
        return pos

其中minp指的就是min_x或者min_y，index是需要计算的节点的序列号，index乘以像素大小resolution（即我们前面定义的grid_size=2）得到x或者y方向的长度，这个长度值加在边界点min_x或者min_y上即可得到准确的坐标。
在遍历的过程中，我们需要定义

  • open_set：路径规划过程中存放待检测的节点的集合
  • closed_set: 存放已检测过的节点的集合

  • motion: 在网状方格地图中，每个节点（边界点除外）周围有八个节点，对应八个方向，motion是固定存储前进方向的数组

    def get_motion_model():
      # dx, dy, cost: 每个数组中的三个元素分别表示向x，y方向前进的距离以及前进代价
      motion = [[1, 0, 1], #右
                [0, 1, 1], #上
                [-1, 0, 1], #左
                [0, -1, 1], #下
                [-1, -1, math.sqrt(2)], #左下
                [-1, 1, math.sqrt(2)], #左上
                [1, -1, math.sqrt(2)], #右下
                [1, 1, math.sqrt(2)]] #右上

      return motion

    即在贪婪遍历时，会遍历周边八个点的cost以选出最小的cost。

    函数定义

    在介绍完这些变量后，我们开始定义函数，首先是Dijkstra的类，初始化一个简单的地图：

    def __init__(self, ox, oy, resolution, robot_radius):
      """
      Initialize map for a star planning

      ox: x position list of Obstacles [m]
      oy: y position list of Obstacles [m]
      resolution: grid resolution [m]
      rr: robot radius[m]
      """

      self.min_x = None
      self.min_y = None
      self.max_x = None
      self.max_y = None
      self.x_width = None
      self.y_width = None
      self.obstacle_map = None #障碍层地图，遍历时会检测是否是障碍层的地图，如果是则跳过该点，代价为无穷大

      self.resolution = resolution
      self.robot_radius = robot_radius
      self.calc_obstacle_map(ox, oy)
      self.motion = self.get_motion_model()

    然后我们定义搜索区域节点（Node）的类

    class Node:
      def __init__(self, x, y, cost, pind):
          self.x = x  # index of grid
          self.y = y  # index of grid
          self.cost = cost
          self.parent = parent  # index of previous Node 即父节点的序列号

    每个Node都包含坐标x和y，代价cost和其父节点（上一个遍历的点）的序号。在遍历时我们需要计算每个坐标的index

      def calc_xy_index(self, position, minp):
      return round((position - minp) / self.resolution)

  def calc_index(self, node):
      return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)

    同时还有另外两个函数，一个是验证节点的verify_node函数：判断节点的坐标在[min_x,max_x]和[min_y,max_y]的范围内，同时验证该节点不在障碍层；另一个是用来计算障碍层地图的函数calc_obstacle_map，其中障碍物的点是我们在main函数中定义的。

    规划函数

    在介绍完其他功能函数后，就是介绍最重要的规划函数，该函数定义在Dijkstra类中，输入起始点和目标点的坐标(sx,sy)和(gx,gy)，最终输出的结果是路径包含的点的坐标集合rx和ry。首先根据起始点、目标点的输入坐标来定义Node类，同时初始化open set, closed set，将起始点存放在open_set中：

    start_node = self.Node(self.calc_xy_index(sx, self.min_x),
                             self.calc_xy_index(sy, self.min_y), 0.0, -1)
      goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
                            self.calc_xy_index(gy, self.min_y), 0.0, -1)

      open_set, closed_set = dict(), dict()
      open_set[self.calc_index(start_node)] = start_node

    然后在while循环中从起始点(sx,sy)开始遍历所有点寻找最优路径，直到找到目标点(gx,gy)后循环终止。在遍历时会优先选择open_set待遍历点中存储的cost最小的点作为current进行尝试

    c_id = min(open_set, key=lambda o: open_set[o].cost)
          current = open_set[c_id]

    首选判断current点是否微目标点，如果是则遍历结束，break掉这个while循环

    if current.x == goal_node.x and current.y == goal_node.y:
              print("Find goal")
              goal_node.parent = current.parent
              goal_node.cost = current.cost
              break

否则的话将该current点从open_set中删除，并存储在closed_set中:

# Remove the item from the open set
            del open_set[c_id]

            # Add it to the closed set
            closed_set[c_id] = current

并使用前面提到的运动模型motion来遍历该点周边的点，在遍历时需要依次判定这个待遍历的点是否已经被遍历了（是否已经在closed_set中）以及验证该点是否是正确的点，如果该点还未被遍历并且是在地图中的合理的点，那么继续判定该点是否在open_set中，如果不在，那么会存储到open_set中作为候选遍历点，如果在的话则判断该点的cost是否比当前点的小，如果小的话则替换该点作为当前点，并重复上述遍历过程直到到达goal。

for move_x, move_y, move_cost in self.motion:
                 = self.Node(current.x + move_x,
                                 current.y + move_y,
                                 current.cost + move_cost, c_id)
                n_id = self.calc_index(node)

                if n_id in closed_set:
                    continue

                if not self.verify_node(node):
                    continue

                if n_id not in open_set:
                    open_set[n_id] = node  # Discover a new node
                else:
                    if open_set[n_id].cost >= node.cost:
                        # This path is the best until now. record it!
                        open_set[n_id] = node

最后，通过将目标点和closed set传入calc_final_path函数来产生最后的路径并结束while循环

rx, ry = self.calc_final_path(goal_node, closed_set)
    def calc_final_path(self, goal_node, closed_set):
        # generate final course
        rx, ry = [self.calc_position(goal_node.x, self.min_x)], [
            self.calc_position(goal_node.y, self.min_y)]
        parent = goal_node.parent
        while parent != -1:
            n = closed_set[parent]
            rx.append(self.calc_position(n.x, self.min_x))
            ry.append(self.calc_position(n.y, self.min_y))
            parent = n.parent

        return rx, ry

最后的main函数是定义起点和目标点、设置障碍物的位置、调用类以及里面的函数进行规划运算，并且动态展示出来运算结果。

运行结果

代码的动态运行结果如下图所示



其中绿色的网格是经过验证和搜索的点，最终的红色路线即位最佳路径。与A star运行结果相比，可以发现Dijkstra方法中绿色的已经验证过的点的数量明显要更多，这是由于贪婪算法的原因，同时注定了Dijkstra的算法复杂度要更高。

大家可以下载PythonRobotics包并运行文件PathPlanning/Dijkstra下的dijkstra.py来验证。

后记

本文是关于Python Robotics代码中的 Dijkstra路径规划的详细介绍，大家可以在此基础上扩展延伸。觉得有帮助一定要转发点赞关注哦，谢谢！

笔者本科毕业于上海交通大学，现在是SUTD PhD Candidate，有两年多ROS的使用经验及多机器人编队的科研经验（2018.03-2020.11），现在是总结之前所学，希望能有所帮助。本系列Python Robotics代码详解将结合该著名的机器人学代码包讲解机器人SLAM定位建图及路径规划与导航等相关算法及原理，持续不断更新中。如果大家有相关问题或发现作者漏洞欢迎私戳，同时欢迎关注收藏。
同时欢迎关注博主Git和CSDN：
https://github.com/LiHongbo97
https://blog.csdn.net/qq_33742147?spm=1011.2124.3001.5343