Ejercicio 1 trabajorobtica4industriales-121122133821-phpapp02.pdf

QUIROBOT

 ALUMNO: MARTÍNEZ VERDÚ, Jaime  ALUMNO: AZNAR GOMIS, José Luís  ASIGNATURA: Co!"o# $e Ro%o!s INGENIERÍA INDUSTRIAL  INDUSTRIAL CURSO: &'

CAPÍTULO 1: ROBÓTICA QUIRÚRGICA. 1. CIRUGÍA ROBÓTICA VS TELECIRUGÍA CIRUGÍA ROBÓTICA ROBOTS EN TELECIRUGÍA

1–2

2. LA ROBÓTICA MÉDICA HOY EN DÍA

1–3

3. VENTAJAS DE LA CIRUGÍA ROBÓTICA

1–6

4. LA SIGUIENTE FRONTERA

1–

!. "LANTEAMIENTO DEL "ROBLEMA

1–

6. SOLUCIÓN AL "ROBLEMA

1–#

CAPÍTULO 2: CINEMÁTICA DIRECTA DEL QUIROBOT. 1. EL FORMALISMO DE DENAVIT$HARTENBERG

2–1

2. RESOLUCIÓN DEL "ROBLEMA DE LA CINEM%TICA DIRECTA

2–

3. IM"LEMENTACIÓN DEL CÓDIGO DE LA CINEM%TICA DIRECTA

2 – 21

4. EJEM"LOS DE UTILI&ACIÓN DE LA FUNCIÓN IM"LEMENTADA

2 – 23

!. LA CINEM%TICA DIRECTA COMO VENTAJA' EL JACOBIANA

2 – 24

CAPÍTULO 3: CINEMÁTICA INVERSA DEL QUIROBOT. 1. DESCRI"CIÓN DEL MÉTODO DIRECTO

3–4

2. RESOLUCIÓN DEL "ROBLEMA DE LA CINEM%TICA INVERSA

3–!

3. IM"LEMENTACIÓN DEL CÓDIGO DE LA CINEM%TICA INVERSA

3 – 1(

4. EJEM"LOS DE UTILI&ACIÓN DE LA FUNCIÓN IM"LEMENTADA

3 – 12

!. LA CINEM%TICA DIRECTA COMO VENTAJA' LA JACOBIANA INVERSA

3 – 13

CAPÍTULO 4: REPRESENTACIÓN GRÁFICA DEL QUIROBOT. 1. RE"RESENTACIÓN GR%FICA EN M)*L)+ , USANDO ALAMBRES 1. E-/0)05 5+ / 780)90* : /) 780; : 5*)0; 3D 2. I9/9*)0; :/ ;:0< : /) 780; : 5*)0; 3D' DIBUJAROBOT 3. E=9/5 : 8*0/0>)0; : /) 780; 09/9*):)

4–1 4–1 4–2 4–3

2. ANIMACIÓN GR%FICA EN MATLAB, USANDO ALAMBRES 1. E-/0)05 5+ / 780)90* : /) 780; : 5*)0; 3D 2. I9/9*)0; :/ ;:0< : /) 780; : 5*)0; 3D' ANIMA 3. I9/9*)0; :/ ;:0< : /) 780; : 5*)0; 3D' "LANIFICA 4. E=9/5 : 8*0/0>)0; : /) 780; 09/9*):)

4–4 4–4 4–4 4–4 4–6

CAPÍTULO 5: DINÁMICA DE ROBOTS. 1. DIN%MICA INVERSA

!–2

2. DIN%MICA DIRECTA

!–?

CAPÍTULO : SELECCIÓN DE SERVOACCIONAMIEN SERVOACCIONAMIENTOS. TOS. 1. RE@UISITOS @UE DEBEN SATISFACER NUESTROS MOTORES

6 –2

2. BS@UEDA DE MOTORES A"RO"IADOS EN CAT%LOGO

6–!

CAPÍTULO !: CINEMÁTICA INVERSA DEL QUIROBOT. 1. DISEO DE LOS "ID

–1

2. SIMULACIÓN GLOBAL

–6

ROBÓTICA QUIRÚRGICA

1

Capí tulo R OBÓTICA  Q QUIR ÚR GICA "AL PRINCIPIO CREÓ DIOS EL CIELO Y LA TIERRA …"

 El libro del Génesis

La #ir$ía ro%&ti#a 'are#e na idea tomada de na 'e(í#(a de #ien#ia )i##i&n *+ se$ramente+ (a ,isi&n -e tenemos de e((a .a sido in)(ida 'or im/$enes #omo (as de (a  'e(í#(a Star wars+ en (a -e L0e 10*2a0er es atendido médi#amente 'or nos ro%ots -e in#(so (e im'(antan n %razo ro%&ti#o3 Las 'osi%i(idades de a'(i#a#i&n de ro%ots en (a #ir$ía .an moti,ado (a in,esti$a#i&n en este #am'o+ * .o* en día *a son na rea(idad3 La 'a(a%ra ro%ot 'ro,iene de( #.e#o4 se$ún e( di##ionario de (a (en$a es'a5o(a de (a Rea( A#ademia -iere de#ir 6tra%a7o o 'resta#i&n 'ersona(6 * (a de)ine #omo8 6na m/-ina o in$enio e(e#tr&ni#o 'ro$rama%(e+ #a'az de mani'(ar o%7etos * rea(izar o'era#iones antes reser,adas s&(o a (as 'ersonas63 9( #on#e'to de ro%ot (o em'(e& 'or 'rimera ,ez+ en !:;!+ e( es#ritor 3 9( término se deri,a de( #.e#os(o,a#o ro%ota+ -e si$ni)i#a na (a%or tediosa o ser,i(3 9n s (i%ro+
Tabla 1.1 i)erentes ro%ots em'(eados en medi#ina+ se in#(*e s ti'o * a'(i#a#i&n3 !"! Jaime Martínez Verdú

José Lís Aznar Gomis

ROBÓTICA QUIRÚRGICA

1. CIRUG!A ROBÓTICA S TELECIRUG!A. CIRUG!A ROBÓTICA. La #ir$ía ro%&ti#a es n 'aso m/s a,anzado de (o anterior+ *a -e se trata de(  'ro#eso mediante e( #a( es e( ro%ot e( -e e)e#túa n 'ro#edimiento -irúr$i#o %a7o e( #ontro( de n 'ro$rama de #om'ta#i&n3 9n este #aso+ e( #ir7ano 'arti#i'a $enera(mente en (a '(ani)i#a#i&n de(  'ro#edimiento+ 'ero es n o%ser,ador en (a im'(ementa#i&n de( '(an *a -e (a e7e##i&n de( mismo es rea(izada e#(si,a"mente 'or e( ro%ot3 Qé es (o -e se o%tiene #on esta 'r/#ti#aD 1e (o$ra " entre otras #osas " -e no eistan des,ia#iones de (a tra*e#toria '(ani)i#ada+ a(ta se$ridad #on ,e(o#idades de e7e##i&n * manio%ras tota(mente 'rede#i%(es3

ROBOTS EN TELECIRUG!A3 Mientras -e en (a #ir$ía ro%&ti#a es e( ro%ot e( -e " na ,ez 'ro$ramado " rea(iza 'or sí mismo (a o'era#i&n+ en (a te(e#ir$ía eisten ro%ots -e e)e#túan ínte$ramente (os 'ro#edimientos 'ero %a7o (a $ía de( #ir7ano3 Como sa%emos+ te(e'resen#ia im'(i#a n #ir7ano o'erando desde na (o#a(idad remota+ *a sea en (a .a%ita#i&n de a( (ado o en (as antí'odas de( mndo3 9((o se (o$ra mani'(ando %razos ro%&ti#os mediante na #om'(i#ada inter)a#e -e #om%ina retroa(imenta#i&n ,isa(+ aditi,a * t/#ti(3 9sta inter)a#e es )ndamenta(+ *a -e e( #ir7ano s&(o #enta #on (os datos %rindados 'or (os sensores ro%&ti#os -e a#túan so%re e( 'a#iente3 Los mo,imientos de (as manos de( #ir7ano son transmitidos a (os %razos ro%&ti#os -e (os re'rod#en )ie(mente3 ?ara (os o7os de( #ir7ano (a mani7a -e me,e en (a #onso(a * e( instrmento -e re'rod#e ese mo,imiento en e( 'a#iente #onstit*e na úni#a entidad3 9sto 7nto a( E.a'ti#E =dis'ositi,o de E)or#e")eed%a#0E> -e (e da a ss manos (a sensa#i&n de ta#to * resisten#ia so%re (os te7idos -e mani'(an (os %razos ro%&ti#os+ in#rementan notoriamente (a sensa#i&n de inmersi&n3 An-e (a #ir$ía ro%&ti#a * (a te(e#ir$ía tienen m#.os 'ntos en #omún+ (os métodos sados 'ara e( #ontro( de( ro%ot * de (a inter)a#e .om%re"#om'tadora ,arían si$ni)i#ati,amente3 e%ido a (a #om'(e7idad de esta inter)a#e+ (a te(e#ir$ía se sa  'rin#i'a(mente 'ara (as #ir$ías mínima"mente in,asi,as+ en donde se a#túa #on instrmentos -e in$resan 'or 'e-e5as in#isiones+ sin sar (as manos dentro de (a #a,idad -e se est/ o'erando3 M/s aún+ re#ientemente se .an rea(iza"do e'erimentos sando mini"ro%ots -e in*e#tados en (os ,asos )emora(es son $iados .asta (os ,asos #ere%ra(es de .asta !3F mm de di/metro3 1i %ien (os ro%ots tota(mente at&nomos est/n toda,ía en s eta'a de desarro((o e'erimenta(+ eisten *a en e( mer#ado a($nos e-i'amientos ro%otizados de so en #ir$ía =A91O?+ Ro%o"do#+ Re$(s+ et#3>

!"; Jaime Martínez Verdú

José Lís Aznar Gomis

ROBÓTICA QUIRÚRGICA

#. LA ROBÓTICA $%DICA &OY EN D!A. A#ta(mente+ (os ro%ots .an sido inte$rados en di)erentes #am'os+ entre (os -e se en#entran (a man)a#tra de atom&,i(es+ e( mane7o de materia(es 'e(i$rosos 'ara e( .om%re+ e in#(so nos sstit*en en ,ia7es a( es'a#io -e im'(i#arían n $ran ries$o * serían demasiado 'ro(on$ados 'ara n ser ,i,o3 9s'e#í)i#amente en medi#ina se .an em'(eado di,ersas te#no(o$ías ro%&ti#as -e .an )a#i(itado e( tratamiento de ,arios  'ade#imientos3 Ta( es e( #aso+ 'or e7em'(o+ de (a #ir$ía de( o7o asistida 'or #om'tadora+ en (a -e se 'ro'or#iona (a in)orma#i&n a#er#a de (a $eometría * #ara#terísti#as de( $(o%o o#(ar a n sistema #om'tarizado+ e( #a( $ía (os #ortes a rea(izar 'ara #orre$ir (as de)i#ien#ias ,isa(es3 1in em%ar$o+ ro%ots -e ten$an na inte(i$en#ia arti)i#ia( seme7ante a (a .mana toda,ía no eisten4 es )a#ti%(e -e en n )tro no tan (e7ano se dise5en ro%ots #on a($o #om'ara%(e a na #on#ien#ia * mente 'ro'ias+ -e 7nto #on na (i%ertad de mo,imiento s'erior a (a de( .om%re =$ra#ias a (os materia(es #on (os -e estén #onstridos>+ (es ,an a 'ermitir rea(izar a#ti,idades im'osi%(es 'ara nosotros o #on na me7or e)i#ien#ia -e (a de (os .manos3 9n (a medi#ina esto sena atra#ti,o a  'esar de -e .asta (a )e#.a nin$na m/-ina #m'(e #on (o anterior3 9n este /m%ito+ e( #ir7ano ro%ot #orres'ondería a n mani'(ador #ontro(ado 'or #om'tadora+ #a'az de  'er#i%ir (as 'artes de( #er'o .mano * de mo,er (os instrmentos -irúr$i#os 'ara e)e#tar na #ir$ía3 9n (a a#ta(idad se #(asi)i#a a (os ro%ots #omo 'asi,os+ #ando  'ermiten %i#ar * mantener en 'osi#i&n a($nos instrmentos 'ara )a#i(itar a( #ir7ano e(  'ro#edimiento -irúr$i#o+ * a#ti,os+ #ando e( ro%ot me,e (os instrmentos * rea(iza (a #ir$ía3

'()u*a 1.1 Com'onentes de( ro%ot maestro"es#(a,o ti'o a Vin#i ti(izado .o* en día en m#.os .os'ita(es de( mndo3 entro de estos ú(timos eiste (o -e se #ono#e #omo (os sistemas maestro" es#(a,o+ en (os -e e( ro%ot mani'(a (os instrmentos+ 'ero es e( #ir7ano e( -e (e indi#a a( ro%ot #&mo .a#er(o3 e a#erdo #on esta #(asi)i#a#i&n se .an #onstrido ,arios ro%ots 'asi,os -e 'ermiten (a rea(iza#i&n de #ir$ías re(ati,amente sim'(es+ #omo (as  %io'sias estereot/i#as+ en (as -e e( nero#ir7ano a(imenta (as #ara#terísti#as de( tmor a o'erar en n sistema #om'ta#iona( -e #ontro(a n ro%ot en#ar$ado de introd#ir (a a$7a 'ara (a toma de (a mestra de te7ido sos'e#.oso3 !"H Jaime Martínez Verdú

José Lís Aznar Gomis

ROBÓTICA QUIRÚRGICA 9ntre (os ro%ots a#ti,os desta#a no #reado 'or IBM+ denominado Ro%odo#3 1e trata de n sistema e'erimenta( -e 'ermite im'(antar na 'r&tesis de #adera #on ma*or s'er)i#ie 'ara s )i7a#i&n+ en n 'erro3 9( 'rimer ro%ot de( ti'o a#ti,o ti(izado en .manos es e( ?ro%ot+ #reado 'or e( Im'eria( Co((e$e en Londres * -e a*da a rea(izar na rese##i&n de te7ido %eni$no de (a 'r&stata4 este ro%ot in#or'ora en s 'nta n sistema de (trasonido -e (e 'ermite #rear na ima$en tridimensiona( de (a 'r&stata+ así e( #ir7ano se(e##iona -é 'artes de( te7ido de%e #ortar e( ?ro%ot =Ta%(a !3!>3 1e %s#a -e (os ro%ots me7oren (os res(tados de (a #ir$ía tradi#iona( ,o(,iendo (os 'ro#edimientos menos a$resi,os4 esto e'(i#a 'or -é (a ma*oría de (os a,an#es en #ir$ía ro%&ti#a se .an dado en e( #am'o emer$ente de (a #ir$ía mínimamente in,asi,a+ #ono#ida #omo #ir$ía (a'aros#&'i#a3 sta #onsiste en (a introd##i&n en e( #er'o de na #/mara e instrmentos mediante (os #a(es se rea(iza (a #ir$ía4 'ara e((o se .an im'(ementado di)erentes ro%ots+ * no de (os 'rimeros )e e( ro%ot a#ti,ado 'or ,oz #ono#ido #omo A91O? =si$(as en in$(és de 1istema Ó'timo de ?osi#ionamiento 9ndos#&'i#o Atomatizado>+ -e a#ta(mente se ti(iza en )orma rtinaria en #entros es'e#ia(izados en #ir$ía (a'aros#&'i#a 9ste ro%ot #onsiste en n  %razo me#/ni#o #one#tado a na #om'tadora -e re#ono#e &rdenes ,er%a(es sen#i((as * -e e( ro%ot trad#e en mo,imientos de (a #/mara (a'aros#&'i#a3 9( A91O? (i%era n  %razo de( #ir7ano * así se dismin*e e( número de 'ersonas -e se re-ieren 'ara (a #ir$ía+ #on (a ,enta7a de -e (a ima$en de (a #ir$ía no ,a a mo,erse ni a tem%(ar #omo (o .aría n #ir7ano -e sostiene na #/mara drante n 'eriodo (ar$o de tiem'o3 9( #osto 'romedio de este ro%ot es de :K+KKK d&(ares3

'()u*a 1.# 1e o%ser,a #&mo e( ro%ot maestro"es#(a,o ti'o a Vin#i trad#e (os mo,imientos de (a mano de( #ir7ano en mo,imientos de( instrmenta(3 Ro%ots de na ne,a $enera#i&n son (os sistemas maestro"es#(a,o+ -e in#(*en a (os ro%ots a Vin#i * es3 9stos sistemas 'ermiten (o -e #ono#emos #omo #ir$ía asistida 'or ro%ot+ en (a #a( e( #ir7ano ti(iza %razos me#/ni#os -e re'iten (os mo,imientos -e rea(iza en na #onso(a3 9n (a #onso(a #om'tarizada se tiene n ,isor -e transmite (a ima$en -e es #a'tada 'or (a #/mara (a'aros#&'i#a %i#ada en no de (os %razos me#/ni#os3 9( sistema #onsta de n #on7nto de mani,e(as -e se ada'tan a( dedo '($ar e índi#e de( #ir7ano+ #on (os #a(es #ontro(a e( mo,imiento de (os %razos me#/ni#os3 Los %razos me#/ni#os son tres+ no 'ara sostener (a #/mara (a'aros#&'i#a+ * otros dos -e mani'(an (os instrmentos -irúr$i#os =ti7eras+ 'inzas+ e(e#tro#aterios+  'orta"a$7as+ et#étera>3 Una #ara#terísti#a im'ortante de estos instrmentos es s (i%ertad de mo,imiento en seis * siete di)erentes /n$(os+ -e intenta em(ar (os ar#os de mo,imiento e)e#tados 'or (a arti#(a#i&n de (a m5e#a .mana3 9sto es n $ran a,an#e si #onsideramos -e toda (a #ir$ía (a'aros#&'i#a tiene #omo (imitante -e (os mo,imientos se rea(izan sin 'oder )(eionar (os instrmentos+ siendo e( #ir7ano e( -e se ada'ta a estas restri##iones drante (a #ir$ía3 !" Jaime Martínez Verdú

José Lís Aznar Gomis

ROBÓTICA QUIRÚRGICA 9ntre (as ,enta7as -e o)re#e (a #onso(a se en#entra -e e( #ir7ano  'ede rea(izar (a #ir$ía sin estar en #onta#to #on e( 'a#iente+ * no de%e ,estirse #on ro'a estéri(3 La ima$en -e se o%ser,a en e( ,isor es tridimensiona(+ $ra#ias a n sistema de dos #/maras (a'aros#&'i#as en e( 'a#iente+ esto (e 'ermite a( #ir7ano tener na  'er#e'#i&n de 'ro)ndidad -e 'odría en a($na )orma sstitir (a de)i#ien#ia de ta#to -e se tiene en este ti'o de #ir$ía3 ?or otra 'arte+ (a mani'(a#i&n de (as mani,e(as 'ara #ontro(ar (os mo,imientos de (os instrmentos 'or (os %razos me#/ni#os se rea(iza en tiem'o rea(3 9sto tiene na im'ortan#ia )ndamenta( si #onsideramos -e (a #ir$ía im'(i#a mo,imientos r/'idos * de(i#ados 'ara e,itar n da5o en e( 'a#iente3 9n (a te#no(o$ía -e se ti(iza 'ara (os instrmentos se in#(*e (a arti#(a#i&n ti'o m5e#a =9ndo2rist>+ -e 'ermite -e se )(eionen so%re s e7e+ dando na (i%ertad de mo,imiento 'ara e( instrmenta( -irúr$i#o de m/s de tres e7es3 Adem/s+ e( sistema #om'ta#iona( tiene (a #a'a#idad de es#a(ar (os mo,imientos desde ;8! .asta F8!+ así #omo )i(trar e( tem%(or de( #ir7ano+ .a#iendo 'osi%(e (a rea(iza#i&n de #ir$ía #on des'(azamientos mínimos de( #ir7ano * sin (as restri##iones de%idas a s '(so3 Anado a esto eiste (a 'osi%i(idad de #oordinar (os mo,imientos de (a #/mara e instrmenta( #on (os mo,imientos de( 'a#iente4 esto es es'e#ia(mente úti( #ando se trata de #ir$ía #ardia#a+ en (a -e no se re-iere -e e( #oraz&n de( 'a#iente se deten$a3 1e 'eden a'(i#ar stras en e( #oraz&n mientras (ate+ 'esto -e e( #ir7ano $ra#ias a (os )i(tros de (a #om'tadora ,e na ima$en est/ti#a de( #oraz&n+ así mismo esto 'ermite #o(o#ar stras 'ara (a rea(iza#i&n de %*"'ass #oronario ='entes arteria(es en #asos de in)artos> * otras #ir$ías de #oraz&n3 La $ran ma*oría de #ir$ías asistidas 'or ro%ot se rea(izan en 'ro#edimientos (a'aros#&'i#os #omo *a se men#ion&+ en esta #ir$ía se introd#en en e( 'a#iente (os denominados 'ertos+ nos instrmentos -e 'ermiten in)(ar #on $as (a #a,idad -e se ,a a o'erar+ 'ara 'oder #rear n es'a#io en e( #a( dis'oner (os instrmentos * (a #/mara 'ara e)e#tar (a #ir$ía3

'()u*a 1.+ Brazos me#/ni#os3 9( #entra( sostiene * me,e (a #/mara de ,isi&n interna+ * (os dos (atera(es 'ermiten (a introd##i&n * mo,imiento de( instrmenta(3 !"F Jaime Martínez Verdú

José Lís Aznar Gomis

ROBÓTICA QUIRÚRGICA La #ir$ía (a'aros#&'i#a se ini#i& #ando se en#ontr& -e ins)(ando aire en e( a%domen de n anima( e'erimenta( era 'osi%(e insertar na #/mara de #istos#o'ia =#/mara ti(izada 'ara re,isar (a ,e7i$a>+ -e 'ermiti& o%ser,ar (os &r$anos a%domina(es de( anima(3 La (a'aros#o'ia+ en (a 'r/#ti#a+ #omenz& a ti(izarse en $ine#o(o$ía a(rededor de !:K 'ara dia$nosti#ar a(tera#iones en e( útero4 'ara !:N em'ez& a tener n $ran a$e en (a #ir$ía $astrointestina( * a )ina(es de (os a5os !::K+ en ro(o$ía3 A#ta(mente en #asi todas (as es'e#ia(idades -irúr$i#as se ti(iza (a #ir$ía (a'aros#&'i#a3 9sta té#ni#a -irúr$i#a red#e e( da5o a (os te7idos+ 'ro,o#a menos san$rado * do(or 'osto'eratorio+ * )a#i(ita na m/s r/'ida re#'era#i&n de (os 'a#ientes3 ?or e((o+ este ti'o de #ir$ía se est/ *a rea(izando en di)erentes 'artes de( mndo en )orma rtinaria3 9s im'ortante men#ionar -e otro #am'o de a'(i#a#i&n de (os ro%ots es e( entrenamiento de #ir7anos3 La #ir$ía (a'aros#&'i#a tiene na #r,a de a'rendiza7e m* (enta+ (o -e o%(i$a a n entrenamiento es'e#ia(izado * de (ar$a dra#i&n3 1e .a  '(anteado -e (os ro%ots aso#iados a sim(adores 'odrían #ontri%ir si$ni)i#ati,amente en (a 're'ara#i&n de #ir7anos4 tam%ién #on (os sistemas maestro"es#(a,o se 'odría )a#i(itar e( so de( instrmenta( red#iendo e( tiem'o de entrenamiento 'ara e( #ir7ano3

+. ENTA,AS DE LA CIRUG!A ROBÓTICA. Las ,enta7as -e a'orta (a ro%&ti#a a (as o'era#iones son+ 'or e7em'(o4 •

•

•

•

•

•

•

?ermite na ma*or 're#isi&n en (os mo,imientos3 9( ro%ot e7e#ta (as a##iones -e (e son ordenadas 'or e( médi#o+ edit/ndo(a 'or medio de n sistema de #&m'to+ es de#ir e(iminando errores #omo e( tem%(or -e (a mano .mana tiene 'or natra(eza3 ?osee n sistema de mo,imientos a es#a(a de ! a !+ de ! a 3H * de ! a 3F+ -e (es  'ermite a (os #ir7anos .a#er #ir$ía de a(ta 're#isi&n3 Las im/$enes 'or medio de (os ,isores te(es#&'i#os (o$ran amentar .asta ;K ,e#es e( tama5o norma(+ (o -e 'ermite a( #ir7ano ,er (os &r$anos #on m/s deta((e3 ismin*e e( s)rimiento de (os 'a#ientes+ 'es (as in#isiones -e se rea(izan son entre F * !K mi(ímetros de di/metro+ (o -e re'resenta s)i#iente es'a#io 'ara  'ermitir (a entrada de (os instrmentos de( ro%ot3 Red#e e( tiem'o de estan#ia .os'ita(aria de (os 'a#ientes+ -ienes 'eden rein#or'orarse a ss a#ti,idades norma(es en n (a'so no ma*or a siete días3 Otor$a ma*or (i%ertad de mo,imiento a( #ir7ano -e en na #ir$ía La'aros#&'i#a tradi#iona(3 ?ermite rea(izar o'era#iones a distan#ia+ (o #a( e,ita des'(azarse tanto a( 'a#iente #omo a( médi#o -e (a e)e#túa3

!" Jaime Martínez Verdú

José Lís Aznar Gomis

ROBÓTICA QUIRÚRGICA

-. LA SIGUIENTE 'RONTERA. 9n e( a5o ;KKK+ (a PA =Pood and r$ Administration>+ or$aniza#i&n en#ar$ada de re$(ar (a 'r/#ti#a médi#a * e( so de medi#amentos en (os 9stados Unidos+ a'ro%& e( sistema -irúr$i#o a Vin#i 'ara s so en -ir&)anos4 esto (o .a#e e(  'rimer sistema ro%otizado 'ara #ir$ía en .manos3 Lamenta%(emente+ e( #osto de( ro%ot es de #er#a de n mi((&n de d&(ares+ sin in#(ir e( materia( dese#.a%(e em'(eado  'ara #ada #ir$ía =#ada 'inza+ ti7era o #aterio #esta a(rededor de ;+KKK d&(ares * so(amente se 'ede ti(izar en !K #ir$ías>3 1in em%ar$o+ a 'esar de s a(to #osto+ (as ,enta7as de (a #ir$ía ro%&ti#a 'are#en  'rometedoras *a -e 'ermitir/+ 'or e7em'(o+ -e n mismo #ir7ano #ontro(e ,arios ro%ots en di)erentes -ir&)anos+ o in#(so e)e#tar te(e#ir$ías+ en (as -e e( #ir7ano no se en#entre ni si-iera #er#a de (a sa(a de #ir$ía3 ?odemos ima$inar a n es'e#ia(ista rea(izando na inter,en#i&n a distan#ia+ in#(so en e( es'a#io+ donde (os astronatas #o(o#ar/n a( 'a#iente %a7o (os %razos ro%otizados+ * e( #ir7ano en (a Tierra ((e,ar/ a #a%o (a #ir$ía3

. PLANTEA$IENTO DEL PROBLE$A. José María 1a%ater nos 'ro'so na de (as ne#esidades 'rin#i'a(es -e se ei$ía a nestro ro%ot * -e era ti(izar(o 'ara (a rea(iza#i&n de in#isiones en (a ti%ia o e( )émr de modo -e+ mediante n )i7ador eterno+ se 'osi%i(ite (a ni&n de am%as 'artes de .eso )ra#trado * se 'ermita -e+ tras n 'eríodo de tiem'o+ am%as 'artes de .eso roto -eden so(dadas3 Tam%ién de%ía ser #a'az de introd#ir (as ,ari((as de meta( 'or e( interior de (as in#isiones =,er Pi$ra !3>

'()u*a 1.- Artros#o'ia de rodi((a * estr#tra met/(i#a )i7adora3 Tam%ién tenemos #omo o%7eti,o (a ti(iza#i&n de( ro%ot 'ara (a rea(iza#i&n de endos#o'ias -e 'ermitir/n a n do#tor ins'e##iones ,isa(es en determinadas 'artes de( #er'o de( 'a#iente3 La endoscopia  es na té#ni#a dia$n&sti#a ti(izada so%re todo en medi#ina -e #onsiste en (a introd##i&n de n endos#o'io a tra,és de na in#isi&n -irúr$i#a 'ara (a ,isa(iza#i&n de n &r$ano .e#o o #a,idad #or'ora(3 La endos#o'ia adem/s de ser n  'ro#edimiento de dia$n&sti#o mínimamente in,asi,o+ tam%ién 'ede rea(izar manio%ras tera'éti#as #omo na #o(e#iste#tomía (a'aro#&'i#a+ artros#o'ia o (a toma de %io'sias3 9n nestro #aso+ orientaremos a( ro%ot a (a rea(iza#i&n de artroscopias -e es (a ,isa(iza#i&n de na #a,idad arti#(ar+ $enera(mente de (as rodi((as3 !" Jaime Martínez Verdú

José Lís Aznar Gomis

ROBÓTICA QUIRÚRGICA

/. SOLUCIÓN AL PROBLE$A. Uti(izaremos 'ara rea(izar todas estas tareas n ro%ot de F GL ta( * #omo  'odemos o%ser,ar en (a Pi$ra !3F+ donde en e( mode(o a(/m%ri#o de( ro%ot es 'osi%(e di)eren#iar dos $r'os #(aros de arti#(a#iones * na ú(tima adi#iona(8 H arti#(a#iones de #ar/#ter PRIS$0TICO 3 •

•

•

Una ,enta7a de estas arti#(a#iones es -e nos 'ermiten na colocación de( ro%ot *a sea atorni((ado a( te#.o #omo in#(ido dentro de n %astidor de  're'arado 'ara (a .i$iene ne#esaria de n -ir&)ano3 1i (o s7etamos de( te#.o tenemos (a $ran ,enta7a de -e 'odríamos tener(o arri%a de( todo #ando no se sase dando m/s es'a#io a (a sa(a de o'era#iones3 Adem/s+ s es'a#io de tra%a7o ser/ tan $rande #omo sea de $rande (a sa(a de o'era#iones3 9stas arti#(a#iones son (as -e #ontri%*en #on (a ma*or 'arte de todo e( espacio de trabajo de( #a( dis'one e( ro%ot3 9s de#ir+ #asi (a tota(idad de( es'a#io de( tra%a7o -e 'resenta e( ro%ot es %/si#amente de%ido a estas tres arti#(a#iones3 9stas arti#(a#iones ser/n (as -e esen#ia(mente a'orten (a  posición del efector final + es de#ir+ ser/ (as -e m/s 6'eso6 ten$an en e( 'osi#ionamiento de( e)e#tor )ina(3

; arti#(a#iones de #ar/#ter ROTATIO3 •

•

9ste $r'o de arti#(a#iones+ a di)eren#ia de (as anteriores+ se #ara#teriza 'or ser e( $r'o de arti#(a#iones -e 'ro'or#ionar/+ en $ran medida+ (a orientación a nuestro efector final 3  No es una muñeca esférica  

'es son úni#amente dos arti#(a#iones

rota#iona(es3 Con #in#o $rados de (i%ertad tendríamos s)i#iente siem're -e 6ata#/ramos6 a (a rodi((a desde arri%a #on n a%ani#o de !NK m/imo3 os .aría )a(ta n $rado de (i%ertad m/s si -isiéramos rea(izar in#isiones a (a a(tra de! $eme(o 'ero #omo no es así no ti(izamos ese $rado de (i%ertad3 9n ($ar de eso+ ti(izamos na .erramienta -e sea #a'az de rea(izar n mo,imiento de #ar/#ter PRIS$0TICO ADICIONAL 3 9( ú(timo es(a%&n #onsta de dos 'artes -e en e( di%7o est/n de)inidas3 Una  'arte es e( es(a%&n #*o )ina( es donde se s7etar/ (a  Black&Decker   * e( resto .a#e re)eren#ia a (a 'ro'ia  Black&Decker   -e s'onemos es #a'az de rea(izar n mo,imiento (inea(3 Tendríamos n ro%ot de F GL S 6!6 -e es a7eno a( ro%ot en #anto a #inem/ti#a3 La .erramienta a#tar/ #omo na arti#(a#i&n ti'o 'rism/ti#a -e ser/ (a -e rea(i#e e( mo,imiento (inea( de in#isi&n3 9sto es así 'ara s'erar e( 'ro%(ema -e #on((e,a rea(izar n movimiento lineal en el espacio  #on na m5e#a es)éri#a 'es ésta  'ro,o#a%a errores de im're#isi&n ta( * #omo ,imos en e( tema de '(ani)i#a#i&n de tra*e#torias3 !"N Jaime Martínez Verdú

José Lís Aznar Gomis

ROBÓTICA QUIRÚRGICA e i$a( modo+ es n F GL S 6!6 si (e a5adimos e( ta(adro * n F GL S 6;6 si (e a5adimos (a #/mara3

d 1(t)

Tres articulaciones prismáticas d 3(  t) d 2(  t)

Dos articulaciones de rotación θ 3(  t)

BLACK&DECKER

"Articulación" extra prismática

θ 4(  t)

'()u*a 1. 9s-ema $enera( de( Qiro%ot3

!": Jaime Martínez Verdú

José Lís Aznar Gomis

CINEMÁTICA DIRECTA DEL QUIROBOT

2

Capí tulo CINEMÁTICA  D DIR ECTA  D DEL  Q QUIR OBOT

"TODO LO QUE SE MUEVE ES MOVIDO POR OTRO"  Aristóteles

Des&!s de todo 'o (isto) estamos *a en +ondi+iones de a,ordar e' &ro,'ema de 'a +inem-ti+a dire+ta) e' +!a' trata de en+ontrar 'a .orma e/&'í+ita de 'a .!n+i0n 1!e re'a+iona e' es&a+io arti+!'ar +on e' es&a+io +artesiano de &osi+iones2orienta+iones3 Esta .!n+i0n4  f  4 J  → ℜ

5

toma +omo ar6!mento !n (e+tor en e' es&a+io de arti+!'a+iones) 1!e tiene tantas +om&onentes +omo 6rados de 'i,ertad ten6a 'a +adena +inem-ti+a 1!e se +onsidere 71!e en n!estro +aso es úni+amente de 8 6rados de 'i,ertad9) * de(!e'(e !n (e+tor de 5 +om&onentes: 'as tres &rimeras ser-n 'a &osi+i0n en e' es&a+io de' &!nto termina' de 'a +adena) e/&resada en !n sistema de re.eren+ia e/terno) * 'as tres ú'timas) 'a orienta+i0n) e/&resada ,ien +omo 'as +om&onentes 7 a x) a y) a z 9 de' (e+tor de a&ro/ima+i0n) ,ien +omo -n6!'os de orienta+i0n 7E!'er o roll, pitch and yaw 93

1. EL FORMALISMO DE DENAVIT!ARTENBER. La .orma en 1!e +onse6!iremos +ono+er e' (e+tor de ℜ5 antes men+ionado sermediante 'a +onstr!++i0n de 'a matriz de trans.orma+i0n ;omo6nea T 1!e re'a+iona e' sistema so'idario a' &!nto termina' +on !n sistema de re.eren+ia .i: >: >: %9) 1!e e/&resa 'as +oordenadas ;omo6neas de'  &!nto termina' res&e+to a s! &ro&io sistema) o,tendremos stas res&e+to a' sistema de' m!ndo3 ?) &or otra &arte) se6ún se (io en 'a en e' +a&ít!'o @ de' 'i,ro Fundamentos de  Robótica  de Antonio Barrientos  Herramientas matemáticas para la localización espacial ) e/isten .0rm!'as 1!e re'a+ionan 'os -n6!'os de orienta+i0n en +!a'1!iera de s!s e/&resiones +on 'os e'ementos de 'a s!,matriz de rota+i0n de T3 #$% Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT =e6!idamente se des+ri,e +on deta''e e' &ro+eso de +onstr!++i0n de T  &ara +adenas +inem-ti+as a,iertas) en 'as 1!e +ada arti+!'a+i0n ten6a !n s0'o 6rado de 'i,ertad3 E' +aso de arti+!'a+iones +on m-s de !n 6rado de 'i,ertad no &resenta nin6ún  &ro,'ema4 ,astaría +on +onsiderar 'a arti+!'a+i0n +omo si se tratase de dos) !nidas &or !n en'a+e .i+ti+io de 'on6it!d n!'a3 =!stan+ia'mente) e' &ro+eso +onsiste en .i) * no se +!enta +omo 6rado de 'i,ertad3 La arti+!'a+i0n % ser- 'a 1!e +one+te 'a ,ase a' &rimer en'a+e m0(i': 'as arti+!'a+iones +omienzan) &!es) a n!merarse desde %) * no e/iste arti+!'a+i0n a' .ina' de' ú'timo en'a+e3 E' e
La dire++i0n de des&'azamiento) en arti+!'a+iones tras'a+iona'es3

•

E' e
A +ontin!a+i0n) 'os &ar-metros D de +ada en'a+e son +!atro números rea'es) dos de e''os re&resentando -n6!'os) * 'os otros dos) distan+ias) de.inidos de' si6!iente modo4 •

•

•

ai 

es 'a mínima distan+ia 7distan+ia &er&endi+!'ar9 entre e' e
d i  es 'a distan+ia entre 'os &!ntos de interse++i0n i 2 i  % +on e' e
de 'a norma' a 'os e
medida a 'o 'ar6o de este e
θ i es e' -n6!'o entre 'a norma' a 'os e
'a norma' a 'os e
#$# Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT Vase 'a re&resenta+i0n de estos &ar-metros en 'a .i6!ra %3%3 O,sr(ese 1!e  ,asta e' +ono+imiento de 'os dos e
F#$u%a 2.14 ar-metros D de !n en'a+e 6enri+o3 A;ora) 'as normas &ara determinar 'os e
E' e
•

E' e
•

E' e
•

E' ori6en se .i
F#$u%a 2.24 =istemas de +oordenadas .i
Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT Vanse en 'a .i6!ra #3# 'os sistemas de +oordenadas i  % e i sit!ados so,re 'os en'a+es de' e
E' e de,e .i) dado 1!e no e/iste nin6ún  z % a' +!a' ;a+er'e ser tam,in &er&endi+!'ar3 An-'o6amente) e' ori6en de' sistema > es !n &!nto ar,itrario so,re e' e3

•

De,e e/istir !n ú'timo sistema de +oordenadas) .i
•

C!ando dos e
•

C!ando dos e
•

C!ando dos e3

 N0tese 1!e4 •

ara e' +aso de !na arti+!'a+i0n rota+iona') θ i es e' -n6!'o de rota+i0n 1!e se trata de !na .!n+i0n tem&ora') * d i) ai * αi son +onstantes3

•

En !na arti+!'a+i0n tras'a+iona') d i es distan+ia de tras'a+i0n .!n+i0n de' tiem&o) * θ i) * αi son +onstantes3 ai es tam,in +onstante) * !s!a'mente > 7s!e'e +orres&onder a' ú'timo de 'os +asos &arti+!'ares anteriores93

Todo &ar-metro +onstante en !n ro,ot 'o es &or +onstr!++i0n de' mismo) *  &ermane+e +onstante en toda +ir+!nstan+ia) sa'(o 1!e se a'tere me+-ni+amente a' ro,ot3 La a++i0n de 'os a+t!adores 1!e &ro(o+ar-n e' mo(imiento s0'o +am,ia e' &ar-metro (aria,'e de +ada arti+!'a+i0n) θ i &ara rota+iona'es) * d i &ara tras'a+iona'es3 or otra &arte) 'o norma' es +onstr!ir ro,ots en 'os 1!e 'os e) >) %K> 0 #>3 #$G Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT E' si6!iente &aso es determinar 'a trans.orma+i0n 1!e ''e(aría e' sistema i  % ;asta e' i3 N0tese 1!e &odemos ;a+er'os +oin+idir a&'i+ando s!+esi(amente 'as si6!ientes trans.orma+iones4 •

Rotar a'rededor de  z i-% !n -n6!'o θ i3 Esto de
•

Tras'adar a 'o 'ar6o de' e
•

Tras'adar a 'o 'ar6o de' e
•

Rotar a'rededor de  x'' i-% 7o de  xi) a;ora +oin+iden9 !n -n6!'o αi3 Esto ;ar- +oin+idir  z'' i-% +on z i 7*) &or tanto) y'' i-% +on yi9 * est- +on+'!ido3 Así &!es) &odemos es+ri,ir 'a trans.orma+i0n desde e' sistema i  % ;asta e' i

+omo i −%

 !i = RO ( z i −% )θ i ) ⋅ r (>)>) d i ) ⋅ r (ai )>)>) ⋅ RO ( x i −% ) α i )

