z

UNIVERSIDAD
NACIONAL DE INFORME
INGENIERÍA
FACULTAD DE INGENIERÍA
N° 02
MECÁNICA
ACULTAD DE INGENIERÍA MECÁNICA

TEMAS:
CINEMATICA INVERSA
CINEMATICA DIRECTA
JACOBIANO
DOCENTE
DINAMICA FREEDY SOTELO
DISEÑO INTEGRANTES

SIMULACION
 CONISLLA CONTRERAS, Oskar Steven
 RAU ESPINOZA, Ulises Piero
 SALAZAR ARÉVALO, Renzo Sebastián
2017 – II SARMIENTO DÍAZ, Fernando Gabriel
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

INDICE

1) Cinemática Directa……………………………………………2
2) Cinemática Inversa……………………………………………6
3) Jacobianos………………………………………………………16
 De los centros de masa de cada link
 De los centros de masa de cada motor
4) Dinámica………………………………………………………….20
 Calculo de los centros de masa e inercias
5) Método de Lagrange……………………………………….23
 Calculo de la energía cinética
 Calculo de la energía potencial

1
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

1) CINEMATICA DIRECTA
El problema cinemático directo consiste en determinar cuál es la posición y orientación del
extremo final del robot, con respecto a un sistema de coordenadas que se toma como
referencia, conocidos los valores de las articulaciones y los parámetros geométricos de los
elementos del robot.

En general, un robot de n grados de libertad está formado por n eslabones unidos por n
articulaciones, de forma que cada par articulación - eslabón constituye un grado de libertad.

A cada eslabón se le puede asociar un sistema de referencia solidario a él y, utilizando las

transformaciones homogéneas, es posible representar las rotaciones y traslaciones
relativas entre los distintos eslabones que componen el robot. La matriz de transformación
homogénea (A) es la que representa la posición y orientación relativa entre los distintos
sistemas asociados a dos eslabones consecutivos del robot. Cuando se consideran todos los
grados de libertad se le denomina T, matriz de transformación que relaciona la posición y
orientación del extremo final del robot respecto del sistema fijo situado en la base del
mismo.

Figura. 1 Brazo robótico similar a un brazo humano

2
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Para describir la relación que existe entre dos sistemas de referencia asociados a eslabones,
se utiliza la representación Denavit - Hartenberg (D-H). Denavit y Hartenberg propusieron
en 1955 un método matricial que permite establecer de manera sistemática un sistema de
coordenadas {Si} ligado a cada eslabón i de una cadena articulada. Además, la
representación D-H permite pasar de un sistema de coordenadas a otro mediante 4
transformaciones básicas que dependen exclusivamente de las características geométricas
del eslabón.

Estas transformaciones básicas consisten en una sucesión de rotaciones y traslaciones que

permiten relacionar el sistema de referencia del elemento i con el sistema del elemento i −
Las transformaciones en cuestión son las siguientes:

 Rotación alrededor del eje zi−1 un ángulo θi.

 Traslación a lo largo de zi−1 una distancia di.
 Traslación a lo largo de xi una distancia ai.
 Rotación alrededor del eje xi un ángulo αi.

Figura. 2 Parámetros Denavit - Hartenberg

3
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Figura. 3 Diseño del robot a construir

CALCULANDO LA CINEMATICA DIRECTA DE NUESTRO ROBOT

Parámetros DENAVIT-HARTENBERG

Θ d a α
1 θ1 l1 0 π/2
2 θ2 l2 l3 0

Dónde: L1=15cm L2=5cm L3=15cm

Teniendo en cuenta:

ci  si ci si si ai ci 

 
 s ci ci ci si ai si 
Aii 1 (qi )   i
0 si ci di 
 
 0 0 0 1 

4
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

c1 0 s1 0
s 0 c1 0 
T1  A1   1
0 1 0 l1 
 
0 0 0 1

c2  s2 0 l3c2 
s c2 0 l3 s2 
A2   2
0 0 1 l2 
 
0 0 0 1 

c1c2 c1s2 s1 c1c2l3  l2 s1 

