简介本资源面向机器人路径规划方向的学习者与开发者提供一套基于RRT快速扩展随机树算法的C完整实现并结合MFC框架搭建可视化交互界面帮助理解随机采样、最近邻搜索与路径收敛等核心机制。压缩包共49个文件约32.78MB以cpp、h源码文件承载算法与视图逻辑配合sln、vcxproj等工程文件可直接编译运行另有exe可执行程序、txt数据文件及日志、图标等辅助资源目录结构清晰便于按模块查阅。目前已有689人学习下载。资源涵盖节点管理、障碍物与参考路径绘制、随机数生成、插值计算及图形实时更新等关键环节读者可借此观察路径逐步优化并避开障碍物的完整过程既能加深对RRT算法原理的理解也能提升C与MFC图形界面编程的实战能力。1. 基于 RRT 算法的路径规划从随机采样到可落地避障的完整拆解很多人第一次接触路径规划是从 A* 或 Dijkstra 开始的——栅格地图上跑一遍路径又短又直看着很舒服。但一旦把场景换成高维机械臂、连续状态空间或者障碍物形状不规则的场地栅格法的内存和分辨率就开始翻车栅格太粗会撞障碍栅格太细算不动。RRTRapidly-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 拆开其实只有四个动作循环执行在状态空间里随机采一个点q_rand在已有树中找到离q_rand最近的节点q_near从q_near朝q_rand方向走一小步step_size得到新节点q_new检查q_near到q_new的连线是否碰撞不碰撞就把q_new加入树并记录父节点。循环直到q_new与目标点的距离小于阈值或者达到最大迭代次数。这里每一步都藏着参数采样范围、最近邻搜索方式、步长、碰撞检测精度。下面先给一个二维平面的最小实现把逻辑跑通再逐项拆参数。import numpy as np class RRT: def __init__(self, start, goal, bounds, step_size0.5, goal_sample_rate0.1, max_iter5000): self.start np.array(start, dtypefloat) self.goal np.array(goal, dtypefloat) 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, resolution0.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 Trueresolution这个参数没有万能值。障碍物最小厚度如果是 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, dtypefloat) 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_size0.6, goal_sample_rate0.12, max_iter8000) 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, colorgray)) else: ax.add_patch(plt.Rectangle((obs.xmin, obs.ymin), obs.xmax - obs.xmin, obs.ymax - obs.ymin, colorgray)) 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]], colorlightblue, linewidth0.5) if path: px [p[0] for p in path] py [p[1] for p in path] ax.plot(px, py, colorred, linewidth2) ax.plot(start[0], start[1], go, markersize10) ax.plot(goal[0], goal[1], bo, markersize10) ax.set_xlim(0, 10); ax.set_ylim(0, 10) ax.set_aspect(equal) plt.show()跑通之后你会看到一棵浅蓝色的树从起点蔓延红色路径是最终结果。如果树长满了整个空间还没到目标先看max_iter是不是太小再看goal_sample_rate是不是太低。如果路径贴着障碍物边缘走那是碰撞检测分辨率不够把resolution调小。3.3 三个必调参数与推荐范围参数作用二维平面推荐机械臂关节空间推荐调大后果调小后果step_size每次扩展步长0.3~1.00.05~0.2 rad狭窄通道易碰撞迭代次数暴涨goal_sample_rate目标偏置采样概率0.05~0.150.05~0.1目标被围时浪费采样收敛慢max_iter最大迭代次数5000~1000010000~50000耗时增加可能提前失败这张表不是拍脑袋来的是我在不同场景里反复试出来的区间。机械臂关节空间的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, runs50): 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[i1] - 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 往往不在算法本身而在碰撞检测这个黑匣子里。希望帮到你。本文还有配套的精品资源点击获取