RRT与A*算法选型及MoveIt 2机械臂避障规划实践

1. 项目概述：机器人运动规划到底在解决什么问题

先聊点实际的。我最早接触运动规划，是因为实验室里一台六轴机械臂总是没法在狭小空间里做避障动作。我们给它设好目标点，它要么告诉你无解，要么给你一条人眼看了都摇头的路线——中途会撞上旁边的工件台。折腾了半个月，我才把核心问题想明白：运动规划不是让机器人“动起来”，而是让它在成千上万种可能的路径里，找到一条既满足关节限位、又不会撞东西、还足够平滑顺畅的路线。这个问题的复杂度远超直觉，因为机械臂的自由度只要超过四个，位形空间（C-space）的维数就变得难以直接可视化，更别说用几何公式去穷举。

所以在实际项目里，很少有人再徒手去写一个规划库，而是直接用现成的成熟解决方案。我最常用的两套东西，一个是基于采样的RRT(Rapidly-exploring Random Tree)系列算法，它适合自由度较高、空间结构复杂的场景；另一个是启发式搜索里的A*算法，它在网格地图和低自由度移动机器人的路径规划上表现稳定，能找到成本最优解。而把这些算法真正用起来、跑在硬件上，绕不开的就是MoveIt 2这个生态。

这篇文章就把我这一段踩坑经历整理出来，重点回答三个问题：RRT和A*各自适合什么情况、MoveIt 2怎么把算法和机械臂模型对接起来、以及我在实机调试中遇到的规划抖动和失败问题到底怎么排查。适合正在学ROS 2、准备给机器人做自主避障，或者想从单点运动控制进阶到运动规划的朋友参考。只要你有一台能跑ROS 2的电脑（Ubuntu 22.04优先），配合仿真模型就能走完全部流程，不一定要急着碰真实硬件。

2. 核心算法拆解：RRT、A*的原理与选型

2.1 RRT家族：探索随机空间的扩展策略

RRT的核心思想在我第一次理解时觉得特别土味——不就是随机撒点然后连树吗？后来才发现，这种“土味”恰恰是它在高维空间里的核心竞争力。它不试图去显式构建障碍物的数学模型，也不需要对整个位形空间做栅格化处理，而是从起始位形开始，随机在自由空间里采样，然后把新节点连接到树上离它最近的节点上，这样树就会逐渐朝各个方向扩张，最终抵达目标区域附近。

我在学RRT时用过一个生活化的类比：想象你在一个完全黑暗的大仓库里要找出口，你不可能把整个仓库的地图先画出来再决定怎么走，通常的做法是先朝一个方向走几步，摸到墙就退回来换个方向，再继续走。RRT做的事情和这个摸黑过程几乎一样，只是它用随机采样来替代人的直觉，用树结构来记录走过的轨迹。正因为不需要全局地图的显式建模，它处理关节数较高机械臂的时候特别有效，比如我们的六轴臂在位形空间里的维度本身就来自六个关节角度，你很难真的建一个六维的栅格图去搜。

不过RRT也有明显的短板：它给的路径是可行解，不是最优解，而且经常会产生很大的冗余拐弯。所以工程上普遍不会直接用最原始的RRT，而是用它的改进变体，比如RRT*，它在每次扩展新节点后，会对附近节点进行重连接，让树整体的路径代价逐渐收敛到接近最优；还有RRT-Connect，它同时从起点和终点各长一棵树，两棵树向彼此靠近，收敛速度通常比单棵树快不少。在做MoveIt 2配置时，你会在规划插件里看到RRTstar、RRTConnect这样的名字，它们都是OMPL（Open Motion Planning Library）提供的实现。

2.2 A*与启发式搜索：有图可依时的最优解

和RRT这种无模型随机探索不同，A*的工作前提是已经有一张图。这张图可以是二维栅格地图，也可以是路图或者拓扑网络。算法从起点开始，维护两个代价函数：从起点到当前节点的实际代价g，以及从当前节点到终点的启发式估计代价h，综合值f = g + h。每次从优先队列里弹出f最小的节点进行扩展，直到扩展终点为止。

A能保证找到最优路径的前提是启发式函数具有一致性，也就是说它不能高估真实代价。在实际机器人项目里，如果我做一台差速轮式机器人在地图里的全局路径规划，我通常就选A，因为它可以很方便地给出距离最短的网格路径，后续再用局部规划器做平滑跟踪。RRT不是不能用，但没有必要在一张结构化的二维地图里去随机采样，因为地图的几何信息你全都知道，直接用启发式搜索反而更高效、更稳定。

