1. 在使用2D相机进行引导时,如果已知机器人的TCP或者标定的针尖相对机器人法兰盘没有偏移,则可以使用9点标定(视觉9点标定原理及RAPID计算)。若TCP位置且机器人上特征相对法兰盘有偏心,则需要使用12点标定(9点平移+3点旋转)进行标定。 2. 此类功能在Hik等视觉软件中已经集成,也可以在机器人侧实现标定计算(相机仅给出特征的像素坐标) 3. 假设相机固定安装底拍(顶拍相同),机器人末端有一特征。机器人按照文首的轨迹(使用tool0)进行平移和旋转(前9个点平移,第10个点为移动到中间点5后绕法兰盘tool0的位置旋转-θ°,第11个点与第5点相同,第12个点为移动到中间点5后绕法兰盘tool0的位置旋转θ°) PERS num cam_x:=0.00189328; PERS num cam_y:=3E-9; PERS num count:=1; PERS point testPoint:=[705.005,-7.10543E-15];
PERS num calib_x:=10; ! 标定移动时的x距离 PERS num calib_y:=10; ! 标定移动时的y距离 PERS num calib_rz:=5; ! 标定移动时的绕tool0旋转的角度
PROC main() MoveJ p0,v1000,fine,tool0\WObj:=wobj0;
count:=1; rCalib p0,calib_x,calib_y,calib_rz; stop; rCam2; calNewPoint [cam_x,cam_y],testPoint;
ENDPROC
PROC rCalib(robtarget p,num x,num y,num angle) MoveL offs(p,-x,-y,0),v100,fine,tool0; rCam; MoveL offs(p,0,-y,0),v100,fine,tool0; rCam; MoveL offs(p,x,-y,0),v100,fine,tool0; rCam; MoveL offs(p,x,0,0),v100,fine,tool0; rCam; MoveL offs(p,0,0,0),v100,fine,tool0; rCam; MoveL offs(p,-x,0,0),v100,fine,tool0; rCam; MoveL offs(p,-x,y,0),v100,fine,tool0; rCam; MoveL offs(p,0,y,0),v100,fine,tool0; rCam; MoveL offs(p,x,y,0),v100,fine,tool0; rCam; MoveL offs(p,0,0,0),v100,fine,tool0;
MoveL rotWobj(p,0,0,-angle),v100,fine,tool0; rCam; MoveL rotWobj(p,0,0,0),v100,fine,tool0; rCam; MoveL rotWobj(p,0,0,angle),v100,fine,tool0; rCam; calibCal; ENDProc
PROC calibCal() VAR pos posCir{3}; VAR point realPoint; VAR pos posCenter; VAR num r; VAR pos n; calculateAffineMatrix\PointNum:=9,img_points,real_points,Affine;
calNewPoint img_points{10},realPoint; posCir{1}.x:=realPoint.x; posCir{1}.y:=realPoint.y;
calNewPoint img_points{11},realPoint; posCir{2}.x:=realPoint.x; posCir{2}.y:=realPoint.y;
calNewPoint img_points{12},realPoint; posCir{3}.x:=realPoint.x; posCir{3}.y:=realPoint.y;
fitcircle posCir,posCenter,r,n; Affine{3}:=Affine{3}+(posCir{2}.x-posCenter.x); Affine{6}:=Affine{6}+(posCir{2}.y-posCenter.y);
ENDPROC
FUNCrobtarget rotWobj(robtarget p,num rx,num ry,num rz) VAR orient o; o:=p.rot; o:=orientzyx(rz,ry,rx)*o; p.rot:=o; RETURN p; ENDFUNC
PROC rCam2() waittime 0.5; PulseDO do_cam; waittime 0.5;
ENDPROC
PROC rCam() VAR robtarget p; rCam2; p:=CRobT(); img_points{count}.x:=cam_x; img_points{count}.y:=cam_y; real_points{count}.x:=p.trans.x; real_points{count}.y:=p.trans.y; incr count; ENDPROC
MODULE affTrans RECORD point num x; num y; ENDRECORD PERS point img_points{12}:=[[-0.00667989,-0.01], [0.00332009,-0.01], [0.0133201,-0.01], [0.0133201,0], [0.00332009,0], [-0.0066799,0], [-0.00667989,0.01], [0.00332009,0.01], [0.0133201,0.01],[0.00262382,-0.0159475], [0.00332009,0], [0.00262382,0.0159475]]; PERS point real_points{12}:=[[512.014,-10], [522.014,-10],[532.014,-10], [532.014,-2.15548E-14],[522.014,-2.11713E-14],[532.014,10], [522.014,-7.80626E-15], [522.014,-1.86001E-14],[522.014,-3.98986E-14]];
PERS num Affine{6}:=[999.998,-1.16251E-13,701.685,5.28272E-15,1000,-1.42284E-14];
PERS point NewPointReal:=[683.18,0]; PERS point NewPointImg:=[250,250];
PROC calNewPoint(point img_points,INOUT point real_points) real_points.x:=Affine{1}*img_points.x+Affine{2}*img_points.y+Affine{3}; real_points.y:=Affine{4}*img_points.x+Affine{5}*img_points.y+Affine{6}; ENDPROC
PROC calculateAffineMatrix(\num PointNum,point img_points{*},point real_points{*},INOUT num affine_matrix{*}\INOUT pos affine_pos) VAR dnum A{20,6}; VAR dnum b{20}; VAR dnum x{6}; VAR num A_m;
A_m:=Dim(img_points,1)*2; IF Present(PointNum) THEN A_m:=PointNum*2; ENDIF
FOR i FROM 1 TO A_m/2 DO A{2*i-1,1}:=NumToDnum(img_points{i}.x); A{2*i-1,2}:=NumToDnum(img_points{i}.y); A{2*i-1,3}:=1; A{2*i-1,4}:=0; A{2*i-1,5}:=0; A{2*i-1,6}:=0;
A{2*i,1}:=0; A{2*i,2}:=0; A{2*i,3}:=0; A{2*i,4}:=NumToDnum(img_points{i}.x); A{2*i,5}:=NumToDnum(img_points{i}.y); A{2*i,6}:=1;
b{2*i-1}:=NumToDnum(real_points{i}.x); b{2*i}:=NumToDnum(real_points{i}.y); ENDFOR
MatrixSolve A\A_m:=A_m,b,x; FOR i FROM 1 TO 6 DO affine_matrix{i}:=DnumToNum(x{i}); ENDFOR ENDPROC ENDMODULE
********************************
|