Simulatore cinematico di Robot 3R

Transcript

1 Descrzone generle Smultore cnemtco d Robot R Voglmo smulre l movmento d un mnoltore ntroomorfo R costtuto d tre gunt rotzonl e tre brcc (o lnk) d ess conness. Tle robot è n grdo d muovers nello szo grze tre grd d mobltà fornt dll struttur mostrt n fgur: ogn gunto confersce un sngolo grdo d mobltà n qunto l mnoltore è costtuto d un cten cnemtc ert, con un estremo vncolto ll bse e l'ltro connesso ll'orgno termnle, detto nche end-effector. Questo elemento è tcmente movmentto ttrverso un olso sferco (ltr grd d mobltà), nvece nel nostro cso è semlcemente ncorto ll estremtà del robot. L'sse d rotzone del gunto d bse (ntone) è ortogonle gl ltr due, tr d loro rllel. Per nlog con l brcco umno l secondo gunto è detto gunto d sll, mentre l terzo d gomto. Vntggo d quest struttur è l grnde gltà dovut ll ntur rotodle d tutt gunt. D'ltro cnto l recsone nel oszonmento dell'end-effector vr n tutto lo szo d lvoro e vene meno l corrsondenz tr grd d mobltà e grd d lbertà. Per szo d lvoro d un robot s ntende l'nseme de unt (oszon) dello szo che l robot uò rggungere con l'orgno termnle. Nell fgur sottostnte s vede che lo szo d lvoro er l mnoltore R è rossmtvmente un orzone d sfer: Desdermo fr svolgere l mnoltore due comt: ) ssegnre ll'end effector un trettor rettlne unto-unto con un certo roflo d veloctà ) ssegnre l R un confgurzone d gunt nzle e frgl rggungere un'ltr confgurzone monendo rtcolr condzon d moto er gunt.

2 Prm d mlementre comt del robot e commentre rsultt seghmo le descrzon mtemtche e le funzon necessre tle scoo: Descrzone geometrc Per descrvere l robot utlzzmo l convenzone d Denvt-Hrtenberg l qule fornsce un metodo generle er le ctene cnemtche erte che consente d fssre sstem d rfermento su gunt (e lnk) er oterne determnre rmetr crtterstc. D quest rresentzone è ossble, trmte l'uso d mtrc d roto-trslzone de sstem d rfermento, trovre un legme fr rmetr de gunt e l oszone e l'orentmento dell'end-effector. Rferendoc ll fgur ssummo come sse l sse del gunto che connette l brcco l brcco e er l defnzone dell tern (soldle l brcco ) utlzzmo l Convenzone d Denvt-Hrtenberg s scegle l sse z gcente lungo l sse del gunto + ; s ndvdu O ll ntersezone dell sse z con l normle comune gl ss z e z, e con O ' s ndc l ntersezone dell normle comune con z ; s scegle l sse x dretto lungo l normle comune gl ss z e z con verso ostvo dl gunto l gunto + ; s scegle l sse y n modo d comletre un tern levogr. c sono cs (come l R) n cu due o ù terne del robot non sono unvocmente determnte: o con rfermento ll tern, er l qule solo l drezone dell sse z rsult secfct: s ossono qund sceglere rbtrrmente O ed x ; o con rfermento ll tern n oché non v è gunto n +, z n non è unvocmente defnto mentre l sse x n, deve essere normle ll sse z n : tcmente, l gunto n è rotodle, er cu z n v llneto con z n ; o qundo due ss consecutv sono rllel, n qunto l normle comune tr d loro non è unvocmente defnt; o qundo due ss consecutv s ntersecno, n qunto l verso d x è rbtrro; o qundo l gunto è rsmtco, nel qul cso solo l drezone dell sse z è determnt.

