### RRT算法结合matplotlib实现避障功能的示例 RRT（Rapidly-exploring Random Tree）是一种用于路径规划的随机化算法，特别适合解决高维空间和复杂环境中的路径问题。通过使用Python的`matplotlib`库，可以直观地展示RRT算法在二维空间中的路径规划过程，并结合障碍物进行避障。 以下是一个完整的示例代码，展示如何使用`matplotlib`结合RRT算法实现避障功能： ```python import numpy as np import matplotlib.pyplot as plt import random import math class RRT: def __init__(self, start, goal, obstacle_list, width, height, expand_dist=1.0, goal_sample_rate=10, max_iter=500): """ 初始化RRT参数。 """ self.start = Node(start[0], start[1]) self.goal = Node(goal[0], goal[1]) self.obstacle_list = obstacle_list self.width = width self.height = height self.expand_dist = expand_dist self.goal_sample_rate = goal_sample_rate self.max_iter = max_iter self.node_list = [self.start] def planning(self): """ 执行RRT路径规划。 """ for i in range(self.max_iter): rand_node = self.get_random_node() nearest_ind = self.get_nearest_node_index(rand_node) nearest_node = self.node_list[nearest_ind] new_node = self.steer(nearest_node, rand_node, self.expand_dist) if self.check_collision(new_node, self.obstacle_list): self.node_list.append(new_node) if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dist: final_node = self.steer(self.node_list[-1], self.goal, self.expand_dist) if self.check_collision(final_node, self.obstacle_list): return self.generate_final_course(len(self.node_list) - 1) return None def steer(self, from_node, to_node, extend_length=float("inf")): """ 生成从from_node到to_node的新节点。 """ new_node = Node(from_node.x, from_node.y) dist, angle = self.calc_distance_and_angle(new_node, to_node) new_node.path_x = [new_node.x] new_node.path_y = [new_node.y] if extend_length > dist: extend_length = dist n_expand = math.floor(extend_length / 0.1) for _ in range(n_expand): new_node.x += 0.1 * math.cos(angle) new_node.y += 0.1 * math.sin(angle) new_node.path_x.append(new_node.x) new_node.path_y.append(new_node.y) dist, _ = self.calc_distance_and_angle(new_node, to_node) if dist <= 0.1: new_node.x = to_node.x new_node.y = to_node.y new_node.path_x.append(to_node.x) new_node.path_y.append(to_node.y) new_node.parent = from_node return new_node def generate_final_course(self, goal_ind): """ 生成最终路径。 """ path = [[self.goal.x, self.goal.y]] node = self.node_list[goal_ind] while node.parent is not None: path.append([node.x, node.y]) node = node.parent path.append([node.x, node.y]) return path def get_random_node(self): """ 获取随机节点。 """ if random.randint(0, 100) > self.goal_sample_rate: rand_x = random.uniform(0, self.width) rand_y = random.uniform(0, self.height) else: # 目标点采样 rand_x = self.goal.x rand_y = self.goal.y return Node(rand_x, rand_y) def check_collision(self, node, obstacle_list): """ 检查是否与障碍物发生碰撞。 """ if node is None: return False for (ox, oy, size_x, size_y) in obstacle_list: 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_x/2)**2 + (size_y/2)**2: return False # 碰撞 return True # 安全 def calc_dist_to_goal(self, x, y): """ 计算到目标点的距离。 """ dx = x - self.goal.x dy = y - self.goal.y return math.sqrt(dx ** 2 + dy ** 2) @staticmethod def get_nearest_node_index(node_list, rand_node): """ 获取最近节点的索引。 """ dlist = [(node.x - rand_node.x) ** 2 + (node.y - rand_node.y) ** 2 for node in node_list] return dlist.index(min(dlist)) @staticmethod def calc_distance_and_angle(from_node, to_node): """ 计算距离和角度。 """ dx = to_node.x - from_node.x dy = to_node.y - from_node.y return math.hypot(dx, dy), math.atan2(dy, dx) class Node: def __init__(self, x, y): self.x = x self.y = y self.path_x = [] self.path_y = [] self.parent = None def plot_path(path, obstacle_list, start, goal): """ 绘制路径。 """ plt.figure(figsize=(8, 8)) plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r', linewidth=2) for (ox, oy, size_x, size_y) in obstacle_list: rect = plt.Rectangle((ox, oy), size_x, size_y, color="gray") plt.gca().add_patch(rect) plt.plot(start[0], start[1], 'bo', markersize=10, label='Start') plt.plot(goal[0], goal[1], 'ro', markersize=10, label='Goal') plt.legend() plt.axis("equal") plt.show() # 参数设置 start = (10, 10) goal = (90, 90) obstacle_list = [ (20, 20, 40, 40), # (x, y, width, height) (60, 60, 20, 20) ] rrt = RRT(start, goal, obstacle_list, width=100, height=100) path = rrt.planning() if path is None: print("无法找到路径") else: plot_path(path, obstacle_list, start, goal) ``` ### 说明 - 上述代码实现了基于RRT算法的路径规划，并结合`matplotlib`绘制了路径和障碍物[^1]。 - `RRT`类封装了RRT算法的核心逻辑，包括随机节点生成、最近节点查找、新节点扩展以及碰撞检测等功能[^2]。 - `plot_path`函数用于绘制最终路径和障碍物，提供可视化效果[^3]。