简介这是一份围绕aubo i5机械臂与Intel Realsense D435i深度相机联动的抓取实践源码包面向机器人开发者、智能制造及机器视觉方向的技术人员可用于快速复现物体识别抓取完整流程。资源共3个文件包含inscode环境配置、html说明页及gitignore文件压缩包仅6KB轻量易用。目前已有107人学习。源码按抓取流程组织覆盖结构体定义、工件位置信息获取、临时工件坐标系发布、关键点位姿计算、机械臂初始化及moveit控制执行等核心步骤重点演示了如何将相机识别到的物体位姿通过坐标转换映射到机械臂基坐标系从而实现精确抓取。代码示例清晰简洁并附有流程总结可帮助理解视觉引导抓取的关键环节同时结合工程实际建议通过server或action机制触发检测与抓取为后续功能扩展提供了切实指导。 前阵子实验室进了一台遨博aubo i5协作臂手里正好还躺着一块Intel RealSense D435i深度相机。两样设备放一块很自然就会冒出个念头让机械臂看一眼桌面然后自动把目标物抓起来。这个需求听起来直接真动手才发现中间全是坎——相机内参、手眼标定、点云分割、逆解规划、TCP标定每一个环节掉链子最后的抓取动作就会歪。我前后折腾了三个周末才把整条流水线跑通代码也整理成了可运行版本。这篇文章把aubo i5与D435i抓取实践的原理、代码结构和踩坑过程从头到尾讲清楚。如果你也在做机械臂视觉抓取或者正打算给协作臂配一个深度相机这篇文章应该能帮你省下不少弯路。1. 为什么是aubo i5搭配D435i需求拆解与选型逻辑1.1 抓取系统的需求拆解动手之前先别急着连线先把系统到底要干什么拆清楚。我这套场景定义得很朴素一个固定工作台目标物比如纸箱放在桌面上D435i和机械臂都固定不动系统要做的是通过相机识别目标物位置控制机械臂末端移动到目标上方夹爪闭合抬起完成一次抓取。拆成模块看系统需要三部分能力感知从RGB图加深度图中找到目标物求它在相机坐标系下的三维位姿规划根据目标位姿计算机械臂末端应该去的位置和姿态也就是抓取点执行驱动aubo i5从当前位姿运动到抓取点完成夹取动作每个模块都有对应的坑。感知要处理深度噪声、物体表面材质对深度值的影响规划要保证目标点逆解存在、路径无碰撞执行则要保证TCP标定足够准。这三个模块看着独立实际耦合很深——感知给的位姿偏了规划执行全白费。1.2 硬件选型逻辑与局限选aubo i5有几个很实际的理由。它是六自由度协作臂工作半径886.5mm覆盖一张普通桌面绰绰有余负载5kg抓纸箱、小工件没有压力重复定位精度±0.02mm这个精度对视觉抓取来说非常充裕。更关键的是它原生支持ROS有对应的驱动和MoveIt配置省掉了自己写运动学逆解的麻烦。协作臂本身带力矩限制调试时人凑近一点也不需要太担心安全问题。D435i这边它是主动红外立体视觉方案不依赖环境光白天晚上都能出深度。内参出厂已经标定好读出来就能用省去了棋盘格标定内参这一步。深度范围0.2m到10m桌面抓取的工作距离在0.4m到0.8m之间恰好落在它精度最好的区间。内置IMU对后面做手眼标定、位姿融合也有扩展空间。但这套组合也有明显的局限。D435i对黑色吸光物体基本是深度瞎子反光表面深度值会跳变透明物体更不用想。这个是主动红外方案的物理极限不是调参能完全解决的。后面踩坑章节我会给出实用解法。1.3 手眼方案的总体设计手眼标定分两种eye-in-hand是相机装在机械臂末端跟着臂一起动eye-to-hand是相机固定在外部的某个位置。我选的是eye-to-hand相机用支架固定在工作台斜上方约45度俯视桌面。为什么这么选因为eye-to-hand标定一次外参就固定了机械臂运动过程中相机不会抖动深度配准更稳定调试时也更好定位问题。它的缺点是如果桌面场景有遮挡相机视野可能被机械臂挡住一部分所以摆放时要让目标和机械臂工作区域尽量都在视野中心附近。系统架构因此变得很清晰D435i固定、aubo i5固定、目标物放在两者视野和臂展的交叉区域内坐标变换是常数关系不随机械臂运动变化。2. 坐标系耦合手眼标定和像素到基座的换算2.1 eye-to-hand标定流程与核心代码手眼标定的本质是求相机坐标系到机械臂基坐标系的变换矩阵T_cam2base。因为相机和机械臂基座都固定这个矩阵是常数。eye-to-hand的标定方程是AXXB的形式机械臂末端固定一块标定板运动到多个不同位姿记录每组机械臂基座到末端的变换A以及相机到标定板的变换B然后用Tsai或Park算法解出相机到基座的X。标定板我用的是aruco码贴在机械臂末端法兰盘上。检测aruco在相机系下的位姿用OpenCV的solvePnP机械臂末端位姿通过MoveIt接口直接读。核心代码如下import cv2 import numpy as np def collect_handeye_samples(robot, camera, n_samples25): R_base2ee, t_base2ee [], [] R_cam2marker, t_cam2marker [], [] for i in range(n_samples): # 机械臂运动到预设位姿角度要有明显变化 robot.goto_joint_pose(PRESET_POSES[i % len(PRESET_POSES)]) # 读取机械臂末端位姿 R_ee, t_ee robot.get_ee_pose() # 检测aruco码在相机系下的位姿 rvec, tvec detect_aruco_marker(camera.get_color_frame()) if rvec is None: continue R_base2ee.append(R_ee) t_base2ee.append(t_ee) R_cam2marker.append(cv2.Rodrigues(rvec)[0]) t_cam2marker.append(tvec.reshape(3, 1)) R, t cv2.calibrateHandEye( np.array(R_base2ee), np.array(t_base2ee), np.array(R_cam2marker), np.array(t_cam2marker), methodcv2.CALIB_HAND_EYE_TSAI ) T_cam2base np.eye(4) T_cam2base[:3, :3] R T_cam2base[:3, 3] t.flatten() return T_cam2base这里有个很重要的经验标定数据不是采得越多越好而是要覆盖足够大的姿态变化。如果机械臂只在同一个方向小幅挪动旋转分量激励不足标定方程接近病态算出来的结果看着重投影误差很小实际抓取会偏得离谱。我建议让机械臂做俯仰、偏航、横滚都变化的大范围动作至少采20组有效数据。2.2 D435i深度数据的正确打开方式用realsense-ros驱动时有一个参数很关键align_depth。D435i的RGB成像和深度成像存在视差如果不做对齐RGB图上某个像素对应的深度值其实来自另一个视角直接采样会有偏差。在launch文件里这样配置launch include file$(find realsense2_camera)/launch/rs_camera.launch arg namealign_depth valuetrue/ arg namedepth_width value640/ arg namedepth_height value480/ arg namefps value30/ /include /launch对齐之后深度话题变成/camera/aligned_depth_to_color/image_raw每个像素的深度值和RGB像素一一对应。这个细节能省掉手动做立体校正的麻烦。另外D435i的深度单位是毫米mm换算成米要除以1000。别笑我亲眼见过有人在代码里把深度值当成米直接用目标点直接飞到十万八千里外。还有个细节深度图上的值代表相机光心到物体的深度在Z轴方向的分量不是欧氏距离直接用就是对的不需要额外换算。2.3 像素坐标到机械臂基坐标系坐标这是整个抓取里最核心的数学关系。给定RGB图像上的目标点(u, v)和该点的深度值d先恢复相机坐标系下的三维坐标def pixel_to_camera(u, v, d, fx, fy, cx, cy): z d / 1000.0 # D435i深度单位是mm x (u - cx) / fx * z y (v - cy) / fy * z return np.array([x, y, z])有了相机系坐标再乘手眼标定得到的变换矩阵就得到了机械臂基坐标系下的坐标def camera_to_base(pt_cam, T_cam2base): p np.array([pt_cam[0], pt_cam[1], pt_cam[2], 1.0]) pt_base T_cam2base.dot(p)[:3] return pt_base这一步看着简单但有一个容易混淆的点相机内参(fx, fy, cx, cy)一定要和图像的尺寸、分辨率匹配。比如你用的是1280x720的图像但内参是从640x480的分辨率读出来的算出来的坐标全部翻倍这个问题排查起来非常隐蔽。3. 抓取流水线源码解析感知、规划、执行如何协作3.1 源码结构与启动方式项目代码按功能拆成几个独立模块这样调试时能单独跑某一环。目录结构如下aubo_d435i_grasp/ ├── config/ │ ├── camera.yaml # 相机内参、深度阈值、滤波参数 │ ├── handeye.yaml # 手眼标定结果 T_cam2base │ └── grasp.yaml # 抓取参数接近高度、夹爪开合等 ├── launch/ │ └── grasp_pipeline.launch ├── scripts/ │ ├── camera_node.py # D435i数据获取与封装 │ ├── detect_target.py # 点云分割目标检测 │ ├── grasp_planner.py # 生成抓取点 │ ├── aubo_driver_node.py # aubo i5控制封装 │ └── main_grasp.py # 抓取主流程 └── README.md启动方式分两步先launch整个感知和控制环境再运行主流程脚本。主流程代码如下def main(): camera D435iCamera() detector TargetDetector() planner GraspPlanner() robot AuboArm() robot.move_to(home_pose) while not rospy.is_shutdown(): color, depth camera.get_frame() target detector.detect(color, depth) if target is None: continue pregrasp, grasp planner.generate(target) robot.move_to(pregrasp.pos, pregrasp.quat) # 先到预抓取点 robot.move_to(grasp.pos, grasp.quat) # 再到抓取点 robot.gripper_close() robot.move_to(home_pose) break # 当前版本做单次抓取3.2 视觉感知点云分割与目标位姿目标检测这里我没有选择跑深度学习目标检测网络而是用点云几何分割。原因很实际不依赖标注数据换个目标物不用重新训练而且桌面场景结构简单点云方法又快又稳。处理流程是一套标准流水线把对齐后的深度图转成点云去除深度值为0的无效点直通滤波只保留机械臂可抓取范围内的点0.3m到1.0mRANSAC平面分割把桌面平面剥离对剩余点做欧式聚类得到目标物点云簇计算点云簇质心和方向包围盒(OBB)得到6自由度位姿点云处理我用的open3d代码很简洁import open3d as o3d import numpy as np def segment_target(points_xyz): pcd o3d.geometry.PointCloud() pcd.points o3d.utility.Vector3dVector(points_xyz) # 平面分割去掉桌面 plane_model, inliers pcd.segment_plane( distance_threshold0.01, ransac_n3, num_iterations1000) obj_cloud pcd.select_by_index(inliers, invertTrue) # 欧式聚类取点数最多的簇作为目标 labels np.array(obj_cloud.cluster_dbscan(eps0.02, min_points20)) if len(labels) 0: return None largest_cluster_idx np.bincount(labels[labels 0]).argmax() target_cloud obj_cloud.select_by_index(np.where(labels largest_cluster_idx)[0]) return target_cloud平面分割的distance_threshold这个参数值得单独说。它决定什么样的点算作平面内点设大了会把目标物底部的点一起吞掉设小了桌面点云会被误判为多个平面。我调参的方法是可视化把分割结果和原始点云叠在一起看确保桌面完全去除且目标物完好。3.3 抓取点生成质心、姿态、接近方向拿到目标物点云就能算质心和OBBobb target_cloud.get_oriented_bounding_box() center obb.get_center() rot_mat np.array(obb.R) # 3x3方向矩阵 extent np.array(obb.extent) # 长宽高抓取逻辑上要生成两个位姿预抓取点和抓取点。预抓取点位于目标上方一定距离我设15cm机械臂先快速运动到这里抓取点在目标正上方夹爪闭合后正好包住目标。我默认实现的是垂直下抓也就是末端Z轴朝下接近。这种抓取方式对姿态约束最小逆解成功率最高非常适合第一版跑通流程。代码如下def generate_topdown_grasp(center, extent, gripper_offset0.05): # 抓取点目标中心上方加上夹爪中心到目标的间隙 grasp_pos np.array([ center[0], center[1], center[2] extent[2] / 2 gripper_offset ]) # 末端Z轴朝下的旋转矩阵 rot_mat np.array([[1, 0, 0], [0, 1, 0], [0, 0, -1]]) grasp_quat Rotation.from_matrix(rot_mat).as_quat() # 预抓取点沿Z轴后退15cm pregrasp_pos grasp_pos np.array([0, 0, 0.15]) return pregrasp_pos, grasp_quat, grasp_pos, grasp_quat这里gripper_offset需要根据实际夹爪来调。如果夹爪的钳口比较长闭合中心到目标顶面有一段距离offset要相应加大否则夹爪还没到位就被目标物顶住了。3.4 机械臂控制MoveIt与aubo i5驱动aubo i5的控制我用的是MoveIt加moveit_commander。MoveIt的好处是集成了运动规划、碰撞检测和逆解不用自己写IK求解器。import moveit_commander from geometry_msgs.msg import Pose class AuboArm: def __init__(self): moveit_commander.roscpp_initialize([]) self.move_group moveit_commander.MoveGroupCommander(aubo_i5) self.move_group.set_planning_time(5.0) self.move_group.set_planner_id(RRTConnectkConfigDefault) self.move_group.set_max_velocity_scaling_factor(0.5) def move_to_pose(self, position, quaternion): pose Pose() pose.position.x position[0] pose.position.y position[1] pose.position.z position[2] pose.orientation.x quaternion[0] pose.orientation.y quaternion[1] pose.orientation.z quaternion[2] pose.orientation.w quaternion[3] self.move_group.set_pose_target(pose) ok, plan, _, _ self.move_group.plan() if ok: self.move_group.execute(plan)有一点必须强调MoveIt的set_pose_target设置的是工具坐标系TCP的目标位姿不是法兰盘中心。所以机械臂末端一定要先配置好工具坐标系MoveIt才知道TCP相对法兰的偏移。如果TCP没标定或者标定错了后面所有抓取点都会整体偏移这时你会看到一个诡异的现象机械臂每次都能看准目标但每次抓都偏同样的距离。4. 实测踩坑记录从标定到抓取的六个典型问题4.1 黑色和反光物体让深度图直接罢工这是D435i在这个项目里暴露的最大问题。黑色物体对红外光吸收严重深度图对应区域全是空洞反光物体表面产生镜面反射深度值跳变剧烈。我第一次测试时图省事用黑色电工胶布在纸箱上贴了个标记结果深度图在胶布区域全部归零点云分割直接把这个区域当成背景丢弃了。实际项目里针对这个问题有几种解法开启发射器自动调节让D435i的laser_power跟随场景变化把相机位置放低一些让红外补光更充分地照射目标区域用RGB信息兜底先用颜色或边缘检测得到物体在图像中的mask把mask内深度为0的像素用周围有效深度值插值填补对规则目标物直接贴aruco标签用标签位姿代替点云质心aruco方案是最省心的检测鲁棒、位姿精度高代码量也小def detect_aruco_pose(color, camera_matrix, dist_coeffs, marker_id0): gray cv2.cvtColor(color, cv2.COLOR_BGR2GRAY) corners, ids, _ aruco.detectMarkers( gray, aruco.Dictionary_get(aruco.DICT_4X4_50)) if ids is None or marker_id not in ids: return None idx np.where(ids marker_id)[0][0] rvec, tvec, _ aruco.estimatePoseSingleMarkers( corners[idx], 0.05, camera_matrix, dist_coeffs) return rvec, tvec4.2 手眼标定误差会在抓取时放大标定结果看着很好平均重投影误差不到1像素但实际抓取时偏了差不多10mm。这个坑排查了很久问题根源是标定板本身不够平整。aruco码我用普通纸打印后贴在机械臂末端纸面有细微褶皱加上安装板没有绝对垂直于末端法兰标定板相对于末端的位姿存在一个固定偏差。这个偏差在标定求解时被平均进了外参里但真实抓取时它会以同一个方向误差的形式暴露出来。解决方法是双管齐下硬件上aruco板用刚性背板固定确保标定板平面和法兰面尽量平行软件上标定完成后用一组未参与标定的位姿做验证对比机械臂末端的理论位姿和相机检测到的标定板位姿两者误差稳定在2到3毫米内才合格4.3 奇异点、TCP标定和运动规划失败aubo i5在工作空间边界附近时逆解很容易不收敛MoveIt规划会报IK失败。我给MoveIt加了笛卡尔路径约束限制末端姿态在规划过程中不能剧烈变化同时把实际工作区域缩小到距底座中心50cm到70cm的一个环形区间避开边界区。TCP标定是另一个容易忽略的环节。一开始我以为把夹爪装上去就算完事结果抓取点总是偏。后来用四点法做了TCP标定让夹爪尖端从四个不同方向指向空间同一个固定点记录四组法兰位姿求解TCP相对法兰的固定偏移。这个问题最大的迷惑性在于TCP没标好时视觉和运动学看起来都是对的每次误差方向都一致只是距离不对。如果你遇到百抓百偏且误差方向恒定优先查TCP标定。4.4 深度单位与坐标系方向的小坑这类小坑通常最浪费时间。D435i深度单位是毫米我刚才已经提过。另一个坑是图像坐标系和点云坐标系的Y轴方向问题图像上Y轴向下而相机三维坐标系里Y轴通常向上或者按OpenCV惯例是向下取决于你用的库我第一次做转换时没注意这个方向差异目标点直接翻到了相机后方。推荐一个排查方法拿到点云后先可视化把点云和RGB图叠加显示确认图像上的物体位置和点云中的位置对应一致再进行坐标变换。这一步能省下大量的debug时间。5. 源码使用指南与二次开发建议5.1 快速跑通demo的步骤如果你拿到了源码按下面的顺序操作最快跑通环境准备Ubuntu 18.04加ROS Melodic安装librealsense、realsense-ros、aubo_robot驱动、aubo_i5_moveit_config、open3d和scipy先单独启动相机和机械臂确认话题都正常相机输出/camera/color/image_raw和/camera/aligned_depth_to_color/image_raw机械臂能通过MoveIt控制修改config/handeye.yaml填入你自己标定的T_cam2base矩阵不要用仓库里的默认值运行launch/grasp_pipeline.launch启动完整环境把一个白色纸箱放在相机视野中央、距离0.5m左右运行main_grasp.py第一次测试强烈建议用白色或者浅色纸箱表面不要有反光标签放在光线均匀的位置。这样能最大限度排除深度失效的干扰先把整个流程验证通。5.2 更换目标物和硬件时改哪里换成其他目标物时点云分割方案并不挑物体形状只要目标物和桌面在大小、高度上能区分开就行。如果场景变成多个物体堆叠聚类之后要增加一步目标筛选根据目标物的高度、点云数量、尺寸先验过滤掉不想要的簇。换机械臂时改动集中在两处aubo_driver_node.py里把MoveIt的group名改成新臂的名字以及MoveIt配置换成对应机械臂的包。底层运动学全部由MoveIt接管不需要自己改。换相机时要注意三件事内参矩阵一定要换成新相机的深度单位要确认是毫米还是米深度对齐方式要重新验证。不同相机的深度质量差异很大之前调好的平面分割参数很可能要重调。5.3 后续扩展方向这套流水线的骨架搭好之后可扩展的空间很大。我列几个正在考虑的方向一是增加侧向抓取根据目标物OBB的长宽比自动选择垂直下抓还是水平侧抓二是在机械臂末端再加一个相机接近目标时做精定位能显著提高抓取精度三是加入抓取失败重试机制夹爪闭合后如果检测到没有抓取到目标改变预抓取点高度再来一次四是把固定目标检测换成YOLO类网络对复杂场景的鲁棒性会更强。这次抓取实践做下来最大的体会是视觉抓取看起来是识别加运动两个大模块但实际上大量时间花在坐标系、标定、TCP这些看不见的环节上。设备之间的数学关系搞不清楚路径规划再花哨都是白搭。完整代码我已经整理到配套的项目包里需要的朋友直接拿去跑跑通后再按自己的场景改参数就行。最后再分享一个小技巧调试阶段不要一上来就本文还有配套的精品资源点击获取