s c  s1s2 c1 s1c2l3  c1l2 
T2  A1 A2   1 2
 s2 c2 0 l1  l3 s2 
 
 0 0 0 1 

5
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

2) CINEMATICA INVERSA
El problema cinemático inverso consiste en encontrar los valores que deben adoptar las
coordenadas articulares del robot q = [q1, q2, . . . , qn] para que su extremo se posicione y
oriente según una determinada localización espacial. Al contrario que el problema
cinemático directo, el cálculo de la cinemática inversa no es sencilla ya que consiste en la
resolución de una serie de ecuaciones fuertemente dependiente de la configuración del
robot, además de existir diferentes n − uplas q = [q1, q2, . . . , qn] que resuelven el problema.

En la actualidad existen procedimientos genéricos susceptibles de ser programados para la

resolución de la cinemática inversa y obtener la n−upla de valores articulares que
posicionen y orienten el extremo final. Sin embargo, el principal inconveniente de estos
procedimientos es que son métodos numéricos iterativos, que no siempre garantizan tener
la solución en el momento adecuado. De esta manera, a la hora de resolver el problema
cinemático inverso es mucho más adecuado encontrar una solución cerrada.

Figura. 4 Ejemplo de cinemática inversa

6
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Para poder conseguir esta relación suele ser habitual emplear métodos geométricos, que
consisten en la utilización de las relaciones trigonométricas y la resolución de los triángulos
formados por los elementos y articulaciones del robot. La mayoría de los robots suelen
tener cadenas cinemáticas relativamente sencillas, y los tres primeros gdl, que posicionan
al robot en el espacio, suelen tener una estructura planar. Esta condición facilita la
resolución de la n − upla. Además, los tres últimos grados de libertad suelen usarse para la
orientación de la herramienta, lo cual permite la resolución desacoplada (desacoplo
cinemático) de la posición del extremo delrobot y de la orientación de la herramienta. Como
alternativa para resolver el mismo problema se puede recurrir a manipular directamente
las ecuaciones correspondientes al problema cinemático directo. Es decir, a partir de la
relación entre la matriz de transformación y las ecuaciones en función de las coordenadas
articulares q = [q1, q2, . . . , qn].

CALCULANDO LA CINEMATICA INVERSA DE NUESTRO ROBOT

Figura 6 Eje X-Y

Figura 5 Eje Y-Z

7
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Calculo de ϴ2:
De la anterior figura

z  l1
sen 2  D
l3
cos  2  1  D 2
 2  a tan 2( D, 1  D 2 )

Calculo de ϴ1:
Se calcula el ángulo  (ángulo entre eje x y eje y) de la siguiente manera:

y
tan  
x
 y
  tan 1  
 
x
  a tan 2  y, x 
Mientras el ángulo  :

l3 cos( 2 )
tan  
l2
  a tan 2(l3 cos( 2 ), l2 )

1    
1  a tan 2( yc , xc )  a tan 2(l3 cos( 2 ), l2 )

8
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

3) DISEÑO
El material a usar para los links será el acrílico (6cm de espesor, 1180 kg/m3). Se usará este
material ya que tiene densidad relativamente baja y bajo costo.

Las medidas aproximadas para el tamaño del brazo son de 10 cm x 3cm el primer link y
de 5cm x 3cm para el segundo link, con todos estos datos podemos hacer los cálculos para
calcular el torque mínimo requerido para los motores a usar.

 CALCULO DEL TORQUE MINIMO A USAR

Momento de inercia de un cilindro

1 1 2
I  ml  mr
2

12 4

Momento de inercia de un paralelepipedo

1
I m( a 2  b 2 )
12

9
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Momento de inercia de un disco

1
I MR 2
2

El primer motor soportara la inercia de los siguientes elementos:

Densidad del material:

Acrílico =1180 kg/m3

10
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Mientras que el segundo motor:

 MOTORES SELECCIONADOS
