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

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

上一篇文章记录了机械臂手眼标定的两种方式（注意都是基于单相机），以眼在手上为例，可以通过棋盘格标定出相机坐标系和夹爪坐标系的关系。标定出这个关系，怎么通过视觉指挥抓手去抓取实际的物体呢，而且有的时候机械臂末端安装的可能仅仅是一台普通的 2D 相机，根本就没有办法得到深度信息，并且现实物体并不是简单一个点，机械臂运动过去就行了，于是出现了下面一些系列问题：

1. 只有一个相机，如何得到被抓物体在相机坐标系下的坐标
2. 被抓物体通常不是一个点而是一个有体积的物体，应该计算哪个点
3. 被抓物体有体积，不管返回物体上的哪个点，机械臂直接运动到那个点的话，必然发生碰撞，怎么解决
4. 机械臂夹爪坐标系通常是建立在机械臂末端法兰盘上，如果我在末端安装了类似抓手、吸盘、探针之类的工装，这些工装如何能按照我们的要求抓取、吸附、接触物体？

这样一看，只完成了相机内参数标定和手眼标定是远远不够的，在不同的项目不同的场景中还有很多实际问题要解决，标定只是建立了相机和机械臂之间的关系，相当于给机械臂装上了眼睛。至于怎么能够在不同场景下利用眼睛解决不同的问题，还有很多工作要做。

本文以一个眼在手外（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 × 高 50mm

6 个特征点定义（在物体坐标系中的坐标）：

特征点	X (mm)	Y (mm)	Z (mm)	说明
P1	-150	100	25	上表面左上角
P2	150	100	25	上表面右上角
P3	-150	-100	25	上表面左下角
P4	150	-100	25	上表面右下角
P5	-100	0	25	上表面左侧中心
P6	100	0	25	上表面右侧中心

// 配置文件中的定义
"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: 上表面右侧中心
]

为什么选择这些点？

1. 全部位于上表面，相机可以直接拍摄
2. 4 个角点 + 2 个边缘点，分布均匀，提高 PnP 求解精度
3. 关于原点对称，便于计算几何中心

2.3 深度学习特征点检测

传统方法（如角点检测）在光照变化、遮挡情况下稳定性差。本项目采用深度学习特征点检测方法。

整体流程：

输入图像 → 深度学习模型 → 特征点热力图 → 亚像素角点 → 6 个特征点坐标

网络结构：

┌─────────────┐     ┌─────────────┐     ┌─────────────┐
│ 输入图像     │     │ 特征提取     │     │ 特征点检测   │
│ 512×512×3   │ →   │ Encoder     │ →   │ 6 个热力图    │
└─────────────┘     └─────────────┘     └─────────────┘
                                          ↓
                                    512×512×6
                                    (每个通道对应一个特征点)

特征点提取代码：

// 1. 深度学习推理，得到 6 个特征点的热力图
cv::Mat heatmaps = onnx_inference->inference(input_image);

// 2. 对每个热力图，找到响应最强的位置
std::vector<cv::Point2f> image_points;
for (int i = 0; i < 6; i++) {
    cv::Mat single_heatmap = heatmaps[i].clone();

    // 找到最大值位置（亚像素精度）
    cv::Point maxLoc;
    double minVal, maxVal;
    cv::minMaxLoc(single_heatmap, &minVal, &maxVal, nullptr, &maxLoc);

    // 亚像素优化
    cv::Point2f refined_point = subpixel_refinement(single_heatmap, maxLoc);
    image_points.push_back(refined_point);
}

为什么用深度学习？

1. 鲁棒性强：对光照变化、轻微遮挡不敏感
2. 精度高：可以达到亚像素级检测精度
3. 端到端：不需要手工设计特征

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_target2cam = cv::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 平面

抓取点选择原则：

1. 便于夹爪接近，无遮挡
2. 夹持稳定，不易滑落
3. 考虑工件重心，避免翻转

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] = 0：X 方向无偏移
• tvec[1] = -50：Y 方向偏移 -50mm（夹爪中心在法兰盘后方 50mm）
• tvec[2] = 120：Z 方向偏移 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_theory = T_cam2base * T_target2cam;

// 实际需要的夹爪位姿（考虑补偿）
cv::Mat T_gripper2target = build_tool_offset(tvec, eulRPY);
cv::Mat T_actual = T_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 处
int approach_distance = 150;  // mm

cv::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_approach2base = T_tool2base * T_approach2tool;

为什么是 Z 方向？

在眼在手外配置中，相机正对下方，Z 轴垂直向上。预抓取位在抓取点正上方，可以避免侧向碰撞。

4.3 完整的抓取位姿计算

// 1. 检测特征点
std::vector<cv::Point2f> image_points = detect_feature_points(image);

// 2. PnP 解算物体位姿
cv::Mat T_target2cam = solve_pnp(object_points, image_points);

// 3. 坐标变换：物体→相机→基座
cv::Mat T_target2base = T_cam2base * T_target2cam;

// 4. 工装补偿：计算工具中心点 (TCP) 位姿
cv::Mat T_tool2target = build_tool_offset(transform_params);
cv::Mat T_tool2base = T_target2base * T_tool2target;

// 5. 计算预抓取位
cv::Mat T_approach2tool = build_approach_offset(approach_distance);
cv::Mat T_approach2base = T_tool2base * T_approach2tool;

// 6. 提取 XYZ 和欧拉角，发送给机械臂
std::vector<double> pick_pose = matrix_to_xyzrpy(T_tool2base);
std::vector<double> approach_pose = matrix_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, eulRPY

6.2 眼在手外配置特点

优点：

1. 相机位置固定，标定一次即可
2. 视野范围大，可同时检测多个工件
3. 机械臂运动不受相机线缆限制

缺点：

1. 标定精度受相机安装位置影响
2. 机械臂可能遮挡相机视野
3. 需要较大的工作空间

6.3 核心公式

抓取位姿：T_tool2base = T_cam2base × T_target2cam × T_tool2target
预抓取位：T_approach2base = T_tool2base × T_approach2tool

6.4 工程经验

1. 特征点选择：应分布在物体不同位置，避免共面，提高 PnP 精度
2. 深度学习训练：需要覆盖各种光照、角度、遮挡情况
3. TCP 标定：使用四点法或激光标定，精度直接影响抓取成功率
4. 预抓取距离：根据工件高度和夹爪开合距离调整，一般 100-200mm
5. 坐标系验证：用已知位置的标定板验证整个坐标变换链的准确性

附录：配置文件示例

{
    "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,  // 相机在工作台上方 800mm
        0.0, 0.0, 0.0, 1.0
    ],
    "transform": {
        "tvec": [0, -50, 120],
        "eulRPY": [0.5, -0.3, 0.1],
        "approach_distance": 150
    }
}