一、UR机器人位姿表示
默认情况下UR机器人的基坐标系和TCP如下图所示：
注意这里的TCP位置和坐标系都是在默认TCP配置的情况下，默认的TCP配置如下图所示：
如果用户想要自己设置TCP的位置和坐标系就可以在这个基础上进行配置，上图中的X,Y,Z,RX,RY,RZ表示用户自定义TCP与机器人默认TCP之间的关系（注意不是相对于基坐标系的关系）。X,Y,Z表示位置关系，RX,RY,RZ是使用的旋转矢量法表示的姿态关系，旋转矢量法见下一节。
当UR机器人回零后，机器人默认的TCP相对于基座标系的位姿如下图：
注意这里是实际值，与理论值相比有一些误差，比如理论值X=0，而实际可能不是0。这里的姿态描述方式使用的是旋转矢量法。
UR机器人的Pose表示的是TCP坐标系相对于基座标系的位置和姿态。一个坐标系相对于另一个坐标系的姿态的表示方法有很多种，比如：X-Y-Z固定角，Z-Y-X欧拉角，RPY角，四元数等。而UR机器人里面使用的是旋转矢量法。
在介绍旋转矢量法之前先看一下等效轴角坐标系表示法。在《机器人学导论》中对等效轴角法有详细讲解，如下图所示，这种方法是用一个单位向量
和一个转动角度θ来表示两个坐标系之间的旋转关系。具体介绍如下图所示：
而旋转矢量法用一个旋转矢量：
来表示两个坐标系之间的旋转关系，其中旋转矢量法与等效轴角坐标系之间的关系是：Rx=θkx，Ry=θky，Rz=θkz。
由旋转矢量求旋转矩阵的方法可以按照如下公式：
使用上述公式之前需要先求出θ和kx,ky,kz，求法如下：
可以看出如果θ=0°或180°时，旋转轴根本无法确定，上式也将无解。这也是旋转矢量法的一个明显缺点。
反过来，如果已知旋转矩阵，可以通过如下公式求解等效轴角表示法，进而求出旋转矢量表示法：
注意2-82中的r33应改为r23，这是书中的一处错误。
由以上叙述可以知道，对于同一姿态，旋转矩阵的表示不唯一。所以在UR机器人中，通过面板观察到的TCP Pose可能与通过脚本命令get_actual_tcp_pose()所得到的不一样，这是正常现象。如果不相信可以将它们都转换成RPY角就会发现，它们表示的是同一个姿态。
二、UR机器人手眼标定
获取相机坐标系和机器人TCP之间的旋转关系
相机坐标系下三维坐标
PW=T6TmPC
则变成：
Pw*(T6^-1)=Tm*Pc
获取5个相机坐标系下三维坐标点以及5个机器人坐标系下三维坐标点，
以及采集5个相机坐标系下三维坐标点时机器人的位姿（x,y,z,Rx,Ry,Rz）
由相机位姿获取T6矩阵：
MATLAB程序：
function [ Rt ] = URP2Rt( x, y, z, Rx, Ry, Rz )
theta = (Rx^2 + Ry^2 + Rz^2)^0.5;
kx = Rx/theta;
ky = Ry/theta;
kz = Rz/theta;
v = 1 - cos(theta);
s = sin(theta);
c = cos(theta);
R = [kx*kx*v + c, kx*ky*v - kz*s, kx*kz*v + ky*s;
kx*ky*v + kz*s, ky*ky*v + c, ky*kz*v - kx*s;
kx*kz*v - ky*s, ky*kz*v + kx*s, kz*kz*v + c;];
t = [x, y, z]';
Rt = [R t; 0 0 0 1];
end
C++程序：
Mat Cal_URT6(Mat T6, float Robot_postion[7])//计算T6矩阵
{
float Px, Py, Pz, rota, rotb, rotc;
Px = Robot_postion[1] * 1000;
Py = Robot_postion[2] * 1000;
Pz = Robot_postion[3] * 1000;
rota = Robot_postion[4];
rotb = Robot_postion[5];
rotc = Robot_postion[6];
float theta = sqrt(Robot_postion[4]*Robot_postion[4]+Robot_postion[5]*Robot_postion[5]+Robot_postion[6]*Robot_postion[6]);
float kx = rota/theta;
float ky = rotb/theta;
float kz = rotc/theta;
float v= 1-cos(theta);
float s = sin(theta);
float c = cos(theta);
float m1[3][3]={kx*kx*v+c,kx*ky*v-kz*s,kx*kz*v+ky*s,kx*ky*v + kz*s, ky*ky*v + c, ky*kz*v - kx*s,kx*kz*v - ky*s, ky*kz*v + kx*s, kz*kz*v + c};
Mat R1(3, 3, CV_32F, m1);
float m4[3][1] = { Px, Py, Pz };
Mat T1(3, 1, CV_32F, m4);
float m5[1][4] = { 0, 0, 0, 1 };
Mat T2(1, 4, CV_32F, m5);
Mat A;
hconcat(R1, T1, A);//A = [b c]
vconcat(A, T2, T6);//A = [b;c]
return T6;
}
进行手眼标定
void Collectpoint1()
{
Robot_postion[0] = 1 ;
Robot_postion[1] = -0.08126;
Robot_postion[2] = -0.40936;
Robot_postion[3] = -0.62562;
Robot_postion[4] = 5.067 ;
Robot_postion[5] = -0.962;
Robot_postion[6] = -3.564;
float m1[3][1] = {-6.46543, 7.98035, 190.538};
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 1; j++)
{
Ps1.at<float>(i, j) = m1[i][j];
}
}
cout << "Ps1=" << Ps1 << endl;
T61 = Cal_URT6(T6_robot, Robot_postion);
//cout << "T61=" << T61<< endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
Ts1[i][j] = T61.at<float>(i, j);
cout << Ts1[i][j] << ",";
}
cout << endl;
}
}
void Get_Tm()
{
cout << "Ts11=" << endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
cout << Ts1[i][j] << ",";
}
cout << endl;
}
cout << "Ts22=" << endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
cout << Ts2[i][j] << ",";
}
cout << endl;
}
cout << "Ts33=" << endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
cout << Ts3[i][j] << ",";
}
cout << endl;
}
cout << "Ts44=" << endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
cout << Ts4[i][j] << ",";
}
cout << endl;
}
cout << "Ts55=" << endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
cout << Ts5[i][j] << ",";
}
cout << endl;
}
float w1[1][4] = { -82.58,-401.29,-640.25,1 };
Pw1 = Mat(4, 1, CV_32F, w1);
cout << "Pw1=" << Pw1 << endl;
float w2[1][4] = { -82.58,-401.29,-640.25,1 };
Pw2 = Mat(4, 1, CV_32F, w2);
cout << "Pw2=" << Pw2 << endl;
float w3[1][4] = { -82.58,-401.29,-640.25,1 };
Pw3 =Mat(4, 1, CV_32F, w3);
cout << "Pw3=" << Pw3 << endl;
float w4[1][4] = { -82.58,-401.29,-640.25,1};
Pw4 = Mat(4, 1, CV_32F, w4);
cout << "Pw4=" << Pw4 << endl;
float w5[1][4] = { -82.58,-401.29,-640.25,1};
Pw5 =Mat(4, 1, CV_32F, w5);
cout << "Pw5=" << Pw5 << endl;
//Mat TV_61(4, 4, CV_32F);
//Mat TV_62(4, 4, CV_32F);
//Mat TV_63(4, 4, CV_32F);
//Mat TV_64(4, 4, CV_32F);
//Mat TV_65(4, 4, CV_32F);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
T61.at<float>(i, j) = Ts1[i][j];
T62.at<float>(i, j) = Ts2[i][j];
T63.at<float>(i, j) = Ts3[i][j];
T64.at<float>(i, j) = Ts4[i][j];
T65.at<float>(i, j) = Ts5[i][j];
}
}
T61 = Mat(4, 4, CV_32F, Ts1);
T62 = Mat(4, 4, CV_32F, Ts2);
T63 = Mat(4, 4, CV_32F, Ts3);
T64 = Mat(4, 4, CV_32F, Ts4);
T65 = Mat(4, 4, CV_32F, Ts5);
cout << "T61=" << T61 << endl;
cout << "T62=" << T62 << endl;
cout << "T63=" << T63 << endl;
cout << "T64=" << T64 << endl;
cout << "T65=" << T65 << endl;
invert(T61, TV_61, DECOMP_SVD);
cout << "TV_61=" << TV_61 << endl;
invert(T62, TV_62, DECOMP_SVD);
cout << "TV_62=" << TV_62 << endl;
invert(T63, TV_63, DECOMP_SVD);
cout << "TV_63=" << TV_63 << endl;
invert(T64, TV_64, DECOMP_SVD);
cout << "TV_64=" << TV_64 << endl;
invert(T65, TV_65, DECOMP_SVD);
cout << "TV_65=" << TV_65 << endl;
Pw11_1 = TV_61*Pw1;
cout << "Pw11_1=" << Pw11_1 << endl;
Pw22_2 = TV_62*Pw2;
cout << "Pw22_2=" << Pw22_2 << endl;
Pw33_3 = TV_63*Pw3;
cout << "Pw33_3=" << Pw33_3 << endl;
Pw44_4 = TV_64*Pw4;
cout << "Pw44_4=" << Pw44_4 << endl;
Pw55_5= TV_65*Pw5;
cout << "Pw55_4=" << Pw55_5 << endl;
Pw11_1(Rect(0, 0, 1, 3)).copyTo(Pw11);
//cout << "Pw11=" << Pw11 << endl;
Pw22_2(Rect(0, 0, 1, 3)).copyTo(Pw22);
//cout << "Pw22=" << Pw22 << endl;
Pw33_3(Rect(0, 0, 1, 3)).copyTo(Pw33);
//cout << "Pw33=" << Pw33 << endl;
Pw44_4(Rect(0, 0, 1, 3)).copyTo(Pw44);
//cout << "Pw44=" << Pw44 << endl;
Pw55_5(Rect(0, 0, 1, 3)).copyTo(Pw55);
cout << "Pw55=" << Pw55 << endl;
hconcat(Ps1, Ps2, A1);
//cout << "A1=" << A1 << endl;
hconcat(A1, Ps3, A2);
//cout << "A2=" << A2 << endl;
hconcat(A2, Ps4, A3);
//cout << "A3=" << A3 << endl;
hconcat(A3, Ps5, PS);
cout << "PS=" << PS << endl;
hconcat(Pw11, Pw22, B1);
//cout << "B1=" << B1 << endl;
hconcat(B1, Pw33, B2);
//cout << "B2=" << B2 << endl;
hconcat(B2, Pw44, B3);
//cout << "B3=" << B3 << endl;
hconcat(B3, Pw55, PW);
cout << "PW=" << PW << endl;
PS.copyTo(PSS);
cout << "PSS=" << PSS << endl;
PW.copyTo(PWW);
cout << "PWW=" << PWW << endl;
int row = PSS.rows;
int col = PSS.cols;
MatrixXd in(row, col);
MatrixXd out(row, col);
cv2eigen(PSS, in);
cv2eigen(PWW, out);
cout << "in=" << in << endl;
cout << "out=" << out << endl;
MatrixXd T;
T = Eigen::umeyama(in, out, 0);//9ms
cout << "Tm_Matrix = " << T << endl;
参考文献：https://blog.csdn.net/u014170067/article/details/83834043/
662
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
