Lista de Ejercicios de Control de Sistemas Robóticos

Instituto Politécnico Nacional
Unidad Profesional Interdisciplinaria en Ingenierı́a y Tecnologı́as Avanzadas
Control de Sistemas Robóticos
Profesor Dr. Alberto Luviano Juárez
Lista 1
4MM11
Garcı́a Sandoval Juan Jesús
Ibañez Carrasco Claudio Alejandro
Morán Caballero Marco Antonio
Sánchez Garcı́a Leobardo Daniel
marzo de 2023
Índice
1. Ejercicio 1 3
2. Ejercicio 2 4
3. Ejercicio 3 5
4. Ejercicio 4 11
5. Ejercicio 5 18
6. Ejercicio 6 27
7. Ejercicio 7 29
7.1. Modelo Dinámico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
7.2. Antisimetrı́a . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
7.3. Representación Lineal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
8. Ejercicio 8 36
8.1. Modelo Dinámico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
8.2. Antisimetrı́a . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
8.3. Representación Lineal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
9. Ejercicio 9 49
10.Ejercicio 10 50
11.Ejercicio 11 56
12.Ejercicio 12 58
12.1. Ejercicio 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
12.2. Ejercicio 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
12.3. Ejercicio 7 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
12.4. Ejercicio 8 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
13.Ejercicio 13 77
14.Ejercicio 14 81
1. Ejercicio 1
Considere el manipulador de junta flexible como el de la figura: Donde el ángulo del motor está dado
Figura 1: Junta flexible
por q2 , el del eslabón por q1 , ambos son independientes. Considerando la energı́a potencial como:
1
P = M gl(1 − cos q1 ) + k(q1 − q2 )2
2
obtenga el modelo dinámico del robot.
Considerando que q1 y q2 son las coordenadas generalizadas del sistema, la energı́a cinética esta descrita
de la siguiente forma:
K = 21 I q˙1 2 + 12 J q˙2 2
Con esto es posible construir el lagrangiano del sistema, el cual tiene la forma:
L = 21 I q˙1 2 + 12 J q˙2 2 − M gl(1 − cos q1 ) − 21 k(q1 − q2 )2
La formulación de Euler-Lagrange tiene la forma de la siguiente expresión, por lo que se requiere

calcular cada uno de los términos para las dos coordenadas generalizadas:
d ∂L ∂L
− = τi
dt ∂ q˙i ∂qi
Para el caso del manipulador de junta flexible, se tiene lo siguiente:
∂L d ∂L ∂L d ∂L
= I q˙1 , = I q¨1 , = J q˙2 , = J q¨2
∂ q˙1 dt ∂ q˙1 ∂ q˙2 dt ∂ q˙2
∂L ∂L
= −M gl sin q1 − k(q1 − q2 ), = k(q1 − q2 )
∂q1 ∂q2
Desarrollando las ecuaciones para el modelo dinámico, se obtiene:
I q¨1 + M gl sin q1 + k(q1 − q2 ) = 0

J q¨2 − k(q1 − q2 ) = u
3
2. Ejercicio 2
Halle la matriz de inercia de un sólido rectangular uniforme de caras a, b, c respecto a un sistema
coordenado con origen en una de las esquinas y ejes a lo largo de los bordes del sólido.
El tensor de inercia esta dado por:

 
Ixx Ixy Ixz
I = Iyx Iyy Iyz 
Izx Izy Izz
Calculando el término Ixx :
Z cZ bZ a Z cZ b Z c 3
2 2 2 2 b b3 c + bc3
Ixx = ρ(y + z )dxdydz = ρa (y + z )dydz = ρa ( + bz 2 )dz = ρa( )
0 0 0 0 0 0 3 3
De manera similar: Z cZ bZ a
a3 c + ac3
Iyy = ρ(x2 + z 2 )dxdydz = ρb( )
0 0 0 3
Z cZ bZ a
a3 b + ab3
Izz = ρ(x2 + y 2 )dxdydz = ρc( )
0 0 0 3
Calculando el término:
Z cZ bZ a Z cZ b c
a2 a2 b 2 (ab)2
Z
Ixy = Iyx = − ρ(xy)dxdydz = −ρ ydydz = −ρ dz = −ρ c
0 0 0 2 0 0 4 0 4
De manera similar: Z cZ bZ a
(ac)2
Ixz = Izx = − ρ(xz)dxdydz = −ρ b
0 0 0 4
Z cZ bZ a
(bc)2
Iyz = Izy = − ρ(yz)dxdydz = −ρ a
0 0 0 4
Ası́, el tensor de inercia esta definido como:
(ab)2 (ac)2
 3 3

a( b c+bc
3
) − 4
c − 4
b
2 (bc)2
I = ρ(x, y, z)  − (ab) a3 c+ac3
c b( ) − a 
 
4 3 4
2 2 3 3
− (ac)
4
b − (bc)4
a c( a b+ab
3
)
4
3. Ejercicio 3
Obtenga el modelo dinámico del robot SCARA por medio de la técnica de Euler Lagrange, suponiendo
que los centros de masa están en el centro de cada eslabón y que los momentos de inercia son diagonales
(Ii = diag{Iix , Iiy , Iiz }). Muestre que el determinante de la matriz de inercia es diferente de cero para todo
q.
Figura 2: Robot SCARA
Muestre las propiedades de cotas de la matriz de inercia, antisimetrı́a.
Proponga una representación lineal respecto a los parámetros.
Con los marcos definidos en la Figura 2 se tienen los siguientes parámetros de Denavit Hartenberg:
Eslabón ai αi di θi
1 a1 0 d1 q1
2 a2 π 0 q2
3 0 0 d3 0
Cuadro 1: Parámetros de Denavit Hartenberg para robot SCARA.
Para obtener la matriz de inercia, es necesario calcular los Jacobianos en cada centro de masa:

Jv1 z0 × (Ocm1 − O0 ) 03x1 03x1
J1 = =
Jω1 z0 03x1 03x1

Jv2 z0 × (Ocm2 − O0 ) z1 × (Ocm2 − O1 ) 03x1
J2 = =
Jω2 z0 z1 03x1
5

Jv3 z0 × (Ocm3 − O0 ) z1 × (Ocm3 − O1 ) z2
J3 = =
Jω3 z0 z1 03x1
Considerando los vectores Ocmi , zn y On :

       
0 0 0 a1 c 1
z0 = z1 = 0; z2 =  0 ; O0 = 0; O1 = a1 s1 ;
1 −1 0 0
     
lc1 c1 a1 c1 + lc2 c12 a1 c1 + a2 c12
Ocm1 = lc1 s1 ; Ocm2 = a1 s1 + lc2 s12 ; Ocm3 = a1 s1 + a2 s12 
0 0 lc3
Considerando las expresiones anteriores, se desarrollan los jacobianos y se obtienen las siguientes ex-
presiones:
 
−lc1 s1 0 0
 lc1 c1 0 0
 
 0 0 0
J1 =  
 0 0 0 

 0 0 0
1 0 0
 
−a1 s1 − lc2 s12 −lc2 s12 0
 a1 c1 + lc2 c12 lc2 c12 0
 
 0 0 0 
J2 =  

 0 0 0 

 0 0 0
1 1 0
 
−a1 s1 − a2 s12 −a2 s12 0
 a1 c1 + a2 c12 a2 c12 0
 
 0 0 −1 
J3 =  

 0 0 0
 0 0 0
1 1 0
Se procede a realizar el calculo de la matriz de inercia a partir de la siguiente expresión.

3
X
T T
D(q) = mi Jvi Jvi + Jwi Ri (q)Ii RiT (q)Jwi
i=1
Se tiene el cálculo de la parte traslacional:

3
X
T
mi Jvi Jvi =
   i=1   
2
m1 lc1 0 0 m2 (a21 + 2lc2 c2 ) m2 (lc2 2
+ a1 lc2 c2 ) 0 m3 (a21 + 2a1 a2 c2 ) m3 (a22 + a1 a2 c2 ) 0
2 2
 0 0 0 + m2 (lc2 + a1 lc2 c2 ) m2 lc2 0 +  m3 (a22 + a1 a2 c2 ) m3 a22 =
0 0 0 0 0 0 0 0  m3
2 2 2 2 2
m1 lc1 + m2 (a1 + 2lc2 c2 ) + m3 (a1 + 2a1 a2 c2 ) m2 (lc2 + a1 lc2 c2 ) + m3 (a2 + a1 a2 c2 ) 0
2
 m2 (lc2 + a1 lc2 c2 ) + m3 (a22 + a1 a2 c2 ) 2
m2 lc2 + m3 a22 0
0 0 m3
6
En la parte rotacional, las matrices de rotación están dadas por:
   
c1 −s1 0 c12 s12 0
R1 = s1 c1 0; R2 = R3 = s12 −c12 0 
0 0 1 0 0 −1
Calculando la parte rotacional de la matriz de inercia:

       
X3 I1z 0 0 I2z I2z 0 I3z I3z 0 I1z + I2z + I3z I2z + I3z 0
T T
Jwi Ri (q)Ii Ri (q)Jwi = 0 0
 0 + I2z
  I2z 0 + I3z I3z 0 =  I2z + I3z I2z + I3z 0
i=1 0 0 0 0 0 0 0 0 0 0 0 0
Por lo tanto, la matriz de inercia queda de la siguiente forma:

 
d11a + I1z + I2z + I3z d12a + I2z + I3z 0
D(q) =  d21a + I2z + I3z d22a + I2z + I3z 0 
0 0 m3
Donde:
2
d11a = m1 lc1 + m2 (a21 + 2lc2 c2 ) + m3 (a21 + 2a1 a2 c2 )
2
d12a = m2 (lc2 + a1 lc2 c2 ) + m3 (a22 + a1 a2 c2 )
2
d21a = m2 (lc2 + a1 lc2 c2 ) + m3 (a22 + a1 a2 c2 )
2
d22a = m2 lc2 + m3 a22
Para calcular la matriz de coriolis es necesario calcular los sı́mbolos de Christoffel, los cuales están
dados por:
h i
1 ∂dkj ∂dki ∂dij
Cijk = 2 ∂qi + ∂qj − ∂qk
En este caso se deben calcular n3 sı́mbolos, donde n es el número de grados de libertad del robot. Por
lo que, es necesario calcular 27 sı́mbolos.
Realizando los cálculos pertinentes se obtienen los siguientes sı́mbolos:
C111 = C131 = C331 = C231 = C122 = C132 = C222 = C332 = C232 = C113 = C133 = C213 = C223 = C333 =
C323 = C211 = C311 = C321 = C212 = C312 = C322 = C313 = C123 = C323 = 0
C112 = a1 s2 (m3 a2 + m2 lc2 )
Se sabe que para un k fijo, Cijk = Cjik , por lo que se tiene:
C121 = C221 = −a1 s2 (m3 a2 + m2 lc2 )
Una vez obtenidos los sı́mbolos de Christoffel se procede a calcular la matriz de Coriolis, la componente
(k, j) de la matriz C(q, q̇) esta dada por:
n
X
Ckj = Cijk q˙i
i=1
Considerando la expresión anterior se tienen los siguientes términos de la matriz de Coriolis:
7
C11 = C111 q˙1 + C211 q˙2 + C311 q˙3
C12 = C121 q˙1 + C221 q˙2 + C321 q˙3
C13 = C131 q˙1 + C231 q˙2 + C331 q˙3
C21 = C112 q˙1 + C212 q˙2 + C312 q˙3
C22 = C122 q˙1 + C222 q˙2 + C322 q˙3
C23 = C132 q˙1 + C232 q˙2 + C332 q˙3
C31 = C113 q˙1 + C213 q˙2 + C313 q˙3
C32 = C123 q˙1 + C223 q˙2 + C323 q˙3
C33 = C133 q˙1 + C233 q˙2 + C333 q˙3
Realizando los cálculos correspondientes:
C11 = −a1 q˙2 s2 (m2 lc2 + m3 a2 )

C12 = −a1 (q˙1 + q˙2 )s2 (m2 lc2 + m3 a2 )
C13 = C22 = C23 = C31 = C32 = C33 = 0
C21 = a1 q˙1 s2 (m2 lc2 + m3 a2 )
Por lo que, se obtiene la siguiente matriz de Coriolis:

   
C11 C12 C13 −a1 q˙2 s2 (m2 lc2 + m3 a2 ) −a1 (q˙1 + q˙2 )s2 (m2 lc2 + m3 a2 ) 0
C(q, q̇) = C21 C22
 C23  =  a1 q˙1 s2 (m2 lc2 + m3 a2 ) 0 0
C31 C32 C33 0 0 0
Finalmente, para el cálculo de la energı́a potencial, la energı́a potencial de cada eslabón esta dada por
la masa por la gravedad por la altura, es decir:
P = P (q) = 3i=1 g T rcmi mi

P
Por lo que, se obtiene:

     
lc1 c 1 a 1 c 1 + lc2 c 12 a 1 c 1 + a 2 c 12
P = 0 0 ḡ lc1 s1  m1 + 0 0 ḡ a1 s1 + lc2 s12  m2 + 0 0 ḡ a1 s1 + a2 s12  m3
d1 d1 d1 − q3 + 2l
P = ḡ[d1 (m1 + m2 ) + (d1 − q3 + 2l )m3 ]
Dado que la gravedad apunta en −z se considera este signo en el vector gravedad:

∂P
 
 ∂q1   
 ∂P  0
g(q) =  = 0 
  
 ∂q2 
 ∂P  −m3 ḡ
∂q3
T
Finalmente, considerando las entradas externas de control por el vector u = τ1 τ2 F1 , el modelo
dinámico del robot SCARA es:
  
d11a + I1z + I2z + I3z d12a + I2z + I3z 0 q¨1
 d21a + I2z + I3z d22a + I2z + I3z 0  q¨2  +
 0 0 m3  q ¨3    
−a1 q˙2 s2 (m2 lc2 + m3 a2 ) −a1 (q˙1 + q˙2 )s2 (m2 lc2 + m3 a2 ) 0 q˙1 0 τ1
 a1 q˙1 s2 (m2 lc2 + m3 a2 ) 0 0 q˙2  +  0  =  τ2 
0 0 0 q˙3 −m3 ḡ F1
8
Para mostrar que el determinante de la matriz de inercia es diferente de cero para todo q, se calcula el
determinante, empleando un comando de Matlab, por lo que, el determinante de la matriz es el siguiente:
2
m3 (I1z I2z + I1z I3z + I2z a21 m2 + I1z a22 m3 + I2z a21 m3 + I3z a21 m2 + I3z a21 m3 + I2z lc1 2
m1 + I1z lc2 2
m2 + I3z lc1 m1 ) +
a1 a2 m2 m3 + a2 lc1 m1 m3 + a1 lc2 m2 m3 + lc1 lc2 m1 m2 + a1 a2 m3 s2 + a1 lc2 m2 s2 − 2a1 a2 lc2 m2 m3 + 2a1 a2 lc2 m2 m3 s22
2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2
A partir del resultado anterior, se puede identificar que con cualquier valor asignado a q, el determinan-
te nunca se hará cero, ya que, los existen términos independientes a la variable que se pueden considerar
constantes.
Para comprobar la propiedad de antisimetrı́a se calcula la matriz N (q, q̇) y se verifica que njk = −nkj .
Los elementos de la matriz N se calculan con las siguientes expresiones:
3
X ∂dkj
d˙kj = q˙i
i=1
dqi
Construyendo cada término:
d˙11 = ∂d11
∂q1 1
q˙ + ∂d11
∂q2 2
q˙ + ∂d11
∂q3 3
q˙
d˙12 = ∂d12
∂q1 1
q˙ + ∂d12
∂q2 2
q˙ + ∂d12
∂q3 3
q˙
d˙13 = ∂d13
∂q1 1
q˙ + ∂d13
∂q2 2
q˙ + ∂d13
∂q3 3
q˙
d˙21 = ∂d21
∂q1 1
q˙ + ∂d21
∂q2 2
q˙ + ∂d21
∂q3 3
q˙
d˙22 = ∂d22
∂q1 1
q˙ + ∂d22
∂q2 2
q˙ + ∂d22
∂q3 3
q˙
d˙23 = ∂d23
∂q1 1
q˙ + ∂d23
∂q2 2
q˙ + ∂d23
∂q3 3
q˙
d˙31 = ∂d31
∂q1 1
q˙ + ∂d31
∂q2 2
q˙ + ∂d31
∂q3 3
q˙
d˙32 = ∂d32
∂q1 1
q˙ + ∂d32
∂q2 2
q˙ + ∂d32
∂q3 3
q˙
d˙33 = ∂d33
∂q1 1
q˙ + ∂d33
∂q2 2
q˙ + ∂d33
∂q3 3
q˙
Los términos se obtienen con ayuda de Matlab y se observa que todos los términos a excepción de n12
y n21 son 0, por lo que, se procede a comprobar que:
n12 = −n21
a1 s2 (2q˙1 + q˙2 )(m2 lc2 + m3 a2 ) = −a1 s2 (2q˙1 + q˙2 )(m2 lc2 + m3 a2 )
Además, comprobando de forma matricial, se tiene:
N = −N T
 
0 a1 s2 (2q˙1 + q˙2 )(m2 lc2 + m3 a2 ) 0
−a1 s2 (2q˙1 + q˙2 )(m2 lc2 + m3 a2 ) 0 0 =
 0 0 0 
0 −a1 s2 (2q˙1 + q˙2 )(m2 lc2 + m3 a2 ) 0
− a1 s2 (2q˙1 + q˙2 )(m2 lc2 + m3 a2 )
 0 0
0 0 0
Con esto se comprueba que la propiedad de antisimétria.
Considere el modelo dinámico del robot obtenido. Se proponen los siguientes parámetros para expresar
la matriz de inercia D(q) con una representación lineal:
9
2
Θ1 = I2z + I3z + m2 lc2 + m3 a22
Θ2 = I1z + m1 lc1 + m2 a1 + m3 a21
2 2
Θ3 = m3 a1 a2 + m2 a1 lc2
Θ4 = m3
Θ5 = a1 (m3 a2 + m2 lc2 )
Entonces los elementos de la matriz de inercia, la matriz de Coriolis y el vector de gravedad se pueden
expresas de la siguiente forma:
d11 = Θ1 + Θ2 + 2Θ3 c2
d12 = d21 = Θ1 + Θ3 c2
d22 = Θ1
d33 = Θ4
c11 = −Θ5 q˙2 s2
c12 = −Θ5 (q˙1 + q˙2 )s2
g3 = −θ4 ḡ
d13 = d23 = d31 = d32 = c13 = c22 = c23 = c31 = c32 = c33 = g1 = g2 = 0
Sustituyendo los parámetros en las ecuaciones de movimientos se obtiene el regresor:

 
q¨1 + q¨2 q¨1 2c2 q¨1 + c2 q¨2 0 −s2 q˙1 q˙2 + (q˙1 + q˙2 )s2 q˙2
Y (q, q̇, q̈) = q¨1 + q¨2 0 q¨1 c2 0 −q˙1 2 s2 Θ
0 0 0 q¨3 − ḡ 0
Donde el vector de parámetros es:

   2
+ m3 a22

Θ1 I2z + I3z + m2 lc2
2
Θ2  I1z + m1 lc1
   + m2 a21 + m3 a21 

Θ= Θ3
  
= m 3 a 1 a 2 + m 2 a 1 lc2


Θ4   m3 
Θ5 a1 (m3 a2 + m2 lc2 )
10
4. Ejercicio 4
Obtenga el modelo dinámico del robot planar por medio de la técnica de Euler Lagrange, mostrado
en la siguiente figura. Suponga que la gravedad se acelera en la dirección negativa de z0 , las masas de los
eslabones son m1 , m2 y m3 respectivamente. También suponga que los centros de masa están en el centro
de cada eslabón y que los momentos de inercia son diagonales.
Figura 3: Robot planar
Muestre las propiedades de cotas de la matriz de inercia, antisimetrıa.

Para obtener la matriz de inercia, es necesario calcular los Jacobianos en cada centro de masa:

Jv1 z0 × (Ocm1 − O0 ) 03x1 03x1
J1 = =
Jω1 z0 03x1 03x1

Jv2 z0 × (Ocm2 − O0 ) z1 × (Ocm2 − O1 ) 03x1
J2 = =
Jω2 z0 z1 03x1

Jv3 z0 × (Ocm3 − O0 ) z1 × (Ocm3 − O1 ) z2 × (Ocm3 − O2 )
J3 = =
Jω3 z0 z1 z2
Para construir los Jacobianos, se necesita obtener los vectores Ocm , zn y On , los cuales quedan definidos
de la siguiente forma:
   
0 0
z0 = z1 = z2 = 0 ; O0 = 0
  
1 0
11
     
lc1 c1 a1 c1 + lc2 c12 a1 c1 + a2 c12 + lc3 c123
Ocm1 = lc1 s1 ; Ocm2 = a1 s1 + lc2 s12 ; Ocm3 = a1 s1 + a2 s12 + lc3 s123 
0 0 0
   
−lc1 s1 0 0 −a1 s1 − lc2 s12 −lc2 s12 0
 lc1 c1 0 0  a1 c1 + lc2 c12 lc2 c12 0
   
 0 0 0   0 0 0
J1 =   0
; J2 =  
 0 0  
 0 0 0

 0 0 0  0 0 0
1 0 0 1 1 0
 
−a1 s1 − a2 s12 − lc3 s123 −a2 s12 − lc3 s123 −lc3 s123
 a1 c1 + a2 c12 + lc3 c123 a2 c12 + lc3 c123 lc3 c123 
 
 0 0 0 
J3 =   
 0 0 0 

 0 0 0 
1 1 1
De la expresión para obtener la matriz de inercia.

3
X
T T
D(q) = mi Jvi Jvi + Jwi Ri (q)Ii RiT (q)Jwi
i=1
Se tiene el cálculo de la parte traslacional:

2
11 = m3 (a21 + a22 + lc3 + 2a1 a2 c2 + 2a1 lc3 c23 + 2a2 lc3 c3 )
2 2
12 = m3 (a2 + lc3 + a1 a2 c2 + a1 lc3 c23 + 2a2 lc3 c3 )
13 = m3 lc3 (lc3 + a1 c12 + a2 c3 )
21 = m3 (a22 + lc32
+ a1 a2 c2 + a1 lc3 c23 + 2a2 lc3 c3 )
2 2
22 = m3 (a2 + lc3 + 2a2 lc3 c3 )
23 = m3 lc3 (lc3 + a2 c3 )
31 = m3 lc3 (lc3 + a1 c12 + a2 c3 )
32 = m3 lc3 (lc3 + a2 c3 )
2
33 = m3 lc3
 2
    
X 3 m1 lc1 0 0 m2 (a21 + lc22 2
+ 2a1 lc2 c2 ) m2 (lc2 + a1 lc2 c2 ) 0 11 12 13
T 2 2
mi Jvi Jvi =  0 0 0 +  m2 (lc2 + a1 lc2 c2 ) m2 lc2 0 + 21 22 23
i=1 0 0 0 0 0 0 31 32 33
En la parte rotacional, las matrices de rotación están dadas por:

     
c1 −s1 0 c12 −s12 0 c123 −s123 0
R1 = s1 c1 0; R2 = s12 c12 0; R3 = s123 c123 0
0 0 1 0 0 1 0 0 1

     
X3 I1z 0 0 I2z I2z 0 I3z I3z I3z
T
Jwi Ri (q)Ii RiT (q)Jwi =  0 0 0 + I2z I2z 0 + I3z I3z I3z  =
i=1
 0 0 0 0 0 0 I3z I3z I3z
I1z + I2z + I3z I2z + I3z I3z
 I2z + I3z I2z + I3z I3z 
I3z I3z I3z
12
De esta forma la matriz de inercia queda como sigue:
 
δ1 + I1z + I2z + I3z δ2 + I2z + I3z m3 lc3 (lc3 + a1 c12 + a2 c3 ) + I3z
D(q) =  δ2 + I2z + I3z δ3 + I2z + I3z m3 lc3 (lc3 + a2 c3 ) + I3z 
2
δ4 + I3z m3 lc3 (lc3 + a2 c3 ) + I3z m3 lc3 + I3z
Donde:
2
δ1 = m1 lc1 + m2 (a21 + lc2
2
+ 2a1 lc2 c2 ) + m3 (a21 + a22 + lc3
2
+ 2a1 a2 c2 + 2a1 lc3 c23 + 2a2 lc3 c3 )
2
δ2 = m2 (lc2 + a1 lc2 c2 ) + m3 (a22 + lc3
2
+ a1 a2 c2 + a1 lc3 c23 + 2a2 lc3 c3 )
2
δ3 = m2 lc2 2
+ m3 (a22 + lc3 + 2a2 lc3 c3 )
δ4 = m3 lc3 (lc3 + a1 c12 + a2 c3 )
Para calcular la matriz de coriolis es necesario calcular los sı́mbolos de Cristoffel, los cuales están
definidos como:
h i
Cijk = 2 ∂qi + ∂qj − ∂qk
Se deben calcular n3 sı́mbolos, donde n es el numero de grados de libertad del robot.