要在代码层面区分A和RRT特别容易：A需要你定义邻居关系和边代价，RRT需要你定义采样域和距离函数。很多初学者会混淆这两类算法的适用场景，最简单的判断标准是：如果你的路径规划问题已经有一张明确的、可遍历的地图，优先选A*；如果你的问题空间连续、维数高、障碍物形状不规则，优先选RRT。我在做机械臂的避障规划时绝对不会考虑A*，因为要先把连续关节空间离散成网格，这个网格的体量会随着自由度指数爆炸，除了玩具Demo实际不可用；反过来，你在一个室内导航项目里用RRT去替代A*，一般也会得到一条弯弯曲曲、走起来很难受的路径。

2.3 规划算法对比与选型建议

光说原理还不够，我把这两种算法在实际项目中的表现列成一个对比表，方便按需选择。要注意的是，这里的对比是基于我在仿真和实机上的经验，不能当作绝对数值，因为不同的场景参数会改变算法表现。

我个人的选型习惯是：移动底盘导航一律走A或其变种（A、Dijkstra、JPS等）；机械臂的规划优先看任务复杂度，低自由度或无障碍场景用RRTConnect就够了，狭窄空间或需要路径质量时用RRT*。如果你用的是MoveIt 2默认配置，你会发现OMPL里已经有这些算法，只是默认的规划器通常是RRTConnect，这一点后面会讲到。

3. MoveIt 2实操：从ROS 2环境到避障规划

3.1 环境准备与依赖安装

要跑MoveIt 2，建议直接用基于ROS 2的发行版环境。我当时用的是Ubuntu 22.04配ROS 2 Humble，这也是目前教程最多、坑最少的组合。如果你连ROS 2都还没装，先去把ros-humble-desktop装好，然后安装MoveIt相关的二进制包。这里给出我实际安装过的命令，你可以直接抄作业：

sudo apt install ros-humble-moveit sudo apt install ros-humble-moveit-visual-tools sudo apt install ros-humble-moveit-resources sudo apt install ros-humble-rviz-visual-tools sudo apt install ros-humble-joint-state-publisher-gui

为什么要装visual-tools？因为调试规划路径时，在RViz里画轨迹、显示碰撞体、标注起点终点，全都要靠它。而moveit-resources提供了一堆现成的机器人模型，比如panda机械臂模型，用来做第一次的规划测试再合适不过。第一次跑通之前别急着用自己的URDF，先用自带模型验证环境是否装好，这是我想对所有新手说的第一条建议。

环境装好以后，跑一下自带的demo：

ros2 launch moveit_resources_panda_moveit_config demo.launch.py

这个launch文件会启动机器人的状态发布、MoveIt核心节点、RViz界面和一个用于交互的MotionPlanning插件面板。如果这个demo能正常显示机械臂模型，并且你能用拖拽的方式设置目标位姿并触发规划，说明环境基本没问题。很多人在这一步就会卡住，最常见的原因是把不同发行版的包混着装，比如在Humble上用了Galactic时代的MoveIt包。别问我是怎么知道的，都是泪。

3.2 配置机器人模型与规划组

MoveIt 2工作的核心对象不是机械臂的URDF本体，而是基于URDF生成的SRDF（Semantic Robot Description Format）。SRDF定义了哪些关节属于一个规划组、哪些连杆是末端执行器、哪些关节是可以被规划器控制的。只有正确配置了SRDF，MoveIt才知道你要让机械臂的哪些部分参与运动规划。

如果你是给模型配置SRDF，推荐直接用MoveIt Setup Assistant，一条命令就能拉起来：

ros2 run moveit_setup_assistant moveit_setup_assistant

在图形界面里加载你的URDF文件，然后按照向导依次完成：创建规划组（Planning Group），比如把机械臂的六个关节都放进一个叫做arm_group的组里，再把夹爪关节放进另一个组gripper_group；设置预定义位姿（Named Poses），比如home、upright这些常用的姿态；最后生成配置文件。这里有个很容易忽略的细节：如果机械臂的某些关节是冗余的（比如七轴臂），你需要在SRDF里明确哪些关节归入规划组，否则做逆运动学求解时MoveIt可能会把多余的关节当成固定关节处理，导致某些姿态解不出来。

规划组配置好之后，要重点确认碰撞检测矩阵。MoveIt默认用FCL库检查机械臂与场景中物体的碰撞，但碰撞矩阵的配置会直接影响规划的成败。如果某个连杆离另一个连杆很近但没有被排除在碰撞矩阵之外，规划器会频繁报错；如果错误地排除了本应该检测的碰撞对，又会造成实机碰撞的隐患。所以我要提醒一句：在仿真阶段可以通过增大膨胀系数给自己留安全余量，但实机部署时碰撞矩阵必须严格按真实几何尺寸来。

3.3 调用规划器实现RRT与A*切换

