t6 Dinamica Ocw - Revision

TEMA 6.
DINÁMICA DE ROBOTS Y
CONTROL
ROBÓTICA
ÍNDICE
 DINÁMICA
 INTRODUCCIÓN
 FORMULACIÓN LAGRANCE-EULER
 CONTROL CINEMÁTICO
 INTRODUCCIÓN
Ó
 GENERACIÓN TRAYECTORIAS
 CONTROL CINEMÁTICO DE MOVIMIENTO
2 Jon Legarreta / Raquel Martinez

DINÁMICA - INTRODUCCIÓN
La dinámica se ocupa de la relación entre las fuerzas que actúan sobre un

cuerpo y el movimiento que éstas generan. En el caso del modelo dinámico de
un robot, se trataría de conocer la relación entre el movimiento del brazo del
robot y las fuerzas que se originan.
originan Matemáticamente relacionaría:
1) Las variable articulares y sus derivadas (velocidad, aceleración).

2) Las fuerzas y los momentos aplicados a las articulaciones.
articulaciones
3) Los parámetros propios del robot (longitud, masa e inercias).
El modelo dinámico para el caso de robots de uno o dos grados de libertad no

es muy complejo, pero el modelado de robots con mas grados de libertad
aumenta en complejidad y es necesario el empleo de métodos
computacionales para resolverlo.

DINÁMICA - INTRODUCCIÓN
Obtención del modelo dinámico
Formulación Lagrange-Euler Formulación Newton-Euler
Se basa en el balance de energía a Se basa en efectuar un balance

través del lagrangiano. Permite de fuerzas y momentos o pares
describir la dinámica del robot existentes Para ello,
existentes. ello se formulan
considerándolo como una caja las ecuaciones que describen el
negra y teniendo en cuenta la movimiento lineal y angular de
energía almacenada en términos cada eslabón del robot.
d energía
de í cinética
i éti y potencial.
t i l
 
Lagrangiano L(q, q )  K (q, q )  U ( q ) Newton Euler
 
d  L  L
 F  mv  T  I w  w x ( I w)
Ecuación de movimiento  
dt   q  q
 

DINÁMICA - FORMULACIÓN DE LAGRANGE
Modelo eslabón
EJEMPLO 1: Modelo eslabón con masa concentrada con masa concentrada
qi= coord. articulaciones L

d  L  L
=vector de fuerzas y pares
  siendo
aplicados en las q1
dt   q  q L=lagrangiano
g g 
  K=energía cinética 
U=energía potencial
  1  2
L ( q, q )  K ( q, q )  U ( q ) L  K  U  ML   M g L sen
2
2
1er paso: Energía cinética (K) L Ecuación de movimiento
  MgLM L cos 
1  2
q 
K  I ML   M g L cos   
2
2 L 

 ML  2
I  ML2 q
2º paso: Energía potencial (U)  
d  L  

dt   q  ML 
2
U  Mgh  M g L sen
 

EJEMPLO 2 : Robot plano de 2 grados de libertad x2
d  L  L y0
0 y2
  L  K U q2
dt   q  q
  n
1er paso clculo de la Energía cinética (K) K   Ki L1 yy1 x1

L2
11 Lc2
La energía cinética es la suma de las energías cinéticas de los eslabones del robot. Lc1
La energía cinética de un eslabón se compone de dos términos, el primero debido a
la velocidad lineal, y el segundo a la velocidad angular:
q1
m1= masa eslabón i
1 T 1 x0
K i  mi vi vi  iT I i i vi=velocidad lineal del CM del eslabón i con respecto al CM
2 2 Ii=tensor de inercia del CM del eslabón i con respecto al CM
 v   J (q )   Recordando:
    J (q )  q
v
     tema 5 Jacobiano
Nota: mientras que la velocidad angular es la misma en todos los puntos del eslabón, no ocurre lo mismo con la
velocidad lineal que es distinta en cada punto,
punto por ello se considera la velocidad del centro de masas.
masas

x2
 v   J (q )   Recordando:
    J (q )  q
v
y0
0 y2
   
 tema 5 Jacobiano q2
1 T 1
Ki  mi vi vi  iT I i
i L1 yy1 x1
L2
2 2 Lc2
Lc1
Generalizando para n eslabones:

q1
K
1

n
2 i 1
m
i [ J vi q   q
 ]
T
 J vi q  q  [ J wi q   q ]T  I i J wi q   q  x0
[ J wi q   q ]T  q T J wi q 
T
1 T n
2 i 1
T

K  q  mi  J vi q   J vi q   J wi q   I i J wi q  q
T
 matricialmente: K
1 T
q D(q ) q
2

EJEMPLO 2 : Robot plano de 2 grados de libertad
1er paso cálculo de la Energía cinética (K) y0
0
1 T 1
Eslabón 1: K1  m1 v1 v1  1T I 1
1
2 2 L1
Calculamos inicialmente la MTH del primer eslabón. y1
Lc1
x1
 Cq 1  Sq 1 0 L c 1 Cq 1  x  Lc1Cq1
 Sq
