2D-3D定位引导算法原理及实现

2D-3D定位引导算法原理及实现

在自动化仓库中，AGV搭载机械臂进行物料抓取是一项常见的应用场景。然而，由于AGV存在位置变动，且待抓取物料可能在三维空间中存在倾角，因此需要精确的定位引导算法来确保抓取精度。本文将详细介绍2D-3D定位引导算法的原理及实现方法，帮助读者理解如何通过2D相机实现6D位姿的求解，并提供具体的标定、示教与作业流程，以及相应的代码实现。

应用场景

某2D-3D应用场景示意图如下图所示。

项目需求：

• AGV搭载机械臂，在仓库中按照指令前往不同的机台夹取物料
• AGV有±20mm、±5°的位置变动，夹取精度要求±1mm，故需要在AGV小车上安装固定相机，引导机械手上下料
• 机台上待抓取的物料不是平面放置，在三维空间中会有绕X/Y/Z轴的倾角，抓取位姿为3D空间坐标（6D位姿）
• 机台上待抓取的物料透明，相机无法直接拍到，需要通过拍摄机台上Mark点获得待抓取物料的位置

3D偏移抓取

由于待抓取工件透明，且无法被相机直接拍到，因此需要在工作台面上贴附作为引导基准的标识区，标识区与工件的相对位置关系保持不变。可通过贴附标签或印刷蚀刻等手段设置其内部特征，优选方案为贴附二维码标签或十字标签。示教和作业时，相机只拍摄标识区内固定特征点（Mark点）。

因此，该场景为典型的使用2D相机求解6D位姿问题。

问题定义

通常将使用2D相机求解6D位姿问题，称为2D-3D定位引导问题。

任务拆解

视觉定位引导任务可以拆分为三步：标定、示教与作业。

• 标定：求解相机坐标系（或像素坐标系）到机械臂坐标系的变换关系
• 示教：获得待抓取目标在机械臂坐标系下的基准位姿
• 作业：相机采集图像，根据特征点坐标及角度，求解待抓取目标的运行位姿

标定

针对2D-3D定位引导场景，有两种标定方案：

1. 采用2D标定，即九点（或9+3点）标定，求解机械臂坐标系到像素坐标系的XOY平面单应矩阵；
2. 采用3D标定，求解相机的内、外参和畸变系数，以及相机坐标系到机械臂坐标系的手眼转换矩阵。

示教

• 控制机械臂运动到标识区，使得末端示教工装垂直于工作台面点触示教标识区Mark点，记录此时机械臂物理坐标为Mark基准位姿（6D位姿），如下图所示。
• 控制机械臂运动至待抓放目标工件位置，使得末端夹具准确抓放工件，记录此时机械臂物理坐标为目标工件抓放基准位姿（6D位姿），如下图所示。
• 控制机械臂运动至示教拍照位置，相机拍摄标识区Mark点，记录此时Mark点像素坐标为基准像素坐标、图像中选取一条边的角度为基准角度。

作业

根据标定方式的不同，作业阶段的3D位姿求解方法亦有所区别。

采用2D标定

1. 利用Mark点基准像素坐标、运行像素坐标、Mark基准位姿，以及N点标定矩阵，使用单点抓取模块得到Mark点运行位姿（二维平面坐标）。
2. 利用示教得到的目标工件抓放基准位姿、Mark基准位姿，求解得到目标工件到Mark基准位姿的4x4相对位姿矩阵。
3. 将Mark基准位姿(X,Y,Z,Rz,Ry,Rx)中的X/Y/Rz用Mark点运行位姿（二维平面坐标）替换，然后转换为4x4齐次位姿矩阵，右乘相对位姿矩阵，得到目标工件的抓放运行位姿齐次矩阵。将该齐次位姿矩阵转换为欧拉角，并发送给机械臂，即可控制机械臂到达目标工件抓放位置，实现抓取。

采用3D标定

记手眼标定矩阵为cam2grip；示教时PnP算法得到外参矩阵为board2cam，示教拍照位为grip2robot，目标工件抓放基准位姿为obj2robot；作业时PnP算法得到外参矩阵为board’2cam’，作业拍照位为grip’2robot，目标工件抓放运行位姿为obj’2robot，则有：

obj’2robot = board’2robot * obj’2board’ = grip’2robot * cam’2grip’ * board’2cam’ * obj’2board’

由于手眼关系保持不变，标识区与工件的相对位置关系保持不变，故有：

cam’2grip’ = cam2grip
obj’2board’ = obj2board

故有：

obj’2robot = grip’2robot * cam2grip * board’2cam’ * (cam2grip * board2cam).inv() * grip2robot.inv() * obj2robot

(目标工件抓放运行位姿矩阵 = 生产拍照位姿矩阵 * 手眼标定矩阵 * 作业时PnP外参矩阵 * 逆矩阵(手眼标定矩阵 * 示教时PnP外参矩阵) * 逆矩阵(示教拍照位姿矩阵) * 目标工件抓放基准位姿矩阵)

将该齐次位姿矩阵转换为欧拉角，并发送给机械臂，即可控制机械臂到达目标工件抓放位置，实现抓取。

VM方案

标定方案

使用VM搭建标定方案如下，采用TCP通讯实现自动触发标定。

作业方案

使用VM搭建作业方案如下：

运行结果

3D偏移抓取位姿计算结果如下图所示。

测试验证

手动旋转和平移标定板，模拟目标工件位置发生变动。机器人到达示教拍照位拍照。

