常用路径规划算法简介及python程序

1、前言

在这篇博文中，我将为您介绍A算法、D算法、人工势场算法和Dijkstra算法等路径规划算法。我将详细解释它们的原理和优缺点，并展示一些学者编写的Python程序来演示它们的运行效果。通过阅读本文，您将对这些算法有一定的了解，并能够在实际应用中选择最适合的算法。

2、D*算法

2.1简介

D*规划算法是一种用于路径规划的增量式搜索算法。它可以在已知环境地图的情况下，根据实时的传感器信息和环境变化来动态更新路径规划结果。

D*规划算法的核心思想是通过不断地修正路径的代价值来逐步优化路径规划结果。它使用了两个重要的数据结构：状态图和代价图。状态图表示了地图中的各个位置以及它们之间的连接关系，而代价图则记录了每个位置到目标位置的代价值。

D*规划算法的基本流程如下：

1. 初始化：设置起始位置和目标位置，并初始化状态图和代价图。
2. 通过启发式函数计算起始位置到目标位置的估计代价值，并将起始位置加入到开放列表中。
3. 进入循环，直到找到最优路径或者无法找到路径：
   a. 选择开放列表中代价最小的位置作为当前位置。
   b. 更新当前位置周围的邻居节点的代价值，并将其加入到开放列表中。
   c. 如果目标位置的代价值发生变化，则重新计算路径。
4. 根据最终的路径结果进行导航或执行其他操作。
   D*规划算法具有较好的实时性能和适应性，可以应对环境变化和动态障碍物的情况。

2.2优缺点

2.2.1 优点

1. 实时性：D*规划算法能够在运行时根据环境的变化实时更新路径信息，适用于动态环境中的路径规划问题。
2. 高效性：D*规划算法通过增量式的更新方式，避免了对整个地图进行重新规划，从而减少了计算量，提高了路径规划的效率。
3. 适应性：D*规划算法能够根据环境的变化自适应地调整路径，可以应对障碍物的移动或新增等情况。

2.2.2 缺点

1. 初始规划开销较大：D*规划算法需要进行一次全局规划来初始化路径信息，这个过程可能会消耗较多的计算资源和时间。
2. 对地图信息要求高：D*规划算法需要准确的环境地图信息，如果地图信息不准确或者不完整，可能会导致路径规划结果不理想。
3. 对计算资源要求高：D*规划算法在实时更新路径信息时需要进行大量的计算，对计算资源要求较高。

2.3 python程序

python程序如下：

