工业机器人

当前位置:前位置:首页 > 工业机器人
全部 728

【ABB】工件坐标系计算实现原理与代码

时间:2023-08-16   访问量:0

三大重要数据

ABB机器人的三大重要数据分别是工件数据(wobjdata)、工具数据(tooldata)和负载数据(loaddata)。

我们今天来讨论一下如何利用空间上的任意三点(不在同一直线上)来自定义工件数据(wobjdata)。

工件数据简介

ABB机器人的工件数据(wobjdata)是由逻辑状态bool数据robhold和ufprog、字符串string数据ufmec、坐标系姿态pose数据uframe和oframe复合而成的。

一般情况下,我们建立的工件数据的工件安装形式、工装安装形式、运动单元名称、工件坐标系与wobj0是一致的。所以,在我们建立工件数据时,只需要确定用户坐标系的原点位置和坐标轴方向即可。ABB机器人默认工件数据wobj0的定义如下所示:

PERS wobjdata wobj0 := [FALSE, TRUE, "", [[0, 0, 0],[1, 0, 0, 0]],[[0, 0, 0],[1, 0, 0, 0]]];

工件数据wobjdata

自定义工件数据的建立

如下图所示,已知空间不在同一直线上的任意三点p1(x1,y1,z1)、p2 (x2,y2,z2)和p3(x3,y3,z3)的坐标。

从工件数据的定义可知,我们自定义的工件数据只需要确定两个方面的内容,分别是用户坐标系的原点位置和用户坐标系的坐标轴原点姿态(方向)。

1、 原点位置的确定

1、如上图所示,假设p1、p2点所确定的直线为L1为,我们从直线外一点p3向直线L1做垂线,其垂足为p4。

2、ABB机器人默认定义工件坐标系时,采用上述方法,即p1、p2所在的直线构成x轴方向,p3到p1、p2的垂线交点为坐标系原点,即p4(x,y,z)点。p3、p4点所在的直线为y轴方向,记为直线L2。

3、由空间直线方程式,我们可以把经过p1、p2的直线L1可以表述为公式一:

4、p1、p2所在的直线L1,垂直于p3、p4所在的直线L2 。由直线垂直向量关系可知:

把各个坐标代入可得公式二:

5、为了简便计算,我们假设:

联立公式一和公式二,可以得到如下矩阵方程式:

6、使用PAPID指令MatrixSolve A1,b1,x1 可以求解如上方程组,注意MatrixSolve指令后的数据为dnum类型,其代码如下所示:

FUNC pos cal_frame2(robtarget p1,robtarget p2,robtarget p3)

VAR dnum a;

VAR dnum b;

VAR dnum c;

VAR dnum A1{3,3};

VAR dnum b1{3};

VAR dnum x1{3};

VAR pos pos_return;

VAR pose pose_return;

VAR num rz;

VAR num ry;

VAR num rx;

a:=NumToDnum((p1.trans.x-p2.trans.x));

b:=NumToDnum((p1.trans.y-p2.trans.y));

c:=NumToDnum((p1.trans.z-p2.trans.z));

A1:=[[a,b,c],[b,-a,0],[c,0,-a]];

b1{1}:=(a*NumToDnum(p3.trans.x)+b*NumToDnum(p3.trans.y)+c*NumToDnum(p3.trans.z));

b1{2}:=(NumToDnum(p1.trans.x)*b-NumToDnum(p1.trans.y)*a);

b1{3}:=(NumToDnum(p1.trans.x)*c-NumToDnum(p1.trans.z)*a);

MatrixSolve A1,b1,x1;

pos_return.x:=DnumToNum(x1{1});

pos_return.y:=DnumToNum(x1{2});

pos_return.z:=DnumToNum(x1{3});

RETURN pos_return;

ENDFUNC

2、坐标轴原点姿态的确定

1、通过上述步骤我们得到了原点p4的坐标(x,y,z),那么就可获取向量p4p1,单位化后得到ox,获取向量p4p3,单位化后得到oy。单位向量计算公式如下所示:

2、求单位向量oz,向量oz等于单位向量ox叉乘单位向量oy。

3、将旋转矩阵[ox,oy,oz]T转化为欧拉角。

