DINAMICA DE SISTEMAS MULTICUERPOS

Aplicacin de Dinmica de sistemas Multicuerpos

En la figura siguiente se muestra un manipulador de estructura posicional esfrica de

tres grados de libertad, constituido por elementos rgidos cuyas masas se encuentran
distribuidas homogneamente a lo largo de sus longitudes. El elemento E1 de masa M1
es un cilindro slido de longitud L1 y radio R1. El elemento E2 de masa M3 es un cilindro
hueco de pared delgada de longitud L2 y radio R2. El elemento E3 de masa M3 es un
cilindro delgado de longitud L3.

z0

x3
z2
z3
x2
P
L2 y3
4 E3
2
q3
q2 E2
d3

z1
x1
L1 E1

q1

y0

0
Elemento x1
fijo 1

x0
DINAMICA DE SISTEMAS MULTICUERPOS

Se pide:

a) Determinar la cinemtica directa de posicin del punto P.

b) Determinar la Energa Cintica del sistema.

c) Determinar la Energa Potencial del sistema.

d) Determinar la expresin de Lagrange del sistema.

e) Determinar las ecuaciones de la dinmica inversa del sistema.

f) Representar las ecuaciones de la dinmica directa del sistema mediante

variables de estado.

g) Realizar la simulacin del sistema eligiendo esfuerzos adecuados de

activacin.

Solucin

Solucin a

En la siguiente tabla se muestran los parmetros del sistema multicuerpos para aplicar el
algoritmo de Denavit-Hartemberg:

i ai i di i

1 0 90 L1 1=q1

2 0 90 0 2=q2

3 0 0 d3=q3 0

Las matrices correspondientes a cada par articulacin-eslabn teniendo en cuenta la

tabla anterior son:

2 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

C1 0 S 1 0 C2 0 S2 0 1 0 0 0
1 0 0
1 S 1 0 C1 0 2 S2 0 C2 0 3 0
0T , T
0
, T
L1 1 0 0 0 1 d3
2
0 1 0 1 0

0 0 0 1 0 0 0 1 0 0 0 1

Multiplicando las dos primeras matrices anteriores se obtiene la matriz de

transformacin homognea que relaciona el referencial x2y2z2 con respecto del
referencial x0y0z0 de la siguiente forma:

C1C 2 S1 C1 S 2 0
C1 0
2 S 1C 2 S1 S 2
0T
S2 0 C2 L1

0 0 0 1

Multiplicando las tres matrices anteriores se obtiene la matriz total de transformacin

homognea que relaciona el referencial x3y3z3 respecto del referencial x0y0z0 de la
siguiente manera:

C1C 2 S1 C1 S 2 C1 S 2 d 3
C1 S1 S 2 d 3
3 S 1C 2 S1 S 2
0T
S2 0 C2 L1 C 2 d 3

0 0 0 1

De la ltima ecuacin o matriz se obtiene la Cinemtica Directa de la posicin del

sistema multicuerpos propuesto:

C1 S 2 d 3

P(q) S 1 S 2 d 3
L1 C 2 d 3

Solucin b

3 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

En la tabla se muestran los momentos inerciales de objetos respecto de su centro de

masa, longitud H a lo largo del eje z:

Objeto Ixx Iyy Izz

Cilindro o 3R 2 H 2 3R 2 H 2 MR 2
M( ) M( )
disco 12 12 2

Cascarn 6R2 H 2 6R2 H 2 MR 2

M( ) M( )
cilndrico muy 12 12

delgado

Cilindro muy H2 H2
M M
delgado 12 12 0

Los vectores desde el origen del sistema de coordenadas de cada elemento a sus
respectivos centros de masa son respectivamente:

0 0 0
L 0 0
C1 1 , C 2 , C 3
2 L2 L3
0
4 2

Los tensores de inercia de los elementos que constituyen al sistema respectos de sus
respectivos referenciales trasladados a sus centros de masa son respectivamente:

3R12 L12
M1 ( ) 0 0
12
M1R12
I1 0 0
2
2
3R L1
2

0 0 M1 ( 1 )
12

4 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

6 R 2 L22
M 2 ( 2 ) 0 0
12
6 R2 L2
2 2
I2 0 M2( ) 0
12
2
0 0 M 2 R2

L23
M3 0 0
12
L23
I3 0 M3 0
12
0 0 0

Los tensores de inercia de los elementos que constituyen al sistema respectos del
referencial base x0y0z0 trasladado a sus centros de masa son respectivamente:

C1 0 S1 I1xx 0 0 C1 S1 0

