Facultad Ingeniera Electrnica y Mecatrnica

wcp 2010
Laboratorio 6. Dinmica del robot

Objetivo.
-

Obtener el modelo dinmico del robot MOTOMAN HP3

Marco terico.
La obtencin del modelo dinmico de un mecanismo y en particular de un robot se basa
fundamentalmente en el planteamiento del equilibrio de fuerzas establecidas en la segunda ley
de Newton o su equivalente para movimientos de rotacin, la denominada ley de Euler.

[1]

La formulacin Langraniana establece la ecuacin

L = Ec Ep

[2]
Donde
L: funcin Langrangiana
Ec Energa cintica
Ep: Energa potencial
Qi Coordenadas generalizadas (en este caso articulares)
i : fuerza o pares aplicados sobre el grado de libertad qi
En el caso del robot monorticular de la figura l se tendra:
Aunque la obtencin del modelo mediante la formulacin de Lagrange resulta mas tediosa que
mediante la formulacin de Newton, la primera muestra sus ventajas a medida que aumenta el
numero de grados de libertad.
Sea cual sea el procedimiento seguido para la obtencin del modelo dinmico del robot este
presenta la forma

[3]
Donde

(t) = vector de par generalizado aplicado a las ecuaciones

q(t)= vector (n)de las variables de las articulaciones del brazo
q(t) y q(t) = vectores (n) de la velocidad y aceleracin del brazo

D(q) = matriz (nxn) simtrica inercial denominada matriz de inercia

H (q,q) = vector (n)de fuerzas de Coriolis y centrfuga
c(q) = vector (n) de las fuerzas de carga gravitatoria
n. : numero de grados de libertad del robot.
Hay que tener presente que el vector de pares (t) presupone pares efectivos, por lo que de
existir pares perturbadores o de rozamiento (viscoso o seco) estos debern a ser tomados en
cuenta

ROBOT MOTOMAN HP3

En la figura 1. Se muestra el robot MOTOMAN HP3

Figura 1. Robot MOTOMAN HP3

Parmetros del robot.
1. Masa del i-simo eslabn
Masa del eslabn 1 (base): m1 = 7.1402 kg
Masa del eslabn 2 (brazo): m2 = 2.2483kg
Masa del eslabn 3 (antebrazo): m3 = 1.957 kg
2. Masa del efector final + carga.
Masa del efector final (gripper): m f = 1kg
3. Longitud del i-simo eslabn
Longitud del eslabn 1 (altura): l1 = 32 cm
Longitud del eslabn 2: l2 = 22 cm
Longitud del eslabn 3: l3 = 22 cm
4. Distancia del centro de giro al centro de masas de los eslabones
Eslabn 1: lc1 = 0 cm
Eslabn 2: lc 2 = 5.2 cm
Eslabn 3: lc 3 = 13.76 cm
5. I i i = 1, 2,3 : Momento de inercia del i-simo eslabn.
Momento de inercia del eslabn 1: I1 = 0.04624 kgm 2
Momento de inercia del eslabn 2: I 2 = 0.02595 kgm 2
Momento de inercia del eslabn 3: I 3 = 0.03616 kgm 2
6. i i = 1, 2,3 : Angulo de giro de la i-sima articulacin.

7. Masa del i-simo eslabn

Masa del eslabn 1 (base): m1 = 7.1402 kg
Masa del eslabn 2 (brazo): m2 = 2.2483kg
Masa del eslabn 3 (antebrazo): m3 = 1.957 kg
8. Masa del efector final + carga.
Masa del efector final (gripper): m f = 1kg
9. Longitud del i-simo eslabn
Longitud del eslabn 1 (altura): l1 = 32 cm
Longitud del eslabn 2: l2 = 22 cm
Longitud del eslabn 3: l3 = 22 cm
10. Distancia del centro de giro al centro de masas de los eslabones
Eslabn 1: lc1 = 0 cm
Eslabn 2: lc 2 = 5.2 cm
Eslabn 3: lc 3 = 13.76 cm
11. I i i = 1, 2,3 : Momento de inercia del i-simo eslabn.
Momento de inercia del eslabn 1: I1 = 0.04624 kgm 2
Momento de inercia del eslabn 2: I 2 = 0.02595 kgm 2
Momento de inercia del eslabn 3: I 3 = 0.03616 kgm 2
12. i i = 1, 2,3 : Angulo de giro de la i-sima articulacin.
Procedimiento.
MODELO DINMICO
Desarrollar el modelado cinemtico de los primeros tres gdl del robot MOTOMAN HP3 los
cuales son los encargados de posicionar al efector final
En la figura 2 muestra la cadena cinemtica para el robot analizado.
Para desarrollar el modelo cinemtico directo, se sigue el algoritmo de Denavit-Hartenberg
(DH).

