ROS2下机械手的手眼标定

摄像头是垂直拍摄工作平面，只需要用棋盘格一次性标定就够了，这里的棋盘格是便于捕捉到特征点，特征点的实际坐标是已知的（通过设置每个单元格的实际物理尺寸可以计算得到），通过特征点确定用户坐标系，然后计算两个坐标之间的变换矩阵（旋转、平移、缩放），然后就可以确定相机采集到的图像上的一个坐标点映射到用户坐标系的实际坐标点。标定板的坐标系与用户坐标系重合（物理上）、我们建立了标定板坐标与图像坐标的映射（计算

韭菜钟

3662人浏览 · 2023-04-25 17:17:14

韭菜钟 · 2023-04-25 17:17:14 发布

文章目录

0.前期准备
1.坐标的标定

0.前期准备

0.1机械手要先映射（标定好）世界坐标系与用户坐标系

这个基本应该可以通过机械手自带的坐标系建立完成
一般机械手需要处理三种坐标：世界坐标系（基座标系）、工具坐标系、用户坐标系，分别对应下图的Base、Tool、Plane_1坐标系。
在这里插入图片描述一般来说，世界坐标系的原点在基座底部，工具坐标系的原点在机械手末端（工具末端），用户坐标系的原点在用户设置的位置。
世界坐标系是与机械手结构有关系的，一般来说厂家已经设定好、写死了的，用示教器或者示教软件无法修改。
工具坐标系乍一看没啥用，但是在一些特殊情形下会很方便（比如让机械手绕着工具末端旋转，例如【工业机器人舞刀，这运动精确度太精准了！】里面演示的两把剑末端相对旋转的运动）。
用户坐标系需要用户自行设定，设定的方法不同的厂家不一样。下图的是大族机械手的三点设定方式：点位1为原点、点位二为x正方向、点位三为y轴方向，z轴可根据左手或者右手定则得到。

在这里插入图片描述
理论上，每当更换机械手末端的工具（夹具、钻头、焊接头），都需要重新设置一次工具坐标系？（要提前设好，然后同步做坐标系切换就行）

0.2.相机的标定

0.2.1.标定的目的

相机的标定主要目的是：

a、消除相机的镜头畸变，比如鱼眼镜头，以获得更好的成像效果；
b、获取相机的内参；

这里稍微注意一下，在获取到相机的内参+畸变参数后，假如利用opencv的undistort（或者其他方式）消除畸变后，并且后续使用的图像是消除了畸变后的图像，记得要使用新的相机内参进行后续运算。【opencv畸变校正的两种方法】

0.2.2.标定的方式

一般会使用棋盘格标定（张正友标定），标定时需要通过多组结果来联合求解，因此需要拍摄多张不同角度、不同位置的棋盘格图像进行识别计算。
相机标定、消除畸变之后，图像看起来就是符合常规透视的（横平竖直）。
虽然我目前选用的是AstraPro深度相机，它本身的畸变很小很小，无需消除畸变，但是我还是需要它的内参与后续的运算，因此，还是必须要进行一次标定。
普通相机的标定可以查看【计算机视觉相机标定】、【用OpenCV进行相机标定(张正友标定,有代码)】
这里记录几个在线生成标定图案的网站：

【ArUco markers generator!】
【PatternGenerator】

1.坐标的标定

手眼标定一般分为二维（平面）标定、三维（空间）标定。

1.1.二维标定（平面标定）

二维标定就比较简单。摄像头是垂直拍摄工作平面（好像，不完全垂直也是可以的？只要保证工件是放在工作平面上就可以？）（是的，不需要垂直，两个平面成任意角度都行，只要不太极端），只需要用棋盘格一次拍照标定就够了，这里的棋盘格主要目的是提供便于捕捉到的特征点，从而确定一个坐标系。
在这里插入图片描述
当采用二维标定时，一般都是在完成某次作业（拾取、钻孔等）后，机械手会复位到原始位置（标定采图时的位置）。如此便可以保证在进行一次标定之后，只要相机、机械手、作业平面三者之间没有发生相对移动，便可以继续使用标定的结果

1.1.1.九点标定

二维标定常用的是9点标定，这种方式好理解。选择图上的（任意）9个角点，并且分别填入这9个点的世界坐标（用户坐标也行，具体要看你机械臂选择的运动坐标系），然后计算出变换矩阵。
halcon9点标定的理解

9点标定是一种二维手眼标定方法，其中的重要假设为标定板所在平面与实际检测物体处于同一平面，相机所在平面与标定板平面的关系固定不变，可以相对标定板所在平面xy方向平移，z方向保持不变。有了以上假设，那么相机的成像面与标定板平面为仿射变换关系。仿射变换矩阵有6个未知数，那么至少需要3对点进行求解。

实际操作时，九点标定先从上面的标定板中选9个点，记录下这九个点的像素坐标，然后让机械手走这几个点，分别得到他们的机械坐标。最后利用工具，计算出变换矩阵。
【手眼标定篇】九点标定
在这里插入图片描述

此时可以使用以下opencv函数进行标定及转换：

/* 下面这些都不行，使用这些函数时，要求两个平面是重合或者平行的
// 计算变换矩阵
cv::getAffineTransform()  // 这个函数只能使用三对点对
cv::estimateAffine2D()
cv::estimateAffinePartial2D()

// 坐标点应用变换矩阵
 cv::transform()

// 图像应用变换矩阵
cv::warpAffine()
*/

// 必须得使用这些才行，这些才是允许两个平面之间成任意夹角的
// 计算两个平面之间的变换矩阵
cv::findHomography

// 坐标点应用变换矩阵
perspectiveTransform

// 图像应用变换矩阵
cv::warpPerspective

