从原理到实现：Python实战RRT与RRT*路径规划算法
1. 从“盲人摸象”到“智能生长”：RRT算法到底在干什么？
想象一下，你被蒙上眼睛，放在一个布满家具的陌生房间里，任务是找到通往门口的路。你会怎么做？大概率会伸出手，小心翼翼地向前摸索，摸到墙或者桌子就换个方向，一点点地探索整个空间，直到手指触碰到门框。快速随机探索树（Rapidly-exploring Random Tree, RRT） 算法的核心思想，和这个“盲人摸象”的过程惊人地相似。它不是为了从一开始就计算出一条完美的最短路径，而是为了在复杂、高维的空间里，快速找到一条可行的通路。
在机器人、自动驾驶、游戏AI甚至机械臂运动规划中，我们经常面临这样的问题：起点和终点之间充满了各种形状的障碍物，空间可能是二维的（像一张地图），也可能是六维甚至更高维的（比如机械臂每个关节的角度）。用传统的网格搜索方法（比如A*算法）在庞大空间里会非常慢，而RRT另辟蹊径，它用一种“生长”的方式来解决这个问题。
你可以把RRT想象成一棵不断生长的树。起点就是这颗树的种子（根节点）。算法开始运行后，它会在整个空间里“瞎扔飞镖”——也就是随机采样一个点。然后，它看看当前这棵“树”上所有的“枝丫”（已有的节点），找到离这个随机点最近的那个树枝节点。接着，它不会直接长到随机点那里去（因为可能太远，中间有障碍），而是朝着随机点的方向，小心翼翼地生长一小步（这一步的长度是固定的，称为步长），长出一个新的树枝节点。如果这一步生长过程中撞到了障碍物，那这次生长就作废，重新扔飞镖。如此反复，这棵树就会像触手一样，在空间里随机但快速地蔓延开来，直到有某个新长出的树枝节点碰到了终点区域。最后，我们从终点那个节点开始，倒着找回它的父节点、父节点的父节点……一路回溯到起点，就得到了一条从起点到终点的路径。
我第一次在机器人项目里用RRT时，感觉它特别“聪明”又特别“笨”。“聪明”在于它完全跳出了遍历网格的思维定式，用随机采样来高效探索未知区域；“笨”在于它找到的路径往往歪歪扭扭，像喝醉了酒走的路线，完全不是最优解。但这恰恰是它的设计目的：在最短的时间内，找到一条能走通的路，而不是找到最好的路。对于很多实时性要求高的场景，比如无人机紧急避障，有路可走比路好不好更重要。
2. 手把手图解：RRT算法的五大核心步骤拆解
光说原理可能还有点抽象，我们直接来看RRT是怎么一步步“长”出路径的。我会用一个非常简单的二维地图示例，并配上详细的Python代码片段，让你能真正看懂并复现。
2.1 第一步：初始化地图与“种子”
首先，我们得定义我们的世界。假设我们有一个25x12的网格空间，起点在(6,4)，终点在(17,5)。中间有一堵竖直的墙作为障碍物，位于x=10，y从2到8的位置。我们用matplotlib来可视化这一切。
import numpy as np
import matplotlib.pyplot as plt
# 初始化地图
x_width, y_width = 25, 12
# 创建一个全零的网格，0代表自由空间
occupancy_grid = np.zeros((y_width, x_width))
# 设置障碍物（墙）
for y in range(2, 9):
occupancy_grid[y][10] = 1 # 1代表障碍物
# 设置起点和终点
start = (6, 4)
goal = (17, 5)
occupancy_grid[start[1]][start[0]] = 4 # 起点标记为4
occupancy_grid[goal[1]][goal[0]] = 3 # 终点标记为3
# 可视化初始化
fig, ax = plt.subplots(figsize=(10, 5))
ax.set_xlim(-1, x_width)
ax.set_ylim(-1, y_width)
ax.set_xticks(np.arange(x_width))
ax.set_yticks(np.arange(y_width))
ax.grid(True)
ax.plot(start[0], start[1], 'ro', markersize=10, label='Start') # 红色起点
ax.plot(goal[0], goal[1], 'yo', markersize=10, label='Goal') # 黄色终点
# 绘制障碍物墙
ax.plot([10]*7, list(range(2, 9)), 'ks', markersize=15, label='Obstacle')
ax.legend()
plt.title("RRT Path Planning - Initial Map")
plt.show()
这棵树的初始列表tree_list，我们用一个列表来存储，每个元素是一个节点。节点我们怎么表示呢？一个简单的办法是存储[x, y, parent_x, parent_y]，即自己的坐标和父节点的坐标。起点没有父节点，我们就让它指向自己。tree_list一开始就是：[[6, 4, 6, 4]]。
2.2 第二步：随机采样——向未知区域“扔飞镖”
RRT探索的动力来源于随机性。我们在地图的自由空间（非障碍物区域）内，随机生成一个点x_rand。这个点不能是树上已经有的点。
import random
import itertools
def sample_random_point(tree_nodes, x_range, y_range):
"""
在整个空间内随机采样一个点。
参数：
tree_nodes: 当前树中所有节点的坐标列表，用于避免重复采样。
x_range, y_range: 空间的边界。
返回：
一个(x, y)坐标的列表。
"""
# 生成所有可能的网格点（这里为了简单，采样离散的整数点）
all_points = list(itertools.product(range(x_range), range(y_range)))
# 转换为numpy数组方便比较
tree_array = np.array(tree_nodes)[:, :2] if tree_nodes else np.array([]).reshape(-1,2)
point = random.choice(all_points)
# 确保采样的点不在已有的树节点中（避免重复）
# 注意：这里用循环判断，在实际高维或连续空间，需要用更高效的方法（如KD-Tree）
while tree_array.size > 0 and any(np.array_equal(point, node) for node in tree_array):
point = random.choice(all_points)
return list(point)
在实际的高性能实现中，我们通常不会生成所有点再随机选，而是在连续空间内直接生成随机坐标。这里为了清晰和理解，我们用了离散网格。这个函数就是算法的“眼睛”，负责东张西望，寻找新的探索方向。
2.3 第三步：寻找最近邻——确定生长方向
随机点x_rand出来了，树该往哪个方向长呢？答案是朝着离x_rand最近的那个已有树节点长。我们需要遍历整棵树，找到距离x_rand最近的节点x_near。距离计算通常用欧几里得距离。
def find_nearest_node(tree_nodes, random_point):
"""
在树中找到距离随机点最近的节点。
参数：
tree_nodes: 树节点列表，每个元素为[x, y, parent_x, parent_y]。
random_point: 随机采样点[x, y]。
返回：
最近节点的坐标[x, y]。
"""
min_dist = float('inf')
nearest_node = None
for node in tree_nodes:
node_pos = node[:2] # 取节点的坐标部分
# 计算欧几里得距离
dist = np.sqrt((node_pos[0] - random_point[0])**2 + (node_pos[1] - random_point[1])**2)
if dist < min_dist:
min_dist = dist
nearest_node = node_pos
return nearest_node
这里有一个可以优化的关键点：当树有成千上万个节点时，每次都用循环遍历找最近邻会非常慢。工业级实现一定会用空间索引数据结构，比如KD-Tree，它能把查找最近邻的时间复杂度从O(N)降到O(log N)。这是你从玩具代码走向实用代码的第一个坎。
2.4 第四步：步长生根——向目标迈出一小步
找到了最近的树枝x_near，我们不是直接连接x_near和x_rand，而是从x_near出发，朝着x_rand的方向，生长一个固定步长step_size，得到一个新节点x_new。这就像你朝着目标伸手，但只伸一臂的长度。
def steer(from_point, to_point, step_size):
"""
从from_point向to_point方向生长一个步长，得到新点。
参数：
from_point: 起始点[x, y]。
to_point: 目标点[x, y]。
step_size: 生长步长。
返回：
新点的坐标[x, y]。
"""
# 计算方向向量
direction = np.array(to_point) - np.array(from_point)
length = np.linalg.norm(direction) # 计算两点间的距离
if length == 0:
return from_point # 如果两点重合，直接返回
# 单位化方向向量，并乘以步长
unit_vector = direction / length
new_point = np.array(from_point) + unit_vector * step_size
# 返回整数坐标（因为我们用了离散网格示例）
return new_point.astype(int).tolist()
这个steer函数是RRT生长的核心动作。步长step_size是个关键参数：设得太小，树长得太慢，规划时间过长；设得太大，可能会“跨过”狭窄的通道，或者导致路径不平滑。我一般会根据环境尺度来设置，比如地图尺寸的5%-10%。
2.5 第五步：碰撞检测与循环生长——安全第一，直到终点
新点x_new长出来了，但我们不能直接把它加入树。必须检查从x_near到x_new的这条线段是否穿过了障碍物。这就是碰撞检测。在我们的简单例子里，障碍物是一堵墙，我们可以用直线方程判断线段是否与墙相交。
def collision_free(near, new, obstacle_line_x=10, obstacle_y_range=(2, 8)):
"""
简单的碰撞检测：检查线段是否穿过特定的障碍物墙。
这是一个简化的示例，真实场景需要更通用的几何碰撞检测。
"""
# 如果线段横跨了障碍物的x坐标
if min(near[0], new[0]) <= obstacle_line_x <= max(near[0], new[0]):
# 计算线段在x=10处的y值（直线方程）
if near[0] == new[0]: # 垂直线
return not (obstacle_y_range[0] <= near[1] <= obstacle_y_range[1] and near[0] == obstacle_line_x)
else:
k = (new[1] - near[1]) / (new[0] - near[0])
y_at_obstacle = k * (obstacle_line_x - near[0]) + near[1]
# 如果交点y值在障碍物y范围内，则发生碰撞
if obstacle_y_range[0] <= y_at_obstacle <= obstacle_y_range[1]:
return False
return True
如果碰撞检测通过，我们就把x_new加入树：tree_list.append([x_new[0], x_new[1], x_near[0], x_near[1]])。然后，我们进入一个大的循环，重复“采样-找最近邻-步长生根-碰撞检测”这个过程。循环的终止条件，通常是x_new进入了终点的一个小邻域内（比如距离终点小于步长），或者达到了最大迭代次数（防止无限循环）。
当循环结束，我们找到了一条连接到终点区域的路径。最后一步是路径提取：从终点区域的节点开始，利用每个节点存储的父节点信息，一路回溯到起点，就得到了从起点到终点的坐标序列。
def extract_path(tree_nodes, start, goal_node):
"""
从树中提取从起点到指定目标节点的路径。
"""
path = [goal_node[:2]] # 从目标节点开始
current = goal_node
# 不断回溯父节点，直到起点
while not (current[0] == start[0] and current[1] == start[1]):
parent = (current[2], current[3]) # 父节点坐标
# 在树中找到父节点
for node in tree_nodes:
if node[0] == parent[0] and node[1] == parent[1]:
path.append(parent)
current = node
break
path.append(start) # 加入起点
path.reverse() # 反转，变成从起点到终点
return path
把以上所有步骤串起来，就是完整的RRT算法。你可以看到，它逻辑清晰，实现起来并不复杂。但跑一次你就会发现，这条路径通常很“随机”，弯弯绕绕，长度远非最优。这就是RRT的局限性，也是RRT*算法要解决的问题。
3. 从“有路走”到“走好路”：RRT*算法的两大精妙改进
RRT算法像个开拓者，只管打通道路，不管路好不好走。而RRT*（RRT Star） 算法则像一位精益求精的工程师，它在RRT的基础上，增加了两个至关重要的后优化步骤：重选父节点（Rewiring） 和重布线（Random Relink）。这两个步骤让RRT*具备了渐进最优的特性，也就是说，只要给它足够的时间运行，它找到的路径会无限接近理论上的最短路径。
3.1 改进一：重选父节点——为“新芽”找个更好的“家”
在基础RRT中，新节点x_new的父节点永远是离它最近的树节点x_near。但x_near到起点的路径不一定是最短的。RRT*在加入x_new后，会做一次“寻亲”优化。
它会在树上找一个以x_new为圆心、半径为r的圆形区域（r通常与步长相关，比如r = 1.5 * step_size）。在这个区域内的所有已有节点，都被视为x_new的潜在父节点候选。然后，算法会计算：如果x_new以候选节点A为父节点，那么从起点到x_new的路径成本（通常是路径长度）是多少？这个成本等于“起点到A的成本”加上“A到x_new的直线距离”。算法会遍历所有候选父节点，选择那个能使得x_new总路径成本最小的节点，作为它最终的父节点。
def choose_parent(tree_nodes, new_node, step_size, obstacle_func):
"""
为new_node选择一个最优的父节点。
参数：
tree_nodes: 树节点列表，每个元素为[x, y, parent_x, parent_y, cost_from_start]。
new_node: 新节点坐标[x, y]。
step_size: 步长，用于定义搜索半径。
obstacle_func: 碰撞检测函数。
返回：
最优父节点的索引，以及new_node的最小成本。
"""
search_radius = 1.5 * step_size
potential_parents = []
for i, node in enumerate(tree_nodes):
node_pos = node[:2]
dist_to_new = np.linalg.norm(np.array(node_pos) - np.array(new_node))
# 1. 在搜索半径内
# 2. 且从node到new_node是无碰撞的
if dist_to_new < search_radius and obstacle_func(node_pos, new_node):
# 计算以node为父节点时，new_node的成本
cost_via_node = node[4] + dist_to_new # node[4]存储了从起点到node的成本
potential_parents.append((i, cost_via_node, dist_to_new))
if not potential_parents:
# 如果没有候选父节点，则返回最近邻（即原RRT逻辑）
nearest_idx, min_dist = find_nearest_index(tree_nodes, new_node)
return nearest_idx, tree_nodes[nearest_idx][4] + min_dist
# 选择成本最小的候选父节点
best_idx, min_cost, _ = min(potential_parents, key=lambda x: x[1])
return best_idx, min_cost
这个步骤的意义在于，它让新节点有机会“嫁接”到一条更优的“枝干”上，从而从一开始就降低整条分支的路径成本。这就像公司来了个新员工（x_new），不是随便交给离他最近的经理（x_near）带，而是考察周围所有合适的经理（候选父节点），看谁带他能使他的成长路径（到起点的成本）最优。
3.2 改进二：重布线——优化整棵“家族树”
重选父节点只优化了新节点自己。重布线则更进一步，它考虑新节点x_new能否成为其他已有节点的“更好父亲”。
在x_new加入树并确定了父节点后，RRT*会再次检查x_new附近一定半径（比如1.6 * step_size）内的所有已有节点。对于每一个这样的邻居节点，算法会问：如果我把你的父节点从原来的那个，改成新来的x_new，你到起点的路径会不会更短？如果会，那就“改宗”，让x_new当你的新父节点，并更新你和你的所有子节点的路径成本。
def rewire(tree_nodes, new_node_idx, step_size, obstacle_func):
"""
重布线：尝试以new_node为父节点，优化其邻居节点的路径。
参数：
tree_nodes: 树节点列表（会被修改）。
new_node_idx: 新加入节点的索引。
step_size: 步长，用于定义影响半径。
obstacle_func: 碰撞检测函数。
返回：
更新后的树节点列表。
"""
new_node = tree_nodes[new_node_idx]
new_node_pos = new_node[:2]
new_node_cost = new_node[4] # 起点到new_node的成本
rewire_radius = 1.6 * step_size
for i, node in enumerate(tree_nodes):
if i == new_node_idx:
continue # 跳过自己
node_pos = node[:2]
dist = np.linalg.norm(np.array(node_pos) - np.array(new_node_pos))
# 如果节点在重布线半径内，且无碰撞
if dist < rewire_radius and obstacle_func(new_node_pos, node_pos):
# 计算如果以new_node为父节点，该节点的成本
potential_cost = new_node_cost + dist
# 如果新成本更低，则重布线
if potential_cost < node[4]:
# 更新该节点的父节点和成本
tree_nodes[i][2] = new_node_pos[0] # parent_x
tree_nodes[i][3] = new_node_pos[1] # parent_y
tree_nodes[i][4] = potential_cost # cost_from_start
# 注意：这里需要递归更新该节点所有子节点的成本，这是一个简化版，实际需要遍历子树
# 为了清晰，此处省略了递归更新子树的代码，但思想是必须的。
return tree_nodes
重布线是RRT*实现渐进最优的关键。它让优化不仅仅发生在局部（新节点），还能反向传播到已有的树结构中。随着采样点越来越多，树的结构会通过无数次这样的重布线操作，变得越来越高效，最终生成的路径也越来越平滑、越来越短。你可以把它想象成不断修剪和优化一棵盆景，让养分（路径成本）的输送效率达到最高。
4. Python实战：对比RRT与RRT*的路径优化效果
理论说了这么多，是骡子是马得拉出来遛遛。我们现在就用完整的Python代码，在同一个地图上分别运行RRT和RRT*，直观地感受它们的差异。为了公平对比，我们使用相同的起点、终点、障碍物、步长和最大迭代次数。
首先，我们把前面拆解的函数整合成两个完整的类：RRTPlanner 和 RRTStarPlanner。为了代码清晰和可复用，我们会把碰撞检测、采样、最近邻搜索等公共功能抽象出来。
import numpy as np
import random
import matplotlib.pyplot as plt
from math import sqrt
from scipy.spatial import KDTree # 用于高效最近邻搜索
class RRTBase:
"""RRT和RRT*算法的基类，包含公共方法和属性"""
def __init__(self, start, goal, obstacle_list, bounds, step_size=1.0, max_iter=500):
self.start = np.array(start)
self.goal = np.array(goal)
self.obstacle_list = obstacle_list # 障碍物列表，每个障碍物用(x, y, radius)或更复杂的表示
self.bounds = bounds # 空间边界 [(x_min, x_max), (y_min, y_max)]
self.step_size = step_size
self.max_iter = max_iter
self.tree = [] # 存储所有节点，格式为 [node, parent_index, cost]
self.path = []
self.goal_threshold = step_size * 1.5 # 认为到达目标的距离阈值
def random_sample(self):
"""在边界内随机采样一个点"""
# 以一定概率直接采样目标点，加速收敛（目标偏置采样）
if random.random() > 0.1:
sample = [random.uniform(self.bounds[0][0], self.bounds[0][1]),
random.uniform(self.bounds[1][0], self.bounds[1][1])]
else:
sample = self.goal.tolist()
return np.array(sample)
def nearest(self, tree, point):
"""找到树中离point最近的节点（使用KDTree加速）"""
if not tree:
return None
# 提取所有节点坐标
nodes = np.array([node[0] for node in tree])
kdtree = KDTree(nodes)
dist, idx = kdtree.query(point)
return idx
def steer(self, from_point, to_point):
"""从from_point向to_point方向生长一个步长"""
direction = to_point - from_point
distance = np.linalg.norm(direction)
if distance == 0:
return from_point
# 如果距离小于步长，直接返回to_point
if distance < self.step_size:
return to_point
# 否则，按步长生长
unit_vector = direction / distance
new_point = from_point + unit_vector * self.step_size
return new_point
def is_collision_free(self, point1, point2):
"""检查两点连线是否与任何障碍物碰撞（简化版，假设障碍物为圆形）"""
for (ox, oy, radius) in self.obstacle_list:
# 计算线段到圆心的最短距离
# 这里使用向量投影的方法进行简化碰撞检测
line_vec = point2 - point1
line_len = np.linalg.norm(line_vec)
if line_len == 0:
continue
line_unit_vec = line_vec / line_len
circ_vec = np.array([ox, oy]) - point1
proj_length = np.dot(circ_vec, line_unit_vec)
proj_length = max(0, min(line_len, proj_length))
closest_point = point1 + line_unit_vec * proj_length
dist_to_center = np.linalg.norm(closest_point - np.array([ox, oy]))
if dist_to_center <= radius:
return False
return True
def is_goal_reached(self, node):
"""检查节点是否到达目标区域"""
return np.linalg.norm(node - self.goal) <= self.goal_threshold
def extract_path(self, goal_idx):
"""从目标节点索引回溯到起点，提取路径"""
path = [self.goal]
node_idx = goal_idx
while node_idx is not None:
node = self.tree[node_idx][0]
path.append(node)
node_idx = self.tree[node_idx][1] # 父节点索引
path.append(self.start)
path.reverse()
return np.array(path)
class RRTPlanner(RRTBase):
"""基础RRT路径规划器"""
def plan(self):
# 初始化树，起点作为根节点
self.tree = [(self.start, None, 0.0)] # (node, parent_index, cost)
for iteration in range(self.max_iter):
# 1. 随机采样
rand_point = self.random_sample()
# 2. 找最近邻
nearest_idx = self.nearest(self.tree, rand_point)
nearest_node = self.tree[nearest_idx][0]
# 3. 步长生根
new_node = self.steer(nearest_node, rand_point)
# 4. 碰撞检测
if self.is_collision_free(nearest_node, new_node):
# 计算新节点的成本（到起点的路径长度）
new_cost = self.tree[nearest_idx][2] + np.linalg.norm(new_node - nearest_node)
# 加入树
self.tree.append((new_node, nearest_idx, new_cost))
# 5. 检查是否到达目标
if self.is_goal_reached(new_node):
print(f"RRT: Path found after {iteration} iterations!")
self.path = self.extract_path(len(self.tree)-1)
return True
print("RRT: Max iterations reached, path not found.")
return False
class RRTStarPlanner(RRTBase):
"""RRT*路径规划器"""
def __init__(self, start, goal, obstacle_list, bounds, step_size=1.0, max_iter=500, search_radius=1.5):
super().__init__(start, goal, obstacle_list, bounds, step_size, max_iter)
self.search_radius = search_radius # 重选父节点和重布线的搜索半径系数
def plan(self):
# 初始化树
self.tree = [(self.start, None, 0.0)]
for iteration in range(self.max_iter):
# 1. 随机采样
rand_point = self.random_sample()
# 2. 找最近邻
nearest_idx = self.nearest(self.tree, rand_point)
nearest_node = self.tree[nearest_idx][0]
# 3. 步长生根
new_node = self.steer(nearest_node, rand_point)
# 4. 碰撞检测
if not self.is_collision_free(nearest_node, new_node):
continue # 碰撞，跳过此次迭代
# --- RRT* 核心改进开始 ---
# 5. 在搜索半径内寻找潜在父节点
near_indices = self.find_near_nodes(new_node)
# 选择最优父节点
best_parent_idx = nearest_idx
min_cost = self.tree[nearest_idx][2] + np.linalg.norm(new_node - nearest_node)
for near_idx in near_indices:
near_node = self.tree[near_idx][0]
# 检查从near_node到new_node是否无碰撞
if self.is_collision_free(near_node, new_node):
# 计算通过near_node到达new_node的成本
cost = self.tree[near_idx][2] + np.linalg.norm(new_node - near_node)
if cost < min_cost:
min_cost = cost
best_parent_idx = near_idx
# 6. 添加新节点到树，使用最优父节点
new_node_idx = len(self.tree)
self.tree.append((new_node, best_parent_idx, min_cost))
# 7. 重布线：尝试用new_node优化附近节点的父节点
self.rewire(new_node_idx, near_indices)
# --- RRT* 核心改进结束 ---
# 8. 检查是否到达目标
if self.is_goal_reached(new_node):
print(f"RRT*: Path found after {iteration} iterations!")
self.path = self.extract_path(new_node_idx)
return True
print("RRT*: Max iterations reached, path not found.")
return False
def find_near_nodes(self, new_node):
"""找到在搜索半径内的所有节点索引"""
if len(self.tree) == 0:
return []
nodes = np.array([node[0] for node in self.tree])
kdtree = KDTree(nodes)
radius = self.search_radius * self.step_size * np.sqrt(np.log(len(self.tree)+1) / (len(self.tree)+1)) # 渐进最优的半径公式
near_indices = kdtree.query_ball_point(new_node, radius)
return near_indices
def rewire(self, new_node_idx, near_indices):
"""重布线：尝试用新节点优化其邻居的父节点"""
new_node, _, new_cost = self.tree[new_node_idx]
for near_idx in near_indices:
if near_idx == new_node_idx:
continue
near_node, _, near_cost = self.tree[near_idx]
# 检查从new_node到near_node是否无碰撞
if self.is_collision_free(new_node, near_node):
# 计算如果以new_node为父节点，near_node的新成本
potential_cost = new_cost + np.linalg.norm(near_node - new_node)
if potential_cost < near_cost:
# 更新父节点和成本
self.tree[near_idx] = (near_node, new_node_idx, potential_cost)
现在，我们创建一个测试场景来对比两种算法。我们设置几个圆形障碍物，让路径规划更有挑战性。
def run_comparison():
# 定义场景
start = (2, 2)
goal = (18, 8)
bounds = [(0, 20), (0, 10)] # x和y的范围
# 定义几个圆形障碍物 (x, y, radius)
obstacles = [(5, 5, 1.5), (8, 3, 1.0), (12, 6, 2.0), (10, 8, 1.2), (15, 4, 1.8)]
# 创建规划器实例
rrt_planner = RRTPlanner(start, goal, obstacles, bounds, step_size=0.8, max_iter=2000)
rrt_star_planner = RRTStarPlanner(start, goal, obstacles, bounds, step_size=0.8, max_iter=2000, search_radius=2.0)
# 运行规划
print("Running RRT...")
rrt_success = rrt_planner.plan()
print("Running RRT*...")
rrt_star_success = rrt_star_planner.plan()
# 可视化结果
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 6))
# 绘制RRT结果
ax1.set_title("RRT Path Planning")
ax1.set_xlim(bounds[0][0]-1, bounds[0][1]+1)
ax1.set_ylim(bounds[1][0]-1, bounds[1][1]+1)
ax1.set_aspect('equal')
# 绘制障碍物
for (ox, oy, r) in obstacles:
circle = plt.Circle((ox, oy), r, color='gray', alpha=0.7)
ax1.add_patch(circle)
# 绘制起点和终点
ax1.plot(start[0], start[1], 'go', markersize=10, label='Start', markeredgecolor='black')
ax1.plot(goal[0], goal[1], 'ro', markersize=10, label='Goal', markeredgecolor='black')
# 绘制树
for i, (node, parent_idx, cost) in enumerate(rrt_planner.tree):
if parent_idx is not None:
parent_node = rrt_planner.tree[parent_idx][0]
ax1.plot([node[0], parent_node[0]], [node[1], parent_node[1]], 'lightblue', linewidth=0.5, alpha=0.6)
# 绘制路径
if rrt_success and len(rrt_planner.path) > 0:
path = rrt_planner.path
ax1.plot(path[:, 0], path[:, 1], 'b-', linewidth=3, label='RRT Path')
rrt_length = np.sum(np.linalg.norm(np.diff(path, axis=0), axis=1))
ax1.text(0.05, 0.95, f'Path Length: {rrt_length:.2f}', transform=ax1.transAxes, fontsize=12,
verticalalignment='top', bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.8))
ax1.legend()
ax1.grid(True)
# 绘制RRT*结果
ax2.set_title("RRT* Path Planning")
ax2.set_xlim(bounds[0][0]-1, bounds[0][1]+1)
ax2.set_ylim(bounds[1][0]-1, bounds[1][1]+1)
ax2.set_aspect('equal')
# 绘制障碍物
for (ox, oy, r) in obstacles:
circle = plt.Circle((ox, oy), r, color='gray', alpha=0.7)
ax2.add_patch(circle)
# 绘制起点和终点
ax2.plot(start[0], start[1], 'go', markersize=10, label='Start', markeredgecolor='black')
ax2.plot(goal[0], goal[1], 'ro', markersize=10, label='Goal', markeredgecolor='black')
# 绘制树（可选，树太密可以注释掉）
# for i, (node, parent_idx, cost) in enumerate(rrt_star_planner.tree):
# if parent_idx is not None:
# parent_node = rrt_star_planner.tree[parent_idx][0]
# ax2.plot([node[0], parent_node[0]], [node[1], parent_node[1]], 'lightgreen', linewidth=0.5, alpha=0.4)
# 绘制路径
if rrt_star_success and len(rrt_star_planner.path) > 0:
path_star = rrt_star_planner.path
ax2.plot(path_star[:, 0], path_star[:, 1], 'darkorange', linewidth=3, label='RRT* Path')
rrt_star_length = np.sum(np.linalg.norm(np.diff(path_star, axis=0), axis=1))
ax2.text(0.05, 0.95, f'Path Length: {rrt_star_length:.2f}', transform=ax2.transAxes, fontsize=12,
verticalalignment='top', bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.8))
ax2.legend()
ax2.grid(True)
plt.tight_layout()
plt.show()
# 打印对比结果
if rrt_success and rrt_star_success:
print("\n=== 算法对比 ===")
print(f"RRT 路径节点数: {len(rrt_planner.path)}")
print(f"RRT* 路径节点数: {len(rrt_star_planner.path)}")
print(f"RRT 路径长度: {rrt_length:.2f}")
print(f"RRT* 路径长度: {rrt_star_length:.2f}")
improvement = ((rrt_length - rrt_star_length) / rrt_length) * 100
print(f"RRT* 路径优化了: {improvement:.1f}%")
# 运行对比
run_comparison()
跑完这段代码，你会在同一张地图上看到左右两幅图。左边是RRT的结果，路径通常像一根随意生长的藤蔓，有很多不必要的拐弯。右边是RRT的结果，路径明显更直接、更平滑，长度也更短。这就是重选父节点和重布线两大“魔法”带来的直观优化效果。RRT的树看起来也更“茂密”且更有条理，因为它不断在优化连接关系。
5. 避坑指南与性能调优：让算法真正能用起来
纸上谈兵终觉浅，绝知此事要躬行。在实际项目中把RRT/RRT*用起来，你会遇到一堆原始论文里不会提的“坑”。这里我结合自己的经验，分享几个关键的调优点和避坑指南。
5.1 参数调优：步长、搜索半径与采样策略
-
步长（Step Size）：这是最重要的参数之一。步长太大，算法可能“跨过”狭窄通道，导致规划失败；步长太小，树生长太慢，规划时间激增。我的经验法则是，让步长略小于环境中最窄通道的宽度。你也可以实现一个自适应步长的策略，比如在开阔区域用大步长快速探索，在靠近障碍物或目标时用小步长精细调整。
-
搜索半径（对于RRT）*：这个半径决定了“重选父节点”和“重布线”的优化范围。原始论文给出了一个理论上的最优值：
r = γ * (log(n) / n)^(1/d)，其中n是节点数，d是空间维度，γ是一个常数。在实际中，我通常设为一个与步长相关的固定值（如1.5 * step_size到2.0 * step_size），然后根据效果微调。半径太小，优化效果有限；半径太大，计算量会剧增。 -
采样策略：纯随机采样效率不高。可以引入目标偏置采样，即以一个小概率（如5%）直接采样目标点，这能显著加快算法收敛到目标的速度。更高级的还有启发式采样，比如在可能存在的“瓶颈”区域增加采样密度。
5.2 性能瓶颈与加速技巧
RRT/RRT*的瓶颈主要在最近邻搜索和碰撞检测。
- 最近邻搜索：当树有上万个节点时，线性遍历找最近邻是灾难。必须使用空间索引！
scipy.spatial.KDTree或sklearn.neighbors.KDTree是Python中非常好用的工具，能将复杂度从O(N)降到O(log N)。我在代码示例中已经使用了KDTree。 - 碰撞检测：这是另一个计算大户。对于简单的几何形状（圆形、矩形），可以直接计算。对于复杂的多边形障碍物或三维网格地图，需要更高效的碰撞检测库，如
shapely（2D）或pybullet/trimesh（3D）。一个优化技巧是进行两级碰撞检测：先用一个快速的包围盒（AABB）进行粗略排除，再对可能碰撞的物体进行精确检测。
5.3 常见问题与调试
- 算法卡住，找不到路径：首先检查最大迭代次数是否足够。然后检查步长是否过大，导致无法穿过狭窄区域。可以尝试增加目标偏置采样的概率。最后，用可视化工具把树和障碍物画出来，看看树是不是被障碍物“困住”了。
- RRT*路径仍然不优：增加最大迭代次数。RRT*是渐进最优的，需要时间“打磨”路径。检查搜索半径是否设置合理，可以适当增大。确保重布线的逻辑正确实现了，特别是子节点成本的递归更新。
- 路径不平滑：RRT/RRT*生成的路径是由直线段组成的折线。对于机器人或车辆，这样的路径无法直接跟踪。需要在后处理阶段进行路径平滑，比如使用贝塞尔曲线、B样条或者简单的梯度下降平滑算法，在保持无碰撞的前提下，让路径变得可执行。
5.4 进阶方向：从RRT*到更高级的变种
当你掌握了基础的RRT和RRT*，可以探索更强大的变种算法：
- Informed RRT*：当找到第一条路径后，它会将采样范围限制在一个椭圆形的“启发式”区域内，这个椭圆以起点和终点为焦点，以当前最佳路径长度为长轴。这能极大地提高在找到可行解后的优化效率。
- RRT-Connect (Bi-RRT)：同时从起点和终点生长两棵树，直到它们连接在一起。这种双向搜索策略通常比单棵树快得多。
- Anytime RRT*：一种随时可中断的算法，它持续运行并不断优化当前找到的最佳路径，非常适合实时性要求高的场景，你可以在任何时间点获取当前的最佳解。
我在一个机械臂抓取项目里就用到了Anytime RRT*。机械臂的工作空间有六个维度（关节角度），障碍物是各种不规则的工作台和零件。基础RRT能在几百毫秒内找到一条勉强能用的路径，但动作很突兀。换成RRT*并让它多跑几秒后，得到的路径就平滑自然多了，关节电机的负载也明显下降。这让我深刻体会到，在路径规划里，“快”和“好”往往需要权衡，而算法的选择就是你的权衡工具。
最后，把完整的、可运行的代码放到一个文件里，多跑几次，调整参数，观察树是如何生长和优化的。路径规划算法最有魅力的地方就在于它的“可视化”，看着一条路径从无到有、从差到好地被规划出来，那种感觉就像在指挥一个智能生命体探索世界。希望这篇文章和代码能成为你探索这个有趣领域的起点。
Agent 垂直技术社区，欢迎活跃、内容共建。
更多推荐
2
0- 0
已为社区贡献4条内容
所有评论(0)