【启发式算法】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，但路径质量显著提升。