donde 'as matri+es se ;an &ostm!ti&'i+ado) &!es 'as trans.orma+iones se e.e+túan siem&re res&e+to a 'os n!e(os e   >

− # θ i

>

>  %

>

>

> %

>

> ai   %

" θ i

>

>  >

%

>

> >

%

>

>

%

>

%

>

%

>

>

>

>

>

>

 

⋅ >  >   %   >

 

⋅ d i   >   %   >

 

>  >

⋅ >  >   %   >

>

>

> 

" α i

− # α i

> 

# α i

" α i

>

>



 >   % 

Una (ez o&erado e' &rod!+to de di+;as matri+es o,tenemos 1!e 'a matriz de trans.orma+i0n ;omo6nea 6enera'izada &ara &ar-metros θ i) d i) ai * αi es 'a si6!iente4    i −% Ai =    

+os7θ i 9

− +os7α i 9 ⋅ sen7θ i 9

sen7α i 9 ⋅ sen7θ i 9

ai ⋅ +os7θ i 9

sen7θ i 9

+os7α i 9 ⋅ +os7θ i 9

− sen7α i 9 ⋅ +os7θ i 9

ai ⋅ sen7θ i 9

>

sen7α i 9

+os7α i 9

d i

>

>

>

%

      

E' res!'tado .ina' es 'a matriz D &ara e' en'a+e i) 'a +!a') +ono+iendo 'as +ara+terísti+as 6eomtri+as de di+;a arti+!'a+i0n * de s! en'a+e) da 'a trans.orma+i0n 1!e ''e(a de +oordenadas e/&resadas en e' sistema i  a +oordenadas e/&resadas en e' sistema i   %3 I6!a'mente) da 'a &osi+i0n de' ori6en de' sistema i  res&e+to a' i  % 7(iendo 'a G +o'!mna9) así +omo s! orienta+i0n 7(iendo 'a s!,matriz de rota+i0n93

#$8 Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT A;ora estamos en +ondi+iones de dar 'as de.ini+iones a'ternati(as &ara 'os  &ar-metros de D) 1!e son4 es 'a distan+ia &er&endi+!'ar 7distan+ia mínima9 entre 'os e
•

ai

•

αi

•

θ i es

•

d i es

es e' -n6!'o 1!e .orma e' e
Una (ez todos 'os &ar-metros estn identi.i+ados) * 'as matri+es D es+ritas) re+ordemos 1!e 'o 1!e se &retende es en+ontrar 'a trans.orma+i0n entre e' sistema de' m!ndo 7sistema >9 * e' ú'timo 7sistema n93 Es o,(io 1!e >

>

%

n −%

 !n =  !%⋅ !# L  !n

Cada e'emento de 'a matriz i%Ai es .!n+i0n de ai: αi: θ i * d i) 7ai * αi +onstantes  &ara +ada ro,ot) * d i o ,ien θ i (aria,'es &ara +ada ti&o de arti+!'a+i0n9) * &or tanto +ada e'emento de >An es) en &rin+i&io) .!n+i0n de todos 'os 7 ai: αi: θ i * d i9 +on i F %) #) @)) n3

R&'u(#&)*o. Los &asos 1!e de,en se6!irse &ara 'a +onstr!++i0n de 'a +inem-ti+a dire+ta son4 %3 Identi.i+ar +!-ntos 6rados de 'i,ertad tiene e' ro,ot) * +!-ntas arti+!'a+iones: si t!(iera a'6!na arti+!'a+i0n +on m-s de !n 6rado de 'i,ertad 7di6amos) n9 ;a,r- n sistemas s!&er&!estos en !n &!nto3 #3 Asi6nar 'os eAn3 #$5 Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

2. RESOLUCI+N DEL PROBLEMA DE LA CINEMÁTICA DIRECTA. PARÁMETROS DENAVIT!ARTENBER. 1. I*&)t#,#-a% -u)to' $%a*o' *& l#/&%ta* t#&)& &l %o/ot0  -u)ta' a%t#-ula-#o)&' '# tu3#&%a al$u)a a%t#-ula-#4) -o) (' *& u) $%a*o *& l#/&%ta* 5*#$a(o'0 n6 7a/% n '#'t&(a' 'up&%pu&'to' &) u) pu)to. Este mani&!'ador) diseado &or Jos L!ís Aznar * Jaime Martínez +on  &ro&0sitos &rin+i&a'mente mdi+os) tiene 8 6rados de 'i,ertad) tres de e''os tras'a+iona'es * dos rota+iona'es3 ara ana'izar'o !ti'izando e' &ro+edimiento de Dena(it$arten,er6 ne+esitamos e' es1!ema si6!iente donde se re&resenta s! mode'o a'-m,ri+o3 artimos de !na +on.i6!ra+i0n +!a'es1!iera) si ,ien es a+onse
> # %

@

G

F#$u%a 2.8a 4 Re&resenta+i0n de' es1!ema a'-m,ri+o de' ro,ot 1!irúr6i+o3  N0tese tiene 8 arti+!'a+iones 1!e en!meraremos desde +ero ;asta +in+o) siendo 'a eti1!etada +on > a1!e''a .i
Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

2. A'#$)a% lo' &9&' z 0 'a/#&)*o :u& z i-1 &' &l &9& *& la a%t#-ula-#4) i . Es ;orizonta') es e' e

 

;orizonta') es e' e
Es

d 1(t)

 z >

(erti+a') es e' e
Es

 z %

=i6!e

θ 4(t)  

 z @

e' se6!ndo * ú'timo e
Es

θ 5(t)  

 z G

sitúa de manera 1!e +oin+ida +on 'a dire++i0n de  z G3 Aún si no ;!,iera !n en'a+e .ísi+o a' 1!e (a*a !nido) es ne+esario in+'!ir'o &ara 1!e de +!enta de 'a ú'tima rota+i0n3 or otra &arte) e' ro,ot tiene 8 6rados de 'i,ertad: &or e''o) de,e ;a,er 8 sistemas m0(i'es 7'os números de 'a arti+!'a+i0n % a %a 8) re+ordemos 1!e e' > era .i
 z #

d 2(t)  

'a dire++i0n de  z #) * es e' &rimer e
d 3(t)  

  z 8  se

 z 8 F#$u%a 2.8/4 E'e++i0n de 'os e
#$K Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

8. A'#$)a% to*o' lo' &9&' x 0 'a/#&)*o :u& x i   &' p&%p&)*#-ula% a z i0   a z i-1  3a &) la *#%&--#4) *& la p&%p&)*#-ula% -o(;). * &or 'o tanto (erti+a') &ero a' no ;a,er !n e

=er- &er&endi+!'ar a  z >)

 x>

 x%

ser &er&endi+!'ar a s!  &ro&io  z  7 z %9 * a'  z anterior 7 z >93 Estos dos e
De,e

 z >

 z %  x#

&er&endi+!'ar a  z % * a  z #) 1!e tam,in se +ortan) determinando !n &'ano (erti+a') &or 'o 1!e  x# de,erir ;a+ia .!era o ;a+ia dentro3 or e
Es

Es

 z G  xG  z 8  x8

&er&endi+!'ar a  z @ * a  z G) * de,ido a 1!e am,os interse+tan determinando !n &'ano 1!e +ontendría a' e
 z @

 x@

!n +aso es&e+ia'3 Como  z # * son +oin+identes) ;a* in.initas dire++iones trans(ersa'es3 C!ando dos e
 z #

=er-

F#$u%a 2.8-4 E'e++i0n de 'os e
!n +aso es&e+ia'3 Como  z G *  z 8  son &ara'e'os) ;a* in.initas dire++iones  &er&endi+!'ares3 =e a+onse
  Es

#$ Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

<. A'#$)a% lo' &9&' y0 *& (o*o :u& '& -u(pla :u& x i  × yi = z i.   x>  y> d 1(t)

 z >

 x%

 y #  z %  y%

d 3 (t)

 x#

d 2 (t)

 z #

 y@

θ 4 (t)

 x@

 z @

θ 5 (t)

 xG

 z G

 yG

 z 8  x8

 y8 F#$u%a 2.8*4 E'e++i0n de 'os e
Res&e+to a 'os orí6enes) e' de' sistema  # >S se .i  +orta a  z %) * otro &!nto úni+o donde  z %  +orta a  z #3 E''os tres de&enden .!ertemente de' .i Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT Con res&e+to a esto) nos &re6!ntamos si ne+esitamos &oner otro sistema e/tra en 'a mano3 O,sr(ese 1!e e' ú'timo sistema 1!e nos res!'t0 &or a&'i+a+i0n de 'as re6'as era e' 8) * 1!e ste tiene4 •

=! e
•

=! e
•

=! e
θ 5 (t)

 xG

 z G

 y G

r

o r

)

 z 8  x8

r

a

 y8

F#$u%a 2.8&4 Cam,io de ,ase &ara tener !n triedro ) o a en e' e.e+tor .ina'3 De +'ases de teoría sa,emos 1!e de,e e/istir !n ú'timo sistema de +oordenadas) .i  > 8 A8 =  %   >

>

%

−%

>

>

>

>

>

>

r   x8 +oin+ide +on 'a dire++i0n de a La dire++i0n de' e  r  → La dire++i0n de' e  La dire++i0n de' e
# $ %% Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT >. D&t&%(#)a% lo' pa%(&t%o' 5ai  αi  θ i  d i 6 *& -a*a a%t#-ula-#4) po% #)'p&--#4) 3#'ual.

Pa%(&t%o' D! pa%a #% *&'*& &l '#'t&(a ? S @ 7a'ta &l ? S 1 (a1; α1; θ 1; d 16.

•

 x>  y> d 1(t)

 z >

 x%  x>

 x>

 z %

 y>  y%

 z >

d 2(t)  

 z >

 y>

 x>

 x>

 y> θ 1=

 y>

 y> d 1= d 1(t )

0º

 x>

 z >

a 1= 0 

 z >

α1= 90º

 z >

F#$u%a 2.S a' sistema # %S3 •

Pa%(&t%o' D! pa%a #% *&'*& &l '#'t&(a ? S 1 7a'ta &l ? S 2 (a2; α2; θ 2; *26.

d 1(t)

 x%  y#

 x%

 z %  y%

 z %

 x#

d 2(t)  

 y%

d 3(t)  

 z #  x%  z %

 y%

 z % θ 2=  

90º

 x%

 y%

 z % d 2=   d 2(t    )

 x%

 y%

 z % a 2=   0 

 x%

 y%

α2 = –90º

F#$u%a 2.
Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

Pa%(&t%o' D! pa%a #% *&'*& &l '#'t&(a ? S 2 7a'ta &l ? S 8 (a8; α8; θ 8; *86.

•

 y#

d 2(t)  

 y#  x#

 z #

 x#

 y#

 z #  y@

θ 4(t)  

 x@  y# θ 3=  

0º

 x#

 z @

 y#  z #

d 3=   d 3(t    )

 z #

 x#

 y#

 z #

 x#

a 3=   0 

 x#

 z #

α3 = 0º

F#$u%a 2.<-4 ar-metros D &ara &asar de' sistema  # #S a' sistema  # @S3 Pa%(&t%o' D! pa%a #% *&'*& &l '#'t&(a ? S 8 7a'ta &l ? S < (a<; α<; θ <; *<6.

•

 y@

θ 4(t)  

 x@

 y@

 z @  z @

 x@

 x@

θ 5(t)  

 z @

 z G

 xG  y@

θ 4=   θ  5 (t )

 x@

 yG  y@

 z @

d 4=   l 4 

 y@

 x@

 y@  z @

a 4=   0 

 x@

 z @ α4 = 90º

F#$u%a 2.<*4 ar-metros D &ara &asar de' sistema  # @S a' sistema  # GS3 # $ %@ Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

Pa%(&t%o' D! pa%a #% *&'*& &l '#'t&(a ? S  ? S < 7a'ta &l ? S  ? S > (a>; α>; θ >; *>6.

•

θ 5  5(t)  

 xG

 z G  xG

 z G  z G

 yG

 yG  xG

 z 8  x8

 y8

 z G

θ 5  5=   θ  5   5 (t )

 xG

 y G

 yG

 z G d 5  5=   0 

 xG

 yG

 z G

a 5  5=   l 5  5 

 x G

 y G

α5 = 0º

F#$u%a 2.<&4 ar-metros D &ara &asar de' sistema  # GS a' sistema  # 8S3

# $ %G Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

. Co)'t%u#% la ta/la *& pa%(&t%o'0  a pa%t#% *& -a*a u)a *& 'u' ,#la'0 u'a)*o la ,4%(ula $&)&%al *& la (at%# D!0 &'-%#/#% -a*a u)a *& la' #1Ai . La ta,'a de 'os &ar-metros de Dena(it$arten,er6 &ara +ada !no de 'os es'a,ones es 'a si6!iente4

 ESLAB! % # @ G 8

θ i i  > > > θ 7t 9 θ (7t 9

d i i  d $7t 9 d %7t 9 d &7t 9

ai  > > > >

l 

>

l (

Las matri+es de trans.orma+i0n ;omo6nea 'as %  > > A% =  >   >

>

>

>

>

−%

>

%

>

>

>

>  % % A# =  >   >



 d %   % 

 +os7θ G 7t 99   sen7θ G 7t 99 @ AG =   >   >  +os7θ 8 7t 99   sen7θ 8 7t 99 G A8 =   >   >

αi  > > > > >

i%

>

−%

>

>

>

>

−%

>

>

>

Ai son 'as si6!ientes4



 d #   % 

%  > # A@ =  >   >

>

sen7θ G 7t 99

>

>

− +os7θ G 7t 99

>

%

>

l G

>

>

%

>

>

>

%

>

>

>

%

>

>



 d @   % 

      

− sen7θ 8 7t 99

>

l 8 ⋅ +os7θ 8 7t 99

+os7θ 8 7t 99

>

l 8 ⋅ sen7θ 8 7t 99

>

%

>

>

>

%

    

Tam,in se +a'+!'an &or ins&e++i0n (is!a' dire+ta 'as matri+es &ara e' +am,io de  ,ase tanto &ara &ara e' sistema de' es'a,0n es'a,0n .i  > > A> =  %   >

>

%

>

−%

>

>

>

>

>

>

>  > 8 A8 =  %   >



 >  % 

>

%

>

−%

>

>

>

>

>

>



 >  % 

En 'as &-6inas si6!ientes rea'izaremos 'as o&era+iones 1!e nos ;an &ermitido o,tener 'as e/&resiones de 'as matri+es i%Ai3

# $ %8 Jaime Martínez Verdú

Jos L!ís Aznar "omis

ara 'a &rimera arti+!'a+i0n 1!e es de +ar-+ter tras'a+iona') s! matriz de trans.orma+i0n ;omo6nea >A% es4

•

>

A % = RO ( z > )> J ) ⋅ r (>)>) d % 7t 9 ) ⋅ r (>)>)> ) ⋅ RO ( x M M > )>J )

   > A% =     •

%

#

−

+os7>J 9 ⋅ sen7>J 9

sen7> J 9 ⋅ sen7>J 9

sen7>J 9

+os7>J 9 ⋅ +os7>J 9

>

sen7> J 9

+os7>J 9

d % 7t 9

>

>

>

%

−

      =      

> ⋅ +os7>J 9

sen7>J 9 ⋅ +os7>J 9

> ⋅ sen7>J 9

> 

%

>

>

>

>

>

%

>

d % (t )

>

>

>

% 

−



> 

%

 

ara 'a se6!nda arti+!'a+i0n 1!e es de +ar-+ter tras'a+iona') s! matriz de trans.orma+i0n ;omo6nea %A# es4 A # = RO ( z % )>J ) ⋅ r (>)>) d # 7t 9 ) ⋅ r (>)>)> ) ⋅ RO ( x M M% )−> J )

   % A# =     •

+os7>J 9

+os7>J 9

−

+os7 −>J 9 ⋅ sen7>J 9

sen7−>J 9 ⋅ sen7>J 9

sen7>J 9

+os7 −>J 9 ⋅ +os7>J 9

>

sen7−>J 9

+os7−>J 9

d # 7t 9

>

>

>

%

−

sen7 −>J 9 ⋅ +os7>J 9

      =      

> ⋅ +os7>J 9 > ⋅ sen7>J 9

>

>

%

>

> >

−

%

>

ara 'a ter+era arti+!'a+i0n 1!e es de +ar-+ter tras'a+iona') s! matriz de trans.orma+i0n ;omo6nea #A@ es4 A@

=

 RO ( z # )>J ) ⋅ r (>)>) d @ 7t 9 ) ⋅ r (>)>)> )⋅ RO ( x M M # )>J )

   # A@ =    

+os7>J 9

−

+os7>J 9 ⋅ sen7>J 9

sen7>J 9 ⋅ sen7>J 9

sen7>J 9

+os7>J 9 ⋅ +os7>J 9

>

sen7>J 9

+os7>J 9

d @ 7t 9

>

>

>

%

−

sen7>J 9 ⋅ +os7>J 9

> ⋅ +os7>J 9 > ⋅ sen7>J 9

      =      

%

>

>

> 

>

%

>

> 

>

>

%

d @ (t )

>

>

>

% 

  

−

%

> 



>

> 

>

d # (t )

>

% 

 

• @

ara 'a +!arta arti+!'a+i0n 1!e es de +ar-+ter rota+iona') s! matriz de trans.orma+i0n ;omo6nea @AG es4 A G = RO ( z @ ) θ G 7t 9 ) ⋅ r (>)>) l G ) ⋅ r (>)>)>) ⋅ RO ( x M M @ )>J )

   @ AG =     • G

−

+os7>J 9 ⋅ sen7θ G 7t 99

sen7>J 9 ⋅ sen7θ G 7t 99

sen7θ G 7t 99

+os7>J 9 ⋅ +os7θ G 7t 99

>

sen7>J 9

+os7>J 9

l G

>

>

>

%

−

  +os7θ G 7t 99     sen7θ G 7t 99 =   >     >

> ⋅ +os7θ G 7t 99

sen7>J 9 ⋅ +os7θ G 7t 99

> ⋅ sen7θ G 7t 99

> >

sen7θ G 7t 99 −

+os7θ G 7t 99

> >

%

>

l G

>

>

%

      

ara 'a 1!inta arti+!'a+i0n 1!e es de +ar-+ter rota+iona') s! matriz de trans.orma+i0n ;omo6nea GA8 es4 A 8 = RO ( z @ ) θ 8 7t 9 ) ⋅ r (>)>)>) ⋅ r (>)>) l 8 ) ⋅ RO ( x M M @ )>J )

   G A8 =     •

+os7θ G 7t 99

+os7θ 8 7t 99

−

+os7>J 9 ⋅ sen7θ 8 7t 99

sen7>J 9 ⋅ sen7θ 8 7t 99

sen7θ 8 7t 99

+os7>J 9 ⋅ +os7θ 8 7t 99

>

sen7>J 9

+os7>J 9

>

>

>

−

sen7>J 9 ⋅ +os7θ 8 7t 99

l 8 ⋅ +os7θ 8 7t 99 l 8 ⋅ sen7θ 8 7t 99 > %

  +os7θ 8 7t 99     sen7θ 8 7t 99 =   >     >

>

l 8 ⋅ +os7θ 8 7t 99

+os7θ 8 7t 99

>

l 8 ⋅ sen7θ 8 7t 99

>

%

>

>

>

%

−

sen7θ 8 7t 99

    

>

La matriz &ara rea'izar e' +am,io de ,ase +an0ni+a a' sistema R# )S A> es4

>  > > A> =  %   >

> −

%

% >

>

>

>

>

 z e

>

 La dire++i0n de' e >   → La dire++i0n de' e >   La dire++i0n de' e % 

Em&'eado esta matriz de +am,io de ,ase &odemos e(itarnos errores a' o,tener 'as +oordenadas de' e.e+tor .ina'3 =i no !ti'izamos 'a matriz >A>  * 'a matriz T  nos in.orma so,re 1!e e' e.e+tor .ina' est- en 'as +oordenadas T x*  y*  z *U ) esto si6ni.i+aría 1!e z F  x*) * F $ y*) /F  z *3 =i !ti'izamos esta >A>  'as +oordenadas son dire+tas &!es !n (e+tor de  &osi+ionamiento T x* y* z *U  im&'i+aría 1!e 'a ;erramienta estaría en z F  x*) * F y*) /F z *3

 x >

 y e

 x e

 y >

 z >

F#$u%a 2.>4 Cam,io de ,ase &ara tener !n triedro +an0ni+o en e' sistema de' m!ndo3

CINEMÁTICA DIRECTA DEL QUIROBOT

. Mult#pl#-a% to*a' &lla' pa%a $&)&%a% la @A). =i ;a+emos !na re+o&i'a+i0n de 'as matri+es de trans.orma+iones ;omo6neas tenemos 1!e a &artir de 'as o&era+iones antes rea'izadas tenemos 1!e 'as matri+es i$%Ai * 'a matriz T son 'as si6!ientes4 >  > > A> =  %   >

>

%

>

−%

>

>

>

>

>

>

>  > 8 A8 =  %   >



 >  %  %  > > A% =  >   > >  % % A# =  >   > %  > # A@ =  >   >

 +os7θ G 7t 99   sen7θ G 7t 99 @ AG =   >   >  +os7θ 8 7t 99   sen7θ 8 7t 99 G A8 =   >   >

T=> A@ ⋅@ A8

>

>

>

>

−%

>

%

>

>

>

 d %   % 

>

−%

>

>

>

>

−%

>

d # 

>

>

 % 

>

>

>

%

>

>

%

>

>

>

%

>

−%

>

>

>

>

>

>



 >  % 



 

 >  d @   % 

>

sen7θ G 7t 99

>

>

− +os7θ G 7t 99

>

%

>

l G

>

>

%

− sen7θ 8 7t 99

>

+os7θ 8 7t 99

>

>

%

>

>

      

l 8 ⋅ +os7θ 8 7t 99

 l 8 ⋅ sen7θ 8 7t 99   >   %

T= > A > ⋅> A% ⋅% A # ⋅# A @ ⋅@ A G ⋅ G A 8 ⋅8 A 8 > A@ => A> ⋅> A%⋅% A# ⋅# A@ * @A8 =@ AG ⋅G A8 ⋅8 A8 +on

# $ %K Jaime Martínez Verdú

Jos L!ís Aznar "omis

   > A@ =    

> >

> −

%

% >

%

>

>

>

>

>

>  %

  >  > ⋅ >  >   %   >

> >

> −

%

%

>

>

>

>   >

  >   % ⋅ d % 7t 9  >   %   >

> > −

%

>

−

%

> > >

>   %

  >   > ⋅ d # 7t 9  >   %   >

> sen7θ G 7t 99 > − sen7θ 8 7t 99  +os7θ G 7t 99   +os7θ 8 7t 99     sen7θ G 7t 99   sen7θ 8 7t 99 − +os7θ G 7t 99 > > +os7θ 8 7t 99 @ ⋅ A8 =     > % > l G > >       > > > % > >  sen7θ G 7t 99 +os7θ G 7t 99 ⋅ sen7θ 8 7t 99 +os7θ G 7t 99 ⋅ +os7θ 8 7t 99 l 8 ⋅ +os7θ G 7t 99 ⋅ +os7θ 8 7t 99   − +os7θ G 7t 99 sen7θ G 7t 99 ⋅ sen7θ 8 7t 99 sen7θ G 7t 99 ⋅ +os7θ 8 7t 99 l 8 ⋅ sen7θ G 7t 99 ⋅ +os7θ 8 7t 99  =    − +os7θ 8 7t 99 > sen7θ 8 7t 99 l G + l 8 ⋅ sen7θ 8 7t 99     > > > %

>

>

%

>

>

%

>

> > > % >

> 

 >   d @ 7t 9   % 

=

      

% >

> −

%

>

>

>

>

l 8 ⋅ +os7θ 8 7t 99  >

  l 8 ⋅ sen7θ 8 7t 99  > ⋅  % >     > %

> > d %7t 9  +os7θ G 7t 99 ⋅ +os7θ 8 7t 99 − +os7θ G 7t 99 ⋅ sen7θ 8 7t 99 sen7θ G 7t 99 l 8 ⋅ +os7θ G 7t 99 ⋅ +os7θ 8 7t 99  %      > −% > d # 7t 9  sen7θ G 7t 99 ⋅ +os7θ 8 7t 99 − sen7θ G 7t 99 ⋅ sen7θ 8 7t 99 − +os7θ G 7t 99 l 8 ⋅ sen7θ G 7t 99 ⋅ +os7θ 8 7t 99 ⋅  T=  > −% − d @ 7t 9   > sen7θ 8 7t 99 +os7θ 8 7t 99 > l G + l 8 ⋅ sen7θ 8 7t 99       >  > > %   > > > % +os7θ G 7t 99 ⋅ +os7θ 8 7t 99 d %7t 9 + l 8 ⋅ +os7θ G 7t 99 ⋅ +os7θ 8 7t 99 sen7θ G 7t 99 +os7θ G 7t 99 ⋅ sen7θ 8 7t 99   +os7θ G 7t 99 − sen7θ G 7t 99 ⋅ sen7θ 8 7t 99 − sen7θ G 7t 99 ⋅ +os7θ 8 7t 99 d # 7t 9 − l 8 ⋅ sen7θ G 7t 99 ⋅ +os7θ 8 7t 99  =    − sen7θ 8 7t 99 − d @ 7t 9 − l G − l 8 ⋅ sen7θ 8 7t 99 > +os7θ 8 7t 99     > > > %

>

d % 7t 9 

>

d # 7t 9 

−

 

%

−

>

%

%

>

>

>

>

>

>

>

>

% 

> −

%

d @ 7t 9 

 = 

 

CINEMÁTICA DIRECTA DEL QUIROBOT La matriz de trans.orma+i0n ;omo6nea &ara n!estro ro,ot &resenta 'a si6!iente .orma4  n x   n y   n z    >

o x

a x

o y

a y

o z 

a z 

>

>

 p x 

  p y    p z    % 

=

 sen7 θ G 7 t 99   +os7 θ G 7 t 99   >   >

+os7 θ G 7 t 99 ⋅ sen7 θ 8 7 t 99 −

sen7 θ G 7 t 99 ⋅ sen7 θ 8 7 t 99

+os7 θ G 7 t 99 ⋅ +os7 θ 8 7 t 99 −

sen7 θ G 7 t 99 ⋅ +os7 θ 8 7 t 99

+os7 θ 8 7 t 99

−

>

sen7 θ 8 7 t 99 >

d % 7 t 9 + l 8 ⋅ +os7 θ G 7 t 99 ⋅ +os7 θ 8 7 t 99 



d # 7 t 9 − l 8 ⋅ sen7 θ G 7 t 99 ⋅ +os7 θ 8 7 t 99  − d @ 7 t 9 − l G − l 8 ⋅ sen7

%

θ 8 7 t 99

   

# $ #> Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

8. IMPLEMENTACI+N DEL C+DIO DE LA CINEMÁTICA DIRECTA. La .!n+i0n dire+ta res!e'(e e' &ro,'ema +inem-ti+o dire+to de' ro,ot 1!irúr6i+o de 8 6rados de 'i,ertad3 ara e''o toma +omo úni+o &ar-metro !n (e+tor : donde (ienen a'ma+enadas +omo +om&onentes +ada !na de 'as (aria,'es arti+!'ares de 'a si6!iente manera4 + = [+% +% 4= d % 7t 9

+#

+@

+G

+ # 4= d # 7t 9

+ G 4= θ G 7t 9

 

+8 ]

+@ 4= d @ 7t 9

+8 4= θ 8 7t 9

a de tenerse en +onsidera+i0n 1!e 'as 'on6it!des d $7t 9) d %7t 9 * d &7t 9 es&e+i.i+adas &or e' !s!ario demandadas a 'as arti+!'a+iones &rism-ti+as no &!eden ser ma*ores a 'as 'on6it!des de 'as 6!ías &or donde +o'o+adas 'as arti+!'a+iones3 Adem-s) no est- &ermitido introd!+ir -n6!'os &ara 'as (aria,'es rotati(as 1!e &ro(o1!en sin6!'aridades *) de,ido a esto) no se &odr-n a'+anzar a'6!nas +on.i6!ra+iones3 or e''o) es re+omenda,'e tener en +!enta) &or e *) tam,in) +on 'a (aria,'e arti+!'ar θ (7t 9 de,emos tra,a ≤ d % 7t 9 ≤ # m > ≤ d # 7t 9 ≤ # m > ≤ d @ 7t 9 ≤ # m > ≤ θ G 7t 9 ≤ @5> − 8 ≤ θ G 7t 9 ≤ #88

