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

大家好，欢迎来到IT知识分享网。

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.,-0.,-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.E-08,-0.,-0.,1.E-08],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pPickStd:=[[302,0,558],[5.E-08,-0.,-0.,1.E-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=&fromuid=1

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

#非标自动化#