"""D* grid planningauthor: Nirnay Roy"""
import mathfrom sys import maxsizeimport matplotlib.pyplot as pltshow_animation = Trueclass State:def __init__(self, x, y):self.x = xself.y = yself.parent = Noneself.state = "."self.t = "new"  # tag for stateself.h = 0self.k = 0def cost(self, state):if self.state == "#" or state.state == "#":return maxsizereturn math.sqrt(math.pow((self.x - state.x), 2) +math.pow((self.y - state.y), 2))def set_state(self, state):""".: new#: obstaclee: oparent of current state*: closed states: current state"""if state not in ["s", ".", "#", "e", "*"]:returnself.state = stateclass Map:def __init__(self, row, col):self.row = rowself.col = colself.map = self.init_map()def init_map(self):map_list = []for i in range(self.row):tmp = []for j in range(self.col):tmp.append(State(i, j))map_list.append(tmp)return map_listdef get_neighbors(self, state):state_list = []for i in [-1, 0, 1]:for j in [-1, 0, 1]:if i == 0 and j == 0:continueif state.x + i < 0 or state.x + i >= self.row:continueif state.y + j < 0 or state.y + j >= self.col:continuestate_list.append(self.map[state.x + i][state.y + j])return state_listdef set_obstacle(self, point_list):for x, y in point_list:if x < 0 or x >= self.row or y < 0 or y >= self.col:continueself.map[x][y].set_state("#")class Dstar:def __init__(self, maps):self.map = mapsself.open_list = set()def process_state(self):x = self.min_state()if x is None:return -1k_old = self.get_kmin()self.remove(x)if k_old < x.h:for y in self.map.get_neighbors(x):if y.h <= k_old and x.h > y.h + x.cost(y):x.parent = yx.h = y.h + x.cost(y)elif k_old == x.h:for y in self.map.get_neighbors(x):if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y) \or y.parent != x and y.h > x.h + x.cost(y):y.parent = xself.insert(y, x.h + x.cost(y))else:for y in self.map.get_neighbors(x):if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y):y.parent = xself.insert(y, x.h + x.cost(y))else:if y.parent != x and y.h > x.h + x.cost(y):self.insert(y, x.h)else:if y.parent != x and x.h > y.h + x.cost(y) \and y.t == "close" and y.h > k_old:self.insert(y, y.h)return self.get_kmin()def min_state(self):if not self.open_list:return Nonemin_state = min(self.open_list, key=lambda x: x.k)return min_statedef get_kmin(self):if not self.open_list:return -1k_min = min([x.k for x in self.open_list])return k_mindef insert(self, state, h_new):if state.t == "new":state.k = h_newelif state.t == "open":state.k = min(state.k, h_new)elif state.t == "close":state.k = min(state.h, h_new)state.h = h_newstate.t = "open"self.open_list.add(state)def remove(self, state):if state.t == "open":state.t = "close"self.open_list.remove(state)def modify_cost(self, x):if x.t == "close":self.insert(x, x.parent.h + x.cost(x.parent))def run(self, start, end):rx = []ry = []self.insert(end, 0.0)while True:self.process_state()if start.t == "close":breakstart.set_state("s")s = starts = s.parents.set_state("e")tmp = startwhile tmp != end:tmp.set_state("*")rx.append(tmp.x)ry.append(tmp.y)if show_animation:plt.plot(rx, ry, "-r")plt.pause(0.01)if tmp.parent.state == "#":self.modify(tmp)continuetmp = tmp.parenttmp.set_state("e")return rx, rydef modify(self, state):self.modify_cost(state)while True:k_min = self.process_state()if k_min >= state.h:breakdef main():m = Map(100, 100)ox, oy = [], []for i in range(-10, 60):ox.append(i)oy.append(-10)for i in range(-10, 60):ox.append(60)oy.append(i)for i in range(-10, 61):ox.append(i)oy.append(60)for i in range(-10, 61):ox.append(-10)oy.append(i)for i in range(-10, 40):ox.append(20)oy.append(i)for i in range(0, 40):ox.append(40)oy.append(60 - i)print([(i, j) for i, j in zip(ox, oy)])m.set_obstacle([(i, j) for i, j in zip(ox, oy)])start = [10, 10]goal = [50, 10]if show_animation:plt.plot(ox, oy, ".k")plt.plot(start[0], start[1], "og")plt.plot(goal[0], goal[1], "xb")plt.axis("equal")start = m.map[start[0]][start[1]]end = m.map[goal[0]][goal[1]]dstar = Dstar(m)rx, ry = dstar.run(start, end)if show_animation:plt.plot(rx, ry, "-r")plt.show()if __name__ == '__main__':main()

3、A*算法

A*算法是一种常用的启发式搜索算法，用于在图形结构中找到最短路径。它综合了广度优先搜索和贪婪最佳优先搜索的特点，通过评估函数来选择下一步要扩展的节点。

A*算法的原理如下：
首先，定义一个开放列表和一个关闭列表。开放列表用于存储待扩展的节点，关闭列表用于存储已经扩展过的节点。
将起始节点加入到开放列表中，并将其评估函数值设为0。
重复以下步骤直到找到目标节点或者开放列表为空：
a. 从开放列表中选择评估函数值最小的节点作为当前节点。
b. 将当前节点从开放列表中移除，并加入到关闭列表中。
c. 对当前节点的相邻节点进行遍历：
如果相邻节点已经在关闭列表中，则忽略。
如果相邻节点不在开放列表中，则将其加入到开放列表，并计算其评估函数值。
如果相邻节点已经在开放列表中，比较当前路径和之前路径的评估函数值，如果当前路径更优，则更新评估函数值和父节点。
如果开放列表为空，则表示无法找到目标节点，搜索失败。

3.1 优缺点：

3.1.1 优点：

可以保证找到最短路径，因为它综合了广度优先搜索和贪婪最佳优先搜索的特点。
在启发函数选择合适的情况下，可以高效地搜索大规模图形结构。

3.1.2 缺点：

启发函数的选择对算法的性能有很大影响，不同的启发函数可能导致不同的结果。
在某些情况下，A*算法可能会陷入局部最优解，无法找到全局最优解。

3.2 python程序

python程序：

