手眼标定：原理、方法及 C++ 代码实战

*注意：9 点标定要求机械臂在同一水平面（Z 值固定），无法直接获取姿态信息。

② 具体实现

找圆点函数 findCirclesGrid

OpenCV 提供 findCirclesGrid 函数检测圆点。

CV_EXPORTS_W bool findCirclesGrid(
    InputArray image, 
    Size patternSize, 
    OutputArray centers,
    int flags = CALIB_CB_SYMMETRIC_GRID,
    const Ptr<FeatureDetector>& blobDetector = SimpleBlobDetector::create()
);

参数说明：

• image: 输入图像 (cv::Mat)。
• patternSize: 图像宽高方向的圆点数量 (cv::Size(w, h))。
• centers: 输出检测到的圆心坐标 (std::vector<cv::Point2f>)。
• flags: 标定板类型（对称 CALIB_CB_SYMMETRIC_GRID 或非对称 CALIB_CB_ASYMMETRIC_GRID）。
• blobDetector: 斑点检测器对象。

示例代码：

int w = 3; int h = 5;
cv::Mat image = cv::imread("calibration_board.jpg");
cv::SimpleBlobDetector::Params params;
params.maxArea = std::numeric_limits<float>::max();
params.minArea = 2;
params.minDistBetweenBlobs = 1;
cv::Ptr<cv::SimpleBlobDetector> blobDetector = cv::SimpleBlobDetector::create(params);
std::vector<cv::Point2f> corners;
bool found = cv::findCirclesGrid(image, cv::Size(w, h), corners, cv::CALIB_CB_ASYMMETRIC_GRID, blobDetector);

斑点检测器参数设置

核心在于 cv::SimpleBlobDetector::Params 的配置，关键参数包括：

• thresholdStep, minThreshold, maxThreshold: 二值化阈值范围。
• minRepeatability: 特征点最小重复次数。
• filterByColor, filterByArea, filterByCircularity: 过滤条件（颜色、面积、圆度）。

③ 完整代码

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>

struct Point2D {
    float x;
    float y;
};

// 计算仿射变换矩阵
cv::Mat calculateAffineTransform(const std::vector<Point2D>& imagePoints, 
                                 const std::vector<Point2D>& robotPoints, 
                                 int points_num) {
    cv::Mat src(points_num, 2, CV_64F);
    cv::Mat dst(points_num, 2, CV_64F);
    for (int i = 0; i < points_num; ++i) {
        src.at<double>(i, 0) = imagePoints[i].x;
        src.at<double>(i, 1) = imagePoints[i].y;
        dst.at<double>(i, 0) = robotPoints[i].x;
        dst.at<double>(i, 1) = robotPoints[i].y;
    }
    return cv::estimateAffine2D(src, dst);
}

// 将图像坐标转换为机器人坐标
Point2D transformPoint(const cv::Mat& transformMatrix, const Point2D& imagePoint) {
    cv::Mat src(3, 1, CV_64F);
    src.at<double>(0, 0) = imagePoint.x;
    src.at<double>(1, 0) = imagePoint.y;
    src.at<double>(2, 0) = 1.0;
    cv::Mat dst = transformMatrix * src;
    Point2D robotPoint;
    robotPoint.x = dst.at<double>(0, 0);
    robotPoint.y = dst.at<double>(1, 0);
    return robotPoint;
}

