【3D系列】2D-3D定位引导算法原理及实现
通常将使用2D相机求解6D位姿问题,称为2D-3D定位引导问题。本文主要介绍2D-3D定位引导算法原理以及2D标定、3D标定两种技术路线,尤其适用于复合机器人应用场景。

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

应用场景

某2D-3D应用场景示意图如下图所示。
d6405ff27144ea17a57396603185151a.png
项目需求:

  • 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位姿),如下图所示。
    d6405ff27144ea17a57396603185151a.png
  • 控制机械臂运动至待抓放目标工件位置,使得末端夹具准确抓放工件,记录此时机械臂物理坐标为目标工件抓放基准位姿(6D位姿),如下图所示。
    222.png
  • 控制机械臂运动至示教拍照位置,相机拍摄标识区Mark点,记录此时Mark点像素坐标为基准像素坐标、图像中选取一条边的角度为基准角度。

作业

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

  1. 采用2D标定
    1.1 利用Mark点基准像素坐标、运行像素坐标、Mark基准位姿,以及N点标定矩阵,使用单点抓取模块得到Mark点运行位姿(二维平面坐标)。
    1.2 利用示教得到的目标工件抓放基准位姿、Mark基准位姿,求解得到目标工件到Mark基准位姿的4x4相对位姿矩阵。
    1.3 将Mark基准位姿(X,Y,Z,Rz,Ry,Rx)中的X/Y/Rz用Mark点运行位姿(二维平面坐标)替换,然后转换为4x4齐次位姿矩阵,右乘相对位姿矩阵,得到目标工件的抓放运行位姿齐次矩阵。将该齐次位姿矩阵转换为欧拉角,并发送给机械臂,即可控制机械臂到达目标工件抓放位置,实现抓取。
  2. 采用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通讯实现自动触发标定。
33.png

作业方案

使用VM搭建作业方案如下:
44.png

运行结果

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

测试验证

手动旋转和平移标定板,模拟目标工件位置发生变动。机器人到达示教拍照位拍照。
66.png
机器人接收VM发送的3D偏移抓取位姿坐标(内旋ZYX欧拉角形式),到达抓取位置。
77.png

总结分析

根据标定方式的不同,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); }
附件:
版权声明:本文为V社区用户原创内容,转载时必须标注文章的来源(V社区),文章链接、文章作者等基本信息,否则作者和本社区有权追究责任。如果您发现本社区中有涉嫌抄袭的内容,欢迎发送邮件至:v-club@hikrobotics.com 进行举报,并提供相关证据,一经查实,本社区将立刻删除涉嫌侵权内容。
上一篇

【开发有道系列】从0到1开发算法模块

下一篇

【2D算法系列】圆拟合算法剖析与硬核实战

评论请先登录 登录
全部评论 0
Lv.0
0
关注
0
粉丝
0
创作
0
获赞
相关阅读
  • 浅谈VM的使用技巧篇二之VM脚本中调用opencv库进行图像处理
    2025-01-15 浏览 0
  • 【2D算法系列】机器视觉核心算法之N点标定
    2025-02-07 浏览 0
  • RoboX屏蔽升级提示
    2025-01-20 浏览 0
  • [新功能上线]V学院更新弹幕功能!
    2025-01-13 浏览 0
  • 移动机器人实操短视频目录
    2025-01-17 浏览 0

请升级浏览器版本

您正在使用的浏览器版本过低,请升级最新版本以获得更好的体验。

推荐使用以下浏览器