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