E' +0di6o .!ente de 'a .!n+i0n DIRECTA &ara n!estro ro,ot de 8 6rados de 'i,ertad es e' si6!iente4

1 2 8 < >   G H 1@ 11 12 18 1< 1> 1 1 1G 1H 2@

.!n+tion A>8 F dire+ta719 'GF>3G: '8F>3#:  ar-metros Dena(it$arten,er6 de' ro,ot teta F  > &i2# > d F  17%9 17#9 17@9 a F > > > a'.a F  &i2# &i2# >

17G9 'G > &i2#

1789 > '8 >

: : : :

 Matri+es de trans.orma+i0n ;omo6nea entre sistemas de +onse+!ti(os A>> F > > % >:> $% > >:% > > >:> > > %: A>% F dena(it7teta7%9) d7%9) a7%9) a'.a7%99: A%# F dena(it7teta7#9) d7#9) a7#9) a'.a7#99: A#@ F dena(it7teta7@9) d7@9) a7@9) a'.a7@99: A@G F dena(it7teta7G9) d7G9) a7G9) a'.a7G99:  Matriz de trans.orma+i0n de' &rimer a' ú'timo sistema de +oordenadas A>8 F A>> W A>% W A%# W A#@ W A@G W AG8 W A>>: # $ #%

Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT En 'as &rimeras 'íneas de' +0di6o introd!+imos 'as dimensiones de' ro,ot) donde l ( re+ordemos 1!e en6'o,a,a tanto 'a 'on6it!d de' e.e+tor .ina' +omo 'a 'on6it!d de 'a ta'adradora3 Ta' +omo &odemos o,ser(ar) e' (e+tor de +oordenadas arti+!'ares 1 re&resenta 8 6rados de 'i,ertad de' ro,ot) @ &rism-ti+as * # rota+iona'es * di+;o (e+tor se introd!+e en 'a ta,'a D3 En e' +0di6o se &!ede +om&ro,ar 1!e !ti'iza 'a .!n+i0n DENAVIT &ara rea'iza 'os +-'+!'os ne+esarios &ara o,tener 'a matriz de trans.orma+i0n ;omo6nea entre dos sistemas +onse+!ti(os !ti'izando 'os &ar-metros D4

1 2 8 < >   G H 1@ 11 12

 DENAVIT Matriz de trans.orma+i0n ;omo6nea3  D F DENAVIT7TETA) D) A) ALXA9  de(!e'(e 'a matriz de trans.orma+i0n  ;omo6nea G / G a &artir de 'os &ar-metros de Dena(it$arten,er6  D) ALXA) A * TETA3   Ver tam,in DIRECTA3 .!n+tion d;Fdena(it7teta) d) a) a'.a9 d;F+os7teta9 $+os7a'.a9Wsin7teta9 sin7a'.a9Wsin7teta9 aW+os7teta9: sin7teta9 +os7a'.a9W+os7teta9 $sin7a'.a9W+os7teta9 aWsin7teta9: > sin7a'.a9 +os7a'.a9 d: > > > %:

Ta' * +omo se &!ede o,ser(ar en e' +0di6o) 'a matriz d; es
+os7θ i 9

− +os7α i 9 ⋅ sen7θ i 9

sen7α i 9 ⋅ sen7θ i 9

ai ⋅ +os7θ i 9

sen7θ i 9

+os7α i 9 ⋅ +os7θ i 9

− sen7α i 9 ⋅ +os7θ i 9

ai ⋅ sen7θ i 9

>

sen7α i 9

+os7α i 9

d i

>

>

>

%

      

or tanto) tendríamos res!e'to e' &ro,'ema de 'a +inem-ti+a dire+ta de n!estro ro,ot de !na manera m-s e.i+az 1!e nos &ermite o,tener 'a +inem-ti+a in(ersa de +!a'1!ier matriz T4  x =  f % (+% ) +# ) +@ ) +G ) +8 )  y =  f # (+% ) +# ) +@ ) +G ) +8 )  z =  f @ (+% ) +# ) +@ ) +G ) +8 )

α  =  f G (+% ) +# ) +@ ) +G ) +8 )  β  =  f 8 (+% ) +# ) +@ ) +G ) +8 ) γ  =  f 5 (+% ) +# ) +@ ) +G ) +8 )

RE=OLUCIYN DEL ROBLEMA DE LA CINEMÁTICA DIRECTA  n x   n y   n z    >

o x

a x

o y

a  y

o z 

a z 

>

>

 p x 

  p y    p z    % 

=

 sen7   +os7    

θ G 7 t 99 θ G 7 t 99

+os7 θ G 7 t 99 ⋅ sen7 θ 8 7 t 99 −

sen7 θ G 7 t 99 ⋅ sen7 θ 8 7 t 99

>

+os7 θ 8 7 t 99

>

>

+os7 θ G 7 t 99 ⋅ +os7 θ 8 7 t 99 −

sen7 θ G 7 t 99 ⋅ +os7 θ 8 7 t 99 −

sen7 θ 8 7 t 99 >

d % 7 t 9 + l 8 ⋅ +os7 θ G 7 t 99 ⋅ +os7 θ 8 7 t 99 



d # 7 t 9 − l 8 ⋅ sen7 θ G 7 t 99 ⋅ +os7 θ 8 7 t 99  − d @ 7 t 9 − l G − l 8 ⋅ sen7

%

θ 8 7 t 99

   

# $ ## Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

<. EEMPLOS DE UTILIJACI+N DE LA FUNCI+N IMPLEMENTADA. ZZ 1F%38 %38 >38 &i2G &i2G

ZZ 1F$%38 $%38 >38 &i25 &i2#

1F

1F

 %38>>> %38>>> >38>>> >3K8G >3K8G

 %38>>> %38>>> >38>>> >38#@5 %38>K

ZZ TFdire+ta719

ZZ TFdire+ta719

TF

TF

>3>% >38>>> >38>>> %3G>>> >3>% >38>>> >38>>> %35>>> >3>>>> >3>% >3>% %3>G%G >3>>>> >3>>>> >3>>>> %3>>>>

>38>>> >3K55> >3>>>> %38>>> >3K55> >38>>> >3>>>> %38>>> >3>>>> >3>>>> %3>>>> %3%>>> >3>>>> >3>>>> >3>>>> %3>>>>

ZZ 1F% % >38 &i25 &i2#

ZZ 1F% % >38 &i25 &i2@

1F

1F  %3>>>> %3>>>> >38>>> >38#@5 %3>G#

 %3>>>> %3>>>> >38>>> >38#@5 %38>K ZZ TFdire+ta719

ZZ TFdire+ta719

TF

TF

 >38>>> >3K55>  >3>>>> >3>>>>

>3K55> >3>>>> >38>>> >3>>>> >3>>>> %3>>>> >3>>>> >3>>>>

 >38>>> >38>> >3K55> >3G@@>  >3>>>> >38>>> >3>>>> >3>>>>

%3>>>> %3>>>> %3%>>> %3>>>>

>3G@@> >3#8>> >3K55> >3>>>>

>3%@G >38>> >3#5K %3>>>>

F#$u%a 2.4 E
# $ #@ Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

>. LA CINEMÁTICA DIRECTA COMO VENTAA EL ACOBIANO. La (enta
S#)$ula%#*a*&' &) la' lí(#t&' &) *&l &'pa-#o *& t%a/a9o *&l %o/ot.

=e &resentan +!ando e' e/tremo de' ro,ot est- en a'6ún &!nto de' 'ímite de tra,a
0º < θ 4(  t) < 360º

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

=IN"ULARIDAD OR TRABAJO E[TERIOR

=IN"ULARIDAD OR TRABAJO INTERIOR

F#$u%a 2.>4 Ti&os de sin6!'aridades 1!e &resenta e' ro,ot3 a,'ando de sin6!'aridades de tra,a
Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

  x&      y&      z  &     α &     β &     γ     &

=

 d & %     d & #     ⋅  d & @  ⇒     θ &   G  θ &   8

=

 ∂ x  ∂ d   %  ∂ y  ∂ d %  ∂ z    ∂ d %  ∂ α   ∂ d   %  ∂ β   ∂ d %  ∂ β    ∂ d %

∂ x

∂ x

∂ x

∂ x

∂ d # ∂ y

∂ d @ ∂ y

∂ θ  G ∂ y

∂ θ 8 ∂ y

∂ d # ∂ z 

∂ d @ ∂ z 

∂ θ  G ∂ z 

∂ θ 8 ∂ z 

∂ d # ∂ α 

∂ d @ ∂ α 

∂ θ  G ∂ α 

∂ θ 8 ∂ α 

∂ d # ∂ β 

∂ d @ ∂ β 

∂ θ  G ∂ β 

∂ θ 8 ∂ β 

∂ d # ∂ β 

∂ d @ ∂ β 

∂ θ  G ∂ β 

∂ θ 8 ∂ β 

∂ d #

∂ d @

∂ θ  G

∂ θ 8

               

Las deri(adas de 'os (a'ores  x)  y *  z   res&e+to a +ada !na de 'as (aria,'es arti+!'ares son4  x = d % 7t 9 + l 8 ⋅ +os7θ G 7t 99 ⋅ +os7θ 8 7t 99 ∂ x ∂d % ∂ x ∂d # ∂ x ∂d @ ∂ x ∂θ G ∂ x ∂θ 8

 y = d # 7t 9 − l 8 ⋅ sen7θ G 7t 99 ⋅ +os7θ 8 7t 99 ∂ y

& = % ⋅ d  %

∂d % ∂ y

=>

∂d # ∂ y

=>

∂d @ ∂ y

& = −l 8 ⋅ sen7θ G 9 ⋅ +os7θ 8 9θ  G

∂θ G ∂ y

& = −l 8 ⋅ +os7θ G 9 ⋅ sen7θ 8 9θ  8

∂θ 8

 z = − d @ 7t 9 − l G − l 8 ⋅ sen7θ 8 7t 99 ∂ z 

=>

∂d % ∂ z 

& = % ⋅ d  #

∂d # ∂ z 

=>

∂d @

& = −l 8 ⋅ +os7θ G 9 ⋅ +os7θ 8 9θ  G & = +l 8 ⋅ sen7θ G 9 ⋅ sen7θ 8 9θ  8

∂ z  ∂θ G ∂ z  ∂θ 8

=> => & = −% ⋅ d  @

=> & = −l 8 ⋅ +os7θ 8 9θ  8

=i tomamos +omo -n6!'os de E!'er α) /  * 0 +omo 'os -n6!'os 1!e .orma n!estro (e+tor a de a&ro/ima+i0n +on 'os (e+tores #)  9 * K   de dire++i0n de 'a ,ase +an0ni+a) tenemos 1!e 'os -n6!'os de 'os +osenos dire+tores son 'os si6!ientes4

Á)$ulo *& Eul&% α r

r

r

r

i ⋅a = i ⋅ a r

+os7α 9 r

}

a = % ⇒ Am,os son

i = % 1 

(e+tor es ortonorma'es3

 +os7θ G 7t 99 ⋅ +os7θ 8 7t 99    r r +os7α 9 = i ⋅ a ⇒ [% > >] ⋅ − sen7θ G 7t 99 ⋅ +os7θ 8 7t 99 = +os7θ G 7t 99 ⋅ +os7θ 8 7t 99     − sen7θ 8 7t 99 +os7α 9 = +os7θ G 7t 99 ⋅ +os7θ 8 7t 99 ⇒ α  = ar++os7+os7θ G 7t 99 ⋅ +os7θ 8 7t 999

Á)$ulo *& Eul&% "  r

r

r

r

 2 ⋅ a =  2 ⋅ a r

 2 = % 1 

+os7 β 9 r

}

a = % ⇒ Am,os son (e+tor es ortonorma'es3

 +os7θ G 7t 99 ⋅ +os7θ 8 7t 99    r r +os7 β 9 =  2 ⋅ a ⇒ [> % >] ⋅ − sen7θ G 7t 99 ⋅ +os7θ 8 7t 99 = − sen7θ G 7t 99 ⋅ +os7θ 8 7t 99     − sen7θ 8 7t 99 +os7 β 9 = − sen7θ G 7t 99 ⋅ +os7θ 8 7t 99 ⇒  β  = ar++os7− sen7θ G 7t 99 ⋅ +os7θ 8 7t 999

# $ #8 Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

Á)$ulo *& Eul&% # r

r

r

r

3  ⋅ a = 3  ⋅ a r

3  = % 1 

+os7γ  9

}

r

a = % ⇒ Am,os son (e+tor es ortonorma'es3

 +os7θ G 7t 99 ⋅ +os7θ 8 7t 99    r r +os7γ  9 =  2 ⋅ a ⇒ [> > %] ⋅ − sen7θ G 7t 99 ⋅ +os7θ 8 7t 99 = − sen7θ 8 7t 99     − sen7θ 8 7t 99 +os7γ  9 = − sen7θ 8 7t 99 ⇒ γ  = ar++os7− sen7θ 8 7t 999

Las deri(adas de 'os -n6!'os de E!'er res&e+to a +ada !na de 'as (aria,'es arti+!'ares son4 α  = ar++os7+os7θ G 7t 99 ⋅ +os7θ 8 7t 999 ∂a y ∂d % ∂a y ∂d # ∂a y ∂d @ ∂a y ∂θ G ∂a y ∂θ 8

 β  = ar++os7− sen7θ G 7t 99 ⋅ +os7θ 8 7t 999 ∂a x

=>

∂d % ∂a x

=>

∂d # ∂a x

=>

∂d @

sen7θ G 9 ⋅ +os7θ 8 9

=

% − +os # 7θ G 9 ⋅ +os # 7θ 8 9 +os7θ G 9 ⋅ sen7θ 8 9

=

% − +os # 7θ G 9 ⋅ +os # 7θ 8 9

∂a x

θ &G

∂θ G ∂a x

θ &8

∂θ 8

=> => => =

+os7θ G 9 ⋅ +os7θ 8 9 #

=−

#

% − sen 7θ G 9 ⋅ +os 7θ 8 9 sen7θ G 9 ⋅ sen7θ 8 9 #

#

θ &G

% − sen 7θ G 9 ⋅ +os 7θ 8 9

θ &8

γ  = ar++os7− sen7θ 8 7t 999 ∂a z  ∂d % ∂a z  ∂d # ∂a z  ∂d @ ∂a z  ∂θ G ∂a z  ∂θ 8

=> => => => =

−% #

% − [− sen7θ 8 7t 99]

7− +os7θ 8 99θ &8

& = ±% ⋅ θ  8

=i s0'o tenemos en +!enta 'as (e'o+idades en  x  y *  z   * tra,a     z  &  >

>

>

− l 8 ⋅ sen7θ G 9 ⋅ +os7θ 8 9

%

>

− l 8 ⋅ +os7θ G 9 ⋅ +os7θ 8 9

>

−%

>

&   d  %   &  − l 8 ⋅ +os7θ G 9 ⋅ sen7θ 8 9   d  #    &  + l 8 ⋅ sen7θ G 9 ⋅ sen7θ 8 9  ⋅  d  @     θ &G  − l 8 ⋅ +os7θ 8 9   θ &   8

# $ #5 Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

 

 J  ⋅ J  =

%  >   >

>

>

− l 8 ⋅ # G ⋅ " 8

%

>

− l 8 ⋅ " G ⋅ " 8

>

−%

>

 ⋅   

  − l 8 ⋅ " G ⋅ # 8     + l 8 ⋅ # G ⋅ # 8  ⋅    − l 8 ⋅ " 8   − l 8   − l   8

=

#

#

l 8 + % l 8

%

>

>

%

>

>

⋅ # G ⋅ " 8

− l 8 ⋅ " G ⋅ " 8

⋅ " G ⋅ # 8

+ l 8 ⋅ # G ⋅ # 8

  >   −%   >   − l 8 ⋅ " 8 

+os # 7θ 8 7t 99 + %

Esta es !na manera de demostrar 1!e n!estro ro,ot no tiene +on.i6!ra+iones sin6!'ares &!esto 1!e e' determinante de s! matriz no &!ede an!'arse3 Este ;e+;o  &'asma 'a idea de 1!e 'as &rism-ti+as no nos restri6en e' mo(imiento &!esto 1!e nos 'o restrin6e 'as &aredes * te+;o de 'a sa'a de o&era+iones3 =i tenemos en +!enta 1!e tam,in se toman 'os -n6!'os de 'os +osenos dire+tores α /  * 04

%  x&     >  y&     >  z  &    = > α  &      β &     >  γ     &  > 

>

>

− l 8 ⋅ sen7θ G 9 ⋅ +os7θ 8 9

%

>

− l 8 ⋅ +os7θ G 9 ⋅ +os7θ 8 9

>

−%

>

>

+

> sen7θ G 9 ⋅ +os7θ 8 9 % − +os

>

>

>

>

+

#

+

#

7θ 8 9 +os7θ G 9 ⋅ +os7θ 8 9 #

7θ G 9 ⋅ +os

− l 8 ⋅ +os7θ G 9 ⋅ sen7θ 8 9

−

#

% − sen 7θ G 9 ⋅ +os 7θ 8 9 >

 ⋅   

  & + l 8 ⋅ sen7θ G 9 ⋅ sen7θ 8 9   d %     &   d  # − l 8 ⋅ +os7θ 8 9    +os7θ G 9 ⋅ sen7θ 8 9 &   ⋅  d  @   # #  % − +os 7θ G 9 ⋅ +os 7θ 8 9   &  θ G sen7θ G 9 ⋅ sen7θ 8 9     &  % − sen # 7θ G 9 ⋅ +os # 7θ 8 9  θ 8  % 

=>

Esto es de,ido &rin+i&a'mente a 1!e n!estro ro,ot es de +!atro 6rados de 'i,ertad * no &!ede dar '!6ar a 5 6rados de 'i,ertad3 or e''o 1!itamos !no) &or e  y&     >  z  &    = > α  &      β &     >  γ     &  > 

>

>

− l 8 ⋅ sen7θ G 9 ⋅ +os7θ 8 9

%

>

− l 8 ⋅ +os7θ G 9 ⋅ +os7θ 8 9

>

−%

>

>

> >

> >

+

+

> sen7θ G 9 ⋅ +os7θ 8 9 #

#

#

#

% − +os 7θ G 9 ⋅ +os 7θ 8 9 +os7θ G 9 ⋅ +os7θ 8 9 % − sen 7θ G 9 ⋅ +os 7θ 8 9 >

− l 8 ⋅ +os7θ G 9 ⋅ sen7θ 8 9

+

−

  & + l 8 ⋅ sen7θ G 9 ⋅ sen7θ 8 9   d %     &   d  # − l 8 ⋅ +os7θ 8 9    +os7θ G 9 ⋅ sen7θ 8 9 &   ⋅  d   @ # #  % − +os 7θ G 9 ⋅ +os 7θ 8 9   &  θ G sen7θ G 9 ⋅ sen7θ 8 9     &  % − sen # 7θ G 9 ⋅ +os # 7θ 8 9  θ 8  % 

# $ # Jaime Martínez Verdú

Jos L!ís Aznar "omis

>

CINEMÁTICA DIRECTA DEL QUIROBOT or tanto) 'a matriz sería 'a si6!iente4 %    >  y&     >  z  & =    > α  &      β &  >      x& 

>

>

− l 8 ⋅ sen7θ G 9 ⋅ +os7θ 8 9

%

>

− l 8 ⋅ +os7θ G 9 ⋅ +os7θ 8 9

>

−%

>

>

> sen7θ G 9 ⋅ +os7θ 8 9

+

% − +os >

>



− l 8 ⋅ +os7θ G 9 ⋅ sen7θ 8 9

#

+

#

7θ 8 9 +os7θ G 9 ⋅ +os7θ 8 9

+

7θ G 9 ⋅ +os

#

#

% − sen 7θ G 9 ⋅ +os 7θ 8 9

−

 &    d  % + l 8 ⋅ sen7θ G 9 ⋅ sen7θ 8 9    &   d  #    − l 8 ⋅ +os7θ 8 9  ⋅  d  &  +os7θ G 9 ⋅ sen7θ 8 9   @ % − +os # 7θ G 9 ⋅ +os # 7θ 8 9  θ &G    sen7θ G 9 ⋅ sen7θ 8 9  &   θ 8  % − sen # 7θ G 9 ⋅ +os # 7θ 8 9 

sen7θ 8 9 ⋅ +os7θ 8 9

=

% − sen # 7θ G 9 ⋅ +os # 7θ 8 9 % − +os # 7θ G 9 ⋅ +os # 7θ 8 9

E/isten dos &osi,i'idades de 1!e se an!'e4 − 8 ≤ θ 8 ≤ ##8

sen7θ 8 9 = > ⇒ θ 8

 θ  => )%K>

= >  )%K>

sen7θ 8 9 ⋅ +os7θ 8 9

= #

#

= #

#

% − sen 7θ G 9 ⋅ +os 7θ 8 9 % − +os 7θ G 9 ⋅ +os 7θ 8 9

8

> ⋅ 7±%9

=

% − sen # 7θ G 9 ⋅ % % − +os # 7θ G 9 ⋅ % #

% − sen 7θ G 9 ≠ % +os7θ 8 9 ≠ ±% ⇒ θ G

 θ  => )> )%K>) #>

≠ > )%K>

=>⇔

θ 8 =>  )%K> 

% − sen # 7θ G 9 ≠ %  y

% − +os # 7θ G 9 ≠ % sen7θ G 9 ≠ ±% ⇒ θ G

% − +os # 7θ G 9 ≠ %

≠ > ) #>

=>

G

− 8 ≤ θ 8 ≤ ##8

+os7θ 8 9 = > ⇒ θ 8 = > )#>

 θ  =>) #>

sen7θ 8 9 ⋅ +os7θ 8 9

=

8

#

#

%⋅ > % − sen # 7θ G 9 ⋅ > % − +os # 7θ G 9 ⋅ > θ 8 = >

 y

∀θ G

 y θ 8 = #> 

#

% − sen 7θ G 9 ⋅ +os 7θ 8 9 % − +os 7θ G 9 ⋅ +os 7θ 8 9

=

∀θ G

= #

θ G ≠ > )>  )%K> )#>  y θ G ≠ > )> )%K>  )#>  y

=

θ 8 =>  ) #>

> => % %

Caso % Límite e/terior de' es&a+io de tra,a θ 8 = %K> Caso G Límite e/terior de' es&a+io de tra,a
# $ #K Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT

∀ θ 4 (t)

 y@

∀ θ 4 (t)

 x8

 z 8 θ 5 (t)

 y8

 yG

= 90º

θ 5(t)  

 z G

= 270º

 xG

 xG

 z G  y8

 yG  z 8

 x8 F#$u%a 2.a4 Casos % * #3 Ti&os de sin6!'aridades en 'os 'ímites 1!e &resenta en θ 4 (t)

 y@

= 90º, 270º.