S 1 Cq 1
C 0 S 1 
L c 1 Sq y  Lc1Sq
S 1
q1
0
A   x0
1
 0 0 1 0  z0 i di ai i
 
 0 0 0 1  1 q1 0 Lc1 0º
Haciendo uso del jacobiano,
jacobiano este nos queda de la siguiente manera:
La velocidad lineal del
eslabón 1 seria:
 x x x 
    Lc1Sq 

  S 1 q 1
 q1 q 2 q3   L Sq1 0 0
   Lc1Sq1 0 0 q1   v   J (q )  
    J (q )  q

y y y   c1 v1   Lc1Cq1 0 0  0    Lc1Cq1 q 1   v1
v
J   Lc1Cq1 0 0    
 q1 q 2 q3        
 z   0 0  0 0 0  0   0 
 z z   0    
 q1 q 2 q3 

1 T 1
Energía cinética K1 m1 v1 v1  1T I 1 1
2 2
L1
 
 y1
  L c1 Sq 1 q 1 Lc1
T   
 
 2

2
x1
v1 v1 
  Lc1 Sq1 q 1 Lc1Cq1 q 1 0   Lc1 Cq 1 q 1

 Lc1 q 1
q1
 0 
  x0
La velocidad angular del eslabón 1 seria:

  
w1  q1 z0
Por lo que la energía cinética del eslabón 1:
1 2

2 1
K 1  m 1 L c1 q 1  I
2
1
q
 1
2 2

0 y2
2
q2
1 T 1
Eslabón 2: K2  m2 v2 v2   2T I 2
2
2 2 L1 yy1 x1
Lc2 L2
Calculamos inicialmente la MTH del segundo eslabón Lc1
 Sq  Sq
q1
 Cq
C 1 S 1 0 C 1   Cq
L 1 Cq C 2 S 2 0 C 2
L c 2 Cq
 Sq 1 Cq 1 0  
L 1 Sq 1   Sq 2 Cq 2 0 L c 2 Sq 2  x0
0
A  0
A 21 A   i di ai i
2 1
 0 0 1 0  0 0 1 0 
   1 q1 0 L1 0º
 0 0 0 1  0 0 0 1 
2 q2 0 Lc2 0º
 Cq 1 Cq 2  Sq 1 Sq 2  Cq 1 Sq 2  Sq 1 Cq 2 0 Cq 1 L c 2 Cq 2  Sq 1 L c 2 Sq 2  L 1 Cq 1  =x
 Sq 1 Cq 2  Cq 1 Cq 2  Sq 1 Sq 2  Cq 1 Cq 2 0 Sq 1 L c 2 Cq 2  Cq 1 L c 2 Sq 2  L 1 Sq 1  =y
0
A  10 A2 A  
1
2
 0 0 1 0  =z
 
 0 0 0 1 

0 y2
2
q2
1 T 1
Eslabón 2: K2  m2 v2 v2   2T I 2
2
2 2 L1 yy1 x1
recordatorio L2
Lc2
Haciendo uso del jacobiano, nos S (q1  q 2)  Sq1Cq 2  Cq1Sq 2
Lc1
queda de la siguiente manera: C (q1  q 2)  Cq1Cq 2  Sq1Sq 2
 x x x  q1
  x0
 q1 q2 q3   L Sq1  L [ S (q1  q 2)]  L [ S (q1  q 2)] 0
1 c2 c2
y y y  
J    L1Cq1  Lc 2 [C (q1  q 2)] Lc 2 [C (q1  q 2)] 0
 q1 q2 q3 
 z  0
 z z   0 0
 q1 q2 q3 
   ( L Sq1  L [ S (q1  q 2)]) q1 L [ S (q1  q 2)] q2

 L1Sq1  Lc 2 [ S (q1  q 2)]  Lc 2 [ S (q1  q 2)] 0  q1  1 c2 c2 
  
 
v2   L1Cq1  Lc 2 [C (q1  q 2)] Lc 2 [C (q1  q 2)] 0 q 2  ( L1Cq1  Lc 2 [C (q1  q 2)] )q1 Lc 2 [C (q1  q 2)] q 2   v2
  
   
 0 0 0  0   0 
   

1er paso cálculo de la Energía cinética (K)
1 T 1
Eslabón 2: K2  m2 v2 v2   2T I 2
2
2 2
  

 (  L1 Sq1  Lc2 [ S ( q1  q 2)]) q 1 Lc2 [ S ( q1  q 2)] q 2
    
  
v2 v2  ( L1Sq1  Lc 2 [ S (q1  q 2)]) q1 Lc 2 [ S (q1  q 2)] q 2 ( L1Cq1  Lc 2 [C (q1  q 2)]) q1 Lc 2 [C (q1  q 2)] q 2 0  ( L1Cq1  Lc 2 [C (q1  q 2)]) q1 Lc 2 [C (q1  q 2)] q 2 
T
  
 0 
 
La velocidad angular del eslabón 2 seria:
   
w 2  (q1  q 2 ) z0
La energía cinética queda finalmente:
K2 
1
2