4、利用OrientZYX函数将欧拉角转化为四元数。其代码如下所示:

PROC main()

p4.trans:=cal_frame2(p1,p2,p3);

cal_frame_orient p1.trans,p2.trans,p3.trans,rz,ry,rx;

p4.rot:=OrientZYX(rz,ry,rx);

UserwobjTest.uframe.trans:=p4.trans;

UserwobjTest.uframe.rot:=p4.rot;

ENDPROC

PROC cal_frame_orient(pos px1,pos px2,pos py,INOUT num a,INOUT num b,INOUT num c)

!A:Rot_z() , B:Rot_y() ,c: Rot_x()

VAR pos vpx;

VAR pos vpxy;

VAR pos vpy;

VAR pos vpz;

vpx:=px2-px1;

!获取向量x1x2

vpx:=vec_nor(vpx);

!单位化向量vpx

vpxy:=py-px1;

!获取向量x1y1

vpxy:=vec_nor(vpxy);

!单位化向量vpx

vpz:=vpx*vpxy;

!向量vpx叉乘向量vpxy

vpy:=vpz*vpx;

!向量vpz叉乘向量vpx

!将单位向量存入数组nrT

nrT{1,1}:=vpx.x;

nrT{1,2}:=vpy.x;

nrT{1,3}:=vpz.x;

nrT{2,1}:=vpx.y;

nrT{2,2}:=vpy.y;

nrT{2,3}:=vpz.y;

nrT{3,1}:=vpx.z;

nrT{3,2}:=vpy.z;

nrT{3,3}:=vpz.z;

MatrixToRpy nrT,a,b,c;

!调用旋转矩阵转欧拉角函数

ENDPROC

PROC MatrixToRpy(num nrT{*,*},INOUT num nrA,INOUT num nrB,INOUT num nrC)

!Transform Trafo-Matrix to RPY-Angle A, B, C

!T = Rot_z(A) * Rot_y(B) * Rot_x(C)

VAR num nrSinA;

VAR num nrCosA;

VAR num nrSinB;

VAR num nrAbsCosB;

VAR num nrSinC;

VAR num nrCosC;

nrA:=ATan2(nrT{2,1},nrT{1,1});

nrSinA:=Sin(nrA);

nrCosA:=Cos(nrA);

nrSinB:=-nrT{3,1};

nrAbsCosB:=nrCosA*nrT{1,1}+nrSinA*nrT{2,1};

!Value: -90 := B := +90 !!

nrB:=ATan2(nrSinB,nrAbsCosB);

nrSinC:=nrSinA*nrT{1,3}-nrCosA*nrT{2,3};

nrCosC:=-nrSinA*nrT{1,2}+nrCosA*nrT{2,2};

nrC:=ATan2(nrSinC,nrCosC);

ENDPROC

FUNC pos vec_nor(pos pos1)

VAR pos pos2;

pos2.x:=pos1.x/VectMagn(pos1);

pos2.y:=pos1.y/VectMagn(pos1);

pos2.z:=pos1.z/VectMagn(pos1);

RETURN pos2;

ENDFUNC

完整代码如下所示

MODULE UserWobjdata

!计算出来的工件数据

PERS wobjdata UserwobjTest:=[FALSE,TRUE,"",[[1159.71,-499.998,1100],[0.965931,0,0,0.258798]],[[0,0,0],[1,0,0,0]]];