En este caso, es necesario calcular 27 sı́mbolos: C111 , C112 , C113 , C121 = C211 , C122 = C212 , C123 = C213 ,
C131 = C311 , C132 = C312 , C133 = C313 , C221 , C222 , C223 , C231 = C321 , C232 = C322 , C233 = C323 ,
C331 , C332 , C333 ,
Reduciendo los cálculos para un k fijo, Cijk = Cjik . Se tiene que se deben calcular 18 términos:
C111 = 0; C112 = m3 (a1 a2 s2 + a1 lc3 s23 ) + m2 a1 lc2 s2 ; C113 = m3 a1 l3 (s12 − s23 )

C121 = C211 = m3 (a1 a2 s2 + a1 lc3 s23 ) + m2 a1 lc2 s2 ; C122 = C212 = 0;
C123 = C213 = m3 (a2 lc3 s3 + 12 a1 lc3 s23 − 21 a1 lc3 s12 ); C131 = C311 = m3 a1 lc3 s23 ; C132 = C312 = m3 a2 lc3 s3 ;
C133 = C313 = 21 m3 a1 lc3 s23 ; C221 = m3 (a1 a2 s2 + a1 lc3 s23 ) + m2 a1 lc2 s2 ; C222 = 0; C223 = m3 a2 lc3 s3 ;
C231 = C321 = m3 (a2 lc3 s3 + a1 lc3 s23 ); C232 = C322 = m3 a2 lc3 s3 ; C233 = C323 = 0;
C331 = m3 lc3 (a1 s23 + a2 s3 ); C332 = m3 a2 lc3 s3 ; C333 = 0;
Para calcular la matriz de Coriolis, la componente (k, j) de la matriz C(q, q̇) esta dada por:
n
X
Ckj = Cijk q˙i
i=1
Calculando los términos de la matriz C:
C11 = C111 q˙1 + C211 q˙2 + C311 q˙3

C12 = C121 q˙1 + C221 q˙2 + C321 q˙3
C13 = C131 q˙1 + C231 q˙2 + C331 q˙3
C21 = C112 q˙1 + C212 q˙2 + C312 q˙3
C22 = C122 q˙1 + C222 q˙2 + C322 q˙3
C23 = C132 q˙1 + C232 q˙2 + C332 q˙3
C31 = C113 q˙1 + C213 q˙2 + C313 q˙3
C32 = C123 q˙1 + C223 q˙2 + C323 q˙3
C33 = C133 q˙1 + C233 q˙2 + C333 q˙3
La matriz de Coriolis es:

 
C11 C12 C13
C(q, q̇) = C21 C22 C23 
C31 C32 C33
13
C11 = m3 (a1 a2 s2 + a1 lc3 s23 )q˙2 + m2 a1 lc2 s2 q˙2 + m3 a1 lc3 s23 q˙3
C12 = m3 (a1 a2 s2 +a1 lc3 s23 )q˙1 +m2 a1 lc2 s2 q˙1 +m3 (a1 a2 s2 +a1 lc3 s23 )q˙2 +m2 a1 lc2 s2 q˙2 +m3 (a2 lc3 s3 +a1 lc3 s23 )q˙3
C13 = m3 a1 lc3 s23 q˙1 + m3 (a2 lc3 s3 + a1 lc3 s23 )q˙2 + m3 lc3 (a1 s23 + a2 s3 )q˙3
C21 = m3 (a1 a2 s2 + a1 lc3 s23 )q˙1 + m2 a1 lc2 s2 q˙1 + m3 a2 lc3 s3 q˙3
C22 = m3 a2 lc3 s3 q˙3
C23 = m3 a2 lc3 s3 q˙1 + m3 a2 lc3 s3 q˙2 + m3 a2 lc3 s3 q˙3
C31 = m3 a1 l3 (s12 − s23 )q˙1 + m3 (a2 lc3 s3 + 21 a1 lc3 s23 − 21 a1 lc3 s12 )q˙2 + 21 m3 a1 lc3 s23 q˙3
C32 = m3 (a2 lc3 s3 + 21 a1 lc3 s23 − 12 a1 lc3 s12 )q˙1 + m3 a2 lc3 s3 q˙2
C33 = 12 m3 a1 lc3 s23 q˙1
Finalmente, para el cálculo de la energı́a potencial, la energı́a potencial de cada eslabón esta dada por
la masa por la gravedad por la altura.
P1 = 0
P2 = 0
P3 = 0
P = P 1 + P2 + P3 = 0
Puesto que, la gravedad actúa en el eje z0 no existe energı́a potencial, ya que se supone que los eslabones
no sufren deformación, por lo tanto, el vector de gravedad es igual a 0:
∂P
 
 ∂q1   
 ∂P  0
g(q) =  = 0
   
 ∂q2 
 ∂P  0
∂q3
Finalmente, considerando las entradas externas de control por el vector u = [τ1 τ2 τ3 ]T , el modelo
dinámico del robot planar de triple revoluta es:
  
δ1 + I1z + I2z + I3z δ2 + I2z + I3z m3 lc3 (lc3 + a1 c12 + a2 c3 ) + I3z q¨1
 δ2 + I2z + I3z δ3 + I2z + I3z m3 lc3 (lc3 + a2 c3 ) + I3z  q¨2  +
2
δ4 + I3z m3 lc3(lc3 + a2 c3 ) + I3z
    m3 lc3 + I3z q¨3
C11 C12 C13 q˙1 τ1
C21 C22 C23  q˙2  = τ2 
C31 C32 C33 q˙3 τ3
˙ − 2C(q, q̇) es antisimétrica, para calcularla usamos:

La matriz N (q, q̇) := D(q)
3
X ∂dkj
d˙kj = q˙i
i=1
dqi
d˙11 = ∂d11
∂q1 1
q˙ + ∂d11
∂q2 2
q˙ + ∂d11
∂q3 3
q˙
d˙12 = ∂d12
∂q1 1
q˙ + ∂d12
∂q2 2
q˙ + ∂d12
∂q3 3
q˙
d˙13 = ∂d13
∂q1 1
q˙ + ∂d13
∂q2 2
q˙ + ∂d13
∂q3 3
q˙
d˙21 = ∂d21
∂q1 1
q˙ + ∂d21
∂q2 2
q˙ + ∂d21
∂q3 3
q˙
d˙22 = ∂d22
∂q1 1
q˙ + ∂d22
∂q2 2
q˙ + ∂d22
∂q3 3
q˙
14
d˙23 = ∂d23
∂q1 1
q˙ + ∂d23
∂q2 2
q˙ + ∂d23
∂q3 3
q˙
d˙31 = ∂d31
∂q1 1
q˙ + ∂d31
∂q2 2
q˙ + ∂d31
∂q3 3
q˙
d˙32 = ∂d32
∂q1 1
q˙ + ∂d32
∂q2 2
q˙ + ∂d32
∂q3 3
q˙
d˙33 = ∂d33
∂q1 1
q˙ + ∂d33
∂q2 2
q˙ + ∂d33
∂q3 3
q˙
Se obtiene:
d˙11 = −(m3 (2a1 a2 s2 + 2a1 lc3 s23 ) + 2m2 a1 lc2 s2 )q˙2 − 2m3 a1 lc3 s23 q˙3
˙
d12 = −(m3 (a1 a2 s2 + a1 lc3 s23 ) + m2 a1 lc2 s2 )q˙2 − m3 (2a2 lc3 s3 + a1 lc3 s23 )q˙3
d˙13 = −m3 a1 lc3 s23 q˙2 − m3 lc3 (a2 s3 + a1 s23 )q˙3
d˙21 = −(m3 (a1 a2 s2 + a1 lc3 s23 ) + m2 a1 lc2 s2 )q˙2 − m3 (2a2 lc3 s3 + a1 lc3 s23 )q˙3
d˙22 = −2m3 a2 lc3 s3 q˙3
d˙23 = −m3 a2 lc3 s3 q˙3
d˙31 = −m3 a1 lc3 s12 q˙1 − m3 a1 lc3 s12 q˙2 − m3 a2 lc3 s3 q˙3
d˙32 = −m3 a2 lc3 s3 q˙3
d˙33 = 0
Finalmente, calculamos los elementos de la matriz N :
nkj = d˙kj − 2ckj
Es decir:
n11 = d˙11 − 2c11 = −(m3 (2a1 a2 s2 + 2a1 lc3 s23 ) + 2m2 a1 lc2 s2 )q˙2 − 2m3 a1 lc3 s23 q˙3 − 2m3 (a1 a2 s2 +
a1 lc3 s23 )q˙2 − 2m2 a1 lc2 s2 q˙2 − 2m3 a1 lc3 s23 q˙3
˙
n12 = d12 − 2c12 = −(m3 (a1 a2 s2 + a1 lc3 s23 ) + m2 a1 lc2 s2 )q˙2 − m3 (2a2 lc3 s3 + a1 lc3 s23 )q˙3 − 2m3 (a1 a2 s2 +
a1 lc3 s23 )q˙1 − 2m2 a1 lc2 s2 q˙1 − 2m3 (a1 a2 s2 + a1 lc3 s23 )q˙2 − 2m2 a1 lc2 s2 q˙2 − 2m3 (a2 lc3 s3 + a1 lc3 s23 )q˙3
n13 = d˙13 − 2c13 =
−m3 a1 lc3 s23 q˙2 − m3 lc3 (a2 s3 + a1 s23 )q˙3 − 2m3 a1 lc3 s23 q˙1 − 2m3 (a2 lc3 s3 + a1 lc3 s23 )q˙2 − 2m3 lc3 (a1 s23 + a2 s3 )q˙3
n21 = d˙21 − 2c21 = −(m3 (a1 a2 s2 + a1 lc3 s23 ) + m2 a1 lc2 s2 )q˙2 − m3 (2a2 lc3 s3 + a1 lc3 s23 )q˙3 − 2m3 (a1 a2 s2 +
a1 lc3 s23 )q˙1 − 2m2 a1 lc2 s2 q˙1 − 2m3 a2 lc3 s3 q˙3
n22 = d˙22 − 2c22 = −2m3 a2 lc3 s3 q˙3 − 2m3 a2 lc3 s3 q˙3
n23 = d˙23 − 2c23 = −m3 a2 lc3 s3 q˙3 − 2m3 a2 lc3 s3 q˙1 − 2m3 a2 lc3 s3 q˙2 − 2m3 a2 lc3 s3 q˙3
n31 = d˙31 − 2c31 = −m3 a1 lc3 s12 q˙1 − m3 a1 lc3 s12 q˙2 − m3 a2 lc3 s3 q˙3 − 2m3 a1 l3 (s12 − s23 )q˙1 − 2m3 (a2 lc3 s3 +
1
a l s − 21 a1 lc3 s12 )q˙2 − m3 a1 lc3 s23 q˙3
2 1 c3 23
n32 = d˙32 − 2c32 = −m3 a2 lc3 s3 q˙3 − 2m3 (a2 lc3 s3 + 21 a1 lc3 s23 − 21 a1 lc3 s12 )q˙1 − 2m3 a2 lc3 s3 q˙2
n33 = d˙33 − 2c33 = −m3 a1 lc3 s23 q˙1
la matriz de inercia D(q):
2
Θ1 = m1 lc1 + m2 (a21 + lc2
2
) + m3 (a21 + a22 + lc3
2
) + I1z + I2z + I3z
Θ2 = m2 a1 lc2
Θ3 = m3 a1 a2
Θ4 = m3 a1 lc3
Θ5 = m3 a2 lc3
Θ6 = m2 lc2 + m3 (a22 + lc3
2 2
) + I2z + I3z
2
Θ7 = m3 lc3
2
Θ8 = m2 lc2 + m3 (a22 + lc32
) + I2z + I3z
2
Θ9 = m3 lc3 + I3z
15
d11 = Θ1 + 2Θ2 c2 + 2Θ3 c2 + 2Θ4 c23 + 2Θ5 c3
d12 = d21 = Θ6 + Θ2 c2 + Θ3 + Θ4 c23 + Θ5 c3
d13 = d31 = Θ7 + Θ4 c12 + Θ5 c3
d22 = Θ8 + Θ5 c3
d23 = d32 = Θ9 + Θ5 c3
d33 = Θ9
No se requieren más términos para la matriz de Coriolis, por que lo queda de la siguiente forma:
C11 = (Θ3 s2 + Θ4 s23 + Θ2 )q˙2 + Θ4 s23 q˙3

C12 = (Θ3 s2 + Θ4 s23 + Θ2 s2 )q˙1 + (Θ3 s2 + Θ4 s23 + Θ2 s2 )q˙2 + (Θ5 s3 + Θ4 s23 )q˙3
C13 = Θ4 s23 q˙1 + (Θ5 s3 + Θ4 s23 )q˙2 + (Θ4 s23 + Θ5 s3 )q˙3
C21 = (Θ3 s2 + Θ4 s23 )q˙1 + Θ2 s2 q˙2 + Θ5 s3 q˙3
C22 = Θ5 s3 q˙3
C23 = Θ5 s3 (q˙1 + q˙2 + q˙3 )
1 1 1
C31 = Θ4 (s12 − s23 )q˙1 + (Θ5 s3 + Θ4 s23 − Θ4 s12 )q˙2 + Θ4 s23 q˙3
2 2 2
1 1
C32 = (Θ5 s3 + Θ4 s23 − Θ4 s12 )q˙1 + Θ5 s3 q˙2
2 2
1
C33 = Θ4 s23 q˙1
2
Sustituyendo los parámetros en las ecuaciones de movimientos se obtiene el regresor:
τ1 = Θ1 + 2Θ2 c2 + 2Θ3 c2 + 2Θ4 c23 + 2Θ5 c3 + Θ6 + Θ2 c2 + Θ3 + Θ4 c23 + Θ5 c3 + Θ7 + Θ4 c12 + Θ5 c3 +

(Θ3 s2 + Θ4 s23 + Θ2 )q˙2 + Θ4 s23 q˙3 + (Θ3 s2 + Θ4 s23 + Θ2 s2 )q˙1 + (Θ3 s2 + Θ4 s23 + Θ2 s2 )q˙2 + (Θ5 s3 +
Θ4 s23 )q˙3 + Θ4 s23 q˙1 + (Θ5 s3 + Θ4 s23 )q˙2 + (Θ4 s23 + Θ5 s3 )q˙3
τ1 = Θ1 + (2c2 + c2 + q˙2 + s2 q˙1 + s2 q˙2 )Θ2 + (3c2 + 1 + 2s2 q˙2 + s2 q˙1 )Θ3 + (2c23 + c12 + 4s23 q˙2 + 2s23 q˙3 +
2s23 q˙1 )Θ4 + (4c3 + 2s3 q˙3 + s3 q˙2 )Θ5 + Θ6
τ2 = Θ6 + Θ2 c2 + Θ3 + Θ4 c23 + Θ5 c3 + Θ8 + Θ5 c3 + Θ9 + Θ5 c3 + (Θ3 s2 + Θ4 s23 )q˙1 + Θ2 s2 q˙2 + Θ5 s3 q˙3 +
Θ5 s3 q˙3 + Θ5 s3 (q˙1 + q˙2 + q˙3 )
τ2 = (c2 + s2 q˙2 )Θ2 + (1 + s2 q˙1 )Θ3 + (c23 + s23 q˙1 )Θ4 + (3c3 + 2s3 q˙3 + s3 (q˙1 + q˙2 + q˙3 ))Θ5 + Θ6 + Θ8 + Θ9
1 1 1
τ3 = Θ7 + Θ4 c12 + Θ5 c3 + Θ9 + Θ5 c3 + Θ9 + Θ4 (s12 − s23 )q˙1 + (Θ5 s3 + Θ4 s23 − Θ4 s12 )q˙2 + Θ4 s23 q˙3 +
2 2 2
1 1 1
(Θ5 s3 + Θ4 s23 − Θ4 s12 )q˙1 + Θ5 s3 q˙2 + Θ4 s23 q˙1
2 2 2
τ3 = (c12 + (s12 − s23 )q˙1 + 2 s23 q˙2 − 12 s12 q˙2 + 12 s23 q˙3 + s23 q˙1 − 21 s12 q˙1 )Θ4 + (c3 + 2s3 q˙2 + s3 q˙1 )Θ5 + Θ7 + 2Θ9
1
 
1 Y12 Y13 Y14 Y15 1 0 0 0
Y (q, q̇, q̈) = 0 Y22 Y23 Y24 Y25 1 0 1 1
0 0 0 Y34 Y35 0 1 0 2
Y12 = 2c2 + c2 + q˙2 + s2 q˙1 + s2 q˙2

Y13 = 3c2 + 1 + 2s2 q˙2 + s2 q˙1
Y14 = 2c23 + c12 + 4s23 q˙2 + 2s23 q˙3 + 2s23 q˙1
Y15 = 4c3 + 2s3 q˙3 + s3 q˙2
Y22 = c2 + s2 q˙2
Y23 = 1 + s2 q˙1
Y24 = c23 + s23 q˙1
Y25 = 3c3 + 2s3 q˙3 + s3 (q˙1 + q˙2 + q˙3 )
Y34 = c12 + (s12 − s23 )q˙1 + 21 s23 q˙2 − 12 s12 q˙2 + 12 s23 q˙3 + s23 q˙1 − 21 s12 q˙1
Y35 = c3 + 2s3 q˙2 + s3 q˙1
16
Y el vector de parámetros es:
2
+ m2 (a21 + lc2
2
) + m3 (a21 + a22 + lc3 2
   
Θ1 m1 lc1 ) + I1z + I2z + I3z
Θ2   m2 a1 lc2 
   
Θ3   m 3 a 1 a 2

   
Θ4   m3 a1 lc3 
   
Θ = Θ5  = 
   m3 a2 lc3 

Θ6   2 2 2
   m2 lc2 + m3 (a2 + lc3 ) + I2z + I3z 

Θ7   2
   m l
3 c3


2
Θ8   m2 lc2 + m3 (a22 + lc3 2
) + I2z + I3z 
2
Θ9 m3 lc3 + I3z
17
5. Ejercicio 5
La siguiente figura muestra un manipulador RRR espacial por medio de la técnica de Euler Lagrange.
Suponiendo que las masas de los eslabones son m1 , m2 , m3 , los centros de masa están en los centros
geométricos y las inercias son de tipo diagonal, obtenga el modelo dinámico.
Figura 4: Robot RRR

Para obtener la matriz de inercia se inicia calculando los Jacobianos en cada centro de masa:

Jv1 z0 × (Ocm1 − O0 ) 03×1 03×1
J1 = =
Jw1 z0 03×1 03×1

Jv2 z0 × (Ocm2 − O0 ) z1 × (Ocm2 − O1 ) 03×1
J2 = =
Jw2 z0 z1 03×1

Jv3 z0 × (Ocm3 − O0 ) z1 × (Ocm3 − O1 ) z2 × (Ocm3 − O2 )
J3 = =
Jw3 z0 z1 z2
Es necesario construir los vectores Ocmi , zi y Oi para desarrollar los Jacobianos. El método propuesto
por Denavit Hartenberg también nos ayudará a encontrar las matrices de rotación Ri .
A partir de los marcos coordenados propuestos en la figura 4, se obtuvieron los parámetros de Denavit
Hartenberg mostrados en la tabla 2. Donde qi = θi .
1 0 π/2 0 q1 +π
2 a2 0 0 q2
3 a3 0 0 q3
Cuadro 2: Parámetros de Denavit Hartenberg para robot RRR.
18
De la misma forma, se obtuvieron los parámetros respecto a los centros de masa de cada eslabón.
1 0 π/2 0 q1 +π
1
2 a
2 2
0 0 q2
1
3 a
2 3
0 0 q3
Cuadro 3: Parámetros de Denavit Hartenberg para robot RRR respecto a los centros de masa.
Calculando las matrices de transformación homogénea para cada marco y centro de masa:
     
−c1 0 −s1 0 c2 −s2 0 a2 c2 c3 −s3 0 a3 c3
−s1 0 c1 0
 A2 = s2 c2 0 a2 s2  A3 = s3 c3 0 a3 s3 
   
A1 =   0 1 0 0 0 0 1 0  0 0 1 0 
0 0 0 1 0 0 0 1 0 0 0 1
a2
c3 −s3 0 a23 c3
   
c2 −s2 0 2 c2
s2 c2 0 a2 s2  a3 
 A3cm = s3 c3 0 2 s3 

A1cm = A1 A2cm =  2
0 0 1 0  0 0 1 0 
0 0 0 1 0 0 0 1
Ası́:
 
−c1 c2 c1 s2 −s1 −a2 c1 c2
−s1 c2 s1 s2 c1 −a2 s1 c2 
T1 = A1 T2 = A1 A2 =   s2

c2 0 a2 s 2 
0 0 0 1
 
−c1 c23 c1 s23 −s1 −c1 (a3 c23 + a2 c2 )
−s1 c23 s1 s23 c1 −s1 (a3 c23 + a2 c2 )
T3 = A1 A2 A3 = 
 s23

c23 0 a2 s2 + a3 s23 
0 0 0 1
Para los centros de masa, se tiene:

−c1 c2 c1 s2 −s1 − a22 c1 c2
 
−s1 c2 s1 s2 c1 − a2 s1 c2 
Tcm1 = Acm1 Tcm2 = A1 Acm2 = 2 
 s2 a2
c2 0 2
s 2

0 0 0 1
c1
 
−c1 c23 c1 s23 −s1 − 2 (2a2 c2 + a3 c23 )
−s1 c23 s1 s23 c1 − s21 (2a2 c2 + a3 c23 )
Tcm3 = A1 A2 Acm3 = 
 s23 c23 0 a2 s2 + a23 s23 
0 0 0 1
Los vectores zi y Oi son los primeros 3 elementos de la tercera y cuarta columna de las matrices Ti
respectivamente. Ası́ mismo, la matriz de rotación Ri corresponde a los primeros 3 elementos de las 3
primeras columnas de la matriz Ti . Además:
   
0 0
z0 = 0 O0 = 0
1 0
Desarrollando los Jacobianos:
19
 a2 a2
 s1
(2a2 c2 + a3 c23 ) c1 (a2 s2 + a23 s23 ) 1
   
0 0 0 sc
2 1 2
cs
2 1 2
0 2
acs
2 3 1 23
a2 a2 − c1 (2a2 c2 + a3 c23 ) s1 (a2 s2 + a3 s23 ) 1
− 2 c1 c2
0 0 0 ss 0 ass 
2 1 2 2 3 1 23 
 
 a2
  2 a3
2
a3
0 0 0  0 c 0 0 a c + c c 
2 2 2 2 2 23 2 23 
 
J1 =  J2 =   J3 = 
0
 0 0
  0
 −s1 0


 0 −s 1 −s1 

0 0 0  0 c1 0  0 c1 c1 
1 0 0 1 0 0 1 0 0
La expresión para obtener la matriz de inercia:
3
X
T T
D(q) = [mi Jvi Jvi + Jwi Ri (q)Ii RiT (q)Jwi ]
i=1
Donde las matrices Jvi y Jwi corresponden a las primeras y últimas 3 filas respectivamente de las matrices
Ji . Las matrices de rotación Ri se encuentran en las matrices de transformación Ti y se propuso el siguiente
tensor de inercia para el eslabón i:  
Iix 0 0
Ii =  0 Iiy 0 
0 0 Iiz
Calculando la parte traslacional de la matriz de inercia:
3
X
T T T T
mi Jvi Jvi = m1 Jv1 Jv1 + m2 Jv2 Jv2 + m3 Jv3 Jv3
i=1
Calculando cada término por separado:

  1 2 
0 0 0 a m c2
4 2 2 2
0 0
T T 1 2 T
m1 Jv1 Jv1 = 0 0 0 m2 Jv2 Jv2 =  0 a m 0 m3 Jv3
4 2 2
Jv3 =
0 0 0 0 0 0
 m3
(2a2 c2 + a3 c23 )2

4
0 0
m3
 0 4
