工控網首頁
>

應用設計

>

工件坐標系計算實現原理與代碼

工件坐標系計算實現原理與代碼

2020/4/30 14:12:23

三大重要數據

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

審核編輯(
王靜
)
投訴建議

提交

查看更多評論
其他資訊

查看更多

電機的五種啟動方式原理對比

智能倉儲體系打造的4大關鍵點

工業機器人變頻器的電路分析及維修方式

工業機器人變頻器的電路分析及維修方式

機器視覺相機標定的目的、原理及步驟