线激光相机眼在手上六轴机器人手眼标定备忘记录

原创已于 2025-09-26 18:52:03 修改 · 1.7k 阅读

29 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

于 2025-09-17 18:53:34 首次发布

该文章已生成可运行项目，

线激光相机眼在手上六轴机器人手眼标定

之前一直有用线激光做跟踪和寻位焊接，但是手眼标定一直是使用公司的库做的，自己根据理解和查的资料写一下，有点问题，还需要右面再看看，先记录一下吧
说明：公司那套软甲是拿一个具有明显直角边上面绘制一个点，使用机器人不同位姿，线激光穿过那个点，记录相机坐标和机器人位姿，大于6个点拟合的
相机坐标是直线相交交点，或者标定球拟合球心也行，方式不一样
核心原理：
P(标定点机器人坐标) = T(工具->基座) x T(相机->工具，待求矩阵) x P（相机点）
由于P(标定点机器人坐标) 是固定的，所以可以列出多个方程
T(点1：工具->基座) x T(相机->工具，待求矩阵) x P（点1：相机点） = T(点2：工具->基座) x T(相机->工具，待求矩阵) x P（点2：相机点）
但是这个和传统的手眼标定（AX=XB）还不一样，获取的直接是点坐标，不是转换矩阵，所以就难住了，不太会解方程组，所以想来想去，还是把P(标定点机器人坐标) 当成已知量来用吧

原理部分：

可以直接借鉴【线激光手眼标定】这篇文章吧，这篇文章和该硕士论文【面向焊缝跟踪的线激光检测技术研究】 原理一致，都可以参考，把相机坐标点、机器人位姿和标定点在机器人下的坐标当成已知量做，后面会解方程了，再把（标定点在机器人下的坐标）这种方式写一下

原理简介：

1.这个就是基础的方程，和我上面那个一样
在这里插入图片描述
2.这个方程拆解出来如下：
左侧T(BF)为机器人位姿的旋转矩阵，这个组合的时候需要看三轴的旋转顺序，如果是ABB这种是四元素表示的，需要对应变换到旋转矩阵
XL 和 ZL 为线激光某个点的坐标，是在线激光平面，所以只有两维，Z表示深度，X表示线激光走向方向，Y方向默认0或者其他常数
右侧xyz 为基座坐标系下标记点的坐标，可以直接用机器人末端指点获取
在这里插入图片描述
3.机器人POS矩阵左乘逆矩阵，变换如下

4.上面等号右侧为已知量，直接用XYZ代替

5.把4公式拆出来

6.再变换成XA=B格式，此处公式显示的是3个点对，如果是10个点，就是3x3 x 3x10 = 3x10

7.这个方程模仿AX=B，也可以做如下变换
XA = B
XAA(T) = BA(T) 此时AA(T)是方阵有逆矩阵
X = BA(T)(AA(T))(-1) 按这个求解
8. 求出来X了，即代表求出来了r11 r21 r31 r13 r23 r33 x y z但是还缺r12 r22 r23三个变量，由于旋转矩阵是两两正交的，所以可以如下求出来，但是要注意该向量的朝向，如果我r11 r21 r31 是X旋转，r13 r23 r33是Z旋转，那我的Y就是Z叉乘X，下面公式右侧要对调一下，叉乘前看是否要对向量归一化，看需求吧
在这里插入图片描述
9.就算求完了，把旋转矩阵组合出来就行

实践一下：