3 Il robot ntroomorfo R è crtterzzto enmente d queste condzon: oltre tern bse e tern soldle ll end-effector c sono due gunt rllel e noltre gl ss consecutv del rmo e del secondo gunto s ntersecno nel secondo gunto. Vedremo n bse ll lgortmo come effetture le tutte le scelte er l rresentzone szle del mnoltore. Intnto bst dre che un volt scelte le terne soldl brcc, l oszone e l orentmento dell tern rsetto ll tern rsultno comletmente secfct d seguent rmetr: dstnz d O d O ', d coordnt su z d O ' (mgr rscrvere come st su dsense +chro) α ngolo ntorno ll sse x tr l sse z e l sse z vlutto ostvo n senso ntorro ngolo ntorno ll sse z tr l sse x e l sse x vlutto ostvo n senso ntorro ϑ I rmetr e α sono semre costnt e dendono soltnto dll geometr d connessone de gunt consecutv dettt dll resenz del brcco. Degl ltr due, uno soltnto è vrble n dendenz del to d gunto utlzzto er connettere l brcco l brcco ; n rtcolre: se l gunto è rotodle l vrble è ϑ se l gunto è rsmtco l vrble è d Pertnto un rtcolre confgurzone del R srà ndvdut d un vettore mentre rmetr d, d, d sono fss. q = ], T [ ϑ ϑ ϑ Vedmo dunque come mettere n relzone tutt rferment e rmetr del robot er o utlzzrl ne clcol delle smulzon: Algortmo d Denvt-Hrtenberg. Indvdure e numerre consecutvmente gl ss de gunt; ssegnre, rsettvmente, le drezon gl ss z,..., z n.. Fssre l tern oszonndone l'orgne sull'sse z ; gl ss x e y sono scelt n mner tle d ottenere un tern levogr. ss -5 vnno retut er =,..., n :. Indvdure l orgne O ll ntersezone d z con l normle comune gl ss z e z. Se gl ss z e z sono rllel e l gunto è rotodle, oszonre O n modo d nnullre d ; se l gunto è rsmtco, sceglere O n corrsondenz d un oszone d rfermento er l cors del gunto (d esemo un fne-cors). 4. Fssre l sse x dretto lungo l normle comune gl ss z e z con verso ostvo dl gunto l gunto Fssre l sse y n modo d ottenere un tern levogr. 6. Fssre l tern n, llnendo z n lungo l drezone d z n se l gunto n è rotodle, ovvero sceglendo z n n mner rbtrr se l gunto n è rsmtco; fssre l sse x n n ccordo l unto 4.

4 Gl ultm due ss dell lgortmo sono secfcmente enst er l determnzone delle mtrc d trsformzone convolte nel roblem dell Cnemtc drett Tenendo resente che, vendo seguto ss snor descrtt, dlle scelte oerte rsult l seguente ssegnzone de sstem d rfermento, voglmo or esrmere le relzon che, rtre dlle confgurzon de gunt del robot, c ermetternno d rcvre le coordnte dell tern soldle ll'end-effector, qund l su oszone, oss rsolvere l roblem dell cnemtc drett. Proseguendo con Denvt-Hrtenberg lchmo l lgortmo er defnre le relzon del mnoltore ntroomorfo R: 7. Costrure er =,..., n l tbell de rmetr, d,α, ϑ. Per l R comlmo l seguente tbell: Brcco α d ϑ π / ϑ ϑ ϑ s not che l tern è stt scelt con orgne ll ntersezone d z e z (coè sul secondo gunto), d cu d = e che z e z sono rllel e gl ss x e x sono scelt lungo gl ss de reltv brcc, d cu gl ltr vlor. In quest ssegnzone vlor e corrsondono lle lunghezze del secondo e del terzo brcco. 8. Clcolre sull bse de rmetr d cu l unto 7 le mtrc d trsformzone omogene A ( q ) er =,..., n. Queste esrmono l legme tr l tern soldle l gunto e l tern soldle l gunto, contengono le oerzon d effetture sull un erché s sovrong ll ltr. Per l R, n bse l clcolo delle oerzon s h: c s c s c s c ( ) = A ϑ ( ) s c s A ϑ = =, s c,..., j,..., j = sn( ϑ ϑ ) = cos( ϑ ϑ ) j j

5 n 9. Clcolre l trsformzone omogene T n ( q) = A... A n che fornsce oszone e orentmento dell tern n rsetto ll tern. cc cs s c ( c + c) Nel nostro cso sc ss c s( c + c) ( ) = = T T q A A A q = [ ϑ ϑ ϑ] s + c s s b. Assegnte T e T n e, clcolre l funzone cnemtc drett T b b n e (q) = T Tn Te che fornsce oszone e orentmento dell tern utensle rsetto ll tern bse. Nel nostro cso esste solo l tern n che non concde con un eventule tern utensle, dto che non è resente nell nostr smulzone er cu non consdermo questo ultmo sso dell lgortmo. Segue l relzzzone dell funzone cnemtc drett n Mtlb, nel rqudro sottostnte; ognuno de rqudr delle funzon llustrte srà semre receduto dl nome del fle e d un nseme d lnk form d con, fcendo clc su qul srnno ntrrese determnte zon: srà vvto Mtlb che rrà dll crtell rogrms l fle er oterlo edtre e/o esegure; l fle srà erto nzché con Mtlb con l edtor ssocto gl m-fle ( scelt dell utente); quest rm due lnk funzonno nel cso n cu s stto nstllto l smultore su dsco rgdo; l fle srà erto con l edtor degl m-fle drettmente dll crtell rogrmm ( ) contenut nel CD-ROM. A second del sstem oertvo nstllto lcun lnk otrebbero rsultre non funzonnt er v dell confgurzone (d esemo se s legge l CD ttrverso un msterzztore n mbente XP con l ozone d msterzzzone d Wndows ttvt); srà oortuno rferrs ll Aendce C d questo documento er l nstllzone e l dsnstllzone del smultore cnemtco sul roro comuter. Delle funzon sono rortte le rghe rncl e d lcune rtcolrmente lunghe soltnto comment essenzl er un comrensone normc. cndr.m % Clcol l cnemtc drett del mnoltore ntroomorfo R, % con n ngresso l vettore d confgurzone de gunt e resttusce % l vettore oszone dell'orgne dell tern soldle ll'end-effector functon []=cndr(q) globl l l % Tbell de rmetr d Denvt-Hrtenberg robot R =; d=; tw=/; th=q(); =l; d=; tw=; th=q(); =l; d=; tw=; th=q(); % oszon comode er scrvere l mtrce comlet c=cos(q()); s=sn(q()); c=cos(q()); s=sn(q()); c=cos(q()+q()); s=sn(q()+q()); Tdr= [ c*c -c*s s c*(*c+*c) ; s*c -s*s -c s*(*c+*c) ; s c *s+*s ; ]; =[Tdr(,4) Tdr(,4) Tdr(,4)]'; L funzone, er un ngresso q nel formto vettore (d esemo q = [..4.5]' ), resttusce l oszone, semre n formto vettore ( = [...]') dell tern soldle ll end-effector n quell dt confgurzone utlzzndo le nformzon su rmetr d Denvt-Hrtenberg fornt dll esterno. I rmetr che seguono l comndo globl sono comun tutte le funzon del worksce d Mtlb che rortno gl stess rmetr doo l struzone globl, n modo che un volt fornt dt dll esterno (d un m-fle o dl romt d Mtlb) e mmess nello szo globle tutte le funzon che useremo frnno uso d tl dt. Se d esemo s coml l fle con dt, nel qule è contenut l strng globl l l l, srà ossble rchmre cndr che utlzzerà roro vlor v defnt. Questo vle nche er tutte le funzon successvmente llustrte.

6 Cnemtc dfferenzle Mentre l cnemtc drett è un legme sttco tr szo de gunt e szo crtesno, così come l cnemtc nvers nltc (che vedremo mmedtmente doo), l cnemtc dfferenzle utlzz l legme dnmco tr l veloctà crtesn dell'end-effector e le veloctà de gunt. Tl relzon s bsno su un oertore lnere detto Jcobno che n generle dende dll confgurzone n cu s trov l Robot. Jcobno Esstono due t d Jcobno: Jcobno geometrco e Jcobno nltco. Il rmo descrve l legme tr le veloctà de gunt e l veloctà lnere dell'end-effector trmte un mtrce d trsformzone dendente dll confgurzone del mnoltore. Il secondo sfrutt un rresentzone n form mnm dell ostur dell'end-effector, n bse d un clcolo dretto, mednte un'oerzone d dfferenzzone dell funzone cnemtc drett rsetto lle vrbl d gunto. Nel cso del mnoltore ntroomorfo R (nel nostro cso rvto del olso) due t d Jcobno concdono. L struttur bse dello Jcobno è l seguente: = er un gunto rotodle z z rsmtco er un gunto z j j O P ) ( Nel nostro cso s h: = ) ( ) ( ) ( z z z z z z J l clcolo de vettor d oszone de brcc fornsce = = s c s c c = ) ( ) ( s s c c s c c c e quello de versor degl ss d rotzone de gunt: = = = c s z z z

7 sosttuendo s ottene lo Jcobno comleto (oszone e orentmento) s( c + c) c ( s + s) cs c( c + c) s( s + s) ss + c c c J P J = oss J = s s JO c c Nel nostro cso, vendo un mnoltore m rdonnte con sol tre grd d lbertà, bbmo solo tre rghe lnermente ndendent (le rme tre). Cò s gustfc ntutvmente con l ftto che l end-effector è ncorto l terzo brcco e dunque non è ossble sceglerne rbtrrmente l orentmento un volt fsst l oszone. Pertnto lo Jcobno che no renderemo n consderzone s rduce ll seguente esressone: J P s( c + c) = c( c + c) c ( s s ( s c + s + s + c ) ) c s s s c L funzone che scrvmo n Mtlb er clcolre lo Jcobno del R è bnlmente l lczone dell formul: Jc.m % L funzone Jc clcol lo Jcobno del robot R che % rsult defnto er un determnt confgurzone d % gunt n ngresso q. functon j=jc(q) globl l l; t=q(); %=; l è ndfferente n qunto l rfermento è sul ntone e = t=q(); =l; t=q(); =l; j=[-sn(t)*(*cos(t)+*cos(t+t)) -cos(t)*(*sn(t)+*sn(t+t)) -*cos(t)*sn(t+t); cos(t)*(*cos(t)+*cos(t+t)) -sn(t)*(*sn(t)+*sn(t+t)) -*sn(t)*sn(t+t); *cos(t)+*cos(t+t) *cos(t+t)]; Cnemtc nvers Or l nostr ttenzone unt rsolvere l roblem cnemtco nverso e n seguto ll mlementzone d un funzone che c fcc sere non solo qul sono le soluzon, qundo è ossble trovrle, m nche ù n generle se esstono e se sono n numero fnto o nfnto. Queste funzonltà srnno rtcolrmente utl nelle successve fs d nfczone del moto. Tle nls è res ossble monte dllo studo dello Jcobno en esosto. Esttmente l roblem è l seguente: dt un determnt os er l'end-effector, cerchmo, se ossble, un o ù soluzon er l corrsondente confgurzone de gunt. A dfferenz dell cnemtc drett, che mmette semre un'unc soluzone, ossmo vere: nessun soluzone: se l confgurzone non rtene llo szo d lvoro del mnoltore un numero fnto d soluzon : qundo esstono ù mod er oszonre lnk, er l R soltmente sono quttro, m ossono degenerre n due coe d soluzon concdent. nfnte soluzon : qundo esstono nfnt mod d oszonre lnk, questo succede n rtcolr condzon d sngolrtà.

8 T Qund cerchmo un vettore q = [ ϑ ϑ ϑ] d ngol de gunt tl che l'orgno termnle s trov nel unto desderto. Rrendmo n consderzone l struttur del robot: Dl cso del mnoltore lnre R (tenendo fsso ϑ l descrzone er ϑ e ϑ è nlog): onmo W x W y = = x y c φ s φ = c + c = s + s Nel cso lnre W x è l roezone sull' sse x dell'orgne dell tern soldle l gunto W mentre nel cso trdmensonle rresent, er un vlore fssto dell'ngolo reltvo l rmo gunto, l roezone sull'sse x dell'orgne dell tern soldle ll'end-effector dl qule voglmo rcvre trmte nversone l confgurzone de gunt. Anloghe consderzon vlgono er e P Wz, dto che rgonmo or n tre dmenson. Per l rmo gunto ottenmo due soluzon: ϑ = Atn( W, y W x ϑ = π + Atn( W, y W x A dfferenz dell semlce funzone rco-tngente l funzone Atn, consderndo segn degl rgoment, resttusce l'rco-tngente nel qudrnte corretto. ) ) W y

