OpenCV机械臂手眼标定

一、手眼标定原理

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

眼在手上

眼在手上的情况下，标定板不动，移动机械臂拍照，此时标定板与机械臂基座标系相对位置保持固定，我们将这个转化矩阵分解成：
$$T_t^b=T_g^b\ast T_c^g\ast T_t^c$$
其中,$T_t^c$表示标定板到相机的转换关系，也就是相机外参，一幅图像对应一个外参，$T_c^g$表示相机到机械臂末端转换矩阵，也就是手眼关系，这就是我们要求的东西，这是固定不变的，$T_g^b$是表示机械臂末端到基座标系的转换矩阵，也就是欧拉角所对应的位姿。特别注意：末端欧拉角代表的就是末端至基座标系的转换位姿。$T_t^b$表示标定板到基座标系的转换矩阵。
移动相机拍照，由于$T_t^b$保持不变所以可以将两次拍照结果联立方程:
$$T^{(0)}{}_g^b\ast T_c^g\ast T^{(0)}{}_t^c=T^{(1)}{}_g^b\ast T_c^g\ast T^{(1)}{}_t^c$$
换一下形式：
$$T^{-1(1)}{}_g^b\ast T^{(0)}{}_g^b\ast T_c^g=T_c^g\ast T^{(1)}{}_t^c\ast T^{-1(0)}{}_t^c$$
很显然，我们得到了$AX=XB$方程，其中$A=T^{-1(1)}{}_g^b\ast T^{(0)}{}_g^b$，$B=T^{(1)}{}_t^c\ast T^{-1(0)}{}_t^c$，$X=T_c^g$

眼在手外

眼在手外的情况下，标定板夹在机械臂末端，即标定板和机械臂末端位置固定，移动机械臂，相机拍照，将标定板和机械臂末端转换矩阵分解成：
$$T_t^g=T_b^g\ast T_c^b\ast T_t^c$$
其中，$T_c^b$是相机到机械臂基座标系的转换矩阵，这个是固定的。$T_b^g$是机械臂基座标系到机械臂末端的转换矩阵，注意，这里与眼在手上的矩阵$T_g^b$是反的，也就是欧拉角得到的位姿矩阵要求逆。$T_t^g$是标定板到机械臂末端的转换矩阵，这个是固定的，所以我们利用这个关系联立方程：
$$T^{(0)}{}_b^g\ast T_c^b\ast T^{(0)}{}_t^c=T^{(1)}{}_b^g\ast T_c^b\ast T^{(1)}{}_t^c$$
换一下形式：
$$T^{-1(1)}{}_b^g\ast T^{(0)}{}_b^g\ast T_c^b=T_c^b\ast T^{(1)}{}_t^c\ast T^{-1(0)}{}_t^c$$
很显然，这里也是得到了$AX=XB$方程，其中$A=T^{-1(1)}{}_b^g\ast T^{(0)}{}_b^g$，$B=T^{(1)}{}_t^c\ast T^{-1(0)}{}_t^c$，$X=T_c^b$
注意，这边求出来的$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=Rz\ast Ry\ast 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自带的函数套用参数即可完成眼在手上和眼在手外两种标定。