简介：本资源面向机器人路径规划方向的学习者与开发者，提供一套基于RRT（快速扩展随机树）算法的C++完整实现，并结合MFC框架搭建可视化交互界面，帮助理解随机采样、最近邻搜索与路径收敛等核心机制。压缩包共49个文件，约32.78MB，以cpp、h源码文件承载算法与视图逻辑，配合sln、vcxproj等工程文件可直接编译运行，另有exe可执行程序、txt数据文件及日志、图标等辅助资源，目录结构清晰便于按模块查阅。目前已有689人学习下载。资源涵盖节点管理、障碍物与参考路径绘制、随机数生成、插值计算及图形实时更新等关键环节，读者可借此观察路径逐步优化并避开障碍物的完整过程，既能加深对RRT算法原理的理解，也能提升C++与MFC图形界面编程的实战能力。

1. 基于 RRT 算法的路径规划：从随机采样到可落地避障的完整拆解

很多人第一次接触路径规划，是从 A* 或 Dijkstra 开始的——栅格地图上跑一遍，路径又短又直，看着很舒服。但一旦把场景换成高维机械臂、连续状态空间或者障碍物形状不规则的场地，栅格法的内存和分辨率就开始翻车：栅格太粗会撞障碍，栅格太细算不动。RRT（Rapidly-exploring Random Tree，快速扩展随机树）就是为这类场景准备的备选方案，它不要求你把空间离散成格子，而是靠随机采样在连续空间里“长”出一棵树，直到某个节点摸到目标附近。这篇笔记围绕“基于 RRT 算法的路径规划”这个方向，把概率完备性、采样策略、碰撞检测、参数调优和工程落地路径讲清楚，适合正在做机器人导航、机械臂避障、游戏 AI 寻路，或者单纯想把 RRT 从论文公式变成能跑代码的从业者。下面所有代码都是最小可复现版本，不依赖特定仿真平台，Python 加 NumPy 就能跑。

2. RRT 到底在做什么：概率完备性与采样逻辑

2.1 为什么随机采样能保证找到路径

RRT 的核心假设很朴素：如果起点和终点之间存在一条可行路径，那么只要采样点足够多、覆盖足够广，树总会朝着空白区域生长，最终触达目标。这个性质叫概率完备性——不是“一定找到”，而是“采样次数趋于无穷时，找不到路径的概率趋于零”。它和 A* 的完备性有本质区别：A* 在有限栅格上一定找到最优路径，但栅格本身是对连续空间的近似；RRT 直接在连续空间工作，代价是路径不保证最优，甚至不保证每次都能在有限步内找到。

理解这一点很关键，因为它决定了你调参的方向。如果你追求“最短路径”，RRT 不是首选，应该看 RRT* 或 Informed RRT*；如果你追求“高维空间里快速找到一条可行路径”，RRT 的随机性反而是优势。我一般会把 RRT 用在机械臂关节空间规划上，因为关节空间维度高、障碍物映射复杂，栅格法基本没法用。

2.2 一棵树的生长过程拆成四步

把 RRT 拆开，其实只有四个动作循环执行：

1. 在状态空间里随机采一个点q_rand；
2. 在已有树中找到离q_rand最近的节点q_near；
3. 从q_near朝q_rand方向走一小步step_size，得到新节点q_new；
4. 检查q_near到q_new的连线是否碰撞，不碰撞就把q_new加入树，并记录父节点。

循环直到q_new与目标点的距离小于阈值，或者达到最大迭代次数。这里每一步都藏着参数：采样范围、最近邻搜索方式、步长、碰撞检测精度。下面先给一个二维平面的最小实现，把逻辑跑通，再逐项拆参数。

