Nghiên cứu thiết kế, chế tạo các robot thông minh phục vụcho các ứng dụng quan trọng

åm cã 1 khíp quay R, 1 khíp tÞnh tiÕn P vµ 1 khíp cÇu S), th−êng ®−îc thiÕt kÕ ®Ó mang ph«i gia c«ng hay mang c«ng cô ®Ó gia c«ng. Ba ch©n víi chiÒu dµi cã thÓ thay ®æi ®−îc ®iÒu khiÓn bëi c¸c ®éng c¬ sÏ dÉn ®éng cho bÖ di ®éng mang ph«i hay c«ng cô chuyÓn ®éng theo quÜ ®¹o x¸c ®Þnh tr−íc. Hai ®Çu cña c¸c ch©n nµy ®−îc liªn kÕt víi ®Õ cè ®Þnh vµ bÖ di ®éng b»ng c¸c khíp cÇu. ¦u ®iÓm cña lo¹i R«-bèt nµy lµ khèi l−îng nhá, cÊu tróc gän nhÑ, ®é cøng v÷ng cao, cã 3 bËc tù do vµ ®é chÝnh x¸c cao. TÊt c¶ c¸c thµnh phÇn c¬ khÝ ®−îc lùa chän vµ thiÕt kÕ cµng nhá gän cµng tèt vµ kh«ng cã khe hë theo chiÒu däc trôc cña c¸c ch©n, c¸c ch©n ®−îc ®iÒu khiÓn cña R«-bèt ®−îc dÉn ®éng b»ng c¸c c¬ cÊu chÊp hµnh tuyÕn tÝnh. H×nh 2.1 m« t¶ s¬ ®å cña r«-bèt nµy. - Chi tiÕt 1 : Bµn di ®éng cã 3 bËc tù do trong kh«ng gian, trong tr−êng hîp cô thÓ ë ®©y lµ phÇn bÒ mÆt dïng ®Ó g¸ dông cô c¾t kim lo¹i (®Çu dao phay,...) hoÆc l¾p ®å g¸ ph«i (th−íc chia ®é, kÑp ph«i gia c«ng ...) cã d¹ng tam gi¸c (th−êng lµ tam gi¸c ®Òu). Trªn bµn di ®éng sÏ l¾p ®Æt c¸c lo¹i ®å g¸ ®Ó kÑp chi tiÕt hoÆc l¾p ®Æt côm ®éng c¬ - ®µi dao gia c«ng. Bµn ®−îc thiÕt kÕ cã c¸c lç, chèt ®Þnh vÞ ®Ó l¾p ®å g¸. §å g¸ ®−îc l¾p chÆt trªn bµn di ®éng b»ng c¸c bul«ng. 8 H×nh 4.2.1: C¬ cÊu chÊp hµnh song song 3 RPS - Chi tiÕt 2: Lµ mét phÇn cña chi tiÕt thanh tr−ît lång, èng tr−ît trong. TÊt c¶ c¸c èng tr−ît trong cã d¹ng thanh trô ®Æc. Khíp tr−ît ®−îc truyÒn ®éng b»ng c¬ cÊu chÊp hµnh sö dông ®éng c¬ servo, bé truyÒn ®éng vµ côm c¬ cÊu trôc vÝt - ®ai èc bi. C¸c ch©n cña R«-bèt ®−îc nèi víi bÖ di ®éng vµ ®Õ cè ®Þnh b»ng c¸c khíp cÇu 6. - Chi tiÕt 3 : Lµ mét phÇn cña chi tiÕt thanh tr−ît lång, èng tr−ît ngoµi. TÊt c¶ c¸c èng tr−ît ngoµi cã d¹ng h×nh trô rçng. - Chi tiÕt 4 : Khíp quay, nèi ch©n víi ®Õ cè ®Þnh. - Chi tiÕt 5 : MÆt ®Õ cè ®Þnh, cã d¹ng tÊm ph¼ng trßn. BÖ cè ®Þnh ®−îc l¾p ®Æt trªn bµn g¸ chi tiÕt cña m¸y phay hoÆc cã thÓ ®−îc l¾p ®Æt cè ®Þnh trªn mét vËt kh¸c. Trªn ®Õ cè ®Þnh cã gia c«ng c¸c lç phôc vô, viÖc cè ®Þnh ®Õ trªn bµn g¸ hoÆc c¸c vËt kh¸c b»ng c¸c bul«ng. Trªn ®Õ cè ®Þnh cßn ®−îc gia c«ng c¸c r·nh ®Þnh vÞ phôc vô c«ng t¸c c¨n chØnh, l¾p ®Æt r«-bèt. - Chi tiÕt 6 : Khíp cÇu, nèi ch©n víi bµn m¸y di ®éng. 2.1.2. BËc tù do Sö dông c«ng thøc: 9 1 ( 1) j i b i F n j f fλ = = − − + −∑ Víi 6, 8, 9, 0n j fbλ = = = = , ba khíp quay, ba khíp l¨ng trô, vµ ba khíp cÇu, ta cã: 6(8 9 1) (3.1 3.1 3.3) 0 3F = − − + + + − = Nh− vËy bµn di ®éng sÏ chuyÓn ®éng víi ba bËc tù do trong kh«ng gian. 2.1.3. HÖ trôc to¹ ®é Trªn h×nh 2.2 m« t¶ s¬ ®å ®éng häc cña r«-bèt song song 3 RPS B3 1B A1 A2 3A B2 O P z y x 1 1 2z 3z 0x x 3 z y x 2 α1 α2 α3 0 0 z x H×nh 4.2.2: S¬ ®å ®éng häc cña robot song song 3 RPS Do yªu cÇu cña kÕt cÊu Robot nªn AiBi ⊥Zi (c¸c trôc quay) O vµ P lµ träng t©m cña hai tam gi¸c A1A2A3 vµ B1B2B3. Ta ®Æt c¸c hÖ täa ®é: {Ox0y0z0} : HÖ cè ®Þnh. {Pxyz} : HÖ täa ®é ®éng g¾n liÒn víi bµn m¸y ®éng. 10 {Aixiyizi}(i=1,2,3) : HÖ ®éng g¾n víi ch©n thø i. Trong ®ã i i ix A B≡ uuuur vµ zi ≡ trôc quay, cßn yi x¸c ®Þnh theo tam diÖn thuËn (hay qui t¾c bµn tay ph¶i). Ta ®−a thªm vµo 3 täa ®é suy réng iα (i=1,2,3) nh− h×nh vÏ. 0i iz xα = Sö dông c¸c ký hiÖu: ARB : Ma trËn cosin chØ h−íng cña hÖ {Pxyz} so víi hÖ cè ®Þnh {Ox0y0z0}. ARi : Ma trËn cosin chØ h−íng cña hÖ {Aixiyizi} so víi hÖ cè ®Þnh {Ox0y0z0}. ia : Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm Ai trªn hÖ cè ®Þnh. ib : Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm Bi trªn hÖ cè ®Þnh. B ib : Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm Bi trªn hÖ ®éng. P: Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm P trªn hÖ cè ®Þnh. di : §é dµi ch©n thø i. Trong ®ã : C¸c ma trËn A iR cã thÓ biÓu diÔn d−íi d¹ng: A iR = 01 1 01 2 01 3 02 1 02 2 02 3 03 1 03 2 03 3 i i i i i i i i i e e e e e e e e e e e e e e e e e e ⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ r r r r r r r r r r r r r r r r r r (i=1,2,3) (2.1) 01 02 03, ,e e e r r r : Lµ 3 vector ®¬n vÞ trªn c¸c trôc Ox0, Oy0,Oz0. 1 2 3, ,i i ie e e r r r : Lµ 3 vector ®¬n vÞ trªn c¸c trôc Aixi, Aiyi, Aizi (i=1,2,3). C¸c phÇn tö cña ma trËn nµy tïy theo kÕt cÊu cña bµn ®Õ cè ®Þnh, lµ hµm cña gãc iα . 11 Ma trËn A BR cã thÓ biÓu diÔn d−íi d¹ng 3 phÐp quay Roll, Pitch, Yaw t−¬ng øng víi 3 gãc ,ϕ θ vµ ψ . ia vµ B ib : X¸c ®Þnh ®−îc tõ h×nh d¸ng, kÕt cÊu cña R«-bèt. 2.1.4. Bµi to¸n vÞ trÝ Víi c¸ch ®Æt vµ biÓu diÔn c¸c ®¹i l−îng nh− trªn, vÞ trÝ cña ®iÓm Bi trªn hÖ cè ®Þnh cã thÓ biÓu diÔn d−íi d¹ng: i i i iOB OA A B= + uuur uuur uuuur (i=1,2,3) (2.2) vµ : i iOB OP PB= + uuur uuur uuur (i=1,2,3) (2.3) Hay d−íi d¹ng ®¹i sè: = +i ib a A .iR 0 0 id⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (i=1,2,3) (2.4) vµ : = +ib P A BR .B ib (i=1,2,3) (2.5) KÕt hîp hai ph−¬ng tr×nh trªn ta cã: +P A BR .B ib = +ia A iR . 0 0 id⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (i=1,2,3) (2.6) Trong ®ã: P 1, 2 3, T p p p⎡ ⎤= ⎣ ⎦ ; B ib = , , T ix iy izb b b⎡ ⎤⎣ ⎦ ; =ia 1, 2 3, T i i ia a a⎡ ⎤⎣ ⎦ A BR = x x x y y y z z z u v w u v w u v w ⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ ; A iR = ix ix ix iy iy iy iz iz iz u v w u v w u v w ⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (i=1,2,3) (2.7) 12 X0 1A 3A 2A 1Z 0Y 2Z 3Z O 3β 1β 2βη2 3 η H×nh 4.2.3 C¸c ma trËn cosin chØ h−íng A iR ®−îc x¸c ®Þnh bëi c¸c phÐp quay liªn tiÕp: Ma trËn A 1R ®−îc x¸c ®Þnh bëi c¸c phÐp quay sau : • Quay mét gãc ( 1/ 2π β− ) quanh trôc z. • Quay quanh trôc x mét gãc ( / 2π ). • Quay quanh trôc z mét gãc ( 1/ 2π α+ ). Ma trËn A 2R ®−îc x¸c ®Þnh bëi c¸c phÐp quay sau : • Quay mét gãc 2η quanh trôc z. • Quay mét gãc ( 2/ 2π β− ) quanh trôc z. • Quay quanh trôc x mét gãc ( / 2π ). • Quay quanh trôc z mét gãc ( 2/ 2π α+ ). Ma trËn A 3R ®−îc x¸c ®Þnh bëi c¸c phÐp quay sau : • Quay mét gãc 3η quanh trôc z. • Quay mét gãc ( 3/ 2π β− ) quanh trôc z. 13 • Quay quanh trôc x mét gãc ( / 2π ). • Quay quanh trôc z mét gãc ( 3/ 2π α+ ). NÕu ta coi iAα lµ ma trËn cosin chØ h−íng cña 2 phÐp quay liªn tiÕp quanh trôc x mét gãc ( / 2π ) vµ quanh trôc z mét gãc ( / 2 iπ α+ ) th× : α =iA 1 0 0 sin cos 0 0 0 1 . cos sin 0 0 1 0 0 0 1 i i i i α α α α − −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥− −⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ α =iA sin cos 0 0 0 1 cos sin 0 i i i i α α α α − −⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥−⎣ ⎦ (2.8) §Æt : ( / 2 )iπ β− = iγ ; VËy ta cã : A 1 1 1( ). αγ= zR A A A 1 =R 1 1 1 1 1 1 1 1 cos sin 0 sin cos 0 sin cos 0 0 0 1 0 0 1 cos sin 0 γ γ α α γ γ α α − − −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦ 1 =A R 1 1 1 1 1 1 1 1 1 1 1 1 cos sin cos cos sin sin sin sin cos cos cos sin 0 γ α γ α γ γ α γ α γ α α − −⎡ ⎤⎢ ⎥−⎢ ⎥⎢ ⎥⎣ ⎦ 2 2 2 2( ). ( ). A z zR A A Aαη γ= A 2 =R 2 2 2 2 2 2 2 2 2 2 2 2 cos sin 0 cos sin 0 sin cos 0 sin cos 0 sin cos 0 0 0 1 0 0 1 0 0 1 cos sin 0 η η γ γ α α η η γ γ α α − − − −⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥−⎣ ⎦ ⎣ ⎦ ⎣ ⎦ 14 2 =A R 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 ( ) ( ) ( ) ( ) 0 η γ η γ α η γ η γ α η γ η γ η γ η γ α η γ η γ α η γ η γ α α − − + − −⎡ ⎤⎢ ⎥− + − + − +⎢ ⎥⎢ ⎥−⎣ ⎦ C C S S S C S S C C C S S C S C C S S S C C S C S S C C C S Trong ®ã : C 3η = cos 3η ; S 3η = sin 3η ; C 3γ = cos 3γ ; S 3γ = sin 3γ C 2α =cos 2α ; S 2α = sin 2α 3 3 3 3( ). ( ). A z zR A A Aαη γ= − 3 =A R 3 3 3 3 3 3 3 3 3 3 3 3 cos sin 0 cos sin 0 sin cos 0 sin cos 0 sin cos 0 0 0 1 0 0 1 0 0 1 cos sin 0 η η γ γ α α η η γ γ α α − − −⎡ ⎤⎡ ⎤ ⎡ ⎤ ⎢ ⎥⎢ ⎥ ⎢ ⎥− ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ −⎣ ⎦ ⎣ ⎦ ⎣ ⎦ 3 =A R 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 ( ) ( ) ( ) ( ) 0 η γ η γ α η γ η γ α η γ η γ η γ η γ α η γ η γ α η γ η γ α α − + − + − +⎡ ⎤⎢ ⎥− − + − − + +⎢ ⎥⎢ ⎥⎣ ⎦ C C S S S C C S S C C S S C S C C S S S C C S C S S C C C S Trong ®ã : C 3η = cos 3η ; S 3η = sin 3η ; C 3γ = cos 3γ ; S 3γ = sin 3γ ; C 3α = cos 3α ; S 3α = sin 3α Ta thÊy c¸c thµnh phÇn cña c¸c ma trËn A iR chØ chøa c¸c Èn lµ c¸c gãc iα cßn 2η , 3η vµ iβ ®· biÕt do kÕt cÊu cña r«-bèt. Ta viÕt l¹i ph−¬ng tr×nh (2.6) d−íi d¹ng ®¹i sè. Chó ý: Do Ai thuéc mÆt ph¼ng X0Y0 nªn 3ia = 0 (i=1,2,3) A1 trªn trôc X0 nªn 12 0a = Vµ Bi thuéc mÆt ph¼ng X0Y0 nªn izb = 0 (i=1,2,3) 15 +Víi i =1: 1 1 11 1 1 2 1 1 1 3 1 1 1 .x x x y x y z x z p u b a u d p u b u d p u b u d ⎧ + = +⎪ + =⎨⎪ + =⎩ Suy ra : 11 1 1 1 1 1 1 2 1 1 1 3 1 + −⎧ =⎪⎪⎪ −=⎨⎪⎪ −=⎪⎩ x x x y y x z z x a u d pu b u d p u b u d pu b (2.9) +Víi i=2 1 2 2 21 2 2 2 2 2 22 2 2 3 2 2 2 2 (a) (b) (c) x x x y x y x y y y z x z y z p u b v b a u d p u b v b a u d p u b v b u d ⎧ + + = +⎪ + + = +⎨⎪ + + =⎩ (2.10) +Víi i=3 1 3 3 31 3 3 2 3 3 32 3 3 3 3 3 3 3 (a) (b) (c) x x x y x y x y y y z x z y z p u b v b a u d p u b v b a u d p u b v b u d ⎧ + + = +⎪ + + = +⎨⎪ + + =⎩ (2.11) Thùc hiÖn c¸c phÐp biÕn ®æi sau: ((3.10 ) (3.11 )) ((3.10 ) (3.11 )) ((3.10 ) (3.11 )) λ λ λ −⎧⎪ −⎨⎪ −⎩ a a b b c c Ta ®−îc: 1 1 21 2 2 31 3 3 2 1 22 2 2 32 3 3 3 1 2 2 3 3 ( 1) ( ) ( ) ( 1) ( ) ( ) ( 1) ) λ λ λ λ λ λ λ λ λ ⎧ − + = + − +⎪ − + = + − +⎨⎪ − + = −⎩ x x x y y y z z z p u a u d a u d p u a u d a u d p u u d u d (2.12) 16 Víi 3 2 y y b b λ = , 1 2 3x xb bλ λ= − Thay c¸c kÕt qu¶ cña hÖ (2.9) vµo hÖ (2.12) ta ®−îc: 11 1 1 1 1 1 21 2 2 31 3 3 1 1 1 2 2 1 22 2 2 32 3 3 1 1 1 3 3 1 2 2 3 3 1 ( 1) ( ) ( ) ( 1) ( ) ( ) ( 1) ) x x x x y y y x z z z x a u d pp a u d a u d b u d p p a u d a u d b u d pp u d u d b λ λ λ λ λ λ λ λ λ + −⎧ − + = + − +⎪⎪⎪ −− + = + − +⎨⎪⎪ −− + = −⎪⎩ (2.13) MÆt kh¸c, dùa vµo kÕt cÊu cña bµn di ®éng B ta cã : B1 B2 3B b1 3b 2b H×nh 2.2.4 2 1 2 = uuuur B B 1 2 1 2( ) ( ) Tb b b b− − =b32 2 1 3 = uuuur B B 1 3 1 3( ) ( ) Tb b b b− − =b22 2 2 3 = uuuur B B 2 3 2 3( ) ( ) Tb b b b− − =b12 víi : = +i ib a A .iR 0 0 id⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (i=1,2,3) 17 1⇒ =b 11 1 1 1 1 1 1 x y z a u d u d u d +⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ ; 2 =b 21 2 2 22 2 2 2 2 x y z a u d a u d u d +⎡ ⎤⎢ ⎥+⎢ ⎥⎢ ⎥⎣ ⎦ ; 3 =b 31 3 3 32 3 3 3 3 x y z a u d a u d u d +⎡ ⎤⎢ ⎥+⎢ ⎥⎢ ⎥⎣ ⎦ 11 21 1 1 2 2 11 21 1 1 2 2 2 1 1 22 2 2 1 1 22 2 2 3 1 2 2 1 2 2 . T x x x x y y y y z z z z a a u d u d a a u d u d u d a u d u d a u d b u u d u u d − + − − + −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⇒ − − − − =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦ 11 31 1 1 3 3 11 31 1 1 3 3 2 1 1 32 3 3 1 1 32 3 3 2 1 1 3 3 1 1 3 3 . T x x x x y y y y z z z z a a u d u d a a u d u d u d a u d u d a u d b u d u d u d u d − + − − + −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥− − − − =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦ 21 31 2 2 3 3 21 31 2 2 3 3 2 22 32 2 2 3 3 22 32 2 2 3 3 1 2 2 3 3 2 2 3 3 . T x x x x y y y y z z z z a a u d u d a a u d u d a a u d u d a a u d u d b u d u d u d u d − + − − + −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥− + − − + − =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦ Hay : 2 2 2 2 11 21 1 1 2 2 1 1 22 2 2 1 1 2 2 3 2 2 2 2 11 31 1 1 3 3 1 1 32 3 3 1 1 3 3 2 2 2 2 2 21 31 2 2 3 3 22 32 2 2 3 3 2 2 3 3 1 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) x x y y z z x x y y z z x x y y z z a a u d u d u d a u d u d u d b a a u d u d u d a u d u d u d b a a u d u d a a u d u d u d u d b ⎧ − + − + − − + − =⎪⎪ − + − + − − + − =⎨⎪ − + − + − + − + − =⎪⎩ KÕt hîp víi hÖ (2.13) ta cã hÖ 6 ph−¬ng tr×nh, 6 Èn: (2.14) 18 HÖ ph−¬ng tr×nh (2.14) chøa 9 Èn sè 1 2 3 1 2 3 1 2 3, , , , , , , ,d d d p p pα α α . C¸c thµnh phÇn , ,ix iy izu u u ®· x¸c ®Þnh ®−îc, c¸c thµnh phÇn , ,x y zu u u x¸c ®Þnh theo (2.9) Khi gi¶i quyÕt bµi to¸n ®éng häc thuËn hay ng−îc, ta biÕt tr−íc ®−îc 3 Èn. C«ng viÖc cßn l¹i chØ ph¶i gi¶i hÖ 6 ph−¬ng tr×nh 6 Èn sè. a. Bµi to¸n ®éng häc thuËn Bµi to¸n ®éng häc thuËn lµ bµi to¸n biÕt ®é dµi c¸c ch©n di (i=1,2,3), ta ph¶i t×m vÞ trÝ cña bµn m¸y ®éng P vµ ma trËn ARB. Theo phÇn trªn ta thay c¸c gi¸ trÞ di (i=1,2,3) vµo hÖ (4.14), ta sÏ ®−îc hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ : 1 2 3 1 2 3, , , , ,p p pα α α Chó ý lµ 3 ph−¬ng tr×nh sau cña hÖ (2.14) chØ chøa di vµ iα nªn viÖc gi¶i 6 ph−¬ng tr×nh ®−îc ®¬n gi¶n l¹i cßn gi¶i hÖ 3 ph−¬ng tr×nh víi 3 Èn lµ iα . Sau ®ã thay c¸c gi¸ trÞ cña di vµ iα vµo 3 ph−¬ng tr×nh ®Çu ta sÏ tÝnh ®−îc c¸c gi¸ trÞ cña P. C¸c gi¸ trÞ cßn l¹i tÝnh ®−îc b»ng c¸ch thay trùc tiÕp vµo c¸c ph−¬ng tr×nh (2.9), (2.10), (2.11). b. Bµi to¸n ®éng häc ng−îc Bµi to¸n ®éng häc ng−îc lµ bµi to¸n biÕt vÞ trÝ bµn m¸y ®éng P, ta ph¶i t×m ®é dµi c¸c ch©n di (i=1,2,3) vµ c¸c gãc iα (i=1,2,3) . T−¬ng tù nh− c¸ch lµm ®èi víi bµi to¸n ®éng häc thuËn ta thay c¸c gi¸ trÞ cña P vµo hÖ (2.14), ta sÏ ®−îc hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ : 1 2 3 1 2 3, , , , ,d d dα α α . C¸c gi¸ trÞ cßn l¹i tÝnh ®−îc b»ng c¸ch thay trùc tiÕp vµo c¸c ph−¬ng tr×nh (2.9), (2.10), (2.11). 2.1.5. VÝ dô tÝnh to¸n Ta tÝnh to¸n cho mét r«-bèt song song 3 RPS cô thÓ : - Tam gi¸c A1A2A3 vµ tam gi¸c B1B2B3 lµ c¸c tam gi¸c ®Òu. 19 - PB1 = h; OA1 = g; 2 3 2 /3η η π= = - Do kÕt cÊu cña c¬ cÊu ta cã i i iz A B⊥ - Trôc i iz OA⊥ ⇒ / 2iβ π= Khi ®ã c¸c ®¹i l−îng trong c«ng thøc (4.6) trë thµnh : 1 Bb = 0 0 ⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ h ; 2 Bb = 2 3 2 0 ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ h h ; 3 B b = 2 3 2 0 ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ h h (2.15) 1a = 0 0 ⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ g ; 2a = 2 3 2 0 ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ g g ; 3a = 2 3 2 0 ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ g g (2.16) Do / 2iβ π= nªn iγ = ( / 2 )iπ β− = 0 Khi ®ã c¸c ma trËn cosin chØ h−íng ARi trë thµnh: 1 A R = 1 1 1 1 1 1 1 1 1 1 1 1 cos sin cos cos sin sin sin sin cos cos cos sin 0 γ α γ α γ γ α γ α γ α α − −⎡ ⎤⎢ ⎥−⎢ ⎥⎢ ⎥⎣ ⎦ 1 A R = 1 1 1 1 sin cos 0 0 0 1 cos sin 0 α α α α −⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (2.17) 20 2 A R = 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 1 3 1 3 1 3( cos sin )sin ( cos sin )cos sin cos 2 2 2 2 2 2 3 1 3 1 3 1( cos sin )sin ( cos sin )cos sin cos 2 2 2 2 2 2 cos sin 0 γ γ α γ γ α γ γ γ γ α γ γ α γ γ α α ⎡ ⎤+ − + −⎢ ⎥⎢ ⎥⎢ ⎥− + − − −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ 2 A R = 2 2 2 2 2 2 1 1 3sin cos 2 2 2 3 3 1sin cos 2 2 2 cos sin 0 α α α α α α ⎡ ⎤− −⎢ ⎥⎢ ⎥⎢ ⎥− −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (2.18) 3 A R = 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 1 3 1 3 1 3( cos sin )sin ( cos sin ) sin cos 2 2 2 2 2 2 3 1 3 1 3 1( cos sin )sin ( cos sin )cos sin cos 2 2 2 2 2 2 cos sin 0 γ γ α γ γ α γ γ γ γ α γ γ α γ γ α α ⎡ ⎤− − + +⎢ ⎥⎢ ⎥⎢ ⎥+ − − −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ cos 3 A R = 3 3 3 3 3 3 1 1 3sin cos 2 2 2 3 3 1sin cos 2 2 2 cos sin 0 α α α α α α ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥− −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (2.19) Khi ®ã : 3 2 y y b b λ = = -1 ; 1 2 3x xb bλ λ= − = h; Thay vµo hÖ (2.14) ta ®−îc : 21 1 2 2 3 3 1 1 2 2 2 3 3 3 1 1 2 2 3 3 2 2 2 1 1 2 2 1 2 1 2 1 2 13 ( sin sin ) sin 2 33 ( sin sin ) 2 3 cos cos cos (4.20) 3 3 sin 3 sin sin sin p d d d p d d p d d d g gd gd d d d d α α α α α α α α α α α α = + − −= − = + + − − + + + 21 2 1 2 2 2 2 2 1 1 3 3 1 3 1 3 1 3 1 3 1 3 2 2 2 2 2 2 3 3 2 3 2 3 2 3 2 3 2 3 2 cos cos 3 3 3 sin 3 sin sin sin 2 cos cos 3 3 3 sin 3 sin sin sin 2 cos cos 3 d d h g gd gd d d d d d d h g gd gd d d d d d d h α α α α α α α α α α α α α α ⎧⎪⎪⎪⎪⎪⎨⎪ − =⎪⎪ − − + + + − =⎪⎪ − − + + + − =⎩ a) Bµi to¸n ®éng häc thuËn Bµi to¸n ®éng häc thuËn lµ bµi to¸n biÕt ®é dµi c¸c ch©n di (i=1,2,3), ta ph¶i t×m vÞ trÝ cña bµn m¸y ®éng P vµ ma trËn ARB. Theo phÇn trªn ta thay c¸c gi¸ trÞ di (i=1,2,3) vµo hÖ (2.20), ta sÏ ®−îc hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ : 1 2 3 1 2 3, , , , ,p p pα α α Chó ý lµ 3 ph−¬ng tr×nh sau cña hÖ (2.20) chØ chøa di vµ iα nªn viÖc gi¶i 6 ph−¬ng tr×nh ®−îc ®¬n gi¶n l¹i cßn gi¶i hÖ 3 ph−¬ng tr×nh víi 3 Èn lµ iα . Sau ®ã thay c¸c gi¸ trÞ cña di vµ iα vµo 3 ph−¬ng tr×nh ®Çu ta sÏ tÝnh ®−îc c¸c gi¸ trÞ cña P C¸c gi¸ trÞ cßn l¹i tÝnh ®−îc b»ng c¸ch thay trùc tiÕp vµo c¸c ph−¬ng tr×nh (2.9), (2.10), (2.11). b) Bµi to¸n ®éng häc ng−îc Bµi to¸n ®éng häc ng−îc lµ bµi to¸n biÕt vÞ trÝ bµn m¸y ®éng P, ta ph¶i t×m ®é dµi c¸c ch©n di (i=1,2,3) vµ c¸c gãc iα (i=1,2,3) . T−¬ng tù nh− c¸ch lµm ®èi víi bµi to¸n ®éng häc thuËn ta thay c¸c gi¸ trÞ P vµo hÖ (2.20), ta sÏ ®−îc hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ : 1 2 3 1 2 3, , , , ,d d dα α α . C¸c gi¸ trÞ cßn l¹i tÝnh ®−îc b»ng c¸ch thay trùc tiÕp vµo c¸c ph−¬ng tr×nh (2.9), (2.10), (2.11). 22 2.2. Robot song song 3 RSS 2.2.1. KÕt cÊu h×nh häc H×nh 4.2.5: KÕt cÊu h×nh häc cña Robot song song 3 RSS Deltarca cÊu t¹o gåm ba ch©n, bµn di ®éng 12 vµ bµn cè ®Þnh 1. Mçi ch©n gåm cã kh©u trªn, kh©u d−íi vµ n¨m khíp, trong ®ã cã bèn khíp cÇu 4, 6, 8, 10 (S) vµ mét khíp quay 2 (R). Kh©u AC ®−îc gäi lµ kh©u c¸nh tay, kh©u BC ®−îc gäi lµ kh©u c¼ng tay. Kh©u AC nèi víi bµn cè ®Þnh bëi khíp quay b¶n lÒ 2, kh©u BC nèi víi bµn di ®éng bëi hai khíp cÇu 8, 10 vµ nèi víi kh©u AC bëi hai khíp cÇu 4, 6. Kh©u BC thùc chÊt lµ c¬ cÊu h×nh b×nh hµnh gåm c¸c kh©u 5, 7, 9, 11 víi c¸c khíp lµ khíp cÇu chø kh«ng ph¶i lµ khíp quay. Nh− vËy Deltarca cã tÊt c¶ 11 kh©u vµ 15 khíp, ®−îc gäi lµ r«-bèt song song 3 RSS (H×nh 4.2.5). 2.2.2. BËc tù do Sö dông c«ng thøc: 1 ( 1) j i b i F n j f fλ = = − − + −∑ Víi 6, 11, 15, 6n j fbλ = = = = , 12 khíp cÇu vµ 3 khíp quay, ta cã: 6(11 15 1) (12.3 3.1) 6 3F = − − + + − = 23 Nh− vËy bµn di ®éng sÏ chuyÓn ®éng víi ba bËc tù do trong kh«ng gian. 2.2.3. HÖ trôc täa ®é vµ kÝ hiÖu HÖ trôc Oxyz g¾n víi t©m bµn cè ®Þnh, hÖ trôc Puvw g¾n víi t©m bµn di ®éng. C¸c trôc x, y n»m trong bµn cè ®Þnh, c¸c trôc u, v n»m trong bµn di ®éng. T¹i mäi thêi ®iÓm x vµ y lu«n song song cïng chiÒu víi u vµ v. Dïng hÖ trôc i i i iA x y z g¾n víi t©m cña khíp quay iA , sao cho ix n»m theo h−íng kÐo dµi cña iOA vµ trôc iy däc theo trôc c¸c khíp quay, cßn trôc iz song song víi z. Gãc iα ®o tõ x ®Õn ix lµ mét h»ng sè cña qu¸ tr×nh thiÕt kÕ r«-bèt (H×nh 2.6). Gãc iϕ lµ gãc t¹o bëi trôc ix vµ i iAC . H×nh 4.2.6: §Æt hÖ trôc täa ®é lªn r«-bèt 24 2.2.4. Ph−¬ng tr×nh liªn kÕt §Ó thuËn tiÖn cho qu¸ tr×nh thµnh lËp hÖ ph−¬ng tr×nh liªn kÕt, ta quy ®Þnh mét sè ®iÓm sau:  Chän hÖ trôc cè ®Þnh lµ hÖ trôc Oxyz  Ta dïng ch÷ in nghiªng ®Ó kÝ hiÖu vÐc-t¬ ®¹i sè, vÝ dô ai lµ vÐc-t¬ ®¹i sè biÓu diÔn täa ®é cña ®iÓm iA so víi hÖ täa ®é cè ®Þnh Oxyz. Cßn biP lµ to¹ ®é cña ®iÓm iB trong hÖ täa ®é ®éng Puvw.  Khi nãi tíi täa ®é cña mét ®iÓm nµo ®ã mµ ta kh«ng nãi râ lµ xÐt trong hÖ to¹ ®é cô thÓ nµo th× hiÓu r»ng ta ®ang xet täa ®é cña ®iÓm ®ã trong hÖ täa ®é cè ®Þnh Oxyz. Sau ®©y ta x¸c ®Þnh täa ®é cña tõng ®iÓm  Täa ®é ®iÓm iA (H×nh 4.2.7): tuú thuéc vµo kÝch th−íc bµn cè ®Þnh [ ] [ ], , *cos( ), *sin( ),0 TT i iai xai yai zai ra raα α= = (2.21)  Täa ®é ®iÓm iC [ ], , .Tci xci yci zci ai Ri ciA= = + (2.22) Trong ®ã: Š Ri lµ ma trËn cosin chØ h−íng cña hÖ trôc i i i iA x y z so víi hÖ Oxyz. §Ó tÝnh Ri ta chØ viÖc xoay hÖ Oxyz quanh trôc z mét gãc iα , khi ®ã: cos( ) sin( ) 0 ( , ) sin( ) cos( ) 0 0 0 1 i i i i i iR R z α α α α α −⎡ ⎤⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.23) Š ciA lµ täa ®é cña ®iÓm iC trong hÖ trôc i i i iA x y z (H×nh 4.2.4) [ ]*cos( ),0, *sin( ) Ti iciA la laϕ ϕ= − (2.24) 25 H×nh 4.2.7 H×nh 4.2.8 Thay (2.23) vµ (2.24) vµo (2.22) ta ®−îc: [ ]cos( )*( *cos( )),sin( )*( *cos( )), *sin( ) Ti i i i ici ra la ra la laα ϕ α ϕ ϕ= + + − (2.25)  X¸c ®Þnh täa ®é ®iÓm B [ ], , *Tbi xbi ybi zbi p Rp biP= = + (2.26) Trong ®ã: [ ], , Tp px py pz= (2.27) lµ vÐc-t¬ täa ®é cña ®iÓm P trong hÖ trôc täa ®é Oxyz Š Rp lµ ma trËn cosin chØ h−íng cña hÖ Puvw so víi Oxyz. V× bµn di ®éng chuyÓn ®éng song ph¼ng so víi bµn cè ®Þnh nªn: 1 0 0 0 1 0 0 0 1 Rp ⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.28) Š biP lµ täa ®é cña ®iÓm iB trong hÖ trôc täa ®é Puvw: [ ] [ ], , *cos( ), *sin( ),0 TT i ibiP xbiP ybiP zbiP rb rbβ β= = (2.29) Thay (2.27), (2.28), (2.29) vµo (4.26) ta ®−îc: 26 *cos( ) *sin( ) i i px rb bi py rb pz α α +⎡ ⎤⎢ ⎥= +⎢ ⎥⎢ ⎥⎣ ⎦ (2.20) C¸c ®iÓm iB vµ iC rµng buéc víi nhau bëi ®iÒu kiÖn: 2 2 2 2* ( ) ( ) ( )Tlb bici bici xci xbi yci ybi zci zbi= = − + − + − (2.21) Víi i=1,2,3 th× hÖ (2.21) lµ hÖ gåm ba ph−¬ng tr×nh vµ s¸u Èn: 1 2 3, , ; , ,px py pz ϕ ϕ ϕ . Th«ng th−êng ta ®· biÕt tr−íc ba Èn vµ chØ cßn ba Èn ch−a biÕt. Nh− vËy hÖ (2.21) víi sè ph−¬ng tr×nh lµ 3 vµ b»ng sè Èn nªn lu«n cho ta lêi gi¶i. 2.2.5. Bµi to¸n ®éng häc thuËn Khi cho biÕt quy luËt chuyÓn ®éng cña ba gãc: 1 2 3, ,ϕ ϕ ϕ ta sÏ x¸c ®Þnh ®−îc px, py vµ pz tõ hÖ (2.21) vµ do vËy x¸c ®Þnh ®−îc quy luËt chuyÓn ®éng cña bµn di ®éng. 2.2.6. Bµi to¸n ®éng häc ng−îc Khi cho biÕt quy luËt chuyÓn ®éng cña bµn di ®éng, tøc lµ cho biÕt px, py, pz ta sÏ x¸c ®Þnh ®−îc 1 2 3, ,ϕ ϕ ϕ tõ hÖ (2.21). 2.3. Robot song song ph¼ng 3 ch©n RBSS ph¼ng 3 ch©n cã ®Æc ®iÓm lµ mçi ch©n gåm cã 2 kh©u vµ 3 khíp, mçi khíp cÇn 1 bËc tù do, tøc lµ khíp p5: khíp quay (R), khíp tr−ît (ký hiÖu lµ T - Translatrion hoÆc P – Prismatic). V× vËy cã 7 ph−¬ng ¸n: RRR, RRP, RPR, PRR, RPP, PPR, PPP H×nh 2.8 minh häa c¬ cÊu RBSS ph¼ng 3 ch©n dïng cÊu tróc RRR H×nh 2.9 dïng cÊu tróc PRR H×nh 2.10. dïng cÊu tróc PRP, trong ®ã 3 khíp quay R vu«ng gãc víi mÆt ph¼ng chøa 1 ph−¬ng tÞnh tiÕn cña 1 khíp P trªn tÊm cè ®Þnh, cßn khíp P kia liªn hÖ víi tÊm ®éng. 27 RBSS ph¼ng 3 ch©n th−êng ®−îc dïng ®Ó t¹o ra c¸c ®å g¸ CNC ®Ó mang ph«i gia c«ng hay mang dông cô gia c«ng. A1 B1 B2 3B A3 2A D E C H×nh 4.2.8. RBSS ph¼ng 3 ch©n RRR H×nh 4.2.9. RBSS ph¼ng 3 ch©n PRR B1 B2 3B D E C A3 A2 A1 28 H×nh 4.2.10. RBSS ph¼ng 3 ch©n PRP 2.2. §å g¸ gia c«ng vá hép ®éng c¬ xe m¸y Trong phÇn phô lôc tr×nh bµy b¶n thuyÕt minh “C«ng tr×nh thiÕt kÕ chÕ t¹o c¬ cÊu kÑp ph«i nhanh cho m¸y gia c«ng CNC” vµ giíi thiÖu mét sè b¶n vÏ thiÕt kÕ ®å g¸ vÒ hÖ thèng c¸c xi lanh khÝ nÐn ®−îc dïng cho ®å g¸ nµy. A B C F D E G H I P R P TÊm ®éng TÊm cè ®Þnh 29 III. Robot song song RBSS – 322 30 3.1. Giíi thiÖu chung Vấn đề Robot song song (RBSS) trở nên hấp dẫn nhiều nhà nghiên cứu từ giữa thập kỷ 90 khi nó được ứng dụng dưới dạng thiết bị có tên là Hexapod để tạo ra máy công cụ CNC 5 trục. Hexapod là một môđun RBSS được kết cấu trên nguyên lý cơ cấu Stewart. Cơ cấu này gồm có 6 chân, với độ dài thay đổi được, nối với giá và tấm động đều bằng các khớp cầu. Bằng cách thay đổi độ dài của các chân có thể điều khiển sự định vị và định hướng của tấm động theo ý muốn. Ngoài các ứng dụng trong ngành chế tạo máy công cụ RBSS còn được áp dụng hiệu quả trong dụng cụ y học, trong hệ thống mô phỏng, trong thiết bị thiên văn, trắc địa và trong kỹ thuật phòng không v.v. Cơ cấu RBSS cũng có nhiều biến thể khác nhau và xuất hiện nhiều tên gọi khác nhau, ví dụ như Hexapod, Hexaglide [4], Hexagons [5], Hexa [2], Delta [3] v.v. Dưới đây trình bày kết quả nghiên cứu, thiết kế, chế tạo và điều khiển một kiểu robot song song. Trong thiết kế đã tham khảo phần tạo dáng từ mẫu chào hàng robot Flex Picker của hãng ABB. 3.2. Thiết kế kết cấu và lập trình điều khiển chuyển động của Robot. Trên hình 3.1 là bản vẽ 3D của Robot đã thiết kế và trên hình 3.2 là ảnh chụp sau khi đã lắp đặt vận hành. Các chân của Robot được xếp thành 3 nhánh đôi và được truyền động từ 3 động cơ riêng biệt. Mỗi chân gồm có 2 phần: phần trên (đùi) và phần dưới (ống chân). Chân nhánh nối với giá đỡ bằng khớp quay (khớp háng) và nối với tấm động bằng “khớp cổ chân”. Chỗ “cổ chân” và đầu gối” đều bố trí một khớp quay với trục quay có 2 đầu khớp cầu 31 Hình 4.3.1. Bản vẽ 3D thiết kế RBSS-322 Hình 4.3.2. Ảnh chụp RBSS - 322 32 Gọi điểm đầu mút của dụng cụ gắn liền với tấm động là “điểm tác động cuối” (end - effector) của Robot. Khi lập trình điều khiển chuyển động của Robot, ta quan tâm đến sự dịch chuyển của ‘điểm tác động cuối” này. Ngôn ngữ lập trình là Visual C++ tạo điều kiện dễ dàng kết nối với các thư viện đồ hoạ mạnh và phổ biến như OpenGL. Sơ đồ giải thuật chương trình điều khiển giới thiệu trên hình 4.3.3. Hình 4.3.3. Sơ đồ giải thuật chương trình điều khiển Sau khi đã nhập dữ liệu, chúng kiểm tra có nằm trong vùng làm việc không, rồi được gửi tới các chương trình con để giải bài toán động học ngược. Các kết quả đó được xử lý và truyền tới cổng nối tiếp R232C để điều khiển hoạt động của Robot qua bộ thiết bị điều khiển. Sau khi di chuyển tới điểm đã định, tín hiệu phản hồi về sẽ là thông điệp gửi tiếp thông tin về điểm di chuyển tiếp theo. Quá trình này được thực hiện lặp lại một cách nhịp nhàng, không phải chờ đợi, cho tới khi đã tới đích cuối cùng của hành trình. Begin Nhập dữ liệu Trong vùng làm việc Giải bài toán Động học nghịch Truyền tín hiệu điều khiển tới R232C Nhận Tín hiệu từ R232C End Đúng Sai Đúng Sai 33 Trên hình 4.3.4 là minh hoạ kết quả tính toán về hành trình di chuyển của “điểm tác động cuối”. Hình 4.3. 4. Trich kết quả tính toán về hành trình Ngoài ra kết quả tính toán cho chương trình điều khiển cũng đồng thời được gửi tới chương trình mô phỏng hoạt hình của Robot. Hình 4.3.5: Sơ đồ chức năng bảng mạch chính 3.3. Sơ đồ hệ thống điều khiển Trên hình 4.3.5 là sơ đồ chức năng bảng mạch chính (Main board). Chương trình điều khiển được nạp trong bộ nhớ trong bộ vi xử lý của 89C51 DB9 2 3 5 MAX2 32 TxD PO RxD 89C51 P1 P2 Joy- stick AD 0809 ANALOG INPUT CONTROL BUS 34 thực hiện các chức năng kiểm soát cổng P1 để nhận lệnh điều khiển từ Joystick trong chế độ điều khiển bằng tay (manual), kiểm tra cổng nối tiếp để nhận lệnh điều khiển từ máy tính PC. Chuỗi lệnh từ máy tính điều khiển truyền tới gồm 11byte. Khi nhận xong một lệnh từ cổng nối tiếp thì Main board sẽ truyền trở về PC một byte trả lời. Khi thực hiện xong lệnh đã nhận thì nó lại tiếp tục truyền trở về PC một byte báo cáo thứ 2. Trên hình 4.3.6 là sơ đồ chức năng bảng mạch bộ điều khiển động cơ (Driver board). Hình 4.3.6. Sơ đồ chức năng bảng mạch bộ điều khiển động cơ H×nh 4.3.7.Chu trình nhận và thực hiện lệnh điều khiển động cơ Driver board 89c2051 CONTROL BUS Mạch công suất Kiểm soát dòng Mạch phối hợp DC EN SENSOR Mạch phối hợp BUS điều khiển Nhận lệnh Thực hiện lệnh Kiểm soát dòng ĐC Đọc Encoder SENSER vị trí ĐK Động cơ 35 Chương trình điều khiển được nạp vào bộ nhớ trong của bộ vi xử lý 89C 2051 thực hiện chức năng kiểm tra bus điều khiển (control bus). Giá trị xung đếm vòng quay từ encoder đưa về qua mạch phối hợp được so dánh với giá trị vòng quay từ lệnh điều khiển là tín hiệu để động cơ dừng hay hoạt động tiếp tục. Dòng điện tiêu thụ của động cơ cũng được kiểm soát trong quá trình hoạt động. Nếu giá trị này vượt quá ngưỡng đã đặt thì chương trình sẽ cho dừng động cơ và báo lỗi về bảng mạch chính. Lỗi sẽ được hiển thị bằng đèn LED và có thể thông báo về máy tính điều khiển. Các sensor chỉ báo vị trí ban đầu của Robot cũng được kiểm soát để khi có lệnh chúng sẽ trở về vị trí xuất phát. Chu trình nhận và thực hiện lệnh của Driver board được mô tả trên hình 3.7. Hệ thống thiết bị đã được vận hành thử nghiệm tốt và đang chuẩn bị triển khai ứng dụng trong gia công điêu khắc. 3.4. Ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn RBSS - 322 Chương trình gồm các file vitridau.m, nhap.m, tinhdulieu.m, chayhopmay.m %vitridau.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; s=serial('COM4'); set(s, 'Terminator', 'CR'); s.Timeout=7; %s so_xung1=2000; %21100 1 vong highbyte1=floor(so_xung1/256); lowbyte1=mod(so_xung1,256); so_xung2=1000; %21100 1 vong highbyte2=floor(so_xung2/256); 36 lowbyte2=mod(so_xung2,256); so_xung3=1000; %21100 1 vong highbyte3=floor(so_xung3/256); lowbyte3=mod(so_xung3,256); fopen(s) fwrite(s,254) %ma Header1 fwrite(s,253) %ma Header2 fwrite(s,104) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115 dung) %(h or 104 vi tri dau) fwrite(s,lowbyte1) %so xung lowbyte dc1 fwrite(s,highbyte1) %so xung highbyte dc1 fwrite(s,104) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau) fwrite(s,lowbyte2) %so xung lowbyte dc2 fwrite(s,highbyte2) %so xung highbyte dc2 fwrite(s,104) %ma1 dc3 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau) fwrite(s,lowbyte3) %so xung lowbyte dc3 fwrite(s,highbyte3) %so xung highbyte dc3 fprintf(s,'%s','s') %ma4 dc4 (+ len) (- xuong) (s dung) fwrite(s,1) fwrite(s,0) data=fscanf(s) if data=='b' fwrite(s,254) fwrite(s,253) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') 37 fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) end fclose(s); %nhap.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; % ch=input('\n ban nhap so lieu hay doc tu dia (n/d) : ','s'); % if (ch == 'd') % filename=input('\n Nhap ten file: ','s'); % [command,mss]=sprintf('load %s Aa Bb Cc OA OB OC TAa TBb % TCc.OtTA zTA OtTB zTB OtTC zTC',filename); % eval(command); % fprintf('\n Aa=%10.3f',Aa); % fprintf('\n Bb=%10.3f',Bb); % fprintf('\n Cc=%10.3f',Cc); % fprintf('\n OA=%10.3f',OA); % fprintf('\n OB=%10.3f',OB); % fprintf('\n OC=%10.3f',OC); % fprintf('\n TAa=%10.3f',TAa); % fprintf('\n TBb=%10.3f',TBb); % fprintf('\n TCc=%10.3f',TCc); 38 % fprintf('\n khoang cach TA den tam=%10.3f',OtTA); % fprintf('\n zTA=%10.3f',zTA); % fprintf('\n khoang cach TB den tam=%10.3f',OtTB); % fprintf('\n zTB=%10.3f',zTB); % fprintf('\n khoang cach TC den tam=%10.3f',OtTC); % fprintf('\n zTC=%10.3f',zTC); % elseif (ch == 'n') % Aa=input('Aa='); % Bb=input('Bb='); % Cc=input('Cc='); % OA=input('OA='); % OB=input('OB='); % OC=input('OC='); % TAa=input('TAa='); % TBb=input('TBb='); % TCc=input('TCc='); % OtTA=input('khoang cach TA den tam='); % zTA=input('zTA='); % OtTB=input('khoang cach TB den tam='); % zTB=input('zTB='); % OtTC=input('khoang cach TC den tam='); % zTC=input('zTC='); % filesave=input('\n Nhap ten file save: ','s'); % [command,mss]=sprintf('save %s Aa Bb Cc OA OB OC TAa TBb % TCc OtTA zTA OtTB zTB OtTC zTC',filesave); % eval(command); % end %-------------------------------------------------------------------------- 39 Aa=800 Bb=800 Cc=800 OA=40 OB=40 OC=40 TAa=365 TBb=365 TCc=365 OtTA=200 zTA=615 OtTB=200 zTB=615 OtTC=200 zTC=615 xO=input('xO='); yO=input('yO='); zO=input('zO='); xA=xO-OA yA=yO zA=zO xB=xO+OB*sin(30*pi/180) yB=yO-OB*cos(30*pi/180) zB=zO xC=xO+OC*sin(30*pi/180) yC=yO+OC*cos(30*pi/180) zC=zO 40 xTA=-OtTA yTA=0 zTA xTB=OtTB*sin(30*pi/180) yTB=-OtTB*cos(30*pi/180) zTB xTC=OtTC*sin(30*pi/180) yTC=OtTC*cos(30*pi/180) zTC %----------------- ya=0; y=ya; Aa_binh=Aa^2; TAa_binh=TAa^2; eqt1=convert(xA,yA,zA,Aa_binh); eqt2=convert(xTA,yTA,zTA,TAa_binh); syms x z x1 z1 [x,z]=solve(eqt1,eqt2,'x,z'); x1=eval(x) z1=eval(z) for i=1:length(z1) xa=x1(i); za=z1(i); if z1(i)>zA 41 if x1(i)<xTA xa=x1(i); ya; za=z1(i); break; end end end xa ya za %------------------------------------------------------ Bb_binh=Bb^2; TBb_binh=TBb^2; syms x y z x2 y2 z2 eqt3=y+x*cot(30*pi/180); eqt1=convert(xB,yB,zB,Bb_binh); eqt2=convert(xTB,yTB,zTB,TBb_binh); [x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z'); x2=eval(x) y2=eval(y) z2=eval(z) for i=1:length(z2) xb=x2(i); yb=y2(i); zb=z2(i); if z2(i)>zB if x2(i)>xTB xb=x2(i); 42 yb=y2(i); zb=z2(i); break; end end end xb yb zb %------------------------------------------------------ Cc_binh=Cc^2; TCc_binh=TCc^2; syms x y z x3 y3 z3 eqt3=y-x*cot(30*pi/180); eqt1=convert(xC,yC,zC,Cc_binh); eqt2=convert(xTC,yTC,zTC,TCc_binh); [x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z'); x3=eval(x) y3=eval(y) z3=eval(z) for i=1:length(z3) xc=x3(i); yc=y3(i); zc=z3(i); if z3(i)>zC if x3(i)>xTC xc=x3(i); 43 yc=y3(i); zc=z3(i); break; end end end xc yc zc %-------------------------------------------------------- %tinh cac goc di xuong goc1=asin((zTA-za)/TAa)*180/pi goc2=asin((zTB-zb)/TBb)*180/pi goc3=asin((zTC-zc)/TCc)*180/pi s=serial('COM2'); set(s, 'Terminator', 'CR'); s.Timeout=5; %s so_xung1=21100*goc1/360; %21100 1 vong highbyte1=floor(so_xung1/256); lowbyte1=mod(so_xung1,256); so_xung2=21100*goc2/360; %21100 1 vong highbyte2=floor(so_xung2/256); lowbyte2=mod(so_xung2,256); so_xung3=21100*goc3/360; %21100 1 vong highbyte3=floor(so_xung3/256); lowbyte3=mod(so_xung3,256); 44 if goc1>0 chieu1=45; else chieu1=43; end if goc2>0 chieu2=45; else chieu2=43; end if goc3>0 chieu3=45; else chieu3=43; end fopen(s) fwrite(s,254) %ma Header1 fwrite(s,253) %ma Header2 fwrite(s,chieu1) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau) fwrite(s,lowbyte1) %so xung lowbyte dc1 fwrite(s,highbyte1) %so xung highbyte dc1 fwrite(s,chieu2) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115 dung) (h or 104 vi tri dau) fwrite(s,lowbyte2) %so xung lowbyte dc2 fwrite(s,highbyte2) %so xung highbyte dc2 fwrite(s,chieu3) %ma1 dc3 (+ or 43 len) (- or 45 xuong) (s or 115 dung) (h or 104 vi tri dau) fwrite(s,lowbyte3) %so xung lowbyte dc3 45 fwrite(s,highbyte3) %so xung highbyte dc3 fprintf(s,'%s','s') %ma4 dc4 (+ len) (- xuong) (s dung) fwrite(s,1) fwrite(s,0) data=fscanf(s,'%s') if data=='b' fwrite(s,254) fwrite(s,253) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) end fclose(s); %tinhdulieu.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; Aa=800 Bb=800 Cc=800 46 OA=40 OB=40 OC=40 TAa=365 TBb=365 TCc=365 OtTA=200 zTA=615 OtTB=200 zTB=615 OtTC=200 zTC=615 ch=input('\n ban nhap so lieu hay doc tu dia (n/d) toa do mui khoan: ','s'); if (ch == 'd') filename1=input('\n Nhap ten file luu toa do: ','s'); [command,mss]=sprintf('load %s n xO yO zO',filename1); eval(command); for i=1:n fprintf('\n du lieu tai diem :%d \n',i); fprintf('\n xO=%10.3f',xO(i)); fprintf('\n yO=%10.3f',yO(i)); fprintf('\n zO=%10.3f',zO(i)); end elseif (ch == 'n') n=input('So diem tren can nhap ='); for i=1:n fprintf('\n Nhap du lieu tai diem :%d \n',i); 47 xO(i)=input('xO='); yO(i)=input('yO='); zO(i)=input('zO='); end filesave1=input('\n Nhap ten file save luu toa do: ','s'); [command,mss]=sprintf('save %s n xO yO zO',filesave1); eval(command); end for j=1:n xA(j)=xO(j)-OA yA(j)=yO(j) zA(j)=zO(j) xB(j)=xO(j)+OB*sin(30*pi/180) yB(j)=yO(j)-OB*cos(30*pi/180) zB(j)=zO(j) xC(j)=xO(j)+OC*sin(30*pi/180) yC(j)=yO(j)+OC*cos(30*pi/180) zC(j)=zO(j) xTA=-OtTA yTA=0 zTA xTB=OtTB*sin(30*pi/180) yTB=-OtTB*cos(30*pi/180) zTB 48 xTC=OtTC*sin(30*pi/180) yTC=OtTC*cos(30*pi/180) zTC end %----------------- for j=1:n ya(j)=0; y=ya(j); Aa_binh=Aa^2; TAa_binh=TAa^2; eqt1=convert(xA(j),yA(j),zA(j),Aa_binh); eqt2=convert(xTA,yTA,zTA,TAa_binh); syms x z x1 z1 [x,z]=solve(eqt1,eqt2,'x,z'); x1=eval(x) z1=eval(z) for i=1:length(z1) xa(j)=x1(i); za(j)=z1(i); if z1(i)>zA(j) if x1(i)<xTA xa(j)=x1(i); ya(j); za(j)=z1(i); break; end end 49 end xa(j) ya(j) za(j) %------------------------------------------------------ Bb_binh=Bb^2; TBb_binh=TBb^2; syms x y z x2 y2 z2 eqt3=y+x*cot(30*pi/180); eqt1=convert(xB(j),yB(j),zB(j),Bb_binh); eqt2=convert(xTB,yTB,zTB,TBb_binh); [x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z'); x2=eval(x) y2=eval(y) z2=eval(z) for i=1:length(z2) xb(j)=x2(i); yb(j)=y2(i); zb(j)=z2(i); if z2(i)>zB(j) if x2(i)>xTB xb(j)=x2(i); yb(j)=y2(i); zb(j)=z2(i); break; end end 50 end xb(j) yb(j) zb(j) %------------------------------------------------------ Cc_binh=Cc^2; TCc_binh=TCc^2; syms x y z x3 y3 z3 eqt3=y-x*cot(30*pi/180); eqt1=convert(xC(j),yC(j),zC(j),Cc_binh); eqt2=convert(xTC,yTC,zTC,TCc_binh); [x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z'); x3=eval(x) y3=eval(y) z3=eval(z) for i=1:length(z3) xc(j)=x3(i); yc(j)=y3(i); zc(j)=z3(i); if z3(i)>zC(j) if x3(i)>xTC xc(j)=x3(i); yc(j)=y3(i); zc(j)=z3(i); break; end end 51 end xc(j) yc(j) zc(j) end %-------------------------------------------------------- filesave2=input('\n Nhap ten file save luu toa do z de tinh: ','s'); [command,mss]=sprintf('save %s n za zb zc zTA zTB zTC TAa TBb TCc',filesave2); eval(command); % filename2=input('\n Ghi ten file de save toa do z de tinh: ','s'); % fid = fopen(filename2,'w'); % fprintf(fid,'"%d"',n); % fprintf(fid,'"%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f"',zTA,zTB,zTC,TAa,TBb,TCc); % for j=1:n % fprintf(fid,'"%8.4f,%8.4f,%8.4f"',za(j),zb(j),zc(j)); % end % fclose(fid) %chayhopmay.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; filename2=input('\n Nhap ten file: ','s'); [command,mss]=sprintf('load %s n za zb zc zTA zTB zTC TAa TBb TCc',filename2); eval(command); za; 52 zb; zc; so_xung1_xuong=21100*6.2138/360; %21100 1 vong highbyte1_xuong=floor(so_xung1_xuong/256); lowbyte1_xuong=mod(so_xung1_xuong,256); so_xung2_xuong=21100*6.2138/360; %21100 1 vong highbyte2_xuong=floor(so_xung2_xuong/256); lowbyte2_xuong=mod(so_xung2_xuong,256); so_xung3_xuong=21100*6.2138/360; %21100 1 vong highbyte3_xuong=floor(so_xung3_xuong/256); lowbyte3_xuong=mod(so_xung3_xuong,256); for j=1:n %tinh cac goc di xuong goc1(j)=asin((zTA-za(j))/TAa)*180/pi; goc2(j)=asin((zTB-zb(j))/TBb)*180/pi; goc3(j)=asin((zTC-zc(j))/TCc)*180/pi; s=serial('COM4'); set(s, 'Terminator', 'CR'); s.Timeout=10; %s so_xung1(j)=21100*goc1(j)/360; %21100 1 vong highbyte1(j)=floor(so_xung1(j)/256); lowbyte1(j)=mod(so_xung1(j),256); so_xung2(j)=21100*goc2(j)/360; %21100 1 vong highbyte2(j)=floor(so_xung2(j)/256); lowbyte2(j)=mod(so_xung2(j),256); 53 so_xung3(j)=21100*goc3(j)/360; %21100 1 vong highbyte3(j)=floor(so_xung3(j)/256); lowbyte3(j)=mod(so_xung3(j),256); %di len if goc1(j)>0 chieu1(j)=45; else chieu1(j)=43; end if goc2(j)>0 chieu2(j)=45; else chieu2(j)=43; end if goc3(j)>0 chieu3(j)=45; else chieu3(j)=43; end fopen(s) fwrite(s,254) %ma Header1 fwrite(s,253) %ma Header2 fwrite(s,chieu1(j)) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau) fwrite(s,lowbyte1(j)) %so xung lowbyte dc1 fwrite(s,highbyte1(j)) %so xung highbyte dc1 fwrite(s,chieu2(j)) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau) 54 %--------------------------------------------------------------------- s=serial('COM4'); set(s, 'Terminator', 'CR'); s.Timeout=5; fopen(s) fwrite(s,254) %ma Header1 fwrite(s,253) %ma Header2 fwrite(s,104) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau) fwrite(s,0) %so xung lowbyte dc1 fwrite(s,0) %so xung highbyte dc1 fwrite(s,104) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115 dung) %(h or 104 vi tri dau) fwrite(s,0) %so xung lowbyte dc2 fwrite(s,0) %so xung highbyte dc2 fwrite(s,104) %ma1 dc3 (+ or 43 len) (- or 45 xuong) (s or 115 dung) %(h or 104 vi tri dau) fwrite(s,0) %so xung lowbyte dc3 fwrite(s,0) %so xung highbyte dc3 fprintf(s,'%s','s') %ma4 dc4 (+ len) (- xuong) (s dung) fwrite(s,1) fwrite(s,0) data=fscanf(s,'%s') if data=='b' fwrite(s,254) fwrite(s,253) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) 55 fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) end %------------------------------------------- fclose(s); end 56 IV. Robot “NhÖn n−íc 57 4.1. Giíi thiÖu chung Robot “NhÖn n−íc” lµ mét ®Ò xuÊt míi trong nghiªn cøu øng dông robot song song Robot song song (RBSS) còng lµ mét lo¹i robot, nh−ng s¬ ®å ®éng cña c¬ cÊu cã c¸c kh©u kh«ng nèi liªn tiÕp víi nhau nh− c¬ cÊu chuçi cña robot th«ng th−êng (serie robot) mµ ë mét kh©u nÇo ®ã nèi víi nhiÒu kh©u kh¸c, tøc lµ nèi song song víi nhau. §èi víi tr−êng hîp RBSS th× bµi to¸n thuËn l¹i khã h¬n nhiÒu so víi bµi to¸n ng−îc. Mét trong nh÷ng lÜnh vùc øng dông thµnh c«ng cña RBSS lµ ®å g¸ cho m¸y gia c«ng CNC, trong ®ã c¬ cÊu Hexapod lµ vÝ dô ®iÓn h×nh. Trong nh÷ng n¨m gÇn ®©y RBSS cßn më réng ra nhiÒu lÜnh vùc n©ng vËn chuyÓn vµ xuÊt hiÖn tªn gäi míi Robocrane (robot cÈu). Robot-cÈu lµ tªn gäi chung lo¹i robot lµm viÖc víi chøc n¨ng nh− mét thiÕt bÞ n©ng vËn chuyÓn dïng d©y c¸p n©ng t¶i. VÒ ®¹i thÓ, robot - cÈu còng ho¹t ®éng theo nguyªn lý c¬ cÊu Stewart. Trªn h×nh 4.4.1 lµ s¬ ®å c¬ cÊu Stewart, gåm cã tÊm ®éng (1) nèi víi tÊm g¸ ®ì (3) b»ng 6 ch©n (2) cã c¸c khíp cÇu ë c¶ 2 ®Çu. Khi thay ®æi ®é dµi cña 6 ch©n nµy th× “®Þnh vÞ vµ ®Þnh h−íng” cña tÊm ®éng sÏ thay ®æi theo. 58 H×nh 4.4.1. S¬ ®å c¬ cÊu Stewart Trong thiÕt bÞ robot - cÈu, tÊm ®éng (1) ®−îc treo trªn tÊm gi¸ cè ®Þnh (3) b»ng 6 d©y c¸p cã ®é dµi thay ®æi ®−îc nhê c¬ cÊu kiÓu têi quay vµ ®¶m b¶o linh ho¹t nèi ghÐp gi÷a d©y c¸p víi c¸c tÊm nhê cã khíp cÇu. Trong tr−êng hîp robot – cÈu th× tÊm ®éng g¾n liÒn víi ®èi t−îng vËt thÓ cÇn ®−îc n©ng chuyÓn hoÆc xÕp ®Æt v.v. Khi ®ã, viÖc ®iÒukhiÓn “®Þnh vÞ vµ ®Þnh h−íng” cña tÊm ®éng trë thµnh viÖc ®iÒu khiÓn ®ång thêi 6 ®éng c¬ cña 6 têi quay nãi trªn Robot “NhÖn n−íc” gåm cã 2 phÇn: PhÇn trªn cã tÊm ®éng treo trªn gi¸ ®ì nhê 6 d©y c¸p, ®iÒu khiÓn b»ng 6 têi quay. §ã lµ mét robot – cÈu. PhÇn d−íi lµ mét hÖ thèng c¸c c¬ cÊu mang c¸c vßi phun dïng ®Ó tÈy röa c¸c khoang hÇm tÇu. HÖ thèng c¸c c¬ cÊu nµy cã 4 nh¸nh, mçi nh¸nh ®Òu cã thÓ “co côm” l¹i hoÆc “xße réng” ra, nªn tr«ng h×nh d¸ng bªn ngoµi gièng nh− “con nhÖn n−íc”. 4.2. M« t¶ Robot “NhÖn n−íc” “Robo - NhÖn n−íc” lµ tªn gäi t¾t cña mét thiÕt bÞ míi ®−îc ®Ò xuÊt. §©y lµ mét vÝ dô øng dông cña RBSS. Nã gåm 2 phÇn (h×nh 4.4.2): PhÇn trªn 1 2 3 59 cã tÊm ®éng treo trªn gi¸ ®ì nhê 6 d©y c¸p, ®iÒu khiÓn b»ng 6 têi quay. §ã lµ mét robo - cÈu. PhÇn d−íi lµ mét hÖ thèng víi 4 c¬ cÊu nh¸nh h×nh b×nh hµnh. HÖ thèng c¬ cÊu nµy cã thÓ “xoÌ ra” hoÆc “co l¹i” nhê chuyÓn ®éng tÞnh tiÕn cña thanh trôc gi÷a trong khíp ®éng tÞnh tiÕn, g¾n liÒn víi tÊm ®éng cña robo - cÈu. ThiÕt bÞ nµy lµ mét c¬ cÊu mang c¸c èng n−íc mÒm cã vßi phun dïng ®Ó tÈy röa c¸c khoang hÇm ngÇm, mµ ë ®ã con ng−êi rÊt khã kh¨n hoÆc kh«ng thÓ thao t¸c, vÝ dô v× m«i tr−êng qu¸ ®éc h¹i. ThiÕt bÞ nµy cã thÓ ‘co côm” l¹i vµ h¹ thÊp dÇn qua miÖng hÇm cã kÝch th−íc giíi h¹n vµ sÏ “xoÌ réng” ra, ®ång thêi nhê cã thÓ thay ®æi ®−îc “sù ®Þnh vÞ vµ ®Þnh h−íng” cña tÊm ®éng, mµ quü ®¹o phun n−íc ®−îc ®iÒu khiÓn theo ý muèn. H×nh 4.4.2 lµ cöa sæ mµn h×nh víi mét pha ho¹t ®éng cña Robo - NhÖn n−íc ®· ®−îc m« pháng ho¹t h×nh. Trªn c¬ së ®ã cã thÓ kh¶o s¸t c¸c chuyÓn ®éng cña Robo - NhÖn n−íc vµ thö nghiÖm c¸c ch−¬ng tr×nh ®iÒu khiÓn. H×nh 4. 4.2. Robo - NhÖn n−íc 60 4.3. TÝnh to¸n, x©y dùng ch−¬ng tr×nh ®iÒu khiÓn Robot - “NhÖn n−íc” §øc H×nh 4. 4.3. VÝ dô tÝnh to¸n theo ch−¬ng tr×nh ®iÒu khiÓn robo-cÈu 4.4. M« pháng ho¹t h×nh Robot - “NhÖn n−íc” H×nh 4.4.4. M« pháng RBSS b»ng Open GL dïng cho “NhÖn n−íc” 61 V. M« pháng RBSS dïng lµm ®å g¸ CNC 62 5.1. M« pháng RBSS ch©n RPS Bµi to¸n ®éng häc thuËn lµ bµi to¸n biÕt ®é dµi c¸c ch©n di (i=1,2,3), ta ph¶i t×m vÞ trÝ cña bµn m¸y ®éng P vµ ma trËn ARB. Theo phÇn trªn ta thay c¸c gi¸ trÞ di (i=1,2,3) vµo hÖ (4.20), ta sÏ ®−îc hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ : 1 2 3 1 2 3, , , , ,p p pα α α Chó ý lµ 3 ph−¬ng tr×nh sau cña hÖ (4.20) chØ chøa di vµ iα nªn viÖc gi¶i 6 ph−¬ng tr×nh ®−îc ®¬n gi¶n l¹i cßn gi¶i hÖ 3 ph−¬ng tr×nh víi 3 Èn lµ iα . Sau ®ã thay c¸c gi¸ trÞ cña di vµ iα vµo 3 ph−¬ng tr×nh ®Çu ta sÏ tÝnh ®−îc c¸c gi¸ trÞ cña P VÝ dô: - C¸c ®iÒu kiÖn ®Çu 10 20 30 / 2α α α π= = = d10 = d20 = d30 = 4.44 Hai tam gi¸c lµ ®Òu vµ h = g = 8.89 - Qui luËt chuyÓn ®éng cña c¸c ch©n r«-bèt d1 = d10(1+0.08cos10t) d2 = d20(1+0.1sin10t) d3 = d30(1+0.05cos10t) 63 - Giao diÖn chÝnh cña ch−¬ng tr×nh - KÕt qu¶ m« pháng sè b»ng Matlab: H×nh 4.5.1: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng x 64 H×nh 4.5.2: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng y H×nh 4.5.3: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng z 65 H×nh 4.5.4: §å thÞ vÞ trÝ ®iÓm P H×nh 4.5.5: §å thÞ vËn tèc ®iÓm P theo ph−¬ng x 66 H×nh 4.5.6: §å thÞ vËn tèc ®iÓm P theo ph−¬ng y H×nh 4.5.7: §å thÞ vËn tèc ®iÓm P theo ph−¬ng z 67 - KÕt qu¶ m« pháng ®éng b»ng Matlab: H×nh 4.5.8: VÝ dô m« pháng ®éng RBSS 3 ch©n RPS (bµi to¸n ®éng häc thuËn) 5.1.2. Bµi to¸n ®éng häc ng−îc Bµi to¸n ®éng häc ng−îc lµ bµi to¸n biÕt vÞ trÝ bµn m¸y ®éng P, ta ph¶i t×m ®é dµi c¸c ch©n di (i=1,2,3) vµ c¸c gãc iα (i=1,2,3) . T−¬ng tù nh− c¸ch lµm ®èi víi bµi to¸n ®éng häc thuËn ta thay c¸c gi¸ trÞ P vµo hÖ (4.20), ta sÏ ®−îc hÖ 6 ph−¬ng tr×nh víi 6 Èn lµ : 1 2 3 1 2 3, , , , ,d d dα α α . Gi¶i hÖ ph−¬ng tr×nh nµy ta sÏ thu ®−îc c¸c nghiÖm cÇn thiÕt. VÝ dô: - C¸c ®iÒu kiÖn ®Çu 10 20 30 / 2α α α π= = = 68 p10 =0; p20 =0; p30 = 4.44 Hai tam gi¸c lµ ®Òu vµ h = g = 8.89 - Qui luËt chuyÓn ®éng cña bµn di ®éng p1=0; p2=0; p3=p30*(1+0.08*sin(10*t)); - KÕt qu¶ m« pháng sè b»ng Matlab: H×nh 4.5.9: §å thÞ vÞ trÝ ch©n sè 1 69 H×nh 4.5.10: §å thÞ vÞ trÝ ch©n sè 2 H×nh 4.5.11: §å thÞ vÞ trÝ ch©n sè 3 70 H×nh 4.5.12: §å thÞ vËn tèc ch©n sè 1 H×nh 4.5.13: §å thÞ vËn tèc ch©n sè 2 71 H×nh 5.1.14: §å thÞ vËn tèc ch©n sè 3 - KÕt qu¶ m« pháng ®éng b»ng Matlab: H×nh 4.5.15: VÝ dô m« pháng ®éng RBSS 3 ch©n RPS (bµi to¸n ®éng häc ng−îc) 72 5.2. Robot song song 3 RSS KÝch th−íc cña Deltarca nh− sau: B¸n kÝnh bµn cè ®Þnh: ra = 200 (mm) B¸n kÝnh bµn di ®éng: rb = 50 §é dµi c¸nh tay: AC = 365 §é dµi c¼ng tay: BC = 800 VÞ trÝ c¸c ch©n r«-bèt x¸c ®Þnh bëi c¸c gãc: 1 2 30, 120, 240α α α= = = - Dao diÖn chÝnh cña ch−¬ng tr×nh 5.2.1. Bµi to¸n ®éng häc thuËn - Cho biÕt qui luËt chuyÓn ®éng cña c¸c gãc ë kh©u c¸nh tay r«-bèt. 1ϕ = t 2ϕ = t 3ϕ = t 73 Trong ®ã ®iÒu kiÖn ®Çu: 10 20 300, 0, 0ϕ ϕ ϕ= = = - KÕt qu¶ m« pháng sè b»ng Matlab: H×nh 4.5.16: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng x H×nh 4.5.17: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng y 74 H×nh 4.5.18: §å thÞ vÞ trÝ ®iÓm P theo ph−¬ng z H×nh 4.5.19: §å thÞ vÞ trÝ ®iÓm P 75 - KÕt qu¶ m« pháng ®éng b»ng Matlab: H×nh 4.5.20: VÝ dô m« pháng ®éng RBSS 3 ch©n RSS 5.2.2. Bµi to¸n ®éng häc ng−îc - C¸c ®iÒu kiÖn ®Çu p10 =50; p20 =0; p30 = -615 (mm) - Cho biÕt tr−íc quy luËt chuyÓn ®éng cña bµn di ®éng p1=50cos(t); p2=50sin(t); p3=p30; 76 - KÕt qu¶ m« pháng sè: H×nh 4.5.21: §å thÞ vÞ trÝ gãc ch©n sè 1 H×nh 4.5.22: §å thÞ vÞ trÝ gãc ch©n sè 2 77 H×nh 4.5.23: §å thÞ vÞ trÝ gãc ch©n sè 3 - KÕt qu¶ m« pháng ®éng b»ng Matlab: H×nh 4.5.24: VÝ dô m« pháng ®éng RBSS 3 ch©n RSS 78 VI. kÕt luËn 79 Mét lÜnh vùc øng dông næi bËt cña robot song song (RBSS) lµ lµm thiÕt bÞ ®å g¸ (CNC) ®iÒu khiÓn theo ch−¬ng tr×nh m¸y tÝnh. TiÕp cËn víi lÜnh vùc nµy còng nh− víi c¸c lÜnh vùc øng dông kh¸c cña RBSS, §Ò tµi ®· nhËn ®−îc kÕt qu¶ b−íc ®Çu, chñ yÕu vÒ ch−¬ng tr×nh tÝnh to¸n, ®iÒu khiÓn, nh−ng rÊt c¬ b¶n v× ®ã lµ nh÷ng c¬ së ®Ó triÓn khai c¸c øng dông trong thùc tÕ khi cã ®−îc sù ®Çu t− hoÆc ®Æt hµng cô thÓ sau nµy. Trong nh÷ng kÕt qu¶ ®ã nªn kÓ ®Õn: - Ph−¬ng ph¸p t¹o ra c¸c thiÕt bÞ ®å g¸ nhanh phèi gia c«ng b»ng c¸ch theo ch−¬ng tr×nh ®iÒu khiÓn sè thay ®æi chiÒu dµi 3 ch©n cña RBSS ph¼ng. - T¹o ra robot song song RBSS - 322 ho¹t ®éng nh− mét thiÕt bÞ gia c«ng CNC trong c«ng viÖc ®iªu kh¾c trªn nh÷ng vËt thÓ lín, cång kÒnh kh«ng ®−a lªn bÖ c¸c m¸y th«ng th−êng ®−îc. - §Ò xuÊt mét kiÓu Robot - “NhÖn n−íc” nh»m phôc vô c«ng viÖc phun röa c¸c khoang hÇm tÇu chë dÇu th«, rÊt khã thao t¸c b»ng tay vµ rÊt ®éc h¹i. 80 Tµi liÖu tham kh¶o 1. ABB Robotics - Catalog 2. Bruyninckx H. The 321 - HEXA - a fully - parallel manipulator with closed-form position and velocity kinematics. IEEE Int Conf. Robotics and Automation, 1997 3 .Clavel R. Delta, a fast robot with parallel geometry - Int. Symp. Industrial Robots, Lausanne, Switzerland, 1988 4. Honegger M., Codourey a., Burdet E. Adaptive control of the Hexaglide, a 6 dof. Parallel manipulator IEEE Int Conf. Robotics and Automation, Albuquerque, 1997 5. Lee H.Y And Roth B. A closed form solution of the forward displacement analysis of a class of in parallel mechanisnas IEEE Int. Conf. Robotics and Automation, Atlanta, 1993. 6. Proceedings of the 10th World Congress on the Theory of Machines and Mechanisms. Oulu, Finland, 1999. 7. Waldron K. J., Kincel G.L., The relation between actuator geometry and mechanical effcienncy in robots. Preprints 4th CISM – IFToMM Symposium on Theory and Pract. Of Manipul. Warsawa, 1981 8. Shimon Y. Nof, Handbook of Industrial Robotics, 2nded. John Wiley & Sons. Inc. 1999. 9. NguyÔn ThiÖn Phóc, Ng−êi m¸y pháng sinh vµ m« h×nh ®éng häc d¹ng tæng qu¸t. T¹p chÝ C¬ häc, sè 4, 1995. 10. NguyÔn ThiÖn Phóc “Robot c«ng nghiÖp” NXB khoa häc vµ kü thuËt, Hµ Néi, 2004. 81 11. NguyÔn ThiÖn Phóc, TrÇn Minh NghÜa, NguyÔn §×nh Nin. Nghiªn cøu t¹o dùng robot song song lo¹i hexa. TuyÓn tËp b¸o c¸o khoa häc Héi nghÞ Tù ®éng hãa toµn quèc lÇn thø VI.