手眼标定概述原理常用方法汇总与 C++ 代码实战

Ⅱ. 相机固定，拍一张标定板照片

Ⅲ. 通过圆查找相关算法，依次识别出标定板中的 9 个圆点的像素坐标 $(x_{img_i}, y_{img_i})$

Ⅳ. 机械臂保持同一水平面，依次去触碰标定板的圆心，并依次记录这 9 个机械臂位姿 $(x_{robot_i}, y_{robot_i})$

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());

举例：

int w = 3; int h = 5;
// 读取图像
cv::Mat image = cv::imread("beyondyanyu.jpg");
// 设置 SimpleBlobDetector 参数
cv::SimpleBlobDetector::Params params;
params.maxArea = std::numeric_limits<float>::max();
params.minArea = 2;
params.minDistBetweenBlobs = 1;
// 创建 SimpleBlobDetector 对象
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_ASYMMETRIC_GRID, blobDetector);

Ⅱ. 斑点检测器参数设置——cv::SimpleBlobDetector

findCirclesGrid函数的核心在于斑点检测器参数的设置，也就是cv::SimpleBlobDetector::Params params;。

举例：

// 设置 SimpleBlobDetector 参数
cv::SimpleBlobDetector::Params params;
params.maxArea = std::numeric_limits<float>::max();
params.minArea = 2;
params.minDistBetweenBlobs = 1;
params.filterByColor = true;
params.blobColor = 255;

Ⅲ. 需要改动的地方

1. 你的图片宽高实际多少个圆点

int w = 3; int h = 5;

2. 你的圆点标定板的路径

cv::Mat image = cv::imread("35_1.jpg");

3. 你的圆点标定板是否是对称的

对称：cv::CALIB_CB_SYMMETRIC_GRID 非对称：cv::CALIB_CB_ASYMMETRIC_GRID

bool found = cv::findCirclesGrid(gray, cv::Size(w, h), corners, cv::CALIB_CB_SYMMETRIC_GRID, blobDetector);

4. 你的机械臂依次指向对应的圆点坐标时机械臂的位置，因为我这边标定板是 3*5 的对称圆点标定板，故需要得到机械臂的 15 个位置

// 机器人坐标系中的点的坐标
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}};

③ 完整代码

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

// 定义一个结构体来存储点的坐标
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;
    }
    cv::Mat transformMatrix = cv::estimateAffine2D(src, dst);
    return transformMatrix;
}

// 将图像坐标点通过仿射变换矩阵转换为机器人坐标点
// 机械臂坐标 = 仿射变换矩阵 * 图像坐标
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("35_1.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);
    // 设置 SimpleBlobDetector 参数
    cv::SimpleBlobDetector::Params params;
    params.maxArea = std::numeric_limits<float>::max();
    params.minArea = 2;
    params.minDistBetweenBlobs = 1;
    // 创建 SimpleBlobDetector 对象
    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 (int i = 0; i < corners.size(); i++) {
            std::cout << "corner: " << corners[i] << std::endl;
            imagePoints.at(i).x = corners[i].x;
            imagePoints.at(i).y = corners[i].y;
        }
        std::cout << corners.size() << std::endl;
        // 优化角点位置
        cv::cornerSubPix(gray, corners, cv::Size(w, h), cv::Size(-1, -1), criteria);
        // 绘制角点
        cv::drawChessboardCorners(image, cv::Size(w, h), cv::Mat(corners), found);
        cv::namedWindow("findCorners", cv::WINDOW_NORMAL);
        cv::imshow("findCorners", image);
        cv::waitKey(0);
        cv::destroyAllWindows();
    } else {
        std::cerr << "Could not find the circle grid" << std::endl;
    }
    for (auto point : imagePoints) {
        std::cout << "x: " << point.x << " y: " << point.y << std::endl;
    }
    // 机器人坐标系中的点的坐标
    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;
    // 测试一个图像坐标点的变换
    std::cout << "Testing a testImagePoint transformation to robot point..." << std::endl;
    Point2D testImagePoint1 = {590.203, 885.33};
    Point2D testRobotPoint1 = transformPoint(affineMatrix, testImagePoint1);
    std::cout << "Image Point1 (" << testImagePoint1.x << ", " << testImagePoint1.y << ") transforms to Robot2 Point (" << testRobotPoint1.x << ", " << testRobotPoint1.y << ")" << std::endl;
    /* //变换矩阵 affineMatrix 会存在不可逆情况
    Point2D testRobotPoint2 = { 10, 10 };
    Point2D testImagePoint2 = transformPoint(affineMatrix.inv(), testRobotPoint2);
    std::cout << "Robot Point2 (" << testRobotPoint2.x << ", " << testRobotPoint2.y << ") transforms to Image2 Point (" << testImagePoint2.x << ", " << testImagePoint2.y << ")" << std::endl;
    */
    return 0;
}