我找了三组数据，各有缺陷，也不知道算不算能验证，先将就看吧
第一组数据
数据是XYZ旋转模式下的，但是我的数据只有相机坐标系点和位姿，没有实际标定物的基座坐标，我直接用了公司标定计算出来的那个位置当已知量（我记得这个和实际机器人指出来还是有误差得）用了

	//机器人位姿
	std::vector<Pos>BasePos;
	BasePos.resize(8);
	BasePos[0].x = 1092.010; BasePos[0].y = 126.6960; BasePos[0].z = 117.653; BasePos[0].u = 36.310; BasePos[0].v = -1.360; BasePos[0].w = -90.776;
	BasePos[1].x = 1146.710; BasePos[1].y = 102.007; BasePos[1].z = 151.027; BasePos[1].u = 20.003; BasePos[1].v = 23.049; BasePos[1].w = -100.17;
	BasePos[2].x = 1097.360; BasePos[2].y = 78.745; BasePos[2].z = 188.125; BasePos[2].u = 10.537; BasePos[2].v = 37.359; BasePos[2].w = -105.62;
	BasePos[3].x = 1097.310; BasePos[3].y = 26.030; BasePos[3].z = 212.116; BasePos[3].u = 13.159; BasePos[3].v = 53.515; BasePos[3].w = -121.90;
	BasePos[4].x = 1097.170; BasePos[4].y = 127.129; BasePos[4].z = 61.216; BasePos[4].u = 24.756; BasePos[4].v = -33.425; BasePos[4].w = -82.095;
	BasePos[5].x = 1096.990; BasePos[5].y = 106.164; BasePos[5].z = 23.957; BasePos[5].u = 31.359; BasePos[5].v = -53.551; BasePos[5].w = -77.848;
	BasePos[6].x = 1062.870; BasePos[6].y = 110.995; BasePos[6].z = 48.866; BasePos[6].u = 5.284; BasePos[6].v = -55.024; BasePos[6].w = -48.495;
	BasePos[7].x = 1071.050; BasePos[7].y = 111.934; BasePos[7].z = 24.186; BasePos[7].u = 22.083; BasePos[7].v = -53.473; BasePos[7].w = -84.305;
	//相机坐标
	std::vector<Eigen::Vector3d>src;
	src.push_back(Eigen::Vector3d(5.207, 3.058, 212.184));
	src.push_back(Eigen::Vector3d(-14.651, 3.058, 187.370));
	src.push_back(Eigen::Vector3d(33.843, 3.058, 223.949));
	src.push_back(Eigen::Vector3d(-3.927, 3.058, 257.037));
	src.push_back(Eigen::Vector3d(4.244, 3.058, 195.328));
	src.push_back(Eigen::Vector3d(-1.143, 3.058, 189.084));
	src.push_back(Eigen::Vector3d(-6.016, 3.058, 218.697));
	src.push_back(Eigen::Vector3d(37.936, 3.058, 200.879));
	//世界坐标
	Eigen::Vector4d Points(1156.62, 36.5753, 73.0081, 1.0);

这组数据，之前标定结果如下，我直接把最下边的坐标当成已知量用了，人家这个相比于上面这个少了三个已知量，更简单一点，后面再研究研究吧:
在这里插入图片描述
用这组数据算法来的转换矩阵，反正是有误差的，Z方向误差有4mm，我们就理解是世界坐标这个已知量有问题吧

第二组数据
第二组数据用的是上面链接博客【线激光手眼标定】里的数据

这个是四元素形式的，变成转换矩阵可以参考我另一篇博客【ABB机器人四元素处理】
计算下来，我理解和他这个应该是一样的，但是不知道他第二列第三列反了一下还有有个轴取反了是啥意思，看不太懂
在这里插入图片描述
第三组数据
这个数据是上面论文里的数据，算出来也和他稍微不一样，不知，道问题出在哪里，唉，我确定我没抄错，将就着看吧，反正算出来看着是不一样，差点意思，中间这个向量反向了，都行吧，不太影响
在这里插入图片描述

