|
video: https://mp.weixin.qq.com/mp/readtemplate?t=pages/video_player_tmpl&action=mpvideo&auto=0&vid=wxv_3671948884349206535
利用单个2D相机，也可以完成机器人TCP的自动标定。
参考2D相机12点标定RAPID完整实现一文，机器人与相机完成在机器人机器人xy平面的标定。此时假设有一个下图的预设TCP，则可以利用2D相机，将预设TCP调整到下图的xy平面内修正过的TCP位置（因为2D相机无法返回高度）。
此时可以让机器人以xy平面内修正后的TCP绕着机器人的y轴旋转θ（如以下图的O点），则实际TCP会从A点转动到B点。记录A点和B点的相机拍摄数据，讲转化后的两次坐标的x相减（下图的GB距离），由于OA=OB，OB =(x1-x2)/sin(θ),可以得到OA。OA即为需要调整的TCP 在xz平面的高度值。
PERStooldata MyTool:=[TRUE,[[31.792631019,0,229.638935148],[0.945518576,0,0.325568154,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];! myTool为理论值PERS tooldata tNor:=[TRUE,[[31.7847,-1.42108E-14,229.651],[0.945519,0,0.325568,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];
PROCrCalib() tNor:=mytool; tNor.tframe.trans:=[30,2,200]; ! 给定一个参考的不正确的TCP ! 已经完成了12点标定，机器人走回5号点MoveLp,v100,fine,tool0; CalibCal p,tNor;ENDPROC
PROC CalibCal(robtarget pRef0,inout tooldata t) VAR point realPoint; VAR pose pTool0;VARpose poseNor; VAR pose poseResult;VARrobtarget pCurr; pTool0:=[pRef0.trans,pRef0.rot];poseNor:=PoseMult(pTool0,t.tframe); ! 计算参考TCP点的绝对位姿realPoint :=CamGetData(); ! 拍照获取此时针尖的绝对xy位置poseNor.trans.x:=realPoint.x;poseNor.trans.y:=realPoint.y; ! 利用相机修正参考TCP点的xy绝对位姿
poseResult:=PoseMult(PoseInv(pTool0),poseNor);t.tframe.trans:=poseResult.trans; ! 在xy平面内修正TCP pCurr:=CRobT(\Tool:=t); ! 获取基于修正后TCP的末端位姿（TCP末端在xy平面正确，xz平面不正确） calib_ry:=5; !绕XY平面内修正后的TCP旋转5°CalibInZpCurr,calib_ry,t;ENDPROC
PROCCalibInZ(robtarget p,num angle,inout tooldata t) ! 以传入工具t（即在XY平面内修正过的）对应的点位p进行旋转和计算VARrobtarget ptmp;VARnum x1;VARnum x2;VARnum dz;VARrobtarget p0;VARrobtarget pTnor;VARpose p1;VARpose p2;VARpose p3;VARpoint realPoint;
realPoint:=CamGetData();x1:=realPoint.x;p0:=CRobT(\Tool:=tool0);pTnor:=CRobT(\Tool:=t);
ptmp:=rotWobj(p,0,angle,0);MoveLptmp,v100,fine,t\WObj:=wobj0; ! 以XY平面内修正后的TCP绕着y方向旋转5°realPoint:=CamGetData(); ! 获取旋转后针尖的位置x2:=realPoint.x;dz:=(x1-x2)/sin(angle); pTnor.trans.z:=pTnor.trans.z-dz; ! 修正旋转前的针尖z到实际位置p1:=[p0.trans,p0.rot];p2:=[pTnor.trans,pTnor.rot];p3:=posemult(poseinv(p1),p2); t.tframe.trans:=p3.trans;ENDPROC
********************************
|
