›

Báo cáo Các kết quả nghiên cứu theo nhiệm vụ 4 - Đề tài KC.03.08 - Nhóm sản phẩm đồ gá CNC

pdf 123 trang yendo 8691
Download
Bạn đang xem 20 trang mẫu của tài liệu "Báo cáo Các kết quả nghiên cứu theo nhiệm vụ 4 - Đề tài KC.03.08 - Nhóm sản phẩm đồ gá CNC", để tải tài liệu gốc về máy bạn click vào nút DOWNLOAD ở trên

Tài liệu đính kèm:

  • pdfbao_cao_cac_ket_qua_nghien_cuu_theo_nhiem_vu_4_de_tai_kc_03.pdf

Nội dung text: Báo cáo Các kết quả nghiên cứu theo nhiệm vụ 4 - Đề tài KC.03.08 - Nhóm sản phẩm đồ gá CNC

  1. BỘ KHOA HỌC VÀ CÔNG NGHỆ CHƯƠNG TRÌNH KC.03 YZ YZ YZ YZ YZ YZY YZ YZ YZY YZ YZ YZY “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à SỐ: KC.03.08 BÁO CÁO CÁC KẾT QUẢ NGHIÊN CỨU THEO NHIỆM VỤ 4 - ĐỀ TÀI KC.03.08 Nhãm s¶n phÈm ®å g¸ cnc 6246-4 25/12/2006 HÀ NỘI 2006
  2. Môc lôc I. Më ®Çu 3 II. C¬ cÊu RBSS 3 ch©n 6 2.1. Robot song song 3 ch©n RPS 7 2.1.1 KÕt cÊu h×nh häc 7 2.1.2. BËc tù do 8 2.1.3. HÖ trôc täa ®é 9 2.1.4. Bµi to¸n vÞ trÝ 11 2.1.5. VÝ dô tÝnh to¸n 18 2.2. Robot song song 3 RSS 22 2.2.1 KÕt cÊu h×nh häc 22 2.2.2. BËc tù do 22 2.2.3. HÖ trôc täa ®é vµ ký hiÖu 23 2.2.4. Ph−¬ng tr×nh liªn kÕt 24 2.2.5. Bµi to¸n ®éng häc ng−îc 26 2.2.6. Bµi to¸n ®éng häc thuËn 26 2.3. Robot song song ph¼ng 3 ch©n 26 2.4. §å g¸ gia c«ng vá hép ®éng c¬ xe m¸y 28 III. Robot song song RBSS 29 3.1. Giíi thiÖu chung 30 3.2. ThiÕt kÕ kÕt cÊu vµ lËp tr×nh ®iÒu khiÓn chuyÓn ®éng cña robot 30 3.3. S¬ ®å hÖ thèng ®iÒu khiÓn 33 3.4. Ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn RBSS – 322 35 IV. Robot “NhÖn n−íc” 56 4.1. Giíi thiÖu chung 57 1
  3. 4.2. M« t¶ Robot “NhÖn n−íc” 58 4.3. TÝnh to¸n x©y dùng ch−¬ng tr×nh ®iÒu khiÓn Robot 60 “NhÖn n−íc” 4.4. M« pháng ho¹t h×nh Robot “NhÖn n−íc” 60 V. M« pháng RBSS dïng lµm ®å g¸ CNC 61 5.1. M« pháng RBSS ch©n RPS 62 5.2. Robot song song 3 RSS 72 VI. KÕt luËn 78 Tµi liÖu tham kh¶o 80 2
  4. B¸o c¸o c¸c kÕt qu¶ nghiªn cøu theo nhiÖm vô 4 ®Ò tµi kc.03.08 Nhãm s¶n phÈm ®å g¸ cnc I. më ®Çu §å g¸ gia c«ng phô thuéc vµo lo¹i h×nh ph«i gia c«ng vµ quy tr×nh c«ng nghÖ ®èi víi lo¹i s¶n phÈm ®ang ®−îc gia c«ng. Trªn m¸y c«ng cô CNC mäi thao t¸c ®Òu ®−îc ®iÒu khiÓn b»ng sè. C¸c s¶n phÈm ®−îc gia c«ng trªn m¸y CNC rÊt ®a d¹ng. Ngoµi nh÷ng s¶n phÈm cã thÓ gia c«ng b»ng nh÷ng ®å g¸ cã s½n cña m¸y, cßn cã nhiÒu tr−êng hîp ph¶i tù t¹o ra ®å g¸ cho phï hîp. NÕu s¶n phÈm kh«ng ph¶i chØ chÕ t¹o ®¬n chiÕc mµ cã sè l−îng lín th× viÖc t¹o ra c¸c ®å g¸ nh¸nh, chÝnh x¸c vµ còng ®−îc ®iÒu khiÓn sè lµ lo¹i viÖc cã nhu cÇu bøc xóc. Mét trong nh÷ng gi¶i ph¸p kü thuËt lµm ®å g¸ CNC cã thÓ dïng robot song song (RBSS). VÊn ®Ò 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. Thùc chÊt, còng cã thÓ hiÓu Hexapod lµ mét lo¹i ®å g¸ CNC. 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 ®−îc ®Þnh vÞ vµ ®Þnh h−íng cña tÊm ®éng theo ý muèn. Víi ý thøc tiÕp cËn mét lÜnh vùc míi mÎ cña Robotics, vÊn ®Ò RBSS, §Ò tµi khi ®¨ng ký muèn nghiªn cøu t¹o dùng c¸c s¶n phÈm ®å g¸ CNC, trong ®ã néi dung chñ yÕu lµ Hexapod. Tuy nhiªn néi dung Hexapod kh«ng ®−îc duyÖt ®Ó cÊp kinh phÝ v× néi dung nµy trïng l¾p víi mét ®Ò tµi kh¸c. V× thÕ néi dung vÒ Hexapod kh«ng cã trong §Ò tµi nµy. 3
  5. C¬ cÊu RBSS cßn cã nhiÒu lo¹i h×nh kh¸c nhau. §Ò tµi ®· ®i s©u nghiªn cøu vµo 3 lo¹i sau vµ ®¹t ®−îc nh÷ng kÕt qu¶ b−íc ®Çu: 1. C¬ cÊu RBSS ph¼ng, 3 ch©n lµ lo¹i c¬ cÊu RBSS d¹ng ®¬n gi¶n nµy rÊt thÝch hîp cho ®å g¸ CNC. Phô thuéc vµo ®èi t−îng ®−îc gia c«ng, cã thÓ dïng lo¹i c¬ cÊu nµy víi nh÷ng biÕn thÓ rÊt kh¸c nhau. C¸c ch©n ®−îc thay ®æi ®é dµi cã thÓ b»ng xilanh thñy lùc, xilanh khÝ nÐn hoÆc b»ng vÝtme bi. Liªn quan ®Õn vÊn ®Ò nµy §Ò tµi ®· hoµn thµnh mét c«ng tr×nh nghiªn cøu t¹o ra thiÕt bÞ g¸ l¾p nhanh ®Ó gia c«ng vá hép ®éng c¬ xe m¸y trªn Trung t©m gia c«ng CNC nhËp tõ Anh Quèc cña C«ng ty §ång Th¸p. Ph−¬ng ¸n thiÕt kÕ ®· chän lùa lµ dïng hÖ thèng xilanh khÝ nÐn, bè trÝ t¸c ®éng lÇn l−ît vµo 3 ch©n cña c¬ cÊu RBSS. Hîp ®ång vµ b¶n thuyÕt minh c«ng tr×nh nµy giíi thiÖu trong phÇn phô lôc. 2. Robot song song RBSS - 322 cã thÓ ho¹t ®éng nh− mét thiÕt bÞ gia c«ng CNC. C¸c ch©n cña RBSS ®−îc xÕp thµnh 3 nh¸nh ®«i, mçi ch©n cã 2 phÇn: phÇn ch©n trªn vµ phÇn ch©n d−íi. D¸ng dÊp bªn ngoµi cña RBSS - 322 gièng nh− kiÓu Robot Flex - Picker cña H·ng ABB. Khi thiÕt kÕ ®· tham kh¶o c¸c th«ng tin chµo hµng cña H·ng nµy, nh−ng kÕt cÊu c¸c bé truyÒn bªn trong ®· ®¬n gi¶n hãa ®i nhiÒu cho phï hîp kh¶ n¨ng tù t¹o trong n−íc víi kho¶n kinh phÝ dµnh cho c«ng viÖc nµy rÊt h¹n chÕ. §Þnh h−íng øng dông RBSS trong c«ng viÖc ®iªu kh¾c trªn c¸c vËt liÖu dÔ gia c«ng nh−ng cã kÝch th−íc lín, cång kÒnh, kh«ng ®−a lªn c¸c m¸y gia c«ng th«ng th−êng ®−îc. PhÇn ®¹t ®−îc kÕt qu¶ h¬n vµ thu ho¹ch ®−îc nhiÒu h¬n lµ x©y dùng ch−¬ng tr×nh phÇn mÒm ®iÒu khiÓn RBSS - 322. 3. Robot “NhÖn n−íc” lµ mét ®Ò xuÊt míi, ®−îc ph¸t triÓn trªn nguyªn lý RBSS vµ gåm 2 phÇn. PhÇn thø nhÊt lµ mét RBSS ho¹t ®éng theo nguyªn lý c¬ cÊu Stewart. “TÊm ®éng” ®−îc treo trªn “tÊm gi¸” cè ®Þnh 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. PhÇn thø 2 g¾n víi tÊm ®éng 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ã 4
  6. thÓ “xße réng” ra hoÆc “co côm” l¹i. HÖ thèng nµy lµm nhiÖm vô 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 ®−îc, vÝ dô v× m«i tr−êng qu¸ ®éc h¹i. HÖ thèng 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 h¹n hÑp vµ khi ®· lät qua miÖng hÇm sÏ “xße réng” ra, ®ång thêi nhê kh¶ n¨ng thay ®æi “®Þnh vÞ vµ ®Þnh h−íng” cña tÊm ®éng, g¾n liÒn víi hÖ thèng 4 c¬ cÊu nµy, mµ quü ®¹o phun n−íc ®ang ®−îc ®iÒu khiÓn theo ý muèn. Mäi thao t¸c nh− lµm thay ®æi ®é dµi cña 6 d©y c¸p treo tÊm ®éng vµ lµm “co côm” hoÆc “xße réng” 4 c¬ cÊu b×nh hµnh mang c¸c vßi phun n−íc, ®Òu ®−îc ®iÒu khiÓn theo ch−¬ng tr×nh vµ c¸c thiÕt bÞ ®iÒu khiÓn ®Òu ®−îc l¾p ®Æt phÝa trªn miÖng hÇm, nªn rÊt thuËn tiÖn. Néi dung nghiªn cøu cña c«ng tr×nh nµy ®−îc xuÊt ph¸t tõ nh÷ng lÇn trao ®æi, bµn b¹c víi Vietxopetro vÒ nhiÖm vô rÊt bøc xóc thay thÕ c¸c lao ®éng d−íi c¸c khoang hÇm tÇu chë dÇu th«, võa rÊt ®éc h¹i, võa rÊt khã röa s¹ch nªn rÊt chãng rØ lµm thñng vá tµu. C¸c kÕt qu¶ nghiªn cøu tÝnh to¸n vµ m« pháng ho¹t h×nh c¸c thao t¸c cña “Robot - NhÖn n−íc” phôc vô cho nhiÖm vô nµy ®· ®−îc tr×nh bµy víi XÝ nghiÖp C¬ ®iÖn dÇu khi thuéc liªn doanh DÇu khÝ Vietxopetro vµ ®· ®−îc ®−a vµo kÕ ho¹ch triÓn khai. Nh−ng v× cã sù thay ®æi cña c¬ quan øng dông nªn ®Õn nay vÉn ch−a thùc hiÖn ®−îc. 5
  7. II. C¬ cÊu RBSS 3 ch©n 6
  8. 2.1. Robot song song 3 ch©n RPS 2.1.1. KÕt cÊu h×nh häc R«-bèt song song 3 RPS (Tªn gäi R«-bèt song song 3 RPS lµ do lo¹i r«-bèt nµy cã cÊu t¹o gåm 3 ch©n, mçi ch©n gå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. 7
  9. 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: 8
  10. j Fnj=−−+−λ(1)∑ ffib i=1 Víi λ ===6,njfb 8, 9, = 0 , ba khíp quay, ba khíp l¨ng trô, vµ ba khíp cÇu, ta cã: F =−−+++−=6(8 9 1) (3.1 3.1 3.3) 0 3 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 z B3 x P 3 α y 3 B1 B2 x z 3 z0 α1 x x 2 1 α 2 A 3 x 0 A 1 O y z 1 0 z 2 A2 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. 9
  11. {Aixiyizi}(i=1,2,3) : HÖ ®éng g¾n víi ch©n thø i. uuuur Trong ®ã xiii≡ AB 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Ï. αii= zx0 Sö dông c¸c ký hiÖu: A RB : Ma trËn cosin chØ h−íng cña hÖ {Pxyz} so víi hÖ cè ®Þnh {Ox0y0z0}. A Ri : Ma trËn cosin chØ h−íng cña hÖ {Aixiyizi} so víi hÖ cè ®Þnh {Ox0y0z0}. ai : Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm Ai trªn hÖ cè ®Þnh. bi : Vector ®¹i sè chøa c¸c täa ®é cña ®iÓm Bi trªn hÖ cè ®Þnh. B bi : 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 ®ã : A C¸c ma trËn Ri cã thÓ biÓu diÔn d−íi d¹ng: rr rr rr ⎡⎤ee01ii 1 ee 01 2 ee 01 i 3 AR = ⎢⎥eerr eerr eerr (i=1,2,3) (2.1) i ⎢⎥02ii 1 02 2 02 i 3 rr rr rr ⎣⎦⎢⎥ee03ii 1 ee 03 2 ee 03 i 3 rrr eee01,, 02 03 : Lµ 3 vector ®¬n vÞ trªn c¸c trôc Ox0, Oy0,Oz0. rrr eeeii123,, i : 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 . 10
  12. A Ma trËn RB 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µ ψ . B ai vµ bi : 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: uuur uuur uuuur OBiiii=+ OA A B (i=1,2,3) (2.2) uuur uuuruuur vµ : OBii=+ OP PB (i=1,2,3) (2.3) Hay d−íi d¹ng ®¹i sè: ⎡di ⎤ ba=+A R . ⎢ 0 ⎥ (i=1,2,3) (2.4) ii i ⎢ ⎥ ⎣⎢ 0 ⎦⎥ A B vµ : bPi =+RB . bi (i=1,2,3) (2.5) KÕt hîp hai ph−¬ng tr×nh trªn ta cã: ⎡di ⎤ P + A R .B b = a + A R .0⎢ ⎥ (i=1,2,3) (2.6) B i i i ⎢ ⎥ ⎣⎢ 0 ⎦⎥ Trong ®ã: T T T B ⎡⎤ P = ⎣⎦⎡⎤p1,pp 2, 3 ; bi = ⎣⎦bbbix,, iy iz ; ai = ⎣⎡aaii1, 2, a i 3 ⎦⎤ ⎡⎤uvwx xx ⎡uvwix ix ix ⎤ A R = ⎢⎥uvw ; A R = ⎢uvw⎥ (i=1,2,3) (2.7) B ⎢⎥yy y i ⎢ iy iy iy ⎥ ⎣⎦⎢⎥uvwz zz ⎣⎢uvwiz iz iz ⎦⎥ 11
  13. A3 Z 3 β3 η 3 O η β Z 2 2 2 A1 β1 A2 X0 Z 1 Y0 H×nh 4.2.3 R C¸c ma trËn cosin chØ h−íng A i ®−îc x¸c ®Þnh bëi c¸c phÐp quay liªn tiÕp: A Ma trËn R1 ®−îc x¸c ®Þnh bëi c¸c phÐp quay sau : • Quay mét gãc (π /2− β1) quanh trôc z. • Quay quanh trôc x mét gãc (π /2). • Quay quanh trôc z mét gãc (π /2+α1 ). A Ma trËn R2 ®−î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 ). A Ma trËn R3 ®−î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 (π /2− β3 ) quanh trôc z. 12
  14. • Quay quanh trôc x mét gãc (π /2). • Quay quanh trôc z mét gãc (π /2+α3 ). NÕu ta coi Aαi 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× : ⎡⎤⎡10 0−− sinαii cos0α ⎤ A = ⎢⎥⎢00−− 1.cosαα sin 0 ⎥ αi ⎢⎥⎢ii ⎥ ⎣⎦⎣⎢⎥⎢01 0 0 0 1 ⎦⎥ ⎡⎤−−sinαii cosα 0 A = ⎢⎥001 (2.8) αi ⎢⎥ ⎣⎦⎢⎥cosααii− sin 0 (/2π − β ) γ §Æt : i = i ; VËy ta cã : A RA111= z ().γ Aα ⎡⎤⎡⎤cosγ11−−− sinγαα 0 sin 1 cos 1 0 AR = ⎢⎥⎢⎥sincos00γγ 01 1 ⎢⎥⎢⎥11 ⎢⎥⎢⎥⎣⎦⎣⎦001cossin0αα11− ⎡⎤−−cosγ11 sinαγαγ cos 1 cos 1 sin 1 A R = ⎢⎥−sinγ sinαγαγ sin cos cos 1 ⎢⎥11 1 1 1 ⎣⎦⎢⎥cosαα11 sin 0 A RA2222= zz().().ηγ A Aα ⎡⎤⎡⎤⎡⎤cosη22−−−− sinηγγαα 0 cos 22 sin 0 sin 2 cos 2 0 A R = ⎢⎥⎢⎥⎢⎥sinηη cos 0 sin γ cos γ 0 0 0 1 2 ⎢⎥⎢⎥⎢⎥22 22 ⎣⎦⎣⎦⎣⎦⎢⎥⎢⎥⎢⎥001001cossin0αα22− 13
  15. A R2 = ⎡⎤()()CCη2γηγα 2−−+−− SS 22 S 2 CS ηγηγα 22 SC 2 2 C 2 CS ηγηγ 22 SC 2 2 ⎢⎥−+()()SCη γηγα CS S −+ SC ηγηγαηγηγ CS C −+ SS CC ⎢⎥22 22 2 22 22 2 22 22 ⎢⎥⎣⎦CSαα22− 0 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 A RA3333=−zz().().ηγ A Aα ⎡⎤⎡⎤cosη33 sinηγγαα 0 cos 3− sin 3 0⎡−− sin 3 cos 3 0⎤ A R = ⎢⎥⎢⎥−sinηη cos 0 sin γ cos γ 0⎢ 0 0 1⎥ 3 ⎢⎥⎢⎥33 3 3⎢ ⎥ ⎣⎦⎣⎦⎢⎥⎢⎥001001cossin0⎣⎢ αα33− ⎦⎥ A R3 = ⎡⎤−+()()CCη3γηγα 3 SS 33 S 3 −+ CC ηγηγα 3 3 SS 33 C 3 −+ CS ηγηγ 33 SC 3 3 ⎢⎥−−()()SCη γηγα + CS S −− SC ηγηγαηγηγ + CS C SS + CC ⎢⎥33 33 3 33 33 3 33 33 ⎣⎦⎢⎥CSαα330 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 A Ta thÊy c¸c thµnh phÇn cña c¸c ma trËn Ri 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 ai3 = 0 (i=1,2,3) A1 trªn trôc X0 nªn a12 = 0 Vµ Bi thuéc mÆt ph¼ng X0Y0 nªn biz = 0 (i=1,2,3) 14
  16. ⎧ p111111+=+ubxx a u x. d ⎪ +Víi i =1: ⎨ pubud2111+=yx y ⎪ ⎩ pubud3111+=zx z ⎧ audp+− u = 11 1x 1 1 ⎪ x b ⎪ 1x ⎪ ud11y − p 2 Suy ra : ⎨uy = (2.9) ⎪ b1x ⎪ ud11z − p 3 ⎪uz = ⎩ b1x +Víi i=2 ⎧ pubvba12++=+xx xy 22122 ud x (a) ⎪ ⎨ pubvba22++=+yx yy 22222 ud y (b) (2.10) ⎪ ⎩ pubvbud32++=zx zy 222 z (c) +Víi i=3 ⎧ pubvba13++=+xx xy 33133 ud x (a) ⎪ ⎨ pubvba23++=+yx yy 33233 ud y (b) (2.11) ⎪ ⎩ pubvbud33++=zx zy 333 z (c) Thùc hiÖn c¸c phÐp biÕn ®æi sau: ⎧((3.10aa )λ − (3.11 )) ⎪ ⎨((3.10bb )λ − (3.11 )) ⎪ ⎩((3.10cc )λ − (3.11 )) ⎧ p1(1)λλλ−+uaudaudxxx 1 = ( 21 + 2 2 )( − 31 + 3 3 ) ⎪ Ta ®−îc: ⎨ p2(1)λλλ−+uaudaudyyy 1 = ( 22 + 2 2 )( − 32 + 3 3 ) (2.12) ⎪ ⎩ puudud312233(1)λλλ−+zzz = − ) 15
  17. b3 y Víi λ = , λ12=−bbxλ 3x b2 y Thay c¸c kÕt qu¶ cña hÖ (2.9) vµo hÖ (2.12) ta ®−îc: ⎧ audp+− p (1)λλλ−+11 1x 1 1 = (audaud + )( − + ) ⎪ 1b 1 21 2xx 2 31 3 3 ⎪ 1x ⎪ ud11y − p 2 ⎨ paudaud2(1)λλλ−+ 1 = ( 22 + 2yy 2 )( − 32 + 3 3 ) (2.13) ⎪ b1x ⎪ ud11z − p 3 ⎪ pudud312233(1)λλλ−+ =zz − ) ⎩ b1x MÆt kh¸c, dùa vµo kÕt cÊu cña bµn di ®éng B ta cã : B3 b 2 b 1 B1 b B2 3 H×nh 2.2.4 2 uuuur T 2 BB12 = ()()bb12−− bb 12=b3 2 uuuur T 2 BB13 = ()()bb13−−= bb 13b2 2 uuuur T 2 BB23= ()()bb23−− bb 23=b1 ⎡⎤di víi : ba=+A R . ⎢⎥0 (i=1,2,3) ii i ⎢⎥ ⎣⎦⎢⎥0 16
  18. ⎡⎤aud11+ 1x 1 ⎡aud21+ 2x 2 ⎤ ⎡aud31+ 3x 3 ⎤ ⇒=b ⎢⎥ud ; b = ⎢aud+ ⎥ ; b = ⎢aud+ ⎥ 1 ⎢⎥11y 2 ⎢ 22 2y 2 ⎥ 3 ⎢ 32 3y 3 ⎥ ⎣⎦⎢⎥ud11z ⎣⎢ ud22z ⎦⎥ ⎣⎢ ud33z ⎦⎥ T ⎡⎤⎡⎤a11−+ a 21 ud 1xx 1 − ud 2 2 a 11 −+ a 21 ud 1 xx 1 − ud 2 2 ⇒−−⎢⎥⎢⎥ud a ud. ud −−= a ud b2 ⎢⎥⎢⎥11yy 222 2 11 yy 222 2 3 ⎣⎦⎣⎦⎢⎥⎢⎥uud122zz−− uud 122 zz T ⎡⎤⎡⎤aaududaaudud11−+ 31 1xx 1 − 3 3 11 −+ 31 1 xx 1 − 3 3 ⎢⎥⎢⎥ud−− a ud. ud −− a ud = b2 ⎢⎥⎢⎥11yy 3233 11 yy 3233 2 ⎣⎦⎣⎦⎢⎥⎢⎥ud11zz−− ud 33 ud 11 zz ud 33 T ⎡⎤⎡⎤aaududaaudud21−+ 31 2xx 2 − 3 3 21 −+ 31 2 xx 2 − 3 3 ⎢⎥⎢⎥a−+ a ud − ud. a −+ a ud − ud = b2 ⎢⎥⎢⎥22 32 2yy 2 3 3 22 32 2 yy 2 3 3 1 ⎣⎦⎣⎦⎢⎥⎢⎥ud22zz−− ud 33 ud 22 zz ud 33 Hay : 2222 ⎧()()()aaudud11−+ 21 1xx 1 − 2 2 + udaud 1 y 1 −− 22 2 y 2 + ududb 1 zz 1 − 2 2 = 3 ⎪ ⎪ 2222 ⎨()()()aaudududaud11−+ 31 1xx 1 − 3 3 + 1 y 1 −− 32 3 y 3 + ududb 1 zz 1 − 3 3 = 2 ⎪ 2222 ⎩⎪()()()a21−+ a 31 ud 2xx 2 − ud 3 3 + a 22 −+ a 32 ud 2 yy 2 − ud 3 3 + ud 2 zz 2 − ud 3 3 = b 1 KÕt hîp víi hÖ (2.13) ta cã hÖ 6 ph−¬ng tr×nh, 6 Èn: (2.14) 17
  19. HÖ ph−¬ng tr×nh (2.14) chøa 9 Èn sè α123123123,,,,,,,,ααddd pp p. C¸c thµnh phÇn uuuix,, iy iz ®· x¸c ®Þnh ®−îc, c¸c thµnh phÇn uuux ,,yz 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 A ph¶i t×m vÞ trÝ cña bµn m¸y ®éng P vµ ma trËn RB. 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µ : α123123,,,,,αα p pp 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µ : α123123,,,,,ααddd. 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. 18
  20. - PB1 = h; OA1 = g; η23==ηπ2/3 - Do kÕt cÊu cña c¬ cÊu ta cã zABiii⊥ - Trôc zOAii⊥ ⇒ βi = π /2 Khi ®ã c¸c ®¹i l−îng trong c«ng thøc (4.6) trë thµnh : ⎡ h ⎤ ⎡ h ⎤ − − ⎢ 2 ⎥ ⎢ 2 ⎥ ⎡⎤h ⎢ ⎥ ⎢ ⎥ B B ⎢ 3 ⎥ B ⎢ 3 ⎥ b = ⎢⎥0 ; b = h ; b = −h (2.15) 1 ⎢⎥ 2 ⎢ 2 ⎥ 3 ⎢ 2 ⎥ ⎣⎦⎢⎥0 ⎢ ⎥ ⎢ ⎥ ⎢ 0 ⎥ ⎢ 0 ⎥ ⎣⎢ ⎦⎥ ⎣⎢ ⎦⎥ ⎡⎤g ⎡ g ⎤ − − ⎢⎥2 ⎢ 2 ⎥ ⎡⎤g ⎢⎥ ⎢ ⎥ ⎢⎥3 ⎢ 3 ⎥ a = ⎢⎥0 ; a = g ; a = −g (2.16) 1 ⎢⎥ 2 ⎢⎥2 3 ⎢ 2 ⎥ ⎣⎦⎢⎥0 ⎢⎥ ⎢ ⎥ ⎢⎥0 ⎢ 0 ⎥ ⎣⎦⎢⎥ ⎣⎢ ⎦⎥ Do βi = π /2 nªn γ i =(/2π − βi ) = 0 A Khi ®ã c¸c ma trËn cosin chØ h−íng Ri trë thµnh: ⎡⎤−−cosγ11 sinαγαγ cos 1 cos 1 sin 1 A R = ⎢⎥−sinγ sinαγαγ sin cos cos 1 ⎢⎥11 1 1 1 ⎣⎦⎢⎥cosαα11 sin 0 ⎡⎤−sinα11 cosα 0 A R = ⎢⎥001 (2.17) 1 ⎢⎥ ⎣⎦⎢⎥cosαα11 sin 0 19
  21. A R2 = ⎡ 13 13 13⎤ ⎢ ( cosγ 22222222+−+− sinγα )sin ( cos γ sin γ )cos α sin γ cos γ⎥ ⎢ 22 22 22⎥ ⎢ 31 31 31⎥ ⎢(−+ cosγ 222 sinγα )sin ( cos γ 222 − sin γ )cos α −− sin γ 2 cos γ 2⎥ ⎢ 22 22 22⎥ ⎢ cosαα22 sin 0 ⎥ ⎢ ⎥ ⎣ ⎦ ⎡⎤113 ⎢⎥sinαα22−− cos ⎢⎥222 A ⎢⎥33 1 R2 = ⎢⎥−−sinαα22 cos (2.18) ⎢⎥22 2 ⎢⎥cosαα22 sin 0 ⎢⎥ ⎣⎦ A R3 = ⎡⎤13 13 13 ⎢⎥( cosγ 33333333−−++ sinγα )sin ( cos γ sin γα )cos sin γ cos γ ⎢⎥22 22 22 ⎢⎥31 31 31 ⎢⎥( cosγ 333+−− sinγα )sin ( cos γ 3333 sin γ )cos α sin γ − cos γ 3 ⎢⎥22 22 22 ⎢⎥cosαα33 sin 0 ⎢⎥ ⎣⎦ ⎡⎤113 ⎢⎥sinαα33− cos ⎢⎥222 A ⎢⎥331 R3 = ⎢⎥sinαα33−− cos (2.19) ⎢⎥222 ⎢⎥cosαα33 sin 0 ⎢⎥ ⎣⎦ b3 y Khi ®ã : λ = = -1 ; λ12= bbxλ − 3x = h; b2 y Thay vµo hÖ (2.14) ta ®−îc : 20
  22. ⎧ 1 3pd=+− ( sinαα d sin ) d sin α ⎪ 12233112 ⎪ ⎪ − 3 ⎪3(sinsin)pdd22233=−αα ⎪ 2 ⎨3pd=+ cosααα d cos + d cos (4.20) ⎪ 31 12 23 3 2 22 2 ⎪3ggd−− 311 sinαααα 3 gddd 2 sin 2121212 + sin sin ++ dd−=2coscos3dd12αα 1 2 h ⎪ 2 22 2 ⎪3ggd−− 31 sinαααα 1 3 gddd 3 sin 3 + 13 sin 1 sin 3 ++−= dddd 1 3 2 13 cos αα 1 cos 3 3 h ⎪ 2 22 2 ⎩3ggd−−+ 322 sinαααα 3 gddd 33232323232 sin sin sin ++−= dddd 2 cos αα cos 3 3 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), A ta ph¶i t×m vÞ trÝ cña bµn m¸y ®éng P vµ ma trËn RB. 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µ : α123123,,,,,αα p pp 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µ : α123123,,,,,ααddd. 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). 21
  23. 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: j Fnj=−−+−λ(1)∑ ffib i=1 Víi λ ==6,njfb 11, = 15, = 6 , 12 khíp cÇu vµ 3 khíp quay, ta cã: F =−−++−=6(11 15 1) (12.3 3.1) 6 3 22
  24. 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 Axyziiii g¾n víi t©m cña khíp quay Ai , sao cho xi n»m theo h−íng kÐo dµi cña OAi vµ trôc yi däc theo trôc c¸c khíp quay, cßn trôc zi song song víi z. Gãc αi ®o tõ x ®Õn xi 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 xi vµ ACii. H×nh 4.2.6: §Æt hÖ trôc täa ®é lªn r«-bèt 23
  25. 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 Ai so víi hÖ täa ®é cè ®Þnh Oxyz. Cßn biP lµ to¹ ®é cña ®iÓm Bi 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 Ai (H×nh 4.2.7): tuú thuéc vµo kÝch th−íc bµn cè ®Þnh T T ai==[] xai,, yai zai[] ra *cos(),*sin(),0ααii ra (2.21)  Täa ®é ®iÓm Ci ci==+[] xci,, yci zciT ai Ri . ciA (2.22) Trong ®ã: Š Ri lµ ma trËn cosin chØ h−íng cña hÖ trôc Axyziiii 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(αii )− sin(α ) 0 RRz==(,ααα )⎢⎥ sin() cos() 0 (2.23) ii⎢⎥ i i ⎣⎦⎢⎥001 Š ciA lµ täa ®é cña ®iÓm Ci trong hÖ trôc Axyziiii (H×nh 4.2.4) T ciA=−[] la*cos(ϕϕii ),0, la *sin( ) (2.24) 24
  26. H×nh 4.2.7 H×nh 4.2.8 Thay (2.23) vµ (2.24) vµo (2.22) ta ®−îc: T ci=+[]cos(αϕαϕϕiiiii )*( ra la *cos( )),sin( )*( ra +− la *cos( )), la *sin( ) (2.25)  X¸c ®Þnh täa ®é ®iÓm B T bi==+[ xbi,, ybi zbi] p Rp * biP (2.26) Trong ®ã: T p = [ 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: ⎡⎤100 Rp = ⎢⎥010 ⎢⎥ (2.28) ⎣⎦⎢⎥001 Š biP lµ täa ®é cña ®iÓm Bi trong hÖ trôc täa ®é Puvw: T T biP==[] xbiP,, ybiP zbiP[] rb *cos(),*sin(),0ββii rb (2.29) Thay (2.27), (2.28), (2.29) vµo (4.26) ta ®−îc: 25
  27. ⎡⎤px+ rb*cos(αi ) bi=+⎢⎥ py rb*sin(α ) ⎢⎥i (2.20) ⎣⎦⎢⎥pz C¸c ®iÓm Bi vµ Ci rµng buéc víi nhau bëi ®iÒu kiÖn: lb2222==−+−+− biciT *( 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: px,,;,, py pz ϕ123ϕϕ. 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: ϕ123,,ϕϕ 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 ϕ123,,ϕϕ 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. 26
  28. 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. A3 E C B 3 B 2 D A1 B1 A2 H×nh 4.2.8. RBSS ph¼ng 3 ch©n RRR E A3 C B3 B2 A2 A1 D B 1 H×nh 4.2.9. RBSS ph¼ng 3 ch©n PRR 27
  29. C H F E TÊm ®éng P I R A B P G TÊm cè ®Þnh D 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. 28
  30. III. Robot song song RBSS – 322 29
  31. 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 30
  32. Hình 4.3.1. Bản vẽ 3D thiết kế RBSS-322 Hình 4.3.2. Ảnh chụp RBSS - 322 31
  33. 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. Begin Nhập dữ liệu Sai Trong vùng làm việc Đúng Giải bài toán Động học nghịch Truyền tín hiệu điều khiển tới R232C Đúng Nhận Tín hiệu từ Sai R232C End 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. 32
  34. 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. DB9 2 MAX2 TxD AD ANALOG 3 32 PO 0809 INPUT RxD 5 89C51 P1 P2 Joy- CONTROL stick BUS 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 33
  35. 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). CONTROL BUS Mạch công DC EN SENSOR suất Kiểm soát dòng 89c2051 Mạch phối hợp Mạch phối hợp Hình 4.3.6. Sơ đồ chức năng bảng mạch bộ điều khiển động cơ SENSER vị trí Thực hiện Kiểm soát BUS Nhận lệnh điều lệnh dòng ĐC khiển ĐK Động cơ Đọc Encoder 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 34
  36. 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); 35
  37. 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') 36
  38. 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); 37
  39. % 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 % 38
  40. 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 39
  41. 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 40
  42. if x1(i) zB if x2(i)>xTB xb=x2(i); 41
  43. 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); 42
  44. 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); 43
  45. 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 44
  46. 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 45
  47. 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); 46
  48. 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 47
  49. 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 48
  50. 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 49
  51. 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 50
  52. 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; 51
  53. 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); 52
  54. 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) 53
  55. % 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) 54
  56. fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) end % fclose(s); end 55
  57. IV. Robot “NhÖn n−íc 56
  58. 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. 57
  59. 3 2 1 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 58
  60. 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 59
  61. 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” 60
  62. V. M« pháng RBSS dïng lµm ®å g¸ CNC 61
  63. 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 A ph¶i t×m vÞ trÝ cña bµn m¸y ®éng P vµ ma trËn RB. 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µ : α123123,,,,,αα p pp 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) 62
  64. - 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 63
  65. 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 64
  66. 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 65
  67. 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 66
  68. - 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µ : α123123,,,,,ααddd. 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 67
  69. 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 68
  70. H×nh 4.5.10: §å thÞ vÞ trÝ ch©n sè 2 H×nh 4.5.11: §å thÞ vÞ trÝ ch©n sè 3 69
  71. 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 70
  72. 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) 71
  73. 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: α12= 0,αα== 120, 3 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 72
  74. Trong ®ã ®iÒu kiÖn ®Çu:ϕ10= 0,ϕϕ 20== 0, 30 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 73
  75. 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 74
  76. - 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; 75
  77. - 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 76
  78. 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 77
  79. VI. kÕt luËn 78
  80. 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. 79
  81. 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. 80
  82. 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. 81