Tabla 1. Parmetros DH del robot MOTOMAN HP3+

Eslabn

di

ai

l1

90

2
3

0
0

l2

0
0

l3

Figura1. Cadena cinemtica para el robot MOTOMAN HP3

.
Con los parmetros de la tabla anterior se obtiene la matriz de transformacin de cada uno de
los elementos de la cadena cinemtica. Por simplicidad Ci = Cosi y Si = Seni .
C1 0 S1 0
S 0 C 0
1
0

A1 = 1
0 1 0 l1

0 0 0 1

C2
S
1
A2 = 2
0

S2
C2

C3
S
2
A3 = 3
0

S3
C3

0
0

0
0

(1)

0 l2 C2
0 l2 S 2
1
0

0
1

(2)

0 l3C3
0 l3 S3
1 0

0 1

(3)

Con las matrices anteriores, se puede obtener la matriz de transformacin homognea de la

cadena cinemtica completa.
C1C2 + 3
S C
T = 1 2+3
S2+3

C1 S2 + 3

S1

S1 S2 + 3

C1

C2 + 3

l3C1C2 + 3 + l2 C1C2
l3 S1C2 + 3 + l2 S1C2
l3 S 2 + 3 + l2 S 2 + l1

(4)

La ltima columna de la matriz T de la Ec. (4), corresponde a las ecuaciones de la posicin del
efector final.
px = l3C1C2 C3 l3C1 S2 S3 + l2 C1C2

(5)
(6)
(7)

p y = l3 S1C2 C3 l3 S1 S 2 S3 + l2 S1C2
pz = l3 S 2 C3 + l3C2 S3 + l2 S 2 + l1

Las Ec. (5), (6) y (7) forman el modelo cinemtico directo del robot MOTOMAN HP3

Desarrollar el modelo dinmico del robot manipulador por medio de las ecuaciones de EulerLagrange. Como primer paso se deben obtener las energas cinticas de los eslabones del robot
manipulador. En un robot manipulador la energa cintica se encuentra conformada por suma de
la energa cintica traslacional y la energa cintica rotacional.
K i ( q, q& ) =

1
1
mi v 2 + I i&&2
2
2

(11)

Enseguida, se obtienen las energas potenciales de los eslabones del robot manipulador.
U i ( q ) = mi gh

(12)

Como tercer paso se obtiene el Lagrangiano del sistema. El cual se encuentra determinado por la
diferencia entre la energa cintica total1 y la energa potencial total2.
L ( q, q& ) = K ( q, q& ) U ( q )

(13)

Enseguida, se desarrolla la siguiente ecuacin para cada una de las coordenadas articulares.
d L

dt &i

L
=

(14)

A continuacin se desarrolla el modelo dinmico del robot MOTOMAN HP3 por medio de las
ecuaciones de Euler-Lagrange. Como primer paso se obtienen las energas cinticas de cada uno
de los eslabones. Como el primer eslabn, solamente rota alrededor del eje z, nicamente cuenta
con energa cintica rotacional.

1
K1 &, = I1&12
2

(15)

Para los siguientes eslabones las ecuaciones de sus energas cinticas son las siguientes:

1
1
K 2 &, = m2 lc 2 2&2 2 + lc 2 2 cos 2 2&12 + I 2 &12 + &2 2
2
2

(16)

( )

1
1
1
1
K 3 &, = m3lc 32&2 2 + m3lc 32&32 + m3lc 32&2&3 + m3l2 2&2 2 + m3l2 lc 3 cos ( 2 ) cos ( 2 + 3 )&12 + m3lc 32 cos 2 ( 2 + 3 ) &12
2
2
2
2
2
1
1
+ m3l2 2 cos 2 ( 2 ) &12 + m3l2 lc 3 cos ( 3 ) &2 &2 + &3 + I 3 &12 + &2 + &3

