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

已于 2023-02-28 14:59:13 修改
阅读量2.4k 收藏 36
点赞数 9
分类专栏： 路径规划与轨迹优化(IROS) 文章标签： 算法 python 人工智能
于 2022-06-23 17:49:10 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_42731062/article/details/125424496
版权

文章目录

算法讲解

  • 图1：括号内表示当前点到起点的距离
    在这里插入图片描述
  • 图2：完成节点收录后(从open到closede)，遍历邻接节点，寻找到起点的距离，找到距离最小的结点进行收录，下图是4号节点
    在这里插入图片描述
  • 图3：4号节点收录进去后，需要对4号节点更新距离(有3、6、7三个节点)，找到路径最小节点，将2号节点收录到closed，遍历2号节点的邻接节点
    在这里插入图片描述
  • 图4：2号的邻接节点是4和5，由于4已经在closed list中，所以求5号节点到1的距离，选择距离最小的节点3收录进来
    在这里插入图片描述
  • 图5：节点3到节点1不需要更新，更新3号到6号的节点路径
    在这里插入图片描述
  • 图6：由于前面得到到6号节点的距离是9，而从3到6路径的距离更短，所以需要进行路径距离更新，更新为8
    在这里插入图片描述
  • 图7：找到距离最小的节点7收录
    在这里插入图片描述
  • 图8：遍历7邻接节点，到6的距离进行更新
    在这里插入图片描述
  • 图9：找到距离最小的节点6，收录进去
    在这里插入图片描述
  • 图10
    在这里插入图片描述

重要说明

栅格地图

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

栅格地图的尺寸大小
在这里插入图片描述

有权图

在这里插入图片描述

1 def main()

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

- 这个main函数，是整个程序主要函数 def main()

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] 设置栅格的大小
    # 就是将地图分为一个一个2*2的方格，方格的4个角可以用坐标表示
    robot_radius = 1.0  # [m] 机器人的半径

1.2 设置障碍物的位置

  • 如下图的黑点所示

在这里插入图片描述

  • range范围是[stop,end)左开右闭
# set obstacle positions
   ox, oy = [], [] # 设置障碍物的位置，图中的黑点
    # 设置周围的4堵墙
    for i in range(-10, 60): # 下方的墙 y = -10 x = -10 ~ 59
        ox.append(i)
        oy.append(-10.0)
    for i in range(-10, 60): # 右方的墙 x = 60 y = -10 ~ 59
        ox.append(60.0)
        oy.append(i)
    for i in range(-10, 61): # 上方的墙 y = 60 x = -10 ~ 60
        ox.append(i)
        oy.append(60.0)
    for i in range(-10, 61): # 左边的墙 x = -10 y = -10 ~ 60
        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)

1.3 绘制步骤1和2的图

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

2 class Dijkstra 申请类

 # class Dijkstra 是一个类
 # 申请一个对象Dijkstra，输入障碍物位置，栅格大小，机器人半径
 dijkstra = Dijkstra(ox, oy, grid_size, robot_radius) 
 # 调用对象里面的planning方法，输入起点和终点，得到经过的路径点rx,ry
 rx, ry = dijkstra.planning(sx, sy, gx, gy)

2.1 def __init__ 传入障碍物位置、栅格、机器人半径信息

  • 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() # 定义机器人的运动方式