import numpy as np class RRT: def __init__(self, start, goal, bounds, step_size=0.5, goal_sample_rate=0.1, max_iter=5000): self.start = np.array(start, dtype=float) self.goal = np.array(goal, dtype=float) self.bounds = bounds # [(xmin,xmax),(ymin,ymax)] self.step_size = step_size self.goal_sample_rate = goal_sample_rate self.max_iter = max_iter self.nodes = [self.start] self.parents = {0: None} def sample(self): # 以一定概率直接采目标点，加速收敛 if np.random.rand() < self.goal_sample_rate: return self.goal.copy() return np.array([np.random.uniform(lo, hi) for lo, hi in self.bounds]) def nearest(self, q_rand): dists = [np.linalg.norm(q_rand - n) for n in self.nodes] return int(np.argmin(dists)) def steer(self, q_near, q_rand): direction = q_rand - q_near length = np.linalg.norm(direction) if length < 1e-9: return q_near.copy() return q_near + (direction / length) * min(self.step_size, length) def is_collision_free(self, q_from, q_to): # 实际项目替换为连续碰撞检测 return True def plan(self): for _ in range(self.max_iter): q_rand = self.sample() idx_near = self.nearest(q_rand) q_new = self.steer(self.nodes[idx_near], q_rand) if not self.is_collision_free(self.nodes[idx_near], q_new): continue self.nodes.append(q_new) self.parents[len(self.nodes) - 1] = idx_near if np.linalg.norm(q_new - self.goal) < self.step_size: return self.extract_path(len(self.nodes) - 1) return None def extract_path(self, leaf_idx): path = [] idx = leaf_idx while idx is not None: path.append(self.nodes[idx]) idx = self.parents[idx] return path[::-1]

这段代码里goal_sample_rate是第一个值得调的参数。纯随机采样时，树会均匀铺开，但目标附近采样密度不够，收敛慢。把 5% 到 15% 的采样直接指向目标，能明显加快找到路径的速度。代价是如果目标被障碍物包围，这部分采样会浪费，所以不要设太高。step_size决定树的精细程度：太小则迭代次数暴涨，太大则容易在狭窄通道里撞障碍。二维平面常用 0.3 到 1.0，机械臂关节空间要按关节弧度来定，一般 0.05 到 0.2 弧度比较稳。

2.3 碰撞检测才是真正的性能瓶颈

上面代码里is_collision_free直接返回 True，这是为了先跑通逻辑。真实项目里，碰撞检测往往占整个规划耗时的 70% 以上。常见做法是把q_from到q_to的线段按固定分辨率离散成若干点，逐点查障碍物。分辨率取step_size的十分之一到五分之一，太粗会漏检，太细会拖慢。

def is_collision_free(self, q_from, q_to, obstacles, resolution=0.05): dist = np.linalg.norm(q_to - q_from) n = max(int(dist / resolution), 1) for i in range(n + 1): q = q_from + (q_to - q_from) * (i / n) for obs in obstacles: if obs.contains(q): return False return True

resolution这个参数没有万能值。障碍物最小厚度如果是 0.1，分辨率至少要 0.05 才能保证不漏检。我一般会先量一下场景里最薄的障碍物，再取它的一半作为分辨率上限。如果性能吃紧，可以先用粗分辨率快速筛，再用细分辨率复核可疑段，这是常见的两级检测思路。

3. 把 RRT 跑起来：环境搭建、参数配置与可视化验证

3.1 最小依赖与目录结构

这套实现不需要 ROS、不需要仿真器，Python 3.8 以上加 NumPy 和 Matplotlib 就够。目录我一般这样放：

rrt_demo/ rrt.py # 算法主体 env.py # 障碍物定义与碰撞检测 run_demo.py # 入口，跑规划并画图

env.py里用圆形和矩形障碍物做示例，每个障碍物实现一个contains(point)方法。这样碰撞检测和算法解耦，换场景只改env.py。

import numpy as np class CircleObstacle: def __init__(self, center, radius): self.center = np.array(center, dtype=float) self.radius = radius def contains(self, point): return np.linalg.norm(point - self.center) <= self.radius class RectObstacle: def __init__(self, xmin, xmax, ymin, ymax): self.xmin, self.xmax = xmin, xmax self.ymin, self.ymax = ymin, ymax def contains(self, point): return (self.xmin <= point[0] <= self.xmax and self.ymin <= point[1] <= self.ymax)

