OpenCV机械臂手眼标定

文章目录

• 一、手眼标定原理
• • 眼在手上
  • 眼在手外
• 二、OpenCV手眼标定函数
• 三、眼在手外使用示例
• 四、欧拉角转位姿矩阵
• 总结

---

一、手眼标定原理

手眼标定即标定相机坐标系与机械臂坐标系转换关系，使用一块棋盘格即可完成标定。眼在手上和眼在手外原理类似，其本质都是求解 A X = X B AX=XB AX=XB方程，只是这里的A和B含义不一样。

眼在手上

眼在手上的情况下，标定板不动，移动机械臂拍照，此时标定板与机械臂基座标系相对位置保持固定，我们将这个转化矩阵分解成：
T t b = T g b ∗ T c g ∗ T t c T_t^b=T_g^b*T_c^g*T_t^c Ttb=Tgb∗Tcg∗Ttc

其中, T t c T_t^c Ttc表示标定板到相机的转换关系，也就是相机外参，一幅图像对应一个外参， T c g T_c^g Tcg表示相机到机械臂末端转换矩阵，也就是手眼关系，这就是我们要求的东西，这是固定不变的， T g b T_g^b Tgb是表示机械臂末端到基座标系的转换矩阵，也就是欧拉角所对应的位姿。特别注意：末端欧拉角代表的就是末端至基座标系的转换位姿。 T t b T_t^b Ttb表示标定板到基座标系的转换矩阵。

移动相机拍照，由于 T t b T_t^b Ttb保持不变所以可以将两次拍照结果联立方程:
T ( 0 ) g b ∗ T c g ∗ T ( 0 ) t c = T ( 1 ) g b ∗ T c g ∗ T ( 1 ) t c T^{(0)}{_g^b}*T_c^g*T^{(0)}{_t^c}=T^{(1)}{_g^b}*T_c^g*T^{(1)}{_t^c} T(0)gb∗Tcg∗T(0)tc=T(1)gb∗Tcg∗T(1)tc

换一下形式：
T − 1 ( 1 ) g b ∗ T ( 0 ) g b ∗ T c g = T c g ∗ T ( 1 ) t c ∗ T − 1 ( 0 ) t c T^{-1(1)}{_g^b}*T^{(0)}{_g^b}*T_c^g=T_c^g*T^{(1)}{_t^c}*T^{-1(0)}{_t^c} T−1(1)gb∗T(0)gb∗Tcg=Tcg∗T(1)tc∗T−1(0)tc

很显然，我们得到了 A X = X B AX=XB AX=XB方程，其中 A = T − 1 ( 1 ) g b ∗ T ( 0 ) g b A=T^{-1(1)}{_g^b}*T^{(0)}{_g^b} A=T−1(1)gb∗T(0)gb， B = T ( 1 ) t c ∗ T − 1 ( 0 ) t c B=T^{(1)}{_t^c}*T^{-1(0)}{_t^c} B=T(1)tc∗T−1(0)tc， X = T c g X=T_c^g X=Tcg

眼在手外

眼在手外的情况下，标定板夹在机械臂末端，即标定板和机械臂末端位置固定，移动机械臂，相机拍照，将标定板和机械臂末端转换矩阵分解成：
T t g = T b g ∗ T c b ∗ T t c T_t^g=T_b^g*T_c^b*T_t^c Ttg=Tbg∗Tcb∗Ttc

其中， T c b T_c^b Tcb是相机到机械臂基座标系的转换矩阵，这个是固定的。 T b g T_b^g Tbg是机械臂基座标系到机械臂末端的转换矩阵，注意，这里与眼在手上的矩阵 T g b T_g^b Tgb是反的，也就是欧拉角得到的位姿矩阵要求逆。 T t g T_t^g Ttg是标定板到机械臂末端的转换矩阵，这个是固定的，所以我们利用这个关系联立方程：
T ( 0 ) b g ∗ T c b ∗ T ( 0 ) t c = T ( 1 ) b g ∗ T c b ∗ T ( 1 ) t c T^{(0)}{_b^g}*T_c^b*T^{(0)}{_t^c}=T^{(1)}{_b^g}*T_c^b*T^{(1)}{_t^c} T(0)bg∗Tcb∗T(0)tc=T(1)bg∗Tcb∗T(1)tc

换一下形式：
T − 1 ( 1 ) b g ∗ T ( 0 ) b g ∗ T c b = T c b ∗ T ( 1 ) t c ∗ T − 1 ( 0 ) t c T^{-1(1)}{_b^g}*T^{(0)}{_b^g}*T_c^b=T_c^b*T^{(1)}{_t^c}*T^{-1(0)}{_t^c} T−1(1)bg∗T(0)bg∗Tcb=Tcb∗T(1)tc∗T−1(0)tc

