Robótica IICinemática Directa del Manipulador Stanford

Jorge Enrique Lavín Delgado

Universidad La Salle

Viernes 10 de Agosto de 2012

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 1 / 23

Diagrama del Manipulador Stanford

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 2 / 23

Asignación de los referenciales (D-H)

0o0x

0y

0z

6o

6x6y

1o

1x1y

1z2o

2x2y

2z

3o3x

3y3z 4,5o4x 4y

4z5y

5x

5z

1l

*3d

4l

6l

2l

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 3 / 23

Tabla de Parámetros

0o0x

0y0z

1o

1x1y

1z

1l 1θθi di ai αiθ�1 l1 0 �90�

θi - ángulo entre los ejes xi�1 y xi , medido alrededor del eje zi�1di - distancia del origen oi�1 al eje xi , medida a lo largo del eje zi�1ai - distancia del eje zi�1 al origen oi , medida a lo largo del eje xiαi - ángulo entre los ejes zi�1 y zi , medido alrededor del eje xi

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 4 / 23

Tabla de Parámetros

1o

1x

1z2o

2x2y

2z2l

2θ

1y

θi di ai αiθ�2 l2 0 90�

θi - ángulo entre los ejes xi�1 y xi , medido alrededor del eje zi�1di - distancia del origen oi�1 al eje xi , medida a lo largo del eje zi�1ai - distancia del eje zi�1 al origen oi , medida a lo largo del eje xiαi - ángulo entre los ejes zi�1 y zi , medido alrededor del eje xi

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 5 / 23

Tabla de Parámetros

2o2x

2y

2z

3o3x

3y3z

*3d

θi di ai αi0� d�3 0 0�

θi - ángulo entre los ejes xi�1 y xi , medido alrededor del eje zi�1di - distancia del origen oi�1 al eje xi , medida a lo largo del eje zi�1ai - distancia del eje zi�1 al origen oi , medida a lo largo del eje xiαi - ángulo entre los ejes zi�1 y zi , medido alrededor del eje xi

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 6 / 23

Tabla de Parámetros

3o3x 3y

3z

4o

4x 4y

4z

4l4θ θi di ai αiθ�4 l4 0 �90�

θi - ángulo entre los ejes xi�1 y xi , medido alrededor del eje zi�1di - distancia del origen oi�1 al eje xi , medida a lo largo del eje zi�1ai - distancia del eje zi�1 al origen oi , medida a lo largo del eje xiαi - ángulo entre los ejes zi�1 y zi , medido alrededor del eje xi

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 7 / 23

Tabla de Parámetros

4,5o4x 4y

4z5y

5x

5z5θ θi di ai αi

θ�5 0 0 90�

θi - ángulo entre los ejes xi�1 y xi , medido alrededor del eje zi�1di - distancia del origen oi�1 al eje xi , medida a lo largo del eje zi�1ai - distancia del eje zi�1 al origen oi , medida a lo largo del eje xiαi - ángulo entre los ejes zi�1 y zi , medido alrededor del eje xi

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 8 / 23

Tabla de Parámetros

6o

6x

6y6z

5o

5y5x

5z 6l

6θ θi di ai αiθ�6 l6 0 0�

θi - ángulo entre los ejes xi�1 y xi , medido alrededor del eje zi�1di - distancia del origen oi�1 al eje xi , medida a lo largo del eje zi�1ai - distancia del eje zi�1 al origen oi , medida a lo largo del eje xiαi - ángulo entre los ejes zi�1 y zi , medido alrededor del eje xi

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 9 / 23

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 (1)

Para obtener las matrices de paso Ai se sustituyen los parámetros θi ,di , ai y αi (mostrados en la siguiente tabla) en la matriz dada en (1).

i θi di ai αi1 θ1 l1 0 �90�2 θ2 l2 0 90�

3 0� d3 0 0�

4 θ4 l4 0 �90�5 θ5 0 0 90�

