Robótica II Cinemá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) o6 y6 x6 l6
z5 y5 z4
x5 x4
l2
x3
y4 z3 o3
o 4,5
d3*
z2 z1
o1 x1 l1
y2
x2
y1 z0
x0 Jorge E. Lavín D. (ULSA)
o2
l4
y3
o0
y0
C.D. Manipulador Stanford
10/Agosto/2012
3 / 23
Tabla de Parámetros
o1 x1 l1
θi di ai αi
y1 z0
x0
z1 θi θ1
θ1
o0
di l1
ai 0
αi 90
y0
- ángulo entre los ejes xi 1 y xi , medido alrededor del eje zi 1 - distancia del origen oi 1 al eje xi , medida a lo largo del eje zi - distancia del eje zi 1 al origen oi , medida a lo largo del eje xi - ángulo entre los ejes zi 1 y zi , medido alrededor del eje xi Jorge E. Lavín D. (ULSA)
C.D. Manipulador Stanford
1
10/Agosto/2012
4 / 23
Tabla de Parámetros
l2 θ2
x1 θi di ai αi
z2 o1
z1
y1
o2
y2
θi θ2
di l2
ai 0
x2
- ángulo entre los ejes xi 1 y xi , medido alrededor del eje zi 1 - distancia del origen oi 1 al eje xi , medida a lo largo del eje zi - distancia del eje zi 1 al origen oi , medida a lo largo del eje xi - ángulo entre los ejes zi 1 y zi , medido alrededor del eje xi Jorge E. Lavín D. (ULSA)
αi 90
C.D. Manipulador Stanford
1
10/Agosto/2012
5 / 23
Tabla de Parámetros
z3 x3
o3
y3 d3*
z2 o2 x2 θi di ai αi
θi 0
di d3
ai 0
αi 0
y2
- ángulo entre los ejes xi 1 y xi , medido alrededor del eje zi 1 - distancia del origen oi 1 al eje xi , medida a lo largo del eje zi - distancia del eje zi 1 al origen oi , medida a lo largo del eje xi - ángulo entre los ejes zi 1 y zi , medido alrededor del eje xi Jorge E. Lavín D. (ULSA)
C.D. Manipulador Stanford
1
10/Agosto/2012
6 / 23
Tabla de Parámetros
z4
o4
y4
x4 z3 x3 θi di ai αi
θ4
l4
θi θ4
di l4
ai 0
αi 90
o3 y3
- ángulo entre los ejes xi 1 y xi , medido alrededor del eje zi 1 - distancia del origen oi 1 al eje xi , medida a lo largo del eje zi - distancia del eje zi 1 al origen oi , medida a lo largo del eje xi - ángulo entre los ejes zi 1 y zi , medido alrededor del eje xi Jorge E. Lavín D. (ULSA)
C.D. Manipulador Stanford
1
10/Agosto/2012
7 / 23
Tabla de Parámetros
z5
θ5
z4
θi di ai αi
di 0
ai 0
αi 90
y5
x5 x4
θi θ5
y4
o 4,5
- ángulo entre los ejes xi 1 y xi , medido alrededor del eje zi 1 - distancia del origen oi 1 al eje xi , medida a lo largo del eje zi - distancia del eje zi 1 al origen oi , medida a lo largo del eje xi - ángulo entre los ejes zi 1 y zi , medido alrededor del eje xi Jorge E. Lavín D. (ULSA)
C.D. Manipulador Stanford
1
10/Agosto/2012
8 / 23
Tabla de Parámetros
o6 x6
z6 y6
θ6
θi θ6
di l6
ai 0
αi 0
l6
z5 y5 x5 θi di ai αi
o5
- ángulo entre los ejes xi 1 y xi , medido alrededor del eje zi 1 - distancia del origen oi 1 al eje xi , medida a lo largo del eje zi - distancia del eje zi 1 al origen oi , medida a lo largo del eje xi - ángulo entre los ejes zi 1 y zi , medido alrededor del eje xi Jorge E. Lavín D. (ULSA)
C.D. Manipulador Stanford
1
10/Agosto/2012
9 / 23
Matrices de paso 2
Cθ i 6 Sθ i Ai = 6 4 0 0
Sθ i Cαi Cθ i Cαi Sαi 0
Sθ i Sαi Cθ i Sαi Cαi 0
3 ai C θ i ai S θ i 7 7 di 5 1
(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 1 2 3 4 5 6 Jorge E. Lavín D. (ULSA)
θi θ1 θ2 0 θ4 θ5 θ6
di l1 l2 d3 l4 0 l6
ai 0 0 0 0 0 0
C.D. Manipulador Stanford
αi 90 90 0 90 90 0 10/Agosto/2012
10 / 23
Matrices de paso 2
Cθ i 6 Sθ i Ai = 6 4 0 0
Sθ i Cαi Cθ i Cαi Sαi 0
Sθ i Sαi Cθ i Sαi Cαi 0
3 ai C θ i ai S θ i 7 7 di 5 1
i 1
θi θ1
di l1
ai 0
αi 90
Para A1 se tiene:
A1
2
Cθ 1 6 Sθ 1 = 6 4 0 0 2 Cθ 1 6 Sθ 1 = 6 4 0 0
Jorge E. Lavín D. (ULSA)
Sθ 1 C 90 Cθ 1 C 90 S 90 0 0 0 1 0
Sθ 1 Cθ 1 0 0
Sθ 1 S 90 Cθ 1 S 90 C 90 0 3
0 0 7 7 l1 5 1
C.D. Manipulador Stanford
3 (0 ) Cθ 1 (0) Sθ 1 7 7 5 l1 1
10/Agosto/2012
11 / 23
Matrices de paso 2
Cθ i 6 Sθ i Ai = 6 4 0 0
Sθ i Cαi Cθ i Cαi Sαi 0
3 ai C θ i ai S θ i 7 7 di 5 1
Sθ i Sαi Cθ i Sαi Cαi 0
i 2
θi θ2
di l2
ai 0
αi 90
Para A2 se tiene:
A2
Jorge E. Lavín D. (ULSA)
2
Cθ 2 6 Sθ 2 = 6 4 0 0 2 Cθ 2 6 Sθ 2 = 6 4 0 0
Sθ 2 C90 Cθ 2 C90 S90 0 0 0 1 0
Sθ 2 Cθ 2 0 0
Sθ 2 S90 Cθ 2 S90 C90 0 3
0 0 7 7 l2 5 1
C.D. Manipulador Stanford
3 (0 ) Cθ 2 (0 ) Sθ 2 7 7 5 l2 1
10/Agosto/2012
12 / 23
Matrices de paso 2
Cθ i 6 Sθ i Ai = 6 4 0 0
Sθ i Cαi Cθ i Cαi Sαi 0
Sθ i Sαi Cθ i Sαi Cαi 0
3 ai C θ i ai S θ i 7 7 di 5 1
i 3
θi 0
di d3
ai 0
αi 0
Para A3 se tiene:
A3
Jorge E. Lavín D. (ULSA)
2
C0 6 S0 = 6 4 0 0 2 1 6 0 = 6 4 0 0
S0 C0 C0 C0 S0 0 0 1 0 0
3 0 0 0 0 7 7 1 d3 5 0 1
S0 S0 C0 S0 C0 0
C.D. Manipulador Stanford
3 (0) C0 ( 0 ) S0 7 7 5 d3 1
10/Agosto/2012
13 / 23
Matrices de paso 2
Cθ i 6 Sθ i Ai = 6 4 0 0
Sθ i Cαi Cθ i Cαi Sαi 0
Sθ i Sαi Cθ i Sαi Cαi 0
3 ai C θ i ai S θ i 7 7 di 5 1
i 4
θi θ4
di l4
ai 0
αi 90
Para A4 se tiene:
A4
2
Cθ 4 6 Sθ 4 = 6 4 0 0 2 Cθ 4 6 Sθ 4 = 6 4 0 0
Jorge E. Lavín D. (ULSA)
Sθ 4 C 90 Cθ 4 C 90 S 90 0 0 0 1 0
Sθ 4 Cθ 4 0 0
Sθ 4 S 90 Cθ 4 S 90 C 90 0 3
0 0 7 7 l4 5 1
C.D. Manipulador Stanford
3 (0 ) Cθ 4 (0) Sθ 4 7 7 5 l4 1
10/Agosto/2012
14 / 23
Matrices de paso 2
Cθ i 6 Sθ i Ai = 6 4 0 0
Sθ i Cαi Cθ i Cαi Sαi 0
3 ai C θ i ai S θ i 7 7 di 5 1
Sθ i Sαi Cθ i Sαi Cαi 0
i 5
θi θ5
di 0
ai 0
αi 90
Para A5 se tiene:
A5
Jorge E. Lavín D. (ULSA)
2
Cθ 5 6 Sθ 5 = 6 4 0 0 2 Cθ 5 6 Sθ 5 = 6 4 0 0
Sθ 5 C90 Cθ 5 C90 S90 0 0 0 1 0
Sθ 5 Cθ 5 0 0
Sθ 5 S90 Cθ 5 S90 C90 0 3
0 0 7 7 0 5 1
C.D. Manipulador Stanford
3 (0 ) Cθ 5 (0 ) Sθ 5 7 7 5 0 1
10/Agosto/2012
15 / 23
Matrices de paso 2
Cθ i 6 Sθ i Ai = 6 4 0 0
Sθ i Cαi Cθ i Cαi Sαi 0
Sθ i Sαi Cθ i Sαi Cαi 0
3 ai C θ i ai S θ i 7 7 di 5 1
i 6
θi θ6
di l6
ai 0
αi 0
Para A6 se tiene:
A6
Jorge E. Lavín D. (ULSA)
2
Cθ 6 6 Sθ 6 = 6 4 0 0 2 Cθ 6 6 Sθ 6 = 6 4 0 0
Sθ 6 C0 Cθ 6 C0 S0 0 Sθ 6 Cθ 6 0 0
0 0 1 0
S θ 6 S0 C θ 6 S0 C0 0 3 0 0 7 7 l6 5 1
C.D. Manipulador Stanford
3 (0 ) Cθ 6 (0 ) Sθ 6 7 7 5 l6 1
10/Agosto/2012
16 / 23
Matriz de transformación homogénea La matriz de transformación homogénea que relaciona los referenciales base y del efector …nal se calcula como: z6 o6
y6
x6 l6
T06
l4
Tn0 =
n
∏ Ai = A1 A2 A3
An
(2)
i =1 l2
l1
z0
x0
d3*
o0
Jorge E. Lavín D. (ULSA)
y0
C.D. Manipulador Stanford
10/Agosto/2012
17 / 23
Matriz de transformación homogénea De la expresión (2) se tiene: T60 = 2
6
∏ Ai = A1 A2 A3 A4 A5 A6
i =1
Cθ 1 0 Sθ 1 6 Sθ 0 C θ1 6 1 4 0 1 0 0 0 0 2 Cθ 4 0 Sθ 4 6 Sθ Cθ 4 6 4 0 4 0 1 0 0 0 0
32 Cθ 2 0 6 Sθ 0 7 76 2 l1 5 4 0 0 1 32 0 Cθ 5 7 6 0 7 6 Sθ 5 l4 5 4 0 1 0
0 0 1 0 0 0 1 0
Sθ 2 Cθ 2 0 0 Sθ 5 Cθ 5 0 0
32 1 0 6 0 0 7 76 l2 5 4 0 0 1 32 0 Cθ 6 7 6 0 7 6 Sθ 6 0 54 0 1 0
0 1 0 0
3 0 0 0 0 7 7 1 d3 5 0 1
Sθ 6 Cθ 6 0 0
Tenga en cuenta que en general el producto de matrices no es conmutativo, es decir, AB 6= BA Jorge E. Lavín D. (ULSA)
C.D. Manipulador Stanford
10/Agosto/2012
0 0 1 0
3 0 0 7 7 l6 5 1
18 / 23
Matriz de transformación homogénea Realizando las operaciones A1 A2 , A3 A4 y A5 A6 resulta: 2 3 Cθ 1 Cθ 2 Sθ 1 Cθ 1 Sθ 2 l2 Sθ 1 6 Sθ Cθ Cθ 1 Sθ 1 Sθ 2 l2 Cθ 1 7 1 2 7 T60 = 6 4 5 Sθ 2 0 Cθ 2 l1 0 0 0 1 3 2 Cθ 4 0 Sθ 4 0 7 6 Sθ 0 C 0 θ4 7 6 4 4 0 1 0 d3 + l4 5 0 0 0 1 2 Cθ 5 Cθ 6 Cθ 5 Sθ 6 Sθ 5 l6 Sθ 5 6 Sθ Cθ Sθ 5 Sθ 6 Cθ 5 l6 Cθ 5 6 5 6 4 Sθ C 0 0 θ6 6 0 0 0 1 Jorge E. Lavín D. (ULSA)
C.D. Manipulador Stanford
3 7 7 5
10/Agosto/2012
19 / 23
Matriz de transformación homogénea La matriz de transformación homogéna T60 2 r11 r12 r13 6 r21 r22 r23 T60 = 6 4 r31 r32 r33 0 0 0
donde
r11 = Cθ 1 Cθ 2 (Cθ 4 Cθ 5 Cθ 6
Sθ 4 Sθ 6 )
está dada por: 3 dx dy 7 7 dz 5 1
Sθ 1 (Sθ 4 Cθ 5 Cθ 6 + Cθ 4 Sθ 6 )
Cθ 1 Sθ 2 Sθ 5 Cθ 6 r12 = Cθ 1 Cθ 2 ( Cθ 4 Cθ 5 Sθ 6 r13
Sθ 4 Cθ 6 )
Sθ 1 ( Sθ 4 Cθ 5 Sθ 6 + Cθ 4 Cθ 6 )
+ Cθ 1 Sθ 2 Sθ 5 Sθ 6 = Cθ 1 Cθ 2 Cθ 4 Sθ 5 Sθ 1 Sθ 4 Sθ 5 + Cθ 1 Sθ 2 Cθ 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 2 r11 r12 r13 6 r21 r22 r23 T60 = 6 4 r31 r32 r33 0 0 0
donde
r21 = Sθ 1 Cθ 2 (Cθ 4 Cθ 5 Cθ 6
está dada por: 3 dx dy 7 7 dz 5 1
Sθ 4 Sθ 6 ) + Cθ 1 (Sθ 4 Cθ 5 Cθ 6 + Cθ 4 Sθ 6 )
Sθ 1 Sθ 2 Sθ 5 Cθ 6 r22 = Sθ 1 Cθ 2 ( Cθ 4 Cθ 5 Sθ 6 r23
Sθ 4 Cθ 6 ) + Cθ 1 ( Sθ 4 Cθ 5 Sθ 6 + Cθ 4 Cθ 6 )
+Sθ 1 Sθ 2 Sθ 5 Sθ 6 = Sθ 1 Cθ 2 Cθ 4 Sθ 5 + Cθ 1 Sθ 4 Sθ 5 + Sθ 1 Sθ 2 Cθ 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 2 r11 r12 r13 6 r21 r22 r23 T60 = 6 4 r31 r32 r33 0 0 0
donde
r31 =
Sθ 2 Cθ 4 Cθ 5 Cθ 6
r32 =
Sθ 2 ( Cθ 4 Cθ 5 Sθ 6
r33 =
Sθ 2 Cθ 4 Sθ 5 + Cθ 2 Cθ 5
Jorge E. Lavín D. (ULSA)
está dada por: 3 dx dy 7 7 dz 5 1
Cθ 2 Sθ 5 Cθ 6 + Sθ 2 Sθ 4 Sθ 6 Sθ 4 Cθ 6 ) + Cθ 2 Sθ 5 Sθ 6
C.D. Manipulador Stanford
10/Agosto/2012
22 / 23
Matriz de transformación homogénea La matriz de transformación homogéna T60 2 r11 r12 r13 6 r21 r22 r23 T60 = 6 4 r31 r32 r33 0 0 0
donde dx dy dz
=
l2 Sθ 1 + l6 (Cθ 1 Cθ 2 Cθ 4 Sθ 5
está dada por: 3 dx dy 7 7 dz 5 1
Sθ 1 Sθ 4 Sθ 5 + Cθ 1 Sθ 2 Cθ 5 )
+l4 Cθ1 Sθ2 + d3 Cθ1 Sθ2 = l6 (Sθ1 Cθ2 Cθ4 Sθ5 + Cθ1 Sθ4 Sθ5 + Sθ1 Sθ2 Cθ5 ) + l2 Cθ1 +l4 Sθ1 Sθ2 + d3 Sθ1 Sθ2 = l6 ( Sθ2 Cθ4 Sθ5 + Cθ2 Cθ5 ) + d3 Cθ2 + l4 Cθ2 + l1
Jorge E. Lavín D. (ULSA)
C.D. Manipulador Stanford
10/Agosto/2012
23 / 23
