机械手眼标定实验步骤

1、数据采集

将标定板放置在机械臂工作区域下，控制机械臂从不同位置和角度采集标定板的图像；
每次数据采集从机械臂控制程序中读取机械臂基于基坐标下的空间位姿，并同该位姿下的图像对应保存；

2、Matlab相机标定

使用Malab相机标定APP程序自动标定相机，然后从保存到工作变量空间中的标定参数获取相机外参；

3、统一位姿数据格式

位姿数据格式：X,Y,Z.Rx,Ry,Rz或者直接以T(4*4)保存
相机外参和机械臂位姿分别保存到camerapose.txt、pose.txt中
文章给出部分数据用作演示
camerapose.txt中数据：

-284.5458163	199.5328555	561.7994622	0.02543238	-0.032339235	-1.46773162
-428.7450521	207.53699	561.0147856	0.028880285	-0.035330731	-1.466652101
-437.985438	24.09775162	561.9402875	0.032031532	-0.030350479	-1.467212499
-360.1936111	-49.65152803	562.2985865	0.031830793	-0.02587328	-1.465911774
-130.7310566	-14.55617974	562.9750081	0.020735301	-0.014674864	-1.465181478
-54.00656037	69.04763743	562.360561	0.028131168	-0.014680305	-1.467106572

pose.txt中数据

1044.6	-276.87	448.32	-169.76	-0.03	179.99
904.78	-312.89	448.29	-169.75	-0.03	179.99
859.57	-134.79	448.12	-169.75	-0.05	179.99
922.05	-47.76	448.03	-169.76	-0.05	179.98
1154.89	-37.83	447.99	-169.76	-0.06	179.98
1246.43	-104.77	447.92	-169.76	-0.07	179.98

4、标定程序

这里改写https://blog.csdn.net/weixin_42203839/article/details/103882739文章提供的手眼标定代码

main.cpp

#include"test.h"

int main()
{
	// 文件名  
	string posefile = "pose.txt";
	string cameraposefile = "camerapose.txt";

	// 存储所有行的vector  
	vector<vector<double>> camerapose = ObtainData(cameraposefile);
	vector<vector<double>>pose = ObtainData(posefile);

	//定义手眼标定矩阵
	std::vector<Mat> R_gripper2base;
	std::vector<Mat> t_gripper2base;
	std::vector<Mat> R_target2cam;
	std::vector<Mat> t_target2cam;
	Mat R_cam2gripper = (Mat_<double>(3, 3));
	Mat t_cam2gripper = (Mat_<double>(3, 1));

	vector<Mat> images;
	size_t num_images = 28;//13组手眼标定数据

	// 读取末端，标定板的姿态矩阵 4*4
	std::vector<cv::Mat> vecHg, vecHc;
	cv::Mat Hcg;//定义相机camera到末端grab的位姿矩阵
	Mat tempR, tempT;

	for (size_t i = 0; i < num_images; i++)//计算标定板位姿
	{
		cv::Mat tmp = attitudeVectorToMatrix(camerapose[i], "xyz"); //转移向量转旋转矩阵
		vecHc.push_back(tmp);
		RT2R_T(tmp, tempR, tempT);

		R_target2cam.push_back(tempR);
		t_target2cam.push_back(tempT);
	}

	for (size_t i = 0; i < num_images; i++)//计算机械臂位姿
	{
		cv::Mat tmp = attitudeVectorToMatrix(pose[i], "xyz"); //机械臂位姿为欧拉角-旋转矩阵
		vecHg.push_back(tmp);
		RT2R_T(tmp, tempR, tempT);

		R_gripper2base.push_back(tempR);
		t_gripper2base.push_back(tempT);
	}
	//手眼标定，CALIB_HAND_EYE_TSAI法为11ms，最快
	calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper, t_cam2gripper, CALIB_HAND_EYE_TSAI);

	Hcg = R_T2RT(R_cam2gripper, t_cam2gripper);//矩阵合并

	std::cout << "Hcg 矩阵为： " << std::endl;
	std::cout << Hcg << std::endl;
	cout << "是否为旋转矩阵：" << isRotationMatrix(Hcg) << std::endl << std::endl;//判断是否为旋转矩阵

	//Tool_In_Base*Hcg*Cal_In_Cam，用第一组和第二组进行对比验证
	cout << "第一组和第二组手眼数据验证：" << endl;
	cout << vecHg[0] * Hcg * vecHc[0] << endl << vecHg[1] * Hcg * vecHc[1] << endl << endl;//.inv()

	cout << "标定板在相机中的位姿：" << endl;
	cout << vecHc[1] << endl;
	cout << "手眼系统反演的位姿为：" << endl;
	//用手眼系统预测第一组数据中标定板相对相机的位姿，是否与vecHc[1]相同
	cout << Hcg.inv() * vecHg[1].inv() * vecHg[0] * Hcg * vecHc[0] << endl << endl;

	cout << "----手眼系统测试----" << endl;
	cout << "机械臂下标定板XYZ为：" << endl;
	for (int i = 0; i < vecHc.size(); ++i)
	{
		cv::Mat cheesePos{ 0.0,0.0,0.0,1.0 };//4*1矩阵，单独求机械臂下，标定板的xyz
		cv::Mat worldPos = vecHg[i] * Hcg * vecHc[i] * cheesePos;
		cout << i << ": " << worldPos.t() << endl;
	}
	getchar();

	return 0;
}

