Robot RPP

Marcos de referencia y parmetros de eslabn por DH

X1 q1 P2
P1
R1
X3
X2 q2 {3}
{1}
Y1 Y3
Z2 X4
Y2
{2}
Z3
Z1 Y4 q3
l1
X0 {4 }
Z4
Y0
{0}

Z0

Nota: el marco referencial {2} que es de la articulacin prismtica 1 , se encuentra

en palmado con el marco {1} ya que por convencin de D.H no se puede tener una
distancia en la revoluta 1, eso hace que fsicamente la articulaciones revoluta 1 y
prismtica 1 se encuentren empalmados, pero en el bosquejo del robot se puso
separado para mayor compresin en el anlisis del robot.

Tabla de parmetros:

i Ai i di i
1 l1 0 0 0

2 0 90 0 q1

3 0 -90 q2 0

4 0 0 q3 0
Restricciones mecnicas de las articulaciones.

Articulacin Limitacin
R1 Su movimiento angular puede variar de
Revoluta
z1
0 a 180 en . Corresponde a la
q1
variable .

P1 Debido a su estructura su movimiento

- Prismtica
queda restringido de 15 cm a 50cm. a lo
z2
largo de . Corresponde a la variable
q2
.

P2 Debido a su estructura su movimiento

- Prismtico
queda restringido de 10 cm a 50cm. a lo
z3
largo de . Corresponde a la variable
q3
.

l1
Con un constante de 15 cm.

Cinemtica Directa:
Matriz Generalizada

[ ]
C i Si C i Si S i ai Ci
i1 S i Ci C i Ci S i ai Si
T=
i
0 S i C i di
0 0 0 1

Cinemtica para el primer eslabn.

 1 0 0 l1
0 1 0 0
1T
0
0 0 1 0

0 0 0 1

Cinemtica para el segundo eslabn.

C1 0 S1 0
S 0 C 0
2T
1 1 1

0 1 0 0

0 0 0 1

Cinemtica para el tercer eslabn

1 0 0 0
0 0 1 0
T
2
3
0 1 0 q2

0 0 0 1

Cinemtica para el cuarto eslabn

1 0 0 0
0 1 0 0
4T
3
0 0 1 q3

0 0 0 1

Modelado Cinemtica directa:

 0 1 2 3
Estas matrices pueden multiplicarse de esta forma 1 T 2T 3 T 4 T para poder
0
obtener el resultado de 4 T .

0
4T 01T 21T 23T 34T
Sustituyendo obtenemos:

1 0 0 l1 C1 0 S1 0 1 0 0 0 1 0 0 0
0 1 0 0 S 0 C 0 0 0 1 0 0 1 0 0
1 1
0 0 1 0 0 1 0 0 0 1 0 q2 0 0 1 q3
0
4T
0 0 0 1

0 0 0 1

0 0 0 1

0 0 0 1

= * * *

0
T
4
Al realizar las multiplicaciones se obtiene nuestra Matriz

C1 S1 0 q2 S1 l1
S C1 0 q2 C1
0
T 1
4
0 0 1 q3

0 0 0 1

CINEMATICA INVERSA:
Para encontrar la inversa de este robot solo utilizaremos el mtodo
algebraico, este mtodo trata en encontrar el vector "q" (variables del
robot) solo usando la matriz homognea de nuestro modelo de
Cinemtica Directa.
 C1 S1 0 q2 S1 l1
S C1 0 q2 C1
0
T 1
4
0 0 1 q3

0 0 0 1

Esta es la matriz de nuestra Cinemtica Directa, esta matriz se componen de

una matriz de rotacin, as como un vector de posicin:

r11 r12 r13 PX

r PY
r22 r23 R P
0
T 21

4
r31 r32 r33 PZ 0 1

0 0 0 1

Mtodo algebraico:
Vector de posicin;

P Px , Py , Pz T

Matriz de Rotacin;

c1 s1 0
s c1 0
R 1
0 0 1

Con la matriz de cinemtica directa podemos calcular el vector de

q1 , q2 , q3
parmetros de unin q = [ ]

q3
Para encontrar usamos el vector de posicin.

P Px , Py , Pz T

[ ]
S 1 q 2+ l1

P= C 1 q2
q3

q3 =P z

q1
Para encontrar , necesitamos la matriz de rotacin.

c1 s1 0
s c1 0
R 1
0 0 1

r 21
q1 =
r 11
tan

q r 21
1= tan1
r 11

Para encontrar q 2 , tambin necesitamos del vector de

posicin pero para eso ya tenemos que haber calculado
q1
.

Vector de posicin:

[ ][ ]
PX S 1 q2 +l 1

P= PY = C1 q2
PZ q3
 S 1 q2 +l 1=P X ..(ec .1)

C 1 q2=PY (ec .2)

Con estas dos ecuaciones vamos a encontrar q 2

S 1 q2 +l 1C 1 q 2=P X + PY ..ec .(3)

S 1 q2C 1 q 2=P X + P Y l 1

q 2(C 1+ S 1)=P X + PY l 1

Por lo tanto:
1
q 2=
(C 1 +S 1)
[ P X + PY l1 ]

Ya tenemos los tres parmetros del vector de unin:

q r 21
1= tan1
r 11

1
q 2=
(C 1 +S 1)
[ P X + PY l1 ]

q3 =P z

Vamos a comprobar nuestra cinemtica Directa as como nuestra

Cinemtica Inversa.
 Primero le daremos valores a nuestras variables "q" y veremos cmo quedan
nuestros marcos de referencia y comparar visualmente si cumple nuestra D.H con
la visualizacin del robot, despus con nuestra cinemtica inversa veremos que
los parmetros que salgan sean iguales a los que introducimos inicialmente a
nuestro sistema.

Primer ejemplo:
El vector q es:

[]
q1
q = q 2
q3

Sustituyendo por valores:

[]

0
q = 30
40

Vamos a ver como quedara nuestra cinemtica Directa

1 0 0 15
0 1 0 30
4T
0
0 0 1 40

0 0 0 1

Los marcos de nuestro robot quedan de la siguiente forma:

X1 q1 =

