iis网站下载,南宁本地有几家网站开发,光学设计软件有哪些,高端网站设计企业路径规划#xff08;Path Planning#xff09;是指在给定地图、起始点和目标点的情况下#xff0c;确定应该采取的最佳路径。常见的路径规划算法包括A* 算法、Dijkstra 算法、RRT#xff08;Rapidly-exploring Random Tree#xff09;等。
目录
一.A*
1.算法原理
2.实…路径规划Path Planning是指在给定地图、起始点和目标点的情况下确定应该采取的最佳路径。常见的路径规划算法包括A* 算法、Dijkstra 算法、RRTRapidly-exploring Random Tree等。
目录
一.A*
1.算法原理
2.实例
二.Dijkstra
1.算法原理
2.实例
三.RRT
1.算法原理
2.实例
一.A*
A*算法找到的1条由图中节点以及边组成的路径。一般而言A*算法基于BFS(Breath First Search,广度优先遍历)同时也可以认为是对BFS搜索算法的优化。
1.算法原理
具体执行过程如下
(1) 搜索区域
以2维平面坐标下的简单搜索为例。 绿色方框代表起点和终点红色方框代表不可经过的点。
(2) 搜索起点
A*算法从起点开始向四周检索相邻方格并进行扩展直到扩展扫描到终点算法结束。
流程如下
① 从起点S开始把S作为一个等待检查的方格将其放入open_list中。
open_list待走的位置。相当于候选点集合每到1点将周围点放入候选集合中往后有可能往候选点去走。
闭集合已经走过的位置不会再被考虑。
② 寻找起点S周围可以到达的方格(最多8个)并将它们加入open_list同时设置他们的父方格为S;
③ 从open_list中删除起点S同时将S加入close_list;
④ 计算每个周围方格的F值(FGH);
G:从起点S按照生成的路径移动到指定方格的移动代价。有2种距离计算方式欧拉距离、曼哈顿距离。
H从指定方格移动到终点的估算成本。使用Manhattan方法忽略路径中的障碍物计算当前方格横向或纵向移动到达目标所经过的方格数。
⑤ 从open_list中选取F值最小的方格a并将其从open_list中删除放入close_list中;
⑥ 继续检查a的邻接方格
a). 忽略不可经过的方格以及被标记在close_list中的方格将剩余的方格加入open_list并计算这些方格的F值同时设置父方格为a
b). 如果某邻接的方格c已经在open_list中则需计算新的路径从S到c的值G判断是否需要更新
如果新的G值更小一些则修改父方格为方格a重新计算F值。注意H值不需要改变因为方格到达终点的预计消耗是固定的不需要重复计算。
如果新的G值更大一些那么说明新路径不是一条更优的路径则不需要更新值。
⑦ 继续从open_list中找出F值最小的跳跃至第⑤步继续循环执行。
⑧ 当open_list出现终点E时代表路径已经被找到搜索成功。
如果open_list为空那么说明没有找到合适的路径搜索失败。
2.实例
A*
import time
import numpy as np
from matplotlib.patches import Rectangle
import matplotlib.pyplot as plt
import heapq# 地图上的每1个点都是1个Point对象用于记录该点的类别、代价等信息
class Point:def __init__(self, x0, y0):self.x xself.y y# 0代表可通行1代表障碍物self.val 0 self.cost_g 0 self.cost_h 0self.cost_f 0self.parent None# 0不在开集合 1在开集合 -1在闭集合self.is_open 0# 用于heapq小顶堆的比较def __lt__(self, other):return self.cost_f other.cost_fclass Map:def __init__(self, map_size):self.map_size map_sizeself.width map_size[0]self.height map_size[1]self.map [[Point(x, y) for y in range(self.map_size[1])] for x in range(self.map_size[0])]# 手动设置障碍物可多次调用设置地图def set_obstacle(self, topleft, width, height):for x in range(topleft[0], topleft[0] width):for y in range(topleft[1], topleft[1] height):self.map[x][y].val 1class AStar:def __init__(self, map, start_point, end_point, connect_num4):self.map: Map mapself.start_point start_pointself.end_point end_point# 开集合先放入起点从起点开始遍历self.open_set [self.start_point]self.start_point.is_open 1 # 连通数目前支持4连通或8连通 self.connect_num connect_numself.diffuse_dir [(1, 0), (-1, 0), (0, 1), (0, -1)]def g_cost(self, p):计算 g 代价:当前点与父节点的距离 父节点的 g 代价欧氏距离x_dis abs(p.parent.x - p.x)y_dis abs(p.parent.y - p.y)return np.sqrt(x_dis ** 2 y_dis ** 2) p.parent.cost_gdef h_cost(self, p):计算 h 代价:当前点与终点之间的距离欧氏距离x_dis abs(self.end_point.x - p.x)y_dis abs(self.end_point.y - p.y)return np.sqrt(x_dis ** 2 y_dis ** 2)def is_valid_point(self, x, y):# 无效点超出地图边界或为障碍物if x 0 or x self.map.width:return Falseif y 0 or y self.map.height:return Falsereturn self.map.map[x][y].val 0def search(self):self.start_time time.time()p self.start_pointwhile not (p self.end_point):# 弹出代价最小的开集合点若开集合为空说明没有路径try:p heapq.heappop(self.open_set)except:raise No path found, algorithm failed!!!p.is_open -1# 遍历周围点for i in range(self.connect_num):dir_x, dir_y self.diffuse_dir[i]self.diffusion_point(p.x dir_x, p.y dir_y, p)return self.build_path(p) # p self.end_pointdef diffusion_point(self, x, y, parent):# 无效点或者在闭集合中跳过if not self.is_valid_point(x, y) or self.map.map[x][y].is_open -1:returnp self.map.map[x][y]pre_parent p.parentp.parent parent# 先计算出当前点的总代价cost_g self.g_cost(p)cost_h self.h_cost(p)cost_f cost_g cost_h# 如果在开集合中判断当前点和开集合中哪个点代价小换成小的相同x,y的点h值相同g值可能不同if p.is_open 1:if cost_f p.cost_f:# 如果从当前parent遍历过来的代价更小替换成当前的代价和父节点p.cost_g, p.cost_h, p.cost_f cost_g, cost_h, cost_felse:# 如果从之前父节点遍历过来的代价更小保持之前的代价和父节点p.parent pre_parentelse:# 如果不在开集合中说明之间没遍历过直接加到开集合里就好p.cost_g, p.cost_h, p.cost_f cost_g, cost_h, cost_fheapq.heappush(self.open_set, p)p.is_open 1def build_path(self, p):print(search time: , time.time() - self.start_time, seconds)# 回溯完整路径path []while p ! self.start_point:path.append(p)p p.parentprint(search time: , time.time() - self.start_time, seconds)# 打印开集合、闭集合的数量print(open set count: , len(self.open_set))close_count 0for x in range(self.map.width):for y in range(self.map.height):close_count 1 if self.map.map[x][y].is_open -1 else 0print(close set count: , close_count)print(total count: , close_count len(self.open_set))# path path[::-1] # path为终点到起点的顺序可使用该语句翻转return pathif __name__ __main__:map Map((6, 6))# 用于显示plt图ax plt.gca()ax.xaxis.set_ticks_position(top)ax.set_xlim([0, map.width])ax.set_ylim([0, map.height])plt.tight_layout()plt.grid(True, linestyle--, colorblack, linewidth1, axisboth)ax.xaxis.set_ticks_position(top)ax.invert_yaxis()# 设置障碍物map.set_obstacle([2, 2], 3, 1)map.set_obstacle([2, 3], 1, 2)# 设置起始点和终点并创建astar对象start_point map.map[1][1]end_point map.map[4][4]# 将障碍物及起点、终点显示到plt上ax.add_patch(Rectangle([2, 2], width3, height1, colorred))ax.add_patch(Rectangle([2, 3], width1, height2, colorred))ax.add_patch(Rectangle([1, 1], width1, height1, colorgreen))ax.add_patch(Rectangle([4, 4], width1, height1, colorgreen))astar AStar(map, start_point, end_point)path astar.search()# 可视化起点到终点完整路径for p in path:if p!end_point:ax.add_patch(Rectangle([p.x, p.y], width1, height1, colorblue))plt.savefig(res.jpg) plt.show()二.Dijkstra
Dijkstra是1种适用于非负权值网络的单源最短路径算法可以给出从指定节点到图中其他节点的最短路径以及任意2点的最短路径。
1.算法原理
下面用1个示例讲述算法的原理过程如图所示1个有向权值图利用Dijkstra算法找到从节点1到节点6的最短路径。 (1) 首先进行初始化初始化1个空的open listclose list 以及parent。将起点及其距离信息放入到open list 中将起点及其父亲节点放入parent中起点的父节点为None。
open list 存放已经访问的从该节点到起点有路径的结点(有路径但不一定是最优路径) close list 存放那些已经找到最优路径的结点。 parent 存放结点的父子关系方便后面路径回溯。 (2) 按步骤执行 (3) 按步骤执行 (4) 按步骤执行 (5) 按步骤执行 (6) 按步骤执行 (7) 按步骤执行 (8) 路径回溯
根据parent中的继承关系从终点向起点回溯得到从起点到终点的最短路径为1476最短路径长为6。
2.实例
import heapqdef dijkstra(graph,start):distances {vertex: float(infinity) for vertex in graph}distances[start] 0# 初始化父亲节点parent {vertex: None for vertex in graph}priority_queue [(0,start)]while priority_queue:# 弹出堆中距离最小的节点current_distance, current_vertex heapq.heappop(priority_queue)# 如果当前距离已经大于已知的最短距离则跳过if current_distance distances[current_vertex]:continue# 更新相邻节点的距离for neighbor, weight in graph[current_vertex].items():distance current_distance weight# 如果找到更短的路径则更新距离并将节点加入优先队列if distance distances[neighbor]:distances[neighbor] distanceparent[neighbor] current_vertexheapq.heappush(priority_queue, (distance, neighbor))# print(加入后的队列:,priority_queue)return distances, parent# 输出路径回溯
def get_path(parent,end):path []current endwhile current is not None:path.append(current)current parent[current]path.reverse()return pathgraph {1: {2: 2, 4: 1},2: {4: 3, 5: 11 },3: {1: 4, 6: 5},4: {5: 2, 6: 8, 7: 4},5: {7: 6},6:{},7: {6: 1},
}
start_node1
end_node 6
distances, parent dijkstra(graph, start_node)
path get_path(parent, end_node)
print(f从节点 {start_node} 到节点 {end_node} 的路径:, path)三.RRT
1.算法原理
快速随机树算法Rapid Random Tree,RRT以初始的1个根节点通过随机采样的方法在空间搜索然后添加叶节点来不断扩展随机树。当目标点进入随机树里面后随机树扩展立即停止此时能找到1条从起始点到目标点的路径。
算法的计算过程如下
(1) 初始化随机树。设置起点目标点和搜索空间将起点作为随机树搜索的起点此时树中只包含1个节点即根节点
(2)在中随机采样。在中随机采样1个点若不在障碍物范围内则计算随机树中所有节点到的欧式距离并找到距离最近的节点若在障碍物范围内则重新采样
(3)生成新节点。在和连线方向由固定步长距离生成1个新的节点并判断是否在障碍物范围内若不在障碍物范围内则将添加到随机树中并将的父节点设置为否则的话返回(2)重新对环境进行随机采样
(4)停止搜索。重复步骤(2)、(3)当和之间的距离小于设定的阈值时则代表随机树已经到达了目标点将作为最后1个路径节点加入到随机树中算法结束并得到所规划的路径 。
2.实例
import math
import random
import matplotlib.pyplot as plt
import numpy as np
from celluloid import Cameraclass RRT:class Node:def __init__(self, x, y):self.x xself.y yself.path_x []self.path_y []self.parent Noneclass AreaBounds:def __init__(self, area):self.xmin float(area[0])self.xmax float(area[1])self.ymin float(area[2])self.ymax float(area[3])def __init__(self,start,goal,obstacle_list,rand_area,expand_dis1.0,goal_sample_rate5,max_iter500,play_areaNone,robot_radius0.0,):Setting Parameterstart:起点 [x,y]goal:目标点 [x,y]obstacleList:障碍物位置列表 [[x,y,size],...]rand_area: 采样区域 x,y ∈ [min,max]play_area: 约束随机树的范围 [xmin,xmax,ymin,ymax]robot_radius: 机器人半径expand_dis: 扩展的步长goal_sample_rate: 采样目标点的概率百分制.default: 5即表示5%的概率直接采样目标点self.start self.Node(start[0], start[1]) # 根节点(0,0)self.end self.Node(goal[0], goal[1]) # 终点(6,10)self.min_rand rand_area[0] # -2 树枝生长区域xminself.max_rand rand_area[1] # 15 xmaxif play_area is not None:self.play_area self.AreaBounds(play_area) # 树枝生长区域左下(-2,0)右上(12,14)else:self.play_area None # 数值无限生长self.expand_dis expand_dis # 树枝一次的生长长度self.goal_sample_rate goal_sample_rate # 多少概率直接选终点self.max_iter max_iter # 最大迭代次数self.obstacle_list obstacle_list # 障碍物的坐标和半径self.node_list [] # 保存节点self.robot_radius robot_radius # 随机点的搜索半径# 路径规划def planning(self, animationTrue, camaraNone):# 先在节点列表中保存起点self.node_list [self.start]for i in range(self.max_iter):# 随机选取一个节点rnd_node self.sample_free()# 从已知节点中选择和目标节点最近的节点nearest_ind self.get_nearest_node_index(self.node_list, rnd_node)nearest_node self.node_list[nearest_ind]new_node self.steer(nearest_node, rnd_node, self.expand_dis)# 判断新点是否在规定的树的生长区域内新点和最近点之间是否存在障碍物if self.is_inside_play_area(new_node, self.play_area) and \self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):# 都满足才保存该点作为树节点self.node_list.append(new_node)# 如果此时得到的节点到目标点的距离小于扩展步长则直接将目标点作为xrand。if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) self.expand_dis:# 以新点为起点向终点画树枝final_node self.steer(self.node_list[-1], self.end, self.expand_dis)# 如果最新点和终点之间没有障碍物Trueif self.obstacle_free(final_node, self.obstacle_list, self.robot_radius):return self.generate_final_course(len(self.node_list) - 1)if animation and i % 5 0:self.draw_graph(rnd_node, camara)return None # cannot find path# 距离最近的已知节点坐标随机坐标从已知节点向随机节点的延展的长度def steer(self, from_node, to_node, extend_lengthfloat(inf)):# d已知点和随机点之间的距离theta两个点之间的夹角d, theta self.calc_distance_and_angle(from_node, to_node)# 如果树枝的生长长度超出了随机点就用随机点位置作为新节点if extend_length d:new_x to_node.xnew_y to_node.y# 如果树生长长度没达到随机点长度就截取长度为extend_length的节点作为新节点else:new_x from_node.x math.cos(theta) * extend_lengthnew_y from_node.y math.sin(theta) * extend_lengthnew_node self.Node(new_x, new_y)new_node.path_x [from_node.x]new_node.path_y [from_node.y]new_node.path_x.append(new_x)new_node.path_y.append(new_y)new_node.parent from_nodereturn new_nodedef generate_final_course(self, goal_ind):path [[self.end.x, self.end.y]]node self.node_list[goal_ind]while node.parent is not None:path.append([node.x, node.y])node node.parentpath.append([node.x, node.y])return pathdef calc_dist_to_goal(self, x, y):计算(x,y)离目标点的距离dx x - self.end.xdy y - self.end.yreturn math.hypot(dx, dy)def sample_free(self):if random.randint(0, 100) self.goal_sample_rate:rnd self.Node(random.uniform(self.min_rand, self.max_rand),random.uniform(self.min_rand, self.max_rand))else: # goal point samplingrnd self.Node(self.end.x, self.end.y)return rnd# 绘制搜索过程def draw_graph(self, rndNone, cameraNone):if camera None:plt.clf()# 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 rnd is not None:plt.plot(rnd.x, rnd.y, ^k)if self.robot_radius 0.0:self.plot_circle(rnd.x, rnd.y, self.robot_radius, -r)# 画生成的树for node in self.node_list:if node.parent:plt.plot(node.path_x, node.path_y, -g)# 画障碍物for (ox, oy, size) in self.obstacle_list:self.plot_circle(ox, oy, size)# 画出可行区域if self.play_area is not None:plt.plot([self.play_area.xmin, self.play_area.xmax,self.play_area.xmax, self.play_area.xmin,self.play_area.xmin],[self.play_area.ymin, self.play_area.ymin,self.play_area.ymax, self.play_area.ymax,self.play_area.ymin],-k)# 画出起点和目标点plt.plot(self.start.x, self.start.y, xr)plt.plot(self.end.x, self.end.y, xr)plt.axis(equal)plt.axis([-2, 15, -2, 15])plt.grid(True)plt.pause(0.01)if camera ! None:camera.snap()staticmethoddef plot_circle(x, y, size, color-b):deg list(range(0, 360, 5))deg.append(0)xl [x size * math.cos(np.deg2rad(d)) for d in deg]yl [y size * math.sin(np.deg2rad(d)) for d in deg]plt.plot(xl, yl, color)staticmethoddef get_nearest_node_index(node_list, rnd_node):# 计算所有已知节点和随机节点之间的距离dlist [(node.x - rnd_node.x) ** 2 (node.y - rnd_node.y) ** 2for node in node_list]minind dlist.index(min(dlist))return minindstaticmethoddef is_inside_play_area(node, play_area):if play_area is None:return Trueif node.x play_area.xmin or node.x play_area.xmax or \node.y play_area.ymin or node.y play_area.ymax:return Falseelse:return Truestaticmethoddef obstacle_free(node, obstacleList, robot_radius):# 目标点障碍物中点和半径移动时的占地半径if node is None:return Falsefor (ox, oy, size) in obstacleList:dx_list [ox - x for x in node.path_x]dy_list [oy - y for y in node.path_y]d_list [dx * dx dy * dy for (dx, dy) in zip(dx_list, dy_list)]if min(d_list) (size robot_radius) ** 2:return Falsereturn Truestaticmethoddef calc_distance_and_angle(from_node, to_node):计算两个节点间的距离和方位角dx to_node.x - from_node.xdy to_node.y - from_node.yd math.hypot(dx, dy)theta math.atan2(dy, dx)return d, thetadef main(gx6.0, gy10.0):print(start __file__)fig plt.figure(1)camera Camera(fig) # 保存动图时使用show_animation TrueobstacleList [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),(9, 5, 2), (8, 10, 1)] # [x, y, radius]# Set Initial parametersrrt RRT(start[0, 0],goal[12, 12],rand_area[-2, 15],obstacle_listobstacleList,play_area[-2, 15, 0, 15],# 搜索半径robot_radius0.2)path rrt.planning(animationshow_animation, camaracamera)if path is None:print(Cannot find path)else:print(found path!!)# 绘制最终路径if show_animation:print(start drawing the final path)rrt.draw_graph(cameracamera)plt.grid(True)plt.pause(0.01)plt.plot([x for (x, y) in path], [y for (x, y) in path], -r)if camera ! None:camera.snap()animation camera.animate()animation.save(trajectory_res.gif)plt.show()if __name__ __main__:main()