2
2

(17)

1
1
1
1
K 4 &, = m f l32&2 2 + m f l32&32 + m f l32&2&3 + m f l2 2&2 2 + m f l2 l3 cos ( 2 ) cos ( 2 + 3 ) &12 + m f l32 cos 2 ( 2 + 3 ) &12
2
2
2
2
1
2
2
2
+ m f l2 cos ( 2 ) &1 + m f l2 l3 cos ( 3 ) &2 &2 + &3
2

(18)

Siguiendo con el proceso de modelado, se deben obtener las expresiones de las energas
potenciales.
U1 ( ) = m1 gh

(19)

U 2 ( ) = m2 g ( h + lc 2 sen 2 )

(20)

U 3 ( ) = m3 g ( h + l2 sen 2 + lc 3 sen ( 2 + 3 ) )

(21)

U 4 ( ) = m f g ( h + l2 sen 2 + l3 sen ( 2 + 3 ) )

(22)

El Lagrangiano del sistema es el siguiente:

1
1
1
1
1
1
L &, = I1&12 + m2 lc 2 2&2 2 + lc 2 2 cos 2 2&12 + I 2 &12 + &2 2 + m3lc 32&2 2 + m3lc 32&32 + m3lc 3 2&2&3 + m3l2 2&2 2
2
2
2
2
2
2
1
1
2
2
2
2
2
2
2
&
&
&
&
&
+ m3l2 lc 3 cos ( 2 ) cos ( 2 + 3 ) 1 + m3lc 3 cos ( 2 + 3 )1 + m3l2 cos ( 2 ) 1 + m3l2 lc 3 cos (3 ) 2 2 + &3
2
2
2
1 &2
1
1
1
2
2
2
2
2
+ I 3 1 + &2 + &3 + m f l3 &2 + m f l3 &3 + m f l3 &2&3 + m f l2 l3 cos ( 2 ) cos ( 2 + 3 )&12 + m f l2 2&2 2
2
2
2
2
1
1
+ m f l32 cos 2 ( 2 + 3 ) &12 + m f l2 2 cos 2 ( 2 ) &12 + m f l2 l3 cos ( 3 )&2 &2 + &3 m1 gh m2 g ( h + lc 2 sen 2 )
2
2
m3 g ( h + l2 sen 2 + lc 3 sen ( 2 + 3 ) ) m f g ( h + l2 sen 2 + l3 sen ( 2 + 3 ) )

(23)
Desarrollando la Ec. (14) para cada una de las articulaciones del robot manipulador se obtienen
las Ec. (24), (25) y (26).
I1&&1 + I 2&&1 + I 3&&1 + m2 lc 2 2 cos 2 2&&1 2m2 lc 2 2 cos 2 sen 2&1&2 2 ( m3l2 lc 3 + m f l2 l3 ) sen 2 cos ( 2 + 3 ) &1&2

2 ( m3l2 lc 3 + m f l2 l3 ) cos 2 sen ( 2 + 3 ) &1 &2 + &3 + 2 ( m3l2 lc 3 + m f l2 l3 ) cos 2 cos ( 2 + 3 ) &&1

2 ( m3lc 32 + m f l32 ) sen ( 2 + 3 ) cos ( 2 + 3 ) &1 &2 + &3 + ( m3lc 32 + m f l32 ) cos 2 ( 2 + 3 ) &&1
2 ( m3 + m f ) l2 2 sen 2 cos 2&1&2 + ( m3 + m f ) l2 2 cos 2 ( 2 ) &&1 = 1

(24)

( m l + m l + m l + m l + m l )&& + ( m l + m l )&& 2 ( m l l + m l l ) sen ( )& &

( m l l + m l l ) sen ( ) & + ( m l l + m l l ) sen ( ) cos ( + ) & + ( m + m ) l cos ( ) sen ( ) &
+ ( m l l + m l l ) cos ( ) && + ( m l l + m l l ) cos ( ) sen ( + )& + ( m + m ) gl cos
+ m l cos sen & + ( m l + m l ) cos ( + ) sen ( + ) & + 2 ( m l l + m l l ) cos ( ) &&
+ I (&& + && ) + ( m l + m l ) g cos ( + ) + m gl cos + I && =
2

