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