(4a22 + 4a2 a3 c3 + a23 ) a34m3 (a3 + 2a2 c3 )
a3 m3 a32 m3
0 4
(a3 + 2a2 c3 ) 4
Ası́:
 m3
(2a2 c2 + a3 c23 )2 + 14 a22 m2 c22

4
0 0
P3 T m3 2 2 1 2 a3 m3
i=1 mi Jvi Jvi =  0 4
(4a 2 + 4a 2 3 3 + a3 ) + 4 a2 m2
a c 4
(a3 + 2a2 c3 )
a3 m3 a32 m3
0 4
(a3 + 2a2 c3 ) 4

3
X
T
Jwi Ri (q)Ii RiT (q)Jwi = Jw1
T
R1 (q)I1 R1T (q)Jw1 + Jw2
T
R2 (q)I2 R2T (q)Jw2 + Jw3
T
R3 (q)I3 R3T (q)Jw3
i=1
Calculando cada término por separado:

 
I1y 0 0
T
Jw1 R1 (q)I1 R1T (q)Jw1 =  0 0 0
0 0 0
 
I2y + s22 (I2x − I2y ) 0 0
T
Jw2 R2 (q)I2 R2T (q)Jw2 = 0 I2z 0
0 0 0
20
 
I3y + s223 (I3x − I3y ) 0 0
T
Jw3 R3 (q)I3 R3T (q)Jw3 = 0 I3z I3z 
0 I3z I3z
Ası́:
 
3
X I1y + I2y + I3y + s22 (I2x − I2y ) + s223 (I3x − I3y ) 0 0
T
Jwi Ri (q)Ii RiT (q)Jwi = 0 I2z + I3z I3z 
i=1 0 I3z I3z
La matriz de inercia queda como:

 
D(1, 1) 0 0
m3 2 2 1 2 a3 m3
D(q) =  0 4
(4a 2 + 4a a c
2 3 3 + a 3 ) + 4 a2 m2 + I2z + I3z 4
(a3 + 2a2 c3 ) + I3z 
a3 m3 a32 m3
0 4
(a3 + 2a2 c3 ) + I3z 4
+ I3z
Donde D(1, 1) = m43 (2a2 c2 + a3 c23 )2 + 41 a22 m2 c22 + I1y + I2y + I3y + s22 (I2x − I2y ) + s223 (I3x − I3y )
Para calcular la matriz de Coriolis es necesario calcular los sı́mbolos de Christoffel. Se deben calcular
n3 sı́mbolos, donde n es el numero de grados de libertad del robot.
En este caso, es necesario calcular 27 sı́mbolos: C111 , C112 , C113 , C121 = C211 , C122 = C212 , C123 = C213 ,
C131 = C311 , C132 = C312 , C133 = C313 , C221 , C222 , C223 , C231 = C321 , C232 = C322 , C233 = C323 ,
C331 , C332 , C333 , Reduciendo los cálculos para un k fijo, Cijk = Cjik . Se tiene que se deben calcular 18
términos.
C111 = 0
1 1
C112 = −q˙1 2 [− (m3 a3 s23 (a3 c23 + 2a2 c2 ) + 2a2 s2 ) − a22 m2 s2 c2 + s2 c2 (I2x − I2y ) + s23 c23 (I3x − I3y )]
4 4
1
C113 = −q˙1 2 [− (a3 m3 s23 (a3 c23 + 2a2 c2 )) + s23 c23 (I3x − I3y )]
4
1
C121 = −q˙1 q˙2 [− (a3 m3 s23 (a3 c23 + 2a2 c2 )) + s23 c23 (I3x − I3y )]
4
C122 = 0
C123 = 0
1
C131 = q˙1 q˙3 [− (a3 m3 s23 (a3 c23 + 2a2 c2 )) + s23 c23 (I3x − I3y )]
4
C132 = 0
C133 = 0
C221 = 0
C222 = 0
1
C223 = q˙2 2 a2 a3 m3 s3
2
C231 = 0
1
C232 = −q˙2 q˙3 a2 a3 m3 s3
2
C233 = 0
C331 = 0
1
C332 = −q˙3 2 a2 a3 m3 s3
2
21
C333 = 0
Para calcular la matriz de Coriolis, la componente (k, j) de la matriz C(q, q̇) esta dada por:
n
X
Ckj = Cijk q˙i
i=1
Calculando los términos de la matriz C:
C11 = C111 q˙1 + C211 q˙2 + C311 q˙3
C12 = C121 q˙1 + C221 q˙2 + C321 q˙3

C13 = C131 q˙1 + C231 q˙2 + C331 q˙3
C21 = C112 q˙1 + C212 q˙2 + C312 q˙3
C22 = C122 q˙1 + C222 q˙2 + C322 q˙3
C23 = C132 q˙1 + C232 q˙2 + C332 q˙3
C31 = C113 q˙1 + C213 q˙2 + C313 q˙3
C32 = C123 q˙1 + C223 q˙2 + C323 q˙3
C33 = C133 q˙1 + C233 q˙2 + C333 q˙3
La matriz de Coriolis es:  
C11 C12 C13
C(q, q̇) = C21 C22 C23  (1)
C31 C32 C33
Donde:
1
C11 = −q˙1 (q˙2 2 + q˙3 2 )[− a3 m3 s23 (a3 c23 + 2a2 c2 ) + s23 c23 (I3x − I3y )]
4
1
C12 = −q˙1 2 q˙2 [− a3 m3 s23 (a3 c23 + 2a2 c2 ) + s23 c23 (I3x − I3y )]
4
1
C13 = q˙1 2 q˙3 [− a3 m3 s23 (a3 c23 + 2a2 c2 ) + s23 c23 (I3x − I3y )]
4
1 1
C21 = −q˙1 3 [− m3 (a3 c23 + 2a2 c2 )(a3 s23 + 2a2 s2 ) − a22 m2 s2 c2 + s2 c2 (I2x − I2y ) + s23 c23 (I3x − I3y )]
4 4
1
C22 = −q˙2 q˙3 2 [ a2 a3 m3 s3 ]
2
1
C23 = −q˙3 (q˙2 2 + q˙3 2 )[ a2 a3 m3 s3 ]
2
1
C31 = −q˙1 3 [− a3 m3 s23 (a3 c23 + 2a2 c2 ) + s23 c23 (I3x − I3y )]
4
1
C32 = q˙2 3 [ a2 a3 m3 s3 ]
2
C33 = 0
Finalmente, se obtiene la energı́a potencial de cada eslabón respecto a su centro de masa:
a1
P1 = m1 ḡ(− )
2
a2
P2 = m2 ḡ( s2 )
2
22
a3
P3 = m3 ḡ(a2 s2 + s23 )
2
Ası́ la energı́a potencial del robot es:
1 1
P = P1 + P2 + P3 = ḡ[m3 ( a3 s23 + a2 s2 ) − (a1 m1 + a2 m2 s2 )]
2 2
Calculando el vector de gravedad se tiene:
 ∂P   
∂q1 0
∂P 
g(q) =  ∂q
2
= ḡ m3 ( 21 a3 c23 + a2 c2 ) + 12 a2 m2 c2 
∂P 1
∂q3
amc
2 3 3 23

3
X ∂dkj
d˙kj = q˙i
i=1
dqi

∂d11 ∂d11 ∂d11
d˙11 = q˙1 + q˙2 + q˙3
∂q1 ∂q2 ∂q3
∂d12 ∂d12 ∂d12
d˙12 = q˙1 + q˙2 + q˙3
∂q1 ∂q2 ∂q3
∂d13 ∂d13 ∂d13
d˙13 = q˙1 + q˙2 + q˙3
∂q1 ∂q2 ∂q3
∂d21 ∂d21 ∂d21
d˙21 = q˙1 + q˙2 + q˙3
∂q1 ∂q2 ∂q3
∂d22 ∂d22 ∂d22
d˙22 = q˙1 + q˙2 + q˙3
∂q1 ∂q2 ∂q3
∂d23 ∂d23 ∂d23
d˙23 = q˙1 + q˙2 + q˙3
∂q1 ∂q2 ∂q3
∂d31 ∂d31 ∂d31
d˙31 = q˙1 + q˙2 + q˙3
∂q1 ∂q2 ∂q3
∂d32 ∂d32 ∂d32
d˙32 = q˙1 + q˙2 + q˙3
∂q1 ∂q2 ∂q3
∂d33 ∂d33 ∂d33
d˙33 = q˙1 + q˙2 + q˙3
∂q1 ∂q2 ∂q3
Ası́:
1 1
d˙11 = q˙2 [− (m3 )(a3 c23 + 2a2 c2 )(a3 s23 + 2a2 s2 ) − a22 m2 s2 c2 + 2s2 c2 (I2x − I2y ) + 2s23 c23 (I3x − I3y )]...
2 2
1
...q˙3 [− a3 m3 s23 (a3 c23 + 2a2 c2 ) + 2s23 c23 (I3x − I3y )]
2
d˙12 = 0
23
d˙13 = 0
d˙21 = 0
d˙22 = −q˙3 a2 a3 m3 s3
1
d˙23 = −q˙3 [ a2 a3 m3 s3 ]
2
d˙31 = 0
1
d˙32 = −q˙3 [ a2 a3 m3 s3 ]
2
d˙33 = 0
Finalmente, calculamos los elementos de la matriz N :
nkj = d˙kj − 2ckj
Es decir:
n11 = d˙11 − 2c11
n12 = d˙12 − 2c12
n13 = d˙13 − 2c13
n21 = d˙21 − 2c21
n22 = d˙22 − 2c22
n23 = d˙23 − 2c23
n31 = d˙31 − 2c31
n32 = d˙32 − 2c32
n33 = d˙33 − 2c33

Ası́:
1
n11 = 2q˙1 q˙2 2 [− a3 m3 s23 (a3 c23 + 2a2 c2 ) + s23 c23 (I3x − I3y )] + ...
4
1 1
... + q˙2 [− m3 (a3 c23 + 2a2 c2 )(a3 s23 + 2a2 s2 ) − a22 m2 s2 c2 + 2s23 c23 (I3x − I3y ) + 2s2 c2 (I2x − I2y )] − ...
2 2
24
1
... − 2q˙1 q˙3 2 [− a3 m3 s23 (a3 c23 + 2a2 c2 )] + ...
4
1
... + q˙3 [− a3 m3 s23 (a3 c23 + 2a2 c2 ) + 2s23 c23 (I3x − I3y )]
2
1
n12 = 2q˙1 2 q˙2 [− a3 m3 s23 (a3 c23 + 2a2 c2 ) + s23 c23 (I3x − I3y )]
4
1
n13 = −2q˙1 2 q˙3 [− a3 m3 s23 (a3 c23 + 2a2 c2 ) + s23 c23 (I3x − I3y )]
4
1 1
n21 = 2q˙1 3 [− m3 (a3 c23 + 2a2 c2 )(a3 s23 + 2a2 s2 ) − a22 m2 s2 c2 + s2 c2 (I2x − I2y ) + s23 c23 (I3x − I3y )]
4 4
n22 = (q˙2 q˙3 2 − q˙3 )a2 a3 m3 s3
1
n23 = (q˙2 2 q˙3 + q˙3 3 − q˙3 )a2 a3 m3 s3
2
1
n31 = 2q˙1 3 [− a3 m3 s23 (a3 c23 + 2a2 c2 ) + s23 c23 (I3x − I3y )]
4
1
n32 = −(q˙2 3 + q˙3 )[a2 a3 m3 s3 ]
2
n33 = 0
la matriz de inercia D(q):
Θ1 = I1y + I2y + I3y
Θ2 = m3 a22
Θ3 = a2 a3 m3
Θ4 = m3 a23
Θ5 = m2 a22
Θ6 = I2x − I2y
Θ7 = I3x − I3y
Θ8 = I2z − I3z
Θ9 = I3z
Ası́:
d11 = Θ1 + Θ6 s22 + Θ7 s223 + Θ2 c22 + Θ3 c2 c23 + 14 (Θ4 c223 + Θ5 c22 )
25
 
d11 0 0
D(q) = 0 Θ2 + Θ3 c3 + Θ8 + 14 (Θ4 + Θ5 )
 1
Θ + 21 Θ3 c3 + Θ9 
4 4
1
0 Θ + 21 Θ3 c3 + Θ9
4 4
1
Θ + Θ9
4 4
No se requieren parámetros adicionales para expresar la matriz de Coriolis:

C11 = −q˙1 (q˙2 2 + q˙3 2 )[s23 c23 (− 41 Θ4 + Θ7 ) − 21 Θ3 c2 ]
C21 = −q˙1 3 [s23 c23 (− 41 Θ4 + Θ7 ) − 12 Θ3 (s2 c23 + s23 c2 ) + s2 c2 (−Θ2 + Θ6 )]
C31 = −q˙1 3 [s23 c23 (− 41 Θ4 + Θ7 ) − 12 Θ3 s23 c2 ]
C11 −q˙1 2 q˙2 [s23 c23 (− 14 Θ4 + Θ7 ) − 12 Θ3 c2 ] q˙1 2 q˙3 [s23 c23 (− 41 Θ4 + Θ7 ) − 21 Θ3 c2 ]

 
C(q, q̇) = C21 −q˙2 q˙3 2 ( 12 Θ3 s3 ) −q˙3 (q˙2 2 + q˙3 2 )( 21 Θ3 s3 ) 

3 1
C31 q˙2 ( 2 Θ3 s3 ) 0
Proponiendo los siguientes parámetros para expresar el vector de gravedad:
Θ10 = m3 a3
Θ11 = m3 a2
Θ12 = m2 a2
El vector de gravedad g(q) queda:
 
0
g(q) = ḡ  12 Θ10 c23 + c2 (Θ11 + 21 Θ12 )
1
Θ c
2 10 23
El vector de parámetros es:

  
Θ1 I1y + I2y + I3y
 Θ2  
   m3 a22 

 Θ3   a2 a3 m3 
   2

 Θ4   m3 a3 
   2

 Θ5   m 2 a 2

   
 Θ6   I2x − I2y 
Θ=
 Θ7  =  I3x − I3y 
  
   
 Θ8   I2z − I3z 
   
 Θ9   I3z 
   
Θ10   m 3 a 3

   
Θ11   m3 a2 
Θ12 m2 a2
26
6. Ejercicio 6
Considere un cuerpo rı́gido bajo movimientos puros de rotación sin fuerzas externas. La energı́a cinética
está dada por
K − 12 (Ixx ωx2 + Iyy ωy2 + Izz ωz2 )
respecto al marco coordenado localizado en el centro de masa cuyos ejes coordenados son los ejes
principales. Considerando como coordenadas generalizadas los ángulos de Euler, muestre que el modelo
dinámico está dado por:
Ixx ω˙x + (Izz − Iyy )ωy ωz = 0
Iyy ω˙y + (Ixx − Izz )ωz ωx = 0
Izz ω˙z + (Iyy − Ixx )ωx ωy = 0
Dado que se considera un cuerpo rı́gido bajo movimientos puros de rotación, se tiene que su Jacobiano
de velocidad es el siguiente:
 
ωx
Jω1 = ωy 
ωz
Considerando el modelado de Newton-Euler, el momento de inercia está atado al cuerpo, por lo que,
se considera la energı́a cinética de la siguiente forma:
K = 21 q̇ T D(q)q̇
Al tener únicamente movimiento rotacional, la matriz de inercia se calcula de la siguiente forma:
P1 :0

T T
D(q) = m1 Jv Jv1 + Jω R1 I1 R1T Jω1

i=1
1 1
Se sabe que el momento de inercia del cuerpo sobre marco inercial cuyo origen es su centro de masa:
I0 = R1 I1 R1T
Por lo tanto:
  
Ixx 0 0 ωx
D(q) = JωT1 I0 Jω1 = ωx ωy ωz  0 Iyy 0   ωy 
0 0 Izz ωz
El momento angular expresado en el marco inercial es:
h = I0 ω0
Considerando ω0 como:
ω0 = Rω
Se tiene:
T :I
h = I0 ω0 = RI
R Rω = RIω

Derivando la expresión anterior respecto al tiempo y considerando I constante, se tiene:

ḣ = ṘIω + RI ω̇
27
Además:
S(ω0 ) = ṘRT
Ṙ = S(ω0 )R
Ası́, ḣ se puede reescribir como:
ḣ = S(ω0 )RIω + RI ω̇
La ecuación anterior es respecto al marco inercial, para que esté respecto al marco del cuerpo se pre-
multiplica por RT . Es decir:
RT ḣ = RT S(ω0 )RIω + RT RI ω̇
Usando propiedades de la matriz antisimétrica:
RT ḣ = S(RT (ω0 ))Iω + I ω̇
RT ḣ = S(ω)Iω + I ω̇
RT ḣ = ω × (Iω) + I ω̇
donde a ω × (Iω) se le conoce como término giroscópico.
La matriz de inercia queda expresada como:

 
Ixx 0 0
I =  0 Iyy 0 
0 0 Izz
Aplicando la ecuación de movimiento angular en términos de un marco rı́gido atado al cuerpo, por modelado
dinámico de Newton-Euler, se tiene:
I ω̇ + ω × (Iω) = τ
donde:
 
ωx
ω = Jω1 = ωy 
ωz
 
ω˙x
˙
ω̇ = Jω1 = ω˙y 

ω˙z
Por lo tanto, se tiene:
         
Ixx 0 0 ω˙x ωx Ixx 0 0 ωx
 0 Iyy 0  ω˙y  + ωy  ×  0 Iyy 0  ωy  = τ
0 0 Izz ω˙z ωz 0 0 Izz ωz
     
Ixx ω˙x ωx Ixx ωx
 Iyy ω˙y  + ωy  ×  Iyy ωy  = τ
Izz ω˙z ωz Izz ωz
   
Ixx ω˙x ωy Izz ωz − Iyy ωy ωz
 Iyy ω˙y  + −(ωx Izz ωz − Ixx ωx ωz ) = τ
Izz ω˙z ωx Iyy ωy − Ixx ωx ωy
Finalmente, reescribiendo y agrupando términos, se tienen las siguientes expresiones:
Ixx ω˙x + (Izz − Iyy )ωy ωz = 0 Iyy ω˙y + (Ixx − Izz )ωz ωx = 0 Izz ω˙z + (Iyy − Ixx )ωx ωy = 0
Con lo que se demuestra que el modelo dinámico es el mismo que el esperado al inicio del ejercicio.
28
7. Ejercicio 7
7.1. Modelo Dinámico
Obtenga el modelo dinámico por medio de Euler Lagrange del Robot RP de la figura.
Figura 5: Robot RP
Para obtener el modelo dinámico, primero hay que calcular la matriz de inercia, la cual se obtiene
calculando los Jacobianos en cada centro de masa:

Jv1 z0 × (Ocm1 − O0 ) 03×1
J1 = =
Jw1 z0 03×1

Jv2 z0 × (Ocm2 − O0 ) z1
J2 = =
Jw2 z0 03×1
Hartenberg mostrados en la tabla 4.
π
1 h /2 0 q1 + π /2
2 0 0 lc1 + q2 + l2 /2 0
Cuadro 4: Parámetros de Denavit Hartenberg para robot RP.
29
1 lc1 0 0 q1
2 0 0 lc1 + q2 0
Cuadro 5: Parámetros de Denavit Hartenberg para robot RP respecto a los centros de masa.
Considerando:
Cπ/2+q1 = Cos(π/2 + q1 ) = Cos(π/2)Cos(q1 ) − Sen(π/2)Sen(q1 ) = −Sen(q1 )
Sπ/2+q1 = Sen(π/2 + q1 ) = Sen(π/2)Cos(q1 ) + Cos(π/2)Sen(q1 ) = Cos(q1 )
C1 = Cos(q1 ) y S1 = Sen(q1 )
Se obtiene:
   
Cπ/2+q1 0 Sπ/2+q1 hCπ/2+q1 −S1 0 C1 −hS1
 Sπ/2+q1 0 −Cπ/2+q1 hSπ/2+q1   C1 0 S1 hC1 
A1 =   0
= 
1 0 0   0 1 0 0 
0 0 0 1 0 0 0 1
   
C1 −S1 0 lc1 C1 1 0 0 0
 S1 C1 0 lc1 S1 
; A2 = 0 1 0 0
 
Ac1 = 0

0 1 0  0 0 1 lc1 + q2 + l2 /2
0 0 0 1 0 0 0 1
 
1 0 0 0
0 1 0 0 
Ac2 = 
0 0 1 lc1 + q2 

0 0 0 1
    
−S1 0 C1 −hS1 1 0 0 0 −S1 0 C1 (lc1 + q2 )C1 − hS1
 C1 0 S1 hC1  0 1 0 0  =  C1 0 S1 (lc1 + q2 )S1 + hC1 
 
A1 Ac2 = 
 0 1 0

0  0 0 1 lc1 + q2   0 1 0 0 
0 0 0 1 0 0 0 1 0 0 0 1
  
−S1 0 C1 −hS1 1 0 0 0
 C1 0 S1 hC1  0 1 0 0 
T20 = A1 A2 =  0 1 0
 
0  0 0 1 lc1 + q2 + l2 /2
0 0 0 1 0 0 0 1
 
−S1 0 C1 (lc1 + q2 + l2 /2)C1 − hS1
 C1 0 S1 (lc1 + q2 + l2 /2)S1 + hC1 
T20 = 
 0 1 0

0 
0 0 0 1
A partir de lo anterior se tiene:

       
lc1 C1 (lc1 + q2 )C1 − hS1 0 C1
Ocm1 =  lc1 S1 ; Ocm2 = (lc1 + q2 )S1 + hC1 ; z0 = 0; z1 =  S1 
0 0 1 0
     
0 lc1 C1 −lc1 S1
z0 × (Ocm1 − O0 ) = 0 ×  lc1 S1  =  lc1 C1 
1 0 0
30
     
0 (lc1 + q2 )C1 − hS1 −(lc1 + q2 )S1 − hC1
z0 × (Ocm2 − O0 ) = 0 × (lc1 + q2 )S1 + hC1  =  (lc1 + q2 )C1 − hS1 
1 0 0
Con lo anterior se obtienen los Jacobianos:

   
−lc1 S1 0 −(lc1 + q2 )S1 − hC1 C1
 lc1 C1 0  (lc1 + q2 )C1 − hS1 S1 
   
J 0 0 Jv2 0 0
J1 = v1 = 
  
; J 2 = =  
Jw1  0
 0 
 Jw2

 0 0 

 0 0  0 0
1 0 1 0
Ahora, procedemos a calcular la matriz de inercia:

2
X
D(q) = mi JvTi Jvi + JwTi Ri Ii RiT Jwi
i=1
Primero calculamos la parte traslacional de la matriz de inercia.

 
−lc1 S1 0 2
T −lc1 S1 lc1 C1 0  lc1 0
m1 Jv1 Jv1 = m1 lc1 C1 0 = m1

