★ FEATURED ARTICLE 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这样的名字它们都是OMPLOpen 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 规划算法对比与选型建议光说原理还不够我把这两种算法在实际项目中的表现列成一个对比表方便按需选择。要注意的是这里的对比是基于我在仿真和实机上的经验不能当作绝对数值因为不同的场景参数会改变算法表现。维度RRT / RRT*A*适用空间高维连续空间、多关节机械臂二维或三维的离散网格、路图最优性原始RRT不保证最优RRT*渐进最优保证启发式一致时得到最优解地图依赖只需要碰撞检测接口不需要显式地图必须存在可遍历的图或栅格计算特点随机采样开销大但对高维空间扩展友好节点数多时内存消耗和搜索时间增长明显典型场景机械臂避障、行走机器人的全身运动规划移动机器人导航、扫地机路径规划我个人的选型习惯是移动底盘导航一律走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先用自带模型验证环境是否装好这是我想对所有新手说的第一条建议。环境装好以后跑一下自带的demoros2 launch moveit_resources_panda_moveit_config demo.launch.py这个launch文件会启动机器人的状态发布、MoveIt核心节点、RViz界面和一个用于交互的MotionPlanning插件面板。如果这个demo能正常显示机械臂模型并且你能用拖拽的方式设置目标位姿并触发规划说明环境基本没问题。很多人在这一步就会卡住最常见的原因是把不同发行版的包混着装比如在Humble上用了Galactic时代的MoveIt包。别问我是怎么知道的都是泪。3.2 配置机器人模型与规划组MoveIt 2工作的核心对象不是机械臂的URDF本体而是基于URDF生成的SRDFSemantic Robot Description Format。SRDF定义了哪些关节属于一个规划组、哪些连杆是末端执行器、哪些关节是可以被规划器控制的。只有正确配置了SRDFMoveIt才知道你要让机械臂的哪些部分参与运动规划。如果你是给模型配置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-4RRT*会因为很难精确采样到目标附近的解而频繁失败设得太松又会让机械臂停在一个看起来不太对的位置。我最终把位置容差设为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”。好在绝大多数失败不是算法不行而是配置不对。我把这段时间遇到的典型问题整理成一个表格方便你按图索骥。现象常见原因排查手段规划总是失败没有任何路径目标约束与当前机械臂运动学不可达用IK求解接口单独验证目标位姿是否可解规划偶尔成功但路径明显穿模碰撞检测矩阵中对应碰撞对被错误排除检查SRDF中的allowed collision matrix恢复可疑排除项规划成功但速度很慢采样步长过小或优化迭代次数过多适当增大步长、减少优化迭代、改用RRTConnect规划出的路径抖动剧烈平滑参数与原始路径点密度不匹配调整路径平滑收缩率或降低平滑权重启动demolaunch时报找不到moveit配置文件MoveIt配置目录未正确安装在install下用colcon build重新构建确认配置文件被复制到install还有一个新手必踩的坑是场景里没有显式添加障碍物但MoveIt自带的“允许碰撞矩阵”仍然把机械臂自己的一些连杆碰撞对标记为安全。这会导致机械臂在运动时可能出现“贯穿自身”的奇异路径。排查方法很简单在RViz里勾选显示碰撞矩阵看哪些连杆对是暗色的。正常情况下相邻连杆之间应为绿色忽略碰撞但非相邻连杆之间如果出现大面积绿色就要检查是否误配了碰撞矩阵。5.2 我的避坑技巧与实测建议跑完整条流程我最大的体会是运动规划工程里最难的往往不是算法本身而是参数的“度”。什么叫“度”比如采样步长太大容易跳进障碍物太小容易卡死在局部收敛碰撞矩阵膨胀太大导致路径低效甚至无解太小导致实机碰撞风险。这些平衡没有公式只能靠实验和记录。所以我强烈建议你每次调参时都把这些内容记录下来算法类型、采样步长、目标偏置、规划时间、成功率、路径点数量、路径长度。记录到一定量后你会发现自己对参数的判断速度快很多而不是每次都在猜测。第二个建议是做避障调试时尽量不要一上来就上真实机器人。先用仿真环境把所有能撞的情况都撞一遍至少要把路径平滑、碰撞检测、奇异点规避这几个模块在仿真里验证到“连续规划100次不出错”的阶段。为什么是100次因为机械臂规划本身具有随机性一次两次成功不代表算法稳定。我实测过在狭窄场景下同一个目标位姿RRT*规划50次大概会有2到3次失败如果你对稳定性有要求就必须通过加重试机制或者检查参数来把成功率推到99%以上。第三个建议其实是个小技巧当你发现某条路径在仿真里很好、实机上却抖动时很可能是运动插补计算太快或者控制周期太长导致相邻路径点的关节差分过大控制器追不上。这种情况下不要急着调规划参数先把执行速度降下来或者给路径增加时间戳约束让关节在相同时间内变化量更均匀。我之前遇到过机械臂走一条特别完美的RRT*路径结果实机上一到高速段就肩关节啸叫后来发现不是规划问题而是时间参数没有按照控制器最大速度重算。我这个项目最终是把RRTConnect作为默认规划器、RRT作为离线高质量规划器并在MoveIt的ompl配置里给两个规划器都开了引导采样。这样的组合既保证了日常调试的高效也满足了离线任务的路径质量要求。如果你也在做类似的工作建议从这一套配置开始试再根据你的机械臂几何尺寸和任务特点逐步调整。当然如果你把目标场景换成移动机器人的楼层导航那就要回到A的轨道上用navigation2去配置全局代价地图和规划器那是另一套完全不同的流程了。
阅读完成 · 觉得有帮助？