2.1.1 def calc_obstacle_map 构建栅格地图

  • def calc_obstacle_map(self, ox, oy)
  • round函数用法：round函数进行四舍五入
    讲解1
  • sx，sy，gx，gy，x，y，iox，ioy均是真实坐标，在（-10,60，-10,60）的坐标系里
  • ix 与 iy均是索引值，范围均是从0~34，表示栅格所在坐标，现在有35 * 35个栅格，(0,0)->(34,34)
  • 红色表示的就是(ix，iy)，蓝色表示的就是(x，y)
  • 根据 calc_position(self, index, minp) 算法，可以得到每一个栅格左下角(绿色)坐标 x,y 与 栅格坐标 ix，iy有着一下的关系
  • x = -10 + ix * 2
  • y = -10 + iy * 2
    在这里插入图片描述
  • 红色栅格刚开始都设置为false，障碍物所在位置(用蓝色坐标表示，而且是位于栅格左下角的坐标)，其对应的栅格设置为ture，表示此栅格有障碍物
    def calc_obstacle_map(self, ox, oy):
        ''' 第1步：构建栅格地图 '''
        self.min_x = round(min(ox)) # 取ox的最小值
        self.min_y = round(min(oy)) # 取oy的最小值
        self.max_x = round(max(ox)) # 取ox的最大值
        self.max_y = round(max(oy)) # 取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
        # false 表示还没有设置障碍物
        # 初始化地图，使用二维向量表示，采用两层列表，内层是y方向的栅格个数，外层是x方向的栅格个数
        self.obstacle_map = [[False for _ in range(self.y_width)] # 所有值都初始化为false，内层循环是y方向栅格数
                             for _ in range(self.x_width)] # 采用两层的列表表示，for_in range表示循环
        
       # 设置障碍物     range是从0开始,range默认步长为1，前两层for循环用来遍历栅格,range(35)->0--34
        for ix in range(self.x_width): # ix表示栅格在x方向的下标, ix 范围是 0~34
            x = self.calc_position(ix, self.min_x) # 通过栅格下标计算位置 x = -10 ~ 58(每个值间隔为2)
            for iy in range(self.y_width): # iy表示栅格在y方向的下标, iy 范围是 0~34
                y = self.calc_position(iy, self.min_y) # 通过栅格下标计算位置 y = -10 ~ 58(每个值间隔为2)
                for iox, ioy in zip(ox, oy): # 遍历障碍物, zip相当于是组合函数, 组合成各个点(iox,ioy), (ox,oy)障碍物坐标
                    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
2.1.1.1 def calc_position 使用栅格计算真实位置
  • def calc_position(self, index, minp)，返回pos信息，pos
  • 对于x来说，minx = -10，index 范围是 0 ~ 34，则 x范围是-10 ~ 58(每个值间隔为2)
  • 对于y来说，miny = -10，index 范围是 0 ~ 34，则 y范围是-10 ~ 58(每个值间隔为2)
    def calc_position(self, index, minp):
    	# 下标*栅格大小，minp表示偏移，等同于加上最小值minx或miny,index范围是0~34
        pos = index * self.resolution + minp 
        return pos

2.1.2 def get_motion_model 设置机器人运动模式

  • 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

2.2 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)

2.3 def planning 定义方法

  • def planning(self, sx, sy, gx, gy)

2.3.1 def calc_xy_index 由初始位置得栅格坐标(节点)

  • position用 -10 ~ 58 数字表示
  • 栅格坐标用 0 ~ 34表示
  • 就是将当前的位置转化为节点坐标
  • def calc_xy_index(self, position, minp)
 def calc_xy_index(self, position, minp): # 当前结点和父节点,得到x和y的索引
        return round((position - minp) / self.resolution) # 当前位置-最小值，再除以栅格大小2，得到x索引
  • 计算起点和终点的节点信息，即起点和终点所在的栅格位置坐标
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)

2.3.2 def calc_index 对任一栅格节点进行独立编号(0~35*35-1)

  • def calc_index(self, node)，node节点坐标从(0,0)—>(34,34)，返回的范围是0~35*35-1
   def calc_index(self, node):
        return node.y * self.x_width + node.x # 以10为例
  • 以10为例解释，比如10所在位置是(0,1)，那么按照顺序的话，y表示1，宽度为10，x为0，则10 = 1*10+0

在这里插入图片描述

2.3.3 def verify_node 计算邻接结点是否可行，是否超出范围

    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

2.3.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

2.3.5 def calc_final_path 计算最后的路径

  • 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算法

"""

Grid based Dijkstra planning

author: Atsushi Sakai(@Atsushi_twi)

"""

