【眼在手上自动手眼标定Opencv-C++】_opencv手眼标定

眼在手上自动手眼标定Opencv-C++

• 背景
• 一、算子原理
• • 1、坐标系介绍
  • 2、opencv主要算子功能介绍
• 二、标定过程及原理
• 三、关键代码
• • 1、计算圆心或角点，做相机内外参标定
  • 2、相机内外参及机械手坐标做手眼标定
  • • A、机械手坐标需要根据内外旋和旋转顺序转为旋转和平移矩阵，代码如下：
    • B、手眼标定，代码如下：
  • 3、像素坐标转换为机械手坐标（机械手底座为坐标原点）
  • 4、二维码角度纠正机械手末端旋转
• 四、经验总结

背景

  标准六轴机械臂做眼在手上的自动手眼标定，过程为机械手末端抓取矩形块，将矩形块插入墙面凹槽内。

一、算子原理

1、坐标系介绍

  如图所示，眼在手上的手眼标定主要确定相机坐标系与工具坐标系的旋转平移矩阵；

2、opencv主要算子功能介绍

  算子calibrateCamera：主要输入有①标定板图片棋盘格角点或者圆心坐标，②标定板三维对象点，③标定板的尺寸信息；
  算子calibrateHandEye：输入有calibrateCamera算子求出的相机与标定板的外参即旋转平移矩阵，以及对应的机械手末端坐标（比如拍摄26张图，拍摄每组图的机械手坐标要与图片一一对应），机械手末端坐标有X，Y，Z，Rx，Ry，Rz，需要将这六个坐标转换为旋转平移矩阵（转换的时候要确定机械手的末端旋转方式与旋转顺序，比如外旋“ZYX”），输入至算子内，即可求得相机与机械手末端之间的旋转平移矩阵；

二、标定过程及原理

  标定过程：标定板放至地面保证水平（或者垂直墙面），机械手带着相机多姿态拍摄标定板，拍摄的每次记录机械手末端夹爪的坐标（以机械手基座为原点），标定图如下所示：

  标定原理：
    博主写的很赞，受到很大启发，多谢好心人分享，此处直接引用，博客地址：链接: link.

三、关键代码

1、计算圆心或角点，做相机内外参标定

  标定板三维对象点需要根据标定板信息进行建立，代码如下：

// 生成标定板的三维坐标点vector<Point3f> generateObjectPoints(int boardX, int boardY, float squareSize) { vector<Point3f> objectPoints; for (int i = 0; i < boardY; ++i) { for (int j = 0; j < boardX; ++j) { objectPoints.emplace_back(j * squareSize, i * squareSize, 0.0f); // 0.0f表示假设标定板放在世界坐标系中z=0的平面上（标定板坐标系） } } return objectPoints;}

  标定板图片棋盘格角点，代码如下：