6 θ6 l6 0 0�

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 10 / 23

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 l1 0 �90�

Para A1 se tiene:

A1 =

2664Cθ1 �Sθ1C�90� Sθ1S�90� (0)Cθ1

Sθ1 Cθ1C�90� �Cθ1S�90� (0) Sθ1

0 S�90� C�90� l10 0 0 1

3775

=

2664Cθ1 0 �Sθ1 0Sθ1 0 Cθ1 00 �1 0 l10 0 0 1

3775Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 11 / 23

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 l2 0 90�

Para A2 se tiene:

A2 =

2664Cθ2 �Sθ2C90� Sθ2S90� (0)Cθ2

Sθ2 Cθ2C90� �Cθ2S90� (0) Sθ2

0 S90� C90� l20 0 0 1

3775

=

2664Cθ2 0 Sθ2 0Sθ2 0 �Cθ2 00 1 0 l20 0 0 1

3775Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 12 / 23

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 D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 13 / 23

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 �90�

Para A4 se tiene:

A4 =

2664Cθ4 �Sθ4C�90� Sθ4S�90� (0)Cθ4

Sθ4 Cθ4C�90� �Cθ4S�90� (0) Sθ4

0 S�90� C�90� l40 0 0 1

3775

=

2664Cθ4 0 �Sθ4 0Sθ4 0 Cθ4 00 �1 0 l40 0 0 1

3775Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 14 / 23

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 αi5 θ5 0 0 90�

Para A5 se tiene:

A5 =

2664Cθ5 �Sθ5C90� Sθ5S90� (0)Cθ5

Sθ5 Cθ5C90� �Cθ5S90� (0) Sθ5

0 S90� C90� 00 0 0 1

3775

=

2664Cθ5 0 Sθ5 0Sθ5 0 �Cθ5 00 1 0 00 0 0 1

3775Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 15 / 23

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 αi6 θ6 l6 0 0�

Para A6 se tiene:

A6 =

2664Cθ6 �Sθ6C0� Sθ6S0� (0)Cθ6

Sθ6 Cθ6C0� �Cθ6S0� (0) Sθ6

0 S0� C0� l60 0 0 1

3775

=

2664Cθ6 �Sθ6 0 0Sθ6 Cθ6 0 00 0 1 l60 0 0 1

3775Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 16 / 23

Matriz de transformación homogénea

La matriz de transformación homogénea que relaciona losreferenciales base y del efector �nal se calcula como:

0o0x

0y

0z

6o

6x6y

6z

1l

*3d

4l

6l

2l

60T Tn0 =

n

∏i=1Ai = A1A2A3 � � �An (2)

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 17 / 23

Matriz de transformación homogénea

De la expresión (2) se tiene:

T60 =6

∏i=1Ai = A1A2A3A4A5A62664

Cθ1 0 �Sθ1 0Sθ1 0 Cθ1 00 �1 0 l10 0 0 1

37752664Cθ2 0 Sθ2 0Sθ2 0 �Cθ2 00 1 0 l20 0 0 1

377526641 0 0 00 1 0 00 0 1 d30 0 0 1

37752664Cθ4 0 �Sθ4 0Sθ4 0 Cθ4 00 �1 0 l40 0 0 1

37752664Cθ5 0 Sθ5 0Sθ5 0 �Cθ5 00 1 0 00 0 0 1

37752664Cθ6 �Sθ6 0 0Sθ6 Cθ6 0 00 0 1 l60 0 0 1

3775Tenga en cuenta que en general el producto de matrices no esconmutativo, es decir, AB 6= BA

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 18 / 23

Matriz de transformación homogénea

Realizando las operaciones A1A2, A3A4 y A5A6 resulta:

T60 =

2664Cθ1Cθ2 �Sθ1 Cθ1Sθ2 �l2Sθ1

Sθ1Cθ2 Cθ1 Sθ1Sθ2 l2Cθ1

