以下是一个使用Python实现的简单RRT（快速探索随机树）算法的代码示例： ```python import numpy as np import matplotlib.pyplot as plt class RRT: def __init__(self, start, goal, obstacle_list, rand_area, expand_dis=3.0, goal_sample_rate=5, max_iter=500): self.start = Node(start[0], start[1]) self.goal = Node(goal[0], goal[1]) self.obstacle_list = obstacle_list self.min_rand = rand_area[0] self.max_rand = rand_area[1] self.expand_dis = expand_dis self.goal_sample_rate = goal_sample_rate self.max_iter = max_iter self.node_list = [] def planning(self, animation=True): self.node_list = [self.start] for i in range(self.max_iter): rnd_node = self.get_random_node() nearest_node = self.get_nearest_node(self.node_list, rnd_node) new_node = self.steer(nearest_node, rnd_node, self.expand_dis) if self.check_collision(new_node, self.obstacle_list): self.node_list.append(new_node) if animation and i % 5 == 0: self.draw_graph(rnd_node) last_index = self.get_best_last_index() if last_index is not None: path = self.generate_final_course(last_index) return path else: return None def get_random_node(self): if np.random.randint(0, 100) > self.goal_sample_rate: rnd = Node(np.random.uniform(self.min_rand, self.max_rand), np.random.uniform(self.min_rand, self.max_rand)) else: # goal point sampling rnd = Node(self.goal.x, self.goal.y) return rnd def get_nearest_node(self, node_list, rnd_node): dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y) ** 2 for node in node_list] minind = dlist.index(min(dlist)) return node_list[minind] def steer(self, from_node, to_node, extend_length=float("inf")): new_node = Node(from_node.x, from_node.y) d, theta = 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 > d: new_node.x = to_node.x new_node.y = to_node.y else: new_node.x = from_node.x + extend_length * np.cos(theta) new_node.y = from_node.y + extend_length * np.sin(theta) new_node.path_x.append(new_node.x) new_node.path_y.append(new_node.y) new_node.parent = from_node return new_node def check_collision(self, node, obstacle_list): for (ox, oy, size) 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 ** 2: return False # collision return True # safe def calc_distance_and_angle(self, from_node, to_node): dx = to_node.x - from_node.x dy = to_node.y - from_node.y d = np.sqrt(dx ** 2 + dy ** 2) theta = np.arctan2(dy, dx) return d, theta def get_best_last_index(self): disglist = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list] goal_inds = [disglist.index(i) for i in disglist if i <= self.expand_dis] if len(goal_inds) == 0: return None min_cost = min([self.node_list[i].cost for i in goal_inds]) for i in goal_inds: if self.node_list[i].cost == min_cost: return i return None def calc_dist_to_goal(self, x, y): dx = x - self.goal.x dy = y - self.goal.y return np.sqrt(dx ** 2 + dy ** 2) 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 draw_graph(self, rnd=None): 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: plt.plot(node.path_x, node.path_y, "-g") for (ox, oy, size) in self.obstacle_list: self.plot_circle(ox, oy, size) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.goal.x, self.goal.y, "xr") plt.axis("equal") plt.axis([-2, 15, -2, 15]) plt.grid(True) plt.pause(0.01) @staticmethod def plot_circle(x, y, size, color="-b"): # pragma: no cover deg = list(range(0, 360, 5)) deg.append(0) xl = [x + size * np.cos(np.deg2rad(d)) for d in deg] yl = [y + size * np.sin(np.deg2rad(d)) for d in deg] plt.plot(xl, yl, color) class Node: def __init__(self, x, y): self.x = x self.y = y self.path_x = [] self.path_y = [] self.parent = None def main(): # Start and goal positions start = [0, 0] goal = [10, 10] # Obstacle positions and sizes obstacle_list = [ (5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2), (9, 5, 2) ] # Random sampling area rand_area = [-2, 15] # Create RRT planner rrt = RRT(start, goal, obstacle_list, rand_area) path = rrt.planning(animation=True) if path is None: print("Cannot find path") else: print("Found path!!") # Draw final path rrt.draw_graph() plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.grid(True) plt.show() if __name__ == '__main__': main() ``` 这个代码实现了简单的RRT算法，用于在二维空间进行路径规划。代码中定义了`RRT`类和`Node`类，`RRT`类包含了RRT算法的核心逻辑，如随机采样、寻找最近节点、扩展树等操作，`Node`类用于表示树的节点。 ### 代码解释 1. **初始化**：在`__init__`方法中初始化起点、终点、障碍物列表、随机采样区域等参数。 2. **路径规划**：`planning`方法是主要的路径规划方法，通过多次迭代，不断生成随机节点，扩展树，直到找到到达目标的路径或达到最大迭代次数。 3. **随机采样**：`get_random_node`方法以一定的概率选择目标点作为随机节点，否则在随机区域内生成随机节点。 4. **寻找最近节点**：`get_nearest_node`方法通过计算距离找到离随机节点最近的树节点。 5. **扩展树**：`steer`方法从最近节点向随机节点扩展一定的距离，生成新的节点。 6. **碰撞检测**：`check_collision`方法检查新节点是否与障碍物发生碰撞。 7. **生成最终路径**：`generate_final_course`方法从目标节点回溯到起点，生成最终的路径。 ### 运行代码 将上述代码保存为一个Python文件（例如`rrt.py`），然后在终端中运行： ```bash python rrt.py ``` 运行代码后，会弹出一个窗口显示RRT算法的搜索过程，最终会显示找到的路径（如果找到的话）。