技术文章

当前页面: 首页 >技术文章 >工件坐标系计算实现原理与代码

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

供稿:公号三0智工 2020/4/30 14:12:23

0 人气:695

  • 关键词: 工件坐标系
  • 摘要:ABB机器人的三大重要数据分别是工件数据(wobjdata)、工具数据(tooldata)和负载数据(loaddata)。如何利用空间上的任意三点(不在同一直线上)来自定义工件数据(wobjdata)

三大重要数据

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

审核编辑(王静)
更多内容请访问 公号三0智工(http://home.gongkong.com/profile/?uid=S020030315304000001)

手机扫描二维码分享本页

工控网APP下载安装

 

我来评价

评价:
一般