简介面向机械臂视觉抓取开发者的可运行源码包整合aubo i5机械臂与Intel RealSense D435i深度相机的识别抓取实践。压缩包仅6KB内含3个文件以html说明与inscode工程配置为主附带gitignore版本管理配置结构精简便于快速查阅与复用。已有108人学习下载。资源完整梳理抓取流程借助相机获取物体位姿并建立临时工件坐标系通过坐标转换将关键点映射至机械臂基座标系再经由moveit控制执行抓取。代码示例覆盖结构体定义、工件位置信息获取、坐标系发布、关键点位姿计算及机械臂初始化执行等全链路为工程开发提供可直接参考的实现框架。资源同时建议实际项目采用server或action机制触发检测与抓取对提升响应速度具有实用价值。1. 抓取demo跑通不是终点真正让机器人抓到目标才是抓取demo跑通不是终点真正让机器人抓到手的物体才是。很多人在AUBO i5和D435i这套组合里栽跟头问题不在神经网络而在坐标系D435i最终给你的是相机坐标系下的三维点云AUBO i5要的是机器人基座坐标系下的目标位姿中间隔着手眼矩阵。这个矩阵标错一厘米视觉检测再准也会抓空。针对“aubo i5与D435i抓取实践[可运行源码]”这个方向我围绕硬件选型、手眼标定、点云分割、抓取位姿生成和现场调试的坑做一次完整拆解。适合有ROS基础、想让机械臂具备视觉抓取能力的开发者也适合那些已经跑通源码但抓取成功率不稳定的朋友对照排查。2. 手眼标定与相机配置坐标系对了抓取就成功一半2.1 眼在手上还是眼在手外抓取场景的选型差异AUBO i5是六轴协作机器人D435i是双目结构光深度相机。两者组合后第一个要决定的问题不是算法而是相机装在哪里。眼在手外Eye-to-Hand指相机固定在工作台支架上视野覆盖整个工作区域眼在手上Eye-in-Hand则把相机装在机械臂末端法兰上视角随手臂移动。桌面抓取场景我一般会固定安装相机。原因很直接D435i的深度在移动状态下会受到振动和加速影响眼在手上标定得更勤而且机械臂接近目标时相机视角会被手臂本身挡住。眼在手外的视野固定标定一次后只要相机和底座不松动就能长期使用。代价是工作区域有限物体进入遮挡区就检测不到。手眼标定解决的是相机坐标系和机器人基座坐标系的变换关系。对眼在手外标定板固定? 不标定板固定在机器人末端或固定在桌上核心是求解 AX XB。常见做法是使用ArUco码贴在机器人末端法盘上机器人走多个姿态相机检测标定板在相机系下的位姿同时记录机器人控制器给出的末端在基座系的位姿。import cv2 import numpy as np # robot_poses: 机器人末端在基座系下的齐次矩阵列表 (Nx4x4) # cam_poses: ArUco标定板在相机系下的齐次矩阵列表 (Nx4x4) # 两组数据一一对应姿态需要覆盖不同旋转方向 R_gripper2base [] t_gripper2base [] R_target2cam [] t_target2cam [] for r_gb, c_tc in zip(robot_poses, cam_poses): R_gripper2base.append(r_gb[:3, :3]) t_gripper2base.append(r_gb[:3, 3]) R_target2cam.append(c_tc[:3, :3]) t_target2cam.append(c_tc[:3, 3]) # Tsai方法是工程里最稳定的求解方法 R_cam2gripper, t_cam2gripper cv2.calibrateHandEye( R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, methodcv2.CALIB_HAND_EYE_TSAI)代码逻辑是把手眼标定转化为两组旋转矩阵的方程关系机器人末端到基座的变换、标定板到相机的变换、再加上未知的相机到末端变换。cv2.calibrateHandEye直接解这个闭环方程。采集数据时注意三点第一采集姿态数不要少于15组少了旋转矩阵的约束不足第二姿态要覆盖不同的俯仰和偏航只平移标定板会导致求解退化第三每组姿态之间尽量拉开角度比如绕X轴转20度、绕Y轴转30度交替。标定过程中机械臂速度放慢避免运动模糊导致ArUco角点检测偏移。眼在手外的最终标定结果是相机在基座系下的固定变换。求解出来的R_cam2gripper是相机相对末端的变换还需要通过当前机器人位姿的正向运动学把它转换到基座下。我一般会在标定脚本里多加一步把标定时刻的末端位姿记录下来然后按base_T_camera base_T_gripper gripper_T_camera算出来。这里的gripper_T_camera就是手眼标定的输出。这一步最容易出错后面的避坑章节会专门展开。2.2 D435i点云与RGB对齐分辨率、曝光和深度单位D435i的深度原理是双目匹配加红外投影补光输出的深度图和RGB图来自不同传感器直接叠加会有错位。抓取场景必须做点云对齐否则目标物体边缘会有一圈错误点云直接导致聚类尺寸偏大。import pyrealsense2 as rs pipe rs.pipeline() cfg rs.config() cfg.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) profile pipe.start(cfg) # 深度单位默认是0.001米即1mm读出的原始像素值乘0.001得到米 sensor profile.get_device().first_depth_sensor() sensor.set_option(rs.option.enable_auto_exposure, 0) sensor.set_option(rs.option.depth_units, 0.001) align_to rs.stream.color align rs.align(align_to) frames pipe.wait_for_frames() aligned_frames align.process(frames) depth_frame aligned_frames.get_depth_frame() color_frame aligned_frames.get_color_frame()这段配置里有几个参数会直接影响抓取稳定性。第一个是depth_units如果把它设成0.0001深度值精度更高但范围缩小抓取工作台距离超过1米时会出现深度溢出推荐保持0.001。第二个是自动曝光默认开启在室内场景会频繁波动尤其是目标物体移动时画面亮度突变导致深度跳变手动固定曝光后点云稳定很多。第三个是rs.align的作用它把深度图重投影到彩色图视角生成的彩色点云每个点同时带RGB和深度后续平面分割和聚类可以直接用对齐后的点云。工程里还有一个容易被忽略的坑D435i的红外投影在室外强光下基本失效因为阳光的红外分量会淹没投影散斑。抓取工位尽量放在室内避免阳光直射。如果工作台上放的是玻璃杯、金属罐这类反光物体点云会出现大面积空洞后面避坑章节会讲具体应对。3. 点云分割与抓取位姿生成从一堆点云到机器人能用的xyz3.1 平面分割与目标聚类先去掉桌面再谈目标D435i输出的点云包含了工作台、背景墙和所有目标物体。机器人不可能去抓整张桌子所以第一步是把桌面平面拟合出来并剔除掉剩下的点云才是候选物体。import open3d as o3d import numpy as np pcd o3d.geometry.PointCloud() pcd.points o3d.utility.Vector3dVector(points) # points: Nx3单位米 pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], # 注意相机坐标系的Y轴通常向下 [0, 0, -1, 0], [0, 0, 0, 1]]) # RANSAC平面拟合桌面的法向量通常是z轴方向 plane_model, inliers pcd.segment_plane(distance_threshold0.015, ransac_n3, num_iterations1000) table_point plane_model[:3] * plane_model[3] objects_pcd pcd.select_by_index(inliers, invertTrue) # 去除离群点避免桌面边缘的噪声点参与聚类 objects_pcd objects_pcd.remove_statistical_outlier(nb_neighbors20, std_ratio2.0) labels objects_pcd.cluster_dbscan(eps0.02, min_points30) for label in np.unique(labels): if label -1: continue cluster objects_pcd.select_by_index(np.where(labels label)[0]) work_on_cluster(cluster)这段代码的关键参数有三个。distance_threshold0.015是点到拟合平面的最大距离桌面平整且点云噪声小时放0.01桌面有垫布或点云质量差时放到0.025太大容易把贴近桌面的物体点云也吞进平面。cluster_dbscan的eps0.02是聚类半径单位是米它决定了多近的点会被归为一类。物体间的间距小于2厘米时容易粘成一片物体表面有孔洞时又会被拆成多块这个值要按实际目标尺寸去调。min_points30是过滤小杂点的最少点数防止把桌面灰尘和相机噪声也当成目标。平面分割的另一个作用是拿到桌面法向量这个向量会作为抓取姿态的基准方向。注意plane_model返回的是(a, b, c, d)法向量是(a, b, c)但RANSAC拟合出的法向量方向可能是朝下也可能是朝上需要跟相机坐标系的Z轴做点积小于0就取反。3.2 抓取点与抓取姿态计算质心坐标加上方向约束聚类得到的是物体表面点云不是物体位姿。常见做法是用PCA提取点云的主轴方向作为抓取方向再用质心作为抓取点。实际机器人抓取还需要一个约束夹爪的接近方向必须沿桌面法向量这样才不会斜着戳进物体。def grasp_pose_from_cluster(cluster_pcd, base_T_camera): pts np.asarray(cluster_pcd.points) centroid np.mean(pts, axis0) # PCA提取主轴 cov np.cov(pts.T) eigvals, eigvecs np.linalg.eigh(cov) # 点云分布最广的方向作为夹爪开合方向 x_axis eigvecs[:, -1] # 桌面法向量作为接近方向 z_axis np.array([0.0, 0.0, 1.0]) y_axis np.cross(z_axis, x_axis) y_axis / np.linalg.norm(y_axis) x_axis np.cross(y_axis, z_axis) # 质心转换到机器人基座系 centroid_camera np.append(centroid, 1.0) centroid_base base_T_camera centroid_camera R_matrix np.column_stack([x_axis, y_axis, z_axis]) return centroid_base[:3], R_matrix这段代码里质心坐标转换是重头戏。base_T_camera就是第2章手眼标定的最终结果它是一个4x4齐次矩阵。所有相机系下的点云坐标都要先拼上1变成齐次坐标再左乘这个矩阵才能得到机器人能直接使用的基座系坐标。x_axis取PCA最大特征值对应的主方向表示物体在哪个方向延伸最长夹爪开合方向通常沿着它。但PCA主轴有两个方向正反都可能需要在代码里加一个判断如果主轴方向与夹爪开合的物理参考方向夹角太大就取反或换次轴。还有一个细节点云噪声会让质心偏离真实几何中心尤其是物体表面有大片空洞时。我一般在计算质心前先对聚类点云做体素下采样比如voxel_size0.01这样能让点云密度均匀化避免相机近距离处点云过密把质心拉偏。抓取姿态生成后还不能直接发指令给机器人。工程上要加一层验证判断夹爪开合宽度是否大于物体在开合方向上的投影尺寸否则夹爪闭合时根本碰不到物体表面。这个宽度可以由聚类点云的最小包围盒估算代码里用np.min(pts x_axis)和np.max(pts x_axis)的差就能算出来。4. 环境搭建与跑通可运行源码的最小步骤4.1 依赖安装ROS、RealSense驱动和AUBO驱动这套抓取实践最常见的运行环境是Ubuntu加ROS我用Noetic版本做说明。准备工作分三块RealSense相机驱动、AUBO机械臂驱动、点云处理库。# 安装RealSense ROS驱动和点云处理依赖 sudo apt install ros-noetic-realsense2-camera sudo apt install ros-noetic-rgbd-launch sudo apt install python3-open3d python3-pyrealsense2 # AUBO驱动编译 sudo apt install ros-noetic-moveit ros-noetic-industrial-core cd ~/catkin_ws/src # 将aubo_ros源码放入src后编译 catkin_makeROS Noetic对应Python3所以点云处理库用python3-open3d。RealSense的ROS驱动安装后会自动发布/camera/color/image_raw和/camera/depth/color/points等话题。AUBO驱动编译前要先确认机器人控制器IP和网络连通性工程上常见做法是用网线直连机械臂控制柜和工控机网段配置成和控制柜一致。驱动节点启动后会发布/aubo_robot/move_group等MoveIt接口抓取节点通过MoveIt发送笛卡尔空间目标位姿。源码包里的抓取节点一般遵循同样的结构订阅点云话题调用手眼矩阵换算发布抓取位姿到MoveIt的规划接口。跑通前先确认两个话题能收到数据rostopic hz /camera/depth/color/points应该稳定输出15Hz左右rostopic pub /aubo_robot/set_io这类简单指令能收到机器人的反馈。4.2 跑通抓取Demo的三条命令# 终端1启动RealSense相机 roslaunch realsense2_camera rs_camera.launch \ depth_fps:30 color_fps:30 \ align_depth:true # 终端2启动AUBO机械臂驱动 roslaunch aubo_ros i5_moveit.launch \ robot_ip:192.168.1.50 \ sim:false # 终端3启动抓取节点加载手眼标定文件 rosrun aubo_grasp grasp_node.py \ _camera_to_base_file:${HOME}/calib/cam_to_base.yaml第一条命令启动相机align_depth:true让驱动直接发布对齐到彩色图的点云省去在Python里做对齐的步骤。第二条命令启动机械臂驱动robot_ip换成控制柜实际IPsim:false表示连接真实机器人如果是初次调试可以先sim:true做纯仿真验证。第三条命令是抓取主程序它通过参数文件读取手眼标定矩阵不需要改代码就能切换标定结果。抓取节点启动后可以在RViz里看两个关键点彩色点云里目标物体验证分割结果是否正确机器人模型上是否出现一个箭头一样的抓取目标姿态。我的习惯是先让机械臂停在安全高度把抓取目标发布到一个虚拟小物体上确认规划轨迹没有碰撞再放真实物体。源码里的抓取节点一般都有调试开关输出每个聚类物体的质心坐标和抓取姿态的四元数这些打印信息比看屏幕更直接。4.3 必调参数表从初始值到工程值参数名推荐初始值现场调整依据深度分辨率640x480物体小且精度要求高时试1280x720但点云计算会变慢平面分割阈值0.015m桌面不平或有点状凹陷时上调到0.025聚类最小点数30有细小物体螺丝、垫圈时降到10抓取接近速度0.03m/s易碎物体降到0.01金属件可提到0.05抓取高度补偿0.02m夹爪实际中心低于法兰时按TCP偏差调整允许规划超时1.0s工作环境复杂时调到2.0避免路径规划失败直接退出速度参数是机器人抓取比仿真多出来的关键维度。AUBO i5是协作机器人自带碰撞检测但抓取接近阶段速度过快会让物体被碰倒而不是被抓起来。我一般把接近段分成两段从安全高度到工作台上方10厘米处速度0.1m/s从10厘米到接触点速度降到0.03m/s。源码里如果只有一个速度参数那就让它控制末端接近段的线速度粗略加速度直接设成0.05m/s²。5. AUBO i5与D435i抓取实践避坑标定、点云与安全保护5.1 手眼标定结果用不了坐标系方向搞反现象手眼标定完成后把相机点云里的目标坐标发给机器人机械臂朝着目标相反的方向移动或者在RViz里机械臂模型和目标点明显错位。原因眼在手外模式下求解出的变换是相机相对末端的位姿但抓取节点需要的是相机相对机器人基座的位姿。很多源码直接把这个矩阵当作base_T_camera用等于漏乘了一次机器人当前末端位姿。还有一种情况是标定板安装在机器人末端求解出的R_target2cam方向定义反了导致旋转轴上整体转180度。解决第一标定脚本里一定要输出完整的转换链路写成base_T_camera base_T_gripper_calib gripper_T_camera其中base_T_gripper_calib是标定采样时记录的末端位姿gripper_T_camera才是手眼标定函数返回的结果。第二在RViz里先加载相机点云再加载机械臂模型开启TF显示看/camera_link是否出现在机械臂基座附近的合理位置。第三用一个已知位置物体验证把某个物体放在机械臂工作空间内读取相机视觉坐标用手眼矩阵转换后发给机器人机器人末端应指向该点。5.2 D435i点云在强光下出现大面积空洞现象光源充足时工作台中心区域深度图上出现黑色空洞物体表面点云像被啃掉一块聚类后物体尺寸偏小。原因D435i的红外投影散斑在强光下被环境红外光淹没双目匹配找不到对应点。另一个常见原因是物体表面光滑比如金属罐、磨砂塑料外壳红外光发生镜面反射入射光没有回到相机传感器。解决第一步改变环境把光源改成柔光灯箱或者拉上遮光帘避免阳光和射灯直射工作台。第二步设置深度传感器的laser_power参数在室内低光环境下可以调高投影强度但强光下调高也没作用。第三步是工程上最直接的土办法在反光物体表面贴哑光胶带实验场景里这比调算法快得多。代码层面还有一个针对空心点云的兜底对深度图做形态学闭运算把空洞用周围像素的中值填充但这种方法只适用于小洞大空洞必须从物理层面解决。5.3 机器人抓取中突然急停报碰撞检测错误现象机械臂按规划路径向目标移动走到半路突然停下来示教器显示碰撞检测触发但实际上机械臂周围没有碰到任何障碍物。原因AUBO i5的碰撞检测基于关节力矩变化抓取接近段速度快、方向朝下末端自重大力矩变化容易超过默认阈值。工作台边缘在点云里是障碍物MoveIt规划出的路径贴着桌面边缘通过也会触碰安全距离触发保护。解决第一把抓取接近速度降到0.02m/s加速度限制在0.1m/s²以内让力矩变化平缓。第二在MoveIt的规划场景里给工作台加一个膨胀层比如把工作台边界向外扩5厘米路径就不会贴得太近。第三调整机器人控制器的碰撞检测灵敏度参数协作机器人通常有高中低档位抓取实验用中档即可不能为了跑通彻底关闭安全保护必须保留。如果急需验证流程可以先把灵敏度调到最低但正式抓取前一定要恢复。5.4 抓取位姿穿模夹爪中心与法兰中心没对齐现象视觉计算出的抓取点正好在物体中心但机器人到位后夹爪一侧碰到物体边缘或者夹爪张开放下时正中心并没有对准物体把物体推倒而不是抓住。原因抓取节点生成的位姿是机器人法兰末端的位置而夹爪的实际抓取中心在法兰下方十到二十厘米处。代码里如果只下发质心位置就把物体中心当成了法兰位置夹爪末端自然会穿进物体。还有一个因素是点云质心在物体表面不在几何中心抓取高度需要往下补偿一个物体半径。解决第一在MoveIt里配置好夹爪的TCP工具中心点让grasp_pose代表夹爪开口中心而不是法兰中心这样所有抓取目标位姿都自动被换算到夹具中心。第二增加抓取高度补偿参数比如夹爪实际中心到法兰距离是0.17米接近距离就加上这个偏移让夹爪下降0.17米后抓取中心才恰好到达点云质心位置。第三用示教模式手动移动机器人到目标物体上方肉眼观察夹爪开口中心是否与物体对齐把误差反馈到补偿参数里。6. 验证抓取效果与延迟补偿统计成功率比肉眼观察更可靠抓取demo能跑起来只是第一步判断这套系统是不是真正可用要看重复抓取的成功率。我的验证方法是准备十个相同形状的塑料方块每次随机摆放在工作台网格的不同位置连续跑三十次抓取记录每次结果成功、碰倒、抓空、脱手然后按原因归类。这个统计比直接看几次成功更有价值它能暴露出视觉误差和机器人重复定位精度的边界。延迟补偿是抓取成功率的关键细节。D435i的深度流和点云处理加起来通常有几十毫秒的延迟机械臂规划路径的时间也超过视觉采样的间隔。如果目标物体是静态摆放这个问题不明显如果物体有轻微滑动或工作台有振动视觉给出的位姿就已经过期了。我通常的做法是在抓取节点里加一个时间戳校验比较点云时间戳和当前时刻的差值超过0.15秒就直接丢弃这一帧重新等待下一帧有效点云。同时把机器人等待视觉数据的超时时间设定0.5秒超时后让机械臂退回安全位置而不是盲目执行旧的抓取指令。源码里如果提供了抓取测试脚本我建议自己加一个输出字段视觉检测到的质心坐标与机器人实际接触点坐标的偏差。这个偏差在几次抓取内看不出问题但三十次后能算出系统性的漂移方向比如总是偏X轴正方向3毫米那大概率是相机安装位置有松动或手眼标定矩阵有误差可以直接修正比反复调算法参数高效得多。我自己的习惯是每次换相机位置或机械臂底座之后就重做手眼标定绝不沿用旧矩阵。标定一次半小时换来的是一整天的调试顺滑。这套AUBO i5与D435i抓取实践的坑基本踩遍了希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/10/18 23:07:57 · 阅读 1.2k AUBO i5与D435i抓取实践：手眼标定、点云分割与避坑指南
简介面向机械臂视觉抓取开发者的可运行源码包整合aubo i5机械臂与Intel RealSense D435i深度相机的识别抓取实践。压缩包仅6KB内含3个文件以html说明与inscode工程配置为主附带gitignore版本管理配置结构精简便于快速查阅与复用。已有108人学习下载。资源完整梳理抓取流程借助相机获取物体位姿并建立临时工件坐标系通过坐标转换将关键点映射至机械臂基座标系再经由moveit控制执行抓取。代码示例覆盖结构体定义、工件位置信息获取、坐标系发布、关键点位姿计算及机械臂初始化执行等全链路为工程开发提供可直接参考的实现框架。资源同时建议实际项目采用server或action机制触发检测与抓取对提升响应速度具有实用价值。1. 抓取demo跑通不是终点真正让机器人抓到目标才是抓取demo跑通不是终点真正让机器人抓到手的物体才是。很多人在AUBO i5和D435i这套组合里栽跟头问题不在神经网络而在坐标系D435i最终给你的是相机坐标系下的三维点云AUBO i5要的是机器人基座坐标系下的目标位姿中间隔着手眼矩阵。这个矩阵标错一厘米视觉检测再准也会抓空。针对“aubo i5与D435i抓取实践[可运行源码]”这个方向我围绕硬件选型、手眼标定、点云分割、抓取位姿生成和现场调试的坑做一次完整拆解。适合有ROS基础、想让机械臂具备视觉抓取能力的开发者也适合那些已经跑通源码但抓取成功率不稳定的朋友对照排查。2. 手眼标定与相机配置坐标系对了抓取就成功一半2.1 眼在手上还是眼在手外抓取场景的选型差异AUBO i5是六轴协作机器人D435i是双目结构光深度相机。两者组合后第一个要决定的问题不是算法而是相机装在哪里。眼在手外Eye-to-Hand指相机固定在工作台支架上视野覆盖整个工作区域眼在手上Eye-in-Hand则把相机装在机械臂末端法兰上视角随手臂移动。桌面抓取场景我一般会固定安装相机。原因很直接D435i的深度在移动状态下会受到振动和加速影响眼在手上标定得更勤而且机械臂接近目标时相机视角会被手臂本身挡住。眼在手外的视野固定标定一次后只要相机和底座不松动就能长期使用。代价是工作区域有限物体进入遮挡区就检测不到。手眼标定解决的是相机坐标系和机器人基座坐标系的变换关系。对眼在手外标定板固定? 不标定板固定在机器人末端或固定在桌上核心是求解 AX XB。常见做法是使用ArUco码贴在机器人末端法盘上机器人走多个姿态相机检测标定板在相机系下的位姿同时记录机器人控制器给出的末端在基座系的位姿。import cv2 import numpy as np # robot_poses: 机器人末端在基座系下的齐次矩阵列表 (Nx4x4) # cam_poses: ArUco标定板在相机系下的齐次矩阵列表 (Nx4x4) # 两组数据一一对应姿态需要覆盖不同旋转方向 R_gripper2base [] t_gripper2base [] R_target2cam [] t_target2cam [] for r_gb, c_tc in zip(robot_poses, cam_poses): R_gripper2base.append(r_gb[:3, :3]) t_gripper2base.append(r_gb[:3, 3]) R_target2cam.append(c_tc[:3, :3]) t_target2cam.append(c_tc[:3, 3]) # Tsai方法是工程里最稳定的求解方法 R_cam2gripper, t_cam2gripper cv2.calibrateHandEye( R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, methodcv2.CALIB_HAND_EYE_TSAI)代码逻辑是把手眼标定转化为两组旋转矩阵的方程关系机器人末端到基座的变换、标定板到相机的变换、再加上未知的相机到末端变换。cv2.calibrateHandEye直接解这个闭环方程。采集数据时注意三点第一采集姿态数不要少于15组少了旋转矩阵的约束不足第二姿态要覆盖不同的俯仰和偏航只平移标定板会导致求解退化第三每组姿态之间尽量拉开角度比如绕X轴转20度、绕Y轴转30度交替。标定过程中机械臂速度放慢避免运动模糊导致ArUco角点检测偏移。眼在手外的最终标定结果是相机在基座系下的固定变换。求解出来的R_cam2gripper是相机相对末端的变换还需要通过当前机器人位姿的正向运动学把它转换到基座下。我一般会在标定脚本里多加一步把标定时刻的末端位姿记录下来然后按base_T_camera base_T_gripper gripper_T_camera算出来。这里的gripper_T_camera就是手眼标定的输出。这一步最容易出错后面的避坑章节会专门展开。2.2 D435i点云与RGB对齐分辨率、曝光和深度单位D435i的深度原理是双目匹配加红外投影补光输出的深度图和RGB图来自不同传感器直接叠加会有错位。抓取场景必须做点云对齐否则目标物体边缘会有一圈错误点云直接导致聚类尺寸偏大。import pyrealsense2 as rs pipe rs.pipeline() cfg rs.config() cfg.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) profile pipe.start(cfg) # 深度单位默认是0.001米即1mm读出的原始像素值乘0.001得到米 sensor profile.get_device().first_depth_sensor() sensor.set_option(rs.option.enable_auto_exposure, 0) sensor.set_option(rs.option.depth_units, 0.001) align_to rs.stream.color align rs.align(align_to) frames pipe.wait_for_frames() aligned_frames align.process(frames) depth_frame aligned_frames.get_depth_frame() color_frame aligned_frames.get_color_frame()这段配置里有几个参数会直接影响抓取稳定性。第一个是depth_units如果把它设成0.0001深度值精度更高但范围缩小抓取工作台距离超过1米时会出现深度溢出推荐保持0.001。第二个是自动曝光默认开启在室内场景会频繁波动尤其是目标物体移动时画面亮度突变导致深度跳变手动固定曝光后点云稳定很多。第三个是rs.align的作用它把深度图重投影到彩色图视角生成的彩色点云每个点同时带RGB和深度后续平面分割和聚类可以直接用对齐后的点云。工程里还有一个容易被忽略的坑D435i的红外投影在室外强光下基本失效因为阳光的红外分量会淹没投影散斑。抓取工位尽量放在室内避免阳光直射。如果工作台上放的是玻璃杯、金属罐这类反光物体点云会出现大面积空洞后面避坑章节会讲具体应对。3. 点云分割与抓取位姿生成从一堆点云到机器人能用的xyz3.1 平面分割与目标聚类先去掉桌面再谈目标D435i输出的点云包含了工作台、背景墙和所有目标物体。机器人不可能去抓整张桌子所以第一步是把桌面平面拟合出来并剔除掉剩下的点云才是候选物体。import open3d as o3d import numpy as np pcd o3d.geometry.PointCloud() pcd.points o3d.utility.Vector3dVector(points) # points: Nx3单位米 pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], # 注意相机坐标系的Y轴通常向下 [0, 0, -1, 0], [0, 0, 0, 1]]) # RANSAC平面拟合桌面的法向量通常是z轴方向 plane_model, inliers pcd.segment_plane(distance_threshold0.015, ransac_n3, num_iterations1000) table_point plane_model[:3] * plane_model[3] objects_pcd pcd.select_by_index(inliers, invertTrue) # 去除离群点避免桌面边缘的噪声点参与聚类 objects_pcd objects_pcd.remove_statistical_outlier(nb_neighbors20, std_ratio2.0) labels objects_pcd.cluster_dbscan(eps0.02, min_points30) for label in np.unique(labels): if label -1: continue cluster objects_pcd.select_by_index(np.where(labels label)[0]) work_on_cluster(cluster)这段代码的关键参数有三个。distance_threshold0.015是点到拟合平面的最大距离桌面平整且点云噪声小时放0.01桌面有垫布或点云质量差时放到0.025太大容易把贴近桌面的物体点云也吞进平面。cluster_dbscan的eps0.02是聚类半径单位是米它决定了多近的点会被归为一类。物体间的间距小于2厘米时容易粘成一片物体表面有孔洞时又会被拆成多块这个值要按实际目标尺寸去调。min_points30是过滤小杂点的最少点数防止把桌面灰尘和相机噪声也当成目标。平面分割的另一个作用是拿到桌面法向量这个向量会作为抓取姿态的基准方向。注意plane_model返回的是(a, b, c, d)法向量是(a, b, c)但RANSAC拟合出的法向量方向可能是朝下也可能是朝上需要跟相机坐标系的Z轴做点积小于0就取反。3.2 抓取点与抓取姿态计算质心坐标加上方向约束聚类得到的是物体表面点云不是物体位姿。常见做法是用PCA提取点云的主轴方向作为抓取方向再用质心作为抓取点。实际机器人抓取还需要一个约束夹爪的接近方向必须沿桌面法向量这样才不会斜着戳进物体。def grasp_pose_from_cluster(cluster_pcd, base_T_camera): pts np.asarray(cluster_pcd.points) centroid np.mean(pts, axis0) # PCA提取主轴 cov np.cov(pts.T) eigvals, eigvecs np.linalg.eigh(cov) # 点云分布最广的方向作为夹爪开合方向 x_axis eigvecs[:, -1] # 桌面法向量作为接近方向 z_axis np.array([0.0, 0.0, 1.0]) y_axis np.cross(z_axis, x_axis) y_axis / np.linalg.norm(y_axis) x_axis np.cross(y_axis, z_axis) # 质心转换到机器人基座系 centroid_camera np.append(centroid, 1.0) centroid_base base_T_camera centroid_camera R_matrix np.column_stack([x_axis, y_axis, z_axis]) return centroid_base[:3], R_matrix这段代码里质心坐标转换是重头戏。base_T_camera就是第2章手眼标定的最终结果它是一个4x4齐次矩阵。所有相机系下的点云坐标都要先拼上1变成齐次坐标再左乘这个矩阵才能得到机器人能直接使用的基座系坐标。x_axis取PCA最大特征值对应的主方向表示物体在哪个方向延伸最长夹爪开合方向通常沿着它。但PCA主轴有两个方向正反都可能需要在代码里加一个判断如果主轴方向与夹爪开合的物理参考方向夹角太大就取反或换次轴。还有一个细节点云噪声会让质心偏离真实几何中心尤其是物体表面有大片空洞时。我一般在计算质心前先对聚类点云做体素下采样比如voxel_size0.01这样能让点云密度均匀化避免相机近距离处点云过密把质心拉偏。抓取姿态生成后还不能直接发指令给机器人。工程上要加一层验证判断夹爪开合宽度是否大于物体在开合方向上的投影尺寸否则夹爪闭合时根本碰不到物体表面。这个宽度可以由聚类点云的最小包围盒估算代码里用np.min(pts x_axis)和np.max(pts x_axis)的差就能算出来。4. 环境搭建与跑通可运行源码的最小步骤4.1 依赖安装ROS、RealSense驱动和AUBO驱动这套抓取实践最常见的运行环境是Ubuntu加ROS我用Noetic版本做说明。准备工作分三块RealSense相机驱动、AUBO机械臂驱动、点云处理库。# 安装RealSense ROS驱动和点云处理依赖 sudo apt install ros-noetic-realsense2-camera sudo apt install ros-noetic-rgbd-launch sudo apt install python3-open3d python3-pyrealsense2 # AUBO驱动编译 sudo apt install ros-noetic-moveit ros-noetic-industrial-core cd ~/catkin_ws/src # 将aubo_ros源码放入src后编译 catkin_makeROS Noetic对应Python3所以点云处理库用python3-open3d。RealSense的ROS驱动安装后会自动发布/camera/color/image_raw和/camera/depth/color/points等话题。AUBO驱动编译前要先确认机器人控制器IP和网络连通性工程上常见做法是用网线直连机械臂控制柜和工控机网段配置成和控制柜一致。驱动节点启动后会发布/aubo_robot/move_group等MoveIt接口抓取节点通过MoveIt发送笛卡尔空间目标位姿。源码包里的抓取节点一般遵循同样的结构订阅点云话题调用手眼矩阵换算发布抓取位姿到MoveIt的规划接口。跑通前先确认两个话题能收到数据rostopic hz /camera/depth/color/points应该稳定输出15Hz左右rostopic pub /aubo_robot/set_io这类简单指令能收到机器人的反馈。4.2 跑通抓取Demo的三条命令# 终端1启动RealSense相机 roslaunch realsense2_camera rs_camera.launch \ depth_fps:30 color_fps:30 \ align_depth:true # 终端2启动AUBO机械臂驱动 roslaunch aubo_ros i5_moveit.launch \ robot_ip:192.168.1.50 \ sim:false # 终端3启动抓取节点加载手眼标定文件 rosrun aubo_grasp grasp_node.py \ _camera_to_base_file:${HOME}/calib/cam_to_base.yaml第一条命令启动相机align_depth:true让驱动直接发布对齐到彩色图的点云省去在Python里做对齐的步骤。第二条命令启动机械臂驱动robot_ip换成控制柜实际IPsim:false表示连接真实机器人如果是初次调试可以先sim:true做纯仿真验证。第三条命令是抓取主程序它通过参数文件读取手眼标定矩阵不需要改代码就能切换标定结果。抓取节点启动后可以在RViz里看两个关键点彩色点云里目标物体验证分割结果是否正确机器人模型上是否出现一个箭头一样的抓取目标姿态。我的习惯是先让机械臂停在安全高度把抓取目标发布到一个虚拟小物体上确认规划轨迹没有碰撞再放真实物体。源码里的抓取节点一般都有调试开关输出每个聚类物体的质心坐标和抓取姿态的四元数这些打印信息比看屏幕更直接。4.3 必调参数表从初始值到工程值参数名推荐初始值现场调整依据深度分辨率640x480物体小且精度要求高时试1280x720但点云计算会变慢平面分割阈值0.015m桌面不平或有点状凹陷时上调到0.025聚类最小点数30有细小物体螺丝、垫圈时降到10抓取接近速度0.03m/s易碎物体降到0.01金属件可提到0.05抓取高度补偿0.02m夹爪实际中心低于法兰时按TCP偏差调整允许规划超时1.0s工作环境复杂时调到2.0避免路径规划失败直接退出速度参数是机器人抓取比仿真多出来的关键维度。AUBO i5是协作机器人自带碰撞检测但抓取接近阶段速度过快会让物体被碰倒而不是被抓起来。我一般把接近段分成两段从安全高度到工作台上方10厘米处速度0.1m/s从10厘米到接触点速度降到0.03m/s。源码里如果只有一个速度参数那就让它控制末端接近段的线速度粗略加速度直接设成0.05m/s²。5. AUBO i5与D435i抓取实践避坑标定、点云与安全保护5.1 手眼标定结果用不了坐标系方向搞反现象手眼标定完成后把相机点云里的目标坐标发给机器人机械臂朝着目标相反的方向移动或者在RViz里机械臂模型和目标点明显错位。原因眼在手外模式下求解出的变换是相机相对末端的位姿但抓取节点需要的是相机相对机器人基座的位姿。很多源码直接把这个矩阵当作base_T_camera用等于漏乘了一次机器人当前末端位姿。还有一种情况是标定板安装在机器人末端求解出的R_target2cam方向定义反了导致旋转轴上整体转180度。解决第一标定脚本里一定要输出完整的转换链路写成base_T_camera base_T_gripper_calib gripper_T_camera其中base_T_gripper_calib是标定采样时记录的末端位姿gripper_T_camera才是手眼标定函数返回的结果。第二在RViz里先加载相机点云再加载机械臂模型开启TF显示看/camera_link是否出现在机械臂基座附近的合理位置。第三用一个已知位置物体验证把某个物体放在机械臂工作空间内读取相机视觉坐标用手眼矩阵转换后发给机器人机器人末端应指向该点。5.2 D435i点云在强光下出现大面积空洞现象光源充足时工作台中心区域深度图上出现黑色空洞物体表面点云像被啃掉一块聚类后物体尺寸偏小。原因D435i的红外投影散斑在强光下被环境红外光淹没双目匹配找不到对应点。另一个常见原因是物体表面光滑比如金属罐、磨砂塑料外壳红外光发生镜面反射入射光没有回到相机传感器。解决第一步改变环境把光源改成柔光灯箱或者拉上遮光帘避免阳光和射灯直射工作台。第二步设置深度传感器的laser_power参数在室内低光环境下可以调高投影强度但强光下调高也没作用。第三步是工程上最直接的土办法在反光物体表面贴哑光胶带实验场景里这比调算法快得多。代码层面还有一个针对空心点云的兜底对深度图做形态学闭运算把空洞用周围像素的中值填充但这种方法只适用于小洞大空洞必须从物理层面解决。5.3 机器人抓取中突然急停报碰撞检测错误现象机械臂按规划路径向目标移动走到半路突然停下来示教器显示碰撞检测触发但实际上机械臂周围没有碰到任何障碍物。原因AUBO i5的碰撞检测基于关节力矩变化抓取接近段速度快、方向朝下末端自重大力矩变化容易超过默认阈值。工作台边缘在点云里是障碍物MoveIt规划出的路径贴着桌面边缘通过也会触碰安全距离触发保护。解决第一把抓取接近速度降到0.02m/s加速度限制在0.1m/s²以内让力矩变化平缓。第二在MoveIt的规划场景里给工作台加一个膨胀层比如把工作台边界向外扩5厘米路径就不会贴得太近。第三调整机器人控制器的碰撞检测灵敏度参数协作机器人通常有高中低档位抓取实验用中档即可不能为了跑通彻底关闭安全保护必须保留。如果急需验证流程可以先把灵敏度调到最低但正式抓取前一定要恢复。5.4 抓取位姿穿模夹爪中心与法兰中心没对齐现象视觉计算出的抓取点正好在物体中心但机器人到位后夹爪一侧碰到物体边缘或者夹爪张开放下时正中心并没有对准物体把物体推倒而不是抓住。原因抓取节点生成的位姿是机器人法兰末端的位置而夹爪的实际抓取中心在法兰下方十到二十厘米处。代码里如果只下发质心位置就把物体中心当成了法兰位置夹爪末端自然会穿进物体。还有一个因素是点云质心在物体表面不在几何中心抓取高度需要往下补偿一个物体半径。解决第一在MoveIt里配置好夹爪的TCP工具中心点让grasp_pose代表夹爪开口中心而不是法兰中心这样所有抓取目标位姿都自动被换算到夹具中心。第二增加抓取高度补偿参数比如夹爪实际中心到法兰距离是0.17米接近距离就加上这个偏移让夹爪下降0.17米后抓取中心才恰好到达点云质心位置。第三用示教模式手动移动机器人到目标物体上方肉眼观察夹爪开口中心是否与物体对齐把误差反馈到补偿参数里。6. 验证抓取效果与延迟补偿统计成功率比肉眼观察更可靠抓取demo能跑起来只是第一步判断这套系统是不是真正可用要看重复抓取的成功率。我的验证方法是准备十个相同形状的塑料方块每次随机摆放在工作台网格的不同位置连续跑三十次抓取记录每次结果成功、碰倒、抓空、脱手然后按原因归类。这个统计比直接看几次成功更有价值它能暴露出视觉误差和机器人重复定位精度的边界。延迟补偿是抓取成功率的关键细节。D435i的深度流和点云处理加起来通常有几十毫秒的延迟机械臂规划路径的时间也超过视觉采样的间隔。如果目标物体是静态摆放这个问题不明显如果物体有轻微滑动或工作台有振动视觉给出的位姿就已经过期了。我通常的做法是在抓取节点里加一个时间戳校验比较点云时间戳和当前时刻的差值超过0.15秒就直接丢弃这一帧重新等待下一帧有效点云。同时把机器人等待视觉数据的超时时间设定0.5秒超时后让机械臂退回安全位置而不是盲目执行旧的抓取指令。源码里如果提供了抓取测试脚本我建议自己加一个输出字段视觉检测到的质心坐标与机器人实际接触点坐标的偏差。这个偏差在几次抓取内看不出问题但三十次后能算出系统性的漂移方向比如总是偏X轴正方向3毫米那大概率是相机安装位置有松动或手眼标定矩阵有误差可以直接修正比反复调算法参数高效得多。我自己的习惯是每次换相机位置或机械臂底座之后就重做手眼标定绝不沿用旧矩阵。标定一次半小时换来的是一整天的调试顺滑。这套AUBO i5与D435i抓取实践的坑基本踩遍了希望帮到你。本文还有配套的精品资源点击获取