test.h

#pragma once
#include <algorithm>
#include <iostream>
#include <vector>  
#include <fstream>  
#include <sstream>  
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include<Eigen/Dense>

using namespace std;
using namespace cv;

// 函数来将字符串分割为double类型的vector  
vector<double> splitToDoubles(const string& line);

vector<vector<double>> ObtainData(string filename);

Mat attitudeVectorToMatrix(vector<double> pose, const string& seq);

Mat quaternionToRotatedMatrix(const cv::Vec4d& q);

Mat eulerAngleToRotatedMatrix(const Mat& eulerAngle, const string& seq);

bool isRotationMatrix(const Mat& R);

Mat R_T2RT(Mat& R, Mat& T);

//RT转R和T矩阵
void RT2R_T(Mat& RT, Mat& R, Mat& T);

test.cpp

#include"test.h"

vector<double> splitToDoubles(const string& line)
{
	vector<double> result;
	istringstream iss(line);
	double value;
	while (iss >> value) 
	{
		result.push_back(value);
		// 如果遇到空格以外的分隔符（如制表符），可以选择忽略它  
		// 在这里，我们假设数据由空格或制表符分隔，不需要额外处理  
	}
	return result;
}

vector<vector<double>> ObtainData(string filename)
{
	vector<vector<double>> data;
	ifstream file(filename);

	string line;
	// 逐行读取文件内容  
	while (getline(file, line))
	{
		// 去除行尾的换行符  
		if (!line.empty() && line.back() == '\n')
		{
			line.pop_back();
		}
		// 将每行数据分割为double并添加到allData中  
		data.push_back(splitToDoubles(line));
	}

	// 关闭文件  
	file.close();
	return data;
}

Mat attitudeVectorToMatrix(vector<double> pose, const string& seq)
{
	// 创建一个4x4的变换矩阵T  
	cv::Mat_<double> T_matrix = cv::Mat::zeros(4, 4, CV_64FC1); // 初始化一个4x4的零矩阵  

	// 一个1x3的旋转向量R和一个1x3的平移向量t  
	cv::Mat_<double> t_vector = (cv::Mat_<double>(1, 3) << pose[0], pose[1], pose[2]);
	cv::Mat_<double> R_vector = (cv::Mat_<double>(1, 3) << pose[3], pose[4], pose[5]);

	cv::Mat R_matrix = cv::Mat::eye(3, 3, CV_64FC1);
	if (0 == seq.compare(""))
		cv::Rodrigues(R_vector, R_matrix);
	else
		eulerAngleToRotatedMatrix(R_vector, seq).copyTo(R_matrix({ 0, 0, 3, 3 }));

	// 将R的元素复制到T的左上角3x3部分  
	R_matrix.copyTo(T_matrix({ 0,0,3,3 }));
	for (int i = 0; i < 3; i++)
	{
		T_matrix(i, 3) = t_vector(0, i);
	}

	// 将T的最后一行的最后一个元素设为1  
	T_matrix(3, 3) = 1;
	
	return T_matrix;
}