θ 4 (t

 x@

θ 5 (t)

) 4 )5, 6)5, $7)5, %8)5 

 z @

 y@

= 0º

 x@

 z G  xG

 y8

 z 8

 z @

 y G

 y G

 x8

 xG  z G

 z 8  x8

θ 5 (t)

= 180º

 y8

F#$u%a 2./4 Casos @ * G3 Ti&os de sin6!'aridades en 'os 'ímites 1!e &resenta en θ 4 (t) 4 >) >) %K>) #> * θ 4 (t ) = 0º, 180º.3

# $ # Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT •

S#)$ula%#*a*&' &) #)t&%#o% *&l &'pa-#o *& t%a/a9o *&l %o/ot.

O+!rren dentro de 'a zona de tra,a3

θ G = >  )%K>   y θ 8 = >  θ G = > )#>   y θ 8 = %K> 

 x>

 x>

 y>

 y>

 x%

 y#

 y#

 z %

 z %  x#

 y%

 z >

 x%

 z >

 z #

 x#

 y%

 z #

 x@  y@

θ 4 (t θ 4 (t

 x@

) =  $7)5 

 y@

) =  )5 

 z @

 z @

 x8

 z 8  y8

θ 5 (t)

 xG

= 0º

 z G

 z G  xG

θ 5 (t)

 y G

= 0º

 y G

 z 8  x8

 y8

F#$u%a 2.a4 Caso 8 de sin6!'aridades de e Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT  x>  y>

 x>  y>

 z >

 x%

 y#  z %

 z >

 x%

 x#

 y%

 y#

 z #

 z %  x#

 y%

 z # θ 4 (t

 y@

) =  %8)5 

 y@

 x@ θ 4 (t

 y8

) =  6)5 

 z @

 z 8

 x@  z @

 y G

 z G

 x8  xG θ 5 (t)

= 180º

 y G

 y8  xG

 z G

θ 5 (t)

= 180º

 x8  z 8

F#$u%a 2./4 Caso 5 de sin6!'aridades de e3 La (enta  Caso @ Límite e/terior de' es&a+io de tra,a )>  )%K> )#>  y θ 8 = > θ G ≠ > )> )%K>  )#>  y θ 8 = %K> Caso G Límite e/terior de' es&a+io de tra,a  )%K>   y θ 8 = >  Caso 84 z G * z 8 se a'inean +on  z %3 θ G = > )#>   y θ 8 = %K>  Caso 54 z G * z 8 se a'inean +on  z >3 # $ @% ∀θ G

 y

θ 8 = >

Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT •

Co),#$u%a-#o)&' *&l %o/ot (&)o' '#)$ula%&' a-o/#a)o (#(o o (í)#(o.

A +ontin!a+i0n intentaremos en+ontrar 'os (a'ores an6!'ares &ara 'os +!a'es e' (a'or a,so'!to de' Ja+o,iano es m-/imo3 Con estos (a'ores en+ontraremos a1!e''as +on.i6!ra+iones &ara 'as +!a'es e' ro,ot tra,a
sen7θ 8 9 ⋅ +os7θ 8 9

=

% − sen # 7θ G 9 ⋅ +os # 7θ 8 9 % − +os # 7θ G 9 ⋅ +os # 7θ 8 9

∂



= >

  ⇒ Condi+iones de o&tima'idad ∂  =>  ∂θ 8  ∂θ G

A +ontin!a+i0n +a'+!'aremos am,as deri(adas e intentaremos reso'(er e' sistema de e+!a+iones4 ∂ J  ∂θ G

=

sen7θ G 9 ⋅ +os7θ G 9 ⋅ +os # 7θ 8 9 ⋅ 7% − # +os # 7θ G 99

(% − sen

#

@

@

7θ G 9 ⋅ +os 7θ 8 9 ) # (% − +os 7θ G 9 ⋅ +os 7θ 8 9 ) #

#

#

=> #

=a,iendo esto) tenemos 1!e 'a e/&resi0n anterior se &!ede e/&resar +omo4 sen7θ G 9 ⋅ +os7θ G 9 ⋅ +os # 7θ 8 9 ⋅ 7% − # +os # 7θ G 99 = > @

@

(% − sen # 7θ G 9 ⋅ +os # 7θ 8 9 ) # (% − +os # 7θ G 9 ⋅ +os # 7θ 8 9 ) # ≠ > Una (ez o,tenida 'a &rimera e+!a+i0n &asamos a 'a se6!nda4 ∂ J  ∂θ 8

=

sen # 7θ G 9 ⋅ +os # 7θ G 9 ⋅ +os G 7θ 8 9 + +os # 7θ 8 9 ⋅ 7% + sen # 7θ 8 99 − %

(% − sen

#

@

@

7θ G 9 ⋅ +os 7θ 8 9 ) (% − +os 7θ G 9 ⋅ +os 7θ 8 9 ) #

#

#

#

=>

#

=a,iendo esto) tenemos 1!e 'a e/&resi0n anterior se &!ede e/&resar +omo4 sen # 7θ G 9 ⋅ +os # 7θ G 9 ⋅ +os G 7θ 8 9 + +os # 7θ 8 9 ⋅ 7% + sen # 7θ 8 99 − % = > @

@

(% − sen # 7θ G 9 ⋅ +os # 7θ 8 9) # (% − +os # 7θ G 9 ⋅ +os # 7θ 8 9) # ≠ > Las so'!+iones a este sistema de e+!a+iones son4 sen7θ 8 9 = > ⇒ θ 8

(% − sen

#

@

@

7θ G 9 ⋅ %) (% − +os 7θ G 9 ⋅ %) #

#

#

= > )%K>

!nto +ríti+o % ≠ > ⇒ θ G ≠ > )> )%K> )#>

+os7θ 8 9 = > ⇒ θ 8 = > )#>

(% − sen

#

@

@

7θ G 9 ⋅ > ) (% − +os 7θ G 9 ⋅ >) #

#

#

!nto +ríti+o #

≠ > ⇒ ∀θ G

# $ @# Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT sen7θ G 9 = > ⇒ θ G

(% − > ⋅ +os

#

@

= > )%K> @

7θ 8 9 ) (% − % ⋅ +os 7θ 8 9 ) #

#

!nto +ríti+o @ #

≠ > ⇒ θ 8 ≠ > )%K>

+os7θ G 9 = > ⇒ θ G = > )#>

(% − % ⋅ +os

#

@

@

7θ 8 9 ) (% − > ⋅ +os 7θ 8 9 ) #

#

+os7θ G 9 = @

# ⇒ θ G #

#

≠ > ⇒ θ 8 ≠ >  )%K>

= G8 )@%8

%   %     # # % − ⋅ +os 7θ 8 9  % − ⋅ +os 7θ 8 9    #     #   #  

+os7θ G 9 = − @

!nto +ríti+o G

@

!nto +ríti+o 8 #

≠ > ⇒ ∀θ 8

# ⇒ θ G = %@8 )##8 #

%   %     # # % − ⋅ +os 7θ 8 9  % − ⋅ +os 7θ 8 9    #     #   #  

@

#

 !nto

!nto +ríti+o 5

≠ > ⇒ ∀θ 8

Estos son &!ntos +ríti+os es de+ir) mínimos * m-/imos o de ensi''ad!ra4 •

Pu)to -%ít#-o 14 Re+ordemos 1!e se trata de 'os +asos @ * G de 'ímite e/terior de' es&a+io de tra,a
•

Pu)to -%ít#-o 24 Re+ordemos 1!e se trata de 'os +asos % * # de 'ímite e/terior e interior) res&e+ti(amente) de' es&a+io de tra,a
•

Pu)to -%ít#-o 84 =e trata de !n +on )%K> 3 =! (a'or (ariar- en .!n+i0n de θ 8 ≠ > )%K> 3 =e trata de 'os &!ntos 1!e +om&'ementan a 'os +asos @ * G3

•

Pu)to -%ít#-o <4 =e trata de !n +on )%K> 3 =! (a'or (ariar- en .!n+i0n de θ 8 ≠ > )%K> 3 =e trata de 'os &!ntos 1!e +om&'ementan a 'os +asos @ * G3

•

Pu)to -%ít#-o >4 =e trata de !n +on
∂ ∂θ 8

=

# ⋅ (% − @ sen # 7θ 8 9 ) #

(% + sen # 7θ 8 9)

=

sen7θ 8 9 ⋅ +os7θ 8 9 % % − ⋅ +os # 7θ 8 9 #

=>⇒

% − @ sen # 7θ 8 9

= > ⇒ sen7θ 8 9 = ±

@ @

# $ @@ Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT O,tenemos +omo res!'tado 1!e &ara 'os -n6!'os θ 8  F @83#5GG) %GG3@85 e' determinante de 'a matriz Ja+o,iana es m-/imo * &ara -n6!'os θ 8 F @#G3@85) #%83#85G * s!s (a'ores son4



=

−



=

  %   %  ⋅ %−   @   @ 

%    % − ⋅ % −  #   

#

% # ⋅ @ @ = # %    % − % ⋅ #   # @ @  

  %   %  ⋅ %−   @   @ 

=+

# #

#

% # ⋅ @ @ =− # % # %    %− ⋅   # @ @  

%    % − ⋅ % −  #   

=

# @ # @

=−

# @ # @

=−

# #

Ta' +omo &odemos o,ser(ar nos da i6!a' 1!e sean m-/imos o mínimos &!esto 1!e 'o 1!e nos re+om&ensa es 1!e e' (a'or a,so'!to de' determinante sea m-/imo) es de+ir) 1!e e' Ja+o,iano est 'o m-s a'e
Pu)to -%ít#-o 4 =e trata de !n +on
∂ ∂θ 8

=

# ⋅ (% − @ sen # 7θ 8 9 ) #

(% + sen # 7θ 8 9)

=

sen7θ 8 9 ⋅ +os7θ 8 9 % % − ⋅ +os # 7θ 8 9 #

=>⇒

% − @ sen # 7θ 8 9

= > ⇒ sen7θ 8 9 = ±

@ @

O,tenemos +omo res!'tado 1!e &ara 'os -n6!'os θ 8  F @83#5GG) %GG3@85 e' determinante de 'a matriz Ja+o,iana es m-/imo * &ara -n6!'os θ 8 F @#G3@85) #%83#85G * s!s (a'ores son4 

=+

# #



=−

# #

Ta' +omo &odemos o,ser(ar nos da i6!a' 1!e sean m-/imos o mínimos &!esto 1!e 'o 1!e nos renta es 1!e e' (a'or a,so'!to de' determinante sea m-/imo) es de+ir) 1!e e' Ja+o,iano est 'o m-s a'e
Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT a6amos !n ,re(e re&aso de 'as sin6!'aridades * no sin6!'aridades de n!estro ro,ot4 =UN"ULARIDADE= Caso % Límite e/terior de' es&a+io de tra,a  Caso # Límite interior de' es&a+io de tra,a )>  )%K> )#>  y θ 8 = > θ G ≠ > )> )%K>  )#>  y θ 8 = %K> Caso G Límite e/terior de' es&a+io de tra,a  )%K>   y θ 8 = >  Caso 84 z G * z 8 se a'inean +on  z %3 θ G = > )#>   y θ 8 = %K>  Caso 54 z G * z 8 se a'inean +on  z >3 ∀θ G

 y

θ 8 = >

ANTI$=IN"ULARIDADE= Con.i6!ra+i0n %4 θ G F G8 * θ 8 F @83#5GG Con.i6!ra+i0n #4 θ G F G8 * θ 8 F %GG3@85 Con.i6!ra+i0n @4 θ G F G8 * θ 8 F @#G3@85 Con.i6!ra+i0n G4 θ G F G8 * θ 8 F #%83#85G Con.i6!ra+i0n 84 θ G F %@8 * θ 8 F @83#5GG Con.i6!ra+i0n 54 θ G F %@8 * θ 8 F %GG3@85 Con.i6!ra+i0n 4 θ G F %@8 * θ 8 F @#G3@85 Con.i6!ra+i0n K4 θ G F %@8 * θ 8 F #%83#85G Con.i6!ra+i0n 4 θ G F ##8 * θ 8 F @83#5GG Con.i6!ra+i0n %>4  θ G F ##8 * θ 8 F %GG3@85 Con.i6!ra+i0n %%4  θ G F ##8 * θ 8 F @#G3@85 Con.i6!ra+i0n %#4  θ G F ##8 * θ 8 F #%83#85G Con.i6!ra+i0n %@4  θ G F G8 * θ 8 F @83#5GG Con.i6!ra+i0n %G4  θ G F G8 * θ 8 F %GG3@85 Con.i6!ra+i0n %84  θ G F G8 * θ 8 F @#G3@85 Con.i6!ra+i0n %54  θ G F G8 * θ 8 F #%83#85G A s! (ez) &odemos sim&'i.i+ar estas %5 +on.i6!ra+iones a K &!esto 1!e son +on.i6!ra+iones e1!i(a'entes dos a dos3 A +ontin!a+i0n (emos +!a'es son e1!i(a'entes4 Con.i6!ra+i0n %4 θ G F G8 * θ 8 F @83#5GG Con.i6!ra+i0n K4 θ G F %@8 * θ 8 F #%83#85G

Con.i6!ra+i0n #4 θ G F G8 * θ 8 F %GG3@85 Con.i6!ra+i0n 4 θ G F %@8 * θ 8 F @#G3@85

Con.i6!ra+i0n @4 θ G F G8 * θ 8 F @#G3@85 Con.i6!ra+i0n 54 θ G F %@8 * θ 8 F %GG3@85

Con.i6!ra+i0n G4 θ G F G8 * θ 8 F #%83#85G Con.i6!ra+i0n 84 θ G F %@8 * θ 8 F @83#5GG

Con.i6!ra+i0n 4 θ G F ##8 * θ 8 F @83#5GG Con.i6!ra+i0n %54 θ G F G8 * θ 8 F #%83#85G

Con.i6!ra+i0n %>4 θ G F ##8 * θ 8 F %GG3@85 Con.i6!ra+i0n %84 θ G F G8 * θ 8 F @#G3@85

Con.i6!ra+i0n %%4 θ G F ##8 * θ 8 F @#G3@85 Con.i6!ra+i0n %G4 θ G F G8 * θ 8 F %GG3@85

Con.i6!ra+i0n %#4 θ G F ##8 * θ 8 F #%83#85G Con.i6!ra+i0n %@4 θ G F G8 * θ 8 F @83#5GG

# $ @8 Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA DIRECTA DEL QUIROBOT or tanto) tenemos 1!e 'as +on.i6!ra+iones osn en rea'idad K 1!e son Con.i6!ra+i0n %4 θ G F G8 * θ 8 F @83#5GG Con.i6!ra+i0n #4 θ G F G8 * θ 8 F %GG3@85 Con.i6!ra+i0n @4 θ G F G8 * θ 8 F @#G3@85 Con.i6!ra+i0n G4 θ G F G8 * θ 8 F #%83#85G Con.i6!ra+i0n %@4  θ G F G8 * θ 8 F @83#5GG Con.i6!ra+i0n %G4  θ G F G8 * θ 8 F %GG3@85 Con.i6!ra+i0n %84  θ G F G8 * θ 8 F @#G3@85 Con.i6!ra+i0n %54  θ G F G8 * θ 8 F #%83#85G or tanto) 'a +inem-ti+a dire+ta ;a sido !na (enta
# $ @5 Jaime Martínez Verdú

Jos L!ís Aznar "omis

CINEMÁTICA INVERSA DEL QUIROBOT

3

Capí tulo CINEMÁTICA  IINVER SA  D DEL  Q QUIR OBOT

"DE VUELTA DE TODO"  Pau Donés

'ata a(ora) (a*íamo o*tenido +a ,oordenada -ara +a -oi,i.n de+ -"nto termina+ a -artir de +o /a+ore de +a /aria*+e de arti,"+a,i.n0 A(ora no -+anteamo e+  -ro*+ema in/ero1 o*tener +o /a+ore de +a /aria*+e arti,"+are) 2"e on rea+mente +o 2"e e en/ía a+ itema de ,ontro+ de+ mani-"+ador) a -artir de +a -oi,i.n 3 orienta,i.n deeada -ara e+ -"nto termina+0 Eto e +o m4 "ado) 3 a*o+"tamente ne,eario) dado 2"e +a tarea a rea+izar o tra3e,toria a re,orrer -or "n mani-"+ador e dan ,ai iem-re en ,oordenada ,arteiana re5erida a a+6ún itema 5i7o0 8E+ o*7eti/o de+  problema cinemático inverso ,onite en en,ontrar +o /a+ore 2"e de*en ado-tar +a ,oordenada arti,"+are de+ ro*ot q 9 : q1 , q2 , q 3 , q4 , q5;T -ara 2"e " e
E+ -"nto -edido et4) -or " +e7anía) 5"era de+ a+,an,e de+ ro*ot0

•

E+ -"nto -edido et4 5"era de+ a+,an,e) -ero de*ido a -ro*+ema 6eom!tri,o ="no en+a,e de+ ro*ot ,(o,an ,on otro>0

•

E+ -"nto -edido -"ede a+,anzare mediante do o m4 ,om*ina,ione =/a+ore> de+ /e,tor de arti,"+a,i.n0 Cada "na de eta ,om*ina,ione e ++ama ,on5i6"ra,i.n? e tí-i,o e+ ,ao de +o *razo SCARA) ,omo e+ de+ ro*ot RT&@@) donde ,"a+2"ier -"nto a+,anza*+e -"ede a+,anzare ,on e+ ,odo do*+ado (a,ia +a dere,(a) o (a,ia +a iz2"ierda =+o ,"a+ tam*i!n modi5i,a) nat"ra+mente) +o /a+ore de +a dem4 arti,"+a,ione>0

•

La e,"a,ione 2"e re+a,ionan +a /aria*+e de arti,"+a,i.n ,on +a /aria*+e ,arteiana no on o+"*+e) en e+ entido de 2"e no e -"eden en,ontrar o+",ione e-ara*+e) ++amada 5orma ,errada) 2"e den e+ /a+or de ,ada /aria*+e ,arteiana ,omo 5"n,i.n e<,+"i/amente de +a /aria*+e de arti,"+a,i.n0 $%&

Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT A di5eren,ia de +a ,inem4ti,a dire,ta) no e
$0&

q$ =  f $ ( x) y) z ) α ) β ) γ  ) q F =  f F ( x) y) z ) α ) β ) γ  ) q =  f  ( x) y) z ) α ) β ) γ  )

Ete ti-o de o+",i.n -reenta) entre otra) +a i6"iente /enta7a1 &0 En m",(a a-+i,a,ione) e+ -ro*+ema ,inem4ti,o in/ero (a de reo+/ere en tiem-o rea+ =-or e7em-+o) en e+ e6"imiento de "na determinada tra3e,toria>0 Gor tanto) "na o+",i.n de ti-o iterati/o no 6arantiza tener +a o+",i.n en e+ momento ade,"ado0 Ete e e+ ,ao 2"e no ataHe -"eto 2"e ,"ando e+ ro*ot et! rea+izando +a in,ii.n tiene 2"e er "na in,ii.n +im-ia 3 -re,ia in tiem-o m"erto ni nin6ún -ro*+ema de ete ti-o0 0 A+ ,ontrario de +o 2"e o,"rría en e+ -ro*+ema ,inem4ti,o dire,to) ,on ,ierta 5re,"en,ia +a o+",i.n de+ -ro*+ema no e úni,a? e
Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT Gara reo+/er +a ,inem4ti,a in/era de n"etro ro*ot -odríamo "ti+izar e+ m!todo de reo+",i.n 6eom!tri,o -"eto 2"e !te m!todo -ermite o*tener +o /a+ore de +a  -rimera /aria*+e arti,"+are) 2"e on +a 2"e ,oni6"en -oi,ionar e+ ro*ot =-re,indiendo de +a orienta,i.n de " e 3 -oteriormente reo+/er +a /aria*+e arti,"+are 2"e e en,ar6an de +a orienta,i.n 2"e en n"etro ,ao erían +a do /aria*+e an6"+are0  No o*tante) -odemo em-+ear "n m!todo m4 en,i++o aún 2"e e+ m!todo 6eom!tri,o 3 2"e ir/e de a+ternati/o -ara reo+/er e+ mimo -ro*+ema0 Gor tanto) e -"ede re,"rrir a mani-"+ar dire,tamente +a e,"a,ione ,orre-ondiente a+ -ro*+ema ,inem4ti,o dire,to0 E de,ir) -"eto 2"e !te eta*+e,e +a re+a,i.n entre +a matriz de orienta,i.n n o a  3 -oi,ionamiento p  ,on +a matriz de tran5orma,i.n (omo6!nea de+ ro*ot ta+ 2"e1 r

r

r

r

[n o a p ] =

$0

,on i,j ∈ [&)F]

T ij

Lo e+emento t ij on 5"n,i.n de +a ,oordenada arti,"+are : q1 , q2 , q3 , q4 , q5;T) 3 e -oi*+e -enar 2"e mediante ,ierta ,om*ina,ione de +a & e,"a,ione -+anteada en eta matriz e -"edan de-e7ar +a  /aria*+e arti,"+are en 5"n,i.n de +a ,om-onente de +o /e,tore n) o) a 3 p0 Gara +a reo+",i.n de+ -ro*+ema de +a ,inem4ti,a in/era ne,eitaremo re,ordar +a ("e++a de+ ro*ot 3 " re-reenta,i.n de Dena/it%'arten*er61

 ESLABÓN &  $ F 

θ i  @ @ @ θ 4=t > θ 5=t >

d i  d 1=t > d 2=t > d 3=t > l 4

@

ai  @ @ @ @ l 5

αi  @ @ @ @ @

Tabla 3.1 Ta*+a de +o -ar4metro D'0

$%$ Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT

1. DESCRI!CIN DEL M#TODO DIRECTO. En ete -ro,edimiento e trata de "ar +a norma de ,ontr",,i.n de +a matriz de tran5orma,i.n (omo6!nea T  entre e+ M"ndo 3 e+ G"nto Termina+ =e de,ir) entre e+ e-a,io de ,oordenada ,arteiana 3 arti,"+are) ,inem4ti,a dire,ta>) e,ri*iendo di,(a matriz de modo im*.+i,o) e de,ir) de7ando ,ada e+emento en 5"n,i.n de +a /aria*+e de ,ada arti,"+a,i.n qi=t > de +a 2"e de-enda0 A ,ontin"a,i.n) 3 "ando +a inter-reta,i.n dada -ara +a matriz T) -odemo e,ri*ir 2"e  n x o x a x  p x  T &&    n y o y a y  p y  T &  =  n z  o z  a z   p z   T $&     @ @ @ &  T F&

T &

T &$

T &F 

T 

T $

T F 

T $

T $$

T F

T F$



 T $F   T FF 

$0$

3 dado =n x , n y , n z  , o x , o y , o z  , a x , a y , a z  , p x , p y , p z > 2"e e+ ""ario de*e -ro/eer ,omo e-e,i5i,a,i.n de +a -oi,i.n 3 orienta,i.n) e o*tiene +a e,"a,ione1 q& =t > ≡ d & =t >

q  =t > ≡ d  =t >

q $ =t > ≡ d $ =t >

q F =t > ≡ θ F =t >

q  =t > ≡ θ  =t >

d & =t > =  f & n x ) n y ) n z ) o x ) o y ) o z ) a x ) a y ) a z ) p x ) p y ) p z  d  =t > =  f  (n x ) n y ) n z ) o x ) o y ) o z ) a x ) a y ) a z ) p x ) p y ) p z  ) d $ =t > =  f $ (n x ) n y ) n z ) o x ) o y ) o z ) a x ) a y ) a z ) p x ) p y ) p z  )

$0F

θ F =t > =  f F (n x ) n y ) n z ) o x ) o y ) o z ) a x ) a y ) a z ) p x ) p y ) p z  ) θ  =t > =  f  (n x ) n y ) n z ) o x ) o y ) o z ) a x ) a y ) a z ) p x ) p y ) p z  )

Se -"eden dar di5erente e,"a,ione -ara +o 4n6"+o de orienta,i.n) "ando otro e+emento de +a matriz0 Adem4) +a e,"a,ione erían -or "-"eto di5erente en ,ao de (a*er o-tado -or otra re-reenta,i.n -ara +a orienta,i.n en +"6ar de +a de +o ángulos de Euler   =-or e7em-+o) +o 4n6"+o roll pitc and ya! >0 En ,"a+2"ier ,ao) +a idea e im-+emente -+antear dire,tamente tanta e,"a,ione ,omo 6rado de +i*ertad (a3a) 3 tratar de reo+/er+o ,omo e -"eda0 Si ("*iera meno de  6rado de +i*ertad ,omo e n"etro ,ao -"e tenemo úni,amente  6rado de +i*ertad) (a*ría) -or "-"eto) meno e,"a,ione0 #enera+mente) "e+en er e,"a,ione no +inea+e *atante ,om-+e7a) 3 e di5í,i+ en,ontrar de modo im-+e "na o+",i.n en 5orma ,errada0 E a+  -rod",to de +a matri,e de +a ,inem4ti,a dire,ta) 3 -rem"+ti-+i,ar am*o +ado de +a i6"a+dad -or +a in/era de ,ada "na) de-"! de (a*er (a++ado "tit",ione im*.+i,a  -ara +a /aria*+e arti,"+ar de +a ,"a+ de-enda +a 2"e en ete momento ea -rimera matriz de+ -rod",to0

$%F Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT

$. RESOLUCIN DE LA CINEMÁTICA INVERSA. M#TODO DIRECTO%%MANI!ULACIN DE MATRICES SIMBLICAS. E+ -rimer -ao a dar -ara ,one6"ir reo+/er e+ -ro*+ema ,inem4ti,a in/ero e o*tener +a e<-rei.n :0$; ,orre-ondiente a n"etro ro*ot0 E de,ir) de*emo o*tener +a matriz T 2"e re+a,iona e+ itema de re5eren,ia K " # ao,iado a +a *ae ,on e+ itema K " 5 ao,iado a " e
@ @ & @

@

&

@

−&

@

@

@

@

@

@

@

@

 −& @   @ d &   @ & 

Ai 3 +a matriz T1

@  @  A =  &   @



 @  &  @  & & A =  @   @

   $ AF =    

i%&

@

−&

@

@

−&

@

@

@

@

@

" θ F

" θ F

@

− $ θ F

@

&

@

@

@

@

 $ θ    " θ  F A =   @   @

&

@

−&

@

@

@

@

@

@

&  @  A$ =  @   @

 @  d    & 

$ θ F

@



 @  & 

@

@

&

@

@

&

@

@

@

 @  d $   & 

@ 

 @   l F   & 

− " θ 

@

l  ⋅ $ θ  

$ θ 

@

l  ⋅ " θ  

@

&

@

@

@

&

    

T= @ A @ ⋅@ A& ⋅& A  ⋅ A $ ⋅$ A F ⋅ F A  ⋅ A  T= @ A $ ⋅$ A  @

A $ = @ A @ ⋅@ A& ⋅& A  ⋅  A $ $

A  = $ A F ⋅ F A  ⋅ A 

$% Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT La matriz de tran5orma,i.n (omo6!nea -ara n"etro ro*ot -reenta +a i6"iente 5orma1  n x   n y   n z    @

o x

a x

o y

a y

o z 

a z 

@

@

 en= θ F = t >>    ,o= θ F = t >>  p y   =   @  p z     &   @  p x 

,o= θ F = t >> ⋅ en= θ  = t >>

,o= θ F = t >> ⋅ ,o= θ  = t >>

d & = t > + l  ⋅ ,o= θ F = t >> ⋅ ,o= θ  = t >> 

− en= θ F = t >> ⋅ en= θ  = t >>

− en= θ F = t >> ⋅ ,o= θ  = t >>

d  = t > − l  ⋅ en= θ F = t >> ⋅ ,o= θ  = t >> 

,o= θ  = t >>

− en= θ  = t >>

− d $ = t > − l F − l  ⋅ en= θ  = t >>

@

@

&

    

Una /ez o*tenida +a e<-rei.n de +a matriz de tran5orma,i.n (omo6!nea en 5"n,i.n de +a ,oordenada arti,"+are : d 1=t > , d 2=t > , d 3=t > , θ 4=t > , θ 5=t > ;T) 3 "-"eta "na +o,a+iza,i.n de detino -ara e+ e>

o x = + ,o=θ F =t >> ⋅ en=θ  =t >> a x = + ,o=θ F =t >> ⋅ ,o=θ  =t >>  p x = d & =t > + l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>

n y = + ,o=θ F =t >> o y = − en=θ F =t >> ⋅ en=θ  =t >> a y = − en=θ F =t >> ⋅ ,o=θ  =t >>  p y = d  =t > − l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >> n z  = @

•

o z  = + ,o=θ  =t >>

a z  = − en=θ  =t >>

 p z  = −d $ =t > − l F − l  ⋅ en=θ  =t >>

Obt&n'()n *& la +a,(abl& a,t('ula, θ 4-t 

Godemo o*tener e+ /a+or de eta /aria*+e de /aria manera -"eto 2"e e ,on +a ,om-onente de +a matriz T1 n x = + en=θ F =t >>

o x = + ,o=θ F =t >> ⋅ en=θ  =t >>

a x = + ,o=θ F =t >> ⋅ ,o=θ  =t >>  p x = d & =t > + l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>

n y = + ,o=θ F =t >> o y = − en=θ F =t >> ⋅ en=θ  =t >>

a y = − en=θ F =t >> ⋅ ,o=θ  =t >>  p y = d  =t > − l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >>

n z  = @

a z  = − en=θ  =t >>

o z  = + ,o=θ  =t >>

n x = + en=θ F =t >> 

n x

n y = + ,o=θ F =t >> 

n y

→

=

+ en=θ F =t >> + ,o=θ F =t >>

→

n x n y

 p z  = −d $ =t > − l F − l  ⋅ en=θ  =t >>

 n x     n y     

= t6=θ F =t >> → θ F =t > = ar,t6 

o y = − en=θ F =t >> ⋅ en=θ  =t >>

o y o y  o y   − en=θ F =t >> ⋅ en=θ  =t >>  = → = − t6=θ F =t >> → θ F =t > = − ar,t6  → o x o x o + ,o=θ F =t >> ⋅ en=θ  =t >> o x = + ,o=θ F =t >> ⋅ en=θ  =t >>     x   a y = − en=θ F =t >> ⋅ ,o=θ  =t >>

a y a y   a y   − en=θ F =t >> ⋅ ,o=θ  =t >>  = → = − t6=θ F =t >> → θ F =t > = − ar,t6  → a x a x + ,o=θ F =t >> ⋅ ,o=θ  =t >> a x = + ,o=θ F =t >> ⋅ ,o=θ  =t >>    a x  

Como -odemo o*er/ar) e 2"e e ,ara,terizan -or re+a,ionar di,(a /aria*+e arti,"+ar ,on +o /e,tore de orienta,i.n de+ e5e,tor 5ina+0 A -ear de 2"e toda e++a on /4+ida a -riori) eto no e aí iem-re -"eto ,"ando 2"ii!ramo en,ontrar ,"a+ e e+ /a+or ,orre-ondiente de θ 4=t > ,"ando +a /aria*+e θ 5=t > e "n /a+or de @) π o π etaríamo di/idiendo -or ,ero 3 no -odríamo o*tener "n /a+or de θ 4=t >0 Como ,on,+"i.n) -odemo a5irmar 2"e +a úni,a o+",i.n 5a,ti*+e -ara o*tener "na re+a,i.n entre +a /aria*+e arti,"+ar θ 4=t > 3 +a ,om-onente de +a matriz T  e +a  -rimera o*tenida =-oteriormente /eremo 2"e eto no e tota+mente ,ierto>0 $% Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT •

Obt&n'()n *& la +a,(abl& a,t('ula, θ -t 

De 5orma imi+ar a+ ,ao anterior) -odemo o*tener e+ /a+or de eta /aria*+e de /aria manera -"eto 2"e e ,on +a ,om-onente de +a matriz T1 n x = + en=θ F =t >>

o x = + ,o=θ F =t >> ⋅ en=θ  =t >>

a x = + ,o=θ F =t >> ⋅ ,o=θ  =t >>  p x = d & =t > + l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>

n y = + ,o=θ F =t >> o y = − en=θ F =t >> ⋅ en=θ  =t >>

a y = − en=θ F =t >> ⋅ ,o=θ  =t >>  p y = d  =t > − l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >>

n z  = @

a z  = − en=θ  =t >>

o z  = + ,o=θ  =t >>

 p z  = −d $ =t > − l F − l  ⋅ en=θ  =t >>

a z  = − en=θ  =t >>

 a   a z  − en=θ  =t >> a = →  z  = − t6=θ  =t >> → θ  =t > = − ar,t6  z   → o z  = + ,o=θ  =t >> o z  + ,o=θ  =t >> o z   o z   o x = + ,o=θ F =t >> ⋅ en=θ  =t >> 

  o   + ,o=θ F =t >> ⋅ en=θ  =t >> o x o = →  x = t6=θ  =t >> → θ  =t > = ar,t6   x  → a x = + ,o=θ F =t >> ⋅ ,o=θ  =t >>  a x a x + ,o=θ F =t >> ⋅ ,o=θ  =t >>   a x  

o y = − en=θ F =t >> ⋅ en=θ  =t >> 

  − o y   o y − o y − en=θ F =t >> ⋅ en=θ  =t >>  = → = t6=θ  =t >> → θ  =t > = ar,t6  →  − a y  a y = − en=θ F =t >> ⋅ ,o=θ  =t >> − en=θ F =t >> ⋅ ,o=θ  =t >> − a y a y    

Como -odemo o*er/ar) de manera an4+o6a a+ ,ao anterior) e 2"e e ,ara,terizan -or re+a,ionar di,(a /aria*+e arti,"+ar ,on +o /e,tore de orienta,i.n de+ e5e,tor 5ina+0 A -ear de 2"e toda e++a on /4+ida a  -riori) eto no e aí iem-re -"eto ,"ando 2"ii!ramo en,ontrar ,"a+ e e+ /a+or ,orre-ondiente de θ 5=t > ,"ando +a /aria*+e θ 4=t > e "n /a+or de π o $ π etaríamo di/idiendo -or ,ero 3 no -odríamo o*tener "n /a+or de θ 5=t >0 Gor tanto) +a úni,a o+",i.n 5a,ti*+e -ara o*tener "na re+a,i.n entre +a /aria*+e arti,"+ar θ 5=t > 3 +a ,om-onente de +a matriz T e +a -rimera o*tenida =a+ 5ina+ /eremo 2"e eto no e aí>0 •