0 X3
Y 1Z 1 { 1 } X 2 Z2 Y 4 Z4 Y3
Y2
{2}
 q 2=30

{3}

X4 Z3
l 1=15

q3 =40
X0 {4 }
{0}
Y0

Z0

Vamos a encontrar la matriz homognea va visual, como ya sabemos la matriz se

componen de una matriz de rotacin y un vector de posicin.

1 0 0
0 1 0
4R
0
0 0 1

Como vemos esta matriz nos queda la identidad por estar coincidentes
los marcos del efector final con el marco {0}

[ ]
15
0
4 p = 30
40

Este es el vector de posicin obtenido del marco 0 al 4.

Matriz homognea obtenida mediante la visualizacin de los marcos.

 1 0 0 15
0 1 0 30
4T
0
0 0 1 40

0 0 0 1

Como vemos esta matriz es igual a la matriz encontrada con la D.H.

Ahora vamos a comprobar nuestra Cinemtica Inversa.

Formulas ya encontradas para cinemtica inversa:

q r 21
1= tan1
r 11

1
q 2= [ P + P l ]
(C 1 +S 1) X Y 1

q3 =P z

r11 r12 r13 PX

r r22 r23 PY
0
T 21

4
r31 r32 r33 PZ

0 0 0 1

1 0 0 15
0 1 0 30
4T
0
0 0 1 40

0 0 0 1
 q r 21 0
1= tan1 =tan1 =tan1 0=0
r 11 1

1 1
q 2=
(C 1 +S 1)
[ P X + PY l 1 ]=
1
[ 153015 ] =30 cm

q3 =P =40
z

Como vemos los valores encontrados de las variables q con las formulas de la
cinemtica inversa, son igual a los valores introducidos al inicio del robot para
sacar el modelos cinemtica directa.

[]

0
q = 30
40

As que este ejemplo cumple con la validacin.

segundo ejemplo:
El vector q es:

[]
q1
q = q 2
q3

[]

90
q = 35
45

Vamos a ver como quedara nuestra cinemtica Directa

 1 0 0 15
0 1 0 30
4T
0
0 0 1 40

0 0 0 1

Los marcos de nuestro robot quedan de la siguiente forma:

X3
{3}
q3 =
Y3
Z3
45
X4 Z2

q2 =
q1 =
{4 } Y4
{2}
Z4 35
X90
1
{1}
Y1
X2
Y2

Z1
l 1=15
X0
{0}
Y0

Z0

Vamos a encontrar la matriz homognea va visual, como ya sabemos la matriz se

componen de una matriz de rotacin y un vector de posicin.
 0 1 0
1 0 0
4R
0
0 0 1


"Matriz de rotacion"

[]
50
0
4 p= 0 Vector de posicin obtenido del marco 0 al 4.
45

Matriz homognea obtenida mediante la visualizacin de los marcos.

0 1 0 50
1 0 0 0
4T
0
0 0 1 45

0 0 0 1

Como vemos esta matriz es igual a la matriz encontrada con la D.H.

Ahora vamos a comprobar nuestra Cinemtica Inversa.

Si esta es correcta mediante nuestra matriz homognea.

Formulas ya encontradas para cinemtica inversa:

q r 21
1= tan1
r 11

1
q 2=
(C 1 +S 1)
[ P X + PY l1 ]
 q3 =P z

r11 r12 r13 PX

r PY
T
0 21 r22 r23

4
r31 r32 r33 PZ

0 0 0 1

0 1 0 50
1 0 0 0
4T
0
0 0 1 45

0 0 0 1

q r 21 1
1= tan1 =tan1 =tan1 =9 0
r 11 0

1 1
q 2=
(C 1 +S 1)
[ P X + PY l 1 ] = [ 5015 ]=35 cm
1

q3 =P =45
z

Como vemos los valores encontrados de las variables q con las formulas de la
Cinemtica inversa, son igual a los valores introducidos al inicio del robot para
sacar el modelos cinemtica directa.

q = 35
45[]
90
 Jacobianos
Primeramente para sacar la matriz jacobiana necesitamos saber los de
velocidades lineales y angulares de nuestros eslabones ya que el jacobiano se
compone de estos dos.

Jacobiano de velocidad angular:

La matriz de velocidades angulares nos dice que

w 14=[ 1 R00 Z 0 q 1+ 2 R01 Z 1 q 2 + 3 R02 Z 2 q 3 ]

Nota: nosotros en nuestro robot usamos la velocidad angular w 14 de 4 a 1 ya

que como el marco {0}, es usada para encontrar la DH. Y ese marco no tiene
articulacin por lo tanto no tiene masa y eso hace que no tenga velocidad
angular ni lneal.

Donde
Z i es el vetor donde nuestro eslabon estarotando por esos es conveniente
usar la convencion de DH ya que esta nos indica que todas la traslaciones o

rotaciones se coloque en
Z1

NOTA: dependiendo el tipo de elemento tendra velocidad angular ya que si es

un elemnto rotatorio posee velocidad angular pero un primatico no por lo tanto

=1 siel elemento es rotatorio .

=0 si el elemento es primatico .

R=es lamatriz de rotacion.

Sustituyendo la formula tenemos que

[[ [ ][ ] [ ][ ] ]
cos 1 0 sen 1 cos 1 sen 1 0

][ ]
1 0 0 0 0 0
w 14= ( 1 ) 0 1 0 0 q 1 +0 sen 1 0 cos 1 sen 1 cos 1 0
0 q 2+ 0 0 q 3
0 0 1 1 0 1 0 1 0 0 1 1
0 0 0 0 0 0

[ ][ ]
0 0 0 q 1
1
w = 0 0 0 q 2
4
1 0 0 q 3

Donde:

[ ]
0 0 0
J= 0 0 0
1 0 0

Jacobiano velocidad Lineal:

Ahora se va a calcular la velocidad lineal de cada elemento donde se utilizara
la orientacion del efector final respecto al marco referencial cero ya que los
cambios de posicion afectan ala velocidad y para saber los cambios de
posicion respecto al tiempo se utilizaran derivadas parciales entonces la
ecuacion quedaria

[ ]
Px Px Px
q 1 q2 q3
Py Py Py