2 2 2 1
m2 L1 q1  Lc 2 (q1  q 2 ) 2  2 L1 Lc 2 (q1  q1q 2 ) cos q2  I 2 (q1  q 2 ) 2
2


2º paso cálculo de la Energía Potencial (U)

Para el calculo de la energía potencial, se deben considerar cada una de las energías potenciales almacenadas
en cada eslabón
eslabón, esto es:
mi= masa eslabón i
n
U  U i
g= vector de gravedad
U i  mi g T pci pci=vector que localiza el CM del eslabón i respecto del sistema referencia
11 del mundo
U  U (q ) y0
q2
h1
L1 g
U1  m1 g Lc1sen q1 y1
Lc1 h2
h2 x1
q1 g h1
U 2  m2 g [ L1sen q1  Lc 2 sen(q1  q2 )] x0

3º y 4º paso cálculo de Lagrangiano y ecuación de movimiento

1 T 1 n n
  El lagrangiano
L(q, q)  q D(q) q  U (q )   d ij (q )  qi  q j  U (q )
L(q, q )  K (q, q )  U ( q ) en forma matricial :
2 2 i 1 j 1
Ecuación de movimiento :
d  L  L
 
L n dt   q  q
  d kj (q)  q j  
q k j 1 L 1 n n d ij U
  qi  q j 
qk 2 i 1 j 1 qk qk
d  L  n n
d
    d kj (q )  q j   d kj (q )  q j 
d  q k
dt  j 1 j 1 dt
d n n n
 d kj 1 d ij  U
 d kj ( q )  q
 j   
i 1 j 1  qi

2 qk
qi  q j 
qk
k
n n n d kj
j 1 
  d kj (q)  q j   qi  q j n n n
q j
j 1 i 1 j 1
d
j 1
kj (q)  q j   cijk (q) qi  q j   (q)   k
i 1 j 1
Forma matricial D(q)  q  C (q, q )  q  g (q)  

Inercial Coriolis gravedad

3º paso cálculo de Lagrangiano L ( q, q )  K  U
 1 T
L ( q, q )  q D(q) q  U (q)
2
 d11 d12   q1  1

1 T 1
K  q D(q) q  q1 q 2   
      q1d11  q 2 d 21 q1d12  q 2 d 22       q1 
2 2 d 21 d 22  q2  2  q2 
1

 d11q1q1  d 21q1q 2  d12 q1q 2  d 22 q 2 q 2
2
 1
K  d 11 q 1 q 1  2  d 21 q 1 q 2  d 22 q 2 q 2 
d12  d 21 2
 1 T 1 n n
L(q, q)  q D(q) q  U (q)   d ijj (q)  qi  q j  U (q)
2 2 i 1 j 1
Jon Legarreta / Raquel Martinez


3º paso cálculo de Lagrangiano L ( q, q )  K  U
Teniendo en cuenta las ecuaciones obtenidas 1 T 1 n n

anteriormente: L(q, q)  q D(q) q  U (q)   d ij (q)  qi  q j  U (q)

2 2 i 1 j 1
L ( q , q )  K1  K 2  U 1  U 2
1 1
 

L
2 2 1 1
m1 Lc1 q1  I 1 q12 m2 L12 q12  Lc 2 2 ( q1  q 2 ) 2  2 L1 Lc 2 (q12  q1q 2 ) cos q2  I (q1  q 2 ) 2 m1 g Lc1sen q1  m2 g [ L1sen q1  Lc 2 sen(q1  q2 )]
2 2 2 2 2
d11 d11 d11 d11
d11 d11
d22 d12=d
d21 d22
d12=d1 d12=d12
2
Como se puede observar en la fórmula, los términos dij: solo aparecen en la energía cinética.
Los términos donde aparece d11 es porque el término q1 está elevado al cuadrado (q1 * q1 ) y aparece dos veces
Los términos donde aparece d22 es porque el término q2 está elevado al cuadrado (q2 * q2 ) y aparece dos veces
Los términos donde aparece d12 o d21 es porque

porq e el término q1 y q2

4º paso cálculo de la ecuación de movimiento
d11 1 d11
c111   0
Forma matricial D(q)  q  C (q, q )  q  g (q)   q1 2 q1
Inercial Coriolis gravedad
g d12 1 d12
c121   0
n n n
q1 2 q1
d kj (q)  q j   cijk (q) qi  q j  k (q)   k c112 
d 21 1 d11
  m2 L1 Lc 2 sen q2
j 1 i 1 j 1 q1 2 q2
d 22 1 d12 1
c122     m2 L1 Lc 2 sen q2
Teniendo en cuenta las ecuaciones obtenidas anteriormente, se q1 2 q2 2
calculan los coeficientes cijk (k=1,2): d11 1 d 21
c211    2m2 L1 Lc 2 sen q2
2 2 2 q2 2 q1
d11  m1Lc1  I1  m2L1  m2Lc2  2m2L1Lc2 cosq2  I2
d12 1 d 22
c221    m2 L1 Lc 2 sen q2
2 q2 2 q1
d12  d21  2 m2Lc2  2 m2L1Lc2 cosq2  2 I2  d kj 1 d ij 
cijk    
c212 
d 21 1 d 21