Mediante cálculos anteriormente realizados, se determinó que el torque del primer
motor debe ser mayor o igual a 3Kg-cm. Este motor tiene que poseer un torque
relativamente alto con respecto al segundo ya que debe soportar la estructura de todo
el brazo incluyendo el segundo motor.

El segundo motor no requiere un torque alto debido a que solo soportara mover el link
2. Gracias a los cálculos anteriores se determinó que este motor debe resisitir un torque
mínimo de 0.5Kg-cm.

Para poder controlar la posición del brazo de 2GDL en todo momento es necesario que
los motores tengan encoder de 2 canales. También se requerirá que las RPM que
entregaran las cajas reductoras de los motores sean bajas ya que no necesitamos
demasiada velocidad, pero si un torque necesario.

11
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

 PRIMER MOTOR

El mejor motor que se adecua a nuestros requerimientos anteriormente especificados

es el motor de la marca Pololu con especificaciones: 75:1 Metal Gearmotor 25Dx54L
mm LP 6V with 48 CPR Encoder

Este motor cuenta con un reductor de velocidad y encoder. Presenta las siguientes
características: trabaja a 6v, posee un torque de 6.8 Kg-cm y 78RPM. Además, presenta
un encoder de efecto Hall, el cual tiene una resolución de 48 cuentas por revolución.
También se muestra las especificaciones brindadas por el fabricante.

Figura7. Primer motor a usar

Figura 8 Especificaciones del primer motor

12
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Figura 9 Geometría del primer motor

 SEGUNDO MOTOR

De acuerdo a las especificaciones necesarias en nuestro diseño del robot de 2GDL el motor más

adecuado tiene como nombre: 50:1 Micro Metal Gearmotor MP 6V with 48 CPR Encoder. Este
motor cuenta con reductor de velocidad y encoder, presenta las siguientes características:
trabaja a 6v, posee un torque de 1.0 Kg-cm y 420 RPM. Además, presenta un encoder de efecto
Hall, el cual tiene una resolución de 48 cuentas por revolución. También se muestra las
especificaciones brindadas por el fabricante.

13
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Figura 10 Segundo motor

escogido

Figura 11 Especificaciones del segundo motor

14
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Figura 12 Geometría del segundo motor

15
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

4) JACOBIANOS

 Cálculo de jacobianos para el link ‘i’:

J v(li )   J v(1li ) J v(li2 ) ... ... J vi(li ) 0 ... ... 0 

J w( mi )   J w( li1) J w( li2) ... ... J wi(li ) 0 ... ... 0 

Donde cada columna de este vector se calcula a partir de:

