算法讲解

• 图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 positionsx = -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 positionsox, oy = [], [] # 设置障碍物的位置，图中的黑点# 设置周围的4堵墙for i in range(-10, 60): # 下方的墙 y = -10 x = -10 ~ 59ox.append(i)oy.append(-10.0)for i in range(-10, 60): # 右方的墙 x = 60 y = -10 ~ 59ox.append(60.0)oy.append(i)for i in range(-10, 61): # 上方的墙 y = 60 x = -10 ~ 60ox.append(i)oy.append(60.0)for i in range(-10, 61): # 左边的墙 x = -10 y = -10 ~ 60ox.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 = trueif 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,ryrx, ry = dijkstra.planning(sx, sy, gx, gy)

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

• def __init__(self, ox, oy, resolution, robot_radius)

 # 将传进来的值进行初始化，resolution就是传进来的grid_sizedef __init__(self, ox, oy, resolution, robot_radius): """Initialize map for planningox: x position list of Obstacles [m]oy: y position list of Obstacles [m]resolution: grid resolution [m]rr: robot radius[m]"""self.min_x = Noneself.min_y = Noneself.max_x = Noneself.max_y = Noneself.x_width = Noneself.y_width = Noneself.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: -10print("min_y:", self.min_y) # min_y: -10print("max_x:", self.max_x) # max_x: 60print("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=35self.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--34for ix in range(self.x_width): # ix表示栅格在x方向的下标, ix 范围是 0~34x = self.calc_position(ix, self.min_x) # 通过栅格下标计算位置 x = -10 ~ 58(每个值间隔为2)for iy in range(self.y_width): # iy表示栅格在y方向的下标, iy 范围是 0~34y = 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 # 将障碍物所在栅格设置为truebreak

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~34pos = 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 gridself.y = y  # index of gridself.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 Falseif py < self.min_y:return Falseif px >= self.max_x:return Falseif py >= self.max_y:return Falseif self.obstacle_map[node.x][node.y]: # 判断是不是障碍物return Falsereturn 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_idcurrent = open_set[c_id] # current表示cost值最小的节点,open_set[c_id]类似g(n),c_id表示g,cost和n是一致的# show graph 动画仿真if show_animation:  # pragma: no coverplt.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_indexgoal_node.cost = current.costbreak# Remove the item from the open setdel open_set[c_id] # 将该节点在open_set中删掉# Add it to the closed setclosed_set[c_id] = current # 将删除的节点加入到closed_set# expand search grid based on motion modelfor move_x, move_y, move_cost in self.motion: # 遍历邻接节点，以运动方式进行遍历，这里要遍历8种情况node = self.Node(current.x + move_x, # 先求邻接节点，根据当前x,y与运动方式得到的邻接节点的x,ycurrent.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:continueif not self.verify_node(node): # 计算邻接结点是否可行，有没有超出范围，如果为false，则跳出下面操作重新循环continueif n_id not in open_set: # 如果新的n_id不在open_set中，就添加进去open_set[n_id] = node  # Discover a new nodeelse: # 如果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 courserx, 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设置为-1n = closed_set[parent_index] # 这里的parent_index就是node里面的c_idrx.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 planningauthor: Atsushi Sakai(@Atsushi_twi)"""# 这里需要file->setting->tools->Python scientific中取消勾选show plots in tool window
import matplotlib.pyplot as plt
import mathshow_animation = Trueclass Dijkstra:def __init__(self, ox, oy, resolution, robot_radius): # 调用_init_将传进来的值进行初始化，resolution就是传进来的grid_size"""Initialize map for planningox: x position list of Obstacles [m]oy: y position list of Obstacles [m]resolution: grid resolution [m]rr: robot radius[m]"""self.min_x = Noneself.min_y = Noneself.max_x = Noneself.max_y = Noneself.x_width = Noneself.y_width = Noneself.obstacle_map = None# resolution就是传进来的grid_size,传进来之后赋值给类里面的成员self.resolution = resolutionself.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 gridself.y = y  # index of gridself.cost = cost  # g(n) 代价值就是路径权值# index of previous Node，记录自己的上一个栅格节点，用来存储路径self.parent_index = parent_indexdef __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 searchinput: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 pathry: 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 coverplt.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_indexgoal_node.cost = current.costbreak# Remove the item from the open setdel open_set[c_id] # 将该节点在open_set中删掉# Add it to the closed setclosed_set[c_id] = current # 将删除的节点加入到closed_set# expand search grid based on motion modelfor move_x, move_y, move_cost in self.motion: # 遍历邻接节点，以运动方式进行遍历，这里要遍历8种情况node = self.Node(current.x + move_x, # 先求邻接节点，根据当前x,y与运动方式得到邻接节点的x,ycurrent.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:continueif not self.verify_node(node): # 计算邻接结点是否可行，有没有超出范围，如果为false，则跳出下面操作重新循环continueif n_id not in open_set: # 如果新的n_id不在open_set中，就添加进去open_set[n_id] = node  # Discover a new nodeelse: # 如果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, rydef calc_final_path(self, goal_node, closed_set): # 使用rx和ry两个列表来存储路径，求出具体的真实下标# generate final courserx, 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设置为-1n = closed_set[parent_index] # 这里的parent_index就是node里面的c_idrx.append(self.calc_position(n.x, self.min_x)) # n.x是0~34，n.y是0~34ry.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~ 58pos = index * self.resolution + minpreturn posdef 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 Falseif py < self.min_y:return Falseif px >= self.max_x:return Falseif py >= self.max_y:return Falseif self.obstacle_map[node.x][node.y]: # 判断是不是障碍物return Falsereturn Truedef 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: -10print("min_y:", self.min_y) # min_y: -10print("max_x:", self.max_x) # max_x: 60print("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=35self.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--34for ix in range(self.x_width): # ix表示栅格在x方向的下标, ix 范围是 0~34x = self.calc_position(ix, self.min_x) # 通过栅格下标计算位置 x = -10 ~ 58(每个值间隔为2)for iy in range(self.y_width): # iy表示栅格在y方向的下标, iy 范围是 0~34y = 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 # 将障碍物所在栅格设置为truebreak@staticmethoddef 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 motiondef main():# start and goal positionsx = -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 positionsox, oy = [], [] # 设置障碍物的位置，图中的黑点# 设置周围的4堵墙for i in range(-10, 60): # 下方的墙 y = -10 x = -10 ~ 59ox.append(i)oy.append(-10.0)for i in range(-10, 60): # 右方的墙 x = 60 y = -10 ~ 59ox.append(60.0)oy.append(i)for i in range(-10, 61): # 上方的墙 y = 60 x = -10 ~ 60ox.append(i)oy.append(60.0)for i in range(-10, 61): # 左边的墙 x = -10 y = -10 ~ 60ox.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 = trueif 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) # 申请一个对象Dijkstrarx, ry = dijkstra.planning(sx, sy, gx, gy) # 调用planning方法# 最开始的show_animation是trueif show_animation:  # pragma: no coverplt.plot(rx, ry, "-r")plt.pause(0.01)plt.show()if __name__ == '__main__':main()