int HandEyeFindChessboard(uchar* imgData, int boardX, int boardY, int width, int height, double*& pointsX, double*& pointsY){ try { Mat src = Mat(Size(width, height), CV_8UC3, imgData); vector<Point2f> pointbuf; Size patternSize(boardX, boardY); Mat grayImg; cvtColor(src, grayImg, cv::COLOR_BGR2GRAY); bool found = findChessboardCorners(grayImg, patternSize, pointbuf, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE); if (found) { // 亚像素精确化 cornerSubPix(grayImg, pointbuf, cv::Size(11, 11), cv::Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1)); for (int i = 0; i < pointbuf.size(); i++) { pointsX[i] = pointbuf[i].x; pointsY[i] = pointbuf[i].y; std::cout << \"点 \" << i << \": (\" << pointsX[i] << \", \" << pointsY[i] << \")\" << std::endl; } return 0; } else { std::cout << \"未找到角点\" << std::endl; return -1; } } catch (const std::exception& ex) { }}

  相机内外参标定，代码如下：

// 第一步：找圆CalibrateHandEye Calibrate;cv::String folder = Calibrate.calibDatasHandEyePath + \"/calibImg\";std::vector<cv::String> filenames;cv::glob(folder, filenames);vector<vector<Point2f>> imagePoints = FindAllChessboard(filenames, boardX, boardY);vector<vector<Point3f>> objectPoints; // 存储标定板上的三维坐标// 第二步：相机标定// 生成标定板三维对象点vector<Point3f> singleObjectPoints = generateObjectPoints(boardX, boardY, squareSize);for (int i = 0; i < filenames.size(); i++){ objectPoints.push_back(singleObjectPoints);}Mat cameraMatrix = Mat::eye(3, 3, CV_64F); // 相机的内参矩阵3*3Mat distCoeffs; // 相机的5个畸变系数vector<Mat> rvecs, tvecs;  // 旋转向量和平移向量double rms_t = calibrateCamera(objectPoints, imagePoints, Size(width, height), cameraMatrix, distCoeffs, rvecs, tvecs);

2、相机内外参及机械手坐标做手眼标定

A、机械手坐标需要根据内外旋和旋转顺序转为旋转和平移矩阵，代码如下：

/** @brief 欧拉角 -> 3*3 的R*@param eulerAngle角度值*@param seq指定欧拉角xyz的排列顺序如：\"xyz\" \"zyx\"*///注意自己带入的欧拉角是内旋还是外旋,在传入旋转顺序之前一定要确定旋转方式//内旋：绕着自身轴进行旋转，如果旋转顺序为X-Y-Z，则旋转矩阵为Rx*Ry*Rz//外旋：绕着固定轴进行旋转，如果旋转顺序为X-Y-Z，则旋转矩阵为Rz*Ry*RxMat eulerAngleToRotatedMatrix(const Mat& eulerAngle, const string& rotationseq, const string& seq){ bool angle = true; // 输入为角度true，为弧度false if (angle) { CV_Assert(eulerAngle.rows == 1 & eulerAngle.cols == 3); eulerAngle /= (180 / CV_PI); // 将角度转为弧度，C++的三角函数是以弧度进行计算的 } Matx13d m(eulerAngle); auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2); auto xs = sin(rx), xc = cos(rx); auto ys = sin(ry), yc = cos(ry); auto zs = sin(rz), zc = cos(rz); Mat rotX = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, xc, -1 * xs, 0, xs, xc); Mat rotY = (cv::Mat_<double>(3, 3) << yc, 0, ys, 0, 1, 0, -1 * ys, 0, yc); Mat rotZ = (cv::Mat_<double>(3, 3) << zc, -1 * zs, 0, zs, zc, 0, 0, 0, 1); Mat rotMat; if (rotationseq == \"External rotation\") { // 外旋左乘 if (seq == \"zyx\")rotMat = rotX * rotY * rotZ; else if (seq == \"yzx\")rotMat = rotX * rotZ * rotY; else if (seq == \"zxy\")rotMat = rotY * rotX * rotZ; else if (seq == \"xzy\")rotMat = rotY * rotZ * rotX; else if (seq == \"yxz\")rotMat = rotZ * rotX * rotY; else if (seq == \"xyz\")rotMat = rotZ * rotY * rotX; else { error(Error::StsAssert, \"Euler angle sequence string is wrong.\", __FUNCTION__, __FILE__, __LINE__); } if (!isRotationMatrix(rotMat)) { error(Error::StsAssert, \"Euler angle can not convert to rotated matrix\", __FUNCTION__, __FILE__, __LINE__); } } else if (rotationseq == \"Internal rotation\") { // 内旋右乘 if (seq == \"xyz\")rotMat = rotX * rotY * rotZ; else if (seq == \"xzy\")rotMat = rotX * rotZ * rotY; else if (seq == \"yxz\")rotMat = rotY * rotX * rotZ; else if (seq == \"yzx\")rotMat = rotY * rotZ * rotX; else if (seq == \"zxy\")rotMat = rotZ * rotX * rotY; else if (seq == \"zyx\")rotMat = rotZ * rotY * rotX; else { error(Error::StsAssert, \"Euler angle sequence string is wrong.\", __FUNCTION__, __FILE__, __LINE__); } if (!isRotationMatrix(rotMat)) { error(Error::StsAssert, \"Euler angle can not convert to rotated matrix\", __FUNCTION__, __FILE__, __LINE__); } } return rotMat;}

B、手眼标定，代码如下：

// 第三步：手眼标定Mat tempR, tempT;vector<Mat> R_gripper2base, t_gripper2base, R_target2cam, t_target2cam;// 从机械手坐标文件提取末端坐标((rx,ry,rz) 是末端的欧拉角（或轴角），表示旋转方向)Mat_<double> ToolPoses = readMatrixFromFile1(\"./DeepLearning/dynamicHand/toolPose/ToolPose.txt\", filenames.size(), 6);for (size_t i = 0; i < filenames.size(); i++){ Mat row = ToolPoses.row(i); Mat tmp = attitudeVectorToMatrix(row, false, \"External rotation\", \"xyz\"); // 在这里设置-可选择内旋（Internal rotation）与外旋（External rotation）-内旋zyx RT2R_T(tmp, tempR, tempT); R_gripper2base.push_back(tempR); t_gripper2base.push_back(tempT); // 提取相机外参（旋转和平移） Mat R_cam; Rodrigues(rvecs[i], R_cam); // 将 rvec (3×1) 转换为 3×3 旋转矩阵 R_target2cam.push_back(R_cam); t_target2cam.push_back(tvecs[i]);}// 手眼标定Mat R_cam2gripper, t_cam2gripper;cv::calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper, t_cam2gripper, CALIB_HAND_EYE_TSAI); // CALIB_HAND_EYE_DANIILIDIS CALIB_HAND_EYE_TSAI(适用于旋转角度较大的标定数据)std::cout << \"手眼旋转矩阵:\\n\" << R_cam2gripper << endl; // 3*3std::cout << \"手眼平移向量:\\n\" << t_cam2gripper << endl; // 3*1// 保存标定矩阵（手眼标定矩阵）string fileName = fileNameChar;string calibrationFile = Calibrate.calibDatasHandEyePath + \"/calibDatas/\" + fileName; saveCalibrationResults(calibrationFile, cameraMatrix, distCoeffs, R_cam2gripper, t_cam2gripper);

3、像素坐标转换为机械手坐标（机械手底座为坐标原点）

  像素坐标转为机械手坐标，需要先将像素坐标转为相机坐标，然后矩阵相乘即可得到机械手坐标，代码如下所示：

/** * @brief 将像素坐标（带深度）通过手眼标定结果转换为机器人基坐标系下的坐标 * @param pixel 输入的像素坐标 (u, v) * @param depth Z 轴深度值 (在相机坐标系下)，单位：mm * @param cameraMatrix 相机内参矩阵 (3x3, CV_64F) * @param distCoeffs 相机畸变系数 (1xN, CV_64F) * @param R_cam2gripper 相机到夹具的旋转矩阵 (3x3, CV_64F) * @param t_cam2gripper 相机到夹具的平移向量 (3x1, CV_64F)，单位：mm * @param R_gripper2base 夹具相对于基座的旋转矩阵 (3x3, CV_64F) (拍照时的位姿) * @param t_gripper2base 夹具相对于基座的平移向量 (3x1, CV_64F)，单位：mm (拍照时的位姿) * @param baseCoord 输出：机器人基坐标系下的3D点 (X, Y, Z)，单位：mm * @return bool 是否转换成功 */bool pixelToRobotBaseCoord_Homogeneous(const Point2f& pixel, double depth, const Mat& cameraMatrix, const Mat& distCoeffs, const Mat& R_cam2gripper, const Mat& t_cam2gripper, const Mat& R_gripper2base, const Mat& t_gripper2base, Point3d& baseCoord){ // Step 1: 像素坐标 -> 相机坐标 Point3d P_cam; if (!pixelToCameraCoord(pixel, cameraMatrix, distCoeffs, depth, P_cam)) { std::cerr << \"像素到相机坐标转换失败\" << std::endl; return false; } // Step 2: 构造齐次坐标点 P_cam_h Mat P_cam_h = (Mat_<double>(4, 1) << P_cam.x, P_cam.y, P_cam.z, 1.0); // Step 3: 构造 T_ee_cam = [R_cam2gripper | t_cam2gripper] Mat T_ee_cam = Mat::eye(4, 4, CV_64F); R_cam2gripper.copyTo(T_ee_cam(Rect(0, 0, 3, 3))); t_cam2gripper.copyTo(T_ee_cam(Rect(3, 0, 1, 3))); // Step 4: 构造 T_base_ee = [R_gripper2base | t_gripper2base] Mat T_base_ee = Mat::eye(4, 4, CV_64F); R_gripper2base.copyTo(T_base_ee(Rect(0, 0, 3, 3))); t_gripper2base.copyTo(T_base_ee(Rect(3, 0, 1, 3))); // Step 5: 统一右乘方式求 P_base Mat P_base_h = T_base_ee * T_ee_cam * P_cam_h; // Step 6: 提取输出 baseCoord.x = P_base_h.at<double>(0); baseCoord.y = P_base_h.at<double>(1); baseCoord.z = P_base_h.at<double>(2); return true;}

  其中，像素坐标转相机坐标，代码如下：

/** * @brief 将像素坐标转换为相机坐标系下的3D点 * @param pixel 输入的像素坐标 (u, v) * @param cameraMatrix 相机内参矩阵 (3x3, CV_64F) * @param distCoeffs 相机畸变系数 (1xN, CV_64F) * @param depth Z 轴深度值 (在相机坐标系下)，单位：mm * @param camCoord 输出：相机坐标系下的3D点 (X, Y, Z)，单位：mm * @return bool 是否转换成功 */bool pixelToCameraCoord(const Point2f& pixel, const Mat& cameraMatrix, const Mat& distCoeffs, double depth, Point3d& camCoord) { if (cameraMatrix.empty() || cameraMatrix.rows != 3 || cameraMatrix.cols != 3 || cameraMatrix.type() != CV_64F) { myLog.error(\"pixelToCameraCoord: 无效的相机内参矩阵\"); return false; } if (depth <= 0) { myLog.error(\"pixelToCameraCoord: 深度值必须为正\"); return false; } vector<Point2f> srcPoints = { pixel }; vector<Point2f> dstPoints; // 1. 去畸变: 使用内参矩阵作为新的相机矩阵P，得到去畸变后的像素坐标 try { undistortPoints(srcPoints, dstPoints, cameraMatrix, distCoeffs, noArray(), cameraMatrix); } catch (const cv::Exception& e) { myLog.error(\"pixelToCameraCoord: undistortPoints 发生异常: \" + string(e.what())); return false; } if (dstPoints.empty()) { myLog.error(\"pixelToCameraCoord: 去畸变后点为空\"); return false; } Point2f undistortedPixel = dstPoints[0]; // 2. 提取内参 double fx = cameraMatrix.at<double>(0, 0); double fy = cameraMatrix.at<double>(1, 1); double cx = cameraMatrix.at<double>(0, 2); double cy = cameraMatrix.at<double>(1, 2); if (fx == 0 || fy == 0) { myLog.error(\"pixelToCameraCoord: 相机内参 fx 或 fy 为零\"); return false; } // 3. 像素坐标 -> 归一化图像坐标 double x_normalized = (undistortedPixel.x - cx) / fx; double y_normalized = (undistortedPixel.y - cy) / fy; // 4. 归一化图像坐标 -> 相机坐标 (单位: mm) camCoord.x = x_normalized * depth; camCoord.y = y_normalized * depth; camCoord.z = depth; // Z 轴即为深度 return true;}

  通过上述代码，即可得到机械手需要到达的XYZ坐标；

4、二维码角度纠正机械手末端旋转

  有时，不仅需要XYZ坐标，还需要有角度补偿，比如：需要将矩形工件插入凹槽内，如何计算角度补偿呢？直接看代码；

// Step 6: 检测二维码Mat rectifiedImage;vector <Point2f> bboxPoint;QRCodeDetector qrDecoder = QRCodeDetector::QRCodeDetector();std::string data = qrDecoder.detectAndDecode(sharpFinal, bboxPoint, rectifiedImage);//获取二维码中的数据if (bboxPoint.size() > 0){ // 二维码中心坐标 RotatedRect qrBox = minAreaRect(bboxPoint); Point2f centerqr = qrBox.center; result.x = centerqr.x; result.y = centerqr.y; CalibrateHandEye Calibrate; string FileName = Calibrate.calibDatasHandEyePath + \"/calibDatas/\" + fileNameChar; // 从标定文件读取数据 Mat loade_cameraMatrix, loade_distCoeffs, loade_Rcam2gripper, loade_tcam2gripper; if (Calibrate.loadCalibrationResult(FileName, loade_cameraMatrix, loade_distCoeffs, loade_Rcam2gripper, loade_tcam2gripper)) { // 构建二维码四角在世界坐标中的位置，原点在左上角 vector<Point3f> objectPoints = { Point3f(0, 0, 0), Point3f(qrSize, 0, 0), Point3f(qrSize, qrSize, 0), Point3f(0, qrSize, 0) }; Mat rvec, tvec; bool success = solvePnP(objectPoints, bboxPoint, loade_cameraMatrix, loade_distCoeffs, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE); if (success) { result.z = tvec.at<double>(2); // 相机与二维码的距离 } else { return; } // 计算二维码的旋转角度用于纠正插入25-06-23 double rz_correction = computeQRCodeTiltZCorrection(rvec, loade_Rcam2gripper); rzAngle = rz_correction; myLoclog.info(\"二维码在末端坐标系下角度为 Rz: \" + to_string(rzAngle)); } else { return; }}

  其中，角度计算函数computeQRCodeTiltZCorrection为：

/// @brief 计算二维码绕 Z 轴偏离垂直姿态的角度（单位：度）/// @param rvec solvePnP 输出的旋转向量/// @param R_cam2ee 相机到末端的旋转矩阵（手眼标定结果）/// @return 末端坐标系下二维码绕 Z 轴的偏差角度（正为逆时针，负为顺时针）double computeQRCodeTiltZCorrection(const Mat& rvec, const Mat& R_cam2ee){ // 1. 将旋转向量转为旋转矩阵（二维码相对于相机） Mat R_tag2cam; Rodrigues(rvec, R_tag2cam); // 3x3 // 2. 得到二维码在末端坐标系下的姿态 Mat R_tag2ee = R_cam2ee * R_tag2cam; // 3. 转换为欧拉角（单位：度） Vec3d euler_rad = rotationMatrixToEulerXYZ(R_tag2ee); Vec3d euler_deg = euler_rad * (180.0 / CV_PI); double rawAngle = euler_deg[2]; // 绕 Z 轴 // 4. 归一化到 [-180, 180] if (rawAngle > 180.0) rawAngle -= 360.0; if (rawAngle < -180.0) rawAngle += 360.0; // 5. 比较最接近的垂直方向（±90°） double nearestVertical = (abs(rawAngle - 90.0) < abs(rawAngle + 90.0)) ? 90.0 : -90.0; // 6. 计算偏差角 double diffAngle = rawAngle - nearestVertical; return diffAngle;}

四、经验总结

  1、做手眼标定拍摄图片数量采用26张，一般采用20~32张；
  2、前面提到项目需求，六轴机械臂需要将矩形块插入墙面凹槽内，标定板放至与地面平行和标定板与墙面平行都可以。（地面做标定进行墙面抓取，会与墙面标定板做标定抓取有7mm的误差，对精度要求不高的小伙伴可以任意选择）。所以，只进行墙面抓取时，标定板放置于墙面水平精度会高一些。
  3、通过二维码可以计算二维码至相机光心之间的距离，这个距离会存在毫米级别的波动，可用于参考。如果，用于像素坐标转为机械手坐标时，此距离会造成7mm级别的误差波动（拍照位动态拍照波动误差）。
  4、欢迎大家一起交流，谢谢~