【启发式算法】RRT*算法详细介绍（Python）
RRT* 算法原理
RRT*（Rapidly-exploring Random Tree Star）是RRT算法的优化版本，通过渐进最优的方式改进路径质量。核心思想是在扩展树的过程中重新选择父节点和重布线，以降低路径成本。
- 采样：在配置空间中随机采样点。
- 最近邻搜索：找到树上距离采样点最近的节点。
- 扩展：从最近节点向采样点方向扩展新节点。
- 父节点优化：在新节点附近半径内寻找成本更低的父节点。
- 重布线：优化附近节点的父节点以降低整体路径成本。
Python 实现步骤
初始化环境
定义二维空间、起点、终点和障碍物：
import numpy as np import matplotlib.pyplot as plt class RRTStar: def __init__(self, start, goal, obstacles, bounds, max_iter=1000, step_size=0.5, radius=1.0): self.start = np.array(start) self.goal = np.array(goal) self.obstacles = obstacles # List of (center, radius) self.bounds = bounds # (x_min, x_max, y_min, y_max) self.max_iter = max_iter self.step_size = step_size self.radius = radius self.nodes = [{'point': start, 'parent': None, 'cost': 0}] 采样与扩展
随机采样并扩展树：
def sample(self): if np.random.rand() < 0.1: # 10%概率采样目标点 return self.goal return np.random.uniform(low=[self.bounds[0], self.bounds[2]], high=[self.bounds[1], self.bounds[3]]) def nearest(self, point): distances = [np.linalg.norm(point - node['point']) for node in self.nodes] return np.argmin(distances) def steer(self, from_point, to_point): direction = to_point - from_point distance = np.linalg.norm(direction) if distance <= self.step_size: return to_point return from_point + direction * (self.step_size / distance) 碰撞检测
检查路径是否与障碍物相交：
def is_collision_free(self, point1, point2): for (center, radius) in self.obstacles: if self.line_circle_intersection(point1, point2, center, radius): return False return True def line_circle_intersection(self, p1, p2, center, r): # 线段与圆的相交检测 d = p2 - p1 f = p1 - center a = np.dot(d, d) b = 2 * np.dot(f, d) c = np.dot(f, f) - r**2 discriminant = b**2 - 4 * a * c if discriminant < 0: return False t1 = (-b - np.sqrt(discriminant)) / (2 * a) t2 = (-b + np.sqrt(discriminant)) / (2 * a) return 0 <= t1 <= 1 or 0 <= t2 <= 1 父节点优化与重布线
def find_best_parent(self, new_point, near_indices): min_cost = float('inf') best_parent = None for idx in near_indices: node = self.nodes[idx] if self.is_collision_free(node['point'], new_point): cost = node['cost'] + np.linalg.norm(new_point - node['point']) if cost < min_cost: min_cost = cost best_parent = idx return best_parent, min_cost def rewire(self, new_node_idx, near_indices): new_node = self.nodes[new_node_idx] for idx in near_indices: node = self.nodes[idx] if self.is_collision_free(new_node['point'], node['point']): new_cost = new_node['cost'] + np.linalg.norm(node['point'] - new_node['point']) if new_cost < node['cost']: node['parent'] = new_node_idx node['cost'] = new_cost 主算法流程
def plan(self): for _ in range(self.max_iter): rand_point = self.sample() nearest_idx = self.nearest(rand_point) nearest_point = self.nodes[nearest_idx]['point'] new_point = self.steer(nearest_point, rand_point) if self.is_collision_free(nearest_point, new_point): near_indices = self.find_near_indices(new_point) best_parent, cost = self.find_best_parent(new_point, near_indices) if best_parent is not None: self.nodes.append({'point': new_point, 'parent': best_parent, 'cost': cost}) self.rewire(len(self.nodes)-1, near_indices) if np.linalg.norm(new_point - self.goal) <= self.step_size: if self.is_collision_free(new_point, self.goal): self.nodes.append({'point': self.goal, 'parent': len(self.nodes)-1, 'cost': self.nodes[-1]['cost'] + np.linalg.norm(self.goal - new_point)}) return self.extract_path() return None 可视化与路径提取
def extract_path(self): path = [] node = self.nodes[-1] while node['parent'] is not None: path.append(node['point']) node = self.nodes[node['parent']] path.append(self.start) return np.array(path[::-1]) def plot(self, path=None): plt.figure(figsize=(10, 8)) for (center, radius) in self.obstacles: circle = plt.Circle(center, radius, color='r', alpha=0.5) plt.gca().add_patch(circle) for node in self.nodes[1:]: parent = self.nodes[node['parent']] plt.plot([node['point'][0], parent['point'][0]], [node['point'][1], parent['point'][1]], 'b-', alpha=0.3) if path is not None: plt.plot(path[:,0], path[:,1], 'g-', linewidth=2) plt.scatter(*self.start, color='g', s=100, label='Start') plt.scatter(*self.goal, color='y', s=100, label='Goal') plt.xlim(self.bounds[0], self.bounds[1]) plt.ylim(self.bounds[2], self.bounds[3]) plt.legend() plt.show() 示例用法
# 定义障碍物和边界 obstacles = [((3, 3), 1.5), ((6, 7), 2), ((8, 2), 1)] bounds = (0, 10, 0, 10) # 运行RRT* rrt = RRTStar(start=(1, 1), goal=(9, 9), obstacles=obstacles, bounds=bounds) path = rrt.plan() rrt.plot(path) 关键参数说明
max_iter：最大迭代次数，影响路径优化程度。step_size：扩展步长，平衡探索速度与精度。radius：重布线半径，决定局部优化的范围。
通过调整参数和增加迭代次数，RRT*能够逐渐逼近最优路径。算法复杂度高于RRT，但路径质量显著提升。