④ 运行效果

得到仿射变换矩阵 R 为 R = $$ \begin{bmatrix} 0.02821525305435079 & -5.038386495271226e{-08} & -1.652816243402001 \ 3.693734340049677e{-08} & 0.02822203196237374 & 0.01414373627149304 \ \end{bmatrix} $$

图像中的点 (590.203, 885.33)，也就是最后一个点，对应我们输入的机械臂的最后一个位置，可以映射到机械臂位置为 (15, 25)，可以看到最终实际的求解效果为 (14.9999, 25)。

$$ \begin{cases} Point_{robot} = R \cdot Point_{img} \ Point_{img} = R^{-1} \cdot Point_{robot} \end{cases} $$

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

2D 相机手眼标定常用九点标定法，其只能得到 X 和 Y 的位置关系，无法获取机械臂姿态信息，毕竟 2D 相机和 3D 相机的成本有很大差距。

2D 相机手眼标定只需要拍照一张照片 + 机械臂的 9 个点位即可，但只能得到 X 和 Y 信息，Z 轴信息是固定的。 而 3D 相机的手眼标定可以求解机械臂的姿态，但需要多组拍摄标定板进行组成 AX=XB 多组方程求解。

OpenCV 提供了求解方法，核心函数是solvePnP和cv::calibrateHandEye。

① 操作流程

Ⅰ. 准备工作

1. 标定板——使用棋盘格（如 9x6 内角点），尺寸需已知（单位：米或毫米）
2. 相机内参——提前完成相机标定，获取 camera_matrix 和 dist_coeffs

Ⅱ. 数据采集

1. 固定标定板——将标定板固定在工作区域内，确保机械臂运动时相机始终可观测到它
2. 移动机械臂——控制机械臂移动至不同位姿（需包含旋转和平移），每个位姿下：
   • 记录机械臂末端的 XYZWPR（基座坐标系下）
   • 拍摄标定板图像。
3. 数据量——至少 10 组数据，位姿需多样化（建议 20 组以上）

Ⅲ. 数据预处理

1. 机械臂位姿转换——将 XYZWPR 转换为旋转矩阵R_base2gripper和平移向量t_base2gripper
2. 标定板检测——对每张图像检测棋盘格角点，使用solvePnP计算相机到标定板的位姿R_target2cam和t_target2cam

Ⅳ. 眼标定计算

调用 OpenCV 的calibrateHandEye函数，输入机械臂和相机的位姿数据，求解变换矩阵X

② 具体实现

Ⅰ. 准备标定板参数，这里使用 opencv 自带的棋盘格

路径为opencv\sources\samples\data下

Ⅱ. 获取相机的内参以及畸变系数

求解得到相机的内参矩阵camera_matrix和透镜畸变系数dist_coeffs

Ⅲ. 采集机械臂和相机的数据

1. 收集机械臂姿态数据（xyzwpr） 机械臂夹取标定板在相机视野下运动 9 个位姿，相机拍摄 9 组数据，当然数据越多，切姿态越丰富标定效果越好。 机械臂的位姿通过示教器读取并转换为矩阵形式。 例如 fanuc 示教器上的读数是：xyzwpr，w 表示 Rx，p 表示 Ry，r 表示 Rz。 Fanuc 机器人示教器上的 XYZWPR 是基于固定轴的位姿表示，其中： X, Y, Z：末端执行器在基座坐标系中的平移分量（单位：毫米或米）。 W, P, R：分别表示绕 X、Y、Z 轴 的旋转角度（单位：度），即 Roll-Pitch-Yaw 顺序（但按 Z-Y-X 轴顺序 组合旋转） 假设我们已经移动了 9 组位置并进行记录机械臂位置存放到std::vector<std::vector<double>> xyzwpr_data中。 函数convertXYZWPRToMat化将 xyzwpr 转化为 RT 矩阵