3.2 入口脚本与可视化

run_demo.py负责组装场景、调用规划、画树和路径。可视化不是装饰，它是排查“为什么没找到路径”的第一手段。树如果只朝一个方向长，说明采样范围有问题；树如果密集堆在起点附近，说明步长太小或者碰撞检测太严。

import numpy as np import matplotlib.pyplot as plt from rrt import RRT from env import CircleObstacle, RectObstacle start = (0.5, 0.5) goal = (9.0, 9.0) bounds = [(0, 10), (0, 10)] obstacles = [ CircleObstacle((4, 4), 1.2), CircleObstacle((6, 7), 1.0), RectObstacle(2, 3, 6, 9), RectObstacle(7, 9, 2, 4), ] rrt = RRT(start, goal, bounds, step_size=0.6, goal_sample_rate=0.12, max_iter=8000) rrt.is_collision_free = lambda a, b: _collision_free(a, b, obstacles) path = rrt.plan() fig, ax = plt.subplots(figsize=(7, 7)) for obs in obstacles: if isinstance(obs, CircleObstacle): ax.add_patch(plt.Circle(obs.center, obs.radius, color='gray')) else: ax.add_patch(plt.Rectangle((obs.xmin, obs.ymin), obs.xmax - obs.xmin, obs.ymax - obs.ymin, color='gray')) for i, node in enumerate(rrt.nodes): p = rrt.parents[i] if p is not None: ax.plot([node[0], rrt.nodes[p][0]], [node[1], rrt.nodes[p][1]], color='lightblue', linewidth=0.5) if path: px = [p[0] for p in path] py = [p[1] for p in path] ax.plot(px, py, color='red', linewidth=2) ax.plot(start[0], start[1], 'go', markersize=10) ax.plot(goal[0], goal[1], 'bo', markersize=10) ax.set_xlim(0, 10); ax.set_ylim(0, 10) ax.set_aspect('equal') plt.show()

跑通之后你会看到一棵浅蓝色的树从起点蔓延，红色路径是最终结果。如果树长满了整个空间还没到目标，先看max_iter是不是太小，再看goal_sample_rate是不是太低。如果路径贴着障碍物边缘走，那是碰撞检测分辨率不够，把resolution调小。

3.3 三个必调参数与推荐范围

这张表不是拍脑袋来的，是我在不同场景里反复试出来的区间。机械臂关节空间的max_iter之所以要更大，是因为维度高、可行区域稀疏，树需要更多次尝试才能穿过去。如果你发现规划时间超过预期，优先降max_iter并观察失败率，而不是盲目加。

4. 避坑与排查：RRT 落地时最容易翻车的五个地方

4.1 现象：树一直长但永远到不了目标

原因通常有两个：一是goal_sample_rate太低，目标附近采样密度不够；二是目标点本身落在障碍物内部或者紧贴障碍物，导致最后一步碰撞检测永远不过。解决方法是先把目标点往外挪一点，确认目标可达；再把goal_sample_rate临时调到 0.3 观察是否能到，如果能到再逐步降回正常值。我一般会在规划前加一个断言：目标点必须不在任何障碍物内。

4.2 现象：路径贴着障碍物边缘，实际执行时撞了

这是碰撞检测分辨率不够的典型表现。线段离散时，如果分辨率大于障碍物最小厚度，中间某个点可能刚好跳过障碍物。解决方法是把resolution降到最薄障碍物厚度的一半以下。另一个容易忽略的点是机器人本身有体积，碰撞检测不能只查中心点，要把机器人半径膨胀到障碍物上，或者对线段上的每个姿态做完整外形检测。

4.3 现象：每次跑出来的路径都不一样，无法复现

RRT 本身是随机算法，路径不唯一是正常的。但如果连“能否找到路径”都不稳定，说明参数卡在临界值上。解决办法是固定随机种子，在调试阶段保证可复现；上线时再放开种子，用多次规划取最短路径。固定种子的方式很简单，在sample之前调用np.random.seed(42)，但要注意这会影响全局随机状态，生产环境慎用。