1
  m2 L1 Lc 2 sen q2

 i 2 qk
2
d12  d21  m L  m2L1Lc2 cosq2  I2
2 c2
q  q2 2 q2 2
d 22 1 d 22
2
d22  m2Lc2  I2 c222   0
q2 2 q2

4º paso cálculo de la ecuación de movimiento
U   m1 g Lc1sen q1  m2 g [ L1sen q1  Lc 2 sen(q1  q2 )]
U
El término de gravedad: 1    m1 g Lc1 cos q1  m2 g L1 cos q1  m2 gLc 2 cos( q1  q2 )]
q1
U
2    m2 gLc 2 cos( q1  q2 )
q2
n n n
d
j 1
kj (q)  q j   cijk (q) qi  q j  k (q)   k
i 1 j 1
K=1 K=2
n n n n n n
d
j 1
1j (q)  q j   cij1 (q) qi  q j  1 (q)   1
i 1 j 1
d
j 1
2j (q)  q j   cij 2 (q) qi  q j  2 (q)   2
i 1 j 1
Solución a las ecuaciones de movimiento

 1  d11  q1  d12 q2  c111 q12  (c121  c211 )q1  q 2  c221  q 2 2  1
 2  d 21  q1  d 22 q2  c112 q12  (c122  c212 )q1  q 2  c222  q 2 2  2

CONTROL CINEMÁTICO - INTRODUCCIÓN
El control cinemático se basa en el modelo cinemático del robot y utiliza
posiciones
i i y velocidades.
l id d
OBJETIVO:
1) Establecer cuales son las trayectorias que debe

seguir cada articulación del robot a lo largo del
tiempo para conseguir los objetivos fijados por el
usuario:
– Punto de destino.
– Tipo de trayectoria del extremo.
– Tiempo invertido.
invertido DESTINO
– etc.
2) Es necesario atender a las restricciones físicas de

los accionamientos y criterios de calidad (suavidad,
precisión, ...) ORIGEN

CONTROL CINEMÁTICO - INTRODUCCIÓN
Esquema de Control de Robots
USUARIO Planificación tareas

MPUTADOR
Planificación de trayectorias
SENSORES
Ejecución de trayectorias
COM
Generación de trayectorias
DESTINO
CONTROLADOR DE LOS
MOTORES PARA CADA UNO
DE LOS EJES DEL ROBOT ENTORNO
ORIGEN

CONTROL CINEM.
CINEM - GENERACIÓN TRAYECTORIAS
Para p
poder controlar el movimiento de un robot,, es necesario generar
g
previamente una trayectoria en el espacio para que este lo realice. Esta
trayectoria puede realizarse de dos maneras: q(t)
1) Generación de trayectoria en el espacio articular (qi). Desplazar qf
el brazo de un punto a otro en un tiempo determinado, sin importar
la trayectoria en concreto.
qi
tf t
2) Generación de trayectoria en el espacio cartesiano. Cuando es ti=0
necesario que el robot siga una determinada trayectoria (ejemplo
un línea de soldadura).

CONTROL CINEM.
Movimiento eje a eje
Trayectorias punto a punto Movimiento de cada articulación se

produce de manera consecutiva.
Tiempos
p de ejecución
j altos.
Movimiento de cada articulación
de manera independiente al resto
Movimiento simultaneo de ejes
Movimiento de cada articulación se
Tipos de trayectorias produce de manera simultanea, pero
en el espacio articular cada articulación alcanzara su posición
final en tiempos distintos dependiendo
de las velocidades asignadas etc.. Se
producen
d D
Desgastes
t i
innecesarios
i all
exigir máximas velocidades
Trayectorias
y coordinadas o isócronas
Movimiento de cada articulación se produce de manera simultanea. Todas las
articulaciones comenzaran a moverse al mismo tiempo y alcanzaran también su
posición final simultáneamente, ajustando para ello las velocidades de giro de cada
articulación .

CONTROL CINEM.
Trayectorias en el espacio articular
1) Conversión de los puntos cartesianos inicial, final e intermedios a valores

articulares para cada articulación (cinemática inversa, tema 5).
2) Interpolación de los valores de las articulares de cada articulación con o sin
puntos intermedios.
3) Movimiento de cada articulación de forma independiente siguiendo la
trayectoria elegida.
I t
Interpolación
l ió trayectoria articular
Cúbica Lineal con ajuste Puntos inicio y fin Puntos intermedios

parabólico

CONTROL CINEM.
Interpolación de trayectoria articular con puntos inicio y fin
Se considera solo los puntos inicio y final, no se consideran puntos intermedios, por lo que
las posibles trayectorias son diversas (ver figura). Para producir movimientos suaves es
necesario imponer 4 condiciones de contorno:
q (tini )  q (0)  qini Aseguran que comienza

A i y acaba
b
q( )
q(t)
en los puntos adecuados. qf
q (t fin )  q fin
qi
q (tini )  q (0)  0 Aseguran que las velocidades tf t

ti=0
de inicio y final son nulas
nulas.
q (t fin )  0