下图的框框我是将这几个坐标点投射到原图上绘制出来的，棋盘格的大小为10*10。

    srcPoints.push_back(Point2f(-10, -10));
    srcPoints.push_back(Point2f(60, -10));
    srcPoints.push_back(Point2f(60, 90));
    srcPoints.push_back(Point2f(-10, 90));

在这里插入图片描述
以0.5个格子为标准距离绘制的点阵，也能很好地适配画面。说明这种方式进行的标定还是很可靠的。

1.1.2.简化标定（慎用，仅供参考）

在实际的生产环境中，有时候懒得一个点位一个点位地走，就可以采用简化的标定方法。
使用这种方法的前提是：

a、标定板的X、Y轴已经和机械手的坐标系的X、Y轴方向一致（假使不一致，也要知道他们之间的夹角，后期在软件上进行补偿修正）；
b、标定板的物理尺寸已知；
c、机械手只需要接收坐标偏移量，而不是绝对坐标。
d、对精度要求不高。

比如标定板的格子尺寸为30mm，那么，假设左上角第一个角点的位置为（0，0），那个横向第二个的为（30，0），以此类推，我们就可以通过手动计算得知选定的每个点的机械坐标。
得到机械坐标后，配合各个点的像素坐标，计算好变换矩阵。
作业工况为：

a、对产品进行拾取（或者其他操作）。
b、每次都有产品出现在相机画面中，且能被视觉分析工具识别定位（图像坐标）到。

此时一次完整的作业流程是：

a、在标定完成后，先进行初始化设置：记录下此次机械手的姿态A。
b、让机械手执行抓取（也就是沿Ｚ轴移动），放一个产品在机械手抓取的位置，记录下此时产品的位置Ｂ（标定后的物理坐标）。
c、开始作业（开动流水线）
d、在新的一个产品出现时，通过视觉工具识别得到产品位置B1.；将B1减去Ｂ，得到偏移坐标。
e、将偏移坐标发送给机械手，让机械手配合姿态A，在拾取前调整好偏移。
f、执行拾取。

1.2.三维标定（空间标定）

三维标定就需要用到深度相机（RGBD相机）。
参考这两个动图，胜过千言万语。图像来源：【3D 视觉之手眼标定】
在这里插入图片描述
在ros2中标定相机（求内参、外参），请看这个：
【ORBBEC（奥比中光）AstraPro相机在ROS2下的标定与D2C（标定与配准）】

三维手眼标定的主要流程如下：

1.对深度相机的IR、RGB相机进行张正友标定，得到内参、畸变系数。
2.用得到的内参、畸变系数对IR、RGB相机进行畸变矫正，及进行D2C配准（可做可不做）。
3.用IR相机进行手眼标定（需要用到相机内参）。标定过程参考：【3D 视觉之手眼标定】
4.求解AX=XB。
5.得到相机到机械手基座的坐标变换矩阵T。
6.将点云应用变换矩阵T，得到映射后的点云。
7.完成。

现在重点介绍一下第3点：用IR相机进行手眼标定。
我目前用的是自己组装的机械手，拿不动深度相机，所以只能进行眼在手外的手眼标定了。也就是这个：
在这里插入图片描述

我们分析一下其中的A、B、C、D变换矩阵如何得到。
A可以直接通过ros2的工具得到

ros2 run tf2_ros tf2_echo B E

在这里插入图片描述
或者通过代码得到：

int MyUtil::getTransform(std::string inputFrame,
                         std::string targetFrame,
                         geometry_msgs::msg::TransformStamped &transform)
{
//    static rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);

    static rclcpp::Clock::SharedPtr clock = rclcpp::Node("some_node",
                                                         rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))
                                                . get_clock();

    // 创建 Transform Listener 和 Buffer
    static tf2_ros::Buffer tf_buffer(clock);
    static tf2_ros::TransformListener tf_listener(tf_buffer);

    qDebug() << (void*)clock.get() << (void*)&tf_buffer << (void*)&tf_listener;

    auto now = clock->now();
    // 创建输入点和输出点
    geometry_msgs::msg::PointStamped in, out;
    in.header.frame_id = inputFrame;
    in.header.stamp.sec = now.seconds();
    in.header.stamp.nanosec = now.nanoseconds() - ((int64_t)now.seconds()) * 1e9;
    in.point.x = 1.0;
    in.point.y = 2.0;
    in.point.z = 3.0;

    // 等待变换关系可用
    bool valid = false;
    try {
      valid = tf_buffer.canTransform(targetFrame, inputFrame, tf2::TimePointZero);
      if(valid == false)
      {
        return -1;
      }
    }
    catch (tf2::TransformException& ex) {
      qDebug() << "can not transform:" << ex.what();
      return -2;
    }

    qDebug() << "---valid-----" << valid;

    // 查询变换关系
    try{
      transform =
          tf_buffer.lookupTransform(targetFrame, inputFrame, tf2::TimePointZero);
    }
    catch(tf2::LookupException& ex){
      qDebug() << "lookupTransform:" << ex.what();
      return -3;
    }

    // 并进行转换
    //    tf2::doTransform(in, out, transform);

    return 0;
}

B未知。
C也就是我们前面提到的外参的逆矩阵
D是我们最终要求的。
总的来说也就是A、C都是可以知道的。
在这里插入图片描述
已经知道了A1、C1、A2、C2，那么就可以用解“AX=XB”（这里的A、X、B和前面的字母不是表示同一个意思）的方法求得变换矩阵B，然后D也就出来了：

求AX=XB的，可以看一下这里，不求知其所以然的话，代码还是挺简单的：
【手眼标定中利用Eigen求解AX=XB问题】