Robot quirúrgico

DISEÑO DE UN ROBOT

QUIRÚRGICO IIntroducción

Cinemática directaCinemática inversa

Matriz Jacobiana

Realización de incisiones en la pierna del paciente, con el fin de implantar un conjunto metálico que mantenga unidas las partes rotas del hueso que sirva de ayuda para la soldadura del hueso.

La endoscopia es una técnica diagnóstica utilizada sobre todo en medicina que consiste en la introducción de un endoscopio a través de una incisión quirúrgica para la visualización de un órgano hueco o cavidad corporal.

La endoscopia además de ser un procedimiento de diagnóstico mínimamente invasivo, también puede realizar maniobras terapéuticas como una colecistectomía laparocópica, artroscopia o la toma de biopsias.

MOTIVACIÓN Y OBJETIVOS

INTRODUCCIÓNINTRODUCCIÓN

En general, se admite que la robótica aporta numerosas ventajas con respecto al acto realizado por el hombre:

Permite una mayor precisión en los movimientos. El robot ejecuta las acciones que le son ordenadas por el médico, editándola por medio de un sistema de cómputo, es decir eliminando errores como el temblor que la mano humana tiene por naturaleza.

Posee un sistema de movimientos a escala de 1 a 1, de 1 a .3 y de 1 a .5, que les permite a los cirujanos hacer cirugía de alta precisión .

Las imágenes por medio de los visores telescópicos logran aumentar hasta 20 veces el tamaño normal, lo que permite al cirujano ver los órganos con más detalle.

Son más rápidos en la ejecución del trabajo asignado y tienen alta seguridad con velocidades de ejecución. Un cirujano no es capaz de ir rápido pues debe tener cuidado de no dañar órganos durante la intervención quirúrgica

Disminuye el sufrimiento de los pacientes, pues las incisiones que se realizan son entre 5 y 10 milímetros de diámetro, lo que representa suficiente espacio para permitir la entrada de los instrumentos del robot.

Reduce el tiempo de estancia hospitalaria de los pacientes, quienes pueden reincorporarse a sus actividades normales en un lapso no mayor a siete días.

Otorga mayor libertad de movimiento al cirujano que en una cirugía tradicional. El cirujano puede realizar la cirugía sin estar en contacto con el paciente, y no debe vestirse

con ropa estéril. Están mejor adaptados a una labor específica y mantienen mejor la atención durante el

procedimiento. Son más resistentes a la fatiga, por lo que no necesitan un período de tiempo de descanso. Tienen una salud de hierro y no están sometidos a la legislación laboral. Maniobras totalmente predecibles y no existen desviaciones de la trayectoria planificada .

¿POR QUÉ QUEREMOS UN ROBOT EN LOS QUIRÓFANOS?

INTRODUCCIÓN

CINEMÁTICA DIRECTA

MODELO ALÁMBRICO DEL ROBOT

Tres articulaciones prismáticas

Dos articulaciones de rotación

BLACK&DECKERArticulación extra

prismática

d1(t)

d2(t)

d3(t)

θ3(t)

θ4(t)

CINEMÁTICA DIRECTA“LO MEJOR ES ENEMIGO

DE LO BUENO”

MODELO ALÁMBRICO DEL ROBOT

d1(t)

d2(t)

d3(t)

θ3(t)

θ4(t)

CINEMÁTICA DIRECTA Singularidades en las límites en del espacio de trabajo del robot.

Se presentan cuando el extremo del robot está en algún punto del límite de trabajo interior o exterior.

0º < θ4(t) < 360º

-75º < θ5(t) < 225º

Singularidades en interior del espacio de trabajo del robot. Ocurren dentro de la zona de trabajo y se producen

generalmente por el alineamiento de dos o más ejes de las articulaciones del robot.

ESLABÓN θi di ai αi

1 0º d1(t) 0 90º

2 90º d2(t) 0 –90º

3 0º d3(t) 0 0º

4 θ4(t) l4 0 90º

5 θ5(t) 0 l5 0º

d1(t)

d2(t)

d3(t)

θ3(t)

θ4(t)

0x

0y

0z1x

1y

1z

2x

2y

2z

3x

3y

3z

4x

4y

4z

5x

5y

5z

HUELLA DEL ROBOT

CINEMÁTICA DIRECTA

CINEMÁTICA DIRECTA

1000

100

0010

0001

3

32

tdA

1000

010

0001

0100

2

21

tdA

1000

