aubo i5与D435i视觉抓取实战：手眼标定到点云抓取全流程

简介：这是一份围绕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的标定方程是AX=XB的形式：机械臂末端固定一块标定板，运动到多个不同位姿，记录每组机械臂基座到末端的变换（A），以及相机到标定板的变换（B），然后用Tsai或Park算法解出相机到基座的X。

标定板我用的是aruco码，贴在机械臂末端法兰盘上。检测aruco在相机系下的位姿用OpenCV的solvePnP，机械臂末端位姿通过MoveIt接口直接读。核心代码如下：

import cv2 import numpy as np def collect_handeye_samples(robot, camera, n_samples=25): 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), method=cv2.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 name="align_depth" value="true"/> <arg name="depth_width" value="640"/> <arg name="depth_height" value="480"/> <arg name="fps" value="30"/> </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 视觉感知：点云分割与目标位姿

目标检测这里我没有选择跑深度学习目标检测网络，而是用点云几何分割。原因很实际：不依赖标注数据，换个目标物不用重新训练，而且桌面场景结构简单，点云方法又快又稳。

处理流程是一套标准流水线：

1. 把对齐后的深度图转成点云，去除深度值为0的无效点
2. 直通滤波，只保留机械臂可抓取范围内的点（0.3m到1.0m）
3. RANSAC平面分割，把桌面平面剥离
4. 对剩余点做欧式聚类，得到目标物点云簇
5. 计算点云簇质心和方向包围盒(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_threshold=0.01, ransac_n=3, num_iterations=1000) obj_cloud = pcd.select_by_index(inliers, invert=True) # 欧式聚类，取点数最多的簇作为目标 labels = np.array(obj_cloud.cluster_dbscan(eps=0.02, min_points=20)) 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 抓取点生成：质心、姿态、接近方向

拿到目标物点云，就能算质心和OBB：

obb = 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_offset=0.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_id=0): 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, tvec

4.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的步骤

如果你拿到了源码，按下面的顺序操作最快跑通：

1. 环境准备：Ubuntu 18.04加ROS Melodic，安装librealsense、realsense-ros、aubo_robot驱动、aubo_i5_moveit_config、open3d和scipy
2. 先单独启动相机和机械臂，确认话题都正常：相机输出/camera/color/image_raw和/camera/aligned_depth_to_color/image_raw，机械臂能通过MoveIt控制
3. 修改config/handeye.yaml，填入你自己标定的T_cam2base矩阵，不要用仓库里的默认值
4. 运行launch/grasp_pipeline.launch，启动完整环境
5. 把一个白色纸箱放在相机视野中央、距离0.5m左右，运行main_grasp.py

第一次测试强烈建议用白色或者浅色纸箱，表面不要有反光标签，放在光线均匀的位置。这样能最大限度排除深度失效的干扰，先把整个流程验证通。

5.2 更换目标物和硬件时改哪里

换成其他目标物时，点云分割方案并不挑物体形状，只要目标物和桌面在大小、高度上能区分开就行。如果场景变成多个物体堆叠，聚类之后要增加一步目标筛选，根据目标物的高度、点云数量、尺寸先验过滤掉不想要的簇。

换机械臂时改动集中在两处：aubo_driver_node.py里把MoveIt的group名改成新臂的名字，以及MoveIt配置换成对应机械臂的包。底层运动学全部由MoveIt接管，不需要自己改。

换相机时要注意三件事：内参矩阵一定要换成新相机的，深度单位要确认是毫米还是米，深度对齐方式要重新验证。不同相机的深度质量差异很大，之前调好的平面分割参数很可能要重调。

5.3 后续扩展方向

这套流水线的骨架搭好之后，可扩展的空间很大。我列几个正在考虑的方向：一是增加侧向抓取，根据目标物OBB的长宽比自动选择垂直下抓还是水平侧抓；二是在机械臂末端再加一个相机，接近目标时做精定位，能显著提高抓取精度；三是加入抓取失败重试机制，夹爪闭合后如果检测到没有抓取到目标，改变预抓取点高度再来一次；四是把固定目标检测换成YOLO类网络，对复杂场景的鲁棒性会更强。

这次抓取实践做下来，最大的体会是：视觉抓取看起来是"识别加运动"两个大模块，但实际上大量时间花在坐标系、标定、TCP这些"看不见"的环节上。设备之间的数学关系搞不清楚，路径规划再花哨都是白搭。完整代码我已经整理到配套的项目包里，需要的朋友直接拿去跑，跑通后再按自己的场景改参数就行。

最后再分享一个小技巧：调试阶段不要一上来就

本文还有配套的精品资源，点击获取