机器人标定:相机9点标定的补充-12点计算旋转中心

1. 机器人在使用相机时，不论相机固定安装或者相机安装在机械臂末端，都需要先执行手眼标定。

2. 最常见的标定为9点标定，例如相机装在机械臂末端，机器人将法兰盘处于标准位置5处的x,y坐标及后续运动的x，y方向的间距发给相机，机械臂按照规则沿着机械臂base的x和y方向走如下轨迹，完成标定。此时机械臂走回标准拍照位置5，拍照得到的相机返回产品坐标所在的坐标系与机器人base坐标系平行。

3. 虽然机械臂在位置5处，将法兰盘的位置发给相机，但相机实际安装如下图（即相机安装不会在法兰盘位置5，相机与机械臂法兰盘有偏置）。所以需要计算得到相机和法兰盘的关系，这样相机返回的产品坐标可以直接是机器人base坐标下的值。

4. 海康相机直接提供了12点标定，其中最后3点为机器人以法兰盘为中心绕着大地的z旋转（若法兰盘平行base，直接旋转6轴），如下图。通过在相机中同一个mark点的不同位置，计算出3个mark位置对应的圆心（即法兰盘）。后续输出结果只需加上该偏差即可。

5. 若相机标定功能不具备自动计算旋转中心功能，则在完成9点标定后，旋转中心的计算和偏差计算可以放在机器人侧完成。具体代码如下：

