C++AI算法
手眼标定概述原理常用方法汇总与 C++ 代码实战
本文介绍手眼标定的基本概念、坐标系及分类（眼在手内/外）。详细阐述了 2D 九点标定法和 3D 相机标定法的原理与流程，重点提供了基于 OpenCV 的 C++ 代码实现，涵盖圆点检测、仿射变换矩阵计算及 solvePnP 求解过程，帮助开发者实现机械臂与相机的坐标转换。
本文介绍手眼标定的基本概念、坐标系及分类（眼在手内/外）。详细阐述了 2D 九点标定法和 3D 相机标定法的原理与流程，重点提供了基于 OpenCV 的 C++ 代码实现，涵盖圆点检测、仿射变换矩阵计算及 solvePnP 求解过程，帮助开发者实现机械臂与相机的坐标转换。
手眼标定的目的：让机械臂和相机关联，相机充当机械臂的'眼睛'，最终实现指哪打哪。
相机的使用前提首先需要进行相机标定。当通过相机内参和畸变纠正之后再进行后续的处理。
| 坐标系 | 描述 |
|---|---|
| base | 机械臂的基坐标系 |
| cam | 相机坐标系 |
| end | 机械臂的法兰坐标系，也称为 tcl |
| obj | 物体坐标系，一般情况下法兰会有夹爪夹取物体，end 和 obj 是相对静止状态 |
手眼标定分为眼在手上和眼在手外两种。
以眼在手外为例进行分析。
首先，列举出有用的已知条件：
Ⅰ. base下的end是已知的，可以通过示教器读取，$T^{base}{end}$
Ⅱ. end和obj是一个整体，相对静止关系，是个恒等式可作为桥梁，$T^{end}{obj}$
Ⅲ. 相机可以拍摄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} $$
$$ (T^{end_2}{base_2})^{-1} \cdot T^{end_1}{base_1} \cdot T^{base_1}{cam_1} = T^{base_2}{cam_2} \cdot T^{cam_2}{obj_2} \cdot T^{obj_1}{cam_1} $$
$$ A X = X B $$ 其中 A 和 B 是已知的，目标是求解 X。所有的手眼标定都是为了求解 AX=XB，只不过求解的方法和思路不同，比较出名的是 Tsai-Lenz 算法。
同理眼在手上是将 $T^{obj}_{base}$ 作为中间桥梁，因为 base和obj是相对静止关系。
因为相机分为两类，2D 相机和 3D 相机，故手眼标定方法又可以进行细分 2D 和 3D 类别。
2D 相机进行手眼标定的本质是真实的世界坐标系到相机的像素坐标系的映射，本质就是仿射变换。比如：一张图片上的某个像素坐标通过变换矩阵进行映射到实际的世界坐标系下，前期是不考虑 Z 轴，机械臂的运作均在同一个水平面。最常用的方法是九点标定法。
3D 相机拍摄的数据为点云，故可以通过ICP进行点云匹配求解得到变换矩阵。也可以通过 OpenCV 的solvePnP求解变换矩阵。
9 点标定必须要求机械臂所在同一水平面（Z 是固定值），无法得到机械臂的姿态
相机拍摄得到数据，通过 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 | 输入：检测圆点标定板的类型 | 对称圆点标定板cv::CALIB_CB_SYMMETRIC_GRID、非对称圆点标定板cv::CALIB_CB_ASYMMETRIC_GRID、还有另一个不常用 |
| blobDetector | 输入：斑点检测器，有很多参数可以调节 | cv::Ptr<cv::SimpleBlobDetector> |
举例：
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);
findCirclesGrid函数的核心在于斑点检测器参数的设置，也就是cv::SimpleBlobDetector::Params params;。
| 参数 | 描述 |
|---|---|
| thresholdStep | 二值化的阈值步长 |
| minThreshold | 二值化的最小阈值 |
| maxThreshold | 二值化的最大阈值 |
| minRepeatability | 重复的最小次数，属于斑点且斑点数量大于该值时才被认为是特征点 |
| minDistBetweenBlobs | 最小的斑点距离，不同斑点之间距离大于该值才被认为是不同斑点 |
| filterByColor | bool，需要设置为 true 才生效，通过斑点颜色进行限制 |
| blobColor | 只提取斑点的颜色，0 为黑色，255 为白色 |
| filterByArea | bool，需要设置为 true 才生效，通过斑点的面积进行限制 |
| minArea | 斑点最小面积 |
| maxArea | 斑点最大面积 |
| filterByCircularity | bool，需要设置为 true 才生效，通过斑点的圆度进行限制 |
| minCircularity | 斑点的最小圆度 |
| maxCircularity | 斑点的最大圆度 |
| filterByInertia | bool，需要设置为 true 才生效，通过斑点的惯性率进行限制 |
| minInertiaRatio | 斑点的最小惯性率 |
| maxInertiaRatio | 斑点的最大惯性率 |
| filterByConvexity | bool，需要设置为 true 才生效，通过斑点的凸度进行限制 |
| minConvexity | 斑点的最小凸度 |
| maxConvexity | 斑点的最大凸度 |
| collectContours | bool，需要设置为 true 才生效，是否收集斑点的轮廓 |
举例：
// 设置 SimpleBlobDetector 参数
cv::SimpleBlobDetector::Params params;
params.maxArea = std::numeric_limits<float>::max();
params.minArea = 2;
params.minDistBetweenBlobs = 1;
params.filterByColor = true;
params.blobColor = 255;
int w = 3; int h = 5;
cv::Mat image = cv::imread("35_1.jpg");
对称：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);
// 机器人坐标系中的点的坐标
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.<>(, ) = 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++) {
std::cout << << corners[i] << std::endl;
imagePoints.(i).x = corners[i].x;
imagePoints.(i).y = corners[i].y;
}
std::cout << corners.() << std::endl;
cv::(gray, corners, cv::(w, h), cv::(, ), criteria);
cv::(image, cv::(w, h), cv::(corners), found);
cv::(, cv::WINDOW_NORMAL);
cv::(, image);
cv::();
cv::();
} {
std::cerr << << std::endl;
}
( point : imagePoints) {
std::cout << << point.x << << point.y << std::endl;
}
std::vector<Point2D> robotPoints = {{,},{,},{,},{,},{,},{,},{,},{,},{,},{,},{,},{,},{,},{,},{,}};
cv::Mat affineMatrix = (imagePoints, robotPoints, w * h);
std::cout << << std::endl << affineMatrix << std::endl;
std::cout << << std::endl;
Point2D testImagePoint1 = {, };
Point2D testRobotPoint1 = (affineMatrix, testImagePoint1);
std::cout << << testImagePointx << << testImagePointy << << testRobotPointx << << testRobotPointy << << std::endl;
;
}
得到仿射变换矩阵 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} $$
2D 相机手眼标定常用九点标定法，其只能得到 X 和 Y 的位置关系，无法获取机械臂姿态信息，毕竟 2D 相机和 3D 相机的成本有很大差距。
2D 相机手眼标定只需要拍照一张照片 + 机械臂的 9 个点位即可，但只能得到 X 和 Y 信息，Z 轴信息是固定的。 而 3D 相机的手眼标定可以求解机械臂的姿态，但需要多组拍摄标定板进行组成 AX=XB 多组方程求解。
OpenCV 提供了求解方法，核心函数是solvePnP和cv::calibrateHandEye。
R_base2gripper和平移向量t_base2grippersolvePnP计算相机到标定板的位姿R_target2cam和t_target2cam调用 OpenCV 的calibrateHandEye函数，输入机械臂和相机的位姿数据，求解变换矩阵X
路径为opencv\sources\samples\data下
求解得到相机的内参矩阵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下
涉及的方法都是一样的：
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;
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
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, ) << (P), , (P), , , , -(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.());
std::string image_path = + std::(i + ) + ;
cv::Mat image = cv::(image_path, );
std::cout << image.() << std::endl;
std::vector<cv::Point2f> corners;
found = cv::(image, board_size, corners);
(found) {
cv::(image, corners, cv::(, ), cv::(, ), cv::(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, , ));
cv::Mat rvec, tvec;
cv::(object_points, corners, 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中的解
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML 转 Markdown 互为补充。 在线工具，Markdown 转 HTML在线工具，online