Post on 16-Jun-2015
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