4.4 现象：高维空间里规划时间爆炸

维度一高，最近邻搜索就成了瓶颈。上面代码用的是线性扫描，节点数上万后每次找最近点都要遍历全树，耗时肉眼可见。常见做法是用 KD-Tree 加速，scipy.spatial.cKDTree可以直接用，每次插入新节点后重建或者用增量式结构。另一个方向是限制采样范围，不要在整个关节空间均匀采，而是根据任务先验缩小到可能区域。

4.5 现象：路径拐来拐去，执行时机器人抖得厉害

RRT 产出的路径是折线，节点之间方向突变，直接发给底层控制器会导致频繁加减速。这不是算法错误，而是缺少后处理。常见做法是路径剪枝加平滑：先做贪心剪枝，把能直线连通的中间节点删掉；再用样条或者梯度下降做平滑。剪枝的逻辑很简单，从起点开始，尽量找最远的能无碰撞直达的节点作为下一跳，重复到终点。

def prune_path(path, collision_free): if len(path) <= 2: return path pruned = [path[0]] i = 0 while i < len(path) - 1: j = len(path) - 1 while j > i + 1: if collision_free(path[i], path[j]): break j -= 1 pruned.append(path[j]) i = j return pruned

这个剪枝是 O(n²) 的，路径节点不多时够用。剪枝后路径会明显变直，但仍然是折线，需要再上平滑。平滑不属于 RRT 本身，但工程落地基本绕不开。

5. 从 RRT 到 RRT*：什么时候值得升级，怎么验证效果

RRT 找到的是可行路径，不是最优路径。如果你对路径长度有要求，比如移动机器人要省电、机械臂要减少关节磨损，那就该看 RRT*。RRT* 在 RRT 基础上加了两步：一是新节点加入后，在邻域内重新选择父节点，看能否降低从起点到新节点的代价；二是对新节点的邻域做重连，把原本绕远的节点接到新节点上。这两步让 RRT* 具备渐近最优性——采样越多，路径越接近最优。

升级的代价是计算量。每次插入节点都要查邻域、算代价、做重连，耗时比 RRT 高不少。我的经验是：如果场景简单、路径长度差个百分之二三十无所谓，RRT 够用；如果场景复杂且路径质量直接影响任务成败，再上 RRT*。验证效果的方法很直接：固定同一张地图和同一组参数，各跑 50 次，统计成功率和平均路径长度。下面是一个简单的统计脚本框架。

import numpy as np def benchmark(planner_cls, env, runs=50): success = 0 lengths = [] for seed in range(runs): np.random.seed(seed) planner = planner_cls(env) path = planner.plan() if path: success += 1 length = sum(np.linalg.norm(path[i+1] - path[i]) for i in range(len(path) - 1)) lengths.append(length) return { 'success_rate': success / runs, 'avg_length': np.mean(lengths) if lengths else None, 'std_length': np.std(lengths) if lengths else None, }

跑完对比 RRT 和 RRT* 的success_rate和avg_length，你就能判断升级是否值得。注意 RRT* 的max_iter通常要设得更大，否则渐近最优性体现不出来。另一个容易踩的坑是邻域半径的选择：半径太小，重连几乎不发生，退化成 RRT；半径太大，每次迭代计算量飙升。常见做法是按节点数和空间体积动态计算，比如radius = min(gamma * (log(n)/n)^(1/d), step_size * k)，其中d是空间维度，gamma和k需要按场景调。

最后说一个我自己的习惯：不管用 RRT 还是 RRT*，我都会先把碰撞检测单独测一遍，用已知会碰和已知不会碰的线段各跑一百次，确认没有漏检和误检，再去调规划参数。因为路径规划里最隐蔽的 bug 往往不在算法本身，而在碰撞检测这个黑匣子里。希望帮到你。

本文还有配套的精品资源，点击获取