D1 (q) =[ 0 R (q)] I1[ 1 R (q)]= S1 0 C1 0
1 0
I1zz 0 0 0 1
0 1 0 0
I1xx S1 C1 0
0

3R 2 L12
M1 ( 1 ) 0 0
I1xx 12
0 0
3R12 L12
D1 (q) = 0 I1xx 0 0 M1 ( ) 0
12
0 0 I1zz 2
M1R1
0 0
2

C1 C2 S1 C1 S2 I 2 xx 0 0 C1 C2 S1 C2 S2

D2 (q) =[ 0 R 2 (q)] I2 [ 2 R0 (q)]= S1 C2 C1 S1 S2 0 I 2 xx 0 S1 C1 0

S2 0 C2 0 0 I 2 zz C1 S2 S1 S2 C2

5 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

I C 2C 2 I S 2 I C 2 S 2 I 2 xxC1S1S22 I 2 zz C1S1S22 I 2 xxC1S2C2 I 2 zz C1S2C2

2 xx 1 2 2 xx 1 2 zz 1 2
D2 (q) = I 2 xxC1S1S2 I 2 zz C1S1S22
2
I 2 xx S12C22 I 2 xxC12 I 2 zz S12 S22 I 2 xx S1S2C2 I 2 zz S1S2C2

I C S C I C S C I 2 xx S1S2C2 I 2 zz S1S2C2 I 2 xx S2 I 2 zz C2
2 2
2 xx 1 2 2 2 zz 1 2 2

d211 d212 d213

D2 (q) = d212 d222 d223

d213 d223 d233

C1 C2 S1 C1 S2 I3 xx 0 0 C1 C2 S1 C2 S2

D3 (q) =[ 0 R 3 (q)] I3[ 3 R0 (q)]= S1 C2 C1 S1 S2 0 I3 xx 0 S1 C1 0

S2 0 C2 0 0 0 C1 S2 S1 S2 C2

I C 2C 2 I S 2 I3 xxC1S1S22 I3 xxC1S2C2 d3
3 xx 1 2 3 xx 1 11 d312 d313
D3 (q) = I3 xx C1S1S22 I3 xx S12C22 I3 xxC12 I3 xx S1S2C2 = d312 d322 d323

I3 xx C1S2C2 I3 xx S1S2C2 I3 xx S22 d313 d323 d333

Los vectores desde el origen del referencial base hasta el centro de masa de cada

elemento son descritos respectivamente como

0
0
C 1(q) P1 (q) 0 R 1 (q)C 1
L1

2

L2
C1 S 2
4
L
C 2 (q) P2 (q) 0 R 2 (q)C 2 S 1 S 2 2
4
L
L1 C 2 2
4

6 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

L3
1 2 3
C S ( d )
2
L
C 3 (q) P3 (q) 0 R 3 (q)C 3 S 1 S 2 (d 3 3 )
2
L
L1 C 2 (d 3 3 )
2

Derivando los vectores de los respectivos centros de masa se obtiene las matrices que
relacionan la velocidad lineal cartesiana del centro de masa con el vector velocidad
articular

0 0 0
J v1 (q) 0 0 0
0 0 0

S S C1C 2 0
2 L2 1 2
J v (q) C1 S 2 S1C 2 0
4
0 S2 0

L3 L3
S 1 S 2 (d 3 ) C1C 2 (d 3 ) C1 S 2
2 2
L3 L L
3
J v (q) C1 S 2 (d 3 ) S 1C 2 (d 3 ) S 1 S 2
3 3
2 2 2
L
0 S 2 (d 3 3 ) C2
2

Las matrices que relacionan las velocidades angulares relativas al centro de masa de
cada elemento respecto del referencial base con el vector velocidad articular
respectivamente son

0 0 0 0 S1 0 0 S1 0
1
Jw (q) 0 0 0 J w2 (q) 0 C1 0 3
Jw (q) 0 C1 0
1 0 0 1 0 0 1 0 0

Los tensores de inercia reflejados a los ejes de giro de cada articulacin expresados
respecto del referencial base se expresan respectivamente como:

7 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

D1 (q) [ J v1 (q)]T M 1[ J v1 (q)] [ J w

1
(q)]T D1 (q)[ J w
1
(q)]

0 0 0 0 0 0 0 0 1 I 1xx 0 0 0 0 0 I 1zz 0 0

D1 (q) M 1 0 0 0 0 0 0 0 0 0 0 I 1xx 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 I 1zz 1 0 0 0 0 0

R12
M 1 0 0
2
D1 (q) 0 0 0
0 0 0