2 c2

3 c3

f 3

3 2

f 2

3 c3

f 3

3 2 c3

3 2 c3

f 2 3

3 2 c3

f 2 3

2 c2

2
2 1

3 c3

3 2 c3

f 2 3

3 2 c3

f 2 3

3 c3

f 3

f 3

c2

2
1

2
1

2 2

f 2 3

2
1

2 3

3 2 c3

2
1

f 2 3

(25)
m3lc 32&&3 + m3lc 32&&2 m3l2 lc 3 sen ( 3 ) &2&3 + m f l32&&3 + m f l32&&2 m f l2 l3 sen (3 )&2&3 + m f l2 l3 cos ( 3 ) &&2
+ m3l2 lc 3 cos ( 2 ) sen ( 2 + 3 ) &12 + m3l2 lc 3 cos (3 ) &&2 + m3lc 32 cos ( 2 + 3 ) sen ( 2 + 3 ) &12 + m f gl3 cos ( 2 + 3 )

(
+ m l l sen ( ) & (&

)
+ & ) + m gl

+ m3l2 lc 3 sen ( 3 ) &2 &2 + &3 + m f l2 l3 cos ( 2 ) sen ( 2 + 3 ) &12 + m f l32 cos ( 2 + 3 ) sen ( 2 + 3 ) &12
f 2 3

c3

cos ( 2 + 3 ) + I 3 &&2 + &&3 = 3

(26)
Las Ec. (24), (25) y (26) forman el modelo dinmico del robot manipulador MOTOMAN
HP3. El modelo formado por las ecuaciones anteriores puede descomponerse en la siguiente
forma:

M ( ) && + C , & & + g ( ) =

(27)

A continuacin se enlistan los elementos que componen a las matrices de la Ec. (27).

M 11 ( ) = m2 lc 2 2 cos 2 2 + 2m3l2 lc 3 cos 2 cos ( 2 + 3 ) + ( m3lc 32 + m f l32 ) cos 2 ( 2 + 3 ) + ( m3 + m f ) l2 2 cos 2 ( 2 )

+2m f l2 l3 cos 2 cos ( 2 + 3 ) + I1 + I 2 + I 3

(28)

M 12 ( ) = 0

(29)

M 13 ( ) = 0

(30)

M 21 ( ) = 0

(31)

M 22 ( ) = m2 lc 2 2 + I 2 + m3lc 32 + m3l2 2 + 2m3l2 lc 3 cos 3 + I 3 + m f l32 + m f l2 2 + 2m f l2 l3 cos ( 3 )

(32)

M 23 ( ) = m3lc 3 2 + m3l2 lc 3 cos 3 + I 3 + m f l32 + m f l2 l3 cos 3

(33)

M 31 ( ) = 0

(34)

M 32 ( ) = m3lc 32 + m3l2 lc 3 cos 3 + m f l32 + m f l2 l3 cos 3 + I 3

(35)

M 33 ( ) = m3lc 3 2 + m f l32 + I 3

(36)

C11 ,& = ( m3l2 lc 3 + m f l2 l3 ) sen 2 cos ( 2 + 3 ) &2 ( m3l2 lc 3 + m f l2 l3 ) cos 2 sen ( 2 + 3 ) &2 + &3

( m3lc 32 + m f l32 ) sen ( 2 + 3 ) cos ( 2 + 3 ) &2 + &3 ( m3 + m f ) l2 2 sen 2 cos 2&2 m2 lc 2 2 cos 2 sen 2&2

C12 , & = ( m3 + m f ) l2 2 sen 2 cos 2&1 ( m3l2 lc 3 + m f l2 l3 ) sen 2 cos ( 2 + 3 ) &1 m2 lc 2 2 cos 2 sen 2&1
( m3l2 lc 3 + m f l2 l3 ) cos 2 sen ( 2 + 3 ) &1 ( m3lc 32 + m f l32 ) sen ( 2 + 3 ) cos ( 2 + 3 ) &1

(37)

(38)

( )