9 A questo unto, determnto ϑ, er l clcolo d ϑ e ϑ ossmo consderre l mnoltore lnre: W x + W y + W z c = s = ± c dove con W z s ntende l roezone del vettore oszone sull'sse z del rfermento, qund: ϑ = Atn( s, c) er ϑ s h: s ( + c ) s + + c ) W z W x W y W x W y = c = W x + W y + W z W x + W y + W z ( + + s W z ϑ = Atn( s, c) D recedent ssgg s rconosce l'esstenz d quttro soluzon l vrre de vlor d ϑ, ϑ e ϑ : sll destr-gomto lto, sll snstr-gomto lto, sll destr-gomto bsso, sll snstr-gomto bsso. Queste confgurzon sono llustrte nell successv fgur. S uò osservre che l roblem cnemtco nverso esste un'unc soluzone solo se W x e W y ; nftt n cso contrro s ossono ottenere nfnte soluzon rescndere dl vlore d ϑ ; Stuzon del genere vengono defnte sngolrtà cnemtche. Questo non sgnfc che n tutte le condzon d sngolrtà le soluzon sono semre nfnte: d esemo er tutte le confgurzon del to ϑ = m con W x e W y esstono due soluzon; o meglo, le quttro soluzon reltve sll e gomto s rducono due coe d soluzon concdent. Inftt n tle stuzone l gomto è steso, er cu l confgurzone sll-destr gomto lto corrsonde sll-destr gomto bsso e sll snstr gomto-lto è dentc sll snstr-gomto bsso. Quest è un ltr stuzone d sngolrtà. È evdente dunque l necesstà d un crtterzzzone d tutte queste confgurzon.

10 Clssfczone delle sngolrtà cnemtche Le sngolrtà o confgurzon sngolr s hnno qundo l mtrce che esrme lo Jcobno erde d rngo (determnnte nullo); n tl confgurzon del mnoltore uò ccdere che: lcune drezon d moto non sno relzzbl bsse veloctà dello szo oertvo corrsondono veloctà molto elevte gunt non s h un ben defnt soluzone del roblem cnemtco nverso. È chro dunque che tl unt (o zone) vnno evtt oure ffrontt con oortun metod. Esstono due clss d sngolrtà: confn dello szo d lvoro: l mnoltore è comletmente esteso o retrtto; sono fclmente ggrbl evtndo d ortre l mnoltore confn dello szo d lvoro. Per l mnoltore R se le lunghezze degl ultm due brcc non sono ugul s cre un zon d non rggungbltà sferc e centrt nell orgne vente come rggo l dfferenz del ù lungo con l ù corto. È fcle vederlo sul cso del R lnre, l estensone l cso szle è o mmedt ggungendo nello szo l rmo lnk: Szo d lvoro: R lnre Robot R con l>l Inoltre se s voglono consderre fne-cors de gunt lo szo d lvoro è ncor ù rstretto: esemo d worksce d R lnre con ϑ [, ] ; ϑ [ 9,9. ] Per l R s trovno rsultt sml Nel modello cnemtco del smultore no gnormo quest ossbltà lscndo tutt gunt lber d ruotre nell ntervllo [, π ] comendo nche ù gr (nell reltà questo è uttosto rro ). I contorn verd descrvono le zone d sngolrtà gomto steso/retrtto. All nterno dello szo d lvoro: llnemento d ss del moto o confgurzon rtcolr dell orgno termnle; sono n generle d evtre n fse d nfczone dell trettor.

11 Anlzzmo or mtemtcmente le sngolrtà del nostro robot: non essendo dotto d olso uò mbtters soltnto n sngolrtà d struttur ortnte. Il determnnte dello Jcobno d no consderto è det( J P ) = s( c + c). Nell'otes d, l determnnte s nnull er: s = s h qundo ϑ = o ϑ = π, coè qundo l robot h l gomto tutto steso (come nell fgur sottostnte) o regto su se stesso. Queste vengono chmte sngolrtà d gomto. ( c + c) = s h qundo l'orgne del sstem d rfermento soldle ll'end-effector s trov lungo l'sse d rotzone d bse z (nfnte soluzon), coè qundo x = y =. Quest vene dett sngolrtà d sll. In rtcolre nell orgne del sstem d rfermento d no dottto, se ess è rggungble, coè se l = l, s hnno ddrttur soluzon. Inftt un modo d descrverle è er esemo fssre ϑ π oure π e fr vrre ndendentemente gl ltr due rmetr ϑ n [ π /, + π /] e ϑ n [, π ]. Aggungmo nfne che nel cso d sngolrtà sul confne nterno dello szo d lvoro, qulor brcc l ed l fossero d lunghezz dvers, s trovno semre quttro soluzon dstnte ( meno che l unto non s su z ) sebbene l unco orentmento ossble er l end-effector, che essendo ncorto l terzo brcco è esttmente ϑ, è comune tutte e quttro le soluzon.

