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

手眼标定：原理、方法及 C++ 代码实战 | 极客日志

一、手眼标定简述

手眼标定的目的是建立机械臂与相机之间的空间关联，使相机成为机械臂的'眼睛'，最终实现精准定位。

在使用相机前，首先需要进行相机标定（获取内参和畸变系数），完成后再进行后续的手眼标定处理。

1. 坐标系

坐标系	描述
base	机械臂基坐标系
cam	相机坐标系
end	机械臂法兰坐标系（Tool Center Point）
obj	物体坐标系（通常与法兰相对静止）

2. 分类

手眼标定主要分为两种模式：

眼在手上 (Eye-in-Hand)：相机安装在机械臂末端。
眼在手外 (Eye-to-Hand)：相机固定在工作区域外。

![图片]

3. 原理

以 眼在手外 为例进行分析。已知条件如下：

$T^{base}_{end}$：基座到末端的位姿，可通过示教器读取。
$T^{end}_{obj}$：末端到物体的位姿，两者相对静止，可视为恒等式桥梁。
$T^{cam}_{obj}$：相机到物体的位姿，通过图像识别获得。

目标是求解 $T^{base}_{cam}$（相机相对于基座的位姿）。为了形成闭合回路，各坐标系间需满足变换关系：

$$ T^{end_1}{base_1} \cdot T^{base_1}{cam_1} \cdot T^{cam_1}{obj_1} = T^{end_2}{base_2} \cdot T^{base_2}{cam_2} \cdot T^{cam_2}{obj_2} $$

整理后得到经典的 AX=XB 形式：

$$ A \cdot X = X \cdot B $$

其中：

$A = (T^{end_2}{base_2})^{-1} \cdot T^{end_1}{base_1}$ （机械臂运动矩阵）
$B = T^{cam_2}{obj_2} \cdot (T^{cam_1}{obj_1})^{-1}$ （相机观测变化矩阵）
$X = T^{base}_{cam}$ （待求的手眼标定矩阵）

所有手眼标定算法的核心均为求解该方程，常用算法包括 Tsai-Lenz 法等。

同理，眼在手上 是将 $T^{obj}_{base}$ 作为中间桥梁，因为基座和物体在特定配置下相对静止。

二、手眼标定方法汇总

根据相机类型，分为 2D 和 3D 两类。

1. 2D 相机手眼标定

本质是世界坐标系到像素坐标系的映射（仿射变换）。通常假设 Z 轴固定，机械臂在同一水平面运作。

最常用方法是九点标定法（或多点标定）。

2. 3D 相机手眼标定

3D 相机拍摄数据为点云或深度图，可通过 ICP 点云匹配或 OpenCV 的 solvePnP 求解变换矩阵。

三、代码实战

1. 2D 相机手眼标定——九点标定法

① 操作流程

制作含有圆点的标定板置于相机正下方（点数越多精度越高，示例使用 3x5=15 个点）。
相机固定，拍摄标定板照片。
使用圆查找算法识别标定板中圆点的像素坐标 $(x_{img}, y_{img})$。
机械臂保持同一水平面，依次触碰标定板圆心，记录机械臂位姿 $(x_{robot}, y_{robot})$。

*注意：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.<>(, ) = imagePoint.x;
    src.<>(, ) = imagePoint.y;
    src.<>(, ) = ;
    cv::Mat dst = transformMatrix * src;
    Point2D robotPoint;
    robotPoint.x = dst.<>(, );
    robotPoint.y = dst.<>(, );
     robotPoint;
}

{
     w = ;  h = ;
    ;
    std::vector<Point2D> imagePoints;

    
    cv::Mat image = cv::();
     (image.()) {
        std::cerr <<  << std::endl;
         ;
    }

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

    
    cv::SimpleBlobDetector::Params params;
    params.maxArea = std::numeric_limits<>::();
    params.minArea = ;
    params.minDistBetweenBlobs = ;
    cv::Ptr<cv::SimpleBlobDetector> blobDetector = cv::SimpleBlobDetector::(params);

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

     (found) {
        imagePoints.(corners.());
         ( i = ; i < corners.(); ++i) {
            imagePoints[i].x = corners[i].x;
            imagePoints[i].y = corners[i].y;
        }
        
        cv::(gray, corners, cv::(, ), cv::(, ), criteria);
    }  {
        std::cerr <<  << std::endl;
         ;
    }

    
    std::vector<Point2D> robotPoints = {
        {, }, {, }, {, }, {, }, {, }, {, },
        {, }, {, }, {, }, {, }, {, }, {, },
        {, }, {, }, {, }
    };

    
    cv::Mat affineMatrix = (imagePoints, robotPoints, w * h);
    std::cout <<  << std::endl << affineMatrix << std::endl;

    
    Point2D testImagePoint = {, };
    Point2D testRobotPoint = (affineMatrix, testImagePoint);
    std::cout <<  << testImagePoint.x <<  << testImagePoint.y <<  
              << testRobotPoint.x <<  << testRobotPoint.y <<  << std::endl;

     ;
}

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

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

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

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