CONTROL CINEM.
Interpolación CÚBICA
Las 4 condiciones se puede cumplir con un polinomio

de grado 3 del tipo:
q (t )  a  b t  c t 2  d t 3
q (tini )  q (0)  qini a  qini

derivando
q (t fin )  q fin q (t fin )  a  b t fin  c t 2fin  d t 3fin  q fin
q (t )  b  2c t  3 d t 2
q (tini )  q (0)  0 q (tini )  q (0)  b  b  0
q (t fin )  0 q (t fin )  2 c t fin  3 d t 2fin  0

CONTROL CINEM.
q(t)
qfin
q (t fin )  a  b t fin  c t 2fin  d t 3fin  q fin

a  qini b0 qini
3q fin  qini 

ti=0 tfin t
qini
i i  c t 2
fi
fin  d t fi  q fin
3
fin fi c
t 2fin
2 c t fin  3 d t 2fin  0  2q fin  qini 

d
t 3fin

CONTROL CINEM.
Ejemplo:
Consideremos un robot de una articulación rotacional que inicialmente se encuentra girada
5º. Calcular la interpolación cubica que haga que el robot gire 20º en 2 s.
a  qini  5º t=2
b0
3q fin  qini 
20º
20 t=0
325  5 5º
c   15
t 2fin 4
 2q fin  qini   225  5 q (t )  5  15t 2  5 t 3
d   5
t 3fin 8
CONTROL CINEM.
Interpolación lineal con ajuste PARABÓLICO
Un segundo método de interpolación es realizar un ajuste lineal entre los puntos inicio y fin. fin
Sin embargo, con este tipo de ajuste la velocidad de la articulación permanece constante
en todo el intervalo de tiempo, lo que lleva a aceleraciones infinitas al principio y al final.
q(t) lineal
Para evitar este inconveniente, se realiza la trayectoria en 3
tramos, el primero parabólico, el intermedio lineal y el final qfin
nuevamente parabólico. El primer tramo permite alcanzar una qfin-p
fin p
velocidad deseada con aceleración constante. En el segundo

qm
tramo lineal se mantiene la velocidad constante (aceleración
cero). Finalmente en el ultimo tramo se aplica una deceleración qp
constante
t t hasta
h t alcanzar
l l velocidad
la l id d nula
l en ell punto
t qfin. qini
tp tm tfin-p tfin t
tini=0
parabolico

