DISEÑO DE DISEÑO DE UN ROBOT UN ROBOT

QUIRÚRGICO QUIRÚRGICO IIII

Dinámica inversaDinámica inversaDinámica directaDinámica directa

Selección de servoaccionamientosSelección de servoaccionamientosControl y SimulaciónControl y Simulación

DINÁMICADINÁMICA

ROBOT REALROBOT REAL

MÓDULOS

COMERCIALES

ROBOT REALROBOT REAL

CARTESIANO TORREBLANCA

DINÁMICADINÁMICA

DINÁMICADINÁMICA

NUESTRO ROBOTNUESTRO ROBOT

DINÁMICA INVERSADINÁMICA INVERSAfunctionfunction tau = newtoneuler5(q,qp,qpp,g,mext,Iext) tau = newtoneuler5(q,qp,qpp,g,mext,Iext)

CÓDIGO NEWTON-EULERCÓDIGO NEWTON-EULER

Masas Masas elementoselementos

M1= 5 kgM1= 5 kg

M2= 5 kgM2= 5 kg

M3= 5 kgM3= 5 kg

M4= 0 kgM4= 0 kg

M5= 7 kgM5= 7 kg

Rozamiento Rozamiento viscosoviscoso

B1= 0.06B1= 0.06

B2= 0.06B2= 0.06

B3= 0.06B3= 0.06

B4= 0.05B4= 0.05

B5= 0.05B5= 0.05

PosicionamientoPosicionamiento

cdmcdm

F1= -0.5F1= -0.5

F2= -0.5F2= -0.5

F3= -0.5F3= -0.5

F4= -0.5F4= -0.5

F5= -0.5F5= -0.5

DINÁMICA INVERSADINÁMICA INVERSA

CÓDIGO NEWTON-EULERCÓDIGO NEWTON-EULER

Cálculo de los momentos de inercia: Cálculo de los momentos de inercia: Teorema SteinerTeorema Steiner

2

22

22

2

13

1

2

13

1

2

1

RMI

LMRMI

LMRMI

DZ

DY

DX

DINÁMICA INVERSADINÁMICA INVERSA

RI0WIRI0WI: cálculo de las velocidades angulares de : cálculo de las velocidades angulares de las articulaciones.las articulaciones.

RI0WPIRI0WPI: cálculo de la aceleraciones angulares : cálculo de la aceleraciones angulares de las articulaciones. de las articulaciones.

DHDH: cálculo de las matrices de transformación.: cálculo de las matrices de transformación. RI0PIRI0PI + + RI0VPI_RRI0VPI_R o o RI0VPI_PRI0VPI_P: cálculo de las : cálculo de las

aceleraciones lineales.aceleraciones lineales. RI0SIRI0SI + + RI0AIRI0AI: cálculo de las aceleraciones del : cálculo de las aceleraciones del

centro de masa de cada elemento.centro de masa de cada elemento.

CÓDIGO NEWTON-EULERCÓDIGO NEWTON-EULER

Funciones utilizadas a las que llama NEWTONEULER5.M:Funciones utilizadas a las que llama NEWTONEULER5.M:

DINÁMICA INVERSADINÁMICA INVERSA

RI0FIRI0FI: cálculo de las fuerzas en el centro de : cálculo de las fuerzas en el centro de masas de cada elemento.masas de cada elemento.

RI0NIRI0NI: cálculo de los pares en el centro de : cálculo de los pares en el centro de masas de cada elemento.masas de cada elemento.

RI0FIARI0FIA: cálculo de las fuerzas articulares. : cálculo de las fuerzas articulares. RI0NIARI0NIA: cálculo de los pares articulares. : cálculo de los pares articulares. T_RT_R: cálculo de los pares de accionamientos.: cálculo de los pares de accionamientos. F_PF_P: cálculo de las fuerzas de accionamientos : cálculo de las fuerzas de accionamientos

CÓDIGO NEWTON-EULERCÓDIGO NEWTON-EULER

DINÁMICA DIRECTADINÁMICA DIRECTA

CÓDIGO NEWTON-EULERCÓDIGO NEWTON-EULER

Vector de aceleración de la gravedadVector de aceleración de la gravedad

Inicialmente [-g,0,0] Inicialmente [-g,0,0] Vector de aceleración de la gravedadVector de aceleración de la gravedad

Finalmente [0,0,-g] Finalmente [0,0,-g]

D-H 1ª articulaciónD-H 1ª articulación

WALTER-ORINWALTER-ORIN

DINÁMICA DIRECTADINÁMICA DIRECTAfunctionfunction qpp = walkerorin5(q,qp,tau,mext,Iext qpp = walkerorin5(q,qp,tau,mext,Iext););

kqKqGqqqCB T )()(),(..

kqKqGqqqCqqH T )()(),()(....

BqqH ..

)( B0

..

q

b=newtoneuler5(q,qp,zeros(5,1),9.8,masaext,inerciaext);b=newtoneuler5(q,qp,zeros(5,1),9.8,masaext,inerciaext);

H = h5(q,masaext,inerciaext);H = h5(q,masaext,inerciaext);..

q

DINÁMICADINÁMICA

EJEMPLO RESUMENEJEMPLO RESUMEN

MOTORES ARTICULARESMOTORES ARTICULARES

SERVOACCIONAMIENTOSSERVOACCIONAMIENTOSArticulación 1

Articulación 2

Articulación 3

Articulación 4Articulación 5

SERVOACCIONAMIENTOSSERVOACCIONAMIENTOS

MOTORES ARTICULARESMOTORES ARTICULARES

Parámetros Símbolo Valor

Resistencia R 0.6 Ω

Inductancia L 1.01 mH

Constante de par KT 0.155 Nm/A

Constante de voltaje KV 0.155 V/rad/s

Corriente máxima Imáx 38.7 A

Parámetros Símbolo Valor

Resistencia R 6.9 Ω

Inductancia L 1.28 mH

Constante de par KT 0.035 Nm/A

Constante de voltaje KV 0.035 V/rad/s

Corriente máxima Imáx 3.6 A

Articulación 1 2 3 4 5

τ pico (Nm) 3.6665 3.6162 2.5789 7.4278x10^-3 0.0299

τ nominal (Nm) 0.0246 1.3333x10^-3 1.2385 1.0472x10^-4 0.0138

Articulación 1 2 3 4 5

τ pico (Nm) 5.4998 5.4243 3.8684 11.142x10^-3 0.04485

τ nominal (Nm) 0.0369 19.99x10^-3 1.8577 1.5708x10^-4 0.0207

DA42HBB-10 (prismáticas) DB17CDB-10 (rotacionales)

CONTROL PIDCONTROL PID

DISEÑO REGULADORESDISEÑO REGULADORES

sDs

IPGPID

K I D

PID 1 95 0 0.12

PID 2 98 0 0.12

PID 3 95 0 0.1

PID 4 55 0 0.29

PID 5 78 0 0

CONTROL PIDCONTROL PID

DISEÑO REGULADORESDISEÑO REGULADORES

Respuestas finales conseguidas:

Articulación 1Articulación 2Articulación 3Articulación 4Articulación 5

SIMULACIÓN FINALSIMULACIÓN FINAL

SIMULACIÓN ROBOTSIMULACIÓN ROBOT

Evolución articular:Señal de referencia salida del planificador:

SIMULACIÓN FINALSIMULACIÓN FINAL

SIMULACIÓN ROBOTSIMULACIÓN ROBOT

Respuestas finales conseguidas utilizando un planificador de trayectorias correcto:

Curvas mucho más suaves respuestas correctas

FINFINQUIROBOTQUIROBOT