0 0 0 0 0
0 0
lc21 S12 + lc21 C12 = lc21

 
−(lc1 + q2 )S1 − hC1 C1
−(lc1 + q2 )S1 − hC1 (lc1 + q2 )C1 − hS1 0 
m2 JvT2 Jv2 = m2 (lc1 + q2 )C1 − hS1 S1 
C1 S1 0
0 0
((
[−(lc1 + q2 )S1 − hC1 ]2 + [(lc1 + q2 )C1 − hS1 ]2 = (lc1 + q2 )2 S12 + h2 C12 + (
2(l(c1(+(q( 2 2
2 )hS1 C1 + (lc1 + q2 ) C1 +
((
(
h2 S12 − ( 2 2
(
2(l(c1(+(q(
2 )hS1 C1 = (lc1 + q2 ) + h
((
2 ( 2 (
[−(lc1 + q2 )S1 − hC1 ]C1 + [(lc1 + q2 )C1 − hS1 ]S1 = −( 1 + q2 )S1 C1 − hC1 + ( 1 + q2 )C1 S1 − hS1 = −h
( (
(lc( (((( (lc( ((((

T (lc1 + q2 )2 + h2 −h
m2 Jv2 Jv2 = m2
−h 1
2
X m1 lc21 0 m2 [(lc1 + q2 )2 + h2 ] −m2 h
mi JvTi Jvi = +
0 0 −m2 h m2
i=1
2
X m1 lc21 + m2 [(lc1 + q2 )2 + h2 ] −m2 h
mi JvTi Jvi =
−m2 h m2
i=1
Después calculamos la parte rotacional de la matriz de inercia.
Del análisis de Denavit Hartenberg se tiene que las rotaciones son:

 
−S1 0 C1
R1 = R2 =  C1 0 S1 
0 1 0
31
    
−S1 0 C1 I1x 0 0 −S1 C1 0 0 0
T T 0 0 1 
Jw1 R1 I1 R1 Jw1 = C1 0 S1   0 I1y 0   0 0 1 0 0
0 0 0
0 1 0 0 0 I1z C1 S1 0 1 0
   
I1x 0 0 −S1 C1 0 0 0
T T 0 1 0 
Jw1 R1 I1 R1 Jw1 = 0 I1y 0   0 0 1   0 0
0 0 0
0 0 I1z C1 S1 0 1 0
  
−S1 C1 0 0 0
0 I1y 0 
JwT1 R1 I1 R1T Jw1 = 0 0 1 0 0
0 0 0
C1 S 1 0 1 0
 
0 0
T T 0 I1y 0  I1 0 I2y 0
Jw1 R1 I1 R1 Jw1 = 0 0 = y
∴ JwT2 R2 I2 R2T Jw2 =
0 0 0 0 0 0 0
1 0
2
X I1y 0 I2y 0 I1y + I2y 0
JwTi Ri Ii RiT Jwi = + =
0 0 0 0 0 0
i=1
Por lo que la matriz de inercia es:

2
X m1 lc21 + m2 [(lc1 + q2 )2 + h2 ] −m2 h I1y + I2y 0
D(q) = mi JvTi Jvi + JwTi Ri Ii RiT Jwi = +
−m2 h m2 0 0
i=1

m1 lc21 + m2 [(lc1 + q2 )2 + h2 ] + I1y + I2y −m2 h
D(q) =
−m2 h m2
Para continuar con el calculo del modelo dinámico, se debe obtener la matriz de coriolis, la cual se
obtiene a través de los sı́mbolos de christoffel, los cuales se calculan con:
cijk = [ + − ]
2 ∂qi ∂qj ∂qk
El numero de sı́mbolos de christoffel depende de n3 , siendo n el numero de grados de libertad en el sis-

tema, en este caso son 2, por lo que 23 = 8 sı́mbolos, también sabiendo que k es fijo tenemos que cijk = cjik .
0
1 ∂d11 ∂d11 ∂d11
c111 = [ + − ]=0
2 ∂q1 ∂q1 ∂q1
1 ∂d12 ∂d11 ∂d12 1
c121 = c211 = [ + − ] = [2m2 (lc1 + q2 )] = m2 (lc1 + q2 )
2 ∂q1 ∂q2 ∂q1 2
0 0
1 ∂d12 ∂d22
c221 = [2 − ]=0
2 ∂q2 ∂q1
0
1 ∂d21 ∂d11
c112 = [2 − ] = −m2 (lc1 + q2 )
2 ∂q1 ∂q2
0
1 ∂d21 ∂d22 ∂d21
c212 = c122 = [ + − ]=0
2 ∂q2 ∂q1 ∂q2
0
1 ∂d22 ∂d22 ∂d22
c222 = [ + − ]=0
2 ∂q2 ∂q2 ∂q2
Con lo anterior calculamos los elementos de la matriz de coriolis.
32
2
X
Ckj = cijk q˙i
i=1
C11 = c111q˙10 + c211 q˙2 = m2 q˙2 (lc + q2 )

:
1
C12 = c121 q˙1 + :q˙20 = m2 q˙1 (lc + q2 )
c221

1
C21 = c112 q˙1 + q˙20 = −m2 q˙1 (lc + q2 )
c212
:
1
C22 = q˙10 +
c122
: :q˙20 = 0
c222

Por lo que la matriz de coriolis es:

m2 q˙2 (lc1 + q2 ) m2 q˙1 (lc1 + q2 )
C(q, q̇) =
−m2 q˙1 (lc1 + q2 ) 0
Para obtener el vector de gravedad primero calculamos la energı́a potencial.

P1 = m1 ḡlc1 (1 + S1 )
P2 = m2 ḡ[(lc1 + q2 )(1 + S1 ) + h(1 + C1 )]
P = P1 + P2 = m1 ḡlc1 (1 + S1 ) + m2 ḡ[(lc1 + q2 )(1 + S1 ) + h(1 + C1 )]
A partir de la energı́a potencial calculamos el vector de gravedad.
∂P
dq1 m1 ḡlc1 C1 + m2 ḡ[(lc1 + q2 )C1 − hS1 ]
g(q) = ∂P =
dq2
m2 ḡ(1 + S1 )
Ahora si tenemos todos los elementos para armar el modelo dinámico:
D(q)q̈ + C(q, q̇)q̇ + g(p) = τ

m1 lc21 + m2 [(lc1 + q2 )2 + h2 ] + I1y + I2y −m2 h q¨1
−m2 h m2 q¨2

m2 q˙2 (lc1 + q2 ) m2 q˙1 (lc1 + q2 ) q˙1 m1 ḡlc1 C1 + m2 ḡ[(lc1 + q2 )C1 − hS1 ] τ
+ + = 1
−m2 q˙1 (lc1 + q2 ) 0 q˙2 m2 ḡ(1 + S1 ) τ2
7.2. Antisimetrı́a
2
X ∂dkj
d˙kj = q˙i
i=1
dqi
0
∂d11 > ∂d11
d˙11 = q˙1 + q˙2 = 2m2 q˙2 (lc1 + q2 )
dq1 dq2
0 0
˙ ∂d12> ∂d12 >
d12 = q˙1 + q˙2 = 0
dq1 dq2
0 0
∂d 21
> ∂d21
>
d˙21 = q˙1 + q˙2 = 0

dq1 dq2
0 0
˙ ∂d22 >
∂d22>
d22 = q˙1 + q˙2 = 0
dq1 dq2
Finalmente calculamos los elementos de la matriz N:
nkj = d˙kj − 2Ckj
33
n11 = d˙11 − 2C11 = (
2m q˙2(
(2( (lc( ((( 2m (
1 + q2 ) − ( (2 q˙2(
(lc( (((
1 + q2 ) = 0
0
˙
n12 = d12 − 2C12 = −2m2 q˙1 (lc + q2 )

1
0
n21 = d˙21

− 2C21 = 2m2 q˙1 (lc1 + q2 )
0
0
n22 = d˙21

− 2
C21
*=
0

0 −2m2 q˙1 (lc1 + q2 )
N (q, q̇) =
2m2 q˙1 (lc1 + q2 ) 0
Ya que n12 = −n21 , por lo tanto N es una matriz antisemétrica.
7.3. Representación Lineal

Considerando el Modelo dinámico:

m1 lc21 + m2 [(lc1 + q2 )2 + h2 ] + I1y + I2y −m2 h q¨1
−m2 h m2 q¨2

m2 q˙2 (lc1 + q2 ) m2 q˙1 (lc1 + q2 ) q˙1 m1 ḡlc1 C1 + m2 ḡ[(lc1 + q2 )C1 − hS1 ] τ
+ + = 1
−m2 q˙1 (lc1 + q2 ) 0 q˙2 m2 ḡ(1 + S1 ) τ2
y tomando en cuenta:
(lc1 + q2 )2 = lc21 + 2lc1 q2 + q22
La matriz de inercia y la matriz de coriolis se puede expresar con los siguientes parámetros:
Θ1 = m1 lc21 + m2 (lc21 + h2 ) + I1y + I2y

Θ2 = m2 lc1
Θ3 = m2
Θ4 = −m2 h
Ası́, los elementos de la matriz de inercia y de la matriz de coriolis se pueden escribir como:
d11 = Θ1 + 2Θ2 q2 + Θ3 q22

d12 = d21 = Θ4
d22 = Θ3
C11 = Θ2 q˙2 + Θ3 q2 q˙2
C12 = −C21 = Θ2 q˙1 + Θ3 q2 q˙1
Para el vector de gravedad se define este parámetro extra:
Θ5 = m1 lc1
El vector de gravedad queda:
g1 = Θ5 ḡC1 + Θ2 ḡC1 + Θ3 ḡq2 C1 + Θ4 ḡS1

g2 = Θ3 ḡ(1 + S1 )
34
Para pasarlo a una función ”Y”se realizo lo siguiente:
τ1 = [Θ1 + 2Θ2 q2 + Θ3 q22 ]q¨1 + Θ4 q¨2 + [Θ2 q˙2 + Θ3 q2 q˙1 ]q˙1 + [Θ2 q˙1 + Θ3 q2 q˙1 ]q˙2 + Θ5 ḡC1 + Θ2 ḡC1 + Θ3 ḡq2 C1 +
Θ4 ḡS1 = Θ1 q¨1 + Θ2 (2q2 q¨1 + q˙2 q˙1 + q˙1 q˙2 + ḡC1 ) + Θ3 (q22 q¨1 + q2 q˙2 q˙1 + q2 q˙1 q˙2 + ḡq2 C1 ) + Θ4 (q¨2 + ḡS1 ) + Θ5 ḡC1
τ2 = Θ4 q¨1 + Θ3 q¨2 − [Θ2 q˙1 + Θ3 q2 q˙1 ]q˙1 + Θ3 ḡ(1 + S1 ) = −Θ2 q˙1 2 + Θ3 [q¨2 − q2 q˙1 2 + ḡ(1 + S1 )] + Θ4 q¨1
Sustituyendo los parámetro en las ecuaciones de movimiento se obtiene el regresor:
12 = 2q2 q¨1 + 2q˙1 q˙2 + ḡC1

13 = q22 q¨1 + 2q2 q˙1 q˙2 + ḡq2 C1
23 = q¨2 + q2 q˙1 2 + ḡ(1 + S1 )

q¨1 2q2 q¨1 + 2q˙1 q˙2 + ḡC1 q22 q¨1 + 2q2 q˙1 q˙2 + ḡq2 C1 q¨2 + ḡS1 ḡC1
Y (q, q̇, q̈) =
0 −q˙1 2 q¨2 + q2 q˙1 2 + ḡ(1 + S1 ) q¨1 0
y el vector de parámetros Θ es:
  
m1 lc21 + m2 (lc21 + h2 ) + I1y + I2y

Θ1
Θ2   m2 lc1 
   
Θ3  = 
Θ= m2
  

Θ4   −m2 h 
Θ5 m1 lc1
35
8. Ejercicio 8
8.1. Modelo Dinámico
Obtenga el modelo dinámico por medio de Euler Lagrange del Robot RPR de la figura.
Figura 6: Robot RPR
Para obtener el modelo dinamico, primero hay que calcular la matriz de inercia, la cual se obtiene
calculando los Jacobianos en cada centro de masa:

Jv1 z0 × (Ocm1 − O0 ) 03×1 03×1
J1 = =
Jw1 z0 03×1 03×1

Jv2 z0 × (Ocm2 − O0 ) z1 03×1
J2 = =
Jw2 z0 03×1 03×1

Jv3 z0 × (Ocm3 − O0 ) z1 z2 × (Ocm3 − O2 )
J3 = =
Jw3 z0 03×1 z2
Hartenberg mostrados en la tabla 6.
π
1 0 /2 0 q1 + π /2
2 0 - π /2 lc1 + q2 + lc2 0
3 l3 0 0 q3 - π /2
Cuadro 6: Parámetros de Denavit Hartenberg para robot RPR.
36
1 lc1 0 0 q1
π
2 0 - /2 lc1 + q2 0
3 lc3 0 0 q3 - π /2
Cuadro 7: Parámetros de Denavit Hartenberg para robot RPR respecto a los centros de masa.
Considerando:
Cπ/2+q1 = Cos(π/2 + q1 ) = Cos(π/2)Cos(q1 ) − Sen(π/2)Sen(q1 ) = −Sen(q1 )
Sπ/2+q1 = Sen(π/2 + q1 ) = Sen(π/2)Cos(q1 ) + Cos(π/2)Sen(q1 ) = Cos(q1 )
C−π/2+q3 = Cos(−π/2 + q3 ) = Cos(−π/2)Cos(q3 ) − Sen(π/2)Sen(q3 ) = Sen(q3 )
S−π/2+q3 = Sen(−π/2 + q3 ) = Sen(−π/2)Cos(q3 ) + Cos(π/2)Sen(q3 ) = −Cos(q3 )
C13 = Cos(q1 + q3 ) = Cos(q1 )Cos(q3 ) − Sen(q1 )Sen(q3 )
S13 = Sen(q1 + q3 ) = Sen(q1 )Cos(q3 ) + Cos(q1 )Sen(q3 )
C1 = Cos(q1 ), S1 = Sen(q1 ), C3 = Cos(q3 ) y S3 = Sen(q3 )
Se obtiene:
   
Cπ/2+q1 0 Sπ/2+q1 0 −S1 0 C1 0
 Sπ/2+q1 0 −Cπ/2+q1 0  C1 0 S1 0
A1 =  0
= 
1 0 0  0 1 0 0
0 0 0 1 0 0 0 1
   
C1 −S1 0 lc1 C1 1 0 0 0
 S1 C1 0 lc1 S1 
; A2 = 0 0 1 0
 
Ac1 = 0 0 −1 0 lc1 + q2 + lc2 

0 1 0 
0 0 0 1 0 0 0 1
 
1 0 0 0
0 0 1 0 
Ac2 = 0 −1

lc1 + q2 
0 0 0 1
   
C−π/2+q3 −S−π/2+q3 0 l2 C−π/2+q3 S3 C3 0 l3 S3
 S−π/2+q3 C−π/2+q3 0 l2 S−π/2+q3  −C3 S3 0 −l3 C3 
A3 =  = 
 0 0 1 0   0 0 1 0 
0 0 0 1 0 0 0 1
 
S3 C3 0 lc3 S3
−C3 S3 0 −lc3 C3 
Ac3 =  0

0 1 0 
0 0 0 1
    
−S1 0 C1 0 1 0 0 0 −S1 −C1 0 (lc1 + q2 )C1
 =  C1 −S1 0 (lc1 + q2 )S1 
 C1 0 S1 0 0 0 1 0   
A1 Ac2 = 
 0 1 0 0 0 −1

lc1 + q2   0 0 1 0 
0 0 0 1 0 0 0 1 0 0 0 1
    
−S1 0 C1 0 1 0 0 0 −S1 −C1 0 (lc1 + q2 + lc2 )C1
 C1 0 S1 0 0 0 1 0   C1 −S1 0 (lc1 + q2 + lc2 )S1 
A1 A2 =  0 1 0 0 0 −1
 = 
lc1 + q2 + lc2   0 0 1 0 
0 0 0 1 0 0 0 1 0 0 0 1
37
   
−S1 −C1 0 (lc1 + q2 + lc2 )C1 S 3 C3 0 lc3 S3
 C1 −S1 0 (lc1 + q2 + lc2 )S1  −C3 S3 0 −lc3 C3 
A1 A2 Ac3 = 
 0
  
0 1 0   0 0 1 0 
0 0 0 1 0 0 0 1
 
−S1 S3 + C1 C3 −S1 C3 − C1 S3 0 −lc3 S1 S3 + lc3 C1 C3 + (lc1 + q2 )C1
 C1 S3 + S1 C3 C1 C3 − S1 S3 0 lc3 C1 S3 + lc3 S1 C3 + (lc1 + q2 )S1 
A1 A2 Ac3 =  
 0 1 0 0 
0 0 0 1
 
C13 −S13 0 lc3 C13 + (lc1 + q2 )C1
 S13 C13 0 lc3 S13 + (lc1 + q2 )S1 
A1 A2 Ac3 = 
 0

0 1 0 
0 0 0 1
 
C13 −S13 0 l3 C13 + (lc1 + q2 + lc2 )C1
 S13 C13 0 l3 S13 + (lc1 + q2 + lc2 )C1 
T30 = A1 A2 A3 = 
 0

0 1 0 
0 0 0 1
A partir de lo anterior se tiene:

     
lc1 C1 (lc1 + q2 )C1 lc3 C13 + (lc1 + q2 + lc2 )C1
Ocm1 =  lc1 S1 ; Ocm2 =  (lc1 + q2 )S1 ; Ocm3 =  lc3 S13 + (lc1 + q2 + lc2 )S1 
0 0 0
     
(lc1 + q2 + lc2 )C1 0 C1
O2 = (lc1 + q2 + lc2 )S1 ; z0 = z2 = 0 ; z1 = S1 
    
0 1 0
 (( ((   
(lc1
lc3 C13 + ( (( +( 2 )C1 − (
q2(+(lc( (lc1
( +(
( q2(+(lc(
2 )C1 l c 3 C 13
(( ((( (
Ocm3 − O2 = lc3 S13 + (
(lc1
(( +( 2 )S1 − (
q2(+(lc( (lc1
(( +(q2(+(+l c2 )S1
 =  lc3 S13 
0 0
     
0 lc1 C1 −lc1 S1
z0 × (Ocm1 − O0 ) = 0 ×  lc1 S1  =  lc1 C1 
1 0 0
     
0 (lc1 + q2 )C1 −(lc1 + q2 )S1
z0 × (Ocm2 − O0 ) = 0 ×  (lc1 + q2 )S1  =  (lc1 + q2 )C1 
1 0 0
     
0 lc3 C13 + (lc1 + q2 )C1 −lc3 S13 − (lc1 + q2 )S1
z0 × (Ocm3 − O0 ) = 0 ×  lc3 S13 + (lc1 + q2 )S1  =  lc3 C13 + (lc1 + q2 )C1 
1 0 0
     
0 lc3 C13 −lc3 S13
z2 × (Ocm3 − O2 ) = 0 ×  lc3 S13  =  lc3 C13 
1 0 0
Con lo anterior se obtienen los Jacobianos:
38
   
−lc1 S1 0 0 −(lc1 + q2 )S1 C1 0
 lc1 C1 0 0  (lc1 + q2 )C1 S1 0
 
 
J  0 0 0 J 0 0 0
J1 = v1 =  ; J2 = v2 = 
 

Jw1  0
 0 0 Jw2 
 0 0 0
 0 0 0   0 0 0
1 0 0 1 0 0
 
−lc3 S13 − (lc1 + q2 + lc2 )S1 C1 −lc3 S13
 lc3 C13 + (lc1 + q2 + lc2 )C1 S1 lc3 C13 
 
J 0 0 0 
J3 = v3 = 


Jw3 
 0 0 0  
 0 0 0 
1 0 1

2
X
i=1

   2 
−lc1 S1 lc1 C1 0 −lc1 S1 0 0 lc1 0 0
T
m1 Jv1 Jv1 = m1  0 0 0  lc1 C1 0 0 = m1  0 0 0
0 0 0 0 0 0 0 0 0
lc21 S12 + lc21 C12 = lc21

  
−(lc1 + q2 )S1 (lc1 + q2 )C1 0 −(lc1 + q2 )S1 C1 0
m2 JvT2 Jv2 = m2  C1 S1 0  (lc1 + q2 )C1 S1 0
0 0 0 0 0 0
[−(lc1 + q2 )S1 ]2 + [(lc1 + q2 )C1 ]2 = (lc1 + q2 )2 S12 + (lc1 + q2 )2 C12 = (lc1 + q2 )2

( (
[−(lc1 + q2 )S1 ]C1 + [(lc1 + q2 )C1 ]S1 = −(
( (
(lc(
1 + q2 )S1 C1 + (
(((( (lc(
1 + q2 )C1 S1 = 0
((((
S12 + C12 = 1
 
(lc1 + q2 )2 0 0
m2 JvT2 Jv2 = m2  0 1 0
0 0 0
 
−lc3 S13 − (lc1 + q2 + lc2 )S1 lc3 C13 + (lc1 + q2 + lc2 )C1 0
m3 JvT3 Jv3 = m3  C1 S1 0
−lc3 S13 lc3 C13 0
 
−lc3 S13 − (lc1 + q2 + lc2 )S1 C1 −lc3 S13
 lc3 C13 + (lc1 + q2 + lc2 )C1 S1 lc3 C13 
0 0 0
39
[−lc3 S13 − (lc1 + q2 + lc2 )S1 ]2 + [lc3 C13 + (lc1 + q2 + lc2 )C1 ]2 = lc23 S13
2
+
2 2
(lc1 + q2 + lc2 ) S1 + 2lc3 (lc1 + q2 + lc2 )S1 S13 + lc3 C13 + (lc1 + q2 + lc2 ) C1 + 2lc3 (lc1 + q2 + lc2 )C1 C13 = lc23 +
2 2 2 2
::::::::::::::::: ::::::::::::::::::
(lc1 + q2 + lc2 )2 + 2lc3 (lc1 + q2 + lc2 )C3

2lc3 (lc1 + q2 + lc2 )(S1 S13 + C1 C13 ) = 2lc3 (lc1 + q2 + lc2 ) [S1 (S1 C3 + C1 S3 ) + C1 (C1 C3 − S1 S3 )] = 2lc3 (lc1 +
q2 + lc2 )(S12 C3 + S1
C 2
1 S3 + C1 C3 − C1S
1 S3 ) = 2lc3 (lc1 + q2 + lc2 )C3

(((
[−lc3 S13 − (lc1 + q2 + lc2 )S1 ]C1 + [lc3 C13 + (lc1 + q2 + lc2 )C1 ]S1 = −lc3 C1 S13 − ((lc( +(
1 ( q2(+(lc(
2 )S1 C1 +
( (
lc3 S1 C13 + ((lc( +(q2(+ lc2 )C1 S1 = −lc3 S3
( ( (
1 (
2 2
**−C1 S13 + S1 C13 = −C1 (S1 C3 + C1 S3 ) + S1 (C1 C3 − S1 S3 ) = − 1 C3 − C1 S3 +
C1S
S1C1 C3 − S1 S3 = −S3

2
[−lc3 S13 −(lc1 +q2 +lc2 )S1 ](−lc3 S13 )+[lc3 C13 +(lc1 +q2 +lc2 )C1 ](lc3 C13 ) = lc23 S13 +lc3 (lc1 + q2 + lc2 )S1 S13 +
::::::::::::::::::::::
lc23 C13
2
+ lc3 (lc1 + q2 + lc2 )C1 C13 = lc23 + lc3 (lc1 + q2 + lc2 )C3
::::::::::::::::::::::
2 
lc3 + (lc1 + q2 + lc2 )2 + 2lc3 (lc1 + q2 + lc2 )C3 −lc3 S3 lc23 + lc3 (lc1 + q2 + lc2 )C3
= m3  −lc3 S3 1 −lc3 S3 
2
lc3 + lc3 (lc1 + q2 + lc2 )C3 −lc3 S3 lc23
   
X 2 m1 lc21 0 0 m2 (lc1 + q2 )2 0 0
T
mi Jvi Jvi =  0 0 0 +  0 m2 0
i=1 0 0 0 0 0 0
 
m3 [lc23 + (lc1 + q2 + lc2 )2 + 2lc3 (lc1 + q2 + lc2 )C3 ] −m3 lc3 S3 m3 [lc23 + lc3 (lc1 + q2 + lc2 )C3 ]
+ −m3 lc3 S3 m3 −m3 lc3 S3 
2 2
m3 [lc3 + lc3 (lc1 + q2 + lc2 )C3 ] −m3 lc3 S3 m3 lc3
11 = m1 lc21 + m2 (lc1 + q2 )2 + m3 [lc23 + (lc1 + q2 + lc2 )2 + 2lc3 (lc1 + q2 + lc2 )C3 ]

13 = 31 = m3 [lc23 + lc3 (lc1 + q2 + lc2 )C3 ]
 
X2 11 −m 3 lc 3 S 3 13
mi JvTi Jvi = −m3 lc3 S3 m2 + m3 −m3 lc3 S3 
i=1 31 −m3 lc3 S3 m3 lc23

     
−S1 0 C1 1 0 0 S3 C3 0
R1 =  C1 0 S1 ; R2 =  0 0 1; R3 = −C3 S3 0
0 1 0 10 −1 0 0 0 1
     
0 0 1 −S1 0 C1 I1x 0 0 −S1 C1 0 0 0 0
JwT1 R1 I1 R1T Jw1 = 0 0 0  C1 0 S1   0 I1y 0   0 0 1 0 0 0
0 0 0 0 1 0 0 0 I1z C1 S 1 0 1 0 0
    
0 1 0 I1x 0 0 −S1 C1 0 0 0 0
JwT1 R1 I1 R1T Jw1 = 0 0 0  0 I1y 0   0 0 1 0 0 0
0 0 0 0 0 I1z C1 S 1 0 1 0 0
   
0 I1y 0 −S1 C1 0 0 0 0
JwT1 R1 I1 R1T Jw1 = 0 0 0  0 0 1 0 0 0
0 0 0 C1 S1 0 1 0 0
40
    
0 0 I1y 0 0 0 I1y 0 0
JwT1 R1 I1 R1T Jw1 = 0 0 0  0 0 0 =  0 0 0
0 0 0 1 0 0 0 0 0
        
0 0 1 1 0 0 I2x 0 0 1 0 0 0 0 0
JwT2 R2 I2 R2T Jw2 = 0 0 0 0 0 1  0 I2y 0  0 0 −1 0 0 0
0 0 0 0 −1 0 0 0 I2z 0 1 0 1 0 0
      
0 −1 0 I2x 0 0 1 0 0 0 0 0
JwT2 R2 I2 R2T Jw2 = 0 0 0  0 I2y 0  0 0 −1 0 0 0
0 0 0 0 0 I2z 0 1 0 1 0 0
    
0 −I2y 0 1 0 0 0 0 0
JwT2 R2 I2 R2T Jw2 = 0 0 0 0 0 −1 0 0 0
0 0 0 0 1 0 1 0 0
    
0 0 I2y 0 0 0 I2y 0 0
JwT2 R2 I2 R2T Jw2 = 0 0 0  0 0 0 =  0 0 0
0 0 0 1 0 0 0 0 0
     
0 0 1 S3 C3 0 I3x 0 0 S3 −C3 0 0 0 0
JwT3 R3 I3 R3T Jw3 = 0 0 0 −C3 S3 0  0 I3y 0  C3 S3 0 0 0 0
0 0 1 0 0 1 0 0 I3z 0 0 1 1 0 1
    
0 0 1 I3x 0 0 S3 −C3 0 0 0 0
JwT3 R3 I3 R3T Jw3 = 0 0 0  0 I3y 0  C3 S3 0 0 0 0
0 0 1 0 0 I3z 0 0 1 1 0 1
   
0 0 I3z S3 −C3 0 0 0 0
JwT3 R3 I3 R3T Jw3 = 0 0 0  C3 S3 0 0 0 0
0 0 I3z 0 0 1 1 0 1
     
0 0 I3z 0 0 0 I3z 0 I3z
JwT3 R3 I3 R3T Jw3 = 0 0 0  0 0 0 =  0 0 0 
0 0 I3z 1 0 1 I3z 0 I3z
       
2
X I1y 0 0 I2y 0 0 I3z 0 I3z I1y + I2y + I3z 0 I3z
JwTi Ri Ii RiT Jwi =  0 0 0 +  0 0 0 +  0 0 0  =  0 0 0
i=1 0 0 0 0 0 0 I3z 0 I3z I3z 0 I3z

11 = m1 lc21 + m2 (lc1 + q2 )2 + m3 [lc23 + (lc1 + q2 + lc2 )2 + 2lc3 (lc1 + q2 + lc2 )C3 ]
13 = 31 = m3 [lc23 + lc3 (lc1 + q2 + lc2 )C3 ]
 
2
X 11 −m3 lc3 S3 13
D(q) = mi JvTi Jvi + JwTi Ri Ii RiT Jwi = −m3 lc3 S3 m2 + m3 −m3 lc3 S3 
i=1 31 −m3 lc3 S3 m3 lc23
 
I1y + I2y + I3z 0 I3z
+ 0 0 0
I3z 0 I3z
41
11 = m1 lc21 + m2 (lc1 + q2 )2 + m3 [lc23 + (lc1 + q2 + lc2 )2 + 2lc3 (lc1 + q2 + lc2 )C3 ] + I1y + I2y + I3z
13 = 31 = m3 [lc23 + lc3 (lc1 + q2 + lc2 )C3 ] + I3z
 
11 −m3 lc3 S3 13
D(q) = −m3 lc3 S3 m2 + m3 −m3 lc3 S3 
31 −m3 lc3 S3 m3 lc23 + I3z
Para continuar con el calculo del modelo dinámico, se debe obtener la matriz de coriolis, la cual se
obtiene a través de los sı́mbolos de christoffel, los cuales se calculan con:
cijk = [ + − ]
2 ∂qi ∂qj ∂qk
El numero de sı́mbolos de christoffel depende de n3 , siendo n el numero de grados de libertad en el sis-
tema, en este caso son 3, por lo que 33 = 27 sı́mbolos, también sabiendo que k es fijo tenemos que cijk = cjik .
0
1 ∂d11 ∂d11 ∂d11
c111 = [ + − ]=0
2 ∂q1 ∂q1 ∂q1
1 ∂d12 ∂d11 ∂d12 1
c121 = c211 = [ + − ] = {2m2 (lc1 + q2 ) + m3 [2(lc1 + q2 + lc2 ) + 2lc3 C3 ]} = m2 (lc1 + q2 ) +
2 ∂q1 ∂q2 ∂q1 2
m3 [(lc1 + q2 + lc2 ) + lc3 C3 ]
0 0
1 ∂d12 ∂d22
c221 = [2 − ]=0
2 ∂q2 ∂q1
1 ∂d13 ∂d11 ∂d13 1
c131 = c311 = [ + − ] = [−2m3 lc3 (lc1 + q2 + lc2 )S3 ] = −m3 lc3 (lc1 + q2 + lc2 )S3
2 ∂q1 ∂q3 ∂q1 2
0
1 ∂d13 ∂d12 ∂d23 1
c231 = c321 = [ + − ] = (
m3 lc3 C3 −

m3 lc3 C3 ) = 0

2 ∂q2 ∂q3 ∂q1 2
0
1 ∂d13 1 ∂d33
c331 = [2 − ] = [−2m3 lc3 (lc1 + q2 + lc2 )S3 ] = −m3 lc3 (lc1 + q2 + lc2 )S3
2 ∂q3 ∂q1 2
0
1 ∂d21 ∂d11 1
c112 = [2 − ] = {−2m2 (lc1 + q2 ) − 2m3 [(lc1 + q2 + lc2 ) + lc3 C3 ]} = −m2 (lc1 + q2 ) − m3 [(lc1 +
2 ∂q1 ∂q2 2
q2 + lc2 ) + lc3 C3 ]
0
1 ∂d22 ∂d21 ∂d21
c122 = c212 = [ + − ]=0
2 ∂q1 ∂q2 ∂q2
0
1 ∂d22 ∂d22 ∂d22
c222 = [ + − ]=0
2 ∂q2 ∂q2 ∂q2
0
1 ∂d23 ∂d21 ∂d13 1
c132 = c312 = [ + − ] = (−m3 lc3 C3 − m3 lc3 C3 ) = −m3 lc3 C3
2 ∂q1 ∂q3 ∂q2 2
0
1 ∂d23 ∂d22 ∂d23
c232 = c322 = [ + − ]=0
2 ∂q2 ∂q3 ∂q1
1 ∂d23 ∂d33 1
c332 = [2 − ] = (−2m3 lc3 C3 ) = −m3 lc3 C3
2 ∂q2 ∂q2 2
0
1 ∂d31 ∂d11 1
c113 = [2 − ] = [2m3 lc3 (lc1 + q2 + lc2 )S3 ] = m3 lc3 (lc1 + q2 + lc2 )S3
2 ∂q1 ∂q3 2
0
1 ∂d32 ∂d31 ∂d12 1
c123 = c213 = [ + − ] = (m3 lc3 C3 + m3 lc3 C3 ) = m3 lc3 C3
2 ∂q1 ∂q2 ∂q3 2
42
0 0
1 ∂d32 ∂d22
c223 = [2 − ]=0
2 ∂q2 ∂q3
0
1 ∂d33 ∂d31 ∂d13
c133 = c313 = [ + − ]=0
2 ∂q1 ∂q3 ∂q3
0
1 ∂d33 ∂d32 ∂d23
c233 = c323 = [ + − ]=0
2 ∂q2 ∂q3 ∂q3
0
1 ∂d33 ∂d33 ∂d3
c332 = [ + − ]=0
2 ∂q3 ∂q3 ∂q3

Con lo anterior calculamos los elementos de la matriz de coriolis.
2
X
Ckj = cijk q˙i
i=1
C11 = c111q˙10 + c211 q˙2 + c311 q˙3 = {m2 (lc + q2 ) + m3 [(lc1 + q2 + lc ) + lc C3 ]} q˙2 − m3 q˙3 lc (lc1 + q2 + lc )S3
:
1 2 3 3 2
C12 = c121 q˙1 + :q˙20 +
c221
:q˙30 = {m2 (lc + q2 ) + m3 [(lc1 + q2 + lc ) + lc C3 ]} q˙1
c321

1 2 3
C13 = c131 q˙1 + :q˙20 + c331 q˙3 = −m3 lc (lc1 + q2 + lc )S3 (q˙1 + q˙3 )
c231

3 2
C21 = c112 q˙1 + :q˙20 + c312 q˙3 = {m2 (lc + q2 ) + m3 [(lc1 + q2 + lc ) + lc C3 ]} q˙1 − m3 q˙3 lc C3
c212

1 2 3 3
C22 = :q˙10 +
c122
:q˙20 +
c222
:q˙30 = 0
c322

C23 = c132 q˙1 + :q˙20 + c332 q˙3 = −m3 lc C3 (q˙1 + q˙3 )
c232

3
C31 = c113 q˙1 + c213 q˙2 + :q˙30 = m3 q˙1 lc (lc1 + q2 + lc )S3 + m3 q˙2 lc C3
c313

3 2 3
C32 = c123 q˙1 + :q˙20 +
c223
:q˙30 = m3 q˙1 lc C3
c323

3
C33 = :q˙10 +
c133
:q˙20 +
c233
:q˙30 = 0
c333

 
C11 C12 C13
C(q, q̇) = C21 0 C23 
C31 C32 0
Para obtener el vector de gravedad primero calculamos la energı́a potencial.

P1 = m1 ḡlc1 (1 + S1 )
P2 = m2 ḡ(lc1 + q2 )(1 + S1 )
P3 = m3 ḡ[(lc1 + q2 + lc2 )(1 + S1 ) + lc3 (1 + S13 )]
P = P1 + P2 + P3 = m1 ḡlc1 (1 + S1 ) + m2 ḡ(lc1 + q2 )(1 + S1 ) + m3 ḡ[(lc1 + q2 + lc2 )(1 + S1 ) + lc3 (1 + S13 )]
A partir de la energı́a potencial calculamos el vector de gravedad.
 ∂P   
dq1 m1 ḡlc1 C1 + m2 ḡ(lc1 + q2 )C1 + m3 ḡ[(lc1 + q2 + lc2 )C1 + lc3 C13 ]
∂P 
g(q) =  dq
2
= m2 ḡ(1 + S1 ) + m3 ḡ(1 + S1 ) 
∂P m3 ḡlc3 C13
dq3
Ahora si tenemos todos los elementos para armar el modelo dinámico:
D(q)q̈ + C(q, q̇)q̇ + g(p) = τ
13 = 31 = m3 [lc23 + lc3 (lc1 + q2 + lc2 )C3 ] + I3z
C11 = {m2 (lc1 + q2 ) + m3 [(lc1 + q2 + lc2 ) + lc3 C3 ]} q˙2 − m3 q˙3 lc3 (lc1 + q2 + lc2 )S3
C12 = {m2 (lc1 + q2 ) + m3 [(lc1 + q2 + lc2 ) + lc3 C3 ]} q˙1
C13 = −m3 lc3 (lc1 + q2 + lc2 )S3 (q˙1 + q˙3 )
43
C21 = − {m2 (lc1 + q2 ) + m3 [(lc1 + q2 + lc2 ) + lc3 C3 ]} q˙1 − m3 q˙3 lc3 C3
C23 = −m3 lc3 C3 (q˙1 + q˙3 )
C31 = m3 q˙1 lc3 (lc1 + q2 + lc2 )S3 + m3 q˙2 lc3 C3
C32 = m3 q˙1 lc3 C3
      
11 −m3 lc3 S3 13 q¨1 C11 C12 C13 q˙1
−m3 lc3 S3 m2 + m3 −m3 lc3 S3  q¨2  + C21 0 C23  q˙2 
31 −m3 lc3 S3 m3 lc23 + I3z q¨3 C31 C32 0 q˙3
   
m1 ḡlc1 C1 + m2 ḡ(lc1 + q2 )C1 + m3 ḡ[(lc1 + q2 + lc2 )C1 + lc3 C13 ] τ1
+ m2 ḡ(1 + S1 ) + m3 ḡ(1 + S1 )  = τ2 
m3 ḡlc3 C13 τ3
8.2. Antisimetrı́a
2
X ∂dkj
d˙kj = q˙i
i=1
dqi
0
∂d11
> ∂d11 ∂d11
d˙11 = q˙1 + q˙2 + q˙3 = {2m2 (lc1 + q2 ) + m3 [2(lc1 + q2 + lc2 ) + 2lc3 C3 ]} q˙2 − 2m3 q˙3 lc3 (lc1 +
dq1 dq2 dq3
q2 + lc2 )S3
0 0
∂d12 > ∂d12 > ∂d12
d˙12 = q˙1 + q˙2 + q˙3 = −m3 q˙3 lc3 C3
dq1 dq2
dq3
0
∂d 13
> ∂d13
∂d13
d˙13 = q˙1 +
q˙2 + q˙3 = m3 q˙2 lc3 C3 − m3 q˙3 lc3 (lc1 + q2 + lc2 )S3
dq1 dq2 dq3
0 0
˙ ∂d21> ∂d21 >∂d21
d21 = q˙1 + q˙2 q˙3 = −m3 q˙3 lc3 C3
dq1 dq2
dq3
0 0 0
∂d22
> ∂d22 > ∂d22
d˙22 = q˙1 + q˙2 + =0
dq1 dq2
dq3
0 0
∂d23
> ∂d23 >∂d23
d˙23 = q˙1 + q˙2
q˙3 = −m3 q˙3 lc3 C3
dq1 dq2
dq3
0
∂d31
> ∂d31 ∂d31
d˙31 = q˙1 + q˙2 + q˙3 = m3 q˙2 lc3 C3 − m3 q˙3 lc3 (lc1 + q2 + lc2 )S3
dq1 dq2 dq3
0 0
∂d32
> ∂d32 >∂d32
d˙32 = q˙1 + q˙2
q˙3 = −m3 q˙3 lc3 C3
dq1 dq2
dq3
0 0 0
∂d33
> ∂d33 > ∂d33
d˙33 = q˙1 + q˙2 + =0
dq1 dq2 dq3
Finalmente calculamos los elementos de la matriz N:
nkj = d˙kj − 2Ckj
44
(
(((
( ((((
n11 = d˙11 −2C11 = [( c2 + lc3 C3 )]q˙2 −2m3( c1 + q2 + lc2 )S3 −[(
( (
2m
((2((lc(1 + q2 )+2m3(
(( (lc1
(( +(
q2(+(l( q˙3(
lc3(
(l( ((( 2m (lc(
(2( 1 + q2 )+
((
( ( ( ( ( ( ( ( (
((
c1 − q2 + lc2 )S3 = 0
2m3( (lc1 (+( q2(+(l(
c2 + lc3 C3 )]q˙2 + 2m3( q˙3(
lc3(
(l( ((((
(
n12 = d˙12 − 2C12 = −m3 q˙3 lc3 C3 − 2[m2 (lc1 + q2 ) + m3 (lc1 + q2 + lc2 + lc3 C3 )]q˙1
( ( ( (
n13 = d˙13 − 2C13 = m3 q˙2 lc3 C3 − m3 q˙3 lc3 (lc1 + q2 + lc2 )S3 + 2m3 lc3 (lc1 + q2 + lc2 )S3 (q˙1 + q˙3 ) = m3 q˙2 lc3 C3 +
m3 q˙3 lc3 (lc1 + q2 + lc2 )S3 + 2m3 q˙1 lc3 (lc1 + q2 + lc2 )S3
n21 = d˙21 − 2C21 = −m3 q˙3 lc3 C3 + 2[m2 (lc1 + q2 ) + m3 (lc1 + q2 + lc2 + lc3 C3 )]q˙1 + 2m3 q˙3 lc3 C3 = m3 q˙3 lc3 C3 +
2[m2 (lc1 + q2 ) + m3 (lc1 + q2 + lc2 + lc3 C3 )]q˙1
0
0
n22 = d˙22
− 2C22
*=

0
˙
n23 = d23 − 2C23 = −m3 q˙3 lc3 C3 + 2m3 lc3 C3 (q˙1 + q˙3 ) = 2m3 q˙1 lc3 C3 + m3 q˙3 lc3 C3
n31 = d˙31 − 2C31 = m3 q˙2 lc3 C3 − m3 q˙3 lc3 (lc1 + q2 + lc2 )S3 − 2m3 q˙1 lc3 (lc1 + q2 + lc2 )S3 − 2m3 q˙2 lc3 C3 =
−m3 q˙3 lc3 (lc1 + q2 + lc2 )S3 − 2m3 q˙1 lc3 (lc1 + q2 + lc2 )S3 − m3 q˙2 lc3 C3
n32 = d˙32 − 2C32 = −m3 q˙3 lc3 C3 − 2m3 q˙3 lc3 C3
0
0
n33 = d˙33

− 2
C33
*= 0

 
0 n12 n13
N (q, q̇) = n21 0 n23 
n31 n32 0
Ya que n12 = −n21 , n13 = −n31 y n23 = −n32 por lo tanto N es una matriz antisemétrica.
8.3. Representación Lineal

Considerando el Modelo dinámico:
13 = 31 = m3 [lc23 + lc3 (lc1 + q2 + lc2 )C3 ] + I3z
C11 = {m2 (lc1 + q2 ) + m3 [(lc1 + q2 + lc2 ) + lc3 C3 ]} q˙2 − m3 q˙3 lc3 (lc1 + q2 + lc2 )S3
C12 = {m2 (lc1 + q2 ) + m3 [(lc1 + q2 + lc2 ) + lc3 C3 ]} q˙1
C13 = −m3 lc3 (lc1 + q2 + lc2 )S3 (q˙1 + q˙3 )
C21 = − {m2 (lc1 + q2 ) + m3 [(lc1 + q2 + lc2 ) + lc3 C3 ]} q˙1 − m3 q˙3 lc3 C3
C23 = −m3 lc3 C3 (q˙1 + q˙3 )
C31 = m3 q˙1 lc3 (lc1 + q2 + lc2 )S3 + m3 q˙2 lc3 C3
C32 = m3 q˙1 lc3 C3
      
11 −m3 lc3 S3 13 q¨1 C11 C12 C13 q˙1
−m3 lc3 S3 m2 + m3 −m3 lc3 S3  q¨2  + C21 0 C23  q˙2 
31 −m3 lc3 S3 m3 lc23 + I3z q¨3 C31 C32 0 q˙3
   
m1 ḡlc1 C1 + m2 ḡ(lc1 + q2 )C1 + m3 ḡ[(lc1 + q2 + lc2 )C1 + lc3 C13 ] τ1
+ m2 ḡ(1 + S1 ) + m3 ḡ(1 + S1 )  = τ2 
m3 ḡlc3 C13 τ3
y tomando en cuenta:
(lc1 + q2 )2 = lc21 + 2lc1 q2 + q22

(lc1 + q2 + lc2 )2 = lc21 + lc22 + 2lc1 lc2 + q22 + 2lc1 q2 + 2lc2 q2
45
La matriz de inercia se puede expresar con los siguientes parámetros:
Θ1 = m1 lc21 + m2 lc1 2
+ m3 (lc21 + lc22 + 2lc1 lc2 ) + I1y + I2y
Θ2 = m3 lc23 + I3z
Θ3 = m3 lc3 (lc1 + lc2 )
Θ4 = m2 lc1
Θ5 = m2
Θ6 = m3 lc1
Θ7 = m3 lc2
Θ8 = m3
Θ9 = m3 lc3
Ası́, los elementos de la matriz de inercia y de la matriz de coriolis se pueden escribir como:
d11 = Θ1 + Θ2 + Θ3 + 2Θ4 q2 + Θ5 q22 + 2Θ6 q2 + 2Θ7 q2 + Θ8 q22

d12 = d21 = d23 = d32 = −Θ9 S3
d13 = d31 = Θ2 + Θ3 C3 + Θ9 q2 C3
d22 = Θ5 + Θ8
d33 = Θ2
Para la matriz coriolis se define este parámetro extra:
Θ10 = m3 (lc1 + lc2 )
C11 = Θ4 q˙2 + Θ5 q2 q˙2 + Θ10 q˙2 + Θ8 q2 q˙2 + Θ9 C3 q˙2 − Θ3 S3 q˙3 − Θ9 q2 S3 q˙3

C12 = Θ4 q˙1 + Θ5 q2 q˙1 + Θ10 q˙1 + Θ8 q2 q˙1 + Θ9 C3 q˙1
C13 = −Θ3 S3 q˙1 − Θ9 q2 S3 q˙1 − Θ3 S3 q˙3 − Θ9 q2 S3 q˙3
C21 = −Θ4 q˙1 − Θ5 q2 q˙1 − Θ10 q˙1 − Θ8 q2 q˙1 − Θ9 C3 q˙1 − Θ9 C3 q˙3
C23 = −Θ9 C3 q˙1 − Θ9 C3 q˙3
C31 = Θ3 S3 q˙1 + Θ9 q2 S3 q˙1 + Θ9 C3 q˙2
C32 = Θ9 C3 q˙1
Para el vector de gravedad se define este parámetro extra:
Θ11 = m1 lc1
El vector de gravedad queda:
g1 = Θ11 ḡC1 + Θ4 ḡC1 + Θ5 ḡq2 C1 + Θ10 ḡC1 + Θ8 ḡq2 C1 + Θ9 ḡC13

g2 = Θ5 ḡ(1 + S1 ) + Θ8 ḡ(1 + S1 )
g3 = Θ9 ḡC13
Para pasarlo a una función ”Y”se realizo lo siguiente:
τ1 = [Θ1 +Θ2 +Θ3 +2Θ4 q2 +Θ5 q22 +2Θ6 q2 +2Θ7 q2 +Θ8 q22 ]q¨1 −Θ9 S3 q¨2 +[Θ2 +Θ3 C3 +Θ9 q2 C3 ]q¨3 +[Θ4 q˙2 +
Θ5 q2 q˙2 + Θ10 q˙2 + Θ8 q2 q˙2 + Θ9 C3 q˙2 − Θ3 S3 q˙3 − Θ9 q2 S3 q˙3 ]q˙1 + [Θ4 q˙1 + Θ5 q2 q˙1 + Θ10 q˙1 + Θ8 q2 q˙1 + Θ9 C3 q˙1 ]q˙2 +
[−Θ3 S3 q˙1 − Θ9 q2 S3 q˙1 − Θ3 S3 q˙3 − Θ9 q2 S3 q˙3 ]q˙3 + Θ11 ḡC1 + Θ4 ḡC1 + Θ5 ḡq2 C1 + Θ10 ḡC1 + Θ8 ḡq2 C1 + Θ9 ḡC13 =
Θ1 q¨1 + Θ2 (q¨1 q¨3 ) + Θ3 (q¨1 + C3 q¨3 − S3 q˙3 q˙1 − q˙1 q˙3 − S3 q˙3 2 ) + Θ4 (2q2 q¨1 + q˙2 q˙1 + q˙1 q˙2 + ḡC1 ) + Θ5 (q22 q¨1 + q2 q˙2 q˙1 +
q2 q˙1 q˙2 + ḡq2 C1 ) + 2Θ6 q2 q¨1 + 2Θ7 q2 q¨1 + Θ8 (q22 q¨1 + q2 q˙2 q˙1 + q2 q˙1 q˙2 + ḡq2 C1 ) + Θ9 (−S3 q¨2 + q2 C3 q¨3 + C3 q˙2 q˙1 −
46
q2 S3 q˙3 q˙1 + C3 q˙1 q˙2 − q2 S3 q˙1 q˙3 − q2 S3 q˙3 2 + ḡC13 ) + Θ10 (q˙2 q˙1 + q˙1 q˙2 + ḡC1 ) + Θ11 ḡC1
τ2 = −Θ9 S3 q¨1 + [Θ5 + Θ8 ]q¨2 − Θ9 S3 q¨3 + [−Θ4 q˙1 − Θ5 q2 q˙1 − Θ10 q˙1 − Θ8 q2 q˙1 − Θ9 C3 q˙1 − Θ9 C3 q˙3 ]q˙1 +
[−Θ9 C3 q˙1 − Θ9 C3 q˙3 ]q˙3 + Θ5 ḡ(1 + S1 ) + Θ8 ḡ(1 + S1 ) = −Θ4 q˙1 q˙1 + Θ5 [q¨2 − q2 q˙1 2 + ḡ(1 + S1 )] + Θ8 [q¨2 − q2 q˙1 2 +
ḡ(1 + S1 )] + Θ9 (−S3 q¨1 − S3 q¨3 − C3 q˙1 2 − C3 q˙1 2 − C3 q˙1 q˙3 − C3 q˙1 2 − C3 q˙1 q˙3 − C3 q˙3 2 ) − Θ10 q˙1
τ3 = [Θ2 + Θ3 C3 + Θ9 q2 C3 ]q¨1 − Θ9 S3 q¨2 + Θ2 q¨3 + [Θ3 S3 q˙1 + Θ9 q2 S3 q˙1 + Θ9 C3 q˙2 ]q˙1 + Θ9 C3 q˙1 q˙2 + Θ9 ḡC13 =
Θ2 (q¨1 + q¨3 ) + Θ3 (C3 q¨1 + S3 q˙1 2 ) + Θ9 (q2 C3 q¨1 + S3 q¨2 + q2 S3 q˙1 2 + C3 q˙2 q˙1 + C3 q˙1 q˙2 + ḡC13 )
Sustituyendo los parámetro en las ecuaciones de movimiento se obtiene el regresor:
11 = q¨1
12 = q¨1 q¨3
13 = q¨1 + C3 q¨3 − S3 q˙3 q˙1 − q˙1 q˙3 − S3 q˙3 2
14 = 2q2 q¨1 + q˙2 q˙1 + q˙1 q˙2 + ḡC1
15 = q22 q¨1 + q2 q˙2 q˙1 + q2 q˙1 q˙2 + ḡq2 C1
16 = 2q2 q¨1
17 = 2q2 q¨1
18 = q22 q¨1 + q2 q˙2 q˙1 + q2 q˙1 q˙2 + ḡq2 C1
19 = −S3 q¨2 + q2 C3 q¨3 + C3 q˙2 q˙1 − q2 S3 q˙3 q˙1 + C3 q˙1 q˙2 − q2 S3 q˙1 q˙3 − q2 S3 q˙3 2 + ḡC13
110 = q˙2 q˙1 + q˙1 q˙2 + ḡC1
111 = ḡC1
24 = −q˙1 q˙1
25 = [q¨2 − q2 q˙1 2 + ḡ(1 + S1 )]
28 = [q¨2 − q2 q˙1 2 + ḡ(1 + S1 )]
29 = (−S3 q¨1 − S3 q¨3 − C3 q˙1 2 − C3 q˙1 2 − C3 q˙1 q˙3 − C3 q˙1 2 − C3 q˙1 q˙3 − C3 q˙3 2 )
210 = −q˙1
32 = (q¨1 + q¨3 )
33 = (C3 q¨1 + S3 q˙1 2 )
39 = (q2 C3 q¨1 + S3 q¨2 + q2 S3 q˙1 2 + C3 q˙2 q˙1 + C3 q˙1 q˙2 + ḡC13 )
 
11 12 13 14 15 16 17 18 19 110 111
Y (q, q̇, q̈) =  0 0 0 24 25 0 0 28 29 210 0 
0 32 33 0 0 0 0 0 39 0 0
y el vector de parámetros Θ es:
47
m1 lc21 + m2 lc1
2
+ m3 (lc21 + lc22 + 2lc1 lc2 ) + I1y + I2y
   
Θ1
 Θ2  
   m3 lc23 + I3z 

 Θ3   m 3 lc 3 (lc1 + lc2 ) 
   
 Θ4   m2 lc1 
   
 Θ5   m 2

   
 Θ6  = 
Θ= m3 lc1
  

 Θ7   m 3 lc 2

   
 Θ8   m 3

   
 Θ9   m3 lc3 
   
Θ10   m3 (lc1 + lc2 ) 
Θ11 m1 lc1
48
9. Ejercicio 9
1
Una partı́cula con energı́a cinética K = mẋ2 , el momentum se define como:
2
dK
p = mẋ =
dẋ
Entonces, para un sistema mecánico con coordenadas generalizadas q1 , q2 , ..., qn , se define el momentum
generalizado pk como:
∂L
pk =
∂ q˙k
1
donde L es el lagrangiano del sistema. Con K = q̇ T D(q)q̇ y L = K − P , muestre que:
2
n
X
q˙k pk = 2K
k=1
Considerando que la energı́a cinética puede estar dado por una función cuadrática del vector q̇ con la
forma siguiente:
n
X
1
K= 2
dij (q)q˙i q˙j
ij
Y el lagrangiano de la siguiente forma:

n
X
1
L=K −P = 2
dij (q)q˙i q˙j − P (q)
ij
Al derivar la expresión anterior tenemos:

n
∂L X
= dkj q˙j
q˙k j=1
Considerando que el resultado anterior es el momentum generalizado pk , se obtiene lo siguiente:

n
X n
X n
X n X
X n n
X
q˙k pk = q˙k dkj q˙j = dkj q˙k q˙j = dkj q˙k q˙j
k=1 k=1 j=1 k=1 j=1 kj
n
X
Considerando que 2K = dij (q)q˙i q˙j el resultado es valido para un cambio en k por i, de la siguiente
ij
forma:
n
X n
X
dij q˙i q˙j = dij (q)q˙i q˙j
ij ij
49
10. Ejercicio 10
Existe una formulación alterna para obtener las ecuaciones de movimiento llamada formulación por
Hamiltoniano: Se define la función Hamiltoniana H por:
n
X
H= q˙k pk − L
k=1
a) Muestre que H = K + P
Xn
Sabiendo que: q˙k pk = 2K y L = K − P
k=1
Sustituyendo lo anterior en la función Hamiltoniana:
n
X
H= q˙k pk − L = 2K − (K − P ) = 2K − K + P = K + P
k=1
b) Usando las ecuaciones de Lagrange, derive las ecuaciones de Hamilton:

n
X
∂ q˙k pk − L n
∂H k=1
X
q˙k = = = q˙k
∂pk ∂pk k=1
n
X
∂ q˙k pk − L n
∂H k=1
X
p˙k = − + τk = − + τk = − q˙k + τk
∂pk ∂pk k=1
donde τk es la entrada de fuerza generalizada.

c) Para el siguiente robot de doble revoluta, calcule las ecuaciones de Hamilton en forma matricial.
Figura 7: Robot RR
Para calcular el hamiltoniano usamos la formula H = K + P .
50
2 n
1 T X
T T T
X
Sabiendo que K = q̇ D(q)q̇, D(q) = mi Jvi Jvi + Jwi Ri Ii Ri Jwi y que P = g T rci mi , primero
2 i=1 i=1
realizamos el analisis Denavit-Hartenberg:
Por lo que a partir de los marcos coordenados propuestos en la figura 7, se obtuvieron los parámetros
de Denavit Hartenberg mostrados en la tabla 12.
1 l1 0 0 q1
2 l2 0 0 q2
Cuadro 8: Parámetros de Denavit Hartenberg para robot RR.
1 lc1 0 0 q1
2 lc2 0 0 q2
Cuadro 9: Parámetros de Denavit Hartenberg para robot RR respecto a los centros de masa.
Considerando:
C12 = Cos(q1 + q2 ) = Cos(q1 )Cos(q2 ) − Sen(q1 )Sen(q2 )
S12 = Sen(q1 + q2 ) = Sen(q1 )Cos(q2 ) + Cos(q1 )Sen(q2 )
C1 = Cos(q1 ), S1 = Sen(q1 ), C2 = Cos(q2 ) y S2 = Sen(q2 )
Se obtiene:
   
C1 −S1 0 l1 C1 C1 −S1 0 lc1 C1
 S1 C1 0 l1 S1   S1 C1 0 lc1 S1 
A1 =  ; A = 
0 1 0  c1  0

0 0 1 0 
0 0 0 1 0 0 0 1
   
C2 −S2 0 l2 C2 C2 −S2 0 lc2 C2
 S 2 C2 0 l2 S2 
  S2 C2 0 lc2 S2 
A2 = 
0 ; Ac2 =  
0 1 0   0 0 1 0 
0 0 0 1 0 0 0 1
    
C1 −S1 0 l1 C1 C2 −S2 0 lc2 C2 C12 −S12 0 l1 C1 + lc2 C12
 S 1 C1 0 l1 S1   S2 C2
  0 lc2 S2   S12 C12
  0 l1 S1 + lc2 S12 
A1 Ac2 = 
0 = 
0 1 0 0 0 1 0   0 0 1 0 
0 0 0 1 0 0 0 1 0 0 0 1
 
C12 −S12 0 l1 C1 + l2 C12
 S12 C12 0 l1 S1 + l2 S12 
T20 = A1 A2 = 
 0

0 1 0 
0 0 0 1
Con lo anterior podemos calcular calcular la matriz de inercia D(q), la cual se obtiene calculando los
Jacobianos en cada centro de masa:
51

Jv1 z0 × (Ocm1 − O0 ) 03×1
J1 = =
Jw1 z0 03×1

Jv2 z0 × (Ocm2 − O0 ) z1 × (Ocm2 − O1 )
J2 = =
Jw2 z0 z1
Por lo que obtenemos los orı́genes y z’s a partir de las matrices homogéneas.:
       
lc1 C1 l1 C1 + lc2 C12 l1 C1 0
Ocm1 =  lc1 S1 ; Ocm2 =  l1 S1 + lc2 S12 ; O1 =  l1 S1 ; z0 = z1 = 0
0 0 0 1
   
l1 C1 + lc2 C12 − l1 C1 lc2 C12
Ocm2 − O1 =  l1 S1 + lc2 S12 − l1 S1  =  lc2 S12 
0 0
     
0 lc1 C1 −lc1 S1
z0 × (Ocm1 − O0 ) = 0 ×  lc1 S1  =  lc1 C1 
1 0 0
     
0 l1 C1 + lc2 C12 −l1 S1 − lc2 S12
z0 × (Ocm2 − O0 ) = 0 ×  l1 S1 + lc2 S12  =  l1 C1 + lc2 C12 
1 0 0
     
0 lc2 C12 −lc2 S12
z0 × (Ocm2 − O1 ) = 0 ×  lc2 S12  =  lc2 S12 
1 0 0
   
−lc1 S1 0 −l1 S1 − lc2 S12 −lc2 S12
 lc1 C1 0  l1 C1 + lc2 C12 lc2 S12 

  
J  0 0 Jv2 0 0 
J1 = v1 = 
 
; J2 = =
 
Jw1  0
 0 Jw2  0 0 

 0 0  0 0 
1 0 1 1

2
X
i=1

  
−lc1 S1 lc1 C1 0 −lc1 S1 0 2
T lc 0
m1 Jv1 Jv1 = m1  0 0 0  lc1 C1 0 = m1 1
0 0
0 0 0 0 0
lc21 S12 + lc21 C12 = lc21

 
−l1 S1 − lc2 S12 −lc2 S12
−l1 S1 − lc2 S12 l1 C1 + lc2 C12 0 
m2 JvT2 Jv2 = m2 l1 C1 + lc2 C12 lc2 C12 
−lc2 S12 lc2 C12 0
0 0
52

l12 + lc22 + 2l1 lc2 C2 lc22 + l1 lc2 C2
= m2
lc22 + l1 lc2 C2 lc22
(−l1 S1 − lc2 S12 )2 + (l1 C1 + lc2 C12 )2 = l12 S12 + lc22 S12
2
+ 2l1 lc2 S1 S12 + l12 C12 + lc22 C12
2
+ 2l1 lc2 C1 C12 =
::::: :::::
l12 + lc22 + 2l1 lc2 C2

**(S1 S12 + C1 C12 ) = S1 (S1 C2 + C1 S2 ) + C1 (C1 C2 − S1 S2 ) = S12 C2 +
S1
C 2
1 S2 + C1 C2 −
C1S
1 S2 = C2

2 2 2 2 2
(−l1 S1 −lc2 S12 )(−lc2 S12 )+(l1 C1 +lc2 C12 )(lc2 C12 ) = lc2 S12 +l:::::::::
1 lc2 S1 S12 +lc2 C12 +l::::::::::
1 lc2 C1 C12 = lc2 +l1 lc2 C2
2
X m1 lc21 0 m2 (l12 + lc22 + 2l1 lc2 C2 ) m2 (lc22 + l1 lc2 C2 )
mi JvTi Jvi = +
0 0 m2 (lc22 + l1 lc2 C2 ) m2 lc22
i=1

m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) m2 (lc22 + l1 lc2 C2 )
=
m2 (lc22 + l1 lc2 C2 ) m2 lc22

   
C1 −S1 0 C12 −S12 0
R1 =  S1 C1 0; R2 =  S12 C12 0
0 0 1 0 0 1
    
C1 −S1 0 I1x 0 0 C1 S1 0 0 0
0 0 1 
JwT1 R1 I1 R1T Jw1 = S1 C1 0  0 I1y 0  −S1 C1 0 0 0
0 0 0
0 0 1 0 0 I1z 0 0 1 1 0
   
I1x 0 0 C1 S1 0 0 0
0 0 1 
JwT1 R1 I1 R1T Jw1 = 0 I1y 0  −S1 C1 0 0 0
0 0 0
0 0 I1z 0 0 1 1 0
  
C1 S1 0 0 0
0 0 I1z
JwT1 R1 I1 R1T Jw1 = −S1 C1 0 0 0
0 0 0
0 0 1 1 0
 
0 0
0 0 I1z  I1z 0
JwT1 R1 I1 R1T Jw1 = 0 0 =

0 0 0 0 0
1 0
    
C12 −S12 0 I2x 0 0 C12 S12 0 0 0
0 0 1 
JwT2 R2 I2 R2T Jw2 = S12 C12 0  0 I2y 0  −S12 C12 0 0 0
0 0 1
0 0 1 0 0 I2z 0 0 1 1 1
   
I2x 0 0 C12 S12 0 0 0
0 0 1 
JwT2 R2 I2 R2T Jw2 = 0 I2y 0  −S12 C12 0 0 0
0 0 1
0 0 I2z 0 0 1 1 1
  
C12 S12 0 0 0
0 0 I2z 
JwT2 R2 I2 R2T Jw2 = −S12 C12 0 0 0
0 0 I2z
0 0 1 1 1
53
 
0 0
0 0 I2z  I2z I2z
JwT2 R2 I2 R2T Jw2 = 0 0 =

0 0 I2z I2z I2z
1 1
2
X I1z 0 I2z I2z I1z + I2z I2z
JwTi Ri Ii RiT Jwi = + =
0 0 I2z I2z I2z I2z
i=1
2
X m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) m2 (lc22 + l1 lc2 C2 )
D(q) = mi JvTi Jvi + JwTi Ri Ii RiT Jwi =
m2 (lc22 + l1 lc2 C2 ) m2 lc22
i=1

I1z + I2z I2z m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) + I1z + I2z m2 (lc22 + l1 lc2 C2 ) + I2z
+ =
I2z I2z m2 (lc22 + l1 lc2 C2 ) + I2z m2 lc22 + I2z
1
K = q̇ T D(q)q̇
2

1 m1 lc2 + m2 (l12 + lc2 + 2l1 lc2 C2 ) + I1z + I2z m2 (lc2 + l1 lc2 C2 ) + I2z q˙1
= q˙1 q˙2 1 2 2
2 m2 (lc22 + l1 lc2 C2 ) + I2z m2 lc22 + I2z q˙2
11 = [m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) + I1z + I2z ]q˙1 + [m2 (lc22 + l1 lc2 C2 ) + I2z ]q˙2
12 = [m2 (lc22 + l1 lc2 C2 ) + I2z ]q˙1 + [m2 lc22 + I2z ]q˙2

1 T 1 q˙1
K = q̇ D(q)q̇ = 11 12
2 2 q˙2
1
K= m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) + I1z + I2z ]q˙1 + [m2 (lc22 + l1 lc2 C2 ) + I2z ]q˙2 q˙1 +
2
1
[m2 (lc22 + l1 lc2 C2 ) + I2z ]q˙1 + [m2 lc22 + I2z ]q˙2 q˙2
2
1 1 1
K = q̇ T D(q)q̇ = m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) + I1z + I2z ]q˙1 2 + [m2 (lc22 + l1 lc2 C2 ) + I2z ]q˙1 q˙2 +
2 2 2
1 1
[m2 (lc22 + l1 lc2 C2 ) + I2z ]q˙1 q˙2 + [m2 lc22 + I2z ]q˙2 2
2 2
1
K= [m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) + I1z + I2z ]q˙1 2 + [m2 lc22 + I2z ]q˙2 2 + [m2 (lc22 + l1 lc2 C2 ) + I2z ]q˙1 q˙2
2
Después calculamos la energı́a potencial.
P1 = m1 ḡlc1 (1 + S1 )
P2 = m2 ḡ(l1 + lc2 + l1 S1 + lc2 S12 )
P = P1 + P2 = m1 ḡlc1 (1 + S1 ) + m2 ḡ(l1 + lc2 + l1 S1 + lc2 S12 )
1
H =K +P = [m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) + I1z + I2z ]q˙1 2 + [m2 lc22 + I2z ]q˙2 2 + [m2 (lc22 + l1 lc2 C2 ) +
2
I2z ]q˙1 q˙2 + m1 ḡlc1 (1 + S1 ) + m2 ḡ(l1 + lc2 + l1 S1 + lc2 S12 )
1
L=K −P = [m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) + I1z + I2z ]q˙1 2 + [m2 lc22 + I2z ]q˙2 2 + [m2 (lc22 + l1 lc2 C2 ) +
2
I2z ]q˙1 q˙2 − m1 ḡlc1 (1 + S1 ) − m2 ḡ(l1 + lc2 + l1 S1 + lc2 S12 )
54
∂L
pk = = [m1 lc21 + m2 (l12 + lc22 + 2l1 lc2 C2 ) + I1z + I2z ]q˙1 + [m2 lc22 + I2z ]q˙2 + [m2 (lc22 + l1 lc2 C2 ) + I2z ](q˙1 + q˙2 )
∂ q˙k
Xn
q˙k = q˙k = q˙1 + q˙2
k=1
n
X
p˙k = − q˙k + τk = −q˙1 − q˙2 + τk
k=1
55
11. Ejercicio 11
Obtenga el modelo dinámico del robot Mitsubishi RV-M1, para los primeros 3 grados de libertad con
los siguientes datos
Figura 8: Robot Mitsubishi RV-M1
Considerando que la configuración del robot Mitsubishi RV-M1 es la misma que la del robot RRR
analizado en el ejercicio 5, se retomó el código utilizado agregando las propiedades mostradas en la figura
8. Modificando las variables utilizadas en la definición de los parámetros de Denavit-Hartenberg:
1 0 π/2 0 q1 +π
2 l1 0 0 q2
3 l2 0 0 q3
Cuadro 10: Parámetros de Denavit Hartenberg para robot Mitsubishi RV-M1 respecto a los marcos coor-
denados.
56
Eslabón ai α i di θi
1 0 π/2 0 q1 +π
2 l11 0 0 q2
3 l22 0 0 q3
Cuadro 11: Parámetros de Denavit Hartenberg para robot Mitsubishi RV-M1 respecto a los centros de
masa de cada eslabón.
Realizando los mismos cálculos utilizados en Matlab para la resolución del ejercicio 5 y sustituyendo
los valores numéricos de las propiedades del robot, se obtuvo el siguiente modelo:
 
D1,1 0 0
3
D(q) =  0 25
cos(q3 ) + 12193
40000
3
50
73 
cos(q3 ) + 2500
3 73 73
0 50
cos(q3 ) + 2500 2500
Donde:
3 2141 107 3 17153
D1,1 = cos(2q2 + q3 ) + cos(2q2 ) − cos(2q2 + 2q3 ) + cos(q3 ) +
50 16000 5000 50 80000
La matriz de Coriolis quedo definida como:
 
C1,1 C1,2 C1,3
3 3
C(q, q̇) = C2,1 − 50 q˙3 s3 − 50 s3 (q˙2 + q˙3 )
3
C3,1 50 q˙3 s3 0
Donde:
3 2141 107 3 107 3
C1,1 = −q˙2 [ sin(2q2 + q3 ) + sin(2q2 ) − sin(2q2 + 2q3 )] − q˙3 [ sin(2q2 + q3 ) − sin(2q2 + 2q3 ) + s3 ]
50 16000 5000 100 5000 100
3 2141 107
C1,2 = −q˙1 [ sin(2q2 + q3 ) + sin(2q2 ) − sin(2q2 + 2q3 )]
50 16000 5000
3 107 3
C1,3 = −q˙1 [ sin(2q2 + q3 ) − sin(2q2 + 2q3 ) + s3 ]
100 5000 100
3 2141 107
C2,1 = q˙1 [ sin(2q2 + q3 ) + sin(2q2 ) − sin(2q2 + 2q3 )]
50 16000 5000
3 107 3
sin(2q2 + q3 ) −
C3,1 = q˙1 [ sin(2q2 + 2q3 ) + s3 ]
100 5000 100
Finalmente, el vector de gravedad queda de la siguiente manera:
 
0
1
g(q) =  200 ḡ[48c2 + 275c2 ]
1
25
6ḡc23
57
12. Ejercicio 12
Para los robots de los problemas 3, 4, 7, 8, repita el cálculo del modelo por medio de la técnica de
Newton Euler.
12.1. Ejercicio 3
Considerando la Figura 9 se escriben los términos independientes de la configuración (orientación local):
Figura 9: Robot SCARA

     
lc1 lc2 0
r1,c1 = 0 ; r2,c2 = 0 ; r3,c3 = 0 
    
0 0 lc3
     
lc1 − a1 lc2 − a2 0
r2,c1 =  0 ; r3,c2 =  0 ; r4,c3 =  0 
0 0 lc3 − d3
     
a1 a2 0
r1,2 = 0 ; r2,3 = 0 ; r3,4 = 0 
    
0 0 d3
Se consideran las siguientes condiciones iniciales: ω0 = 0, α0 = 0, ac,0 = 0 y ae,0 = 0. Se calcula la

recursión hacia adelante.
Para i = 1:
i
ωi = Ri−1 ωi−1 + bi q˙i
0
ω1 = (R10 )T
>0 + (R10 )T z0 q˙1
ω

58
    
c1 s 1 0 0 0
ω1 = −s1 c1 0 0 q˙1 = 0 q˙1
    
0 0 1 1 1
αi = (Rii−1 )T αi−1 + bi q¨i + ωi × bi q˙i

0
α1 = (R10 )T
>0 + (R10 )T z0 q¨1 + ωi × (R10 )T z0 q˙1
α

      0  
0 0 0* 0
1 × 0 q˙1 = 0 q¨1

α1 = 0 q¨1 + 0 q˙
      
1 1 1 1

i
ac,i = Ri−1 ae,i−1 + αi × ri,ci + ωi × (ωi × ri,ci )
0
ac,1 = (R10 )T
ae,0 + α1 × r1,c1 + ω1 × (ω1 × r1,c1 )
−lc1 q˙1 2
               
0 lc1 0 0 lc1 0 −lc1
ac,1 = 0 q¨1 ×  0  + 0 q˙1 × 0 q˙1 ×  0  = lc1  q¨1 +  0  q˙1 2 =  lc1 q¨1 
1 0 1 1 0 0 0 0
i
ae,i = Ri−1 ae,i−1 + αi × ri,i+1 + ωi × (ωi × ri,i+1 )
0
ae,1 = (R10 )T
ae,0 + α1 × r1,2 + ω1 × (ω1 × r1,2 )
−a1 q˙1 2
               
0 a1 0 0 a1 0 −a1
ae,1 = 0 q¨1 ×  0  + 0 q˙1 × 0 q˙1 ×  0  = a1  q¨1 +  0  q˙1 2 =  a1 q¨1 
1 0 1 1 0 0 0 0
Se calcula la componente de gravedad del eslabón 1:

      
0 c1 s 1 0 0 0
g1 = −(R10 )T 0 = − −s1 c1 0 0 = − 0
ḡ 0 0 1 ḡ ḡ
Para i = 2:
ω2 = (R21 )T ω1 + (R20 )T z1 q˙2
         
c2 s 2 0 0 c1 c2 − s 1 s 2 s 1 c2 + c1 s 2 0 0 0 0
ω2 = s2 −c2 0  0 q˙1 + c1 s2 + s1 c2 s1 s2 − c1 c2 0  0 q˙2 =  0  q˙1 +  0  q˙2 =
0 0 −1 1 0   0 −1 1 −1 −1
0
 0  (q˙1 + q˙2 )
−1
α2 = (R21 )T α1 + (R20 )T z1 q¨2 + ω2 × (R20 )T z1 q˙2

     
0 0 0
α2 = 0 q¨1 + 0 q¨2 = 0  (q¨1 + q2)
     ¨
−1 −1 −1
ac,2 = (R21 )T ae,1 + α2 × r2,c2 + ω2 × (ω2 × r2,c2 )
59
−a1 q˙1 2
            
c2 s 2 0 0 lc2 0 0 lc2
ac,2 = s2 −c2 0
   a1 q¨1 + 0 (q¨1 + q¨2 )× 0 + 0 (q˙1 + q˙2 )×
         0 (q˙1 + q˙2 ) × 0  =
 
0 0 −1  0  −1 0 −1  −1 0
c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )
  
0 −lc2
s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) + −lc2  (q¨1 + q¨2 ) +  0  (q˙1 + q˙2 )2 = s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )
0 0 0 0
ae,2 = (R21 )T ae,2 + α2 × r2,3 + ω2 × (ω2 × r2,3 )
c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 )

     
0 −a2
ae,2 = s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) + −a2  (q¨1 + q¨2 ) +  0  (q˙1 + q˙2 )2 =
0 2
0 0
c2 (−a1 q˙1 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )
s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )
0

      
0 c1 c2 − s 1 s 2 s 1 c2 + c1 s 2 0 0 0
0 T  
g2 = −(R2 ) 0 = − c1 s2 + s1 c2 s1 s2 − c1 c2 0
   0 = 0
 
ḡ 0 0 −1 ḡ ḡ
Para i = 3:
ω3 = (R32 )T ω2 + (R30 )T z2 q˙3

         
1 0 0 0 c1 c2 − s 1 s 2 s 1 c2 + c1 s 2 0 0 0 0
ω3 = 0 1 0  0  (q˙1 + q˙2 ) + c1 s2 + s1 c2 s1 s2 − c1 c2 0  0 q˙3 =  0  (q˙1 + q˙2 ) +  0  q˙3 =
0 0 1 −1 0  0 −1 1 −1 −1
0
 0  (q˙1 + q˙2 + q˙3 )
−1
α3 = (R32 )T α2 + (R30 )T z2 q¨3 + ω3 × (R30 )T z2 q˙3

     
0 0 0
α3 =  0  (q¨1 + q¨2 ) +  0  q¨3 =  0  (q¨1 + q¨2 + q¨3 )
−1 −1 −1
ac,3 = (R32 )T ae,2 + α3 × r3,c3 + ω3 × (ω3 × r3,c )
c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )

        
1 0 0 0 0 0
2
ac,3 = 0 1 0 s2 (−a1 q˙1 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ) + 0 (q¨1 + q¨2 + q¨3 ) × 0 + 0  (q˙1 + q˙2 +
        
0 0 1 0 −1 lc3 −1
2
     
0 0 c2 (−a1 q˙1 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )
q˙3 ) ×  0  (q˙1 + q˙2 + q˙3 ) ×  0  = s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )
−1 lc3 0
Como no hay eslabón 4, no se requiere el cálculo de ae,3 . Por lo que, se procede al cálculo de la
componente de gravedad del eslabón
60
      
0 c1 c2 − s 1 s 2 s 1 c2 + c1 s 2 0 0 0
0 T  
g3 = −(R3 ) 0 = − c1 s2 + s1 c2 s1 s2 − c1 c2 0
   0 = 0
 
ḡ 0 0 −1 ḡ ḡ
Una vez terminada la recursión hacia delante, se realiza la recursión hacia atrás, es decir, el cálculo de
fuerzas y pares.
Considerando condiciones iniciales, f4 = 0 y τ4 = 0, se tiene:
Para i = 3
i
fi = Ri+1 fi+1 + mi ac,i − mi gi
0
f3 = R43 f4 + m3 ac,3 − m3 g3
c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )

   
0
2
f3 = m3 s2 (−a1 q˙1 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ) − m3 0
  
0 ḡ
c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )

 
= m3 s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )

−ḡ
i i
τi = Ri+1 τi+1 − fi × ri,ci + (Ri+1 fi+1 ) × ri+1,ci + Ii αi + ωi × Ii ωi
0 :0

τ3 = R43 τ 4 − f3 × r3,c3 + (R43 f4)×
r
4,c4 + I3 α3 + ω3 × I3 ω3
c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )

       
0 0 0
2
τ3 = −m3 s2 (−a1 q˙1 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ) × 0 + I3 0 (q¨1 + q¨2 + q¨3 ) + 0  (q˙1 + q˙2 + q˙3 ) ×
      
−ḡ   lc3 −1 −1
0
I3 0  (q˙1 + q˙2 + q˙3 )

−1
lc3 [m3 (s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ))]
 
τ3 = −lc3 [m3 (c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ))]
−I3z (q¨1 + q¨2 + q¨3 )
Para i = 2
f2 = R32 f3 + m2 ac,2 − m2 g2
c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )

     
1 0 0 0
2
f2 = 0 1 0 f3 + m2 s2 (−a1 q˙1 ) − c2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 ) − m2 0  =
    
0 0 1 0 −ḡ
2 2
 
m3 [c2 (−a1 q˙1 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [c2 (−a1 q˙1 ) + s2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )]
m3 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )]
−ḡ(m3 + m2 )
τ2 = R32 τ3 − f2 × r2,c2 + (R32 f3 ) × r3,c2 + I2 α2 + ω2 × I2 ω2
61
lc3 [m3 (s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ))]
 
τ2 = −lc3 [m3 (c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ))] −
 −I3z (q¨1 + q¨2 + q¨3 ) 
0
 (lc2 )[−ḡ(m3 + m2 )] +
2 2
−(lc2 )[m
3 [s2 (−a1 q˙1 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [s2 (−a1 q˙1 ) − c2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )]]
  
0 0
 (lc2 − a2 )[m3 (−ḡ)] + 0 
2
−(lc2 − a2 )[m3 [s2 (−a1 q˙1 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )]] −I2z (q¨1 + q¨2 )
Considerando los siguientes términos:
τ2y = −lc3 [m3 (c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ))] − (lc2 )[−ḡ(m3 + m2 )] + (lc2 − a2 )[m3 (−ḡ)]
τ2z = −I3z (q¨1 + q¨2 + q¨3 ) + (lc2 )[m3 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) −
lc2 (q¨1 + q¨2 )]] − (lc2 − a2 )[m3 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )]] − I2z (q¨1 + q¨2 )
Se tiene:
lc3 [m3 (s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ))]
 
τ2 =  τ2y 
τ2z
Para i = 1.
f1 = R21 f2 + m1 ac,1 − m1 g1
−lc1 q˙1 2 m1 (−lc1 q˙1 2 )

         
c2 s 2 0 0 c2 s 2 0
f1 = s2 −c2 0  f2 + m1  lc1 q¨1  + m1 0 = s2 −c2 0  f2 +  m1 (lc1 q¨1 ) 
0 0 −1 0 ḡ 0 0 −1 m1 ḡ
Considerando los siguientes términos, como se muestra:
f1x = c2 [m3 [c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )]] +
s2 [m3 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )]] + m1 (−lc1 q˙1 2 )
f1y = s2 [m3 [c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )]] −
c2 [m3 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )]] + m1 (lc1 q¨1 )
Se tiene:
 
f1x
f1 =  f1y 
ḡ(m3 + m2 + m1 )

            
c2 s 2 0 f1x lc1 c2 s 2 0 lc1 − a1 0
τ1 = s2 −c2 0  τ2 −  f1y  ×  0  + s2 −c2 0  f2  ×  0  + I1 0 q¨1
0 0 −1 ḡ(m3 + m2 + m1 ) 0 0 0 −1 0 1
Finalmente, y considerando τ1x , τ1y y τ1z como:

τ1x = c2 [lc3 [m3 (s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ))]] + s2 (τ2y )
62
τ1y = s2 [lc3 [m3 (s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 ))]] − c2 (τ2y ) − lc1 [ḡ(m3 + m2 + m1 )] + (lc1 − a1 )[ḡ(m3 + m2 )]
τ1z = −[τ2z ] + lc1 (f1y ) + (a1 − lc1 )[s2 [m3 [c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [c2 (−a1 q˙1 2 ) + s2 (a1 q¨1 ) −
lc2 (q¨1 + q¨2 )]] − c2 [m3 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − a2 (q¨1 + q¨2 )] + m2 [s2 (−a1 q˙1 2 ) − c2 (a1 q¨1 ) − lc2 (q¨1 + q¨2 )]]] + I1z q¨1
Se tiene:
 
τ1x
τ1 = τ1y 
τ1z
12.2. Ejercicio 4
Figura 10: Robot planar
Considerando la figura 10, se describirán aquellos términos de la configuración (orientación local), se

tomara como valor desde la junta al centro de masa de cada eslabón como lcn , siendo n el número del
eslabón, considerando esto se tiene:
     
lc1 lc2 lc3
r1,c1 = 0 ; r2,c2 = 0 ; r3,c3 = 0 
    
0 0 0
     
lc1 − a1 lc2 − a2 lc3 − a3
r2,c1 =  0 ; r3,c2 =  0 ; r4,c3 =  0 
0 0 0
     
a1 a2 a3
r1,2 =  0 ; r2,3 =  0 ; r3,4 =  0 
0 0 0
63
Tomando como condiciones iniciales, tales como: ω0 = 0, α0 = 0, ac,0 = 0 y ae,0 = 0. Se calcula la
recursión hacia adelante, para i = 1:
i
0
>0 + (R10 )T z0 q˙1
ω1 = (R10 )T
ω

    
c1 s 1 0 0 0
ω1 = −s1 c1 0 0 q˙1 = 0 q˙1
    
0 0 1 1 1

0
>0 + (R10 )T z0 q¨1 + ωi × (R10 )T z0 q˙1
α1 = (R10 )T
α

      0  
0 0 0* 0
1 × 0 q˙1 = 0 q¨1

α1 = 0 q¨1 + 0 q˙
      
1 1 1 1

i
0
ac,1 = (R10 )T
ae,0 + α1 × r1,c1 + ω1 × (ω1 × r1,c1 )
−lc1 q˙1 2
               
0 lc1 0 0 lc1 0 −lc1
ac,1 = 0 q¨1 ×  0  + 0 q˙1 × 0 q˙1 ×  0  = lc1  q¨1 +  0  q˙1 2 =  lc1 q¨1 
1 0 1 1 0 0 0 0
i
0
ae,1 = (R10 )T
ae,0 + α1 × r1,2 + ω1 × (ω1 × r1,2 )
−a1 q˙1 2
               
0 a1 0 0 a1 0 −a1
ae,1 = 0 q¨1 ×  0  + 0 q˙1 × 0 q˙1 ×  0  = a1  q¨1 +  0  q˙1 2 =  a1 q¨1 
1 0 1 1 0 0 0 0
Para la componente de gravedad del eslabón 1, se tiene:

      
0 c1 s 1 0 0 0
0 T  
g1 = −(R1 ) 0 = − −s1 c1 0
   0 = 0
 
ḡ 0 0 1 ḡ −ḡ
Para i = 2:
ω2 = (R21 )T ω1 + (R20 )T z1 q˙2

       
c2 s 2 0 0 c12 s12 0 0 0
ω2 = −s2 c2
 0 0 q˙1 + −s12 c12 0 0 q˙2 = 0 (q˙1 + q˙2 )
       
0 0 1 1 0 0 1 1 1
α2 = (R21 )T α1 + (R20 )T z1 q¨2 + ω2 × (R20 )T z1 q˙2
64
           
c2 s 2 0 0 c12 s12 0 0 0 0 :0 0

α2 = −s2 c2 0 0 q¨1 + −s12 c12 0 0 q¨2 + 0 (q
˙1 × 0 q˙2 = 0 (q¨1 + q¨2 )
+q˙2)

0 0 1 1 0 0 1 1 1 1 1
ac,2 = (R21 )T ae,1 + α2 × r2,c2 + ω2 × (ω2 × r2,c2 )
0 −a1 q˙1 2
            
c2 s 2 0 lc2 0 0 lc2
ac,2 = −s2 c2
 0  a1 q¨1  + 0 (q¨1 + q¨2 ) × 0 + 0 (q˙1 + q˙2 ) ×
       0 (q˙1 + q˙2 ) × 0 
 
0 0 1 0  1 0 1  1 0
−a1 q˙1 2 c2 + a1 q¨1 s2 − lc2 (q˙1 + q˙2 )2
ac,2 = a1 q˙1 2 s2 + a1 q¨1 c2 + lc2 (q¨1 + q¨2 ) + lc2 (q˙1 + q˙2 )2 
0
ae,2 = (R21 )T ae,1 + α2 × r2,3 + ω2 × (ω2 × r2,3 )
0 −a1 q˙1 2
            
c2 s 2 0 a2 0 0 a2
ae,2 = −s2 c2 0  a1 q¨1  + 0 (q¨1 + q¨2 ) ×  0  + 0 (q˙1 + q˙2 ) × 0 (q˙1 + q˙2 ) ×  0 
0 0 1 0  1 0 1  1 0
2 2
−a1 q˙1 c2 + a1 q¨1 s2 − a2 (q˙1 + q˙2 )
ac,2 = a1 q˙1 2 s2 + a1 q¨1 c2 + a2 (q¨1 + q¨2 ) + a2 (q˙1 + q˙2 )2 
0

      
0 c12 s12 0 0 0
0 T  
g2 = −(R2 ) 0 = − −s12 c12 0
   0 = 0
 
ḡ 0 0 1 ḡ −ḡ
Para i = 3:
ω3 = (R32 )T ω2 + (R30 )T z2 q˙3

       
c3 s 3 0 0 c123 s123 0 0 0
ω3 = −s3 c3
 0 0 (q˙1 + q˙2 ) + −s123 c123 0 0 q˙3 = 0 (q˙1 + q˙2 + q˙3 )
       
0 0 1 1 0 0 1 1 1
α3 = (R32 )T α2 + (R30 )T z2 q¨3 + ω3 × (R30 )T z2 q˙3

          
c3 s 3 0 0 c123 s123 0 0 0 c123 s123 0 0
α3 = −s3 c3 0 0 (q¨1 + q¨2 ) + −s123 c123 0 0 q¨3 + 0 (q˙1 + q˙2 + q˙3 ) × −s123 c123 0 0 q˙3
0 0 1 1 0 0 1 1 1 0 0 1 1
        0  
0 0 0 0
:
0
 
α3 = 0 (q¨1 + q¨2 ) + 0 q¨3 + 0 (q˙1 +q˙2+
      +q˙3 ) × 0 q˙3 = 0 (q¨1 + q¨2 + q¨3 )


1 1 1
1 1
ac,3 = (R32 )T ae,2 + α3 × r3,c3 + ω3 × (ω3 × r3,c3 )
−a1 q˙1 2 c2 + a1 q¨1 s2 − a2 (q˙1 + q˙2 )2

      
c3 s 3 0 0 lc3
ac,3 = −s3 c3 0 a1 q˙1 2 s2 + a1 q¨1 c2 + a2 (q¨1 + q¨2 ) + a2 (q˙1 + q˙2 )2  + 0 (q¨1 + q¨2 + q¨3 ) ×  0  +
0 0 1   0   1 0
0 0 lc3
0 (q˙1 + q˙2 + q˙3 ) × 0 (q˙1 + q˙2 + q˙3 ) ×  0 
1 1 0
65
−a1 q˙1 2 c2 c3 + a1 q¨1 s2 c3 − a2 c3 (q˙1 + q˙2 )2 + a1 q˙1 2 s2 s3 + a1 q¨1 c2 s3 + a2 s3 (q¨1 + q¨2 ) + a2 s3 (q˙1 + q˙2 )2
 
ac,3 =  a1 q˙1 2 c2 s3 − a1 q¨1 s2 s3 + a2 s3 (q˙1 + q˙2 )2 + a1 q˙1 2 s2 c3 + a1 q¨1 c2 c3 + a2 c3 (q¨1 + q¨2 ) + a2 c3 (q˙1 + q˙2 )2  +
  0 
0 −lc3
lc3  (q¨1 + q¨2 + q¨3 ) +  lc3  (q˙1 + q˙2 + q˙3 )2
0 0
11 = −a1 q˙1 2 c2 c3 + a1 q¨1 s2 c3 − a2 c3 (q˙1 + q˙2 )2 + a1 q˙1 2 s2 s3 + a1 q¨1 c2 s3 + a2 s3 (q¨1 + q¨2 ) + a2 s3 (q˙1 + q˙2 )2 −
lc3 (q˙1 + q˙2 + q˙3 )2
21 = a1 q˙1 2 c2 s3 − a1 q¨1 s2 s3 + a2 s3 (q˙1 + q˙2 )2 + a1 q˙1 2 s2 c3 + a1 q¨1 c2 c3 + a2 c3 (q¨1 + q¨2 ) + a2 c3 (q˙1 + q˙2 )2 + lc3 (q˙1 +
q˙2 + q˙3 )2 + lc3 (q¨1 + q¨2 + q¨3 )
 
11
ac,3 = 21 
0
Debido a que no hay eslabón 4, no se requiere el calculo de ae,3 .

      
0 c123 s123 0 0 0
0 T  
g3 = −(R3 ) 0 = − −s123 c123 0
   0 = 0
 
ḡ 0 0 1 ḡ −ḡ
Con esto se tiene completo la recursión hacia adelante.

Rercursión hacia atrás. Para i = 3.
i
0
f3 = R43 f4 + m3 ac,3 − m3 g3
lc3 (q˙1 + q˙2 + q˙3 )2
q˙2 + q˙3 )2 + lc3 (q¨1 + q¨2 + q¨3 )
 
11
f3 = m3 21  
−ḡ
i i
0 :0

τ3 = R43 (R43 f4
τ 4 − f3 × r3,c3 + )×r
4,c4 + I3 α3 + ω3 × I3 ω3
 
lc3
τ3 = −f3 ×  0  + I3 α3 + ω3 × I3 ω3
0
66
 
lc3
τ3 = −f3 × 0  + I3 α3 + ω3 × I3 ω3

0
q˙2 + q˙3 )2 + lc3 (q¨1 + q¨2 + q¨3 )
   
0 0
τ3 = m3 lc3 −ḡ  + I3 0 (q¨1 + q¨2 + q¨3 )
13 1
Para i = 2.
f2 = R32 f3 + m2 ac,2 − m2 g2
lc3 (q˙1 + q˙2 + q˙3 )2
q˙2 + q˙3 )2 + lc3 (q¨1 + q¨2 + q¨3 )
  
c3 −s3 0 11
R32 f3 = m3 s3 c3 0  12 
0 0 1 −ḡ

A
R32 f3 = m3  B 
−ḡ
−a1 q˙1 2 c2 + a1 q¨1 s2 − lc2 (q˙1 + q˙2 )2

 
m2 ac,2 = m2 a1 q˙1 2 s2 + a1 q¨1 c2 + lc2 (q¨1 + q¨2 ) + lc2 (q˙1 + q˙2 )2 
0


0
m2 g2 = m2  0 
−ḡ
A − a1 q˙1 2 c2 + a1 q¨1 s2 − lc2 (q˙1 + q˙2 )2

 
f2 = m2 B + a1 q˙1 2 s2 + a1 q¨1 c2 + lc2 (q¨1 + q¨2 ) + lc2 (q˙1 + q˙2 )2 

0
Donde A y B, son:
A = −a1 q˙1 2 c2 + a1 q¨1 s2 − a2 (q˙1 + q˙2 )2 − lc3 c3 (q˙1 + q˙2 + q˙3 )2 − lc3 s3 (q˙1 + q˙2 + q˙3 )2 − lc3 s3 (q¨1 + q¨2 + q¨3 )
B = −a1 q˙1 2 c2 c3 s3 + a1 q˙1 2 c2 s3 c3 + a1 q¨1 s2 c3 s3 − a2 c3 s3 (q˙1 + q˙2 )2 + a2 s3 c3 (q˙1 + q˙2 )2 + a1 q˙1 2 s2 + a1 q¨1 c2 +
a2 (q¨1 + q¨2 ) + a2 (q˙1 + q˙2 )2 − lc3 s3 (q˙1 + q˙2 + q˙3 )2 + lc3 c3 (q˙1 + q˙2 + q˙3 )2 + lc3 c3 (q¨1 + q¨2 + q¨3 )

 
ḡs3
R32 τ3 = m3 lc3 −ḡc3 
C
67
C = a1 q˙1 2 c2 s3 − a1 q¨1 s2 s3 + a2 s3 (q˙1 + q˙2 )2 + a1 q˙1 2 s2 c3 + a1 q¨1 c2 c3 + a2 c3 (q¨1 + q¨2 ) + a2 c3 (q˙1 + q˙2 )2 + lc3 (q˙1 +
q˙2 + q˙3 )2 + lc3 (q¨1 + q¨2 + q¨3 ) + I3 (q¨1 + q¨2 + q¨3 )
 
0
f2 × r2,c2 = −lc2 m2  0 
2 2
B + a1 q˙1 s2 + a1 q¨1 c2 + lc2 (q¨1 + q¨2 ) + lc2 (q˙1 + q˙2 )
 
0
(R32 f3 ) × r3,c2 = m3 (lc2 − a2 )  ḡs3 
Ds3 + Ec3
Donde D y E, son:
D = −a1 q˙1 2 c2 c3 +a1 q¨1 s2 c3 −a2 c3 (q˙1 +q˙2 )2 +a1 q˙1 2 s2 s3 +a1 q¨1 c2 s3 +a2 s3 (q¨1 +q¨2 )+a2 s3 (q˙1 +q˙2 )2 −lc3 (q˙1 +q˙2 +q˙3 )2
E = a1 q˙1 2 c2 s3 − a1 q¨1 s2 s3 + a2 s3 (q˙1 + q˙2 )2 + a1 q˙1 2 s2 c3 + a1 q¨1 c2 c3 + a2 c3 (q¨1 + q¨2 ) + a2 c3 (q˙1 + q˙2 )2 + lc3 (q˙1 +
q˙2 + q˙3 )2 + lc3 (q¨1 + q¨2 + q¨3 )
 
0
I2 α2 + ω2 × I2 ω2 = 0  (q¨1 + q¨2 )

I2z
     
ḡs3 0 0
τ2 = −ḡc3  + lc2 m2  0  +  0  (q¨1 + q¨2 )
C B + a1 q˙1 2 s2 + a1 q¨1 c2 + lc2 (q¨1 + q¨2 ) + lc2 (q˙1 + q˙2 )2 I2z
 
m3 lc3 ḡs3
τ2 =  −m3 lc3 ḡc3 
′
Cm3 lc3 + lc2 m2 B + I2z (q¨1 + q¨2 )
Siendo:
B ′ = B + a1 q˙1 2 s2 + a1 q¨1 c2 + lc2 (q¨1 + q¨2 ) + lc2 (q˙1 + q˙2 )2
Para i = 1.
f1 = R21 f2 + m1 ac,1 − m1 g1
−lc1 q˙1 2 f2x c2 − f2y s3 − lc1 m1 q˙1 2

       
f2x c2 − f2y s3 0
f1 = f2x s2 + f2y c3  + m1  lc1 q¨1  − m1  0  =  f2x s2 + f2y c3 + lc1 m1 q¨1 
0 0 −ḡ m1 ḡ

 
ḡs3 c2 + ḡs3 s2
R21 τ2 =  ḡs3 s2 − ḡs3 c2 
′
C + lc2 m2 B + I2z (q¨1 + q¨2 )
 
0
(R21 f2 ) × r2,c1 = (lc1 − a1 )  0 
f2x s2 + f2y c3 + lc1 m1 q¨1
68

0
I1 α1 + ω1 × I1 ω1 =  0  q¨1
I1z
     
m3 lc3 ḡs3 c2 + m3 lc3 ḡs3 s2 0 0
τ1 =  m3 lc3 ḡs3 s2 − m3 lc3 ḡs3 c2  − (lc1 − a1 )  0  + 0  q¨1

Cm3 lc3 + lc2 m2 B ′ + I2z (q¨1 + q¨2 ) f2x s2 + f2y c3 + lc1 m1 q¨1 I1z
 
ḡs3 c2 + ḡs3 s2
τ1 =  ḡs3 s2 − ḡs3 c2 
C + lc2 m2 B ′ + I2z (q¨1 + q¨2 ) − (lc1 − a1 )F + I1z q¨1
Siendo:
F = f2x s2 + f2y c3 + lc1 m1 q¨1
Finalmente las ecuaciones dinámicas obtenidas por Newton Euler están dadas por las siguientes ecua-
ciones de τ1 , τ2 y τ3 .
τ1 = m3 lc3 ḡs3 c2 + m3 lc3 ḡs3 s2 + m3 lc3 ḡs3 s2 − m3 lc3 ḡs3 c2 + Cm3 lc3 + lc2 m2 B ′ + I2z (q¨1 + q¨2 ) − (lc1 − a1 )F + I1z q¨1
τ2 = m3 lc3 ḡs3 − m3 lc3 ḡc3 + Cm3 lc3 + lc2 m2 B ′ + I2z (q¨1 + q¨2 )
τ3 = −m3 lc3 ḡ + m3 lc3 G + I3z (q¨1 + q¨2 + q¨3 )
Siendo G:
G = a1 q˙1 2 c2 s3 − a1 q¨1 s2 s3 + a2 s3 (q˙1 + q˙2 )2 + a1 q˙1 2 s2 c3 + a1 q¨1 c2 c3 + a2 c3 (q¨1 + q¨2 ) + a2 c3 (q˙1 + q˙2 )2 + lc3 (q˙1 +
q˙2 + q˙3 )2 + lc3 (q¨1 + q¨2 + q¨3 )
12.3. Ejercicio 7
A partir de la figura 11 se escribirán aquellos términos independientes de la configuración (orientación
local):
           
0 0 −h 0 h 0
r1,c1 =  0  r2,c2 =  0  r2,c1 =  0  r3,c2 =  0  r1,2 =  0  r2,3 =  0 
1
lc1 lc1 + q2 lc1 −l2 /2 0 lc1 + q2 + 2 l2
Considerando que los momentos de inercia son diagonales:
 
I1x 0 0
I1 =  0 I1y 0 
0 0 I1z
 
I2x 0 0
I2 =  0 I2y 0 
0 0 I2z
Y que los parámetros de Denavit-Hartenberg son los siguientes:
1
1 h 2
π 0 q1 + 21 π
2 0 0 lc1 + q2 + 21 l2 0
Cuadro 12: Parámetros de Denavit Hartenberg para robot RP.
Considerando condiciones iniciales:
69
Figura 11: Robot RP
 
0
ω0 = α0 = ac,0 = ae0 = 0

0
Y el vector unitario z0 :
 
0
z0 = 0

1
Ahora, calcularemos la recursión hacia adelante. Para i = 1:
 
0
1 T 0 1 T 1 T
ω1 = (R0 ) ω*0 + (R0 ) z0 q˙1 = (R0 ) z0 q˙1 = 1 q˙1
 
0
 
0
1 T 0 1 T 1 T
α1 = (R0 ) α* 0 + (R0 ) z0 q¨1 + ω1 × ((R0 ) z0 q˙1 ) = 1 q¨1
 
0
 2
q˙1
1 T
ae,1 = (R0 ) ae,0 + α1 × r1,2 + ω1 × (ω1 × r1,2 ) = −h 0  
q¨1
 
q¨1
ac,1 = (R01 )T ae,0 + α1 × r1,c1 + ω1 × (ω1 × r1,c1 ) = lc1  0 
−q˙1 2
Calculando la componente de gravedad en el eslabón 1, es decir g1 :
70
   
0 −sq1 + π2
g(1) = −(R01 )T ḡ  = ḡ  0 
0 cq1 + π2
Continuando para i = 2:  
0
2 T 2 T
ω2 = (R1 ) ω1 + (R0 ) z1 q˙2 = q˙1 

q˙2
 
q˙1 q˙2
α2 = (R12 )T α1 + (R02 )T z1 q¨2 + ω2 × ((R02 )T z1 q˙2 ) =  q¨1 
q¨2
−hq˙1 2 + q¨1 (lc1 + q2 )
 
ac,2 = (R12 )T ae,1 + α2 × r2,c2 + ω2 × (ω2 × r2,c2 ) =  0 

2
−hq¨1 − q˙1 (lc1 + q2 )
Calculando la componente de gravedad en el eslabón 2, es decir g2 :
   
0 c1
g(2) = −(R02 )T ḡ  = −ḡ  0 
0 s1
Con esto se completa la recursión hacia adelante. Procedemos con la recursión hacia atras. Considerando
las condiciones finales:
 
0
f3 = τ3 = 0
0
Para i = 2:
ḡc1 − hq˙1 2 + q¨1 (lc1 + q2 )
 
0
f2 = R233 + m2 ac,2 − m2 g2 = m2 ac,2 − m2 g2 = m2
f
7  0 
2
ḡs1 − hq¨1 + q˙1 (lc1 + q2 )
0 0
τ2 = R23
τ
> 3
3 − f2 × r2,c2 + (R2 3 ) × r3,c2 + I2 α2 + ω2 × (I2 ω2 )
f7
 
q˙1 q˙2 (I2x − I2y + I2z )
τ2 = I2y q¨1 + m2 (ḡc1 − hq˙1 2 + q¨1 (lc1 + q2 ))(lc1 + q2 )
I2z q¨2
Ahora, para i = 1:
f1 = R12 f2 + m1 ac,1 − m1 g1
m1 lc1 q¨1 + ḡm1 c1 + m2 (ḡc1 − hq˙1 2 + q¨1 (lc1 + q2 ))

 
f1 =  0 
2 2
−m1 lc1 q˙1 + ḡm1 s1 + m2 (ḡs1 − hq¨1 − q˙1 (lc1 + q2 ))
τ1 = R12 τ2 − f1 × r1,c1 + (R12 f2 ) × r2,c1 + I1 α1 + ω1 × (I1 ω1 )

 
q˙1 q˙2 (I2x − I2y + I2z )
τ1 = q¨1 [I1y + I2y + h2 m2 + lc1
2
(m1 + m2 ) + m2 q22 + 2lc1 m2 q2 ] + ḡ[lc1 c1 (m1 + m2 ) + m2 (q2 c1 − hs1 )]
I2z q¨2
71
12.4. Ejercicio 8
Considerando la Figura 12 se escriben los términos independientes de la configuración (orientación
local):
Figura 12: Robot RPR
     
0 0 lc3
r1,c1 = 0 ; r2,c2 = q2 ; r3,c3 = 0 
    
lc1 0 0
     
0 0 l3 − lc3
r2,c1 =  0 ; r3,c2 = −lc2 ; r4,c3 =  0 
−q2 0 0
     
0 0 2lc3
r1,2 =  0 ; r2,3 = q2 + lc2 ; r3,4 =  0 
lc1 + q2 0 0
Se consideran las siguientes condiciones iniciales: ω0 = 0, α0 = 0, ac,0 = 0 y ae,0 = 0. Se calcula la

recursión hacia adelante.
Para i = 1:
i
0
ω1 = (R10 )T
>0 + (R10 )T z0 q˙1
ω

    
c1+π/2 s1+π/2 0 0 0
ω1 =  0 0 1 0 q˙1 = 1 q˙1
   
s1+π/2 −c1+π/2 0 1 0

0
α1 = (R10 )T
>0 + (R10 )T z0 q¨1 + ωi × (R10 )T z0 q˙1
α

72
      0  
0 0 0* 0
×

α1 = 1 q¨1 + 1 q˙
1 1 q˙1 = 1 q¨1
0 0 0 0

i
0
ac,1 = (R10 )T
ae,0 + α1 × r1,c1 + ω1 × (ω1 × r1,c1 )
               
0 0 0 0 0 lc1 0 lc1 q¨1
ac,1 = 1 q¨1 ×  0  + 1 q˙1 × 1 q˙1 ×  0  =  0  q¨1 +  0  q˙1 2 =  0 
0 lc1 0 0 lc1 0 −lc1 −lc1 q˙1 2
i
0
ae,1 = (R10 )T
ae,0 + α1 × r1,2 + ω1 × (ω1 × r1,2 )
             
0 0 0 0 0 lc1 + q2 0
ae,1 = 1 q¨1 ×  0  + 1 q˙1 × 1 q˙1 ×  0  =  0  q¨1 +  0  q˙1 2 =
0 lc1 + q2 0 0 lc1 +q2 0 −lc1 − q2
lc1 + q2 q¨1
 0 
2
−(lc1 + q2 )q˙1

      
0 c1+π/2 s1+π/2 0 0 −ḡs1+π/2
g1 = −(R10 )T ḡ  = −  0 0 1 ḡ  =  0 
0 s1+π/2 −c1+π/2 0 0 ḡc1+π/2
Para i = 2:
ω2 = (R21 )T ω1 + (R20 )T z1 q˙2

           
1 0 0 0 c1+π/2 s1+π/2 0 0 0 0 0
ω2 = 0 0 −1 1 q˙1 + −s1+π/2 c1+π/2 0 0 q˙2 = 0 q˙1 + 0 q˙2 = 0 (q˙1 + q˙2 )
0 1 0 0 0 0 1 1 1 1 1
α2 = (R21 )T α1 + (R20 )T z1 q¨2 + ω2 × (R20 )T z1 q˙2

     
0 0 0
α2 = 0 q¨1 + 0 q¨2 = 0 (q¨1 + q2)
     ¨
1 1 1
ac,2 = (R21 )T ae,1 + α2 × r2,c2 + ω2 × (ω2 × r2,c2 )

           
(lc1 + q2 )q¨1 0 0 0 0 0
ac,2 = (lc1 + q2 )q˙1 2  + 0 (q¨1 + q¨2 ) × q2  + 0 (q˙1 + q˙2 ) × 0 (q˙1 + q˙2 ) × q2  =
 0  1 0  1  1 0 
(lc1 + q2 )q¨1 −q2 0 (lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 )
(lc1 + q2 )q˙1 2  +  0  (q¨1 + q¨2 ) + q˙1 2 −q2  (q˙1 + q˙2 )2 = (lc1 + q2 )(q˙1 )2 − q2 (q˙1 + q˙2 )2 
0 0 0 0
73
ae,2 = (R21 )T ae,2 + α2 × r2,3 + ω2 × (ω2 × r2,3 )
           
lc1 q¨1 0 0 0 0 0
ae,2 = lc1 (q˙1 2  + 0 (q¨1 + q¨2 ) × q2 + lc2  + 0 (q˙1 + q˙2 ) × 0 (q˙1 + q˙2 ) × q2 + lc2  =
 0  1  0 1  1 0 
lc1 q¨1 −(q2 + lc2 ) 0 (lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )
lc1 (q˙1 )2  +  0  (q¨1 + q¨2 ) + −(q2 + lc2  (q˙1 + q˙2 )2 = (lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 
0 0 0 0

      
0 1 0 0 0 0
0 T  
g2 = −(R2 ) ḡ = − 0 0 −1 ḡ = 0 
    
0 0 1 0 0 −ḡ
Para i = 3:
ω3 = (R32 )T ω2 + (R30 )T z2 q˙3

           
c3 s 3 0 0 −s13 c13 0 0 0 0 0
ω3 = −c3 s3
 0 0 (q˙1 + q˙2 ) + −c13 −s13 0 0 q˙3 = 0 (q˙1 + q˙2 ) + 0 q˙3 = 0 (q˙1 + q˙2 + q˙3 )
           
0 0 1 1 0 0 1 1 1 1 1
α3 = (R32 )T α2 + (R30 )T z2 q¨3 + ω3 × (R30 )T z2 q˙3

     
0 0 0
α3 = 0 (q¨1 + q¨2 ) + 0 q¨3 = 0 (q¨1 + q¨2 + q¨3 )
    
1 1 1
ac,3 = (R32 )T ae,2 + α3 × r3,c3 + ω3 × (ω3 × r3,c )

        
c3 s 3 0 (lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 ) 0 lc3 0
2 2
ac,3 = −c3 s3 0 (lc1 (q˙1 ) − (q2 + lc2 )(q˙1 + q˙2 ) + 0 (q¨1 + q¨2 + q¨3 ) × 0 + 0 (q˙1 + q˙2 + q˙3 ) ×
       
0 0 1 0 
 1   0 1
0 lc3
0 (q˙1 + q˙2 + q˙3 ) ×  0  =
 1 0     
c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 ) − (q2 + lc2 )(q˙1 + q˙2 )2 )
2
0 −lc3
−c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) + lc3  (q¨1 + q¨2 + q¨3 )+  0  (q˙1 +
 0 0 0 
c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 ) − (q2 + lc2 )(q˙1 + q˙2 ) ) − lc3 (q˙1 + q˙2 + q˙3 )2
2 2
q˙2 + q˙3 )2 = −c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) + lc3 (q¨1 + q¨2 + q¨3 )
0
Como no hay eslabón 4, no se requiere el cálculo de ae,3 . Por lo que, se procede al cálculo de la
componente de gravedad del eslabón
      
0 −s13 c13 0 0 −ḡc13
g3 = −(R30 )T ḡ  = − −c13 −s13 0 ḡ  =  ḡs13 
0 0 0 1 0 0
74
Una vez terminada la recursión hacia delante, se realiza la recursión hacia atrás, es decir, el cálculo de
fuerzas y pares).
Para i = 3
i
0
3
f3 = R4 f 4
+ m3 ac,3 − m3 g3
c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) − lc3 (q˙1 + q˙2 + q˙3 )2
   
−ḡc13
2 2
f3 = m3 −c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 ) − (q2 + lc2 )(q˙1 + q˙2 ) ) + lc3 (q¨1 + q¨2 + q¨3 ) − m3  ḡs13  =
0 0
c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) − lc3 (q˙1 + q˙2 + q˙3 )2 + ḡc13
 
−c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) + lc3 (q¨1 + q¨2 + q¨3 ) − ḡs13 
0
i i
0 :0

τ3 = R43 (R43 f4
τ 4 − f3 × r3,c3 + )×r
4,c4 + I3 α3 + ω3 × I3 ω3
 
c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) − lc3 (q˙1 + q˙2 + q˙3 )2 + ḡc13
τ3 = − −c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) + lc3 (q¨1 + q¨2 + q¨3 ) − ḡs13  ×
      0  
lc3 0 0 0
 0  + I3 0 (q¨1 + q¨2 + q¨3 ) + 0 (q˙1 + q˙2 + q˙3 ) × I3 0 (q˙1 + q˙2 + q˙3 )
0 1 1 1
Considerando τ3k como:

τ3k = −lc3 [−c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) + lc3 (q¨1 + q¨2 + q¨3 ) − ḡs13 ] +
I3z (q˙1 + q˙2 + q˙3 )
Se tiene:
 
0
τ3 =  0 
τ3k
Para i = 2
f2 = R32 f3 + m2 ac,2 − m2 g2
Considerando:
f3x = c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) − lc3 (q˙1 + q˙2 + q˙3 )2 + ḡc13
f3y = −c3 ((lc1 q¨1 ) − (q2 + lc2 )(q¨1 + q¨2 )) + s3 ((lc1 (q˙1 )2 − (q2 + lc2 )(q˙1 + q˙2 )2 ) + lc3 (q¨1 + q¨2 + q¨3 ) − ḡs13
     
c3 −s3 0 (lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 ) 0
2 2
f2 = s3 c3 0 f3 + m2 (lc1 + q2 )(q˙1 ) − q2 (q˙1 + q˙2 ) − m2 0  =
   
0 0 1 0  −ḡ
c3 f3x + s3 f3y + m2 [(lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 )]
s3 f3x + c3 f3y + m2 [(lc1 + q2 )(q˙1 )2 − q2 (q˙1 + q˙2 )2 ]
m2 ḡ
75
   τ2 = R32 τ3 − f2 × r2,c2 + (R32 f3 ) × r3,c2+ I2α2 + ω2 × I2 ω2 =   
0 −q2 [m2 ḡ] 0 0
 0 − 0 + 0 + 0 
τ3k q2 [c3 f3x + s3 f3y + m2 [(lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 )]] −lc2 [c3 f3x + s3 f3y ] I2z (q¨1 + q¨2 )
 
q2 [m2 ḡ]
τ2 =  0 
τ3k − q2 [c3 f3x + s3 f3y + m2 [(lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 )]] − lc2 [c3 f3x + s3 f3y ] + I2z (q¨1 + q¨2 )
Para i = 1.
f1 = R21 f2 + m1 ac,1 − m1 g1
     
c3 f3x + s3 f3y + m2 [(lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 )] lc1 q¨1 −ḡs1+π/2
f1 =  m2 ḡ  + m1  0  − m1  0 =
2 2 2
−[s
 3 f3x + c3 f3y + m2 [(lc1 + q2 )(q˙1 ) − q2 (q˙1 + q˙2 ) ]] −lc1 q˙1 ḡc1+π/2

c3 f3x + s3 f3y + m2 [(lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 )] + m1 [lc1 q¨1 ) + ḡs1+π/2 ]
 m2 ḡ 
2 2 2
−[s3 f3x + c3 f3y + m2 [(lc1 + q2 )(q˙1 ) − q2 (q˙1 + q˙2 ) ]] + m1 [−lc1 q˙1 − (ḡc1+π/2 )]

   
1 0 0 lc1 (m2 ḡ)
τ1 = 0 0 1 τ2 − −lc1 [c3 f3x + s3 f3y + m2 [(lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 )] + m1 [lc1 q¨1 ) + ḡs1+π/2 ] +
0 −1 0  0   
−q2 (m2 ḡ) 0
q2 [c3 f3x + s3 f3y + m2 [(lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 )]] + I1y  q¨1
0 0
Finalmente, y considerando τ1y como:
τ1y = τ3k − q2 [c3 f3x + s3 f3y + m2 [(lc1 + q2 )q¨1 − q2 (q¨1 + q¨2 )]] − lc2 [c3 f3x + s3 f3y ] + I2z (q¨1 + q¨2 ) − [−lc1 [c3 f3x +
s3 f3y +m2 [(lc1 +q2 )q¨1 −q2 (q¨1 + q¨2 )]+m1 [lc1 q¨1 )+ ḡs1+π/2 ]]+q2 [c3 f3x +s3 f3y +m2 [(lc1 +q2 )q¨1 −q2 (q¨1 + q¨2 )]]+I1y
Se tiene:
 
q2 [m2 ḡ] − lc1 (m2 ḡ) − q2 (m2 ḡ)
τ1 =  τ1y 
0
76
13. Ejercicio 13
Desarrolle el modelo dinámico de un robot RP (azimutal -elevación) y valide el modelo a través de una
simulación en simscape.
Modelo en Solid Work:
Figura 13: Ensamble en Solid Work
Modelo capturado en Simulink:
Figura 14: Esquema en Simulink
77
Simulaciones:
Figura 15: Gráfica de q1
Figura 16: Gráfica de q2
78
Simulaciones:
Figura 17: Gráfica de q˙1
Figura 18: Gráfica de q˙2
79
Simulaciones:
Figura 19: Gráfica de q¨1
Figura 20: Gráfica de q¨2
80
14. Ejercicio 14
Para el modelo dinámico de un robot doble revoluta (visto en clase) valide el modelo a través de una
simulación en simscape (la simulación solo en el periodo de validez, pueden ser unos cuantos segundos).
Modelo visto en clase

u
D(q)q̈ + C(q, q̇)q̇ + g(q) = 1
u2
Donde:
2

m1 lc1 + m2 l12 + m2 lc2
2 2
+ 2m2 l1 lc2 cos(q2 ) + I1z + I2z m2 lc1 + m2 l1 lc2 cos(q2 ) + I2z
D(q)) = 2 2
m2 lc2 + m2 l1 lc2 cos(q2 ) + I2z m2 lc2 + I2z

−m2 l1 lc2 sin(q2 )q˙2 −m2 l1 lc2 sin(q2 )(q˙1 + q˙2 )
C(q, q̇) = ˙
m2 l1 lc2 sin(q2 )q1 0

m1 glc1 cos(q1 ) + m2 gl1 cos(q1 ) + m2 glc2 cos(q1 + q2 )
g(q) =
m1 glc2 cos(q1 + q2 )
Modelo en Solid Work:
Figura 21: Ensamblaje en Solid Work
81
Modelo capturado en Simulink:
Figura 22: Ecuación de u1
Figura 23: Ecuación de u2
Figura 24: Esquema en Simulink
82
Resultados comparando Multibody y simulación en Simulink:
Figura 25: Comparación de q1
Figura 26: Comparación de q2
83
Al ser el sistema doble péndulo un sistema muy caótico cualquier ligera variación crece exponencial-
mente en el tiempo y se desajusta las simulaciones
84

Lista de Ejercicios de Control de Sistemas Robóticos

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Lista de Ejercicios de Control de Sistemas Robóticos

Cargado por

Copyright:

Formatos disponibles

Instituto Politécnico Nacional

Unidad Profesional Interdisciplinaria en Ingenierı́a y Tecnologı́as Avanzadas

Control de Sistemas Robóticos

Profesor Dr. Alberto Luviano Juárez

Figura 1: Junta flexible

L = 21 I q˙1 2 + 12 J q˙2 2 − M gl(1 − cos q1 ) − 21 k(q1 − q2 )2

La formulación de Euler-Lagrange tiene la forma de la siguiente expresión, por lo que se requiere

I q¨1 + M gl sin q1 + k(q1 − q2 ) = 0

El tensor de inercia esta dado por:

Figura 2: Robot SCARA

Muestre las propiedades de cotas de la matriz de inercia, antisimetrı́a.

Proponga una representación lineal respecto a los parámetros.

Cuadro 1: Parámetros de Denavit Hartenberg para robot SCARA.

Considerando los vectores Ocmi , zn y On :

Se procede a realizar el calculo de la matriz de inercia a partir de la siguiente expresión.

Se tiene el cálculo de la parte traslacional:

Calculando la parte rotacional de la matriz de inercia:

Por lo tanto, la matriz de inercia queda de la siguiente forma:

C112 = a1 s2 (m3 a2 + m2 lc2 )

Se sabe que para un k fijo, Cijk = Cjik , por lo que se tiene:

C121 = C221 = −a1 s2 (m3 a2 + m2 lc2 )

Considerando la expresión anterior se tienen los siguientes términos de la matriz de Coriolis:

Realizando los cálculos correspondientes:

C11 = −a1 q˙2 s2 (m2 lc2 + m3 a2 )

Por lo que, se obtiene la siguiente matriz de Coriolis:

P = P (q) = 3i=1 g T rcmi mi

Por lo que, se obtiene:

P = ḡ[d1 (m1 + m2 ) + (d1 − q3 + 2l )m3 ]

Dado que la gravedad apunta en −z se considera este signo en el vector gravedad:

Construyendo cada término:

Además, comprobando de forma matricial, se tiene:

Con esto se comprueba que la propiedad de antisimétria.

Sustituyendo los parámetros en las ecuaciones de movimientos se obtiene el regresor:

Donde el vector de parámetros es:

Figura 3: Robot planar

Muestre las propiedades de cotas de la matriz de inercia, antisimetrıa.

De la expresión para obtener la matriz de inercia.

Se tiene el cálculo de la parte traslacional:

En la parte rotacional, las matrices de rotación están dadas por:

Calculando la parte rotacional de la matriz de inercia:

Se deben calcular n3 sı́mbolos, donde n es el numero de grados de libertad del robot.

C111 = 0; C112 = m3 (a1 a2 s2 + a1 lc3 s23 ) + m2 a1 lc2 s2 ; C113 = m3 a1 l3 (s12 − s23 )

Calculando los términos de la matriz C:

C11 = C111 q˙1 + C211 q˙2 + C311 q˙3

La matriz de Coriolis es:

˙ − 2C(q, q̇) es antisimétrica, para calcularla usamos:

Construyendo cada término:

Finalmente, calculamos los elementos de la matriz N :

nkj = d˙kj − 2ckj

C11 = (Θ3 s2 + Θ4 s23 + Θ2 )q˙2 + Θ4 s23 q˙3

τ1 = Θ1 + 2Θ2 c2 + 2Θ3 c2 + 2Θ4 c23 + 2Θ5 c3 + Θ6 + Θ2 c2 + Θ3 + Θ4 c23 + Θ5 c3 + Θ7 + Θ4 c12 + Θ5 c3 +

Y12 = 2c2 + c2 + q˙2 + s2 q˙1 + s2 q˙2

Figura 4: Robot RRR

Muestre las propiedades de cotas de la matriz de inercia, antisimetrı́a.

Cuadro 2: Parámetros de Denavit Hartenberg para robot RRR.

Para los centros de masa, se tiene:

Desarrollando los Jacobianos:

Calculando cada término por separado:

Calculando la parte rotacional de la matriz de inercia:

Calculando cada término por separado:

La matriz de inercia queda como:

Calculando los términos de la matriz C:

C11 = C111 q˙1 + C211 q˙2 + C311 q˙3

C11 = c111q˙10 + c211 q˙2 = m2 q˙2 (lc + q2 )