简介基于RRTRapidly-exploring Random Trees算法的路径规划源码包主要面向机器人路径规划学习者与C/MFC开发者解决高维配置空间下的随机搜索与避障路径规划问题。压缩包共49个文件包含C源码cpp/h、MFC工程文件dsp/dsw/vcxproj/sln、界面资源rc/ico/res及调试生成文件obj/pdb/exe等整体约32.78MB适合在Visual Studio环境中直接打开工程查看与运行。已有688人学习下载。资源中不仅有RRT算法核心类和多边形障碍物绘制逻辑还提供了基于MFC的交互界面用户可实时添加障碍物与参考路径直观观察随机树扩展、最近邻搜索与路径生成过程。通过阅读源码和注释可以同时掌握RRT算法原理、C工程组织方式以及MFC图形界面与事件处理流程是一份便于动手实践与二次开发的完整示例。 我第一次在项目里跑通RRT路径规划时脑子里只有一个念头这算法也太笨了吧。不做全局建模、不指望最优解就是漫天撒点、往树上长枝条最后长出来一棵歪七扭八的藤蔓可偏偏就是这棵藤蔓在很多把A*和势场法卡死的环境里稳稳地给机器人找到了一条能走的路。后来做毕设、带课程设计、帮同事救火凡是和基于rrt算法的路径规划沾边的压缩包解压之后几乎都是同一套流程二值化地图、RRT主循环、碰撞检测、路径可视化。每个包都能跑但每个包里踩过的坑注释里一句都没写。这篇就把这些没写的东西全部翻出来从原理到代码从调参到排错一条条讲清楚。1. 为什么路径规划这么难RRT却能乱拳打死老师傅路径规划的本质一句话就能说清给定起点和终点在布满障碍物的空间里找一条不撞墙的路线。听起来简单真正的难点藏在维度这两个字里。二维栅格地图上Dijkstra和A*表现很漂亮逐个格子搜索只要地图不大效率高、路径也够好。但问题在于栅格法的计算量和空间维度是指数关系这也就是常说的维度诅咒。一旦把问题换成六自由度机械臂或者无人机在三维空间带偏航角规划网格数量瞬间爆炸再大的内存也装不下完整的搜索空间。势场法倒是能绕开网格可它天生容易陷入局部极小值一个常见的翻车场面是目标点在障碍物正后方引力把机器人往墙上摁斥力又把它往外推最后机器人在墙面前疯狂抖动就是过不去。RRTRapidly-exploring Random Tree快速扩展随机树的思路和这两类方法完全不同。它不试图摸清整个空间而是从起点开始长一棵树每次随机往地图里撒一个点找到树上离这个点最近的节点朝着这个方向生长一小段固定长度只要这一小段没有碰到障碍物就把它作为新节点接在树上。这样反复迭代树就一步步向外蚕食自由空间直到某一棵树梢长到了目标点附近于是反向回溯得到一条从起点到目标的路径。这里最关键的思想转变是它把路径搜索从有序遍历换成了概率采样。代价是不再保证最优性换来的是对高维空间的适应能力。从理论性质上说RRT是概率完备的——当采样次数趋于无穷时找到可行路径的概率趋近于1但它不是最优的找到的第一条路往往弯弯绕绕离最短路径差了十万八千里。这两句话建议所有打开源码的人先写在注释里后面调参会用到。什么场景适合RRT也要先有个判断场景适合程度原因二维简单地图一般A*更快更优RRT属于降维打击的对手高维空间机械臂、无人机很合适采样不受维度暴涨影响狭窄通道地图看运气纯随机采样很难碰进窄缝需要变体要求路径平滑的移动机器人需要后处理原始RRT输出的是折线后续必须平滑很多刚接触RRT的人有个误区觉得它啥都能干直接拿它跑室内巡逻车结果被一段走廊、一扇门折腾得够呛。实际上RRT最典型的应用场景就是空间复杂度高但不需要最优、只需要可行且足够快的场合比如无人机在线重规划、机械臂避障、野外环境搜索这类场合里A*的心有余而力不足恰恰是RRT的主场。2. 把RRT讲成人话从随机撒点到可行路径的完整链路先用一个生活化的场景建立直觉。想象你在一个没有灯光的巨大仓库里手里拿着一个很长的卷尺你站在门口起点想去仓库另一侧的后门目标点但仓库里堆满了货架你完全看不见路。你的策略是把卷尺朝一个随机方向甩出固定长度的一段打开手电筒检查这一段有没有碰到货架——没有就走过去再换个方向甩下一段碰到了就换方向重新甩。甩了很多很多次之后你发现自己离后门很近了于是加速朝后门甩最终抵达。你走过的这些折线连起来就是RRT找到的路径。当然真正的RRT比这个例子聪明一点它不是只有一根卷尺而是一棵树每个分支都在独立生长谁先摸到终点谁就是赢家。算法的核心流程可以拆成五个步骤初始化把起点作为树的根节点。随机采样在地图范围内随机生成一个点这里有一个工程小技巧——以一定概率直接采样目标点而不是全图乱撒目的后面细说。寻找最近节点遍历当前树上的所有节点找到离采样点最近的那个。扩展新节点从最近节点朝采样点方向走一个固定步长这就是新节点的位置。碰撞检测检查最近节点到新节点这一段路径是否碰到障碍物。没碰把新节点插入树中碰了丢弃重复第2步。伪代码写出来就长这样function RRT(start, goal, map): tree [start] for iter 1 to max_iter: sample random_point(goal, goal_sample_rate) nearest find_nearest_node(tree, sample) new_node steer(nearest, sample, step_size) if collision_free(nearest, new_node, map): new_node.parent nearest tree.append(new_node) if distance(new_node, goal) goal_threshold: return build_path(new_node) return None # 达到最大迭代仍无解这套流程看起来简单真正决定算法能不能用、快不快的是下面这几个参数参数含义典型取值影响step_size每次生长的步长地图尺寸的1%~5%步长太大容易穿过窄通道太小则迭代次数暴涨goal_sample_rate直接采样目标点的概率0.05~0.1越大树越直奔目标但容易被障碍物挡死max_iter最大迭代次数500~5000越大越可能找到路径但耗时线性增长goal_threshold判定够到终点的距离1~2倍步长太大路径终点离目标很远太小可能永远够不着goal_sample_rate这个参数值得单独拿出来说。它的存在完全是个工程妥协纯随机采样虽然理论上完备但树会把大量计算浪费在朝错误方向生长上尤其当目标点在一大块空地的对角时纯随机可能要几千次迭代才能碰巧靠近终点。每次采样时以5%~10%的概率直接选目标点树就会被引导着优先朝终点方向长。但这里有个陷阱——概率不能设太高因为如果目标点正后方压着一堵墙高概率的目标偏置会让树的生长方向一直被墙挡住反而陷入低效循环。我一般把它压在0.05到0.1之间效果最稳。碰撞检测是整个算法最容易被忽略、又最容易出错的地方。它做的事是判断最近节点到新节点这一段线段是否穿过障碍物。在地图是栅格图像的情况下通用的做法是沿着线段按固定分辨率取一系列离散点逐一检查这些点在地图上对应的栅格是不是障碍物。这个分辨率怎么选直接影响正确性我后面在踩坑部分展开讲。3. 项目实战代码结构、核心函数与调参笔记网上流传的rrt路径规划压缩包解压之后结构其实大差不差一般长这样rrt_path_planning/ ├── main.py # 入口建图、跑算法、可视化 ├── rrt.py # RRT核心类 ├── map_env.py # 地图构建与可视化辅助 ├── config.py # 参数配置集中管理 └── output/ └── path_result.png一个值得借鉴的工程习惯是把参数单独放一个配置文件而不是散落在代码里。原因是RRT算法的表现对参数极其敏感集中管理之后跑实验时只需要改config不用动逻辑代码调参效率高很多。这里给出一份基于Python和matplotlib的RRT核心实现你可以直接照着改成自己的版本import numpy as np class Node: def __init__(self, x, y): self.x x self.y y self.parent None class RRT: def __init__(self, start, goal, obstacle_map, config): self.start Node(start[0], start[1]) self.goal Node(goal[0], goal[1]) self.obstacle_map obstacle_map # 二值化地图1为障碍 self.config config self.node_list [self.start] def random_sample(self): # 目标偏置以一定概率直接采目标点加速收敛 if np.random.random() self.config[goal_sample_rate]: return np.array([self.goal.x, self.goal.y]) return np.array([ np.random.uniform(0, self.obstacle_map.shape[1]), np.random.uniform(0, self.obstacle_map.shape[0]) ]) def nearest_node_index(self, sample): dists [ (node.x - sample[0]) ** 2 (node.y - sample[1]) ** 2 for node in self.node_list ] return int(np.argmin(dists)) def steer(self, from_node, sample): dx sample[0] - from_node.x dy sample[1] - from_node.y dist np.hypot(dx, dy) if dist self.config[step_size]: return Node(sample[0], sample[1]) theta np.arctan2(dy, dx) new_x from_node.x self.config[step_size] * np.cos(theta) new_y from_node.y self.config[step_size] * np.sin(theta) return Node(new_x, new_y) def collision_free(self, from_node, to_node): # 沿线段离散采样逐个检查栅格 dx to_node.x - from_node.x dy to_node.y - from_node.y dist np.hypot(dx, dy) if dist 1e-6: return False step self.config[collision_check_resolution] n_steps int(np.ceil(dist / step)) for i in range(1, n_steps 1): t i / n_steps x int(round(from_node.x dx * t)) y int(round(from_node.y dy * t)) if not self._in_bounds(x, y): return False if self.obstacle_map[y, x] 1: return False return True def _in_bounds(self, x, y): h, w self.obstacle_map.shape return 0 x w and 0 y h def plan(self): for _ in range(self.config[max_iter]): sample self.random_sample() idx self.nearest_node_index(sample) nearest self.node_list[idx] new_node self.steer(nearest, sample) if not self.collision_free(nearest, new_node): continue new_node.parent idx self.node_list.append(new_node) dist_to_goal np.hypot( new_node.x - self.goal.x, new_node.y - self.goal.y ) if dist_to_goal self.config[goal_threshold]: return self._build_path(new_node) return None def _build_path(self, node): path [] while node is not None: path.append((node.x, node.y)) node self.node_list[node.parent] if node.parent is not None else None return path[::-1]这段代码里有两个细节值得强调。一是steer函数里判断了采样点距离当前节点不足一步的情况此时直接返回采样点本身避免在一个点附近反复打转。二是collision_free里的离散化采样它决定了碰撞检测的精度这里的参数选择和step_size的匹配关系会在下一节的坑里详细说。实际跑起来可视化是必不可少的。用matplotlib画地图、画树、画路径实时刷新能看到树的生长过程这对理解算法行为非常有帮助。我建议你把树画成浅灰色细线路径画成红色粗线障碍物画成黑色块这样一眼就能看出算法在哪些区域浪费了大量采样。调参方面我的经验是先固定step_size再调goal_sample_rate最后确定max_iter。step_size的基准值取地图短边长度的2%到3%比如一张500x500像素的地图取10到15像素。太小树长得太慢太大树会横跳窄通道基本穿不过去。goal_sample_rate从0.05开始调如果发现树在空地上磨蹭太久就适当提高到0.08如果发现树老往障碍墙上怼就降回0.03。max_iter先给一个较大的值比如5000跑通再慢慢往下压压到恰好能在绝大多数测试地图上成功为止这个值就是该场景下的性能甜点。4. 仿真跑通之后必须面对的四个坑4.1 采样点落在障碍物内部或地图边界外这是最容易踩的第一个坑表现是树生长到一定阶段后新节点频繁被丢弃算法效率断崖式下降。原因很直接random_sample在全图范围内均匀撒点完全没有考虑地图边界和障碍物——很多点直接撒到了障碍物内部甚至地图外面后续的碰撞检测必然失败这些迭代全部浪费。排查这类问题第一步看可视化里的树形结构如果树的分支大量集中在障碍物轮廓附近反复撞墙基本就是采样环节没设过滤。解决办法也很简单在random_sample里加一个拒绝采样生成随机点后先检查对应栅格是否为空闲不是则重新采样。这个检查带来的开销很小收益却很明显能让有效迭代率提高一大截。4.2 碰撞检测分辨率与步长不匹配导致的穿墙问题这个坑最隐蔽因为代码看起来完全正确但路径就是会莫名其妙穿过障碍物的边角。根本原因在于碰撞检测的离散化采样点没有和步长形成配合。假设step_size设成0.5而collision_check_resolution设成0.8那么从最近节点到新节点这一段检测点只在0和0.8两个位置取样中间有一段0.3的距离完全没有被检查如果障碍物的尖角恰好藏在这段空隙里路径就直接穿墙了。解决思路是让采样间距小于等于步长的一半。我在工程上一律取碰撞检测分辨率为步长的五分之一到十分之一这样既保证不穿墙又不至于因为采样点过密拖慢速度。如果你用的地图是栅格图像另一个更稳妥的做法是直接用Bresenham直线算法逐格扫描线段经过的所有栅格而不是均匀插值采样它能保证不会漏掉任何一格。4.3 终点被障碍物包围时算法看似死机第三个坑的表现是程序跑起来以后CPU狂转但树就是不长到终点附近直到max_iter耗尽输出None。刚接触RRT的同学第一反应是代码写错了反复检查逻辑却没想过还有一种可能——环境本身无解终点被障碍物完全围死或者终点栅格本身就是障碍物占据的。这种问题要从两个方向下手。首先代码层面要加一个最近节点是否在持续逼近终点的判断以及max_iter的显式提示迭代结束还没找到路径时明确输出未找到可行路径请检查地图或参数而不是返回一个空数组。其次地图构建时要对终点格做合法性校验确认它是空闲栅格且周围存在至少一个可达的空闲连通域。这两种处理加在一起能帮你区分算法有问题和环境无解两种情况省去大量无意义的debug时间。4.4 原始RRT路径是折线机器人根本没法直接跟踪第四个坑是跑通算法之后才暴露的。RRT找出来的路径是一长串节点连成的折线转角处经常出现近乎直角的急转差速驱动机器人勉强能原地转弯凑合阿克曼转向的车辆底盘直接歇菜。更麻烦的是节点之间可能还残留着大量冗余的绕路段。这时必须做后处理业内最常用的两个操作是路径剪枝和平滑。路径剪枝的贪心思路很直观从起点开始尝试直接连接更远的节点如果连线不碰障碍物就跳过中间的所有节点一直尝试到终点得到一条节点数量大幅减少的折线。剪枝之后再做平滑常用的有三次样条插值或者贝塞尔曲线拟合让路径曲率连续机器人跟踪起来才不会一顿一顿的。做完这两步RRT出来的路径才真正具备落地价值。5. 从RRT到RRT*我的优化路线与扩展思路RRT基本版跑通只是走出了第一步。真正让我觉得这个算法家族有意思的是从RRT到RRT的进化。RRT在每次插入新节点之后多了一个rewire重连操作它不只是检查新节点能不能接在最近的邻居上而是搜索一个半径范围内的所有邻近节点从中挑选代价值最小的作为父节点同时尝试把邻近节点的父节点改接到新节点上如果这样能缩短它们的累计代价。这个操作的直接效果是即使一开始找到了一条可行路径树也会继续生长和优化路径的总长会随着迭代次数增加不断收敛到渐近最优。代价是每次插入新节点都要做邻近搜索和重连计算单次迭代的开销比RRT大得多。我个人的看法是如果应用场景对路径长度有要求比如移动机器人续航有限RRT*值得这个开销如果只是应急避险、快速出一条可行路线原始RRT加上剪枝平滑反而更实用。另一个在工程中高频出现的变体是RRT-Connect它同时从起点和终点长两棵树交替扩展每次扩展后再尝试直接把两棵树的最近节点连接起来。这个方案在窄通道环境里表现尤其突出因为两棵树从两端同时往中间夹击比单棵树从一端硬闯的效率高太多。我实测在同样一张迷宫地图上RRT-Connect找到路径的耗时大约是原始RRT的三分之一。再往后扩展就得考虑机器人的运动学约束了。原始RRT的steer是直线段但一辆车不能原地转弯、一架无人机不能瞬掉头于是就有了基于Dubins曲线或Reeds-Shepp曲线的扩展方式——把steer操作从走直线换成按车辆最小转弯半径画弧线。想做泊车路径规划这方面的内容这个方向几乎是必经之路。最近还经常看到有人拿强化学习和RRT类方法做对比我觉得这俩其实不在一个赛道上。强化学习适合环境动态变化、需要实时重规划的交互场景但它训练成本高、可解释性差换个环境基本要从头训RRT类方法没有训练过程换地图立刻能用行为逻辑清楚出问题也好排查。实践中更聪明的做法是让它们互补全局规划用RRT给一条参考路径局部动态避障用强化学习或者DWA这类反应式规划器去应对突发障碍各管一段各发挥各的长处。拿我这个项目来说二维栅格地图只是起点。同样的RRT框架把节点从二维坐标改成三维坐标碰撞检测从二维栅格换成八叉树或者球体模型就能直接扩展到无人机三维路径规划。再把地图换成ROS里的costmap把RRT*封装成ROS的global_planner插件就能接到move_base导航栈里跑真实机器人。这个扩展路线我建议所有做完RRT项目的人都走一遍每一步踩的坑都会让你对路径规划的理解更深一层。最后一个实际操作层面的建议跑任何RRT变体之前先把随机种子固定下来这样每次实验的可视化结果可复现对比参数优劣时才有说服力。我在调参阶段靠这一招省了至少一半的无效对比时间。本文还有配套的精品资源点击获取
