牛眼标定TCP原理

牛眼（bullseye)设备为ABB机器人推出的针对焊枪等工具自动标定TCP的设备。具体可以参见bullseye 用户手册。牛眼本质上就是一个光电开关，工具挡住了光电信号为1，未挡住信号为0，该设备不具有测距等功能。那么如何通过单光电信号计算TCP的xyz位置呢？预设光电安装方向与机器人base的xy平面平行。

，时长01:54

机器人以不同姿态在光电开关附近进行平动，可以搜索得到工具末端刚好在光电所在这条线上的位置（如上图所示），记录这4个位置对应tool0的位置为p1、p2、p3、p4。用位姿表示p1如下：

假设要求的工具tool为TCP表示如下：

则p1（基于tool0）对应的tool末端在坐标系下的值为p10，可以表达为：

即tool末端在坐标系下的z=a31*xt+a32*yt+a33*zt+z1，记A1=[a31,a32,a33]，TCP=[xt,yt,zt]，则z=A1*TCP+z1。p20、p30、p40同理。

tool末端对应的4个点成一条直线，也可理解为：在某个坐标系下tool末端对应的4个点高度z相同，即p10、p20、p30、p40的z相同，即

整理上式，得到

使用MatrixSolve A,b,TCP；求解得到TCP的xyz位置。计算过程RAPID代码参考如下：

LOCAL PROC calTCP1(robtarget p1,robtarget p2,robtarget p3,robtarget p4,INOUT tooldata t)    ! neddle in same line, same height with different orient    ! robtarget with tool0    ! calculate TCP.trans
    VAR dnum A1{3,3};    VAR dnum A2{3,3};    VAR dnum A3{3,3};    VAR dnum A4{3,3};
    VAR dnum A{3,3};    VAR dnum b{3};    VAR dnum resultTCP{3};
    QuadToPostran p1.rot,A1;    QuadToPostran p2.rot,A2;    QuadToPostran p3.rot,A3;    QuadToPostran p4.rot,A4;
    FOR i FROM 1 TO 3 DO        A{1,i}:=A1{3,i}-A2{3,i};    ENDFOR
    FOR i FROM 1 TO 3 DO        A{2,i}:=A2{3,i}-A3{3,i};    ENDFOR
    FOR i FROM 1 TO 3 DO        A{3,i}:=A3{3,i}-A4{3,i};    ENDFOR
    b{1}:=NumToDnum(p2.trans.z-p1.trans.z);    b{2}:=NumToDnum(p3.trans.z-p2.trans.z);    b{3}:=NumToDnum(p4.trans.z-p3.trans.z);
    MatrixSolve A,b,resultTCP;    t.tframe.trans.x:=DnumToNum(resultTCP{1});    t.tframe.trans.y:=DnumToNum(resultTCP{2});    t.tframe.trans.z:=DnumToNum(resultTCP{3});ENDPROC
LOCAL PROC QuadToPostran(orient Quad,INOUT dnum Postran{*,*})    ! orient to matrix{3*3}    Postran{1,1}:=NumToDnum(1-2*(Quad.q3*Quad.q3+Quad.q4*Quad.q4));    Postran{1,2}:=NumToDnum(2*Quad.q2*Quad.q3-2*Quad.q1*Quad.q4);    Postran{1,3}:=NumToDnum(2*Quad.q1*Quad.q3+2*Quad.q2*Quad.q4);    Postran{2,1}:=NumToDnum(2*Quad.q2*Quad.q3+2*Quad.q1*Quad.q4);    Postran{2,2}:=NumToDnum(1-2*(Quad.q2*Quad.q2+Quad.q4*Quad.q4));    Postran{2,3}:=NumToDnum(-2*Quad.q1*Quad.q2+2*Quad.q3*Quad.q4);    Postran{3,1}:=NumToDnum(-2*Quad.q1*Quad.q3+2*Quad.q2*Quad.q4);    Postran{3,2}:=NumToDnum(2*Quad.q1*Quad.q2+2*Quad.q3*Quad.q4);    Postran{3,3}:=NumToDnum(1-2*(Quad.q3*Quad.q3+Quad.q2*Quad.q2));ENDPROC

搜索末端点可采取z字折线往返方式（如上图所示），搜索过程RAPID代码参考如下：