2. 收集相机拍摄机械臂所夹持的棋盘格图像 为例方便演示，这里我采用的是 opencv 自带的棋盘格图像数据。 路径为opencv\sources\samples\data下

Ⅳ. 检测棋盘格位姿

涉及的方法都是一样的： findChessboardCorners找标定板的角点 cornerSubPix亚像素优化角点 solvePnP计算相机到标定板的位姿

Ⅴ. 手眼标定

手眼标定核心函数calibrateHandEye，输入机械臂和相机的位姿数据，输出旋转X_rot矩阵和平移X_trans向量，然后通过copyTo拼接成变换矩阵（4x4）即可。 calibrateHandEye 参数： R_base2gripper, t_base2gripper：机械臂末端在基座坐标系下的位姿。 R_target2cam,t_target2cam：标定板在相机坐标系下的位姿。 X_rot, X_trans：输出的相机到基座的旋转矩阵和平移向量。 flags：标定算法选择（推荐 CALIB_HAND_EYE_TSAI）

③ 完整代码

需要修改的地方：

1. 实际你使用的棋盘格单个尺寸和内角点数量

const float square_size = 0.025f; // 棋盘格单格尺寸 (m)
const cv::Size board_size(9, 6); // 棋盘格内角点数量

2. 相机内参和畸变系数

cv::Mat camera_matrix, dist_coeffs;

3. 多组从示教器读取的机械臂位姿

std::vector<std::vector<double>> xyzwpr_data

4. 机械臂运动时，相机所拍摄的标定板图片路径

std::string image_path = "D:/opencv_4.7/opencv/sources/samples/data/right0" + std::to_string(i+1) + ".jpg";

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>
#include <vector>
#include <cmath>
#include <Windows.h>
#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
    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;
    // 绕 Z 轴的旋转矩阵
    cv::Mat Rz = (cv::Mat_<double>(3, 3) << cos(W), -sin(W), 0, sin(W), cos(W), 0, 0, 0, 1);
    // 绕 Y 轴的旋转矩阵
    cv::Mat Ry = (cv::Mat_<double>(3, 3) << cos(P), 0, sin(P), 0, 1, 0, -sin(P), 0, cos(P));
    // 绕 X 轴的旋转矩阵
    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; // 棋盘格单格尺寸 (m)
    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.3547430629291, 0, 328.3241889680722, 0, 541.614996071113, 246.9472922233208, 0, 0, 1);
    dist_coeffs = (cv::Mat_<double>(5, 1) << -0.2805430825289494, 0.1043237314547243, -0.0005582136242986065, 0.001303557702627711, -0.02372163114377487);
    // --------------- 3. 采集数据 ---------------
    // 示例数据：每组数据是一个包含 6 个元素的 std::vector<double>
    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) {
        // ------------------- 3.1 获取机械臂位姿 -------------------
        // 假设从 Fanuc 机器人读取基座到末端的变换矩阵
        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].at(0), xyzwpr_data[i].at(1), xyzwpr_data[i].at(2), xyzwpr_data[i].at(3), xyzwpr_data[i].at(4), xyzwpr_data[i].at(5), R_base2grip, t_base2grip);
        R_base2gripper.push_back(R_base2grip.clone());
        t_base2gripper.push_back(t_base2grip.clone());
        // ------------------- 3.2 检测棋盘格位姿 -------------------
        // 假设从相机捕获图像
        std::string image_path = "D:/opencv_4.7/opencv/sources/samples/data/right0" + std::to_string(i + 1) + ".jpg";
        cv::Mat image = cv::imread(image_path, -1);
        std::cout << image.size() << std::endl;
        std::vector<cv::Point2f> corners;
        bool found = cv::findChessboardCorners(image, board_size, corners);
        if (found) {
            // 亚像素优化角点
            cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1));
            // 计算相机到标定板的位姿
            cv::Mat rvec, tvec;
            cv::solvePnP(object_points, corners, 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 << "相机到基座的变换矩阵 X = \n" << X << std::endl;
    return 0;
}

④ 运行效果

得到相机到基座的变换矩阵X，也就是AX = XB中的解