D2 (q) [ J v2 (q)]T M 2 [ J v2 (q)] [ J w

2
(q)]T D2 (q)[ J w
2
(q)]

S S C1 S 2 0 S 1 S 2 C1C 2 0
L2 2 1 2
D2 (q) M 2 ( ) C1C 2 S 1C 2 S 2 C1 S 2 S 1C 2 0
4
0 0 0 0 S2 0
0 0 1 d 211 d 212 d 213 0 S 1 0

S 1 C1 0 d 212 d 222 d 223 0 C1 0
0 0 0 d 213 d 223 d 233 1 0 0

L2 2 2
M 2 ( ) S 2 0 0 d 233 S1d 213 C1d 223 0
4
D2 (q) 0 1 0 S1d 213 C1d 223 S12 d 211 2 S1C1d 212 C12 d 222 0
0 0 0 0 0 0

L22 6 R22 2
L2 2 2
M2( ) S 2 M 2 R22 C 22 0
0
M 2 ( ) S 2 0 0 12
4 2 2
L 6 R2
D2 ( q ) 0 1 0 0 M2( 2 ) 0
12
0 0 0 0 0 0

8 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

L2 L2 6 R22 2
M 2 ( 2 2 ) S 2 M 2 R22 C 22 0 0
16 12
L2 2 2
L 6 R2
D2 ( q ) 0 M2( 2 2 ) 0
16 12
0 0 0

D3 (q) [ J v3 (q)]T M 3 [ J v3 (q)] [ J w

3
(q)]T D3 (q)[ J w
3
(q)]

S 1 S 2 D3 C1 S 2 D3 0 S 1 S 2 D3 C1C 2 D3 C1 S 2

D3 (q) M 3 C1C 2 D3 S 1C 2 D3 S 2 D3 C1 S 2 D3 S1C 2 D3 S1 S 2
C1 S 2 S1 S 2 C 2 0 S 2 D3 C 2
0 0 1 d 311 d 312 d 313 0 S 1 0
L
S 1 C1 0 d 312 d 322 d 323 0 C1 0 ; D3 d 3 3
2
0 0 0 d 313 d 323 d 333 1 0 0

D 2 S 2 0 0 d 233 S1d 213 C1d 223 0

3 2
D3 (q) M 3 0 D32 0 S1d 213 C1d 223 2
S1 d 211 2 S1C1d 212 C12 d 222 0
0 0 1 0 0 0

L23
L3 2 2 M S 22 0
M 3 (d 3 ) S 2 0 0 3 0
2 12

L L23
D3 (q) 0 M 3 (d 3 3 ) 2 0 0 M3 0
2 12
0 0 M3 0 0 0

L3 2 L23 2
M 3 {(q3 ) }S 2 0 0
2 12
L3 2 L23
D3 (q) 0 M 3 {(q3 ) } 0
2 12
0 0 M3

El tensor de inercia total neto del sistema respecto de los ejes de rotacin o traslacin
relativos al referencial base se obtiene sumando los tensores de cada elemento como

9 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

D(q) D1 (q) D2 (q) D3 (q)

d 11 (q) 0 0

D(q ) 0 d 22 (q) 0
0 0 d 33 (q)

donde

R12 L22 L22 6 R22 2 2 2 L3 2 L23 2

d 11 (q) M 1 M2( ) S 2 M 2 R2 C2 M 3 {(q3 ) }S 2
2 16 12 2 12

L2 L2 6 R22 L L2
d 22 (q) M 2 ( 2 2 ) M 3 {(q3 3 ) 2 3 }
16 12 2 12

d 33 (q) M 3

La expresin de la energa cintica total se describe como:

1
Ec (q, q ) q T D(q) q
2

d 11 (q) 0 0 q1
Ec (q, q ) q1 q 3 0
1
q 2 d 22 (q) 0 q 2
2
0 0 d 33 (q) q 3

1

Ec (q, q ) d 11 (q)q12 d 22 (q)q 22 d 33 (q)q 32
2

La expresin de la energa potencial total del sistema se obtiene como

g T M i Ci (q) g T M 1C1 (q) M 2C2 (q) M 3C3 (q)

3
E p (q)
i 1

Proyectando los vectores de los centros al vector antiparalelo al vector de la aceleracin

de la gravedad se expresa como:

E p (q) g0 M 1C1z (q) M 2 C2 z (q) M 3C3 z (q) ; g0 = 9.8 m/s2

La ecuacin de la energa potencial explcitamente se representa como

10 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

L L L
E p (q) g 0 M 1 2 M 2 ( L1 C2 2 ) M 3 ( L1 C2 (q3 3 ))
2 4 2

