机械臂视觉抓取(三)：从手眼标定到实际抓取

机械臂视觉抓取从手眼标定到实际抓取上一篇文章记录了机械臂手眼标定的两种方式注意都是基于单相机以眼在手上为例可以通过棋盘格标定出相机坐标系和夹爪坐标系的关系。标定出这个关系怎么通过视觉指挥抓手去抓取实际的物体呢而且有的时候机械臂末端安装的可能仅仅是一台普通的 2D 相机根本就没有办法得到深度信息并且现实物体并不是简单一个点机械臂运动过去就行了于是出现了下面一些系列问题只有一个相机如何得到被抓物体在相机坐标系下的坐标被抓物体通常不是一个点而是一个有体积的物体应该计算哪个点被抓物体有体积不管返回物体上的哪个点机械臂直接运动到那个点的话必然发生碰撞怎么解决机械臂夹爪坐标系通常是建立在机械臂末端法兰盘上如果我在末端安装了类似抓手、吸盘、探针之类的工装这些工装如何能按照我们的要求抓取、吸附、接触物体这样一看只完成了相机内参数标定和手眼标定是远远不够的在不同的项目不同的场景中还有很多实际问题要解决标定只是建立了相机和机械臂之间的关系相当于给机械臂装上了眼睛。至于怎么能够在不同场景下利用眼睛解决不同的问题还有很多工作要做。本文以一个眼在手外Eye-to-Hand的工件抓取项目为例记录从手眼标定完成到实际抓取过程中需要解决的各种问题。一、系统配置1.1 眼在手外 vs 眼在手上眼在手上 (Eye-in-Hand) 眼在手外 (Eye-to-Hand) 相机 机械臂 ↓ ↓ ┌────┴────┐ ┌──────┴──────┐ │机械臂末端│ │ 固定支架 │ └─────────┘ │ ↓ │ │ 相机 │ └─────────────┘本项目采用眼在手外配置相机固定在机械臂工作区域正上方相机视野覆盖整个工作区域机械臂在相机视场内运动1.2 应用场景┌─────────────────────────────┐ │ 固定相机 │ ← 正对下方拍摄 └──────────────┬──────────────┘ │ ↓ 视野范围 ┌─────────────────────────────┐ │ ┌──────┐ │ │ │ 工件 1│ ┌──────┐ │ │ └──────┘ │ 工件 2│ │ │ └──────┘ │ │ ┌──────┐ │ ← 工作台 │ │ 工件 3│ │ │ └──────┘ │ │ 机械臂 │ └─────────────────────────────┘二、问题 1如何得到物体在相机坐标系下的坐标2.1 2D 相机的局限我们使用的是一台普通的 2D 相机只能得到图像的像素坐标 (u, v)没有深度信息 Z。但机械臂运动需要的是 3D 空间坐标 (X, Y, Z)。解决思路对于眼在手外配置如果工作平面是已知的比如工作台平面可以利用平面约束将 2D 图像坐标转换为 3D 空间坐标。2.2 建立物体坐标系对于规则形状的工件我们可以选择一个容易识别的特征点作为坐标系原点。工件示例长方形带孔工件工件坐标系定义 Y 轴 ↑ │ ┌─────────────────┼─────────────────┐ │ │ │ │ ● P1 │ ● P2 │ │ │ │ │ ○ 原点 (0,0,0) │ │ (几何中心) │ │ │ │ │ ● P3 │ ● P4 │ │ │ │ └─────────────────┼─────────────────┘ │ │ Z 轴垂直向上 │ X 轴 →工件尺寸长 400mm × 宽 300mm × 高 50mm6 个特征点定义在物体坐标系中的坐标特征点X (mm)Y (mm)Z (mm)说明P1-15010025上表面左上角P215010025上表面右上角P3-150-10025上表面左下角P4150-10025上表面右下角P5-100025上表面左侧中心P6100025上表面右侧中心// 配置文件中的定义object_points:[[-150.0,100.0,25.0],// P1: 上表面左上角[150.0,100.0,25.0],// P2: 上表面右上角[-150.0,-100.0,25.0],// P3: 上表面左下角[150.0,-100.0,25.0],// P4: 上表面右下角[-100.0,0.0,25.0],// P5: 上表面左侧中心[100.0,0.0,25.0]// P6: 上表面右侧中心]为什么选择这些点全部位于上表面相机可以直接拍摄4 个角点 2 个边缘点分布均匀提高 PnP 求解精度关于原点对称便于计算几何中心2.3 深度学习特征点检测传统方法如角点检测在光照变化、遮挡情况下稳定性差。本项目采用深度学习特征点检测方法。整体流程输入图像 → 深度学习模型 → 特征点热力图 → 亚像素角点 → 6 个特征点坐标网络结构┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ 输入图像 │ │ 特征提取 │ │ 特征点检测 │ │ 512×512×3 │ → │ Encoder │ → │ 6 个热力图 │ └─────────────┘ └─────────────┘ └─────────────┘ ↓ 512×512×6 (每个通道对应一个特征点)特征点提取代码// 1. 深度学习推理得到 6 个特征点的热力图cv::Mat heatmapsonnx_inference-inference(input_image);// 2. 对每个热力图找到响应最强的位置std::vectorcv::Point2fimage_points;for(inti0;i6;i){cv::Mat single_heatmapheatmaps[i].clone();// 找到最大值位置亚像素精度cv::Point maxLoc;doubleminVal,maxVal;cv::minMaxLoc(single_heatmap,minVal,maxVal,nullptr,maxLoc);// 亚像素优化cv::Point2f refined_pointsubpixel_refinement(single_heatmap,maxLoc);image_points.push_back(refined_point);}为什么用深度学习鲁棒性强对光照变化、轻微遮挡不敏感精度高可以达到亚像素级检测精度端到端不需要手工设计特征2.4 PnP 求解物体位姿已知 6 个特征点的 3D 坐标物体坐标系和对应的 2D 图像坐标使用 PnP 算法求解物体位姿// object_points: 6 个特征点在物体坐标系中的 3D 坐标已知// image_points: 6 个特征点在图像中的 2D 坐标深度学习检测// camera_matrix: 相机内参标定得到// dist_coeffs: 畸变系数标定得到cv::Mat rvec,tvec;cv::solvePnP(object_points,image_points,camera_matrix,dist_coeffs,rvec,tvec);// 旋转矢量转旋转矩阵cv::Mat rotation_matrix;cv::Rodrigues(rvec,rotation_matrix);// 组合成 4x4 变换矩阵 T_target2cam物体→相机cv::Mat T_target2camcv::Mat::eye(4,4,CV_64FC1);rotation_matrix.copyTo(T_target2cam(cv::Rect(0,0,3,3)));tvec.copyTo(T_target2cam(cv::Rect(3,0,1,3)));输出物体相对于相机的位置和姿态6 自由度三、问题 2物体是一个立体应该抓取哪个点3.1 定义抓取点对于长方体工件合理的抓取点应该是Z 轴高度方向 ↑ │ ┌────────────┼────────────┐ │ │ │ │ · ← 抓取点 │ ← 上表面中心 │ (0,0,25) │ └────────────┼────────────┘ │ │ XY 平面抓取点选择原则便于夹爪接近无遮挡夹持稳定不易滑落考虑工件重心避免翻转3.2 坐标变换链眼在手外眼在手外配置的坐标变换链T_tool2base T_cam2base × T_target2cam × T_tool2target各变换矩阵含义变换矩阵含义来源T_cam2base相机→基座手眼标定结果T_target2cam物体→相机PnP 解算结果T_tool2target工具→物体工装补偿注意眼在手外配置中T_cam2base是固定的相机位置不变而眼在手上配置中这个矩阵会随机械臂运动而变化。四、问题 3 和 4工装补偿和预抓取位4.1 抓手工装补偿问题夹爪直接固定在法兰盘上没有单独标定怎么补偿本项目使用的是两指平行夹爪直接固定在机械臂末端法兰盘上。由于夹爪是刚性安装没有进行单独的 TCP 标定而是通过试错法 经验补偿来确定补偿参数。机械臂法兰盘 │ │ L1 (长度偏差) │ ┌────┴────┐ │ 夹爪底座 │ └────┬────┘ │ │ L2 (指尖长度) │ ┌─────────┴─────────┐ │ ● ● │ ← 两个指尖 │ P_left P_right │ └─────────────────────┘ ↕ L3 (指尖间距)补偿参数transform:{tvec:[0,-50,120],// [X, Y, Z] 工具中心点偏移eulRPY:[0.5,-0.3,0.1]// [Rx, Ry, Rz] 姿态补偿度}参数含义tvec[0] 0X 方向无偏移tvec[1] -50Y 方向偏移 -50mm夹爪中心在法兰盘后方 50mmtvec[2] 120Z 方向偏移 120mm指尖到法兰盘的距离eulRPY微小角度补偿确保夹爪平行于工件表面4.1.1 补偿参数的获取方法方法 1人工测量法粗略值1. 用卡尺测量法兰盘中心到指尖的距离 → Z 方向补偿 2. 观察夹爪安装方向确定 X/Y 偏移 → X/Y 方向补偿 3. 使用水平仪测量夹爪姿态 → 欧拉角补偿方法 2试错法精确值1. 用人工测量的粗略值作为初始补偿参数 2. 让机械臂运动到一个已知位置的工件上方 3. 观察实际抓取点与理论抓取点的偏差 (ΔX, ΔY, ΔZ, ΔRx, ΔRy, ΔRz) 4. 将偏差反向补偿到参数中 5. 重复步骤 2-4直到抓取精度满足要求方法 3三点法推荐1. 在工件上选择一个特征点 P 2. 控制机械臂以不同姿态接近 P 点记录三次位置 (x1,y1,z1), (x2,y2,z2), (x3,y3,z3) 3. 通过几何计算反推出 TCP 相对于法兰盘的偏移补偿公式// 理论抓取位姿cv::Mat T_theoryT_cam2base*T_target2cam;// 实际需要的夹爪位姿考虑补偿cv::Mat T_gripper2targetbuild_tool_offset(tvec,eulRPY);cv::Mat T_actualT_theory*T_gripper2target;// 如果抓取有偏差测量实际偏差 (dx, dy, dz, drx, dry, drz)// 更新补偿参数tvec[0]dx;tvec[1]dy;tvec[2]dz;eulRPY[0]drx;eulRPY[1]dry;eulRPY[2]drz;经验值参考夹爪类型Z 方向补偿 (mm)姿态补偿 (度)两指平行夹爪100-150±1° 以内三指定心夹爪80-120±0.5° 以内真空吸盘50-100±2° 以内4.2 预抓取位Approach Position抓取流程Step 1: 运动到预抓取位上方 ↓ Step 2: 垂直下降到预抓取位 ↓ Step 3: 继续下降到抓取位 ↓ ┌─────────────┐ │ 工 件 │ └─────────────┘预抓取位计算// 预抓取位抓取点正上方 150mm 处intapproach_distance150;// mmcv::Mat T_approach2tool(cv::Mat_double(4,4)1,0,0,0,0,1,0,0,0,0,1,-approach_distance,// Z 方向偏移向上0,0,0,1);// 预抓取位 抓取位 × 接近偏移cv::Mat T_approach2baseT_tool2base*T_approach2tool;为什么是 Z 方向在眼在手外配置中相机正对下方Z 轴垂直向上。预抓取位在抓取点正上方可以避免侧向碰撞。4.3 完整的抓取位姿计算// 1. 检测特征点std::vectorcv::Point2fimage_pointsdetect_feature_points(image);// 2. PnP 解算物体位姿cv::Mat T_target2camsolve_pnp(object_points,image_points);// 3. 坐标变换物体→相机→基座cv::Mat T_target2baseT_cam2base*T_target2cam;// 4. 工装补偿计算工具中心点 (TCP) 位姿cv::Mat T_tool2targetbuild_tool_offset(transform_params);cv::Mat T_tool2baseT_target2base*T_tool2target;// 5. 计算预抓取位cv::Mat T_approach2toolbuild_approach_offset(approach_distance);cv::Mat T_approach2baseT_tool2base*T_approach2tool;// 6. 提取 XYZ 和欧拉角发送给机械臂std::vectordoublepick_posematrix_to_xyzrpy(T_tool2base);std::vectordoubleapproach_posematrix_to_xyzrpy(T_approach2base);五、坐标系说明5.1 各坐标系定义世界坐标系/基座标系 (Base Frame) - 原点机械臂基座中心 - Z 轴垂直向上 - X 轴指向机械臂正前方 相机坐标系 (Camera Frame) - 原点相机光心 - Z 轴沿光轴向下指向工作台 - X 轴图像水平向右 物体坐标系 (Object Frame) - 原点工件几何中心 - Z 轴垂直于工件上表面 - XY 平面工件上表面 工具坐标系 (Tool Frame) - 原点夹爪中心点 (TCP) - Z 轴沿夹爪闭合方向 - X 轴两指尖连线方向5.2 坐标变换可视化T_cam2base (固定) 相机 ──────────────────→ 基座 ↑ ↑ T_target2cam │ T_tool2base │ │ │ │ T_tool2target │ 物体 ──────────────────→ 工具六、总结6.1 核心问题与解决方案问题解决方案关键参数2D 相机无深度PnP 算法 已知物体 3D 模型object_points (6 个特征点)特征点检测不稳定深度学习特征点检测ONNX 模型物体是立体定义抓取点几何中心-直接运动碰撞预抓取位approach_distance工装偏差工具中心点 (TCP) 补偿tvec, eulRPY6.2 眼在手外配置特点优点相机位置固定标定一次即可视野范围大可同时检测多个工件机械臂运动不受相机线缆限制缺点标定精度受相机安装位置影响机械臂可能遮挡相机视野需要较大的工作空间6.3 核心公式抓取位姿T_tool2base T_cam2base × T_target2cam × T_tool2target 预抓取位T_approach2base T_tool2base × T_approach2tool6.4 工程经验特征点选择应分布在物体不同位置避免共面提高 PnP 精度深度学习训练需要覆盖各种光照、角度、遮挡情况TCP 标定使用四点法或激光标定精度直接影响抓取成功率预抓取距离根据工件高度和夹爪开合距离调整一般 100-200mm坐标系验证用已知位置的标定板验证整个坐标变换链的准确性附录配置文件示例{camera:{resolution:[2560,1440],intrinsic_params:[...],// 内参矩阵distCoeffs:[...]// 畸变系数},object_points:[[-150.0,100.0,25.0],[150.0,100.0,25.0],[-150.0,-100.0,25.0],[150.0,-100.0,25.0],[-150.0,0.0,-25.0],[150.0,0.0,-25.0]],T_cam2base:[// 4x4 手眼标定矩阵眼在手外固定值1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,800.0,// 相机在工作台上方 800mm0.0,0.0,0.0,1.0],transform:{tvec:[0,-50,120],eulRPY:[0.5,-0.3,0.1],approach_distance:150}}