12 Presentmo or l mlementzone dell cnemtc nvers che tene conto d tutte le consderzon ftte fnor. In ù remettmo che, er rgon che srnno chrte qundo esorremo l nfczone de comt, lddove nltcmente trovmo nfnte soluzon, l funzone effettu un rcerc semlfct d vlor mmssbl er le confgurzon de gunt e resttusce, oltre un nseme d nformzon sulle sngolrtà ndvdute, l nseme d vlor trovt. D quest funzone è rortto l commento, che ne costtusce nche l hel, n qunto l codce è urmente tecnco, non è molto sgnfctvo l modo n cu è mlementto: nvnltc.m % Invers nltc: dt un oszone er l centro dell'end-effector del R % nlzz l'esstenz d soluzon e le clcol qundo esstono. Se l mnoltore % s trov n sngolrtà vene secfcto l to d sngolrtà e vengono % clcolte, tr le nfnte soluzon, tnte second dell secfc d recsone % n ngresso; l motvo è che frnno comodo er l nfczone % d trettore vvcnrs n tutte le stuzon ossbl d rtenz e rrvo % quelle che rendono mglore l moto sotto vr sett (d esemo mnmzzno % gl sostment de gunt), nche qulor l unto d rtenz e/o rrvo % fossero n sngolrtà. I comment sottostnt fvorrnno l comrensone % de codc scrtt nell funzone: % % Ingress: % = oszone crtesn del centro del olso % ccurcy=lvello d recsone con cu eslorre le ossbl soluzon n rtcolr % condzon d sngolrtà; n rtc è uno sclre che ndc l numero d rt % cu vene dvs un semcrconferenz e serve fr vrre un ngolo tr e / % n (ccol) ss. Tle vlore srà utlzzto dll funzone 'nvnormmnm' % Non è comunque necessro secfcre tle ngresso. Se l'ngresso non esste % l funzone utlzz un vlore nterno redefnto (volendo uò essere cmbto) % Notre che questo metodo è consderto SOLO n lcune sngolrtà. In tutt gl % ltr cs l'nversone è urmente nltc. % Uscte: 5 % msg = è un messggo che uò essere ndrzzto ll'utente, ndc role l stuzone % nlzzt dll funzone reltvmente l unto e nche l to d sngolrtà; % è rtcolrmente utle se s vuole utlzzre temornemente l funzone d sol % senz fr grre l smultore, fn d vlutre rdmente molt unt d fl % er l nfczone d nserre successvmente nell smulzone. % Esemo: dgtndo l romt d Mtlb >>nvnltc([ ]') % nzchè un comndo comleto del to >>[msg,m,nsng,st,nsol]=nvnltc(,ccurcy) % l funzone resttusce solo l rm delle uscte, coè l messggo che otrebbe essere % d esemo (se l=l=l=5)'il mnoltore s trov n sngolrtà lungo l'sse del ntone % (nfnte soluzon er thet)', e us l recsone ntern dto che ccurcy non è fornto % M = Se esstono soluzon n form chus sono doe o 4, rortte 4 rghe (mtrce 4x). % Qulor erò le soluzon rsultssero nltcmente nfnte, come suddetto l mtrce ssume % dverse dmenson secondo l to d sngolrtà e n bse l lvello d ccurtezz; % Le sngolrtà n cu s hnno nltcmente nfnte soluzon sono qu dvse n due nsem % ) sngolrtà lungo l'sse del ntone m non nell'orgne (mmesso che s rggungble) % n questo cso thet e thet hnno determnte soluzon mentre thet vr n [,] % Pochè come secfcto su bsogn fr vrre un ngolo (thet) tr e % srnno genert *ccurcy vlor er thet essendo ccurcy l numero d dvson % d effetture su un semcrconferenz. Pertnto M vrà dmenson (*ccurcy x ) % ) sngolrtà nell'orgne (ossble solo se l=l ltrment non è neure rggungble) % n questo cso thet vr semre n [,], m n ù vr nche thet n [-,]. % Il dscorso sull dmensone d M è nlogo solo che or s fnno vrre due ngol. % Dunque l mtrce srà comost d ù gru d colonne to quelle vste er l cso ) % L'lgortmo estre le soluzon n questo modo: rte d thet= e thet=- (th=) % o f vrre thet fno ottenendo un colonn d (*ccurcy x ) element; % doodchè ncrement thet d un sso d ccurcy (coè /ccurcy) e rete l rmo % loo. Questo fnchè thet è rrvto. Così ogn loo d thet s gener un % colonn del to [(thet vrble) (thet fsso) (th semre fsso)] % L mtrce h così (ccurcy+)* colonne (+ erchè estrem nclus) e *ccurcy rghe. % In tl modo srnno eslorte molte soluzon nell'ntorno sferco dell'orgne % er l oszonmento del gomto. Rcordmo che questo servrà d ltre funzon % monte dell nfczone d trettore er l scelt ottmle dell'nzlzzzone. % E' chro che ù è grnde ccurcy e ù l'nseme d soluzon s vvcnerà oo % m er generre tnte soluzon occorrerà nche mggor temo. D defult ccurcy vle % dunque srnno generte 4 soluzon nel cso M(4x) e er l cso [M(4x9)] % Esstono o le sngolrtà d brcco steso (m non lungo l'sse z) e, se l<>l, retrtto: % nel rmo cso le soluzon degenerno d 4 (coe d soluzon concdent); nel secondo % sono nvece semre 4 soluzon dstnte n qunto l sngolrtà mlc solo mobltà rdott % M h semre dmenson 4x nche se le rghe ndendent sono soltnto (codce ù comodo) % Infne se l unto s trov fuor dello szo d lvoro l mtrce è ost ugule [](vuoto). % nsng = vrble boolen. Vle TRUE se l mnoltore s trov n sngolrtà ltrment FALSE; % st = è un semlce sclre che vle se l sngolrtà è lungo l'sse z, se è d gomto steso, % se l unto s trov sul confne nterno dello szo d lvoro (solo nel cso l<>l); % n tutt gl ltr cs è osto -. % nsol = numero d soluzon trovte, uò essere (oltre WS), oure oo (sngolrtà z/orgne) % er le sngolrtà confn estern dello szo d lvoro,4 nel WS o confn ntern Rtenmo oortuno, uttosto che rortre qu nche l ntero codce dell funzone, che l lettore consult drettmente l fle nel cso s nteressto dettgl del codce, erltro comletmente commentto fnco.

13 Imlementzone dell cnemtc dfferenzle S dt l relzone υ = J ( q) q che convolge lo Jcobno geometrco. Se lo Jcobno è qudrto e non sngolre, l equzone dell cnemtc dfferenzle nvers è q = J ( q) υ che consente, ssegnt l veloctà dell tern utensle e l confgurzone del mnoltore, d determnre le veloctà de gunt. Tle relzone consente nche d rsolvere l roblem dell cnemtc nvers. Assegnt l trettor dell tern utensle e l veloctà d ess ssoct. υ (t) è ossble determnre le vrbl d gunto corrsondent ntegrndo l relzone q ( t) = J ( q( t)) υ( t) rtre d un condzone nzle not q () ; quest condzone deve essere tle che l tern utensle s nell oszone nzle desdert. L ntegrzone uò essere esegut er v numerc: è esttmente quello che fremo nelle smulzon ttrverso smulnk. Tle metodo è n lterntv ll rsoluzone nltc del roblem cnemtco nverso. Vntgg: l ntegrle numerco uò semre essere clcolto; Svntgg: sono rcheste rsorse n termn d temo d clcolo. Trttmento delle sngolrtà cnemtche Le soluzon dell cnemtc dfferenzle nvers ossono essere clcolte solo nell'otes n cu lo Jcobno s d rngo eno. Inftt nelle confgurzon sngolr l sstem ssocto contene equzon lnermente dendent. È ossble comunque determnre un soluzone d q estrendo tutte le equzon lnermente ndendent, qundo l ercorso ssegnto nello szo oertvo è fscmente eseguble d rte del mnoltore. Bsogn consderre che lo Jcobno uò dr luogo un sere d roblem che rgurdno essenzlmente le veloctà moste gunt. Inftt l determnnte ssume, mn mno che c s vvcn d un sngolrtà vlor tendent llo zero, l che sgnfc morre grnd veloctà gunt, cos d evtre erchè generlmente rrelzzble nell rtc. Questo è l frutto d un'nversone semlce (o elementre) dello Jcobno. Per ovvre questo roblem llor utlzzmo l'nvers mnm qudrt smorzt: J * T T = J ( JJ + λ I ) questo sgnfc ggungere un fttore d correzone che modfc l comortmento delle nverson roro seguendo l'vvcnmento lle zone d sngolrtà. Il fttore λ, dunque, se oortunmente redsosto, roduce uno smorzmento che mmorbdsce l'nversone e dunque mglor l comortmento lvello de gunt. Metod er l'nversone dell cnemtc dfferenzle Pochè effettueremo l smulzone su un clcoltore dovremo consderre che l dscretzzzone delle funzon er l'nversone, rsetto l cso contnuo, comort fenomen d derv er l soluzone: non s h un corrsondenz estt tr confgurzone d gunt clcolte e ostur effettvmente rggunt dll'orgno termnle nello szo oertvo. S uò ovvre questo roblem tenendo conto dell'errore tr l oszone desdert e l oszone clcolt: e = x x, d cu, dervndo s h e = x d x. Alcndolo ll cnemtc dfferenzle s h d e = x d J ( q) q. A

14 Per legre q d e è dunque ossble sfruttre l: Metodo d Newton (e nversone robust) Il metodo d Newton è un'lgortmo generle d rossmzone er le soluzon d roblem numerc (non lner) e s rssume nell seguente esressone: k k k k f ( q) = f ( q ) + J ( q )[ q q ] + o[ q q ]; r se dentfchmo l f (q) come l cnemtc drett del nostro mnoltore e scrtmo l' o ccolo llor ossmo rscrvere tle esressone come: qk+ = qk + J ( qk )[ r f ( qk )]. Iotzzndo J qudrt e non sngolre e lcndo l metodo d Newton s gunge : q = J ( q)( x Ke). A d + Il rmetro K è un mtrce defnt ostv e (tcmente) dgonle che f tendere l'errore zero lungo l trettor desdert con un veloctà d convergenz che dende d suo utovlor: ù sono grnd, ù è veloce l convergenz. L'nvers J uò essere dttt n bse lle esgenze secfche. Se l ercorso ss vcno d un zon d sngolrtà l normle nversone dello Jcobno ort d un "eslosone" dell'lgortmo verso vlor semre ù elevt. È ertnto oortuno modfcre l metodo er l'nversone defnendo un nuov nversone "robust", secondo l metodo gà vsto de mnm qudrt smorzt. L nuov esressone dvent: * q = J ( q)( x + d Ke) con * T T J = J ( JJ + λ I) Nel nostro cso, esmnmo drm un confgurzone n cu l'end-effector rsult rossmo d un sngolrtà. Pù recsmente consdermo un oszone n cu l confgurzone d gunt rsult sngolre er v dell nnullmento del determnnte. L lontnnz dll sngolrtà è come un ε che rresent l dstnz del unto dl centro dell sngolrtà. Il mnoltore è rossmo d un zon d sngolrtà, come l sse z, uttosto che d un unto, llor ε srà semlcemente l dstnz dell end-effector dll sse. Inftt s trovno nfnt centr d sngolrtà vcn l mnoltore e l ε scelto è er l unto quello reltvo l ù vcno. ε

15 Per vror nferor ε, coè d un certo ε fssto n o (n drezone dello zero), lchmo un roflo trngolre l rmetro λ n funzone d ε : λ = λ(ε ) λ mx ε ε ε L'esressone d tle funzone rsult nltcmente: ε λ( ε ) = λmx ε ε ε nell'mlementzone d tle roflo n Mtlb erò scegleremo d utlzzre l funzone solo nel rmo qudrnte (ostvo) n qunto Mtlb non ermette d gestre vettor con coordnte negtve, senz erltro lterre effettvmente l comortmento dell funzone: l centro dell sngolrtà l correzone λ è mssm, mentre, mn mno che l end-effector s llontnn dl centro, λ decresce semre ù er ogn drezone n modo lnere. Con l funzone lm mlementmo tle roflo: lm.m % Quest funzone costrusce un roflo trngolre che vene utlzzto % nell cnemtc dfferenzle er mmorbdre l clcolo dello % Jcobno e fre subentrre grdulmente l'nversone robust % qundo l mnoltore v verso condzon d sngolrtà. % Altrment l ssggo srebbe brusco con robbl dscontnutà % dnnose er gunt functon [l]=lm(err,es,lmx); l=lmx*(-err/es); Gl rgoment n ngresso sono err = ε, è l ndce delle scsse, vrble tr ed ε ; es = ε, è l rggo mssmo entro cu s consder l robot n confgurzone d sngolrtà; lmx = λ mx, è l vlore mssmo d correzone che s h n en sngolrtà. In usct è resttuto l vlore λ d correzone dendente d ε.

16 Ftte queste consderzon commentmo l funzone che n Mtlb mlement l clcolo dell nversone dell cnemtc dfferenzle trttndo le sngolrtà: cndffnv.m % cndffnv: è un funzone rel tme che clcol, secondo lo scorrere del clock d Smulnk (mlcto) % l cnemtc dfferenzle secondo le equzon e metod numerc dottt nell tesn. % d ogn sso d cmonmento dell'mbente Smulnk vvene l seguente ercorso: % l funzone clcol lo jcobno del mnoltore n bse ll confgurzone de gunt ù recente; % clcol o con l cnemtc drett l oszone ttule del mnoltore n bse q del sso recedente % sso er sso; questo gl servrà er lcre gl lgortm del grdente e d Newton. Qund rsolve. % Per l scelt dell rsoluzone sono esmnte le seguent condzon: % ) se l unto è n rossmo d un sngolrtà llor è utlzzt l'nversone mnm qudrt smorzt % con un fttore d correzone roflo trngolre (reltvo ll vcnnz dell sngolrtà) % tle roflo è relzzto rchmndo l funzone lm oortunmente rogettt. % ) se l unto uttosto lontno dl unto d rrvo rte l'lgortmo con l metodo del grdente % ) se l unto è bbstnz vcno l unto d rrvo rte l'lgortmo con l metodo d Newton. % Durnte le smulzon sono ttv de conttor che l termne rortno ss effettut er ognuno de % metod: ngrd (grdente), nnewt (newton), sng (mnm qudrt). dgtndo tl nom comre l vlore de % ss. Per vedere come sono nzlzzt q e qf rferrs ll Aendce A. % Intnto bst dre che se l'utente secfc unto nzle e fnle nzchè confgurzone nzle e fnle, % er entrmb unt se esstono soluzon ne esstono lmeno due. Pertnto esste un'ltr funzone % che scegle tr le soluzon quelle che mnmzzno l dstnz tr le confgurzon nello szo de gunt. % E' chrmente un crtero molto rossmtvo, se ne otrebbero utlzzre d ss ù sofstct, % m gà ermette d mglorre notevolmente le restzon globl rsetto ll scelt dell % "rm soluzone che ct", seclmente qundo uno o entrmb unt sono n zon d sngolrtà % Un recszone: n mbente Smulnk come n molt rsolutor mtemtc, sono "vett" loo lgebrc % e n generle non s ossono utlzzre funzon d Mtlb con ù ngress er blocch delle smulzon, % llor sfruttmo l otenzltà d Mtlb che c ermette d clcolre un rmetro nterno ll funzone % n temo rele e d mndrlo nello szo globle, vendolo dchrto globl, e qund d rutlzzrlo % er ss successv semre rchmndolo dllo szo globle. Questo vle nche er combnzon d ù % rmetr. Il rsultto è che d ogn sso s h l vlore ttule clcolto nternmente ll funzone % e l vlore tenuto n memor er un sso d cmonmento nel worksce d Mtlb. Quest è esttmente % l rocedur er mlementre l nostro smultore temo dscreto qundo s lvor nello szo crtesno. % Dunque nel codce che segue sono contenut mlctmente de loo non vsbl nello schem smulnk, % che otrebbe trrre n ngnno fcendo ensre d un semlce schem n cten drett. % Tl loo sono roro quell resent nelle esresson degl lgortm d nversone rortt nell tesn. functon [qd]= cndffnv(ont) globl l l l f q qf es lmx sng nnewt ngrd K Ks lh Id qd s lh; % l rmetro globle s srà l veloctà crtesn most ll end-effector Lq=dst(q,qf);% dstnz nello szo de gunt tr confgurzone nzle e confgurzone fnle % vene qu rchmt l funzone dst vene segt nell sezone funzon oggo scelterr=lq/.5;% questo è un rmetro, funzone dell suddett dstnz, che serve sceglere qundo % v usto l grdente, e qundo l'lgortmo d Newton (se l robot è fuor d sngolrtà) Jct=Jc(qd); % clcolo dello Jcobno nell confgurzone ttule qd rchm l funzone 'Jc.m' =cndr(qd); % clcolo dell oszone ttule secondo l'ultm confgurzone clcolt (nz l loo) % rchm l funzone dell cnemtc drett 'cndr.m' f test(ont) % rchm l funzone 'test.m' che verfc se un unto è nello szo d lvoro, v. vnt error('end effector oltre confn dello szo d lvoro! Rvedere l nfczone: qus certmente erchè l<>l') end =l; PWx=(); =l; PWy=(); =l; PWz=(); dstz=sqrt(pwx^+pwy^); % dstnz dll'sse delle sngortà d sll z dstconfest=bs((+)-sqrt(pwx^+pwy^+(bs(pwz)^))); % dstnz dl confne esterno del WS f l~=l dstconfnt=sqrt(pwx^+pwy^+pwz^)-(-); % dstnz dl confne nterno del WS (solo se l<>l) end % ANALISI DELLE SINGOLARITA' f ((sqrt(pwx^+pwy^)<=es)... % d sll: l centro del olso gce sull'sse z -> nfnte sngolrtà (bs((+)-sqrt(pwx^+pwy^+(bs(pwz)^)))<=es))... % gomto steso: olso confn estern (< & (sqrt(pwx^+pwy^+pwz^)-(-)<=es)) (> & (sqrt(pwx^+pwy^+pwz^)-(- )<=es)) % gomto tutto regto con l dverso d l: sngolrtà nel confne nterno del WS % vene qu scelto l'eslon er l roflo trngolre d correzone dell'nvers f (dstz<=es) dstsng=dstz; elsef (dstconf<=es) dstsng=dstconfest; elsef ((< & (sqrt(pwx^+pwy^+pwz^)-(-)<=es)) (> & (sqrt(pwx^+pwy^+pwz^)- (-)<=es))) dstsng=dstconfnt; end Js=Jct'*(nv((Jct*Jct')+((lm(dstsng,es,lmx)))*Id)); % INVERSA AI MINIMI QUADRATI SMORZATI qd=qd+(js*(s+(ks*(ont-))))*.5; % ggorn l confgurzone de gunt ttule sng=sng+; % cont ss n sngolrtà elsef dst(,f)>=scelterr qd=qd+(lh*(jct')*(ont-))*.5; %METODO DEL GRADIENTE ngrd=ngrd+; % cont ss lontno d sngolrtà qundo è usto l grdente elsef dst(,f)<scelterr qd=qd+(nv(jct)*(s+(k*(ont-)))*.5); % METODO DI NEWTON nnewt=nnewt+; % cont ss lontno d sngolrtà qundo è usto Newton end

17 Pnfczone delle trettore Abbmo or tutt gl strument er mostre e rsolvere l comto del mnoltore. L obettvo dell nfczone d trettore è quello d rodurre rferment che sscurno l'esecuzone d rte dell'end-effector delle trettore secfcte. Quest rferment consstono n un sequenz temorle de vlor ssunt dll funzone scelt come trettor. L trettor uò essere convenentemente scomost ne due ngress ercorso: luogo de unt dello szo de gunt o dello szo oertvo che l mnoltore deve descrvere er esegure l movmento ssegnto legge orr: funzone che esrme lo szo ercorso dl mnoltore l vrre del temo (legge lnere, cubc, olnomle d grdo n, slne...) rtre dl temo nzle. L nfczone uò essere esegut n due mod: ) nello szo oertvo (rresentzone crtesn) ercorso ssegnto ll end-effector ) nello szo de gunt ercorso ssegnto gunt second dell scelt s ossono vere ro e contro: ) er l nfczone nello szo oertvo, s ossono secfcre un certo numero d rmetr qul d esemo unt estrem, unt ntermed e cmmno d esegure. Per qunto rgurd l legge d moto, ossono essere secfct l temo d ercorrenz, veloctà e ccelerzone mssm. Quello che verrà o generto dll'lgortmo dovrà essere rortto, er l controllo del robot, nello szo de gunt trmte le nverson. In questo contesto ossono essere ffrontt e rsolt meglo roblem qul l resenz d ostcol che ltrment srebbero dffcl d defnre (wnderng) e sorttutto vsulzzre; ) er l nfczone nello szo de gunt s h un mggore fcltà nel rconoscere e trttre le sngolrtà, n bse d un rdotto numero d rmetr. Anlzzmo dunque n dettglo l moto "unto-unto" n entrmb cs: ) nel cso dello szo oertvo s vuole che l'end-effector segu un ercorso redefnto n lne rett ortndos d un unto nzle d un unto fnle f entro un ntervllo d temo refssto. Consdermo llor l segmento che congunge due unt nello szo e che vrà come rresentzone rmetrc: s ( s) = + ( f ) l legge orr utlzzt s = s() t è l legge orr trezodle (n veloctà ) o "bng-costbng" (n ccelerzone) comost d tre fs d moto, nseme ll qule vene mosto che le veloctà nzl e fnl devono essere nulle, trtt d ccelerzone e decelerzon costnt e n modulo ugul, e l trtto ntermedo deve vere veloctà costnte (fgur successv). f

