手眼标定 3D抓取的流程（代码）

原创已于 2025-01-03 17:50:30 修改 · 2.7k 阅读

36 ·

CC 4.0 BY-SA版权

文章标签：

#3d

于 2024-12-25 17:12:19 首次发布

手眼标定 3D抓取的流程记录

用某3D相机，手眼标定标定精度在0.5mm左右，最终抓取精度在1-2mm左右，主要是看icp的匹配精度。
写这个文章只是简单记录，我自己也看，如果有问题，可以私信给我，无偿解答，毕竟在网上找了很久，没几个是完整的流程，我自己也是踩了两周的坑才完成的。大家互相交流就行。

1、手眼标定

1.1 前提

确定3D相机的类型，RGBD相机、IR结构光相机；
（1）如果是RGBD相机，可以直接使用RGB相机进行标定，然后使用RGB图像和深度图进行点云构建，这样避免判断相机和点云坐标系是否一致的情况；

（2）如果使用IR结构光相机，需要使用IR图像进行标定，但必须使用立体校准后的IR图像，否则IR相机的坐标系和点云坐标系会完全无法匹配，而且就算是立体匹配后，仍需要注意IR相机和点云坐标系的一致性。
```
  例如：某3D相机，IR相机已经做了立体校准，但IR相机的坐标系和点云坐标系是绕了z轴转了180度；
  因此，采集的点云数据需要先转到IR相机的坐标系下，即先绕z轴转180度，然后再进行后续的处理。
```
确定机械臂位姿的表示方式，xyzrpy、xyzoat等；
正常来说，我们一般做手眼标定，是使用xyzrpy的，但不同厂家的机械臂，位姿表示方式是不一样的，像川崎的机械臂是xyzoat的，这样需要直接根据川崎官方的文件，把xyzoat转成姿态矩阵RT来使用。

1.2 准备

相机（已经做了相机标定，获取了内参和外参）
如果需要自己标定相机，直接使用opencv的标定功能或者Matlab的标定软件即可。
机械臂（能通过示教器读取位姿）
标定板（不能行列数相同，且不能太小，一般是10cm*10cm以上，例如：8 x 6，11 x 9）
标定板下载链接：https://calib.io/pages/camera-calibration-pattern-generator

1.3 标定数据采集

一般采集30组数据

眼在手外
标定板安装在末端法兰上，控制机械臂不同的位姿，拍摄标定板的图像。需要记录：机械臂位姿。

在这里插入图片描述

眼在手上
标定板固定在某个位置，相机固定在末端法兰上，控制机械臂不同的位姿，拍摄标定板的图像。需要记录：机械臂位姿。

1.4 标定原理

AX = XB

实际使用，就是 opencv

    cv2.calibrateHandEye()

在这里插入图片描述

眼在手外和眼在手上的区别
直接示教器读取的机械臂位姿是 RT_gripper2base，如果是眼在手外，则需要对该位姿计算逆矩阵，得到 RT_base2gripper。

  def base2gripper(self, R_gripper2base, T_gripper2base):
      R_base2gripper = R_gripper2base.T
      T_base2gripper = -R_base2gripper @ T_gripper2base
      
      return R_base2gripper, T_base2gripper

1.5 标定DEMO

针对某3D相机和川崎机器人的眼在手外标定DEMO。

眼在手外标定 DEMO

1.6 验证标定结果

计算棋盘格左上角的第一个角点的空间位姿；
然后角点位姿和手眼矩阵相乘，得到机械臂末端的位姿；
然后控制机械臂移动到该位姿，看看是否能去到那个角点；

2、工具坐标系的建立

建立工具坐标系，把工具坐标系保存在示教器即可。这里不解释了。

3、目标姿态计算

3.1 准备

安装 PCL 库；
安装 CloudCompare 软件，用来查看点云、制作模板；
ICP 算法必须在同一个坐标系下，所以需要把点云全部转到基坐标系；

3.2 抓取姿态计算

制作 ICP 点云模版；
直接使用 CloudCompare 软件创建模板的点云数据，保存成 pcb 文件即可。
控制机械臂移动到抓取该模板的位姿，记录为示教位姿；
把后续待抓取目标和模板的点云通过手眼矩阵，转到基坐标系。
使用 ICP 算法，获取后续待抓取目标和模板的刚性变换矩阵；
常见的点云匹配算法有：
```
ICP
NICP
NDT

推荐使用PCL 的 NICP 算法，速度快，准确度高。
```

ICP 算法代码

目标的抓取姿态 = ICP 刚性变换矩阵 * 示教位姿；

注意

针对不同厂家的机械臂，需要确定位姿的是怎样转成变换矩阵的，例如：xyzrpy、xyzoat，实际计算都是需要转成 RT 矩阵的。以川崎机械臂为例：

这里就不给代码了：
oat -> R 过程：oat -> 四元数 -> R；
R -> oat 过程：R -> 四元数 -> oat；

    # oat转旋转矩阵
    def oat2R(self, o, a, t):
        ...
        return R
    
    
    # 旋转矩阵转oat
    def R2oat(self, R):
        ...
        return o, a, t