日志——如何用UR相对位姿获得相对的齐次变换矩阵（UR相对位姿是位置加旋转矢量）

时间：2024-03-12 15:13:05

 编码任务内容：

已知“UR机器人获得，特殊特征坐标系（例如工件，以下简称特征）相对于基座特征坐标系（以下简称基座）的位姿（UR规范位姿）”，

每次给定一个“空间点（例如机械臂末端工具中心点tcp）相对于基座的（UR规范）位姿”，求对应的“该空间点相对于特征的（UR规范）位姿”。

其中tcp_from_Base由函数 rtde_receive.getActualTCPPose()获取。

1首先确定，什么是UR书写规范的位姿：X，Y，Z，RX，RY，RZ

参考古月居教程（UR机器人（一）：坐标系及位姿表示方法）：https://www.guyuehome.com/33774

 

石锤在UR用户手册里：99419_UR5e_User_Manual_zh_Global.pdf   ——第三节polyscope手册——运动选项卡——变量——位姿

2编码：

 2.1步 编码思路

   2.1.1规律如下

b指代base     f指代feature      t指代tool或tcp

则：

posef2t                              (posef2b用不上)                   poseb2t

f2t_homo_matrix           =  f2b_homo_matrix  * b2t_homo_matrix

 2.1.2方法过程如下：

（1）poseb2f ——》b2f_homo_matrix —求逆—》f2b_homo_matrix

（2） poseb2t——》b2t_homo_matrix

（3）f2t_homo_matrix    =  f2b_homo_matrix  * b2t_homo_matrix

（4）f2t_homo_matrix ——》posef2t  

 

 2.2步

确认pose与齐次变换矩阵的相互转化公式（每一个位姿pose均对应一个齐次变换方阵，反过来每一个齐次变换方阵对应两个等效位姿。）

（1）资料查阅其计算公式：（该网址上也有：古月居知乎https://www.guyuehome.com/33774   或 https://blog.csdn.net/m0_37251750/article/details/108583687）

 

 

 

 

 

 

（2）以上过程的C++实现代码如下，结果用Eigen矩阵封装：（Eigen库是常见的矩阵运算库，用法有链接：）

 1 Eigen::Matrix4d homo_matrix_prepare5(Eigen::VectorXd ur_pose)
 2 {
 3     //该函数hemo_matrix_prepare调用了函数matix_multiple
 4     //定义动系相对于定系的旋转平移,f定系b动系
 5     double px, py, pz, rx, ry, rz;
 6     px = ur_pose(0);
 7     py = ur_pose(1);
 8     pz = ur_pose(2);
 9     rx = ur_pose(3);
10     ry = ur_pose(4);
11     rz = ur_pose(5);
12 
13     double angle = sqrt(rx * rx + ry * ry + rz * rz);
14 
15     double kx = rx / angle;
16     double ky = ry / angle;
17     double kz = rz / angle;
18 
19 
20 
21     //4*4齐次变换方阵初始化
22     Eigen::Matrix4d _homo_matrix;
23 
24 
25     //矩阵各元素赋值为0
26     _homo_matrix.fill(0.0);
27 
28 
29     //由旋转矢量的位姿pose求齐次变换矩阵
30     _homo_matrix(0, 0) = cos(angle) + kx * kx * (1 - cos(angle));
31     _homo_matrix(0, 1) = kx * ky * (1 - cos(angle)) - kz * sin(angle);
32     _homo_matrix(0, 2) = ky * sin(angle) + kx * kz * (1 - cos(angle));
33     _homo_matrix(0, 3) = px;
34 
35     _homo_matrix(1, 0) = kz * sin(angle) + kx * ky * (1 - cos(angle));
36     _homo_matrix(1, 1) = cos(angle) + ky * ky * (1 - cos(angle));
37     _homo_matrix(1, 2) = -kx * sin(angle) + ky * kz * (1 - cos(angle));
38     _homo_matrix(1, 3) = py;
39 
40     _homo_matrix(2, 0) = -ky * sin(angle) + kx * kz * (1 - cos(angle));
41     _homo_matrix(2, 1) = kx * sin(angle) + ky * kz * (1 - cos(angle));
42     _homo_matrix(2, 2) = cos(angle) + kz * kz * (1 - cos(angle));
43     _homo_matrix(2, 3) = pz;
44 
45     _homo_matrix(3, 0) = 0;
46     _homo_matrix(3, 1) = 0;
47     _homo_matrix(3, 2) = 0;
48     _homo_matrix(3, 3) = 1;
49 
50     return _homo_matrix;
51 };