La expresin del lagrangeano queda expresado como:

L(q, q )
1
2

d 11 (q)q12 d 22 (q)q 22 d 33 (q)q 32

L L L
g 0 M 1 2 M 2 ( L1 C 2 2 ) M 3 ( L1 C 2 (q3 3 ))
2 4 2

Para determinar la dinmica del robot se aplica la ecuacin siguiente:

d
L(q ,q ) L(q ,q ) bi (q ) = i ; 1 i 3
dt q i qi

Para i = 1 se utiliza la siguiente expresin:

d
L(q ,q ) L(q ,q ) b1 (q1 ) = 1
dt q1 q1

Desarrollando cada trmino se obtiene:

q1
L(q ,q )

q1

Ec (q ,q ) E p (q)

q1
Ec (q ,q ) d11( q )q1

d
L(q ,q ) d11( q )q1 d11( q )q1 d11( q )q1
d d
dt q1 dt dt

d
L(q ,q ) d11( q )q1 d11( q )q 2 d11( q )q 3 q1 d11( q )q1
dt q1 q1 q2 q3

d11( q ) 0
q1

L2 L2 6 R22 L L2
d11( q ) M 2 ( 2 2 )S 22 M 2 R22 S 22 M 3{(q3 3 ) 2 3 }S 22
q 2 16 12 2 12

L
d11( q ) 2M 3{(q3 3 )}S 22
q3 2

11 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

d
L(q , q ) d11( q )q1
dt q1

L22 L22 6 R22 L3 2 L23 L3

2

2 M ( )S 22 M R 2
S
2 2 22 M 3 {( q 3 ) }S
22 2
q 2 M 3 {( q 3 )}S 2 3 q1
q

16 12 2 12
2

d
L(q ,q ) d11( q )q1
dt q1
7 L22 R22 L L2 L
M 2( )S 22q1q 2 M 3{(q3 3 ) 2 3 }S 22q1q 2 2M 3 {(q3 3 )}S 22 q1q 3
48 2 2 12 2

q1
L(q ,q ) =

q1

Ec (q ,q ) E p (q)

q1
Ec (q ,q )

q1
E p (q )

1
Ec (q ,q ) d11( q )q12 d 22 ( q )q 22 d 33( q )q 32
q1 2 q1 q1 q1

d11( q ) 0
q1

d 22 ( q ) 0
q1

d 33( q ) 0
q1

E p (q ) 0
q1

b1 (q1 ) sgn (q1 ) b1s b1v q1

Entonces la expresin de la dinmica para para accionar la articulacin 1 mediante una

torsin externa se expresa de la siguiente manera:

7 L22 R22 L L2
1 d11( q )q1 M 2 ( )S 22q1q 2 M 3{(q3 3 ) 2 3 }S 22q1q 2
48 2 2 12
L3
2M 3{(q3 )}S 22 q1q 3 sgn (q1 ) b1s b1v q1
2

Para i = 2 se utiliza la siguiente expresin:

12 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

d
L(q ,q ) L(q ,q ) b2 (q 2 ) = 2
dt q 2 q2

Desarrollando cada trmino se obtiene:

q 2
L(q ,q )

q 2
Ec (q,q) E p (q) q Ec (q ,q ) d 22 ( q )q 2
2

d
L(q ,q ) d 22 ( q )q 2 d 22 ( q )q 2 d 22 ( q )q2
d d
dt q 2 dt dt

d
L(q ,q ) d 22 ( q )q1 d 22 ( q )q 2 d 22 ( q )q 3 q 2 d 22 ( q )q2
dt q 2 q1 q2 q3

d 22 ( q ) 0
q1

d 22 ( q ) 0
q 2

L
d 22 ( q ) 2M 3{(q3 3 )}
q3 2

d L
L(q ,q ) d 22 ( q )q2 2M 3{(q3 3 )}q 2 q 3
dt q 2 2

q2
L(q ,q ) =

q2
Ec (q,q) E p (q) q Ec (q ,q )

q2
E p (q )
2

1
Ec (q ,q ) d11( q )q12 d 22 ( q )q 22 d 33( q )q 32
q2 2 q2 q2 q2

L2 L2 6 R22 L L2
d11( q ) M 2 ( 2 2 )S 22 M 2 R22 S 22 M 3{(q3 3 ) 2 3 }S 22
q 2 16 12 2 12

d 22 ( q ) 0
q2

d 33( q ) 0
q2

1 L2 L2 6 R22 L L2
Ec (q ,q ) M 2( 2 2 ) M 2 R22 M 3{(q3 3 ) 2 3 }S 22q12
q2 2 16 12 2 12

