文章目录
- 一、手眼标定原理
- 眼在手上
- 眼在手外
- 二、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_base2gripperInputArrayOfArrays t_gripper2base, // <=> T_base2gripperInputArrayOfArrays R_target2cam, // <=> R_target2camInputArrayOfArrays t_target2cam, // <=> T_target2camOutputArray R_cam2gripper, // <=> R_cam2baseOutputArray t_cam2gripper, // <=> T_cam2baseHandEyeCalibrationMethod 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自带的函数套用参数即可完成眼在手上和眼在手外两种标定。