1 基于搜索的路径规划——Dijkstra算法

文章目录

• Python
• 算法的基本思路
• • 1 设置机器人的起点和终点，栅格大小，机器人半径
  • 2 设置障碍物的位置，如下图的黑点所示
  • 3 绘制1和2的图
  • 4 申请类Dijkstra并且调用planning方法
  • • 4.1 进行初始化，传入障碍物位置、栅格、机器人半径信息：def __init__(self, ox, oy, resolution, robot_radius)
    • 4.2 构建栅格地图：def calc_obstacle_map(self, ox, oy)
    • • 4.2.1 使用下标计算真实位置：def calc_position(self, index, minp)
    • 4.3 设置机器人运动模式：def get_motion_model()
  • 5 定义planning：def planning(self, sx, sy, gx, gy)
  • • 5.1 设置结点：class Node
    • • 5.2 根据初始位置得到x,y的坐标索引：def calc_xy_index(self, position, minp)
      • 5.3 计算是第几个位置的索引值：def calc_index(self, node)
      • 5.4 算法逻辑
      • 5.5 计算最后的路径：def calc_final_path(self, goal_node, closed_set)
• Dijkstra算法

Python

首先必须申明下，以下的方格用(x,y)索引来表示的话，则以左下角的位置信息(x,y)来表示这个方格位置。

算法的基本思路

1 设置机器人的起点和终点，栅格大小，机器人半径

# start and goal position
    sx = -5.0  # [m] 设置机器人的起点和终点
    sy = -5.0  # [m]
    gx = 50.0  # [m]
    gy = 50.0  # [m]
    grid_size = 2.0  # [m] 设置栅格的大小
    robot_radius = 1.0  # [m] 机器人的半径

2 设置障碍物的位置，如下图的黑点所示

# set obstacle positions
    ox, oy = [], [] # 设置障碍物的位置，图中的黑点
    for i in range(-10, 60): # 设置周围的4堵墙
        ox.append(i)
        oy.append(-10.0)
    for i in range(-10, 60):
        ox.append(60.0)
        oy.append(i)
    for i in range(-10, 61):
        ox.append(i)
        oy.append(60.0)
    for i in range(-10, 61):
        ox.append(-10.0)
        oy.append(i)
    for i in range(-10, 40): # 设置中间的两堵墙
        ox.append(20.0)
        oy.append(i)
    for i in range(0, 40):
        ox.append(40.0)
        oy.append(60.0 - i)

3 绘制1和2的图

 # 开头show_animation = true
    if show_animation:  # pragma: no cover
        plt.plot(ox, oy, ".k") # 画图，起点终点障碍物
        plt.plot(sx, sy, "og") # 起点sx,sy是实心圈标记，填充绿色；终点gx，gy是x标记,黄色
        plt.plot(gx, gy, "xb") # 障碍物gx、gy是点，黑色
        plt.grid(True)
        plt.axis("equal")

4 申请类Dijkstra并且调用planning方法

类Dijkstra输入障碍物位置，栅格大小，机器人半径

 dijkstra = Dijkstra(ox, oy, grid_size, robot_radius) # 申请一个对象时Dijkstra
 rx, ry = dijkstra.planning(sx, sy, gx, gy) # 调用planning方法