1 基于搜索的路径规划 —— Dijkstra算法(python)相关推荐

1. 2 基于搜索的路径规划 —— A*算法

   文章目录 Python A*算法和Dijkstra算法本质上增加了启发式函数 增加启发式函数h(n) 完整代码 Python A*算法和Dijkstra算法本质上增加了启发式函数 下面是逻辑结构图,详 ...

2. 无人车路径规划算法—（3）基于搜索的路径规划算法 （BFS/DFS/Dijkstra）

   1.BFS(广度优先搜索) && DFS(深度优先搜索) 广度优先遍历图的方式为,一次性访问当前顶点的所有未访问状态相邻顶点,并依次对每个相邻顶点执行同样处理.因为要依次对每个相邻顶点 ...

3. 无人车路径规划算法---（4）基于搜索的路径规划算法 II（贪心/Astar）

   上篇博客中介绍了一些基本的图搜索算法,其中也重点介绍了基于势场来实现的Dijkstra算法.本篇博客将介绍关于Heuristic Function的图搜索算法 开源了一个结合Dijkstra,Gree ...

4. 自动驾驶路径规划——Dijkstra算法

   目录 前言 1. 深度优先(DFS)和广度优先(BFS) 2. 深度优先搜索(DFS) 2.1 算法基本思想 2.2 深度优先搜索算法(C) 3. 广度优先搜索(BFS) 3.1 算法基本思想 3.2 ...