18 mx t vmx vmxt s( t) = mx ( t T ) mx t [, T ] s t [ T, T T ] + v mx s v T s mx mx t [ T T, T ] Questo to d legge vene ust nell rtc ndustrle erché ermette d oter morre veloctà e ccelerzon n modo comtble con le crtterstche fsche del mnoltore (nelle smulzon llegte ll tesn è ossble sceglere nche un semlce roflo lnere). Nello schem blocch utlzzto er l smulzone l s srà most n veloctà, dunque ttrverso l su dervt nltc. Ecco l mlementzone de blocch reltv ll nfczone nello szo crtesno. Rcordmo che l trettor è scomost n ngress: trezo.m % Relzz l roflo d veloctà trezodle er l % nfczone d trettore nello szo crtesno. % In ngresso rende "cont"=temo dto dl clock n Smulnk; % n usct s=szo ercorso n scss dervto l temo t=cont functon [s]=trezo(cont); globl vmx mx f tme s % s è globle erchè utlzzto non solo come usct drett verso l blocco ercorso, m nche come rmetro nell cnemtc dfferenzle % tme=temo totle del roflo Ts=temo d slt cont è l vrble temo Ts=vmx/mx; % Intervllo d ccelerzone/decelerzone=temo d slt f ((cont>=) & (cont<=ts)) % FASE DI ACCELERAZIONE s=mx*cont; % dervt nltc d s=(mx*(cont^))/; end f ((cont>=ts) & (cont<=(tme-ts))) % FASE A VELOCITA' COSTANTE s=vmx; % dervt nltc d s=(vmx*cont)-((vmx^)/(*mx)); end f ((cont>=(tme-ts)) & (cont<=tme)) % FASE DI DECELERAZIONE s=-mx*(cont-tme); % dervt nltc d s=-(mx*((cont-tme)^)/)+(vmx*tme) end -((vmx^)/mx); lnere.m % Relzz l roflo lnere er l nfczone % d trettore nello szo crtesno % n ngresso rende "cont"=temo dto dl clock % n usct s=szo ercorso n scss dervto l temo t=cont functon [s]=lnere(cont); globl tme L s; f cont<=tme s=l/tme; %L=dstnz tr due unt clcolt nel mn rogrm else s=; end ercorso.m % Quest funzone clcol l trettor d movmento del R tr % confgurzone nzle e fnle n lne rett secondo l % roflo fornto dll legge orr (s uò sceglere un qulss % roflo scrvendo un funzone ost che vd n ngresso ercorso) % In ngresso rende "s"=roflo fornto dll funzone trezo o ltr % doo essere stto ntegrto (nftt l roflo fornsce s); % n usct usct fornsce "ont"=unto clcolto n coordnte crtesne; % notre che l funzone ercorso mlctmente dende dl temo % ttrverso l'ngresso s (dendente d cont nell smulzone ftt n smulnk) % er cu clcol le coordnte dell'end-effector l temo t=cont % n bse l roflo ssegnto. functon [ont]=ercorso(s) globl f tme ont; norm=dst(,f); ont=+(s*(f-)/norm); s st () st () st ()