机器人接收VM发送的3D偏移抓取位姿坐标(内旋ZYX欧拉角形式)，到达抓取位置。

总结分析

根据标定方式的不同，2D-3D定位引导算法有两条路线：

1. 2D标定 + 2D单点抓取 + 3D位姿估计
2. 3D标定(立体标定+手眼标定) + PnP估计 + 3D位姿估计

其中，路线1的标定操作更简单（立体标定+手眼标定退化为单应矩阵标定），且定位引导精度更高，但需要机械臂对标识区Mark点进行点触示教，且需保证机械臂作业拍照位姿与示教拍照位姿一致；路线2无需机械臂对标识区Mark点进行点触示教，且不限制机械臂作业拍照位姿，但标定操作复杂，且定位引导精度不高。

综上所述，具体选择何种路线，应结合项目需求和实际工程场景确定。

核心代码

/*********************************************************************
说明： 欧拉角转换为3*3旋转矩阵R
参数：
    eulerAngle：弧度值
    seq		  ：指定欧拉角xyz的排列顺序如："xyz" "zyx"
**********************************************************************/
Mat EulerAngleToRotatedMatrixPnP(const Mat& eulerAngle, String m_sSeq)
{
    CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);
    eulerAngle /= (180 / CV_PI);
    Matx13d m(eulerAngle);
    auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
    auto rxs = sin(rx), rxc = cos(rx);
    auto rys = sin(ry), ryc = cos(ry);
    auto rzs = sin(rz), rzc = cos(rz);
    //XYZ方向的旋转矩阵
    Mat RotX = (Mat_<double>(3, 3) << 1, 0, 0,
        0, rxc, -rxs,
        0, rxs, rxc);
    Mat RotY = (Mat_<double>(3, 3) << ryc, 0, rys,
        0, 1, 0,
        -rys, 0, ryc);
    Mat RotZ = (Mat_<double>(3, 3) << rzc, -rzs, 0,
        rzs, rzc, 0,
        0, 0, 1);
    //按顺序合成后的旋转矩阵
    cv::Mat rotMat;
    if (m_sSeq == "zyx") rotMat = RotX * RotY * RotZ;
    else if (m_sSeq == "yzx") rotMat = RotX * RotZ * RotY;
    else if (m_sSeq == "zxy") rotMat = RotY * RotX * RotZ;
    else if (m_sSeq == "yxz") rotMat = RotZ * RotX * RotY;
    else if (m_sSeq == "xyz") rotMat = RotZ * RotY * RotX;
    else if (m_sSeq == "xzy") rotMat = RotY * RotZ * RotX;
    else
    {
        cout << "Euler Angle Sequence string is wrong...";
    }
    return rotMat;
}

/**************************************************
* @brief   位姿矩阵转欧拉角
* @note
* @param   Mat_<double> Homo_Mat    4*4位姿矩阵
* @param   CartesianPosePnP* pose   欧拉角位姿
**************************************************/
void HomoMatrixToCartesianPose(Mat_<double> Homo_Mat, CartesianPosePnP* pose)
{
    pose->rx = atan2(Homo_Mat.at<double>(2, 1), Homo_Mat.at<double>(2, 2)) * RAD2ANGLE;
    pose->ry = atan2(-Homo_Mat.at<double>(2, 0), sqrt(pow(Homo_Mat.at<double>(2, 1), 2) + pow(Homo_Mat.at<double>(2, 2), 2))) * RAD2ANGLE;
    pose->rz = atan2(Homo_Mat.at<double>(1, 0), Homo_Mat.at<double>(0, 0)) * RAD2ANGLE;
    pose->x = Homo_Mat.at<double>(0, 3);
    pose->y = Homo_Mat.at<double>(1, 3);
    pose->z = Homo_Mat.at<double>(2, 3);
}

/**************************************************
* @brief   将旋转矩阵与平移向量合成为齐次矩阵
* @note
* @param   Mat& R   3*3旋转矩阵
* @param   Mat& T   3*1平移矩阵
* @return  Mat      4*4齐次矩阵
**************************************************/
Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T)
{
    Mat HomoMtr;
    Mat_<double> R1 = (Mat_<double>(4, 3) <<
        R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
        R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
        R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
        0, 0, 0);
    Mat_<double> T1 = (Mat_<double>(4, 1) <<
        T.at<double>(0, 0),
        T.at<double>(1, 0),
        T.at<double>(2, 0),
        1);
    cv::hconcat(R1, T1, HomoMtr);
    return HomoMtr;
}

/*********************************************************************
说明： 四元数转换为旋转矩阵。数据类型double，四元数定义
q = w + x*i + y*j + z*k
参数：
    q  ：四元数。
    seq：指定欧拉角xyz的排列顺序如："xyz" "zyx"。
返回：
    3*3旋转矩阵。
**********************************************************************/
Mat CalibrateHandEye::QuaternionToRotatedMatrix(const Vec4d& q)
{
    double w = q[0], x = q[1], y = q[2], z = q[3];
    double x2 = x * x, y2 = y * y, z2 = z * z;
    double xy = x * y, xz = x * z, yz = y * z;
    double wx = w * x, wy = w * y, wz = w * z;
    Matx33d res{
        1 - 2 * (y2 + z2),	2 * (xy - wz),		2 * (xz + wy),
        2 * (xy + wz),		1 - 2 * (x2 + z2),	2 * (yz - wx),
        2 * (xz - wy),		2 * (yz + wx),		1 - 2 * (x2 + y2),
    };
    return Mat(res);
}