简介这是一份基于ROSIndigo实现RRT路径规划算法的C工程源码包主要面向学习ROS与采样型运动规划的初、中级开发者也可用于机器人导航课程设计或课题预研。算法针对单障碍环境搜索优化路径并在RVIZ中完成可视化展示代码拆分为ros_node与env_node两大可执行节点便于对照理解RRT扩展流程、环境建模以及ROS Marker显示机制完整演示从随机采样、碰撞检测到最终生成可行路径的规划过程对入门机器人运动规划有直接帮助。压缩包内共有9个文件以cpp源码与h头文件为主辅以md说明文档、xml功能包配置和txt文本文件整体仅11KB体量轻巧且结构紧凑既适合快速阅读也便于在此基础上修改参数或扩展障碍物场景。这份轻量工程省去了冗长依赖可帮助读者把注意力集中在RRT算法本身快速建立ROS节点与可视化调试的直观认识。目前已有2954人学习下载适合作为路径规划入门阶段的实践参考。 做机器人导航这两年我调过的路径规划算法不算少。A*、Dijkstra、PRM、RRT系列基本都过了一遍印象最深的是第一次在ROS里跑通RRT——看着那棵随机树七拐八拐穿过障碍物一点点摸到目标点比A在栅格里一格一格往前推要兴奋得多。后来我干脆把这套实现整理成了一个名为path_planning的ROS功能包专门封装基于RRT的路径规划算法。这篇文章是对这个包的完整复盘从算法原理到工程代码从参数调到踩坑记录再到双向RRT和RRT的进阶思路一次性讲透。如果你正在用ROS做机器人导航、做动态避障小车或者研究智能车泊车路径规划这篇内容应该能帮你省不少时间。1. RRT的随机底色为什么栅格算法搞不定的场景它能搞定1.1 栅格算法的天花板一聊到路径规划大部分人的第一反应就是A*。在ROS的move_base框架里global_planner默认就是A*那一系作用域是一张提前建好的二维栅格地图。栅格算法的思路很直白把空间切成一个个格子要么能走要么不能走然后用启发式搜索从起点一路搜到终点。听起来没什么问题可一旦把环境换成宽阔的停车场、非结构化园区或者把机器人换成一个六轴机械臂问题就冒出来了。分辨率太粗狭窄通道在栅格里直接断掉分辨率调细内存和搜索时间又爆炸。高维空间更是栅格算法的死穴——机械臂的关节空间里每个维度切10段10个关节就是10的10次方个状态穷举搜索完全没有可行性。1.2 RRT的三句话原理这个时候基于采样的方法就体现出优势了。它不关心空间被切成多少格而是直接往工作空间里撒点用一连串可行路径段把离散采样点连起来相当于用少量样本去近似整个连续空间。RRTRapidly-exploring Random Tree快速扩展随机树就是采样方法中最经典的一支。RRT的原理用三句话就能讲完在工作空间里随机采样一个点在已有树上找到离它最近的节点朝这个采样点延伸一小段距离如果这段路没撞上障碍就把新节点挂到树上。循环往复树会像植物的根系一样向整个空间蔓延直到有节点落到目标点附近。为什么叫快速扩展随机树因为它每一步都带随机性天然偏向探索空旷区域又不会彻底忽略角落。算法具备概率完备性——只要迭代次数够多、步长选得合适在任意连通空间里几乎必然能找到一条可行路径。这个性质在工程上意味着你不需要对地图做预处理拿到障碍物信息把步长和规划范围设好剩下的交给随机性就行。1.3 和PRM、A*放在一起比从应用角度来说AGV在工厂地面跑A*绰绰有余但机械臂在线规划、无人机动态避障、车辆在无固定道路环境里搜索路径这些场景里RRT系列天然更合适。这也是ROS社区里RRT相关讨论常年不冷的原因。三类算法的本质差异我用一张表总结过算法空间表达完备性最大优势最大短板A*栅格分辨率内完备实现简单、路径较优高维状态空间会爆炸PRM采样图概率完备适合多次查询复用建图阶段开销大RRT采样树概率完备单次查询快、不依赖栅格路径不平滑、非最优2. 搭建ROS功能包环境、依赖与工程结构2.1 环境与ROS发行版选择我开发这个包的时候测过两套组合Ubuntu 18.04加ROS MelodicUbuntu 20.04加ROS Noetic代码以Python为主。Python在算法验证阶段的效率优势非常明显改完逻辑立刻能跑对于RRT这种重逻辑轻计算的项目很合适等要上真机或压性能了再改写成C版本用于生产。ROS环境的安装说过太多次只提醒一句别自己去跟源和依赖硬刚社区里流行的一键安装脚本比如小鱼ROS能省下大量时间选对Ubuntu版本对应的ROS发行版就行。装完之后一定把rqt_graph、rviz、rosbag这三个工具备好。前两个是排查和可视化的主力rosbag则能把你现场的数据录下来反复回放调试。这三个工具后面都会用到。2.2 创建一个功能包RRT本身不需要多复杂的工程骨架一个标准ROS功能包就够了。用catkin创建cd ~/catkin_ws/src catkin_create_pkg path_planning roscpp rospy std_msgs nav_msgs geometry_msgs visualization_msgs cd ~/catkin_ws catkin_make source devel/setup.bash依赖项里nav_msgs负责发布nav_msgs/Path路径消息visualization_msgs负责把树的生长过程发到RVizgeometry_msgs处理点和位姿。这几类消息在自主导航和动态避障小车项目里都是标配后面接move_base也不会冲突。2.3 功能包目录结构我最终的目录结构是这样path_planning/ ├── src/ │ ├── rrt_planner.py # RRT算法核心纯逻辑不依赖ROS │ ├── rrt_node.py # ROS节点封装订阅地图、发布路径 │ └── utils.py # 几何计算、碰撞检测工具 ├── rviz/ │ └── rrt_demo.rviz # 预先配好的RViz显示配置 ├── maps/ │ ├── nav_simple.yaml │ └── nav_simple.pgm ├── launch/ │ ├── rrt_demo.launch │ └── rrt_rviz.launch ├── CMakeLists.txt └── package.xml这里面我最坚持的一条经验是算法和ROS解耦。src/rrt_planner.py只关心数学逻辑——树、采样、碰撞、路径提取不出现任何rospy代码rrt_node.py只负责订阅地图、读取起终点、调用算法、发布话题。这样算法可以直接在命令行跑单元测试不用启动roscore排bug的体验好一个档次。3. 核心代码逐段拆解采样、最近邻、碰撞检测与树生长3.1 树的定义RRT的树本质上就是一组带父子关系的节点。为了提取路径方便我直接用列表存节点坐标再用字典记录父子关系class RRTPlanner: def __init__(self, start, goal, step_size0.3, goal_tolerance0.2): self.start start self.goal goal self.step_size step_size self.goal_tolerance goal_tolerance self.nodes [] # 所有节点坐标 self.parent {} # 子节点索引 - 父节点索引3.2 主循环核心的plan函数不长但每一行都有讲究def plan(self, max_iter5000): self.nodes [self.start] self.parent {0: -1} for i in range(max_iter): sample self.sample() near_idx self.nearest_neighbor(sample) new_node self.steer(near_idx, sample) if new_node is None: continue if self.collision_free(self.nodes[near_idx], new_node): new_idx len(self.nodes) self.nodes.append(new_node) self.parent[new_idx] near_idx if self.distance(new_node, self.goal) self.goal_tolerance: return self.extract_path(new_idx) return None采样、找最近邻、steer、碰撞检测四个步骤循环往复。steer的逻辑是从最近邻节点出发沿着指向采样点的方向只走step_size那么远返回一个新的候选节点。如果最近邻到采样点的距离本来就小于步长那就直接取采样点。3.3 采样加一点目标偏置纯随机采样的问题在于方向太散收敛慢。工程上最常用的改进是给采样加一个目标偏置以一定概率直接返回目标点def sample(self): if random.random() self.goal_sample_rate: # 一般取0.05~0.15 return self.goal return (random.uniform(self.min_x, self.max_x), random.uniform(self.min_y, self.max_y))这个参数是调参时影响最直观的一个下面第5节我会具体讲。3.4 最近邻搜索基础版RRT每一步都要在全树范围找最近邻节点。树只有几百个节点时线性扫描完全没有压力但树长到几千上万个节点每次都扫一遍全数组就会卡。我第一版就是线性扫描五千次迭代跑下来已经能感觉到延迟。后来改成维护一个KD-Tree索引查询时间从毫秒级降到亚毫秒级。工程上的建议是树规模小别过度设计线性扫描够用规模大了直接用scipy.spatial.cKDTree一行替换的事。3.5 碰撞检测碰撞检测是RRT最耗性能的部分。我的实现里针对两种输入做了区分。地图是nav_msgs/OccupancyGrid栅格时把点坐标换算成栅格索引直接查occupancy数组对应格子是否free障碍物是自定义几何体列表时则沿连线做插值求交def collision_free(self, p1, p2): for t in np.arange(0, 1, self.check_resolution): pt (p1[0] t*(p2[0]-p1[0]), p1[1] t*(p2[1]-p1[1])) if not self.is_free_grid(pt): return False return True这里最容易被忽略的是check_resolution的选取。插值太稀疏细障碍物会被直接隧穿过去插值太密整个规划被拖慢。我一般让它的取值不超过地图分辨率的一半既不会漏检又不至于拖慢整体规划。4. 把规划器接进ROS节点、话题、RViz可视化全流程4.1 节点设计与话题映射rrt_node.py里我做三件事订阅地图、监听起终点、发布路径和可视化。话题设计如下订阅 nav_msgs/OccupancyGrid /map拿障碍物信息订阅 geometry_msgs/PoseStamped /move_base_simple/goal拿目标点跟RViz自带的2D Nav Goal按钮完全兼容发布 nav_msgs/Path /path最终规划路径发布 visualization_msgs/MarkerArray /rrt_tree树的生长过程发布 visualization_msgs/Marker /rrt_start_goal起终点标记把目标点话题直接选成/move_base_simple/goal有个小便宜RViz里的2D Nav Goal按钮点一下目标就自动发过来了做demo演示时不用额外写交互程序。4.2 服务接口还是话题接口如果只是演示话题接口已经够用了。但做动态避障小车这类项目时规划器要被反复调用更适合包成ROS Service。调用方发一个PlanPathRequest服务端返回一条path这样换起点、换采样参数都不需要重启节点。如果你打算把它彻底接进move_base那就要走global planner plugin的路线把算法封装成nav_core::BaseGlobalPlanner插件。这个改造量比较大一般项目不需要先把基础版本调明白再说。4.3 RViz可视化RViz可视化是所有调试工作里回报最高的一项。把每个新节点封装成MarkerArray发出去树就活生生显示在地图上你能直接看到它往哪个方向长、哪片区域始终进不去。我的RViz配置里保留了两个核心显示项一个看树生长用LineStrip把父子节点连起来一个看最终path用Path消息显示成绿色线条。每次改完算法眼睛扫一眼树形就知道效果比盯着终端日志猜高效得多。4.4 launch文件launch文件把地图服务器、RViz和规划节点全部拉起来launch node namemap_server pkgmap_server typemap_server args$(find path_planning)/maps/nav_simple.yaml/ node namerviz pkgrviz typerviz args-d $(find path_planning)/rviz/rrt_demo.rviz/ node namerrt_node pkgpath_planning typerrt_node.py outputscreen/ /launch一条roslaunch命令地图、RViz、规划节点全部就位。RViz里点一下2D Nav Goal树就开始长路径也就出来了。这个流程对第一次接触RRT的同学很友好能快速建立直观感受。4.5 frame_id统一这个细节别忽略可视化还有个容易踩的坑发布Path和Marker时header里的frame_id一定要统一。我全部用map但如果你在TF树还没完全跑起来的调试阶段先确认fixed frame设置的是map而不是odom否则树会画在坐标系原点附近看起来路径整个飘在地图外面。我早期有次排查了半小时最后发现就是fixed frame选错这种问题排查起来特别冤。5. 调参与踩坑步长、阈值、目标偏置实测记录5.1 关键参数速查表把我调过的参数经验整理成一张表参数推荐区间作用step_size地图尺度的1%~5%步长越小路径越精细树扩展越慢goal_tolerancestep_size的一半到一倍太大容易提前收工太小可能一直到不了max_iter2000~10000迭代上限决定成功率goal_sample_rate0.05~0.15目标偏置越大越倾向于直接冲目标check_resolution不超过地图分辨率的一半防隧穿也没必要更小5.2 目标偏置别贪我第一次跑的时候觉得目标偏置越大收敛越快直接把goal_sample_rate调到了0.5。结果树不停地往目标方向冲在障碍物面前反复弹回来整棵树被截成两段完全不在两侧扩张。调回0.1之后树才恢复正常的探索状态。原因很简单RRT要在探索和利用之间找平衡偏置过大会一直冲向目标点丧失对空旷区域的探索能力反而容易把自己困在局部的死胡同里。0.05到0.15这个区间是用多次实验换来的别贪。5.3 起始点被困住的排查经历有次我把起点放在一个三面环墙的凹槽里树怎么长都出不来。我一开始怀疑是碰撞检测写错了把check_resolution和栅格索引换算翻来覆去查都没发现问题。最后真正的原因让我有点意外凹槽入口很窄而步长设得比入口宽度还大steer操作跨越入口时插值点正好落在墙内整条有效路径都被判成了碰撞。解决办法是把步长缩到入口宽度的三分之一树一下就钻出去了。这次排查让我长了个记性步长不是孤立参数它必须小于环境中最小可行通道的宽度否则树根本穿不过去。5.4 路径平滑RRT出来的路径是一堆折线段机器人直接跟会一顿一顿转弯处还容易蹭墙。我没有在规划器内部做复杂平滑而是把RRT的结果当全局参考路径交给局部路径规划器比如TEB或者DWA去跟踪修整效果已经足够好。如果你希望直接输出平滑轨迹比较常见的做法是路径修剪加Bezier曲线插值或者直接上B样条拟合。这个工作量的提升和增益都很大而且在双向RRT的方向上尤其值得做——两棵树拼接处往往有一个非常突兀的折角不处理基本没法直接给机器人用。6. 进阶路线双向RRT和RRT*到底比基础版强在哪6.1 双向RRT两棵树同时长双向RRT的改动并不大从起点和终点各长一棵树每次迭代随机采样一个点让一棵树向它扩展然后尝试连接另一棵树。基础RRT是单点进攻双向版本是两头夹击在窄通道场景里收敛速度实测能快好几个量级。窄通道之所以折磨基础RRT是因为它要恰好采到通道入口附近的点这个概率非常低双向RRT的两棵树各自长到通道两头只需要让它们在中间某个位置相遇即可窗口一下子大了很多。这也是社区里双向RRT话题热度这么高的直接原因。6.2 RRT*用重新布线换最优性基础RRT不保证路径最优因为找到路径就停了。RRT不是这样它找到目标之后不停止搜索而是继续迭代并且每插入一个新节点就检查它附近的已存在节点看看绕道新节点走是否比原来的连接更短如果更短就把旧节点的父指针重新接到新节点上这个操作叫rewire。代价是每步都要多一次邻域搜索耗时上升好处是路径代价会随迭代次数增加逐渐逼近最优。实测在同样的地图和起点终点下给足时间RRT找到的路径往往比基础RRT短不少。6.3 我的选型建议讲一下我实际项目里的选择逻辑。如果是课堂作业或者实验室demo基础RRT加目标偏置已经足够重心放在可视化效果和工程封装上。如果是动态避障小车导航或园区无人车这类项目基础RRT先跑通全链路再升级成双向RRT配上平滑模块基本覆盖大多数场景。RRT适合离线规划和计算时间充足的任务。泊车路径规划这种结构化场景我反而更推荐Hybrid A——它把车辆运动学约束直接揉进搜索过程出来的轨迹天然可跟踪量产自动泊车方案里普遍用的也是这一路。选型这事真的得看任务约束不是越复杂越好。最后再分享一个小经验无论你最终用哪个版本一定要在RViz里把树和路径同时显示。看着树怎么探索、怎么绕障碍是排障时第一手的信息来源。这个包虽然不大但完整调通之后你会发现ROS里地图、话题、可视化这一整套工作流被打通了以后再碰任何规划类项目套路都是相通的。本文还有配套的精品资源点击获取 
长河君 · 2026/9/8 7:45:43 · 阅读 1.2k 基于RRT的路径规划在ROS机器人导航中的实现与调优
简介这是一份基于ROSIndigo实现RRT路径规划算法的C工程源码包主要面向学习ROS与采样型运动规划的初、中级开发者也可用于机器人导航课程设计或课题预研。算法针对单障碍环境搜索优化路径并在RVIZ中完成可视化展示代码拆分为ros_node与env_node两大可执行节点便于对照理解RRT扩展流程、环境建模以及ROS Marker显示机制完整演示从随机采样、碰撞检测到最终生成可行路径的规划过程对入门机器人运动规划有直接帮助。压缩包内共有9个文件以cpp源码与h头文件为主辅以md说明文档、xml功能包配置和txt文本文件整体仅11KB体量轻巧且结构紧凑既适合快速阅读也便于在此基础上修改参数或扩展障碍物场景。这份轻量工程省去了冗长依赖可帮助读者把注意力集中在RRT算法本身快速建立ROS节点与可视化调试的直观认识。目前已有2954人学习下载适合作为路径规划入门阶段的实践参考。 做机器人导航这两年我调过的路径规划算法不算少。A*、Dijkstra、PRM、RRT系列基本都过了一遍印象最深的是第一次在ROS里跑通RRT——看着那棵随机树七拐八拐穿过障碍物一点点摸到目标点比A在栅格里一格一格往前推要兴奋得多。后来我干脆把这套实现整理成了一个名为path_planning的ROS功能包专门封装基于RRT的路径规划算法。这篇文章是对这个包的完整复盘从算法原理到工程代码从参数调到踩坑记录再到双向RRT和RRT的进阶思路一次性讲透。如果你正在用ROS做机器人导航、做动态避障小车或者研究智能车泊车路径规划这篇内容应该能帮你省不少时间。1. RRT的随机底色为什么栅格算法搞不定的场景它能搞定1.1 栅格算法的天花板一聊到路径规划大部分人的第一反应就是A*。在ROS的move_base框架里global_planner默认就是A*那一系作用域是一张提前建好的二维栅格地图。栅格算法的思路很直白把空间切成一个个格子要么能走要么不能走然后用启发式搜索从起点一路搜到终点。听起来没什么问题可一旦把环境换成宽阔的停车场、非结构化园区或者把机器人换成一个六轴机械臂问题就冒出来了。分辨率太粗狭窄通道在栅格里直接断掉分辨率调细内存和搜索时间又爆炸。高维空间更是栅格算法的死穴——机械臂的关节空间里每个维度切10段10个关节就是10的10次方个状态穷举搜索完全没有可行性。1.2 RRT的三句话原理这个时候基于采样的方法就体现出优势了。它不关心空间被切成多少格而是直接往工作空间里撒点用一连串可行路径段把离散采样点连起来相当于用少量样本去近似整个连续空间。RRTRapidly-exploring Random Tree快速扩展随机树就是采样方法中最经典的一支。RRT的原理用三句话就能讲完在工作空间里随机采样一个点在已有树上找到离它最近的节点朝这个采样点延伸一小段距离如果这段路没撞上障碍就把新节点挂到树上。循环往复树会像植物的根系一样向整个空间蔓延直到有节点落到目标点附近。为什么叫快速扩展随机树因为它每一步都带随机性天然偏向探索空旷区域又不会彻底忽略角落。算法具备概率完备性——只要迭代次数够多、步长选得合适在任意连通空间里几乎必然能找到一条可行路径。这个性质在工程上意味着你不需要对地图做预处理拿到障碍物信息把步长和规划范围设好剩下的交给随机性就行。1.3 和PRM、A*放在一起比从应用角度来说AGV在工厂地面跑A*绰绰有余但机械臂在线规划、无人机动态避障、车辆在无固定道路环境里搜索路径这些场景里RRT系列天然更合适。这也是ROS社区里RRT相关讨论常年不冷的原因。三类算法的本质差异我用一张表总结过算法空间表达完备性最大优势最大短板A*栅格分辨率内完备实现简单、路径较优高维状态空间会爆炸PRM采样图概率完备适合多次查询复用建图阶段开销大RRT采样树概率完备单次查询快、不依赖栅格路径不平滑、非最优2. 搭建ROS功能包环境、依赖与工程结构2.1 环境与ROS发行版选择我开发这个包的时候测过两套组合Ubuntu 18.04加ROS MelodicUbuntu 20.04加ROS Noetic代码以Python为主。Python在算法验证阶段的效率优势非常明显改完逻辑立刻能跑对于RRT这种重逻辑轻计算的项目很合适等要上真机或压性能了再改写成C版本用于生产。ROS环境的安装说过太多次只提醒一句别自己去跟源和依赖硬刚社区里流行的一键安装脚本比如小鱼ROS能省下大量时间选对Ubuntu版本对应的ROS发行版就行。装完之后一定把rqt_graph、rviz、rosbag这三个工具备好。前两个是排查和可视化的主力rosbag则能把你现场的数据录下来反复回放调试。这三个工具后面都会用到。2.2 创建一个功能包RRT本身不需要多复杂的工程骨架一个标准ROS功能包就够了。用catkin创建cd ~/catkin_ws/src catkin_create_pkg path_planning roscpp rospy std_msgs nav_msgs geometry_msgs visualization_msgs cd ~/catkin_ws catkin_make source devel/setup.bash依赖项里nav_msgs负责发布nav_msgs/Path路径消息visualization_msgs负责把树的生长过程发到RVizgeometry_msgs处理点和位姿。这几类消息在自主导航和动态避障小车项目里都是标配后面接move_base也不会冲突。2.3 功能包目录结构我最终的目录结构是这样path_planning/ ├── src/ │ ├── rrt_planner.py # RRT算法核心纯逻辑不依赖ROS │ ├── rrt_node.py # ROS节点封装订阅地图、发布路径 │ └── utils.py # 几何计算、碰撞检测工具 ├── rviz/ │ └── rrt_demo.rviz # 预先配好的RViz显示配置 ├── maps/ │ ├── nav_simple.yaml │ └── nav_simple.pgm ├── launch/ │ ├── rrt_demo.launch │ └── rrt_rviz.launch ├── CMakeLists.txt └── package.xml这里面我最坚持的一条经验是算法和ROS解耦。src/rrt_planner.py只关心数学逻辑——树、采样、碰撞、路径提取不出现任何rospy代码rrt_node.py只负责订阅地图、读取起终点、调用算法、发布话题。这样算法可以直接在命令行跑单元测试不用启动roscore排bug的体验好一个档次。3. 核心代码逐段拆解采样、最近邻、碰撞检测与树生长3.1 树的定义RRT的树本质上就是一组带父子关系的节点。为了提取路径方便我直接用列表存节点坐标再用字典记录父子关系class RRTPlanner: def __init__(self, start, goal, step_size0.3, goal_tolerance0.2): self.start start self.goal goal self.step_size step_size self.goal_tolerance goal_tolerance self.nodes [] # 所有节点坐标 self.parent {} # 子节点索引 - 父节点索引3.2 主循环核心的plan函数不长但每一行都有讲究def plan(self, max_iter5000): self.nodes [self.start] self.parent {0: -1} for i in range(max_iter): sample self.sample() near_idx self.nearest_neighbor(sample) new_node self.steer(near_idx, sample) if new_node is None: continue if self.collision_free(self.nodes[near_idx], new_node): new_idx len(self.nodes) self.nodes.append(new_node) self.parent[new_idx] near_idx if self.distance(new_node, self.goal) self.goal_tolerance: return self.extract_path(new_idx) return None采样、找最近邻、steer、碰撞检测四个步骤循环往复。steer的逻辑是从最近邻节点出发沿着指向采样点的方向只走step_size那么远返回一个新的候选节点。如果最近邻到采样点的距离本来就小于步长那就直接取采样点。3.3 采样加一点目标偏置纯随机采样的问题在于方向太散收敛慢。工程上最常用的改进是给采样加一个目标偏置以一定概率直接返回目标点def sample(self): if random.random() self.goal_sample_rate: # 一般取0.05~0.15 return self.goal return (random.uniform(self.min_x, self.max_x), random.uniform(self.min_y, self.max_y))这个参数是调参时影响最直观的一个下面第5节我会具体讲。3.4 最近邻搜索基础版RRT每一步都要在全树范围找最近邻节点。树只有几百个节点时线性扫描完全没有压力但树长到几千上万个节点每次都扫一遍全数组就会卡。我第一版就是线性扫描五千次迭代跑下来已经能感觉到延迟。后来改成维护一个KD-Tree索引查询时间从毫秒级降到亚毫秒级。工程上的建议是树规模小别过度设计线性扫描够用规模大了直接用scipy.spatial.cKDTree一行替换的事。3.5 碰撞检测碰撞检测是RRT最耗性能的部分。我的实现里针对两种输入做了区分。地图是nav_msgs/OccupancyGrid栅格时把点坐标换算成栅格索引直接查occupancy数组对应格子是否free障碍物是自定义几何体列表时则沿连线做插值求交def collision_free(self, p1, p2): for t in np.arange(0, 1, self.check_resolution): pt (p1[0] t*(p2[0]-p1[0]), p1[1] t*(p2[1]-p1[1])) if not self.is_free_grid(pt): return False return True这里最容易被忽略的是check_resolution的选取。插值太稀疏细障碍物会被直接隧穿过去插值太密整个规划被拖慢。我一般让它的取值不超过地图分辨率的一半既不会漏检又不至于拖慢整体规划。4. 把规划器接进ROS节点、话题、RViz可视化全流程4.1 节点设计与话题映射rrt_node.py里我做三件事订阅地图、监听起终点、发布路径和可视化。话题设计如下订阅 nav_msgs/OccupancyGrid /map拿障碍物信息订阅 geometry_msgs/PoseStamped /move_base_simple/goal拿目标点跟RViz自带的2D Nav Goal按钮完全兼容发布 nav_msgs/Path /path最终规划路径发布 visualization_msgs/MarkerArray /rrt_tree树的生长过程发布 visualization_msgs/Marker /rrt_start_goal起终点标记把目标点话题直接选成/move_base_simple/goal有个小便宜RViz里的2D Nav Goal按钮点一下目标就自动发过来了做demo演示时不用额外写交互程序。4.2 服务接口还是话题接口如果只是演示话题接口已经够用了。但做动态避障小车这类项目时规划器要被反复调用更适合包成ROS Service。调用方发一个PlanPathRequest服务端返回一条path这样换起点、换采样参数都不需要重启节点。如果你打算把它彻底接进move_base那就要走global planner plugin的路线把算法封装成nav_core::BaseGlobalPlanner插件。这个改造量比较大一般项目不需要先把基础版本调明白再说。4.3 RViz可视化RViz可视化是所有调试工作里回报最高的一项。把每个新节点封装成MarkerArray发出去树就活生生显示在地图上你能直接看到它往哪个方向长、哪片区域始终进不去。我的RViz配置里保留了两个核心显示项一个看树生长用LineStrip把父子节点连起来一个看最终path用Path消息显示成绿色线条。每次改完算法眼睛扫一眼树形就知道效果比盯着终端日志猜高效得多。4.4 launch文件launch文件把地图服务器、RViz和规划节点全部拉起来launch node namemap_server pkgmap_server typemap_server args$(find path_planning)/maps/nav_simple.yaml/ node namerviz pkgrviz typerviz args-d $(find path_planning)/rviz/rrt_demo.rviz/ node namerrt_node pkgpath_planning typerrt_node.py outputscreen/ /launch一条roslaunch命令地图、RViz、规划节点全部就位。RViz里点一下2D Nav Goal树就开始长路径也就出来了。这个流程对第一次接触RRT的同学很友好能快速建立直观感受。4.5 frame_id统一这个细节别忽略可视化还有个容易踩的坑发布Path和Marker时header里的frame_id一定要统一。我全部用map但如果你在TF树还没完全跑起来的调试阶段先确认fixed frame设置的是map而不是odom否则树会画在坐标系原点附近看起来路径整个飘在地图外面。我早期有次排查了半小时最后发现就是fixed frame选错这种问题排查起来特别冤。5. 调参与踩坑步长、阈值、目标偏置实测记录5.1 关键参数速查表把我调过的参数经验整理成一张表参数推荐区间作用step_size地图尺度的1%~5%步长越小路径越精细树扩展越慢goal_tolerancestep_size的一半到一倍太大容易提前收工太小可能一直到不了max_iter2000~10000迭代上限决定成功率goal_sample_rate0.05~0.15目标偏置越大越倾向于直接冲目标check_resolution不超过地图分辨率的一半防隧穿也没必要更小5.2 目标偏置别贪我第一次跑的时候觉得目标偏置越大收敛越快直接把goal_sample_rate调到了0.5。结果树不停地往目标方向冲在障碍物面前反复弹回来整棵树被截成两段完全不在两侧扩张。调回0.1之后树才恢复正常的探索状态。原因很简单RRT要在探索和利用之间找平衡偏置过大会一直冲向目标点丧失对空旷区域的探索能力反而容易把自己困在局部的死胡同里。0.05到0.15这个区间是用多次实验换来的别贪。5.3 起始点被困住的排查经历有次我把起点放在一个三面环墙的凹槽里树怎么长都出不来。我一开始怀疑是碰撞检测写错了把check_resolution和栅格索引换算翻来覆去查都没发现问题。最后真正的原因让我有点意外凹槽入口很窄而步长设得比入口宽度还大steer操作跨越入口时插值点正好落在墙内整条有效路径都被判成了碰撞。解决办法是把步长缩到入口宽度的三分之一树一下就钻出去了。这次排查让我长了个记性步长不是孤立参数它必须小于环境中最小可行通道的宽度否则树根本穿不过去。5.4 路径平滑RRT出来的路径是一堆折线段机器人直接跟会一顿一顿转弯处还容易蹭墙。我没有在规划器内部做复杂平滑而是把RRT的结果当全局参考路径交给局部路径规划器比如TEB或者DWA去跟踪修整效果已经足够好。如果你希望直接输出平滑轨迹比较常见的做法是路径修剪加Bezier曲线插值或者直接上B样条拟合。这个工作量的提升和增益都很大而且在双向RRT的方向上尤其值得做——两棵树拼接处往往有一个非常突兀的折角不处理基本没法直接给机器人用。6. 进阶路线双向RRT和RRT*到底比基础版强在哪6.1 双向RRT两棵树同时长双向RRT的改动并不大从起点和终点各长一棵树每次迭代随机采样一个点让一棵树向它扩展然后尝试连接另一棵树。基础RRT是单点进攻双向版本是两头夹击在窄通道场景里收敛速度实测能快好几个量级。窄通道之所以折磨基础RRT是因为它要恰好采到通道入口附近的点这个概率非常低双向RRT的两棵树各自长到通道两头只需要让它们在中间某个位置相遇即可窗口一下子大了很多。这也是社区里双向RRT话题热度这么高的直接原因。6.2 RRT*用重新布线换最优性基础RRT不保证路径最优因为找到路径就停了。RRT不是这样它找到目标之后不停止搜索而是继续迭代并且每插入一个新节点就检查它附近的已存在节点看看绕道新节点走是否比原来的连接更短如果更短就把旧节点的父指针重新接到新节点上这个操作叫rewire。代价是每步都要多一次邻域搜索耗时上升好处是路径代价会随迭代次数增加逐渐逼近最优。实测在同样的地图和起点终点下给足时间RRT找到的路径往往比基础RRT短不少。6.3 我的选型建议讲一下我实际项目里的选择逻辑。如果是课堂作业或者实验室demo基础RRT加目标偏置已经足够重心放在可视化效果和工程封装上。如果是动态避障小车导航或园区无人车这类项目基础RRT先跑通全链路再升级成双向RRT配上平滑模块基本覆盖大多数场景。RRT适合离线规划和计算时间充足的任务。泊车路径规划这种结构化场景我反而更推荐Hybrid A——它把车辆运动学约束直接揉进搜索过程出来的轨迹天然可跟踪量产自动泊车方案里普遍用的也是这一路。选型这事真的得看任务约束不是越复杂越好。最后再分享一个小经验无论你最终用哪个版本一定要在RViz里把树和路径同时显示。看着树怎么探索、怎么绕障碍是排障时第一手的信息来源。这个包虽然不大但完整调通之后你会发现ROS里地图、话题、可视化这一整套工作流被打通了以后再碰任何规划类项目套路都是相通的。本文还有配套的精品资源点击获取