int main() {
    int w = 3; int h = 5;
    cv::TermCriteria criteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001);
    std::vector<Point2D> imagePoints;

    // 读取图像
    cv::Mat image = cv::imread("calibration_board.jpg");
    if (image.empty()) {
        std::cerr << "Could not open or find the image" << std::endl;
        return -1;
    }

    cv::Mat gray;
    cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

    // 设置斑点检测器
    cv::SimpleBlobDetector::Params params;
    params.maxArea = std::numeric_limits<float>::max();
    params.minArea = 2;
    params.minDistBetweenBlobs = 1;
    cv::Ptr<cv::SimpleBlobDetector> blobDetector = cv::SimpleBlobDetector::create(params);

    std::vector<cv::Point2f> corners;
    bool found = cv::findCirclesGrid(gray, cv::Size(w, h), corners, cv::CALIB_CB_SYMMETRIC_GRID, blobDetector);

    if (found) {
        imagePoints.resize(corners.size());
        for (size_t i = 0; i < corners.size(); ++i) {
            imagePoints[i].x = corners[i].x;
            imagePoints[i].y = corners[i].y;
        }
        // 亚像素优化
        cv::cornerSubPix(gray, corners, cv::Size(5, 5), cv::Size(-1, -1), criteria);
    } else {
        std::cerr << "Could not find the circle grid" << std::endl;
        return -1;
    }

    // 机器人坐标系中的点坐标（示例数据）
    std::vector<Point2D> robotPoints = {
        {5, 5}, {10, 5}, {15, 5}, {5, 10}, {10, 10}, {15, 10},
        {5, 15}, {10, 15}, {15, 15}, {5, 20}, {10, 20}, {15, 20},
        {5, 25}, {10, 25}, {15, 25}
    };

    // 计算仿射变换矩阵
    cv::Mat affineMatrix = calculateAffineTransform(imagePoints, robotPoints, w * h);
    std::cout << "Affine Transformation Matrix:" << std::endl << affineMatrix << std::endl;

    // 测试变换
    Point2D testImagePoint = {590.203f, 885.33f};
    Point2D testRobotPoint = transformPoint(affineMatrix, testImagePoint);
    std::cout << "Image Point: (" << testImagePoint.x << ", " << testImagePoint.y << ") -> Robot Point: (" 
              << testRobotPoint.x << ", " << testRobotPoint.y << ")" << std::endl;

    return 0;
}

运行效果将输出仿射变换矩阵及转换后的坐标。

2. 3D 相机手眼标定——OpenCV

2D 标定仅能获取 XY 位置，3D 标定可求解机械臂姿态（XYZWPR）。需要多组拍摄数据构建 AX=XB 方程组。

OpenCV 提供核心函数 solvePnP 和 cv::calibrateHandEye。

① 操作流程

1. 准备工作：准备已知尺寸的棋盘格标定板，提前完成相机内参标定。
2. 数据采集：
   • 固定标定板，移动机械臂至不同位姿（至少 10 组，建议 20 组以上）。
   • 每个位姿下记录机械臂末端位姿（XYZWPR）并拍摄图像。
3. 数据预处理：
   • 将 XYZWPR 转换为旋转矩阵和平移向量。
   • 使用 solvePnP 计算相机到标定板的位姿。
4. 手眼标定计算：调用 calibrateHandEye 求解变换矩阵。

② 具体实现

准备标定板参数

使用 OpenCV 自带棋盘格数据，定义单格尺寸和内角点数量。

获取相机内参

需预先完成相机标定，获取 camera_matrix 和 dist_coeffs。

采集机械臂和相机数据

• 收集机械臂姿态数据（xyzwpr），转换为 RT 矩阵。
• 收集相机拍摄的标定板图像，检测角点并计算位姿。

手眼标定核心

调用 cv::calibrateHandEye，输入机械臂位姿和相机位姿，输出旋转和平移向量。

③ 完整代码

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>
#include <vector>
#include <cmath>
#include <iostream>

// 将 Fanuc 风格的 XYZWPR 转换为旋转矩阵 R 和平移向量 t
void convertXYZWPRToMat(double X, double Y, double Z, double W, double P, double R, 
                        cv::Mat& R_base2grip, cv::Mat& t_base2grip) {
    t_base2grip = (cv::Mat_<double>(3, 1) << X, Y, Z);
    W *= CV_PI / 180.0; P *= CV_PI / 180.0; R *= CV_PI / 180.0;
    
    cv::Mat Rz = (cv::Mat_<double>(3, 3) << cos(W), -sin(W), 0, sin(W), cos(W), 0, 0, 0, 1);
    cv::Mat Ry = (cv::Mat_<double>(3, 3) << cos(P), 0, sin(P), 0, 1, 0, -sin(P), 0, cos(P));
    cv::Mat Rx = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, cos(R), -sin(R), 0, sin(R), cos(R));
    
    R_base2grip = Rz * Ry * Rx;
}

