OpenCV机械臂手眼标定_opencv手眼标定

文章目录

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

一、手眼标定原理

手眼标定即标定相机坐标系与机械臂坐标系转换关系，使用一块棋盘格即可完成标定。眼在手上和眼在手外原理类似，其本质都是求解$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​
很显然，我们得到了$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​
很显然，这里也是得到了$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$和眼在手上是不一样的，这边是直接相机到机械臂基座标系，眼在手上求出来的是相机到机械臂末端坐标系。

二、OpenCV手眼标定函数

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

voidcv::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自带的函数套用参数即可完成眼在手上和眼在手外两种标定。