第四章：基于采样的路径规划：RRT、RRT*、Informed RRT*、工程中的采样策略与碰撞检测加速

各位同学，欢迎来到第四章。这一章我们聊点硬核的——基于采样的路径规划。

说实话，我在刚入行那会儿，最头疼的就是高维空间里的路径搜索。A* 在二维栅格图上跑得飞快，但一旦到了六自由度机械臂或者无人车的复杂场景，状态空间一爆炸，传统搜索方法基本就歇菜了。这时候，采样方法就成了救命稻草。

4.1 为什么需要采样？

你想想看，真实世界的环境是连续的。道路边界、障碍物形状，都不是规整的格子。如果我们用栅格去离散化，要么精度不够撞上障碍物，要么栅格太密导致计算量爆炸。

采样的思路很直接：我不需要知道整个空间的完整地图，我只需要随机撒点，然后把这些点连起来，找到一条从起点到终点的通路。说白了，就是用概率去赌——只要我撒的点足够多，总能找到一条路。

核心思想： 用随机采样代替穷举搜索，用概率完备性代替确定性完备性。

4.2 RRT：快速探索随机树

RRT 是我最早接触的采样算法。它的逻辑特别朴素：从起点开始，随机生长一棵树，直到这棵树碰到终点。

具体步骤是这样的：

  1. 以起点为根节点，初始化一棵树
  2. 在状态空间中随机采样一个点 q_rand
  3. 在树上找到离 q_rand 最近的节点 q_near
  4. 从 q_near 向 q_rand 方向延伸一个步长，得到新节点 q_new
  5. 检查 q_near 到 q_new 的路径是否碰撞，没碰撞就加入树中
  6. 重复直到新节点接近终点

代码实现其实不长：

def rrt_plan(start, goal, obstacle_map, max_iter=1000, step_size=0.5):
    tree = {start: None}  # 节点 -> 父节点
    
    for i in range(max_iter):
        # 以一定概率直接采样终点，引导树向目标生长
        if random.random() < 0.1:
            q_rand = goal
        else:
            q_rand = random_sample()
        
        q_near = nearest_neighbor(tree, q_rand)
        q_new = steer(q_near, q_rand, step_size)
        
        if not collision_check(q_near, q_new, obstacle_map):
            tree[q_new] = q_near
            
            if distance(q_new, goal) < step_size:
                return extract_path(tree, q_new)
    
    return None  # 没找到路径

我的经验： 实际工程中，RRT 的采样概率不能死板。我习惯在 5%-15% 之间动态调整——如果连续多次没找到新节点，就提高目标偏置概率，引导树往终点方向冲一冲。

RRT 的好处是快，特别快。但它有个致命问题：找到的路径通常很丑，弯弯绕绕的，而且不保证最优。你想想看，随机撒点出来的路径，怎么可能最优呢？

4.3 RRT*：渐进最优的改进

RRT* 就是在 RRT 基础上加了两个关键操作：重选父节点和重新布线。我当年第一次看到这个改进时，觉得设计者真是聪明——它让随机树有了「自我优化」的能力。

核心区别在于，RRT* 找到新节点后，不是直接把它挂到最近的节点上，而是：

  1. 在半径 r 范围内搜索所有邻近节点
  2. 选择一个能使从起点到新节点路径代价最小的邻近节点作为父节点
  3. 检查邻近节点，如果通过新节点能获得更优路径，就重新连接

看代码更清楚：

def rrt_star_plan(start, goal, obstacle_map, max_iter=2000, step_size=0.5):
    tree = {start: Node(start, cost=0)}
    
    for i in range(max_iter):
        q_rand = random_sample() if random.random() > 0.1 else goal
        q_near = nearest_neighbor(tree, q_rand)
        q_new = steer(q_near, q_rand, step_size)
        
        if collision_check(q_near, q_new, obstacle_map):
            continue
        
        # 查找邻近节点
        near_nodes = near_neighbors(tree, q_new, radius=step_size * 2)
        
        # 重选父节点：找代价最小的连接
        best_parent = q_near
        min_cost = tree[q_near].cost + distance(q_near, q_new)
        
        for node in near_nodes:
            if not collision_check(node, q_new, obstacle_map):
                cost = tree[node].cost + distance(node, q_new)
                if cost < min_cost:
                    min_cost = cost
                    best_parent = node
        
        # 添加新节点
        tree[q_new] = Node(q_new, cost=min_cost, parent=best_parent)
        
        # 重新布线：检查邻近节点能否通过新节点获得更优路径
        for node in near_nodes:
            if node == best_parent:
                continue
            new_cost = tree[q_new].cost + distance(q_new, node)
            if new_cost < tree[node].cost and not collision_check(q_new, node, obstacle_map):
                tree[node].parent = q_new
                tree[node].cost = new_cost
    
    return extract_best_path(tree, goal)

注意： RRT* 的邻近搜索半径 r 很关键。半径太小，找不到更好的父节点；半径太大，计算量爆炸。理论上最优半径跟节点数有关：r = γ * (log(n)/n)^(1/d)，其中 d 是空间维度。实际工程中我一般取步长的 1.5-2 倍，效果还行。

RRT* 的收敛速度其实挺慢的。我记得有次做泊车规划，跑了 5000 次迭代路径还在抖。后来我加了 early termination——如果连续 200 次迭代路径代价没改善，就直接返回当前最优解。

