去哪找想做网站的客户,潍坊网站建设小程序制作,广州装修公司排行榜,地方门户网站系统建设方案路径规划——RRT算法
算法原理
RRT算法的全称是快速扩展随机树算法(Rapidly Exploring Random Tree)#xff0c;它的思想是选取一个初始点作为根节点#xff0c;通过随机采样#xff0c;增加叶子节点的方式#xff0c;生成一个随机扩展树#xff0c;当随机树中的叶子节点…路径规划——RRT算法
算法原理
RRT算法的全称是快速扩展随机树算法(Rapidly Exploring Random Tree)它的思想是选取一个初始点作为根节点通过随机采样增加叶子节点的方式生成一个随机扩展树当随机树中的叶子节点包含了目标点或进入了目标区域边可以在随机树中通过回溯的方式找到这条从初始点到目标点的路径。
此算法的重点随机采样步⻓限制碰撞检测
算法流程 1.初始化以起点start为根节点创建一棵树通常用二叉树表示树的根节点表示起始位置。 2.随机采样在搜索空间中随机生成一个点x_rand。这个点可能在自由空间中也可能在障碍物中。 3.寻找最近的节点在当前的树中找到距离x_rand最近的节点x_near。 4.扩展树从x_near沿着指向x_rand的方向移动一小步生成一个新的节点x_new。如果x_new在自由空间中即不与障碍物碰撞则将x_new加入到树中并将x_near和n_new用一条边连接。 5.检查目标检查x_new是否在目标区域附近这里的“附近”可以设置一个搜索距离来量化。如果是则生成一条路径从起点到x_new并结束算法。 6.迭代重复步骤2~步骤5直到找到目标点goal或达到预设的迭代次数。
由于RRT采用随机采样的方法RRT生成的路径通常不一定是最优路径但可以通过多次运行RRT或结合其他优化算法来获得更优路径。 算法实现
import numpy as np
import random
import math
from itertools import combinations
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import matplotlib.patches as patchesclass RRT:def __init__(self,start,goal,obstacles,board_size,max_try,max_dist,goal_sample_rate,env) - None:self.start self.Node(start,None,0)self.goal self.Node(goal,None,0)self.obstacles obstaclesself.board_size board_sizeself.max_try max_try # Number of iterationsself.max_dist max_dist # Maximum sampling distanceself.goal_sample_rate goal_sample_rateself.env envself.inflation 1self.searched []class Node:def __init__(self,position,parent,cost):self.position positionself.parent parentself.cost costdef run(self):cost,path self.plan()self.visualize(cost,path)def plan(self):self.searched.append(self.start)closed_list {self.start.position: self.start}# plan max_try timesfor i in range(self.max_try):node_rand self.get_random_node()# visitedif node_rand.position in closed_list:continue# Get the nearest neighbor nodenode_near self.get_nearest_neighbor(list(closed_list.values()),node_rand)# Get the new nodenode_new self.get_new_node(node_rand,node_near)if node_new:closed_list[node_new.position] node_newself.searched.append(node_new)dist self.distance(node_new,self.goal)# Found goal successfullyif dist self.max_dist and not self.isCollision(node_new,self.goal):self.searched.append(self.goal)self.goal.parent node_newself.goal.cost node_new.cost self.distance(self.goal,node_new)closed_list[self.goal.position] self.goalcost, path self.extractPath(closed_list)print(Exploring {} nodes..format(i))return cost,pathreturn 0,Nonedef get_random_node(self) :Return a random node.if random.random()self.goal_sample_rate:node self.Node((random.uniform(0,self.env.height),random.uniform(0,self.env.width)),None,0)else:node self.Node(self.goal.position,None,0)return nodedef get_nearest_neighbor(self,node_list,node) - Node:Return node that is nearest to node in node_listdist [self.distance(node, n) for n in node_list]node_near node_list[int(np.argmin(dist))]return node_neardef get_new_node(self,node_rand,node_near):Return node found based on node_near and node_rand.dx node_rand.position[0] - node_near.position[0]dy node_rand.position[1] - node_near.position[1]dist math.hypot(dx,dy)theta math.atan2(dy, dx)d min(self.max_dist,dist)position ((node_near.position[0]d*math.cos(theta)),node_near.position[1]d*math.sin(theta))node_new self.Node(position,node_near,node_near.costd)if self.isCollision(node_new, node_near):return Nonereturn node_newdef isCollision(self,node1,node2):Judge collision from node1 to node2 if self.isInObstacles(node1) or self.isInObstacles(node2):return Truefor rect in self.env.obs_rectangle:if self.isInterRect(node1,node2,rect):return Truefor circle in self.env.obs_circle:if self.isInterCircle(node1,node2,circle):return Truereturn Falsedef distance(self,node1,node2):dx node2.position[0] - node1.position[0]dy node2.position[1] - node1.position[1]return math.hypot(dx,dy)def isInObstacles(self,node):Determine whether it is in obstacles or not.x,y node.position[0],node.position[1]for (ox,oy,w,h) in self.env.boundary:if ox-self.inflationxoxwself.inflation and oy-self.inflationyoyhself.inflation:return Truefor (ox,oy,w,h) in self.env.obs_rectangle:if ox-self.inflationxoxwself.inflation and oy-self.inflationyoyhself.inflation:return Truefor (ox,oy,r) in self.env.obs_circle:if math.hypot(x-ox,y-oy)rself.inflation:return Truereturn Falsedef isInterRect(self,node1,node2,rect):Judge whether it will cross the rectangle when moving from node1 to node2ox,oy,w,h rectvertex [[ox-self.inflation,oy-self.inflation],[oxwself.inflation,oy-self.inflation],[oxwself.inflation,oyhself.inflation],[ox-self.inflation,oyhself.inflation]]x1,y1 node1.positionx2,y2 node2.positiondef cross(p1,p2,p3):x1 p2[0]-p1[0]y1 p2[1]-p1[1]x2 p3[0]-p1[0]y2 p3[1]-p1[0]return x1*y2 - x2*y1for v1,v2 in combinations(vertex,2):if max(x1,x2) min(v1[0],v2[0]) and min(x1,x2)max(v1[0],v2[0]) and \max(y1,y2) min(v1[1],v2[1]) and min(y1,y2) max(v1[1],v2[1]):if cross(v1,v2,node1.position) * cross(v1,v2,node2.position)0 and \cross(node1.position,node2.position,v1) * cross(node1.position,node2.position,v2):return Truereturn Falsedef isInterCircle(self,node1,node2,circle):Judge whether it will cross the circle when moving from node1 to node2ox,oy,r circledx node2.position[0] - node1.position[0]dy node2.position[1] - node1.position[1]# Projectiont ((ox - node1.position[0]) * dx (oy - node1.position[1]) * dy) / (dx * dx dy * dy)# The projection point is on line segment ABif 0 t 1:closest_x node1.position[0] t * dxclosest_y node1.position[1] t * dy# Distance from center of the circle to line segment ABdistance math.hypot(ox-closest_x,oy-closest_y)return distance rself.inflationreturn Falsedef extractPath(self,closed_list):Extract the path based on the closed list.node closed_list[self.goal.position]path [node.position]cost node.costwhile node ! self.start:parent node.parentnode_parent closed_list[parent.position]node node_parentpath.append(node.position)return cost,pathdef visualize(self, cost, path):Plot the map.....结果图