q 1 q2 q3
Pz Pz Pz
q 1 q2 q3

Donde px,py y pz son las componentes de la orientacion de 4 a 1

Nota: Usaremos la orientacin de 4 a 1 ya que en el primer marco {0}

no tiene velocidad lneal.
 [ ]
s1 q2 +l 1
1
4 O c 1 q 2
q3

1
O q1
Derivamos 4 parcialmente respecto a y nos queda:

[ ]
14O c 1 q2
s q
q 1 1 2
0

q2
Ahora respecto a :

14O S1
q 2
C1
0 [ ]
q3
y para terminar respecto a :

14O 0
q 3
0
1 []
Por lo tanto tenemos que la velocidad lineal es

[ ]
c1 q2 S 1 0
JV = s 1 q 2 C 1 0
0 0 1

Y la matriz jacobiana de velocidad angular y lineal quedaria de la siguiente

forma:
 [ ]
c1 q 2 S1 0
s1 q2 C1 0
Jv 0 0 1
J= =
Jw 0 0 0
0 0 0
1 0 0

Calculo de jacobiano pseudo-inversa

Tenemos que recurrir a este tipo de inversa porque nuestra matriz jacobiana no es
cuadrada. El clculo de la pseudo inversa est dado por la siguiente manipulacin
algebrica:

Pseudo inversa a izquierda:

Para matrices con mayor nmero de filas que columnas.

+=(J T J )1 J T

.J

[ ]
c1 q 2 S1 0
s1 q2 C1 0
Jv 0 0 1
J= =
Jw 0 0 0
0 0 0
1 0 0

Para el caso de este robot se utiliz la pseudoinversa a izquierdas, debido a que

tiene un mayor nmero de filas que de columnas. Utilizar la pseudoinversa a
derechas lanzara como resultado indeterminacin.

Desarrollando la pseudoinversa a izquierdas.

Nota: Resultado obtenido por medio del programa Matlab.
T 1 T
+=(J J ) J

.J
 [ ]
q2 c 1 q 2 s1 1
2 2
0 0 0 2
+= q +1 2 q +1
2 q +1
2
s1 c 1 0 0 0 0
0 0 1 0 0 0

. J

Modelo Dinmico:
Modelo Dinmico por medio de Euler-Lagrange.

Energa cintica:

K= Energa cintica.
1 T 1 T
EK i= mi v i v i + I zzi i i
2 2

Nota: para realizar este anlisis se va hacer en las articulaciones por lo tanto en el
marco {0} no se llevara a cabo dicho anlisis por qu no contiene masa y por lo
tanto es cero.

Energa cintica 1:

EK1 0
a1
 a2

Energa cintica 2:
El segundo eslabn es revoluta, por lo que la frmula para calcular su
energa cintica es:

0
T
1 1
EK 2 m1V2 V2 I zz1 0 0 q1 0
T

2 2
q1

Centro de masa 1 para calcular la energa cintica 2:

[] []
l1 l1
0
O 2= 0 Sustituyendo por el centro de masa 0
0 0

Ahora ya que se sustituy se tiene que derivar para encontrar la velocidad

 [][]
l 0
d 1
V 2= = 0
dt 0
0 0

Ahora volviendo para calcular la energa cintica del eslabn 2

1 1
EK 2= m1( 0)+ I
2 2

[]
1 0
EK 2=0+ I zz 1 [ 0 0 q1 ] 0
2
q 1

1
EK 2=0+ I zz 1 q 12
2

Nota: como vemos no tenemos una velocidad en esta articulacin por que al
no tener una posicin variable al moverse esto hace que no tenga velocidad.

Energa cintica 3:
El tercer eslabn es prismtico, por lo que la frmula para calcular su
energa cintica es:

1 T
E k 3= m 3 V 3 V 3
2
 [ ] [ ]
l 1 + S1 q2 l 1+ S 1 ( q2a1 )
0
O = C 1 q2
3 Sustituyendo por el centro de masa C 1 ( q 2a1 )
0 0

Ahora ya que se sustituy se tiene que derivar respecto al tiempo para

encontrar la velocidad

[ ][ ]
l +S ( q a ) S1 q 2+C 1 q2 q1C 1 a1 q1
d 1 1 2 1
V 3= C 1 ( q 2a 1) = C 1 q2 +S 1 q2 q1S 1 a1 q1
dt
0 0

Ahora calcularemos la energa cintica del eslabn 3 solo que como es

prismtico este no posee velocidad angular

1 02
E K 3= 2 m 2 V 3

[ ]
S1 q2 +C 1 q 2 q1C 1 a 1 q1
1 T
EK 3= m2 [ S1 q2 +C 1 q 2 q1C 1 a 1 q1 C1 q2 + S1 q2 q1S 1 a 1 q1 0 ] C 1 q2+ S 1 q 2 q1S1 a1 q1
2
0

Al desarrollarla nos resulta de la siguiente manera:

EK 3=
[ 1
2
1 1
m2 a21 q21m2 a1 q21 q2 + m2 q 22 q21 + m2 q 22
2 2 ]

Energa cintica 4:
El cuarto eslabn es prismtico, por lo que la frmula para calcular su
energa cintica es:

1
Ek 4 = m 4 V 4T V 4
2

[ ] [ ]
l 1+ q2 S 1 l 1+ q2 S 1
0
O = q2 C1 q 2 C 1
4 Sustituyendo por el centro de masa
q3 q3 a2

Ahora ya que se sustituy se tiene que derivar respecto al tiempo para

encontrar la velocidad

[ ][ ]
l C q S l 1 S 1 q 1q2 S1q2 C1 q 1
d 1 1 2 1
V 4= l S +q C = l 1 C1 q 1 + q2 C 1q 2 S1 q 1
dt 1 1 2 1
q3a3 q3

Ahora calcularemos la energa cintica del eslabn 4 solo que como es

prismtico este no posee velocidad angular

1
EK 4= m3 V 02
4
2