CONTROL CINEM.
I tramo parabólico: t  ( 0 , tp )
derivando
Ecuación: qI (t )  aI  bI t  cI t 2 derivando
q I (t )  bI  2cI t qI (t )  2cI
Condiciones iniciales:
qI (tini )  q I (0)  qini q I (tini )  q I (0)  0 q  cte
q(t)
q
qfin
aI  qini bI  0 cI 
qfin-p 2
qm La ecuación del I tramo parabólico es:
qp
qini q 2
qI (t )  qini  t t  ( 0 , tp )
tini=0
tp tm tfin
fin-p
p
tfin
2
CONTROL CINEM.
II tramo LINEAL: t  (tp , tfin-p

fin p )
Velocidad de articulación = velocidad en tp del tramo I
q II (t )  
derivando
Ecuación: q II (t )     ( t  t p ) q I (t p )  q t p
qI (t p )  q II (t p )  q p
q 2
qI (t p )  q p  qini  t p   qp   q t p
2
q(t))
q( qII (t p )  
2
qfin
qm  q p q fin  qini  2 q p q fin  qini  q t p
qfin-p
tg    q t p  
tm  t p t fin  t ini  2 t p t fin  t ini  2 t p
qm
qp  t fin  t ini
tm  2
qini 2 q2 t m  q( q fin  qini )
q ffin  qini t p  tm 
tp tm tfin-p tfin
fi qm  q
tini=0 2

CONTROL CINEM.
q(t)
qfin
qfin-p
III tramo parabólico: t  (tfin-p
fin p , tfin )
qm
Ecuación: qIII (t )  a III  bIII t  cIII t 2
qp
Derivando: qini
q III (t )  bIII  2cIII t tini=0

tp tm tfin-p tfin
q III (t fin )  0 º / s  0  bIII  2cIII t fin  bIII  2cIII t fin q III (t ffin )  q ffin  a III  bIII t ffin  cIII t ffin
2
Velocidad tramo II en tfin-p= velocidad en tfin-p del tramo III
q III (t fin  t p )  q t p  q t p  bIII  2cIII (t fin  t p ) q

a III  q fin  t 2fin
2
q
c III   bIII  q t fin
2

CONTROL CINEM.
Ejemplo:
Consideremos un robot de una articulación rotacional que inicialmente se encuentra girada 5º y con una
aceleración de 40º/s2 . Calcular la interpolación lineal con ajuste parabólico que haga que el robot gire 20º
en 2 s.
q
40 212  40 ( 25  5)   q t p  40  0.293  11.71 º / s a III  q fin  t 2fin  55
Datos: tp  1  0.293 s 2
40
q p    5  20 t p  6.716º
2
q
t fin  2 s c III  
2
 20
tm  1 s q(t) bIII  q t fin  80
qini  5º 25 q
23.27 qI (t )  qini  t 2  5  20 t 2 t  ( 0 , 0.293 s )
2
q fin  25º 15
qII (t )     ( t  t p )  6.716  11.71(t  0.293) 0.293 < t < 1.707
qm  15 º qp
5
q  40º / s 2 0.293 1 1.707 2 q III (t )  a III  bIII t  cIII t 2  55  80 t  20 t 2 1 707 < t < 2
1.707
tini=0

CONTROL CINEM.
Interpolación de trayectoria articular con puntos INTERMEDIOS
Los valores de las velocidades intermedias se tienen que indicar por el usuario. Generalmente, si las
pendientes de los tramos consecutivos son de signo contrario, las velocidades se toman nulas. En
cambio si la pendientes no cambian de signo, se toma la media de las dos velocidades de cada tramo.
Ejemplo:
Velocidad punto a= media de las velocidades tramo I y II
Velocidad punto b= 0, pendientes de signo contrario
Velocidad punto c= media de las velocidades tramo III y IV
q(t)
(t)
b
c
a fin
ini
I II III IV
ti=0 t
CONTROL CINEM.
En ocasiones es necesario definir puntos intermedios en las trayectorias a realizar por cada
articulación. En principio se podrían utilizar los métodos vistos anteriormente, tomando cada punto de
paso como el fin de un tramo y principio de otro; el único inconveniente es que obliga a que en esos
puntos la velocidad sea cero,
cero cuando en realidad no es necesario.
necesario
q(t )  a  b t  c t 2  d t 3 derivando
q (t )  b  2c t  3 d t 2
las velocidades:
q (tini )  q (0)  qini a  qini q (tini )  q (0)  qini
q (tini )  q (0)  b  b  qini
q (t fin )  q fin q (t fin )  q fin
3(q fin  qini ) 2qini  q fin

c 
q (t fin )  qini  qini t fin  c t 2fin  d t 3fin  q fin t 2fin t fin
 2(q fin  qini ) q fin  qini
q (t fin )  qini  2 c t fin  3 d t 2 fin  q fin d 
t 3fin t 2fin

CONTROL CINEM.
Interpolación CÚBICA t=2
20º
Ejemplo:
Consideremos un robot de una articulación rotacional qque inicialmente se 15º
encuentra girada 5º y acaba en 20º en 2 s. Calcular las interpolaciones

cubicas a realizar de forma que pase por un punto intermedio en el que el t=0
5º
ángulo de giro sea 15º y con una velocidad de 5º/s. Se consideran las
velocidades iniciales y finales cero.
Tramo I Tramo II
qI (t )  aI  bI t  cI t 2  d I t 3 q II ( t )  a II  b II t  c II t 2  d II t 3
aI  qini  5º aII  qII ini  15º

bI  qini  0 bII  q II ini  5
3(q fin  qini ) 2qini  q fin 3(15  5) 2  0  5 3(q fin  qII ini ) 2q II ini  q fin 3(15  15) 2  5  0
cI      25 cI     5
t 2fin t fin 12 1 t 2fin t fin 12 1
 2(q fin  qini ) q fin  qini  2(15  5) 5  0  2(q fin  qII ini ) q fin  q II ini  2(20  15) 0  5
dI     2  15 dI  3
 2
  2  5
t 3
fin t 2
fin 13 1 t fin t fin 13 1

CONTROL CINEM.
En este tipo de interpolación y debido a que en los puntos intermedios las velocidades de paso son
distintas, los tramos parabólicos iniciales y finales en cada tramo no pueden ser simétricos como si lo
eran en el caso anterior (sin puntos intermedios).
q(t) tpn= intervalo de tiempo del ajuste
PARABOLICO en un qn
qfin
tpn-1 tn-1, n tpn tn,n+1 tpn+1
tn-1, n= intervalo de tiempo del ajuste
qini
LINEAL entre los puntos de paso qn-1 y qn
qn
Donde:
tp1
q n 1,n tpfin t n 1,n  (t n  t n 1 )  0.5 t pn  0.5t pn 1
q n , n 1
qn+1 Y la velocidad del tramo lineal es:
qn-1
qn  qn 1
q n 1, n 
t n  t n 1
tini=0 tn-1 tn tfin-tpfin tfin t

CONTROL CINEM.
q(t) Interpolación lineal con ajuste PARABÓLICO
qfin Datos requeridos para la interpolación

tpn-1 tn-1, n tpn tn,n+1 tpn+1 serian:
qini 1) Puntos de paso (q1 …)
2) Tiempo en esos puntos (tn …)
qn
3) Aceleración de los tramos
tp1
q n 1,n tpfin parabólicos que seria:
parabólicos,
q n ,n 1
qn+1
qn-1 qn  signo ( q n ,n 1  q n 1, n ) qn

Para tener determinada la trayectoria hay que calcular el tiempo transcurrido durante el tramo parabólico en
el que le punto se aproxima a un punto de paso:
q n ,n 1  q n 1,n
t ppn 
qn
CONTROL CINEM.
q(t)qfin
qini
qn Los tramos inicial y final