Obt&n'()n *& la +a,(abl& a,t('ula, * !-t 

Si o*er/amo +a ,om-onente T&F) /emo r4-idamente 2"e eta ,om-onente e +a úni,a de toda +a matriz donde a-are,e e+ t!rmino de d 1=t > 3) -or tanto) e de eta ,om-onente de donde ,one6"iremo +a e<-rei.n 2"e no re+a,ione +a /aria*+e arti,"+ar ,on +o /e,tore de  n) o) a 3 p1  p x = d & =t > + l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >> ↓ d & =t > =  p x − l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>

L+e6ado a ete -"nto -odemo -enar 2"e -ara o*tener +a e<-rei.n de di,(a /aria*+e arti,"+ar *ataría ,on "tit"ir +a re+a,ione o*tenida ante de θ 4=t > 3 θ 5=t > de +a i6"iente manera1  n x      n y         a         ⇒ d  =t > =  p − l  ⋅ ,o   ar,t6 n x   ⋅ ,o   − ar,t6  z      x &     n y     o z     a z            θ  =t > = − ar,t6   o z   θ F =t > = ar,t6

$% Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT Eto -are,e +.6i,o -ero -ara im-+i5i,ar m4 +o ,4+,"+o -ara e/itar 2"e e+ ,om-"tador rea+i,e o-era,ione 2"e -"eden er inne,earia) -odemo o*tener +o /a+ore de ,o= θ 4=t >> 3 ,o=θ 4=t >> de "na manera dire,ta a -artir de +a matriz de modo 2"e en /ez de rea+izar ,"atro ad2"ii,ione de +a matriz T ,orre-ondiente a +o /a+ore n x) n y) a z  3 o z ) do di/iione) do ar,o tan6ente) do ,oeno 3 "n -rod",to) e+ ,om-"tador o+amente tendría rea+izar do ad2"ii,ione de +o /a+ore n y 3 o z  3 "n -rod",to1 n x = + en=θ F =t >>

o x = + ,o=θ F =t >> ⋅ en=θ  =t >>

a x = + ,o=θ F =t >> ⋅ ,o=θ  =t >>  p x = d & =t > + l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>

n y = + ,o=θ F =t >> o y = − en=θ F =t >> ⋅ en=θ  =t >>

a y = − en=θ F =t >> ⋅ ,o=θ  =t >>  p y = d  =t > − l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >>

n z  = @

a z  = − en=θ  =t >>

o z  = + ,o=θ  =t >>

 p z  = −d $ =t > − l F − l  ⋅ en=θ  =t >>

d & =t > =  p x − l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >> → d & =t > =  p x − l  ⋅ n y ⋅ o z 

De ete modo (emo ,one6"ido "na e<-rei.n 2"e no re+a,ione dire,tamente +a /aria*+e arti,"+ar d 1=t > ,on +a ,om-onente de +a matriz T0 •

Obt&n'()n *& la +a,(abl& a,t('ula, * "-t 

Si o*er/amo en +a ,om-onente TF) /emo r4-idamente 2"e eta ,om-onente e +a úni,a de toda +a matriz donde a-are,e e+ t!rmino de d 2=t > 3) -or tanto) e de eta ,om-onente de donde ,one6"iremo +a e<-rei.n 2"e no re+a,ione +a /aria*+e arti,"+ar ,on +o /e,tore de  n) o) a 3 p1  p y = d  =t > − l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >> ↓ d  =t > =  p y + l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >>

L+e6ado a ete -"nto -odemo -enar 2"e -ara o*tener +a e<-rei.n de di,(a /aria*+e arti,"+ar *ataría ,on "tit"ir +a re+a,ione o*tenida ante de θ 4=t > 3 θ 5=t > de 5orma an4+o6a a+ ,ao anterior0 Eto -are,e +.6i,o -ero -ara im-+i5i,ar m4 +o ,4+,"+o -ara e/itar 2"e e+ ,om-"tador rea+i,e o-era,ione 2"e -"eden er inne,earia) -odemo o*tener +o /a+ore de en= θ 4=t >> 3 ,o=θ 4=t >> de "na manera dire,ta a -artir de +a matriz de modo 2"e en /ez de rea+izar ,"atro ad2"ii,ione de +a matriz T ,orre-ondiente a +o /a+ore n x) n y) a z  3 o z ) do di/iione) do ar,o tan6ente) "n eno 3 "n ,oeno 3 "n -rod",to) e+ ,om-"tador o+amente tendría rea+izar do ad2"ii,ione de +o /a+ore n x 3 o z   3 "n  -rod",to1 n x = + en=θ F =t >>

o x = + ,o=θ F =t >> ⋅ en=θ  =t >>

a x = + ,o=θ F =t >> ⋅ ,o=θ  =t >>  p x = d & =t > + l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>

n y = + ,o=θ F =t >> o y = − en=θ F =t >> ⋅ en=θ  =t >>

a y = − en=θ F =t >> ⋅ ,o=θ  =t >>  p y = d  =t > − l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >>

n z  = @

a z  = − en=θ  =t >>

o z  = + ,o=θ  =t >>

 p z  = −d $ =t > − l F − l  ⋅ en=θ  =t >>

d  =t > =  p x + l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >> → d  =t > =  p y + l  ⋅ n x ⋅ o z 

De ete modo (emo ,one6"ido "na e<-rei.n 2"e no re+a,ione dire,tamente +a /aria*+e arti,"+ar d 2=t > ,on +a ,om-onente de +a matriz T0 $% Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT •

Obt&n'()n *& la +a,(abl& a,t('ula, * #-t 

Si o*er/amo en +a ,om-onente T$F) /emo r4-idamente 2"e eta ,om-onente e +a úni,a de toda +a matriz donde a-are,e e+ t!rmino de d 3=t > 3) -or tanto) e de eta ,om-onente de donde ,one6"iremo +a e<-rei.n 2"e no re+a,ione +a /aria*+e arti,"+ar ,on +o /e,tore de  n) o) a 3 p1  p z  = − d $ =t > − l F − l  ⋅ en=θ  =t >> ↓ d $ =t > = − p z  − l F − l  ⋅ en=θ  =t >>

L+e6ado a ete -"nto -odemo -enar 2"e -ara o*tener +a e<-rei.n de di,(a /aria*+e arti,"+ar *ataría ,on "tit"ir +a re+a,i.n o*tenida ante de θ 5=t > de 5orma an4+o6a a+ ,ao anterior0 Eto -are,e +.6i,o -ero -ara im-+i5i,ar m4 +o ,4+,"+o -ara e/itar 2"e e+ ,om-"tador rea+i,e o-era,ione 2"e -"eden er inne,earia) -odemo o*tener e+ /a+or de en=θ 4=t >> de "na manera dire,ta a -artir de +a matriz de modo 2"e en /ez de rea+izar do ad2"ii,ione de +a matriz T ,orre-ondiente a +o /a+ore a z  3 o z ) "na di/ii.n) "n ar,o tan6ente) "n eno 3 "n -rod",to) e+ ,om-"tador o+amente tendría rea+izar "na ad2"ii,i.n de+ /a+or a z 1 n x = + en=θ F =t >>

o x = + ,o=θ F =t >> ⋅ en=θ  =t >>

a x = + ,o=θ F =t >> ⋅ ,o=θ  =t >>  p x = d & =t > + l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>

n y = + ,o=θ F =t >> o y = − en=θ F =t >> ⋅ en=θ  =t >>

a y = − en=θ F =t >> ⋅ ,o=θ  =t >>  p y = d  =t > − l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >>

n z  = @

a z  = − en=θ  =t >>

o z  = + ,o=θ  =t >>

 p z  = −d $ =t > − l F − l  ⋅ en=θ  =t >>

d $ =t > = − p z  − l F − l  ⋅ en=θ  =t >> → d $ =t > = − p z  − l F + l  ⋅ a z 

De ete modo (emo ,one6"ido "na e<-rei.n 2"e no re+a,ione dire,tamente +a /aria*+e arti,"+ar d 3=t > ,on +a ,om-onente de +a matriz T0 Gor tanto) tendríamo re"e+to e+ -ro*+ema de +a ,inem4ti,a in/era de n"etro ro*ot -"eto 2"e (a*ríamo en,ontrado tanta 5"n,ione ,omo 6rado de +i*ertad tiene n"etro ro*ot 2"e re+a,ionen +a /aria*+e arti,"+are ,on +a ,om-onente de +a matriz de tran5orma,i.n (omo6!nea T1 q& =  f & ( x) y) z ) α ) β ) γ  ) q  =  f  ( x) y) z ) α ) β ) γ  ) q$ =  f $ ( x) y) z ) α ) β ) γ  ) q F =  f F ( x) y) z ) α ) β ) γ  ) q =  f  ( x) y) z ) α ) β ) γ  )

RESOLUCIN DEL GROBLEMA DE LA CINEMÁTICA INVERSA d & =t > =  p x − l  ⋅ n y ⋅ o z 

d  =t > =  p y + l  ⋅ n x ⋅ o z 

  n   θ F =t > = ar,t6  x   n y  

d $ =t > = − p z  − l F + l  ⋅ a z 

 a   θ  =t > = − ar,t6  z    o z  

$% Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT

3. IM!LEMENTACIN DEL CDI/O DE LA CINEMÁTICA INVERSA. La 5"n,i.n in/era re"e+/e +a ,inem4ti,a in/era de+ ro*ot 2"irúr6i,o de  6rado de +i*ertad0 Gara e++o toma ,omo úni,o -ar4metro +a matriz de tran5orma,i.n (omo6!nea T0 'a de tenere en ,onidera,i.n 2"e determinado -"nto 3 orienta,ione 2"e -ertenez,an a+ e-a,io de tra*a7o de+ ro*ot) no -odr4n a+,anzare ,on a+6"na ,on5i6"ra,ione0 Gor e++o e re,omenda*+e tener en ,"enta 2"e) -or e7em-+o) todo +o /e,tore de orienta,i.n de*en er ortonorma+e 3 2"e tam-o,o e e<,edan +o /a+ore de +a 6"ía de +a arti,"+a,ione -rim4ti,a1

1 $ 3 0  2  4 5 16 11 1$ 13 10

5"n,tion 2 9 in/era=T> +F9@0F? +9@0? P Ini,ia+iza,i.n de +a /aria*+e arti,"+are a ,a+,"+ar 29:@ @ @ @ @;? P So+",i.n de +a arti,"+a,ione 2=&>9T=&)F>%+T=)&>T=$)>? 2=>9T=)F>+T=&)&>T=$)>? 2=$>9%T=$)F>%+F+T=$)$>? 2=F>9atan=T=&)&>)T=)&>>? 2=>9atan=%T=$)$>)T=$)>>?

Como e -"ede o*er/ar) ete m!todo e m"3 6enera+) -ero tiene e+ in,on/eniente de +o i6no de +o 4n6"+o) 2"e de*ería e+iminare "ando +a 5"n,i.n atan=> iem-re 2"e ea -oi*+e0 En ,"a+2"ier ,ao) e+ ma3or in,on/eniente +o (emo a+/a6"ardado -"e no iem-re tenemo -or 2"! en,ontrar e,"a,ione de +a ,"a+e  -odamo en ,ada -ao de-e7ar +a /aria*+e 2"e no interea -ara -oder ,ontin"ar0

A'la,a'()n A+ -ro6ramar +a e,"a,ione o*tenida) e ne,eario ,om-ro*ar ,"4ndo a-are,en en +o denominadore ,antidade 2"e -"edan an"+are 3 2"e (a,en re5eren,ia a  -oi,ione donde e -rod",irían errore a +a (ora de ,a+,"+ar +a ,inem4ti,a in/era -or +o 2"e no ería /4+ida +a ,inem4ti,a in/era -era ,"a+2"ier matriz T0 En ee ,ao) e de*ería *",ar +a 5.rm"+a -ara tratar ee ,ao en -arti,"+ar0 Ete -ro*+ema a-are,e en +a 5.rm"+a re+a,ionada ,on +o 4n6"+o -"e a-are,en di/idiendo a y 3 o z 1   n   θ F =t > = ar,t6  x   n y  

 a   θ  =t > = − ar,t6  z    o z  

Gara e/itar eo -odríamo (a,er+o m4 en,i++o aún -"eto 2"e mirando en +a matriz en,ontramo otra -oi*i+idade de +a ei 3a ana+izada -ara re+a,ionar θ 4=t > 3 θ 5=t > ,on +a ,om-onente de +a matriz de tran5orma,i.n (omo6!nea1 n x = + en=θ F =t >>

o x = + ,o=θ F =t >> ⋅ en=θ  =t >>

a x = + ,o=θ F =t >> ⋅ ,o=θ  =t >>  p x = d & =t > + l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>

n y = + ,o=θ F =t >> o y = − en=θ F =t >> ⋅ en=θ  =t >>

a y = − en=θ F =t >> ⋅ ,o=θ  =t >>  p y = d  =t > − l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >>

n z  = @

a z  = − en=θ  =t >>

o z  = + ,o=θ  =t >>

 p z  = −d $ =t > − l F − l  ⋅ en=θ  =t >>

$ % &@ Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT Gor tanto -ara o*tener "na e<-rei.n 2"e re"e+/a e+ 4n6"+o θ 4=t > -odemo em-+ear ,"a+2"iera de +a do i6"iente e<-reione1 n x = + en=θ F =t >> → θ F =t > = ar,en= n x > n y = + ,o=θ F =t >> → θ F =t > = ar,,o=n y >

G"eto 2"e -odemo e+e6ir ,"a+2"iera de +a do) e+e6iremo +a -rimera in  -ro*+ema -"e ,"a+2"iera de +a do e /4+ida0 Con re-e,to a+ 4n6"+o θ 4=t >) o-eramo de 5orma imi+ar1 a z  = − en=θ  =t >> → θ  =t > = ar,en= − a z  > o z  = + ,o=θ  =t >> → θ  =t > = ar,,o= +o z  >

De i6"a+ modo) o-tamo -or e+e6ir +a -rimera e<-rei.n0 Si e5e,t"amo eto ,am*io en e+ ,.di6o im-+ementado) tenemo +o i6"iente1

1 $ 3 0  2  4 5 16 11 1$ 13 10

5"n,tion 2 9 in/era=T> +F9@0F? +9@0? P Ini,ia+iza,i.n de +a /aria*+e arti,"+are a ,a+,"+ar 29:@ @ @ @ @;? P So+",i.n de +a arti,"+a,ione 2=&>9T=&)F>%+T=)&>T=$)>? 2=>9T=)F>+T=&)&>T=$)>? 2=$>9%T=$)F>%+F+T=$)$>? 2=F>9ain=T=&)&>>? 2=>9ain=%T=$)$>>?

Gor tanto) tendríamo re"e+to e+ -ro*+ema de +a ,inem4ti,a in/era de n"etro ro*ot de "na manera m4 e5i,az 2"e no -ermite o*tener +a ,inem4ti,a in/era de ,"a+2"ier matriz T1 q& =  f & ( x) y) z ) α ) β ) γ  ) q  =  f  ( x) y) z ) α ) β ) γ  ) q$ =  f $ ( x) y) z ) α ) β ) γ  ) q F =  f F ( x) y) z ) α ) β ) γ  ) q =  f  ( x) y) z ) α ) β ) γ  )

RESOLUCIN DEL GROBLEMA DE LA CINEMÁTICA INVERSA d & =t > =  p x − l  ⋅ n y ⋅ o z 

d  =t > =  p y + l  ⋅ n x ⋅ o z 

θ F =t > = ar,en (n x )

d $ =t > = − p z  − l F + l  ⋅ a z 

θ  =t > = ar,en (− a z  )

$ % && Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT

0. E7EM!LOS DE UTILI8ACIN DE LA 9UNCIN IM!LEMENTADA.  29:&0 &0 @0 -iF -iF;

 29:%&0 %&0 @0 -i -i;

29

29

 &0@@@ &0@@@ @0@@@ @0F @0F

 &0@@@ &0@@@ @0@@@ @0$ &0@

 T9dire,ta=2>

 T9dire,ta=2>

T9

T9

@0@& @0@@@ @0@@@ &0F@@@ @0@& @0@@@ @0@@@ &0@@@ @0@@@@ @0@& @0@& &0@F&F @0@@@@ @0@@@@ @0@@@@ &0@@@@

@0@@@ @0@ @0@@@@ &0@@@ @0@ @0@@@ @0@@@@ &0@@@ @0@@@@ @0@@@@ &0@@@@ &0&@@@ @0@@@@ @0@@@@ @0@@@@ &0@@@@

 in/era=T>

 in/era=T>

an 9

an 9

 &0@@@ &0@@@ @0@@@ @0F @0F

 &0@@@ &0@@@ @0@@@ @0$ &0@

 29:& & @0 -i -i;

 29:& & @0 -i -i$;

29

29  &0@@@@ &0@@@@ @0@@@ @0$ &0@F

 &0@@@@ &0@@@@ @0@@@ @0$ &0@  T9dire,ta=2>

 T9dire,ta=2>

T9

T9

 @0@@@ @0@  @0@@@@ @0@@@@

@0@ @0@@@@ @0@@@ @0@@@@ @0@@@@ &0@@@@ @0@@@@ @0@@@@

 @0@@@ @0@@ @0@ @0F$$@  @0@@@@ @0@@@ @0@@@@ @0@@@@

&0@@@@ &0@@@@ &0&@@@ &0@@@@

 in/era=T>

 in/era=T>

an 9

an 9

@0F$$@ @0@@ @0@ @0@@@@

@0&$F @0@@ @0 &0@@@@

 &0@@@@ &0@@@@ @0@@@ @0$ &0@F

 &0@@@@ &0@@@@ @0@@@ @0$ &0@

$ % & Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT . LA CINEMÁTICA INVERSA COMO VENTA7A MATRI8 7ACOBIANA INVERSA.

La ,inem4ti,a in/era de n"etro ro*ot e m"3 en,i++a -"eto 2"e tan .+o ,on "na ad2"ii,ione en memoria 3 o-era,ione a+6e*rai,a ,omo "ma) reta o m"+ti-+i,a,ione 3) tam*i!n "na o-era,i.n de arcoseno  e ,oni6"e o*tener +a ,inem4ti,a in/era0 Eto ,on++e/a "n 6ato ,om-"ta,iona+ m4 *a7o ,on re-e,to a "n ro*ot ,on m4 6rado de +i*ertad0 Adem4 +a matriz Ja,o*iana in/era e -"ede o*tener -or "n -ro,edimiento  -are,ido a+ de +a o*ten,i.n de +a Ja,o*iana dire,ta) -ero a(ora -artiendo de+ mode+o ,inem4ti,o in/ero1 q& =  f & ( x) y) z ) α ) β ) γ  )

q  =  f  ( x) y ) z ) α ) β ) γ  )

q F =  f F ( x) y ) z ) α ) β ) γ  )

q $ =  f $ ( x) y) z ) α ) β ) γ  )

q  =  f  ( x) y ) z ) α ) β ) γ  )

La matriz Ja,o*iana e o*tendr4 -or di5eren,ia,i.n ,on re-e,to de+ tiem-o de am*o miem*ro de +a i6"a+dad1  ∂d &  x&   ∂ x &   d     &    y&   ∂d  &  d     ∂ x     z  &  ∂d $  d  &  = 7 −& ⋅   ⇒ 7 −& =   $ α   ∂ x & θ &     ∂θ F F    β &   ∂ x θ &        γ     ∂θ  &    ∂ x

∂d &

∂d &

∂d &

∂d &

∂d &  ∂γ     ∂d  

∂ y ∂d 

∂ z  ∂d 

∂α  ∂d 

∂ β  ∂d 

∂ y ∂d $

∂ z  ∂d $

∂α  ∂d $

∂ β  ∂d $

∂γ    ∂d $ 

∂ y ∂θ F

∂ z  ∂θ F

∂α  ∂θ F

∂ β  ∂θ F

∂γ    ∂θ F 

∂ y ∂θ 

∂ z  ∂θ 

∂α  ∂θ 

∂ β  ∂θ 

∂γ    ∂θ  

∂ y

∂ z 

∂α 

∂ β 

 

∂γ   

Si tomamo ,omo 4n6"+o de E"+er %) &  3 ' ,omo +o 4n6"+o 2"e 5orma n"etro /e,tor a de a-ro
Án
r

r

r

i ⋅a = i ⋅ a

,o=α >

,o=α > = ,o=θ F =t >> ⋅ ,o=θ  =t >> ⇒ α  = ar,,o=,o=θ F =t >> ⋅ ,o=θ  =t >>>

Án
r

r

r

 j ⋅ a =  j ⋅ a

,o= β >

,o= β > = − en=θ F =t >> ⋅ ,o=θ  =t >> ⇒ β  = ar,,o=− en=θ F =t >> ⋅ ,o=θ  =t >>>

Án
r

r

r

(  ⋅ a = (  ⋅ a

,o=γ  >

,o=γ  > = − en=θ  =t >> ⇒ γ  = ar,,o=− en=θ  =t >>>

$ % &$ Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT A ,ontin"a,i.n *",aremo +a re+a,ione 2"e e1 ,o= β > = − en=θ F =t >> ⋅ ,o=θ  =t >>  ,o=α > = ,o=θ F =t >> ⋅ ,o=θ  =t >>  ,o  = β > = [− en=θ F =t >> ⋅ ,o=θ  =t >>]  

,o  =α > = [+ ,o=θ F =t >> ⋅ ,o=θ  =t >>]  ,o  =α > + ,o  = β > = [+ ,o=θ F =t >> ⋅ ,o=θ  =t >>] + [− en=θ F =t >> ⋅ ,o=θ  =t >>] ,o  =α > + ,o  = β > = ,o  =θ F =t >> ⋅ ,o  =θ  =t >> + en  =θ F =t >> ⋅ ,o  =θ  =t >> ,o  =α > + ,o  = β > = [,o  =θ F =t >> + en  =θ F =t >>]⋅ ,o  =θ  =t >> ,o  =α > + ,o  = β > = ,o  =θ  =t >> ⇒ ,o=θ  =t >> = ,o  =α > + ,o  = β >   θ  =t > = ar,,o   ,o =α > + ,o = β >     

& = ,o  =θ  =t >> + en  =θ  =t >> ⇒ en  =θ  =t >> = & − ,o  =θ  =t >> en=θ  =t >> = & − ,o  =θ  =t >> ⇒ en=θ  =t >> = & − =,o  =α > + ,o  = β >>   θ  =t > = ar,en   & − =,o =α > + ,o = β >>     

Con ete dato) -odemo o*tener e+ /a+or de +a otra /aria*+e arti,"+ar de rota,i.n θ F1 ,o= β > = − en=θ F =t >> ⋅ ,o=θ  =t >>  

,o=θ  =t >> = ,o  =α > + ,o  = β >  ,o= β > = − en=θ F =t >> ⋅ ,o  =α > + ,o  = β > ⇒ en=θ F =t >> =

− ,o= β >

,o  =α > + ,o  = β >

    − ,o= β >   θ F =t > = ar,en  ,o  =α > + ,o  = β >     

,o=α > = ,o=θ F =t >> ⋅ ,o=θ  =t >>  

,o=θ  =t >> = ,o  =α > + ,o  = β >  ,o=α > = ,o=θ F =t >> ⋅ ,o  =α > + ,o  = β > ⇒ ,o=θ F =t >> =

,o=α > ,o  =α > + ,o  = β >

    ,o=α >   θ F =t > = ar, ,o  ,o  =α > + ,o  = β >     

$ % &F Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT La otra arti,"+a,ione e o*tienen de +a i6"iente manera1  x = d & =t > + l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >> ⇒ d & =t > = x − l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>

 d & =t > = x − l  ⋅ ,o=θ F =t >> ⋅ ,o=θ  =t >>  ,o=α > ,o=α >    ⇒ d & =t > = x − l  ⋅ ,o=θ F =t >> = ⋅ ,o =α > + ,o = β > >      ,o =α > + ,o = β >  ,o =α > + ,o = β >  ,o=θ  =t >> = ,o  =α > + ,o  = β >  d & =t > = x − l  ⋅ ,o=α >

 y = d  =t > − l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >> ⇒ d  =t > =  y + l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >>



d  =t > =  y + l  ⋅ en=θ F =t >> ⋅ ,o=θ  =t >>

en=θ F =t >> =

− ,o= β >

,o  =α > + ,o  = β >

,o=θ  =t >> = ,o  =α > + ,o  = β >

 − ,o= β >    ⋅ ,o =α > + ,o = β >  ⇒ d  =t > =  y + l  ⋅  ,o  =α > + ,o  = β >  

d  =t > =  y − l  ⋅ ,o= β >

 z = − d $ =t > − l F − l  ⋅ en=θ  =t >> ⇒ d $ =t > = − z + −l F − l  ⋅ en=θ  =t >> d $ =t > = − z + −l F − l  ⋅ en=θ  =t >>

    ⇒ d $ =t > = − z  + −l F − l  ⋅ & − =,o =α > + ,o = β >>   en=θ  =t >> = & − =,o =α > + ,o = β >>  d $ =t > = − z + −l F − l  ⋅

& − =,o  =α > + ,o  = β >>

'a6amo "n -e2"eHo re"men de +a e<-reione o*tenida1 d & =t > =  x − l  ⋅ ,o=α > d  =t > =  y − l  ⋅ ,o= β > d $ =t > = − z + −l F − l  ⋅

& − =,o  =α > + ,o  = β >>

    ,o=α >   θ F =t > = ar, ,o  ,o  =α > + ,o  = β >     

  θ  =t > = ar,,o   ,o =α > + ,o = β >     

$ % & Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT A ,ontin"a,i.n) -ro,edemo a o*tener +a deri/ada ne,earia -ara ,on5e,,ionar +a matriz Ja,o*iana in/era1 



d & =  x − l  ⋅ ,o=α >>

d  =  y − l  ⋅ ,o= β >

d $ = − z + −l F − l  ⋅ & − =,o =α > + ,o = β >>

∂d &

∂d 

∂d $

= & ⋅ x&

∂ x ∂d &

∂ x ∂d 

=@

∂ y ∂d &

∂ y ∂d 

=@

∂ z  ∂d &

& = +l  ⋅ en=α >>α 

∂α  ∂d &

=@

∂ β  ∂d & ∂γ  

∂ z  ∂d  ∂α  ∂d  ∂ β  ∂d 

=@

∂γ  

=@ = & ⋅ y&

∂ x ∂d $

=@

∂d $

∂ y ∂ z  ∂d $

=@

∂α 

& = +l  ⋅ en= β >> β 

∂d $ ∂ β 

=@

∂d $ ∂γ  

    ,o=α >   θ F = ar, ,o  ,o  =α > + ,o  = β >      ∂θ F ∂ x ∂θ F ∂ y ∂ z  ∂θ F

∂θ 

=@

∂ y

=+

∂ z  ∂θ  & α 

∂α 

,o  =α > + ,o  = β > en= β > ⋅ ,o=α > & ∂θ F =−  β  ∂ β  ,o  =α > + ,o  = β >

∂θ 

∂θ F

∂θ 

∂α 

∂γ  

=@

en=α > ⋅ ,o=α >

=−

=+









en =α > − en = β >> en= β > ⋅ ,o= β >

& α 

 β &

en =α > − en = β >> =@



 

∂ x ∂θ 

en=α > ⋅ ,o= β >

& = −&⋅ z 



=@

=@

=@

θ  = ar,,o   ,o =α > + ,o = β > 

∂θ 

∂θ F

=@

∂ β  ∂γ  

 

=@ =@ =@

en=α > ⋅ ,o=α >

=+

=−

















en =α > − ,o = β > ,o =α > + ,o = β > en=α > ⋅ ,o=α >

& α 

 β &

en = β > − ,o =α > ,o =α > + ,o = β > =@

Gor tanto) +a matriz Ja,o*iana in/era ería1 & &    d  &    @ &  d      @  d  & =   $  θ &  @  F  θ &     @  

