亳州有做网站的吗,全国电子商务公共服务网,做网站襄樊,个人备案后可以做电影网站吗目录 1、前言2、D*算法2.1简介2.2优缺点2.2.1 优点2.2.2 缺点 2.3 python程序 3、A*算法3.1 优缺点#xff1a;3.1.1 优点#xff1a;3.1.2 缺点#xff1a; 3.2 python程序 4、人工势场算法4.1优缺点4.1.1优点#xff1a;4.1.2缺点#xff1a; 4.2 python程序 5、Dijkstr… 目录 1、前言2、D*算法2.1简介2.2优缺点2.2.1 优点2.2.2 缺点 2.3 python程序 3、A*算法3.1 优缺点3.1.1 优点3.1.2 缺点 3.2 python程序 4、人工势场算法4.1优缺点4.1.1优点4.1.2缺点 4.2 python程序 5、Dijkstra算法5.1优缺点5.1.1优点5.1.2缺点 5.2python程序 1、前言
在这篇博文中我将为您介绍A算法、D算法、人工势场算法和Dijkstra算法等路径规划算法。我将详细解释它们的原理和优缺点并展示一些学者编写的Python程序来演示它们的运行效果。通过阅读本文您将对这些算法有一定的了解并能够在实际应用中选择最适合的算法。
2、D*算法
2.1简介
D*规划算法是一种用于路径规划的增量式搜索算法。它可以在已知环境地图的情况下根据实时的传感器信息和环境变化来动态更新路径规划结果。
D*规划算法的核心思想是通过不断地修正路径的代价值来逐步优化路径规划结果。它使用了两个重要的数据结构状态图和代价图。状态图表示了地图中的各个位置以及它们之间的连接关系而代价图则记录了每个位置到目标位置的代价值。
D*规划算法的基本流程如下
初始化设置起始位置和目标位置并初始化状态图和代价图。通过启发式函数计算起始位置到目标位置的估计代价值并将起始位置加入到开放列表中。进入循环直到找到最优路径或者无法找到路径 a. 选择开放列表中代价最小的位置作为当前位置。 b. 更新当前位置周围的邻居节点的代价值并将其加入到开放列表中。 c. 如果目标位置的代价值发生变化则重新计算路径。根据最终的路径结果进行导航或执行其他操作。 D*规划算法具有较好的实时性能和适应性可以应对环境变化和动态障碍物的情况。
2.2优缺点
2.2.1 优点
实时性D*规划算法能够在运行时根据环境的变化实时更新路径信息适用于动态环境中的路径规划问题。高效性D*规划算法通过增量式的更新方式避免了对整个地图进行重新规划从而减少了计算量提高了路径规划的效率。适应性D*规划算法能够根据环境的变化自适应地调整路径可以应对障碍物的移动或新增等情况。
2.2.2 缺点
初始规划开销较大D*规划算法需要进行一次全局规划来初始化路径信息这个过程可能会消耗较多的计算资源和时间。对地图信息要求高D*规划算法需要准确的环境地图信息如果地图信息不准确或者不完整可能会导致路径规划结果不理想。对计算资源要求高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 stateif 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, keylambda 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 openself.open_list.add(state)def remove(self, state):if state.t open:state.t closeself.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 (nkanatee.gr)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 pathstart_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,keylambda 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, rystaticmethoddef 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] Truebreakstaticmethoddef 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, vmax100.0, cmapplt.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 pathstart_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, keylambda 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] Truebreakstaticmethoddef 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作为最短路径的坐标列表。
总之该算法使用迪杰斯特拉算法在给定的路径地图中寻找起点到目标点的最短路径并返回路径的坐标列表。
