机器人规划与控制研究所 ——机器人/自动驾驶规划与控制方向综合、全面、专业的平台。3万人订阅的微信大号。点击标题下蓝字“机器人规划与控制研究所”关注，我们将为您提供有价值、有深度的延伸阅读。
路径规划是机器人技术、自动驾驶汽车和人工智能领域的一项基本挑战。应对这一挑战的最流行算法之一是快速探索随机树 (RRT)。它可以有效地搜索非凸高维空间。然而，RRT 有时会生成最优路径。这时，RRT*应运而生——它是该算法的增强版本，可以找到路径并将其优化为最短路径。
在本文中，我们将深入研究RRT*算法，用 Python 实现它，并使用 可视化结果matplotlib。在本教程结束时，您将对 RRT* 有清晰的理解，并能够修改代码以探索自定义环境。
快速探索随机树星 ( RRT* ) 是一种增量式路径规划算法，它从起点构建树，并通过在定义的环境中随机选择新节点进行扩展。RRT* 相对于标准 RRT 的关键优势在于，它会重新连接树以优化路径。RRT* 并非简单地增长树，而是通过根据节点的接近度和成本调整树中的父子关系，确保到达目标的路径尽可能短。
该算法的工作原理如下：
1.树扩展：该算法从给定点开始，在环境中随机采样节点。它将每个新节点连接到树中最近的现有节点。2.路径优化
：添加新节点后，RRT* 会检查新节点是否为其邻近节点提供了更高效的路径。如果是，则会重新连接以反映更短的路径。3.避障：随着树的扩展，该算法确保没有节点放置在障碍物内，从而允许生成的路径绕过它们。4.路径优化：树持续生长，直到找到通往目标的路径。即使找到解决方案后，RRT* 仍会随着新节点的添加而持续优化路径。
为了演示 RRT* 的工作原理，我们将介绍一个 Python 实现。我们将生成随机圆形障碍物，并实时可视化树的扩展和路径规划过程。在本实现中，我们将使用它matplotlib进行可视化和numpy数值计算。
1.要求
为了继续学习，你需要安装 Python 和numpy。matplotlib它们通常内置在 Python 中，但如果没有安装，你可以运行：
pip install numpy matplotlib
2.RRT* 代码实现
下面的代码实现了 RRT* 算法，用于在二维空间中规划路径并避开障碍物。实现的关键组件如下：
（1）节点类：表示空间中点的类，包含其坐标、父节点、成本。- 初始化
：该类初始化起始节点和目标节点，生成随机障碍物，并设置步长和最大迭代次数等参数。
- 障碍物生成：在指定的地图尺寸内生成大小和位置各异的随机障碍物。
- 路径规划：主要
plan方法迭代采样随机节点，找到树中最近的节点，并尝试转向该随机节点。如果找到无碰撞路径，则将新节点添加到树中，并重新连接附近的节点以优化路径。 - 碰撞检查：该算法检查与障碍物的碰撞以确保路径仍然有效。
- 路径生成：到达目标区域后，算法通过从目标节点回溯到起始节点来生成最终路径。
3.可视化：
- 该实现利用 Matplotlib 进行可视化，绘制起始和目标位置、障碍物以及树的生长过程。
- 定义动画函数（
animate）来实时可视化 RRT* 迭代。
4.动画保存：
可视化完成后，动画保存为GIF文件，并打印消息确认动画保存成功。
5.执行：
主块使用指定的起始和目标坐标、障碍物数量和地图大小初始化 RRT* 实例，然后运行动画并保存。
import numpy as npimport matplotlib.pyplot as pltimport matplotlib.animation as animationimport mathimport randomclass Node: def __init__(self, x, y): self.x = x self.y = y self.parent = None self.cost = 0class RRTStar: def __init__(self, start, goal, num_obstacles, map_size, step_size=1.0, max_iter=500): self.start = Node(start[0], start[1]) self.goal = Node(goal[0], goal[1]) self.map_size = map_size self.obstacles = self.generate_random_obstacles(num_obstacles) self.step_size = step_size self.max_iter = max_iter self.node_list = [self.start] self.goal_region_radius = 1.5 self.search_radius = 2.0 self.path = None self.goal_reached = False self.fig, self.ax = plt.subplots() self.setup_visualization() def generate_random_obstacles(self, num_obstacles): """Randomly generate obstacles with random positions and sizes.""" obstacles = [] for _ in range(num_obstacles): ox = random.uniform(2, self.map_size[0] - 2) oy = random.uniform(2, self.map_size[1] - 2) size = random.uniform(1, 3) obstacles.append((ox, oy, size)) return obstacles def setup_visualization(self): """Set up the visualization environment (grid, start, goal, obstacles).""" self.ax.plot(self.start.x, self.start.y, 'bo', label='Start') self.ax.plot(self.goal.x, self.goal.y, 'ro', label='Goal') self.ax.set_xlim(0, self.map_size[0]) self.ax.set_ylim(0, self.map_size[1]) self.ax.grid(True) self.draw_obstacles() def draw_obstacles(self): """Draw the static obstacles on the map.""" for (ox, oy, size) in self.obstacles: circle = plt.Circle((ox, oy), size, color='r') self.ax.add_artist(circle) def plan(self): """Main RRT* planning loop.""" for i in range(self.max_iter): rand_node = self.get_random_node() nearest_node = self.get_nearest_node(self.node_list, rand_node) new_node = self.steer(nearest_node, rand_node) if self.is_collision_free(new_node): neighbors = self.find_neighbors(new_node) new_node = self.choose_parent(neighbors, nearest_node, new_node) self.node_list.append(new_node) self.rewire(new_node, neighbors) if self.reached_goal(new_node): self.path = self.generate_final_path(new_node) self.goal_reached = True return def get_random_node(self): """Generate a random node in the map.""" if random.random() > 0.2: rand_node = Node(random.uniform(0, self.map_size[0]), random.uniform(0, self.map_size[1])) else:
rand_node = Node(self.goal.x, self.goal.y) return rand_node def steer(self, from_node, to_node): """Steer from one node to another, step-by-step.""" theta = math.atan2(to_node.y - from_node.y, to_node.x - from_node.x) new_node = Node(from_node.x + self.step_size * math.cos(theta), from_node.y + self.step_size * math.sin(theta)) new_node.cost = from_node.cost + self.step_size new_node.parent = from_node return new_node def is_collision_free(self, node): """Check if the node is collision-free with respect to obstacles.""" for (ox, oy, size) in self.obstacles: if (node.x - ox) ** 2 + (node.y - oy) ** 2 <= size ** 2: return False return True def find_neighbors(self, new_node): """Find nearby nodes within the search radius.""" return [node for node in self.node_list if np.linalg.norm([node.x - new_node.x, node.y - new_node.y]) < self.search_radius] def choose_parent(self, neighbors, nearest_node, new_node): """Choose the best parent for the new node based on cost.""" min_cost = nearest_node.cost + np.linalg.norm([new_node.x - nearest_node.x, new_node.y - nearest_node.y]) best_node = nearest_node for neighbor in neighbors: cost = neighbor.cost + np.linalg.norm([new_node.x - neighbor.x, new_node.y - neighbor.y]) if cost < min_cost and self.is_collision_free(neighbor): best_node = neighbor min_cost = cost new_node.cost = min_cost new_node.parent = best_node return new_node def rewire(self, new_node, neighbors): """Rewire the tree by checking if any neighbor should adopt the new node as a parent.""" for neighbor in neighbors: cost = new_node.cost + np.linalg.norm([neighbor.x - new_node.x, neighbor.y - new_node.y]) if cost < neighbor.cost and self.is_collision_free(neighbor): neighbor.parent = new_node neighbor.cost = cost def reached_goal(self, node): """Check if the goal has been reached.""" return np.linalg.norm([node.x - self.goal.x, node.y - self.goal.y]) < self.goal_region_radius def generate_final_path(self, goal_node): """Generate the final path from the start to the goal.""" path = [] node = goal_node while node is not None: path.append([node.x, node.y]) node = node.parent return path[::-1] def get_nearest_node(self, node_list, rand_node): """Find the nearest node in the tree to the random node.""" distances = [np.linalg.norm([node.x - rand_node.x, node.y - rand_node.y]) for node in node_list] nearest_node = node_list[np.argmin(distances)] return nearest_node def draw_tree(self, node): """Draw a tree edge from the current node to its parent.""" if node.parent: self.ax.plot([node.x, node.parent.x], [node.y, node.parent.y], "-b") def draw_path(self): """Draw the final path from start to goal.""" if self.path: self.ax.plot([x[0] for x in self.path], [x[1] for x in self.path], '-g', label='Path')def animate(i): if i < rrt_star.max_iter and not rrt_star.goal_reached: rand_node = rrt_star.get_random_node() nearest_node = rrt_star.get_nearest_node(rrt_star.node_list, rand_node) new_node = rrt_star.steer(nearest_node, rand_node) if rrt_star.is_collision_free(new_node):
neighbors = rrt_star.find_neighbors(new_node) new_node = rrt_star.choose_parent(neighbors, nearest_node, new_node) rrt_star.node_list.append(new_node) rrt_star.rewire(new_node, neighbors) rrt_star.draw_tree(new_node) if rrt_star.reached_goal(new_node): rrt_star.path = rrt_star.generate_final_path(new_node) rrt_star.draw_path() rrt_star.goal_reached = True if __name__ == "__main__": start = [1, 1] goal = [18, 18] num_obstacles = 6 map_size = [20, 20] rrt_star = RRTStar(start, goal, num_obstacles, map_size) ani = animation.FuncAnimation(rrt_star.fig, animate, frames=rrt_star.max_iter, interval=10, repeat=False) filename='rrt_star_animation.gif' ani.save(filename=filename, writer='pillow', fps=30) print(f"Animation completed and saved as {filename} .")
要运行代码，请打开终端并导航到包含 RRT* 代码的文件的目标位置。
运行：
该脚本将生成一个可视化 RRT* 路径规划过程的图表，并将动画保存为rrt_star_animation.gif同一目录中的 GIF 文件。动画保存后，将显示一条确认保存成功的消息。
RRT* 算法是解决障碍环境中复杂路径规划问题的强大工具。其迭代优化路径的能力使其有别于 RRT 等简单算法，非常适合对效率和最优性要求高的应用，例如自动驾驶汽车、无人机和机器人导航。
通过学习本教程，现在已经拥有一个具有实时可视化功能的 RRT* 实现。您可以进一步扩展此代码，添加对不同类型障碍物的支持，或将其扩展到 3D 环境。
源代码：https://github.com/Jefferson-Aggor/rrt-star