@

@

+ l  ⋅ en=α >>

&

@

@

−&

@ en=α > ⋅ ,o=α >

@

@

@

@

− + +

@

+

en  =α > − en  = β >> en=α > ⋅ ,o= β > ,o  =α > + ,o  = β > en=α > ⋅ ,o=α >

−

en  =α > − ,o  = β > ,o  =α > + ,o  = β >

+ l  ⋅ en= β >> en= β > ⋅ ,o= β >

en  =α > − en  = β >> en= β > ⋅ ,o=α > − ,o  =α > + ,o  = β > en=α > ⋅ ,o=α > en  = β > − ,o  =α > ,o  =α > + ,o  = β >

@

  x&  @  &    y @  &    z  ⋅  α  & @     β &    @   &   γ   

Gero ta+ 3 ,omo -odemo o*er/ar (emo o*tenido "na ,o+"mna ++ena de ,ero  -"eto 2"e n"etro ro*ot no -odía ++e6ar a tener ,in,o 6rado de +i*ertad0 Gor e++o e+iminamo +a ú+tima ,o+"mna0 $ % & Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT L+e6amo a+ i6"iente re"+tado &  &   d  &   @  &   d     @   d  &  = $    θ &  @  F  θ &      @  

@

@

+ l  ⋅ en=α >>

&

@

@

−&

@ en=α > ⋅ ,o=α >

@

@

@

@

−

  x&       & + l  ⋅ en= β >>   y  en= β > ⋅ ,o= β >    + &     z  en =α > − en = β >> ⋅    & en= β > ⋅ ,o=α >  α  −     ,o =α > + ,o = β >  &   β  en=α > ⋅ ,o=α >    −     & en = β > − ,o =α > ,o =α > + ,o = β >   γ  

en  =α > − en  = β >> en=α > ⋅ ,o= β > + ,o  =α > + ,o  = β > en=α > ⋅ ,o=α >

+

7

@

en  =α > − ,o = β > ,o =α > + ,o = β > −&

=

en  =α > ⋅ ,o=α > ⋅ ,o= β > − ,o  =α > ⋅ en=α > ⋅ en= β > =,o  =α > + ,o  = β >>

$



en  = β > − ,o  =α >

A+ i6"a+ 2"e en +a Ja,o*iana dire,ta -odemo o*tener ,"ando e an"+a e+ determinante 3 ,om-ro*ar 2"e orienta,ione -ro/o,arían 2"e e an"+ae n"etra Ja,o*iana in/era1 7

−&

=@⇒

en  =α > ⋅ ,o=α > ⋅ ,o= β > − ,o  =α > ⋅ en=α > ⋅ en= β > 



=,o =α > + ,o = β >>

$







=@

en = β > − ,o =α >

E+ denominador no e -"ede an"+ar -"eto 2"e i no no e − ,o  =α > ≠ @ ⇒ en  = β > ≠ ,o  =α > ⇒ en= β > ≠ ± ,o=α > ⇒  β  ≠ @ ± α  =,o  =α > + ,o  = β >>

$



≠ @ → N"n,a -"ede an"+are0

en  =α > ⋅ ,o=α > ⋅ ,o= β > − ,o  =α > ⋅ en=α > ⋅ en= β > = @ en=α > ⋅ ,o=α >[en=α > ⋅ ,o= β > − ,o=α > ⋅ en= β >] = @ en=α > = @ ⇒ α  = @ )&@ )$@ ,o=α > = @ ⇒ α  = @ )@ en=α > ⋅ ,o= β > − ,o=α > ⋅ en= β > = @ ⇒ en=α > ⋅ ,o= β > = ,o=α > ⋅ en= β > en=α > en= β > ⇒ tg =α > = tg = β > ⇒ α  =  β  = ,o=α > ,o= β >

