Robtica II
Cinemtica Directa del Manipulador Stanford
Jorge Enrique Lavn Delgado
Universidad La Salle
Viernes 10 de Agosto de 2012
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 1 / 23
Diagrama del Manipulador Stanford
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 2 / 23
Asignacin de los referenciales (D-H)
0
o
0
x
0
y
0
z
6
o
6
x
6
y
1
o
1
x
1
y
1
z
2
o
2
x
2
y
2
z
3
o
3
x
3
y
3
z
4,5
o 4
x 4
y
4
z
5
y
5
x
5
z
1
l
*
3
d
4
l
6
l
2
l
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 3 / 23
Tabla de Parmetros
0
o
0
x
0
y
0
z
1
o
1
x
1
y
1
z
1
l
1
i
d
i
a
i
i
1
l
1
0 90
i
- ngulo entre los ejes x
i 1
y x
i
, medido alrededor del eje z
i 1
d
i
- distancia del origen o
i 1
al eje x
i
, medida a lo largo del eje z
i 1
a
i
- distancia del eje z
i 1
al origen o
i
, medida a lo largo del eje x
i
i
- ngulo entre los ejes z
i 1
y z
i
, medido alrededor del eje x
i
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 4 / 23
Tabla de Parmetros
1
o
1
x
1
z
2
o
2
x
2
y
2
z
2
l
2
1
y
i
d
i
a
i
i
2
l
2
0 90
i
- ngulo entre los ejes x
i 1
y x
i
, medido alrededor del eje z
i 1
d
i
- distancia del origen o
i 1
al eje x
i
, medida a lo largo del eje z
i 1
a
i
- distancia del eje z
i 1
al origen o
i
, medida a lo largo del eje x
i
i
- ngulo entre los ejes z
i 1
y z
i
, medido alrededor del eje x
i
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 5 / 23
Tabla de Parmetros
2
o
2
x
2
y
2
z
3
o
3
x
3
y
3
z
*
3
d
i
d
i
a
i
i
0
3
0 0
i
- ngulo entre los ejes x
i 1
y x
i
, medido alrededor del eje z
i 1
d
i
- distancia del origen o
i 1
al eje x
i
, medida a lo largo del eje z
i 1
a
i
- distancia del eje z
i 1
al origen o
i
, medida a lo largo del eje x
i
i
- ngulo entre los ejes z
i 1
y z
i
, medido alrededor del eje x
i
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 6 / 23
Tabla de Parmetros
3
o
3
x
3
y
3
z
4
o
4
x
4
y
4
z
4
l
4
i
d
i
a
i
i
4
l
4
0 90
i
- ngulo entre los ejes x
i 1
y x
i
, medido alrededor del eje z
i 1
d
i
- distancia del origen o
i 1
al eje x
i
, medida a lo largo del eje z
i 1
a
i
- distancia del eje z
i 1
al origen o
i
, medida a lo largo del eje x
i
i
- ngulo entre los ejes z
i 1
y z
i
, medido alrededor del eje x
i
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 7 / 23
Tabla de Parmetros
4,5
o 4
x 4
y
4
z
5
y
5
x
5
z
5
i
d
i
a
i
i
5
0 0 90
i
- ngulo entre los ejes x
i 1
y x
i
, medido alrededor del eje z
i 1
d
i
- distancia del origen o
i 1
al eje x
i
, medida a lo largo del eje z
i 1
a
i
- distancia del eje z
i 1
al origen o
i
, medida a lo largo del eje x
i
i
- ngulo entre los ejes z
i 1
y z
i
, medido alrededor del eje x
i
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 8 / 23
Tabla de Parmetros
6
o
6
x
6
y
6
z
5
o
5
y
5
x
5
z
6
l
6
i
d
i
a
i
i
6
l
6
0 0
i
- ngulo entre los ejes x
i 1
y x
i
, medido alrededor del eje z
i 1
d
i
- distancia del origen o
i 1
al eje x
i
, medida a lo largo del eje z
i 1
a
i
- distancia del eje z
i 1
al origen o
i
, medida a lo largo del eje x
i
i
- ngulo entre los ejes z
i 1
y z
i
, medido alrededor del eje x
i
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 9 / 23
Matrices de paso
A
i
=
2
6
6
4
C
i
S
i
C
i
S
i
S
i
a
i
C
i
S
i
C
i
C
i
C
i
S
i
a
i
S
i
0 S
i
C
i
d
i
0 0 0 1
3
7
7
5
(1)
Para obtener las matrices de paso A
i
se sustituyen los parmetros
i
,
d
i
, a
i
y
i
(mostrados en la siguiente tabla) en la matriz dada en (1).
i
i
d
i
a
i
i
1
1
l
1
0 90
2
2
l
2
0 90
3 0
d
3
0 0
4
4
l
4
0 90
5
5
0 0 90
6
6
l
6
0 0
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 10 / 23
Matrices de paso
A
i
=
2
6
6
4
C
i
S
i
C
i
S
i
S
i
a
i
C
i
S
i
C
i
C
i
C
i
S
i
a
i
S
i
0 S
i
C
i
d
i
0 0 0 1
3
7
7
5
i
i
d
i
a
i
i
1
1
l
1
0 90
Para A
1
se tiene:
A
1
=
2
6
6
4
C
1
S
1
C
90
1
S
90
(0) C
1
S
1
C
1
C
90
1
S
90
(0) S
1
0 S
90
C
90
l
1
0 0 0 1
3
7
7
5
=
2
6
6
4
C
1
0 S
1
0
S
1
0 C
1
0
0 1 0 l
1
0 0 0 1
3
7
7
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 11 / 23
Matrices de paso
A
i
=
2
6
6
4
C
i
S
i
C
i
S
i
S
i
a
i
C
i
S
i
C
i
C
i
C
i
S
i
a
i
S
i
0 S
i
C
i
d
i
0 0 0 1
3
7
7
5
i
i
d
i
a
i
i
2
2
l
2
0 90
Para A
2
se tiene:
A
2
=
2
6
6
4
C
2
S
2
C
90
2
S
90
(0) C
2
S
2
C
2
C
90
2
S
90
(0) S
2
0 S
90
C
90
l
2
0 0 0 1
3
7
7
5
=
2
6
6
4
C
2
0 S
2
0
S
2
0 C
2
0
0 1 0 l
2
0 0 0 1
3
7
7
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 12 / 23
Matrices de paso
A
i
=
2
6
6
4
C
i
S
i
C
i
S
i
S
i
a
i
C
i
S
i
C
i
C
i
C
i
S
i
a
i
S
i
0 S
i
C
i
d
i
0 0 0 1
3
7
7
5
i
i
d
i
a
i
i
3 0
d
3
0 0
Para A
3
se tiene:
A
3
=
2
6
6
4
C
0
S
0
C
0
S
0
S
0
(0) C
0
S
0
C
0
C
0
C
0
S
0
(0) S
0
0 S
0
C
0
d
3
0 0 0 1
3
7
7
5
=
2
6
6
4
1 0 0 0
0 1 0 0
0 0 1 d
3
0 0 0 1
3
7
7
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 13 / 23
Matrices de paso
A
i
=
2
6
6
4
C
i
S
i
C
i
S
i
S
i
a
i
C
i
S
i
C
i
C
i
C
i
S
i
a
i
S
i
0 S
i
C
i
d
i
0 0 0 1
3
7
7
5
i
i
d
i
a
i
i
4
4
l
4
0 90
Para A
4
se tiene:
A
4
=
2
6
6
4
C
4
S
4
C
90
4
S
90
(0) C
4
S
4
C
4
C
90
4
S
90
(0) S
4
0 S
90
C
90
l
4
0 0 0 1
3
7
7
5
=
2
6
6
4
C
4
0 S
4
0
S
4
0 C
4
0
0 1 0 l
4
0 0 0 1
3
7
7
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 14 / 23
Matrices de paso
A
i
=
2
6
6
4
C
i
S
i
C
i
S
i
S
i
a
i
C
i
S
i
C
i
C
i
C
i
S
i
a
i
S
i
0 S
i
C
i
d
i
0 0 0 1
3
7
7
5
i
i
d
i
a
i
i
5
5
0 0 90
Para A
5
se tiene:
A
5
=
2
6
6
4
C
5
S
5
C
90
5
S
90
(0) C
5
S
5
C
5
C
90
5
S
90
(0) S
5
0 S
90
C
90
0
0 0 0 1
3
7
7
5
=
2
6
6
4
C
5
0 S
5
0
S
5
0 C
5
0
0 1 0 0
0 0 0 1
3
7
7
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 15 / 23
Matrices de paso
A
i
=
2
6
6
4
C
i
S
i
C
i
S
i
S
i
a
i
C
i
S
i
C
i
C
i
C
i
S
i
a
i
S
i
0 S
i
C
i
d
i
0 0 0 1
3
7
7
5
i
i
d
i
a
i
i
6
6
l
6
0 0
Para A
6
se tiene:
A
6
=
2
6
6
4
C
6
S
6
C
0
6
S
0
(0) C
6
S
6
C
6
C
0
6
S
0
(0) S
6
0 S
0
C
0
l
6
0 0 0 1
3
7
7
5
=
2
6
6
4
C
6
S
6
0 0
S
6
C
6
0 0
0 0 1 l
6
0 0 0 1
3
7
7
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 16 / 23
Matriz de transformacin homognea
La matriz de transformacin homognea que relaciona los
referenciales base y del efector nal se calcula como:
0
o
0
x
0
y
0
z
6
o
6
x
6
y
6
z
1
l
*
3
d
4
l
6
l
2
l
6
0
T
T
n
0
=
n
i =1
A
i
= A
1
A
2
A
3
A
n
(2)
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 17 / 23
Matriz de transformacin homognea
De la expresin (2) se tiene:
T
6
0
=
6
i =1
A
i
= A
1
A
2
A
3
A
4
A
5
A
6
2
6
6
4
C
1
0 S
1
0
S
1
0 C
1
0
0 1 0 l
1
0 0 0 1
3
7
7
5
2
6
6
4
C
2
0 S
2
0
S
2
0 C
2
0
0 1 0 l
2
0 0 0 1
3
7
7
5
2
6
6
4
1 0 0 0
0 1 0 0
0 0 1 d
3
0 0 0 1
3
7
7
5
2
6
6
4
C
4
0 S
4
0
S
4
0 C
4
0
0 1 0 l
4
0 0 0 1
3
7
7
5
2
6
6
4
C
5
0 S
5
0
S
5
0 C
5
0
0 1 0 0
0 0 0 1
3
7
7
5
2
6
6
4
C
6
S
6
0 0
S
6
C
6
0 0
0 0 1 l
6
0 0 0 1
3
7
7
5
Tenga en cuenta que en general el producto de matrices no es
conmutativo, es decir, AB 6= BA
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 18 / 23
Matriz de transformacin homognea
Realizando las operaciones A
1
A
2
, A
3
A
4
y A
5
A
6
resulta:
T
6
0
=
2
6
6
4
C
1
C
2
S
1
C
1
S
2
l
2
S
1
S
1
C
2
C
1
S
1
S
2
l
2
C
1
S
2
0 C
2
l
1
0 0 0 1
3
7
7
5
2
6
6
4
C
4
0 S
4
0
S
4
0 C
4
0
0 1 0 d
3
+ l
4
0 0 0 1
3
7
7
5
2
6
6
4
C
5
C
6
C
5
S
6
S
5
l
6
S
5
S
5
C
6
S
5
S
6
C
5
l
6
C
5
S
6
C
6
0 0
0 0 0 1
3
7
7
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 19 / 23
Matriz de transformacin homognea
La matriz de transformacin homogna T
6
0
est dada por:
T
6
0
=
2
6
6
4
r
11
r
12
r
13
d
x
r
21
r
22
r
23
d
y
r
31
r
32
r
33
d
z
0 0 0 1
3
7
7
5
donde
r
11
= C
1
C
2
(C
4
C
5
C
6
S
4
S
6
) S
1
(S
4
C
5
C
6
+ C
4
S
6
)
C
1
S
2
S
5
C
6
r
12
= C
1
C
2
(C
4
C
5
S
6
S
4
C
6
) S
1
(S
4
C
5
S
6
+ C
4
C
6
)
+C
1
S
2
S
5
S
6
r
13
= C
1
C
2
C
4
S
5
S
1
S
4
S
5
+ C
1
S
2
C
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 20 / 23
Matriz de transformacin homognea
La matriz de transformacin homogna T
6
0
est dada por:
T
6
0
=
2
6
6
4
r
11
r
12
r
13
d
x
r
21
r
22
r
23
d
y
r
31
r
32
r
33
d
z
0 0 0 1
3
7
7
5
donde
r
21
= S
1
C
2
(C
4
C
5
C
6
S
4
S
6
) + C
1
(S
4
C
5
C
6
+ C
4
S
6
)
S
1
S
2
S
5
C
6
r
22
= S
1
C
2
(C
4
C
5
S
6
S
4
C
6
) + C
1
(S
4
C
5
S
6
+ C
4
C
6
)
+S
1
S
2
S
5
S
6
r
23
= S
1
C
2
C
4
S
5
+ C
1
S
4
S
5
+ S
1
S
2
C
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 21 / 23
Matriz de transformacin homognea
La matriz de transformacin homogna T
6
0
est dada por:
T
6
0
=
2
6
6
4
r
11
r
12
r
13
d
x
r
21
r
22
r
23
d
y
r
31
r
32
r
33
d
z
0 0 0 1
3
7
7
5
donde
r
31
= S
2
C
4
C
5
C
6
C
2
S
5
C
6
+ S
2
S
4
S
6
r
32
= S
2
(C
4
C
5
S
6
S
4
C
6
) + C
2
S
5
S
6
r
33
= S
2
C
4
S
5
+ C
2
C
5
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 22 / 23
Matriz de transformacin homognea
La matriz de transformacin homogna T
6
0
est dada por:
T
6
0
=
2
6
6
4
r
11
r
12
r
13
d
x
r
21
r
22
r
23
d
y
r
31
r
32
r
33
d
z
0 0 0 1
3
7
7
5
donde
d
x
= l
2
S
1
+ l
6
(C
1
C
2
C
4
S
5
S
1
S
4
S
5
+ C
1
S
2
C
5
)
+l
4
C
1
S
2
+ d
3
C
1
S
2
d
y
= l
6
(S
1
C
2
C
4
S
5
+ C
1
S
4
S
5
+ S
1
S
2
C
5
) + l
2
C
1
+l
4
S
1
S
2
+ d
3
S
1
S
2
d
z
= l
6
(S
2
C
4
S
5
+ C
2
C
5
) + d
3
C
2
+ l
4
C
2
+ l
1
Jorge E. Lavn D. (ULSA) C.D. Manipulador Stanford 10/Agosto/2012 23 / 23