4.4 Informed RRT*：聚焦采样，加速收敛

RRT* 虽然能收敛到最优，但它在整个空间里瞎采样，效率太低了。Informed RRT* 的思路很直接：既然我们已经有了一条路径，那最优路径一定在这条路径周围的某个椭圆区域内。

这个椭圆怎么定义？以起点和终点为两个焦点，当前路径长度为长轴。任何比当前路径更优的路径，长度一定更短，所以它一定落在这个椭圆里。

采样策略就变成了：只在椭圆内采样，而不是全空间随机撒点。

def informed_sample(ellipse_params):
    # 在单位圆内均匀采样
    theta = random.uniform(0, 2 * pi)
    r = sqrt(random.uniform(0, 1))  # 均匀面积分布
    
    x = r * cos(theta)
    y = r * sin(theta)
    
    # 变换到椭圆空间
    c_min = ellipse_params['c_min']  # 起点到终点的距离
    c_max = ellipse_params['c_max']  # 当前路径长度
    
    a = c_max / 2
    b = sqrt(c_max**2 - c_min**2) / 2
    
    # 旋转和平移
    L = rotation_matrix(ellipse_params['theta'])
    point = L @ [a * x, b * y] + ellipse_params['center']
    
    return point

效果对比： 在相同迭代次数下，Informed RRT* 的收敛速度比 RRT* 快 3-5 倍。特别是当初始路径已经比较优时，椭圆区域很小，采样效率极高。

不过要注意，Informed RRT* 依赖初始路径。如果初始路径质量太差，椭圆区域太大，优势就不明显了。我一般先用 RRT 快速找一条可行路径，再用 Informed RRT* 去优化。

4.5 工程中的采样策略

理论说完了，聊聊工程落地。我在实际项目中踩过不少坑，总结了几条采样策略：

策略 适用场景 我的建议
目标偏置采样 狭窄通道、终点附近 概率设 5%-15%，动态调整
边界采样 障碍物密集区域 在障碍物边界附近增加采样密度
路径引导采样 已知粗略路径 沿已有路径周围高斯采样
自适应采样 未知环境探索 根据 Voronoi 图引导，偏向未探索区域

避坑指南： 我曾经在一个项目中，采样点全撒在障碍物里了，导致树根本长不出来。后来加了 rejection sampling——如果连续 10 次采样都碰撞，就切换到边界采样模式，往障碍物边缘撒点。效果立竿见影。

4.6 碰撞检测加速：工程落地的关键

说实话，采样算法本身的计算量并不大，真正的瓶颈在碰撞检测。你想想看，每生成一个新节点，就要做一次碰撞检测。如果场景里有几百个障碍物，每个障碍物几千个多边形，那碰撞检测就成了整个算法的阿喀琉斯之踵。

我常用的加速手段有这几个：

  1. 空间哈希网格： 把空间划分成网格，每个网格记录包含的障碍物。检测时只查新节点所在网格及相邻网格的障碍物。复杂度从 O(N) 降到 O(1)。
  2. 层次包围盒树： 对复杂障碍物构建 BVH 树，先检测大包围盒，再逐步细化。大部分情况下，第一层检测就排除了碰撞。
  3. 增量式检测： 如果新节点和父节点距离很近，可以复用父节点的碰撞检测结果，只检测新增的路径段。
  4. GPU 并行加速： 对于超大规模场景，把碰撞检测扔到 GPU 上并行跑。我试过，1000 个障碍物的场景，加速比能达到 20 倍以上。
# 空间哈希网格的简单实现
class SpatialHashGrid:
    def __init__(self, cell_size=1.0):
        self.cell_size = cell_size
        self.grid = {}  # (i, j) -> list of obstacles
    
    def add_obstacle(self, obstacle):
        # 计算障碍物覆盖的网格
        min_i, max_i = self._to_grid_range(obstacle.x_min, obstacle.x_max)
        min_j, max_j = self._to_grid_range(obstacle.y_min, obstacle.y_max)
        
        for i in range(min_i, max_i + 1):
            for j in range(min_j, max_j + 1):
                self.grid.setdefault((i, j), []).append(obstacle)
    
    def query(self, x, y):
        i, j = self._to_grid(x, y)
        return self.grid.get((i, j), [])

注意： 网格大小的选择很讲究。太小了，每个网格的障碍物少，但网格数量多，内存占用大；太大了，每个网格的障碍物多，查询效率下降。我一般取步长的 2-3 倍作为网格大小，这样每个网格平均 3-5 个障碍物，查询效率最高。

4.7 总结与工程建议

这一章我们聊了 RRT 家族的三兄弟。简单总结一下：

  • RRT： 快，但不优。适合快速找可行解。
  • RRT*： 能收敛到最优，但慢。适合离线规划。
  • Informed RRT*： 聚焦采样，加速收敛。适合需要高质量路径的场景。

最后给各位同学一个工程建议：不要死磕纯采样算法。在实际项目中，我经常把采样方法和优化方法结合起来——先用 RRT 找初始路径，再用二次规划或者样条曲线去平滑。这样既保证了路径的可行性，又得到了平滑可执行的轨迹。

下一章我们会聊基于优化的路径规划，到时候再细讲怎么把采样和优化结合起来。嗯，今天就到这里，有问题随时交流。

← 上一章 📖 返回目录 下一章 →