联系我们
库卡机器人编程之3点法算Base原点
进行库卡机器人调试的时候，或多或少避免不了通过3点法来标定基座标(BASE)，具体步骤如下：
1. 在主菜单中选择 投入运行 > 测量 > 基坐标 > 3 点。
2. 为待测定的基坐标系选择一个号码并给定一个名称。
3. 选择已经测量过的工具编号。
4. 用 TCP 驶至新基座的原点。
5. 将TCP 移至新基座正向 X 轴上一个点。
6. 用 TCP 在 XY 平面接近带正 Y 值一点。
7. 在需要时，可让测量点的坐标和姿态以增量和角度显示（法兰坐标系为基准）。
8. 点击保存。
具体算法可以通过如下KRL程序实现：
DECL REAL P_X[3], P_XY[3], P_Y[3], P_Z[3], T[3,3], P1P2, P1P3
DECL INT I
P_X[1] = PT_IN_WORLD[2].X - PT_IN_WORLD[1].X
P_X[2] = PT_IN_WORLD[2].Y - PT_IN_WORLD[1].Y
P_X[3] = PT_IN_WORLD[2].Z - PT_IN_WORLD[1].Z
NORM_VECTOR (P_X[], 3)
P_XY[1] = PT_IN_WORLD[3].X - PT_IN_WORLD[1].X
P_XY[2] = PT_IN_WORLD[3].Y - PT_IN_WORLD[1].Y
P_XY[3] = PT_IN_WORLD[3].Z - PT_IN_WORLD[1].Z
NORM_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 3
T[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].X
NEW_BASE.Y = PT_IN_WORLD[1].Y
NEW_BASE.Z = PT_IN_WORLD[1].Z
RETURN (NEW_BASE)
ENDFCT
DEFFCT REAL ARCTAN2 (Y: IN, X: IN)
DECL REAL X, Y
DECL REAL ATAN_EPS
ATAN_EPS = 0.00011
IF ( (ABS(X) < ATAN_EPS) AND (ABS(Y) < ATAN_EPS) ) THEN ;check if x and y is numerically 0
RETURN (0)
ELSE
RETURN ( ATAN2(Y, X) )
ENDIF
ENDFCT
DEFFCT REAL ASIN (X: IN)
; Calculate the arcsine value
REAL X
RETURN ( 90 - ACOS(X) )
ENDFCT
DEFFCT REAL POD_PROD (V[]:IN,W[]:IN,N :IN)
DECL REAL V[],W[]
DECL INT N
DECL REAL SK_PROD
DECL INT I
SK_PROD=0
FOR I=1 TO N
SK_PROD=SK_PROD+V[I]*W[I]
ENDFOR
RETURN (SK_PROD)
ENDFCT
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
DEFFCT REAL VECTOR_LENGTH (V[]:IN, N :IN)
DECL REAL V[]
DECL INT N
RETURN (SQRT(POD_PROD(V[],V[],N)))
ENDFCT
DEF NORM_VECTOR (V[]:OUT,N :IN )
REAL V[]
INT N
REAL Length
INT I
Length=VECTOR_LENGTH(V[], N)
FOR I=1 TO N
V[I]=V[I]/Length
ENDFOR
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_C
COS_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_B
T[1,2] = -SIN_A*COS_C + COS_A*SIN_B*SIN_C
T[1,3] = SIN_A*SIN_C + COS_A*SIN_B*COS_C
T[2,1] = SIN_A*COS_B
T[2,2] = COS_A*COS_C + SIN_A*SIN_B*SIN_C
T[2,3] = -COS_A*SIN_C + SIN_A*SIN_B*COS_C
T[3,1] = -SIN_B
T[3,2] = COS_B*SIN_C
T[3,3] = COS_B*COS_C
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, C
REAL 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
DECL INT I
P_X[1] = PT_IN_WORLD[2].X - PT_IN_WORLD[1].X
P_X[2] = PT_IN_WORLD[2].Y - PT_IN_WORLD[1].Y
P_X[3] = PT_IN_WORLD[2].Z - PT_IN_WORLD[1].Z
NORM_VECTOR (P_X[], 3)
P_XY[1] = PT_IN_WORLD[3].X - PT_IN_WORLD[1].X
P_XY[2] = PT_IN_WORLD[3].Y - PT_IN_WORLD[1].Y
P_XY[3] = PT_IN_WORLD[3].Z - PT_IN_WORLD[1].Z
NORM_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 3
T[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].X
NEW_BASE.Y = PT_IN_WORLD[1].Y
NEW_BASE.Z = PT_IN_WORLD[1].Z
RETURN (NEW_BASE)
ENDFCT
DEFFCT REAL ARCTAN2 (Y: IN, X: IN)
DECL REAL X, Y
DECL REAL ATAN_EPS
ATAN_EPS = 0.00011
IF ( (ABS(X) < ATAN_EPS) AND (ABS(Y) < ATAN_EPS) ) THEN ;check if x and y is numerically 0
RETURN (0)
ELSE
RETURN ( ATAN2(Y, X) )
ENDIF
ENDFCT
DEFFCT REAL ASIN (X: IN)
; Calculate the arcsine value
REAL X
RETURN ( 90 - ACOS(X) )
ENDFCT
DEFFCT REAL POD_PROD (V[]:IN,W[]:IN,N :IN)
DECL REAL V[],W[]
DECL INT N
DECL REAL SK_PROD
DECL INT I
SK_PROD=0
FOR I=1 TO N
SK_PROD=SK_PROD+V[I]*W[I]
ENDFOR
RETURN (SK_PROD)
ENDFCT
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
DEFFCT REAL VECTOR_LENGTH (V[]:IN, N :IN)
DECL REAL V[]
DECL INT N
RETURN (SQRT(POD_PROD(V[],V[],N)))
ENDFCT
DEF NORM_VECTOR (V[]:OUT,N :IN )
REAL V[]
INT N
REAL Length
INT I
Length=VECTOR_LENGTH(V[], N)
FOR I=1 TO N
V[I]=V[I]/Length
ENDFOR
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_C
COS_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_B
T[1,2] = -SIN_A*COS_C + COS_A*SIN_B*SIN_C
T[1,3] = SIN_A*SIN_C + COS_A*SIN_B*COS_C
T[2,1] = SIN_A*COS_B
T[2,2] = COS_A*COS_C + SIN_A*SIN_B*SIN_C
T[2,3] = -COS_A*SIN_C + SIN_A*SIN_B*COS_C
T[3,1] = -SIN_B
T[3,2] = COS_B*SIN_C
T[3,3] = COS_B*COS_C
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, C
REAL 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
更多信息请咨询：库卡KUKA机器人
