很多老铁进行库卡机器人调试的时候，或多或少避免不了通过3点法来标定基座标(BASE)，具体步骤如下：

1. 在主菜单中选择 投入运行 > 测量 > 基坐标 > 3 点。

2. 为待测定的基坐标系选择一个号码并给定一个名称。

3. 选择已经测量过的工具编号。

4. 用 TCP 驶至新基座的原点。

5. 将TCP 移至新基座正向 X 轴上一个点。

6. 用 TCP 在 XY 平面接近带正 Y 值一点。

7. 在需要时，可让测量点的坐标和姿态以增量和角度显示（法兰坐标系为基准）。

8. 点击保存。

具体算法可以通过如下KRL程序实现：

DEFFCT FRAME Calculate_Base(PT_IN_WORLD[]:IN );FOLD DESCRIPTION; ==========================================================================================================; DESCRIPTION :NULL; CUSTOMER : Standard;-----------------------------------------------------------------------------------------------------------; Software Release-KUKA KR C4 V8.3.21;-----------------------------------------------------------------------------------------------------------; AUTHOR : 24.12.2016 Andrew wang; Copyright @ KUKA Robotics China Co.,Ltd.; FUNCTION: Calculate the new base data via 3 random points in the WCS; Comment: Calculate euler angles and matrix;===========================================================================================================;ENDFOLD (DESCRIPTION)DECL FRAME PT_IN_WORLD[]DECL FRAME NEW_BASEDECL REAL P_X[3], P_XY[3], P_Y[3], P_Z[3], T[3,3], P1P2, P1P3DECL INT IP_X[1] = PT_IN_WORLD[2].X - PT_IN_WORLD[1].XP_X[2] = PT_IN_WORLD[2].Y - PT_IN_WORLD[1].YP_X[3] = PT_IN_WORLD[2].Z - PT_IN_WORLD[1].ZNORM_VECTOR (P_X[], 3)
P_XY[1] = PT_IN_WORLD[3].X - PT_IN_WORLD[1].XP_XY[2] = PT_IN_WORLD[3].Y - PT_IN_WORLD[1].YP_XY[3] = PT_IN_WORLD[3].Z - PT_IN_WORLD[1].ZNORM_VECTOR (P_XY[], 3)
CROSS_PROD (P_X[], P_XY[], P_Z[])NORM_VECTOR (P_Z[], 3)CROSS_PROD (P_Z[], P_X[], P_Y[])
FOR I=1 TO 3T[I,1] = P_X[I]T[I,2] = P_Y[I]T[I,3] = P_Z[I]ENDFOR
MATRIX_TO_RPY (T[,], NEW_BASE.A, NEW_BASE.B, NEW_BASE.C)
NEW_BASE.X = PT_IN_WORLD[1].XNEW_BASE.Y = PT_IN_WORLD[1].YNEW_BASE.Z = PT_IN_WORLD[1].Z
RETURN (NEW_BASE)
ENDFCT; ------------------- Library - Function: ----------------------------DEFFCT  REAL ARCTAN2 (Y: IN, X: IN)DECL REAL       X, YDECL REAL ATAN_EPSATAN_EPS = 0.00011IF  (  (ABS(X) < ATAN_EPS)  AND  (ABS(Y) < ATAN_EPS)  )  THEN  ;check if x and y is numerically 0RETURN (0)ELSERETURN ( ATAN2(Y, X) )ENDIFENDFCT; ------------------- Library - Function: ----------------------------DEFFCT REAL ASIN (X: IN); Calculate the arcsine valueREAL   XRETURN ( 90 - ACOS(X) )ENDFCT; ------------------- Library - Function: ----------------------------DEFFCT  REAL POD_PROD (V[]:IN,W[]:IN,N :IN)DECL REAL V[],W[]DECL INT NDECL REAL SK_PRODDECL INT ISK_PROD=0FOR I=1 TO NSK_PROD=SK_PROD+V[I]*W[I]ENDFORRETURN  (SK_PROD)ENDFCT; ------------------- Library - Function: ----------------------------DEF  CROSS_PROD (U[]:IN, V[]:IN, W[]:OUT)DECL REAL U[], V[], W[]W[1] = U[2]*V[3] - U[3]*V[2]W[2] = U[3]*V[1] - U[1]*V[3]W[3] = U[1]*V[2] - U[2]*V[1]END; ------------------- Library - Function: ----------------------------DEFFCT  REAL VECTOR_LENGTH (V[]:IN, N :IN)DECL REAL V[]DECL INT NRETURN  (SQRT(POD_PROD(V[],V[],N)))ENDFCT; ------------------- Library - Function: ----------------------------DEF  NORM_VECTOR (V[]:OUT,N :IN )REAL V[]INT NREAL LengthINT ILength=VECTOR_LENGTH(V[], N)FOR I=1 TO  NV[I]=V[I]/LengthENDFOREND; ------------------- Library - Function: ----------------------------DEF  RPY_TO_MATRIX (A :IN,B :IN,C :IN ,T[,]:OUT);Transform from RPY-angle A,BC to Trafo_Matrix T; T = Rot_z(A) * Rot_y(B) * Rot_x(C)DECL REAL T[,], A, B, C DECL REAL COS_A, SIN_A, COS_B, SIN_B, COS_C, SIN_CCOS_A=COS(A)SIN_A=SIN(A)COS_B=COS(B)SIN_B=SIN(B)COS_C=COS(C)SIN_C=SIN(C)
T[1,1] = COS_A*COS_BT[1,2] = -SIN_A*COS_C + COS_A*SIN_B*SIN_CT[1,3] = SIN_A*SIN_C + COS_A*SIN_B*COS_C
T[2,1] = SIN_A*COS_BT[2,2] = COS_A*COS_C + SIN_A*SIN_B*SIN_CT[2,3] = -COS_A*SIN_C + SIN_A*SIN_B*COS_C
T[3,1] = -SIN_BT[3,2] = COS_B*SIN_CT[3,3] = COS_B*COS_C
END; ------------------- Library - Function: ----------------------------DEF  MATRIX_TO_RPY (T[,]:IN, A:OUT, B:OUT, C:OUT); Transform Trafo-Matrix to RPY-Angle A, B, C; T = Rot_z(A) * Rot_y(B) * Rot_x(C)REAL     T[,], A, B, CREAL     SIN_A, COS_A, SIN_B, ABS_COS_B, SIN_C, COS_C
A = ARCTAN2 (T[2,1], T[1,1])SIN_A = SIN(A)COS_A = COS(A)
SIN_B = -T[3,1]ABS_COS_B = COS_A*T[1,1] + SIN_A*T[2,1]B = ARCTAN2 (SIN_B, ABS_COS_B); Value: -90 <= B <= +90 !!
SIN_C = SIN_A*T[1,3] - COS_A*T[2,3]COS_C = -SIN_A*T[1,2] + COS_A*T[2,2]C = ARCTAN2 (SIN_C, COS_C)END

--end--

声明：本文章由网友投稿作为教育分享用途，如有侵权原作者可通过邮件及时和我们联系删除：freemanzk@qq.com