1.

- Parmetros Denavith-Hartemberg:
i
i
d
i
a
i

i

1
1
d
1
L
1
0
o

2
2
-d
2
L
2
180
o

3 0
o
d
3
0 0
o


Variables articulares:
i q
i

1 q
1
=
1

2 q
2
=
2

3 q
3
= d
3



2.- Representacin Denavith-Hartemberg:
Con n=3 grados de libertad. Para k=1, 2, 3.
(
(
(
(

1 0 0 0
cos 0
a cos cos cos
cos a cos cos
1
k k k
k k k k k k k
k k k k k k k
k
k
d sen
sen sen sen
sen sen sen
T
o o
u u o u o u
u u o u o u

(
(
(
(

= =
1 0 0 0
0 0 0
0 0 0
0 0 0
3
2
2
1
1
0
3
0
Pz az sz nz
Py ay sy ny
Px ax sx nx
T T T T

Resolviendo:
(
(
(
(

+ + + +
+ + + +
=
1 0 0 0
1 0 0
) ( 0 ) cos( ) (
) cos( cos 0 ) ( ) cos(
3 2 1
2 1 2 1 1 2 1 2 1
2 1 2 1 1 2 1 2 1
3
0
q d d
q q sen L senq L q q q q sen
q q L q L q q sen q q
T
Donde:
La matriz de rotacin respecto al sistema referencial base es:
(
(
(

+ +
+ +
=
1 0 0
0 ) cos( ) (
0 ) ( ) cos(
2 1 2 1
2 1 2 1
3
0
q q q q sen
q q sen q q
R
El vector posicin del efector final es:

(
(
(

+ +
+ +
=
(
(
(

=
3 2 1
2 1 2 1 1
2 1 2 1 1
3
0
) ( ) (
) cos( ) cos(
q d d
q q sen L q sen L
q q L q L
P
P
P
P
z
y
x

Cinemtica:
Clculo del Jacobiano:
Primero definamos matemticamente el jacobiano.
El Jacobiano de Traslacin es:
(
(
(
(
(
(
(

c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
=
(

c
c
c
c
c
c
=
3 2 1
3 2 1
3 2 1
3 2 1
q
P
q
P
q
P
q
P
q
P
q
P
q
P
q
P
q
P
q
P
q
P
q
P
Jv
z z z
y y y
x x x

El Jacobiano de Rotacin es:
| |
2
2
1
1
0
0
z z z J e =
Donde:
{ base. l referencia sistema el desde visto n, articulaci sima - k la de z" " direccin la en unitario Vector
n). traslaci (de prismtica n articulaci una es Si 0
rotacin). (de cilndrica n articulaci una es Si 1
k z
k
k
=

=


El sistema a pesar de tener 3 grados de libertad, a la hora de moverse, no utiliza los 3 grados, de
tal forma que es redundante.
2 Movimientos secuenciales:
1) Movimiento Horizontal (Emplea 2 Grados de libertad:
1
q y
2
q ).
Para este caso el robot se mueve en un plano, es decir que la variable articular q3
permanece constante. Al calcular el jacobiano obtendremos un sistema con determinante
cero. Por este motivo si ignoramos la variable q3, el jacobiano sera de 2x2 si admite
inversa, y es posible calcular la cinemtica inversa.
2) Movimiento Vertical (Emplea un solo grado de libertad:
3
q ).
Para este caso el robot se mueve en una lnea vertical, primero hacia arriba, y luego hacia
abajo; mientras que las variables articulares q1 y q2 permanecen constantes. Es decir el
jacobiano obtendra determinante cero. Si ignoramos q1 y q2, entonces el jacobiano sera
de 1x1. Y s admitira inversa, de esta forma podremos calcular la cinemtica inversa del
robot taladro.
Para cada uno de estos 2 tipos de movimientos se realizar por separado el anlisis Cinemtico y
Dinmico.
3.- Cinemtica:
3.1.- Anlisis del Movimiento Horizontal:
3.1.1.-Geometra Directa del ROBOT:
(

+ +
+ +
=
(

=
) ( ) (
) cos( ) cos(
2 1 2 1 1
2 1 2 1 1
q q sen L q sen L
q q L q L
P
P
P
y
x
H

Ambas son articulaciones de rotacin, es decir: 1
1 0
= =
Los vectores unitarios de los ejes de articulacin son:
1
0
0
,
1
0
0
1 0
(
(
(

=
(
(
(

= z z
3.1.2.- Geometra Inversa del Robot:
Las variables articulares q
1
y q
2
en funcin de la posicin horizontal:
...(2) ) ( ) (
...(1) ) cos( ) cos(
2 1 2 1 1
2 1 2 1 1
q q sen L q sen L P
q q L q L P
y
x
+ + =
+ + =

Elevamos al cuadrado ambas ecuaciones miembros y sumamos:
2 2
) 2 ( (1) +
) ( ) ( 2
) cos( ) cos( 2 ) ( ) ( cos ) ( ) ( cos
2 1 1 2 1
2 1 1 2 1 2 1
2
2
2 2 1
2
2
2 1
2
2
1 1
2
2
1
2 2
q q sen q sen L L
q q q L L q q sen L q q L q sen L q L P P
y x
+
+ + + + + + + + = +

Observamos que fcilmente pueden simplificarse senos y cosenos cuadrados, suma de senos y
cosenos al cuadrado del mismo ngulo es 1.
Recordando identidades trigonomtricas:
) tan ( ) ( ) cos(
cos cos ) cos(
cos cos ) (
1 cos
1 2 2
2 2
|
.
|

\
|
+ + = +
= +
+ = +
= +

b
a
x sen b a x bsen x a
sen sen
sen sen sen
x x sen
| o | o | o
o | | o | o


Aplicando la primera identidad:
)) ( ) ( ) cos( ) (cos( 2
2 1 1 2 1 1 2 1
2
2
2
1
2 2
q q sen q sen q q q L L L L P P
y x
+ + + + + = +
Aplicando la tercera identidad, con
2 1
y q q = = | o .
) cos( 2
2 2 1
2
2
2
1
2 2
q L L L L P P
y x
+ + = +
Despejando de esta a q
2
:
2 1
2
2
2
1
2 2
2
2
) cos(
L L
L L P P
q
y x
+
=
Despejando:
|
|
.
|

\
|
+
=

2 1
2
2
2
1
2 2
1
2
2
cos
L L
L L P P
q
y x
(3)
De la ecuacin 1 y 2, despejamos los trminos que suman q
1
+q
2
.
...(5) ) ( ) (
...(4) ) cos( ) cos(
2 1 2 1 1
2 1 2 1 1
q q sen L q sen L P
q q L q L P
y
x
+ =
+ =


Elevo al cuadrado ambos trminos y los sumo:
2 2
(5) (4) +
) ( ) ( cos ) ( 2 ) cos( 2 ) ( ) ( cos
2 1
2
2
2 2 1
2
2
2 1 1 1 1 1
2
2
1 1
2
2
1
2 2
q q sen L q q L q sen L P q L P q sen L q L P P
y x y x
+ + + = + + +
Aplicando nuevamente la primera identidad trigonomtrica:
2
2 1 1 1 1
2
1
2 2
) ( 2 ) cos( 2 L q sen L P q L P L P P
y x y x
= + +
Despejamos q1:
1
2
) ( ) cos(
1
2
2
2
1
2 2
1 1
cte
L
L L P P
q sen P q P
y x
y x
=
+ +
= + (6)
Alternativa del Ing. Machuca:
Elevamos al cuadrado (6):
( )
2
1 1 1
2
2
1
2
2
1 ) ( ) cos( 2 ) ( ) ( cos cte q sen q P P q sen P q P
y x y x
= + +

Dividimos todo entre coseno al cuadrado.
( )
) ( cos
1
) ( cos
) ( ) cos(
2
) ( cos
) (
) ( cos
) ( cos
1
2
2
1
2
1 1
1
2
1
2
2
1
2
1
2
2
q
cte
q
q sen q
P P
q
q sen
P
q
q
P
y x y x
= + +

Simplificando:
( ) )) ( (sec 1 ) tan( 2 ) ( tan
1
2
2
1 1
2
2 2
q cte q P P q P P
y x y x
= + +

( ) )) ( tan 1 ( 1 ) tan( 2 ) ( tan
1
2
2
1 1
2
2 2
q cte q P P q P P
y x y x
+ = + +

( ) ( ) 0 1 ) tan( 2 ) ( tan ) 1 (
2 2
1 1
2
2 2
= + + cte P q P P q cte P
x y x y

Aplicando la cuarta identidad trigonomtrica:
1
2
2
2
1
2 2
1
1
2
) tan (
L
L L P P
P
P
q sen
y x
y
x
+ +
=
|
|
.
|

\
|
+


Despejando:
|
|
.
|

\
|
+ +
+
|
|
.
|

\
|
=

1
2
2
2
1
2 2
1 1
1
2
tan
L
L L P P
sen
P
P
q
y x
y
x
(7)
Entonces, resumiendo las ecuaciones de la geometra inversa, para este caso, pudo obtenerse
analticamente. En la mayora de robots de ms grados de libertad y/o configuraciones ms
complejas, muchas veces no pueden obtenerse expresiones analticas, en tales casos aplicamos un
mtodo recursivo, dicho mtodo emplea el jacobiano inverso de la velocidad.
|
|
.
|

\
|
+ +
+
|
|
.
|

\
|
=

1
2
2
2
1
2 2
1 1
1
2
tan
L
L L P P
sen
P
P
q
y x
y
x
(7)


|
|
.
|

\
|
+
=

2 1
2
2
2
1
2 2
1
2
2
cos
L L
L L P P
q
y x
(3)




JACOBIANOS:
Los jacobianos respectivos de traslacin y rotacin son:
| |
(
(
(

= =
(

+ + +
+ +
=
(
(
(
(

c
c
c
c
c
c
c
c
=
(

c
c
c
c
=
1 1
0 0
0 0
) cos( ) cos( ) cos(
) ( ) ( ) (
1
1
0
0
2 1 2 2 1 2 1 1
2 1 2 2 1 2 1 1
2 1
2 1
2 1
z z J
q q L q q L q L
q q sen L q q sen L q sen L
q
P
q
P
q
P
q
P
q
P
q
P
Jv
H
y y
x x
H
e


3.1.2.-PUNTOS DE SINGULARIDAD:
Para el jacobiano de Traslacin: podemos obtener los puntos de singularidad:
En otras palabras, Cules son los valores de q tales que 0 =
H
Jv ?
Al operar y simplificar obtenemos que el jacobiano es: 0
2 2 1
= = senq L L Jv
H

Es decir: 0
2
= senq Esto se cumple para mltiplos enteros de
2
q .
Z e = con ,..., 2 , , 0
2
m m q t t t .
Lo que significa que en dichos valores la cinemtica inversa no puede calcularse, pues depende de
la matriz inversa jacobiana.

3.1.3.-CALCULO DE VELOCIDADES:
3.1.3.1.-Velocidad de Traslacin:
La velocidad de traslacin es:
o
H p
o
o
p
p
q Jv V
q
q
q
P
q
P
t
q
t
q
q
P
q
P
V
t
q
q
P
t
q
q
P
dt
dP
V
=
(
(

c
c
c
c
=
(
(
(

c
c
c
c
(

c
c
c
c
=
c
c
c
c
+
c
c
c
c
= =
2
1
2 1
2
1
2 1
2
2
1
1

Reemplazando valores tendremos los siguientes resultados:
(
(

+ + +
+ +
=
o
o
p
q
q
q q L q q L q L
q q sen L q q sen L q sen L
V
2
1
2 1 2 2 1 2 1 1
2 1 2 2 1 2 1 1
) cos( ) cos( ) cos(
) ( ) ( ) (



3.1.3.2.-Velocidad de Traslacin:
La velocidad de rotacin es:
o
H p
q Je e =
(
(

(
(
(

=
o
o
p
q
q
2
1
1 1
0 0
0 0
e
3.2.- Cinemtica Inversa del Robot Taladro:

El objetivo de este anlisis es encontrar un mtodo analtico o numrico recursivo, que muestre
las variables articulares en funcin de las coordenadas cartesianas horizontales.

Se sabe que:
|
.
|

\
|
=
dt
dq
J Vp
H
) (
Es lo mismo decir que:
|
.
|

\
|
=
dt
dq
J
dt
dP
H
) (
Si lo analizamos en forma diferencial sera: ( ) dq J dP
H
) ( =
Si el jacobiano admite inversa, entonces: ( ) dP J dq
H
) (
1
=
Un anlisis en forma iterativa de esta ecuacin diferencial sera:
) ))( ( (
1
1
1 k k k H k k
P P q J q q =
+

+

Despejando el k+1-simo valor de q:
) ))( ( (
1
1
1 k k k H k k
P P q J q q + =
+

+

Pero, la posicin
1 + k
P es la posicin que deseamos tener, es decir es la posicin final deseada.
Deseado P P
f k
= =
+1

Luego, nuestra ecuacin diferencial iterativa sera:
) ))( ( (
1
1 k f k H k k
P P q J q q + =

+

Pero: ) (
k k
q P P =
El nmero de iteraciones, lo restringimos nosotros, dependiendo de un valor de error mnimo:
El sistema seguir iterando mientras: ) (
k
q P Pf Eps <
Para nuestro sistema:
El Jacobiano es:
(

+ + +
+ +
=
) cos( ) cos( ) cos(
) ( ) ( ) (
2 1 2 2 1 2 1 1
2 1 2 2 1 2 1 1
q q L q q L q L
q q sen L q q sen L q sen L
Jv
H

El Jacobiano inversa es:
2 2 1
2 1 2 1 1 2 1 2 1 1
2 1 2 2 1 2
1
) ( ) ( ) cos( ) cos(
) ( ) cos(
senq L L
q q sen L q sen L q q L q L
q q sen L q q L
Jv
H
(

+ +
+ +
=

En forma iterativa:

4.- C
5.- D D
6.- Di I:
#include<>
Int main(){
}