"""A* grid planningauthor: Atsushi Sakai(@Atsushi_twi)Nikos Kanargias ([e-mail])"""import mathimport matplotlib.pyplot as pltshow_animation = Trueclass AStarPlanner:def __init__(self, ox, oy, resolution, rr):"""Initialize grid map for a star planningox: x position list of Obstacles [m]oy: y position list of Obstacles [m]resolution: grid resolution [m]rr: robot radius[m]"""self.resolution = resolutionself.rr = rrself.min_x, self.min_y = 0, 0self.max_x, self.max_y = 0, 0self.obstacle_map = Noneself.x_width, self.y_width = 0, 0self.motion = self.get_motion_model()self.calc_obstacle_map(ox, oy)class Node:def __init__(self, x, y, cost, parent_index):self.x = x  # index of gridself.y = y  # index of gridself.cost = costself.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):"""A star path searchinput:s_x: start x position [m]s_y: start y position [m]gx: goal x position [m]gy: goal y position [m]output:rx: x position list of the final pathry: y position list of the final path"""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_grid_index(start_node)] = start_nodewhile True:if len(open_set) == 0:print("Open set is empty..")breakc_id = min(open_set,key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,open_set[o]))current = open_set[c_id]# show graphif show_animation:  # pragma: no coverplt.plot(self.calc_grid_position(current.x, self.min_x),self.calc_grid_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]# Add it to the closed setclosed_set[c_id] = current# expand_grid search grid based on motion modelfor i, _ in enumerate(self.motion):node = self.Node(current.x + self.motion[i][0],current.y + self.motion[i][1],current.cost + self.motion[i][2], c_id)n_id = self.calc_grid_index(node)# If the node is not safe, do nothingif not self.verify_node(node):continueif n_id in closed_set:continueif n_id not in open_set:open_set[n_id] = node  # discovered a new nodeelse:if open_set[n_id].cost > node.cost:# This path is the best until now. record itopen_set[n_id] = noderx, ry = self.calc_final_path(goal_node, closed_set)return rx, rydef calc_final_path(self, goal_node, closed_set):# generate final courserx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [self.calc_grid_position(goal_node.y, self.min_y)]parent_index = goal_node.parent_indexwhile parent_index != -1:n = closed_set[parent_index]rx.append(self.calc_grid_position(n.x, self.min_x))ry.append(self.calc_grid_position(n.y, self.min_y))parent_index = n.parent_indexreturn rx, ry@staticmethoddef calc_heuristic(n1, n2):w = 1.0  # weight of heuristicd = w * math.hypot(n1.x - n2.x, n1.y - n2.y)return ddef calc_grid_position(self, index, min_position):"""calc grid position:param index::param min_position::return:"""pos = index * self.resolution + min_positionreturn posdef calc_xy_index(self, position, min_pos):return round((position - min_pos) / self.resolution)def calc_grid_index(self, node):return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)def verify_node(self, node):px = self.calc_grid_position(node.x, self.min_x)py = self.calc_grid_position(node.y, self.min_y)if px < self.min_x:return Falseelif py < self.min_y:return Falseelif px >= self.max_x:return Falseelif py >= self.max_y:return False# collision checkif self.obstacle_map[node.x][node.y]:return Falsereturn Truedef calc_obstacle_map(self, ox, oy):self.min_x = round(min(ox))self.min_y = round(min(oy))self.max_x = round(max(ox))self.max_y = round(max(oy))print("min_x:", self.min_x)print("min_y:", self.min_y)print("max_x:", self.max_x)print("max_y:", self.max_y)self.x_width = round((self.max_x - self.min_x) / self.resolution)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 generationself.obstacle_map = [[False for _ in range(self.y_width)]for _ in range(self.x_width)]for ix in range(self.x_width):x = self.calc_grid_position(ix, self.min_x)for iy in range(self.y_width):y = self.calc_grid_position(iy, self.min_y)for iox, ioy in zip(ox, oy):d = math.hypot(iox - x, ioy - y)if d <= self.rr:self.obstacle_map[ix][iy] = Truebreak@staticmethoddef get_motion_model():# dx, dy, costmotion = [[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 motiondef main():print(__file__ + " start!!")# start and goal positionsx = 10.0  # [m]sy = 10.0  # [m]gx = 50.0  # [m]gy = 50.0  # [m]grid_size = 2.0  # [m]robot_radius = 1.0  # [m]# set obstacle positionsox, oy = [], []for i in range(-10, 60):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)if show_animation:  # pragma: no coverplt.plot(ox, oy, ".k")plt.plot(sx, sy, "og")plt.plot(gx, gy, "xb")plt.grid(True)plt.axis("equal")a_star = AStarPlanner(ox, oy, grid_size, robot_radius)rx, ry = a_star.planning(sx, sy, gx, gy)if show_animation:  # pragma: no coverplt.plot(rx, ry, "-r")plt.pause(0.001)plt.show()if __name__ == '__main__':main()