代码贴上来吧，如果大家发现了什么问题，或者哪里有问题，自己修改了别忘记给我留个评论说一下，让我也同步一下，有参考意义的话，可以留下点赞和收藏

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>
#include <vector>
#include <cmath>
#include <vector>
#include <unsupported/Eigen/KroneckerProduct>
#define M_PI 3.141592653
using namespace Eigen;
struct Pos
{
	double x;
	double y;
	double z;
	double u;
	double v;
	double w;
	double q;
	Pos() {}
	Pos(double x_, double y_, double z_, double u_, double v_, double w_) {
		x = x_;
		y = y_;
		z = z_;
		u = u_;
		v = v_;
		w = w_;
	}
};
Eigen::Matrix4d PosToMatrix4d(Pos pos) {
	//构建三个单独的旋转矩阵
	Eigen::Matrix3d Matrix_rx;
	Eigen::Matrix3d Matrix_ry;
	Eigen::Matrix3d Matrix_rz;
	Matrix_rx << 1, 0, 0,
		0, cos(pos.u*M_PI / 180), -1.0*sin(pos.u*M_PI / 180),
		0, sin(pos.u*M_PI / 180), cos(pos.u*M_PI / 180);
	Matrix_ry << cos(pos.v*M_PI / 180), 0, sin(pos.v*M_PI / 180),
		0, 1, 0,
		-1.0*sin(pos.v*M_PI / 180), 0, cos(pos.v*M_PI / 180);
	Matrix_rz << cos(pos.w*M_PI / 180), -1.0*sin(pos.w*M_PI / 180), 0,
		sin(pos.w*M_PI / 180), cos(pos.w*M_PI / 180), 0,
		0, 0, 1;
	//构建绕XYZ旋转顺序的旋转矩阵，从右到左
	Eigen::Matrix3d rxyz = Matrix_rz*Matrix_ry*Matrix_rx;
	//构建4*4的旋转矩阵
	Eigen::Matrix4d xyzdelt;
	xyzdelt << rxyz(0, 0), rxyz(0, 1), rxyz(0, 2), pos.x,
		rxyz(1, 0), rxyz(1, 1), rxyz(1, 2), pos.y,
		rxyz(2, 0), rxyz(2, 1), rxyz(2, 2), pos.z,
		0, 0, 0, 1;
	return xyzdelt;
}
Eigen::Matrix4d ABBPosToMatrix4d(Pos pos) {

	double w = pos.u;
	double x = pos.v;
	double y = pos.w;
	double z = pos.q;
	//构建旋转矩阵
	double a00 = 1.0 - 2.0 * y * y - 2.0 * z * z;
	double a01 = 2.0 * x * y - 2.0 * w * z;
	double a02 = 2.0 * x * z + 2.0 * w * y;
	double a10 = 2.0 * x * y + 2.0 * w * z;
	double a11 = 1.0 - 2.0 * x * x - 2.0 * z * z;
	double a12 = 2.0 * y * z - 2.0 * w * x;
	double a20 = 2.0 * x * z - 2.0 * w * y;
	double a21 = 2.0 * y * z + 2.0 * w * x;
	double a22 = 1.0 - 2.0 * x * x - 2.0 * y * y;
	Eigen::Matrix4d xyzdelt;
	xyzdelt << a00, a01, a02, pos.x,
		a10, a11, a12, pos.y,
		a20, a21, a22, pos.z,
		0, 0, 0, 1;
	return xyzdelt;
}
int main() {
	//机器人位姿
	//std::vector<Pos>BasePos;
	//BasePos.resize(8);
	//BasePos[0].x = 1092.010; BasePos[0].y = 126.6960; BasePos[0].z = 117.653; BasePos[0].u = 36.310; BasePos[0].v = -1.360; BasePos[0].w = -90.776;
	//BasePos[1].x = 1146.710; BasePos[1].y = 102.007; BasePos[1].z = 151.027; BasePos[1].u = 20.003; BasePos[1].v = 23.049; BasePos[1].w = -100.17;
	//BasePos[2].x = 1097.360; BasePos[2].y = 78.745; BasePos[2].z = 188.125; BasePos[2].u = 10.537; BasePos[2].v = 37.359; BasePos[2].w = -105.62;
	//BasePos[3].x = 1097.310; BasePos[3].y = 26.030; BasePos[3].z = 212.116; BasePos[3].u = 13.159; BasePos[3].v = 53.515; BasePos[3].w = -121.90;
	//BasePos[4].x = 1097.170; BasePos[4].y = 127.129; BasePos[4].z = 61.216; BasePos[4].u = 24.756; BasePos[4].v = -33.425; BasePos[4].w = -82.095;
	//BasePos[5].x = 1096.990; BasePos[5].y = 106.164; BasePos[5].z = 23.957; BasePos[5].u = 31.359; BasePos[5].v = -53.551; BasePos[5].w = -77.848;
	//BasePos[6].x = 1062.870; BasePos[6].y = 110.995; BasePos[6].z = 48.866; BasePos[6].u = 5.284; BasePos[6].v = -55.024; BasePos[6].w = -48.495;
	//BasePos[7].x = 1071.050; BasePos[7].y = 111.934; BasePos[7].z = 24.186; BasePos[7].u = 22.083; BasePos[7].v = -53.473; BasePos[7].w = -84.305;
	////相机坐标
	//std::vector<Eigen::Vector3d>src;
	//src.push_back(Eigen::Vector3d(5.207, 3.058, 212.184));
	//src.push_back(Eigen::Vector3d(-14.651, 3.058, 187.370));
	//src.push_back(Eigen::Vector3d(33.843, 3.058, 223.949));
	//src.push_back(Eigen::Vector3d(-3.927, 3.058, 257.037));
	//src.push_back(Eigen::Vector3d(4.244, 3.058, 195.328));
	//src.push_back(Eigen::Vector3d(-1.143, 3.058, 189.084));
	//src.push_back(Eigen::Vector3d(-6.016, 3.058, 218.697));
	//src.push_back(Eigen::Vector3d(37.936, 3.058, 200.879));
	////世界坐标
	//Eigen::Vector4d Points(1156.62, 36.5753, 73.0081, 1.0);

	std::vector<Pos>BasePos;
	BasePos.resize(10);
	BasePos[0].x = 611.927; BasePos[0].y = -337.748; BasePos[0].z = 444.929; BasePos[0].u = -179.9914; BasePos[0].v = -29.6960; BasePos[0].w = -0.0045;
	BasePos[1].x = 612.985; BasePos[1].y = -318.608; BasePos[1].z = 450.754; BasePos[1].u = -179.9907; BasePos[1].v = -29.6987; BasePos[1].w = -0.0065;
	BasePos[2].x = 607.345; BasePos[2].y = -380.231; BasePos[2].z = 444.163; BasePos[2].u = -174.5669; BasePos[2].v = -29.8622; BasePos[2].w = -6.3606;
	BasePos[3].x = 516.088; BasePos[3].y = -229.873; BasePos[3].z = 393.906; BasePos[3].u = 174.6992;  BasePos[3].v = -46.6999; BasePos[3].w = -16.7722;
	BasePos[4].x = 811.703; BasePos[4].y = -280.223; BasePos[4].z = 473.294; BasePos[4].u = 173.4753;  BasePos[4].v = -0.7399;  BasePos[4].w = 20.8908;
	BasePos[5].x = 681.667; BasePos[5].y = -594.331; BasePos[5].z = 364.829; BasePos[5].u = -145.8659; BasePos[5].v = 10.2829;  BasePos[5].w = -42.5905;
	BasePos[6].x = 903.080; BasePos[6].y = -147.626; BasePos[6].z = 400.438; BasePos[6].u = 168.824;   BasePos[6].v = 24.2405;  BasePos[6].w = 32.2090;
	BasePos[7].x = 530.374; BasePos[7].y = -144.912; BasePos[7].z = 338.787; BasePos[7].u = 134.6953;  BasePos[7].v = -16.2830; BasePos[7].w = 46.1090;
	BasePos[8].x = 423.499; BasePos[8].y = -305.380; BasePos[8].z = 316.866; BasePos[8].u = -133.0162; BasePos[8].v = -32.3433; BasePos[8].w = -77.3643;
	BasePos[9].x = 428.198; BasePos[9].y = -175.619; BasePos[9].z = 272.064; BasePos[9].u = 142.1562;  BasePos[9].v = -58.9630; BasePos[9].w = 20.7953;

	std::vector<Eigen::Vector3d>src;
	src.push_back(Eigen::Vector3d(-1.2943, 0, 135.1660));
	src.push_back(Eigen::Vector3d(18.8565, 0, 141.0535));
	src.push_back(Eigen::Vector3d(-22.1264, 0, 139.9686));
	src.push_back(Eigen::Vector3d(-20.0656, 0, 156.3136));
	src.push_back(Eigen::Vector3d(-5.4819, 0, 127.0132));
	src.push_back(Eigen::Vector3d(-24.2554, 0, 118.1987));
	src.push_back(Eigen::Vector3d(18.0071, 0, 99.6658));
	src.push_back(Eigen::Vector3d(9.9690, 0, 125.6090));
	src.push_back(Eigen::Vector3d(-18.7726, 0, 157.2780));
	src.push_back(Eigen::Vector3d(-2.5979, 0, 151.5664));
	//世界坐标
	Eigen::Vector4d Points(792.691, -330.187, 29.959, 1.0);



	//std::vector<Pos>BasePos;
	//BasePos.resize(4);
	//BasePos[0].x = 1431.49; BasePos[0].y = 253.30; BasePos[0].z = 676.29; BasePos[0].u = 0.528101; BasePos[0].v = -0.391551; BasePos[0].w = 0.574547;	 BasePos[0].q = -0.487538;
	//BasePos[1].x = 1444.86; BasePos[1].y = 213.27; BasePos[1].z = 723.87; BasePos[1].u = 0.580717; BasePos[1].v = -0.308164; BasePos[1].w = 0.495077;	 BasePos[1].q = -0.568068;
	//BasePos[2].x = 1396.88; BasePos[2].y = 271.10; BasePos[2].z = 624.55; BasePos[2].u = 0.445193; BasePos[2].v = -0.483734; BasePos[2].w = 0.655849;	 BasePos[2].q = -0.371036;
	//BasePos[3].x = 1424.70; BasePos[3].y = 216.22; BasePos[3].z = 802.07; BasePos[3].u = 0.632536; BasePos[3].v = -0.236723; BasePos[3].w = 0.653778;	 BasePos[3].q = -0.341225;

	//std::vector<Eigen::Vector3d>src;
	//src.push_back(Eigen::Vector3d(46.575, 0, -19.748));
	//src.push_back(Eigen::Vector3d(43.3, 0, 6.772));
	//src.push_back(Eigen::Vector3d(51.675, 0, -19.808));
	//src.push_back(Eigen::Vector3d(56.025, 0, -5.94));
	////世界坐标
	//Eigen::Vector4d Points(1216.45, 66.48, 689.80, 1.0);



	int n = BasePos.size(); // 位姿数量
	std::vector<Eigen::Matrix4d>B;
	B.resize(n);
	for (int i = 0; i < n; i++) 
	{	
		//正常XYZ旋转的机器人
		B[i] = PosToMatrix4d(BasePos[i]);
		//ABB机器人
		//B[i] = ABBPosToMatrix4d(BasePos[i]);
	}


	Eigen::Matrix3d TT;

	MatrixXd AAA, BBB;
	AAA.resize(3, n);
	BBB.resize(3, n);
	for (int i = 0; i < n; i++) {
		AAA(0, i) = src[i][0];
		AAA(1, i) = src[i][2];
		AAA(2, i) = 1;

		Eigen::Vector4d XYZ = B[i].inverse()*Points;

		BBB(0, i) = XYZ[0];
		BBB(1, i) = XYZ[1];
		BBB(2, i) = XYZ[2];
	}
	TT = BBB * AAA.transpose()*(AAA * AAA.transpose()).inverse();


	Eigen::Vector3d r1(TT(0, 0), TT(1, 0), TT(2, 0));
	Eigen::Vector3d r3(TT(0, 1), TT(1, 1), TT(2, 1));

	r1.normalize();
	r3.normalize();
	Eigen::Vector3d r2 = r3.cross(r1);

	std::cout << "r1: " << r1 << std::endl;
	std::cout << "r2: " << r2 << std::endl;
	std::cout << "r3: " << r3 << std::endl;

	Eigen::Matrix4d RT;
	RT << r1[0], r2[0], r3[0], TT(0,2),
		r1[1], r2[1], r3[1], TT(1, 2),
		r1[2], r2[2], r3[2], TT(2, 2),
		0, 0, 0, 1;


	std::cout << "RT :\n" << RT << std::endl;





	return 0;
}