19 ) In questo cso s vuole che l mnoltore s muov d un confgurzone nzle d un fnle entro un temo refssto. l ercorso crtesno seguto dll' end-effector non è d nteresse rmro, mentre è fondmentle che gunt rtno e rggungno le confgurzon rcheste. Nell nostr tesn è rchest l lczone d un roflo d veloctà quntc gunt, esress dunque dll seguente funzone olnomle: 5 4 q ( t) = 5t + 4t + t + t + t +. Per rmetr sceglmo vlor tcmente utlzzt nelle lczon dell robotc n bse l rocedmento llustrto d seguto: ) monmo le se condzon reltve due unt q e q q ( ) = q q ( ) = v q ( ) = q ( ) = q q ( ) = v q ( ) = ) sclmo l rgomento dell funzone q, coè t, trmte un vrble τ := t / T n modo che l vrzone d t [, T ] s roorzonle quell d τ [,] ; quest scltur srà svolt dll funzone convtme.m n modo elementre; ) ossmo dunque scrvere l q n funzone d τ, consderndo le se condzon moste l unto e qund rortre l tutto n un unc esressone comlet τ q( τ ) = ( τ ) [ q + (q + v) τ + ( + 6v + q) ] + ( τ ) + τ [ q + (q v )( τ ) + ( 6v + q) ] 4) rtcolrzzmo l esressone sceglendo l cso notevole v = v = = = d cu, er q = q q, ottenmo l esressone rsultnte: q( τ ) q 5 4 = 6τ 5τ + τ q [,] Possmo fnlmente ntrodurre l funzone che mlement l quntc. È ovvo questo unto che l temo n ngresso non srà drettmente l temo d Smulnk m lo stesso sclto ttrverso l recedente funzone convtme : quntc.m % Relzz l roflo con legge quntc er l comndo lvello de gunt. % L'ngresso lmbd è l temo sclto second del temo totle dell'oerzone functon [Q]=quntc(lmbd) globl q qf Q; % confgurzone d rtenz e d'rrvo f lmbd<= q=q+((qf-q)*(6*((lmbd)^5)-(5*(lmbd)^4)+(*(lmbd)^))); else q=q; end Q=[q() q() q()]'; % oszon de gunt clcolte l temo lmbd Poché lo schem che useremo er l nversone nello szo de gunt revede d morre l roflo n veloctà, oltre d un correzone sull oszone, ntroducmo nche l qurtc, che è l dervt nltc dell quntc (che clcolmo uttosto che ntegrre l qurtc): qurtc.m % qurtc: è l dervt nltc dell quntc er l roflo % d ssegnre nello szo de gunt. functon [Qd]=qurtc(lmbd) globl q qf Qd; qd=(((qf-q)*(*((lmbd)^4)-(6*(lmbd)^)+(*(lmbd)^)))); % qd=qurtc l temo t (d=dot) Qd=[qd() qd() qd()]'; % Qd=vettore q unto (dot) delle veloctà de gunt.