VAR robtarget p1:=[[1221.60,-464.27,1100.00],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

VAR robtarget p2:=[[1332.93,-400.00,1100.00],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

VAR robtarget p3:=[[1059.72,-326.79,1100.00],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

VAR robtarget p4:=[[1253.322,0,1100],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

VAR num nrT{3,3}:=[[0,0,0],[0,0,0],[0,0,0]];

VAR num rx;

【ABB】工件坐标系计算实现原理与代码

VAR num ry;

VAR num rz;

!用示教器创建的工件数据

PROC main()

p4.trans:=cal_frame2(p1,p2,p3);

cal_frame_orient p1.trans,p2.trans,p3.trans,rz,ry,rx;

p4.rot:=OrientZYX(rz,ry,rx);

UserwobjTest.uframe.trans:=p4.trans;

UserwobjTest.uframe.rot:=p4.rot;

ENDPROC

PROC cal_frame_orient(pos px1,pos px2,pos py,INOUT num a,INOUT num b,INOUT num c)

!A:Rot_z() , B:Rot_y() ,c: Rot_x()

VAR pos vpx;

VAR pos vpxy;

VAR pos vpy;

VAR pos vpz;

vpx:=px2-px1;

!获取向量x1x2

vpx:=vec_nor(vpx);

!单位化向量vpx

vpxy:=py-px1;

!获取向量x1y1

vpxy:=vec_nor(vpxy);

!单位化向量vpx

vpz:=vpx*vpxy;

!向量vpx叉乘向量vpxy

vpy:=vpz*vpx;

!向量vpz叉乘向量vpx

!将单位向量存入数组nrT

nrT{1,1}:=vpx.x;

nrT{1,2}:=vpy.x;

nrT{1,3}:=vpz.x;

nrT{2,1}:=vpx.y;

nrT{2,2}:=vpy.y;

nrT{2,3}:=vpz.y;

nrT{3,1}:=vpx.z;

nrT{3,2}:=vpy.z;

nrT{3,3}:=vpz.z;

MatrixToRpy nrT,a,b,c;

!调用旋转矩阵转欧拉角函数

ENDPROC

PROC MatrixToRpy(num nrT{*,*},INOUT num nrA,INOUT num nrB,INOUT num nrC)

!Transform Trafo-Matrix to RPY-Angle A, B, C

!T = Rot_z(A) * Rot_y(B) * Rot_x(C)

VAR num nrSinA;

VAR num nrCosA;

VAR num nrSinB;

VAR num nrAbsCosB;

VAR num nrSinC;

VAR num nrCosC;

nrA:=ATan2(nrT{2,1},nrT{1,1});

nrSinA:=Sin(nrA);

nrCosA:=Cos(nrA);

nrSinB:=-nrT{3,1};

nrAbsCosB:=nrCosA*nrT{1,1}+nrSinA*nrT{2,1};

!Value: -90 := B := +90 !!

nrB:=ATan2(nrSinB,nrAbsCosB);

nrSinC:=nrSinA*nrT{1,3}-nrCosA*nrT{2,3};

nrCosC:=-nrSinA*nrT{1,2}+nrCosA*nrT{2,2};

nrC:=ATan2(nrSinC,nrCosC);

ENDPROC

FUNC pos vec_nor(pos pos1)

VAR pos pos2;

pos2.x:=pos1.x/VectMagn(pos1);

pos2.y:=pos1.y/VectMagn(pos1);

pos2.z:=pos1.z/VectMagn(pos1);

RETURN pos2;

ENDFUNC

FUNC pos cal_frame2(robtarget p1,robtarget p2,robtarget p3)

VAR dnum a;

VAR dnum b;

VAR dnum c;

VAR dnum A1{3,3};

VAR dnum b1{3};

VAR dnum x1{3};

VAR pos pos_return;

VAR pose pose_return;

VAR num rz;

VAR num ry;

VAR num rx;

a:=NumToDnum((p1.trans.x-p2.trans.x));

b:=NumToDnum((p1.trans.y-p2.trans.y));

c:=NumToDnum((p1.trans.z-p2.trans.z));

A1:=[[a,b,c],[b,-a,0],[c,0,-a]];

b1{1}:=(a*NumToDnum(p3.trans.x)+b*NumToDnum(p3.trans.y)+c*NumToDnum(p3.trans.z));

b1{2}:=(NumToDnum(p1.trans.x)*b-NumToDnum(p1.trans.y)*a);

b1{3}:=(NumToDnum(p1.trans.x)*c-NumToDnum(p1.trans.z)*a);

MatrixSolve A1,b1,x1;

pos_return.x:=DnumToNum(x1{1});

pos_return.y:=DnumToNum(x1{2});

pos_return.z:=DnumToNum(x1{3});

RETURN pos_return;

ENDFUNC

ENDMODULE

上一篇:武汉四名大一学生制作“按摩机器人”可捶背捏肩

下一篇:曼彻斯特大学研发化学传感器助力救援机器人搜救幸存者

返回顶部