[ ]
l 1 S1 q 1 q2 S 1q 2 C 1 q 1
1 T
Ek 4 = m3 [l 1 S1 q 1 q2 S 1q 2 C 1 q 1 l 1 C 1 q 1 + q2 C1 q2 S 1 q 1 q3 ] l 1 C 1 q 1+ q2 C1q2 S 1 q 1
2
q3

Al desarrollarla nos resulta de la siguiente manera:

1 2 2 1 2 1
Ek 4 = m3 q 2 q1 + m3 q2+ m3 q3
2
2 2 2

Energa cintica total

Ahora calculando la energa cintica de todo el sistema
 Ek = E k 1+ Ek 2 + Ek 3 + Ek 4

1 1 1 1 1 1 1
Ek = I zz1 q 12 + m2 a 21 q21m2 a1 q21 q2 + m2 q 22 q21+ m2 q22 + m3 q22 q21 + m3 q22 + m3 q23
2 2 2 2 2 2 2

Anlisis de energa potencial

Ahora sacaremos las energas potenciales de sistemas

E p=mgh

Por lo tanto la energa potencial del sistema es

Energa potencial 1:

E p 1=0 g ( b 1) =0

Nota: esta energa potencial es cero por que no tiene masa

Energa potencial 2:

E p 2=m 1 g (l 1)

Energa potencial 3:

E p 3=m2 g ( s 1 ( q2a1 ) +l 1 )=m2 g s1 q 2m2 g s 1 a1+ m2 g l 1

Energa potencial 4:

E p 4 =m3 g s1 q2 +m3 g l 1

Ahora le energa potencial total del sistema es la suma de todas las energas
potenciales:

E p=E p 1+ E p 2+ E p 3 + E p 4

Obtenemos:

E p=m1 g(l 1)+m2 g s1 q 2m2 g s 1 a1+ m2 g l 1+ m3 g s 1 q 2+ m3 g l 1

Anlisis de lagrangiano
Ahora vamos a calcular el lagrangiano, su frmula es la siguiente:

=EK EP

1 2 1 1 2 2 1 2 1 2 2 1 2 1
= I zz 1 q 1 + m2 a1 q1m2 a1 q1 q 2+ m 2 q2 q1 + m2 q2 + m3 q 2 q1+ m3 q 2+ m3 q3m1 g ( l 1) m2 g s1 q2 +m2
2 2 2 2
2 2 2 2 2 2 2

Calculo de torques y fuerzas

Calculando T1

Donde

[ ]
d L

L
dt q i qi

Torque 1:

1=
[ ]
d L

L
dt q 1 q 1

Primero conseguimos la derivada parcial respecto a

q1 :

[ ]

q 1
=m2 a21 q 1+ I zz 1 q 1 +m2 q22 q 12 m2 a1 q 1 q 2 +m3 q1 q 22

Al resultado anterior le sacamos su derivada respecto al tiempo:

[ ]
d
dt q1
=m2 a 21 q 1+ I zz 1 q 1+ m2 q 1 q22 +2 m2 q 2 q 1 q 22 m2 a1 q 2 q12 m2 a 2 q 1 q 2+ m3 q 22 q 1+ 2m3 q1 q2 q2

Para terminar, se deriva parcialmente el lagrangiano respecto a

q1 :

[ ]

q1
=m2 q2 g c1 +m2 a1 g c1m3 q 2 g c 1
Por lo tanto

1=m2 a21 q 1 + I zz 1 q 1 +m2 q1 q22 +2 m2 q 2 q 1 q 22 m2 a1 q 2 q 12 m2 a2 q 1 q 2+m3 q22 q 1 +2 m3 q1 q 2 q2 +m2 q 2 g c 1m2 a1

Fuerza 2:

F2 =
[ ][ ]
d
dt q2


q2

Primero conseguimos la derivada parcial respecto a

q 2 :

[ ]

q 2
=m2 q 2+ m3 q 2

Al resultado anterior le sacamos su derivada respecto al tiempo:

[ ]
d
dt q2
=m2 q2 +m3 q2

Para terminar se deriva parcialmente el lagrangiano respecto a

q2 :

[ ]

q2
=m 2 q 2 q21+ m3 q 2 q21g m2 s1g m3 s 1a1 m 2 q21

Por lo tanto

F2 =m2 q 2+ m3 q 2m2 q 2 q21m3 q2 q21 + g m2 s1 + g m3 s1 +a 1 m2 q21

Fuerza 3:

F3 =
[ ][ ]
d
dt q2


q2

Primero conseguimos la derivada parcial respecto a

q3 :
 [ ]

q3
=m3 q 3

Al resultado anterior le sacamos su derivada respecto al tiempo:

[ ]
d
dt q3
=m3 q 3

Para terminar se deriva parcialmente el lagrangiano respecto a

q3 :

[ ]

q3
=0

Por lo tanto:

F3 =m3 q 3

Matrices de inercia, Coriolis y vector de fuerzas

gravitacionales
Como pudimos observar en el trabajo anterior utilizamos el mtodo de
Lagrange para sacar el modelo dinmico en el cual obtendramos los torques
necesarios que tuvieran que hacer el robot para poderse deslazar en un punto
ahora analizaremos un segundo mtodo en el cual podremos comprobar si
nuestro primer mtodo coincide con este

El modelo dinmico del robot est formado por las siguientes matrices:

[] [] []
1 q 1 q 1
F 2 = D q 2 +C q 2 + [ g ]
F3 q 3 q 3

O vistas de otra forma:

 [ ][ ][ ] [ ][ ] [ ]
1 d 11 d 12 d 13 q 1 c 11 c 12 c 13 q 1 g11
F 2 = d 21 d 22 d 23 q 2 + c 21 c 22 c 23 q 2 + g12
F3 d 31 d 32 d 33 q 3 c 31 c 32 c 33 q 3 g13

Por lo que en base a los resultados obtenidos con nuestros torques,

calcularemos cada una de ellas.

Matriz de Inercias
Para determinar los elementos de
despejamos esta matriz, los
q i
elementos de cada torque que estn multiplicados por un .

Torque 1:

1=m2 a21 q 1 + I zz 1 q 1 +m2 q1 q22 +2 m2 q2 q 1 q 22 m2 a1 q2 q 1 2m2 a2 q 1 q 2 +m3 q22 q 1 +2 m3 q1 q 2 q2+ m2 q 2 g c 1m2 a1