# 这里需要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): # 调用_init_将传进来的值进行初始化，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

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

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

        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)的节点
        # 将起点和终点位置转换为节点下标，起点cost为0，父节点为-1
        # round((position - minp) / self.resolution)
        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()     # key - value: hash表  采用字典形式，通过key可以索引到具体内容
        open_set[self.calc_index(start_node)] = start_node # 将起点放入open_set，将所有栅格按照顺序编号，从0~(35*35-1)

        while 1: # 伪代码 调用的是内置的函数，具体比较数值是open_set.cost,取得当前cost值最小的节点
            # 如果为空，搜索失败结束
            if len(open_set) == 0:
                print("Open set is empty..")
                break

            # c_id和n_id均是用0~35*35-1来表示
            # 取open_set中cost最小的节点，比较cost值，找到cost值最小的节点下标c_id，lamda 匿名函数
            c_id = min(open_set, key = lambda o: open_set[o].cost)
            # current表示cost值最小的节点,open_set[c_id]类似g(n),c_id表示g,cost和n是一致的
            current = open_set[c_id] # 取出当前cost值最小的节点


            # 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表示遍历的新结点的索引，c_id和n_id均是用0~35*35-1来表示

                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 # 首先得到终点的父节点
        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)) # n.x是0~34，n.y是0~34
            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):
        # 下标*栅格大小，minp表示偏移，等同于加上最小值minx或miny, index范围是0~34
        # 对于x来说，minx = -10，index范围是0~ 34，则x范围是 -10~ 58
        # 对于y来说，miny = -10，index范围是0~ 34，则y范围是 -10~ 58
        pos = index * self.resolution + minp
        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表示宽度

    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步：构建栅格地图 '''
        # round函数进行四舍五入
        self.min_x = round(min(ox)) # 取ox的最小值
        self.min_y = round(min(oy)) # 取oy的最小值
        self.max_x = round(max(ox)) # 取ox的最大值
        self.max_y = round(max(oy)) # 取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
        # false 表示还没有设置障碍物
        # 初始化地图，使用二维向量表示，采用两层列表，内层是y方向的栅格个数，外层是x方向的栅格个数，35*35个false
        # 所有值都初始化为false，内层循环是y方向栅格数；采用两层的列表表示，for _ in range表示循环
        self.obstacle_map = [[False for _ in range(self.y_width)]
                             for _ in range(self.x_width)]

        # 设置障碍物     range是从0开始,range默认步长为1，前两层for循环用来遍历栅格,range(35)->0--34
        for ix in range(self.x_width): # ix表示栅格在x方向的下标, ix 范围是 0~34
            x = self.calc_position(ix, self.min_x) # 通过栅格下标计算位置 x = -10 ~ 58(每个值间隔为2)
            for iy in range(self.y_width): # iy表示栅格在y方向的下标, iy 范围是 0~34
                y = self.calc_position(iy, self.min_y) # 通过栅格下标计算位置 y = -10 ~ 58(每个值间隔为2)
                for iox, ioy in zip(ox, oy): # 遍历障碍物, zip相当于是组合函数, 组合成各个点(iox,ioy), (ox,oy)障碍物坐标
                    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]
    # 设置栅格的大小，就是将地图分为一个一个2 * 2 的方格，方格的4个角可以用坐标表示
    grid_size = 2.0  # [m]
    robot_radius = 1.0  # [m] 机器人的半径

    # set obstacle positions
    ox, oy = [], [] # 设置障碍物的位置，图中的黑点
    # 设置周围的4堵墙
    for i in range(-10, 60): # 下方的墙 y = -10 x = -10 ~ 59
        ox.append(i)
        oy.append(-10.0)
    for i in range(-10, 60): # 右方的墙 x = 60 y = -10 ~ 59
        ox.append(60.0)
        oy.append(i)
    for i in range(-10, 61): # 上方的墙 y = 60 x = -10 ~ 60
        ox.append(i)
        oy.append(60.0)
    for i in range(-10, 61): # 左边的墙 x = -10 y = -10 ~ 60
        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
        # 画图，起点终点障碍物
        # 障碍物ox、oy是点，黑色
        plt.plot(ox, oy, ".k")
        # 起点sx,sy是实心圈标记，填充绿色
        plt.plot(sx, sy, "og")
        # 终点gx，gy是x标记,蓝色
        plt.plot(gx, gy, "xb")
        plt.grid(True)
        plt.axis("equal")

    # class Dijkstra 是一个类
    # 申请一个对象Dijkstra，输入障碍物位置，栅格大小，机器人半径
    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()

if __name__ == '__main__':
    main()
2021 Nqq
关注
  • 9
    点赞
  • 踩
  • 36
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
专栏目录
基于搜索的路径规划算法
m0_46490744的博客
11-04 4376
基于搜索的路径规划算法 ——针对A* LPA*算法的理解 1.引言 对于研究系统优化和生产调度相关的课题，掌握对应的路径规划算法是必不可少的能力。移动一个简单的物体看起来是容易的，而路径搜索是复杂的。 根据是否考虑经验法则算法，可以将路径规划算法分为盲目式算法和启发式搜索算法。 解决有向正权值图类规划的Dijkstra算法，可以计算含有负权的Ford算法，系统检查图中所有节点的广度优先算法等等，都属于盲目式算法。这类算法的特点是，可以找到最优路径或最短路径，尽管搜索过程过于盲目即耗费的时间长，搜索的范围广。
路径规划：基于搜索的算法hybrid A*
qq_41667348的博客
05-10 1504
文章目录一、hybrid A*算法1、算法原理及流程2、算法特点两者比较3、其他注意四、项目应用 一、hybrid A*算法 1、算法原理及流程 2、算法特点两者比较 3、其他注意 2、hybrid A* 参考文献整理： https://blog.csdn.net/luohuiwu/article/details/103181609/ 算法解析：https://blog.csdn.net/shuang_yu_/article/details/109051885 ros平台c++中实现： https://gi
全局路径规划之--Dijkstra算法
最新发布
木牛的博客
05-06 179
例如，(s,v)的距离可能大于(s,k)+(k,v)的距离。U包含除s外的其他顶点，且U中顶点的距离为”起点s到该顶点的距离”[例如，U中顶点v的距离为(s,v)的长度，然后s和v不相邻，则v的距离为∞]此时，S={D(0)}, U={A(∞),B(∞),C(3),E(4),F(∞),G(∞)}。此时，S={D(0),C(3),E(4)}, U={A(∞),B(23),F(6),G(12)}。此时，S={D(0),C(3)}, U={A(∞),B(23),E(4),F(9),G(∞)}。
基于搜索的路径规划算法总结
Azahaxia的博客
03-20 1649
全局路径规划部分算法总结 一般而言，全局路径规划都是假设全局地图是完全已知的，然后就可以根据已知的全局信息进行全局规划。当然，也存在假设全局地图是部分未知的或完全未知的规划算法，如D*算法，这部分在本文中暂且不涉及。本文涉及的全局规划算法包括： 广度优先算法（Breadth-first Searching，bfs） 深度优先算法（Depth-first Searching，dfs） Dijkstra算法 最佳优先搜索算法（Best-first Searching，BFS） A*算法 程序参考于：Path
路径规划：基于搜索的算法A*
qq_41667348的博客
05-06 1795
文章目录一、A*算法1、算法原理2、算法流程3、算法特点参考链接一、A*算法1、算法原理2、算法流程3、算法特点4、两者比较代码实现项目应用 一、A*算法 A*(A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，算法中的距离估算值与实际值越接近，最终搜索速度越快。它是一种启发式搜索基于搜索的全局路径规划方法 1、算法原理 2、算法流程 伪代码如下： 流程理解： 3、算法特点 参考链接 一、A*算法 1、算法原理 2、算法流程 3、算法特点 4、两者比较 代码实现 项目应用 ...
基于栅格地图的Dijkstra算法路径规划
01-19
**基于栅格地图的Dijkstra算法路径规划** 在计算机科学和人工智能领域，路径规划是解决寻路问题的关键技术。在复杂环境中，如机器人导航、游戏AI或者地理信息系统中，经常需要寻找从起点到终点的最短或最优路径。...
最短路径算法——Dijkstra算法参照.pdf
11-07
Dijkstra算法是图论中用于寻找单源最短路径的一种经典算法，由荷兰计算机科学家艾兹格·迪科斯彻于1956年提出。该算法主要用于解决非负权有向图中从一个特定顶点（源点）到其他所有顶点的最短路径问题。在实际应用中...
最短路径算法——Dijkstra算法
03-20
Dijkstra算法是求解最短路径问题的经典算法之一，该算法的应用价值非常高，广泛应用于路由选择、网络拓扑分析、交通网络规划等领域。 Dijkstra算法的基本思想是从源结点开始，逐步扩展到网络中的其他结点，寻找从源...
ROS学习笔记之——路径规划及avoid obstacles
热门推荐
gwpscut的博客
11-29 1万+
之前博客《ROS学习笔记之——Navigation Stack及路径规划》介绍了navigation stack，其中涉及到的amcl以及避障还没有详细的展开 AMCL 避障（avoid obstales） Robot Navigation Challenge 参考资料 http://wiki.ros.org/amcl 不错的视频学习链接（https://www.youtube.com/r...
基于搜索的路径规划原理讲解
03-13
基于搜索的路径规划原理讲解
路径规划最全综述+代码+可视化绘图（Dijkstra算法+A*算法+RRT算法等）
阿利同学的博客
12-25 6807
路径规划最全综述+代码+可视化绘图（Dijkstra算法+A*算法+RRT算法等）
【最短路径算法】一文掌握Dijkstra算法，详解与应用示例+代码
qq_35831906的博客
10-22 1万+
Dijkstra算法（迪杰斯特拉算法）是一种用于在加权图中查找从一个起始节点到所有其他节点的最短路径的算法。该算法最初由荷兰计算机科学家Edsger W. Dijkstra于1956年提出。Dijkstra算法适用于带有非负权重的有向图或无向图。log(V))，其中V是节点数。如果使用优先队列来优化，时间复杂度可以减小到O(EDijkstra算法在许多领域广泛应用，包括路线规划、网络路由、资源分配和许多其他需要找到最短路径的应用。
图搜索算法详解
qq_43341279的博客
04-22 630
这些算法通过遍历图的节点和边，寻找从起点到终点的最短路径或满足特定条件的路径。本文将详细介绍几种常见的图搜索算法，包括深度优先搜索（DFS）、广度优先搜索（BFS）、迪杰斯特拉算法（Dijkstra's algorithm）和A*搜索算法。通过深入了解和掌握DFS、BFS、Dijkstra算法和A*算法，我们可以更好地应用这些算法解决实际问题，如导航系统、社交网络分析和人工智能领域中的路径规划问题。A*搜索算法结合了DFS和BFS的特点，它使用启发式函数来指导搜索方向，从而更快地找到最短路径。
无人车路径规划算法---（4）基于搜索的路径规划算法 II（贪心/Astar）
weixin_40884570的博客
12-09 2054
上篇博客中介绍了一些基本的图搜索算法，其中也重点介绍了基于势场来实现的Dijkstra算法。本篇博客将介绍关于Heuristic Function的图搜索算法 开源了一个结合Dijkstra,Greedy,以及Astar算法的代码，更新在了博主的github主页上，链接见文末 1. Greedy Best First Search Algorithm（贪心算法） 如上篇博客所介绍，BFS与Dijstra算法在搜索目标时是向所有方向进行节点拓展的。如果任务中是需要向多个目标点进行搜索，那么此类方法显然.
A*路径搜索超详细代码
qq_16539009的博客
05-15 576
A*路径搜索matlab代码前言一、主函数main.m二、障碍物obstacle_map.m文件三、可视化visualize_map.m文件四、A* 路径搜索A_star_search.m文件五、初始化open 表六、取出open list 一个节点索引值七 扩展open list结果展示 前言 这里只记录代码以及代码详解，不讲解理论 提示：以下是本篇文章正文内容，下面案例可供参考 一、主函数main.m main.m文件 close all; clear all; clc; % 初始化起点，目标点，
Dijkstra算法
Lutte_的博客
10-17 215
题目描述： 你来到一个迷宫前。该迷宫由若干个房间组成，每个房间都有一个得分，第一次进入这个房间，你就可以得到这个分数。还有若干双向道路连结这些房间，你沿着这些道路从一个房间走到另外一个房间需要一些时间。游戏规定了你的起点和终点房间，你首要目标是从起点尽快到达终点，在满足首要目标的前提下，使得你的得分总和尽可能大。现在问题来了，给定房间、道路、分数、起点和终点等全部信息，你能计算在尽快离开迷宫的前
Python 遗传算法路径规划
yryryrkkx的博客
06-16 4817
目录文章目录前言二、主要内容三、使用步骤1.将压缩包下载解压2.读入数据3.最终结果前言 遗传算法（Genetic Algorithm，GA）最早是由美国的 John holland于20世纪70年代提出,该算法是根据大自然中生物体进化规律而设计提出的。是模拟达尔文生物进化论的自然选择和遗传学机理的生物进化过程的计算模型，是一种通过模拟自然进化过程搜索最优解的方法。相信对于路径规划来说，这种方法其实也是一种目前较好的寻找最优解的方法。一、遗传算法原理 原理都是一样的，有很多博客都
python单源最短路径——dijkstra算法
12-22
unvisited算法求解最短路径的Python = set(graph) while unvisited: # 代码示例： ```python class D选择当前ijkstra距: def __init__(self, graph离最小的节点 , start, current goal): self.graph = graph ...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

2021 Nqq

你的鼓励是我学习的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值