20 Ecco dunque come ncstonmo blocch-funzone vst snor rssumendo l tutto ne due schem Smulnk reltv due comt del mnoltore ( blocch rchmno le funzon Mtlb): ) Assegnzone d trettor rettlne nello szo crtesno (con nversone dfferenzle) Vene generto, ttrverso un clock, l temo utlzzto dll funzone trezo.m (gllo) che gener l legge orr s (t) l qule è elbort dll funzone ercorso.m (ros) dndo luogo rferment (s) er l end-effector. Po vlor vengono nvertt con l cndffnv (verde) e sst ll s-functon d nmzone nm che rcostrusce l mmgne del robot n bse ll confgurzone stntne de gunt. I qudrtn ll estrem destr sono gl scoe d Smulnk, vsulzzno n form d grfco bdmensonle le roezon de vlor vettorl rcevut: fcendo doo clc sor cscuno d ess, s durnte, s ll fne dell smulzone, s re un fnestr ost che mostr l ssre del temo l ndmento delle vrbl osservte. D defult sono ert, n fnestre ntorno llo schem Smulnk, quell colort sfondo nero. Il blocco ntegrtor ntegr l ngresso nel temo rtre dll condzone nzle secfct (zero). È ossble sceglere qulss roflo d veloctà er un ercorso rettlneo semlcemente fcendo doo clc sul blocco gllo (legge orr) e scrvendo d esemo l osto d trezo, l nome dell funzone che mlement l roflo desderto: no dsonmo de rofl trezodle e lnere m è ossble scrvere ltr rofl d utlzzre ll nterno dell crtell del smultore e utlzzrl nel modo segto (le funzon sono *.m). Lo schem Smulnk con l roflo lnere.

21 Adesso resentmo un flmto n cu è stto mosto l roflo d veloctà crtesn trezodle lungo un ercorso n lne rett che mostr cos f l smultore vvto con seguent dt: l = l = l = 5, unto nzle = [4 8.5] T, = [ 7 ] T, t = 4s, mx = 5 : vene erto un breve flmto v comresso con codec XvD MPEG4, che è ossble nstllre ll vvo dell utorun del CD-ROM oure d qu clc). Un volt nstllto l codec srà ossble vsulzzre l flmto. f XvD_Instll.exe (fre doo Tle nstlltore s trov sotto l crtell Instller del CD-ROM e uò essere vvto semre fcendov sor doo clc. Se s refersce non nstllre questo comonente (freewre sotto lcenz GNU) è ossble nstllre ltr codec comtbl cere, ltrment l flmto non srà vsble meno che un codec comtble con l formto XvD non s gà nstllto sul sstem oertvo n uso. Rortmo sotto un sequenz reltv d un ltr smulzone nel cso non s ossble vsulzzre l flmto, er vere un de delle smulzon: L fnestr con l robot è nmt dll funzone nm resente nello schem Smulnk d cu sor. Lo schem Smulnk è stto creto con rsoluzone vdeo 4x768 n modo d remre qus tutto lo schermo un volt vvto, er cu è ossble che usndo dfferent rsoluzon vdeo le fnestre rsultno sostte rsetto lle loczon redefnte e che s corno vcend, mentre ll rsoluzone 4x768 ogn volt che l smultore srà vvto le fnestre rendernno l osto gusto mssmzzndo l leggbltà. È noltre ossble esegure cnque demo mmedte nel crtesno con tutt dt gà ront: ) ercorso lungo ) regmento nell orgne ) lungo z 4) ttrverso z 5) errore oltre WS

22 ) Assegnzone d trettor quntc nello szo de gunt Vene generto, ttrverso un clock, l temo t, che vene sclto trmte l funzone convtme second dell ntervllo tme secfcto come l durt totle del comto. Dunque l nuovo temo λ = t' = t/ tme vene utlzzto dll funzone quntc e qurtc.m (quest ultm è l dervt nltc dell quntc). Queste due generno rferment rsettvmente n oszone e veloctà er gunt. È noltre resente un sottrttore, che fornsce n usct lo scrto tr l confgurzone q d desdert l temo t e l confgurzone q ct stntne del mnoltore. Il motvo è che er v dell ntegrzone numerc s ottengono effett d derv che roducono un errore e tle errore vene recuerto ttrverso un semlce gudgno osto n ngresso d un sommtore, nseme ll veloctà stntne q d most dll qurtc. Fcendo doo clc sullo scoe errore d oszone de gunt è ossble osservre come durnte l smulzone l errore ument mn mno che t s vvcn t / (vlore er cu tle errore h vlore mssmo), er o decrescere fno llo zero l fne dell smulzone. È nfne ossble esegure ltre tre demo nello szo de gunt con tutt dt gà ront: ) estensone dll orgne ) elevzone rotnte ) nuove dmenson Il roblem dell nzlzzzone Come vedremo nell Aendce B, l nterfcc del smultore ermette, s nel cso dell nfczone d trettore nello szo oertvo, s qundo s lvor nello szo de gunt, d ssegnre l co (, f ) - oszone nzle e oszone fnle oure l co ( q, q f ), coè confgurzon nzle e fnle de gunt. Al d là dello szo n cu è defnt l trettor, se è ssegnt l rm co s vrà l seguente roblem: con qule delle ossbl soluzon nzlzzre l confgurzone del robot? Inftt s è vsto che utlzzndo l nvers nltc ( nvnltc.m ) s hnno n usct ù vlor er s er s er f, dto che se esstono soluzon sono lmeno due, er cu serve un ltr funzone che selezon tr le vre soluzon quell mglore secondo un crtero. Abbmo rgonto così: rendmo tr le ossbl coe ( q, q f ) quell che mnmzz l dstnz nello szo de gunt dq (, q f ), clcolt secondo l norm euclde. Chrmente è un metodo non molto sofstcto, m roduce scurmente rsultt buon rsetto llo sceglere l rm soluzone che ct er ognuno de due unt: lo bbmo consttto sermentlmente er molt cs. Tle clcolo è svolto dll funzone nvsceltnormmnm che sfrutt nche l ftto che l nvnltc rcerc soluzon nche n cso d sngolrtà, selezonndo le mglor tr le nfnte. Rggrumo or nell Aendce A quest funzone con ltre funzon d oggo del smultore.

23 Aendce A: Funzon d oggo del smultore nvsceltnormmnm.m % Quest funzone non rtene ll'nterfcc con l'utente, n qunto l'utente non % l utlzz drettmente. Vene rchmt solo se l'utente secfc % oszone nzle e fnle, nzchè confgurzon de gunt nzle e fnle. % In generle serve mglorre l scelt er l'nzlzzzone del robot % n qunto trov tr le soluzon (che ossono essere nfnte n sngolrtà) % er due unt, fornte dll nvnltc ttrverso l mn, quelle mglor % secondo l crtero che mnmzz l dstnz delle due confgurzon corrsondent. % Per fre cò rende n ngresso e f, ed noltre ccurcy che è l vlore d recsone % d ssre ll funzone erchè su volt lo sserà ll funzone nvnltc. % L segzone d questo rmetro uò essere ottenut dgtndo 'hel nvnltc'. % Vengono fornte 4 uscte: % messge = è un strng d testo che rort role cò che l funzone nlzz % rgurdo esstenz, numero, to d soluzon. In tlun cs esso è % ndrzzto dl 'mn' (o dll 'smulzone' second d come s vv) % ll'utente er segre d esemo che dt mmess non sono vld. % vettorenorme = è un vettore d dmensone vrble second del to e del numero % d soluzon trovte er due unt e f, e rort % tutte le dstnze nello szo de gunt reltve lle coe ossbl. % chrmente l contnutà delle nfnte soluzon n lcune sngolrtà % è dscretzzt roro ttrverso l rmetro ccurcy (v.nvnltc) % q,qf = sono le due confgurzon "mglor" clcolte ll fne. functon [messge,vettorenorme,q,qf]=nvsceltnormmnm(,f,ccurcy) % l codce dell funzone è strettmente legto l codce dell nvnltc, n qunto l form % delle mtrc delle soluzon che deve l rm deve gestre dende roro d come vengono % fornte dll second, er cu s consgl d rre entrmbe le funzon e consultrle % nseme. test.m % quest funzone controll se l unto n ngresso % s trov o meno ll'nterno dello szo d lvoro % del robot R e one l'usct ugule n cso ffermtvo % ltrment. functon controllunto=test() globl l l l; controllunto=; PWx=(); PWy=(); PWz=(); f sqrt(pwx^+pwy^+pwz^)>l+l... se l unto è troo lontno dll'orgne e suer l somm de brcc l,l; oure se (l<l & (sqrt(pwx^+pwy^+pwz^)<l-l)) (l>l & (sqrt(pwx^+pwy^+pwz^)<l-l)) % unto oltre l confne nterno del WS (l<>l) controllunto=; else controllunto=; end; dst.m % dst = clcol l dstnz tr due unt n uno szo tre dmenson (norm euclde) functon [d]=dst(,) d=sqrt(((()-())^)+((()-())^)+((()-())^)); grrd.m % Conversone d grd rdnt: dto n ngresso uno sclre o un vettore % contenente vlor n grd, l funzone resttusce vlor n rdnt functon rdnt=grrd(grd); rdnt=grd*/8;

