RRT* 算法原理以及在二维仿真环境中的实现 -- Python代码实现

RRT* 算法是在 RRT 的基础上做出了一些改进，主要改进的点有两点：

新结点生成后，优化其父结点。

在生成新结点 new_node 后，首先设置一个搜索区域的半径，搜索该区域中的树结点，并计算其中的每一个结点到新结点 new_node 的距离。

# 改进一：当生成新的结点时，判断在一定范围内的结点是否有比当前父结点路径更有的结点
near_inds = self.find_near_nodes(new_node)
new_node = self.choose_parent(new_node, near_inds)

其中第一步：找到该区域中的树结点。用函数 find_near_nodes() 函数实现，具体代码如下：

def find_near_nodes(self, new_node):""" 获取新结点一定范围内的树中的结点 """n_node = len(self.node_list)r = 50.0 * math.sqrt((math.log(n_node) / n_node)) # 一个渐变的距离d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2for node in self.node_list]near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]return near_inds

第二步：从该区域中选择 cost 最小的作为结点作为其新的父结点。用函数 choose_parent() 函数实现，具体代码如下：

def choose_parent(self, new_node, near_inds):if len(near_inds) == 0:return new_noded_list = []for i in near_inds:dx = new_node.x - self.node_list[i].xdy = new_node.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):d_list.append(self.node_list[i].cost + d)else:d_list.append(float('Inf'))min_cost = min(d_list)min_ind = near_inds[d_list.index(min_cost)]if min_cost == float('Inf'):print("min cost is inf")return new_nodenew_node.cost = min_costnew_node.parent = min_indreturn new_node

在判断新结点和树中结点能否相连时，需要再次检测该路径是否有碰撞，用函数 check_collision()函数来实现：

def check_collision(self, near_node, theta, d):tmp_node = copy.deepcopy(near_node)end_x = tmp_node.x + math.cos(theta) * dend_y = tmp_node.y + math.sin(theta) * dreturn self.check_segment_collision(tmp_node.x, tmp_node.y, end_x, end_y)

新结点生成后，优化周围点的连接关系。

优化代码如下：

def rewire(self, new_node, near_inds):n_node = len(self.node_list)for i in near_inds:near_node = self.node_list[i]d = math.sqrt((near_node.x - new_node.x) ** 2 + (near_node.y - new_node.y) ** 2)s_cost = new_node.cost + dif near_node.cost > s_cost:theta = math.atan2(new_node.y - near_node.y, new_node.x - near_node.x)if self.check_collision(near_node, theta, d):near_node.parent = n_node - 1near_node.cost = s_cost

RRT* 的二维环境仿真实验结果为：

完整代码如下：

