Cinematica directa scara
34
Robtica I CinemÆtica Directa Robot Scara Jorge Enrique Lavn Delgado Universidad La Salle Jueves 04 de Octubre de 2012 Jorge E. Lavn Delgado (ULSA) CinemÆtica Directa Robot Scara 04/Octubre/2012 1 / 34
Transcript of Cinematica directa scara
Robótica ICinemática Directa Robot Scara
Jorge Enrique Lavín Delgado
Universidad La Salle
Jueves 04 de Octubre de 2012
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 1 / 34
Teoría de Tornillos
0z
0x
0y
0o
4z
4x
4y
4o4l
4θ
3d
2θ
2l
1θ
1l
La pose del efector �nal está dada por:
gn0 (θ) = eZ1θ1eZ2θ2 � � � eZnθngn0 (0)
g40 (θ) = eZ1θ1eZ2θ2eZ3d3eZ4θ4g40 (0) (1)
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 2 / 34
Con�guración inicial
0z
0x
0y
0o
4z
4x
4y
4o4l
4θ
3d
2θ
2l
1θ
1l
Con�guración (pose) inicial:
g40 (0) =�R40 (0) d40 (0)0T 1
�=
26641 0 0 l1 + l20 �1 0 00 0 �1 �l40 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 3 / 34
Ejes de rotación y traslación
0z
0x
0y
0o
4θ
3d
2θ1θ
1k 2k
3k
4k
Ejes de rotación y/o traslación (ki ) de las articulaciones:
k1 =�0 0 1
�T k3 =�0 0 �1
�Tk2 =
�0 0 1
�T k4 =�0 0 �1
�TJorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 4 / 34
Puntos sobre los ejes de rotación
0z
0x
0y
0o
4θ
3d
2θ
2l
1θ
1l
1k 2k
4k
1q2q 4q
Puntos �arbitrarios�(qi ) sobre los ejes de rotación:
q1 =�0 0 0
�T ����q2 =
�l1 0 0
�T q4 =�l1 + l2 0 0
�TJorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 5 / 34
Matrices exponenciales
4θ
3d
2θ1θ
1 1Z θe 2 2Z θe
4 4Z θe
3 3Z de
g40 (θ) = eZ1θ1eZ2θ2eZ3d3eZ4θ4g40 (0)
Matrices exponenciales:
Articulación prismática Articulación rotacional
eZidi =�I diki0T 1
�eZi θi =
"eKi θi
�I� eKi θi
�qi
0T 1
#Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 6 / 34
Matrices exponenciales
k1 =�0 0 1
�T k2 =�0 0 1
�T k4 =�0 0 �1
�TMatrices de rotación:
eKi θi =
24 k2x vθi + Cθi kxky vθi � kzSθi kxkzvθi + kySθi
kxky vθi + kzSθi k2y vθi + Cθi kykzvθi � kxSθi
kxkzvθi � kySθi kykzvθi + kxSθi k2z vθi + Cθi
35vθi = 1� Cθi
eK1θ1 =
24 Cθ1 �Sθ1 0Sθ1 Cθ1 00 0 1
35 eK2θ2 =
24 Cθ2 �Sθ2 0Sθ2 Cθ2 00 0 1
35eK4θ4 =
24 Cθ4 Sθ4 0�Sθ4 Cθ4 00 0 1
35Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 7 / 34
Matrices exponenciales
Articulación 1 (Rotacional):
eZi θi =
"eKi θi
�I� eKi θi
�qi
0T 1
#
eK1θ1 =
24 Cθ1 �Sθ1 0Sθ1 Cθ1 00 0 1
35 q1 =�0 0 0
�T�I� eK1θ1
�q1 =
24 1� Cθ1 Sθ1 0�Sθ1 1� Cθ1 00 0 0
3524 000
35 =24 000
35
) eZ1θ1 =
2664Cθ1 �Sθ1 0 0Sθ1 Cθ1 0 00 0 1 00 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 8 / 34
Matrices exponenciales
Articulación 2 (Rotacional):
eZi θi =
"eKi θi
�I� eKi θi
�qi
0T 1
#
eK2θ2 =
24 Cθ2 �Sθ2 0Sθ2 Cθ2 00 0 1
35 q2 =�l1 0 0
�T�I� eK2θ2
�q2 =
24 1� Cθ2 Sθ2 0�Sθ2 1� Cθ2 00 0 0
3524 l100
35 =24 l1 (1� Cθ2)
�l1Sθ2
0
35
) eZ2θ2 =
2664Cθ2 �Sθ2 0 l1 (1� Cθ2)Sθ2 Cθ2 0 �l1Sθ2
0 0 1 00 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 9 / 34
Matrices exponenciales
Articulación 3 (Prismática):
eZidi =�I diki0T 1
�k3 =
�0 0 �1
�T
d3k3 = d3
24 00�1
35 =24 0
0�d3
35
) eZ3d3 =
26641 0 0 00 1 0 00 0 1 �d30 0 0 1
3775
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 10 / 34
Matrices exponenciales
Articulación 4 (Rotacional):
eZi θi =
"eKi θi
�I� eKi θi
�qi
0T 1
#
eK4θ4 =
24 Cθ4 Sθ4 0�Sθ4 Cθ4 00 0 1
35 q4 =�l1 + l2 0 0
�T�I� eK4θ4
�q4 =
24 (l1 + l2) (1� Cθ4)(l1 + l2) Sθ4
0
35
) eZ4θ4 =
2664Cθ4 Sθ4 0 (l1 + l2) (1� Cθ4)�Sθ4 Cθ4 0 (l1 + l2) Sθ4
0 0 1 00 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 11 / 34
Pose del efector �nal
La pose del efector �nal se obtiene con la ecuación (1):
g40 (θ) = eZ1θ1eZ2θ2eZ3d3eZ4θ4g40 (0)
=
2664Cθ1 �Sθ1 0 0Sθ1 Cθ1 0 00 0 1 00 0 0 1
37752664Cθ2 �Sθ2 0 l1 (1� Cθ2)Sθ2 Cθ2 0 �l1Sθ2
0 0 1 00 0 0 1
377526641 0 0 00 1 0 00 0 1 �d30 0 0 1
37752664
Cθ4 Sθ4 0 (l1 + l2) (1� Cθ4)�Sθ4 Cθ4 0 (l1 + l2) Sθ4
0 0 1 00 0 0 1
377526641 0 0 l1 + l20 �1 0 00 0 �1 �l40 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 12 / 34
Pose del efector �nal
La pose del efector �nal se obtiene con la ecuación (1):
g40 (θ) = eZ1θ1eZ2θ2eZ3d3eZ4θ4g40 (0)
eZ1θ1eZ2θ2 =
2664Cθ1 �Sθ1 0 0Sθ1 Cθ1 0 00 0 1 00 0 0 1
37752664Cθ2 �Sθ2 0 l1 (1� Cθ2)Sθ2 Cθ2 0 �l1Sθ2
0 0 1 00 0 0 1
3775
=
2664Cθ1+θ2 �Sθ1+θ2 0 l1 (Cθ1 � Cθ1+θ2)Sθ1+θ2 Cθ1+θ2 0 l1 (Sθ1 � Sθ1+θ2)0 0 1 00 0 0 1
3775sen (x � y) = sen x cos y � cos x sen ycos (x � y) = cos x cos y � sen x sen y
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 13 / 34
Pose del efector �nal
La pose del efector �nal se obtiene con la ecuación (1):
g40 (θ) = eZ1θ1eZ2θ2eZ3d3eZ4θ4g40 (0)
eZ3d3eZ4θ4
=
26641 0 0 00 1 0 00 0 1 �d30 0 0 1
37752664
Cθ4 Sθ4 0 (l1 + l2) (1� Cθ4)�Sθ4 Cθ4 0 (l1 + l2) Sθ4
0 0 1 00 0 0 1
3775
=
2664Cθ4 Sθ4 0 (l1 + l2) (1� Cθ4)�Sθ4 Cθ4 0 (l1 + l2) Sθ4
0 0 1 �d30 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 14 / 34
Pose del efector �nal
La pose del efector �nal se obtiene con la ecuación (1):
g40 (θ) = eZ1θ1eZ2θ2eZ3d3eZ4θ4g40 (0)
eZ3d3eZ4θ4g40 (0)
=
2664Cθ4 Sθ4 0 (l1 + l2) (1� Cθ4)�Sθ4 Cθ4 0 (l1 + l2) Sθ4
0 0 1 �d30 0 0 1
377526641 0 0 l1 + l20 �1 0 00 0 �1 �l40 0 0 1
3775
=
2664Cθ4 �Sθ4 0 l1 + l2�Sθ4 �Cθ4 0 00 0 �1 � (d3 + l4)0 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 15 / 34
Pose del efector �nal
La pose del efector �nal se obtiene con la ecuación (1):
g40 (θ) = eZ1θ1eZ2θ2eZ3d3eZ4θ4g40 (0)
=
2664Cθ1+θ2 �Sθ1+θ2 0 l1 (Cθ1 � Cθ1+θ2)Sθ1+θ2 Cθ1+θ2 0 l1 (Sθ1 � Sθ1+θ2)0 0 1 00 0 0 1
3775 � � �
� � �
2664Cθ4 �Sθ4 0 l1 + l2�Sθ4 �Cθ4 0 00 0 �1 � (d3 + l4)0 0 0 1
3775
=
2664r11 r12 r13 dxr21 r22 r23 dyr31 r32 r33 dz0 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 16 / 34
Pose del efector �nal
Finalmente, la pose del efector �nal está dada por:
g40 (θ) =
2664r11 r12 r13 dxr21 r22 r23 dyr31 r32 r33 dz0 0 0 1
3775donde
r11 = Cθ1+θ2Cθ4 + Sθ1+θ2Sθ4 r13 = 0r21 = Sθ1+θ2Cθ4 � Cθ1+θ2Sθ4 r23 = 0r31 = 0 r33 = �1r12 = �Cθ1+θ2Sθ4 + Sθ1+θ2Cθ4 dx = l1Cθ1 + l2Cθ1+θ2
r22 = �Sθ1+θ2Sθ4 � Cθ1+θ2Cθ4 dy = l1Sθ1 + l2Sθ1+θ2
r32 = 0 dz = � (d3 + l4)
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 17 / 34
Denavit Hartenberg
Diagrama cinemático
4l4θ
3d
2θ
2l
1θ
1l
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 18 / 34
Asignación de referenciales
4l4θ
3d
2θ
2l1l
0z
0x
0y
0o
1θ
1x
1y1z
1o 2x
2y2z
2o
3x
3y3z
3o
4x
4y4z
4o
DH1.- El eje xi debe ser perpendicular al eje zi�1.DH2.- El eje xi debe intersecar al eje zi�1.
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 19 / 34
Tabla de parámetros
1l
0z
0x
0y
0o
1θ
1x
1y1z
1o
i θi di ai αi1 θ1 0 l1 0�
θi - ángulo entre xi�1 y xi medido sobre zi�1di - distancia de oi�1 a xi medida a lo largo de zi�1ai - distancia de zi�1 a oi medida a lo largo de xiαi - ángulo entre zi�1 y zi medido sobre xi
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 20 / 34
Tabla de parámetros
2θ
2l
1x
1y1z
1o 2x
2y2z
2oi θi di ai αi2 θ2 0 l2 180�
θi - ángulo entre xi�1 y xi medido sobre zi�1di - distancia de oi�1 a xi medida a lo largo de zi�1ai - distancia de zi�1 a oi medida a lo largo de xiαi - ángulo entre zi�1 y zi medido sobre xi
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 21 / 34
Tabla de parámetros
3d
2x
2y2z
2o
3x
3y3z
3o
i θi di ai αi3 0� d3 0 0�
θi - ángulo entre xi�1 y xi medido sobre zi�1di - distancia de oi�1 a xi medida a lo largo de zi�1ai - distancia de zi�1 a oi medida a lo largo de xiαi - ángulo entre zi�1 y zi medido sobre xi
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 22 / 34
Tabla de parámetros
4l4θ
3x
3y3z
3o
4x
4y4z
4o
i θi di ai αi4 θ4 l4 0 0�
θi - ángulo entre xi�1 y xi medido sobre zi�1di - distancia de oi�1 a xi medida a lo largo de zi�1ai - distancia de zi�1 a oi medida a lo largo de xiαi - ángulo entre zi�1 y zi medido sobre xi
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 23 / 34
Matrices de paso
Para obtener las matrices de paso Ai , sólo hay que sustituir losparámetros θi , di , ai y αi en (2):
Ai =
2664Cθi �SθiCαi SθiSαi aiCθi
Sθi CθiCαi �CθiSαi aiSθi
0 Sαi Cαi di0 0 0 1
3775 (2)
Tabla de parámetros:
i θi di ai αi1 θ1 0 l1 0�
2 θ2 0 l2 180�
3 0� d3 0 0�
4 θ4 l4 0 0�
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 24 / 34
Matrices de paso
Ai =
2664Cθi �SθiCαi SθiSαi aiCθi
Sθi CθiCαi �CθiSαi aiSθi
0 Sαi Cαi di0 0 0 1
3775 i θi di ai αi1 θ1 0 l1 0�
Para A1 se tiene:
A1 =
2664Cθ1 �Sθ1C0� Sθ1S0� (l1)Cθ1
Sθ1 Cθ1C0� �Cθ1S0� (l1) Sθ1
0 S0� C0� 00 0 0 1
3775
=
2664Cθ1 �Sθ1 0 l1Cθ1
Sθ1 Cθ1 0 l1Sθ1
0 0 1 00 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 25 / 34
Matrices de paso
Ai =
2664Cθi �SθiCαi SθiSαi aiCθi
Sθi CθiCαi �CθiSαi aiSθi
0 Sαi Cαi di0 0 0 1
3775 i θi di ai αi2 θ2 0 l2 180�
Para A2 se tiene:
A2 =
2664Cθ2 �Sθ2C180� Sθ2S180� (l2)Cθ2
Sθ2 Cθ2C180� �Cθ2S180� (l2) Sθ2
0 S180� C180� 00 0 0 1
3775
=
2664Cθ2 Sθ2 0 l2Cθ2
Sθ2 �Cθ2 0 l2Sθ2
0 0 �1 00 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 26 / 34
Matrices de paso
Ai =
2664Cθi �SθiCαi SθiSαi aiCθi
Sθi CθiCαi �CθiSαi aiSθi
0 Sαi Cαi di0 0 0 1
3775 i θi di ai αi3 0� d3 0 0�
Para A3 se tiene:
A3 =
2664C0� �S0�C0� S0�S0� (0)C0�S0� C0�C0� �C0�S0� (0) S0�0 S0� C0� d30 0 0 1
3775
=
26641 0 0 00 1 0 00 0 1 d30 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 27 / 34
Matrices de paso
Ai =
2664Cθi �SθiCαi SθiSαi aiCθi
Sθi CθiCαi �CθiSαi aiSθi
0 Sαi Cαi di0 0 0 1
3775 i θi di ai αi4 θ4 l4 0 0�
Para A4 se tiene:
A4 =
2664Cθ4 �Sθ4C0� Sθ4S0� (0)Cθ4
Sθ4 Cθ4C0� �Cθ4S0� (0) Sθ4
0 S0� C0� l40 0 0 1
3775
=
2664Cθ4 �Sθ4 0 0Sθ4 Cθ4 0 00 0 1 l40 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 28 / 34
Matriz de transformación homogénea
La matriz de transformación homogénea que relaciona losreferenciales base y del efector �nal se calcula como:
Tn0 =n
∏i=1Ai = A1A2 � � �An ) T40 =
4
∏i=1Ai = A1A2A3A4
0z
0x
0y
0o
4z
4x
4y
4o4l
4θ
3d
2θ
2l
1θ
1l
40T
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 29 / 34
Matriz de transformación homogénea
T40 =4
∏i=1Ai = A1A2A3A4
=
2664Cθ1 �Sθ1 0 l1Cθ1
Sθ1 Cθ1 0 l1Sθ1
0 0 1 00 0 0 1
37752664Cθ2 Sθ2 0 l2Cθ2
Sθ2 �Cθ2 0 l2Sθ2
0 0 �1 00 0 0 1
3775 � � �
� � �
26641 0 0 00 1 0 00 0 1 d30 0 0 1
37752664Cθ4 �Sθ4 0 0Sθ4 Cθ4 0 00 0 1 l40 0 0 1
3775Tenga en cuenta que en general el producto de matrices no esconmutativo, es decir, AB 6= BA.Por otro lado, una buena asociación de matrices simpli�ca de maneranotoria los cálculos.
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 30 / 34
Matriz de transformación homogénea
La matriz de transformación homogénea está dada por:
T40 =4
∏i=1Ai = A1A2A3A4
A1A2 =
2664Cθ1 �Sθ1 0 l1Cθ1
Sθ1 Cθ1 0 l1Sθ1
0 0 1 00 0 0 1
37752664Cθ2 Sθ2 0 l2Cθ2
Sθ2 �Cθ2 0 l2Sθ2
0 0 �1 00 0 0 1
3775
=
2664Cθ1+θ2 Sθ1+θ2 0 l1Cθ1 + l2Cθ1+θ2
Sθ1+θ2 �Cθ1+θ2 0 l1Sθ1 + l2Sθ1+θ2
0 0 �1 00 0 0 1
3775sen (x � y) = sen x cos y � cos x sen ycos (x � y) = cos x cos y � sen x sen y
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 31 / 34
Matriz de transformación homogénea
La matriz de transformación homogénea está dada por:
T40 = A1A2A3A4
A3A4 =
26641 0 0 00 1 0 00 0 1 d30 0 0 1
37752664Cθ4 �Sθ4 0 0Sθ4 Cθ4 0 00 0 1 l40 0 0 1
3775
=
2664Cθ4 �Sθ4 0 0Sθ4 Cθ4 0 00 0 1 d3 + l40 0 0 1
3775
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 32 / 34
Matriz de transformación homogénea
Se sigue que la matriz de transformación homogénea T40 es:
T40 = A1A2A3A4
=
2664Cθ1+θ2 Sθ1+θ2 0 l1Cθ1 + l2Cθ1+θ2
Sθ1+θ2 �Cθ1+θ2 0 l1Sθ1 + l2Sθ1+θ2
0 0 �1 00 0 0 1
3775 � � �
� � �
2664Cθ4 �Sθ4 0 0Sθ4 Cθ4 0 00 0 1 d3 + l40 0 0 1
3775
=
2664r11 r12 r13 dxr21 r22 r23 dyr31 r32 r33 dz0 0 0 1
3775Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 33 / 34
Matriz de transformación homogénea
Se sigue que la matriz de transformación homogénea T40 es:
T40 =
2664r11 r12 r13 dxr21 r22 r23 dyr31 r32 r33 dz0 0 0 1
3775donde
r11 = Cθ1+θ2Cθ4 + Sθ1+θ2Sθ4 r13 = 0r21 = Sθ1+θ2Cθ4 � Cθ1+θ2Sθ4 r23 = 0r31 = 0 r33 = �1r12 = �Cθ1+θ2Sθ4 + Sθ1+θ2Cθ4 dx = l1Cθ1 + l2Cθ1+θ2
r22 = �Sθ1+θ2Sθ4 � Cθ1+θ2Cθ4 dy = l1Sθ1 + l2Sθ1+θ2
r32 = 0 dz = � (d3 + l4)
Jorge E. Lavín Delgado (ULSA) Cinemática Directa Robot Scara 04/Octubre/2012 34 / 34