tp1
q n 1,n tpfin deben ser tramos parabólicos
q n ,n 1 completos esto modifica las
completos,
qn+1
expresiones anteriores:
qn-1

INICIAL
2 2 ( q 2  q1 )
t1, 2  t 2  t p1  0.5 t p 2 t p1  t 2  t 2 
q1
q2  q1
q1  signo
i ( q 2  q 1 ) q1 q1, 2 
t 2  0 . 5 t p1
CONTROL CINEM.
q(t)qfin
qini
qn Los tramos inicial y final

tp1
q n 1,n tpfin deben ser tramos parabólicos
q n ,n 1 completos esto modifica las
completos,
qn+1
expresiones anteriores:
qn-1

FINAL
2 ( q fin  q fin 1 )
t fin 1, fin  t fin  t fin 1  t pfin  0.5 t n 1 t p fin  (t fin  t fin 1 )  (t fin  t fin 1 ) 2 
q fin
q fin  q fin 1
q fin  signo
i (q q ) q fin q fin fi 
fi 1, fin
fin fin  1 (t fin  t fin 1 )  0.5 t pfin

CONTROL CINEM.
Ejemplo:
Consideremos un robot de una articulación rotacional que pase por los siguientes puntos articulares 5º,
5º
20º, 15º, 5º. Estos puntos se alcanzan en los tiempos 2, 3, 4 s y que la aceleración es de 30º/s2.
Calcular los parámetros del interpolador con ajuste parabólico necesario. Se consideran las velocidades
iniciales y finales cero.
I tramo
20º II
Aceleración tramo I parabólico:
15º q1  signo
g ( q 2  q 1 ) q1  30 º / s 2
I
2 2 ( q 2  q1 ) 2 ( 20  5)
10º t p1  t 2  t 2   2  22   0 .268 s
q1 30
III
5º Velocidad lineal tramo I:
q2  q1 20  5
q1, 2    8.04 º / s
t 2  0.5 t p1 2  0.5  0.268
tini=0 1 2 3 4 t

CONTROL CINEM.
Ejemplo:
5º
II tramo
20º II Aceleración tramo II parabólico:
15º q2  signo ( q 2 , 3  q 1 , 2 ) q2   30 º / s 2
I Velocidad lineal tramo II:

10º qn  qn 1 q  q2 15  20
q n 1, n   q 2 ,3  3   5º / s
III t n  t n 1 t3  t 2 3 2
5º q n ,n 1  q n 1, n q  q
t pn   t p 2  2 ,3 1, 2  0.435 s
qn q2
tini=0 1 2 3 4 t t1, 2  t 2  t p1  0.5 t p 2  2  0.268  0.5  0.435  1.514 s

CONTROL CINEM.
Ejemplo:
5º
20º Aceleración último tramo parabólico:

II
q fin  signo ( q fin q fin  1 ) q fin  signo ( q 4  q 3 ) q4   30 º / s 2
15º
2 ( q fin  q fin 1 )
I t p fin  (t fin  t fin 1 )  (t fin  t fin 1 ) 2 
q fin

10º
III 2 (5  15 )
t p 4  ( 4  3)  ( 4  3) 2   0 .423 s
 30
5º
Velocidad último tramo lineal:
q 4  q3  10
q3, 4    12.68 º / s
tini=0 1 2 3 4 t (t 4  t3 )  0.5 t p 4 1  0.5  0.423

CONTROL CINEM.
Ejemplo:
5º
20º Aceleración penúltimo tramo parabólico:

II
q3  signo ( q 3 , 4  q 2 , 3 ) q3   30 º / s 2
15º
q n , n 1  q n 1, n q  q 2 , 3  12.68  ( 5)
I t pn   t p 3  3, 4   0.256 s
qn q3  30
10º
III Tiempos del tercer y cuarto tramo lineal:
t n 1,n  (t n  t n 1 )  0.5 t pn  0.5t pn 1
5º
t 2,3  (t3  t 2 )  0.5 t p 3  0.5t p 2  0.654 s
tini=0 1 2 3 4 t t3, 4  (t 4  t3 )  0.5 t p 4  0.5t p 3  0.66 s

CONTROL CINEM.
CINEM - CONTROL CINEMÁTICO DEL MOVIMIENTO
Control del movimiento en el espacio articular

Para que el extremo del robot describa una trayectoria, es necesario que cada articulación siga una
trayectoria determinada. Por tanto, se puede controlar el movimiento del robot en el espacio articular
puesto que son conocidas las referencias de cada una de las trayectorias.
TIPOS DE CONTROL
Control desacoplado o monoarticular Control multiarticular
Se considera el modelo del robot compuesto

p por
p Se considera un modelo del robot en el q
que se
una superposición de articulaciones considera el modelo dinámico global del
independientes, sin tener en cuenta la interacción robot. Esto es, tiene en cuenta el
entre ellas. En este caso el modelo dinámico movimiento de todas las articulaciones
empleado es el de un accionador (motor eléctrico). entre si. Analíticamente la resolución del
La desventaja principal es la influencia del problema es mas compleja
movimiento de una articulación en las siguientes y
en consecuencia en el movimiento global.

