Tìm hiểu Robot công nghiệp

®iÓm cña hÖ thèng servo; Kh¶ n¨ng ®Þnh vÞ tèt cña robot nhê hÖ thèng ®iÒu khiÓn servo sÏ gi¶m ®i ®é phøc t¹p cña tay g¾t. Robot cã kh¶ n¨ng thùc hiÖn nhiÒu chuyÓn ®éng cã yªu cÇu phøc t¹p ®ång thêi cã kh¶ n¨ng thùc thi nhiÒu ch­¬ng tr×nh ®Ó ®¸p øng theo c¸c yªu cÇu s¶n xuÊt cã tr×nh tù thay ®æi kh¸c nhau, gióp cho hÖ thèng s¶n xu¸t cã tÝnh linh ho¹t cao. 3. Hµm truyÒn chuyÓn ®éng mçi khíp ®éng. Trong c¬ cÊu tay m¸y mçi bËc tù do th­êng ®­îc ®iÒu khiÓn b»ng mét hÖ thèng truyÒn ®éng riªng. TruyÒn ®éng cã thÓ lµ thuû lùc, khÝ nÐn nh­ng nhiÒu h¬n c¶ lµ b»ng ®éng c¬ ®iÖn mét chiÒu S¬ ®å sau lµ s¬ ®å ®éng c¬ ®iÖn mét chiÒu víi tÝn hiÖu vµo lµ ®iÖn ¸p Va ®Æt vµo phÇn øng, tÝn hiÖu ra lµ gãc quay θm cña trôc ®éng c¬ cã kÝch tõ cho ®éng c¬ vµ kÝch tõ tõ ®éc lËp Trôc ®éng c¬ th­êng nèi liÒn víi hép gi¶m tèc råi tíi trôc phô t¶i. Gäi n lµ tû sè truyÒn, θL lµ gãc quay cña trôc phô t¶i, ta cã: (1) Momen trªn trôc ®éng c¬ b»ng tæng momen cÇn ®Ó ®éng c¬ quay céng víi momen phô t¶i quy vÒ trôc ®éng c¬ M(t) = Mm(t) + ML* (t) (2) Ký hiÖu Jm - momen qu¸n tÝnh cña ®éng c¬ JL - momen qu¸n tÝnh cña phô t¶i Ta cã : Mm (t) = Jm (t) + fm (t) (3) ML (t) = JL (t) + fL (t) (4) Trong ®ã Fm vµ F2 lµ hÖ sè c¶n cña ®éng c¬ vµ cña phô t¶i Theo ®Þnh luËt b¶o toµn n¨ng l­îng, c«ng do phô t¶i sinh ra tÝnh trªn trôc phô t¶i lµ M2θ2 ph¶i b»ng c«ng quy vÒ trôc ®éng c¬ M2θm. Tõ ®ã ta cã: (5) tÝnh tíi (1) vµ (4) ta cã: (6) thay (3) vµ (6) vµo (2) ta cã: M(t) = (Jm + n2JL ) m (t) + (fm + n2fL) m (t) (7) M(t) = Jm (t) + fm (t) (8) Víi J = Jm + n2JL f = fm + n2fL Trong ®ã : J - momen qu¸n tÝnh tæng hiÖu dông F - hÖ sè tæng hiÖu dông Bëi v× momen trªn trôc ®éng c¬ phô thuéc tuyÕn tÝnh víi c­êng ®é dßng ®iÖn phÇn øng vµ kh«ng phô thuéc vµo gãc qua vµ vËn tèc gãc. M(t) = Kaia (t) (9) Víi ia – c­êng ®é dßng ®iÖn Ka – hÖ sè tØ lÖ momen §èi víi m¹ch ®iÖn phÇn øng (10) Víi Ra, La - ®iÖn trë vµ ®iÖn c¶m phÇn øng eb - søc ph¶n ®iÖn ®éng cña ®éng c¬ eb(t) = Kbm (t) (11) Ke – hÖ sè tû lÖ víi søc ph¶n ®iÖn ®éng Dïng phÐp biÕn ®æi laplace, tõ (10) ta cã : Ia (s) = (12) Ta cã : M(s) = s2Jm (s) + sfm (s) (13) Tõ (9) ta cã : M(s) = KaIa(s) = Ka (14) Tõ (13) vµ (14) ta cã: (15) §©y lµ hµm truyÒn cÇn x¸c ®Þnh, nã lµ tû sè gi÷a tÝn hiÖu ra (gãc quay θm) vµ tÝn hiÖu vµo cña hÖ thèng (®iÖn ¸p Va) BiÕn ®æi (15) d­íi d¹ng sau: (16) Bëi v× hÖ hrèng gåm cã c¶ ®éng c¬ vµ phô t¶i nªn tÝn hiÖu ra lµ gãc quay cña trôc phô t¶i θL nh­ trong c«ng thøc (1). Cuèi cïng hµm truyÒn chuyÓn ®éng mét bËc tù do cña tay m¸y lµ : (17) vµ s¬ ®å khèi t­¬ng øng: n ua(s)+ qL(s) - Hµm truyÒn chuyÓn ®éng cña mét bËc tù do §Ó ®¬n gi¶n ho¸ cã thÓ kh«ng xÐt ®Õn ¶nh h­ëng cña ®iÖn c¶m phÇn øng lµ La v× nã th­êng qu¸ nhá so víi c¸c nh©n tè ¶nh h­ëng c¬ khÝ kh¸c. Ph­¬ng tr×nh (17) cßn l¹i: (18) 4. §iÒu khiÓn chuyÓn ®éng mçi khíp ®éng. NhiÖm vô ®iÒu khiÓn lóc nµy lµ lµm sao cho ®éng c¬ dÞch chuyÓn ®i mét gãc b»ng gãc quay ®· tÝnh to¸n ®Ó ®¶m b¶o theo quü ®¹o ®· chän, ®Ó ®iÒu khiÓn theo quü ®¹o ph¶i ®Æt ®iÖn ¸p vµ ®éng c¬ tû lÖ thuËn víi ®é sai lÖch gãc quay cña khíp ®éng (19) Víi Kp _- hÖ sè truyÒn tÝn hiÖu ph¶n håi vÒ vÞ trÝ e(t) - ®é sai lÖch gãc quay : e(t)= (t) - (t) Gi¸ trÞ gãc quay tøc thêi (t) ®­îc ®o b¸o b»ng c¶m biÕn quang häc hoÆc chiÕt ¸p BiÕt ®æi laplace (19): (20) Thay (20) vµo (18) (21) Sau khi biÕn ®æi ®¹i sè ®¬n gi¶n ta cã hµm truyÒn (22) Tõ pt (22) hÖ ®iÒu chØnh tû lÖ dÞch chuyÓn mçi khíp ®éng lµ hÖ bËc 2. NÕu c¸c hÖ sè ph­¬ng tr×nh bËc 2 nµy lµ d­¬ng th× hÖ sÏ æn ®Þnh. §Ó n©ng sao ®Æc tÝnh ®éng lùc häc cña hÖ vµ ®Ó gi¶m sai sè tÜnh cã thÓ t¨ng hÖ sè Kp vµ xÐt tíi ®¹o hµm cña ®é sai lÖch dÞch chuyÓn qua hÖ sè k. Muèn thÕ th× ®iÖn ¸p ®iÒu khiÓn ®éng c¬ sÏ tØ lÖ thuËn víi ®é sai lÖch dÞch chuyÓn vµ ®¹o hµm cña nã: (23) Nh­ vËy khi cã liªn hÖ ph¶n håi hÖ thèng trë thµnh khÐp kÝn Sau khi biÕn ®æi laplace biÓu thøc (23) vµ thay Uavµo (21) ta cã hµm truyÒn: (24) Tõ ®ã cã: (25) n Ua(s) qL(s)  - S¬ ®å ®iÒu khiÓn chuyÓn dÞch 1 khíp ®éng cã liªn hÖ ph¶n håi 5. §¸nh gi¸ ®Æc tÝnh ®éng lùc cña hÖ ®iÒu khiÓn. Tr­íc hÕt cÇn nghiªn cøu ph¹m vi thay ®æi hÖ sè truyÒn ph¶n håi vÒ vih trÝ vµ vËn tèc (hÖ sè Kp vµ hÖ sè Kv) Nh­ ®· biÕt, ph­¬ng tr×nh ®Æc trung cña hÖ bËc 2 cã d¹ng chuÈn nh­ sau: S2 + 2xwns + wn2 = 0 (26) Víi e vµ wn - hÖ sè gi¶m chÊn vµ tÇn sã riªng cña hÖ dao ®éng §èi chiªu gi÷a 2 ph­¬ng tr×nh (25) vµ (26) ta cã: (27) 2xwn (28) §Ó ®¶m b¶o chÕ ®é lµm viÖc cã gi¶m chÊn cÇn thiÕt th× x ³ 1. Do vËy : x (29) Tõ ®iÒu kiÖn (29): (30) §Ó tr¸nh dßng ®iÖn céng h­ëng cÇn chän wn £ 0,5wr  (31) TÇn sè céng h­ëng cña hÖ kÕt cÊu chñ yÕu phô thuéc vµo vËt liÖu chÕ t¹o m¸y. NÕu gäi ®é cøng v÷ng cña hÖ khíp ®éng lµ Ker th× momen g©y nªn Kevθm(t) , c©n b»ng víi momen qu¸n tÝnh cña ®éng c¬ Jm (t) + Kevθm (t) = 0 (32) Pt ®Æc tr­ng cña biÓu thøc (32) sau khi biÕn ®æi laplace lµ : Js2 + Kev = 0 (33) Gi¶i ph­¬ng tr×nh trªn r×m tÇn sè céng h­ëng (34) NÕu w0 lµ tÇn sè céng h­ëng ®o ®­îc øng víi momen qu¸n tÝnh s0 ®· biÕt th× ®èi víi momen qu¸n tÝnh S1 tÇn sè céng h­ëng t­¬ng øng lµ : (35) Theo ®iÒu kiÖn (31), (27) cã: (36) TÝnh thªm biÓu thøc (34) ta cã: (37) BiÕt Kp thay vµo (30) nhËn ®­îc:  (38) 6. §iÒu khiÓn chuyÓn ®éng robot nhiÒu bËc tù do NhiÖm vô quan träng ®Çu tiªn cña ®iÒu khiÓn robot lµ ®¶m b¶o sao cho ®iÓm t¸c ®éng cuèi E ( End- effector) cña c¬ cÊu tay m¸y ph¶i dich chuyÓn b¸m theo mét quü ®¹o ®Æt tr­íc. Kh«ng nh÷ng thÕ, hÖ to¹ ®é xe, ye , ze g¾n liÒn t¹i ®iÓm E ®ã ph¶i duy tr× theo mét ®Þnh h­íng nµo ®ã. Tuy nhiªn ë ®©y ta ph¶i tÝnh tíi c¸c ®iÒu kiÖn thùc tÕ khi lµm viÖc, nh­ lµ cã sù t¸c ®éng cña momen lùc, cña m«i tr­êng lµm viÖc... C¸c yÕu tè nµy sÏ lÇn l­ît ®­îc tÝnh tíi vµ ph­¬ng ph¸p ®iÒu khiÓn còng trë nªn ®a d¹ng vµ phong phó h¬n, tuú theo yªu cÇu n©ng cao chÊt l­îng ®iÒu khiÓn. Nh­ ®· biÕt c¸c khíp ®éng cña robot ®Òu cã nguån ®éng lùc riªng, nªn ®iÒu khiÓn robot lµ ®iÒu khiÓn c¸c nguån ®éng lùc ®éc lËp ®ã. S¬ ®å khèi tæng qu¸t cña hÖ thèng ®iÒu khiÓn robot ®­îc m« t¶ trªn h×nh sau: FM Robot Bé ®iÒu khiÓn qd e q S¬ ®å hÖ thèng ®iÒu khiÓn qd : vect¬ vÞ trÝ ®Æt cña biÕn khíp : lµ vect¬ tèc ®é thay ®æi biÕn khíp t­¬ng øng FM : vect¬ lùc ®éng t¹o nªn ë khíp ®éng. §ã lµ lùc hoÆc momen t¹o nªn bëi nguån ®éng lùc ë khíp ®éng. Ph­¬ng tr×nh ®éng lùc häc cña Robot ®· ®­îc viÕt d­íi d¹ng ma trËn: (39) CÊu tróc bé ®iÒu khiÓn vso thÓ lùa chän lµ luËt ®iÒu khiÓn tû lÖ ®¹o hµm (PD – prportional – Derivative): (40) Trong ®ã : e - sai sè vÞ trÝ cña khíp ®éng : e = qd – q - sai sè tèc ®é : Kp - ma trËn ®­ßng chÐo c¸c hÖ sè khuyÕch ®¹i cña n líp ®éng : Kp = diag (Kp1, Kp2, ... , Kpn) KD- ma trËn ®­êng chÐo cña hÖ sè ®¹o hµm cña n khíp ®éng : KD = diag (KD1, KD2, ... , KDn) S¬ ®æ hÖ thèng ®iÒu khiÓn ph¶n håi víi cÊu tróc ®iÒu khiÓn PD cã d¹ng ®¬n gi¶n : KP qD e Robot KD S¬ ®å cÊu tróc hÖ thèng víi bé ®iÒu khiÓn PD cã ®é t¾t dÇn Trªn s¬ ®å trªn tÝn hiÖu ®Æt vÞ trÝ qd ®­îc so s¸nh víi vÞ trÝ tøc thêi q vµ ®é sai lÖch e t¸c ®éng vµo kh©u khuyÕch ®¹i víi hÖ sè Kp. TÝn hiÖu ra cña kh©u tû lÖ ®­îc céng ®¹i sè víi tÝn hiÖu t¾t dÇn tû lÖ víi tèc ®é cña khíp ®éng Fm = Kp (qd - q ) + KD (41) HÖ thèng víi cÊu tróc luËt ®iÒu kiÓn PD cã ®é t¾t dÇn sÏ kh«ng thÝch hîp víi mét sè kiÓu robot. Mét d¹ng hÖ thèng ®iÒu khiÓn kh¸c giíi thiÖu trªn h×nh trªn cã bæ sung thªm tÝn hiÖu ®Æt tèc ®é qd vµ ®é sai lÖch tèc ®é e t¸c ®éng vµo kh©u khuyÕch ®¹i KD KP - qD q Robot FM KD - - §é chÝnh x¸c tÜnh cña hÖ thèng ®iÒu khiÓn cã thÓ n©ng cao b»ng c¸ch t¨ng hÖ sè khuyÕch ®¹i Kp. Tuy nhiªn hÖ sè Kp lín sÏ lµm gi¶m ®é æn ®Þnh cña hÖ thèng còng nh­ lµm t¨ng ®é nhiÔu. Bé ®iÒu khiÓn cã cÊu tróc PID sÏ kh¾c phôc ®­îc nh­îc ®iÓm ®ã cña bé ®iÒu khiÓn PD. Khi ®ã ph­¬ng tr×nh lùc ®éng FM t¸c ®éng lªn khíp ®éng sÏ cã d¹ng : (42) Trong ®ã : Ki – ma trËn ®­êng chÐo c¸c hÖ sè tÝch ph©n cña n khíp ®éng Ki = diag ( Ki1, Ki2, ... , Kin) Nh­ vËy tuú thuéc cÊu tróc ®· lùa chän bé ®iÒu khiÓn, ta ®em ®èi chiÕu ph­¬ng tr×nh (41) , (42) víi ph­¬ng tr×nh Lagrange, tõ ®ã nhËn ®­îc c¸c ph­¬ng tr×nh cña hÖ ®iÒu khiÓn t­¬ng øng. Tõ c¸c ph­¬ng tr×nh cña hÖ ®iÒu khiÓn nµy cã thÓ lùa chän Kp, KD vµ Ki sao cho sai lÖch vÞ trÝ e sÏ tiÕn tíi 0. 7. §iÒu khiÓn ®éng c¬ thuû lùc C¸c ®éng c¬ thuû lùc ®­îc ®iÒu khiÓn b»ng c¸ch thay ®æi l­u l­îng dÇu qua b¬m. BÊt kÓ sù kh¸c nhau vÒ cÊu tróc vËt lý, c¸c mèi quan hÖ c¬ b¶n gi÷a l­u l­îng vµ ¸p suÊt, chuyÓn ®éng cña chÊt láng vµ chuyÓn ®éng cña c¸c chi tiÕt, sù c©n b»ng c¬ häc cña c¸c chi tiÕt ®Õu xuÊt ph¸t tõ ®¹o hµm quan hÖ vµo/ra. Gi¶ sö Q lµ l­u l­îng cung cÊp, Qm lµ l­u l­îng vµo cña ®éng c¬, Qt lµ l­u l­îng tæn hao do lät dÇu trªn b¬m, Qc lµ l­u l­îng tæn hao do tÝnh nÐn ®­îc cña dÇu, ta nhËn ®­îc ph­¬ng tr×nh c©n b»ng l­u l­îng nh­ sau: Q = Qm + Ql + Qc (43) C¸c ®¹i l­îng tæn hao Ql vµ Qc ®­îc tÝnh ®Õn khi hÖ thèng lµm viÖc d­íi ¸p suÊt cao, cì hµng tr¨m atmosphe. Gäi P lµ chªnh lÖch ¸p suÊt gi÷a ®Çu ra vµ ®Çu vµo cña b¬m do t¶i, ta cã quan hÖ: Ql = klP (44) Tæn hao n¨ng l­îng do tÝnh nÐn ®­îc cña chÊt láng tû lÖ víi thÓ tÝch tøc thêi cña chÊt láng V vµ ¸p suÊt P hÖ sè nÐn g vµ biÕn thøc s : Qc = g,VsP (45) Tõ (6.12) thÊy r»ng hÖ sè tû lÖ kc = Υ.V gi÷a ®¹o hµm theo thêi gian cña ¸p suÊt P vµ l­u l­îng Qc phô thuéc vµo thÓ tÝch V cña chÊt láng.V× vËy ®èi víi ®éng c¬ quay th× kc lµ h»ng sè, cßn ®èi víi ®éng c¬ tÞnh tiÕn th× V thay ®æi nªn ph¶n øng cña hÖ thèng tuú thuéc ®iÓm c«ng t¸c. L­u l­îng chÊt láng vµo ®éng c¬ tû lÖ víi l­îng biÕn thiªn trong mét ®¬n vÞ thêi gian cña tÓ tÝch chÊt láng trong bÓ chøa. MÆt kh¸c l­îng biÕn thiªn nµy tû lÖ víi vËn tèc gãc cña ®éng c¬. Cuèi cïng nhËn ®­îc: Qm = kqw (46) §iÒu kiÖn c©n b»ng c¬ häc cña c¸c chi tiÕt chuyÓn ®éng ®­îc m« t¶ b»ng ph­¬ng tr×nh t­¬ng tù nh­ (3.3) Cm = (sIm + Fm)w +Cr (47) Cuèi cïng th× momen cña ®éng c¬ tû lÖ víi ®é chªnh lªch ¸p suÊt cña b¬m: Cm = ktP (48) §èi víi van ®iÒu khiÓn, hµm truyÒn gi÷a vÞ trÝ X cña van vµ ®iÖn ¸p ®iÒu khiÓn Vc ®­îc x¸c ®Þnh nh­ sau: (49) V× h»ng sè thêi gian Ts rÊt nhá (cì mili gi©y) so víi c¸c h»ng sè thêi gian kh¸c cña hÖ thèng, nªn Gs ®­îc coi lµ hÖ sè truyÒn t­¬ng ®­¬ng cña van vµ quan hÖ gi÷a Vc vµ X lµ tuyÕn tÝnh. Qm Kq+   X Q Qc P Cm W + + + - - - kq H×nh 4 – 7: S¬ ®å khèi hÖ ®iªu khiÓn ®éng c¬ thuû lùc Trªn c¬ së c¸c ph­¬ng tr×nh ®· nªu, cã thÓ x©y dùng s¬ ®å khèi cña hÖ thèng ®iÒu khiÓn cña ®éng c¬ thuû lùc, gåm van, bé ph©n phèi, ®éng c¬ nh­ trong h×nh 4-7. So s¸nh h×nh 4-2 víi h×nh 4-7 cã thÓ nhËn ra sù t­¬ng tù vÒ h×nh thøc gi÷a ®Æc tÝnh ®éng lùc häc cña ®éng c¬ thuû lùc vµ ®éng c¬ ®iÖn. Tuy nhiªn ®iÒu ®ã kh«ng cã nghÜa lµ cã thÓ dïng ®éng c¬ thuû lùc ®Ó ®iÒu khiÓn vËn tèc vµ ®iÒu khiÓn m«men thay cho ®éng c¬ ®iÖn. VÒ h×nh thøc th× m¹ch ph¶n håi ¸p suÊt cña ®éng c¬ ®iÖn, nh­ng kh«ng thÓ thay ®æi kÕt cÊu cña panel thuû lùc mét c¸ch dÔ dµng nh­ ®èi víi mét b¶ng m¹ch ®iÖn. Ch­¬ng V Ng«n ng÷ lËp tr×nh cña robot I. Giíi thiÖu chung vÒ lËp tr×nh cho robot. Khi xem xÐt vÊn ®Ò lËp tr×nh cho robot, kh«ng nh÷ng cÇn quan t©m ®Õn chuyÓn ®éng cña b¶n th©n robot mµ cßn ph¶i l­u ý ®Õn sù ho¹t ®éng cña c¶ hÖ thèng s¶n xuÊt, mµ robot lµ mét thµnh viªn. HÖ thèng s¶n xuÊt gåm nhiÒu ®¬n nguyªn hoÆc m«®un thiÕt bÞ s¶n xuÊt (Workceell). §ã lµ mét côm thiÕt bÞ, bao gåm mét hoÆc mét vµi robot, m¸y c«ng t¸c hoÆc thanh b¨ng chuyÓn, c¸c c¬ cÊu cÊp tho¸t ph«i hoÆc ®å g¸ phô trî kh¸c v.v… Th«ng th­êng ®Ó lËp tr×nh ®iÒu khiÓn robot tr­íc hÕt ph¶i m« pháng sù ho¹t ®éng cña nã “workcell” cô thÓ. Cã thÓ ph©n t¸ch thµnh 2 ph­¬ng ph¸p lËp tr×nh: lËp tr×nh trùc tuyÕn (on – line programming) vµ lËp tr×nh ngo¹i tuyÕn (off-line programming). Khi lËp tr×nh trùc tuyÕn, ng­êi vËn hµnh robot trùc tiÕp tiÕn hµnh lËp tr×nh trªn b¶n th©n r«bot hoÆc thiÕt bÞ phô trî kÌm theo. Cã ph­¬ng ph¸p lËp tr×nh thñ c«ng (manual input), ph­¬ng ph¸p lËp tr×nh theo kiÓu dËy häc b»ng dÉn d¾t(teach by lead through) vµ ph­¬ng ph¸p lËp tr×nh theo kiÓu d©þ häc b»ng thiÕt bÞd¹y häc gäi lµ “teach pendant” LËp tr×nh thñ c«ng lµ ph­¬ng ph¸p ®¬n gi¶n nhÊt lµ thÝch hîp nhÊt víi c¸c lo¹i robot kh«ng ®­îc trang bÞ thiÕt bÞ phô tr¬ ®éng (non – servo – controlled robots), kh«ng cã tÝn hiÖu ph¶n håi. §Ó lËp tr×nh cã thÓ dïng c¸c c÷ tú, c¬ cÊu cam, b¶ng æ c¾m ®iÖn, c¸c c«ng t¸c hµnh tr×nh v.v… c¸c lo¹i nµy tuy ®¬n gi¶n nh­ng l¹i cã nhiÒu øng dông. Thùc tiÔn s¶n xuÊt ®· chøng tá r»ng cã thÓ chÕ t¹o ra nh÷ng ch­¬ng tr×nh thao t¸c kh«ng ®¬n gi¶n b»ng c¸ch thøc ®¬n gi¶n ®ã. Ph­¬ng ph¸p dÉn d¾t ®Ó “d¹y häc” cho robot cã thÓ thùc hiÖn ®­îc nh÷ng c«ng viÖc tinh tÕ h¬n. Ng­êi vËn hµnh dÉn d¾t robot lµ c¸c thao t¸c theo yªu cÇu vµ ë nh÷ng vÞ trÝ cÇn thiÕt, c¸c tÝn hiÖu ®­îc nhËn biÕt tõ c¸c c¶m biÕn (servo) l¾p trªn robot sÏ ®­îc ghi l¹i. Khi lµm viÖc c¸c th«ng tin tÝn hiÖu ®· ghi nhí nµy sÏ t¸c ®éng vµo c¬ cÊu chÊp hµnh lµm cho robot hoµn toµn lÆp l¹i mét c¸ch tù ®éng c¸c thao t¸c ®· “häc” ®­îc. Cã thÓ “d¹y häc” cho robot theo ph­¬ng ph¸p dïng hép ®iÒu khiÓn “teach pendant”. ®ã lµ mét hép nhá cÇm tay cã c¸c nót bÊm ®iÒu khiÓn. Khi bÊm c¸c nót ®iÒu khiÓn nµy, c¸c khíp ®éng cña robot chuyÓn dÞch mét ®¹i l­îng nµo ®ã theo ý ng­êi ®iÒu khiÓn vµ c¸c gi¸ trÞ ghi l¹i ®Ó lËp tr×nh khi chuyÓn sang chÕ ®é tù ®éng. BiÓu thÞ c¸c ph­¬ng ph¸p lËp tr×nh kÓ trªn nh­ c¸c b­íc ph¸t triÓn hiÖn ®¹i dÇn. LËp tr×nh ngo¹i tuyÕn vµ mét b­íc ph¸t triÓn cao h¬n vÒ chÊt. LËp tr×nh ngoai tuyÕn (off – line programming) kh«ng tiÕn hµnh trùc tiÕp trªn thiÕt bÞ robot, mµ tiÕn hµnh gi¸n tiÕp trªn m¸y tÝnh. §Ó thùc hiÖn ®­îc viÖc trao ®æi th«ng tin gi÷a ng­êi vµ robot ph¶i dïng ®Õn ng«n ng÷ mµ c¶ hai ®Òu hiÓu ®­îc. C¸c ng«n ng÷ ch­¬ng tr×nh lµ ng«n ng÷ t­êng minh (expelicit languages) cho phÐp ng­êi lËp tuyÒn tÝn hiÖu cho m¸y c«ng t¸c thùc hiÖn c¸c thao t¸c cÇn thiÕt. M¸y tÝnh lµ mét thiÕt bÞ v¹n n¨ng, tæ hîp c¸c lÖnh ®Ó m¸y hoµn thµnh c¸c c«ng viÖc rÊt ®a d¹ng. Ng«n ng÷ m¸y cho phÐp dïng c¸c ng«n ng÷ bËc cao, gäi lµ ng«n ng÷ ph¸t triÓn. Trong tin häc cã nhiÒu l¹i ng«n ng÷. tuy nhiªn trong ®iÒu khiÓn nãi chung vµ ®iÒu khiÓn robot nãi riªng cÇn quan t©m tíi sù tiÖn lîi vµ nhanh chãng ®Ó ®¶m b¶o thêi gian thùc. V× thÕ xuÊt hiÖn nhiÒu lo¹i h×nh ng«n ng÷ chuyªn dông, ®Þnh h­íng cho mét lo¹i vÊn ®Ò nµo ®ã. Nh­ vËy, khi lËp tr×nh ngo¹i tuyÕn ®Ó cÇn dïng ng«n ng÷ lËp tr×nh. Møc th«ng th­êng dïng c¸c ng«n ng÷ t­êng minh (explicit language). Møc cao h¬n lµ dïng ng«n ng÷ kh«ng t­êng minh (implicit language). Nh÷ng ng«n ng÷ nµy cho phÐp ng­êi sö dông ra lÖnh ®Ó robot thùc hiÖn c¸c c«ng viÖc mong muèn mét c¸ch trùc tiÕp mµ kh«ng cÇn chØ ra chi tiÕt c¸c ho¹t ®éng cña robot nh­ ng«ng ng÷ lËp tr×nh th«ng th­êng. VÝ dô, ng«n ng÷ AML cña h·ng IBM, Karel cña h·ng General Motor, Fanuc, Rai, cña tËp ®oµn Automatix, Jas cña NASAvµ RPL cña SRI Inte rnational . C¸c ng«n ng÷ nµy cßn ®ang ë giai ®o¹n ph¸t triÓn . VËy ph¹m vi øng dông ng«n ng÷ t­êng minh còng cã 2 møc - Ng«n ng÷ robot chuyªn dông. Nh÷ng ng«n ng÷ lËp tr×nh nµy ®­îc x©y dùng nh­ mét nh«n ng÷ míi có ph¸p (syntax) vµ ng÷ nghÜa (sematics) cöa c¸cng«n ng÷ nµy cÇn ph¶i ®¬n gi¶n v× ng­êi lËp tr×nh cho c¸c øng dông c«ng nghiÖp kh«ng ph¶i lµ c¸c chuyªn gia l©p tr×nh. VÝ dô, ng«n ng÷ VAL 2 cña Unimation, Al cña ®¹i häc Stan®fod. Ng«n ng÷ ph¸t triÓn. Ng«n ng÷ nµy t¹o ra c¸c th­ viÖn robot cho ng«n ng÷ lËp tr×nh bËc cao ®· cã s½n. Nh÷ng ng«n ng÷ nµy ®­îc x©y dùng b»ng c¸ch dùa trªn c¸c ng«n ng÷ lËp tr×nh bËc cao th«ng dông (vÝ dô nh­ Pascal) vµ thªm vµo mét th­ viÖn c¸c thñ tôc vÒ hµm ®Æc biÖt dïng cho robot. Khi viÕt ch­¬ng tr×nh Pascal ®Ó ®iÒu khiÓn robot, ng­êi sö dông gäi c¸c hµm hoÆc c¸c thñ tôc ®· ®Þnh nghÜa tr­íc trong th­ viÖn ®Ó xö lý c¸c néi dung cã liªn quan ®Õn viÖc tÝnh to¸n hoÆc ®iÒu khiÓn robot. VÝ dô, PASRO (Pascal for robot) lµ mét th­ viÖc dïng cho lËp tr×nh robot, cung cÊp nhiÒu thñ tôc vµ hµm ®Æc biÖt ®Ó tÝnh to¸n vµ ®iÒu khiÓn eb dïng trong m«i tr­êng ng«n ng÷ Turbo Pascal, hoÆc Pasro/ C lµ ph¸t triÓn cña Pasro, nh­ng ®­îc biÕt trªn c¬ së ng«n ng÷ Turbo C II. M« t¶ c¸c vËt thÓ vµ nhiÖm vô. 1. M« t¶ c¸c vËt thÓ. C¸c vËt thÓ gäi lµ c¸c ®èi t¸c cña robot trong c«ng viÖc vµ c¸c thiÕt bÞ vµ ®å dïng trong m«i tr­êng lµm viÖc, chóng rÊt ®a d¹ng vµ phong phó. Tuy nhiªn cã thÓ dùa vµo c¸c khèi c¬ b¶n sau ®©y ®Ó m« t¶ chóng : - Khèi h×nh vËt thÓ trßn xoay (rotative) H×nh 5 – 1 :M« t¶ chi tiÕt h×nh trô - Khèi h×nh vËt thÓ l¨ng trô (prismatic) H×nh 5 -2 : M« t¶ vËt thÓ h×nh ®a diÖn C¸c vËt thÓ cã cÊu tróc hçn hîp (combination) cã thÓ m« t¶ b»ng c¸ch ghÐp nèi c¸c khèi c¬ b¶n. Khèi h×nh vËt thÓ xoay trßn ®­îc ®Æc tr­ng b»ng to¹ ®é t©m vµ b¸n kÝnh cña h×nh trßn c¸c tiÕt diÖn. VÝ dô, trªn h×nh 5-1 m« t¶ mét chi tiÕt m¸y h×nh trô b¸n kÝnh r = 0,5; chiÒu dµi l = 6; n»m däc theo trôc z. Khèi h×nh vËt thÓ ®a diÖn ®­îc ®Æc tr­ng b»ng to¹ ®é c¸c ®iÓm gãc c¹nh cña nã. Khi m« t¶ c¸c vËt thÓ trong m«i tr­êng lµm viÖc, còng nh­ khi m« t¶ qu¸ tr×nh thao t¸c cña robot vµ dÞch chuyÓn c¸c ®èi t¸c, cã thÓ dïng c¸c phÐp biÕn ®æi thuÇn nhÊt. XÐt vÝ dô sau ®©y: Cho mét vËt thÓ ®a diÖn trong hÖ to¹ ®é c¬ së xyz nh­ h×nh 5-2. Cã thÓ lËp ma trËn mµ mçi cét lµ mét vÐct¬ më réng biÓu thÞ mét ®iÓm gãc cña tam trô. Cô thÓ ®èi víi h×nh 5-2 cã 6 ®iÓm gãc, ta cã : (5.1) Thùc hiÖn phÐp biÕn ®æi : H = Tp(4,0,0).R(y,900) (5.2) sÏ lÇn l­ît ®­îc c¸c kÕt qu¶ sau : - TÞnh tiÕn vËt thÓ däc theo trôc x mét ®o¹n ng¾n b»ng 4 ®¬n vÞ t­¬ng øng víi phÐp biÕn ®æi H1 = Tp(4,0,0), h×nh 5-4 - Råi cho vËt thÓ quay quanh trôc y gãc 900 vµ kÕt qu¶ cña hai phÐp biÕn ®æi trªn H2 = Tp(4,0,0). R(y, 900) h×nh 5-5 TiÕp tôc quay vËt thÓ quanh trôc z 900 vµ kÕt qu¶ cña phÐp biÕn ®æi H theo (5.2), h×nh 5-3 H×nh 5 -4 : VËt thÓ sau phÐp biÕn ®æi H1 = Tp(4,0,0) H×nh 5-5 :. VËt thÓ sau phÐp biÕn ®æi H2 = Tp(4,0,0).R(y,900) H×nh 5 - 6 :. VËt thÓ sau c¸c phÐp biÕn ®æi H = Tp(4,0,0).R(y,900).R(x,900) Ma trËn H theo biÓu thøc (5.2) cã thÓ x¸c ®Þnh b»ng tÝch ma trËn quen biÕt ®èi víi c¸c phÐp biÕn ®æi c¬ b¶n nãi trªn, ta cã : (5.3) Nh­ vËy dÔ dµng x¸c ®Þnh ma trËn V biÓu thÞ vËt thÓ sau c¸c phÐp biÕn ®æi (5.2) (5.4) 2. M« t¶ nhiÖm vô NhiÖm vô giao cho robot thùc hiÖn th­êng ®­îc m« t¶ b»ng c¸c thao t¸c chuyÓn dÞch. Cã thÓ t×m hiÓu néi dung nµy th«ng qua mét vÝ dô cô thÓ nh­ tr×nh bµy trªn h×nh vÏ 5-4 ë ®©y robot lÊy trªn b¨ng chuyÒn c¸c chiÕc chèt h×nh trô ®­îc m« t¶ trªn h×nh 5-2, ®Ó l¾p vµo c¸c lç trªn th©n m¸y h×nh hép. H×nh 5 – 7 : M« t¶ nhiÖm vô NhiÖm vô nãi trªn cã thÓ m« t¶ b»ng mét d·y liªn tôc c¸c vÞ trÝ thao t¸c sau ®©y. P1 : H­íng tíi chèt P2 : KÑp vµo chèt KÑp chÆt chèt P3 : N©ng cao chèt P4: H­íng tíi lç víi c¸c gãc nghiªng nµo ®ã P5: Dõng l¹i khi ch¹m lç P6: §iÒu chØnh l¹i h­íng gãc nghiªng P7: L¾p chÆt vµo lç Nh¶ chèt ra P8: Rêi ®i. Dïng ph­¬ng ph¸p to¸n ®å chuyÓn ®æi ®Ó m« t¶ nhiÖm vô t¹i c¸c vÞ trÝ thao t¸c, nh­ tr×nh bµy trªn h×nh 5- 8. H×nh 5 - 8. To¸n ®å chuyÓn ®æi m« t¶ nhiÖm vô t¹i c¸c vÞ trÝ. T­¬ng øng: P1: ZT6E = P PA (5.5) P2: ZT6E = P PG (5.6) P3: ZT6E = P PD PG (5.7) P4: ZT6E = H HRi PHA PG (5.8) P5: ZT6E = H HRi PCH PG (5.9) P6: ZT6E = H HRi PAL PG (5.10) P7: ZT6E = H HRi PN PG (5.11) P8: ZT6E = H HRi PN PG (5.12) Trong ®ã ë mçi vÞ trÝ thao t¸c ®­îc m« t¶ b»ng ZT6E (xem h×nh 5-11) víi : Z - M« t¶ vÞ trÝ tr¹ng th¸i cña robot trong hÖ to¹ ®é gèc nµo ®ã T6 - M« t¶ vÞ trÝ tr¹ng th¸i cña ®iÓm cuèi cña c¸nh tay robot trong hÖ to¹ ®é cè ®Þnh g¾n víi gi¸ ®ì cña robot E - M« t¶ vÞ trÝ tr¹ng th¸i ®iÓm cuèi cña c«ng cô kÌm theo. Nãi ®Õn vÞ trÝ tr¹ng th¸i lµ nãi ®Õn vÞ trÝ vµ h­íng cña hÖ to¹ ®é g¾n víi vÞ trÝ ®ang kh¶o s¸t. Víi c¸ch m« t¶ ®ã, khi m« t¶ vÞ trÝ robot th× chØ cÇn thay ®æi Z vµ khi thay ®æi c«ng cô thao t¸c th× chØ cÇn thay ®æi E. §Ó m« t¶ cÊu tróc vÒ nhiÖm vô, ë ®©y còng dïng phÐp biÕn ®æi t­¬ng øng sau ®©y: P- M« t¶ vÞ trÝ tr¹ng th¸i cña chiÕu chèt trong hÖ to¹ ®é gèc PA- M« t¶ vÞ trÝ tr¹ng th¸i cña bµn kÑp ®ang h­íng tíi chèt PG- M« t¶ vÞ trÝ tr¹ng th¸i cña bµn kÑp ®ang kÑp chèt so víi chèt PD- M« t¶ vÞ trÝ tr¹ng th¸i cña bµn kÑp b¾t ®Çu mang chèt ®i H- M« t¶ vÞ trÝ tr¹ng th¸i cña hép th©n m¸y cã hai lç HRi- M« t¶ vÞ trÝ tr¹ng th¸i cña lç thø i trong hép th©n m¸y so víi hÖ to¹ ®é H. PHA- M« t¶ vÞ trÝ tr¹ng th¸i cña chèt ®ang h­íng tíi lç thø i PCH- M« t¶ vÞ trÝ tr¹ng th¸i cña chèt khi ch¹m vµo lç PAL- M« t¶ vÞ trÝ tr¹ng th¸i lóc b¾t ®Çu ®­îc l¾p vµo lç PN- M« t¶ vÞ trÝ tr¹ng th¸i cña chèt khi l¾p vµo lç ViÖc thùc hiÖn c¸c nhiÖm vô trªn cã thÓ trë nªn phøc t¹p h¬n nÕu trong ®ã ngoµi nh÷ng b­íc ®i x¸c ®Þnh, vÝ dô nh­ HRi hoµn toµn cã thÓ x¸c ®Þnh b¶n vÏ kü thuËt, cßn cã c¸c b­íc ®i kh¸c cÇn th«ng tin ®Ó ®iÒu chØnh. Trong tr­êng hîp nµy th­êng sö dông kÕt hîp víi c¸c biÖn ph¸p kh¸c, vÝ dô, ph­¬ng ph¸p d¹y häc cho robot. Trªn h×nh 5-9 tr×nh bµy 3 hÖ to¹ ®é Z, P vµ H g¾n liÒn t­¬ng øng víi robot, chiÕc chèt vµ th©n hép cã hai lç. H×nh 5 - 9: C¸c hÖ to¹ ®é Z, P vµ H ThiÕt lËp quan hÖ cña c¸c hÖ to¹ ®é Z, P vµ H víi nhau vµ víi c¸c hÖ to¹ ®é gèc. VÝ dô, trong hÖ to¹ ®é gèc nÕu chän gèc cña hÖ Z ë to¹ ®é x = - 30, y = 0, z = 50 nh­ trªn h×nh 5 - 9, ta cã: (5.13) Trªn h×nh 5-10 cã 2 hÖ to¹ ®é: T6 g¾n víi ®iÓm cuèi cña c¸nh tay robot vµ E víi t©m cña bµn kÑp. §iÓm gèc cña hÖ E n»m ë vÞ trÝ (0,0,10) trong hÖ to¹ ®é T6. Do vËy, ta cã: ( 5.14) H×nh5 – 10: C¸c hÖ to¹ ®é T6 vµ E. Trªn h×nh 5 - 11 tr×nh bµy c¸ch bè trÝ c¸c hÖ to¹ ®é: H gÇn víi h«p th©n m¸y> HR1 gÇn víi c¸c lç, theo ®ã ta cã: ; (5.15) H×nh 5 – 11: Hép th©n m¸y vµ c¸c hÖ to¹ ®é H×nh 5-12 m« t¶ vÞ trÝ tr¹ng th¸i khi chèt ®· l¾p vµo lç. Khi ®ã trôc Z cña chèt ph¶i trïng víi ®­êng trôc cña lç. V× chèt cã h×nh trô trßn ®èi xøng nªn ph­¬ng cña trôc x vµ y cã thÓ tuú ý. Chän ph­¬ng trôc xz th¼ng ®øng tøc lµ gi÷ nguyªn nh­ ph­¬ng th¼ng ®øng lóc chèt tõ vÞ trÝ n»m ë b¨ng t¶i võa ®­îc n©ng lªn. VËy vÞ trÝ tr¹ng th¸i cña chèt khi ®· l¾p vµo lç cã ®é s©u lµ 4 ®­îc m« t¶ bëi ma trËn chuyÓn ®æi sau so víi HR: (5.16) H×nh 4.13 m« t¶ vÞ trÝ bµn kÑp trªn chiÕc chèt, cßn ma trËn chuyÓn ®æi sau ®©y m« t¶ vÞ trÝ tr¹ng th¸i cña bµn kÑp chèt so víi chèt. H×nh 5 – 12: Chèt l¾p vµo lç (5.17) H×nh 5 – 13: Chèt l¾p vµo lç TiÕp theo x¸c ®Þnh c¸c ma trËn chuyÓn ®æi kh¸c c¸c tõ quan hÖ cÊu tróc ë c¸c vÞ trÝ thao t¸c kh¸c nhau cña robot nh­ ®· ®­îc m« t¶ ë trªn. øng víi vÞ trÝ p1 (5.5) tøc lµ lóc bµn kÑp h­íng tíi chèt: ZT6E = P PA Tõ ®ã : PA = P-1 ZT6E (5.18) øng víi vÞ trÝ p2(5.6) tøc lµ lóc bµn kÑp chÆt chèt : ZT6E = P PG Tõ ®ã : PD = P-1 ZT6E PG-1 (5.19) øng víi vÞ trÝ p3 (5.4) tøc lµ lóc n©ng cao chèt : ZT6E = P PD PG Tõ ®ã : PD = P-1 ZT6E PG-1 (5.20) øng víi vÞ trÝ p4 (5.6) tøc lµ lóc h­íng tíi lç : ZT6E = H HR1 PHA PG Tõ ®ã : PHA = (H HR1) ZT6E PG-1 (5.21) øng víi vÞ trÝ p5 (5.9) tøc lµ lóc dõng l¹i khi ch¹m vµo lç : ZT6E = H HR1 PAL PG Tõ ®ã: PCH = (H HR1)-1 ZT6E PG-1 (5.22) øng víi vÞ trÝ p6 (5.10) tøc lµ lóc ®iÒu chØnh h­íng gãc nghiªng: ZT6E = H HR1 PN PG Tõ ®ã : H = ZT6E( HR1 PN PG)-1 H×nh 5 – 14: C¸c vÞ trÝ thao t¸c p4, p5 vµp6 3.HÖ thèng robot lµm viÖc víi camera. 3.1 BiÕn ®æi chiÕu h×nh BiÕn ®æi chiÕu h×nh lµ phÐp biÕn ®æi quang häc ®Ó chiÒu c¸c ®iÓm trong kh«ng gian 3 chiÒu lªn mét mÆt ph¼ng. PhÐp biÕn ®æi nµy rÊt cÇn thiÕt khi trao ®æi th«ng tin gi÷a c¸c vËt thÓ 3 chiÒu vµ h×nh cña nã trªn c¸c mÆt ph¼ng. Trªn h×nh 5-15 m« t¶ s¬ ®å phÐp chiÒu quang häc H×nh 5 – 15: S¬ ®å hÖ thèng chiÕu hµnh HÖ t¹o ®é x, y, z g¾n liÒn víi camera vµ mÆt ph¼ng xy lµ mÆt ph¼ng chiÕu, trôc z trïng víi trôc chÝnh qua t©m thÊu kÝnh. Nh­ vËy gèc cña hÖ to¹ ®é xyz qua t©m cña mÆt ph¼ng chiÕu, cßn t©m thÊu kÝnh cã to¹ ®é lµ (0,0, l) Víi l lµ kho¶ng c¸ch tiªu cù, ë ®©y, hÖ to¹ ®é x, y, z trïng víi hÖ to¹ c¬ b¶n X, Y, Z. Gi¶ sö X, Y, Z lµ to¹ ®é cña mét ®iÓm bÊt kú trong kh«ng gian 3 chiÒu. NÕu Z > l tøc lµ tr­êng hîp vËt thÓ ®ang quan s¸t n»m tr­íc thÊu kÝnh tõ c¸c quan hÖ gi÷a c¸c h×nh tam gi¸c trªn h×nh 5 - 15 ta cã: (5.25) (5.26) Tõ ®ã: (5.24) (5.28) L­u ý r»ng c¸c ph­¬ng tr×nh trªn lµ phi tuyÕn tÝnh v× cßn tån t¹i phÐp chia cho Z. ë ®©y còng cã thÓ dïng ma trËn thuÇn nhÊt 4x4 ®Ó m« t¶ sù biÕn ®æi chiÕu h×nh. Tuy nhiªn sù kh¸c biÖt c¬ b¶n so víi khi dïng c¸c phÐp biÕn ®æi kh¸c tr­íc ®©y (xem ch­¬ng III) lµ sù kh«ng tuyÕn tÝnh nãi trªn. Mét ®iÓm M bÊt kú cã thÓ x¸c ®Þnh b»ng vect¬ r trong hÖ to¹ ®é X, Y, Z. R = (X, Y, Z)T (5.29) Cßn ë trong kh«ng gian to¹ ®é thuÇn nhÊt (homogeneous coordinate) ®iÓm M cã thÓ x¸c ®Þnh b»ng mét vÐct¬ më réng r: (5.30) Nh­ ®· tr×nh bµy trong ch­¬ng III, k lµ hÖ sè tû lÖ cã gi¸ trÞ kh¸c sè kh«ng vµ khi k = 1 th× c¸c to¹ ®é biÓu diÔn b»ng to¹ ®é cã thùc. Nh­ vËy, ®Ó x¸c ®Þnh to¹ ®é thùc cña ®iÓm M chia 3 thµnh phÇn ®Çu cña vect¬ më réng (5.30) cho thµnh phÇn thø 4: Ma trËn biÕn ®æi chiÕu h×nh cã thÓ x¸c ®Þnh nh­ sau: (5.31) VËy tÝch cña Ac lµ mét vect¬ (5.32) C¸c thµnh phÇn cña vect¬ më réng lµ to¹ ®é cña camera trong kh«ng gian gÇn to¹ ®é thuÇn nh©t. §Ó x¸c ®Þnh c¸c thµnh phÇn thùc trong hÖ to¹ ®é g¾n liÒn víi camera tøc lµ c¸c thµnh phÇn cña vect¬ rc (x,y,z) chia 3 thµnh phÇn ®Çu cña vect¬ më réng cho thµnh phÇn thø 4, ta cã: (5.33) §èi chiÕu víi (5.24) vµ (5.28) nhËn thÊy hai thµnh phÇn ®Çu cña vect¬ rc chÝnh lµ h×nh chiÕu cña ®iÓm M (X,Y,Z) lªn mÆt ph¼ng cña camera. Thµnh phÇn thø 3 kh«ng tån t¹i trªn mÆt ph¼ng chiÕu vµ lµ biÕn sè tù do khi biÕn ®æi ng­îc. BiÕn ®æi ng­îc khi chiÕu h×nh lµ phÐp biÕn ®æi c¸c ®iÓm tõ h×nh chiÕu trªn mÆt ph¼ng chiÕu thµnh c¸c ®iÓm t­¬ng øng trong kh«ng gian 3 chiÒu, tõ ph­¬ng tr×nh (5.32) ta cã: (5.34) Víi: (5.35) Gi¶ sö r»ng (x0, y0, 0) lµ to¹ ®é cña ®iÓm ®ang xÐt trªn h×nh chiÕu. Gi¸ trÞ 0 theo to¹ ®é Z chøng tá r»ng mÆt ph¼ng chiÕu n»m ë to¹ ®é Z = 0. cã thÓ biÓu diÔn ®iÓm nµy trong kh«ng gian to¹ ®é thuÇn nhÊt (5.36) Thay vµo (5.34) ta cã: (5.37) Vµ trong hÖ täa ®é X, Y, Z ta cã: r = (5.38) §©y lµ tr­êng hîp kh«ng chØ lµ h×nh chiÕu cña mét ®iÓm mµ lµ cña tÊt c¶ c¸c ®iÓm trong kh«ng gian X, Y, Z n»m trªn cïng mét ®­êng th¼ng- ®­êng th¼ng ®i qua c¸c ®iÓm (x0, y0, 0) vµ (0,0,l). Nh­ vËy tõ (5.24) vµ (5.25) ta cã ph­¬ng tr×nh cña ®­êng th¼ng nµy trong hÖ to¹ ®é X, Y, Z nh­ sau : (5.39) (5.40) Râ rµng lµ nÕu chØ biÕt to¹ ®é h×nh chiÕu cña mét ®iÓm (x0, y0) trªn mÆt ph¼ng chiÕu th× ch­a ®ñ ®Ó x¸c ®Þnh vÞ trÝ cña ®iÓm ®ã trong kh«ng gian 3 chiÒu, mµ cÇn ph¶i bæ sung thªm th«ng tin kh¸c, vÝ dô, to¹ ®é Z. Bëi thÕ thµnh phÇn thø 3 cña vect¬ rc ph¶i lµ biÕn sè tù do thay v× sè 0 tøc lµ : (5.41) VËy thay vµo (5.35) ta cã : (5.42) Sau khi chia thµnh 3 thµnh phÇn ®Çu cña vect¬ më réng r cho thµnh phÇn thø 4 ®Ó biÕn ®æi vÒ to¹ ®é X, Y, Z ta cã : (5.43) (5.44) (5.45) Tõ (5.45) rót ra Z vµthay vµo (5.43) vµ (5.44) ta cã: X = (5.46) Y = (5.47) 3.2. C¸c ph­¬ng tr×nh c¬ b¶n cña hÖ thèng camera. C¸c ph­¬ng tr×nh (5.32) vµ (5.33) lµ c¸c ph­¬ng tr×nh c¬ b¶n cña hÖ H×nh 5 -16 : S¬ ®å bè trÝ hÖ thèng camera.HÖ thèng camera. Tuy nhiªn khi thµnh lËp c¸c ph­¬ng tr×nh trªn ta ®· xem tr­êng hîp ®Æc biÖt lµ hÖ to¹ ®é camera x, y, z trïng víi hÖ to¹ ®é c¬ b¶n X, Y, Z d­íi ®©y xem xÐt tr­êng hîp chung khi x, y, z kh«ng trïng X, Y,Z. Trªn h×nh 5-16 ký hiÖu M lµ mét ®iÓm kh«ng gian ®­îc x¸c ®Þnh trong hÖ to¹ ®é c¬ b¶n X, Y, X vµ ®iÓm gèc hÖ toa ®é x, y, z ®Æt ë tam mÆt ph¼ng chiÕu, c¸ch t©m khíp quay mét kho¶ng c¸c biÓu thÞ b»ng rp (r1, r2,r3). Vậy điểm gốc của hệ tọa độ x,y,z xác định trong hệ tọa độ X,Y,Z bằng véc tơ r0 r0 = rk + rp (5.48) Khớp quay ở điểm đặt camera đảm bảo góc quay a giữa các trục x và X, góc nghiêng q giữa các trục z và Z. Từ đó ma trận biến đổi hệ tọa độ giữa x,y,z và X,Y,Z như đã trình bày ở chương III, có thể xác định như sau: Trong đó: (5.49) A0 = R(x,a) . R(z,q) - ( Xk + r1 ) - ( Yk + r2 ) - ( Zk + r3 ) 0 0 0 1 1 0 0 0 (5.50) R(x,a) = 0 Cosa sina 0 0 - sina cosa 0 0 0 0 1 cosq sinq 0 0 (5.51) R(z,q) = - sinq cosq 0 0 0 0 1 0 0 0 0 1 Sau phép biến đổi (4.49) có thể áp dụng trường hợp (4.31), (4.32) ta có: =Ac.A0. (5.52) 3.3. Phép chiếu hình lập thể (stereo). Như ở phần trên đây đã đề cập tới, một điểm trên mặt phẳng chiếu có thể là hình chiếu của nhiều điểm trong không gian, vì thế chưa thể xác định được vị trí thực của nó trong không gian ba chiều. Để hình dung được chiều sâu của hình ảnh cần đến phép chiếu lập thể (stereo). Phép chiếu lập thể cần đến 2 chiếu x1, y1 và x2, y2 của cùng một điểm M trong không gian (hình 4.17). Khoảng cách giữa hai tiêu cự của 2 thấu kính gọi là khoảng chuẩn B. Hình 5 - 17. Sơ đồ phép chiếu lập thể Nhiệm vụ đặt ra là phải xác định được tọa độ X,Y,Z của điểm M thông qua các tọa độ hình chiếu x1, y1 và x2, y2 của nó. Giả sử rằng các camera như nhau và cả hai hệ tọa độ gắn liền với camera hoàn toàn trùng khớp nhau, chỉ khác vị trí gốc ban đầu. Cũng như ở phần trên, khi cho các hệ tọa độ gắn liền với các camera trùng với hệ tọa độ cơ bản thì mặt phẳng chiếu xy trùng với mặt phẳng ZY. Khi làm như vậy tọa độ Z của điểm M được giữ như nhau đối với cả hai hệ tọa độ camera. Khi cho hệ tọa độ camera thứ nhất trùng với hệ tọa độ cơ bản X, Y, Z như hình 5-18 ta có: X1 = (l -Z1) (5.53) Tương tự làm như vậy đối với camera thứ hai ta có: X2 = (l -Z2) (5.54) Hình 5-18: hình5-17 nhìn từ trên xuống khi cho trùng hệ tọa độ camera Nhờ có các điều kiện, như đã nêu ở trên X2 =X1 + B (5.55) Z2 =Z1 = Z Từ (5.53) - (5.54) ta có: X2 = X1 + B = ( l - Z1) (5.56) Từ (5.55) - (5.56) ta có: Z = l - (5.57) Như vậy các tọa độ X, Y, Z của điểm M trong không gian 3 chiều hoàn toàn được xác định bằng (5.39), (5.40), (5.57) 3.4. Mô tả quan hệ robot và camera. Phương trình (5.34) cho biết tọa độ điểm của vật thể trong không gian 3 chiều X, Y, Z (véctơ ) thông qua thông tin về hình chiếu của điểm đó (véctơ ) trên mặt phẳng chiếu của camera. Ma trận (5.35) mô tả sự chuyển đổi từ hệ tọa độ gắn liền với camera sang hệ tọa độ cơ bản và có thể biểu thị tổng quát hơn cho cả vật thể là ma trận CAM. Theo các ký hiệu đã trình bày ở phần ví dụ trong phần 5.22 thì P mô tả vị trí trạng thái (vị trí và định hướng) của chiếu chốt vật thể trong hệ tọa độ gốc (hệ tọa độ cơ bản X, Y, Z). Vậy ở đây có thể biểu thị: P = CAM PC (5.58) với PC mô tả vật thể đang trong vị trí trạng thái của vật thể được xác định theo mối quan hệ bố trí không gian của robot, ví dụ (4.19) thì từ (4.60) có thể xác định ma trận chuyển đổi từ hệ tọa độ gắn liền với camera sang hệ tọa độ cơ bản: CAM = PPC1 (5.59) 4. Thể hiện chương trình thao tác của robot. Chương trình thao tác của robot thực hiện các nhiệm vụ được giao. Nhiệm vụ đó được mô tả bằng một dãy liên tiếp các vị trí thao tác. Trong ví dụ là thao tác từ p1 đến p8 (hình 5.8). Các thao tác này được thể hiện bằn một chương trình, trong đó từ các phương trình từ (5.5) đến (5.12) tương ứng với các thao tác từ p1 đến p8, cần xác định ra T, ứng với mỗi vị trí trạng thái của điểm cuối của tay robot trong hệ tọa độ cơ bản (hệ tọa độ cố định). Ví dụ, ứng với thao tác p1, từ (5.5) ta viết: ZT6E= PPA (5.60) Tacó: T6E = Z-1PPA (5.61) Và cứ thế tiếp đến p2 cho đến p8 Nhưng trước khi thể hiện chương trình các thao tác đó, cần sử dụng thêm 2 biến: COORD – Mô tả hệ tọa độ ứng với vị trí đang xét TOOL – Mô tả dụng cụ đang được sử dụng. Như vậy tất cả các phương trình (tương tự 5.61) ứng với các vị trí thao tác đều có thể viết dưới dạng sau: T6TOOL = COORD POS (5.62) Ví dụ, ứng với thao tác p1 ta có : COORD : = - Z + p TOOL : = E Và thao tác là: MOVE PA: Trong cách viết trên, dùng dấu (+) để chỉ phép nhân ma trận và dấu (-) để chỉ phép nhân ma trận nghịch đảo. Theo đó, chương trình thể hiện các thao tác của robot từ p1 đến p8 có thể viết như sau: TOOL : = E Gắn dụng cụ FOR I : = 1,2 DO BEGIN READ (CAMERA, PC): Thông tin từ camera P : = CAM + PC Vị trí của chốt COORD : = - Z + P Tọa độ của chốt MOVE PA: Hướng tới chốt MOVE PG : Sắp kẹp chốt GRASP : Kẹp TOOL : = E – PG Dụng cụ bay giờ bao gồn bàn kẹp đã kép chốt. MOVE PD : Vị trí xuất phát (bắt đầu mang chốt đi) HT : = HR [I] Vị trí lắp vao lỗ COORD : = - Z + H + HT Tọa độ cua lỗ MOVE PHA : Hướng tới lỗ MOVE PCH : Chạm vào lỗ MOVE PAL : Đặt đồng tâm với lỗ (lúc chuẩn bị lắp ) MOVE PN : Lắp vào lỗ RELEASE : Dời đi COORD : = - Z + H + HT + PN Tọa độ của lỗ đã lắp TOOL : + E MOVE PA END 5. Chuyển động giữa các điểm tựa 5.1. Đặt vấn đề. Khi lập trình quỹ đạo chuyển động robot để thực hiện nhiệm vụ đặt ra, thông thường cần biết vị trí và định hướng các khâu tác động cuối của robot ở những điểm khác nhau trong không gian thao tác. Gọi chúng là các điểm tựa. Các điểm tựa này bao gồm các vị trí bắt buộc phải đi qua để thực hiện nhiệm vụ và cả những vị trí cũng phải đi qua để tránh chướng ngại vật trên đường đi. Tùy theo công việc và môi trường làm việc của robot mà quyết định số lượng các điểm tựa. Khi biết vị trí và định hướng của khâu tác động cuối theo các phương pháp giải bài toán động học ngược, xác định được các giá trị biến khớp (tọa độ suy rộng) để điều khiển chuyển đông của từng khớp động và tổng hợp lại thành chuyển động chung của robot theo một quỹ đạo nhất định. Hay nói chính xác hơn là theo các điểm tựa của quỹ đạo. Ở phần này, khảo sát sự chuyển động giữa các điểm tựa đó là để chọn ra những quy luật chuyển động hợp lý. Các ràng buộc ở đây có thể là gia tốc và vận tốc chuyển dịch, còn hạn chế do chướng ngại vật trên đường đi đã được định hướng bằng vị trí các điểm tựa bổ sung. Ở phần cuối sẽ xem xét đến các hạn chế về động lực học khi chọn lựa quy luật chuyển động của robot. Có 2 cách tiếp cận vấn đề về chọn lựa quy luật chuyển động hợp lý giữa các điểm tựa: Cách thứ nhất thường tiến hành trong hệ tọa độ suy rộng, tức là trong không gian các biến khớp qi (t). Khi đó, nếu cho trước các điều kiện như đảm bảo độ liên tục và điều hòa ở những điểm chuyển tiếp, có thể xác định hệ số có hàm đa thức biểu thị gần đúng đường cong quỹ đạo. Cách thứ hai thường dùng trong hệ tọa độ Đề các. Lúc đó cho trước dạng hàm giải tích, ví dụ, hàm bậc nhất biểu thị quỹ đạo gồm nhiều đoạn thẳng nối tiếp nhau. Sau đó, thay thế gần đúng quỹ đạo này bằng quan hệ hàm tương ứng của biến khớp. Như vậy lập trình quỹ đạo qua các điểm tựa có thể tiến hành trong hệ tọa độ các biến khớp hoặc trong hệ tọa độ Đề các. Khi lập trình trong hệ tọa độ các biến khớp, cần biết quan hệ qi (t) và các đạo hàm bậc nhất và bậc hai của chúng. Lập trình quỹ đạo trong hệ tọa độ các biến khớp có các ưu điểm sau: 1) Cho biết thông tin về biến khớp là những thông số điều khiển trực tiếp các khớp động. 2) Vì vậy có nhiều khả năng đảm bảo thời gian thực trong điều khiển. 3) Việc lập trình dễ dàng hơn. Nhược điểm chính là thông qua giá trị của các biến khớp chưa thể hình dung ra ngay vị trí của cơ cấu tay máy trong không gian, mà vì vậy phải tính toán nhiều lần, ví dụ, để kiểm nghiệm xem có bị chạm vào các vật thể xung quanh không. Thường dùng hơn là cách thức sau: từ tọa độ các điểm tựa giải bài toán động học ngược để tìm giá trị các biến khớp, rồi dùng phép nội suy xấp xỉ bằng hàm đa thức bậc thấp nhất để xây dựng quy luật thay đổi thích hợp các giá trị biến khớp, tương ứng với quỹ đạo các điểm tựa bắt buộc. Có nhiều giải pháp giải quyết vấn đề này, ví dụ, mô tả quỹ đạo gồm nhiều đoạn thẳng nối tiếp hoặc dùng Spline bậc 3 . . . 5.2. Mô tả quỹ đạo bằng Spline bậc 3. Spline bậc 3 là phép nội suy hàm đa thức bậc 3 để đảm bảo sự liên tục của đạo hàm bậc nhất và bậc 2 tại các điểm tựa. Phương pháp này cho phép xây dựng quy luật xấp xỉ với độ biến đổi điều hòa và đọ chính xác cần thiết. Xét dưới đây trường hợp quỹ đạo bắt đầu từ điểm xuất phát và đi qua 5 điểm tựa. Khi dùng Spline bậc 3, mỗi quỹ đạo được mô tả bằng một đa thức bậc 3: hj (t) = aj3t3 + aj2t2 + aj1t + aj0 (5.63) Với t - thời gian quy đổi: t = ; t Î [t j = 1, tj] , t Î [0,1] t - thời gian thực, tính bằng giây; tj - thời gian (thực) ứng với điểm cuối của đoạn quỹ đạo thứ j tj-1 - thời gian (thực) ứng với điểm đầu của đoạn quỹ đạo thứ j; Aịj - hệ số thứ i của đa thức ứng với đoạn quỹ đạo thứ j, j = 1, 2, 3, 4,. . .n (n là doạn cuối) Trên hình 5-19 trình bày các đoạn quỹ đạo mô tả bằng Spline bậc 3. Điều kiện biên của các đoạn quỹ đạo cũng ghi trên hình vẽ, bao gồm giá trị qj ở các điểm tựa, vận tốc vj và gia tốc aj Đạo hàm bậc nhất và bậc 2 của hàm đa thức hj (t) theo thời gian thực được tính như sau: v (t) = = . = . = . = .hj (t) , i = 1. . n a (t) = . = . = .hj (t) , i = 1,2 . . . n Hình 5-19: Mô tả quỹ đạo bằng Spline bậc 3 Các hệ số aij được xác định từ các điều kiện biên này Đoạn quỹ đạo thứ nhất mô tả bằng đa thức sau: hj (t) = a13t3 + a12t2 + a11t + a10 ở vị trí xuất phát t = 0 và q0 biết trước, ta có: h1 (0) = a10 = q0 v0 = = Từ đó, ta có: a11 = v0 t1 a12 = Và do đó ta có: a0 = = (5.64) Ở vị trí t = 1 ta có: h1 = a13 + + v0t1 + q0 = q1 (5.65) Từ đó, ta có: a13 = d1 - v0t1 - (5.66) Ở đây cũng như về sau dùng ký hiệu d1 = q1 – q1 +1 Như vậy, đoạn quỹ đạo đầu tiên được biểu thị bằng phương trình sau: h1(t) = (d1 - v0t1 - )t3 + ()t2 + (v0t1)t + q0 (5.67) Từ đó, xác định vận tốc và gia tốc tại điểm cuối của đoạn quỹ đạo thứ nhất: h1(t) = v1 = 3d1 - = - 2v0 - (5.68) h1(t) = a1 = = - - 2a0 (5.69) Đây cũng là vận tốc và gia tốc tại điểm đầu của đoạn quỹ đạo thứ hai Đoạn quỹ đạo cuối được mô tả bằng đa thức sau: hn (t) = an3t3 + an2t2 + an1t + an0 (5.70) Theo các điều kiện ở điểm đầu (t = 0) và điểm cuối (t = 1) của đoạn quỹ đạo, xác định: hn (0) = ano = qn cho biết trước (5.71) h1 (t) = an3 + an2 + an1 + an0 = q1 (5.72) = vf = (5.72) = af = (5.74) Giải các phương trình này để tìm các hệ số an3, an2, an1 cuối cùng ta có: h1(t) = (dn - vftn + )t3 + (-3dn + 3vftn - af)t2 + (3dn + 3vftn + )+ qn (5.75) Với dn = qf – qn Đoạn quỹ đạo thứ hai được mô tả bằng đa thức sau: h2 (t) = a23t3 + a22t2 + a21t + a20 (5.76) Theo các điều kiện biên ở điểm đầu của quỹ đạo này (t = 0) ta có: h2 (0) = a20 = q1 (5.77) a1 = = = (5.78) Do vậy a21 = v1t2 a1 = = = (5.79) Từ đó: a22 = Với các hệ số này đa thức h2 (t) có dạng sau: h2 (t) = a23t3 + ()t2 + (v1t2)t + q1 (5.80) Với v1 = - 2v0 - (5.81) a1 = - - 2a (5.82) Thay vào (5.80) các điều kiện t = 1 ứng với điểm cuối của quỹ đạo thứ hai, cũng tức là điểm đầu của đoạn quỹ đạo tiếp theo: h2 (1) = q2 = a23 + + v1t2 + q1 (5.83) = v2 = = v1 +a1t2 + (5.84) = a2 = = a1 + (5.85) Các vị trí q2 v2 và a2 đều phụ thuộc vào a23 Đoạn quỹ đạo thứ ba được mô tả bằng đa thức sau: h3 (t) = a33t3 + a32t2 + a31t + a30 (5.86) Ở điểm đầu t = 0, ta có: h0 (0) = a30 = q2 = a23 + + v1t2 + q1 (5.87) vf = = = (5.88) Từ đó, a31 = v2t3 Tiếp theo a2 = = = (5.89) Từ đó ta có: a32 = Thay các hệ số vừa xác định được vào (5.87) ta có h3 (t) = a32t3 + ()t2 + (v2t3)t + q2 (5.90) Theo điều kiện biên khi t = 1 ứng với điểm cuối của đoạn quỹ đạo thứ ba, cũng tức là điểm đầu của đoạn quỹ đạo tiếp theo, ta có: h3 (1) = q3 = q2 = a33 + + v2t3 (5.90) = v3 = = v2 +a2t3 + (5.91) = a3 = = a2 + (5.92) Các vị trí q3 v3 và a3 đều phụ thuộc vào a23 Đoạn quỹ đạo thứ ba được mô tả bằng đa thức sau: h4 (t) =a43t3 + a42t2 + a41t + a40 (5.93) v4 = = = (5.94) h4 (0) = a40 = q3 = q2 = a33 + + v2t3 (5.95) Ứng với các điều kiện biên tại điểm đầu, t = 0 ta có: a41 = v3t4 Tiếp theo a3 = = = (5.96) Từ đó ta có: a42 = Thay các hệ số vừa xác định được vào (5.95) ta có h4 (t) = a42t3 + ()t2 + (v3t4)t + q3 (5.97) Theo điều kiện biên tại điểm cuối t = 1, ta có h4 (t) = a43 + + v3t4 + q3 = q4 (5.98) = + a3t4 + vf = v4 = + vf - (5.99) = + a3 = a4 = + - 2af (5.100) Từ phương trình này có thể xác định các hệ số a23, a33, a43 ở phần trên còn là ẩn số Cuối cùng có phương trình các đoạn quỹ đạo như sau: h1 (t) = (d1 - v0t1 - )t3 + ()t2 + (v0t1)t + q0 (5.101) v1 = - 2v0 - ; a1 = - - 2a0 (5.102) h2 (t) = a23t3 + ()t2 + (v1t2)t + q1 (5.103) q2 = a23 + + v1t2 + q1 (5.104) v2 = v1 + a1t1 + ; a2 = a1 + (5.105) h3 (t) = a33t3 + ()t2 + (v2t3)t + q2 (5.106) q3 = a33 + + v2t3 + q2 (5.107) v3 = v1 + a2t3 + ; a3 = a2 + (5.108) h4 (t) = a43t3 + ()t2 + (v3t4)t + q3 (5.109) hn (t) = (dn – vftn - )t3 + (- 3dn + vftn - )t2 + (3dn – 2vftn + )t+ q4 (5.110) v4 = - 2vf + ; a4 = - - 2a0f (5.111) a23 = ; a33 = ; a43 = (5.112) Trong đó: x1 = k1(u – t2) + k2( - d) – k3[(u – t4)d + (t4 – t2)] (5.113) x2 = - k1(u – t3) + k2(c - ) + k3[(u – t4)c + (t4 – t2)] (5.114) x1 = k1(u – t4) + k2(d - c) + k3[(t4 – t4)c - d(u – t2)] (5.115) D = u(u – t2)(u – t4) (5.116) u = t4 + t3 + t2 (5.117) k1 = q4 – q1 v1u – a1 (5.118) k2 = (5.119) k3 = (5.120) c = 3u2 – 3ut2 + (5.121) d = 3 + 3t3t4 + (5.122) 5.3. Lập trình quỹ đạo trong hệ tọa độ Đề các. Trong phần trước đã xây dựng quỹ đạo trong không gian các biến khớp (tọa độ suy rộng) và nội suy bằng đa thức bậc thấp. Mặc dù theo giá trị các biến khớp có thể hoàn toàn xác định vị trí và định hướng của bàn kẹp trong không gian tọa độ Đề các. Tuy nhiên, để dễ hình dung trực quan hơn, người lập trình cần trực tiếp tến hành lập trình quỹ đạo trong hệ tọa độ Đề các. Nhiệm vụ lập trình là xây dựng quỹ đạo chuyển động của bàn kẹp, (mặc dầu nói là bàn kẹp nhưng ở đây hiểu chính xác hơn là của điểm tác động cuối, khi dụng cụ đã gắn vào tay máy hoặc bàn kẹp) đảm bảo vị trí trạng thái (vị trí định hướng) của nó tại các điểm tựa và nối liền các điểm tựa này với nhau. Như đã biết tại từng điểm tựa có thể dùng ma trận thuần nhất 4x4 để mô tả quan ệ giữa tọa độ gắn liền với bàn kẹp và hệ tọa độ cơ bản. Giá trị các biến khớp tại các điểm tựa này được xác định theo các phương pháp giải baìo toán động học ngược. Như vậy ma trận 4x4 nói trên cũng hoàn toàn xác định. Từ điểm tựa này sang điểm tựa kia có thể thực hiện bằng cách di chuyển theo đường thẳng nối hai điểm và theo các chuyển động quay để đảm bảo định hướng tới của bàn kẹp. Trong trường hợp này chúng ta viết: T6 = Cj(t)Pij (5.123) Với T6 – ma trận thuần nhất 4x4, mô tả định vị và định hướng của bàn kẹp trong hệ tọa độ cơ bản. Pij - ma trận thuần nhất 4x4, mô tả định vị và định hướng của vật thể đối tác bị kẹp đang ở vị trí thứ i trong hệ tọa độ tham chiếu thứ j của vật thể đối tác. Cj (t) - ma trận thuần nhất 4x4, mô tả quan hệ giữa hệ tọa độ tham chiếu của vật thể đối tác so với hệ tọa độ cơ bản. Quan hệ này có thể thay đổi theo thời gian t, hoặc không đổi, thậm chí bằng ma trận đơn vị, nếu hệ tọa độ tham chiếu này trùng với hệ tọa độ cơ bản. Nếu chọn gốc hệ tọa độ tham chiếu của vật thể đối tác trùng với điểm thứ (i + 1) khi xét chuyển động của điểm thứ i thì ta có: T6 = Ci, i +1(t)Pi, i +1 (5.124) Việc mô tả di chuyển từ điểm thứ i tới điểm thứ (i + 1) có thể mô tẩ bằng ma trận D(l), bổ sung vào (5.124), ta có: T6(l) = Cj(l)Pi, i +1D(l) (5.125) Với l = , l Î [0, 1] t - thời gian thực tính từ đểm đầu của đoạn quỹ đạo đang xét; T - tổng thời gian di chuyển trên đoạn quỹ đạo này; Vậy l là thời gian quy đổi không thứ nguyên. Ở điểm đầu của đoạn quỹ đạo, điểm i: t = 0; l = 0; D(0) bằng ma trận đơn vị và ở điểm cuối doạn quỹ đạo điểm i + 1, thì t = T; l = 1 và Pi +1,i + 1 = Pi,i + 1D(l) (5.126) Từ đó ta có: D(1) = (Pi,i + 1)- 1Pi +1,i + 1 (5.127) Như đã biết, có thể mô tả định vị và định hướng của bàn kẹp (khi đã kẹp vật thể đối tác) tại vị trí điểm tựa i và (i + 1) bằng các ma trận A và B sau đây: = Pi,i+1 = A = (5.128) nA sA aA pA 0 0 0 1 0 0 0 1 = Pi+1,i+1 = A = (5.129) nB sB aB pB 0 0 0 1 0 0 0 1 Trên cơ sở (5.127), (5.128) và (5.129) ta có: nA.nB nA.sB nA.aB nA.(pB – pA) D(1) = (5.130) sA.nB sA.sB sA.aB sA.(pB – pA) aA.nB aA.sB nA.a aA.(pB – pA) 0 0 0 1 Dấu (.) giữa hai thừa số (ví dụ, nA.nB) là ký hiệu phép nhân vô hướng véc tơ. Ma trận D(l) có thể biểu thị bằng một quan hệ hàm sau: D(l) = L(l) RA(l) RB(l) (5.131) 1 0 0 lx (5.132) Với L(l) = 0 1 1 ly 0 0 1 lz 0 0 0 1 RA(l) = (5.133) RB = (5.134) Trong ®ã: V(lq) = 1 – cos (lq) C(lq) = cos(lq), S(lq) = sin(lq) C(lj) = cos(lj), S(lj) = sin(lj) vµ l y [0,1] Ma trËn RA m« t¶ sù quay ë vÞ trÝ t¹i ®iÓm i, gãc q quay vect¬ h­íng tíi aA. Ma trËn RB (l) m« t¶ sù quay ë vÞ trÝ t¹i diÓm (i +), gãc quay j quanh vec t¬ h­íng tíi aB. Thay (5.132), (5.133) vµ (5.134), ta cã: D(l) = (5.135) Víi ds = da = ; dp = ; dn = ds x da So s¸nh c¸c ph©n tö t­¬ng øng cña 2 ma trËn (4.138) vµ (4.143) khi l = 1, ta cã: (5.136) øng víi c¸c phÇn thø 3 ta cã: -p £ y £ p (5.137) (0 £ q £ p (5.138) Cuèi cïng t×m biÓu thøc tÝnh j: (4.147) (4.148) (5.139) D(l) ®­îc gäi lµ hµm dÉn. Tuú theo c¸c yªu cÇu ®Þnh h­íng bµn kÑp mµ x¸c ®Þnh D(l), cßn nhiÒu vÞ trÝ t©m bµn kÑp ®­îc di chuyÓn trªn ®­êng th¼ng nèi 2 ®iÓm tùa. NhiÒu khi l¹i cßn ph¶i dïng ®o¹n cong chuyÓn tiÕp gi÷a 2 ®o¹n quü ®¹o ®­êng th¼ng ®Ó tr¸nh sù thay ®æi ®ét ngét vÒ tèc ®é di chuyÓn. Trªn ®©y ®· tr×nh bµy tr­êng hîp x©y dung quü ®¹o gi÷a 2 ®iÓm tùa i vµ i + 1. Theo ®ã, vÞ trÝ t©m bµn kÑp ®­îc di chuyÓn däc theo ®­êng nèi 2 t©m ®iÓm tùa. Trong khi ®ã b¶n th©n kÑp thùc hiÖn c¸c chuyÓn ®éng quay (t¹i vÞ trÝ quay phøc hîp víi gãc q vµ y, cßn t¹i vÞ trÝ (i + 1) gãc quay j. §o¹n cong chuyÓn tiÕp t¹i ®iÓm giao nhau cña 2 quü ®¹o th¼ng, ®­îc x©y dung nh­ phÐp néi suy bËc 2. Trong thùc tÕ cßn cã thÓ thùc hiÖn quü ®¹o qua c¸c ®iÓm tùa b»ng nhiÒu ph­¬ng ph¸p kh¸c nhau vµ ph¶i xÐt tíi c¸c ®Æc ®iÓm c«ng nghÖ. Tuú theo lo¹i h×nh c«ng nghÖ ®­îc robot phôc vô mµ x¸c ®Þnh c¸c ®Æc tÝnh ®éng häc, ®éng lùc häc t¹i mét sè ®iÓm tùa quan träng ®Ó ®­a thªm c¸c ®iÒu kiÖn rµng buéc khi x©y dung quü ®¹o ®i qua c¸c ®iÓm tùa nµy. Trªn h×nh 4.20 lµ s¬ ®å thao t¸c cña mét robot cã nhiÖm vô lÊy c¸c chiÕc ®inh èc trong æ chøa ®Ó l¾p vµo c¸c lç vµ ®i qua c¸c ®iÓm tùa theo quü ®¹o lµ c¸c ®o¹n th¼ng. H×nh 5 – 20: S¬ ®å thao t¸c cña robot qua c¸c ®iÓm tùa Theo nhiÖm vô ®Æt ra ®ã ta thiÕt lËp c¸c ph­¬ng tr×nh ma trËn t¹i c¸c vÞ trÝ ®iÓm tùa kh¸c nhau: ë ®iÓm p0: [BASE] [T6] [E] = [p0] (5.140) ë ®iÓm p1: [BASE] [T6] [E] = [B0][p1] (5.141) ë ®iÓm p2: [BASE] [T6] [E] = [B0] [p2] (5.142) ë ®iÓm p3: [BASE] [T6] [E] = [B0] [p3] (5.143) ë ®iÓm p4: [BASE] [T6] [E] = [BR] [p4] (5.144) ë ®iÓm p5: [BASE] [T6] [E] = [BR] [p5] (5.145) Trong c¸c ph­¬ng tr×nh trªn ®· dïng c¸c ký hiÖu sau biÓu thÞ c¸c ma trËn thuÇn nhÊt 4 x 4 : [BASE]- ma trËn quan hÖ gi÷a hÖ to¹ ®é c¬ b¶n x,y,z g¾n liÒn víi robot vµ hÖ to¹ ®é cè ®Þnh x0, y0, z0. [B0]- ma trËn quan hÖ gi÷a hÖ to¹ ®é g¾n liÒn víi vÞ trÝ cña æ chøa ®inh èc, so víi hÖ x0, y0, z0. [BR]- ma trËn quan hÖ gi÷a c¸c to¹ ®é g¾n liÒn víi c¸c vËt thÓ cã c¸c lç ®inh èc, so víi hÖ x0, y0, z0. [P0]- ma trËn quan hÖ gi÷a c¸c to¹ ®é g¾n liÒn víi vÞ trÝ xuÊt ph¸t, so víi hÖ x0, y0, z0. [P1, P2, P3]- c¸c ma trËn quan hÖ gi÷a c¸c to¹ ®é g¾n liÒn víi c¸c ®iÓm tùa P1, P2, P3, so víi hÖ to¹ ®é liªn quan víi [B0]. [T6]- ma trËn quan hÖ gi÷a to¹ ®é g¾n liÒn víi ®iÓm cuèi cña tay m¸y vµ hÖ to¹ ®é c¬ b¶n x, y, z [E]- ma trËn quan hÖ gi÷a to¹ ®é g¾n liÒn víi bµn kÑp so víi hÖ to¹ ®é [T6] §Ó biÓu thÞ sù dÞch chuyÓn tõ ®iÓm Pi ®Õn Pi+1 (i = 0, 1, 2, 3, 4) dïng ma trËn Pi, i+1. VÝ dô, khi m« t¶ sù dÞch chuyÓn tõ P0 ®Õn P1 ta viÕt l¹i (5.140) trong hÖ to¹ ®é P0 : [T6] = [BASE]-1[P00][E]-1 (5.146) Vµ trong hÖ to¹ ®é P1, ta cã: [T6] = [BASE]-1[B0][P01][E]-1 (5.147) Tõ (4.146) vµ (5.147) ta cã : [P01] = [B0]-1[P00] (5.148) Nh­ vËy, viÖc chuyÓn tõ vÞ trÝ ®iÓm tùa P0 ®Õn vÞ trÝ ®iÓm tùa P1 däc theo ®o¹n th¼ng nèi chóng víi nhau, cÊu h×nh cña tay m¸y thay ®æi tõ [T6] = [BASE]-1[B0][P01][E]-1 (5.149) Sang cÊu h×nh sau : [T6] = [BASE]-1[B0][P11][E]-1 (5.150) 5.4 M« t¶ quü ®¹o qua c¸c ®iÓm tùa b»ng ®a thøc bËc 3 cã xÐt ®Õn c¸c h¹n chÕ vÒ ®éng lùc häc Ký hiÖu Qij(t) lµ ®a thøc bËc 3 biÓu diÔn sù biÕn thiªn cña biÕn khíp (to¹ ®é suy réng) thø j gi÷a 2 ®iÓm tùa Bi vµ Bi+1 trong kho¶ng thêi gian [ti, ti+1]. VÊn ®Ò lµ ‘‘nèi ghÐp’’ c¸c ®a thøc Qij nµy l¹i (i = 1, 2, …n – 1) sao cho quü ®¹o ph¶i ®i qua c¸c ®iÓm tùa vµ ®¶m b¶o tÝnh liªn tôc vÒ hµnh tr×nh, vËn tèc, gia tèc trong suèt kho¶ng thêi gian [t1, tn] V× Qij(t) lµ ®a thøc bËc 3 nªn quü ®¹o hµm bËc 2 cña chóng lµ hµm bËc nhÊt theo thêi gian t : (5.151) Víi i = 1, 2, …n – 1; J = 1, 2, …N Ui – Thêi gian di chuyÓn trªn ®o¹n ®­êng thø i, ui = ti+1 - ti LÊy tÝch ph©n 2 lÇn víi c¸c ®iÒu kiÖn biªn: Qij (ti) = qij; Qij (ti+1) = qj,i+1; Ta cã (5.152) I = 1, 2, …n – 1; j = 1, 2, …N Nh­ vËy ®èi víi c¸c ®iÓm i = 1, 2, …n-1 th× Qij(t) lµ x¸c ®Þnh nÕu biÕt c¸c ®¹o hµm bËc 2 cña chóng t¹i c¸c thêi ®iÓm ti vµ ti+1. Trªn c¬ së ®ã ta cã (n – 2) ph­¬ng tr×nh tuyÕn tÝnh víi Èn sè Q3(ti), i = 2…n – 1: AQ = b (5.153) NÕu ui lu«n lu«n d­¬ng, tõ ph­¬ng tr×nh (5.154) ta cã: Q = A-1b (5.155) Vµ sau khi thay vµo (5.152) sÏ ®­îc biÓu thøc x¸c ®Þnh Qji(t). Nh­ vËy Qji(t) phô thuéc vµo kho¶ng thêi gian ui, vµo c¸c gi¸ trÞ cña biÕn khíp, cña vËn tèc vµ gia tèc c¸c ®iÓm tùa. §Ó t¨ng n¨ng suÊt thao t¸c tay, m¸y ph¶i thùc hiÖn quü ®¹o chuyÓn dÞch víi thêi gian ng¾n nhÊt. Cã thÓ ®Æt bµi to¸n tèi ­u chän kho¶ng thêi gian ui sao cho tæng thêi gian T lµ min trong ph¹m vi giíi h¹n vÒ vËn tèc, gia tèc vµ lùc suy réng Hµm môc tiªu: T = Sui ® min Giíi h¹n vÒ vËn tèc j = 1, 2,…N I = 1, 2,…,n – 1 Giíi h¹n vÒ vËn tèc j = 1, 2,…N I = 1, 2,…,n – 1 Giíi h¹n vÒ lùc : [FMj] £Fj j = 1, 2,…, N Trong ®ã Vj; Aj vµ Fj lµ c¸c gi¸ trÞ giíi h¹n vÒ vËn tèc, gia tèc vµ lùc suy réng ®èi víi khíp ®éng thø j. C¸c ®iÒu kiÖn vÒ giíi h¹n nãi trªn cã thÓ viÕt chi tiÕt h¬n nh­ sau: Giíi h¹n vÒ vËn tèc LÊy vi ph©n biÓu thøc (5.152) vµ ®Æt: Qji(ti) = wji (5.156) Q(ti+1) = wji+1 (5.157) Ta cã Qji(ti) = (5.158) (5.159) Trong ®ã wji – gia tèc gãc tÝnh b»ng (5.156) t¹i ®iÓm tùa Bi ëthêi gian Ti. Kho¶ng thêigian [ti, ti+1) vËn tèc cã thÓ ®¹t cùc ®¹i thêi ®iÓm ti nµo ®ã. T¹i thêi ®iÓm nµy ta cã Qji () = 0 (5.160) Nh­ vËy |Qji| = max (5.161) i = 1,2, ... , n = 1; j = 1,2,... N Trong ®ã; Giíi h¹n vÒgia tèc: víi quü ®¹o hµnh t×nh ®· biÓu thÞ b»ng c¸c ®a thøc bË 3 th× quan hÖ gia tèc theo thêi gian lµ hµm bËc nhÊt, cho nªn trong kho¶ng thêi gian [ti, ti+1] gia tèc lín nhÊt ®¹t hoÆc ë thêi ®iÓm ti hoÆc ti+1 vµ b»ng gi¸ trÞ lín nhÊt trong 2 gi¸ trÞ |wji| vµ |wji+1| VËy ®iÒu kiÖn giíi h¹n vÒ gia tèc cã d¹ng sau: (5.162) J = 1,2,3... n