13 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

L L
E p (q ) g 0 M 2 S 2 2 M 3 S 2 ( q 3 3 )
q2 4 2

b2 (q 2 ) sgn (q 2 ) b2s b2v q 2

Entonces la expresin de la dinmica para para accionar la articulacin 2 mediante una

torsin externa se expresa de la siguiente manera:

L3
2 d 22 ( q )q2 2M 3 {(q3 )}q 2 q 3
2
1 L22 L22 6 R22 L3 2 L23
M 2( ) M 2 R2 M 3{(q3 ) } S 22 q12
2
2 16 12 2 12
L L
g 0 M 2 S 2 2 M 3 S 2 ( q3 3 ) sgn (q 2 ) b2s b2v q 2
4 2

Para i = 3 se utiliza la siguiente expresin:

d
L(q ,q ) L(q ,q ) b3 (q 3 ) = 3
dt q 3 q3

Desarrollando cada trmino se obtiene:

q 3
L(q ,q )

q 3
Ec (q,q) E p (q) q Ec (q ,q ) d 33( q )q 3
3

d
L(q ,q ) d 33( q )q 3 d 33( q )q 3 d 33( q )q3
d d
dt q 3 dt dt

d
L(q ,q ) d 33( q )q1 d 33( q )q 2 d 33( q )q 3 q 2 d 33( q )q3
dt q 3 q1 q 2 q3

d 33( q ) 0
q1

d 33( q ) 0
q 2

d 33( q ) 0
q3

d
L(q ,q ) d 33( q )q3
dt q 3

14 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

q3
L(q ,q ) =

q3
Ec (q,q) E p (q) q Ec (q ,q )

q3
E p (q )
3

1
Ec (q ,q ) d11( q )q12 d 22 ( q )q 22 d 33( q )q 32
q3 2 q3 q3 q3

L
d11( q ) 2M 3{(q3 3 )}S 22
q3 2

L
d 22 ( q ) 2M 3 (q3 3 )
q3 2

d 33( q ) 0
q3

1 L L
Ec (q ,q ) 2M 3{(q3 3 )}S 22 q12 2M 3 (q3 3 )q 22
q3 2 2 2

L3 L
Ec (q ,q ) M 3{(q3 )}S 22 q12 M 3 (q3 3 )q 22
q3 2 2

E p (q) g 0 M 3C 2
q3

b3 (q3 ) sgn (q3 ) b3s b3v q3

Entonces la expresin de la dinmica para para accionar la articulacin 3 mediante una

fuerza externa se expresa de la siguiente manera:

L3 L
f 3 d 33( q )q3 M 3{(q3 )}S 22 q12 M 3 (q3 3 )q 22 g 0 M 3C2 sgn (q 3 ) b3s b3v q 3
2 2

La dinmica del robot de forma estructura (matricial-vectorial) se expresa de la

siguiente manera:

1 d11( q ) 0 0 q1 c1( q , q ) h1( q ) b1( q1 )

0 d 22 ( q ) 0 q2 c2 ( q , q ) h2 ( q ) b2 ( q 2 )
2
f 3 0 0 d 33( q ) q3 c3 ( q , q ) h3 ( q ) b2 ( q 2 )

La expresin anterior se sintetiza de la siguiente manera:

+ c(q ,q ) + h(q) + b(q )

= D(q) q

15 Ing. Jos Machuca Mines

DINAMICA DE SISTEMAS MULTICUERPOS

Donde;

7L22 R22 L L2 L
M 2( )S 22q1q 2 M 3{(q3 3 ) 2 3 }S 22q1q 2 2M 3{(q3 3 )}S 22 q1q 3
c1( q , q ) 48 2 2 12 2

c ( q , q ) 2M {(q 3 )}q q M ( 2 2
L 1 L 2
L 2
6 R 2
L L2

2
) M 2 R22 M 3{(q3 3 ) 2 3 }S 22q12
2 3 3 2 2 3
2 2
16 12 2 12
c3 ( q , q )
L3 L3 2
M 3{(q3 )}S 2 q1 M 3 ( q3 )q 2
22

2 2

h1( q ) 0
h ( q ) g M S 2 M S ( q 3 )
L L
2 0 2 2 4 3 2 3
2
h3 ( q )
g 0 M 3C 2

b1( q1 ) sgn (q1 ) b1 b1 q1

s v
b ( q ) sgn (q ) b s b v q
2 2 2 2 2 2
b2 ( q 2 ) sgn (q 3 ) b3 b3v q 3
s

16 Ing. Jos Machuca Mines