C13 , & = ( m3l2 lc 3 + m f l2 l3 ) cos 2 sen ( 2 + 3 ) &1 ( m3lc 32 + m f l32 ) sen ( 2 + 3 ) cos ( 2 + 3 )&1

(39)

( )

C21 , & = m2 lc 2 2 cos 2 sen 2&1 ( m3l2 lc 3 + m f l2 l3 ) cos ( 2 ) sen ( 2 + 3 ) &1 ( m3 + m f ) l2 2 cos ( 2 ) sen ( 2 ) &1
( m3lc 32 + m f l32 ) cos ( 2 + 3 ) sen ( 2 + 3 ) &1 ( m3l2 lc 3 + m f l2 l3 ) sen ( 2 ) cos ( 2 + 3 ) &1

(40)

C22 , & = ( m3l2 lc 3 + m f l2 l3 ) sen ( 3 ) &3

(41)

C23 , & = ( m3l2 lc 3 + m f l2 l3 ) sen (3 ) &2 ( m3l2 lc 3 + m f l2 l3 ) sen (3 ) &3

(42)

C31 , & = ( m3l2 lc 3 + m f l2 l3 ) cos ( 2 ) sen ( 2 + 3 ) &1 + ( m3lc 32 + m f l32 ) cos ( 2 + 3 ) sen ( 2 + 3 ) &1

(43)

( )

( )

C32 ,& = ( m3l2 lc 3 + m f l2 l3 ) sen (3 ) &2 + &3

( )

C33 , & = ( m3l2 lc 3 + m f l2 l3 ) sen ( 3 ) &2

(45)

g1 ( ) = 0

(46)

g 2 ( ) = m2 glc 2 cos 2 + ( m3 + m f ) gl2 cos 2 + ( m3lc 3 + m f l3 ) g cos ( 2 + 3 )

(47)

g3 ( ) = ( m3lc 3 + m f l3 ) g cos ( 2 + 3 )

(48)

UNIDADES Y NOMENCLATURA
mi
li
lci
Ii

i
di
ai

i
Ci
Si
i 1

Ai

T
px

py
pz
r
&

( )

masa del i-simo eslabn (Kg)

longitud del i-simo eslabn (m)
distancia del centro de masas al eje de giro (m)
momento de inercia del i-simo eslabn (Kg m2)
ngulo de giro de la i-sima articulacin (rad)
parmetro de DH para el desplazamiento sobre el eje Z (m)
parmetro de DH para el desplazamiento sobre el eje X (m)
parmetro de DH para el giro alrededor del eje X (m)
coseno del ngulo de desplazamiento de la i-sima articulacin (adimensional)
seno del ngulo de desplazamiento de la i-sima articulacin (adimensional)
matriz de transformacin del i-simo eslabn (adimensional)
matriz de transformacin de la cadena cinemtica completa (adimensional)
coordenada sobre el eje X de la ubicacin del efector final (m)
coordenada sobre el eje Y de la ubicacin del efector final (m)
coordenada sobre el eje Z de la ubicacin del efector final (m)
proyeccin del segundo y tercer eslabn sobre el plano XY (m)
i-sima velocidad articular (rad/s)

K i &,

energa cintica del i-simo eslabn (J)

U i ( )

energa potencial del i-simo eslabn (J)

L &,

Lagrangiano del robot manipulador (J)

vector de pares de entrada (N m)

( )

(44)

&&i

&
&&

M ( )

i-sima aceleracin articular (rad/s2)

vector de coordenadas articulares (adimensional)
vector de velocidades articulares (adimensional)
vector de aceleraciones articulares (adimensional)
matriz de elementos de inercia del robot manipulador (adimensional)

C , &

matriz de elementos de fuerzas centrfugas y de coriolis del robot manipulador

( )

(adimensional)
g ( )
vector de pares gravitacionales del robot manipulador (adimensional)
q (t )

n
M ij ( )

vector de trayectorias deseadas (rad)

nmero de grados de libertad (adimensional)
elemento del i-simo rengln e i-sima columna de la matriz de inercias

(adimensional)
Cij ( , & )
elemento del i-simo rengln e i-sima columna de la matriz de fuerzas
gi ( )

centrifugas y de coriolis (adimensional)

i-simo elemento del vector de pares gravitacionales (adimensional)