① 操作流程

准备工作：准备已知尺寸的棋盘格标定板，提前完成相机内参标定。
数据采集：
- 固定标定板，移动机械臂至不同位姿（至少 10 组，建议 20 组以上）。
- 每个位姿下记录机械臂末端位姿（XYZWPR）并拍摄图像。
数据预处理：
- 将 XYZWPR 转换为旋转矩阵和平移向量。
- 使用 solvePnP 计算相机到标定板的位姿。
手眼标定计算：调用 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, -(P), , (P));
    cv::Mat Rx = (cv::<>(, ) << , , , , (R), -(R), , (R), (R));
    
    R_base2grip = Rz * Ry * Rx;
}

{
    
      square_size = ;
    ;
    std::vector<cv::Point3f> object_points;
     ( i = ; i < board_size.height; ++i) {
         ( j = ; j < board_size.width; ++j) {
            object_points.(j * square_size, i * square_size, );
        }
    }

    
    cv::Mat camera_matrix, dist_coeffs;
    camera_matrix = (cv::<>(, ) << , , , , , , , , );
    dist_coeffs = (cv::<>(, ) << , , , , );

    
    std::vector<std::vector<>> xyzwpr_data = {
        {, , , , , },
        {, , , , , },
        {, , , , , },
        {, , , , , },
        {, , , , , },
        {, , , , , },
        {, , , , , },
        {, , , , , },
        {, , , , , }
    };

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

     ( i = ; i < ; ++i) {
        
        cv::Mat R_base2grip = cv::Mat::(, , CV_64F);
        cv::Mat t_base2grip = (cv::<>(, ) <<  * i, , );
        (xyzwpr_data[i][], xyzwpr_data[i][], xyzwpr_data[i][], 
                           xyzwpr_data[i][], xyzwpr_data[i][], xyzwpr_data[i][], 
                           R_base2grip, t_base2grip);
        R_base2gripper.(R_base2grip.());
        t_base2gripper.(t_base2grip.());

        
        
        cv::Mat rvec, tvec;
        cv::(object_points, std::<cv::Point2f>(), camera_matrix, dist_coeffs, rvec, tvec);
        cv::Mat R_target2cam_i;
        cv::(rvec, R_target2cam_i);
        R_target2cam.(R_target2cam_i);
        t_target2cam.(tvec);
    }

    
    cv::Mat X_rot, X_trans;
    cv::(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, 
                         X_rot, X_trans, cv::CALIB_HAND_EYE_TSAI);

    
    cv::Mat X = cv::Mat::(, , CV_64F);
    X_rot.((cv::(, , , )));
    X_trans.((cv::(, , , )));
    std::cout <<  << X << std::endl;

     ;
}

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

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

一、手眼标定简述

1. 坐标系

2. 分类

3. 原理

二、手眼标定方法汇总

1. 2D 相机手眼标定

2. 3D 相机手眼标定

三、代码实战

1. 2D 相机手眼标定——九点标定法

① 操作流程

② 具体实现

找圆点函数 findCirclesGrid

斑点检测器参数设置

③ 完整代码

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

① 操作流程

② 具体实现

准备标定板参数

获取相机内参

采集机械臂和相机数据

手眼标定核心

③ 完整代码

更多推荐文章

相关免费在线工具

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

一、手眼标定简述

1. 坐标系

2. 分类

3. 原理

二、手眼标定方法汇总

1. 2D 相机手眼标定

2. 3D 相机手眼标定

三、代码实战

1. 2D 相机手眼标定——九点标定法

① 操作流程

② 具体实现

找圆点函数 findCirclesGrid

斑点检测器参数设置

③ 完整代码

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

① 操作流程

② 具体实现

准备标定板参数

获取相机内参

采集机械臂和相机数据

手眼标定核心

③ 完整代码

微信扫一扫，关注极客日志

更多推荐文章

相关免费在线工具