Đồ án Nâng cao chất lượng hệ thống điều khiển quỹ đạo cho Robot Scara 3 bậc tự do ứng dụng phương pháp điều khiển trượt phi tuyến

quü ®¹o ®Æt ban ®Çu. Ch­¬ng 4 X©y dùng m« h×nh hÖ thèng ®iÒu khiÓn 4.1. CÊu tróc hÖ thèng ®iÒu khiÓn Robot. Quü ®¹o ®Æt Bé ®iÒu chØnh C¶m biÕn M«i tr­êng Tay m¸y H×nh 4.1: S¬ ®å cÊu tróc chung cña hÖ thèng ®iÒu khiÓn tay m¸y Robot n Robot th­êng tù ®éng thùc hiÖn c¸c nhiÖm vô dÞch chuyÓn hoÆc c¸c thao t¸c. C¸c ho¹t ®éng cña robot th­êng ®­îc lËp tr×nh. ViÖc thùc hiÖn c¸c nhiÖm vô lµm thay ®æi mét phÇn hoÆc toµn bé vÞ trÝ cña Robot trong kh«ng gian. Cã thÓ coi Robot hiÖn ®¹i nh­ nh÷ng ng­êi m¸y, cã thÓ thùc hiÖn c¸c thao t¸c phøc t¹p, cã ®é chÝnh x¸c cao. §iÒu khiÓn chuyÓn ®éng cña tay m¸y trong kh«ng gian lµ x¸c ®Þnh n thµnh phÇn m«men lùc tæng qu¸t t¸c ®éng lªn c¸c khíp, m«men lùc tæng qu¸t ®­îc cung cÊp bëi c¬ cÊu chÊp hµnh. Qu¸ tr×nh ®iÒu khiÓn ph¶i ®¶m b¶o bé ®iÒu khiÓn sÏ thùc hiÖn ®iÒu khiÓn chuyÓn ®éng cña c¸c khíp theo quü ®¹o q(t) sao cho q(t) lu«n b¸m q®(t), víi q®(t) lµ vect¬ quü ®¹o chuyÓn ®éng mong muèn. S¬ ®å cÊu tróc chung cña hÖ thèng ®iÒu khiÓn tay m¸y cho ë h×nh 4.2, tay m¸y sÏ b¸m theo quü ®¹o ®Æt tr­íc (®­îc lËp tr×nh s½n hoÆc cã thÓ ®­a vµo tõ ch­¬ng tr×nh phÇm mÒm kÕt nèi bªn ngoµi) sö dông bé ®iÒu chØnh ®Ó ®iÒu khiÓn chuyÓn ®éng vµ lùc, sö dông c¸c c¶m biÕn ®Ó thu nhËn c¸c th«ng sè vÒ m«i tr­êng lµm viÖc ®¶m b¶o cho ho¹t ®éng bÒn v÷ng, khö ®­îc nhiÔu ngoµi n, nhËn biÕt vµ ®¸p øng thÝch nghi ®­îc víi nh÷ng sù thay ®æi cña m«i tr­êng còng nh­ sù thay ®æi c¸c tham sè ®éng häc cña cña hÖ thèng: H×nh 4.2: S¬ ®å ®iÒu khiÓn chuyÓn ®éng trong kh«ng gian khíp. Trong ®ã c¶m biÕn trong cã thÓ lµ ®Üa quay, m¸y ph¸t tèc, chiÕt ¸p ®Ó ®o vÞ trÝ, tèc ®é vµ gia tèc cña khíp tay m¸y R«bèt. Tay m¸y robot ®­îc kÕt nèi bëi c¸c khíp, trªn c¸c khíp cã g¾n c¸c ®éng c¬ truyÒn ®éng (nÕu lµ khíp quay) hoÆc g¾n pitt«ng, xi lanh cïng víi bé truyÒn dÉn thñy lùc (nÕu lµ khíp chuyÓn ®éng tÞnh tiÕn). Encoder H×nh 4.3: L­îc ®å khíp tay m¸y Ph¶n håi vËn tèc Ph¶n håi vÞ trÝ TÝn hiÖu ®iÒu khiÓn T¶i C¸nh tay Hép sè §éng c¬ Ph¸t tèc Víi Robot Serpent, truyÒn ®éng cho hai khíp cña tay m¸y vµ cæ tay b»ng ®éng c¬ servo 1 chiÒu cã ph¶n håi vÞ trÝ t¹o thµnh 1 vßng ®iÒu khiÓn kÝn. C¸c khíp trong Robot ®ùoc truyÒn ®éng bëi hÖ thèng truyÒn ®éng ®iÖn, thuû lùc hoÆc khÝ nÐn. Tay m¸y Robot Serpent ®ßi hái ph¶i ®­îc ®iÒu khiÓn víi tèc ®é cao vµ chÝnh x¸c. V× vËy ta ph¶i thiÕt kÕ quü ®¹o dïng ph­¬ng ph¸p ®a thøc bËc, x©y dùng quü ®¹o lµ c¸c ®a thøc bËc cao, sö dông luËt ®iÒu khiÓn u ¸p dông chÕ ®é tr­ît dïng hµm chuyÓn m¹ch Sat víi ®iÒu kiÖn rµng buéc: Liªn tôc vÒ thêi gian Liªn tôc vÒ vÞ trÝ Liªn tôc vÒ vËn tèc 4.1.1. M« h×nh c¬ cÊu chuyÓn ®éng. Ph­¬ng tr×nh ®éng lùc häc cã d¹ng tæng qu¸t: (4.1) Ph­¬ng tr×nh ®éng lùc häc m« t¶ s¬ ®å khèi nh­ h×nh : H×nh 4.4: S¬ ®å khèi c¬ cÊu chuyÓn ®éng 4.1.2. HÖ thèng truyÒn ®éng HÖ thèng truyÒn ®éng ®iÖn sö dông c¸c ®éng c¬ ®iÖn mét chiÒu hoÆc ®éng c¬ ®ång bé. S¬ ®å cÊu tróc ®éng häc hÖ thèng ®iÒu khiÓn cho khíp nh­ ë h×nh 4.5. + §éng c¬ ®iÖn mét chiÒu nam ch©m ®iÖn vÜnh cöu. + J§: m« men qu¸n tÝnh ®éng c¬. + Jt: m« men qu¸n cña khíp. + Mt: m« men khíp. + i: tØ sè truyÒn. + hiÖu suÊt bé truyÒn. H×nh 4.5: M« h×nh truyÒn ®éng ®iÖn R«bèt. M« men c¶n quy ®æi vÒ trôc ®éng c¬: (4.2) Trong ®ã: Mt: M« men khíp. i: Tû sè truyÒn. : HiÖu suÊt cña bé truyÒn. M« h×nh to¸n häc ®éng c¬ vµ hÖ truyÒn ®éng khíp nh­ h×nh 4.6: H×nh 4.6: M« h×nh to¸n häc ®éng c¬. Trong ®ã: E(p) lµ søc ®iÖn ®éng bé biÕn ®æi. B lµ hÖ sè nhít. Th«ng th­êng B = 0 Khi tay Robot tiÕp xóc víi ®èi t­îng, Robot ph¶i sinh ra mét lùc phï hîp ®Ó ®­a c¸c chi tiÕt cÇn l¾p r¸p vµo ®óng vÞ trÝ, gäi lµ ®iÒu khiÓn tinh, liªn quan ®Õn ®iÒu khiÓn lùc, ®ång thêi còng võa ®iÒu khiÓn quü ®¹o. §iÒu khiÓn quü ®¹o cã thÓ th«ng qua ®iÒu khiÓn quü ®¹o khíp hoÆc ®iÒu khiÓn trùc tiÕp quü ®¹o tay. §èi víi viÖc ®iÒu khiÓn chuyÓn ®éng cho R«bèt th× th«ng th­êng trong qu¸ tr×nh lµm viÖc cÇn yªu cÇu ®iÒu khiÓn ®Çu tay n¾m R«bèt chuyÓn ®éng trong kh«ng gian decade. Tuy nhiªn viÖc ®iÒu khiÓn c¬ cÊu chÊp hµnh l¹i kh«ng thùc hiÖn trong kh«ng gian biÕn khíp, ®iÒu ®ã dÉn tíi trong thùc tÕ cã hai d¹ng s¬ ®å ®iÒu khiÓn tæng qu¸t: +S¬ ®å ®iÒu khiÓn chuyÓn ®éng trong kh«ng gian biÕn khíp. + S¬ ®å ®iÒu khiÓn chuyÓn ®éng trong kh«ng gian decade. Bé ®iÒu khiÓn th­êng ®­îc thiÕt kÕ hoÆc ®iÒu khiÓn thÝch nghi hoÆc ®iÒu khiÓn bÒn v÷ng c¸c ®¹i l­îng vËt lý. §iÒu khiÓn thÝch nghi lµ bé thiÕt kÕ lu«n t×m c¸ch tÝnh to¸n c¸c th«ng sè ch­a biÕt cña hÖ thèng tõ ®ã ®iÒu khiÓn hÖ thèng theo yªu cÇu. Nh­ vËy cã thÓ thÊy r»ng vÊn ®Ò x©y dùng m« h×nh to¸n häc cho mét ®èi t­îng ®iÒu khiÓn th­êng gÆp ph¶i hai khã kh¨n lµ ®é thiÕu chÝnh x¸c cña c¸c phÇn ®­îc m« t¶ vµ sù thiÕu hôt c¸c phÇn kh«ng thÓ m« t¶ do vËy lu«n tån t¹i c¸c sai lÖch gi÷a m« h×nh vµ ®èi t­îng thùc. §iÒu ®ã dÉn tíi chÊt l­îng ®iÒu khiÓn kh«ng ®¸p øng ®­îc yªu cÇu c«ng nghÖ vµ ra ®êi ph­¬ng ph¸p ®iÒu khiÓn míi cã chÊt l­îng cao h¬n nh»m kh¾c phôc c¸c h¹n chÕ kÓ trªn, ®ã lµ c¸c ph­¬ng ph¸p ®iÒu khiÓn bÒn v÷ng nh­ ®· tr×nh bµy ë ch­¬ng 2. 4.2. X©y dùng quü ®¹o chuyÓn ®éng chuÈn. Ta chän mét biÕn khíp bÊt kú qi thay cho gãc quay qi (i =1, 2, 3). Thêi gian chuyÓn ®éng tõ vÞ trÝ ®Çu (x0, y0, z0) tíi vÞ trÝ cuèi (xc, yc, zc) lµ td. Gi¸ trÞ ban ®Çu cña q t¹i thêi ®iÓm ban ®Çu (t = 0) lµ q0 vµ gi¸ trÞ t¹i t = td lµ qc. ; ; (4.3) §å thÞ quü ®¹o chuyÓn ®éng chuÈn ë trªn h×nh 4.4: 4.7: Quü ®¹o chuyÓn ®éng xuÊt ph¸t tõ q0 ®Õn qc sÏ qua ba giai ®o¹n: gia tèc, chuyÓn ®éng víi tèc ®é kh«ng ®æi vµ gi¶m tèc. §Ó ®¬n gi¶n cho viÖc tÝnh to¸n, cã thÓ chän tham sè t0 b»ng 1/2 kho¶ng thêi gian gia tèc hoÆc gi¶m tèc. Ta còng x¸c ®Þnh c¸c ®iÓm phô cña quü ®¹o chuyÓn ®éng q01 vµ q02 t¹i t = t0 vµ t = td – t0 lµ: q01 = q0 vµ qc2 = qc. Nèi q01 vµ qc2 b»ng mét ®­êng th¼ng vµ x¸c ®Þnh ®iÓm q02, qc1 t¹i t = 2t0 vµ t = td – 2t0. B»ng c¸ch chän ®iÓm phô nh­ trªn, quü ®¹o ®o¹n cd lµ mét d­êng th¼ng víi tèc ®é kh«ng ®æi, quü ®¹o ®o¹n ac vµ df cã thÓ chän lµ ®a thøc bËc bèn cã d¹ng: q = a0 + a1t + a2t2 + a3t3 + a4t4. (4.4) 4.2.1. X¸c ®Þnh gi¸ trÞ q02 vµ qc1. Nh­ c¸ch vÏ ®· tr×nh bµy ë trªn, ®­êng nèi be lµ ®­êng th¼ng. Gi¸ trÞ q02 vµ qc1 t¹i t = 2t0 vµ t = td – 2t0, cã thÓ ®­îc x¸c ®Þnh tõ gi¸ trÞ ®Çu q0 vµ cuèi qc dùa trªn ph­¬ng tr×nh bËc nhÊt cña ®­êng th¼ng be: (4.5) (4.6) 4.2.2. Ph­¬ng tr×nh ®o¹n cd: Quü ®¹o ®o¹n cd lµ ®­êng th¼ng biÓu diÔn bëi ph­¬ng tr×nh: (4.7) víi (2t0 £ t £ td – 2t0) Thay (4.5) vµo (4.7) vµ viÕt gän l¹i ta cã ®­îc: (4.8) Trong ®ã: (4.9) 4.2.3. Ph­¬ng tr×nh ®o¹n ac: Quü ®¹o ®o¹n ac biÓu diÔn bëi ph­¬ng tr×nh d¹ng ®a thøc bËc bèn (4.6): qac = a0ac + a1act + a2act2 + a3act3 + a4act4. (4.10) C¸c hÖ sè cña ph­¬ng tr×nh ®o¹n ac ®­îc x¸c ®Þnh tõ ®iÒu kiÖn ®Çu vµ cuèi: t¹i t = 0: (4.11) T¹i t = 2t0: (4.12) LÊy ®¹o hµm cÊp 1 vµ cÊp 2 cña (4.10) vµ sö dông (4.11) vµ (4.12), c¸c hÖ sè ®­îc x¸c ®Þnh nh­ sau: (4.13) 4.2.4. Ph­¬ng tr×nh ®o¹n df: Quü ®¹o ®o¹n df biÓu diÔn bëi ph­¬ng tr×nh d¹ng ®a thøc bËc bèn (4.6): qdf = a0df + a1dft + a2dft2 + a3dft3 + a4dft4 (4.14) C¸c hÖ sè cña ph­¬ng tr×nh ®o¹n ac ®­îc x¸c ®Þnh tõ ®iÒu kiÖn ®Çu vµ cuèi: (4.15) Cuèi cïng c¸c hÖ sè ®­îc x¸c ®Þnh nh­ sau: (4.16) Nh­ vËy quü ®¹o chuyÓn ®éng cña 3 khíp 1,2,4 cña Robot Serpent cã thÓ ®­îc x¸c ®Þnh b»ng c¸c ph­¬ng tr×nh (4.8), (4.10) vµ (4.14) víi c¸c hÖ sè cña c¸c ph­¬ng tr×nh ®­îc x¸c ®Þnh tõ c¸c gi¸ trÞ cña vÞ trÝ ®Çu vµ vÞ trÝ cuèi cña c¸c khíp t­¬ng øng bëi c¸c biÓu thøc: (4.9), (4.13) vµ (4.16). Quü ®¹o chuÈn cña 3 khíp ®­îc x©y dùng trªn m¸y tÝnh b»ng ch­¬ng tr×nh qdcdc.m víi néi dung ®­îc tr×nh bµy chi tiÕt trong phÇn Phô lôc 2. 4.3. Thiết kế điều khiển trượt cho tay máy robot Scara ba bậc tự do. 4.3.1. Hệ phương trình động lực học Lagrange Hệ phương trình động lực học Lagrange của tay máy Scara Serpent được viết dưới dạng ma trận sau : (4.17) hay: trong đó t1, t2 và t4 lần lượt là các mômen điều khiển tác động lên khâu 1, khâu 2 và khâu 4 (từ 3.61 đến 3.65). Với các tham số Hij , T được cho theo (3.66), (3.67) (đã xét ở chương 3): vµ: m1234 = m1 + m2 + m3 + m4 ; m234 = m2 + m3 + m4 . m34 = m3 + m4 ; m4 = m40 + mt ; m40 : khèi l­îng cña khíp 4 mt : khèi l­îng cña t¶i ®­îc nèi víi khíp 4. J124 = J1 + J2 + J4 ; J24 = J2 + J4 ; J4 = J40 + Jt J40 : m« men qu¸n tÝnh cña khíp 4. Jt : m« men qu¸n tÝnh cña t¶i ®­îc nèi víi khíp 4. 4.3.2. Hệ phương trình trạng thái Biến trạng thái cho khớp 1, 2 và 4 như cho ở 3.68 ¸ 3.70: và tín hiệu vào Hệ phương trình vi phân trạng thái của các khớp 1, 2 và 4 được viết như sau: Khớp 1: (4.18) Khớp 2: (4.19) Khớp 4: (4.20) Từ các phương trình (4.18) đến (4.20), ta có hệ phương trình trạng thái của khớp 1 và 2, 4 dưới đây: Khớp 1: (4.23) với : (theo 2.30’) (4.24) = Khớp 2: (4.25) với : (4.26) Khớp 4: (4.27) với (4.28) 1 s 1 s Ph­¬ng tr×nh (4.23) x11=q1 1 s 1 s x21=q2 Ph­¬ng tr×nh (4.25) 1 s 1 s x41=q4 Ph­¬ng tr×nh (4.27) Hình 4.8: Mô hình hóa đáp ứng đầu ra thực của Robot Như vậy mô hình tay máy robot ba bậc tự do là một hệ một đầu vào một đầu ra, được mô tả bằng ba hệ nhỏ, mỗi hệ tương ứng với từng khớp 1, 2 và 4, được đặc trưng bởi ba hệ phương trình vi phân trạng thái (4.23), (4.25) và (4.27). Các hệ phương trình này có thể dùng để mô hình hóa trên máy tính cũng như để tổng hợp luật điều khiển cho tay máy. 4.3.3. Thiết kế bộ điều khiển kiểu trượt đơn thuần Chọn mặt trượt cho từng khớp có dạng sau: Khớp 1 S1 = C1.e11 + e12 = 0 (4.29) với e11 = x11d - x11 và e12 = x12d - x12 Khớp 2 S2 = C2.e21 + e22 = 0 (4.30) với e21 = x21d - x21 và e22 = x22d - x22 Khớp 4 S4 = C4.e41 + e42 = 0 (4.31) với e41 = x41d – x41 và e42 = x42d – x42 Tín hiệu điều khiển cho từng khớp có dạng (2.41): mà: * * sgn(S) = [sgn(s1), sgn(s2), ..., sgn(sm)]T * K > 0, lµ ma trËn (n´n) Do ®ã: (4.32) Từ đó: (4.33) hay viết tín hiệu điều khiển cho từng khớp: Khớp 1 (4.34) Khớp 2 (4.35) Khớp 4 (4.36) trong đó: S1 , S2 , S4 được xác định theo công thức (4.29) đến (4.31) [ theo (4.24), (4.26) và (4.28)] âi là giá trị tính toán gần đúng khi có sự thay đổi của các tham số trong quá trình điều khiển. 4.3.4. Dùng phương pháp lớp biên để giải quyết vấn đề chattering Như đã nói ở phần 4.3.3, thay hàm signum bằng hàm bão hòa V(S/f), tín hiệu điều khiển được viết lại như sau: nếu nếu nếu (4.37) với: (4.38) và K là ma trận (n x n) xác định dương và chọn đủ lớn để đảm bảo điều kiện trượt được thỏa mãn dù có tồn tại nhiễu và các tham số không xác định. Tương tự như cách khai triển ở phần 4.3.3, ta có tín hiệu điều khiển cho từng khớp: Khớp 1 (4.39) Khớp 2 (4.40) Khớp 4 (4.41) trong đó được xác định theo (4.24), (4.26) và (4.28). Như vậy, nếu ta xác định được các tín hiệu điều khiển ui thì có thể mô hình hóa Robot trên máy tính và điều khiển được tay máy robot chuyển động theo quỹ đạo đặt đáp ứng được các yêu cầu và hệ điều khiển đó là ổn định và bền vững mặc dù có thể có nhiễu ngoài tác động hoặc có sự thay đổi của tải. C¸c s¬ ®å m« h×nh hãa c¸c kh©u cña bé ®iÒu khiÓn vµ cña Robot ®­îc thùc hiÖn vµ ch¹y trªn nÒn Matlab/Simulink ë ch­¬ng 5. Ch­¬ng 5 M« pháng 5.1. §Æt vÊn ®Ò. Trong ch­¬ng 4 ®· tiÕn hµnh x©y dùng m« h×nh cña c¸c kh©u tÝnh to¸n, m« h×nh Robot vµ bé ®iÒu khiÓn tr­ît c¶i tiÕn. Sau ®ã ®· x©y dùng c¸c ch­¬ng tr×nh phôc vô m« pháng vµ kh¶o s¸t c¸c chÕ ®é lµm viÖc cña hÖ thèng. Trong ch­¬ng nµy sÏ tr×nh bµy c¸c kÕt qu¶ kh¶o s¸t nhËn ®­îc th«ng qua c¸c ch­¬ng tr×nh m« pháng nªu trªn. Th«ng qua viÖc ®¸nh gi¸ sai sè quü ®¹o ®Æt vµ quü ®¹o thùc ®Ó ®¸nh gi¸ chÊt l­îng ®iÒu chØnh b¸m chÝnh x¸c cña ph­¬ng ph¸p ®iÒu khiÓn ®­îc nghiªn cøu. Phương pháp điều khiển kiểu trượt ứng dụng cho tay máy hai bậc tự do với tín hiệu điều khiển được tính chọn để đảm bảo hệ thống kín ổn định và bền vững đối với nhiễu tải cũng như đối với sự thay đổi các thông số của đối tượng điều khiển, phương pháp lớp biên được ứng dụng để loại bỏ hiện tượng chattering của tín hiệu điều khiển khi trạng thái hệ thống ở trên mặt trượt. Khi sử dụng hàm sat trong bộ điều khiển thay cho hàm signum, các kết quả mô phỏng cho thấy, khi thay đổi tải trọng và sự nhiễu loạn có giới hạn cho tay máy thì hệ điều khiển trượt này vẫn giữ được tính ổn định, khử được hiện tượng chattering của các tín hiệu điều khiển cũng như giảm được các sai số quỹ đạo. Việc mô hình hóa và nghiên cứu mô phỏng hệ thống điều khiển chuyển động được thực hiện trên nền Matlab-Simulink, sau đó kiểm nghiệm bằng thực nghiệm trên mô hình thực tế là robot Scara Serpent. Kết quả trên mô phỏng và thực nghiệm chứng minh được tính đúng đắn và khẳng định việc chọn luật điều khiển trong phương pháp điều khiển trượt đề xuất (sat) đã nâng cao chất lượng và khắc phục được nhược điểm của hệ điều khiển trượt truyền thống. 5.2. S¬ ®å m« h×nh ho¸ c¸c kh©u cña hÖ thèng. 5.2.1. M« h×nh chung cña Robot. H×nh 5.1: S¬ ®å khèi m« h×nh ho¸ Robot Scara Serpent T¹o quü ®¹o chuyÓn ®éng chuÈn Bé ®iÒu khiÓn m« men ®éng C¬ cÊu chÊp hµnh (Robot) ui Ph¶n håi §éng lùc häc cña Robot Scara Serpent ®­îc m« t¶ b»ng hÖ ph­¬ng tr×nh tr¹ng th¸i (3.61), (3.62) vµ (3.64) t­¬ng øng víi 3 khíp quay cña Robot. Mçi hÖ ph­¬ng tr×nh vi ph©n cho tõng khíp gåm hai ph­¬ng tr×nh vi ph©n víi biÕn tr¹ng th¸i lµ gãc quay vµ tèc ®é gãc cña khíp . C¸c biÕn tr¹ng th¸i ®ã sÏ ®­îc x¸c ®Þnh b»ng tÝch ph©n c¸c tr¹ng th¸i t­¬ng øng (H×nh 4.8). Toµn bé néi dung ch­¬ng tr×nh thùc hiÖn tÝnh to¸n hÖ ph­¬ng tr×nh tr¹ng th¸i cña Robot th«ng qua c¸c ph­¬ng tr×nh (4.39), (4.40) vµ (4.41). 5.2.2. M« h×nh khèi t¹o quü ®¹o chuyÓn ®éng chuÈn. H×nh 5.2 m« t¶ s¬ ®å khèi t¹o quü ®¹o chuyÓn ®éng chuÈn. C¸c ®iÒu kiÖn ®Çu vµ cuèi cña quü ®¹o chuyÓn ®éng chuÈn ®­îc tÝnh th«ng qua c¸c to¹ ®é ®Æt trong kh«ng gian (x0, y0, z0), (xc, yc, zc) vµ thêi gian chuyÓn ®éng (td). Quü ®¹o chuyÓn ®éng chuÈn cña c¶ 3 khíp ®­îc x¸c ®Þnh theo 3 giai ®o¹n: gia tèc, tèc ®é kh«ng ®æi vµ gi¶m tèc, biÓu diÔn b»ng c¸c ph­¬ng tr×nh (4.13), (4.15) vµ (4.16). Toµn bé ch­¬ng tr×nh t¹o quü ®¹o chuyÓn ®éng chuÈn ®­îc viÕt trong tÖp qdcdc.m (Tham kh¶o phÇn Phô lôc2). §Æt to¹ ®é: (x0, y0, z0) (xc, yc, zc) (td) Quü ®¹o chuÈn khíp 1 Quü ®¹o chuÈn khíp 2 Quü ®¹o chuÈn khíp 4 H×nh 5.2: S¬ ®å khèi t¹o quü ®¹o chuyÓn ®éng chuÈn 5.2.3. M« h×nh bé ®iÒu khiÓn. Để xác định tín hiệu điều khiển cho từng khớp ta đi xây dựng mô hình cho robot với các tín hiệu ui được cho theo các phương trình trên: ui = Hi1.tg1+ Hi2.tg2+ Hi3.tg3; nếu nếu nếu mặt trượt với Hàm bão hòa sat(S)= Với (từ 4.39 đến 4.41) Hình 5.3: Mô hình bộ điều khiển cho khớp1 của Robot Serpent. 5.3. C¸c th«ng sè cña Robot Scara Serpent Khi ch¹y ch­¬ng tr×nh m« pháng ta ®¸nh lÖnh >>RbSerpent trªn cöa sæ chÝnh cña ch­¬ng tr×nh Matlab, trªn mµn h×nh hiÖn ra b¶ng lµm viÖc chÝnh nh­ sau: H×nh 5.4: Menu chÝnh cña ch­¬ng tr×nh C¸c th«ng sè cña Robot Scara Serpent ®­îc nhËp vµo khèi th«ng sè cña Robot nh­ h×nh 5.5. H×nh 5.5: B¶ng th«ng sè cña Robot Scara Serpent. Khi ta nhÊn vµo nót Load m« h×nh: M« h×nh hiÖn ra nh­ sau: H×nh 5.6: S¬ ®å khèi m« h×nh chuÈn Robot Serpent H×nh 5.7: Giao diÖn m« pháng Robot Serpent M« h×nh cña Robot Scara Serpent: H×nh 5.8: M« h×nh Robot Cöa sæ th«ng tin trî gióp: H×nh 5.9: Cöa sæ trî gióp Víi T¶i ®Þnh møc: m t = 2Kg. Khi thực hiện mô phỏng trên một tay máy ba bậc tự do ta sử dụng các tham số như sau: Bảng 5.10: Các tham số của tay máy l (m) lg (m) m (kg) J (kg.m2) Khâu 1 0,25 0,25 4 0.021 Khâu 2 0,15 0,15 1.5 0.0034 Khâu 4 0 0 0.6 0.001 Việc mô phỏng được thực hiện với trường hợp có tải (khối lượng tải mt = 2 kg và mômen quán tính của tải Jt = 0,0004 kg.m2) của các hàm signum, bão hòa (sat) được sử dụng trong bộ điều khiển. Hình 5.11: Các thông số của tải Tay Robot ®­îc thiÕt kÕ chuyÓn ®éng theo mét quü ®¹o xuÊt ph¸t tõ vÞ trÝ ban ®Çu cã to¹ ®é DDau = [x0, y0, z0] ®Õn vÞ trÝ cuèi cã to¹ ®é DCuoi = [xc, yc, zc] víi thêi gian chuyÓn ®éng lµ td (H×nh 5.2). C¸c to¹ ®é ban ®Çu vµ kÕt thóc ®­îc chän th«ng qua ch­¬ng tr×nh gioihan.m (Tham kh¶o Phô lôc) sÏ n»m trong ph¹m vi lµm viÖc cña Robot hay nãi c¸ch kh¸c lµ c¸c to¹ ®é [x, y, z] ®­îc chän sÏ tho¶ m·n ®iÒu kiÖn (5.1). Dùa vµo H×nh 3.6 ë ch­¬ng 3 cã ph¹m vi lùa chän c¸c to¹ ®é kh«ng gian nh­ sau: (5.1) 5.4. KÕt qu¶. 5.4.1. Quü ®¹o ®Æt và quü ®¹o thùc t¹i c¸c khíp: Khi thêi gian ®Æt lµ t­¬ng ®èi dµi, ®¸p øng chËm, ta cã H×nh 5.12: Quü ®¹o chuyÓn ®éng chuÈn cña Robot Scara Serpent Víi ®¸p øng nhanh, gi¶ sö thêi gian chän lµ t=1,2sec th× cã qid nh­ sau: H×nh 5.13: Quü ®¹o ®Æt vµ quü ®¹o thùc khíp 1 H×nh 5.14: Quü ®¹o ®Æt vµ quü ®¹o thùc khíp 2 H×nh 5.15: Quü ®¹o ®Æt vµ quü ®¹o thùc khíp 4 5.4.2. Mômen tác động lên các trục 5.4.2.1. Khi sử dụng Hàm dấu signum Moment tại khớp 1 Moment khớp 2 Moment khớp 4 Hình 5.16: Moment tác động lên các trục 5.4.2.2. Khi sử dụng hàm bão hòa Sat Mô men tác động lên các trục: Mômen động tại Trục 1 Mômen động tại Trục 2 Mômen động tại Trục4 Hình 5.17: Mô men tác động lên các trục Sử dụng phương pháp điều khiển trượt bão hòa Sat ta có thể thấy đáp ứng về mômen động tốt hơn so với phương pháp điều khiển trượt thông thường sgn(S). Mô men động tại các trục có đáp ứng tốt và giảm được hiện tượng lập bập. Bằng việc lựa chọn các thông số phục vụ cho quá trình điều khiển để cho quỹ đạo thực bám quỹ đạo đặt tốt hơn, ta có bảng tham số 5.18 Bảng 5.18: Các tham số lựa chọn cho mạch điều khiển. Ci f Ki Trục 1 0.2 6 20 Trục 2 0.3 4 25 Trục 3 0.25 5 22 Sai sè: e(t)=qd(t) - qt(t) Sai sè vÒ gãc quay ë khíp 1: e1(t) Sai sè vÒ gãc quay ë khíp 2: e2(t) Sai sè vÒ gãc quay ë khíp 4: e4(t) H×nh5.15: Sai sè ei(t) KÕt qu¶ m« pháng cho thÊy c¸c thanh nèi cña Robot b¸m chÝnh x¸c c¸c quü ®¹o chuyÓn ®éng chuÈn víi sai sè gãc quay tÜnh còng nh­ sai sè gãc quay trong giai ®o¹n di chuyÓn tèc ®é kh«ng ®æi rÊt nhá. Sai sè gãc quay trong giai ®o¹n gia tèc vµ gi¶m tèc còng ®ñ nhá ë giíi h¹n cho phÐp. Khi sö dông hµm chuyÓn m¹ch sat(S) vµ hµm sgn(S) ta ®Òu thÊy quü ®¹o thùc b¸m kh¸ s¸t quü ®¹o ®Æt víi sai sè rÊt nhá. Tuy nhiªn khi sö dông hµm sgn(S) th× ®¸p øng vÒ momen ®éng x¶y ra hiÖn t­îng lËp bËp (chattering), gi¶i quyÕt vÊn ®Ò nµy b»ng c¸ch sö dông hµm chuyÓn m¹ch sat(S) trong bé ®iÒu khiÓn th× thÊy hiÖn t­îng chattering gi¶m h¼n. cã thÓ thÊy rÊt râ ®iÒu nµy trªn kÕt qu¶ m« pháng. §iÒu nµy cho thÊy viÖc c¶i tiÕn sö dông hµm sat(S) nh­ ®· nªu ë phÇn ®Çu luËn v¨n lµ hoµn toµn hîp lý. 5.3. KÕt luËn. Tãm l¹i, t¸c gi¶ luËn v¨n ®· tËp trung nghiªn cøu vµ ®· ®­a ra ®­îc mét sè c¸c kÕt qu¶ ®¸nh gi¸ vÒ ph­¬ng ph¸p ®iÒu khiÓn bÒn v÷ng cho chuyÓn ®éng cña tay m¸y robot øng dông ph­¬ng ph¸p ®iÒu khiÓn tr­ît. LuËn v¨n ®· nghiªn cøu hÖ thèng ®iÒu khiÓn tr­ît cho tay m¸y n bËc tù do, ¸p dông cho robot Scara Serpent víi ba khíp ®éng ®Çu tiªn. HÖ thèng ®­îc x©y dùng gåm hai phÇn: PhÇn thø nhÊt gåm bé ®iÒu khiÓn tr­ît ®¬n thuÇn dïng hµm dÊu sgn(); PhÇn thø hai lµ bé ®iÒu khiÓn tr­ît c¶i tiÕn dïng hµm chuyÓn m¹ch b·o hßa Sat ®Ó khö hiÖn t­îng rung lËp bËp trong qu¸ tr×nh ®iÒu khiÓn, sö dông phÐp ­íc l­îng tham sè ch­a biÕt thay cho c¸c tham sè thùc tÕ. Sö dông bé ®iÒu khiÓn tr­ît thÝch nghi ®Ó ­íc l­îng c¸c tham sè ch­a biÕt lµm cho hÖ ®iÒu khiÓn bao gåm c¶ Robot lµ mét hÖ ®iÒu khiÓn MIMO phi tuyÕn trë thµnh c¸c hÖ ®iÒu khiÓn SISO tuyÕn tÝnh m« t¶ b»ng ph­¬ng tr×nh vi ph©n tuyÕn tÝnh cÊp 2 vµ cã thÓ dÔ dµng gi¶i vµ m« pháng b»ng phÇn mÒm chuyªn dông. ChÊt l­îng ®éng vµ ®é t¸c ®éng nhanh cña hÖ thèng còng ®­îc kiÓm nghiÖm víi tÝn hiÖu ®Æt biÕn thiªn nhanh. KÕt qu¶ thu ®­îc cho thÊy hÖ thèng ®¶m b¶m ph¶n øng nhanh víi c¸c tÝn hiÖu ®Æt vµ sai sè quü ®¹o ®ñ nhá. Nh÷ng kÕt qu¶ trªn ®©y lµ c¬ së cho viÖc thiÕt kÕ mét hÖ thèng ®iÒu khiÓn n©ng cao chÊt l­îng ®iÒu khiÓn b¸m chÝnh x¸c quü ®¹o ®Æt cña Robot. LuËn v¨n ®· kiÓm nghiÖm ®­îc tÝnh ®óng ®¾n cña thuËt to¸n ®iÒu khiÓn th«ng qua ®¸nh gi¸ chÊt l­îng tÜnh vµ ®éng cña hÖ thèng b»ng m« h×nh ho¸ hÖ thèng ®· thiÕt kÕ sö dông Simulink khi Robot ®­îc m« pháng lµm viÖc kh«ng t¶i vµ t¶i ®Þnh møc víi quü ®¹o chuyÓn ®éng chuÈn ®Æt tr­íc. KÕt qu¶ m« pháng cho thÊy c¸c thanh nèi cña Robot b¸m rÊt chÝnh x¸c c¸c quü ®¹o chuyÓn ®éng chuÈn víi sai sè gãc quay tÜnh còng nh­ sai sè gãc quay trong giai ®o¹n di chuyÓn tèc ®é kh«ng ®æi rÊt nhá. Sai sè gãc quay trong giai ®o¹n gia tèc vµ gi¶m tèc còng ®ñ nhá ë giíi h¹n cho phÐp. 5.4. KiÕn nghÞ ViÖc lùa chän c¸c tham sè ®iÒu khiÓn ph¶i phô thuéc vµo ®Æc tÝnh m« pháng mµ ch­a cã ph­¬ng ph¸p thuËn tiÖn h¬n trong viÖc ®¸nh gi¸ ¶nh h­ëng cña c¸c tham sè vµ sù æn ®Þnh trong ®iÒu khiÓn. MÆt kh¸c khèi l­îng c«ng viÖc tÝnh to¸n lµ t­¬ng ®èi lín, do vËy cÇn c¶i tiÕn ph­¬ng ph¸p vµ sö dông c¸c c«ng cô hç trî ®ñ m¹nh ®Ó gi¶i quyÕt c«ng viÖc nµy. §Ó bï nhiÔu tèt h¬n vµ gi¶m ®¸ng kÓ hiÖn t­îng rung mµ ph­¬ng ph¸p líp biªn ch­a thÓ ®¸p øng ®­îc ta cã thÓ sö dông ph­¬ng ph¸p ®iÒu khiÓn tr­ît bËc cao hoÆc ph­¬ng ph¸p tr­ît PI - b·o hßa nh­ ®· ®Ò cËp ë ch­¬ng 2. Trong thùc tÕ c¸nh tay Robot lu«n lu«n bÞ ¶nh h­ëng mëi ma s¸t vµ c¸c nhiÔu lo¹n , ®ång thêi trong qu¸ tr×nh chuyÓn ®éng c¸c tham sè cña hÖ còng lu«n thay ®æi, do ®ã ta sö dông ph­¬ng tr×nh tæng qu¸t ®Ó m« h×nh hãa m«men lùc c¸nh tay b»ng ph­¬ng tr×nh ®éng lùc häc: . Tuy nhiªn trong khu«n khæ cña luËn v¨n nµy, c¬ cÊu chÊp hµnh nh­ ®éng c¬ truyÒn ®éng vµ c¬ cÊu truyÒn ®éng trong m« h×nh ho¸ ®­îc coi lµ kh©u qu¸n tÝnh, ®ång thêi c¸c thµnh phÇn lùc kh¸c ¶nh h­ëng ®Õn ®é chÝnh x¸c cña quü ®¹o chuyÓn ®éng ®­îc bá qua. V× vËy, ®Ó hoµn thiÖn hÖ thèng cã thÓ dïng ph­¬ng ph¸p m« pháng trªn m¸y tÝnh ®Ó tÝnh to¸n c¸c th«ng sè thÝch hîp cña bé ®iÒu khiÓn khi xÐt ®Õn tÝnh chÊt ®éng häc cña c¸c c¬ cÊu chÊp hµnh vµ cña b¶n th©n c¸c lùc ®éng t­¬ng t¸c gi÷a c¸c khíp. Cuèi cïng, thuËt to¸n ®iÒu khiÓn tr­ît ch­a tÝnh ®Õn bµi to¸n kiÓm tra vµ giíi h¹n sù tån t¹i cña ma trËn nghÞch ®¶o , còng nh­ c¸ch ®Ó ®o , (thùc tÕ c¸c Robot c«ng nghiÖp chØ trang bÞ c¸c c¶m biÕn ®o vÞ trÝ vµ tèc ®é). H­íng nghiªn cøu tiÕp theo cña luËn v¨n lµ nghiªn cøu, thiÕt kÕ bé ®iÒu khiÓn thÝch nghi bï qu¸n tÝnh vµ gia tèc träng tr­êng th«ng qua hµm Lyapunov ®­îc sö dông ®Ó ph©n tÝch sù æn ®Þnh cña hÖ thèng. KÕt luËn LuËn v¨n ®· nghiªn cøu vÒ lý thuyÕt ®iÒu khiÓn bÒn v÷ng vµ ¸p dông cho Robot Serpent víi ba khíp ®éng. LuËn v¨n ®· kiÓm nghiÖm tÝnh ®óng ®¾n cña thuËt to¸n ®iÒu khiÓn th«ng qua viÖc x©y dùng c¸c ph­¬ng tr×nh ®éng lùc häc cho Robot Serpent dùa vµo th«ng sè ®· cho cña nhµ s¶n xuÊt. §­a ra c«ng thøc tÝnh ®éng häc thuËn vµ ®éng häc ng­îc cho Robot Serpent, x©y dùng m« h×nh to¸n häc cho c¬ cÊu truyÒn ®éng Robot, tæng hîp hÖ truyÒn ®éng. ThiÕt kÕ bé ®iÒu khiÓn BÒn V÷ng ¸p dông tiªu chuÈn æn ®Þnh Lyapunov. §¸nh gi¸ chÊt l­îng tÜnh vµ ®éng cña hÖ thèng b»ng m« h×nh ho¸ hÖ thèng ®· thiÕt kÕ sö dông Simulink khi Robot ®­îc m« pháng lµm viÖc kh«ng t¶i vµ t¶i ®Þnh møc víi quü ®¹o chuyÓn ®éng chuÈn ®Æt tr­íc. KÕt qu¶ m« pháng cho thÊy c¸c thanh nèi cña Robot b¸m chÝnh x¸c c¸c quü ®¹o chuyÓn ®éng chuÈn víi sai sè gãc quay tÜnh còng nh­ sai sè gãc quay trong giai ®o¹n di chuyÓn tèc ®é kh«ng ®æi rÊt nhá. Sai sè gãc quay trong giai ®o¹n gia tèc vµ gi¶m tèc còng ®ñ nhá ë giíi h¹n cho phÐp. KÕt qu¶ nghiªn cøu ¶nh h­ëng cña bé sè (KD,KV) ®Õn ®é chÝnh x¸c ®iÒu khiÓn b¸m theo quü ®¹o chuÈn cho thÊy bé sè (KD,KV) cã gi¸ trÞ cµng lín th× sai sè quü ®¹o chuyÓn ®éng vµ ®é qóa ®iÒu chØnh nhá, tuy nhiªn còng cÇn thiÕt ph¶i chän bé sè (KD,KV) sao cho cã gi¸ trÞ ®ñ lín sÏ ®em l¹i sù tèi ­u cho hÖ thèng ®iÒu khiÓn vÒ c¶ gi¶i ph¸p kü thuËt lÉn kinh tÕ. ChÊt l­îng ®éng vµ ®é t¸c ®éng nhanh cña hÖ thèng còng ®­îc kiÓm nghiÖm víi tÝn hiÖu ®Æt biÕn thiªn nhanh. KÕt qu¶ thu ®­îc cho thÊy hÖ thèng ®¶m b¶m ph¶n øng nhanh víi c¸c tÝn hiÖu ®Æt vµ sai sè quü ®¹o ®ñ nhá. MÆc dï luËn v¨n ch­a cã kÕt qu¶ thùc nghiÖm, song nh÷ng kÕt qu¶ thu ®­îc ®· t¹o c¬ së tèt cho viÖc thiÕt kÕ mét hÖ thèng ®iÒu khiÓn b¸m chÝnh x¸c quü ®¹o chuyÓn ®éng cña Robot trong thùc tÕ. KiÕn nghÞ Trong thùc tÕ mét c¸nh tay Robot lu«n lu«n bÞ ¶nh h­ëng mëi ma s¸t vµ c¸c nhiÔu lo¹n ®ång thêi. Do ®ã chóng ta sÏ tæng qu¸t ho¸ m« h×nh c¸nh tay b»ng ph­¬ng tr×nh ®éng lùc häc: . Tuy nhiªn trong khu«n khæ cña luËn v¨n nµy, c¬ cÊu chÊp hµnh nh­ ®éng c¬ truyÒn ®éng vµ c¬ cÊu truyÒn ®éng trong m« h×nh ho¸ ®­îc coi lµ kh©u kh«ng qu¸n tÝnh, ®ång thêi c¸c thµnh phÇn lùc kh¸c ¶nh h­ëng ®Õn ®é chÝnh x¸c cña quü ®¹o chuyÓn ®éng ®­îc bá qua. V× vËy, ®Ó hoµn thiÖn hÖ thèng cã thÓ dïng ph­¬ng ph¸p m« pháng trªn m¸y tÝnh ®Ó tÝnh to¸n c¸c th«ng sè thÝch hîp cña bé ®iÒu khiÓn khi xÐt ®Õn tÝnh chÊt ®éng häc cña c¸c c¬ cÊu chÊp hµnh vµ cña b¶n th©n c¸c lùc ®éng t­¬ng t¸c gi÷a c¸c khíp. LuËn v¨n còng ch­a ®Ò cËp ®Õn ph­¬ng ph¸p tÝnh chän bé sè (KD,KV) ®Ó cã ®­îc bé sè thÝch hîp ®¶m b¶o chØ tiªu chÊt l­îng tÜnh vµ ®éng yªu cÇu. §Ó gi¶i quyÕt nh÷ng vÊn ®Ò nµy h­íng nghiªn cøu tiÕp theo cña luËn v¨n lµ nghiªn cøu bé ®iÒu khiÓn bÒn v÷ng bï qu¸n tÝnh vµ gia tèc träng tr­êng th«ng qua hµm Lyapunov ®­îc sö dông ®Ó ph©n tÝch sù æn ®Þnh cña hÖ thèng. Ngoµi ra luËn v¨n ch­a ®Ò cËp ®­îc ®Õn viÖc lËp tr×nh vµ øng dông trùc tiÕp vµo Robot Serpent c¸c kÕt qu¶ nghiªn cøu trªn th«ng qua viÖc ghÐp nèi víi c¸c thiÕt bÞ ngo¹i vi. MÆc dï ®· cè g¾ng hÕt søc ®Ó t×m tßi, nghiªn cøu nh»m hoµn thµnh yªu cÇu ®Æt ra, xong v× thêi gian cã h¹n vµ ®iÒu kiÖn nghiªn cøu cßn nhiÒu h¹n chÕ nªn ch¾c ch¾n kh«ng tr¸nh khái nh÷ng khiÕm khuyÕt vµ thiÕu sãt. V× vËy t¸c gi¶ mong nhËn ®­îc sù chØ b¶o vµ gãp ý tËn t×nh cña c¸c ThÇy, C« gi¸o trong héi ®ång b¶o vÖ vµ ph¶n biÖn ®Ó luËn v¨n ®­îc hoµn thiÖn h¬n. T«i xin ch©n thµnh c¶m ¬n ThÇy gi¸o – TiÕn sÜ NguyÔn M¹nh TiÕn ®· tËn t×nh h­íng dÉn vµ gióp ®ì T«i hoµn thµnh luËn v¨n nµy. Phô lôc 1: C¸c s¬ ®å khèi S¬ ®å khèi m¹ch ®iÒu khiÓn Robot Scara Serpent: S¬ ®å khèi m¹ch t¹o quü ®¹o chuyÓn ®éng chuÈn: S¬ ®å khèi Kh©u ®iÒu chØnh: S¬ ®å khèi kh©u TÝnh gi¸ trÞ trung gian: Khi ®iÒu khiÓn tr­ît sö dông hµm sgn(S) ®Ó m« pháng: Khi ®iÒu khiÓn tr­ît sö dông hµm sat(S/F) ®Ó m« pháng: S¬ ®å kh©u Robot Serpent: S¬ ®å khèi c¸c TÝn hiÖu: NhËp c¸c gi¸ trÞ tham sè cho m« h×nh: Phô lôc 2: C¸c ch­¬ng tr×nh m files: (ch¹y trªn nÒn Matlab/simulink) RbSerpent.m % chuong trinh chinh %RbSerpent Robot toolbox demonstrations % Displays popup menu of toolbox demonstration scripts that illustrate: % * homogeneous transformations % * trajectories % * forward kinematics % * inverse kinematics % * robot animation % * inverse dynamics % * forward dynamics % echo off clear all delete( get(0, 'Children') ); while 1, selection = menu('Menu chÝnh cña Robot Scara Serpent', ... 'NhËp c¸c th«ng sè cho m« h×nh', ... 'Load M« h×nh', ... 'Quü ®¹o', ... '§éng häc thuËn', ... '§éng häc ngîc', ... '§éng lùc häc', ... 'HiÓn thÞ', ... 'Tho¸t'); switch selection, case 1, quydaochuan case 2, DktruotRBSP case 3, Dapung case 4, dhthuanRS case 5, dhnguocRS case 6, dlhocRS case 7, Robot_TN case 8, delete( get(0, 'Children') ); break; end end %%======================================================%% %%======================================================%% DhocnguocRS.m%%TÝnh to¸n ®éng häc ng­îc Robot Serpent function theta=DhnguocRS(u) clc; disp('Dong hoc thuan Robot Serpent'); a1=input('chieu dai a1 = '); a2=input('chieu dai a2 = '); disp('Vi tri toa do cua tay may Robot: px,py,pz,nx...'); px=input('vi tri theo toa do x: px ='); py=input('vi tri theo toa do x: py ='); nx=input('nhap gia tri dinh vi nx = '); if (nx>1) disp('Error'); end disp('Tinh toan cac gia tri goc quay cua cac khop khi biet toa do vi tri khop cuoi cung'); % Goc quay theta2: C2=(px^2+py^2-a1^2-a2^2)/(2*a1*a2); S2=sqrt(1-C2^2); theta2=atan2(S2,C2); % Goc quay theta1: C1=((a1+a2*C2)*px+a2*S2*py)/(px^2+py^2); S1=((a1+a2*C2)*py-a2*S2*px)/(px^2+py^2); theta1=atan2(S1,C1); % d3=-pz % Goc quay theta4: kk=sqrt(1-nx^2); theta4=theta1+theta2-atan2(kk,nx); disp('Cac goc quay tuong ung voi vi tri:'); disp(' theta1 theta2 theta4') theta=[theta1,theta2,theta4]; % end %================================================= Gocquaybd.m %% TÝnh to¸n gãc quay ban ®Çu function init=gocquaybd(u) x=u(1); y=u(2); z=u(3); a1=u(4); %(m) a2=u(5); %(m) nx=u(6); %q20=Gocquay2(X0,Y0,a1,a2) %q10=Gocquay1(X0,Y0,q20,a1,a2); %q40=Gocquay4(q10,q20,nx); %Tinh cac gia tri tuong ung goc quay 1: q10,q1c %=================================================== C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); if (C2 >1) disp('Error'); end S2=sqrt(1-C2^2); q20=atan2(S2,C2); %=================================================== %Tinh cac gia tri tuong ung goc quay 2: q20,q2c %=================================================== C1=((a1+a2*cos(q20))*x+a2*sin(q20)*y)/(x^2+y^2); S1=((a1+a2*cos(q20))*y-a2*sin(q20)*x)/(x^2+y^2); if (abs(C1) >1|abs(S1)>1) disp('Error'); end q10=atan2(S1,C1); %===================================================%Tinh cac gia tri tuong ung goc quay 3: q30,q3c %=================================================== if (nx>1) disp('Error'); end KK=sqrt(1-nx^2); tetaphu=atan2(KK,nx); q40=q10+q20-tetaphu; %============================================= init=[q10;q20;q40]; %================================================= %================================================= qdcdc.m %% TÝnh to¸n Quü ®¹o chuyÓn ®éng chuÈn function qdc=qdcdc(u) %=================================================== % Xay dung quy dao chuyen dong chuan cho 3 khop quay cua Robot Serpent %=================================================== % qi0,qic: goc quay dau va cuoi cua khop quay thu i % --------------------------------------------------% Toa do diem dau: DDau=[X0;Y0;Z0] X0=u(1); Y0=u(2); Z0=u(3); % Toa do diem cuoi: DCuoi=[Xc;Yc;Zc] Xc=u(4); Yc=u(5); Zc=u(6); td=u(7); % td: Dat thoi gian chuyen dong; ta=u(8); %Thoi gian gia toc (giam toc). %---------------------------- % Thong so cua Robot: a1,a2. a1=u(9); %(m) a2=u(10); %(m) t=u(11); % thoi gian thuc nx=u(12); %=================================================== % CHUONG TRINH CHINH %=================================================== % Dong hoc nguoc va tinh cac goc quay cua tung khop tai diem dau va diem cuoi %---------------------------- q20=Gocquay2(X0,Y0,a1,a2); q2c=Gocquay2(Xc,Yc,a1,a2); q10=Gocquay1(X0,Y0,q20,a1,a2); q1c=Gocquay1(Xc,Yc,q2c,a1,a2); q40=Gocquay4(q10,q20,nx); q4c=Gocquay4(q1c,q2c,nx); %---------------------------- %Khoi tao cac he so cua quy dao chuan cua 3 khop %---------------------------- h1=Init(q10,q1c,ta,td); h2=Init(q20,q2c,ta,td); h4=Init(q40,q4c,ta,td); %---------------------------- qdc1= Quydaochuan(t,h1,ta,td); qdc2= Quydaochuan(t,h2,ta,td); qdc4= Quydaochuan(t,h4,ta,td); qdc=[qdc1;qdc2;qdc4]; %=================================================== %KET THUC CHUONG TRINH CHINH %=================================================== %=================================================== %Tinh cac gia tri tuong ung goc quay 2: q20,q2c %=================================================== function gq2=Gocquay2(x,y,a1,a2) C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); if (C2 >1) disp('Error'); end S2=sqrt(1-C2^2); gq2=atan2(S2,C2); %=================================================== %Tinh cac gia tri tuong ung goc quay 1: q10,q1c %=================================================== function gq1=Gocquay1(x,y,q2,a1,a2) C1=((a1+a2*cos(q2))*x+a2*sin(q2)*y)/(x^2+y^2); S1=((a1+a2*cos(q2))*y-a2*sin(q2)*x)/(x^2+y^2); if (abs(C1) >1|abs(S1)>1) disp('Error'); end gq1=atan2(S1,C1); %=================================================== %Tinh cac gia tri tuong ung goc quay 4: q40,q4c %=================================================== function gq4=Gocquay4(q1,q2,nx) if (nx>1) disp('Error'); end KK=sqrt(1-nx^2); tetaphu=atan2(KK,nx); gq4=q1+q2-tetaphu; %=================================================== %Xay dung quy dao chuyen dong chuan %=================================================== function quydao=Quydaochuan(t,h,ta,td) %gsi_ac=a0ac+a1ac*t+a2ac*t^2+a3ac*t^3+a4ac*t^4; a0ac=h(1); a3ac=h(2); a4ac=h(3); a0cd=h(4); a1cd=h(5); a0df=h(6); a1df=h(7); a3df=h(8); a4df=h(9); qcuoi=h(10); if td<=0 td=0; quydao=-1;%Error elseif (td>0)&(td<=4*ta) %Gom 2 giai doan: gia toc, giam toc if (t>=0)&(t<=td/2)%Gia toc quydao=a0ac+a3ac*t^3+a4ac*t^4;%Gia toc elseif (t>td/2)&(t<=td) %Giam toc quydao=a0df+a1df*(t-td/2)+a3df*(t-td/2).^3+a4df*(t-td/2).^4; %Giam toc elseif t>td quydao=qcuoi; else quydao=-1;%Error end else %if td>(4*ta) %Gom 3 giai doan: gia toc, chay deu, giam toc if (t>=0)&(t<2*ta) quydao=a0ac+a3ac*t^3+a4ac*t^4;%Gia toc elseif (t>=2*ta)&(t<=(td-2*ta)) quydao=a0cd+a1cd*(t-2*ta); %Chay deu elseif (t>(td-2*ta))&(t<=td) quydao=a0df+a1df*(t-td+2*ta)+a3df*(t-td+2*ta).^3+a4df*(t-td+2*ta).^4; %Giam toc elseif t>td quydao=qcuoi; else quydao=-1;%Error end end %=================================================== %Tinh cac he so cua quy dao chuan ung voi tung khop %=================================================== function heso=Init(qdau,qcuoi,ta,td) if (td>0)&(td<=4*ta) %Quy dao doan ac:(thay ta=td/4) a0ac=qdau; a3ac=(qcuoi-qdau)/(td^3/8); a4ac=-(qcuoi-qdau)/(td^4/8); %Quy dao doan cd: a0cd=0; a1cd=0; %Quy dao doan df: a0df=qcuoi-(qcuoi-qdau)/2; a1df=(qcuoi-qdau)/(td/2); a4df=-(3*qcuoi-3*a0df-a1df*td)/(td^4/16); a3df=-(a1df+a4df*(td^3/2))/(3*td^2/4); %------------------------------------ end if td>(4*ta) %Quy dao doan ac: a0ac=qdau; a3ac=(qcuoi-qdau)/(4*ta^2*(td-2*ta)); a4ac=-(qcuoi-qdau)/(16*ta^3*(td-2*ta)); %Quy dao doan cd: a0cd=qdau+(qcuoi-qdau)*ta/(td-2*ta); a1cd=(qcuoi-qdau)/(td-2*ta); %Quy dao doan df: a0df=qcuoi-(qcuoi-qdau)*ta/(td-2*ta); a1df=(qcuoi-qdau)/(td-2*ta); a4df=-(3*qcuoi-3*a0df-4*a1df*ta)/(16*ta^4); a3df=-(a1df+32*a4df*ta^3)/(12*ta^2); %------------------------------------ end heso=[a0ac,a3ac,a4ac,a0cd,a1cd,a0df,a1df,a3df,a4df,qcuoi]; %=================================================== %=================================================== RobotTN.m % ch­¬ng tr×nh hiÓn thÞ m« h×nh vµ tham sè RB function robot_tn(action); % robot_tn Hien thi cac tham so va mo hinh cua robot trong MATLAB. % Su dung phuong phap dieu khien truot cho he phi tuyen de % dieu khien ben vung cho hoat dong cua robot Scara Serpent % Co the nhap so lieu cho tay may, hien thi cac quy dao dat % va quy dao thuc te va so sanh ve sai so va toc do bam, % Su dung thuat toan dieu khien bao hoa Sat de khu hien tuong chattering % Cac ket qua mo phong duoc hien thi tren man hinh % Thong tin chi tiet tham khao o % C:\matlabR11\toolbox\matlab\demos % copyright by KhangHD - Cao hoc DK_TDH 2004 - 2006 if nargin<1, action='initialize'; end; if strcmp(action,'initialize'), oldFigNumber=watchon; figNumber=figure( ... 'Name','Ch¬ng tr×nh m« pháng cña Robot Scara Serpent', ... 'NumberTitle','off', ... 'Visible','off'); axes( ... 'Units','normalized', ... 'Position',[0.10 0.45 0.65 0.45]); %=================================== % Set up the MiniCommand Window top=0.35; left=0.05; right=0.75; bottom=0.05; labelHt=0.05; spacing=0.005; promptStr=str2mat(' ',' % Ban hay nhan vao nut ben phai man hinh de lua chon', ... ' % cac lenh can thuc hien va hien thi cac ket qua'); % First, the MiniCommand Window frame frmBorder=0.02; frmPos=[left-frmBorder bottom-frmBorder ... (right-left)+2*frmBorder (top-bottom)+2*frmBorder]; uicontrol( ... 'Style','frame', ... 'Units','normalized', ... 'Position',frmPos, ... 'BackgroundColor',[0.50 0.50 0.50]); % Then the text label labelPos=[left top-labelHt (right-left) labelHt]; uicontrol( ... 'Style','text', ... 'Units','normalized', ... 'Position',labelPos, ... 'BackgroundColor',[0.50 0.50 0.50], ... 'ForegroundColor',[1 1 1], ... 'String','Cua so tro giup'); % Then the editable text field mcwPos=[left bottom (right-left) top-bottom-labelHt-spacing]; mcwHndl=uicontrol( ... 'Style','edit', ... 'HorizontalAlignment','left', ... 'Units','normalized', ... 'Max',10, ... 'BackgroundColor',[1 1 1], ... 'Position',mcwPos, ... 'Callback','robot_tn(''eval'')', ... 'String',promptStr); % Save this handle for future use set(gcf,'UserData',mcwHndl); %==================================== % Thong tin cho tat ca cac nut labelColor=[0.8 0.8 0.8]; top=0.95; left=0.80; btnWid=0.15; btnHt=0.08; % Spacing between the button and the next command's label spacing=0.03; %==================================== % Khung CONSOLE (ban dieu khien) frmBorder=0.02; yPos=0.05-frmBorder; frmPos=[left-frmBorder yPos btnWid+2*frmBorder .9+2*frmBorder]; uicontrol( ... 'Style','frame', ... 'Units','normalized', ... 'Position',frmPos, ... 'BackgroundColor',[0.50 0.50 0.50]); %==================================== % The "Mo hinh robot" button - Nut do hoa duong net lien btnNumber=1; yPos=top-(btnNumber-1)*(btnHt+spacing); %labelStr='Line'; labelStr='Mo hinh robot'; callbackStr='robot_tn(''demobutton'')'; cmdStr=str2mat( ... ' % Hien thi mo hinh cua robot Scara Serpent ', ... ' hold on;', ... ' plot([0 .4 .4 0 0],[0 0 .1 .1 0]);', ... ' plot([.1 .3 .3 .1 .1],[.1 .1 .5 .5 .1]);', ... ' plot([0 .5 .5 0 0],[.5 .5 .6 .6 .5]);', ... ' plot([.5 .6 .6 .5 .5],[.45 .45 .65 .65 .45]);', ... ' plot([.6 .8 .8 .6 .6],[.5 .5 .6 .6 .5]);', ... ' plot([.8 .9 .9 .8 .8],[.4 .4 .7 .7 .4]);', ... ' plot([.83 .87 .87 .83 .83],[.3 .3 .4 .4 .3]);', ... ' plot([.83 .87 .87 .83 .83],[.7 .7 .8 .8 .7]);', ... ' hold off;' ... ); % Generic popup button information btnPos=[left yPos-btnHt btnWid btnHt]; uicontrol( ... 'Style','pushbutton', ... 'Units','normalized', ... 'Position',btnPos, ... 'String',labelStr, ... 'Callback',callbackStr, ... 'UserData',cmdStr); %==================================== % The "Quy dao dat" button btnNumber=2; yPos=top-(btnNumber-1)*(btnHt+spacing); labelStr='Quy dao dat'; callbackStr='robot_tn(''demobutton'')'; cmdStr=str2mat( ... ' % Hien thi quy dao dat truoc cua robot', ... ' x = -2.9:0.2:2.9;', ... ' bar(x,exp(-x.*x));' ... ); % Generic popup button information btnPos=[left yPos-btnHt btnWid btnHt]; uicontrol( ... 'Style','pushbutton', ... 'Units','normalized', ... 'Position',btnPos, ... 'String',labelStr, ... 'Callback',callbackStr, ... 'UserData',cmdStr); %==================================== % The "Cac Ket qua mo phong" button btnNumber=3; yPos=top-(btnNumber-1)*(btnHt+spacing); %labelStr='Stair'; labelStr='Cac ket qua'; callbackStr='robot_tn(''demobutton'')'; cmdStr=str2mat( ... ' % Hien thi ca ket qua mo phong quy ve vi tri,van toc va dao thuc', ... ' x=0:0.25:10;', ... ' stairs(x,sin(x));', ... ' grid;' ... ); % Generic popup button information btnPos=[left yPos-btnHt btnWid btnHt]; uicontrol( ... 'Style','pushbutton', ... 'Units','normalized', ... 'Position',btnPos, ... 'String',labelStr, ... 'Callback',callbackStr, ... 'UserData',cmdStr); % The "Sai so" button btnNumber=4; yPos=top-(btnNumber-1)*(btnHt+spacing); labelStr='Sai so'; callbackStr='robot_tn(''demobutton'')'; cmdStr=str2mat( ... ' % Hien thi sai so cua quy dao thuc so voi quy dao dat', ... ' x = 0:0.1:4;', ... ' y = sin(x.^2).*exp(-x);', ... ' stem(x,y)' ... ); % Generic button information btnPos=[left yPos-btnHt btnWid btnHt]; uicontrol( ... 'Style','pushbutton', ... 'Units','normalized', ... 'Position',btnPos, ... 'String',labelStr, ... 'Callback',callbackStr, ... 'UserData',cmdStr); %==================================== % The "Mo phong qua trinh chay" button btnNumber=5; yPos=top-(btnNumber-1)*(btnHt+spacing); labelStr='Qua trinh chay'; callbackStr='robot_tn(''demobutton'')'; cmdStr=str2mat( ... ' % Canh tay cua robot dich chuyen trong khong gian thuc', ... ' x=-2:0.1:2;', ... ' y=erf(x);', ... ' e = rand(size(x))/10;', ... ' errorbar(x,y,e);' ... ); % Thong tin chung cho cac nut % Generic popup button information btnPos=[left yPos-btnHt btnWid btnHt]; uicontrol( ... 'Style','pushbutton', ... 'Units','normalized', ... 'Position',btnPos, ... 'String',labelStr, ... 'Callback',callbackStr, ... 'UserData',cmdStr); % Nut thong tin chung uicontrol( ... 'Style','push', ... 'Units','normalized', ... 'Position',[left bottom+btnHt+spacing btnWid btnHt], ... 'String','Thong tin', ... 'Callback','robot_tn(''Thong tin'')'); %==================================== %Nut dong chuong trinh %The CLOSE button uicontrol( ... 'Style','push', ... 'Units','normalized', ... 'Position',[left bottom btnWid btnHt], ... 'String','Dong', ... 'Callback','close(gcf)'); % Now uncover the figure set(figNumber,'Visible','on'); elseif strcmp(action,'demobutton'), cmdStr=get(gco,'UserData'); mcwHndl=get(gcf,'UserData'); set(mcwHndl,'String',cmdStr); evalmcw(mcwHndl); elseif strcmp(action,'eval'), mcwHndl=get(gcf,'UserData'); cmdStr=get(mcwHndl,'String'); evalmcw(mcwHndl); elseif strcmp(action,'Thong tin'), helpwin(mfilename); end; %================================================= %================================================= Timgoc.m %% - T×m gãc quay t­¬ng øng cho tay m¸y Robot Serpent function[h,cMin,theta1,dp1,theta2,dp2]= timgoc(px,py) % Bai toan cho biet toa do cua vi tri dau va cuoi cua tay ro bot % ta phai tim duoc cac goc quay theta1, theta2 tuong % ung voi hai khop main va fore tuong ung voi cac % diem chia khi quy dao la duong thang disp('Toa do cua diem dau :'); x0 = input('nhap gia tri hoanh do diem dau: '); y0 = input('nhap gia tri tung do diem dau: '); if (x0^2+y0^2)>.16 errordlg('Ban nhap sai roi','H·y nhËp l¹i'); return end disp('Toa do cua diem cuoi :'); xC = input('nhap gia tri hoanh do diem cuoi: '); yC = input('nhap gia tri tung do diem cuoi: '); if (x0^2+y0^2)>.16 errordlg('Ban nhap sai roi','H·y nhËp l¹i'); return end if(x0~=xC) a = (y0-yC)/(x0-xC); b = (yC*x0-y0*xC)/(x0-xC); end format short; a1=.25; a2=.15; dv=6600/192; c=px^2+py^2; c0=x0^2+y0^2; cC=xC^2+yC^2; cMin= sqrt(a1^2+a2^2-2*a1*a2*cos(65*pi/180)); %goc giua gG = abs(atan2(y0,x0)-atan2(yC,xC)); % dien tich s = 1/2*sqrt(c0*cC)*sin(gG); % chieu cao la duong ngan nhat h=s*2/sqrt((x0-xC)^2+(y0-yC)^2); % i = input('Nhap gia tri so khoang chia: '); if sqrt(c) <cMin warndlg('gia tri cua toa do tay khong thoa man','H·y xem l¹i to¹ ®é'); break end if h<cMin errordlg('quy dao theo duong thang nay khong the thuc hien duoc ','H·y xem l¹i to¹ ®é'); break end m = (c-a1^2-a2^2)/(2*a1*a2); %n = sqrt(1-m^2); t2=acos(m); theta2=180*t2/pi; dp2=(115-theta2)*dv; %p=(a1+a2*cos(t2)*py-a2*sin(t2)*px)/(a1^2+a2^2+2*a1*a2*cos(t2)); %q=(px+a2*po*sin(t2))/(a1+a2*cos(t2)); %t1=atan2(p,q); if px~=0 t1=atan2(py,px)-acos((c+ a1^2-a2^2)/(2*a1*sqrt(c))); else t1=pi/2 -acos((c+a1^2-a2^2)/(2*a1*sqrt(c))); end theta1=t1*180/pi; if theta1>0 dp1=(96-theta1)*dv; else dp1=(96-theta1)*dv; end %================================================= %================================================= Tinh_Hij.m %% - Ch­¬ng tr×nh tÝnh Hij function Hij=tinh_Hij(u) % nhap thong so cua tai mt=u(1); Jt=u(2); % dau vao 3 la goc quay theta2 % moment quan tinh cua khop thu 4 J1=0.021; J2=0.0034; J3=.006; J40=0.001; J4=J40+Jt; J124=J1+J2+J4; J24+J2+J4; m1=4; m2=1.5; m3=2; m40=.6; m4=m40+mt; m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; l1=.25; l2=.15; % Tinh toan cac gia tri Hij H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(u(3)); H12=m234*l2^2+J24+m234*l1*l2*cos(u(3)); H13=J4; H21=H12; H22=m234*l2^2+J24; H31=J4; H32=J4; H33=J4; % Cac gia tri dau ra Hij Hij=[H11;H12;H13;H21;H22;H23;H31;H32;H33]; %%--------------------------------------------------------------------------%% %%--------------------------------------------------------------------------%% Tinh_bij_T.m %% Ch­¬ng tr×nh tÝnh bij vµ T function BT=tinh_bij_T(u) % nhap thong so cua tai mt=u(1); Jt=u(2); % dau vao 3 la goc quay theta2 % moment quan tinh cua khop thu 4 J1=0.021; J2=0.0034; J3=.006; J40=0.001; J4=J40+Jt; J124=J1+J2+J4; J24+J2+J4; m1=4; m2=1.5; m3=2; m40=.6; m4=m40+mt; m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; l1=.25; l2=.15; % Tinh T T=m234*l1*l2*sin(u(3)); % Tinh toan cac gia tri Hij H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(u(3)); H12=m234*l2^2+J24+m234*l1*l2*cos(u(3)); H13=J4; H21=H12; H22=m234*l2^2+J24; H31=J4; H32=J4; H33=J4; DH=J4^2*(2*H12-H11-H22)+J4*(H11*H22-H12^2); % Cac gia tri dau ra bij b11=DH^-1*(H22*H33-H23*H32); b12=DH^-1*(H13*H32-H12*H33); b13=DH^-1*(H11*H23-H13*H22); b21=DH^-1*(H23*H31-H21*H33); b22=DH^-1*(H11*H33-H13*H31); b23=DH^-1*(H13*H21-H11*H23); b31=DH^-1*(H21*H32-H31*H22); b32=DH^-1*(H12*H31-H11*H32); b33=DH^-1*(H11*H22-H12^2); % Dau ra BT=[b11;b12;b21;b22;b31;b32;b13;b23;b33;T]; %================================================= Tinh_ai.m% Tinh toan gia tri ai function ai=tinh_ai(u) % cac dau vao b11=u(1); b12=u(2); b21=u(3); b22=u(4); b31=u(5); b32=u(6); h1=u(7); h2=u(8); %tinh toan a1=-(b11*h1+b12*h2); a2=-(b21*h1+b22*h2); a3=-(b31*h1+b32*h2); ai=[a1;a2;a3]; %end %================================================= tinh_ddtheta % TÝnh to¸n gãc ®iÒu khiÓn thùc function d2theta=tinh_ddtheta(u) u1=u(1); u2=u(2); u4=u(3); % ai duoc dua tu khau tinh_ai den a1=u(4); a2=u(5); a4=u(6); % bij duoc tinh o chtrinh tinh_bij_T.m % h1,h2 duoc xac dinh trong subsystem khau_dieu_chinh % chuong trinh tinh_Hij.m b11=u(7); b12=u(8); b21=u(9); b22=u(10); b31=u(11); b32=u(12); b13=u(13); b23=u(14); b33=u(15); % init=u(16); % a1=-(b11*h1+b12*h2); % a2=-(b21*h1+b22*h2); % a4=-(b31*h1+b32*h2); %Tinh dao ham cap hai cua goc quay thuc ddtheta1=a1+b11*u1+b12*u2+b13*u4; ddtheta2=a2+b21*u1+b22*u2+b23*u4; ddtheta4=a4+b31*u1+b32*u2+b33*u4; d2theta=[ddtheta1;ddtheta2;ddtheta4]; Danh môc h×nh vÏ vµ ®å thÞ Trang H×nh 1.1 Ph©n lo¹i robot c¬ b¶n 9 H×nh 1.2 H×nh d¹ng cña c¸c lo¹i robot c¬ b¶n. 10 B¶ng 1.3 C¸c d¹ng c¬ b¶n cña c¸c khíp Robot 10 H×nh 1.4 Kh«ng gian lµm viÖc cña Robot 11 H×nh 1.5 S¬ ®å khèi cña Robot 11 H×nh 1.6 S¬ ®å khèi ®iÒu khiÓn vÞ trÝ Robot 13 H×nh 1.7 æn ®Þnh hÖ phi tuyÕn 17 H×nh 1.8 §iÒu khiÓn tuyÕn tÝnh h×nh thøc b»ng bé ®iÒu khiÓn ph¶n håi tr¹ng th¸i 19 H×nh 1.9 Quan s¸t tr¹ng th¸i 20 H×nh 1.10 ThiÕt kÕ bé ®iÒu khiÓn 21 H×nh 1.11 S¬ ®å cÊu tróc hÖ ®iÒu khiÓn §LH ng­îc 22 H×nh 1.12 HÖ thèng ®iÒu khiÓn thÝch nghi theo sai lÖch 25 H×nh 1.13 HÖ thèng ®iÒu khiÓn thÝch nghi theo m« h×nh chuÈn. 26 H×nh 1.14 S¬ ®å khèi tæng qu¸t hÖ thÝch nghi 27 H×nh 1.15 §èi t­îng ®­îc ®iÒu khiÓn bëi r¬le 2 vÞ trÝ 28 H×nh 1.16 Quü ®¹o pha víi ®­êng chuyÓn ®æi e = 0 29 H×nh 1.17 HÖ tr­ît víi luËt chuyÓn ®æi m¹ch ph¶n håi 29 H×nh 1.18 M¹ch ®iÒu khiÓn víi ph¶n håi néi 30 H×nh 1.19 Quü ®¹o pha víi ®­êng chuyÓn ®æi e = -(y1+ay2) = 0 30 H×nh 1.20 S¬ ®å nguyªn lý ®iÒu khiÓn kiÓu tr­ît 33 H×nh 2.1 S¬ ®å khèi m« h×nh ®èi t­îng cã tÝnh ®Õn c¸c sai lÖch kh«ng cÊu tróc 40 H×nh 2.2 Minh ho¹ kh¸i niÖm æn ®Þnh Lyapunov 42 H×nh 2.3 MÆt ph¼ng tr­ît s(t) 44 H×nh 2.4 Thêi gian tÝn hiÖu ®iÒu khiÓn ch¹m vµo mÆt tr­ît 44 H×nh 2.5 HiÖn t­îng chattering 44 H×nh 2.6 L­îc ®å ®iÒu khiÓn tr­ît c¶i tiÕn sö dông m« h×nh tr­ît SMC 50 H×nh 2.7 Gi¶n ®å pha theo luËt ®iÒu khiÓn HOSM 52 H×nh 2.8 §Þnh nghÜa líp biªn B(t) 54 H×nh 3.1 Robot Scara Serpent vµ c¸c h×nh chiÕu 58 H×nh 3.2 CÊu h×nh vµ hÖ trôc täa ®é g¾n trªn khóc tay cña Robot 59 B¶ng 3.3 C¸c th«ng sè ®éng häc cña robot Serpent 60 B¶ng 3.4 Th«ng sè c¸c ®éng c¬ cña robot 60 H×nh 3.5 Giíi h¹n gãc quay cña 2 khíp 61 H×nh 3.6 Giíi h¹n kh«ng gian lµm viÖc cña robot Serpent 61 B¶ng 3.7 Tham sè Denavit – Hartenberg cña Robot Scara Serpent 63 B¶ng 3.8 Th«ng sè cña R«bèt Serpent 69 H×nh 4.1 S¬ ®å cÊu tróc chung cña hÖ thèng ®iÒu khiÓn tay m¸y Robot 80 H×nh 4.2 S¬ ®å ®iÒu khiÓn chuyÓn ®éng trong kh«ng gian khíp 81 H×nh 4.3 L­îc ®å khíp tay m¸y 81 H×nh 4.4 S¬ ®å khèi c¬ cÊu chuyÓn ®éng 82 H×nh 4.5 M« h×nh truyÒn ®éng ®iÖn R«bèt 83 H×nh 4.6 M« h×nh to¸n häc ®éng c¬ 83 H×nh 4.7 D¹ng quü ®¹o chuyÓn ®éng chuÈn 85 H×nh 4.8 Mô hình hóa đáp ứng đầu ra thực của Robot 91 H×nh 5.1 S¬ ®å khèi m« h×nh ho¸ Robot Scara Serpent 96 H×nh 5.2 S¬ ®å khèi t¹o quü ®¹o chuyÓn ®éng chuÈn 97 H×nh 5.3 Mô hình mạch điều khiển cho khớp 1 của Robot Serpent 97-98 H×nh 5.4 Menu chÝnh cña ch­¬ng tr×nh 98 H×nh 5.5 B¶ng th«ng sè cña Robot Scara Serpent 99 H×nh 5.6 S¬ ®å khèi m« h×nh chuÈn Robot Serpent 99 H×nh 5.7 Giao diÖn m« pháng Robot Serpent 100 H×nh 5.8 M« h×nh Robot 100 H×nh 5.9 Cöa sæ trî gióp 100 B¶ng 5.10 Các tham số của tay máy 101 H×nh 5.11 C¸c th«ng sè cña t¶I 101 H×nh 5.12 Quü ®¹o chuyÓn ®éng chuÈn cña Robot Scara Serpent 102 H×nh 5.13 Quü ®¹o ®Æt vµ quü ®¹o thùc khíp 1 102-103 H×nh 5.14 Quü ®¹o ®Æt vµ quü ®¹o thùc khíp 2 103 H×nh 5.15 Quü ®¹o ®Æt vµ quü ®¹o thùc khíp 4 104 H×nh 5.16 Moment tác động lên các trục (dùng hàm sgn(S)) 106 H×nh 5.17 Mô men tác động lên các trục (dùng hàm sat(S)) 105 B¶ng 5.18 Các tham số lựa chọn cho mạch điều khiển 107 H×nh 5.19 Sai sè ei(t) 107-108 Danh môc c¸c ký hiÖu vµ c¸c ch÷ viÕt t¾t RBCN R« bèt c«ng nghiÖp §KRB §iÒu khiÓn Robot §KC§ §iÒu khiÓn chuyÓn ®éng §CMC §éng c¬ mét chiÒu §CXC §éng c¬ xoay chiÒu §C K§B §éng c¬ kh«ng ®ång bé BB§ Bé biÕn ®æi §KTN §iÒu khiÓn thÝch nghi DTC Direct Torque Control DSP Digital Signal Processor IC Integrated Circuits (M¹ch tÝch hîp) FMS Flexible Manufacturing System SMC Sliding Mode Control (M« h×nh ®iÒu khiÓn tr­ît) PWM Pulse Width Modulation (§iÒu biÕn ®é réng xung) PID Proportional Integral Derivative Controller MRAC Model Refrence Adaptive Control PTP Point to point CPC Continuous Path Control