RRT（Rapidly-Exploring Random Tree）是一种基于树形结构的路径规划算法。它通过随机采样加上局部搜索的方式，逐步建立起一棵探索树，最终找到一条从起点到终点的路径。 以下是 Python 实现 RRT 算法的步骤： 1. 定义节点数据结构 ```python class Node: def __init__(self, x, y): self.x = x self.y = y self.parent = None ``` 2. 定义 RRT 类和相关函数 ```python class RRT: def __init__(self, start, goal, obstacles, max_iter=5000, step_size=0.5, goal_sample_rate=0.05): self.start = Node(*start) self.goal = Node(*goal) self.obstacles = obstacles self.max_iter = max_iter self.step_size = step_size self.goal_sample_rate = goal_sample_rate self.nodes = [self.start] def generate_random_node(self): if random.random() < self.goal_sample_rate: return self.goal else: x = random.uniform(0, 10) y = random.uniform(0, 10) return Node(x, y) def nearest_node(self, node): min_dist = float('inf') nearest = None for n in self.nodes: dist = math.sqrt((node.x - n.x) ** 2 + (node.y - n.y) ** 2) if dist < min_dist: min_dist = dist nearest = n return nearest def collision_free(self, node1, node2): for obs in self.obstacles: if obs.check_collision(node1, node2): return False return True def step_from_to(self, from_node, to_node): dist = math.sqrt((from_node.x - to_node.x) ** 2 + (from_node.y - to_node.y) ** 2) if dist < self.step_size: return to_node else: theta = math.atan2(to_node.y - from_node.y, to_node.x - from_node.x) x = from_node.x + self.step_size * math.cos(theta) y = from_node.y + self.step_size * math.sin(theta) return Node(x, y) def search(self): for i in range(self.max_iter): rand_node = self.generate_random_node() nearest_node = self.nearest_node(rand_node) new_node = self.step_from_to(nearest_node, rand_node) if self.collision_free(nearest_node, new_node): new_node.parent = nearest_node self.nodes.append(new_node) if self.goal.parent: return self.goal return None def get_path(self): path = [] node = self.goal while node: path.append((node.x, node.y)) node = node.parent return path[::-1] ``` 3. 定义障碍物类和相关函数 ```python class Obstacle: def __init__(self, x, y, r): self.x = x self.y = y self.r = r def check_collision(self, node1, node2): x1, y1, x2, y2 = node1.x, node1.y, node2.x, node2.y dx, dy = x2 - x1, y2 - y1 a = dx ** 2 + dy ** 2 b = 2 * (dx * (x1 - self.x) + dy * (y1 - self.y)) c = self.x ** 2 + self.y ** 2 + x1 ** 2 + y1 ** 2 - 2 * (self.x * x1 + self.y * y1) - self.r ** 2 delta = b ** 2 - 4 * a * c if delta < 0: return False t1 = (-b + math.sqrt(delta)) / (2 * a) t2 = (-b - math.sqrt(delta)) / (2 * a) if (0 <= t1 <= 1) or (0 <= t2 <= 1): return True else: return False ``` 4. 测试 RRT 算法 ```python start = (0, 0) goal = (10, 10) obstacles = [Obstacle(5, 5, 1), Obstacle(7, 7, 0.5)] rrt = RRT(start, goal, obstacles) goal_node = rrt.search() if goal_node: path = rrt.get_path() print('Path found:', path) else: print('Path not found') ``` 参考资料： - Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. International Journal of Robotics Research, 30(7), 846-894.