5. 路径规划Dijkstra算法

   Dijkstra搜索最短路径: 整体思路 从起始节点开始,将邻域节点进行遍历,标注好邻域节点最小的累计路径长度,直到遍历到终止节点. 算法复杂度 naive的方式,算法复杂度为O(|V|2)O(|V| ...

6. 2021-11-06关节空间路径规划和算法（采样、搜索）或者末端轨迹优化？

   关节空间路径规划 一些概念 一. 摘自 运动规划ompl 1.1. 运动规划 (Motion Planning) 我们这里讲的 运动规划 ,有别于 轨迹规划 (Path Planning).一般来说, ...

7. 【自动驾驶】基于采样的路径规划算法——PRM（含python实现）

   文章目录 参考资料 1. 基本概念 1.1 基于随机采样的路径规划算法 1.2 概率路图算法(Probabilistic Road Map, PRM) 1.3 PRM算法的优缺点 1.4 PRM算法伪 ...

8. Apollo学习笔记（24）基于采样的路径规划算法

   之前的文章都是基于搜索的路径算法,这两天在又学习了一下基于采样的路径规划算法,这里做一下记录,最后会奉上大神的链接 基于采样的路径规划算法大致可以分为综合查询方法和单一查询方法两种. 前者首先构建路线 ...

9. 路径规划—— A* 算法

   参考原文:路径规划之 A* 算法 (weibo.com) A*(念做:A Star)算法是一种很常用的路径查找和图形遍历算法.它有较好的性能和准确度. 本... 路径规划之 A* 算法 A*(念做:A ...

最新文章

1. 如何使用Android studio创建签名
2. r720支持多少频率的内存吗_关于内存频率，高频和低频的性能差距大吗？明白这3点很重要...
3. 带排序动画的横向条形图
4. 编译时，输出信息重定向到文件
5. 协同遗漏的效果–使用简单的NIO客户端/服务器测量回送延迟
6. WebQML笔记-qml获取canvas中元素是否被按下
7. php文本框长度限制,php截取富文本框中的固定长度的字符
8. SpringBoot之HelloWorld
9. 基于matlab的小波去噪方法研究,基于matlab的小波去噪分析毕业论文.doc
10. 视频教程-WebService实战讲解课程-Java
11. 「吕本富」交易的四个阶段
12. 手机服务器艰辛之路(一)~手机服务器环境部署
13. LeetCode刷题之---上一个排序
14. yii通过uc实现同步登陆
15. ASO马甲包：马甲包上架注意事项
16. 探索 Java 中的 Date, Calendar, TimeZone 和Timestamp
17. 手机触摸屏扫描信号实测波形
18. （私人收藏）[开发必备]最全JQuery离线快速查找手册（可查询可学习，带实例）
19. 如何快速打胖包和瘦包
20. Codeforces - 1166E - The LCMs Must be Large

热门文章

1. 红米android版本,红米note2安卓版本号是多少?红米note2版本介绍
2. cmos存储器中存放了_CMOS存储器里放哪些东西
3. Cenots7安装vim
4. 快速原型模型是什么？
5. 布局资本市场的合生创展集团，能靠“买买买”出圈吗？
6. 索尼M36h怎么刷第三方recovery？
7. centos 安装 janus
8. mysql 1093 1142
9. CityMaker学习教程07 示例代码的使用CSharp
10. Java前端和后端的区别？