4、人工势场算法

Potential Field based path planner是一种基于势场的路径规划算法，它模拟了物体在势场中的运动方式来寻找最优路径。该算法通过将环境划分为障碍物和目标区域，并为每个区域分配一个势能值，来引导机器人或其他移动物体避开障碍物并朝向目标。

该算法的原理如下：

• 定义势能场：将环境划分为障碍物和目标区域，并为每个区域分配一个势能值。通常情况下，障碍物区域的势能值较高，而目标区域的势能值较低。
• 计算合力：根据机器人当前位置和周围环境的势能值，计算合力。合力由两部分组成：引力和斥力。引力指向目标区域，斥力来自障碍物区域。
• 更新机器人位置：根据合力的方向和大小，更新机器人的位置。机器人会受到引力的吸引，同时受到斥力的排斥，从而朝着势能值较低的目标区域移动。
• 重复步骤2和3，直到机器人到达目标区域或无法找到可行路径。

4.1优缺点

4.1.1优点：

• 算法简单易实现：该算法的原理相对简单，容易理解和实现。

• 实时性较好：由于只需要计算当前位置周围的势能值和合力，因此算法具有较好的实时性能。

• 能够处理动态环境：由于势能场是根据当前环境计算的，因此可以适应动态环境的变化。

4.1.2缺点：

• 容易陷入局部最优：由于算法只考虑当前位置周围的势能值，可能会导致机器人陷入局部最优解，无法找到全局最优路径。
• 难以处理复杂环境：在存在大量障碍物或复杂形状的环境中，势能场的计算和更新可能变得复杂且耗时。
• 缺乏路径规划的优化策略：该算法主要通过势能场来引导机器人移动，缺乏对路径规划的优化策略，可能导致路径长度较长或不够平滑。

4.2 python程序

"""Potential Field based path plannerauthor: Atsushi Sakai (@Atsushi_twi)"""from collections import deque
import numpy as np
import matplotlib.pyplot as plt# Parameters
KP = 5.0  # attractive potential gain
ETA = 100.0  # repulsive potential gain
AREA_WIDTH = 30.0  # potential area width [m]
# the number of previous positions used to check oscillations
OSCILLATIONS_DETECTION_LENGTH = 3show_animation = Truedef calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0xw = int(round((maxx - minx) / reso))yw = int(round((maxy - miny) / reso))# calc each potentialpmap = [[0.0 for i in range(yw)] for i in range(xw)]for ix in range(xw):x = ix * reso + minxfor iy in range(yw):y = iy * reso + minyug = calc_attractive_potential(x, y, gx, gy)uo = calc_repulsive_potential(x, y, ox, oy, rr)uf = ug + uopmap[ix][iy] = ufreturn pmap, minx, minydef calc_attractive_potential(x, y, gx, gy):return 0.5 * KP * np.hypot(x - gx, y - gy)def calc_repulsive_potential(x, y, ox, oy, rr):# search nearest obstacleminid = -1dmin = float("inf")for i, _ in enumerate(ox):d = np.hypot(x - ox[i], y - oy[i])if dmin >= d:dmin = dminid = i# calc repulsive potentialdq = np.hypot(x - ox[minid], y - oy[minid])if dq <= rr:if dq <= 0.1:dq = 0.1return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2else:return 0.0def get_motion_model():# dx, dymotion = [[1, 0],[0, 1],[-1, 0],[0, -1],[-1, -1],[-1, 1],[1, -1],[1, 1]]return motiondef oscillations_detection(previous_ids, ix, iy):previous_ids.append((ix, iy))if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):previous_ids.popleft()# check if contains any duplicates by copying into a setprevious_ids_set = set()for index in previous_ids:if index in previous_ids_set:return Trueelse:previous_ids_set.add(index)return Falsedef potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):# calc potential fieldpmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)# search pathd = np.hypot(sx - gx, sy - gy)ix = round((sx - minx) / reso)iy = round((sy - miny) / reso)gix = round((gx - minx) / reso)giy = round((gy - miny) / reso)if show_animation:draw_heatmap(pmap)# 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])plt.plot(ix, iy, "*k")plt.plot(gix, giy, "*m")rx, ry = [sx], [sy]motion = get_motion_model()previous_ids = deque()while d >= reso:minp = float("inf")minix, miniy = -1, -1for i, _ in enumerate(motion):inx = int(ix + motion[i][0])iny = int(iy + motion[i][1])if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:p = float("inf")  # outside areaprint("outside potential!")else:p = pmap[inx][iny]if minp > p:minp = pminix = inxminiy = inyix = minixiy = miniyxp = ix * reso + minxyp = iy * reso + minyd = np.hypot(gx - xp, gy - yp)rx.append(xp)ry.append(yp)if (oscillations_detection(previous_ids, ix, iy)):print("Oscillation detected at ({},{})!".format(ix, iy))breakif show_animation:plt.plot(ix, iy, ".r")plt.pause(0.01)print("Goal!!")return rx, rydef draw_heatmap(data):data = np.array(data).Tplt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)def main():print("potential_field_planning start")sx = 0.0  # start x position [m]sy = 10.0  # start y positon [m]gx = 30.0  # goal x position [m]gy = 30.0  # goal y position [m]grid_size = 0.5  # potential grid size [m]robot_radius = 50.0  # robot radius [m]ox = [15.0, 5.0, 20.0, 25.0]  # obstacle x position list [m]oy = [25.0, 15.0, 26.0, 25.0]  # obstacle y position list [m]if show_animation:plt.grid(True)plt.axis("equal")# path generation_, _ = potential_field_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_radius)if show_animation:plt.show()if __name__ == '__main__':print(__file__ + " start!!")main()print(__file__ + " Done!!")

