RRT算法在ROS中的实现：从插件开发到参数调优的完整指南

简介：这份资源面向在ROS环境下学习机器人路径规划的学生与开发者，聚焦RRT（快速扩展随机树）算法在Turtlebot3小车上的落地实现。内容基于ROS Melodic与Ubuntu 18.04，用C++编写核心节点，涵盖状态表示、距离函数、随机采样、近邻搜索、树扩展、目标检测与路径平滑等关键环节，并涉及激光雷达建图、TF变换、move_base控制与rviz可视化调试，适合具备一定ROS与C++基础、希望从理论走向实车验证的中级学习者。压缩包共204个文件，约2.6MB，包含sdf与dae模型、config与yaml参数、launch启动脚本、world仿真场景、rviz配置、xacro与cpp源码等，覆盖仿真环境搭建到算法运行的完整工程结构。目前已有3405人学习下载，可帮助读者快速复现RRT规划流程、理解随机采样与树扩展的工程细节，并借助现成配置与源码排查定位、建图和控制环节的常见问题。

1. RRT 在 ROS 里到底解决什么问题：从「路径规划跑不起来」说起

很多做移动机器人或机械臂的同行，第一次把 RRT 接到 ROS 里，都会遇到同一个尴尬：仿真里地图是空的，规划器秒出路径；一旦换成真实栅格地图，节点数一上去，要么规划时间爆炸，要么路径贴着障碍物边缘走，要么干脆报Unable to find a valid path。RRT（快速扩展随机树）本身不复杂，难的是它在 ROS 这套move_base/nav_core插件体系里怎么落地、参数怎么调、和全局规划器怎么配合。

这篇笔记就围绕「RRT 算法在 ROS 中的实现」这件事，把随机采样规划器的原理、插件写法、参数配置、调试手段和踩坑记录讲清楚。适合两类人：一类是刚接触nav_core插件、想自己写一个全局规划器的同学；另一类是用过global_planner但觉得 A* 在复杂环境里不够用、想换成采样类规划器的工程师。读完你应该能自己写一个可加载的 RRT 全局规划插件，并知道哪些参数决定成败。

2. 先搞懂 RRT 与 ROS 规划器插件的对接方式

2.1 RRT 的核心机制与它为什么适合高维空间

RRT 的思路很朴素：从起点开始，不断在空间里随机撒点，找到树上离这个随机点最近的节点，朝它方向走一小步，如果这一步不撞障碍物，就把新节点加进树里。重复这个过程，直到某个新节点离目标足够近，再回溯父节点得到路径。

它和 A*、Dijkstra 最大的区别在于：A* 是在离散栅格上做完备搜索，栅格分辨率一高，搜索空间指数增长；RRT 是在连续空间里做概率完备搜索，不要求把空间离散化，所以对高维构型空间（比如六轴机械臂的关节空间）特别友好。代价是路径不平滑、不最优，而且每次结果都不一样——这点后面避坑章节会重点讲。

在 ROS 里，全局规划器要继承nav_core::BaseGlobalPlanner接口，实现initialize和makePlan两个纯虚函数。makePlan的输入是起点和目标点的geometry_msgs::PoseStamped，输出是一条nav_msgs::Path。RRT 要做的就是在这两个函数之间，把随机树建起来，再把路径填进Path消息。

2.2 插件接口的两个必实现函数与坐标系约定

先看接口骨架。initialize负责拿costmap_2d::Costmap2DROS指针、读参数、建内部数据结构；makePlan负责实际规划。这里有个容易翻车的点：makePlan收到的起点通常是机器人当前位姿，在map坐标系下；但 costmap 有自己的global_frame，如果两者不一致，必须做坐标变换，否则你会看到规划器在「另一个世界」里找路。

// rrt_global_planner.h 插件类声明骨架 #include <nav_core/base_global_planner.h> #include <costmap_2d/costmap_2d_ros.h> #include <ros/ros.h> class RRTGlobalPlanner : public nav_core::BaseGlobalPlanner { public: RRTGlobalPlanner(); RRTGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); // 必须实现：初始化，拿 costmap、读参数 void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) override; // 必须实现：输入起点终点，输出路径 bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) override; private: costmap_2d::Costmap2DROS* costmap_ros_; costmap_2d::Costmap2D* costmap_; double step_size_; // 每次扩展的步长 double goal_tolerance_; // 判定到达目标的阈值 int max_iterations_; // 最大迭代次数，防止死循环 bool initialized_; };

