cinemática directa robot stanford

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