|
ABB机器人支持基于线激光的焊接跟踪。在使用功能前，需要使用特殊ABB的标定板对线激光进行标定（即确定线激光的坐标系原点和机器人法兰盘的关系）。 现在通常线激光厂家已经提供了快速标定方法（无需专用板）。 本文介绍快速标定方法及在机器人侧标定方法的实现。
1. 找两块板材搭接放置，并确认线激光能直接识别焊缝上高点 2. 在两块板上画一根线，并让机器人用一个准确的TCP，记录两块搭接板高点的位置（pRef） 3. 平移机器人，使得大概焊缝在激光上的位置处于1/3，记录此时机器人tool0的位置。平移过程，线激光与划线对齐。
4. 平移机器人，使得大概焊缝在激光上的2/3位置。记录机器人tool0位置 5. 机器人抬高约80mm，使得大概焊缝在激光上的位置处于2/3。记录机器人tool0位置6. 使用以下代码，可以计算出线激光坐标系原点相对于机器人tool0的关系（pose）。后期使用时，只需要获取当前机器人tool0位姿，乘标定结果再乘线激光数据，就能获取到焊缝在机器人base下的绝对位置。 p:=crobt(\tool:=tool0);pobj = p * poseCalib *sensor
假设要标定的法兰盘到线激光原点的矩阵如下 则 （2）其中， 为机器人法兰盘位姿，lylz为线激光返回数据，X'Y'Z'为固定特征点的绝对位置。上式中，X1Y1Z1为对应tool0位姿求逆（ ） * X'Y'Z'（固定特征点）的值，ly1lz1为对应线激光数据。线激光与机器人标定实现代码如下： PERS pos linSensorData{3}:=[[0,-89.2848,99.9993], [0,42.6023,45.5392], [0,61.998,191.751]];!此处线激光返回的y和z值 PERS num linSensorY:=-0.00751305;PERS num linSensorZ:=0.172109;
CONST robtarget p3001:=[[758.7522,141.3208,918.8728],[0.4999992,4.906513E-08,0.8660259,-2.435461E-08],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtarget p3002:=[[779.1534,9.433696,868.3784],[0.499999,-4.908812E-09,0.866026,-8.041348E-10],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtarget p3003:=[[724.3817,-9.961946,1003.944],[0.4999988,-1.169715E-08,0.8660261,-3.453761E-09],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];PERS robtarget pArrLaser{3}:=[[[758.752,141.321,918.873],[0.499999,4.90651E-8,0.866026,-2.43546E-8],[0,-1,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]], [[779.153,9.4337,868.378],[0.499999,-4.90881E-9,0.866026,-8.04135E-10],[0,-1,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]], [[724.382,-9.96195,1003.94],[0.499999,-1.16971E-8,0.866026,-3.45376E-9],[-1,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]]];CONST robtarget pFix:=[[979.19,52.04,683.80],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];PERS pose TCP2:=[[31.7939,0.00403287,229.641],[0.945519,1.27586E-7,0.325568,-5.65465E-7]]; ! 标定结果TCP2PROC main() ! 划线高点绝对位姿 MoveL pFix,v1000,fine,myTool\WObj:=wobj0; ! 线激光对准线时的三个机器人tool0位姿 Movel p3001,v1000,fine,tool0\WObj:=wobj0; Movel p3002,v1000,fine,tool0\WObj:=wobj0; Movel p3003,v1000,fine,tool0\WObj:=wobj0; pArrLaser{1}:=p3001; pArrLaser{2}:=p3002; pArrLaser{3}:=p3003; rlinLaserCalib pfix,pArrLaser,linSensorData,TCP2;ENDPROC
PROC rlinLaserCalib(robtarget pRef,robtarget pLaser{*},pos Sensor{*},inout pose calibR)
! pLaser * calibR * Sensor[0,y,z] = pRef.trans ! calibR * Sensor = pLaser-1 *pRef.trans ! [ r1.x r2.x r3.x tx [Sensor.x=0 ! r1.y r2.y r3.y ty * Sensor.y = pLaser-1 *pRef.trans ! r1.z r2.z. r3.z tz Sensor.z ! 0 0 0 1] 1 ]
! [ r2.x r3.x tx [sensor1.y sensor2.y sensor3.y ! r2.y r3.y ty * Sensor1.z sensor2.z sensor3.z = pLaser-1 *pRef.trans ! r2.z. r3.z tz ] 1 1 1 ]
VAR pos posArr1{3}; VAR num matA{3,3}; VAR num matB{3,3}; VAR dnum matADnum{3,3}; VAR dnum matBDnum{3,3}; VAR dnum matCDnum{3,3}; VAR num mag;
VAR pos r1; VAR pos r2; VAR pos r3; var num matResult{9};
FOR i FROM 1 TO 3 DO posArr1{i}:=PoseVect(PoseInv([pLaser{i}.trans,pLaser{i}.rot]),pRef.trans); ENDFOR
matA{1,1}:=Sensor{1}.y; matA{1,2}:=Sensor{2}.y; matA{1,3}:=Sensor{3}.y; matA{2,1}:=Sensor{1}.z; matA{2,2}:=Sensor{2}.z; matA{2,3}:=Sensor{3}.z;
matA{3,1}:=1; matA{3,2}:=1; matA{3,3}:=1;
matB{1,1}:=posArr1{1}.x; matB{1,2}:=posArr1{2}.x; matB{1,3}:=posArr1{3}.x;
matB{2,1}:=posArr1{1}.y; matB{2,2}:=posArr1{2}.y; matB{2,3}:=posArr1{3}.y;
matB{3,1}:=posArr1{1}.z; matB{3,2}:=posArr1{2}.z; matB{3,3}:=posArr1{3}.z; FOR i FROM 1 TO 3 DO FOR j FROM 1 TO 3 DO matADnum{i,j}:=NumToDnum(matA{i,j}); matBDnum{i,j}:=NumToDnum(matB{i,j}); ENDFOR ENDFOR
MatrixInverse matADnum,matCDnum; MatrixMult matBDnum,matCDnum,matCDnum;
r2.x:=DnumToNum(matCDnum{1,1}); r2.y:=DnumToNum(matCDnum{2,1}); r2.z:=DnumToNum(matCDnum{3,1}); r3.x:=DnumToNum(matCDnum{1,2}); r3.y:=DnumToNum(matCDnum{2,2}); r3.z:=DnumToNum(matCDnum{3,2});
calibR.trans.x:=DnumToNum(matCDnum{1,3}); calibR.trans.y:=DnumToNum(matCDnum{2,3}); calibR.trans.z:=DnumToNum(matCDnum{3,3});
mag:=VectMagn(r2); r2.x:=r2.x/mag; r2.y:=r2.y/mag; r2.z:=r2.z/mag;
mag:=VectMagn(r3); r3.x:=r3.x/mag; r3.y:=r3.y/mag; r3.z:=r3.z/mag;
r1:=CrossProd(r2,r3); mag:=VectMagn(r1); r1.x:=r1.x/mag; r1.y:=r1.y/mag; r1.z:=r1.z/mag;
matResult{1}:=r1.x; matResult{2}:=r2.x; matResult{3}:=r3.x; matResult{4}:=r1.y; matResult{5}:=r2.y; matResult{6}:=r3.y; matResult{7}:=r1.z; matResult{8}:=r2.z; matResult{9}:=r3.z; RotationMatrixToQuaternion matResult,calibR.rot;ENDPROC
PROC RotationMatrixToQuaternion(NUM MAT{*},inout orient q) VAR num trace; VAR num m00; VAR num m01; VAR num m02; VAR num m10; VAR num m11; VAR num m12; VAR num m20; VAR num m21; VAR num m22; VAR num t0; VAR num t1; VAR num t2; VAR num t3; VAR num s;
m00:=MAT{1}; m01:=MAT{2}; m02:=MAT{3}; m10:=MAT{4}; m11:=MAT{5}; m12:=MAT{6}; m20:=MAT{7}; m21:=MAT{8}; m22:=MAT{9};
trace:=m00+m11+m22;
IF trace>0 THEN s:=0.5/SQRT(trace+1.0); q.q1:=0.25/s; q.q2:=(m21-m12)*s; q.q3:=(m02-m20)*s; q.q4:=(m10-m01)*s; ELSEIF m00>m11 AND m00>m22 THEN s:=2.0*SQRT(1.0+m00-m11-m22); q.q1:=(m21-m12)/s; q.q2:=0.25*s; q.q3:=(m01+m10)/s; q.q4:=(m02+m20)/s; ELSEIF m11>m22 THEN s:=2.0*SQRT(1.0+m11-m00-m22); q.q1:=(m02-m20)/s; q.q2:=(m01+m10)/s; q.q3:=0.25*s; q.q4:=(m12+m21)/s; ELSE s:=2.0*SQRT(1.0+m22-m00-m11); q.q1:=(m10-m01)/s; q.q2:=(m02+m20)/s; q.q3:=(m12+m21)/s; q.q4:=0.25*s; ENDIFENDPROC
********************************
|