z j 1  ( pn  p j 1 ) Junta de revolucion
(𝑙𝑖)
𝐽𝑣𝑗 = {
z j 1 Junta prismatica

(𝑙𝑖) z j 1 Junta de revolucion

𝐽𝑤𝑗 = {
0 Junta prismatica
 Para los motores, su velocidad lineal se representa de la siguiente forma:

̇ = 𝐽𝑣𝑚𝑖 𝑞̇
𝑝𝑚𝑖
El Jacobiano para los motores se representa de la siguiente manera:

J v( mi )   J v(1mi ) J v( 2mi ) ... ... J vi( mi1) 0 ... ... 0 

Calculo de las columnas:

z j 1  ( pn  p j 1 ) Junta de revolucion
(𝑚𝑖)
𝐽𝑣𝑗 ={
z j 1 Junta prismatica

16
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Para los motores, su velocidad angular se representa de la siguiente forma:

𝑚𝑖
𝑤𝑚𝑖 = 𝐽𝑤 𝑞̇

Calculo de las columnas

(𝑙𝑖)
(𝑚𝑖) 𝐽 Junta de revolucion
𝐽𝑤𝑗 = { 𝑤𝑗
𝑘𝑟𝑖 𝑧𝑚𝑖 Junta prismatica

Cálculo de la posición del centro de masa 1:

1  T1  rcm1
0 0 1
rcm

 0 
 lcm1
1
rcm   
1
 0 
 
 1 

c1 0 s1 0 
s 0 c1 0 
T1  A1   1
0 1 0 l1 
 
0 0 0 1

c1 0 s1 0   0   0 
s 0 c1 0   lcm1  0 
rcm1   1
0
. 
0 1 0 l1   0  l1  lcm1
     
0 0 0 1  1   1 

Cálculo de la posición del centro de masa 1:

2  T2  rcm 2
0 0 2
rcm

17
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

 lcm 2 
 0 
rcm2 2  
 0 
 
 1 

c1c2 c1c2 s1 c1c2l3  l2 s1 

s c  s1s2 c1 s1c2l3  c1l2 
T2  A1 * A2   1 2
 s2 c2 0 l1  l3 s2 
 
 0 0 0 1 

c1c2 c1c2 s1 c1c2l3  l2 s1   lcm 2   c1c2 (l3  lcm 2)  l2 s1 

s c  s1s2 c1 s1c2l3  c1l2   0  c2 (l3 s1  lcm 2s1 )  c1l2 
rcm0 2  1 2 . 
 s2 c2 0 l1  l3 s2   0   l1  l3 s2  lcm 2s2 
     
 0 0 0 1   1   1 

Calculo de los jacobianos

 c1l3 s1 c1l3 s2 
c1l2  2 2 
0 0   
 s1l3 s2 
J vcm1  0 0   1 3 1  l2 s1
cl s
J vcm 2
 2 2 
0 0  
 c2l3 
0
 2 

0 0  0  s1 
J w1  0 0  J w2  0 c1 
1 0  1 0 

18
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Posición del centro de masa del motor1:

 0 
r0
c.motor1   0 
lmotor1

Posición del centro de masa del motor2:

0
r0
c .motor 2   0 
l1

Calculo de los jacobianos:

0 0  0 0
J vmotor1  0 0  J vmotor 2  0 0
0 0  0 0

 0 0 0 kr2 s1 
J wmotor1   0 0 J wmotor 2  0 kr2c1 
 kr1 0 1 0 

19
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

5) DINAMICA
 Calculo de centro de masa e inercias

LINK1 DATOS

Lcm1 6mm

masa 11.92gr

20
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

LINK2 DATOS

Lcm2 75mm

masa2 1.69gramos

 Momentos de inercia LINK 1

21
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

 Momentos de inercia LINK 2

22
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

6) MÉTODO DE LAGRANGE
El modelo dinámico de un manipulador proporciona una descripción de la relación entre los
pares de accionamiento del conjunto y el movimiento de la estructura. Para esto usamos el
Lagrangianoy la formula de Euler-Lagrange.
𝐿 =𝑇−𝑈
𝑑 𝑑𝐿 𝑑𝐿
− =0
𝑑𝑡 𝑑𝑞𝑖̇ 𝑑𝑞𝑖
Donde T y U denotan la energía cinética y potencial del sistema.
Donde 𝜀𝑖 es la fuerza generalizada asociada con el movimiento generalizado 𝑞𝑖
 CALCULO DE LA ENERGIA CINETICA
Considerando un manipulador con n juntas, la energía cinética total está dada por:
𝑛

𝑇 = ∑(𝑇𝑙𝑖 + 𝑇𝑚𝑖 )
𝑖=1

Donde 𝑇𝑙𝑖 es la energía cinética de cada link i y 𝑇𝑚𝑖, es la energía cinética de los motores
de las juntas i
1 1
𝑇𝑙𝑖 = 𝑚𝑙𝑖 𝑝̇𝑙𝑖𝑇 𝑝̇𝑙𝑖 + 𝑤𝑖𝑇 𝑅𝑖 𝐼𝑙𝑖𝑖 𝑅𝑖𝑇 𝑤𝑖
2 2
1 𝑇
1 𝑇 𝑖
𝑇𝑙𝑖 = 𝑚𝑚𝑖 𝑝̇𝑚𝑖 𝑝̇ 𝑚𝑖 + 𝑤𝑚𝑖 𝐼𝑚𝑖 𝑤𝑚𝑖
2 2

(𝑙𝑖)
𝑝̇𝑙𝑖 = 𝐽𝑣 𝑞̇
(𝑙𝑖)
𝑤̇𝑖 = 𝐽𝑤 𝑞̇
(𝑚𝑖)
𝑝̇ 𝑚𝑖 = 𝐽𝑣 𝑞̇
(𝑚𝑖)
𝑤̇𝑚𝑖 = 𝐽𝑤 𝑞̇

23
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Matriz de Rotación para llevar la Inercia de los motores al sistema base, Se debe tener en
cuenta de que la matriz de rotación del motor con respecto a tierra es diferente a la
matriz de rotación del link ’i’ con respecto a tierra, es decir:
0
𝑅𝑚𝑖 ≠ 𝑅𝑙𝑖0
Para encontrar la matriz de rotación adecuada se plantea:
𝑖−1
𝑅𝑚𝑖 = 𝑅𝑜𝑡𝑧,𝜃𝑚𝑖 = 𝑅𝑜𝑡𝑧, 𝑞
𝑘𝑟𝑖
0 0 𝑖−1 0
𝑅𝑚𝑖 = 𝑅𝑖−1 𝑅𝑚𝑖 = 𝑅𝑖−1 𝑅𝑜𝑡𝑧, 𝑞
𝑘𝑟𝑖

Luego de la aclaración anterior finalmente calculamos con la suma de las contribuciones

de cada link y de cada rotor, la energía cinética total del manipulador está definida con la
siguiente forma cuadrática
𝑛 𝑛
1 1
𝑇 = ∑ ∑ 𝑑𝑖𝑗 (𝑞)𝑞̇ 𝑖𝑞̇ 𝑗 = 𝑞̇ 𝑇 𝐷(𝑞)𝑞̇
2 2
𝑖=1 𝑗=𝑖
𝑛
(𝑙𝑖)𝑇 (𝑙𝑖) (𝑙𝑖)𝑇 (𝑙𝑖) (𝑚𝑖)𝑇 (𝑚𝑖)
𝐷(𝑞) = ∑ 𝑚𝑙𝑖 𝐽𝑣 𝐽𝑣 + 𝐽𝑤 𝑅𝑖 𝐼𝑙𝑖𝑖 𝑅𝑖𝑇 𝐽𝑤 + 𝑚𝑚𝑖 𝐽𝑣 𝐽𝑣
𝑖=1
(𝑚𝑖)𝑇 𝑚𝑖 𝑇 (𝑚𝑖)
+ 𝐽𝑤 𝑅𝑚𝑖 𝐼𝑚𝑖 𝑅𝑚𝑖 𝐽𝑤
De la misma manera que se vio en la energía cinética, la energía potencial será la suma de
2 contribuciones, una relativa al link y otra relativa al rotor.
n
U   (U li  U mi )
i 1

Asumiendo links rígidos, la contribución debida solo a la fuerza gravitacional es defina

como:

Uli   goT pi*  dV  mli goT pli

vli

De la misma manera para la contribución de los rotores

U mi  mmi goT pmi

Entonces la energía potencial total será

n
U   (mli g oT pli  mmi g oT pmi )
i 1

Entonces remplazando nuestros valores en el lagrangiano:

24
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

 
L ( q, q )  T ( q , q )  U ( q )

Aplicamos el método de Euler-Lagrange y obtenemos:

d dL dL
(  )    k
dt d q dq
k

d dL d dT n  n dD (q)  n  n n dD (q)  
(  )  (  )   Dkj (q) q j   kj q j  Dkj (q) q j   kj qi q j
dt d q dt d q j 1 j 1 dt j 1 i 1 j 1 dqi
k k

Finalmente remplazando
n  n n  
D
j 1
kj (q) q j   hkji (q) qi q j  g k (q)   k ......k  12,..., n
i 1 j 1

dDkj (q) 1 dDij (q)

hkji (q)  
dqi 2 dqk

El langrangiano puede ser inscrito de la siguiente forma despreciando la fricción y fuerzas

de contacto externas
Obteniéndose así , la ecuación de movimiento:
  
D (q ) q  C (q q ) q  g k (q )  

n  1  dDkj dDki dDij 

Ckj (q)   Cijk qi Ckj (q )     
i 1 2  dqi dq j dqk 

DINAMICA DE UN MANIPULADOR DE 2 GDL

De los cálculos de la sección anterior sobre jacobianos:
Para el link 1:

0 0  0 0 
J vc1  0 0  J wc1  0 0
0 0  1 0

Para el link 2:

25
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

 L2*cos(q1) - (L3*cos(q2)*sin(q1))/2 -(L3*cos(q1)*cos(q2))/2

J vc 2   L2*sin(q1) + (L3*cos(q1)*cos(q2))/2 -(L3*cos(q2)*sin(q1))/2 
 0 (L3*cos(q2))/2 
0 sin(q1) 
J wc 2  0 -cos(q1) 
1 0 

De los cálculos de la sección anterior sobre jacobianos:

Para el motor 1:

0 0  0 0
J vm1  0 0 J wm1   0 0 
0 0  kr1 0 

Para el motor 2:

0 0  0 kr2*sin(q1) 
J vm 2  0 0 J wm 2  0 -kr2*cos(q1) 
0 0 1 0 

Calculamos la matriz de inercias, las cuales serán calculadas en dos partes

Componente traslacional
Para el link 1:

D1v  m1 J vc1 T J vc1

0 0 
D1v   
0 0 

Para el link 2:

D2v  m1 J vc 2 T J vc 2
(m2*(4* L22  L32 * sin(q 2) 2  L32 )) / 4 -(L2*L3*m2*cos(q2))/2 
D2v   
 -(L2*L3*m2*cos(q2))/2 ( L32 * m2*( sin(q 2) 2  1)) / 2 

26
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Calculamos la matriz de inercias, las cuales serán calculadas en dos partes

Componente traslacional
Para el motor 1:

Dm1v  mm1 J vm1 T J vm1

0 0 
Dm1v   
0 0 

Para el motor 2:

Dm 2 v  mm 2 J vm 2 T J vm 2
0 0
Dm 2 v   
0 0

Componente Rotacional
Para el link 1:

 IL1X 0 0 

I1   0 IL1Y 0 
 0 0 IL1Z 
I I 1  R1 I1 RT 1
 IL1X  IL1X * sin(q1) 2  IL1Z * sin(q1) 2 (sin(2*q1)*(IL1X - IL1Z))/2 0 
 
I I1   (sin(2*q1)*(IL1X - IL1Z))/2 IL1Z  IL1X * sin(q1)  IL1Z * sin(q1)
2 2
0 
 0 0 IL1Y 

D1w  J w1T I I 1 J w1
 IL1Y 0 
D1w  
 0 0 

Componente Rotacional
Para el link 2:

27
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

 IL2X 0 0 

I2   0 IL2Y 0 
 0 0 IL2Z 
I I 2  R2 I 2 RT 2
 IL 2 X * cos ( q1) * cos ( q 2 )  IL 2Y * cos ( q1) * sin ( q 2 )  IL 2 Z * sin ( q1)
2 2 2 2 2 2 2
cos ( q 1) * sin ( q 1) * ( IL 2 X  IL 2 Z  IL 2 X * sin ( q 2 )  IL 2 Y * sin ( q 2 ) ) cos(q1)*cos(q2)*sin(q2)*(IL2X - IL2Y)


 cos ( q1) * sin ( q1) * ( IL 2 X  IL 2 Z  IL 2 X * sin ( q 2 )  IL 2Y * sin ( q 2 ) )
2 2 2
 IL 2 X * cos ( q 2 )
2 2
 IL 2 Y * sin ( q1 )
2 2


I IL 2 Z * cos ( q 1) * sin ( q1) * sin ( q 2 ) cos(q2)*sin(q1)*sin(q2)*(IL2X - IL2Y)
I2


2 2
cos(q1)*cos(q2)*sin(q2)*(IL2X - IL2Y) cos(q2)*sin(q1)*sin(q2)*(IL2X - IL2Y) IL 2 Y  IL 2 X * sin ( q 2 )  IL 2 Y * sin ( q 2 )

D2 w  J w 2T I I 2 J w 2
 IL 2Y  IL 2 X * sin(q 2) 2  IL 2Y * sin(q 2) 2 0 
D2 w   
 0 IL2Z 

Componente Rotacional
Para el motor 1:

 Im1xx 0 0 

I m1   0 Im1yy 0 
 0 0 Im1zz 
I Im1  Rm1 I m1 RT m1
 Im1xx  Im1xx * sin(q1) 2  Im1zz * sin(q1) 2 (sin(2*q1)*(Im1xx - Im1zz))/2 0
 
I Im1   (sin(2*q1)*(Im1xx - Im1zz))/2 Im1zz  Im1xx * sin(q1) 2  Im1zz * sin( q1) 2 0
 0 0 Im1yy 

Dm1w  J wm1T I Im1 J wm1
 Im1yy * kr12 , 0 
Dm1w   
 0 0

Componente Rotacional
Para el motor 2:

28
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

 Im2xx 0 0 

Im2   0 Imyy 0 
 0 0 Im2z 
I Im 2  Rm 2 I m 2 RT m 2

Dm 2 w  J wm 2T I Im 2 J wm 2
 Im2 yy  Im2 xx * sin(kr 2* q 2) 2  Im2 yy * sin(kr 2* q 2) 2 0 
Dm 2 w   2
 0 Im2 zz * kr 2 

Luego en base a estos hallaremos la matriz de inercias totales, que es la suma de los
valores calculados anteriormente.

D  D1v  D2v  Dm1v  Dm 2v  D1w  D2 w  Dm1w  Dm 2 w

29
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

Par calcular la matriz C, tenemos que usar la siguiente formula:

n 
Cij   Cijk qk
k 1

Donde:

1  dbij dbik db jk 
Cijk     
2  dqk dq j dqi 

Entonces:

 C11 C12 
C 
C 21 C 22 

(m2 cos(q2) sin(q2) L3)2

C11  dq 2( - IL2X cos(q2) sin(q2) + IL2Y cos(q2) sin(q2) -
4
Im2xx kr2 cos(kr2 q2) sin(kr2 q2) + Im2yy kr2cos(kr2 q2) sin(kr2 q2)

L2 L3 dq2 m2 sin(q2) (m2 cos(q2) sin(q2) L3)2

C12   dq1( )- IL2X cos(q2) sin(q2) +
2 4
IL2Y cos(q2) sin(q2) - Im2xx kr2cos(kr2 q2) sin(kr2 q2) + Im2yy kr2 cos(kr2 q2) sin(kr2 q2))

(m2 cos(q2) sin(q2) L3)2

C 21  dq1( - IL2X cos(q2) sin(q2) + IL2Y cos(q2) sin(q2)
4
- Im2xx kr2 cos(kr2 q2) sin(kr2 q2) + Im2yy kr2cos(kr2 q2) sin(kr2 q2))

L32 dq2 m2 cos(q2) sin(q2)

C 22  
2

 CALCULO DE LA ENERGÍA POTENCIAL

Por último, calcularemos el efecto de la energía potencial en la dinámica de manipulador:
P1L = m1*g*(L1-Lcm1);
P2L = m2*g*(L1 + Lcm2*sin(q2));
P1M=-Mm1*[0;0;-g].'*pm1;

30
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

P2M=-Mm2*[0;0;-g].'*pm2;
P = P1L + P2L+ P1M+ P2M

Luego:
P =L1*Mm2*g + Lcmot1*Mm1*g + g*m2*(L1 + Lcm2*sin(q2)) + g*m1*(L1 - Lcm1)

 0 
g 
 Lcm2 g m2 cos(q2) 

Observación: Se observó en el cálculo de la dinámica que esta no depende de

I L1x , I L1 y , I L 2 x , I L 2 y , I m1xx , I m 2 xx , I m1 yy , I m 2 yy

31
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

32
Universidad Nacional de Ingeniería Facultad de Ingeniería Mecánica

33