简介面向ROS2机器人开发者的机械臂自主避障抓取项目代码解决YOLO目标检测、PCL点云处理与Moveit运动规划三者的集成问题。资源体积小巧共3个文件核心包括inscode工程配置、html说明文档及gitignore过滤规则压缩包仅7KB便于快速导入与理解整体框架。目前已有244人学习适合具有一定机器人基础、希望复现视觉抓取流程的研发人员。代码以奥比中光Gemini335相机获取图像与深度数据YOLO完成目标识别PCL生成三维点云并解析物体位姿Moveit据此规划避障轨迹驱动睿尔曼RM65机械臂执行抓取。资源提供完整工程与配置示例可直接运行或作为二次开发基底尤其适合用于工业自动化和服务机器人的原型验证。1. 为什么这套 ROS2 机械臂避障抓取方案值得照着拆一遍机械臂自主避障抓取听着门槛高拆开其实就是感知、规划、执行三条线的串通相机出点云点云里分出物体和障碍再把障碍注册到 MoveIt2 的规划场景最后规划出一条不碰障碍的轨迹落下去抓。这套 ROS2 机械臂自主避障抓取方案源码包把这整条链路做成了一个能直接启动的仿真闭环代码量不算大但模块边界清楚特别适合正在做机械臂抓取方向课程设计、毕业设计仿真或者想快速搭一套 MoveIt2 避障流程的开发者直接拿来改。不需要多深的机器人学基础但 ROS2 的话题、节点这些基本概念得过一遍不然后面调 TF 和规划参数时会比较吃力。2. 整体架构与运行环境先拆包再启动十分钟跑通闭环2.1 三个核心功能包的职责划分源码包按功能拆成五个目录这在 ROS2 工程里是标准做法职责清楚也方便你只替换其中一环而不影响其他模块。我拆过的类似项目里凡是能顺利跑通的基本都是这种边界划分。src/ ├── arm_description/ # 机械臂 URDF / SRDF / 网格模型 ├── arm_bringup/ # launch 文件与全局参数配置 ├── arm_perception/ # 点云预处理、障碍物检测与发布 ├── arm_planning/ # 规划场景操作、抓取位姿计算与执行 └── arm_gazebo/ # 仿真世界、深度相机传感器配置arm_description 里是机械臂模型的 URDF 和 MoveIt2 需要的 SRDF。前者描述连杆和关节的几何与动力学属性后者定义规划组和末端执行器链接这两个文件决定了 MoveIt2 能不能正确理解你的机械臂结构。arm_perception 负责吃进深度相机的点云处理完输出两类东西障碍物的几何描述和抓取目标的位姿估计。arm_planning 是核心逻辑所在把障碍物写进规划场景、求抓取姿态、调用规划器并执行轨迹。2.2 工作空间初始化与依赖处理拿到源码包之后第一步不是急着编译先把依赖项理清楚。常见做法是把五个目录整体放进src/然后用rosdep把缺失的系统依赖一次补齐再编译。export ROS_DISTROhumble source /opt/ros/$ROS_DISTRO/setup.bash mkdir -p ~/arm_ws/src cp -r 源码包路径/arm_* ~/arm_ws/src/ cd ~/arm_ws rosdep update rosdep install --from-paths src --ignore-src -r -y colcon build --symlink-install source install/setup.bash这里--symlink-install强烈建议在开发期带上它会让 Python 脚本以软链接方式挂进 install 目录改完代码不用重新 build 就能生效。rosdep install会把 MoveIt2、Gazebo 相关的依赖自动装好前提是你之前装过 ROS2 桌面完整版。如果个别依赖拉取失败优先检查 apt 源是否正常其次是确认系统里没有同时 source 多个 distro 导致环境变量混乱。2.3 一键启动并验证链路通断bringup 目录里的 launch 文件把 Gazebo、RViz2、感知节点和规划节点全串在一起。拿到包之后建议先完整启动一次确认链路是通的再逐步深入调试。cd ~/arm_ws source install/setup.bash ros2 launch arm_bringup sim_avoidance.launch.py正常启动后你会看到三个窗口Gazebo 里的机械臂和桌面物体、RViz2 里的 MoveIt2 规划界面、以及终端里滚动的话题日志。验证节点是否全部上线用ros2 node list核心话题用ros2 topic list确认重点检查/camera/depth/points、/planning_scene、/display_planned_path这三个话题是否存在。提示如果 RViz2 里机械臂模型是灰的日志报 No transform from [base_link] to [world]先别急着改代码等两三秒看 TF 是否发布出来这类问题多半是机器人状态发布器在启动顺序上晚于 RViz2。3. 感知模块点云预处理与障碍物注册的三个关键步骤3.1 深度点云的订阅与 QoS 匹配感知模块的输入是深度相机话题仿真环境里用 Gazebo 的 RGB-D 相机插件模拟默认话题是/camera/depth/points。订阅端的 QoS 必须跟相机发布端对齐否则会一直卡在 waiting for matching QoS 的状态。import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy from sensor_msgs.msg import PointCloud2 class CloudSubscriber(Node): def __init__(self): super().__init__(cloud_subscriber) qos QoSProfile( depth10, reliabilityReliabilityPolicy.BEST_EFFORT, historyHistoryPolicy.KEEP_LAST ) self.sub self.create_subscription( PointCloud2, /camera/depth/points, self.cloud_cb, qos ) def cloud_cb(self, msg): frame msg.header.frame_id self.get_logger().info(fReceived cloud in frame: {frame})深度相机用BEST_EFFORT是常见选择点云偶尔丢几帧不影响抓取实时性反而更重要。如果是激光雷达这种需要完整帧的数据就用RELIABLE。注意订阅回调里取出的frame_id后续所有位姿变换都依赖这个坐标系。如果相机的 frame 不在机械臂的 URDF 里挂着后面障碍物注册到规划场景时就会放错位置。3.2 点云滤波、平面移除与目标点云分割拿到原始点云后直接做聚类会把地面、桌边全部算进去。代码包里的处理流水线是标准三步体素下采样、直通滤波、RANSAC 平面分割。三步的顺序和参数直接影响后续物体识别的质量。// 基于 PCL 的处理流程按顺序执行 pcl::VoxelGridpcl::PointXYZRGB voxel; voxel.setInputCloud(cloud_in); voxel.setLeafSize(0.01f, 0.01f, 0.01f); // 1cm 体素边长 pcl::PassThroughpcl::PointXYZRGB pass; pass.setFilterFieldName(z); pass.setFilterLimits(0.1, 1.2); // 保留相机前方 0.1m ~ 1.2m pcl::SACSegmentationpcl::PointXYZRGB seg; seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.02); // 平面距离阈值 2cm体素边长 0.01m 是折中值再小点云密度大、聚类耗时成倍上涨再大细小物体直接被抹平。直通滤波的 z 轴范围要根据相机安装高度和机械臂工作半径来改我一般先在 RViz2 里打开点云看一眼目标物体的实际深度区间再定边界直接沿用默认值很容易把目标切掉。RANSAC 的平面距离阈值 0.02m 适合滤桌面但如果你要抓的物体正好贴在平面上阈值要收到 0.01m 以下否则目标和平面一起被当成背景删掉。3.3 障碍物以 CollisionObject 形式写入规划场景感知模块的最终输出是一组几何描述通过 PlanningSceneInterface 发布到 MoveIt2 的规划场景。代码包里物体的形状统一用立方体包围盒近似对大多数规则物体够用如果目标是不规则异形件可以换成网格模型但性能和稳定性会差不少。moveit::planning_interface::PlanningSceneInterface psi; moveit_msgs::msg::CollisionObject obj; obj.header.frame_id world; obj.id table_obstacle; shape_msgs::msg::SolidPrimitive box; box.type shape_msgs::msg::SolidPrimitive::BOX; box.dimensions {0.6, 0.8, 0.05}; geometry_msgs::msg::Pose pose; pose.position.x 0.5; pose.position.y 0.0; pose.position.z 0.025; obj.primitives.push_back(box); obj.primitive_poses.push_back(pose); obj.operation moveit_msgs::msg::CollisionObject::ADD; psi.applyCollisionObject(obj);header.frame_id必须用规划场景认识的坐标系通常是机械臂基座或 world不要直接用相机坐标系除非 TF 外参做过标定。另一个隐蔽问题是操作类型物体移走后必须发REMOVE消息否则碰撞体一直留在规划场景里路径规划会永远绕着已经不存在的障碍走。调试阶段可以ros2 topic echo /planning_scene --once确认碰撞物体的增删是否真正进入场景。4. 避障规划与抓取执行MoveIt2 接口的调用细节与参数选型4.1 规划组与运动学求解器的前置校验写规划请求代码之前先确认 SRDF 里的规划组名称和代码里用的一致。MoveIt2 对组名严格区分大小写名字对不上规划请求会直接抛出 No planning group found 异常这个问题在 5.1 节会详细展开。代码包默认的规划组名是arm_group末端执行器链接是tool_link如果你替换成自己的机械臂模型这两个字符串必须对应修改。运动学求解器方面代码包配置的是 KDL对六轴机械臂是够用的。如果你的机械臂带冗余自由度或者关节有奇异位形可以换 TRAC-IK求解成功率更高但耗时略涨。在 RViz2 的 Motion Planning 面板里手动拖一下末端目标如果能正常规划出路径说明求解器和规划组配置没问题再往下调代码。4.2 规划请求构造与避障参数选型MoveIt2 的规划请求通过 MoveGroupInterface 构造核心是设置规划时间、规划器 ID 和目标容差。这几个参数直接决定避障成功率和路径质量。moveit::planning_interface::MoveGroupInterface move_group(arm_group); move_group.setPlanningTime(5.0); move_group.setPlannerId(RRTConnectkConfigDefault); move_group.setGoalPositionTolerance(0.01); move_group.setGoalOrientationTolerance(0.05); geometry_msgs::msg::Pose target_pose; target_pose.orientation.w 1.0; target_pose.position.x 0.45; target_pose.position.y 0.1; target_pose.position.z 0.35; move_group.setPoseTarget(target_pose, tool_link); moveit::planning_interface::MoveGroupInterface::Plan plan; bool ok (move_group.plan(plan) moveit::core::MoveItErrorCode::SUCCESS); if (ok) { move_group.execute(plan.trajectory_); } else { RCLCPP_WARN(node-get_logger(), Plan failed, retrying...); }setPlanningTime(5.0)是规划失败前的最大搜索时间障碍物密集场景 5 秒是合理折中设太短路径经常找不到。规划器 IDRRTConnectkConfigDefault在避障场景下表现稳定多障碍时比 PRM 成功率高如果机械臂自由度较高可以换LBKPIECEkConfigDefault试试。位置容差 0.01m、姿态容差 0.05rad 是比较紧的配置实机部署建议放宽一倍换取成功率。参数仿真默认实机建议说明planning_time5.0s8.0s实机场景更复杂留更多搜索余量position_tolerance0.01m0.02m目标定位误差大时放宽orientation_tolerance0.05rad0.10rad姿态敏感度低可放宽max_velocity_scaling1.00.3实机必须降速否则抖动max_acceleration_scaling1.00.3同上保护电机4.3 抓取位姿计算与坐标系变换抓取位姿通常由感知模块给出物体在相机坐标系下的位置通过 TF 变换到机械臂基座坐标系再叠加上夹爪的接近姿态。这个环节最容易出问题的地方是姿态的 RPY 值夹爪朝向和物体坐标系没对齐时规划器会给出很奇怪的路径。tf2::Quaternion q; q.setRPY(0.0, M_PI / 2, 0.0); // 末端绕 Y 轴转 90 度竖直向下接近 geometry_msgs::msg::PoseStamped grasp_pose; grasp_pose.header.frame_id camera_link; grasp_pose.pose.position.x obj_x; grasp_pose.pose.position.y obj_y; grasp_pose.pose.position.z obj_z; grasp_pose.pose.orientation tf2::toMsg(q); // 阻塞等待 TF 可用的变换超时 1 秒 auto base_pose tf_buffer-transform(grasp_pose, base_link, tf2::durationFromSec(1.0)); move_group.setPoseTarget(base_pose, tool_link);这里的 RPY 组合要看夹爪安装方式和抓取面的朝向侧装夹爪需要绕 Y 轴转 90 度正装夹爪可能完全不需要旋转。这类参数是玄学重灾区我见过不少项目仿真里能抓一换机械臂就连姿态都规划不出来十有八九是坐标系旋转没对齐。调试方法很直接在 RViz2 里把目标位姿的坐标系箭头显示出来看 Y 轴方向是否垂直于抓取面。抓取完整流程是预抓取点、下探、闭合夹爪、抬升代码包实现了预抓取点到位姿这一段夹爪控制通过单独的 GripperAction 调用实机部署时速度缩放记得按前面表格调低。5. 避坑记录五个最容易卡住的实测炸点5.1 规划一直失败日志提示 No planning group found现象调用move_group.plan()返回失败终端日志明确提示规划组不存在。原因代码里写死的规划组名是arm_group但 SRDF 文件里实际定义的组名是manipulator。MoveIt2 对组名大小写敏感肉眼扫一遍很可能看不出差异。解决打开arm_description/config/arm.srdf用grep group name查出实际组名把代码字符串改成一致后重新编译。从那以后我拿到任何新包第一件事就是核对 SRDF 里的组名和代码常量不再凭印象。5.2 点云里有物体规划场景里却看不到碰撞体现象RViz2 里点云显示正常目标物体清晰可见但障碍物注册后规划场景里没有任何碰撞物体出现。原因CollisionObject 的header.frame_id填了camera_link而规划场景的根坐标系是base_link。要么相机到基座的 TF 变换没有发布要么发布的值是错的。解决跑ros2 run tf2_ros tf2_echo camera_link base_link确认变换是否存在。不存在就检查 URDF 里有没有定义相机的固定关节存在但坐标值明显不合理重新标定外参。这个问题在仿真里容易忽视因为 Gazebo 会直接给出正确的相机位姿实机上相机位置一动外参就漂了感知和规划就脱节。5.3 物体移走了机械臂还在绕远路现象抓取目标被拿走之后机械臂执行同样的目标位姿路径仍然明显绕行。原因碰撞物体只发了 ADD没发 REMOVE。PlanningSceneInterface 不会自动清理场景里的碰撞体除非收到对应 id 的 REMOVE 操作否则障碍物永久存在。解决感知节点里维护一个障碍物 id 列表每次检测到物体消失补发一条operation REMOVE的消息。我一般会在每轮感知循环里对比前后帧的障碍物集合有增有减就整体刷新一次规划场景比逐帧增量更新干净得多也避免残留。5.4 仿真里路径正常实机末端高频抖动现象同样的代码在 Gazebo 里执行顺畅换到实机后机械臂接近目标时出现高频抖动末端位置偏差肉眼可见。原因仿真里规划器的速度缩放默认 1.0实机电机响应和关节摩擦让实际轨迹滞后于规划轨迹。加上位置容差设得太紧控制器一直在追赶一个根本达不到的精确位置造成抖动。解决执行前调用setMaxVelocityScalingFactor(0.3)和setMaxAccelerationScalingFactor(0.3)同时把位置容差放宽到 0.02m。先把流程跑通再逐步收紧精度不要一上来就追求毫米级。5.5 Gazebo 仿真启动后前几秒点云是空的现象带相机传感器的仿真启动后/camera/depth/points话题有消息但点云内容为空持续好几秒。原因Gazebo 的 RGB-D 相机插件需要时间渲染场景而且深度相机的近裁剪面如果设得太大近距离物体会落到最小深度范围之外生成不了点。解决感知节点在 launch 文件里加execution_delay参数延迟 5~10 秒启动等相机出图稳定。同时把相机配置里的min_depth从默认 0.1m 改到 0.05m防止近距离盲区吞掉抓取目标。注意前两条坑最隐蔽很多人规划失败时怀疑算法参数实际整个链路里 TF 树和规划场景状态才是最大的黑匣子。排查顺序永远是先看 TF、再看规划场景、最后才动规划器参数。6. 进阶技巧用自动化回归脚本验证避障抓取改动调试到这里仿真闭环应该已经稳定跑通了。进入实机之前我建议把验证过程固化成自动化脚本每次改完感知阈值或规划容差就跑一遍而不是在 RViz2 里靠肉眼判断路径好坏。回归验证的思路很简单准备一组已知目标点每个点标注期望的规划结果批量跑。#!/usr/bin/env python3 # 回归测试脚本批量规划并校验结果是否符合预期 VALID_TARGETS [ {x: 0.45, y: 0.0, z: 0.35, expect_success: True}, {x: 0.30, y: -0.2, z: 0.20, expect_success: True}, {x: 0.60, y: 0.3, z: 0.40, expect_success: False}, # 障碍物正上方 ] def run_case(target): # 构造 PoseStamped 并发送规划请求 # 返回 move_group.plan() 的结果状态 ... for case in VALID_TARGETS: result run_case(case) assert result case[expect_success], \ f目标点 ({case[x]}, {case[y]}, {case[z]}) 结果异常第三个用例放在障碍物正上方预期规划失败——这是为了防止你在调参过程中把碰撞检测顺手关掉那种隐性问题肉眼很难发现但脚本一分钟能试十几个点位。跑完成功失败校验后还有一个容易被忽略的检查记录每次规划的轨迹点数和总耗时。如果同一目标点从 200 毫秒涨到 800 毫秒说明某些改动让规划器绕路了即使最终能到目标这个变化也值得追一下原因。这套源码包完整内容可以从资源下载页直接拿到里面包含了本章提到的全部节点实现、launch 文件和模型配置按第 2 章的步骤编译即可跑通。从那以后我每次改完感知阈值或规划容差都强制走一遍回归脚本确认所有用例的期望结果没变化再谈实机部署。省掉了太多在 RViz2 里手动拖目标点的时间希望帮到你。本文还有配套的精品资源点击获取