int main() {
    // 1. 准备标定板参数
    const float square_size = 0.025f;
    const cv::Size board_size(9, 6);
    std::vector<cv::Point3f> object_points;
    for (int i = 0; i < board_size.height; ++i) {
        for (int j = 0; j < board_size.width; ++j) {
            object_points.emplace_back(j * square_size, i * square_size, 0);
        }
    }

    // 2. 读取相机内参和畸变系数
    cv::Mat camera_matrix, dist_coeffs;
    camera_matrix = (cv::Mat_<double>(3, 3) << 542.35, 0, 328.32, 0, 541.61, 246.95, 0, 0, 1);
    dist_coeffs = (cv::Mat_<double>(5, 1) << -0.28, 0.10, -0.0005, 0.0013, -0.0237);

    // 3. 采集数据
    std::vector<std::vector<double>> xyzwpr_data = {
        {100.0, 200.0, 300.0, 45.0, 30.0, 15.0},
        {150.0, 250.0, 350.0, 60.0, 45.0, 30.0},
        {200.0, 300.0, 400.0, 75.0, 60.0, 45.0},
        {250.0, 350.0, 450.0, 90.0, 75.0, 60.0},
        {300.0, 400.0, 500.0, 105.0, 90.0, 75.0},
        {350.0, 450.0, 550.0, 120.0, 105.0, 90.0},
        {400.0, 500.0, 600.0, 135.0, 120.0, 105.0},
        {450.0, 550.0, 650.0, 150.0, 135.0, 120.0},
        {500.0, 600.0, 700.0, 165.0, 150.0, 135.0}
    };

    std::vector<cv::Mat> R_base2gripper, t_base2gripper;
    std::vector<cv::Mat> R_target2cam, t_target2cam;

    for (int i = 0; i < 9; ++i) {
        // 获取机械臂位姿
        cv::Mat R_base2grip = cv::Mat::eye(3, 3, CV_64F);
        cv::Mat t_base2grip = (cv::Mat_<double>(3, 1) << 0.1 * i, 0, 0);
        convertXYZWPRToMat(xyzwpr_data[i][0], xyzwpr_data[i][1], xyzwpr_data[i][2], 
                           xyzwpr_data[i][3], xyzwpr_data[i][4], xyzwpr_data[i][5], 
                           R_base2grip, t_base2grip);
        R_base2gripper.push_back(R_base2grip.clone());
        t_base2gripper.push_back(t_base2grip.clone());

        // 模拟检测棋盘格位姿（实际需从图像读取）
        // 此处省略图像读取逻辑，直接使用 solvePnP 模拟结果
        cv::Mat rvec, tvec;
        cv::solvePnP(object_points, std::vector<cv::Point2f>(), camera_matrix, dist_coeffs, rvec, tvec);
        cv::Mat R_target2cam_i;
        cv::Rodrigues(rvec, R_target2cam_i);
        R_target2cam.push_back(R_target2cam_i);
        t_target2cam.push_back(tvec);
    }

    // 4. 手眼标定
    cv::Mat X_rot, X_trans;
    cv::calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, 
                         X_rot, X_trans, cv::CALIB_HAND_EYE_TSAI);

    // 5. 输出结果
    cv::Mat X = cv::Mat::eye(4, 4, CV_64F);
    X_rot.copyTo(X(cv::Rect(0, 0, 3, 3)));
    X_trans.copyTo(X(cv::Rect(3, 0, 1, 3)));
    std::cout << "Camera to Base Transform Matrix X =\n" << X << std::endl;

    return 0;
}

运行后将输出相机到基座的变换矩阵 X，即 AX=XB 的解。