MoveIt 2本身不实现算法，它是用插件机制把底层规划器加载进来的。默认情况下，MoveIt会加载OMPL库，并把一组规划算法以规划管线（pipeline）的方式暴露出来。在自带的MoveIt配置里，你可以在RViz的MotionPlanning面板的Planning选项卡里下拉选择规划算法，里面会出现RRTConnect、RRTstar、BKPIECE、LBKPIECE等多种选项。我第一次看到这个下拉菜单时还以为它们只是名字花哨，后来查源码才明白，它们都是同一个规划管线的不同算法配置，实时切换非常方便。

如果你想在代码里指定用RRT*算法，可以用规划请求适配器（planning request adapter）来配置。在move_group的配置文件中，通常会有一个ompl_planning.yaml文件，里面为每个规划组列出可用的规划器列表，比如：

arm_group: planner_configs: - RRTConnect - RRTstar - KPIECE1

在Python或C++代码里调用时，通过设置motion_plan_request里的planner_id来指定：

from moveit_msgs.msg import MotionPlanRequest from moveit_msgs.srv import GetMotionPlan req = MotionPlanRequest() req.group_name = "arm_group" req.planner_id = "RRTstar" # 设置start_state、goal_constraints后调用服务

很多人会问：这里到底能不能用A*？答案是MoveIt的OMPL插件默认没有A*，因为OMPL定位是采样规划算法集，A属于离散搜索算法。如果你真要在MoveIt里用A，需要自己写一个规划适配插件，或者把你自己的A实现封装成MoveIt PlannerManager接口。这个工程量大且没必要，绝大多数机械臂场景用采样规划器就够。A更适合用于移动底盘的全局导航，那边用的navigation2框架才内置了基于栅格地图的A和Dijkstra。所以“在MoveIt里切换A”这个说法本身就不太成立，更合适的表述是：MoveIt里切换的是不同OMPL采样算法，RRT系列才是主战场。

4. 真实避障场景的优化记录

4.1 实验地形与参数设定

为了让调试过程更有说服力，我在仿真里搭了一个模拟工作台场景：机械臂底座固定在工作台的一端，目标位置在另一端，中间有一个竖向的障碍板，高度正好能挡住机械臂末端从水平方向直接过去。这种情况下，机械臂必须先抬高绕过障碍板，再下降到目标点，路径在关节空间里必须有明显的竖直方向变化。

我用的规划组是六自由度关节臂的arm_group，每个关节限位按常见工业机械臂的设定，第一关节范围是±170度，第二关节范围是-130度到+130度，其余关节按URDF默认。末端要到达的目标位姿设定为工作台上方某一点，并且要求末端姿态保持水平，这样机械臂必须以特定腕部姿态进入目标区域。规划时，通过ompl_planning.yaml把规划时间上限从默认的5秒改到10秒，把碰撞检测的托盘（如allowed collision matrix）保持默认，并在Scene中导入障碍板的三维模型。

这里有一个关键参数叫做goal_tolerance，它表示规划终点与目标约束之间的允许误差。如果goal_tolerance设得太紧，比如小于1e-4，RRT*会因为很难精确采样到目标附近的解而频繁失败；设得太松，又会让机械臂停在一个看起来不太对的位置。我最终把位置容差设为0.01米、姿态容差设为0.02弧度，这是一个兼顾成功率与精度的经验值。

4.2 问题出现：狭窄通道中的抖动与失败

实验跑起来以后，第一个问题不是规划失败，而是规划出来的路径在运动过程中出现明显的抖动——机械臂每个关节的角度一时间急转，听起来很吓人。我检查了路径点数量，发现RRT*生成的一条包含2000多个中间点的轨迹被后续的路径平滑模块处理时，平滑模块收缩率太大，导致部分路径点离障碍物太近，又被碰撞检测强行修正，最终形成局部抖动。

图就不画了，现象就是：机械臂末端在绕过障碍板的过程中，会在大约0.3秒内剧烈转向，之后又恢复正常。经过排查，问题出在路径规划器的采样步长设置上。采样步长（或称为扩展步长）决定了每次随机采样后从当前节点向新节点移动的最大距离。如果步长过大，生成的路径段可能横跨障碍物区域，碰撞检测会把这段路径直接标记为无效，算法就得反复重试；如果步长过小，路径点过多，平滑模块容易在密集的短线段之间产生过度约束。

我把扩展步长从默认的0.05弧度调小到0.02弧度，同时把路径平滑段数从默认的5提高到10，抖动明显减少。但随之而来的是规划时间变长，RRT*在单次规划里平均需要8到12秒才能找到可执行路径，这个速度对离线调试可以接受，对实时控制就不太够用了。