（3）若结果矩阵用两层vector容器封装，则可用以下代码：

 1 std::vector<std::vector<double>> hemo_matrix_prepare1(std::vector<double> vf_to_b)
 2 {
 3     //该函数hemo_matrix_prepare调用了函数matix_multiple
 4     //定义动系相对于定系的旋转平移,f定系b动系
 5     double px, py, pz, rx, ry, rz;
 6     px = vf_to_b[0];
 7     py = vf_to_b[1];
 8     pz = vf_to_b[2];
 9     rx = vf_to_b[3];
10     ry = vf_to_b[4];
11     rz = vf_to_b[5];
12 
13     //计算旋转角的三角函数值
14     double c1 = cos(rx);
15     double s1 = sin(rx);
16     double c2 = cos(ry);
17     double s2 = sin(ry);
18     double c3 = cos(rz);
19     double s3 = sin(rz);
20 
21     int raw = 4;
22     int col = 4;
23     std::vector<std::vector<double>> matri(raw, std::vector<double>(col));
24     //矩阵各元素初始化为0
25     for (int i = 0; i < raw; i++)
26     {
27         for (int j = 0; j < col; j++)
28         {
29             matri[i][j] = 0.0;
30         }
31     }
32 
33     //为矩阵赋值
34     matri[0][0] = c3*c2;
35     matri[0][1] = -s3*c1 + c3*s2*s1;
36     matri[0][2] = s3*s1 + c3*s2*c1;
37     matri[0][3] = px;
38 
39     matri[1][0] = s3*c2;
40     matri[1][1] = c3*c1 + s3*s2*s1;
41     matri[1][2] = -c3*s1 + s3*s2*c1;
42     matri[1][3] = py;
43 
44     matri[2][0] = -s2;
45     matri[2][1] = c2*s1;
46     matri[2][2] = c2*c1;
47     matri[2][3] = pz;
48 
49     matri[3][0] = 0;
50     matri[3][1] = 0;
51     matri[3][2] = 0;
52     matri[3][3] = 1;
53 
54     //输出显示矩阵
55     for (int i = 0; i < raw; i++)
56     {
57         for (int j = 0; j < col; j++)
58         {
59             std::cout << matri[i][j] << " ";
60         }
61         std::cout << std::endl;
62     }
63 
64     return matri;
65 };

 