Mat quaternionToRotatedMatrix(const cv::Vec4d& q)
{
	double w = q[0], x = q[1], y = q[2], z = q[3];

	double x2 = x * x, y2 = y * y, z2 = z * z;
	double xy = x * y, xz = x * z, yz = y * z;
	double wx = w * x, wy = w * y, wz = w * z;

	cv::Matx33d res{
		1 - 2 * (y2 + z2),	2 * (xy - wz),		2 * (xz + wy),
		2 * (xy + wz),		1 - 2 * (x2 + z2),	2 * (yz - wx),
		2 * (xz - wy),		2 * (yz + wx),		1 - 2 * (x2 + y2),
	};
	return cv::Mat(res);
}

Mat eulerAngleToRotatedMatrix(const Mat& eulerAngle, const string& seq)
{
	CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);

	eulerAngle /= 180 / CV_PI;
	cv::Matx13d m(eulerAngle);
	auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
	auto xs = std::sin(rx), xc = std::cos(rx);
	auto ys = std::sin(ry), yc = std::cos(ry);
	auto zs = std::sin(rz), zc = std::cos(rz);

	cv::Mat rotX = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, xc, -xs, 0, xs, xc);
	cv::Mat rotY = (cv::Mat_<double>(3, 3) << yc, 0, ys, 0, 1, 0, -ys, 0, yc);
	cv::Mat rotZ = (cv::Mat_<double>(3, 3) << zc, -zs, 0, zs, zc, 0, 0, 0, 1);

	cv::Mat rotMat;

	if (seq == "zyx")		
		rotMat = rotX * rotY * rotZ;
	else if (seq == "yzx")	
		rotMat = rotX * rotZ * rotY;
	else if (seq == "zxy")	
		rotMat = rotY * rotX * rotZ;
	else if (seq == "xzy")	
		rotMat = rotY * rotZ * rotX;
	else if (seq == "yxz")
		rotMat = rotZ * rotX * rotY;
	else if (seq == "xyz")	
		rotMat = rotZ * rotY * rotX;
	else 
	{
		cv::error(cv::Error::StsAssert, "Euler angle sequence string is wrong.",
			__FUNCTION__, __FILE__, __LINE__);
	}

	if (!isRotationMatrix(rotMat)) 
	{
		cv::error(cv::Error::StsAssert, "Euler angle can not convert to rotated matrix",
			__FUNCTION__, __FILE__, __LINE__);
	}

	return rotMat;
	//cout << isRotationMatrix(rotMat) << endl;
}

bool isRotationMatrix(const Mat& R)
{
	cv::Mat tmp33 = R({ 0,0,3,3 });
	cv::Mat shouldBeIdentity;

	shouldBeIdentity = tmp33.t() * tmp33;

	cv::Mat I = cv::Mat::eye(3, 3, shouldBeIdentity.type());

	return  cv::norm(I, shouldBeIdentity) < 1e-6;
}

Mat R_T2RT(Mat& R, Mat& T)
{
	Mat RT;
	Mat_<double> R1 = (cv::Mat_<double>(4, 3) << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
		R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
		R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
		0.0, 0.0, 0.0);
	cv::Mat_<double> T1 = (cv::Mat_<double>(4, 1) << T.at<double>(0, 0), T.at<double>(1, 0), T.at<double>(2, 0), 1.0);

	cv::hconcat(R1, T1, RT);//C=A+B左右拼接
	return RT;
}

void RT2R_T(Mat& RT, Mat& R, Mat& T)
{
	cv::Rect R_rect(0, 0, 3, 3);
	cv::Rect T_rect(3, 0, 1, 3);
	R = RT(R_rect);
	T = RT(T_rect);
}