0100

))(sen(0))(cos())(sen(

))(cos(0))(sen())(cos(

5555

5555

54

tltt

tltt

A

MATRICES DE TRANSFORMACIÓN HOMOGÉNEA

1000

010

0100

0001

1

10

tdA

1000

0001

0010

0100

00 A

1000

010

0))(cos(0))(sen(

0))(sen(0))(cos(

4

44

44

43

l

tt

tt

A

1000

)cos()sen(0

)sen()cos()sen()cos()cos()sen(

)cos()sen()sen()sen()cos()cos(

1

iii

iiiiiii

iiiiiii

ii

d

a

a

A

900

)(0

11

111

a

tdd

900

)(90

22

222

a

tdd

00

)(0

33

333

a

tdd

900

)(

44

4444

a

ldt

0

0)(

555

555

la

dt

1000

0000

00)cos()sen(

00)sen()cos(

1000

0000

0010

001

1000

000

0010

0001

1000

0000

00)cos()sen(

00)sen()cos(

1 ii

iii

i

ii

ii

ii

a

d

A

d1(t)

d2(t)

θ3(t)

3x

3y

3z

2x

2y

2z

1x

1y

1z

0x

0y

0z

0z

0y

0x

θ4(t)

4x4y

4z

5x5y

5z

a

o

n

1000

0001

0010

0100

55 A

CINEMÁTICA DIRECTA

1000

100

0010

0001

3d

1000

010

0001

0100

2d

1000

0100

0

0

5555

5555

SlCS

ClSC

RESOLUCIÓN DEL PROBLEMA CINEMÁTICO DIRECTO

1000

010

0100

0001

1dT

1000

010

00

00

4

44

44

l

CS

SC

55

54

43

32

21

10

00 AAAAAAAT

1000

0001

0010

0100

00 A 1

0 A 21 A 3

2 A 43 A 5

4 A

1000

))(sen()())(sen())(cos(0

))(cos())(sen()())(cos())(sen())(sen())(sen())(cos(

))(cos())(cos()())(cos())(cos())(sen())(cos())(sen(

1000

554355

545254544

545154544

tlltdtt

ttltdttttt

ttltdttttt

paon

paon

paon

zzzz

yyyy

xxxx

1000

0001

0010

0100

55 A

CINEMÁTICA INVERSA

1000

))(sen()())(sen())(cos(0

))(cos())(sen()())(cos())(sen())(sen())(sen())(cos(

))(cos())(cos()())(cos())(cos())(sen())(cos())(sen(

1000

554355

545254544

545154544

tlltdtt

ttltdttttt

ttltdttttt

paon

paon

paon

zzzz

yyyy

xxxx

y

x

y

x

y

x

y

x

n

ntt

n

n

t

t

n

ntn

tnarctg)())(tg(

))(cos(

))(sen())(cos(

))(sen(44

4

4

4

4

z

z

z

z

z

z

z

z

o

att

o

a

t

t

o

a

to

taarctg)())(tg(

))(cos(

))(sen(

))(cos(

))(sen(55

5

5

5

5

))(sen()(

))(cos())(sen()(

))(cos())(cos()(

))(sen(

))(cos())(sen(

))(cos())(cos(

))(cos(

))(sen())(sen(

))(sen())(cos(

0

))(cos(

))(sen(

5543

5452

5451

5

54

54

5

54

54

4

4

tlltdp

ttltdp

ttltdp

ta

tta

tta

to

tto

tto

n

tn

tn

z

y

x

z

y

x

z

y

x

z

y

x

RESOLUCIÓN DEL PROBLEMA CINEMÁTICO INVERSO

y

x

n

nt arctg)(4

z

z

o

at arctg)(5

))(cos())(cos()(

))(cos())(cos()(

5451

5451

ttlptd

ttltdp

x

x

))(cos())(cos()( 5451 ttlptd x ))(cos())(sen()(

))(cos())(sen()(

5452

5452

ttlptd

ttltdp

y

y

))(cos())(sen()( 5452 ttlptd y ))(sen()(

))(sen()(

5543

5543

tllptd

tlltdp

z

z

))(sen()( 5543 tllptd z

zyx onlptd 51 )(zxy onlptd 52 )(

zz allptd 543 )(

CÓDIGO IMPLEMENTADO PARA LA CINEMÁTICA INVERSA

CINEMÁTICA INVERSAfunction q = inversa(T)

l4=0.4;l5=0.2;

% Inicialización de las variables articulares a calcularq=[0 0 0 0 0];

% Solución de las articulacionesq(1)=T(1,4)-l5*T(2,1)*T(3,2);q(2)=T(2,4)+l5*T(1,1)*T(3,2);q(3)=-T(3,4)-l4+l5*T(3,3);q(4)=atan2(T(1,1),T(2,1));q(5)=atan2(-T(3,3),T(3,2));

100044434241

34333231

24232221

14131211

zzzz

yyyy

xxxx

paon

paon

paon

TTTT

TTTT

TTTT

TTTT

)arcsen()())(sen( 44 xx nttn )arcsen()(4 xnt )arcsen()())(sen( 55 zz atta )arcsen()(5 zat

function q = inversa(T)

l4=0.4;l5=0.2;

% Inicialización de las variables articulares a calcularq=[0 0 0 0 0];

% Solución de las articulacionesq(1)=T(1,4)-l5*T(2,1)*T(3,2);q(2)=T(2,4)+l5*T(1,1)*T(3,2);q(3)=-T(3,4)-l4+l5*T(3,3);q(4)=asin(T(1,1));q(5)=asin(-T(3,3));

MATRIZ JACOBIANA DIRECTA

SINGULARIDADES

5

4

3

2

1

52

42

54

52

42

54

52

42

54

52

42

54

55

545545

545545

10000

)(cos)(sen1

)sen()sen(

)(cos)(sen1

)cos()cos(000

)(cos)(cos1

)sen()cos(

)(cos)(cos1

)cos()sen(000

)cos(0100

)sen()sen()cos()cos(010

)sen()cos()cos()sen(001

d

d

d

l

ll

ll

z

y

x

5

4

3

2

1

52

42

54

52

42

54

52

42

54

52

42

54

55

545545

545545

)(cos)(sen1

)sen()sen(

)(cos)(sen1

)cos()cos(000

)(cos)(cos1

)sen()cos(

)(cos)(cos1

)cos()sen(000

)cos(0100

)sen()sen()cos()cos(010

)sen()cos()cos()sen(001

d

d

d

l

ll

ll

z

y

x

)(cos)(cos1)(cos)(sen1

)cos()sen(

52

42

52

42

55

J

º9054 y

º27054 y

º0º270,º180,º90,º0 54 y

º180º270,º180,º90,º0 54 y

54 º180,º0 y

54 º270,º90 y

Límite exterior del espacio de trabajo.

Límite interior del espacio de trabajo.

Límite exterior del espacio de trabajo.

Límite exterior del espacio de trabajo. z4 y z5 se

alinean con z1. z4 y z5 se alinean con z0.

0J

SINGULARIDADES

Peores configuraciones del robot


θ4(t)

θ5(t) = 90º

4x

4y

4z

5x

5y

5z

θ4(t)

θ5(t) = 270º

3y

4x

4y

4z

5x

5y5z

Figura 2.6a: Casos 1 y 2. Tipos de singularidades en los límites que presenta en θ4(t) = 90º, 270º.

θ4(t ) ≠ 0º, 90º, 180º, 270º

θ5(t) = 0º

3x

3y

3z

4x4y

4z

5x5y

5z θ5(t) = 180º

3x

3y

3z

4x4y

4z

5x5y

5z

Figura 2.6b: Casos 3 y 4. Tipos de singularidades en los límites que presenta enθ4(t) ≠ 0º, 90º, 180º, 270º y θ4(t ) = 0º, 180º..

SINGULARIDADES

θ4(t ) = 180º

θ5(t) = 0º

3x

3y

3z

4x4y

4z

5x5y

5z

θ5(t) = 0º

3y

3z

4x

4y

4z

5x

5y

5z

θ4(t ) = 0º

3x

0x0y

0z1x

1y

1z

2x

2y

2z

0x0y

0z1x

1y

1z

2x

2y

2z

Figura 2.7a: Caso 5 de singularidades de ejes z4 y z5 que se alinean con z1.

θ4(t ) = 90º

θ5(t) = 180º

3x

3y

3z

4x

4y

4z

5y

5z

θ4(t ) = 270º

θ5(t) = 180º

3x

3y 3z

4x

4y 4z5y

5z

5x

5x

0x0y

0z1x

1y

1z

2x

2y

2z

0x0y

0z1x

1y

1z

2x

2y

2z

Figura 2.7b: Caso 6 de singularidades de ejes z4 y z5 que se alinean con z0.


ANTI-SINGULARIDADES

doptimalida de sCondicione

0

0

)(cos)(cos1)(cos)(sen1

)cos()sen(

5

4

52

42

52

42

55

J

J

J

Configuración 1: θ4 = 45º y θ5 = 35.2644ºConfiguración 2: θ4 = 45º y θ5 = 144.7356ºConfiguración 3: θ4 = 45º y θ5 = 324.7356ºConfiguración 4: θ4 = 45º y θ5 = 215.2564ºConfiguración 5: θ4 = 135º y θ5 = 35.2644ºConfiguración 6: θ4 = 135º y θ5 = 144.7356ºConfiguración 7: θ4 = 135º y θ5 = 324.7356ºConfiguración 8: θ4 = 135º y θ5 = 215.2564ºConfiguración 9: θ4 = 225º y θ5 = 35.2644ºConfiguración 10: θ4 = 225º y θ5 = 144.7356ºConfiguración 11: θ4 = 225º y θ5 = 324.7356ºConfiguración 12: θ4 = 225º y θ5 = 215.2564ºConfiguración 13: θ4 = –45º y θ5 = 35.2644ºConfiguración 14: θ4 = –45º y θ5 = 144.7356ºConfiguración 15: θ4 = –45º y θ5 = 324.7356ºConfiguración 16: θ4 = –45º y θ5 = 215.2564º

Configuración 1: θ4 = 45º y θ5 = 35.2644ºConfiguración 2: θ4 = 45º y θ5 = 144.7356ºConfiguración 3: θ4 = 45º y θ5 = 324.7356ºConfiguración 4: θ4 = 45º y θ5 = 215.2564ºConfiguración 13: θ4 = –45º y θ5 = 35.2644ºConfiguración 14: θ4 = –45º y θ5 = 144.7356ºConfiguración 15: θ4 = –45º y θ5 = 324.7356ºConfiguración 16: θ4 = –45º y θ5 = 215.2564º

Mejores configuraciones del robot

MATRIZ JACOBIANA INVERSA

PSEUDO-SINGULARIDADES

z

y

x

l

l

d

d

d

0)(cos)(cos)(cos)(sen

)cos()sen(

)(cos)(cos)(cos)(sen

)cos()sen(000

0)(cos)(cos

)cos()sen(

)(cos)(cos

)cos()sen(000

0))(sen)(sen

)cos()sen(

))(sen)(sen

)cos()sen(100

0))sen(0010

00))sen(001

22222222

2222

2222

5

5

5

4

3

2

1

z

y

x

l

l

d

d

d


)cos()sen(


)cos()sen(000

)(cos)(cos

)cos()sen(

)(cos)(cos

)cos()sen(000

))(sen)(sen

)cos()sen(

))(sen)(sen

)cos()sen(100

))sen(0010

0))sen(001

22222222

2222

2222

5

5

5

4

3

2

1

01 J º360,º270,º180,º90,º0

)(cos)(sen))(cos)((cos

)sen()sen()(cos)cos()cos()(sen222

322

221

J

Peores orientaciones del efector final

DISEÑO DE UN ROBOT

QUIRÚRGICO II

Dinámica inversaDinámica directa

Selección de servoaccionamientosControl y Simulación

DINÁMICA

ROBOT REAL

MÓDULOS

COMERCIALES

ROBOT REAL

CARTESIANO TORREBLANCA

DINÁMICA

DINÁMICA

NUESTRO ROBOT

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

CÓDIGO NEWTON-EULER

Masas elementos

M1= 5 kg

M2= 5 kg

M3= 5 kg

M4= 0 kg

M5= 7 kg

Rozamiento viscoso

B1= 0.06

B2= 0.06

B3= 0.06

B4= 0.05

B5= 0.05

Posicionamientocdm

F1= -0.5

F2= -0.5

F3= -0.5

F4= -0.5

F5= -0.5

DINÁMICA INVERSA


Cálculo de los momentos de inercia: Teorema Steiner

2

22

22

2

13

1

2

13

1

2

1

RMI

LMRMI

LMRMI

DZ

DY

DX

DINÁMICA INVERSA

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

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

DH: cálculo de las matrices de transformación. RI0PI + RI0VPI_R o RI0VPI_P: cálculo de las

aceleraciones lineales. RI0SI + RI0AI: cálculo de las aceleraciones del

centro de masa de cada elemento.


Funciones utilizadas a las que llama NEWTONEULER5.M:

DINÁMICA INVERSA

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

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

RI0FIA: cálculo de las fuerzas articulares. RI0NIA: cálculo de los pares articulares. T_R: cálculo de los pares de accionamientos. F_P: cálculo de las fuerzas de accionamientos


DINÁMICA DIRECTA


Vector de aceleración de la gravedad

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

Finalmente [0,0,-g]

D-H 1ª articulación

WALTER-ORIN

DINÁMICA DIRECTAfunction 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);

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

q

DINÁMICA

EJEMPLO RESUMEN

MOTORES ARTICULARES

SERVOACCIONAMIENTOSArticulación 1

Articulación 2

Articulación 3

Articulación 4Articulación 5

SERVOACCIONAMIENTOS

MOTORES 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 PID

DISEÑ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 PID

DISEÑO REGULADORES

Respuestas finales conseguidas:

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

SIMULACIÓN FINAL

SIMULACIÓN ROBOT

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

SIMULACIÓN FINAL

SIMULACIÓN ROBOT

Respuestas finales conseguidas utilizando un planificador de trayectorias correcto:

Curvas mucho más suaves respuestas correctas

FINQUIROBOT

Robot quirúrgico

Documents

Transcript of Robot quirúrgico