24 Segue l funzone che dsegn n D l mnoltore stnte er stnte, n bse lle confgurzon stntnee q (t) ottenute d clcol, con le modfche che bbmo ortto rsetto ll'orgnle (d Flvo Cell) evdenzte e commentte n rnco: nm.m functon [sys,x]=nm(t,x,u,flg,ts); % Anmzone del robot R ntroomorfo. globl Robot Anmto VsulzzCmone CmonTrettor PoszoneEndEffector l l l f; NomeFgur='Anmzone Robot R'; NumeroCmonDSltre=; %(MxSteSze=.5) % osto er rresentre tutt cmon x=mx(bs()+bs(f)); % x è un fttore d correzone che tene conto dell dstnz tr unto f flg==, % Vsulzzzone grfc. nzle e unto fnle f ny(get(,'chldren')==robotanmto), f strcm(get(robotanmto,'nme'),nomefgur), set(,'currentfgure',robotanmto); hndl=get(gc,'userdt'); % Or clcol l cnemtc drett (u=vrbl d gunto). L = Lunghezz del brcco -esmo (=,,; L=). Ox,Oy,Oz = Coordnte dell'orgne d SR (=,,,). L=l/x; L=l/x; L=l/x; % le lunghezze de brcc vengono rdotte o mlfcte Ox=; Oy=; Oz=; n roorzone ll dstnz de due unt Ox=; Oy=; Oz=L; s=sn(u()); c=cos(u()); =L*cos(u()); =+L*cos(u()+u()); Ox=*c; Oy=*s; Oz=L+L*sn(u()); Ox=*c; Oy=*s; Oz=Oz+L*sn(u()+u()); % Adesso vsulzz l robot nell confgurzone ttule. x=x*[ox Ox NN Ox Ox NN Ox Ox NN -.. NN ]; y=x*[oy Oy NN Oy Oy NN Oy Oy NN NN -..]; z=x*[oz Oz NN Oz Oz NN Oz Oz NN NN ]; set(hndl(),'xdt',x,'ydt',y,'zdt',z-l); % l quot è corrett consderndo che % Qund vsulzz un cmone dell trettor. l tern d rfermento è sul ntone VsulzzCmone=rem(VsulzzCmone+,NumeroCmonDSltre); f (VsulzzCmone==), CmonTrettor=[CmonTrettor [(Ox)*x; (Oy)*x; (Oz)*x-l]]; set(hndl(),'xdt',cmontrettor(,:),... % nche l trettor vene end drwnow; % Aggorn l fgur end end PoszoneEndEffector=[Ox Oy Oz]; sys=[]; elsef flg== sys=poszoneendeffector; 'YDt',CmonTrettor(,:),... % sclt n roorzone lle 'ZDt',CmonTrettor(,:)); % nuove dmenson del robot elsef flg==4 % Rtorn stnte rossmo cmone. ns = t/ts; sys = ( + floor(ns + e-*(+ns)))*ts; elsef flg==, % Inzlzz l fgur. [exst,robotanmto]=fgflg(nomefgur); f ~exst, oston=get(,'defultfgureposton'); oston(:4)=[5 ]; oston(:)=[666 59]; - ottmzz l leggbltà 4x768 RobotAnmto=fgure(... 'Nme',NomeFgur,... 'NumberTtle','off',... 'BckngStore','off',... 'Poston',oston); xes(... 'Unts','normlzed',... 'Poston',[ ],... 'Vsble','on',... 'DrwMode','fst'); end; cl reset; set(gc,'drwmode','fst'); VsulzzCmone=-; CmonTrettor=[]; xs([-(x+l) (x+l) -(x+l) (x+l) -(x+l) (x+l)]); % gl ss sono sclt secondo % Fss gl ss e l mntene ttv. % l orzone d szo eslorto vew(9-7.5,); % durnte l ercorso hold on; % Chm rocedure grfche er nzlzzzone oggett (robot e trettor). hndl=lot([],[],[],'ersemode','bckground','lnewdth',); hndl=lot([],[],[],'ersemode','bckground','lnewdth',,'color','mgent');% dstngue set(gc,'asectrto',[ ]); l trettor dl robot set(gc,'userdt',[hndl hndl]); sys=[ ]; x=[]; xlbel('x'); ylbel('y'); zlbel('z'); grd on; % mostr un grgl che fclt l vsulzzzone de unt nello szo crtesno end;

25 Aendce B: L nterfcc con l utente e l vvo del smultore Il Smultore uò essere vvto dl seguente lnk: START HERE! Vsto che otrebbe non funzonre er motv gà segt, sfruttre questo collegmento non è l unco modo d vvre l smultore, dto che uò essere nstllto e vvto mnulmente d Mtlb ndendentemente d questo documento. Il CD nftt dsone d un utorun che se ttvto ermette con un resentzone n Power Pont vre ozon tr cu l nstllzone/rmozone del smultore e l vvo dretto s d CD s d hrd-dsk un volt nstllto. È ossble consultre l fle leggm se vete erto questo documento senz essere sst er l schermt d utorun. Dcmo che un volt nstllto, s mnulmente s utomtcmente, l smultore su dsco rgdo, srà suffcente dgtre mn l romt er fre rtre l smulzone ssstt. Il fle mn è l rogrmm rncle m non rresent l unc ossble modltà d mmssone de dt. Inftt sono redsost ltr due fle dove l utente uò crcre tutt dt secfc n modltà non nterttv, o slvre e qund vvre l smulzone. I due fle sono dtr.m e smulzone.m. Il rmo contene le crtterstche descrttve del mnoltore, mentre l secondo dt secfc sul comto d esegure. Crct quest dt e slvt fle le ossbltà sono : vvre l mn e secfcre che sno utlzzt que fle d dt (è un tr le ozon rooste dl mn ), oure nessuno o uno solo de due cere e crcre gl ltr dt mnulmente n modo nterttvo. È un modo er rendere molto elstc l nterfcc con l utente che utlzzerà l smultore come refersce non en cqust un o d fmlrtà. Un ultm ossbltà è quell d by-ssre comletmente l mn rendendo eseguble l fle smulzone.m. Mtlb nftt ermette d mettere commento lcune rt d rogrmm che non vengono lette se c è l smbolo % dvnt (n tutte le funzon vste snor le rt descrttve n verde erno recedute d tle smbolo). No bbmo scrtto n fondo l fle smulzone.m delle rghe d codce, che d defult sono messe commento e che, se rmesse d comment struzon, costturnno l codce eseguble smle quello del fle mn, mentre l rm rte descrve l comto. In questo cso verrnno rchmte le nformzon sul robot contenute nel fle dtr, vsto che non ossono essere mmesse n modo nterttvo. Per toglere l commento lle rghe è suffcente selezonrle con l mouse, fre un clc con l destro sull selezone e sceglere l voce uncomment, qund slvre. Per esegure un fle mtlb (che s eseguble) s uò remere F5 un volt erto, oure dl romt dell commnd wndow s uò dgtrne l nome. Per tutte le funzon del smultore sono redsost degl hel che comono dgtndo hel nome dove nome è l rte del nome del fle che recede l estensone.m. Esemo: hel mn (mn.m).