import random
import math
import copy
import timeimport matplotlib.pyplot as plt
import numpy as npclass RRT:# 初始化def __init__(self,obstacle_list,         # 障碍物rand_area,             # 采样的区域expand_dis=2.0,        # 步长goal_sample_rate=10,   # 目标采样率max_iter=200):         # 最大迭代次数self.start = Noneself.goal = Noneself.min_rand = rand_area[0]self.max_rand = rand_area[1]self.expand_dis = expand_disself.goal_sample_rate = goal_sample_rateself.max_iter = max_iterself.obstacle_list = obstacle_listself.node_list = Nonedef rrt_planning(self, start, goal, animation=True):self.start = Node(start[0], start[1])self.goal = Node(goal[0], goal[1])self.node_list = [self.start]path = Nonelast_path_length = float('inf')for i in range(self.max_iter):# 1. 在环境中随机采样点rnd = self.sample()# 2. 找到结点树中距离采样点最近的结点n_ind = self.get_nearest_list_index(self.node_list, rnd)nearest_node = self.node_list[n_ind]# 3. 在采样点的方向生长一个步长，得到下一个树的结点。theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)new_node = self.get_new_node(theta, n_ind, nearest_node)# 4. 检测碰撞，检测到新生成的结点的路径是否会与障碍物碰撞no_collision = self.check_segment_collision(new_node.x, new_node.y, nearest_node.x, nearest_node.y)if no_collision:# 改进一：当生成新的结点时，判断在一定范围内的结点是否有比当前父结点路径更有的结点near_inds = self.find_near_nodes(new_node)new_node = self.choose_parent(new_node, near_inds)self.node_list.append(new_node)# 改进二：范围内的结点重新相连self.rewire(new_node, near_inds)# 一步一绘制if animation:time.sleep(1)self.draw_graph(new_node, path)# 判断新结点是否临近目标点if self.is_near_goal(new_node):if self.check_segment_collision(new_node.x, new_node.y,self.goal.x, self.goal.y):last_index = len(self.node_list) - 1temp_path = self.get_final_course(last_index)        # 回溯路径temp_path_length = self.get_path_len(temp_path)           # 计算路径的长度if last_path_length > temp_path_length:path = temp_pathlast_path_length = temp_path_lengthprint("当前的路径长度为：{}".format(last_path_length))if animation:self.draw_graph(new_node, path)return pathdef sample(self):""" 在环境中采样点的函数，以一定的概率采样目标点 """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:rnd = [self.goal.x, self.goal.y]return rnd@staticmethoddef get_nearest_list_index(nodes, rnd):""" 计算树中距离采样点距离最近的结点 """d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2for node in nodes]min_index = d_list.index(min(d_list))return min_indexdef get_new_node(self, theta, n_ind, nearest_node):""" 计算新结点 """new_node = copy.deepcopy(nearest_node)new_node.x += self.expand_dis * math.cos(theta)new_node.y += self.expand_dis * math.sin(theta)new_node.cost += self.expand_disnew_node.parent = n_indreturn new_nodedef check_segment_collision(self, x1, y1, x2, y2):""" 检测碰撞 """for (ox, oy, radius) 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 <= radius ** 2:return Falsereturn True@staticmethoddef distance_squared_point_to_segment(v, w, p):""" 计算线段 vw 和 点 p 之间的最短距离"""if np.array_equal(v, w):    # 点 v 和 点 w 重合的情况return (p - v).dot(p - v)l2 = (w - v).dot(w - v)     # 线段 vw 长度的平方t = max(0, min(1, (p - v).dot(w - v) / l2))projection = v + t * (w - v)return (p - projection).dot(p - projection)def find_near_nodes(self, new_node):""" 获取新结点一定范围内的树中的结点 """n_node = len(self.node_list)r = 50.0 * math.sqrt((math.log(n_node) / n_node))d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.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 choose_parent(self, new_node, near_inds):""" 选择处在区域中的当前树的结点中 cost 最小的结点作为其新的父结点"""if len(near_inds) == 0:return new_noded_list = []for i in near_inds:dx = new_node.x - self.node_list[i].xdy = new_node.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):d_list.append(self.node_list[i].cost + d)else:d_list.append(float('Inf'))min_cost = min(d_list)min_ind = near_inds[d_list.index(min_cost)]if min_cost == float('Inf'):print("min cost is inf")return new_nodenew_node.cost = min_costnew_node.parent = min_indreturn new_nodedef check_collision(self, near_node, theta, d):tmp_node = copy.deepcopy(near_node)end_x = tmp_node.x + math.cos(theta) * dend_y = tmp_node.y + math.sin(theta) * dreturn self.check_segment_collision(tmp_node.x, tmp_node.y, end_x, end_y)def rewire(self, new_node, near_inds):""" 范围内的结点重连"""n_node = len(self.node_list)for i in near_inds:near_node = self.node_list[i]d = math.sqrt((near_node.x - new_node.x) ** 2 + (near_node.y - new_node.y) ** 2)s_cost = new_node.cost + dif near_node.cost > s_cost:theta = math.atan2(new_node.y - near_node.y, new_node.x - near_node.x)if self.check_collision(near_node, theta, d):near_node.parent = n_node - 1near_node.cost = s_costdef draw_graph(self, rnd=None, path=None):plt.clf()# 绘制新的结点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')# 绘制起点、终点plt.plot(self.start.x, self.start.y, "og")plt.plot(self.goal.x, self.goal.y, "or")# 绘制障碍物for (ox, oy, size) in self.obstacle_list:plt.plot(ox, oy, "ok", ms=30 * size)# 绘制路径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)def is_near_goal(self, node):d = self.line_cost(node, self.goal)if d < self.expand_dis:return Truereturn False@staticmethoddef line_cost(node1, node2):return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)def get_final_course(self, last_index):""" 回溯路径 """path = [[self.goal.x, self.goal.y]]while self.node_list[last_index].parent is not None:node = self.node_list[last_index]path.append([node.x, node.y])last_index = node.parentpath.append([self.start.x, self.start.y])return path@staticmethoddef get_path_len(path):""" 计算路径的长度 """path_length = 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]path_length += math.sqrt((node1_x - node2_x) ** 2 + (node1_y - node2_y) ** 2)return path_lengthclass Node:def __init__(self, x, y):self.x = xself.y = yself.cost = 0.0self.parent = Nonedef main():print('Start RRT planning!')show_animation = Truestart = [0, 0]goal = [15, 12]# 障碍物 (x, y, radius)obstacle_list = [(3, 3, 1.5),(12, 2, 3),(3, 9, 2),(9, 11, 2)]rrt = RRT(rand_area=[-2, 18], obstacle_list=obstacle_list, max_iter=200)path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation)print('Done!')if show_animation and path:plt.show()if __name__ == '__main__':main()