q 1

d 11=I zz + m2 a21 +m2 q222 m2 a1 q2 +m3 q 22

q 2

d 12=0

q 3

d 13=0

Fuerza 2:

F2 =m2 q 2+ m3 q 2m2 q 2 q1m3 q2 q1 + g m2 s1 + g m3 s1 +a 1 m2 q1

2 2 2

q 1

d 21=0
 q 2

d 22=m2+ m3

q 3

d 23 = 0

Fuerza 3:

F3 =m3 q 3

q 1

d 31=0

q 2

d 32=0

q 3

d 33=m3

Ahora podemos sustituir todos estos valores en el modelo que se da

anteriormente de la matriz de inercias, el cual dara la siguiente
forma:
 a
m2 ( 1 2 a1 q2 +q 22)+m3 (q 22)
2

I zz + 0 0
0 m2 +m3

0

[]
q1
D=[ 0 m3 ] q 2
q3

Matriz de coriolis
Lo que haremos ahora
despejar losser elementos de cada
q
torque/fuerza que estn multiplicados por i .

Torque 1:

1=m2 a21 q 1 + I zz 1 q 1 +m2 q1 q22 +2 m2 q2 q 1 q 22 m2 a1 q2 q 1 2m2 a2 q 1 q 2 +m3 q22 q 1 +2 m3 q1 q 2 q2+ m2 q 2 g c 1m2 a1

q 1

c 11=m2 q2 q 2m2 a 2 q 2+ m3 q 2 q2

q 2

c 12=m 2 q 2 q 1m2 a2 q 1 +m 3 q1 q2
 q 3

c 13= 0

Fuerza 2:

F2 =m2 q 2+ m3 q 2m2 q 2 q21m3 q2 q21 + g m2 s1 + g m3 s1 +a 1 m2 q21

q 1

c 21=m2 q 2 q1m3 q 2 q1 +a1 m2 q1

q 2

c 22=0

q 3

c 23=0

Fuerza 3:

F3 =m3 q 3

c 31=0

c 32=0

c 33=0

Al sustituir en la matriz de coriolis nos queda:

 [ ][ ]
c 11 c12 c13 q1
c21 c22 c23 q2
c31 c32 c33 q3

[ ]
m2 q 2 q2m2 a2 q 2 +m3 q2 q2 m 2 q2 q 1m2 a2 q 1 +m 3 q1 q2 0
C= m2 q 2 q1m3 q2 q1 +a1 m 2 q1 0 0
0 0 0

Vector de fuerzas gravitatorias

Este vector est conformado por los elementos que no se utilizan en las otras
dos matrices anteriores, por lo que el resultado queda:

[]
g11
g= g 12
g 13

[ ]
m2 q 2 g c 1m2 a1 g c 1 +m3 q 2 g c 1
g= g m2 s 1+ g m3 s 1
0

Mtodo por Jacobiano para obtener el modelo

dinmico
Matriz de inercias

Empezamos por obtener la matriz de inercias

3
D= ( m i J v J v +J Ri0 I Ri0 J )
T T 0 0 T
cmi cm i i i
i

Para calcular la matriz de inercia

[ ]
1 1 1
D= + +
i=1 i=2 i=3

Analizando primer eslabn

[ 0
J vcm 1= R0 Z0 X ( O cm2O 1 ) J v 2 J v 3
0 0
]

[
1 0 0 0 l1 l1 0 0
J vcm 1= 0 1 0 . 0 X 0 0 0 0
0 0 1 1 0 0 00 ][ ] [ ] [ ] ]
0

[ ] [] ]
0 00
J vcm 1= 0 X 0 0 0
1 0 00

[ ]
000
J vcm 1= 0 0 0
000

Ahora analizando el jacobiano de velocidad angular

J w 1=[ 0 R00 Z 0 J w 2 J w3 ]
 [ ][ ] ]
1 0 0 0 00
J w 1= 0 1 0 . 0 0 0
0 0 1 1 00

[ ]
0 0 0
J w 1= 0 0 0
1 0 0

Tomando

R00 I R00 T

[ ][ ][ ] [ ]
1 0 0 I xx I xy I xz 1 0 0 I zz 1 0 0
0 1 0 I yx I yy I yz 0 1 0 = 0 0 0
0 0 1 I zx I zy I zz 0 0 1 0 0 0

Volviendo a la formula