这段声明的关键在于三个成员参数：step_size_决定树的稠密程度，太小则迭代次数暴涨，太大则容易跨过窄通道；goal_tolerance_决定什么时候算「到了」，设太小可能永远到不了，设太大路径末端会飘；max_iterations_是保命参数，没有它，遇到死胡同环境规划器会一直转。

2.3 在 CMakeLists 与 plugin.xml 里注册插件

ROS 的插件机制靠pluginlib，你必须显式导出类，否则move_base加载时会报Failed to create the global_planner。这一步新手最容易漏。

<!-- rrt_planner_plugin.xml 放在包根目录 --> <library path="lib/librrt_global_planner"> <class name="rrt_global_planner/RRTGlobalPlanner" type="RRTGlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner"> <description>RRT based global planner for ROS navigation</description> </class> </library>

# CMakeLists.txt 关键片段 add_library(rrt_global_planner src/rrt_global_planner.cpp) target_link_libraries(rrt_global_planner ${catkin_LIBRARIES}) # 导出插件描述文件，路径要和 plugin.xml 里的 library path 对应 install(FILES rrt_planner_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

plugin.xml里的name是全局唯一标识，type必须和 C++ 类名完全一致，base_class_type固定写nav_core::BaseGlobalPlanner。CMake 里add_library生成的库名要和library path里lib/后面的名字对上，差一个字母就是加载失败。写完记得在package.xml里加<nav_core>和<pluginlib>依赖。

3. 手写一个可加载的 RRT 全局规划器：从采样到回溯

3.1 随机采样、最近邻查找与碰撞检测

RRT 主循环就三件事：采样、找最近邻、扩展。采样在 costmap 的物理边界内均匀撒点；最近邻遍历当前树所有节点算欧氏距离；扩展时从最近邻朝采样点走step_size_，得到候选新节点，再做碰撞检测。

碰撞检测是性能大头。常见做法是把候选点从世界坐标转成栅格索引，查costmap_->getCost(mx, my)，如果代价大于等于LETHAL_OBSTACLE就丢弃。注意 costmap 的getCost返回的是unsigned char，INSCRIBED_INFLATED_OBSTACLE（253）代表机器人中心不能进的膨胀区，实际规划里通常把 253 也当障碍，否则路径会贴着墙走。

// 采样：在 costmap 物理范围内均匀随机 bool RRTGlobalPlanner::sample(double& wx, double& wy) { double origin_x = costmap_->getOriginX(); double origin_y = costmap_->getOriginY(); double size_x = costmap_->getSizeInMetersX(); double size_y = costmap_->getSizeInMetersY(); wx = origin_x + (rand() / double(RAND_MAX)) * size_x; wy = origin_y + (rand() / double(RAND_MAX)) * size_y; return true; } // 碰撞检测：世界坐标 -> 栅格索引 -> 查代价 bool RRTGlobalPlanner::isFree(double wx, double wy) { unsigned int mx, my; if (!costmap_->worldToMap(wx, wy, mx, my)) return false; // 出界 unsigned char cost = costmap_->getCost(mx, my); // 253 是内切膨胀障碍，254 是致命障碍，都视为不可通行 return cost < costmap_2d::INSCRIBED_INFLATED_OBSTACLE; }

worldToMap返回 false 说明点在地图外，直接判为不可行。getCost的阈值选择是经验活：只挡 254 会让路径贴墙，挡 253 更安全但可能把窄通道堵死。我一般先用 253，如果发现窄通道过不去，再针对具体场景放宽。

3.2 树的扩展、目标判定与父节点回溯

扩展时要注意：不是直接跳到采样点，而是沿方向走固定步长。这样树才是均匀生长的，不会出现一条超长边。目标判定有两种策略——一种是新节点落在目标goal_tolerance_内就停；另一种是每隔若干次迭代直接朝目标扩展一次（goal bias），能显著加快收敛。