（4）代码实现可以借助eigen库的函数：https://www.cnblogs.com/lovebay/p/11215028.html或https://www.tqwba.com/x_d/jishu/401457.html

 1 Eigen::Matrix4d homo_matrix_prepare6(Eigen::VectorXd ur_pose)
 2 {
 3     //该函数hemo_matrix_prepare调用了函数matix_multiple
 4     //定义动系相对于定系的旋转平移,f定系b动系
 5     double px, py, pz, rx, ry, rz;
 6     px = ur_pose(0);
 7     py = ur_pose(1);
 8     pz = ur_pose(2);
 9     rx = ur_pose(3);
10     ry = ur_pose(4);
11     rz = ur_pose(5);
12 
13     double angle = sqrt(rx * rx + ry * ry + rz * rz);
14 
15     double kx = rx / angle;
16     double ky = ry / angle;
17     double kz = rz / angle;
18 
19     //由UR旋转矢量建立角轴旋转量
20     Eigen::AngleAxisd rotation_vector(angle, Vector3d(kx, ky, kz));
21     //旋转向量转旋转矩阵
22     Eigen::Matrix3d rotation_matrix;
23     rotation_matrix = rotation_vector.matrix();
24 
25     //4*4齐次变换方阵初始化
26     Eigen::Matrix4d _homo_matrix;
27     //矩阵各元素赋值为0
28     _homo_matrix.fill(0.0);
29     //由旋转矢量的位姿pose求齐次变换矩阵
30     _homo_matrix.block(0,0,3,3) = rotation_matrix;
31     //_homo_matrix(0, 0) = rotation_matrix(0,0);
32     //_homo_matrix(0, 1) = rotation_matrix(0, 1);
33     //_homo_matrix(0, 2) = rotation_matrix(0, 2);
34     _homo_matrix(0, 3) = px;
35 
36     //_homo_matrix(1, 0) = rotation_matrix(1, 0);
37     //_homo_matrix(1, 1) = rotation_matrix(1, 1);
38     //_homo_matrix(1, 2) = rotation_matrix(1, 2);
39     _homo_matrix(1, 3) = py;
40 
41     //_homo_matrix(2, 0) = rotation_matrix(2, 0);
42     //_homo_matrix(2, 1) = rotation_matrix(2, 1);
43     //_homo_matrix(2, 2) = rotation_matrix(2, 2);
44     _homo_matrix(2, 3) = pz;
45 
46     _homo_matrix(3, 0) = 0;
47     _homo_matrix(3, 1) = 0;
48     _homo_matrix(3, 2) = 0;
49     _homo_matrix(3, 3) = 1;
50 
51     return _homo_matrix;
52 };

 

（5）矩阵的快捷获得方法可以借助软件RoboDK或在线工具3D Rotation Converter（https://www.andre-gaschler.com/rotationconverter/）辅助，

但工程中开发不能依赖这两个，还是得代码实现。

RoboDK（三个坐标系指代基座、特征、点，位姿项有专门的UR规范位姿）：

 

 

 3D Rotation Converter很方便有旋转矩阵和旋转矢量（和轴角量）转换功能：

 

 

 

3代码运算结果

 3.1

机器人示教盒获得：特征坐标系相对于基座坐标系的位姿（x,y,z,rx,ry,rz），位置和姿态ur姿态是旋转矢量（弧度）;

旋转矢量换算变换矩阵：特征坐标系相对于基座坐标系的4*4齐次变换矩阵；

变换矩阵求逆：基座坐标系相对于特征坐标系的4*4齐次变换矩阵；

 

 

 

 

3.2满足所需功能后，UR采集数据的最新结果：

3.2.1错结果是因为b_to_f位姿输入错了：

 

 

 

3.2.2以下为对的结果（姿态有误差）

tcppose[i],X,Y,Z,RX,RY,RZ
tcppose[0],0.457771,-1.16743,-0.839458,-0.422147,-2.97357,-0.291953,
tcppose[1],0.533914,-1.38673,-0.883963,-0.421599,-2.97358,-0.29254,
tcppose[2],0.608705,-1.60282,-0.927664,-0.421725,-2.97339,-0.292593,
tcppose[3],0.473329,-1.64594,-0.946665,-0.42167,-2.97385,-0.29189,
tcppose[4],0.424685,-1.50492,-0.917941,-0.422007,-2.97387,-0.291572,
tcppose[5],0.3631,-1.32744,-0.88223,-0.421892,-2.9736,-0.291954,
tcppose[6],0.220926,-1.37275,-0.902443,-0.421782,-2.97349,-0.292524,
tcppose[7],0.27585,-1.53036,-0.934056,-0.421551,-2.97373,-0.292211,
tcppose[8],0.317723,-1.65118,-0.95874,-0.421726,-2.97375,-0.292612,

 参考8号点（第9个点）的ur示教盒数据：

（单位：毫米和rad）

X：317.58~317.66           Y：   -1650.99~-1651.07             Z： -958.69~-958.77

RX：0.456~0.457             RY：  3.218                                RZ： 0.316

 

比较得到误差为：

（单位：毫米和rad）

 

X： 0.14~0.06                   Y：   -0.18~-0.11             Z： -0.05~0.03

