简介：这份资源是一套基于ROS实现的RRT路径规划算法工程，面向正在学习机器人路径规划、ROS开发与C++编程的初学者及进阶开发者，可用于单障碍环境下的路径搜索与可视化验证。包内共9个文件，以4个cpp源文件与2个h头文件为核心，配合CMakeLists.txt、package.xml等构建配置，以及README.md说明文档，压缩包约11KB，结构精简、便于快速编译运行。算法提供ros_node与环境节点两个可执行文件，路径结果通过RVIZ以marker形式展示，需将固定框架设为/path_planner、标记主题设为path_planner_rrt，适合用来理解RRT采样、树扩展与避障规划的基本流程。目前已有2957人学习下载，读者可借此掌握ROS节点组织方式、C++实现细节与RVIZ可视化配置，并在此基础上扩展多障碍或三维场景，是入门路径规划与ROS工程实践的实用参考。
1. 从一条走不通的直线说起：RRT 到底在路径规划里扮演什么角色
移动机器人做自主导航，最朴素的想法是「起点到终点连一条直线，沿着走就行」。真放到带障碍物的地图里，这条直线大概率会撞墙，而且你没法用一条解析公式把所有可行绕行路线都写出来。这时候需要的是采样类规划算法，RRT（Rapidly-exploring Random Tree，快速扩展随机树）就是其中最常被拿来落地的一种。它不追求一次算出最优解，而是通过在空间里不断随机撒点、把树往空旷方向拽，快速找到一条从起点到终点的可行路径。ROS 里做路径规划，RRT 几乎是绕不开的入门算法，也是很多巡检、仓储、服务机器人项目里真实跑过的方案。这篇笔记就围绕「用 ROS 实现基于 RRT 的路径规划」这件事，把算法原理、节点搭建、参数调优和踩坑经验一次讲透，适合刚接触 ROS 导航、想自己动手写一个规划器的开发者。
RRT 的核心价值在于「概率完备」：只要路径存在，采样次数足够多，它总能找到。代价是路径通常不平滑、不最优，甚至有点「玄学」——同样的地图跑两次，结果可能不一样。理解这一点，后面调参和排错才不会钻牛角尖。下面从算法本身讲起，再落到 ROS 节点怎么写、怎么跑、怎么改。
2. RRT 算法原理与 ROS 落地选型：为什么不是 A*
2.1 RRT 的扩展逻辑与三个关键动作
RRT 的每一次迭代只做三件事：采样、找最近节点、向采样点方向扩展固定步长。伪代码层面非常短，但每一步都藏着影响最终效果的选择。
采样阶段，最常见的是在整个配置空间里均匀随机撒点。假设地图是 10m×10m，那采样点就在这个矩形里均匀分布。均匀采样的好处是实现简单、概率完备性有保证；坏处是当障碍物密集时，大量采样点落在障碍里，白白浪费计算。工程上常见的改进是目标偏置采样：以一定概率直接把终点当作采样点，让树更快朝目标方向生长。
找最近节点，就是遍历当前树里所有节点，算到采样点的欧氏距离，取最小的那个。这一步是 RRT 的性能瓶颈，节点数上千后每次迭代都要遍历，复杂度是 O(n)。常见优化是用 KD-Tree 加速最近邻查询，ROS 里可以用 nanoflann 这类库，但入门阶段先用暴力遍历也能跑。
扩展阶段，从最近节点朝采样点方向走固定步长 step_size，得到新节点。如果新节点和最近节点之间的连线不碰障碍，就把新节点加入树；否则丢弃这次采样。这个碰撞检测是 RRT 能不能用的关键，后面会专门讲。
提示：RRT 找到的是可行路径，不是最优路径。如果你需要最短路径，应该看 RRT* 或 Informed RRT*，它们通过重连和剪枝逐步优化，但计算量更大。
2.2 在 ROS 里为什么选 RRT 而不是 A*
A* 是栅格地图上的图搜索算法，能找到栅格分辨率下的最优路径，但它要求把整个地图离散成栅格，高维空间或连续空间下状态爆炸。RRT 不需要栅格化，直接在连续空间采样，天然适合高维配置空间，比如机械臂的关节空间规划。
那在 ROS 的二维导航场景里，A* 和 RRT 怎么选？如果你的地图是静态的、栅格化的，且对路径最优性有要求，A* 或 Dijkstra 更合适，ROS 的 global_planner 里就有现成实现。如果你面对的是高维空间、或者地图动态变化、或者你只是想快速验证一个规划思路，RRT 更灵活。实际项目里，很多团队用 A* 做全局规划，用 RRT 做局部避障或机械臂规划，两者并不互斥。
ROS 里实现 RRT，通常有两种落地方式。一种是写一个独立的规划节点，订阅地图和起终点，发布规划出的路径；另一种是把它做成 move_base 的全局规划插件，通过 pluginlib 加载。前者适合学习和快速验证，后者适合集成到现有导航栈。这篇笔记先走第一种，把算法跑通，再讲怎么改成插件。
2.3 最小可跑的 ROS RRT 节点结构
一个能跑的 RRT 规划节点，至少需要这几块：地图订阅、起终点获取、RRT 主循环、路径发布。地图用 nav_msgs/OccupancyGrid，起终点可以用 geometry_msgs/PoseStamped 通过话题传入，路径用 nav_msgs/Path 发布。
下面是一个最小节点的骨架，先看结构，后面再填算法细节。
#!/usr/bin/env python3 import rospy import numpy as np from nav_msgs.msg import OccupancyGrid, Path from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseStamped as Pose class RRTPlanner: def __init__(self): rospy.init_node('rrt_planner') # 订阅地图，只收一次 self.map_data = None rospy.Subscriber('/map', OccupancyGrid, self.map_callback) # 订阅起点和终点 self.start = None self.goal = None rospy.Subscriber('/start', PoseStamped, self.start_callback) rospy.Subscriber('/goal', PoseStamped, self.goal_callback) # 路径发布 self.path_pub = rospy.Publisher('/rrt_path', Path, queue_size=1) # 参数 self.step_size = rospy.get_param('~step_size', 0.5) self.max_iter = rospy.get_param('~max_iter', 5000) self.goal_bias = rospy.get_param('~goal_bias', 0.1) def map_callback(self, msg): self.map_data = msg def start_callback(self, msg): self.start = (msg.pose.position.x, msg.pose.position.y) def goal_callback(self, msg): self.goal = (msg.pose.position.x, msg.pose.position.y) def plan(self): # RRT 主循环，后面填充 pass if __name__ == '__main__': planner = RRTPlanner() rospy.spin()这段代码定义了节点的输入输出和参数入口。step_size 是每次扩展的步长，max_iter 是最大迭代次数，goal_bias 是目标偏置概率。这三个参数直接决定规划成功率和速度，后面会单独讲怎么调。地图回调只存不处理，是因为 RRT 规划是事件触发的，等起终点都到了再算。
3. 手写 RRT 核心循环：采样、碰撞检测与路径回溯
3.1 采样与最近邻查找的实现细节
采样函数要处理两件事：生成随机点，以及按概率决定是否直接用终点。最近邻查找在节点少时直接遍历，节点多时考虑加 KD-Tree。
import random import math def sample(self): # 以 goal_bias 概率直接返回终点，加速收敛 if random.random() < self.goal_bias: return self.goal # 否则在地图范围内均匀采样 x = random.uniform(self.map_info.origin.position.x, self.map_info.origin.position.x + self.map_info.width * self.map_info.resolution) y = random.uniform(self.map_info.origin.position.y, self.map_info.origin.position.y + self.map_info.height * self.map_info.resolution) return (x, y) def nearest(self, nodes, point): # 暴力遍历，节点数少时够用 min_dist = float('inf') nearest_node = None for node in nodes: d = math.hypot(node[0] - point[0], node[1] - point[1]) if d < min_dist: min_dist = d nearest_node = node return nearest_node采样范围必须严格限制在地图边界内，否则会采到地图外，碰撞检测时索引越界。goal_bias 一般设 0.05 到 0.2，太小收敛慢，太大容易在障碍物前反复撞墙。最近邻查找如果节点超过 2000，暴力遍历会明显拖慢规划，这时候用 scipy 的 cKDTree 或自己维护一个网格索引，能把查询降到 O(log n)。
3.2 碰撞检测：RRT 最容易翻车的一环
碰撞检测决定新节点能不能加入树。做法是把最近节点到新节点的线段离散成若干点，逐点查栅格值，只要有一个点落在障碍里就判定碰撞。
def collision_free(self, p1, p2): # 把线段按 resolution 离散 dist = math.hypot(p2[0] - p1[0], p2[1] - p1[1]) steps = int(dist / self.map_info.resolution) + 1 for i in range(steps + 1): t = i / steps if steps > 0 else 0 x = p1[0] + t * (p2[0] - p1[0]) y = p1[1] + t * (p2[1] - p1[1]) if not self.is_free(x, y): return False return True def is_free(self, x, y): # 世界坐标转栅格索引 mx = int((x - self.map_info.origin.position.x) / self.map_info.resolution) my = int((y - self.map_info.origin.position.y) / self.map_info.resolution) if mx < 0 or my < 0 or mx >= self.map_info.width or my >= self.map_info.height: return False index = my * self.map_info.width + mx # OccupancyGrid 中 0 表示空闲，100 表示占用，-1 表示未知 return self.map_data.data[index] == 0这里有几个容易忽略的点。离散步数用 resolution 而不是固定值，是为了保证不漏检细障碍。OccupancyGrid 的 data 是一维数组，索引是my * width + mx，行列别搞反。未知区域（-1）通常按占用处理，否则规划出的路径可能穿过未探索区域，实际跑起来撞墙。膨胀层的问题也要注意，如果地图已经做过膨胀，碰撞检测直接用即可；如果没膨胀，机器人有半径，需要在 is_free 里额外判断周围一圈。
3.3 主循环与路径回溯
主循环把采样、最近邻、扩展、碰撞检测串起来，找到终点附近节点后回溯路径。
def plan(self): if self.map_data is None or self.start is None or self.goal is None: return None nodes = [self.start] parents = {self.start: None} for _ in range(self.max_iter): rnd = self.sample() near = self.nearest(nodes, rnd) # 朝采样点方向走 step_size theta = math.atan2(rnd[1] - near[1], rnd[0] - near[0]) new = (near[0] + self.step_size * math.cos(theta), near[1] + self.step_size * math.sin(theta)) if not self.collision_free(near, new): continue nodes.append(new) parents[new] = near # 判断是否到达终点 if math.hypot(new[0] - self.goal[0], new[1] - self.goal[1]) < self.step_size: return self.backtrack(parents, new) return None def backtrack(self, parents, node): path = [] while node is not None: path.append(node) node = parents[node] path.reverse() return path主循环里判断到达终点用的是「新节点到终点距离小于 step_size」，而不是精确相等，因为随机采样几乎不可能正好落在终点上。回溯用 parents 字典从终点节点一路往回找，最后反转得到从起点到终点的顺序。路径发布前通常还要做一次简化，把不必要的中间点去掉，让路径更直。
4. 参数调优与路径平滑：让 RRT 从能跑到好用
4.1 step_size、max_iter、goal_bias 三个参数怎么设
这三个参数没有万能值，但有一套可操作的调整逻辑。
step_size 决定树的生长粒度。设太小，树长得慢，迭代次数飙升；设太大，容易在窄通道里撞墙，规划失败。经验值是地图最小通道宽度的一半左右。比如走廊宽 1m，step_size 设 0.3 到 0.5 比较稳。如果地图障碍物密集，先调小 step_size 保证成功率，再考虑优化速度。
max_iter 是安全阀。设太小，复杂地图规划不出来；设太大，规划失败时卡很久。一般先设 5000 到 10000，观察成功规划平均用多少次迭代，再留 3 到 5 倍余量。如果经常跑满 max_iter 还没找到，说明 step_size 或 goal_bias 有问题，别一味加迭代次数。
goal_bias 控制朝目标生长的倾向。0.1 是常见起点。地图空旷时可以降到 0.05，让树更均匀探索；障碍物多、目标在狭窄区域时，可以提到 0.2 甚至 0.3，但太高会导致树在障碍前反复尝试直线，反而降低效率。
| 参数 | 典型范围 | 调大影响 | 调小影响 |
|---|---|---|---|
| step_size | 0.2~1.0m | 生长快，窄通道易失败 | 成功率高，迭代多 |
| max_iter | 5000~20000 | 成功率高，失败耗时长 | 失败快，复杂图易放弃 |
| goal_bias | 0.05~0.3 | 收敛快，易局部卡死 | 探索均匀，收敛慢 |
4.2 路径平滑：把折线变成可跟踪的轨迹
RRT 原始路径是一堆折线，机器人跟踪时频繁急转，实际跑起来很别扭。常见做法是路径剪枝加插值。剪枝的思路是：从起点开始，尝试直接连到更远的路径点，如果连线无碰撞就跳过中间点。
def smooth_path(self, path): if len(path) <= 2: return path smoothed = [path[0]] i = 0 while i < len(path) - 1: j = len(path) - 1 # 从最远的点往回找，找到第一个能直连的 while j > i + 1: if self.collision_free(path[i], path[j]): break j -= 1 smoothed.append(path[j]) i = j return smoothed剪枝后再用样条或贝塞尔曲线插值，生成带朝向的平滑轨迹。如果只是给 move_base 用，剪枝后的折线通常就够了，因为局部规划器会做平滑。如果是机械臂或无人机，对轨迹连续性要求高，就得加样条插值，并检查插值后的曲线是否仍然无碰撞。
4.3 用 RViz 验证规划结果
规划节点跑起来后，在 RViz 里加 Path 显示，话题选 /rrt_path，就能看到路径。验证时重点看三件事：路径是否穿过障碍、路径是否绕远、起终点是否连上。如果路径穿障碍，多半是碰撞检测的离散步数不够或地图索引算错；如果绕远，是采样随机性导致，多跑几次或调 goal_bias；如果起终点没连上，检查坐标系是否一致，RRT 内部用的是世界坐标，RViz 显示可能用了别的 frame。
注意：RViz 里看到的路径是规划结果，不代表机器人能跟踪。实际部署前，一定要在仿真里让机器人沿路径走一遍，确认没有碰撞和剧烈转向。
5. 避坑与排查：RRT 落地时最容易踩的五个坑
5.1 路径穿墙：碰撞检测漏检
现象：RViz 里路径明显穿过障碍物，但规划节点没报错。
原因：碰撞检测离散步数按固定值而非 resolution 计算，线段太长时中间漏点；或者 OccupancyGrid 索引行列写反，查了错误栅格。
解决：离散步数用dist / resolution并加 1，确保每个栅格至少被查一次；索引统一用my * width + mx，写个单元测试验证几个已知坐标的栅格值。
5.2 规划失败但地图明明有路
现象：简单地图也经常跑满 max_iter 返回失败。
原因：goal_bias 设太高，树反复朝终点直线扩展，撞墙后没有足够随机探索；或者 step_size 太大，窄通道处新节点总是落在障碍里。
解决：先把 goal_bias 降到 0.05，step_size 降到通道宽度的一半，确认能规划出来后再逐步调回。如果还失败，打印每次迭代的采样点和扩展结果，看树是不是卡在某个区域。
5.3 规划时间忽长忽短
现象：同样地图，有时几十毫秒出结果，有时几秒。
原因：RRT 本身是随机算法，采样点分布不同导致迭代次数波动；最近邻查找用暴力遍历，节点多时每次迭代耗时增加。
解决：对时间敏感的场景，设一个规划超时，超时后返回当前最优或失败；节点超过 2000 时换 KD-Tree。如果波动仍然大，考虑用 RRT* 或加双向搜索，但那是另一个话题。
5.4 路径贴墙走
现象：路径能走通，但紧贴障碍物边缘，实际机器人容易擦碰。
原因：碰撞检测只判断栅格是否占用，没有考虑机器人半径；地图膨胀层不够。
解决：在地图层面做膨胀，膨胀半径至少等于机器人内切圆半径；或者在 is_free 里检查以该点为中心、机器人半径为边长的方形区域是否全空闲。后者计算量大，通常优先用地图膨胀。
5.5 坐标系不一致导致路径偏移
现象：RViz 里路径整体偏移一段距离，或者起点对不上。
原因：地图 origin 不是 (0,0)，而采样和碰撞检测时用了错误的原点；或者起终点话题的 frame_id 和地图 frame_id 不一致。
解决：所有世界坐标转换统一用 map_info.origin 计算，别假设原点在零；订阅起终点时检查 frame_id，必要时用 tf 转换到地图坐标系。
6. 进阶技巧：把 RRT 做成 move_base 插件与双向 RRT 提速
把 RRT 从独立节点改成 move_base 的全局规划插件，是集成到现有导航栈的关键一步。move_base 的全局规划插件需要继承nav_core::BaseGlobalPlanner，实现initialize和makePlan两个接口。initialize 里读参数、存代价地图指针；makePlan 里把代价地图转成 OccupancyGrid 格式，调用 RRT 主循环，把结果填进std::vector<geometry_msgs::PoseStamped>。代价地图的getCost返回 0 到 255，0 到 252 是空闲到致命障碍，判断空闲时用阈值比如 50 以下算可通行。插件写完后在move_base的 launch 文件里把base_global_planner参数设成你的插件名，编译时在 package.xml 和 CMakeLists.txt 里注册 pluginlib。
双向 RRT 是另一个实用提速手段。标准 RRT 从起点单向生长，双向 RRT 同时从起点和终点各长一棵树，每次迭代让两棵树交替扩展，当两棵树的节点距离小于 step_size 时拼接路径。实现上维护两个 nodes 列表和两个 parents 字典，扩展逻辑复用，只是采样时一棵树朝随机点、另一棵树朝对方最新节点。实测在窄通道地图里，双向 RRT 的迭代次数通常能降到单向的一半左右，代价是代码复杂度上升，路径拼接和碰撞检测要处理两棵树的边界情况。
我自己的习惯是：先用单向 RRT 把逻辑跑通，确认碰撞检测和坐标转换没问题，再换双向。因为双向 RRT 出问题时，排查难度翻倍——你分不清是采样问题还是拼接问题。另外，不管单向还是双向，规划完的路径一定要在仿真里让机器人实际走一遍，RViz 里看着没问题的路径，真跑起来可能因为局部规划器跟不上而画龙。这一步没有后悔药，只能老老实实仿真验证。希望帮到你。
本文还有配套的精品资源，点击获取