�Sθ2 0 Cθ2 l10 0 0 1

37752664Cθ4 0 �Sθ4 0Sθ4 0 Cθ4 00 �1 0 d3 + l40 0 0 1

37752664Cθ5Cθ6 �Cθ5Sθ6 Sθ5 l6Sθ5

Sθ5Cθ6 �Sθ5Sθ6 �Cθ5 �l6Cθ5

Sθ6 Cθ6 0 00 0 0 1

3775Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 19 / 23

Matriz de transformación homogénea

La matriz de transformación homogéna T60 está dada por:

T60 =

2664r11 r12 r13 dxr21 r22 r23 dyr31 r32 r33 dz0 0 0 1

3775donde

r11 = Cθ1Cθ2 (Cθ4Cθ5Cθ6 � Sθ4Sθ6)� Sθ1 (Sθ4Cθ5Cθ6 + Cθ4Sθ6) � � ��Cθ1Sθ2Sθ5Cθ6

r12 = Cθ1Cθ2 (�Cθ4Cθ5Sθ6 � Sθ4Cθ6)� Sθ1 (�Sθ4Cθ5Sθ6 + Cθ4Cθ6) � � �+Cθ1Sθ2Sθ5Sθ6

r13 = Cθ1Cθ2Cθ4Sθ5 � Sθ1Sθ4Sθ5 + Cθ1Sθ2Cθ5

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 20 / 23

Matriz de transformación homogénea

La matriz de transformación homogéna T60 está dada por:

T60 =

2664r11 r12 r13 dxr21 r22 r23 dyr31 r32 r33 dz0 0 0 1

3775donde

r21 = Sθ1Cθ2 (Cθ4Cθ5Cθ6 � Sθ4Sθ6) + Cθ1 (Sθ4Cθ5Cθ6 + Cθ4Sθ6) � � ��Sθ1Sθ2Sθ5Cθ6

r22 = Sθ1Cθ2 (�Cθ4Cθ5Sθ6 � Sθ4Cθ6) + Cθ1 (�Sθ4Cθ5Sθ6 + Cθ4Cθ6) � � �+Sθ1Sθ2Sθ5Sθ6

r23 = Sθ1Cθ2Cθ4Sθ5 + Cθ1Sθ4Sθ5 + Sθ1Sθ2Cθ5

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 21 / 23

Matriz de transformación homogénea

La matriz de transformación homogéna T60 está dada por:

T60 =

2664r11 r12 r13 dxr21 r22 r23 dyr31 r32 r33 dz0 0 0 1

3775donde

r31 = �Sθ2Cθ4Cθ5Cθ6 � Cθ2Sθ5Cθ6 + Sθ2Sθ4Sθ6

r32 = �Sθ2 (�Cθ4Cθ5Sθ6 � Sθ4Cθ6) + Cθ2Sθ5Sθ6

r33 = �Sθ2Cθ4Sθ5 + Cθ2Cθ5

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 22 / 23

Matriz de transformación homogénea

La matriz de transformación homogéna T60 está dada por:

T60 =

2664r11 r12 r13 dxr21 r22 r23 dyr31 r32 r33 dz0 0 0 1

3775donde

dx = �l2Sθ1 + l6 (Cθ1Cθ2Cθ4Sθ5 � Sθ1Sθ4Sθ5 + Cθ1Sθ2Cθ5) � � �+l4Cθ1Sθ2 + d3Cθ1Sθ2

dy = l6 (Sθ1Cθ2Cθ4Sθ5 + Cθ1Sθ4Sθ5 + Sθ1Sθ2Cθ5) + l2Cθ1 � � �+l4Sθ1Sθ2 + d3Sθ1Sθ2

dz = l6 (�Sθ2Cθ4Sθ5 + Cθ2Cθ5) + d3Cθ2 + l4Cθ2 + l1

Jorge E. Lavín D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 23 / 23