很显然，这里也是得到了 A X = X B AX=XB AX=XB方程，其中 A = T − 1 ( 1 ) b g ∗ T ( 0 ) b g A=T^{-1(1)}{_b^g}*T^{(0)}{_b^g} A=T−1(1)bg∗T(0)bg， B = T ( 1 ) t c ∗ T − 1 ( 0 ) t c B=T^{(1)}{_t^c}*T^{-1(0)}{_t^c} B=T(1)tc∗T−1(0)tc， X = T c b X=T_c^b X=Tcb
注意，这边求出来的 X X X和眼在手上是不一样的，这边是直接相机到机械臂基座标系，眼在手上求出来的是相机到机械臂末端坐标系。

二、OpenCV手眼标定函数

原OpenCV手眼标定函数输入输出参数是根据眼在手上来定义的，如果想要套用眼在手外的话需要改变一下输入参数的含义，其实看上面的原理公式我们就知道，稍微改一下就成，代码注释在下面：

void
cv::calibrateHandEye(InputArrayOfArrays 	R_gripper2base,	// <=> R_base2gripper
                     InputArrayOfArrays 	t_gripper2base,	// <=> T_base2gripper
                     InputArrayOfArrays 	R_target2cam,	// <=> R_target2cam
                     InputArrayOfArrays 	t_target2cam,	// <=> T_target2cam
                     OutputArray 	        R_cam2gripper,	// <=> R_cam2base
                     OutputArray 	        t_cam2gripper,	// <=> T_cam2base
                     HandEyeCalibrationMethod method = CALIB_HAND_EYE_TSAI)	

三、眼在手外使用示例

std::vector<Mat> R_base2gripper, T_base2gripper, R_target2cam, T_target2cam;
    Mat R_cam2base, t_cam2base;
    for(int i=0;i<poses.size();i++)
    {
        auto& pose = poses[i];
        Mat transform = Mat::eye(4,4,CV_64F);
        pose.R.copyTo(transform(cv::Rect(0,0,3,3)));
        pose.t.copyTo(transform(cv::Rect(3,0,1,3)));
        Mat inv_transform = transform.inv();
        pose.R = inv_transform(cv::Rect(0,0,3,3)).clone();
        pose.t = inv_transform(cv::Rect(3,0,1,3)).clone();
        R_base2gripper.push_back(pose.R);
        T_base2gripper.push_back(pose.t);
        Mat extrinsic = extrinsics[i]; //相机外参
        R_target2cam.push_back(extrinsic(cv::Rect(0,0,3,3)).clone());
        T_target2cam.push_back(extrinsic(cv::Rect(3,0,1,3)).clone());
    }

    cv::calibrateHandEye(R_base2gripper, T_base2gripper, R_target2cam, T_target2cam, R_cam2base, t_cam2base, CALIB_HAND_EYE_TSAI );
    

四、欧拉角转位姿矩阵

下边是会用到的一个辅助函数，欧拉角转成位姿矩阵，需要注意一下你的机械臂欧拉角是怎么定义的，下面代码默认是ZYX，所以 R = R z ∗ R y ∗ R x R=Rz*Ry*Rx R=Rz∗Ry∗Rx

/**
 * @brief 将欧拉角转换为4x4位姿矩阵（齐次变换矩阵）
 * @param euler_angles 欧拉角，顺序为(Z, Y, X) ，单位为弧度
 * @param translation 平移向量，默认为(0,0,0)
 * @return cv::Mat 4x4位姿矩阵
 */
cv::Mat eulerAnglesToPoseMatrix(const cv::Vec3d& euler_angles, const cv::Vec3d& translation = cv::Vec3d(0,0,0)) {
    // 提取欧拉角
    double z = euler_angles[2]; // Yaw (绕Z轴)
    double y = euler_angles[1]; // Pitch (绕Y轴) 
    double x = euler_angles[0]; // Roll (绕X轴)
    
    // 计算各轴旋转矩阵
    cv::Mat Rz = (cv::Mat_<double>(3,3) <<
        cos(z), -sin(z), 0,
        sin(z),  cos(z), 0,
        0,       0,      1);
    
    cv::Mat Ry = (cv::Mat_<double>(3,3) <<
        cos(y), 0, sin(y),
        0,      1, 0,
        -sin(y),0, cos(y));
    
    cv::Mat Rx = (cv::Mat_<double>(3,3) <<
        1, 0,      0,
        0, cos(x), -sin(x),
        0, sin(x), cos(x));
    
    // 组合旋转矩阵：R = Rz * Ry * Rx （固定坐标系Z→Y→X顺序）
    cv::Mat R = Rz * Ry * Rx;
    
    // 构建4x4齐次变换矩阵
    cv::Mat pose = cv::Mat::eye(4, 4, CV_64F);
    R.copyTo(pose(cv::Rect(0, 0, 3, 3)));  // 填充旋转部分
    pose.at<double>(0, 3) = translation[0]; // X平移
    pose.at<double>(1, 3) = translation[1]; // Y平移
    pose.at<double>(2, 3) = translation[2]; // Z平移
    
    return pose;
}

---

总结

这篇文章总结了手眼标定的原理，利用OpenCV自带的函数套用参数即可完成眼在手上和眼在手外两种标定。