如果对您有帮助，记得在下方点赞哟！也欢迎在下方评论区留言讨论，谢谢！

RRT* 算法原理以及在二维仿真环境中的实现 -- Python代码实现相关推荐

RRT路径规划算法在二维仿真环境中的应用 -- Python代码实现
在上一节中,介绍了 RRT 算法的原理,这一节将一步步实现 RRT 路径规划算法在二维环境中的路径规划,来进一步加深对 RRT 算法的理解. 二维环境的搭建我们将搭建下图所示的二维环境,绿色点为起点 ...
Dijkstra 路径规划算法在二维仿真环境中的应用 -- Python代码实现
在上一节中,介绍了 Dijkstra 算法的原理以及在图中的应用,这一节将一步步实现 Dijkstra 路径规划算法在二维环境中的路径规划,来进一步加深对 Dijkstra 算法的理解. 所需要用到的 ...
A* 算法原理以及在二维环境地图中的应用 -- Python 代码实现
上节学习了 Dijkstra 路径规划规划算法,虽然能够找到最短的路径,但是其遍历的搜索过程具有盲目性,因此效率比较低,计算量非常大.而实际中电子地图的结点数量是非常庞大的,Dijkstra 算法在有 ...
DPC密度峰值聚类算法原理详解二
DPC密度峰值聚类算法原理详解二 1.计算数据点两两之间的距离 1.使用 Numpy 模块查找两点之间的欧几里得距离: 2.使用 distance.euclidean() 函数查找两点之间的欧式距离: ...
算法笔记-螺旋输出二维数组
算法笔记-螺旋输出二维数组 1.思路:二维数组看做一个坐标,遍历者当成一个人,那么我们定义这个人的位置,以及当前面朝的方向,还有这个人转向次数.初始位置,人在(x,y)=(0,0)处,面向右方,右方的 ...
四维空间的二维线框投影可视化（附matlab代码）
四维空间的二维线框投影可视化(附matlab代码) 1 三维空间在2维屏幕上的投影 1.1平行投影 1.2透视投影 2 四维空间在2维屏幕上的投影 2.1 四维空间与三维空间的一些区别 2.2 四维空 ...
互联网 4 大发明之二维码，你如何使用 Python 生成二维码？
阅读文本大概需要 8 分钟. 新时代,人们有人信新的追求,自然而然会有新发明的诞生.去年,在"一带一路"国际合作高峰论坛举行期间, 20 国青年投票选出中国的"新四大发明 ...
二维码识别中面临的主要问题
问题描述下面有两个二维码的图片: 这两个二维码其实是一样的,也就是二维码码制在设计的时候,其实不需要那么大的黑点,只需要中心一小部分即可. 问题一:在优化二维码的过程中,能否考虑优化边缘部分,让边缘 ...
python二维元组_python中读入二维csv格式的表格方法详解(以元组/列表形式表示)
如何去读取一个没有表头的二维csv文件(如下图所示)? 并以元组的形式表现数据: ((1.0, 0.0, 3.0, 180.0), (2.0, 0.0, 2.0, 180.0), (3.0, 0.0, ...

RRT* 算法原理以及在二维仿真环境中的实现 -- Python代码实现

RRT* 算法原理以及在二维仿真环境中的实现 -- Python代码实现相关推荐

最新文章

热门文章