4、采样规划算法：RRT与RRT*算法原理、概率完备性分析、在三维空间中的实现与优化
各位同学，欢迎来到第四章。今天我们来聊聊采样规划算法。说实话，在路径规划这个领域里，RRT（Rapidly-exploring Random Tree）和它的升级版RRT*，是我个人用得最多的算法之一。为什么？因为它们简单、实用，而且在高维空间里表现特别好。
你想想看，传统的规划方法比如A*，在三维甚至更高维的空间里，网格数量会爆炸式增长。而RRT这类采样方法，说白了就是「用随机采样来探索空间」，避免了显式构建整个状态空间。嗯，这里有个关键点——它不保证找到最优解，但保证能找到解（如果存在的话）。这就是所谓的概率完备性。
4.1 RRT算法原理
RRT的核心思想其实很朴素：从起点开始，随机撒点，然后一步步把树「长」到目标区域。我刚开始学的时候，觉得这玩意儿跟随机游走差不多，后来才发现它巧妙得很。
基本流程是这样的：
- 初始化：以起点作为树的根节点。
- 随机采样：在状态空间中随机选一个点 \( x_{rand} \)。
- 找最近邻：在树上找到离 \( x_{rand} \) 最近的节点 \( x_{near} \)。
- 扩展：从 \( x_{near} \) 向 \( x_{rand} \) 方向走一步（步长 \(\delta\)），得到新节点 \( x_{new} \)。
- 碰撞检测：检查 \( x_{near} \) 到 \( x_{new} \) 的路径是否与障碍物碰撞。没碰？那就把 \( x_{new} \) 加入树中。
- 终止判断：如果 \( x_{new} \) 离目标点足够近，或者采样次数达到上限，就结束。
这里有个细节：步长 \(\delta\) 的选择很关键。我在项目中遇到过，步长太大容易跳过狭窄通道，步长太小又导致树生长太慢。一般取环境特征尺寸的5%-10%比较稳妥。
核心要点：RRT的随机性保证了它能探索整个空间，但代价是路径通常不是最优的——弯弯绕绕，甚至可能绕远路。
4.2 RRT*算法：向最优性迈进
RRT* 是 RRT 的改进版，由 Karaman 和 Frazzoli 在2011年提出。它解决了RRT的一个大问题——路径质量。说白了，RRT* 在扩展树的时候，不仅考虑「能不能连上」，还考虑「怎么连更划算」。
RRT* 相比 RRT 多了两个关键步骤：
- 重新选择父节点（Rewire）：新节点 \( x_{new} \) 生成后，不是直接连到最近的节点，而是在一个半径 \( r \) 的邻域内，找一条从起点到 \( x_{new} \) 代价最小的路径。这个代价通常是路径长度。
- 重新布线（Reconnect）：检查邻域内的其他节点，看看能不能通过 \( x_{new} \) 让它们的路径更短。如果能，就更新父节点。
我曾经在无人机三维避障项目里对比过RRT和RRT*。同样的环境，RRT跑了5000次采样找到一条可行路径，但路径长度是32米；RRT*跑了8000次采样，路径长度只有24米，而且更平滑。代价就是计算时间多了大概40%。
个人经验：如果你的应用对实时性要求极高（比如毫秒级决策），RRT可能更合适。如果允许几秒钟的规划时间，RRT* 绝对值得一试。
4.3 概率完备性分析
「概率完备性」这个词听起来高大上，其实意思很简单：只要采样次数足够多，找到解的概率趋近于1。RRT和RRT*都满足这个性质。
为什么会这样？因为RRT的采样是均匀覆盖整个状态空间的。随着采样点增多，树会越来越密集地覆盖自由空间。只要存在一条可行路径，树最终一定会「碰到」它。嗯，这里要注意一个前提——路径必须是在自由空间内连续且可通行的。
但概率完备性不保证「最优性」。RRT* 额外保证了渐进最优性：随着采样次数趋于无穷，找到的路径会收敛到全局最优解。这个性质在实际工程中很有用——你可以在有限时间内得到一个「足够好」的解，然后继续优化。
| 性质 | RRT | RRT* |
|---|---|---|
| 概率完备性 | ✅ 是 | ✅ 是 |
| 渐进最优性 | ❌ 否 | ✅ 是 |
| 计算效率 | 高 | 中等 |
| 路径质量 | 一般 | 好 |
4.4 三维空间中的实现与优化
把RRT/RRT*从二维扩展到三维，其实没有本质区别。状态空间从 \((x, y)\) 变成 \((x, y, z)\)，碰撞检测从二维多边形变成三维多面体。但有几个坑需要注意：
- 维度灾难：三维空间体积是二维的立方，同样的采样密度需要更多点。我建议采样次数至少是二维的3-5倍。
- 最近邻搜索：三维空间里用KD-Tree比暴力搜索快得多。我习惯用nanoflann库，轻量又好用。
- 步长调整：三维环境下，步长可以适当放大，因为障碍物通常更稀疏。但遇到密集障碍区（比如室内），步长要缩小。
下面是一个三维RRT*的核心代码片段，我用C++写的，注释里标了关键点：
// 三维RRT*核心扩展函数
bool RRTStar3D::extend(const Vector3d& sample) {
// 1. 找最近邻
Node* nearest = kd_tree->nearest(sample);
// 2. 沿方向步进
Vector3d dir = (sample - nearest->pos).normalized();
Vector3d new_pos = nearest->pos + step_size * dir;
// 3. 碰撞检测（三维AABB检测）
if (!collision_check(nearest->pos, new_pos)) return false;
// 4. 创建新节点
Node* new_node = new Node(new_pos);
// 5. RRT*核心：重新选择父节点
double best_cost = nearest->cost + dist(nearest->pos, new_pos);
Node* best_parent = nearest;
// 在半径r内搜索候选父节点
auto candidates = kd_tree->radius_search(new_pos, rewire_radius);
for (auto* cand : candidates) {
double cand_cost = cand->cost + dist(cand->pos, new_pos);
if (cand_cost < best_cost && collision_check(cand->pos, new_pos)) {
best_cost = cand_cost;
best_parent = cand;
}
}
// 6. 连接新节点
new_node->parent = best_parent;
new_node->cost = best_cost;
tree.push_back(new_node);
kd_tree->insert(new_node);
// 7. 重新布线：检查邻域内其他节点能否通过新节点更优
for (auto* node : candidates) {
double new_cost = new_node->cost + dist(new_node->pos, node->pos);
if (new_cost < node->cost && collision_check(new_node->pos, node->pos)) {
node->parent = new_node;
node->cost = new_cost;
}
}
return true;
}
避坑指南：我曾经在三维RRT*里忘了更新子节点的代价，结果路径虽然短了，但树结构乱了。重新布线后，一定要递归更新所有受影响节点的cost值，否则后续规划会出错。
4.5 优化技巧与工程实践
在实际项目中，我总结了几条优化RRT/RRT*的经验：
- 偏向采样：以一定概率（比如5%-10%）直接采样目标点，能加速收敛。这叫Goal Bias，简单有效。
- 路径平滑：RRT*找到的路径虽然短，但可能还是锯齿状。我习惯用B样条曲线做后处理平滑。
- 并行化：多棵树同时生长（比如双向RRT），在三维空间里能显著提速。我做过测试，双向RRT*比单向快2-3倍。
- 自适应步长：在障碍物密集区域用小步长，空旷区域用大步长。可以用环境密度估计来动态调整。
最后，我画了一张图来总结本章的知识体系。这张图展示了RRT和RRT*的核心流程、概率完备性的逻辑，以及三维实现中的关键优化点。
好了，这一章的内容就到这里。RRT和RRT*是采样规划里的经典方法，理解它们的原理和优化技巧，对后续学习更高级的规划算法（比如PRM、Informed RRT*）会很有帮助。记住，工程实践里没有银弹——根据你的场景选择合适的算法，才是最重要的。