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

三大重要数据

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;

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

    TASK PERS wobjdata Workobject_1:=[FALSE,TRUE,"",[[1159.722,-500,1100],[0.9659258,0,0,0.258819143]],[[0,0,0],[1,0,0,0]]];

    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