眼在手上外参标定保姆级教学（vscode + opencv）

需要准备好的东西

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

数据采集

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

步骤：

启动控制脚本：编写或使用现成脚本（e.g., Python），连接机器人和相机。脚本应包括循环：移动 → 查询位姿 → 拍照 → 保存。

移动机械臂到新位姿：使用控制器界面或脚本命令移动臂（包含旋转和平移）。确保臂稳定（等待 0.5-1 秒，避免振动）。

查询末端位姿：在臂到位后，立即调用 API 获取当前位姿（XYZWPR 或直接 4x4 矩阵）。这确保位姿与拍照时刻一致。

触发相机拍照：紧接着位姿查询，捕获图像。延迟应 < 100ms（大多数系统支持）。

保存数据：将位姿和图像配对保存（e.g., image_001.jpg 与 pose_001.csv）。

重复：移动到下一个位姿，采集 20 组。确保位姿多样：改变 X/Y/Z 平移 > 50mm，旋转 > 30° 围绕各轴。

数据预处理

机械臂位姿转换: 将 XYZWPR 转换为旋转矩阵R_base2gripper和平移向量t_base2gripper

代码示例如下：

python 复制代码

import numpy as np
from scipy.spatial.transform import Rotation as Rot

def xyzwpr_to_rt(x, y, z, w, p, r, degrees=True):
    # 输入: XYZWPR (WPR 默认 degrees=True)
    t = np.array([x, y, z])  # 平移向量
    
    eul = [w, p, r]  # 欧拉角顺序 ZYX
    rot = Rot.from_euler('zyx', eul, degrees=degrees)
    R = rot.as_matrix()  # 旋转矩阵
    return R, t

# 示例调用
R, t = xyzwpr_to_rt(100, 200, 300, 90, 0, 0)
print("旋转矩阵 R:")
print(R)
print("平移向量 t:")
print(t)

标定板检测: 对每张图像检测棋盘格角点，使用 solvePnP计算相机到标定板的位姿R_target2cam和t_target2cam。

眼标定计算

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

②具体实现

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

路径为opencv\sources\samples\data下

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

具体代码之前写的博文有，可参考：（一）相机标定------四大坐标系的介绍、对应转换、畸变原理以及OpenCV完整代码实战（C++版）

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

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

①收集机械臂姿态数据（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矩阵

②收集相机拍摄机械臂所夹持的棋盘格图像

为例方便演示，这里我采用的是opencv自带的棋盘格图像数据

路径为opencv\sources\samples\data下

Ⅳ.检测棋盘格位姿

之前第一篇博文以及介绍，这里就不再赘述，涉及的方法都是一样的

（一）相机标定------四大坐标系的介绍、对应转换、畸变原理以及OpenCV完整代码实战（C++版）

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）

③完整代码

需要修改的地方：

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

const float square_size = 0.025f; // 棋盘格单格尺寸(m)

const cv::Size board_size(9, 6); // 棋盘格内角点数量

Ⅱ.相机内参和畸变系数

cv::Mat camera_matrix, dist_coeffs;

可以参考博文：（一）相机标定------四大坐标系的介绍、对应转换、畸变原理以及OpenCV完整代码实战（C++版）

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

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

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

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

}