RX：0.034274‬~0.035274‬  RY：  0.24425‬                RZ： 0.023388‬

 

用特征相对于基座坐标系的齐次变换方阵，左乘“表示三维位置（工具TCP相对于基座）的四维齐次向量”，

即可得到（表示三维位置的）工具TCP相对于特殊特征坐标系（如工件坐标系）的四维齐次向量。

 

 

4 中间遇到的坑和使用的解决方法：

4.1结合仿真环境，检验转换代码

 

 

 

 

4.2机械臂给的逆位姿是否直接用做feature_to_base？

在仿真空间环境下模拟相对于特征坐标系2逆位姿的坐标系4，并观察坐标系4是否重合回基座坐标系1，发现同样适用：

（1）创建坐标系4，输入相对于坐标系2（Frame2）的UR位姿pose：0.46488, -0.557696, -0.522333, 0.2352, 0.080104, -1.243983

（2）观察坐标系4是否重合回基座坐标系1，确实重合了

 

 

 

 

 

 

 

 

 

 4.3Eigen轴角量AngleAxisd的特殊类型

参考：Eigen的使用总结2——geometry

https://blog.csdn.net/reasonyuanrobot/article/details/86614381

4.3.1 ftr_to_tcp_AngleAxisd.axis和ftr_to_tcp_AngleAxisd.axis（）有区别：

前者编译时会报错，且错误不会显示在源代码任何位置，而是显示在库中，难以排查。后者才能返回一个Eigen::vector3&

用以上方法转换位姿向量，得到的是能cout输出的Vector3d类型 。

 

4.3.2AngleAxisd类型

Eigen中齐次变换矩阵转旋转矢量，离不开AngleAxisd类型，但该类型操作起来繁琐

AngleAxisd类型对象无法cout输出，但可以初始化和调用.angle()或.axis（）

4.3.3以下是排查日志，日志中765.307一项（即featur_from_base位姿的X值）全写错了（正确是-765.307），

导致借助仿真的换算功能完善后在实际机器人上采集数据发现有误。

 

 getActualTCPPose()函数获取UR位姿单位是米和弧度，而不是毫米和弧度

 

 

以这个位姿到达工件上方

0.0556585, 0.247949, -0.0146147, -2.51002, -1.68042, -0.0595131

55.6585,247.949, -14.6147, -2.51002, -1.68042, -0.0595131

 

Framewaypoint3

0.0400149,0.173836,-0.0816024,2.54263,1.70769,-0.0918158

40.0149,173.836,-081.6024,2.54263,1.70769,-0.0918158

 

 Framewaypoint1

0.0367395,0.168845,-0.00321814,-2.58042,-1.73474,-0.0687103

36.7395,168.845,-3.21814,-2.58042,-1.73474,-0.0687103

 

p[0.0367191, 0.168905, -0.0556391, -2.58054, -1.73458, -0.0686292]

36.7191, 168.905, -55.6391, -2.58054, -1.73458, -0.0686292

 

初始点2Framewaypoint2

p[0.0212962,-0.205567,-0.0348749,-2.0185,-2.35853,-0.0462862]

21.2962,-205.567,-34.8749,-2.0185,-2.35853,-0.0462862

进针极限点2-2Framewaypoint2-2limit       (2-2相对于2是竖直下降，近似于工具坐标z方向的进给)

p[0.0212387,-0.20555,-0.122159,-2.01866,-2.35867,-0.0462557]

21.2387,-205.55,-122.159,-2.01866,-2.35867,-0.0462557

 

初始点4Framewaypoint4

p[0.0212962,-0.193537,-0.0348749,-2.0185,-2.35853,-0.0462862]

21.2962,-193.537,-34.8749,-2.0185,-2.35853,-0.0462862

进针极限点4-2Framewaypoint4-2limit       (4-2相对于4是竖直下降，近似于工具坐标z方向的进给)

p[0.0212962,-0.193537,-0.1166549,-2.0185,-2.35853,-0.0462862]

21.2962,-193.537,-116.6549,-2.0185,-2.35853,-0.0462862