LOCAL PROC SearchTheLowestPoint(robtarget StartPoint,INOUT robtarget SearchPoint)    VAR robtarget pStart;    VAR robtarget pEnd;    VAR robtarget pTarget;    VAR num nStep;    VAR num nDirectionZ;    VAR num nDirectionY;    VAR num nReentry;    IDelete intStart;    CONNECT intStart WITH TrapStart;    TriggInt trigStart,0,intStart;
    IDelete intEnd;    CONNECT intEnd WITH TrapEnd;    TriggInt trigEnd,0,intEnd;
    IDelete intCheck;    CONNECT intCheck WITH TrapCheck;    TriggCheckIO trigCheck,0,di1,EQ,0\StopMove,intCheck;
    IDelete intEnter;    CONNECT intEnter WITH TrapEnter;    ISignalDI di1,1,intEnter;    ISleep intEnter;
    IDelete intLeave;    CONNECT intLeave WITH TrapLeave;    ISignalDI di1,0,intLeave;    ISleep intLeave;
    nStep:=1.6;    nDirectionY:=1;    pEnter:=pEmpty;    pLeave:=pEmpty;    pStart:=Offs(StartPoint,0,30,0);    pEnd:=Offs(StartPoint,0,-30,0);    FAST_SEARCH:    TriggL pStart,v100,trigStart\T2:=trigCheck,z5,tool0;    TriggL pEnd,v5,trigEnd,z5\Inpos:=inpos20,tool0;    nDirectionY:=-nDirectionY;    IF count>0 THEN        ErrWrite\I,"Count="+ValToStr(count),"";        IF nDirectionZ<>1 THEN            nDirectionZ:=1;            Incr nReentry;            ErrWrite\I,"nReentry="+ValToStr(nReentry),"";            IF Abs(nStep)<=0.1 THEN                pTarget:=pStart;                pStart:=pEnd;                pEnd:=pTarget;                GOTO FINE_SEARCH;            ELSEIF nReentry>1 THEN                nStep:=-nStep/2;                ErrWrite\I,"nStep="+ValToStr(nStep),"";            ENDIF        ENDIF        IF pEnter.trans<>pEmpty.trans THEN            pStart:=Offs(pLeave,0,10*nDirectionY,nStep);            pEnd:=Offs(pEnter,0,-10*nDirectionY,nStep);            pLeave:=pEmpty;            pEnter:=pEmpty;        ELSE            pTarget:=pStart;            pStart:=Offs(pEnd,0,0,nStep);            pEnd:=Offs(pTarget,0,0,nStep);        ENDIF        GOTO FAST_SEARCH;    ELSE        IF nDirectionZ<>-1 THEN            nDirectionZ:=-1;            Incr nReentry;            ErrWrite\I,"nReentry="+ValToStr(nReentry),"";            IF Abs(nStep)<=0.1 THEN                pTarget:=pStart;                pStart:=Offs(pEnd,0,0,-nStep);                pEnd:=Offs(pTarget,0,0,-nStep);                GOTO FINE_SEARCH;            ELSEIF nReentry>1 THEN                nStep:=-nStep/2;                ErrWrite\I,"nStep="+ValToStr(nStep),"";            ENDIF        ENDIF        pTarget:=pStart;        pStart:=Offs(pEnd,0,0,nStep);        pEnd:=Offs(pTarget,0,0,nStep);        GOTO FAST_SEARCH;    ENDIF    FINE_SEARCH:    TriggL pStart,v100,trigCheck,z5,tool0;    SearchL\Stop,di1,SearchPoint,pEnd,vSlow,tool0;    IF DInput(di1)=0 THEN        SearchL\Stop,di1,SearchPoint,pStart,vFine,tool0;    ENDIFERROR    IF ERRNO=ERR_WHLSEARCH THEN        ErrWrite\I,"ERR_WHLSEARCH","";        StorePath;        !pTarget:=CRobT(\Tool:=tool0\WObj:=wobj0);        !MoveL Offs(pTarget,0,0,50),v100,fine,tool0;        pTarget:=pStart;        pStart:=Offs(pEnd,0,0,-nStep);        pEnd:=Offs(pTarget,0,0,-nStep);        RestoPath;        ClearPath;        StartMove;        RETRY;    ELSE        ErrWrite "Unknown errno."+ValToStr(ERRNO),"";        Stop;    ENDIFENDPROC
LOCAL TRAP TrapStart    count:=0;    !pEnter:=pEmpty;    !pLeave:=pEmpty;    IWatch intEnter;    IWatch intLeave;ENDTRAP
LOCAL TRAP TrapEnd    ISleep intEnter;    ISleep intLeave;ENDTRAP
LOCAL TRAP TrapCheck    ErrWrite "Jog current initial position again","";    Stop;    StartMove;ENDTRAP
LOCAL TRAP TrapEnter    IF pEnter=pEmpty THEN        pEnter:=CRobT(\Tool:=tool0\WObj:=wobj0);    ENDIF    Incr count;ENDTRAP
LOCAL TRAP TrapLeave    pLeave:=CRobT(\Tool:=tool0\WObj:=wobj0);ENDTRAP