PERS robtarget pNewActual:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
PERS robtarget pStdActual:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
! pStdActual是产品在标准位置时，计算得到的在机器人base下的实际位姿（包含了旋转中心偏差的补偿）
PERS robtarget pStdFromCam:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
! pStdFromCam是产品在标准位置时，相机返回的坐标值。相机仅和机器人做了9点标定，没有做旋转中心处理
PERS robtarget ptmp:=[[326.174,111.364,558],[5.75447E-8,-0.707136,-0.707078,-7.91093E-9],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget pcam:=[[302,0,558],[5.505613E-08,-0.3187502,-0.9478387,1.851492E-08],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPickStd:=[[302,0,558],[5.505613E-08,-0.3187502,-0.9478387,1.851492E-08],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS pos pos10{3}:=[[248.922,75.1801,0],
                    [302,0,0],
                    [299.772,-70.4799,0]];


PERS pos camdata:=[0,0,-52.83];
PERS pos camdata_ini:=[0,0,-52.83];
PERS pos offset1:=[124.184,31.3486,0]; ！计算出来的旋转中心偏移


PROC rInit()
    rRotCenterCalib pcam,10; 
    ! 执行该程序前，已经完成相机9点标定，但未做相机到法兰盘的旋转中心补偿
ENDPROC


PROC rRotCenterCalib(robtarget pCam0,num angle)
    ! pCam0 is robot take photo position with tool0
    ! angle will let robot rotate Rz, unit:deg
    VAR robtarget pCamCalib{3};
    pCamCalib{1}:=pCam0;
    pCamCalib{2}:=pCam0;
    pCamCalib{3}:=pCam0;


    pCamCalib{1}.rot:=OrientZYX(-angle,0,0)*pcamcalib{1}.rot;
    pCamCalib{3}.rot:=OrientZYX(angle,0,0)*pcamcalib{3}.rot;


    MoveL pCamCalib{1},v1000,fine,tool0;
    !pos10{1}:=机器人法兰盘旋转-AAA°，相机返回值
    MoveL pCamCalib{2},v1000,fine,tool0;
    !pos10{2}:=机器人法兰盘在标准位置，相机返回值
    MoveL pCamCalib{3},v1000,fine,tool0;
    !pos10{3}:= 机器人法兰盘旋转AAA°，相机返回值


    offset1:=calCamRotOffs(pos10);
    ! 计算得到相机与法兰盘在机器人base方向上的偏差
    stop;
ENDPROC


FUNC pos calCamRotOffs(pos posC{*})
    !posC array are robot rotate Rz with flange(tool0)
    !posC{1} : robot rotate Rz -AAA degree, camera result
    !posC{2} : robot move to pCam0, camera result
    !posC{3} : robot rotate Rz  BBB degree, camera result


    VAR pos pcenter;
    VAR num r;
    VAR pos n;
    VAR pos normal;
    fitcircle posC,pcenter,r,normal;
    ! fitcircle1 pos10{1},pos10{2},pos10{3},pcenter,r;
    ! 若现场机器人没有fitcircle内置函数，可以使用后续内容的fitcircle1函数计算圆心
    RETURN [posC{2}.x-pcenter.x,posC{2}.y-pcenter.y,0];
ENDFUNC


!!!!!  ! 以下为仿真测试
PROC testProcess()
    MoveJ pcam,v1000,fine,tool0;
    ! take photo for new product ，移动到拍照位置     
   
    pStdActual:=pStdFromCam;
    pStdActual.trans.x:=camdata_ini.x;
    pStdActual.trans.y:=camdata_ini.y;
    pStdActual.trans:=pStdActual.trans+offset1;
    
    pNewActual:=pStdFromCam;
    pNewActual.trans.x:=camdata.x;
    pNewActual.trans.y:=camdata.y;
    pNewActual.trans:=pNewActual.trans+offset1;
    pNewActual.rot:=OrientZYX(camdata_ini.z-camdata.z,0,0)*pNewActual.rot;


    ptmp:=calTarget(pStdActual,pNewActual,pPickStd);
    ! 通过标准产品位置，新产品位置和基于tool0的标准抓取位置，得到新的基于tool0抓取位置
    MoveL ptmp,v1000,fine,tool0;
    Stop;
ENDPROC


FUNC robtarget calTarget(robtarget pold,robtarget pnew,robtarget pStdPick)
    VAR pose p1;
    VAR pose p2;
    VAR pose p3;
    VAR pose ppick;
    VAR pose ppicknew;
    VAR robtarget pout:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];


    pout:=pStdPick;
    p1:=[pold.trans,pold.rot];
    p2:=[pnew.trans,pnew.rot];


    p3:=PoseMult(p2,PoseInv(p1));
    ppick:=[pStdPick.trans,pStdPick.rot];
    ppicknew:=PoseMult(p3,ppick);


    pout.trans:=ppicknew.trans;
    pout.rot:=ppicknew.rot;
    RETURN pout;
ENDFUNC

PROC fitCircle1(pos p1,pos p2,pos p3,inout pos pcenter,inout num radius)
    ！ 三点计算圆心
    VAR num x1;
    VAR num y1;
    VAR num z1;
    VAR num x2;
    VAR num y2;
    VAR num z2;
    VAR num x3;
    VAR num y3;
    VAR num z3;


    VAR num a1;
    VAR num b1;
    VAR num c1;
    VAR num d1;


    VAR num a2;
    VAR num b2;
    VAR num c2;
    VAR num d2;


    VAR num a3;
    VAR num b3;
    VAR num c3;
    VAR num d3;


    VAR num x;
    VAR num y;
    VAR num z;


    x1:=p1.x;
    y1:=p1.y;
    z1:=p1.z;


    x2:=p2.x;
    y2:=p2.y;
    z2:=p2.z;


    x3:=p3.x;
    y3:=p3.y;
    z3:=p3.z;


    a1:=(y1*z2-y2*z1-y1*z3+y3*z1+y2*z3-y3*z2);
    b1:=-(x1*z2-x2*z1-x1*z3+x3*z1+x2*z3-x3*z2);
    c1:=(x1*y2-x2*y1-x1*y3+x3*y1+x2*y3-x3*y2);
    d1:=-(x1*y2*z3-x1*y3*z2-x2*y1*z3+x2*y3*z1+x3*y1*z2-x3*y2*z1);


    a2:=2*(x2-x1);
    b2:=2*(y2-y1);
    c2:=2*(z2-z1);
    d2:=x1*x1+y1*y1+z1*z1-x2*x2-y2*y2-z2*z2;


    a3:=2*(x3-x1);
    b3:=2*(y3-y1);
    c3:=2*(z3-z1);
    d3:=x1*x1+y1*y1+z1*z1-x3*x3-y3*y3-z3*z3;


    x:=-(b1*c2*d3-b1*c3*d2-b2*c1*d3+b2*c3*d1+b3*c1*d2-b3*c2*d1)/
    (a1*b2*c3-a1*b3*c2-a2*b1*c3+a2*b3*c1+a3*b1*c2-a3*b2*c1);
    y:=(a1*c2*d3-a1*c3*d2-a2*c1*d3+a2*c3*d1+a3*c1*d2-a3*c2*d1)/
    (a1*b2*c3-a1*b3*c2-a2*b1*c3+a2*b3*c1+a3*b1*c2-a3*b2*c1);
    z:=-(a1*b2*d3-a1*b3*d2-a2*b1*d3+a2*b3*d1+a3*b1*d2-a3*b2*d1)/
    (a1*b2*c3-a1*b3*c2-a2*b1*c3+a2*b3*c1+a3*b1*c2-a3*b2*c1);


    pcenter:=[x,y,z];
    radius:=sqrt((x1-x)*(x1-x)+(y1-y)*(y1-y)+(z1-z)*(z1-z));
ENDPROC

********************************

相机9点标定的补充--12点计算旋转中心

http://bbs.plcjs.com/forum.php?mod=viewthread&tid=500885&fromuid=1

(出处: PLC论坛-全力打造可编程控制器专业技术论坛)

#非标自动化#