【ABB】工件坐标系计算实现原理与代码_工业机器人

三大重要数据

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;

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

工业机器人

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

联系我们