4.1 进行初始化，传入障碍物位置、栅格、机器人半径信息：def init(self, ox, oy, resolution, robot_radius)

	# 将传进来的值进行初始化，resolution就是传进来的grid_size
    def __init__(self, ox, oy, resolution, robot_radius): 
        """
        Initialize map for 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

		# resolution就是传进来的grid_size,传进来之后赋值给类里面的成员
        self.resolution = resolution 
        self.robot_radius = robot_radius # 机器人半径
        self.calc_obstacle_map(ox, oy) # 构建栅格地图
        self.motion = self.get_motion_model() # 定义机器人的运动方式

4.2 构建栅格地图：def calc_obstacle_map(self, ox, oy)

    def calc_obstacle_map(self, ox, oy):
        ''' 第1步：构建栅格地图 '''
        self.min_x = round(min(ox)) # round函数进行四舍五入，取ox的最小值
        self.min_y = round(min(oy)) # 取oy的最大值
        self.max_x = round(max(ox)) # 取ox的最大值
        self.max_y = round(max(oy))
        print("min_x:", self.min_x) # min_x: -10
        print("min_y:", self.min_y) # min_y: -10
        print("max_x:", self.max_x) # max_x: 60
        print("max_y:", self.max_y) # max_y: 60

        # x的最大值减最小值，再除以栅格大小2，得到x方向的栅格个数有35，y方向也有35个栅格
        self.x_width = round((self.max_x - self.min_x) / self.resolution) # [60-(-10)]/2=35
        self.y_width = round((self.max_y - self.min_y) / self.resolution)
        print("x_width:", self.x_width)
        print("y_width:", self.y_width)

        # obstacle map generation
        # 初始化地图，使用二维向量表示，采用两层列表，内层是y方向的栅格个数，外层是z方向的栅格个数
        self.obstacle_map = [[False for _ in range(self.y_width)] # 所有值都初始化为false，内层循环是y方向栅格数
                             for _ in range(self.x_width)] # 采用两层的列表表示，for_in range表示循环
        
        # 设置障碍物     range是从0开始，前两层for循环用来遍历栅格,for ix in range(35)->0--34
        for ix in range(self.x_width): # 前两层循环用来遍历栅格，ix表示栅格在x方向的下标
            x = self.calc_position(ix, self.min_x) # 通过栅格下标计算位置，range默认步长为1
            for iy in range(self.y_width):
                y = self.calc_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy): # 遍历障碍物，zip相当于是组合函数，组合成各个点(iox,ioy)
                    d = math.hypot(iox - x, ioy - y) # hypot用来计算两点之间的距离
                    if d <= self.robot_radius: # 如果距离比机器人半径小，说明机器人不可以通行
                        self.obstacle_map[ix][iy] = True # 将障碍物所在栅格设置为true
                        break

这里sx,sy,gx,gy,x,y,iox,ioy均是真实坐标，在（-10,60，-10,60）的坐标系里。ix与iy均是索引值，从(0,0)->(34,34)

4.2.1 使用下标计算真实位置：def calc_position(self, index, minp)

    def calc_position(self, index, minp):
        pos = index * self.resolution + minp # 下标*栅格大小，minp表示偏移，等同于加上最小值minx
        return pos

4.3 设置机器人运动模式：def get_motion_model()

    def get_motion_model(): # 机器人运动模式
        # dx, dy, cost, 前两个是行走方式，cost行走的代价
        motion = [[1, 0, 1], # 表示x方向增加1，y方向不变，权值为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

5 定义planning：def planning(self, sx, sy, gx, gy) 5.1 设置结点：class Node

class Node: # 把每一个栅格定义为结点
    def __init__(self, x, y, cost, parent_index): 
    # 存储栅格坐标x,y; 路径值；parent_index用来查找来源，用来寻找路径
        self.x = x  # index of grid
        self.y = y  # index of grid
        self.cost = cost  # g(n) 代价值就是路径权值
        self.parent_index = parent_index  # index of previous Node，记录自己的上一个栅格，用来存储路径

    def __str__(self):
        return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.parent_index)

5.2 根据初始位置得到x,y的坐标索引：def calc_xy_index(self, position, minp)

 def calc_xy_index(self, position, minp): # 当前结点和父节点,得到x和y的索引
        return round((position - minp) / self.resolution) # 当前位置-最小值，再除以栅格大小，得到x索引

5.3 计算是第几个位置的索引值：def calc_index(self, node)

   def calc_index(self, node):
        return node.y * self.x_width + node.x # 以12为例

以12为例解释，比如10所在位置是(1,2)，那么按照顺序的话，y表示1，宽度为10，x为0，则10 = 1*10+0

5.4 算法逻辑

 while 1: # 伪代码 调用的是内置的函数，具体比较数值是open_set.cost,取得当前cost值最小的节点
       c_id = min(open_set, key = lambda o: open_set[o].cost)# 取open_set中cost最小的节点，得到cost值最小的节点下标c_id
       current = open_set[c_id] # current表示cost值最小的节点,open_set[c_id]类似g(n),c_id表示g,cost和n是一致的

       # show graph 动画仿真
       if show_animation:  # pragma: no cover
           plt.plot(self.calc_position(current.x, self.min_x),
                    self.calc_position(current.y, self.min_y), "xc")
           # for stopping simulation with the esc key.
           plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])
           if len(closed_set.keys()) % 10 == 0:
               plt.pause(0.001)

       # 判断是否是终点
       if current.x == goal_node.x and current.y == goal_node.y:
           print("Find goal")
           goal_node.parent_index = current.parent_index
           goal_node.cost = current.cost
           break

       # Remove the item from the open set
       del open_set[c_id] # 将该节点在open_set中删掉

       # Add it to the closed set
       closed_set[c_id] = current # 将删除的节点加入到closed_set

       # expand search grid based on motion model
       for move_x, move_y, move_cost in self.motion: # 遍历邻接节点，以运动方式进行遍历，这里要遍历8种情况
           node = self.Node(current.x + move_x, # 先求邻接节点，根据当前x,y与运动方式得到的邻接节点的x,y
                            current.y + move_y, # 路径权值 = 当前的路径值+移动的代价
                            current.cost + move_cost, c_id) # c_id表示当前父节点的下标
           n_id = self.calc_index(node) # 计算出当前新节点的索引值n_id，即0~35*35-1，这里的node和n_id循环生成8个
           # n_id与c_id的区别在于前者存储新产生的节点，后者存储cost值最小的结点

           if n_id in closed_set:
               continue

           if not self.verify_node(node): # 计算邻接结点是否可行，有没有超出范围，如果为false，则跳出下面操作重新循环
               continue

           if n_id not in open_set: # 如果新的n_id不在open_set中，就添加进去
               open_set[n_id] = node  # Discover a new node
           else: # 如果n_id在open_set中，现存的值比新的值大的话，就更新到起点的距离
               if open_set[n_id].cost >= node.cost: # open_set的值是不是大于当前结点的值,大的话就进行更新
                  # This path is the best until now. record it!
                  open_set[n_id] = node

5.5 计算最后的路径：def calc_final_path(self, goal_node, closed_set)

    def calc_final_path(self, goal_node, closed_set): # 使用rx和ry两个列表来存储路径，求出具体的真实下标
        # generate final course
        rx, ry = [self.calc_position(goal_node.x, self.min_x)], [
            self.calc_position(goal_node.y, self.min_y)] # 反向计算真实位置，保存到列表里
        parent_index = goal_node.parent_index # 首先得到终点的父节点，node的
        while parent_index != -1: # 初始节点的parent_index设置为-1
            n = closed_set[parent_index] # 这里的parent_index就是node里面的c_id
            rx.append(self.calc_position(n.x, self.min_x))
            ry.append(self.calc_position(n.y, self.min_y))
            parent_index = n.parent_index # n节点的父节点取出来，直到取到最初始的结点

        return rx, ry # 得到rx和ry就可以得到路径

Dijkstra算法

# 这里需要file->setting->tools->Python scientific中取消勾选show plots in tool window
import matplotlib.pyplot as plt
import math

show_animation = True


class Dijkstra:

    def __init__(self, ox, oy, resolution, robot_radius): # 将传进来的值进行初始化，resolution就是传进来的grid_size
        """
        Initialize map for 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 # resolution就是传进来的grid_size,传进来之后赋值给类里面的成员
        self.robot_radius = robot_radius # 机器人半径
        self.calc_obstacle_map(ox, oy) # 构建栅格地图
        self.motion = self.get_motion_model() # 定义机器人的运动方式

    class Node: # 把每一个栅格定义为结点
        def __init__(self, x, y, cost, parent_index): # 存储栅格坐标x,y; 路径值；parent_index用来查找来源，用来寻找路径
            self.x = x  # index of grid
            self.y = y  # index of grid
            self.cost = cost  # g(n) 代价值就是路径权值
            self.parent_index = parent_index  # index of previous Node，记录自己的上一个栅格节点，用来存储路径

        def __str__(self):
            return str(self.x) + "," + str(self.y) + "," + str(
                self.cost) + "," + str(self.parent_index)

    def planning(self, sx, sy, gx, gy):
        """
        dijkstra path search

        input:
            s_x: start x position [m]
            s_y: start y position [m]
            gx: goal x position [m]
            gx: goal x position [m]

        output:
            rx: x position list of the final path
            ry: y position list of the final path
        """
        # start_node和goal_node是包含(x, y, cost, parent_index)的节点
        start_node = self.Node(self.calc_xy_index(sx, self.min_x), # 将起点和终点位置转换为节点下标，起点cost为0，父节点为-1
                               self.calc_xy_index(sy, self.min_y), 0.0, -1)   # round((position - minp) / self.resolution)
        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()     # key - value: hash表  采用字典形式
        open_set[self.calc_index(start_node)] = start_node # 将起点放入，将所有栅格按照顺序编号，从0~(35*35-1)

        while 1: # 伪代码 调用的是内置的函数，具体比较数值是open_set.cost,取得当前cost值最小的节点
            # 如果为空，搜索失败结束
            if len(open_set) == 0:
                print("Open set is empty..")
                break
            
            c_id = min(open_set, key = lambda o: open_set[o].cost)# 取open_set中cost最小的节点，得到cost值最小的节点下标c_id
            current = open_set[c_id] # current表示cost值最小的节点,open_set[c_id]类似g(n),c_id表示g,cost和n是一致的
            # c_id和n_id均是用0~35*35-1来表示

            # show graph 动画仿真
            if show_animation:  # pragma: no cover
                plt.plot(self.calc_position(current.x, self.min_x),
                         self.calc_position(current.y, self.min_y), "xc")
                # for stopping simulation with the esc key.
                plt.gcf().canvas.mpl_connect(
                    'key_release_event',
                    lambda event: [exit(0) if event.key == 'escape' else None])
                if len(closed_set.keys()) % 10 == 0:
                    plt.pause(0.001)

            # 判断是否是终点
            if current.x == goal_node.x and current.y == goal_node.y:
                print("Find goal")
                goal_node.parent_index = current.parent_index
                goal_node.cost = current.cost
                break

            # Remove the item from the open set
            del open_set[c_id] # 将该节点在open_set中删掉

            # Add it to the closed set
            closed_set[c_id] = current # 将删除的节点加入到closed_set

            # expand search grid based on motion model
            for move_x, move_y, move_cost in self.motion: # 遍历邻接节点，以运动方式进行遍历，这里要遍历8种情况
                node = self.Node(current.x + move_x, # 先求邻接节点，根据当前x,y与运动方式得到的邻接节点的x,y
                                 current.y + move_y, # 路径权值 = 当前的路径值+移动的代价
                                 current.cost + move_cost, c_id) # c_id表示当前父节点的下标，用数字0~35*35-1表示
                n_id = self.calc_index(node) # 计算出当前新节点的索引值n_id，即0~35*35-1，这里的node和n_id循环生成8个
                # n_id与c_id的区别在于前者存储新产生的节点，后者存储cost值最小的结点
                # c_id表示的父节点的索引，n_id表示遍历的新结点的索引

                if n_id in closed_set:
                    continue

                if not self.verify_node(node): # 计算邻接结点是否可行，有没有超出范围，如果为false，则跳出下面操作重新循环
                    continue

                if n_id not in open_set: # 如果新的n_id不在open_set中，就添加进去
                    open_set[n_id] = node  # Discover a new node
                else: # 如果n_id在open_set中，现存的值比新的值大的话，就更新到起点的距离
                    if open_set[n_id].cost >= node.cost: # open_set的值是不是大于当前结点的值,大的话就进行更新
                        # This path is the best until now. record it!
                        open_set[n_id] = node

        # 理论部分未讲解
        rx, ry = self.calc_final_path(goal_node, closed_set)

        return rx, ry

    def calc_final_path(self, goal_node, closed_set): # 使用rx和ry两个列表来存储路径，求出具体的真实下标
        # generate final course
        rx, ry = [self.calc_position(goal_node.x, self.min_x)], [
            self.calc_position(goal_node.y, self.min_y)] # 反向计算真实位置，保存到列表里
        parent_index = goal_node.parent_index # 首先得到终点的父节点，node的
        while parent_index != -1: # 初始节点的parent_index设置为-1
            n = closed_set[parent_index] # 这里的parent_index就是node里面的c_id
            rx.append(self.calc_position(n.x, self.min_x))
            ry.append(self.calc_position(n.y, self.min_y))
            parent_index = n.parent_index # n节点的父节点取出来，直到取到最初始的结点

        return rx, ry # 得到rx和ry就可以得到路径

    def calc_position(self, index, minp):
        pos = index * self.resolution + minp # 下标*栅格大小，minp表示偏移，等同于加上最小值minx
        return pos

    def calc_xy_index(self, position, minp): # 当前结点和父节点,得到x和y的索引
        return round((position - minp) / self.resolution) # 当前位置-最小值，再除以栅格大小，得到x索引，四舍五入

    def calc_index(self, node):
        return node.y * self.x_width + node.x # 以12为例，node.y表示行数，width表示宽度，node.x表示剩下的

    def verify_node(self, node):
        px = self.calc_position(node.x, self.min_x) # 计算出px和py的位置
        py = self.calc_position(node.y, self.min_y)

        if px < self.min_x:
            return False
        if py < self.min_y:
            return False
        if px >= self.max_x:
            return False
        if py >= self.max_y:
            return False

        if self.obstacle_map[node.x][node.y]: # 判断是不是障碍物
            return False

        return True

    def calc_obstacle_map(self, ox, oy):
        ''' 第1步：构建栅格地图 '''
        self.min_x = round(min(ox)) # round函数进行四舍五入，取ox的最小值
        self.min_y = round(min(oy)) # 取oy的最大值
        self.max_x = round(max(ox)) # 取ox的最大值
        self.max_y = round(max(oy))
        print("min_x:", self.min_x) # min_x: -10
        print("min_y:", self.min_y) # min_y: -10
        print("max_x:", self.max_x) # max_x: 60
        print("max_y:", self.max_y) # max_y: 60

        # x的最大值减最小值，再除以栅格大小2，得到x方向的栅格个数有35，y方向也有35个栅格
        self.x_width = round((self.max_x - self.min_x) / self.resolution) # [60-(-10)]/2=35
        self.y_width = round((self.max_y - self.min_y) / self.resolution)
        print("x_width:", self.x_width)
        print("y_width:", self.y_width)

        # obstacle map generation
        # 初始化地图，使用二维向量表示，采用两层列表，内层是y方向的栅格个数，外层是z方向的栅格个数，35*35个false
        self.obstacle_map = [[False for _ in range(self.y_width)] # 所有值都初始化为false，内层循环是y方向栅格数
                             for _ in range(self.x_width)] # 采用两层的列表表示，for_in range表示循环

        # 设置障碍物     range是从0开始，前两层for循环用来遍历栅格,for ix in range(35)->0--34
        for ix in range(self.x_width): # 前两层循环用来遍历栅格，ix表示栅格在x方向的下标
            x = self.calc_position(ix, self.min_x) # 通过栅格下标计算位置，range默认步长为1
            for iy in range(self.y_width):
                y = self.calc_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy): # 遍历障碍物，zip相当于是组合函数，组合成各个点(iox,ioy)
                    d = math.hypot(iox - x, ioy - y) # hypot用来计算两点之间的距离
                    if d <= self.robot_radius: # 如果距离比机器人半径小，说明机器人不可以通行。当x和y不与ox,oy重合即可
                        self.obstacle_map[ix][iy] = True # 将障碍物所在栅格设置为true
                        break

    @staticmethod
    def get_motion_model(): # 机器人运动模式
        # dx, dy, cost, 前两个是行走方式，cost行走的代价
        motion = [[1, 0, 1], # 表示x方向增加1，y方向不变，权值为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

def main():
    # start and goal position
    sx = -5.0  # [m] 设置机器人的起点和终点
    sy = -5.0  # [m]
    gx = 50.0  # [m]
    gy = 50.0  # [m]
    grid_size = 2.0  # [m] 设置栅格的大小
    robot_radius = 1.0  # [m] 机器人的半径

    # set obstacle positions
    ox, oy = [], [] # 设置障碍物的位置，图中的黑点
    for i in range(-10, 60): # 设置周围的4堵墙
        ox.append(i)
        oy.append(-10.0)
    for i in range(-10, 60):
        ox.append(60.0)
        oy.append(i)
    for i in range(-10, 61):
        ox.append(i)
        oy.append(60.0)
    for i in range(-10, 61):
        ox.append(-10.0)
        oy.append(i)
    for i in range(-10, 40): # 设置中间的两堵墙
        ox.append(20.0)
        oy.append(i)
    for i in range(0, 40):
        ox.append(40.0)
        oy.append(60.0 - i)

    # 开头show_animation = true
    if show_animation:  # pragma: no cover
        plt.plot(ox, oy, ".k") # 画图，起点终点障碍物
        plt.plot(sx, sy, "og") # 起点sx,sy是实心圈标记，填充绿色；终点gx，gy是x标记,黄色
        plt.plot(gx, gy, "xb") # 障碍物gx、gy是点，黑色
        plt.grid(True)
        plt.axis("equal")

    dijkstra = Dijkstra(ox, oy, grid_size, robot_radius) # 申请一个对象时Dijkstra
    rx, ry = dijkstra.planning(sx, sy, gx, gy) # 调用planning方法

    # 最开始的show_animation是true
    if show_animation:  # pragma: no cover
        plt.plot(rx, ry, "-r")
        plt.pause(0.01)
        plt.show()