Gabarit Rapport Projet3-Robbotique

download Gabarit Rapport Projet3-Robbotique

of 16

Transcript of Gabarit Rapport Projet3-Robbotique

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    1/16

    GPA546 : Projet 3

    Cinématique d’un robot

    par

    Équipe No. Nom! Prénom "$Nom%! Prenom% "$Nom3! Prénom3 "$

    &ontréa'! 'e

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    2/16

    (A)*+ ,+- &A(/0+-

    1 INTRODUCTION..........................................................................................................................1

    2 CINÉMATIQUE DIRECTE...........................................................................................................1

    2.1 Référentiels D-H................................................................................................................12.2 Tableau es !ara"#tres D-H..............................................................................................22.$ Matri%es D-H.....................................................................................................................22.& Matri%e C...........................................................................................................................$2.' Matri%e T&..........................................................................................................................&2.( Matri%e )* ..........................................................................................................................&2.+ É,uatin e la %iné"ati,ue ire%te....................................................................................&2. É,uatins !ur éter"iner la !se e l/effe%teur...............................................................&

    $ CINÉMATIQUE IN0ERE...........................................................................................................'

    $.1 a "atri%e 3 en fn%tin e la !se e l/effe%teur.............................................................'

    $.2 es !re"i#res u4e é,uatins..........................................................................................($.$ Cal%ul e la 5ariable arti%ulaire 1.......................................................................................($.& Cal%ul e la 5ariable arti%ulaire 2.......................................................................................+$.' Cal%ul e la 5ariable arti%ulaire $.......................................................................................+$.( Cal%ul e la 5ariable arti%ulaire &.......................................................................................+

    & 0ÉRI6ICATION A0EC RO7OTTUDIO.....................................................................................+

    &.1 Tableau8 %"!aratifs es résultats.....................................................................................&.2 Dis%ussins sur les résultats...............................................................................................9

    ' CONCUION...............................................................................................................................9

     !a:e 1

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    3/16

    *-(+ ,+- 1G20+-

    6i:ure 1. %;é"a u rbt < & l a5e% tus les référentiels........................................................................26i:ure 2. Ca!ture /é%ran e la statin Rbttui u rbt.....................................................................

     !a:e 2

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    4/16

    *-(+ ,+- (A)*+A2

    Tableau 1. =ara"#tres D-H !ur le rbt illustré < la fi:ure 1.....................................................................2Tableau 2. Résultats !ur la %iné"ati,ue ire%te.........................................................................................Tableau $. Résultats !ur la %iné"ati,ue in5erse........................................................................................9

     !a:e $

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    5/16

    ntrodu#tion

    Dans %ette se%tin> 5us e5e4 !résenter le rbt et les anal?ses ,ue 5us alle4 effe%tuer. Mentinne4é:ale"ent %""ent 5us aller 5alier 5s résultats.

    =artut ans %e ra!!rt> 5us e5e4 utiliser la bnne ter"inl:ie> nta""ent @• variables articulaires

    • variables cartésiennes Bu la pose de l’effecteur 

    • angles d’Euler selon la convention xyz 

    • degrés de liberté Babré5ié ddl 

    • référentiel   Bu repère> u  système d’axes %""e ans référentiels  D-H > référentiel de l’outil >référentiel de la base> u référentiel de l’atelier 

    • articulation prismatique

    • articulation rotoïde.

    0us e5e4 é%rire> !ar e8e"!le> ,ue la "atri%e ;":#ne Ai  re!résenté la !se u référentiel i  !ar ra!!rt au référentiel i  1. BNte4 l/utilisatin u s?"ble F !ur ési:ner le si:ne "ins> !lutGt ,uel/usa:e in%rre%t "ais ré!anu u trait /unin - F. 0us e5e4 utiliser le ter"e  position %rre%te"ent

    et nn %""e substitut !ur le ter"e  pose. e ter"e axe !eut si:nifier ifférentes %;ses B%""e ans a8e e rtatin F u l/a8e x F> "ais !as 5rai"ent articulation. On !eut se !er"ettre e ire rbt <

    ,uatre a8es F> !lutGt ,ue rbt < & l F u rbt a5e% ,uatre arti%ulatins F> "ais !as un a8e

     !ris"ati,ue F. Aussi> une arti%ulatin rte a un a8e Be rtatin> "ais une arti%ulatin !ris"ati,ue a

    seule"ent une direction.

    % Cinématique dire#te

    %et enrit> 5us e5e4 e8!li,uer %e ,u/est la %iné"ati,ue ire%te et les ifférentes éta!es < sui5reB%;isir les référentiels D-H> tru5er les !ara"#tres D-H> et%..

    %. 0éérentie' ,7

    Dans %ette se%tin> 5us e5e4 !résenter le s%;é"a u rbt a5e% 5s référentiels D-H> ans la fi:ure 1.0us n/a5e4 !as < Justifier l/e"!la%e"ent e 5s référentiels. =ar %ntre> essine4 5s référentiels a5e%

     beau%u! e sin> e8a%te"ent au bn enrit B!ar e8e"!le> les a8es  z  i5ent Ktre essinés sur les li:nes !intillées ési:nant les a8es e rtatin> et nn < %Gté. Enfin> 5us e5e4 ,uan "K"e réi:er une

     !;rase ,ui intruit la fi:ure 1> !ar e8e"!le es référentiels D-H ,ue nus a5ns %;isis snt "ntrés

    < la fi:ure 1 F.

    É5ite4 e référer < l/e"!la%e"ent /une fi:ure Bu /un autre élé"ent tel ,u/une é,uatin u un tableau%""e ans la fi:ure %i-essus F> %ar %et e"!la%e"ent !eut %;an:er !ar la suite.

     !a:e 1L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    6/16

    6i:ure 1. %;é"a u rbt < & l a5e% tus les référentiels.

    %.% (ab'eau de param8tre ,7

    Dans %ette se%tin> 5us e5e4 !résenter les !ara"#tres D-H ,ui %rres!nent < 5tre %;i8 e

    référentiels D-H "ntré < la fi:ure 1. N/ublie4 !as /in%lure les 5aleurs e %"!ensatins au8

     !ara"#tres D-H ,ui %rres!nent au8 5ariables arti%ulaires B!ar e8e"!le> θ 1  9 F. 0us n/a5e4 !as

     besin /e8!li,uer %""ent 5us a5e4 btenu %es 5aleurs> "ais il faut ,uan "K"e réi:er une !;rase !ur intruire le tableau es !ara"#tres D-H.

    Tableau 1. =ara"#tres D-H !ur le rbt illustré < la fi:ure 1.

    i  θ i d i ai   α i

    1   θ 1  9 '& P9

    2

    $

    &

    %.3 &atri#e ,7Dans %ette se%tin> 5us e5e4 !résenter les "atri%es Ai. Il faut réi:er une !;rase !ur intruire %es"atri%es> !ar e8e"!le En utilisant la fr"ule !ur la "atri%e Ai et les !ara"#tres D-H "ntrés ans letableau 1> nus arri5ns < @ F. Nte4 ,u/un ra!!rt te%;ni,ue %""e %elui-%i n/est !as un bruilln @ il neit !as se réuire < une suite /é,uatins. Il faut relier les é,uatins !ar es %""entaires.

    Il est tr#s i"!rtant e res!e%ter les nr"es t?!:ra!;i,ues ans l/éitin /é,uatins et /utiliser l/éiteur /é,uatin Mat;T?!e> si !ssible> u %elui ,ui 5ient a5e% M r. es é,uatins fnt !artie u

     !a:e 2L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    7/16

    te8te et i5ent s/? in%lure %""e tut autre élé"ent. Elles i5ent Ktre réi:ées a5e% la "K"e !li%e etla "K"e taille ,ue le te8te u !résent !ara:ra!;e> sit Ti"es NeS R"an> < 11 !t> !ur les s?"bles e

    l/al!;abet latin Btels ,ue A> d i! ai  et a5e% la !li%e ?"bl> < 11 !t> !ur les s?"bles :re%s Btels ,ue θ i

    et α i. Mal;eureuse"ent> il n/est !as !ssible e %;an:er la !li%e es é,uatins ans l/éiteur 

    /é,uatins e M r. 0i%i une es n"breuses raisns en fa5eur e l/éiteur Mat;T?!e.

    =eu i"!rte l/éiteur /é,uatins ,ue 5us utilise4> il faut é%rire les é,uatins e fan !rfessinnelle et

    nn %""e s/il s/a:it e %e Matlab. 0i%i ,uel,ues e8e"!les t?!i,ues @

    • é%ri5e4 & 1 2 $ &=( A A A A  et nn T& CVA1VA2VA$VA&V)*

    • é%ri5e4

    21

     p

    c u 21  p

    c et nn 21

     ÷  p c

    • é%ri5e4(   )2 2$>1 1>1 2>1atan 2 >− + x x x

     et nn2 2

    $>1 1>1 2>1tan 2B > B CC− +a x sqrt x x .

    On é%rit les é,uatins ,ui snt "ins i"!rtantes et asse4 %urtes ire%te"ent ans le te8te> %""e ans

    l/e8e"!le sui5ant @ =uis,ue a x  1> les 5ariables a y et a z  ne !eu5ent Ja"ais Ktre é:ales < 4ér en "K"ete"!s F. =ur %es !etites é,uatins> n n/est !as bli:é /utiliser un éiteur /é,uatins> "ais %/estfrte"ent re%""ané afin /utiliser le "K"e st?le !ur tutes les é,uatins.

    On é%rit %;a,ue é,uatin i"!rtante u !lus ln:ue sur une nu5elle li:ne en %entrant l/é,uatin et en

    ali:nant < rite sn nu"ér entre !arent;#ses. es nu"érs ser5ent < B!ssible"ent faire référen%e <

    une é,uatin. e !ara:ra!;e sui5ant est l/e8e"!le < sui5re ans 5tre %as.

    En utilisant la fr"ule !ur la "atri%e D-H BAi et les !ara"#tres D-H "ntrés ans le tableau 1> nusarri5ns au8 "atri%es ;":#nes sui5antes @

    1 1

    1 11

    N N

    N NN 1 N '&N

    N N N 1

    − −

    − = −

     s c

    c sA

    > B1

    2

    N N N 1

    =

    L L L L

    L L L L

    L L L LA

    > B2

    $

    N N N 1

    =

    L L L L

    L L L L

    L L L LA

    > B$

     !a:e $L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    8/16

    &

    N N N 1

    =

    L L L L

    L L L L

    L L L LA

    > B&

    W 1 1sinθ = s > 1 1%-sθ =c > et%.

    %.4 &atri#e C

    a "atri%e ;":#ne C ,ui  re!résente  la !se u référentiel e la base Bréférentiel !ar ra!!rt auréférentiel e l/atelier est

    N N N 1

    =

    L L L L

    L L L L

    L L L LC

    . B'

    %.5 &atri#e (4

    En utilisant l/é,uatin & 1 2 $ &=( A A A A  et en re:ru!ant les ter"es en θ $ et θ & B< l/aie e la fn%tintCollect e ntre %al%ulatri%e TI nus arri5ns au résultat sui5ant @

    1 $&

    &

    N N N 1

    − =

    L L L

    L L L L

    L L L L

     s s

    (

    > B(

    W ( )$& $ &sin θ θ = +

     s  et ( )$& $ &%-s θ θ = +

    c .

    %.6 &atri#e G9 

    uite < ntre %;i8 !ur le référentiel & Bfi:ure 1> nus btenns @

    N N N 1

    =

    L L L L

    L L L L

    L L L L! G

    . B+

    %. Équation de 'a #inématique dire#te

    /é,uatin e la %iné"ati,ue ire%te nne la "atri%e ;":#ne ,ui éfinit la !se u référentiel e

    l/util !ar ra!!rt au référentiel e l/atelier. @

     !a:e &L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    9/16

    &

    N N N 1

    = =

    L L L L

    L L L L

    L L L L! 3 C( G

    . B

    %.; Équation pour déterminer 'a poe de '’ee#teur

    Une fis ,ue la "atri%e ;":#ne  a été %al%ulée> nus !u5ns tru5er les %rnnées e l/ri:ine uréférentiel e l/util !ar ra!!rt au référentiel e l/atelier en utilisant les tris é,uatins sui5antes @

    1>&= x x

    > B9

    2>&= y x

    > B1

    $>&= z x > B11

    W x1>& est le %effi%ient e la "atri%e  ,ui se tru5e < l/interse%tin e la li:ne 1 et e la %lnne &>  x2>&est le %effi%ient e la "atri%e  ,ui se tru5e < l/interse%tin e la li:ne 2 et e la %lnne &> et%.

    =ur %e ,ui est e la re!résentatin e l/rientatin u référentiel e l/util !ar ra!!rt au référentiel el/atelier> nus e5ns utiliser les tris an:les /Euler> ">  p  et r > seln la %n5entin utilisée ans la

    fn%tin OrientZYX(r,p,w)  u lan:a:e e !r:ra""atin RA=ID B%e ,u/n 5a a!!eler la

    convention xyz  u bien la convention zy’x#$% Dans %ette %n5entin> les tris rtatins snt effe%tuées ans

    l/rre sui5ant @

    1. rtatin e r  autur e l/a8e z >

    2. rtatin e p autur u nu5el a8e y>

    $. rtatin e " autur u nu5el a8e x.

    Ainsi> les fr"ules !ur tru5er une seule slutin !ur %es tris an:les> ans la !la:e XP1> 1Y> <

     !artir e la "atri%e  snt @

    ( )2 2$>1 1>1 2>1atan 2 >= − + p x x x> B12

    2>1 1>1atan 2 >

    %-s %-s

     =   ÷

     

     x xr 

     p p > B1$

    $>2 $>$atan 2 >

    %-s %-s

     =   ÷

     

     x x"

     p p > B1&

    ans le %as W $>1   1≠ ± x

    . Dans le %as !arti%ulier $>1   1= ± x

    > il ? a une sin:ularité e re!résentatin et nus

     !u5ns tru5er une slutin !ur les tris an:les en utilisant les fr"ules sui5antes @

    $>19N= − ° p x > B1'

    ( )$>1 2 >$ 2 >2atan 2 >= −r x x x > B1(

    N= °" . B1+

    Il est i"!rtant e nter ,ue les é,uatins B121& nnent seule"ent un es eu8 tri!lets /an:les

     !a:e 'L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    10/16

    /Euler !ssibles> alrs ,ue les é,uatins B1'1+ nnent seule"ent une !ar"i un n"bre infini eslutins. Ainsi> il est tut < fait !ssible ,ue les an:les /Euler ,ue nus allns tru5er !ur une

    %nfi:uratin nnée e rbt sernt ifférents B"ais é,ui5alents es an:les /Euler ,u/une autre é,ui!e5a tru5er> !ar e8e"!le.

    3 Cinématique inere %et enrit> 5us e5e4 e8!li,uer %e ,u/est la %iné"ati,ue in5erse et les ifférentes éta!es < sui5re

    Btru5er les !re"i#res u4e é,uatins> ientifier %elles ,ui 5us !er"ettent e résure au "ins

    %ertaines 5ariables arti%ulaires> et%.. 0us !u5e4 é:ale"ent ire si le rbt !eut a5ir !lus /une%nfi:uratin !ur la "K"e !se e l/effe%teur.

    3. *a matri#e en on#tion de 'a poe de '’ee#teur

    it  x>  y  et  z > les %rnnées e l/ri:ine u référentiel e l/effe%teur !ar ra!!rt au référentiel e

    l/atelier> et "> p et r > les an:les /Euler sui5ant la %n5entin  xyz  re!résentant l/rientatin u référentiele l/effe%teur !ar ra!!rt au référentiel e l/atelier. a "atri%e ;":#ne ,ui re!résente la !se u

    référentiel e l/effe%teur !ar ra!!rt au référentiel e l/atelier en fn%tin e %es si8 5ariables B x> y> z > "> pet r  est éfinie %""e

    %-s %-s %-s sin sin sin %-s %-s sin %-s sin sin

    sin %-s sin sin sin %-s %-s sin sin %-s %-s sin

    sin %-s sin %-s %-s

    N N N 1

    − + + − = −

    r p r p " r " r p " r " x

    r p r p " r " r p " r " y

     p p " p " z 3

    . B1

    =ur es raisns /es!a%e et e si"!li%ité> nus allns substituer les %effi%ients e la "atri%e  !ar es5ariables @

    N N N 1

    =

     x x x x

     y y y y

     z z z z 

    n o a pn o a p

    n o a p3

    . B19

    3.% *e premi8re dou=e équation

    En refr"ulant l/é,uatin e la %iné"ati,ue ire%te &=   ! 3 C( G  nus a5ns

    1 1

    &

    − −=   ! ( C 3G > B2W en substituant la "atri%e (& a5e% l/e8!ressin e l/é,uatin B(> la "atri%e   a5e% l/e8!ressin el/é,uatin B1> et%.> et en effe%tuant les %al%uls> nus arri5ns < l/é,uatin "atri%ielle sui5ante @

    1 $&

    N N N 1 N N N 1

    − =

    L L L L L L

    L L L L L L L L

    L L L L L L L L

     y s s a

    . B21

    Cette é,uatin "atri%ielle %nsiste en 12 é,uatins s%alaires ,ue nus e5ns "aintenant essa?er e

     !a:e (L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    11/16

    résure @

    1 $&− =

     y s s a

    > B22

    = > B2$

    = > B2&

    = > B2'

    = > B2(

    = > B2+

    = > B2

    = > B29

    = > B$

    = > B$1

    = > B$2

    = . B$$

    3.3 Ca'#u' de 'a ariab'e arti#u'aire

     Nus !u5ns utiliser les é,uatins B et B !ur tru5er  s1  et c1> et ensuite utiliser la fr"ule e

    t?!e & !ur tru5er la 5ariable arti%ulaire θ 1 @

    ( )1   atan 2 >θ   =   z yo o . B$&Il faut nter ,ue %ette é,uatin est tuJurs éfinie> %ar seln l/é,uatin B > o x  > %e ,ui 5eut ire ,ue

    les 5ariables o y et o z  ne !eu5ent Ja"ais Ktre é:ales < 4ér en "K"e te"!s B%ar2 2 2 2 2

    N+ + = + = x y z y z o o o o o .

    3.4 Ca'#u' de 'a ariab'e arti#u'aire %

    Mal;eureuse"ent> nus ne !u5ns tru5er au%une autre 5ariable arti%ulaire ire%te"ent < !artir es

    é,uatins B22$$. =uis,ue nus a5ns éJ< tru5é l/in%nnu θ 1> nus allns transférer la "atri%e A1 u

    %Gté rit @

    1 1 1

    2 $ & 1

    − − −=   ! A A A A C 3G > B$'%e ,ui :én#re les u4e nu5elles é,uatins sui5antes> W  s1  et c1 nt été substitué !ar o z   et o y>

    res!e%ti5e"ent @

    = > B$(

    = > B$+

    = > B$

    = > B$9

    = > B&

    = > B&1

     !a:e +L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    12/16

    = > B&2

    = > B&$

    = > B&&

    = > B&'

    = > B&(= . B&+

    !artir e l/é,uatin B > n éuit ire%te"ent l/e8!ressin !ur d 2 @

    2 = Ld  . B&

    3.5 Ca'#u' de 'a ariab'e arti#u'aire 3

    Dans %ette se%tin> 5us %ntinue4 ans le "K"e sens !ur tru5er la 5ariable arti%ulaire $.

    3.6 Ca'#u' de 'a ariab'e arti#u'aire 4

    Dans %ette se%tin> 5us %ntinue4 ans le "K"e sens !ur tru5er la 5ariable arti%ulaire &.

    4 érii#ation ae# 0obot-tudio

     Nus a5ns utilisé le "#le u rbt ,ui nus a été furni en tant ,ue statin Rbttui. a fi:ure 2

     !résente une es i8 %nfi:uratins ,ui nt été testées !ur 5alier ns é,uatins e la %iné"ati,ue

    ire%te et in5erse. a fenKtre ,u/n 5it < la fi:ure 2 "ntre la !se e l/effe%teur Bans l/rre>  x> y et  z >

    et ensuite> ">  p  et r  !ar ra!!rt au référentiel e l/atelier !ur la %nfi:uratin u rbt a5e%

    θ 1  P&2.29> d 2  2&$. ""> θ $  11(.+> et θ &  P2+.+.

    6i:ure 2. Ca!ture /é%ran e la statin Rbttui u rbt.

     !a:e L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    13/16

    4. (ab'eau? #omparati de réu'tat

    e tableau 2 énu"#re les résultats e la %iné"ati,ue ire%te btenus !ar ntre fn%tin

    Cinematique_Directe ans Matlab et %eu8 btenus ans Rbttui B5ir fi:ure 2. =areille"ent>

    le tableau $ énu"#re les résultats e la %iné"ati,ue in5erse btenus !ar ntre fn%tin

    Cinematique_Inverse ans Matlab et %eu8 btenus ans Rbttui.

    Tableau 2. Résultats !ur la %iné"ati,ue ire%te.

    0ariables arti%ulaires=se btenue ans Matlab L

    =se btenue ans Rbttuiθ 1

    XYd 2

    X""Yθ $

    XYθ &

    XY x

    X""Y y

    X""Y z 

    X""Y"

    XY p

    XYr 

    XY

    P&2.29 2&$. 11(.+ P2+.+1.$&&2e$

    1$&'.$1&(.$'2&($.1

    P1.112+e$P111&.'&

    P&2.29(P&2.&9

    P.2+$+P.22

    .2&9.2

     !a:e 9L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    14/16

    Tableau $. Résultats !ur la %iné"ati,ue in5erse.

    =se saisie ans Rbttui0ariables arti%ulaires saisies ans

    Rbttui L 0ariables arti%ulaires btenuesans Matlab

     x

    X""Y y

    X""Y z 

    X""Y"

    XY p

    XYr 

    XYθ 1

    XYd 2

    X""Yθ $

    XYθ &

    XY

    1(&(.2& 12.&1 P22'.'2 11+.(' 1.(9 1&.'('+.&

    '+.&$9P1'.(

    P1'.'(2$9.9

    P12.&1P(.&

    P(.$91

    4.% ,i#uion ur 'e réu'tat

    É5ie""ent> 5s résultats i5ent %n%rer a5e% les résultats btenus ans Rbttui. =ar %ntre> il ?

    aura es erreurs nu"éri,ues Baussi :ranes ,ue $ "" u .2 ainsi ,ue es ifféren%es e $( sur 

    %ertaines 5ariables arti%ulaires. En !lus> il se !eut ,ue la slutin es an:les /Euler ,ue Rbttui5us nne n/est !as la "K"e ,ue %elle ,ue 5us a5e4 btenue. Dans %e %as-l il faut s/assurer ,ue les

    eu8 tri!lets /an:les /Euler %rres!nent < la "K"e rientatin.

    5 Con#'uion

    Dans %ette se%tin> 5us e5e4 réi:er 5s %n%lusins. 0us !u5e4> !ar e8e"!le> %""enter le

     !rbl#"e /erreurs nu"éri,ues et la !ssibilité e ifférents tri!lets /an:les /Euler !ur la "K"erientatin. 0us !u5e4 aussi is%uter e la !r:ra""atin en Matlab. Enfin> 5us !u5e4 é:ale"ent

    "entinner si %e !rJet 5us a aié < %"!renre la "élisatin %iné"ati,ue /un rbt.

     !a:e 1L11

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    15/16

    ANN++ A : *-(NG ,2 P0@G0A&&+ CINEMATIQUE_DIRECTE.M 

    % Entrée les quatre varia!les articulaires "u ro!ot% #ortie $, , &, w, p, r "e '_outil par rapport au '_

    unction pose * Cinematique_Directe(t+eta, "-, t+eta., t+eta/)

    % Conversion "e "e0rés en ra"ians (car 1atla! onctionne en ra"ians)t+eta * 222"- * 222t+eta. * 222t+eta/ * 222

    % Calcul "es matrices 343 avec la onction "+ (Denavit 5arten!er0)4 * "+(2224- * "+(2224. * "+(2224/ * "+(222

    % 1atrice "e transormation CC * 222 % 1atrice "e transormation 6767 * 222

    % 1atrice +omo08ne qui "éinit la pose "u réérentiel '_outil% par rapport au réérentiel '_5 * C9494-94.94/967:

    % Calcul "e r, p, w (convention XYZ ";an0les ";Euler) i a!s(5(.,)) **   p * :  w * >: % la valeur "e w peut ?tre ar!itraire, mais on c+oisit w * >  r * atan-(Api: else  % il a "eu$ solutions "ans la pla0e B,@>  p * atan-(Api:  cp * cos(p9piA@>):  r * atan-(5(-,)Acp,5(,)Acp)9@>Api:  w * atan-(5(.,-)Acp,5(.,.)Acp)9@>Api: en"

     % ésultat pose2$ * 5(,/): pose2 * 5(-,/): pose2& * 5(.,/): pose2w * w: pose2p * p: pose2r * r: % 1atrice 4unction 4 * "+(t+eta,",a,t)4 * Bcos(t+eta),

  • 8/17/2019 Gabarit Rapport Projet3-Robbotique

    16/16

    ANN++ ) : *-(NG ,2 P0@G0A&&+ CINEMATIQUE_INVERSE.M 

    % Entrée $, , &, w, p, r "e '_outil par rapport au '_% #ortie les quatre varia!les articulaires "u ro!ot

    unction q * Cinematique_Inverse($,,&,w,p,r)

    % 1atrice +omo08ne qui "éinit la pose "u réérentiel '_outil% par rapport au réérentiel '_w * w9piA@>:p * p9piA@>:r * r9piA@>:5 * B cos(r)9cos(p), cos(r)9sin(p)9sin(w), :

    % Décomposition "e la matrice 5n$ * 5(,):n * 5(-,):n& * 5(.,):o$ * 5(,-):o * 5(-,-):o& * 5(.,-):

    a$ * 5(,.):a * 5(-,.):a& * 5(.,.):p$ * 5(,/):p * 5(-,/):p& * 5(.,/):

    % Calcul "e t+etat+eta * 222

    % Calcul "e "-"- * 222

    % Calcul "e t+eta.t+eta. * 222

    % Calcul "e t+eta/

    t+eta/ * 222

    % Conversion en "e0rést+eta * 222t+eta. * 222:t+eta/ * 222:

    % ésultatq2t+eta * t+eta:q2"- * "-:q2t+eta. * t+eta.:q2t+eta/ * t+eta/:

     !a:e 12L11