|
1.上图为雷尼绍的Probe，很多场合又可以被称作对刀仪。
2.末端一般安装已知直径的红宝石球体。
3.当末端触碰到物体，会发出信号。离开物体，末端会回弹到原始位置。
4.机器人末端若安装Probe，则可以利用Probe做一定测量计算动作。
5.要使用Probe，需要先计算当前Probe末端球心所在TCP。
6.通常测定方法为，在空间固定位置安装一个精加工的球体。利用probe末端的红宝石球体和固定球体碰撞，来自动计算TCP
7.让机器人以一种姿态平移，从不同方向撞击固定球，并记录固定球时的位置。用这组相同姿态的法兰盘位姿，可以计算得到Probe和固定球心重合时的法兰盘位姿1（即用这些位姿算拟合球心，建议6-8组值）。
8.转动机器人，以不同姿态重复步骤7，得到新的位姿2,3,4。然后执行4点法计算TCP即可。
具体代码如下：
PERS num nBallR:=8;PERS num nProbeR:=8;CONSTrobtarget p100:=[[1430.147,2.592542E-06,1181.689],[0.01019623,4.284358E-09,-0.999948,6.299044E-09],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERSrobtarget p101:=[[1431.9,-0.021907,1144.4],[0.0101962,4.28436E-9,-0.999948,6.29904E-9],[0,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];PERSrobtarget p102:=[[1431.24,0.000558027,1151.94],[0.268654,2.50806E-9,-0.963237,7.19328E-9],[0,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];PERSrobtarget p103:=[[1431.33,0.0148201,1150.65],[0.0100413,0.00177056,-0.984757,0.173639],[0,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];PERSrobtarget p104:=[[1431.13,-0.0129624,1152.99],[0.0100413,-0.00177055,-0.984757,-0.173639],[0,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERSjointtarget j101:=[[-0.000387746,2.36321,17.7816,-9.12964E-6,71.0237,-0.00038518],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];PERSjointtarget j102:=[[2.10439E-5,7.29228,13.3917,1.01903E-5,100.484,1.98052E-5],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];PERSjointtarget j103:=[[2.52552,2.45328,17.9302,-20.8999,72.833,9.11657],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];PERSjointtarget j104:=[[-2.52581,2.41617,17.8582,20.8879,72.9351,-9.07611],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
perspos posSen1{6}:=[[1446.65,2.59254E-6,1154.28],PERS tooldata tResult:=[TRUE,[[-13.2306,-3.31139,95.6301],[1,0,0,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];VARnum maxErr;VARnum meanErr;
PROC testMove1() ! 使用一个理论TCP Tooldata_1，避免在调整姿态时撞到。MoveLp100,v1000,fine,Tooldata_1\WObj:=wobj0; ! p100 为probe距离固定球上方大概10mmsearchCenterp100,p101,diProbe,Tooldata_1; ! 记录这个姿态时，probe和固定球心重合时机器人末端位姿searchCenterreltool(p100,0,0,0\Ry:=20),p102,diProbe,Tooldata_1;searchCenterreltool(p100,0,0,0\Rx:=15),p103,diProbe,Tooldata_1;searchCenterreltool(p100,0,0,0\Rx:=-15),p104,diProbe,Tooldata_1;
j101:=CalcJointT(p101,Tooldata_1);j102:=CalcJointT(p102,Tooldata_1);j103:=CalcJointT(p103,Tooldata_1); j104:=CalcJointT(p104,Tooldata_1); MToolTCPCalib j101,j102,j103,j104,tResult,maxErr,meanErr;stop;ENDPROC
PROCsearchCenter(robtarget p,inout robtarget pCenter,var signaldi di1,inout tooldata tNor) VAR robtarget pRef;VARrobtarget p0;VARnum offset1; const num count:=7; !此处每个姿态使用7个位置，可以增加VARrobtarget pStart{count};VARrobtarget pSen{count};VARpos posSen{count};VARpos posCenter;VARnum radius;
offset1:=nBallR+nProbeR+20;search2p,offs(p,0,0,-30),p0,di1,v50,tNor;pRef:=offs(p0,0,0,-(nBallR+nProbeR)*2-2); ! 虚拟目标位置
FORi FROM 0 TO count-1 DOpStart{i+1}:=offs(p0,offset1*cos(i*360/count),offset1*sin(i*360/count),offset1-20);ENDFOR
FORi FROM 1 TO count DOsearch2pStart{i},pRef,pSen{i},di1,v50,tNor; posSen{i}:=pSen{i}.trans;ENDFOR
fitsphere posSen,posCenter,radius; ! 使用多点拟合球心pCenter:=p; pCenter.trans:=posCenter; ENDPROC
PROCsearch2(robtarget pStart,robtarget pEnd,inout robtarget pSen,var signaldi di1,speeddata v,inout tooldata t\inout wobjdata wobj) VAR speeddata v1:=[5,1,0,0];MoveLpStart,v,fine,t\WObj?wobj;SearchL\Stop,di1\PosFlank,pSen,pEnd,v,t\WObj?wobj;SearchL\Stop,di1\NegFlank,pSen,pstart,v1,t\WObj?wobj; ! 记录probe离开固定球时的位置，离开时速度尽量慢MoveLpStart,v,fine,t\WObj?wobj;ENDPROC
********************************
|