Gor tanto) no de*eríamo tra*a7ar ,on e+ ro*ot -ara di,(a orienta,ione -"e ,am*io in5initeima+e en +o motore -ro/o,arían a+tíima /aria,ione en +o 4n6"+o0 La orienta,ione no de*en dar n"n,a "n 4n6"+o  de @) @ &@) @ . $@ ni tam-o,o -"eden er i6"a+e +o 4n6"+o  3 0 De (e,(o) +a /enta7a de +a en,i++ez de +a ,inem4ti,a in/era e 2"e 6ra,ia a eto  -odemo o*tener de 5orma ana+íti,a +a matriz Ja,o*iana in/era 3 -odemo *",ar +a orienta,ione en +a -eore -oi,ione0 A+ i6"a+ 2"e en +a Ja,o*iana dire,ta) -odríamo deri/ar e+ determinante re-e,to  3  reo+/er e+ itema 3 *",ar +a orienta,ione .-tima donde m4 8,.modo8 e en,"entra e+ ro*ot0 $ % & Jaime Martínez Verdú

Jo! L"í Aznar #omi

CINEMÁTICA INVERSA DEL QUIROBOT Si intent4ramo en,ontrar +o /a+ore an6"+are -ara +o ,"a+e e+ /a+or a*o+"to de+ determinante de +a matriz Ja,o*iana in/era e m4
−&

=

en  =α > ⋅ ,o=α > ⋅ ,o= β > − ,o  =α > ⋅ en=α > ⋅ en= β > =,o  =α > + ,o  = β >>

$



en  = β > − ,o  =α >

 = @  ∂α   ⇒ Condi,ione de o-tima+idad −& ∂7  = @ ∂ β   ∂7

−&

$ % & Jaime Martínez Verdú

Jo! L"í Aznar #omi

REPRESENTACIÓN GRÁFICA DEL QUIROBOT

Capí tulo

4

R EPR ESENTACIÓN  G GR ÁFICA  D DEL  Q QUIR OBOT "UNA IMAGEN VALE MÁS QUE MIL PALABRAS"

 Refranero popular 

En e$ta *arte !e +a *r,-ti-a $e .an a *re$entar &na$ $en-i++a$/ *er# *#tente$ 0erramienta$ 1r,2i-a$ .,+i!a$ *ara -#m*r#3ar +#$ re$&+ta!#$ anteri#re$ &ti+izan!# +a$ 2&n-i#ne$ 4a e$t&!ia!a$5

! REPRESENTACIÓN GRÁFICA EN MatLa # USANDO ALAMBRES! ! E$pl%&a&%o'() )o*( (l +u'&%o'a,%('to -( la +u'&%.' -( *(p*()('ta&%.' /D! Para -#m*r#3ar +#$ re$&+ta!#$ !e+ *#$i-i#namient# !e n&e$tr# r#3#t $e 0a &ti+iza!# &na n&e.a 2&n-i6n n#m3ra!a dibujardirecta.m/ 7&e *ermite &na $en-i++a re*re$enta-i6n tri!imen$i#na+ &ti+izan!# +a 0&e++a D(8 !e+ r#3#t mani*&+a!#r5 E+ -6!i1# 7&e m#$trarem#$ a -#ntin&a-i6n -#n$ta !e +# $i1&iente •

•

•

Lí'(a) 0 1 23  Em*+ean!# +a ta3+a -#n +#$ *ar,metr#$ !e D8 9+ínea$ !e +a : a +a );< $erem#$ -a*a-e$ !e -a+-&+ar -a!a &na !e +a$ matri-e$ !e tran$2#rma-i6n 0#m#1%nea$ 7&e re+a-i#nan +#$ $i$tema$ -#n$e-&ti.#$ !#$ a !#$5 Lí'(a) 5 1 /3   A -#ntin&a-i6n/ *r#-e!em#$ a -a+-&+ar +a$ matri-e$ !e tran$2#rma-i6n 0#m#1%nea !e -a!a $i$tema re$*e-t# !e+ $i$tema !e +a 3a$e 2i=a5 Re-#r!em#$ +a ne-e$i!a! !e &$ar +a$ matri-e$ !e -am3i# !e 3a$e 7&e em*+e,3am#$ en e+ $i$tema !e+ m&n!# 4 e+ !e+ e2e-t#r 2ina+5 Lí'(a) 0 1 /03  Em*+ean!# +a$ matri-e$ !e tran$2#rma-i6n !e+ *rimer $i$tema a+ -#rre$*#n!iente tenem#$ in2#rma-i6n $&2i-iente -#m# *ara #3tener +a$ -##r!ena!a$ !e -a!a e$+a36n re$*e-t# a+ 2i=#5 Una .ez #3teni!a$ t#!#$ +#$ *&nt#$ +#$

a+ma-enam#$ en tre$ .e-t#re$ $/ 6 4 75 •

Lí'(a) /2 1 43  Di3&=am#$ e+ r#3#t .i$t# !e$!e e+ $i$tema !e -##r!ena!a$ !e +a 3a$e -an6ni-a !e3i!# a +# 7&e 4a e>*+i-am#$ en +a -inem,ti-a !ire-ta5 Si n# 0&3i%ram#$ m&+ti*+i-a!#  A01 = A00 * A01 0&3i%$em#$ -am3ia!#  plot(x, y, z)  *#r plot(z, -y, x).

'() Jaime Martínez er!"

J#$% L&í$ Aznar G#mi$

REPRESENTACIÓN GRÁFICA DEL QUIROBOT

! I,pl(,('ta&%.' -(l &.-%8o -( la +u'&%.' + u'&%.' -( *(p*()('ta&%.' /D! E+ -6!i1# e$ e+ $i1&iente@ 2&n-ti#n !i3&=ar!ire-ta97<  !i3&=ar!ire-ta97<  2&n-ti#n  / +';5' 4 +;5? 9 0  Par,metr#$ Dena.it(8arten3er1 !e+ r#3#t *i? ; 79'< 79<  : teta   ; 79?< 79H< +' ;  2 !   79)< 79?< ; ; ; +  ; a ; ; *i? ;  5 a+2a   *i? (*i?    Matri-e$ !e tran$2#rma-i6n 0#m#1%nea entre $i$tema$ !e -##r!ena!a$ -#n$e-&ti.#$ / A;;  ; ; ) ; ; () ; ; ) ; ; ; ; ; ; ) 4 A;)  !ena.it9teta9)/ 4/ z< !e -a!a $i$tema !e -##r!ena!a$ 4;  ; z;  ; 2 >;  ; 4)  A;)9?/'< z)  A;)9H/'< ; >)  A;)9)/'< 4?  A;?9?/'< z?  A;?9H/'< /5 >?  A;?9)/'< 4H  A;H9?/'< zH  A;H9H/'< / >H  A;H9)/'< 4'  A;'9?/'< z'  A;'9H/'< / >'  A;'9)/'< 4  A;9?/'< z  A;9H/'< // >  A;9)/'< /4 /9 Se !i3&=a e+ r#3#t /0 >  >; >) >? >H >' > /: 4  4; 4) 4? 4H 4' 4 /2 z  z; z) z? zH z' z /; 45  *+#tH9>/4/z<@ 4  Se -#+#-a &na re=i++a a +#$ e=e$ 4 1ri! 4/  Se e$ta3+e-en +#$ +ímite$ !e +#$ e=e$ 44 a>i$9(? ? (? ? (? ?<

'(? Jaime Martínez er!"

J#$% L&í$ Aznar G#mi$

REPRESENTACIÓN GRÁFICA DEL QUIROBOT

/! E<(,plo) -( ut%l%7a&%.' -( la +u'&%.' %,pl(,('ta-a!  7()5 () ;5 (*iK (*iH 7 ()5;;; ()5;;;; ()5;;;; ;5;;; (;5?HK (;5?HK ()5;':?  !i3&=ar!ire-ta97<

 7() () ;5 (*iK (*iH 7 ()5;;;; ()5;;;; ()5;;;; ;5;;; (;5?HK (;5?HK ()5;':?  !i3&=ar!ire-ta97<

'(H Jaime Martínez er!"

J#$% L&í$ Aznar G#mi$

REPRESENTACIÓN GRÁFICA DEL QUIROBOT

! ANIMACIÓN GRÁFICA EN MatLa # USANDO ALAMBRES! ! E$pl%&a&%o'() )o*( (l +u'&%o'a,%('to -( la +u'&%.' -( *(p*()('ta&%.' *(p*()('ta&%.' /D! A -#ntin&a-i6n/ en e$te a*arta!#/ 0arem#$ &$# !e +a$ 2&n-i#ne$ im*+ementa!a$ 0a$ta a0#ra inversa.m 4 dibujardirecta.m5 Se trata !e -#+#-ar a+ r#3#t en !#$ *#$i-i#ne$ !i$tinta$ 4 animar &na tra4e-t#ria re-ta entre !i-0a$ -#n2i1&ra-i#ne$5 Para e++# $e !e3er,n 1enerar .aria$ *#$i-i#ne$ interme!ia$5 Uti+izarem#$ +a 2&n-i6n *+ani2i-a en +a -&a+ intr#!&-irem#$ +a$ -##r!ena!a$ -arte$iana$ !e +#$ *&nt#$ ini-ia+ 4 2ina+ 4 e+ n"mer# !e *&nt#$ interme!i#$5 A!em,$ !e e$ta 2&n-i6n/ &ti+izarem#$ #tra 2&n-i6n ++ama!a anima-i6n 7&e re*re$enta +a$ *#$i-i#ne$ 7&e 0a i!# t#man!# e+ r#3#t *ara ++e1ar !e &n *&nt# a #tr#5

! I,pl(,('ta&%.' -(l &.-%8o -( la +u'&%.' + u'&%.' -( *(p*()('ta&%.' /D3 ANIMA! E+ -6!i1# im*+ementa!# *ara e$ta 2&n-i6n e$ e+ $i1&iente@

  / 4 9 0 : 2 ; 5   / 4

 ANIMA Anima-i6n !e +a tra4e-t#ria !e &n r#3#t !e  GDL  en +a matriz MATQ/ !e &n r#3#t !e  GDL5 MATQ -#ntiene ' 2i+a$  4 &na -#+&mna *ara -a!a !i$*#$i-i6n !e+ r#3#t5 2&n-ti#n anima9mat7< 2&n-ti#n  anima9mat7< n$ize9mat7/?< 2#r  i)@n  i)@n 7mat79@/i< !i3&=ar!ire-ta97< 0#+! #n #n 1ri! en!

Ta+ 4 -#m# *&e!e #3$er.ar$e/ +a 2&n-i6n m&e$tra *#r *anta++a -a!a &na !e +a$ -#n2i1&ra-i#ne$ 7&e 0a a!#*ta!# e+ r#3#t 0a-ien!# ++ama!a$ a +a 2&n-i6n dibujardirecta.m  em*+ean!# -#m# ar1&ment#$ -a!a &n# !e +#$ .e-t#re$ arti-&+are$ 1enera!#$ -#n +a 2&n-i6n  planiica.m 4 7&e e$t,n a+ma-ena!#$ en +a matriz mat   !5

/! I,pl(,('ta&%.' -(l &.-%8o -( la +u'&%.' + u'&%.' -( *(p*()('ta&%.' /D3 PLANIFICA! Para *#!er -a+-&+ar +a matriz !e -##r!ena!a$ arti-&+are$ -a+-&+am#$  *rimeramente t#!#$ +#$ *&nt#$ 7&e -#m*#nen e+ $e1ment# !e &ni6n entre e+ *&nt# 2ina+ e ini-ia+ en tant#$ $e1ment#$ -#m# npuntos  0a++am#$ in-+&i!# 9*ara e++#/ en -a!a itera-i6n +e $&mam#$ a  p1  &n $e1ment# !e +#n1it&! entre  p1 4  p" !i.i!i!a entre +#$ npuntos en +a !ire--i6n -#rre$*#n!iente<5 Tam3i%n rea+izarem#$ ++ama!a$ a +a 2&n-i6n in.er$a *ara 7&e n#$ -a+-&+e -a!a .e-t#r arti-&+ar5

'(' Jaime Martínez er!"

J#$% L&í$ Aznar G#mi$

REPRESENTACIÓN GRÁFICA DEL QUIROBOT E+ -6!i1# e$ e+ $i1&iente

  / 4 9 0 : 2 ; 5   / 4 9 0 : 2 ; 5   / 4 9 0 : 2 ; /5 / / // /4 /9 /0 /: /2

 PLANIFICA P+ani2i-a-i6n !e tra4e-t$ en +ínea re-ta !e+ r#3#t GDL  Q  PLANIFICA9*)/*?/n/$/a/n*&nt#$< !e.&e+.e +a$ -##r!$ arti-&+are$  -#rre$*#n!iente$ a+ in$tante a-t&a+ !e $im&+a-i6n en &na *+ani2i-a-i6n  !e tra4e-t#ria en +ínea re-ta entre !#$ *&nt#$ -arte$ian#$5  *)  Re*re$enta e+ *&nt# !e *arti!a *)>/ *)4/ *)z/ )  *)  Re*re$enta e+ *&nt# !e ++e1a!a *?>/ *?4/ *?z/ )  n  Re*re$enta e+ .e-t#r 2i+a !e n#rma+i!a! n n> n4 nz ;  $  Re*re$enta e+ .e-t#r 2i+a !e #rienta-i6n $ $> $4 $z ;  a  Re*re$enta e+ .e-t#r 2i+a !e a*r#>ima-i6n a a> a4 az ;  n*&nt#$  Re*re$enta +a -anti!a! !e *&nt#$ 7&e !e$eam#$ int*#+ar  *+ani2i-a +a tra4e-t#ria !e+ e>trem# !e+ r#3#t entre ? *&nt#$ en +#$ 7&e  n#$ !an +a *#$i-i6n 4 #rienta-i6n !e+ e>trem# 9#rienta-i6n i1&a+ en  am3#$ *&nt#$/ 4 *#r +# tant# t3 en +#$ interme!i#$< 2&n-ti#n mat7*+ani2i-a9*)/*?/n/$/a/n*&nt#$< !e2in# mat7 mat7zer#$9/n*&nt#$?< a0#ra -a+-&+# +a !i$tan-ia entre ini-ia+ 4 2ina+/ 4 +&e1# +a !i.i!# en e+ n !e e$*a-i#$ entre *&nt#$ interme!i#/ -#n +# 7 #3ten!# +a !i$tan-ia a $&mar *ara *a$ar !e &n# a #tr# en H! &*?(*) !&9n*&nt#$)< 2#r i;@9n*&nt#$)< -a+-&+# !e +a *#$i-i#n !e+ e>trem# !e+ r#3#t en +a$ n*&nt#$ *#$i-i#ne$ interme!ia$  **)i! Tn $ a * -a+-&+# !e +a$ -##r!ena!a$ arti-&+are$ !e -a!a *&nt# interme!i# 7in.er$a9T< mat79@/i)<7 en!  a0#ra ten!rem#$ 7 ++amar a +a 2&n-i#n anima *ara 7 !i3&=e e+ r#3#t en  -a!a &na !e +a$ *#$i-i#ne$ interme!ia$ -#n#-ien!# +a$ -##r!ena!a$  arti-&+are$ en -a!a *&nt# interme!i# 9a+ma-ena!a$ en mat7<

'( Jaime Martínez er!"

J#$% L&í$ Aznar G#mi$

REPRESENTACIÓN GRÁFICA DEL QUIROBOT

/! E<(,plo) -( ut%l%7a&%.' -( la +u'&%.' %,pl(,('ta-a!

'(K Jaime Martínez er!"

J#$% L&í$ Aznar G#mi$

DINÁMICA DEL QUIROBOT

5

Capí tulo DINÁMICA  D DEL  Q QUIR OBOT "LA FUERZA ES IGUAL A LA MASA POR LA ACECLERACIÓN"  Sir Isaac Newton

La din%mi&a se o&!'a de (a re(a&i)n entre (as *!erzas +!e a&túan so,re !n &!er'o - e( mo.imiento +!e en ( se ori/ina0 Así1 en esta 'r%&ti&a nos dis'!simos a esta,(e&er (a re(a&i)n entre e( mo.imiento de n!estro ro,ot - (as *!erzas im'(i&adas0 N!estro mode(o din%mi&o re(a&iona matem%ti&amente (a (o&a(iza&i)n de( ro,ot 2de*inida 'or s!s .aria,(es arti&!(ares3 - s!s deri.adas4 (a .e(o&idad - a&e(era&i)n1 &on (as *!erzas - 'ares a'(i&ados en (as arti&!(a&iones1 - &on (os 'ar%metros dimensiona(es de( ro,ot4 masas1 (on/it!des1 iner&ias000 La din%mi&a de n!estro ro,ot (a .amos a est!diar desde dos  '!ntos de .ista o'!estos1 es de&ir .amos a est!diar s! din%mi&a dire&ta - s! din%mi&a in.ersa0 La din%mi&a dire&ta &onsiste en o,tener (os .a(ores de 'osi&i)n1 .e(o&idad a&e(era&i)n de &ada !na de (as arti&!(a&iones de n!estro ro,ot a 'artir de !nos determinados 'ares - *!erzas a'(i&ados a &ada arti&!(a&i)n0 5or otro (ado1 (a din%mi&a in.ersa &onsiste en o,tener (os .a(ores de 'ares *!erzas +!e ne&esitamos a'(i&ar a &ada arti&!(a&i)n 'ara +!e n!estro ro,ot a(&an&e !nas determinadas 'osi&iones arti&!(ares &on !nas determinadas .e(o&idades - a&e(era&iones arti&!(ares0

! DINÁMICA INERSA! 5ara (a #$%&'$(a $%)*+,a  !ti(izamos (a *orm!(a&i)n Ne6ton$E!(er0 7e trata de !n mtodo +!e nos da (os .a(ores n!mri&os de 'ares - *!erzas re+!eridos 'ara a(&anzar !nas 'osi&iones1 .e(o&idades - a&e(era&iones determinadas de( ro,ot 're.iamente se(e&&ionadas 2.a(ores introd!&idos1 en e( es'a&io &artesiano30 La *orm!(a&i)n Ne6ton$E!(er 'arte de( e+!i(i,rio de *!erzas - 'ares4 ∑ F  = m v ∑ T  = I ⋅ w+ w ⋅ 2 I ⋅ w3 1 - e( a(/oritmo a( +!e &ond!&en se ,asa en o'era&iones .e&toria(es1 - es &om'!ta&iona(mente m!- e*i&iente4 e( número de o'era&iones a rea(izar de'ende dire&tamente de( número de arti&!(a&iones +!e ten/amos0 #$# Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT La *!n&i)n +!e nos da (a din%mi&a in.ersa a 'artir de este a(/oritmo es newtoneuler5.m1 (a &!a( &a(&!(a e( .e&tor 9:# de 'ares;*!erzas de entrada a (as arti&!(a&iones0 5as%ndo(e !na serie de 'ar%metros 'ara s! &%(&!(o4 Q es e( .e&tor 9:# de &oordenadas arti&!(ares0 Q5 es e( .e&tor 9:# +!e re'resenta (a .e(o&idad de &ada arti&!(a&i)n0 Q55 es e( .e&tor 9:# +!e indi&a (a a&e(era&i)n de &ada arti&!(a&i)n0 " es e( .a(or de (a /ra.edad 2m;s<830 M= es (a masa de (a &ar/a e:terna2>/3 +!e trans'orta e(  ,razo ro,ot0 IE?TER es (a matriz @:@ de iner&ia de (a &ar/a e:terior2>/$m<830 7in em,ar/o estos dos ú(timos 'ar%metros e:ternos son n!(os 'ara n!estro ro,ot1 -a +!e en s! diseo emos de&idido +!e e( ta(adro +!e de,e ((e.ar en s! e:tremo &omo ú(tima arti&!(a&i)n +!ede re'resentado internamente en n!estro ro,ot &omo (a (on/it!d (9 a (a +!e (e emos asi/nado !na determinada masa - (on/it!d 'ara re'resentar(o0 *!n&tion ta!  ne6tone!(er92+1+'1+''1/1m=1Ie:ter3F (GH0GF (9H08F rGH0H=F r9H0HGF  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  5ar%metros Dena.it$arten,er/ de( ro,ot  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ teta  KH 'i;8 H +2G3 +293F d  K+2#3 +283 +2@3 H0G HF a  KH H H H H08F a(*a  K'i;8 $'i;8 H 'i;8 HF  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  a&tores de 'osi&ionamiento de (os &entros de /ra.edad  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ *a&tor#  $H09F *a&tor8  $H09F *a&tor@  $H09F *a&torG  $H09F *a&tor9$H09F  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  Masa de &ada e(emento 2>/3  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ m#  9F m8  9F m@  9F mG  HF m9F  mGH1 'or+!e (a arti&!(a&i)n G est% *ísi&amente dentro de (a arti&!(a&i)n @ 2s!s sistemas de &oordenadas Dena.it$ arten,er/ son &oin&identes31 - mG rea( se &onsidera din%mi&amente dentro de m90  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  Coe*i&iente de rozamiento .is&oso de &ada arti&!(a&i)n  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  ,#  H0H=F ,8  H0H=F ,@  H0H=F ,G  H0H9F ,9H0H9F Las @ 'rimeras 'rism%ti&as tienen !n ma-or &oe*i&iente de rozamiento 'or (os &arri(es  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  Matri&es de Iner&ia 2>/$m<83  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ r#HIrH#  zeros2@1@3F #$8 Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT r8HIrH8  zeros2@1@3F r@HIrH@  zeros2@1@3F # # # # rGHIrHG  K ⋅ m2G3 ⋅ r G 8 + ⋅ m2G3 ⋅ l G 8  H HFH ⋅ m2G3 ⋅ r G 8 + ⋅ m2G3 ⋅ l G 8  HFH H 8 @ 8 @ H0HH8F # # # r9HIrH9  ⋅ m293 ⋅ r 9 8  H HFH ⋅ m293 ⋅ r 9 8 + ⋅ m293 ⋅ l 9 8  HFH H 8 8 @ # # 8 8 ⋅ m293 ⋅ r 9 + ⋅ m293 ⋅ l 9 F 8 @  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  Ve&tores riH'i1 riHsi0  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ r#H'#  riH'i2a2#31 d2#31 a(*a2#33F r8H'8  riH'i2a2831 d2831 a(*a2833F r@H'@  riH'i2a2@31 d2@31 a(*a2@33F rGH'G  riH'i2a2G31 d2G31 a(*a2G33F r9H'9  riH'i2a2931 d2931 a(*a2933F r=H'=  zeros2@1#3F r#Hs#  riHsi2a2#31 d2#31 a(*a2#31 *a&tor#3F r8Hs8  riHsi2a2831 d2831 a(*a2831 *a&tor83F r@Hs@  riHsi2a2@31 d2@31 a(*a2@31 *a&tor@3F rGHsG  riHsi2a2G31 d2G31 a(*a2G31 *a&torG3F r9Hs9  riHsi2a2931 d2931 a(*a2931 *a&tor93F r=Hs=  zeros2@1#3F  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  Matri&es de trans*orma&i)n  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ rH#  d2teta2#31 a(*a2#33F r#H  rH#PF r#8  d2teta2831 a(*a2833F r8#  r#8PF r8@  d2teta2@31 a(*a2@33F r@8  r8@PF r@G  d2teta2G31 a(*a2G33F rG@  r@GPF rG9  d2teta2931 a(*a2933F r9G  rG9PF r9=  e-e2@3F r=9  r9=PF  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  Ve(o&idad an/!(ar de (as arti&!(a&iones  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ rHH6H  zeros2@1#3F r#H6#  r#HrHH6HF r8H68  r8#r#H6#F r@H6@  r@8r8H68F rGH6G  riH6i2rG@1 r@H6@1 +'2G33F r9H69  riH6i2r9G1 rGH6G1 +'2933F r=H6=  riH6i2r=91 r9H691 H3F  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ #$@ Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT  A&e(era&i)n an/!(ar de (as arti&!(a&iones  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ rHH6'H  zeros2@1#3F r#H6'#  r#HrHH6'HF r8H6'8  r8#r#H6'#F r@H6'@  r@8r8H6'8F rGH6'G  riH6'i2rG@1 r@H6'@1 r@H6@1 +'2G31 +''2G33F r9H6'9  riH6'i2r9G1 rGH6'G1 rGH6G1 +'2931 +''2933F r=H6'=  riH6'i2r=91 r9H6'91 r9H691 H1 H3F  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  A&e(era&i)n (inea( arti&!(ar  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ rHH.'H  K$/F HF HF r#H.'#  riH.'i'2r#H1 rHH.'H1 r#H6'#1 r#H6#1 r#H'#1 +'2#31 +''2#33F r8H.'8  riH.'i'2r8#1 r#H.'#1 r8H6'81 r8H681 r8H'81 +'2831 +''2833F r@H.'@  riH.'i'2r@81 r8H.'81 r@H6'@1 r@H6@1 r@H'@1 +'2@31 +''2@33F rGH.'G  riH.'ir2rG@1 r@H.'@1 rGH6'G1 rGH6G1 rGH'G3F r9H.'9  riH.'ir2r9G1 rGH.'G1 r9H6'91 r9H691 r9H'93F r=H.'=  riH.'ir2r=91 r9H.'91 r=H6'=1 r=H6=1 r=H'=3F  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  A&e(era&i)n de( &entro de masa de &ada e(emento  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ r#Ha#  riHai2r#H.'#1 r#H6'#1 r#H6#1 r#Hs#3F r8Ha8  riHai2r8H.'81 r8H6'81 r8H681 r8Hs83F r@Ha@  riHai2r@H.'@1 r@H6'@1 r@H6@1 r@Hs@3F rGHaG  riHai2rGH.'G1 rGH6'G1 rGH6G1 rGHsG3F r9Ha9  riHai2r9H.'91 r9H6'91 r9H691 r9Hs93F r=Ha=  riHai2r=H.'=1 r=H6'=1 r=H6=1 r=Hs=3F  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  !erza en e( &entro de masa de &ada e(emento  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ r=H*=  riH*i2r=Ha=1 m=3F r9H*9  riH*i2r9Ha91 m93F rGH*G  riH*i2rGHaG1 mG3F r@H*@  riH*i2r@Ha@1 m@3F r8H*8  riH*i2r8Ha81 m83F r#H*#  riH*i2r#Ha#1 m#3F  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  5ar en e( &entro de masa de &ada e(emento  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ r=Hn=  riHni2r=H6'=1 r=H6=1 Ie:ter3F r9Hn9  riHni2r9H6'91 r9H691 r9HIrH93F rGHnG  riHni2rGH6'G1 rGH6G1 rGHIrHG3F r@Hn@  riHni2r@H6'@1 r@H6@1 r@HIrH@3F r8Hn8  riHni2r8H6'81 r8H681 r8HIrH83F r#Hn#  riHni2r#H6'#1 r#H6#1 r#HIrH#3F #$G Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT

 $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  !erzas arti&!(ares  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ r=H*=a  r=H*=F r9H*9a  riH*ia2r9=1 r=H*=a1 r9H*93F rGH*Ga  riH*ia2rG91 r9H*9a1 rGH*G3F r@H*@a  riH*ia2r@G1 rGH*Ga1 r@H*@3F r8H*8a  riH*ia2r8@1 r@H*@a1 r8H*83F r#H*#a  riH*ia2r#81 r8H*8a1 r#H*#3F  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  5ares arti&!(ares  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ r8H'#  r8#2r#H'#3F r@H'8  r@82r8H'83F rGH'@  rG@2r@H'@3F r9H'G  r9G2rGH'G3F r=H'9  r=92r9H'93F r=Hn=a  r=Hn=F r9Hn9a  riHnia2r9=1 r=Hn=a1 r=H*=a1 r9Hn91 r9H*91 r=H'91 r9H'91 r9Hs93F rGHnGa  riHnia2rG91 r9Hn9a1 r9H*9a1 rGHnG1 rGH*G1 r9H'G1 rGH'G1 rGHsG3F r@Hn@a  riHnia2r@G1 rGHnGa1 rGH*Ga1 r@Hn@1 r@H*@1 rGH'@1 r@H'@1 r@Hs@3F r8Hn8a  riHnia2r8@1 r@Hn@a1 r@H*@a1 r8Hn81 r8H*81 r@H'81 r8H'81 r8Hs83F r#Hn#a  riHnia2r#81 r8Hn8a1 r8H*8a1 r#Hn#1 r#H*#1 r8H'#1 r#H'#1 r#Hs#3F  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$  !erzas - 'ares de a&&ionamientos  $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ t#  *'2r#H1 r#H*#a1 +'2#31 ,#3F t8  *'2r8#1 r8H*8a1 +'2831 ,83F t@  *'2r@81 r@H*@a1 +'2@31 ,@3F tG  tr2rG@1 rGHnGa1 +'2G31 ,G3F t9  tr2r9G1 r9Hn9a1 +'2931 ,93F ta!  Kt#F t8F t@F tGF t9F es e( .e&tor de *!erzas - 'ares +!e nos de.!e(.e (a *!n&i)n0 La im'(ementa&i)n de( a(/oritmo Ne6ton$E!(er 'ara n!estro ro,ot en (a *!n&i)n anterior se ,asa en (as &ara&terísti&as dimensiona(es - /eomtri&as de n!estro ro,ot1 (as &!a(es .ienen re*(eadas 'or (a si/!iente re'resenta&i)n de( mismo4 La 'arte &artesiana *ísi&amente es &omo se m!estra en (a *i/!ra 90#0

#$9 Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT

F$-u+a 5!. Es+!ema de !n ro,ot &artesiano0 Vo(!men de( es'a&io de tra,ao #m @  2e( &!,o de (a 'arte in*erior30 La 'arte &artesiana  X 1 Y   de( ro,ot se en&!entra en esta ima/en sit!ada en e( '!nto ori/en 2H1H31 mientras +!e (a  Z  est% !n 'o&o des'(azada de s! ori/en0 La 'arte rota&iona( +!edaría re'resentada 'or e( si/!iente es+!ema4

F$-u+a 5!/. Es+!ema de (a 'arte rota&iona(0 #$= Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT La 'arte rota&iona( de( ro,ot se en&!entra en s! '!nto 2H1H3 en esta ima/en0 7o,re esas im%/enes emos ,asado &)mo sería n!estro ro,ot1 esta,(e&iendo (as dimensiones - *ormas /eomtri&as +!e 'resentaría n!estro ro,ot en s! *orma rea(0 En e( &)di/o anterior emos 're*iado ini&ia(mente &iertos 'ar%metros +!e .ienen de*inidos 'or (as dimensiones de n!estro ro,ot1 'ara des'!s !ti(izar(os en o'era&iones 'osteriores0 Así1 a- +!e resa(tar +!e a &ada !no de n!estros es(a,ones (e emos asi/nado !na masa en (a +!e emos tenido en &!enta (os e(ementos me&%ni&os +!e (o &om'onen 2in&(!ido e( motor31 +!edando4 m#  m8  m@  9 S/ &orres'ondientes a (os res'e&ti.os 'atines de (as tres arti&!(a&iones 'rism%ti&as ini&ia(es0 Mientras +!e mG  G S/ - m9  @ S/ son (as masas de (os es(a,ones restantes1 re'resentando m9 a (a masa de( ta(adro0 7in em,ar/o1 en e( &)di/o emos introd!&ido4 mG  H S/ - m9   S/  'or+!e (a arti&!(a&i)n G est% *ísi&amente dentro de (a arti&!(a&i)n @ 2s!s sistemas de &oordenadas Dena.it$arten,er/ son &oin&identes31 - mG rea( se &onsidera din%mi&amente dentro de m90

M

M/

M0

M1

M5

9 S/

9S/

9S/

H S/

 S/

Adem%s emos *iado (as (on/it!des de (os es(a,ones *ina(es &omo4 (G  H0G m (9 H08 m1 &onsider%ndo(os en n!estros es+!emas de tra,ao &omo &i(indros de radio4 rG  H0H= m1 r9  H0HG m0 Los &oe*i&ientes de rozamiento de &ada !na de (as arti&!(a&iones tam,in (os emos *iado ini&ia(mente4 ,#  H0H=F ,8  H0H=F ,@  H0H=F ,G  H0H9F ,9  H0H9F de *orma +!e (as arti&!(a&iones 'rism%ti&as tienen !n ma-or &oe*i&iente de rozamiento de,ido a (os &arri(es 'or (os +!e se des'(azan0 Los *a&tores de &entro de /ra.edad de (os es(a,ones (os emos deado 'or sim'(i*i&ar a( $H091 indi&ando +!e e( &entro de masas se en&!entra en e( &entro de &ada es(a,)n o 'atín0 E( 'rimero de (os &%(&!(os de (a *!n&i)n es o,tener (as matri&es de iner&ia de &ada es(a,)n0 Esta o'era&i)n (a emos indi&ado en *orma a(/e,rai&a a 'artir de (os  'ar%metros dimensiona(es +!e emos esta,(e&ido 'ara &ada es(a,)n de n!estro ro,ot 2m1 r - (30 De *orma +!e emos &a(&!(ado e( momento de iner&ia de &ada es(a,)n res'e&to a &ada !no de s!s ees - res'e&to de( sistema Dena.it$arten,er/ anterior a di&o es(a,)n0 Q!edando &ada matriz de iner&ia *ormada 'or !na dia/ona( en (a +!e introd!&imos (os momentos de iner&ia res'e&to &ada ee0 5ara (os tres 'rimeros es(a,ones 2(os tres 'atines de (as arti&!(a&iones  'rism%ti&as3 (a matriz de iner&ia es n!(a1 -a +!e e( sistema D$ tiene s! ori/en so,re e( momento de masas - (os tres ees 'asan 'or (o tanto 'or (0 Mientras +!e 'ara (os dos es(a,ones *ina(es o,tenemos !n res!(tado no n!(o1 -a +!e s!s &entros de masa no &oin&iden &on e( sistema D$ de (a arti&!(a&i)n anterior1 sino +!e tenemos &a(&!(ar (os momentos de iner&ia a'(i&ando e( teorema de 7teiner +!e se m!estra a &ontin!a&i)n0

#$ Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT # # 8 8 ⋅ M  ⋅ R + ⋅ M  ⋅ L 8 @ # #  I  DY  = ⋅ M  ⋅ R 8 + ⋅ M  ⋅ L8 8 @ #  I  DZ  = ⋅ M  ⋅ R 8 8  I  DX 

Es(a,)n G4

 I  DX 

=

 I  DZ 

=

 I  DY 

=

=

# 8 # 8 ⋅ M G ⋅ R G + ⋅ M G ⋅ LG 8 @

# 8 ⋅ M G ⋅ R G 8 Es(a,)n 9 2ta(adro34

 I  Dx

=

 I  Dz  =

 I  Dy

=

# 8 # 8 ⋅ M 9 ⋅ R 9 + ⋅ M 9 ⋅ L9 8 @

# 8 ⋅ M 9 ⋅ R 9 8

C%(&!(os a 'artir de( ee Dena.it$arten,er/ de (a arti&!(a&i)n anterior a &ada es(a,)n0 5ara e( es(a,)n G emos !sado e( D$ de (a arti&!(a&i)n @1 - 'ara e( es(a,)n 9 e( D$ de (a arti&!(a&i)n 90 7e '!ede desta&ar +!e n!estro ro,ot .a a tener !nos as'e&tos din%mi&os m!- ,!enos1 de,ido a (a estr!&t!ra +!e 'resenta1 -a +!e es &omo !na mesa &on &!atro 'atas an&(adas a( te&o 2o a( s!e(o31 transmitiendo (os es*!erzos de,idos a (as masas e iner&ias a (a estr!&t!ra +!e (o sostiene de !na manera m!e*e&ti.a0 La /eometría de n!estro ro,ot a 'ermitido +!e (os momentos de iner&ia de (as tres 'rimeras arti&!(a&iones 'rism%ti&as a-an res!(tado n!(os0 A &ontin!a&i)n se s!&eden (as distintas o'era&iones de( a(/oritmo1 donde a'are&en ((amadas a otras *!n&iones4  DH, RI0PI, RI0SI, RI0I, RI0IP, RI0!PI"R,  RI0#I, RI0FI, RI0$I, RI0FI#, RI0$I#, T"R, F"P F (as &!a(es se en&ar/an de rea(izar (os &%(&!(os intermedios de( a(/oritmo de *orma estr!&t!rada - 'or 'artes1 'ara 'oder ((e/ar a (a so(!&i)n *ina( de *orma &(ara - ordenada4 RIHI4 &%(&!(o de (as .e(o&idades an/!(ares de (as arti&!(a&iones0 RIH5I4 &%(&!(o de (a a&e(era&iones an/!(ares de (as arti&!(a&iones0 D4 &%(&!(o de (as matri&es de trans*orma&i)n0 RIH5I  RIHV5IR o RIHV5I54 &%(&!(o de (as a&e(era&iones (inea(es0 RIH7I  RIHAI4 &%(&!(o de (as a&e(era&iones de( &entro de masa de &ada e(emento0 RIHI4 &%(&!(o de (as *!erzas en e( &entro de masas de &ada e(emento0 RIHNI4 &%(&!(o de (os 'ares en e( &entro de masas de &ada e(emento0 RIHIA4 &%(&!(o de (as *!erzas arti&!(ares0 RIHNIA4 &%(&!(o de (os 'ares arti&!(ares0 TR4 &%(&!(o de (os 'ares de a&&ionamientos0 54 &%(&!(o de (as *!erzas de a&&ionamientos0

#$ Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT A( tener n!estro ro,ot (as tres 'rimeras arti&!(a&iones 'rism%ti&a - (as dos si/!ientes rota&iona(es 29 en tota(31 emos o,tenido %newtoneluler5.m&   modi*i&ando %newtoneluler'.m& 1 -a +!e e( ro,ot de G /rados de (i,ertad tam,in 'resenta arti&!(a&iones 'rism%ti&as0 Así1 adem%s de (os 'ar%metros ini&ia(es &ara&terísti&os de n!estro ro,ot1 emos modi*i&ado en e( &)di/o (as ((amadas a (as *!n&iones +!e nos &a(&!(an (a .e(o&idad an/!(ar de (as arti&!(a&iones1 (a a&e(era&i)n an/!(ar de (as arti&!(a&iones - (a a&e(era&i)n (inea( arti&!(ar1 es'e&i*i&ando en estos a'artados de (a *!n&i)n newtoneluler5.m &!a( de n!estras arti&!(a&iones es 'rism%ti&a - &!a( rota&iona(1  'ara así ((e.ar a &a,o (os &%(&!(os de estos a'artados de *orma &orre&ta en *!n&i)n de( ti'o de arti&!(a&i)n0 Desta&ar +!e &omo e( ee Dena.it$arten,er/ de (a 'rimera arti&!(a&i)n de n!estro ro,ot es4

1 e( .e&tor de a&e(era&i)n de (a /ra.edad +!e emos introd!&ido en (a *!n&i)n newtoneluler5.m es4 K$/1H1H 1 -a +!e a&túa en e( ee : ne/ati.o de n!estro ro,ot0 De esta *orma -a tenemos (a din%mi&a in.ersa de n!estro ro,ot e:'resada de *orma &orre&ta0

/! DINÁMICA DIRECTA! 5ara (a #$%&'$(a #$+*(ta  !ti(izamos e( ter&er mtodo de a(Ser$Orin0 Este mtodo nos 'ermite reso(.er (a e&!a&i)n /enera( de( ro,ot4 00

0

0

 H 2 )3 ) +  2)1 ) 3 ) + + 2) 3 + * T  2 )3(  = τ  

2+34matriz no sin/!(ar de (os momentos de iner&ia0 0

C2+1 ) 34e*e&to de (as a&e(era&iones &entri*!/as - de Corio(is0 "2+34e*e&tos de (a /ra.edad0 >2+34matriz a&o,iana0 De*inimos4  - =  2)1 )3 ) + +2)3 + * T  2)3( 1 de *orma +!e (a e&!a&i)n ini&ia( se  '!ede e:'resar &omo4  H 2)3 ) = τ   − - 0 Así1 B se &a(&!(a a 'artir de( a(/oritmo de  Ne6ton$E!(er -a +!e  - = τ     'ara ) = H 1 de *orma +!e úni&amente nos *a(ta o,tener 2+3 'ara (!e/o des'ear +''0 Esta es (a 'arte donde nos de&antamos 'or e( ter&er mtodo de a(ter$Orin1 +!e o,tiene  a 'artir de (as masas1 &entros de masas1 momentos de iner&ia - a&e(era&iones de( &entro de masas de &ada es(a,)n1 a'ro.e&ando +!e (a matriz  es dia/ona(0 2Esta ú(tima 'arte (a ar% (a *!n&i)n 5.m dentro de (a *!n&i)n 'rin&i'a(30 #$W Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT La *!n&i)n +!e nos da (a din%mi&a dire&ta a 'artir de este mtodo es w/l(erorn5.m1 (a &!a( &a(&!(a (a din%mi&a dire&ta de( ro,ot de G"DL de.o(.iendo e( .e&tor G:# +!e re'resenta (a a&e(era&i)n de &ada arti&!(a&i)n !ti(izando e( ter&er mtodo de a(Ser - Orin0 Q es e( .e&tor G:# de .aria,(es arti&!(ares0 Q5 es e( .e&tor G:# +!e re'resenta (a .e(o&idad de &ada arti&!(a&i)n0 TAU es e( .e&tor G:# +!e re'resenta e( 'ar de entrada a &ada arti&!(a&i)n0 MA7AE?T es (a masa de (a &ar/a e:terna0 INERCIAE?T es (a iner&ia de (a &ar/a e:terna0 2estos dos ú(timos 'ar%metros tienen .a(or n!(o en n!estro ro,ot3 *!n&tion +''  6a(Serorin92+1+'1ta!1masae:t1iner&iae:t3  7e &a(&!(a e( .e&tor ,0  ,  ne6tone!(er92+1+'1zeros291#31W01masae:t1iner&iae:t3F  7e &a(&!(a (a matriz de momentos de iner&ia 0   92+1masae:t1iner&iae:t3F  7e &a(&!(a e( .e&tor de a&e(era&i)n de &ada arti&!(a&i)n0 +''  in.232ta!$,3F Vemos &omo no emos (o/rado (a inde'enden&ia tota( de (a din%mi&a dire&ta de (a in.ersa1 a( !ti(izar (a *!n&i)n newtoneuler5.m dentro de w/l(erorn5.m0 La *!n&i)n 5.m   !ti(izada dentro de w/l(erorn5.m  &a(&!(a (a matriz de momentos de iner&ia  G:G !ti(izando e( ter&er mtodo de a(Ser - Orin0 Q es e( .e&tor G:# de .aria,(es arti&!(ares0 MA7AE?T es (a masa de (a &ar/a e:terna0 INERCIAE?T es (a iner&ia de (a &ar/a e:terna02estos dos ú(timos son .a(ores n!(os 'ara n!estro ro,ot3 *!n&tion   92+1 masae:t1 iner&iae:t3 (GH0GF (9H08F rGH0=F r9H0HGF  5ar%metros Dena.it$arten,er/ de( ro,ot teta  KH 'i;8 H +2G3 +293 F d  K+2#3 +283 +2@3 (G H F a  KH H H H (9  a(*a  K'i;8 $'i;8 H 'i;8 H F  Masa de &ada e(emento 2>/3 m  K9F 9F 9F HF F  Matri&es de Iner&ias Centroida(es0 2>/$m<803 J  K H H HF H H HF H H HF H H HF H H HF H H HF H H HF H H HF H H HF # $ #H Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT # # # # # 8 8 8 8 8 ⋅ m2 G3 ⋅ r G + ⋅ m2 G3 ⋅ l G  H HFH ⋅ m2 G3 ⋅ r G + ⋅ m 2 G3 ⋅ l G  HFH H ⋅ m2 G3 ⋅ r G F 8 @ 8 @ 8 # # # # # 8 8 8 8 8 ⋅ m293 ⋅ r 9  H HFH ⋅ m 293 ⋅ r 9 + ⋅ m 293 ⋅ l 9  HFH H ⋅ m 293 ⋅ r 9 + ⋅ m 293 ⋅ l 9 F 8 8 @ 8 @  La +!inta iner&ia es (a de (a &ar/a e:terna0 J2#=4#1#4@3  iner&iae:tF  Ve&tor XH0 zH  KHF HF #F  Condi&iones de Car/a E:terna0 M2=3  masae:tF &#  zeros2@1#3F E#  J2#=4#1#4@3F *or    94$#4#  Constante 'ara sa&ar (a Iner&ia J0 S  2$#3@  #F  Ve&tores ' - s0  '  Ka23F d23sin2a(*a233F d23&os2a(*a233F s  $H09'F  Matri&es de trans*orma&ion0 a#  d2teta231a(*a233F a#  a#PF  Centroide e Iner&ia de (os e(ementos anteriores0 &M#  &#F EM#  E#F  Masa de todos (os e(ementos anteriores0 M23  M2#3  m23F  N!e.o &entroide0 &  22s  '3m23  M2#32&M#  '33;M23F &#  a#&F  Distan&ia de tras(ado de (as iner&ias0  '#  2&M#  ' $ &3F d#  dot2'#1'#3e-e2@3 $ 2'#'#P3F  '8  2s  ' $ &3F d8  dot2'81'83e-e2@3 $ 2'8'8P3F  N!e.a Iner&ia0 E#  a#2EM#  M2#3d#  J2S4S81#4@3  m23d83a#F  !erza - 'ar de (os e(ementos  asta N0 i*  2  G3 Y 2  93  Arti&!(a&iones rota&iona(es # $ ## Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT #  &ross2zH1M23&#3F  N#  E#zHF e(se  Arti&!(a&iones 'rism%ti&as #  M23zHF  N#  zeros2@1#3F end  E(emento 0 Com'onente 2130 *i#i  #F ni#i  N#  &ross2&#1 #3F i*  2  G3 Y 2  93  Arti&!(a&iones rota&iona(es 213  ni#i2@1#3F e(se  Arti&!(a&iones 'rism%ti&as 213  *i#i2@1#3F end  E(ementos $# asta #0 Com'onentes 2#4$#13 i   $ #F 6i(e i Z #  Ve&tor '0  '  Ka2i3F d2i3sin2a(*a2i33F d2i3&os2a(*a2i33F  Matri&es de rota&ion0 ai#i  d2teta2i31a(*a2i33F aii#  ai#iPF  !erza - 'ar de( anterior e(emento0 *iiM#  *i#iF niiM#  ni#iF  !erza - 'ar de este e(emento0 *i#i  ai#i*iiM#F ni#i  ai#i2niiM#  &ross2'1 *iiM#33F  Com'onente 2i130 i*  2i  G3 Y 2i  93  Arti&!(a&iones rota&iona(es 2i13  ni#i2@1#3F e(se  Arti&!(a&iones 'rism%ti&as 2i13  *i#i2@1#3F end   es simetri&a0 21i3 2i13F i  i $ #F end end

# $ #8 Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT emos o,tenido (a *!n&i)n 90m .%(ida 'ara n!estro ro,ot de 9 /rados de (i,ertad a 'artir de (a de( ro,ot de G /rados de (i,ertad1 modi*i&ando (os 'ar%metros dimensiona(es 'ertinentes 2masas1 iner&ias10003 - di*eren&iando &orre&tamente en (os &%(&!(os (as arti&!(a&iones rota&iona(es de (as 'rism%ti&as1 -a +!e &iertos .a(ores se o'eran de !na *orma ! otra se/ún e( ti'o de arti&!(a&i)n0 [a tenemos 'or (o tanto (a *!n&i)n +!e nos sim!(a (a din%mi&a dire&ta de n!estro ro,ot4 w/l(erorn5.m0 Una .ez o,tenidas (as *!n&iones +!e nos 'ermiten o,tener (a din%mi&a dire&ta e in.ersa de n!estro ro,ot1 !n *2*'plo +!e nos indi&a +!e s! *!n&ionamiento es &orre&to es e( si/!iente4

7e '!ede .er &omo a( a&er (a din%mi&a in.ersa - des'!s (a dire&ta1 e( res!(tado o,tenido es e( de (as a&e(era&iones !ti(izadas a( ini&io1 (!e/o (as *!n&iones *!n&ionan &orre&tamente1 siendo !na (a o'!esta de (a otra0

# $ #@ Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT  NOTA 5ara (a 'arte &artesiana de n!estro ro,ot a- em'resas +!e 'ro'or&ionan m)d!(os de des'(azamientos (on/it!dina(es &omo e( de (a ima/en4

Una em'resa +!e (os .ende es  #1e2t 1 &ono&ida 'or todos 'or (as 'resenta&iones de a(/!nos de n!estros &om'aeros0 7in em,ar/o en (a im'(ementa&i)n rea( de n!estro ro,ot nos emos de&antado 'or e( ti'o de &artesiano +!e a'are&e en (as e:'(i&a&iones de (a 'r%&ti&a1 e( &!a( es de( mismo ti'o de( +!e a- en Torre,(an&a en e( de'artamento de A!tom%ti&a4

# $ #G Jaime Martínez Verdú

Jos L!ís Aznar "omis

DINÁMICA DEL QUIROBOT Este ti'o de ro,ots no tiene a&t!a(mente a'(i&a&i)n mdi&a1 'or (o +!e es !na a(ternati.a ori/ina( a (os tí'i&os de ="DL rota&iona(es0 A( !sar estos m)d!(os am'(ia,(es de des'(azamientos o (as estr!&t!ras de mesa de( ú(timo1 dis'onen &omo /ran .entaa *rente a (os rota&iona(es e( am'(io es'a&io de tra,ao +!e '!eden ((e/ar a a,ar&ar1 desde !n 'e+!eo .o(!men de #m<@ asta !na a,ita&i)n entera1 '!diendo ((e.ar e( motor a (a 'arte de (a sa(a donde se re+!iera de s!s ser.i&ios o donde '!eda ser (im'iado1 o re'arado1 o a(ma&enado000 Así1 este a sido !no de (os *a&tores +!e nos moti.) a e(e/ir este ti'o de ro,ots0

# $ #9 Jaime Martínez Verdú

Jos L!ís Aznar "omis

 

SERVOACCIONAMIENTOS

6

Capí tulo SER VOACCIONAMIENTOS "LA PACIENCIA ES EL MOTOR DE LA NATURALEZA" O.S.Marden

"na #ez $ue tenemos nuestro ro%ot &orre&tamente re'resentado &inem(ti&a ) din(mi&amente* en esta 'r(&ti&a nos dis'onemos a e+e,ir +os a&tuadotes $ue re$uiere nuestro ro%ot 'ara 'oder e-e&tuar sus mo#imientos &um'+iendo &on sus es'e&i-i&a&iones &inem(ti&as ) din(mi&as. "ti+izaremos 'ara esta e+e&&i/n di#ersos &riterios $ue se ir(n e0'+i&ando a medida $ue +os #a)amos em'+eando. En 'rimer +u,ar se1a+ar $ue +os a&tuadotes ser(n motores e+é&tri&os 2C 3R"S4LESS de+ +os &ua+es tenemos un &at(+o,o* ) &u)o es$uema en simulink   tiene +a -orma5

Figura 6.! Es$uema simulink  de +os a&tuadores e+é&tri&os 2C 3R"S4LESS. 2onde se 'ueden #er +os 'ar(metros $ue +o identi-i&an. Así* 'ara &ada una de nuestras arti&u+a&iones ne&esitamos un motor de este ti'o &on unas &ara&terísti&as &on&retas $ue +o de-inan. 4a) $ue de&ir $ue en +as a'+i&a&iones rea+es +o $ue se usan a%itua+mente son motores de AC* $ue tienen unas &ara&terísti&as su'eriores a +as de +os 2C. En una im'+ementa&i/n rea+ de nuestro ro%ot 'asaríamos a &a+&u+ar +os motores AC $ue re$uiere* 'ero en +a 'r(&ti&a 'or sen&i++ez tra%a7amos &on motores 2C.

6! Jaime Martínez Verdú

José Luís Aznar Gomis

 

SERVOACCIONAMIENTOS

. REUISITOS UE DE#EN SATISFACER NUESTROS MOTORES.  Ne&esitamos 'or +o tanto &ono&er +as &ara&terísti&as de +os motores $ue re$uiere nuestro ro%ot 'ara &um'+ir &on +as e0i,en&ias din(mi&as* 'ara e++o simu+amos e+ &om'ortamiento din(mi&o de &ada arti&u+a&i/n indi#idua+mente uti+izando si,uiente es$uema simulink 5

Figura 6.$! Es$uema simulink  de+ &ontro+ arti&u+ar. E+ es$uema se %asa en +a din(mi&a in#ersa de nuestro ro%ot* usando +a -un&i/n dininv5gdl.m% $ue &a+&u+a e+ #e&tor 90! de 'ares:-uerzas de entrada a +as arti&u+a&iones uti+izando e+ método de Ne;tonEu+er.  ENTRADA(1:5)  re'resenta +a 'osi&i/n de &ada arti&u+a&i/n.  ENTRADA(5:10)  es +a #e+o&idad de &ada arti&u+a&i/n. ENTRADA(11:15) es +a a&e+era&i/n de &ada arti&u+a&i/n. < uti+izamos tam%ién +a -un&i/n newtoneuler.m de +a 'r(&ti&a anterior. -un&tion 'ar = dinin#>,+d?entrada@ $ = entrada?!5>@ B osi&i/n de &ada arti&u+a&i/n $' = entrada?65!D@ B Ve+o&idad de &ada arti&u+a&i/n $'' = entrada?!!5!>@ B A&e+era&i/n de &ada arti&u+a&i/n B ar(metros de +a &ar,a masae0t = D iner&iae0t = D D DD D DD D DF B Se &a+&u+a e+ #e&tor de 'ares:-uerzas uti+izando Ne;tonEu+er  'ar = ne;toneu+er>?$*$'*$''*.H*masae0t*iner&iae0t@ 68 Jaime Martínez Verdú

José Luís Aznar Gomis

 

SERVOACCIONAMIENTOS

E+ es$uema  simulink   anterior nos 'ermite &ono&er &ua+es son +os re$uerimientos en &uanto a 'ares ) -uerzas $ue ne&esita &ada una de +as arti&u+a&iones 'ara 'oder mo#erse &orre&tamente e+ ro%ot se,ún su &inem(ti&a ) din(mi&a.Este estudio se a&e  'ara +a 'osi&i/n m(s &ríti&a* donde estos re$uerimientos ser(n m(0imos* de -orma $ue  %as(ndonos en estos datos a +a ora de e+e,ir +os motores tendremos +a se,uridad de $ue ser(n +os ade&uados 'ara e+ &orre&to -un&ionamiento de nuestro ro%ot médi&o. 2e -orma $ue +as masas $ue en +a 'r(&ti&a anterior +e asi,namos a &ada es+a%/n* 'uedan ser des'+azadas 'or estos motores* siendo así e+ mode+o din(mi&amente &orre&to. ara o%tener estos datos +o $ue a&emos &on e+ es$uema es introdu&ir a &ada arti&u+a&i/n ?re'resentada 'or su &inem(ti&a in#ersa@ una entrada tra'ezoida+ de #e+o&idad $ue simu+e e+ -un&ionamiento en arran$ue ?D.! se,@* en ré,imen 'ermanente ?D.9 se,@ ) en -renado ?D.! se,@* &on su &orres'ondiente se1a+ de a&e+era&i/n. Así* a  'artir de +a -un&i/n de din(mi&a in#ersa* o%tendremos +a re'resenta&i/n ,r(-i&a de +os  'ares m(0imos $ue ne&esita &ada arti&u+a&i/n 'ara su &orre&to -un&ionamiento. Antes de 'oder o%tener estas ,r(-i&as* ne&esitamos esta%+e&er $ue redu&tores #amos a em'+ear 'ara &ada motor5 •

ara +as arti&u+a&iones rota&iona+es em'+eamos redu&tores de >DD* )a $ue su'onemos se #an a uti+izar motores $ue tra%a7an a >DDD r'm ) $ueremos $ue +a #e+o&idad m(0ima de nuestras arti&u+a&iones sea de !D r'm.

•

En &uanto a +as arti&u+a&iones 'rism(ti&as* $ue son +as tres 'rimeras* nuestros &riterios a +a ora de e+e,ir e+ redu&tor m(s a'ro'iado an sido $ue +a #e+o&idad m(0ima -uera de !m:s* ) e+ &riterio de 'rue%a ) error asta $ue nos -uera 'osi%+e uti+izar e+ motor de ,ama m(s a+ta de +os dis'oni%+es* )a $ue en un 'rin&i'io nin,uno de +os dis'oni%+es &um'+ía &on +as e0i,en&ias de 'ares nomina+es ) de 'i&o.

ina+mente* +os r&'u(tor&) em'+eados an sido5

Arti(ula(i*+  Arti(ula(i*+ $ Arti(ula(i*+ , 6/ - 0

Arti(ula(i*+ - Arti(ula(i*+  // //

Las ,r(-i&as de 'ares re$ueridos $ue emos o%tenido de+ es$uema 'ara &ada arti&u+a&i/n son +as $ue se muestran en +a '(,ina si,uiente. Así 'ues* de +as ,r(-i&as o%tenemos +os datos de 'ares m(0imos 'ara &ada arti&u+a&i/n5

K K

Arti&u+a&i/n i&o ?Nm@ omina+ ?Nm@

! .666> D.D896

8 .6!68  !.!D  

 8.>H !.8H>

9 .98H!D    !.D98!D  

> D.D8 D.D!H

Los &ua+es a) $ue mu+ti'+i&ar+os 'or un -a&tor de se,uridad de !.>. Así* -ina+mente o%tenemos +os re$uisitos $ue de%e &um'+ir &ada uno de nuestros motores5

Arti(ula(i*+ 1 i(o 2N34 1 o3i+al 2N34

 .-005 /./,60

$ .-$-, 0.00/78,

, ,.565.599

.-$/78, .9/5/78-

 /./--5 /./$/9

6 Jaime Martínez Verdú

José Luís Aznar Gomis

 

SERVOACCIONAMIENTOS

Arti(ula(i*+  2pri)3:ti(a4

Arti(ula(i*+ $ 2pri)3:ti(a4

Arti(ula(i*+ , 2pri)3:ti(a4

Arti(ula(i*+ - 2rota(io+al4

Arti(ula(i*+  2rota(io+al4

Figura 6.,! Resu+tado de +a simu+a&i/n en Simulink . Ana+izando estas ,r(-i&as ) #a+ores de 'ares* 'odemos #er &omo 'ara +as tres arti&u+a&iones 'rism(ti&as e+ #a+or de 'i&o en e+ arran$ue ) -renado es mu) a+to eso se de%e a $ue estas arti&u+a&iones son +as de ma)or masa* 'or +o tanto ne&esitan un a+to 'ar motor 'ara ini&iar ) 'arar su mo#imiento. Son +as $ue su-ren e+ ma)or es-uerzo en e+ mo#imiento de+ ro%ot &om'+eto. Tam%ién in-+u)e $ue sus #a+ores de &oe-i&iente de rozamiento est(ti&o es de D.D6* su'erior a+ de +as arti&u+a&iones rota&iona+es ?D.D>@. La ter&era arti&u+a&i/n 'rism(ti&a tiene un #a+or de 'ar nomina+ mu) su'erior a+ de +as otras* de%ido en 'arte 'or$ue su mo#imiento se rea+iza en e+ e7e en $ue a&túa +a ,ra#edad* 'or +o $ue de%e so'ortar a&e+era&iones ) de&e+era&iones ma)ores. Las otra dos arti&u+a&iones 'rism(ti&as ne&esitan un 'ar nomina+ %a7o* )a $ue una #ez ini&iado e+ mo#imiento* sus re$uisitos din(mi&os son menores. 69 Jaime Martínez Verdú

José Luís Aznar Gomis

 

SERVOACCIONAMIENTOS

or ú+timo* +as dos arti&u+a&iones rota&iona+es tienen unos #a+ores mu)  'e$ue1os* )a $ue a+ tratase de +as arti&u+a&iones de+ e0tremo sus masas ) re$uisitos din(mi&os son menores a+ resto. En es'e&ia+ son mu) %a7os +os #a+ores de +a arti&u+a&i/n 9* )a $ue se trata de una rota&iona+ &u)o e7e de ,iro es e+ z   &artesiano* ) 'or +o tanto no su-re +a a&&i/n de +a ,ra#edad ) sus re$uerimientos din(mi&os son +os mismos en &ua+$uier 'osi&i/n.

$. #;SUEDA DE MOTORES APROPIADOS EN EL CAT##8/ )a $ue es e+ úni&o $ue &um'+e &on e+  'ar de 'i&o re$uerido 'or +as arti&u+a&iones ! ) 8* ) &on e+ 'ar nomina+ re$uerido 'or +a arti&u+a&i/n . ara +as dos arti&u+a&iones rota(io+al&) e+e,imos e+ motor de ,ama m(s  %a7a* $ue 'ro'or&iona so%radamente +os 'ares re$ueridos 'or estas5 D#9CD#8/. Los datos &ara&terísti&os de +os motores se+e&&ionados son5

DA-$>##8/ 2pri)3:ti(a)4 Par:3&tro) R&)i)t&+(ia I+'u(ta+(ia Co+)ta+t& '& par Co+)ta+t& '& oltaB& Corri&+t& 3:i3a D#9CD#8/ 2rota(io+al&)4 Par:3&tro) R&)i)t&+(ia I+'u(ta+(ia Co+)ta+t& '& par Co+)ta+t& '& oltaB& Corri&+t& 3:i3a

Sí3?olo R L @ T @ V I 3:

Valor

Sí3?olo R L @ T @ V I 3:

Valor

D.6  !.D! m4 D.!>> Nm:A D.!>> V:rad:s H. A

6.  !.8H m4 D.D> Nm:A D.D> V:rad:s .6 A

Los &ua+es nos 'ro'or&ionan +a in-orma&i/n ne&esaria de &ada motor 'ara 'oder &rear un dia,rama de %+o$ues en Simulink   $ue re'resente &orre&tamente su -un&ionamiento5

Figura 6.-! Es$uema simulink  de+ motor. Estos motores son 'or +o tanto +os ade&uados 'ara nuestro ro%ot médi&o* )a $ue todo se a &a+&u+ado 'ara +os #a+ores de tama1os* masas* +on,itudes* momentos de iner&ia... &on&retos de nuestro ro%ot. odemos ase,urar 'or +o tanto $ue mediante estos motores nuestro ro%ot 'odr( rea+izar( sus mo#imientos &um'+iendo &on su din(mi&a ) &inem(ti&a esta%+e&ida. En +a 'r(&ti&a si,uiente nos &entraremos en dise1ar e+ &ontro+ de+ ro%ot 'ara $ue sus mo#imientos sean &orre&tos ) 're&isos* a+,o mu) im'ortante en +a a'+i&a&i/n médi&a $ue se +e #a a dar. 6> Jaime Martínez Verdú

José Luís Aznar Gomis

SIMULACIÓN Y CONTROL DEL QUIROBOT

7

Capí tulo SIMULACIÓN  Y Y  C CONTR OL  D DEL  Q QUIR OBOT "EL CONTROL SOBRE SÍ MISMO ES LA FACULTAD DE SELECCIONAR LOS PENSAMIENTOS PARA CONVERTIR EN ACTOS SOLO CONVENIENTES"

 Manero

En (a )r*+ti+a anterir ,em! rea(izad (a e(e++i-n de (! a++inamient! ade+#ad! )ara n#e!tr r.t/ de 0rma 1#e tenem! (! di!tint! mtre! re)re!entad! en .(1#e! de !im#(in23 A,ra #ti(izarem! e!! re!#(tad! )ara ((e4ar a +a. (a +rre+ta !intniza+i-n de (! mtre! )ara 1#e !# 0#n+inamient !ea e( de!ead3 A!í/ n! di!)nem! a ((e4ar a +a. e( !i!tema de +ntr( de n#e!tr r.t3 5ara e(( 4am! a tener en +#enta #na !erie de !im)(i0i+a+ine!3 5ara 0a+i(itar e( di!e6/ rea(izam! #n +ntr( de!a+)(ad/ de 0rma 1#e +n!ideram! 1#e e( +m)rtamient de +ada arti+#(a+i-n e! inde)endiente de (a! dem*!3 La! !im)(i0i+a+ine! n! )ermiten +entrarn! en e( +*(+#( de #n re7#(adr 5ID8 G PID

=

 P  +

 I   s

+

 D ⋅ s 

 )ara +ada arti+#(a+i-n de 0rma inde)endiente/ #ti(izand (a! t"+ni+a! 1#e 9a +n+em! )ara !# di!e63 5r ( tant n#e!tr +ntr( din*mi+ !e 4a a rea(izar en e( e!)a+i arti+#(ar3 5e!e a n !er e!te m"td 0ie( a (a rea(idad a( '::;/ !#! !im)(i0i+a+ine! en a)(i+a+ine! rea(e! !n 4*(ida! )ara #n 7ran númer de r.t! +mer+ia(e! e
! DISE#O DE LOS PID$S Una 4ez de+idid e( #! de #n 5ID en +ada arti+#(a+i-n/ )a!am! a !# di!e6 teniend en +#enta 1#e e!te re7#(adr )re!enta #na a++i-n )r)r+ina( =5>/ #na a++i-n di0eren+ia( =D> 9 #na a++i-n inte7ra( =I>3 La a++i-n )r)r+ina( n! )ermite  )rin+i)a(mente mdi0i+ar e( tiem) de e!ta.(e+imient3 Cn (a a++i-n di0eren+ia( mdi0i+am! (a !.re!+i(a+i-n3 Y mediante (a a++i-n inte7ra( di!min#im! e( errr en r"7imen )ermanente3 %&' Jaime Martínez Verdú

J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT A!í/ di!e6am! )ara +ada arti+#(a+i-n !# re7#(adr 5ID mediante e( m"td de 5r#e.a&Errr .a!*ndn! en8

2P 2I 2D

T%&'po (& T%&'po (& E,,o, &* ,01 So+,&o-.%la.%/* l&)a*ta'%&*to &-ta+l&.%'%&*to p&,'a*&*t& De+re+e A#menta 5e1#e6 +am.i De+re+e De+re+e A#menta A#menta E(imina 5e1#e6 +am.i De+re+e De+re+e 5e1#e6 +am.i Ta+la 7!3 Varia+ine! de (! )ar*metr! 9 de (a! +ara+terí!ti+a! de( !i!tema3

La ta.(a n! indi+a +m 4arían +iert! )ar*metr! de (a re!)#e!ta de( !i!tema/ ante a#ment! de (a! 4aria.(e! 5/ I/ D3 E( !i7#iente e!1#ema !im#(in2 e! e( 1#e n! )ermite e!t#diar e( +m)rtamient de +ada arti+#(a+i-n indi4id#a(izada3 En "( intrd#+im! #na )!i+i-n a (a 1#e 1#erem! 1#e !e de!)(a+e di+,a arti+#(a+i-n 9 +m)r.am! 7r*0i+amente (a re!)#e!ta de( !i!tema ,a!ta ((e7ar a e!a )!i+i-n3 E!t#diand e!a re!)#e!ta !a.rem! +m ir mdi0i+and (!  )ar*metr! de( 5ID ,a!ta .tener (a re!)#e!ta de!eada3 Tam.i"n intrd#+im! +m entrada! )!i+ine!/ 4e(+idade! 9 )are! de tda! (a! arti+#(a+ine!/ !iend n#(! e!t! 4a(re! )ara e( re!t de arti+#(a+ine!/ 9a 1#e (a !im#(a+i-n !e +entra en #na !(a arti+#(a+i-n3

F%1u,a 7!3 Dia7rama de .(1#e! en Simulink  de( +ntr(3 E( !i!tema e!t* 0rmad/ @#nt +n +iert!  .(1#e! 0i@!/ )r e( 5ID a di!e6ar/ (a 0#n+i-n de din*mi+a dire+ta .tenida de (a )r*+ti+a / 9 e( !#.!i!tema de( mtr de (a arti+#(a+i-n +rre!)ndiente3 E!te !#.!i!tema mtr ,a9 1#e re((enar( +n (! )ar*metr! de mtr +rre!)ndiente!/ .tenid! de (a )r*+ti+a anterir8

%&? Jaime Martínez Verdú

F%1u,a 743 5ar*metr! de( mtr J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT A (a ,ra de di!e6ar e( 5ID/ en e( e!t#di de (a !e6a( de )!i+i-n re!#(tante de( !i!tema n! ,em! +entrad )rin+i)a(mente en #n a!)e+t8 1#e (a re!)#e!ta 0#era -o+,&a'o,t%1ua(a 5 de@and en !e7#nd (#7ar tr! )ar*metr! 6/ )ara 1#e en nin7ún mment !e a(+an+en )!i+ine! !#)erire! a (a 0ina(3 em! +n!iderad +m +ríti+ e!te 0a+tr dada (a a)(i+a+i-n 1#e 4a a tener n#e!tr r.t/ 9a 1#e en (a! )era+ine! m"di+a! en (a! 1#e ( 4a9am! a #ti(izar e! de e
 I D : :3'? PID !  : :3'? PID 4 F : :3' PID 8  : :3? PID 9  : : PID : %F Ta+la 743 Re!#(tad! de e( 0a+tr )r)r+ina(/ di0eren+ia e inte7ra( .tenid!3 En !im#(a+i-n )are+e (-7i+ 1#e I 9 D den 4a(re! m#9 +er+an! a :  in+(#! :  )ara 1#e e( +m)rtamient !im#(ad !ea e( m*! +rre+t3 5er en (a im)(ementa+i-n rea( de e!t! 5ID ,a.r* 1#e a6adir(e! #na ma9r a++i-n di0eren+ia( )ara me@rar e( r"7imen tran!itri 9 #na )e1#e6a a++i-n inte7ra(/ 9a 1#e e<)erimenta(mente !e ,a dem!trad 1#e e! ne+e!aria #na )e1#e6a a++i-n inte7ra( )ara 7arantizar 1#e e( di!e6 de( re7#(adr re!#(te en #n !i!tema e!ta.(e/ an#(and (! errre! en r"7imen )ermanente3 Sin em.ar7/ e!ta a++i-n inte7ra( de.e !er m#9 )e1#e6a/ 9a 1#e n! )r4+a !.re!+i(a+i-n en e( r"7imen tran!itri/ )#diend ,a+er 1#e n#e!tr mtr !e !at#re/ ( +#a( !iem)re ,a9 1#e e4itar3 De e!ta 0rma e( +ntr( 5ID )ermite an#(ar #n )!i.(e errr en r"7imen  )ermanente/ a!í +m a!e7#rar 1#e 0ina(mente (a! !e6a(e! 0ina( 9 de!eada +in+iden/ a!e7#rand #na a(ta )re+i!i-n en (a )era+i-n m"di+a3 E( di!e6 de (! )ar*metr! de( 5ID tam.i"n ,a .ede+id a( mantenimient de (a re!)#e!ta !.reamrti7#ada tant ante )!i+i-n 0ina( +er+ana +m ante )!i+i-n 0ina( (e@ana3 Uti(izand (! 5ID anterire!/ .tenem! (a! !a(ida! de!eada!3 A +ntin#a+i-n !e m#e!tran (a! !a(ida! de )!i+i-n )ara +ada #na de (a! arti+#(a+ine!/ tant )ara #na  )!i+i-n 0ina( +er+ana8 :3' )ara )ri!m*ti+a! 9 :3:F )ara rta+ina(e!G +m )ara #na  )!i+i-n 0ina( (e@ana8 :3 )ara )ri!m*ti+a! 9 ' )ara rta+ina(e!3

%& Jaime Martínez Verdú

J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT

Arti+#(a+i-n ' =)!i+i-n 0ina( :3'm>

Arti+#(a+i-n ' =)!i+i-n 0ina( :3m>

Arti+#(a+i-n ? =)!i+i-n 0ina( :3'm>

Arti+#(a+i-n ? =)!i+i-n 0ina( :3m>

Arti+#(a+i-n  =)!i+i-n 0ina( :3'm>

Arti+#(a+i-n  =)!i+i-n 0ina( :3m>

F%1u,a 783 Sa(ida! de (a )!i+i-n )ara +ada arti+#(a+i-n3

%& Jaime Martínez Verdú

J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT Arti+#(a+i-n  =)!i+i-n 0ina( :3:Frad>

Arti+#(a+i-n  =)!i+i-n 0ina( 'rad>

Arti+#(a+i-n  =)!i+i-n 0ina( :3:Frad>

Arti+#(a+i-n  =)!i+i-n 0ina( 'rad>

En (a! 7r*0i+a! !e )#ede .!er4ar +(aramente +m n#e!tr )rin+i)a( .@eti4 !e ,a +#m)(id8 1#e (a !e6a( n#n+a !#)ere a( 4a(r 0ina( en e( r"7imen tran!itri3 Y !e  )#ede 4er +m !e a(+anza +rre+tamente e( 4a(r 0ina( de!ead en e( mínim tiem)  )!i.(e3 Un )ar*metr de inter"! 1#e !e .tiene de (a! 7r*0i+a! e! e( tiem) e!ta.(e+imient =!e7>/ 1#e )ara +ada +a! e!8 Arti+#(a+i-n ' =)!i+i-n 0ina( :3'm>8 ;;!9: Arti+#(a+i-n ? =)!i+i-n 0ina( :3'm>8 ;;!< Arti+#(a+i-n  =)!i+i-n 0ina( :3'm>8 ;;!9 Arti+#(a+i-n  =)!i+i-n 0ina( :3:Frad>8 ;;8:7 Arti+#(a+i-n  =)!i+i-n 0ina( :3:Frad>8 ;;!9=

Arti+#(a+i-n ' =)!i+i-n 0ina( :3m>8 ;;4: Arti+#(a+i-n ? =)!i+i-n 0ina( :3m>8 ;;489 Arti+#(a+i-n  =)!i+i-n 0ina( :3m>8 ;;449 Arti+#(a+i-n  =)!i+i-n 0ina( 'rad>8 ;;: Arti+#(a+i-n  =)!i+i-n 0ina( 'rad>8 ;;8=

E!t! tiem)! de e!ta.(e+imient !n (! mínim! )!i.(e! 1#e ,em! +n!e7#id )ara 1#e e( !i!tema !e mant#4iera !#.amrti7#ad en +ada +a!3 Se )#ede a)re+iar 1#e ,an !id m#9 .a@! =H:3'!e7>/ +n ( 1#e +#m)(im! tr a!)e+t m#9  )!iti4/ 9a 1#e n#e!tr r.t !er* +a)az de ((e4ar a +a. !#! m4imient! en tiem)! m#9 +rt!/ ( +#a( en (a a)(i+a+i-n m"di+a )ara (a 1#e ( re1#erim! n e! +ríti+/ )r ( 1#e n ( ,em! +n!ideram! ante! +m #n )ar*metr a minimizar en n#e!tr! +riteri! de( +*(+#( de (! 5ID3 Un mti4 )r e( 1#e e!t! tiem)! de re!)#e!ta !ean tan +rt! e! e( tama6 de n#e!tr! mtre!/ 9a 1#e ,a9 1#e re+rdar 1#e en !# e(e++i-n en (a )r*+ti+a anterir #ti(izam! #n 0a+tr de !e7#ridad de '3/ +n ( +#a( )dem! a0irmar 1#e e!t*n !.redimen!inad!/ 9 +#m)(en +n (! re1#i!it! din*mi+! +n +re+e!3 %& Jaime Martínez Verdú

J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT

4 SIMULACIÓN >LOBAL La !e7#nda )arte de e!ta )r*+ti+a +n!i!te en ,a+er #na !im#(a+i-n de (a e4(#+i-n de (a! )!i+ine! de +ada #na de (a! arti+#(a+ine! a )artir de #n e!1#ema !im#(in2 en e( 1#e a7r#)am! td ( 4i!t ,a!ta a,ra8 +inem*ti+a/ din*mi+a/  )(ani0i+a+i-n de tra9e+tria!/ a++inamient! 9 +ntr(8

F%1u,a 793 Dia7rama de .(1#e! en Simulink  de( +ntr( 7(.a(3 Cn e!te e!1#ema rea(izam! #na )(ani0i+a+i-n de (a tra9e+tria 1#e de.e !e7#ir e( e
J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT 5ara e(( di4ide (a tra9e+tria en #n númer de !e7ment! 1#e 4iene im)#e!t  )r (! )ar*metr! +n (! 1#e !e de!ea !im#(ar8 tiem) de !im#(a+i-n e inter4a( de inte7ra+i-n3 ENTRADA='> re)re!enta e( in!tante de tiem) a+t#a( =!e7>3 ENTRADA=?8> e! (a )!i+i-n +arte!iana ini+ia( de (a tra9e+tria3 ENTRADA=8%> e! (a  )!i+i-n +arte!iana 0ina( de (a tra9e+tria3 0#n+tin 1  )(ani0i+a=entrada> t  entrada='>G  )'  entrada=?8>G  )?  entrada=8%>G

; In!tante a+t#a( de !im#(a+i-n =!e7> ; 5!i+i-n +arte!iana ini+ia( ; 5!i+i-n +arte!iana 0ina(

t!  'G inter4a(  'e&?G

; Tiem) de !im#(a+i-n =!e7> ; Inter4a( de inte7ra+i-n =!e7>

; Númer de !e7ment! de (a tra9e+tria n!e7  t!inter4a(G ; C*(+#( de( 4e+tr #nitari #  )?&)'G m#  !1rt=#='>?#=?>?#=>?>G #  ='m#>P#G ; C*(+#( de (a di!tan+ia entre )#nt! d  m#n!e7G ; Númer de )#nt a+t#a( en (a tra9e+tria =e( ini+ia( e! :> i  tPn!e7G ; C*(+#( de (a )!i+i-n +arte!iana a+t#a( de (a man de( mani)#(adr  )!  )'=iPd>P#G ; C*(+#( de (a! +rdenada! arti+#(are! 1  dire+tain4er!a=)!>G En e( .(1#e Cinem*ti+a in4er!aK intrd#+im! (a 0#n+i-n directa_inversa.m: E!ta 0#n+i-n de4#e(4e e( 4e+tr de +rdenada! arti+#(are! +rre!)ndiente a (a !(#+i-n +inem*ti+a in4er!a de (a man de( mani)#(adr en (a )!i+i-n 9 rienta+i-n e<)re!ada! en (a matriz T )ara +ada )#nt de (a tra9e+tria )(ani0i+ada3 5ara ma9r !im)(i0i+a+i-n !e ,a+e 1#e (a rienta+i-n de( e
%&% Jaime Martínez Verdú

J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT 0#n+tin 1  dire+tain4er!a=entrada> ; 5!i+i-n +arte!iana  )  entradaG ; Orienta+i-n de!eada en e( eG Y en e( .(1#e R.t 7d(K intrd#+im! (a 0#n+i-n dindir5gd.m8 E!ta 0#n+i-n +a(+#(a e( 4e+tr <' de a+e(era+i-n de +ada arti+#(a+i-n #ti(izand e( ter+er m"td de a(2er 9 Orin3 ENTRADA='8> re)re!enta e( )ar de entrada a +ada arti+#(a+i-n3 ENTRADA=8':> e! (a )!i+i-n de +ada arti+#(a+i-n3 ENTRADA=''8'> e! (a 4e(+idad de +ada arti+#(a+i-n3 0#n+tin 1))  dindir7d(=entrada> ta#  entrada='8>G 1  entrada=8':>G 1)  entrada=''8'>G

; 5ar de entrada a +ada arti+#(a+i-n ; 5!i+i-n de +ada arti+#(a+i-n ; Ve(+idad de +ada arti+#(a+i-n

; 5ar*metr! de (a +ar7a =n#(! 9a 1#e e( ta(adr 9a ( in+(#im! en n#e!tr r.t8 (> ma!ae ta#='>  ?P)iPta#='>)G ta#=?>  ?P)iPta#=?>)G ta#=>  ?P)iPta#=>)G ; Se +a(+#(a (a a+e(era+i-n #ti(izand e( m"td de a(2er 9 Orin3 1))  a(2errin=1/1)/ta#/ma!aeG = En +ada #na de e!ta! 0#n+ine! ,em! #ti(izad 0#n+ine! de (a +inem*ti+a 9 din*mi+a de n#e!tr r.t/ .tenida! 9 e<)(i+ada! en (! +a)ít#(! anterire!3> De e!ta 0rma/ a tra4"! de td! e!t! .(1#e! en (! 1#e ,em! intrd#+id 0#n+ine! .a!ada! en (a! )r*+ti+a! anterire!/ #nim! .%*&'?t%.a@(%*?'%[email protected]*t,ol/ .teniend a!í #n !im#(adr 0ina( de n#e!tr r.t3 %&F Jaime Martínez Verdú

J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT Ya n !( n! !im#(a (a +inem*ti+a +m en (a! )rimera! )r*+ti+a!/ !in 1#e a,ra !e !im#(a de 0rma +rre+ta (a din*mi+a 9 e( +ntr(/ !iend )r ( tant #na mde(iza+i-n m#+, m*! +rre+ta de( m4imient de n#e!tr r.t/ a tra4"! de (a +#a(  )dem! e!t#dia me@r !# +m)rtamient en .a!e a (a a)(i+a+i-n m"di+a 1#e rea(izar*3 Sin em.ar7/ +#and n! di!)#!im! a !im#(ar e( e!1#ema/ n! dim! +#enta 1#e (a re!)#e!ta de( !i!tema era ine!ta.(e/ e! de+ir/ (! 4a(re! 1#e tmarn n#e!tra! arti+#(a+ine! n eran +,erente!/ 9 en a(7ún +a! tendían a in0init8

F%1u,a 7:3 Re!)#e!ta de( !i!tema3 Sin em.ar7 en (a )rimera )arte de (a )r*+ti+a (a! re!)#e!ta! de (a! arti+#(a+ine! eran +rre+ta! indi4id#a(mente #ti(izand (! mi!m! re7#(adre! 5ID di!e6ad!3 5r tra )arte/ (a !im#(a+i-n n! di ini+ia(mente #n errr indi+*ndn! 1#e (! 4a(re! de ind#+tan+ia de (! mtre! !n dema!iad .a@!/ )er ,em! )#e!t (! 4a(r de +at*(73 La! 0#n+ine! 1#e re)re!entan (a din*mi+a dire+ta e in4er!a8 walkerorin5.m 9 newtoneuler5.m (a! ,em! im)(ementad +rre+tamente/ 9a 1#e !# di!e6 9 re!#(tad en (a )r*+ti+a +rre!)ndiente 0#e )!iti43 A!í/ ,em! ((e7ad a (a +n+(#!i-n de 1#e e( )r.(ema era 1#e (a !e6a( de re0eren+ia 1#e (e entra a n#e!tr r.t/ e! de+ir/ (a 1#e !a(e de( .(1#e de )(ani0i+a+i-n de tra9e+tria!/ e! dema!iad 7rande/ 9a 1#e n#e!tr! re7#(adre! (! ,em! di!e6ad  )ara entrada! m*! )e1#e6a!/ 9 #na entrada de 4a(r 0ina( tan 7rande )r4+a 1#e n#e!tra! 4aria.(e! arti+#(are! !#0ran #na! 4aria+ine! m#9 .r#!+a! 1#e n !e +rre!)nden +n (a rea(idad3 5r ( tant (a 0#n+i-n  planifica5.m  n e! +rre+ta )ara n#e!tr r.t3 $r*0i+amente !e )#ede 4er +m (a! !e6a(e! de re0eren+ia a +ada arti+#(a+i-n !n dema!iad 7rande!/ ( 1#e ,a+e 1#e (a re!)#e!ta arti+#(ar !ea ine!ta.(e3

%& Jaime Martínez Verdú

J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT E( e@em)( m*! radi+a( 1#e ,em! .tenid en e( 1#e !e )#ede 4er e!te e0e+t e! e( !i7#iente8

S&al (& ,&&,&*.%a3

R&-pu&-ta a,t%.ula,3

F%1u,a 7<3 Re!)#e!ta de( !i!tema3 A!í )#e! n! )r)#!im! .tener #na! tra9e+tria! arti+#(are! +,erente!/ dnde (a re!)#e!ta din*mi+a 9 +inem*ti+a !e t#4ieran en +#enta/ #ti(izand tra 0#n+i-n de  )(ani0i+a+i-n3 5ara e(( ,i+im! #! de (a! 0#n+ine! 1#e a)are+ían en (a )r*+ti+a )tati4a de  Pla*%%.a.%/* (& T,a&.to,%a- K8 -%*.,o*%a(o,'/ .al.ulo.o&'/ pla*%%.a(o,'/ &)alpo-'/ &)al)&l' &)ala.&l' 3 Mediante (a! +#a(e! +n!e7#im! ,a+er #na inter)(a+i-n de (a tra9e+tria &&/ .teniend tra9e+tria! arti+#(are! !#a4e! a( #ti(izar )(inmi! de rden  9 3

5 N entram! en deta((e )ara +ada 0#n+i-n de (a! ante! nm.rada!/ dad 1#e !# +-di7 n ,a !#0rid mdi0i+a+ine! im)rtante!/ !-( (a! re0erente! a 4e(+idad/ 9 !e trata de (a! 0#n+ine! #ti(izada! en #na )r*+ti+a )tati4a3 6 % & ': Jaime Martínez Verdú

J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT Intrd#+im! (a! +ara+terí!ti+a! ne+e!aria! de n#e!tr! mtre! en (a! 0#n+ine! ante! +itada!/ 9 )r+edem! a (a! .ten+i-n de (a re!)#e!ta +rre+ta #ti(izand (! !i7#iente! +mand! )ara #na! determinada! )!i+ine! ini+ia( 9 0ina(8

A!í/ ( 1#e e( )(ani0i+adr ,a+e e! a )artir de (a! 0#n+ine! a (a! 1#e ((ama internamente/ !in+rniza (! m4imient! teniend en +#enta (a! +ndi+ine! ini+ia(e! 9 0ina(e!/ (a! +ara+terí!ti+a! din*mi+a! de (a! arti+#(a+ine!3 A +ntin#a+i-n .tiene #n!  )(inmi! +m !e6a( de 4e(+idad 1#e )r)r+inan +#r4a! m*! !#a4e!3 A )artir de e!ta !e6a( !e .tienen (! 4a(re! de )!i+i-n 9 a+e(era+i-n )ara +ada in!tante de tiem)  )ara +ada arti+#(a+i-nG +n e((! tra.a@ar* e( )(ani0i+adr3 % & '' Jaime Martínez Verdú

J!" L#í! Aznar $mi!

SIMULACIÓN Y CONTROL DEL QUIROBOT Wina(mente (a! 0#n+ine! de e4a(#a+i-n de )!i+i-n/ 4e(+idad 9 a+e(era+i-n a (a! 1#e ((ama n! m#e!tran +m ,a id (a +rre+ta e4(#+i-n de (a )!i+i-n de (a! +rdenada! arti+#(are!/ !# 4e(+idad 9 !# a+e(era+i-n8

F%1u,a 77a3 5er0i(e! de )!i+i-n 9 4e(+idad3

% & '? Jaime Martínez Verdú

J!" L#í! Aznar $mi!