[ [ ][ ][ ]
T
1 0 0 0 0 0 0 I zz 1 0 0
m1 0 0 0 . 0 0 0 + 0 0 0
i
0 0 0 000 0 0 0

[ [ ][ ] [ ]
0 0 0 I zz 1 0 0 I zz 0 0
m1 0 0 0 + 0 0 0 = 0 0 0
i
0 0 0 0 0 0 0 0 0

Analizando el segundo eslabn

[ 0 0 0 0 0 0
J vcm 2= R0 Z 0 X ( O cm3O1 ) R2 Z 1 X ( Ocm3 O2 ) J v 3 ]

En el punto de anlisis (O0cm 3O02 )=0 ya que el segundo eslabones un prismatico

[ 0 0 0 0
J vcm 2= R0 Z 0 X ( O cm3O 1 ) R2 Z 1 J v 3 ]
 [[
1 0 0 0

][ ] [
l 1+ S 1 ( q 2a1 )
J vcm 2= 0 1 0 . 0 X C1 ( q 2a1 ) 0
0 0 1 1 0
l1

0 ] [ ][
C 1 0 S1

0 1 0
.
][ ] ]
0 0
S 1 0 C 1 0 0
1 0

[] [ ] ]
0 S1 ( q2 a1 ) S1 0
J vcm 2= 0 X C1 ( q2 a1 ) C1 0
1 0 0 0

Por lo tanto el jacobiano de velocidad 2 es

[
( C1 ( q2 a1 ) )
]
S1 0
J vcm 2= ( S 1 ( q2a1 ) ) C 1 0
0 0 0

Analizando ahora el jacobiano de velocidad angular 2

J w =[ 0 R 00 Z 0 1 R01 Z 1 J w3 ]

0
NOTA: 1 R 1 Z 1=0 ya que estamos analizando en el segundo punto un
prismtico y no tiene velocidad angular por lo tanto es cero

[]
0
J w 2= 0
0

T 0 0T
NOTA: J w 2 R0 I R0 J w 2=0 ya que nueva mente como estamos analizando un
sistema prismtico este por lo tanto su inercia es cero y todo ese punto es
igual a cero

Volviendo a la formula

[[ ][ ]
T
2 ( C 1 ( q2a1 ) ) ( S 1 ( q2a1 ) ) 0 ( C1 ( q 2a1 ) ) S 1 0
m2 S1 C1 0 . ( S 1 ( q2a1 ) ) C 1 0
i
0 0 0 0 0 0
 2

[[(a 1q 2)2 0 0
m2 . 0
i
0
1 0
0 0 ]
a
2 2
m2 ( 1 2 a1 q2 +q 2)

0 0
0 m2

0

2

[ [ 00 ] ]
i

Analizando el tercer eslabn

n

[ m3 J vcm 3T J vcm3 + J w3T R20 I R02 T J w3 ]

i

[
J vcm 3= R 00 Z 0 X ( O 0cm4 O01 ) R 02 Z 1 R03 Z2 ]

[[
1 0 0 0
][ ] [ ] [ ][
l 1+ S 1 q 2 l 1
J vcm 3= 0 1 0 . 0 X C1 q2 0
0 0 1 1 q3a2 0
C 1 0 S1

] [ ][ ] [ ]]
0 C 1 S 1 0 0
S 1 0 C1 . 0 S 1 C1 0 0
0 1 0 1 0 0 1 1

Por lo tanto el jacobiano de velocidad 3 es

[
( C 1 q2 ) S 1 0
J vcm 3= ( S 1 q 2 ) C 1 0
0 0 1 ]
Analizando ahora el jacobiano de velocidad angular 3
 J w =[ 0 R 00 Z 0 1 R01 Z 1 2 R02 Z 2 ]

0
NOTA: 3 R 2 Z 2=0 ya que estamos analizando en el segundo punto un
prismtico y no tiene velocidad angular por lo tanto es cero

[]
0
J w 3= 0
0

T 0 0T
NOTA: J w 3 R0 I R0 J w 3=0 ya que nueva mente como estamos analizando un
sistema prismtico este por lo tanto su inercia es cero y todo ese punto es
igual a cero

Volviendo a la formula

[[ ][ ]
T
3 C 1 q 2 ( S1 q2 ) 0 ( C 1 q2 ) S 1 0
m3 S 1 C 1 0 . ( S 1 q 2 ) C 1 0
i
0 0 1 0 0 1

[[ ]
q22 0 0
m3 . 0 1 0
i
0 0 1

[ ]
3 m3( q22 ) 0 0
0 m3 0
i
0 0 m3

Por lo tanto la matriz de inercia ser todas las sumatorias anteriores

 [ ]
1 1 1
D= + +
i=1 i=2 i=3

a
m2 ( 1 2 a1 q2 +q 22)
2

0 0
0 m2

0

[ ] [ ]
2
I zz 0 0 m3 (q2 ) 0 0
0 0 0 + [ 00 ] + 0 m3 0
0 0 0 0 0 m3

Donde la matriz "D" de inercia ser representada de la siguiente forma

a
m2 ( 1 2 a1 q2 +q 22)+m3 (q 22)
2

I zz + 0 0
0 m2 +m3

0

[]
q1
D=[ 0 m3 ] q 2
q3

cada comoponente sera representado de la siguientemanera

a
m2 ( 1 2 a1 q2 +q 22)+m3 (q 22)
2

I zz + .
d11 =q1
 d 12 =q 2 [ 0 ] .

d 13=q 3 [0] .

d 21=q 1 [0 ] .

d 22 =q 2 [m3 +m2 ] .

d 23 =q 3 [0] .

d 31=q 1 [0 ] .

d 32=q 2 [0 ] .

d 33 =q 3 [ m 3 ] .

Analizando matriz de Coriolis

Utilizando los smbolos de Christofeel.

{
1 dkj dki ij
2 i
+
j k }
Se realizaran las operaciones correspondientes de cada uno:

C111= {
1 d11 d 11 d 11
+
2 q1 q 1 q 1
=0 }
C112 = {
1 d 21 d 21 d 11
+
2 q1 q 1 q 2 }
=m2 a1 m 2 q 2m3 q2

C113 = {
1 d 31 d 31 d 11
+
2 q1 q 1 q 3
=0 }
C121 = {
1 d 12 d 11 d 12
+
2 q1 q 2 q1 }=m3 q2m 2 ( a1q2 )

C122 = {
1 d 22 d 21 d 12
+
2 q1 q2 q2 }=0

C123 = {
1 d 32 d 31 d 12
+
2 q1 q2 q3 }
=0

C131 = {
1 d 13 d 11 d 13
+
2 q1 q3 q1 }=0

C132 = {
1 d 23 d 21 d 13
+
2 q1 q3 q 2 }
=0

C133 = {
1 d 33 d 31 d 13
+
2 q 1 q3 q3 }
=0

C211 = {
1 d 11 d 12 d 21
+
2 q 2 q1 q 1 }
=m3 q 2m 2 ( a1q 2 )

C212 = {
1 d 21 d 22 d 21
+
2 q2 q1 q2 }=0

C213 = {
1 d31 d 32 d 21
+
2 q 2 q1 q3 }
=0

C221 = {
1 d 12 d 12 d 22
+
2 q2 q2 q1 }=0
 C222 = { +
2 q2 q2 q2 }
1 d 22 d 22 d 22
=0

C223 = {
1 d32 d 32 d 22
+
2 q 2 q2 q3 }=0

C231 = { +
2 q2 q3 q1 }
1 d 13 d 12 d 23
=0

C232 = {
1 d 23 d 22 d 23
+
2 q 2 q3 q 2 }=0

C233 = { +
2 q 2 q3 q 3 }
1 d33 d 32 d23
=0

C311 = { +
2 q 3 q1 q 1 }
1 d 11 d 13 d 31
=0

C312 = {
1 d 21 d 23 d 31
+
2 q3 q1 q2 }=0

C313 = {
1 d 31 d 33 d 31
+
2 q3 q1 q3 }
=0

C321 = { +
2 q3 q2 q1 }
1 d 12 d 13 d 32
=0
 C322 = {
1 d 22 d 23 d 32
+
2 q3 q2 q2
=0 }
C323 = {
1 d 32 d 33 d 32
+
2 q3 q2 q3
=0 }

C331 = {
1 d 13 d 13 d 33
+
2 q3 q3 q 1
=0 }
C332 = {
1 d 23 d 23 d33
+
2 q3 q3 q 2
=0 }
C333 = {
1 d 33 d 33 d33
+
2 q3 q3 q 3
=0 }

Ahora calculando la colocacin de cada elemento

[ ]
C 11 C12 C 13
C= C 21 C22 C 23
C 31 C32 C 33

n
donde C kj= ( Cijk ) q
i=1

C11 =C 111 q 1+C 211 q2 +C 311 q3=(m3 q 2m2 ( a2q2 ) ) q2 .

 C12=C 121 q 1+C 221 q2+C 321 q3=( m3 q 2m2 ( a2q 2 )) q 1

C13=C 131 q1+C 231 q2+C331 q3=0 .

C21=C 112 q1 +C 212 q2 +C 312 q3 =(m3 q2 +m2 ( a2q 2 )) q 1 .

C22=C 122 q1+C 222 q2+C 322 q3=0 .

C23=C 132 q1+C 232 q2+ C332 q3=0 .

C31=C 113 q1 +C 213 q 2+C 313 q3=0 .

C32=C 123 q1+C 223 q2+ C323 q3=0 .

C33=C 133 q1+ C233 q2 +C333 q3=0 .

Al final resulta de la siguiente forma:

[ ]
(m3 q2m2 ( a2 q2 ) ) q2 ( m3 q 2m 2 ( a2q 2 )) q1 0
C= (m3 q2 +m2 ( a2q2 ) ) q1 0 0
0 0 0

Anlisis de energa potencial (Para obtener fuerzas

gravitatorias)
Ahora sacaremos las energas potenciales de sistemas para poder obtener la
matrices de fuerzas gravitatorias donde la formula general no indica que:

EP=mgH
 EP 1=0 g ( b1 ) =0

Nota: esta energa potencial es cero por que no tiene masa

EP 2=m1 g(l 1 )

EP 3=m2 g ( s 1 ( q 2a 1) +l 1 ) =m 2 g s1 q2m2 g s 1 a 1+ m2 g l 1

EP 4=m3 g s 1 q 2+ m3 g l 1

Ahora le energa potencial total del sistema es

EP=E P1 + E P2+ E P3 + E P4

EP=m1 g ( l 1 ) +m2 g s1 q2 m2 g s 1 a1+ m2 g l1 +m3 g s 1 q 2+ m3 g l1

Ya que obtuvimos la energa potencial del todo el sistema para obtener los
componente es necesario derivar parcialmente respecto a cada punto de
nuestro robot lo cual quedara de la siguiente forma:

P
qi

P
=g m3 q 2 c 1g m2 c1 (a1q 2)
q1

P
=g m2 s 1 + g m3 s 1
q2

P
=0
q3

Por lo tanto la matriz de fuerzas gravitatoria quedara de la siguiente forma

[ ]
m3 q 2 c1m2 c 1 (a1q2 )
g m2 s1 +m3 s 1
0
Al obtener los resultados anteriores se pueden acomodar de la siguiente
manera:

=D ( q ) +C ( q q ) +G ( q )

[][ ][ ] [ ][ ] [ ]
1 d 11 d 12 d 13 q1 c11 c 12 c 13 q 1 g 11
f 2 = d 21 d 22 d 23 q2 + c 21 c 22 c 23 q 2 +g g 22
f3 d 31 d 32 d 33 q3 c 31 c 32 c 33 q 3 g 33

a
m2 ( 1 2 a1 q2 +q 22)+m3 (q 22)
2

I zz + 0 0
0 m2+ m3

0

[ ] [ ][ ][ ] [ ]
1 q1 (m3 q2m2 ( a2q2 ) ) q2 (m3 q2 m2 ( a 2q 2) ) q1 0 q 1 m3 q 2 c1 m2 c 1 (a1q2 )
f 2 [ 3 ] q 2 (m3 q2 +m2 ( a2q 2 )) q1
= 0 m + 0 0 q 2 + g m2 s1 +m3 s 1
f3 q3
0 0 0 q3
0
 Modelo dinmico de robot RPP en simulink
Como sabemos, el vector de Torque esta dado por:

=D q +C q + g q

Sin embargo, para poder hacer el modelo en Simulink, realizamos el

siguiente despeje:

q =D1 [ C q g q ]

1
En base a esto primero realizamos la matriz de inercias inversa ( D ),

la matriz de Coriolis ( C q ), y el vector de gravedad ( g q ) dentro de
un subsistema para cada una.

a
2 2 2
m2 ( 1 2a 1 q 2+ q2 )+ m3( q2 )

I zz + 0 0
0 m2 +m 3

0

[ ] [ ][ ][ ] [
1 q1 m2 q 2 q 2m2 a2 q 2 +m3 q 2 q2 m2 q 2 q1m2 a2 q 1 +m3 q1 q2 0 q 1 m2 q 2 g c 1m2 a1 g c 1
f 2 =[ 0 m3 ] q 2 + m2 q 2 q 1m3 q 2 q1 +a1 m2 q1 0 0 q 2 + g g m2 s 1+ g m3
f3 q3 0 0 0 q 3 0
Nota: Debido a que nuestro primera articulacin prismtica del {2} esta
empalmada con nuestra primera revoluta {1}, al momento de querer
EK 2
sacar nuestra energa cintica , nuestra velocidad del centro de masa
se convierte en cero, lo cual afecta en nuestro modelo de Simulink, ya que al
no tener algn valor de centro de masa entre la primera revoluta y la primera
prismtica, se muestra en simulink al utilizar Torques y Fuerzas iguales a cero

que el robot se encuentra en

q1 =74 , lo cual no debera ser, ya que lo que

se esperara que mostrara nuestra articulacin es que fuera para abajo a causa
de la gravedad influida.
Aqui se muestra la imagen del robot RPP y sus marcos empalmados

Matriz de inercia:
 a
m2 ( 1 2 a1 q2 +q 22)+m3 (q 22)
2

I zz + 0 0
0 m2 +m3

0

[]
q1
D=[ 0 m3 ] q 2
q3

Matriz de coriolis:

[ ]
m2 q 2 q2m2 a2 q 2 +m3 q2 q2 m 2 q2 q 1m2 a2 q 1 +m 3 q1 q2 0
C= m2 q 2 q1m3 q2 q1 +a1 m 2 q1 0 0
0 0 0
Vector de fuerzas gravitatorias:

[ ]
m2 q 2 g c 1m2 a1 g c 1 +m3 q 2 g c 1
g= g m2 s 1+ g m3 s 1
0
 Comprobaciones
Primer Ejemplo:

Vamos a tomar nuestro ltimo eslabn, y para que los dems eslabones
no afectan a este anlisis, diremos que torque 1 ser igual a cero y
fuerza 2 igual a cero, con esto nuestro ltimo eslabn queda libre del
resto de nuestro robot.

1=0, F 2=0

Analizaremos el ultimo eslabon del robot tomando en cuenta que

F3 =0 q3 =0
para esperar como resultado en con nuestro robot m.

F3 =m3 q3

F3
q3 =
m3

Integramos dos veces q3 :

1
q3 =
0
[ ]
F3
m3
dtdt

F3 2
t
m3
q3 =
2
 Ahora sustituimos los valores:
m3=1 Kg

F3 =0 N

t=1 s

0 2
1
1
q3 = =0 m
2

q3 =0 m
Tenemos que

Ahora se comprobara con simulink

Se puede observar el valor ingresado en Torque y Fuerza, asi el mismo

q3
resultado de .
Segundo Ejemplo:
F3
En este ejemplo se le dara a nuestra un valor de medio metro

expresado en F3 =.5 Newtons para buscar como resultado

q3 =.25 m

F3 2
t
m3
q3 =
2

Entonces:

F3 =.5 Newtons

m3=1 Kg

t=1 s

.5 2
1
1
q3 =
2

q3 =.25 m

Comprobando con simulink

 q3
En la grafica se observa como llega a .25 metros en 1
segundo
Tercer Ejemplo:
 F3
En este ejemplo se le dara a nuestra un valor de 4 metros
expresado en
F3 =.4 Newtons
para buscar como resultado q3 =2 m

F3 2
t
m3
q3 =
2

Entonces:

F3 =2 Newtons

m3=1 Kg

t=1 s

2 2
1
1
q3 =
2

q3 =1 m

Comprobando con simulink

 q3
En la grafica se observa como llega a 1 metro en 1 segundo
 Ajuste de modelo dinmico en Simulink robot RPP
En nuestro modelo anterior del robot RPP echo en simulink,
introducamos los valores de Torque y Fuerza, lo cual nos entregaba
qi , q i y q i
valores de .

Se construyo un subsistema donde se encuentra el modelo dinmico

realizado anteriormente en simulink, colocndolo con entradas de torque
qi y q i
y fuerzas, y con salidas de .

Ahora buscaremos que valores de Torque1, Fuerza2 y Fuerza3 son los

necesarios para que nuestro q1,q2 y q3 resulten con el valor que
queremos obtener.

En este caso deseamos obtener los prximos valores en nuestras q, las

cuales llamaremos "qd" (deseadas).

q1 d=1.5 radianes (85.94)

"1rad=57.2958"
 q1 d
Para encontrar que valor de Torque1 es necesario para nuestro ,
fue necesario hacer muchos chequeos de valores hasta llegar con el
indicado, esto se debe a que no se est utilizando una Ley de control, la
cual nos facilitara la obtencin de resultados.

Estos son algunos valores que se probaron para lograr encontrar el valor
q1
de nuestra .

Torque1 q1

0 1.29
1 1.241
2 1.476
2.1 1.484
2.2 1.492
2.3 1.5
2.4 1.507
2.5 1.515
2.6 1.522
2.7 1.529
2.8 1.536
2.9 1.543
3 1.55
q1 d=1.5
Se logro encontrar que para que nuestro radianes, el valor de
nuestro Torque1 deba ser 2.3Newtons.

q1 d=1.5
Torque1=2.3N para que radianes.
 q 2 d=1 m
Para lograr encontrar el valor de Fuerza2 necesario para que
se tuvo que hacer de la misma forma que el anterior, a base de pruebas.
q 2 d=1 m

Esta es la tabla que se consigui al buscar el valor que deseamos en

q2d
nuestra :

Fuerza2 q2 Fuerza2 q2

0 -1.764 7.2 0.778

1 -1.435 7.3 0.8138
2 -1.114 7.5 0.8856
3 -0.7657 7.6 0.9215
4 -0.3914 7.7 0.9575
5 -0.008921 7.8 0.9935
6 0.3537 7.81 0.9971
7 0.7067 7.819 1
7.1 0.7423 7.9 1.029
Se llego a saber que la Fuerza necesaria es de 7.819N para que nuestra
q 2 d=1 m
.

q 2 d=1
Fuerza2=7.819N para que m.

q3 d=2m
Para lograr encontrar el valor de Fuerza3 necesario para que
se tuvo que hacer de la misma forma que el anterior, a base de pruebas.
q3 d=2m

Esta es la tabla que se consigui al buscar el valor que deseamos en

q3 d
nuestra :

Fuerza3 q2

0 0
1 .5
2 1
 3 1.5
4 2
5 2.5

Se llego a saber que la Fuerza necesaria es de 4N para que nuestra

q3 d=2m
.

q3 d=2
Fuerza3=4N para que m.

Nota: Es muy difcil lograr encontrar el valor de Torque/Fuerza necesario

para lograr obtener el valor deseado de nuestras "q" sin una ley de
control que nos apoye para este modelo dinmico.