struct Node { double x, y; int parent; }; bool RRTGlobalPlanner::makePlan( const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) { plan.clear(); std::vector<Node> tree; tree.push_back({start.pose.position.x, start.pose.position.y, -1}); for (int iter = 0; iter < max_iterations_; ++iter) { double sx, sy; // goal bias：5% 概率直接采样目标点，加速收敛 if (rand() / double(RAND_MAX) < 0.05) { sx = goal.pose.position.x; sy = goal.pose.position.y; } else { sample(sx, sy); } // 找最近邻 int nearest = 0; double best = std::numeric_limits<double>::max(); for (size_t i = 0; i < tree.size(); ++i) { double d = hypot(tree[i].x - sx, tree[i].y - sy); if (d < best) { best = d; nearest = i; } } // 朝采样点走一步 double dx = sx - tree[nearest].x; double dy = sy - tree[nearest].y; double dist = hypot(dx, dy); if (dist < 1e-6) continue; double nx = tree[nearest].x + step_size_ * dx / dist; double ny = tree[nearest].y + step_size_ * dy / dist; if (!isFree(nx, ny)) continue; // 撞障碍，丢弃 tree.push_back({nx, ny, nearest}); // 到达目标附近，回溯 if (hypot(nx - goal.pose.position.x, ny - goal.pose.position.y) < goal_tolerance_) { std::vector<int> idx; for (int k = tree.size() - 1; k != -1; k = tree[k].parent) idx.push_back(k); std::reverse(idx.begin(), idx.end()); for (int k : idx) { geometry_msgs::PoseStamped p = start; p.pose.position.x = tree[k].x; p.pose.position.y = tree[k].y; plan.push_back(p); } return true; } } return false; // 迭代耗尽仍未到达 }

回溯这段是 RRT 的「后悔药」：每个节点只记父节点索引，找到目标后顺着parent一路往回走，再反转就是起点到终点的顺序。goal_tolerance_建议设成 1.5 到 2 倍step_size_，太小会导致新节点总是「擦肩而过」，太大则路径末端离目标有明显偏差。

3.3 参数怎么设：step_size、goal_tolerance 与 max_iterations

这三个参数没有万能值，但有一套起手配置。室内 10m×10m 的栅格地图，分辨率 0.05m，我一般这样起步：

调参顺序建议：先固定max_iterations_保证不卡死，再调step_size_让路径能穿过最窄通道，最后收goal_tolerance_提高末端精度。每次只动一个参数，用同一张地图和同一组起终点对比，否则你根本分不清是哪个参数起的作用。

4. 避坑与排查：RRT 在 ROS 里最常见的五类翻车

4.1 规划器加载失败，move_base 报插件找不到

现象：启动move_base后日志出现Failed to create the global_planner或According to the loaded plugin descriptions the class ... does not exist。

原因：九成是plugin.xml没被正确导出，或者name和move_base参数里写的名字不一致。也可能是add_library的库名和library path对不上。

解决：先rospack plugins --attrib=plugin nav_core看你的插件有没有被识别。没有就检查package.xml是否声明了pluginlib导出，CMakeLists.txt是否install了 xml 文件。名字建议全小写加下划线，避免大小写踩坑。

4.2 路径贴墙甚至穿墙

现象：规划出的路径紧贴障碍物边缘，或者看起来穿过了膨胀层。

原因：碰撞检测只挡了LETHAL_OBSTACLE（254），没挡INSCRIBED_INFLATED_OBSTACLE（253）。另外如果 costmap 的膨胀半径设得比机器人实际半径小，路径自然会贴墙。

解决：碰撞检测阈值改成< costmap_2d::INSCRIBED_INFLATED_OBSTACLE。同时检查costmap_common_params.yaml里的inflation_radius和cost_scaling_factor，膨胀半径至少等于机器人外接圆半径。

4.3 每次规划结果都不一样，路径抖动

现象：同样的起终点，连续规划几次，路径形状完全不同，机器人走起来一顿一顿。

原因：RRT 是随机采样算法，本身不保证可重复性。如果上层控制器直接跟踪这条抖动路径，就会出现反复修正。

解决：这是 RRT 的固有特性，不是 bug。工程上一般加两步后处理——先用 shortcut 平滑（随机取路径上两点，若直线无碰撞就替换中间段），再交给局部规划器做实时避障。如果业务要求路径稳定，考虑换 RRT* 或 Informed RRT*，它们会做重连优化，代价是单次规划更慢。

4.4 窄通道永远规划失败

现象：地图上明明有能过的通道，RRT 就是找不到路，max_iterations_跑满返回 false。

原因：step_size_太大，扩展时一步跨过了通道；或者采样是全局均匀的，落在窄通道里的概率极低。

解决：把step_size_降到通道宽度的一半以下。更有效的做法是加目标偏置采样（goal bias）和通道附近的高斯采样，提高窄区域采样密度。实在不行就上 RRT-Connect，双向生长对窄通道更友好。

4.5 规划耗时忽高忽低，偶尔卡住主线程

现象：大部分时候几十毫秒出路径，偶尔一次规划要好几秒，甚至让move_base超时。

原因：makePlan是在move_base的规划线程里同步执行的，迭代次数和树规模不受控时，单次耗时没有上界。另外最近邻查找是 O(n) 遍历，树大了之后每次扩展都变慢。

解决：给max_iterations_设硬上限，并在循环里加时间检查，超过预算就提前返回 false。最近邻可以用 KD-Tree 加速，节点上千后提升明显。如果对实时性要求高，把规划放到独立线程或异步服务里，别堵住主循环。

5. 让 RRT 真正能用的两个进阶技巧

5.1 用 shortcut 平滑把折线路径变成可跟踪轨迹

原始 RRT 路径是一堆折线段，直接给局部规划器跟踪，机器人会频繁启停。shortcut 平滑的思路很简单：在路径上随机取两个不相邻的点，如果它们之间的直线不撞障碍，就把中间那段替换掉。重复几十次，路径会明显变直。

// shortcut 平滑：随机取两点，无碰撞则截断中间段 void shortcutSmooth(std::vector<geometry_msgs::PoseStamped>& path, Costmap2D* costmap, int iterations) { for (int it = 0; it < iterations && path.size() > 2; ++it) { int i = rand() % path.size(); int j = rand() % path.size(); if (i > j) std::swap(i, j); if (j - i < 2) continue; // 检查 i 到 j 的直线是否全程无碰撞 bool clear = true; for (double t = 0.0; t <= 1.0; t += 0.05) { double x = path[i].pose.position.x + t * (path[j].pose.position.x - path[i].pose.position.x); double y = path[i].pose.position.y + t * (path[j].pose.position.y - path[i].pose.position.y); unsigned int mx, my; if (!costmap->worldToMap(x, y, mx, my) || costmap->getCost(mx, my) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { clear = false; break; } } if (clear) path.erase(path.begin() + i + 1, path.begin() + j); } }

iterations一般取路径点数的 2 到 3 倍，采样步长 0.05m 是经验值，太粗会漏检碰撞，太细拖慢速度。平滑后记得重新检查整条路径的连续性，别把路径截断成两段。

5.2 用 rosbag 回放验证规划器稳定性

调规划器最忌讳「改一次参数跑一次实车」。我的习惯是先把一次完整运行的/map、/move_base/global_costmap/costmap、/move_base/goal录成 rosbag，然后离线反复回放，用同一组数据对比不同参数下的规划成功率和耗时。

# 录制关键话题 rosbag record /map /move_base/global_costmap/costmap /move_base/goal -O rrt_test.bag # 回放并观察规划输出 rosbag play rrt_test.bag --clock rostopic echo /move_base/GlobalPlanner/plan

回放时重点看三个指标：规划成功率（连续 50 次 goal 里成功几次）、平均耗时、路径长度。成功率低于 90% 就先调step_size_和max_iterations_，别急着上 RRT*。路径长度波动大说明随机性影响明显，考虑加平滑或换确定性更强的变体。

我踩过最深的一个坑，是早期没做时间预算，max_iterations_设了 50000，结果在一张 40m×40m 的地图上，遇到一个几乎无解的目标点，规划器硬跑了十几秒，把整个导航栈拖垮。后来养成习惯：任何采样类规划器，循环里必须带时间检查，宁可返回失败让上层重试，也不能让单次规划无界耗时。希望帮到你。

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