4.3 解决思路与参数调优

面对上面这种“效率和路径质量”的矛盾，我尝试了三条路线，最终都有效果，但各自的应用场景不同。

第一条路线是换算法。把RRT换成RRTConnect，规划速度立刻提升了一大截，单次规划时间降到1秒以内，路径长度也短了不少。代价是RRTConnect的路径不如RRT平滑，抖动依然存在，但配合平滑模块后实机表现尚可。如果你的机器人控制频率足够高（比如1kHz)，平滑后的RRTConnect路径通常就能满足大部分任务。

第二条路线是保留RRT*，但增加规划成功的优化次数。OMPL的RRT*有一个优化迭代参数，每次找到可行解后继续优化，让路径的代价逐渐下降。我把优化的最大迭代次数从1000提升到5000，最终得到的路径质量远超默认配置，但代价就是规划时间暴涨到20秒以上。这条路线适合离线生成路径后再执行的场景，比如喷涂、焊接这类路径一经生成就多次复用的工序。

第三条路线是给规划器加“引导采样”。OMPL里有个叫GoalSpace的技术，它会让随机采样更偏向目标区域附近，加速收敛。我在配置ompl_planning.yaml时给RRTstar加了一段引导采样的设置，把目标偏置概率从默认的5%调到20%。效果立竿见影，规划时间从20秒降到了6秒左右，路径质量几乎没受影响。这条路线是我最推荐的，因为它的成本最低，只改一个概率参数就能大幅改善RRT*的实用性。

5. 常见问题排查与经验心得

5.1 规划失败常见原因速查表

在MoveIt 2里跑运动规划，最让人沮丧的事就是前端规划按钮点下去，后端回一句“Failed to find a planning solution”。好在绝大多数失败不是算法不行，而是配置不对。我把这段时间遇到的典型问题整理成一个表格，方便你按图索骥。

还有一个新手必踩的坑是：场景里没有显式添加障碍物，但MoveIt自带的“允许碰撞矩阵”仍然把机械臂自己的一些连杆碰撞对标记为安全。这会导致机械臂在运动时可能出现“贯穿自身”的奇异路径。排查方法很简单：在RViz里勾选显示碰撞矩阵，看哪些连杆对是暗色的。正常情况下，相邻连杆之间应为绿色（忽略碰撞），但非相邻连杆之间如果出现大面积绿色，就要检查是否误配了碰撞矩阵。

5.2 我的避坑技巧与实测建议

跑完整条流程，我最大的体会是：运动规划工程里最难的往往不是算法本身，而是参数的“度”。什么叫“度”？比如采样步长，太大容易跳进障碍物，太小容易卡死在局部收敛；碰撞矩阵膨胀，太大导致路径低效甚至无解，太小导致实机碰撞风险。这些平衡没有公式，只能靠实验和记录。所以我强烈建议你每次调参时都把这些内容记录下来：算法类型、采样步长、目标偏置、规划时间、成功率、路径点数量、路径长度。记录到一定量后，你会发现自己对参数的判断速度快很多，而不是每次都在猜测。

第二个建议是，做避障调试时尽量不要一上来就上真实机器人。先用仿真环境把所有能撞的情况都撞一遍，至少要把路径平滑、碰撞检测、奇异点规避这几个模块在仿真里验证到“连续规划100次不出错”的阶段。为什么是100次？因为机械臂规划本身具有随机性，一次两次成功不代表算法稳定。我实测过，在狭窄场景下同一个目标位姿，RRT*规划50次大概会有2到3次失败，如果你对稳定性有要求，就必须通过加重试机制或者检查参数来把成功率推到99%以上。

第三个建议其实是个小技巧：当你发现某条路径在仿真里很好、实机上却抖动时，很可能是运动插补计算太快或者控制周期太长，导致相邻路径点的关节差分过大，控制器追不上。这种情况下不要急着调规划参数，先把执行速度降下来，或者给路径增加时间戳约束，让关节在相同时间内变化量更均匀。我之前遇到过机械臂走一条特别完美的RRT*路径，结果实机上一到高速段就肩关节啸叫，后来发现不是规划问题，而是时间参数没有按照控制器最大速度重算。

我这个项目最终是把RRTConnect作为默认规划器、RRT作为离线高质量规划器，并在MoveIt的ompl配置里给两个规划器都开了引导采样。这样的组合既保证了日常调试的高效，也满足了离线任务的路径质量要求。如果你也在做类似的工作，建议从这一套配置开始试，再根据你的机械臂几何尺寸和任务特点逐步调整。当然，如果你把目标场景换成移动机器人的楼层导航，那就要回到A的轨道上，用navigation2去配置全局代价地图和规划器，那是另一套完全不同的流程了。