某2D-3D应用场景示意图如下图所示。
项目需求:
由于待抓取工件透明,且无法被相机直接拍到,因此需要在工作台面上贴附作为引导基准的标识区,标识区与工件的相对位置关系保持不变。可通过贴附标签或印刷蚀刻等手段设置其内部特征,优选方案为贴附二维码标签或十字标签。示教和作业时,相机只拍摄标识区内固定特征点(Mark点)。
因此,该场景为典型的使用2D相机求解6D位姿问题。
通常将使用2D相机求解6D位姿问题,称为2D-3D定位引导问题。
视觉定位引导任务可以拆分为三步:标定、示教与作业。
标定:求解相机坐标系(或像素坐标系)到机械臂坐标系的变换关系
示教:获得待抓取目标在机械臂坐标系下的基准位姿
作业:相机采集图像,根据特征点坐标及角度,求解待抓取目标的运行位姿
针对2D-3D定位引导场景,有两种标定方案:
1.采用2D标定,即九点(或9+3点)标定,求解机械臂坐标系到像素坐标系的XOY平面单应矩阵;
2.采用3D标定,求解相机的内、外参和畸变系数,以及相机坐标系到机械臂坐标系的手眼转换矩阵。
根据标定方式的不同,作业阶段的3D位姿求解方法亦有所区别。
使用VM搭建标定方案如下,采用TCP通讯实现自动触发标定。
使用VM搭建作业方案如下:
3D偏移抓取位姿计算结果如下图所示。
手动旋转和平移标定板,模拟目标工件位置发生变动。机器人到达示教拍照位拍照。
机器人接收VM发送的3D偏移抓取位姿坐标(内旋ZYX欧拉角形式),到达抓取位置。
根据标定方式的不同,2D-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);
}