逻辑图在线制作网站,网站基本维护,国际转运网站建设,wordpress图片墙插件目录 1 RRT算法原理
2 RRT算法代码解析
3 RRT完整代码 1 RRT算法原理 RRT算法的全称是快速扩展随机树算法(Rapidly Exploring Random Tree)#xff0c;它的想法就是从根结点长出一棵树当树枝长到终点的时候这样就能找到从终点到根节点的唯一路径。 算法流程#xff1a; 首先…目录 1 RRT算法原理
2 RRT算法代码解析
3 RRT完整代码 1 RRT算法原理 RRT算法的全称是快速扩展随机树算法(Rapidly Exploring Random Tree)它的想法就是从根结点长出一棵树当树枝长到终点的时候这样就能找到从终点到根节点的唯一路径。 算法流程 首先进行初始化设置起点和终点的设置进入循环进行随机采样在空间中随机选择一个点Xrand寻找距离Xrand最近的节点如果是第一次那就是初始节点随后我们进行树的生长将Xnear--Xrand连接起来作为树生长的方向设置步长作为树枝的长度就会产生Xnew节点如下图 将树枝节点和根结点加在一起作为新节点。 我们继续来采样这次采样也是Xrand 它最近的节点是Xnear节点生长的时候会发现它会穿越障碍物。抛弃这次的生长。 在不停的生长过后Xnew越来越接近终点。我们每次产生Xnew都会与终点进行连线看他们的距离是否比步长小并且没有障碍物如果true我们连接Xnew和终点就找到了起点到终点的路径。 我们执行代码 红色的就是我们找到的路径。好像挺糟糕的..... 2 RRT算法代码解析 我们先看算法的节点类 class Node:def __init__(self, x, y):self.x xself.y yself.cost 0.0self.parent None 和Dijkstra和A*算法是一样的不过这里的x、y是真实坐标系的坐标而不是珊格地图的坐标节点代价为距离如果连接了下一个节点这个代价也是进行相加的parent存储了父亲节点方便于找到终点后计算路径。 我们再看准备工作 obstacleList [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),(9, 5, 2), (8, 10, 1)]# Set params# 采样范围 设置的障碍物 最大迭代次数rrt RRT(randArea[-2, 18], obstacleListobstacleList, maxIter200)# 传入的是起点和终点path rrt.rrt_planning(start[0, 0], goal[15, 12], animationshow_animation) 我们设计了一系列的圆柱障碍物并向RRT类中传参圆柱障碍物是中心坐标半径的形式如下图 由上面的算法介绍可知我们传入RRT算法的参数为障碍物、随机距离-2-18、最大迭代次数如果超过200次我就不找路径了我们看RRT类首先看构造函数 def __init__(self, obstacleList, randArea,expandDis2.0, goalSampleRate10, maxIter200):self.start Noneself.goal Noneself.min_rand randArea[0]self.max_rand randArea[1]self.expand_dis expandDisself.goal_sample_rate goalSampleRateself.max_iter maxIterself.obstacle_list obstacleList# 存储RRT树self.node_list None 将起始点、结束点置为null最小随机点和最大随机取样点设置为-2与18单次前进距离X_near -- X_rand为2直接取终点为最终点的采样概率为10%最大迭代次数为200障碍物列表也传进来了。RRT树为none。 我们看导航参数 path rrt.rrt_planning(start[0, 0], goal[15, 12], animationshow_animation) 就是输入了起点和终点的坐标。 我们进入函数 def rrt_planning(self, start, goal, animationTrue):start_time time.time()self.start Node(start[0], start[1])self.goal Node(goal[0], goal[1])# 将起点加入node_list作为树的根结点self.node_list [self.start]path None 记录了起始时间计算时间我们将起始点和终止点封装成node因为我们没有指定代价cost和parent因此代价由类默认设置为0父亲节点设置为-1。将起点加入到node_list中为根结点。初始化路径。 我们进行采样 for i in range(self.max_iter):# 进行采样rnd self.sample()# 取的距离采样点最近的节点下标n_ind self.get_nearest_list_index(self.node_list, rnd)# 得到最近节点nearestNode self.node_list[n_ind]# 将Xrandom和Xnear连线方向作为生长方向# math.atan2() 函数接受两个参数分别是 y 坐标差值和 x 坐标差值。它返回的值是以弧度表示的角度范围在 -π 到 π 之间。这个角度表示了从 nearestNode 指向 rnd 的方向。theta math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)# 生长 输入参数为角度、下标、nodelist中最近的节点 得到生长过后的节点newNode self.get_new_node(theta, n_ind, nearestNode)# 检查是否有障碍物 传入参数为新生城路径的两个节点noCollision self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y) 先看sample函数 def sample(self):# 取得1-100的随机数,如果比10大的话以10%的概率取到终点if random.randint(0, 100) self.goal_sample_rate:# 在空间里随机采样一个点rnd [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]else: # goal point sampling# 终点作为采样点rnd [self.goal.x, self.goal.y]return rnd 先取得了一个1-100的随机数如果大于self.goal_sample_rate传进来的参数10我们就启动if语句 它使用了 Python 中的 random.uniform() 函数来生成两个在指定范围内的随机数并将它们放入列表 rnd 中。-2 -- 18 random.uniform(a, b) 函数会返回一个在 a 和 b 之间的随机浮点数。在这里self.min_rand 和 self.max_rand 可能是两个指定的最小值和最大值。 所以rnd 是一个包含两个随机数的列表这两个随机数分别位于 self.min_rand 2和 self.max_rand 18之间。这个坐标作为我们随机采样的点。 如果是else语句的话我们直接将终点作为采样点。 因此rnd获得了如下图的红圈所示的Xrand坐标。 在来看get_nearest_list_index函数 def get_nearest_list_index(nodes, rnd):# 遍历所有节点 计算采样点和节点的距离dList [(node.x - rnd[0]) ** 2 (node.y - rnd[1]) ** 2 for node in nodes]# 获得最近的距离所对应的索引minIndex dList.index(min(dList))return minIndex 第一行代码创建了一个列表 dList其中包含了所有节点与指定点 rnd 之间的欧几里得距离的平方。 具体来说它使用了列表推导式list comprehension的语法对于 nodes 中的每一个节点 node计算了以下值 (node.x - rnd[0]) ** 2 (node.y - rnd[1]) ** 2 最终dList 中包含了所有节点与 rnd 之间距离的平方值。 minIndex dList.index(min(dList)) 获得了最近的距离所对应的索引。 因此nearestNode self.node_list[n_ind]这个代码就得到了距离采样点Xrand最近的节点Xnear如下图所示 theta math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) 这行代码求了一个角度采样点Xrand与Xnear的角度。 我们来看get_new_node函数 def get_new_node(self, theta, n_ind, nearestNode):newNode copy.deepcopy(nearestNode)# 坐标newNode.x self.expand_dis * math.cos(theta)newNode.y self.expand_dis * math.sin(theta)# 代价newNode.cost self.expand_dis# 父亲节点newNode.parent n_indreturn newNode 我们先把随机采样节点Xrand的最近节点Xnear做了深拷贝利用三角函数计算出新的节点的坐标1.我们传进来的参数expand_dis意为每一次的导航步长 2.expand_dis * costheta就是x的增量y同理因此我们实例化了一个新节点它的代价就是它邻近节点的代价 expand_dis2它的父亲节点为这个邻近节点Xnear保证了递归的顺利进行。 因此 newNode self.get_new_node(theta, n_ind, nearestNode) 这行代码我们利用expand_dis和邻近节点Xnear新生成了一个节点。 在来看check_segment_collision函数 def check_segment_collision(self, x1, y1, x2, y2):# 遍历所有的障碍物for (ox, oy, size) in self.obstacle_list:dd self.distance_squared_point_to_segment(np.array([x1, y1]),np.array([x2, y2]),np.array([ox, oy]))if dd size ** 2:return False # collisionreturn True 这个就是具体问题具体分析了判断两条线间是否有障碍物即Xnear和Xnew间 随后就是最后一部分代码啦~加加油 if noCollision:# 没有碰撞把新节点加入到树里面self.node_list.append(newNode)if animation:self.draw_graph(newNode, path)# 是否到终点附近if self.is_near_goal(newNode):# 是否这条路径与障碍物发生碰撞if self.check_segment_collision(newNode.x, newNode.y,self.goal.x, self.goal.y):lastIndex len(self.node_list) - 1# 找路径path self.get_final_course(lastIndex)pathLen self.get_path_len(path)print(current path length: {}, It costs {} s.format(pathLen, time.time()-start_time))if animation:self.draw_graph(newNode, path)return path 如果没有碰撞的话我们认为这个Xnew是靠谱的将它加入到节点队列中。再判断它是否在终点附近我们来看is_near_goal函数 def is_near_goal(self, node):# 计算距离d self.line_cost(node, self.goal)if d self.expand_dis:return Truereturn False 这里就是计算我们新加的节点到终点的距离是否小于一次的步长2如果小于的话就return true。 若在终点附近我们将这个节点和终点相连看是否中间有障碍物存在 self.check_segment_collision(newNode.x, newNode.y,self.goal.x, self.goal.y) 如果没有障碍物的话我们导航成功开始输出路径 def get_final_course(self, lastIndex):path [[self.goal.x, self.goal.y]]while self.node_list[lastIndex].parent is not None:node self.node_list[lastIndex]path.append([node.x, node.y])lastIndex node.parentpath.append([self.start.x, self.start.y])return path 我们一个一个找终止节点的父节点就可以了。 通关。 3 RRT完整代码 import copy
import math
import random
import timeimport matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as Rot
import numpy as npshow_animation Trueclass RRT:# randArea采样范围[-2--18] obstacleList设置的障碍物 maxIter最大迭代次数 expandDis采样步长为2.0 goalSampleRate 以10%的概率将终点作为采样点def __init__(self, obstacleList, randArea,expandDis2.0, goalSampleRate10, maxIter200):self.start Noneself.goal Noneself.min_rand randArea[0]self.max_rand randArea[1]self.expand_dis expandDisself.goal_sample_rate goalSampleRateself.max_iter maxIterself.obstacle_list obstacleList# 存储RRT树self.node_list None# start、goal 起点终点坐标def rrt_planning(self, start, goal, animationTrue):start_time time.time()self.start Node(start[0], start[1])self.goal Node(goal[0], goal[1])# 将起点加入node_list作为树的根结点self.node_list [self.start]path Nonefor i in range(self.max_iter):# 进行采样rnd self.sample()# 取的距离采样点最近的节点下标n_ind self.get_nearest_list_index(self.node_list, rnd)# 得到最近节点nearestNode self.node_list[n_ind]# 将Xrandom和Xnear连线方向作为生长方向# math.atan2() 函数接受两个参数分别是 y 坐标差值和 x 坐标差值。它返回的值是以弧度表示的角度范围在 -π 到 π 之间。这个角度表示了从 nearestNode 指向 rnd 的方向。theta math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)# 生长 输入参数为角度、下标、nodelist中最近的节点 得到生长过后的节点newNode self.get_new_node(theta, n_ind, nearestNode)# 检查是否有障碍物 传入参数为新生城路径的两个节点noCollision self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)if noCollision:# 没有碰撞把新节点加入到树里面self.node_list.append(newNode)if animation:self.draw_graph(newNode, path)# 是否到终点附近if self.is_near_goal(newNode):# 是否这条路径与障碍物发生碰撞if self.check_segment_collision(newNode.x, newNode.y,self.goal.x, self.goal.y):lastIndex len(self.node_list) - 1# 找路径path self.get_final_course(lastIndex)pathLen self.get_path_len(path)print(current path length: {}, It costs {} s.format(pathLen, time.time()-start_time))if animation:self.draw_graph(newNode, path)return pathdef rrt_star_planning(self, start, goal, animationTrue):start_time time.time()self.start Node(start[0], start[1])self.goal Node(goal[0], goal[1])self.node_list [self.start]path NonelastPathLength float(inf)for i in range(self.max_iter):rnd self.sample()n_ind self.get_nearest_list_index(self.node_list, rnd)nearestNode self.node_list[n_ind]# steertheta math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)newNode self.get_new_node(theta, n_ind, nearestNode)noCollision self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)if noCollision:nearInds self.find_near_nodes(newNode)newNode self.choose_parent(newNode, nearInds)self.node_list.append(newNode)self.rewire(newNode, nearInds)if animation:self.draw_graph(newNode, path)if self.is_near_goal(newNode):if self.check_segment_collision(newNode.x, newNode.y,self.goal.x, self.goal.y):lastIndex len(self.node_list) - 1tempPath self.get_final_course(lastIndex)tempPathLen self.get_path_len(tempPath)if lastPathLength tempPathLen:path tempPathlastPathLength tempPathLenprint(current path length: {}, It costs {} s.format(tempPathLen, time.time()-start_time))return pathdef informed_rrt_star_planning(self, start, goal, animationTrue):start_time time.time()self.start Node(start[0], start[1])self.goal Node(goal[0], goal[1])self.node_list [self.start]# max length we expect to find in our informed sample space,# starts as infinitecBest float(inf)path None# Computing the sampling spacecMin math.sqrt(pow(self.start.x - self.goal.x, 2) pow(self.start.y - self.goal.y, 2))xCenter np.array([[(self.start.x self.goal.x) / 2.0],[(self.start.y self.goal.y) / 2.0], [0]])a1 np.array([[(self.goal.x - self.start.x) / cMin],[(self.goal.y - self.start.y) / cMin], [0]])e_theta math.atan2(a1[1], a1[0])# 论文方法求旋转矩阵2选1# first column of identity matrix transposed# id1_t np.array([1.0, 0.0, 0.0]).reshape(1, 3)# M a1 id1_t# U, S, Vh np.linalg.svd(M, True, True)# C np.dot(np.dot(U, np.diag(# [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),# Vh)# 直接用二维平面上的公式2选1C np.array([[math.cos(e_theta), -math.sin(e_theta), 0],[math.sin(e_theta), math.cos(e_theta), 0],[0, 0, 1]])for i in range(self.max_iter):# Sample space is defined by cBest# cMin is the minimum distance between the start point and the goal# xCenter is the midpoint between the start and the goal# cBest changes when a new path is foundrnd self.informed_sample(cBest, cMin, xCenter, C)n_ind self.get_nearest_list_index(self.node_list, rnd)nearestNode self.node_list[n_ind]# steertheta math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)newNode self.get_new_node(theta, n_ind, nearestNode)noCollision self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)if noCollision:nearInds self.find_near_nodes(newNode)newNode self.choose_parent(newNode, nearInds)self.node_list.append(newNode)self.rewire(newNode, nearInds)if self.is_near_goal(newNode):if self.check_segment_collision(newNode.x, newNode.y,self.goal.x, self.goal.y):lastIndex len(self.node_list) - 1tempPath self.get_final_course(lastIndex)tempPathLen self.get_path_len(tempPath)if tempPathLen cBest:path tempPathcBest tempPathLenprint(current path length: {}, It costs {} s.format(tempPathLen, time.time()-start_time))if animation:self.draw_graph_informed_RRTStar(xCenterxCenter,cBestcBest, cMincMin,e_thetae_theta, rndrnd, pathpath)return pathdef sample(self):# 取得1-100的随机数,如果比10大的话以10%的概率取到终点if random.randint(0, 100) self.goal_sample_rate:# 在空间里随机采样一个点rnd [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]else: # goal point sampling# 终点作为采样点rnd [self.goal.x, self.goal.y]return rnddef choose_parent(self, newNode, nearInds):if len(nearInds) 0:return newNodedList []for i in nearInds:dx newNode.x - self.node_list[i].xdy newNode.y - self.node_list[i].yd math.hypot(dx, dy)theta math.atan2(dy, dx)if self.check_collision(self.node_list[i], theta, d):dList.append(self.node_list[i].cost d)else:dList.append(float(inf))minCost min(dList)minInd nearInds[dList.index(minCost)]if minCost float(inf):print(min cost is inf)return newNodenewNode.cost minCostnewNode.parent minIndreturn newNodedef find_near_nodes(self, newNode):n_node len(self.node_list)r 50.0 * math.sqrt((math.log(n_node) / n_node))d_list [(node.x - newNode.x) ** 2 (node.y - newNode.y) ** 2for node in self.node_list]near_inds [d_list.index(i) for i in d_list if i r ** 2]return near_indsdef informed_sample(self, cMax, cMin, xCenter, C):if cMax float(inf):r [cMax / 2.0,math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]L np.diag(r)xBall self.sample_unit_ball()rnd np.dot(np.dot(C, L), xBall) xCenterrnd [rnd[(0, 0)], rnd[(1, 0)]]else:rnd self.sample()return rndstaticmethoddef sample_unit_ball():a random.random()b random.random()if b a:a, b b, asample (b * math.cos(2 * math.pi * a / b),b * math.sin(2 * math.pi * a / b))return np.array([[sample[0]], [sample[1]], [0]])staticmethoddef get_path_len(path):pathLen 0for i in range(1, len(path)):node1_x path[i][0]node1_y path[i][1]node2_x path[i - 1][0]node2_y path[i - 1][1]pathLen math.sqrt((node1_x - node2_x)** 2 (node1_y - node2_y) ** 2)return pathLenstaticmethoddef line_cost(node1, node2):return math.sqrt((node1.x - node2.x) ** 2 (node1.y - node2.y) ** 2)staticmethoddef get_nearest_list_index(nodes, rnd):# 遍历所有节点 计算采样点和节点的距离dList [(node.x - rnd[0]) ** 2 (node.y - rnd[1]) ** 2 for node in nodes]# 获得最近的距离所对应的索引minIndex dList.index(min(dList))return minIndexdef get_new_node(self, theta, n_ind, nearestNode):newNode copy.deepcopy(nearestNode)# 坐标newNode.x self.expand_dis * math.cos(theta)newNode.y self.expand_dis * math.sin(theta)# 代价newNode.cost self.expand_dis# 父亲节点newNode.parent n_indreturn newNodedef is_near_goal(self, node):# 计算距离d self.line_cost(node, self.goal)if d self.expand_dis:return Truereturn Falsedef rewire(self, newNode, nearInds):n_node len(self.node_list)for i in nearInds:nearNode self.node_list[i]d math.sqrt((nearNode.x - newNode.x) ** 2 (nearNode.y - newNode.y) ** 2)s_cost newNode.cost dif nearNode.cost s_cost:theta math.atan2(newNode.y - nearNode.y,newNode.x - nearNode.x)if self.check_collision(nearNode, theta, d):nearNode.parent n_node - 1nearNode.cost s_coststaticmethoddef distance_squared_point_to_segment(v, w, p):# Return minimum distance between line segment vw and point pif np.array_equal(v, w):return (p - v).dot(p - v) # v w casel2 (w - v).dot(w - v) # i.e. |w-v|^2 - avoid a sqrt# Consider the line extending the segment,# parameterized as v t (w - v).# We find projection of point p onto the line.# It falls where t [(p-v) . (w-v)] / |w-v|^2# We clamp t from [0,1] to handle points outside the segment vw.t max(0, min(1, (p - v).dot(w - v) / l2))projection v t * (w - v) # Projection falls on the segmentreturn (p - projection).dot(p - projection)def check_segment_collision(self, x1, y1, x2, y2):# 遍历所有的障碍物for (ox, oy, size) in self.obstacle_list:dd self.distance_squared_point_to_segment(np.array([x1, y1]),np.array([x2, y2]),np.array([ox, oy]))if dd size ** 2:return False # collisionreturn Truedef check_collision(self, nearNode, theta, d):tmpNode copy.deepcopy(nearNode)end_x tmpNode.x math.cos(theta) * dend_y tmpNode.y math.sin(theta) * dreturn self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y)def get_final_course(self, lastIndex):path [[self.goal.x, self.goal.y]]while self.node_list[lastIndex].parent is not None:node self.node_list[lastIndex]path.append([node.x, node.y])lastIndex node.parentpath.append([self.start.x, self.start.y])return pathdef draw_graph_informed_RRTStar(self, xCenterNone, cBestNone, cMinNone, e_thetaNone, rndNone, pathNone):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[0], rnd[1], ^k)if cBest ! float(inf):self.plot_ellipse(xCenter, cBest, cMin, e_theta)for node in self.node_list:if node.parent is not None:if node.x or node.y is not None:plt.plot([node.x, self.node_list[node.parent].x], [node.y, self.node_list[node.parent].y], -g)for (ox, oy, size) in self.obstacle_list:plt.plot(ox, oy, ok, ms30 * size)if path is not None:plt.plot([x for (x, y) in path], [y for (x, y) in path], -r)plt.plot(self.start.x, self.start.y, xr)plt.plot(self.goal.x, self.goal.y, xr)plt.axis([-2, 18, -2, 15])plt.grid(True)plt.pause(0.01)staticmethoddef plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no covera math.sqrt(cBest ** 2 - cMin ** 2) / 2.0b cBest / 2.0angle math.pi / 2.0 - e_thetacx xCenter[0]cy xCenter[1]t np.arange(0, 2 * math.pi 0.1, 0.1)x [a * math.cos(it) for it in t]y [b * math.sin(it) for it in t]rot Rot.from_euler(z, -angle).as_matrix()[0:2, 0:2]fx rot np.array([x, y])px np.array(fx[0, :] cx).flatten()py np.array(fx[1, :] cy).flatten()plt.plot(cx, cy, xc)plt.plot(px, py, --c)def draw_graph(self, rndNone, pathNone):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)for node in self.node_list:if node.parent is not None:if node.x or node.y is not None:plt.plot([node.x, self.node_list[node.parent].x], [node.y, self.node_list[node.parent].y], -g)for (ox, oy, size) in self.obstacle_list:# self.plot_circle(ox, oy, size)plt.plot(ox, oy, ok, ms30 * size)plt.plot(self.start.x, self.start.y, xr)plt.plot(self.goal.x, self.goal.y, xr)if path is not None:plt.plot([x for (x, y) in path], [y for (x, y) in path], -r)plt.axis([-2, 18, -2, 15])plt.grid(True)plt.pause(0.01)class Node:def __init__(self, x, y):self.x xself.y yself.cost 0.0self.parent Nonedef main():print(Start rrt planning)# create obstacles# obstacleList [# (3, 3, 1.5),# (12, 2, 3),# (3, 9, 2),# (9, 11, 2),# ]# 设置障碍物 (圆点、半径)obstacleList [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),(9, 5, 2), (8, 10, 1)]# Set params# 采样范围 设置的障碍物 最大迭代次数rrt RRT(randArea[-2, 18], obstacleListobstacleList, maxIter200)# 传入的是起点和终点path rrt.rrt_planning(start[0, 0], goal[15, 12], animationshow_animation)# path rrt.rrt_star_planning(start[0, 0], goal[15, 12], animationshow_animation)# path rrt.informed_rrt_star_planning(start[0, 0], goal[15, 12], animationshow_animation)print(Done!!)if show_animation and path:plt.show()if __name__ __main__:main()
