4. 规划器类型与适用场景

好，咱们进入OMPL的世界。OMPL，全称是Open Motion Planning Library，MoveIt默认就用它来做底层规划。说白了，它就是一群顶尖算法工程师帮你写好的路径搜索工具箱。你不需要从零造轮子，但得知道每个轮子适合什么路。

我个人习惯把OMPL里的规划器分成两类：单查询和多查询。单查询就是“从A到B找一条路就行”，多查询是“我要反复在同一个环境里问很多次路径”。今天聊的RRT、RRT-Connect、EST属于前者，PRM和LazyPRM属于后者。

4.1 RRT：快速探索随机树

RRT，全称Rapidly-exploring Random Tree。名字很直白——快速探索随机树。它的思路其实很简单：在C空间（构型空间）里随机撒点，然后从已有的树向新点生长。

你想想看，这就像你在一个黑暗的迷宫里，手里拿着一根可以无限延伸的绳子。你随机朝一个方向扔出去，绳子末端落到哪，你就走到哪，然后继续扔。扔着扔着，整个迷宫的空间就被你探索得差不多了。

RRT的核心步骤就三步：

1. 采样：在C空间随机生成一个点 q_rand。
2. 扩展：从树上找离 q_rand 最近的节点 q_near，朝 q_rand 方向走一步，得到 q_new。
3. 碰撞检测：如果 q_near 到 q_new 的边没碰到障碍，就把 q_new 加入树。

重复这三步，直到 q_new 离目标点足够近，或者达到最大迭代次数。

// 伪代码：RRT核心逻辑
for i = 1 to max_iterations:
    q_rand = sample_random_configuration()
    q_near = find_nearest_node(tree, q_rand)
    q_new = extend_towards(q_near, q_rand, step_size)
    if is_collision_free(q_near, q_new):
        tree.add_node(q_new)
        tree.add_edge(q_near, q_new)
        if distance(q_new, q_goal) < threshold:
            return extract_path(tree, q_new)
return failure

4.2 RRT-Connect：双向生长的RRT

RRT-Connect是RRT的改进版。它同时从起点和终点生长两棵树，让它们“双向奔赴”。

为什么这样更好？你想啊，单棵树从起点往外长，如果终点在障碍物后面，树得绕一大圈才能过去。双向生长相当于两边同时探索，相遇的概率高得多。

我记得有一次做七轴机械臂的避障规划，用普通RRT跑了3万次迭代还没找到路径，换成RRT-Connect后，不到5000次迭代两棵树就碰头了。差距就是这么明显。

特性	RRT	RRT-Connect
树的数量	1棵（从起点生长）	2棵（起点和终点各一棵）
收敛速度	较慢	快（尤其狭窄通道场景）
实现复杂度	简单	中等
适用场景	开阔空间	狭窄通道、复杂障碍

RRT-Connect的扩展策略也有区别。它不是每次只走一步，而是能走多远走多远——只要不撞墙就一直往前冲。这种“贪婪”策略让它在开阔区域能快速推进。

4.3 PRM与LazyPRM：概率路图

PRM（Probabilistic Roadmap）的思路和RRT完全不同。它先在整个C空间里撒一堆点，然后连接相邻的点，形成一个“路图”。之后每次查询路径，就在这个路图上搜索。

你可以把PRM理解成“先修路，再导航”。RRT是“边探索边找路”，PRM是“先把地图画好，以后每次问路直接查地图”。

PRM分两个阶段：

• 学习阶段：随机采样N个点，连接每个点与其K个最近邻（如果边无碰撞），构建路图。
• 查询阶段：把起点和终点连到路图上，然后用Dijkstra或A*找最短路径。

// 伪代码：PRM学习阶段
for i = 1 to N:
    q = sample_free_configuration()
    roadmap.add_node(q)
    
for each node q in roadmap:
    neighbors = find_k_nearest_neighbors(q, K)
    for each neighbor n in neighbors:
        if is_collision_free(q, n):
            roadmap.add_edge(q, n)

LazyPRM是PRM的“偷懒”版本。它先不检查边的碰撞，直接建一个“全连接”的图。等到查询路径时，再逐步检查路径上的边是否可行。如果某条边撞了，就删掉它，重新找路。

为什么这样能快？因为碰撞检测是计算密集型的操作。LazyPRM把碰撞检测推迟到“不得不做”的时候，省掉了大量无效检测。

4.4 EST：扩展空间树

EST（Expansive Space Trees）可能不如RRT那么出名，但它在某些场景下非常有用。EST的核心思想是：在“有潜力”的区域多采样，在“没潜力”的区域少采样。

怎么判断“有潜力”？EST维护一个“权重”概念。如果一个节点周围有大量未被探索的空间，它的权重就高，被选中的概率就大。反之，如果一个节点已经被探索得差不多了，它的权重就低。

这就像你打扫房间——角落里的灰尘多，你就多扫几下；已经干净的地方，随便带过就行。

EST的扩展过程：

1. 根据权重选择一个已有节点 q。
2. 在 q 附近随机生成一个新节点 q_new。
3. 如果 q 到 q_new 的边无碰撞，加入树。
4. 更新 q 和 q_new 的权重。