相机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

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