CONTROL CINEM.
Control monoarticular
En este caso el planificador de trayectorias genera las correspondientes trayectorias para cada una de
las articulaciones por separado, obteniéndose en cada acaso los puntos de referencia necesarios para
generar el movimiento de cada articulación de forma independiente. Los accionadores mas utilizados
en Robótica son los motores de corriente continua.
El modelo dinámico de un motor de corriente continua y el diagrama de bloques seria el siguiente :
Motor
qd Motor q
+ error controlador control carga
g
engranajes
-
encoder
Inercia

CONTROL CINEM.

Control Multiarticular
Para realizar un control preciso del robot es necesario modelarlo como un sistema MIMO (Múltiple Input
Multiple Output).
Output) Teniendo en cuenta los vectores de posición,
posición velocidad y aceleración de las
articulaciones, el controlador enviara un vector de señales de control (ley de control) para accionar
cada uno de los motores articulares.
Recordando la ecuación de movimiento obtenida anteriormente: D(q)  q  C (q, q )  q  g (q)  
Inercia Coriolis gravedad
Y utilizando un controlador PD (Proporcional-Deribativo), la ley de control quedaría de la siguiente manera
para un robot de n grados de libertad:   K (q  q )  K ( q  q )  D( q )  q
  C (q, q )  q  g (q)
p d v d
Donde Kp y Kv son matrices nxn,

correspondiente al controlador PD.
qd Kp q
Diagrama de bloques del sistema del control: Kv q
q d

t6 Dinamica Ocw - Revision

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

t6 Dinamica Ocw - Revision

Uploaded by

Copyright:

Available Formats

TEMA 6.

2 Jon Legarreta / Raquel Martinez

La dinámica se ocupa de la relación entre las fuerzas que actúan sobre un

1) Las variable articulares y sus derivadas (velocidad, aceleración).

El modelo dinámico para el caso de robots de uno o dos grados de libertad no

3 Jon Legarreta / Raquel Martinez

Formulación Lagrange-Euler Formulación Newton-Euler

Se basa en el balance de energía a Se basa en efectuar un balance

4 Jon Legarreta / Raquel Martinez

qi= coord. articulaciones L

5 Jon Legarreta / Raquel Martinez

1er paso clculo de la Energía cinética (K) K   Ki L1 yy1 x1

6 Jon Legarreta / Raquel Martinez

Generalizando para n eslabones:

7 Jon Legarreta / Raquel Martinez

8 Jon Legarreta / Raquel Martinez

La velocidad angular del eslabón 1 seria:

Por lo que la energía cinética del eslabón 1:

9 Jon Legarreta / Raquel Martinez

Calculamos inicialmente la MTH del segundo eslabón Lc1

10 Jon Legarreta / Raquel Martinez

 q1 q2 q3 

   ( L Sq1  L [ S (q1  q 2)]) q1 L [ S (q1  q 2)] q2

11 Jon Legarreta / Raquel Martinez

La velocidad angular del eslabón 2 seria:

La energía cinética queda finalmente:

12 Jon Legarreta / Raquel Martinez

2º paso cálculo de la Energía Potencial (U)

13 Jon Legarreta / Raquel Martinez

3º y 4º paso cálculo de Lagrangiano y ecuación de movimiento

Forma matricial D(q)  q  C (q, q )  q  g (q)  

 d11 d12   q1  1

Jon Legarreta / Raquel Martinez

Teniendo en cuenta las ecuaciones obtenidas 1 T 1 n n

Los términos donde aparece d12 o d21 es porque

16 Jon Legarreta / Raquel Martinez

17 Jon Legarreta / Raquel Martinez

Solución a las ecuaciones de movimiento

18 Jon Legarreta / Raquel Martinez

1) Establecer cuales son las trayectorias que debe

2) Es necesario atender a las restricciones físicas de

19 Jon Legarreta / Raquel Martinez

USUARIO Planificación tareas

20 Jon Legarreta / Raquel Martinez

21 Jon Legarreta / Raquel Martinez

Movimiento eje a eje

Trayectorias punto a punto Movimiento de cada articulación se

22 Jon Legarreta / Raquel Martinez

1) Conversión de los puntos cartesianos inicial, final e intermedios a valores

Cúbica Lineal con ajuste Puntos inicio y fin Puntos intermedios

23 Jon Legarreta / Raquel Martinez

q (tini )  q (0)  qini Aseguran que comienza

q (tini )  q (0)  0 Aseguran que las velocidades tf t

24 Jon Legarreta / Raquel Martinez

Las 4 condiciones se puede cumplir con un polinomio

q (tini )  q (0)  qini a  qini

25 Jon Legarreta / Raquel Martinez

q (t fin )  a  b t fin  c t 2fin  d t 3fin  q fin

3q fin  qini 

2 c t fin  3 d t 2fin  0  2q fin  qini 

26 Jon Legarreta / Raquel Martinez

Interpolación lineal con ajuste PARABÓLICO

velocidad deseada con aceleración constante. En el segundo