5、Dijkstra算法

是一种用于解决单源最短路径问题的经典算法。它可以找到从一个起始节点到其他所有节点的最短路径。

Dijkstra算法的流程如下：

• 创建一个集合S，用于存放已经找到最短路径的节点。 初始化距离数组dist，将起始节点的距离设为0，其他节点的距离设为无穷大。
• 重复以下步骤，直到集合S包含所有节点：
  a. 从距离数组dist中选择一个距离最小的节点u，将其加入集合S。
  b. 对于节点u的每个邻居节点v，如果通过节点u到达节点v的路径比当前记录的最短路径更短，则更新距离数组dist中节点v的距离。
• 距离数组dist中记录的就是从起始节点到每个节点的最短路径。

5.1优缺点

5.1.1优点：

• 算法保证能够找到最短路径。
• 可以处理有向图和无向图。
• 可以处理带有负权边的图，但不能处理带有负权环的图。

5.1.2缺点：

-对于大规模图来说，算法的时间复杂度较高，为O(V^2)，其中V是节点的数量。
-无法处理带有负权环的图，因为在每次迭代中，算法会选择当前距离最小的节点，而负权环会导致无限循环。

5.2python程序

python程序：

"""Grid based Dijkstra planningauthor: Atsushi Sakai(@Atsushi_twi)"""import matplotlib.pyplot as plt
import mathshow_animation = Trueclass Dijkstra:def __init__(self, ox, oy, resolution, robot_radius):"""Initialize map for a star 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 = Noneself.resolution = resolutionself.robot_radius = robot_radiusself.calc_obstacle_map(ox, oy)self.motion = self.get_motion_model()class Node:def __init__(self, x, y, cost, parent_index):self.x = x  # index of gridself.y = y  # index of gridself.cost = costself.parent_index = parent_index  # index of previous Nodedef __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 = 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_nodewhile True:c_id = min(open_set, key=lambda o: open_set[o].cost)current = open_set[c_id]# show graphif 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]# Add it to the closed setclosed_set[c_id] = current# expand search grid based on motion modelfor move_x, move_y, move_cost in self.motion:node = 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:continueif not self.verify_node(node):continueif n_id not in open_set:open_set[n_id] = node  # Discover a new nodeelse:if open_set[n_id].cost >= node.cost:# This path is the best until now. record it!open_set[n_id] = noderx, ry = self.calc_final_path(goal_node, closed_set)return rx, rydef calc_final_path(self, goal_node, closed_set):# 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_indexwhile parent_index != -1:n = closed_set[parent_index]rx.append(self.calc_position(n.x, self.min_x))ry.append(self.calc_position(n.y, self.min_y))parent_index = n.parent_indexreturn rx, rydef calc_position(self, index, minp):pos = index * self.resolution + minpreturn posdef 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)def verify_node(self, node):px = self.calc_position(node.x, self.min_x)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):self.min_x = round(min(ox))self.min_y = round(min(oy))self.max_x = round(max(ox))self.max_y = round(max(oy))print("min_x:", self.min_x)print("min_y:", self.min_y)print("max_x:", self.max_x)print("max_y:", self.max_y)self.x_width = round((self.max_x - self.min_x) / self.resolution)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 generationself.obstacle_map = [[False for _ in range(self.y_width)]for _ in range(self.x_width)]for ix in range(self.x_width):x = self.calc_position(ix, self.min_x)for iy in range(self.y_width):y = self.calc_position(iy, self.min_y)for iox, ioy in zip(ox, oy):d = math.hypot(iox - x, ioy - y)if d <= self.robot_radius:self.obstacle_map[ix][iy] = Truebreak@staticmethoddef get_motion_model():# dx, dy, costmotion = [[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 motiondef main():print(__file__ + " start!!")# start and goal positionsx = -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 positionsox, oy = [], []for i in range(-10, 60):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)if show_animation:  # pragma: no coverplt.plot(ox, oy, ".k")plt.plot(sx, sy, "og")plt.plot(gx, gy, "xb")plt.grid(True)plt.axis("equal")dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)rx, ry = dijkstra.planning(sx, sy, gx, gy)if show_animation:  # pragma: no coverplt.plot(rx, ry, "-r")plt.pause(0.01)plt.show()if __name__ == '__main__':main()

说明：
接下来，根据生成的样本，生成道路地图的。输入参数包括采样点的x坐标和y坐标，机器人安全半径，以及占用栅格的KDTree二叉树。输出是一个表示道路地图的列表。

算法首先对采样点进行了KDTree的构建，以便进行高效的搜索。然后，遍历每个采样点，对其进行搜索和路径生成。对于每个采样点，通过查询KDTree获得与其最近的其他采样点，并检查路径是否可达。如果路径可达，则将相应的索引添加到边的ID列表中。如果边的ID数量达到了最大路径数N_KNN，则结束搜索。最后，将边的ID列表添加到道路地图中。

整个算法的流程是：构建KDTree -> 遍历采样点 -> 查询最近的其他采样点 -> 检查路径可达性 -> 添加边的ID到列表 -> 添加列表到道路地图。

该算法是迪杰斯特拉路径规划算法（Dijkstra Planning Algorithm），用于在给定的地图中寻找起点到目标点的最短路径。算法接受起点和目标点的坐标，以及可能的路径地图、样本坐标列表作为输入。

算法首先将起点节点和目标节点初始化为PNode对象，并将起点节点添加到open_set字典中。然后，进入循环，如果open_set为空，则表示找不到路径，打印提示信息并跳出循环。否则，从open_set中选择cost最小的节点作为当前节点，进行路径搜索的下一步。

在路径搜索的过程中，如果开启了动画展示（show_animation为True），则每次经过闭集中节点数量为偶数时会绘制当前节点，并监听esc键以停止模拟。如果当前节点的索引等于目标节点索引，则表示找到了目标节点，将目标节点的父节点索引设置为当前节点的父节点索引，并跳出循环。

接下来，将当前节点从open_set中删除，并将其添加到closed_set中。然后，根据运动模型，扩展搜索网格，计算当前节点与目标节点之间的距离，并创建一个新的PNode对象。如果新节点的索引已经在closed_set中，则跳过当前循环。否则，如果新节点的索引已经在open_set中，则比较新节点的cost与open_set中已有节点的cost，如果新节点的cost更小，则更新open_set中的节点的cost和父节点索引为新节点的cost和父节点索引。否则，将新节点添加到open_set中。

最后，如果没有找到路径（path_found为False），则返回空列表。否则，生成最终的路径坐标列表rx和ry。从目标节点开始，迭代找到每个节点的父节点，将节点的x和y坐标添加到rx和ry中。直到父节点索引为-1，表示到达起点。返回rx和ry作为最短路径的坐标列表。

总之，该算法使用迪杰斯特拉算法，在给定的路径地图中寻找起点到目标点的最短路径，并返回路径的坐标列表。