Robot cong nghiep

110
"Don't study, don't know - Studying you will know!" NGUYEN TRUNG HOA

Transcript of Robot cong nghiep

Page 1: Robot cong nghiep

"Don't study, don't know - Studying you will know!"

NGUYEN TRUNG HOA

Page 2: Robot cong nghiep

Robot C«ng nghiÖp 1

Ch−¬ng I

GiĂ­i thiÖu chung vÒ robot c«ng nghiÖp 1.1. SÂŹ l−üt quž tr×nh phžt triÓn cña robot c«ng nghiÖp (IR : Industrial Robot) :

ThuËt ngĂ· “Robot” xuÊt phžt tĂ” tiÕng Sec (Czech) “Robota” cĂŁ nghÜa l” c«ng viÖc tÂčp dÞch trong vĂ« kÞch Rossum’s Universal Robots cña Karel Capek, v”o nšm 1921. Trong vĂ« kÞch nÇy, Rossum v” con trai cña «ng ta Ÿ· chÕ tÂčo ra nhĂ·ng chiÕc mžy gÇn giĂšng vĂ­i con ng−ĂȘi ¼Ó phĂŽc vĂŽ con ng−ĂȘi. CĂŁ lÏ Ÿã l” mĂ©t gĂźi Ăœ ban ¼Çu cho cžc nh” sžng chÕ kĂŒ thuËt vÒ nhĂ·ng cÂŹ cÊu, mžy mĂŁc bŸt ch−íc cžc hoÂčt Ÿéng cÂŹ bŸp cña con ng−ĂȘi. §Çu thËp kĂ» 60, c«ng ty MĂŒ AMF (American Machine and Foundry Company) qu¶ng cžo mĂ©t loÂči mžy tĂč Ÿéng vÂčn nšng v” gĂ€i l” “Ng−ĂȘi mžy c«ng nghiÖp” (Industrial Robot). Ng”y nay ng−ĂȘi ta ¼Æt tÂȘn ng−ĂȘi mžy c«ng nghiÖp (hay robot c«ng nghiÖp) cho nhĂ·ng loÂči thiÕt bÞ cĂŁ džng dÊp v” mĂ©t v”i chĂžc nšng nh− tay ng−ĂȘi ¼−üc ÂźiÒu khiÓn tĂč Ÿéng ¼Ó thĂčc hiÖn mĂ©t sĂš thao tžc s¶n xuÊt.

VÒ mÆt kĂŒ thuËt, nhĂ·ng robot c«ng nghiÖp ng”y nay, cĂŁ nguĂ„n gĂšc tĂ” hai lÜnh vĂčc kĂŒ thuËt ra ÂźĂȘi sĂ­m hÂŹn Ÿã l” cžc cÂŹ cÊu ÂźiÒu khiÓn tĂ” xa (Teleoperators) v” cžc mžy c«ng cĂŽ ÂźiÒu khiÓn sĂš (NC - Numerically Controlled machine tool).

Cžc cÂŹ cÊu ÂźiÒu khiÓn tĂ” xa (hay cžc thiÕt bÞ kiÓu chñ-tĂ­) Ÿ· phžt triÓn mÂčnh trong chiÕn tranh thÕ giĂ­i lÇn thĂž hai nh»m nghiÂȘn cĂžu cžc vËt liÖu phĂŁng xÂč. Ng−ĂȘi thao tžc ¼−üc tžch biÖt khĂĄi khu vĂčc phĂŁng xÂč bĂ«i mĂ©t bĂžc t−ĂȘng cĂŁ mĂ©t hoÆc v”i cöa quan sžt ¼Ó cĂŁ thÓ nh×n thÊy ¼−üc c«ng viÖc bÂȘn trong. Cžc cÂŹ cÊu ÂźiÒu khiÓn tĂ” xa thay thÕ cho cžnh tay cña ng−ĂȘi thao tžc; nĂŁ gĂ„m cĂŁ mĂ©t bĂ© kÑp Ă« bÂȘn trong (tĂ­) v” hai tay cÇm Ă« bÂȘn ngo”i (chñ). C¶ hai, tay cÇm v” bĂ© kÑp, ¼−üc nĂši vĂ­i nhau b»ng mĂ©t cÂŹ cÊu sžu bËc tĂč do ¼Ó tÂčo ra cžc vÞ trÝ v” h−íng tuĂș Ăœ cña tay cÇm v” bĂ© kÑp. CÂŹ cÊu dĂŻng ¼Ó ÂźiÒu khiÓn bĂ© kÑp theo chuyÓn Ÿéng cña tay cÇm.

V”o kho¶ng nšm 1949, cžc mžy c«ng cĂŽ ÂźiÒu khiÓn sĂš ra ÂźĂȘi, nh»m Ÿžp Ăžng yÂȘu cÇu gia c«ng cžc chi tiÕt trong ng”nh chÕ tÂčo mžy bay. NhĂ·ng robot ¼Çu tiÂȘn thĂčc chÊt l” sĂč nĂši kÕt giĂ·a cžc kh©u cÂŹ khÝ cña cÂŹ cÊu ÂźiÒu khiÓn tĂ” xa vĂ­i kh¶ nšng lËp tr×nh cña mžy c«ng cĂŽ ÂźiÒu khiÓn sĂš. D−íi Ÿ©y chĂłng ta sÏ ÂźiÓm qua mĂ©t sĂš thĂȘi ÂźiÓm lÞch sö phžt triÓn cña ng−ĂȘi mžy c«ng nghiÖp. MĂ©t trong nhĂ·ng robot c«ng nghiÖp ¼Çu tiÂȘn ¼−üc chÕ tÂčo l” robot Versatran cña c«ng ty AMF, MĂŒ. CĂČng v”o kho¶ng thĂȘi gian nÇy Ă« MĂŒ xuÊt hiÖn loÂči robot Unimate -1900 ¼−üc dĂŻng ¼Çu tiÂȘn trong kĂŒ nghÖ «t«. TiÕp theo MĂŒ, cžc n−íc khžc bŸt ¼Çu s¶n xuÊt robot c«ng nghiÖp : Anh -1967, ThuĂŸ §iÓn v” NhËt -1968 theo b¶n quyÒn cña MĂŒ; CHLB §Þc -1971; Phžp - 1972; Ă« Ăœ - 1973. . . TÝnh nšng l”m viÖc cña robot ng”y c”ng ¼−üc n©ng cao, nhÊt l” kh¶ nšng nhËn biÕt v” xö lĂœ. Nšm 1967 Ă« tr−ĂȘng §Âči hĂ€c tĂŠng hĂźp Stanford (MĂŒ) Ÿ· chÕ tÂčo ra mÉu robot hoÂčt Ÿéng theo m« h×nh “mŸt-tay”, cĂŁ kh¶ nšng nhËn biÕt v” ¼Þnh h−íng b”n kÑp theo vÞ trÝ vËt kÑp nhĂȘ cžc c¶m biÕn. Nšm 1974 C«ng ty MĂŒ Cincinnati ¼−a ra loÂči robot ¼−üc ÂźiÒu khiÓn b»ng mžy vi tÝnh, gĂ€i l” robot T3 (The Tomorrow Tool : C«ng cĂŽ cña t−¬ng lai). Robot nÇy cĂŁ thÓ n©ng ¼−üc vËt cĂŁ khĂši l−üng ¼Õn 40 KG.

CĂŁ thÓ nĂŁi, Robot l” sĂč tĂŠ hĂźp kh¶ nšng hoÂčt Ÿéng linh hoÂčt cña cžc cÂŹ cÊu ÂźiÒu khiÓn tĂ” xa vĂ­i mĂžc Ÿé “tri thĂžc” ng”y c”ng phong phĂł cña hÖ thĂšng ÂźiÒu khiÓn theo ch−¬ng tr×nh sĂš cĂČng nh− kĂŒ thuËt chÕ tÂčo cžc bĂ© c¶m biÕn, c«ng nghÖ lËp tr×nh v” cžc phžt triÓn cña trÝ kh«n nh©n tÂčo, hÖ chuyÂȘn gia ... Trong nhĂ·ng nšm sau nÇy, viÖc n©ng cao tÝnh nšng hoÂčt Ÿéng cña robot kh«ng ngĂ”ng phžt triÓn. Cžc robot ¼−üc trang bÞ thÂȘm cžc loÂči c¶m biÕn khžc nhau ¼Ó nhËn biÕt m«i tr−ĂȘng

TS. PhÂčm §šng Ph−íc

Page 3: Robot cong nghiep

Robot C«ng nghiÖp 2

chung quanh, cĂŻng vĂ­i nhĂ·ng th”nh tĂču to lĂ­n trong lÜnh vĂčc Tin hĂ€c - §iÖn tö Ÿ· tÂčo ra cžc thÕ hÖ robot vĂ­i nhiÒu tÝnh nšng Ÿšc biÖt, SĂš l−üng robot ng”y c”ng gia tšng, giž th”nh ng”y c”ng gi¶m. NhĂȘ vËy, robot c«ng nghiÖp Ÿ· cĂŁ vÞ trÝ quan trĂ€ng trong cžc d©y chuyÒn s¶n xuÊt hiÖn ÂźÂči. MĂ©t v”i sĂš liÖu vÒ sĂš l−üng robot ¼−üc s¶n xuÊt Ă« mĂ©t v”i n−íc c«ng nghiÖp phžt triÓn nh− sau : (B¶ng I.1)

N−íc SX Nšm 1990 Nšm 1994 Nšm 1998 (DĂč tÝnh)

NhËt 60.118 29.756 67.000 MĂŒ 4.327 7.634 11.100 §Þc 5.845 5.125 8.600 Ăœ 2.500 2.408 4.000

Phžp 1.488 1.197 2.000 Anh 510 1.086 1.500

H”n quĂšc 1.000 1.200 MĂŒ l” n−íc ¼Çu tiÂȘn phžt minh ra robot, nh−ng n−íc phžt triÓn cao nhÊt trong lÜnh vĂčc nghiÂȘn cĂžu chÕ tÂčo v” sö dĂŽng robot lÂči l” NhËt. 1.2. Ăžng dĂŽng robot c«ng nghiÖp trong s¶n xuÊt : TĂ” khi mĂ­i ra ÂźĂȘi robot c«ng nghiÖp ¼−üc žp dĂŽng trong nhiÒu lÜnh vĂčc d−íi gĂŁc Ÿé thay thÕ sĂžc ng−ĂȘi. NhĂȘ vËy cžc d©y chuyÒn s¶n xuÊt ¼−üc tĂŠ chĂžc lÂči, nšng suÊt v” hiÖu qu¶ s¶n xuÊt tšng lÂȘn rĂą rÖt.

MĂŽc tiÂȘu Ăžng dĂŽng robot c«ng nghiÖp nh»m gĂŁp phÇn n©ng cao nšng suÊt d©y chuyÒn

c«ng nghÖ, gi¶m giž th”nh, n©ng cao chÊt l−üng v” kh¶ nšng cÂčnh tranh cña s¶n phÈm ŸÄng thĂȘi c¶i thiÖn ÂźiÒu kiÖn lao Ÿéng. §Âčt ¼−üc cžc mĂŽc tiÂȘu trÂȘn l” nhĂȘ v”o nhĂ·ng kh¶ nšng to lĂ­n cña robot nh− : l”m viÖc kh«ng biÕt mÖt mĂĄi, rÊt dÔ d”ng chuyÓn nghÒ mĂ©t cžch th”nh thÂčo, chÞu ¼−üc phĂŁng xÂč v” cžc m«i tr−ĂȘng l”m viÖc Ÿéc hÂči, nhiÖt Ÿé cao, “c¶m thÊy” ¼−üc c¶ tĂ” tr−ĂȘng v” “nghe” ¼−üc c¶ siÂȘu ©m ... Robot ¼−üc dĂŻng thay thÕ con ng−ĂȘi trong cžc tr−ĂȘng hĂźp trÂȘn hoÆc thĂčc hiÖn cžc c«ng viÖc tuy kh«ng nÆng nhĂ€c nh−ng Ÿn ÂźiÖu, dÔ g©y mÖt mĂąi, nhÇm lÉn. Trong ng”nh cÂŹ khÝ, robot ¼−üc sö dĂŽng nhiÒu trong c«ng nghÖ Ÿóc, c«ng nghÖ h”n, cŸt kim loÂči, sÂŹn, phun phñ kim loÂči, thžo lŸp vËn chuyÓn ph«i, lŸp ržp s¶n phÈm . . .

Ng”y nay Ÿ· xuÊt hiÖn nhiÒu d©y chuyÒn s¶n xuÊt tĂč Ÿéng gĂ„m cžc mžy CNC vĂ­i

Robot c«ng nghiÖp, cžc d©y chuyÒn Ÿã ÂźÂčt mĂžc tĂč Ÿéng hož cao, mĂžc Ÿé linh hoÂčt cao . . . Ă« Ÿ©y cžc mžy v” robot ¼−üc ÂźiÒu khiÓn b»ng cĂŻng mĂ©t hÖ thĂšng ch−¬ng tr×nh. Ngo”i cžc ph©n xâˆ’Ă«ng, nh” mžy, kĂŒ thuËt robot cĂČng ¼−üc sö dĂŽng trong viÖc khai thžc thÒm lĂŽc ¼Þa v” ÂźÂči d−¬ng, trong y hĂ€c, sö dĂŽng trong quĂšc phßng, trong chinh phĂŽc vĂČ trĂŽ, trong c«ng nghiÖp nguyÂȘn tö, trong cžc lÜnh vĂčc x· hĂ©i . . . RĂą r”ng l” kh¶ nšng l”m viÖc cña robot trong mĂ©t sĂš ÂźiÒu kiÖn v−üt hÂŹn kh¶ nšng cña con ng−ĂȘi; do Ÿã nĂŁ l” ph−¬ng tiÖn hĂ·u hiÖu ¼Ó tĂč Ÿéng hož, n©ng cao nšng suÊt lao Ÿéng, gi¶m nhÑ cho con ng−ĂȘi nhĂ·ng c«ng viÖc nÆng nhĂ€c v” Ÿéc hÂči. Nh−üc ÂźiÓm lĂ­n nhÊt cña robot l” ch−a linh hoÂčt nh− con ng−ĂȘi, trong d©y chuyÒn tĂč Ÿéng, nÕu cĂŁ mĂ©t robot bÞ hĂĄng cĂŁ thÓ l”m ngĂ”ng hoÂčt Ÿéng cña c¶ d©y chuyÒn, cho nÂȘn robot vÉn lu«n hoÂčt Ÿéng d−íi sĂč gižm sžt cña con ng−ĂȘi.

TS. PhÂčm §šng Ph−íc

Page 4: Robot cong nghiep

Robot C«ng nghiÖp 3

1.3. Cžc khži niÖm v” ¼Þnh nghÜa vÒ robot c«ng nghiÖp : 1.3.1. §Þnh nghÜa robot c«ng nghiÖp :

HiÖn nay cĂŁ nhiÒu ¼Þnh nghÜa vÒ Robot, cĂŁ thÓ ÂźiÓm qua mĂ©t sĂš ¼Þnh nghÜa nh− sau : §Þnh nghÜa theo tiÂȘu chuÈn AFNOR (Phžp) : Robot c«ng nghiÖp l” mĂ©t cÂŹ cÊu chuyÓn Ÿéng tĂč Ÿéng cĂŁ thÓ lËp tr×nh, lÆp lÂči cžc ch−¬ng tr×nh, tĂŠng hĂźp cžc ch−¬ng tr×nh ¼Æt ra trÂȘn cžc trĂŽc toÂč Ÿé; cĂŁ kh¶ nšng ¼Þnh vÞ, ¼Þnh h−íng, di chuyÓn cžc ŸÚi t−üng vËt chÊt : chi tiÕt, dao cĂŽ, gž lŸp . . . theo nhĂ·ng h”nh tr×nh thay ŸÊi Ÿ· ch−¬ng tr×nh hož nh»m thĂčc hiÖn cžc nhiÖm vĂŽ c«ng nghÖ khžc nhau. §Þnh nghÜa theo RIA (Robot institute of America) : Robot l” mĂ©t tay mžy vÂčn nšng cĂŁ thÓ lÆp lÂči cžc ch−¬ng tr×nh ¼−üc thiÕt kÕ ¼Ó di chuyÓn vËt liÖu, chi tiÕt, dĂŽng cĂŽ hoÆc cžc thiÕt bÞ chuyÂȘn dĂŻng th«ng qua cžc ch−¬ng tr×nh chuyÓn Ÿéng cĂŁ thÓ thay ŸÊi ¼Ó ho”n th”nh cžc nhiÖm vĂŽ khžc nhau. §Þnh nghÜa theo ΓOCT 25686-85 (Nga) : Robot c«ng nghiÖp l” mĂ©t mžy tĂč Ÿéng, ¼−üc ¼Æt cĂš ¼Þnh hoÆc di Ÿéng ¼−üc, liÂȘn kÕt giĂ·a mĂ©t tay mžy v” mĂ©t hÖ thĂšng ÂźiÒu khiÓn theo ch−¬ng tr×nh, cĂŁ thÓ lËp tr×nh lÂči ¼Ó ho”n th”nh cžc chĂžc nšng vËn Ÿéng v” ÂźiÒu khiÓn trong quž tr×nh s¶n xuÊt. CĂŁ thÓ nĂŁi Robot c«ng nghiÖp l” mĂ©t mžy tĂč Ÿéng linh hoÂčt thay thÕ tĂ”ng phÇn hoÆc to”n bĂ© cžc hoÂčt Ÿéng cÂŹ bŸp v” hoÂčt Ÿéng trÝ tuÖ cña con ng−ĂȘi trong nhiÒu kh¶ nšng thÝch nghi khžc nhau. Robot c«ng nghiÖp cĂŁ kh¶ nšng ch−¬ng tr×nh hož linh hoÂčt trÂȘn nhiÒu trĂŽc chuyÓn Ÿéng, biÓu thÞ cho sĂš bËc tĂč do cña chĂłng. Robot c«ng nghiÖp ¼−üc trang bÞ nhĂ·ng b”n tay mžy hoÆc cžc cÂŹ cÊu chÊp h”nh, gi¶i quyÕt nhĂ·ng nhiÖm vĂŽ xžc ¼Þnh trong cžc quž tr×nh c«ng nghÖ : hoÆc trĂčc tiÕp tham gia thĂčc hiÖn cžc nguyÂȘn c«ng (sÂŹn, h”n, phun phñ, rĂŁt kim loÂči v”o khu«n Ÿóc, lŸp ržp mžy . . .) hoÆc phĂŽc vĂŽ cžc quž tr×nh c«ng nghÖ (thžo lŸp chi tiÕt gia c«ng, dao cĂŽ, ŸÄ gž . . .) vĂ­i nhĂ·ng thao tžc cÇm nŸm, vËn chuyÓn v” trao ŸÊi cžc ŸÚi t−üng vĂ­i cžc trÂčm c«ng nghÖ, trong mĂ©t hÖ thĂšng mžy tĂč Ÿéng linh hoÂčt, ¼−üc gĂ€i l” “HÖ thĂšng tĂč Ÿéng linh hoÂčt robot ho¾” cho phÐp thÝch Ăžng nhanh v” thao tžc Ÿn gi¶n khi nhiÖm vĂŽ s¶n xuÊt thay ŸÊi.

1.3.2. BËc tĂč do cña robot (DOF : Degrees Of Freedom) : BËc tĂč do l” sĂš kh¶ nšng chuyÓn Ÿéng cña mĂ©t cÂŹ cÊu (chuyÓn Ÿéng quay hoÆc tÞnh

tiÕn). §Ó dÞch chuyÓn ¼−üc mĂ©t vËt thÓ trong kh«ng gian, cÂŹ cÊu chÊp h”nh cña robot ph¶i ÂźÂčt ¼−üc mĂ©t sĂš bËc tĂč do. NĂŁi chung cÂŹ hÖ cña robot l” mĂ©t cÂŹ cÊu hĂ«, do Ÿã bËc tĂč do cña nĂŁ cĂŁ thÓ tÝnh theo c«ng thĂžc :

w = 6n - (1.1) ip ii =∑

1

5

Ă« Ÿ©y : n - SĂš kh©u Ÿéng; pi - SĂš khĂ­p loÂči i (i = 1,2,. . .,5 : SĂš bËc tĂč do bÞ hÂčn chÕ). §Úi vĂ­i cžc cÂŹ cÊu cĂŁ cžc kh©u ¼−üc nĂši vĂ­i nhau b»ng khĂ­p quay hoÆc tÞnh tiÕn (khĂ­p Ÿéng loÂči 5) th× sĂš bËc tĂč do b»ng vĂ­i sĂš kh©u Ÿéng . §Úi vĂ­i cÂŹ cÊu hĂ«, sĂš bËc tĂč do b»ng tĂŠng sĂš bËc tĂč do cña cžc khĂ­p Ÿéng.

§Ó ¼Þnh vÞ v” ¼Þnh h−íng kh©u chÊp h”nh cuĂši mĂ©t cžch tuĂș Ăœ trong kh«ng gian 3 chiÒu robot cÇn cĂŁ 6 bËc tĂč do, trong Ÿã 3 bËc tĂč do ¼Ó ¼Þnh vÞ v” 3 bËc tĂč do ¼Ó ¼Þnh h−íng. MĂ©t sĂš c«ng viÖc Ÿn gi¶n n©ng hÂč, sŸp xÕp... cĂŁ thÓ yÂȘu cÇu sĂš bËc tĂč do Ýt hÂŹn. Cžc robot h”n, sÂŹn... th−ĂȘng yÂȘu cÇu 6 bËc tĂč do. Trong mĂ©t sĂš tr−ĂȘng hĂźp cÇn sĂč khÐo lÐo, linh hoÂčt hoÆc khi cÇn ph¶i tĂši −u hož quĂŒ ÂźÂčo,... ng−ĂȘi ta dĂŻng robot vĂ­i sĂš bËc tĂč do lĂ­n hÂŹn 6.

1.3.3. HÖ toÂč Ÿé (Coordinate frames) : Mçi robot th−ĂȘng bao gĂ„m nhiÒu kh©u (links) liÂȘn kÕt vĂ­i nhau qua cžc khĂ­p (joints),

tÂčo th”nh mĂ©t xÝch Ÿéng hĂ€c xuÊt phžt tĂ” mĂ©t kh©u cÂŹ b¶n (base) ŸÞng yÂȘn. HÖ toÂč Ÿé gŸn vĂ­i

TS. PhÂčm §šng Ph−íc

Page 5: Robot cong nghiep

Robot C«ng nghiÖp 4

kh©u cÂŹ b¶n gĂ€i l” hÖ toÂč Ÿé cÂŹ b¶n (hay hÖ toÂč Ÿé chuÈn). Cžc hÖ toÂč Ÿé trung gian khžc gŸn vĂ­i cžc kh©u Ÿéng gĂ€i l” hÖ toÂč Ÿé suy rĂ©ng. Trong tĂ”ng thĂȘi ÂźiÓm hoÂčt Ÿéng, cžc toÂč Ÿé suy rĂ©ng xžc ¼Þnh cÊu h×nh cña robot b»ng cžc chuyÓn dÞch d”i hoÆc cžc chuyÓn dÞch gĂŁc cu¶ cžc khĂ­p tÞnh tiÕn hoÆc khĂ­p quay (h×nh 1.1). Cžc toÂč Ÿé suy rĂ©ng cßn ¼−üc gĂ€i l” biÕn khĂ­p.

d2

Ξ1

Ξ3

Ξ4Ξ5

n a o

z

yO0

On

x

H×nh 1.1 : Cžc toÂč Ÿé suy rĂ©ng cña robot. Cžc hÖ toÂč Ÿé gŸn trÂȘn cžc kh©u cña robot ph¶i tu©n theo qui tŸc b”n tay ph¶i : DĂŻng tay ph¶i, nŸm hai ngĂŁn tay Ăłt v” žp Ăłt v”o lßng b”n tay, xoÌ 3 ngĂŁn : cži, trĂĄ v” giĂ·a theo 3 ph−¬ng vu«ng gĂŁc nhau, nÕu chĂ€n ngĂŁn cži l” ph−¬ng v” chiÒu cña trĂŽc z, th× ngĂŁn trĂĄ chØ ph−¬ng, chiÒu cña trĂŽc x v” ngĂŁn giĂ·a sÏ biÓu thÞ ph−¬ng, chiÒu cña trĂŽc y (h×nh 1.2).

x

y

O

z

Trong robot ta th−ĂȘng dĂŻng chĂ· O v” chØ sĂš n ¼Ó chØ hÖ toÂč Ÿé gŸn trÂȘn kh©u thĂž n. Nh− vËy hÖ toÂč Ÿé cÂŹ b¶n (HÖ toÂč Ÿé gŸn vĂ­i kh©u cĂš ¼Þnh) sÏ ¼−üc kĂœ hiÖu l” O0; hÖ toÂč Ÿé gŸn trÂȘn cžc kh©u trung gian t−¬ng Ăžng sÏ l” O1, O2,..., On-1, HÖ toÂč Ÿé gŸn trÂȘn kh©u chÊp h”nh cuĂši kĂœ hiÖu l” On.

H×nh 1.2 : Qui tŸc b”n tay ph¶i

1.3.4. Tr−ĂȘng c«ng tžc cña robot (Workspace or Range of motion): Tr−ĂȘng c«ng tžc (hay vĂŻng l”m viÖc, kh«ng gian c«ng tžc) cña robot l” to”n bĂ© thÓ tÝch

¼−üc quÐt bĂ«i kh©u chÊp h”nh cuĂši khi robot thĂčc hiÖn tÊt c¶ cžc chuyÓn Ÿéng cĂŁ thÓ. Tr−ĂȘng c«ng tžc bÞ r”ng buĂ©c bĂ«i cžc th«ng sĂš h×nh hĂ€c cña robot cĂČng nh− cžc r”ng buĂ©c cÂŹ hĂ€c cña cžc khĂ­p; vÝ dĂŽ, mĂ©t khĂ­p quay cĂŁ chuyÓn Ÿéng nhĂĄ hÂŹn mĂ©t gĂŁc 3600. Ng−ĂȘi ta th−ĂȘng dĂŻng hai h×nh chiÕu ¼Ó m« t¶ tr−ĂȘng c«ng tžc cña mĂ©t robot (h×nh 1.3).

R

ÎČ

H

H×nh chiÕu ŸÞng H×nh chiÕu b»ng H×nh 1.3 : BiÓu diÔn tr−ĂȘng c«ng tžc cña robot.

TS. PhÂčm §šng Ph−íc

Page 6: Robot cong nghiep

Robot C«ng nghiÖp 5

1.4. CÊu trĂłc cÂŹ b¶n cña robot c«ng nghiÖp : 1.4.1. Cžc th”nh phÇn chÝnh cña robot c«ng nghiÖp : MĂ©t robot c«ng nghiÖp th−ĂȘng bao gĂ„m cžc th”nh phÇn chÝnh nh− : cžnh tay robot, nguĂ„n Ÿéng lĂčc, dĂŽng cĂŽ gŸn lÂȘn kh©u chÊp h”nh cuĂši, cžc c¶m biÕn, bĂ© ÂźiÒu khiÓn , thiÕt bÞ dÂčy hĂ€c, mžy tÝnh ... cžc phÇn mÒm lËp tr×nh cĂČng nÂȘn ¼−üc coi l” mĂ©t th”nh phÇn cña hÖ thĂšng robot. MĂši quan hÖ giĂ·a cžc th”nh phÇn trong robot nh− h×nh 1.4.

Cžc c¶m biÕn

Cžnh tay robot

DÎng cÎ thao tžc

BĂ© ÂźiÒu khiÓn v” mžy tÝnh

NguĂ„n Ÿéng lĂčc

ThiÕt bÞ dÂčy hĂ€c

C¾c ch−¬ng tr×nh

H×nh 1.4 : Cžc th”nh phÇn chÝnh cña hÖ thĂšng robot. Cžnh tay robot (tay mžy) l” kÕt cÊu cÂŹ khÝ gĂ„m cžc kh©u liÂȘn kÕt vĂ­i nhau b»ng cžc khĂ­p Ÿéng ¼Ó cĂŁ thÓ tÂčo nÂȘn nhĂ·ng chuyÓn Ÿéng cÂŹ b¶n cña robot.

NguĂ„n Ÿéng lĂčc l” cžc Ÿéng cÂŹ ÂźiÖn (mĂ©t chiÒu hoÆc Ÿéng cÂŹ b−íc), cžc hÖ thĂšng xy lanh khÝ nÐn, thuĂ» lĂčc ¼Ó tÂčo Ÿéng lĂčc cho tay mžy hoÂčt Ÿéng.

DĂŽng cĂŽ thao tžc ¼−üc gŸn trÂȘn kh©u cuĂši cña robot, dĂŽng cĂŽ cña robot cĂŁ thÓ cĂŁ nhiÒu kiÓu khžc nhau nh− : dÂčng b”n tay ¼Ó nŸm bŸt ŸÚi t−üng hoÆc cžc c«ng cĂŽ l”m viÖc nh− mĂĄ h”n, Ÿž m”i, ¼Çu phun sÂŹn ...

ThiÕt bÞ dÂčy-hoc (Teach-Pendant) dĂŻng ¼Ó dÂčy cho robot cžc thao tžc cÇn thiÕt theo yÂȘu cÇu cña quž tr×nh l”m viÖc, sau Ÿã robot tĂč lÆp lÂči cžc Ÿéng tžc Ÿ· ¼−üc dÂčy ¼Ó l”m viÖc (ph−¬ng phžp lËp tr×nh kiÓu dÂčy hĂ€c).

Cžc phÇn mÒm ¼Ó lËp tr×nh v” cžc ch−¬ng tr×nh ÂźiÒu khiÓn robot ¼−üc c”i ¼Æt trÂȘn mžy tÝnh, dĂŻng ÂźiÒu khiÓn robot th«ng qua bĂ© ÂźiÒu khiÓn (Controller). BĂ© ÂźiÒu khiÓn cßn ¼−üc gĂ€i l” MoÂźun ÂźiÒu khiÓn (hay Unit, Driver), nĂŁ th−ĂȘng ¼−üc kÕt nĂši vĂ­i mžy tÝnh. MĂ©t moÂźun ÂźiÒu khiÓn cĂŁ thÓ cßn cĂŁ cžc cĂŠng V”o - Ra (I/O port) ¼Ó l”m viÖc vĂ­i nhiÒu thiÕt bÞ khžc nhau nh− cžc c¶m biÕn giĂłp robot nhËn biÕt trÂčng thži cña b¶n th©n, xžc ¼Þnh vÞ trÝ cña ŸÚi t−üng l”m viÖc hoÆc cžc dß t×m khžc; ÂźiÒu khiÓn cžc bšng t¶i hoÆc cÂŹ cÊu cÊp ph«i hoÂčt Ÿéng phĂši hĂźp vĂ­i robot ... 1.4.2. KÕt cÊu cña tay mžy : Nh− Ÿ· nĂŁi trÂȘn, tay mžy l” th”nh phÇn quan trĂ€ng, nĂŁ quyÕt ¼Þnh kh¶ nšng l”m viÖc cña robot. Cžc kÕt cÊu cña nhiÒu tay mžy ¼−üc phĂĄng theo cÊu tÂčo v” chĂžc nšng cña tay ng−ĂȘi; tuy nhiÂȘn ng”y nay, tay mžy ¼−üc thiÕt kÕ rÊt Âźa dÂčng, nhiÒu cžnh tay robot cĂŁ h×nh džng rÊt khžc xa cžnh tay ng−ĂȘi. Trong thiÕt kÕ v” sö dĂŽng tay mžy, chĂłng ta cÇn quan t©m ¼Õn cžc th«ng sĂš h×nh - Ÿéng hĂ€c, l” nhĂ·ng th«ng sĂš liÂȘn quan ¼Õn kh¶ nšng l”m viÖc cña robot nh− : tÇm vĂ­i (hay tr−ĂȘng c«ng tžc), sĂš bËc tĂč do (thÓ hiÖn sĂč khÐo lÐo linh hoÂčt cña robot), Ÿé cĂžng vĂ·ng, t¶i trĂ€ng vËt n©ng, lĂčc kÑp . . .

TS. PhÂčm §šng Ph−íc

Page 7: Robot cong nghiep

Robot C«ng nghiÖp 6

Cžc kh©u cña robot th−ĂȘng thĂčc hiÖn hai chuyÓn Ÿéng cÂŹ b¶n : ‱ ChuyÓn Ÿéng tÞnh tiÕn theo h−íng x,y,z trong kh«ng gian Descarde, th«ng th−ĂȘng

tÂčo nÂȘn cžc h×nh khĂši, cžc chuyÓn Ÿéng nÇy th−ĂȘng kĂœ hiÖu l” T (Translation) hoÆc P (Prismatic).

‱ ChuyÓn Ÿéng quay quanh cžc trĂŽc x,y,z kĂœ hiÖu l” R (Roatation). TuĂș thuĂ©c v”o sĂš kh©u v” sĂč tĂŠ hĂźp cžc chuyÓn Ÿéng (R v” T) m” tay mžy cĂŁ cžc kÕt cÊu khžc nhau vĂ­i vĂŻng l”m viÖc khžc nhau. Cžc kÕt cÊu th−ĂȘng gÆp cña l” Robot l” robot kiÓu toÂč Ÿé §Ò cžc, toÂč Ÿé trĂŽ, toÂč Ÿé cÇu, robot kiÓu SCARA, hÖ toÂč Ÿé gĂŁc (phĂĄng sinh) ... Robot kiÓu toÂč Ÿé §Ò cžc : l” tay mžy cĂŁ 3 chuyÓn Ÿéng cÂŹ b¶n tÞnh tiÕn theo ph−¬ng cña cžc trĂŽc hÖ toÂč Ÿé gĂšc (cÊu h×nh T.T.T). Tr−ĂȘng c«ng tžc cĂŁ dÂčng khĂši chĂ· nhËt. Do kÕt cÊu Ÿn gi¶n, loÂči tay mžy nÇy cĂŁ Ÿé cĂžng vĂ·ng cao, Ÿé chÝnh xžc cÂŹ khÝ dÔ ٦m b¶o v× vËy nĂŁ thuĂȘng dĂŻng ¼Ó vËn chuyÓn ph«i liÖu, lŸp ržp, h”n trong mÆt phÂŒng ...

T.T.T

H×nh 1.5 : Robot kiÓu toÂč Ÿé §Ò cžc R.T.T

H×nh 1.6 : Robot kiÓu toÂč Ÿé trĂŽ

Robot kiÓu toÂč Ÿé trĂŽ : VĂŻng l”m viÖc cña robot cĂŁ dÂčng h×nh trĂŽ rçng. Th−ĂȘng khĂ­p thĂž nhÊt chuyÓn Ÿéng quay. VÝ dĂŽ robot 3 bËc tĂč do, cÊu h×nh R.T.T nh− h×nh vÏ 1.6. CĂŁ nhiÒu robot kiÓu toÂč Ÿé trĂŽ nh− : robot Versatran cña h·ng AMF (Hoa KĂș). Robot kiÓu toÂč Ÿé cÇu : VĂŻng l”m viÖc cña robot cĂŁ dÂčng h×nh cÇu. th−ĂȘng Ÿé cĂžng vĂ·ng cña loÂči robot nÇy thÊp hÂŹn so vĂ­i hai loÂči trÂȘn. VÝ dĂŽ robot 3 bËc tĂč do, cÊu h×nh R.R.R hoÆc R.R.T l”m viÖc theo kiÓu toÂč Ÿé cÇu (h×nh 1.7).

R.R.R R.R.T

H×nh 1.7 : Robot kiÓu toÂč Ÿé cÇu Robot kiÓu toÂč Ÿé gĂŁc (HÖ toÂč Ÿé phĂĄng sinh) : §©y l” kiÓu robot ¼−üc dĂŻng nhiÒu hÂŹn c¶. Ba chuyÓn Ÿéng ¼Çu tiÂȘn l” cžc chuyÓn Ÿéng quay, trĂŽc quay thĂž nhÊt vu«ng gĂŁc vĂ­i hai trĂŽc kia. Cžc chuyÓn Ÿéng ¼Þnh h−íng khžc cĂČng l” cžc chuyÓn Ÿéng quay. VĂŻng l”m viÖc cña tay mžy nÇy gÇn giĂšng mĂ©t phÇn khĂši cÇu. TÊt c¶ cžc kh©u ¼Òu n»m trong mÆt phÂŒng thÂŒng ŸÞng nÂȘn cžc tÝnh tožn cÂŹ b¶n l” b”i tožn phÂŒng. −u ÂźiÓm nĂŠi bËt cña cžc loÂči robot hoÂčt

TS. PhÂčm §šng Ph−íc

Page 8: Robot cong nghiep

Robot C«ng nghiÖp 7

Ÿéng theo hÖ toÂč Ÿé gĂŁc l” gĂ€n nhÑ, tĂžc l” cĂŁ vĂŻng l”m viÖc t−¬ng ŸÚi lĂ­n so vĂ­i kÝch cĂ« cña b¶n th©n robot, Ÿé linh hoÂčt cao. Cžc robot hoÂčt Ÿéng theo hÖ toÂč Ÿé gĂŁc nh− : Robot PUMA cña h·ng Unimation - Nokia (Hoa KĂș - PhÇn Lan), IRb-6, IRb-60 (ThuĂŸ §iÓn), Toshiba, Mitsubishi, Mazak (NhËt B¶n) .V.V... VÝ dĂŽ mĂ©t robot hoÂčt Ÿéng theo hÖ toÂč Ÿé gĂŁc (HÖ toÂč Ÿé phĂĄng sinh), cĂŁ cÊu h×nh RRR.RRR :

H×nh 1.8 : Robot hoÂčt Ÿéng theo hÖ toÂč Ÿé gĂŁc. Robot kiÓu SCARA : Robot SCARA ra ÂźĂȘi v”o nšm 1979 tÂči tr−ĂȘng ÂźÂči hĂ€c Yamanashi (NhËt B¶n) l” mĂ©t kiÓu robot mĂ­i nh»m Ÿžp Ăžng sĂč Âźa dÂčng cña cžc quž tr×nh s¶n xuÊt. TÂȘn gĂ€i SCARA l” viÕt tŸt cña "Selective Compliant Articulated Robot Arm" : Tay mžy mÒm dÏo tuĂș Ăœ. LoÂči robot nÇy th−ĂȘng dĂŻng trong c«ng viÖc lŸp ržp nÂȘn SCARA Ÿ«i khi ¼−üc gi¶i thÝch l” tĂ” viÕt tŸt cña "Selective Compliance Assembly Robot Arm". Ba khĂ­p ¼Çu tiÂȘn cña kiÓu Robot nÇy cĂŁ cÊu h×nh R.R.T, cžc trĂŽc khĂ­p ¼Òu theo ph−¬ng thÂŒng ŸÞng. SÂŹ ŸÄ cña robot SCARA nh− h×nh 1.9.

H×nh 1.9 : Robot kiÓu SCARA

1.5. Ph©n loÂči Robot c«ng nghiÖp : Robot c«ng nghiÖp rÊt phong phĂł Âźa dÂčng, cĂŁ thÓ ¼−üc ph©n loÂči theo cžc cžch sau :

1.4.1. Ph©n loÂči theo kÕt cÊu :

Theo kÕt cÊu cña tay mžy ng−ĂȘi ta ph©n th”nh robot kiÓu toÂč Ÿé §Ò cžc, KiÓu toÂč Ÿé trĂŽ, kiÓu toÂč Ÿé cÇu, kiÓu toÂč Ÿé gĂŁc, robot kiÓu SCARA nh− Ÿ· tr×nh b”y Ă« trÂȘn. 1.4.2. Ph©n loÂči theo hÖ thĂšng truyÒn Ÿéng :

CĂŁ cžc dÂčng truyÒn Ÿéng phĂŠ biÕn l” : HÖ truyÒn Ÿéng ÂźiÖn : Th−ĂȘng dĂŻng cžc Ÿéng cÂŹ ÂźiÖn 1 chiÒu (DC : Direct Current)

hoÆc cžc Ÿéng cÂŹ b−íc (step motor). LoÂči truyÒn Ÿéng nÇy dÔ ÂźiÒu khiÓn, kÕt cÊu gĂ€n. HÖ truyÒn Ÿéng thuĂ» lĂčc : cĂŁ thÓ ÂźÂčt ¼−üc c«ng suÊt cao, Ÿžp Ăžng nhĂ·ng ÂźiÒu kiÖn l”m viÖc nÆng. Tuy nhiÂȘn hÖ thĂšng thuĂ» lĂčc th−ĂȘng cĂŁ kÕt cÊu cĂ„ng kÒnh, tĂ„n tÂči Ÿé phi tuyÕn lĂ­n khĂŁ xö lĂœ khi ÂźiÒu khiÓn. HÖ truyÒn Ÿéng khÝ nÐn : cĂŁ kÕt cÊu gĂ€n nhÑ hÂŹn do kh«ng cÇn dÉn ng−üc nh−ng lÂči ph¶i gŸn liÒn vĂ­i trung t©m taĂ€ ra khÝ nÐn. HÖ nÇy l”m viÖc vĂ­i c«ng suÊt trung b×nh v” nhĂĄ, kÐm chÝnh xžc, th−ĂȘng chØ thÝch hĂźp vĂ­i cžc robot hoÂčt Ÿéng theo ch−¬ng tr×nh ¼Þnh sÂŒn vĂ­i cžc thao tžc Ÿn gi¶n “nhÊc lÂȘn - ¼Æt xuĂšng” (Pick and Place or PTP : Point To Point).

TS. PhÂčm §šng Ph−íc

Page 9: Robot cong nghiep

Robot C«ng nghiÖp 8

1.4.3. Ph©n loÂči theo Ăžng dĂŽng : DĂča v”o Ăžng dĂŽng cña robot trong s¶n xuÊt cĂŁ Robot sÂŹn, robot h”n, robot lŸp ržp, robot chuyÓn ph«i .v.v... 1.4.4. Ph©n loÂči theo cžch thĂžc v” ¼Æc tr−ng cña ph−¬ng phžp ÂźiÒu khiÓn : CĂŁ robot ÂźiÒu khiÓn hĂ« (mÂčch ÂźiÒu khiÓn kh«ng cĂŁ cžc quan hÖ ph¶n hĂ„i), Robot ÂźiÒu khiÓn kÝn (hay ÂźiÒu khiÓn servo) : sö dĂŽng c¶m biÕn, mÂčch ph¶n hĂ„i ¼Ó tšng Ÿé chÝnh xžc v” mĂžc Ÿé linh hoÂčt khi ÂźiÒu khiÓn. Ngo”i ra cßn cĂŁ thÓ cĂŁ cžc cžch ph©n loÂči khžc tuĂș theo quan ÂźiÓm v” mĂŽc ¼Ých nghiÂȘn cĂžu -----------------------------------------------------------------------------------------------------------------

TS. PhÂčm §šng Ph−íc

Page 10: Robot cong nghiep

Robot c«ng nghiÖp 9

Ch−¬ng II

C¾c phÐp biÕn ¼éi thuÇn nhÊt

(Homogeneous Transformation)

Khi xem xÐt, nghiÂȘn cĂžu mĂši quan hÖ giĂ·a robot v” vËt thÓ ta kh«ng nhĂ·ng cÇn quan t©m ¼Õn vÞ trÝ (Position) tuyÖt ŸÚi cña ÂźiÓm, ¼−ĂȘng, mÆt cña vËt thÓ so vĂ­i ÂźiÓm tžc Ÿéng cuĂši (End effector) cña robot m” cßn cÇn quan t©m ¼Õn vÊn ¼Ò ¼Þnh h−íng (Orientation) cña kh©u chÊp h”nh cuĂši khi vËn Ÿéng hoÆc ¼Þnh vÞ taÞ mĂ©t vÞ trÝ. §Ó m« t¶ quan hÖ vÒ vÞ trÝ v” h−íng giĂ·a robot v” vËt thÓ ta ph¶i dĂŻng ¼Õn cžc phÐp biÕn ŸÊi thuÇn nhÊt. Ch−¬ng nÇy cung cÊp nhĂ·ng hiÓu biÕt cÇn thiÕt tr−íc khi Âźi v”o gi¶i quyÕt cžc vÊn ¼Ò liÂȘn quan tĂ­i Ÿéng hĂ€c v” Ÿéng lĂčc hĂ€c robot. 2.1. HÖ tĂ€a Ÿé thuÇn nhÊt :

§Ó biÓu diÔn mĂ©t ÂźiÓm trong kh«ng gian ba chiÒu, ng−ĂȘi ta dĂŻng VectÂŹ ÂźiÓm (Point vector). VectÂŹ ÂźiÓm th−ĂȘng ¼−üc kĂœ hiÖu b»ng cžc chĂ· viÕt th−ĂȘng nh− u, v, x1 . . . ¼Ó m« t¶ vÞ trÝ cña ÂźiÓm U, V, X1 ,. . . TĂŻy thuĂ©c v”o hÖ qui chiÕu ¼−üc chĂ€n, trong kh«ng gian 3 chiÒu, mĂ©t ÂźiÓm V cĂŁ thÓ ¼−üc biÓu diÔn b»ng nhiÒu vectÂŹ ÂźiÓm khžc nhau :

vE

V

F

vF

E

H×nh 2.2 : BiÓu diÔn 1 ÂźiÓm trong kh«ng gian

vE v” vF l” hai vectÂŹ khžc nhau mÆc dĂŻ c¶ hai vectÂŹ cĂŻng m« t¶ ÂźiÓm V. NÕu i, j, k l” cžc vec tÂŹ Ÿn vÞ cña mĂ©t hÖ toÂč Ÿé n”o Ÿã, chÂŒng hÂčn trong E, ta cĂŁ :

r r r rv = ai + bj + ck

vĂ­i a, b, c l” toÂč Ÿé vÞ trÝ cña ÂźiÓm V trong hÖ Ÿã. NÕu quan t©m ŸÄng thĂȘi vÊn ¼Ò ¼Þnh vÞ v” ¼Þnh h−íng, ta ph¶i biÓu diÔn vectÂŹ v trong

kh«ng gian bĂšn chiÒu vĂ­i suÊt vectÂŹ l” mĂ©t ma trËn cĂ©t :

x x/w = a v = y Trong Ÿã y/w = b z z/w = c w

vĂ­i w l” mĂ©t h»ng sĂš thĂčc n”o Ÿã. w cßn ¼−üc gĂ€i l” hÖ sĂš tØ lÖ, biÓu thÞ cho chiÒu thĂž t− ngÇm ¼Þnh, NÕu w = 1 dÔ thÊy :

xw

x x a= = =1

; yw

y y b= = =1

; zw

z z a= = =1

TS. PhÂčm §šng Ph−íc

Page 11: Robot cong nghiep

Robot c«ng nghiÖp 10

Trong tr−ĂȘng hĂźp nÇy th× cžc toÂč Ÿé biÓu diÔn b»ng vĂ­i toÂč Ÿé vËt lĂœ cña ÂźiÓm trong kh«ng gian 3 chiÒu, hÖ toÂč Ÿé sö dĂŽng w=1 ¼−üc gĂ€i l” hÖ toÂč Ÿé thuÇn nhÊt.

VĂ­i w = 0 ta cĂŁ : xw

yw

zw

= = = ∞

GiĂ­i hÂčn ∞ thÓ hiÖn h−íng cña cžc trĂŽc toÂč Ÿé. NÕu w l” mĂ©t h»ng sĂš n”o Ÿã ≠ 0 v” 1 th× viÖc biÓu diÔn ÂźiÓm trong kh«ng gian t−¬ng Ăžng vĂ­i hÖ sĂš tØ lÖ w :

VÝ dî : r r r rv i j k= + +3 4 5

vĂ­i w = 1 (tr−ĂȘng hĂźp thuÇn nhÊt) : v = [3 4 5 1]T

vĂ­i w=-10 biÓu diÔn t−¬ng Ăžng sÏ l” : v = [-30 -40 -50 -10]T KĂœ hiÖu [ . . . . ]T (ChĂ· T viÕt cao lÂȘn trÂȘn ¼Ó chØ phÐp chuyÓn ŸÊi vectÂŹ h”ng th”nh vectÂŹ cĂ©t). Theo cžch biÓu diÔn trÂȘn Ÿ©y, ta qui −íc :

[0 0 0 0]T l” vectÂŹ kh«ng xžc ¼Þnh [0 0 0 n]T vĂ­i n ≠ 0 l” vectÂŹ kh«ng, trĂŻng vĂ­i gĂšc toÂč Ÿé [x y z 0]T l” vectÂŹ chØ h−íng [x y z 1]T l” vectÂŹ ÂźiÓm trong hÖ toÂč Ÿé thuÇn nhÊt.

2.2. NhŸc lÂči cžc phÐp tÝnh vÒ vectÂŹ v” ma trËn : 2.2.1. PhÐp nh©n vÐctÂŹ :

Cho hai vectÂŹ : r r r ra a i a j a kx y z= + +

r r r rb b i b j b kx y z= + +

Ta cĂŁ tÝch v« h−íng a.b = axbx + ayby + azbz

V” tÝch vectÂŹ :

ar x = rb

zyx

zyx

bbbaaakjirrr

= (aybz-azby)ri + (azbx-axbz)

rj + (axby-aybx)

rk

2.2.2. C¾c phÐp tÝnh vÒ ma trËn :

a/ PhÐp cĂ©ng, trĂ” ma trËn : CĂ©ng (trĂ” ) cžc ma trËn A v” B cĂŻng bËc sÏ cĂŁ ma trËn C cĂŻng bËc, vĂ­i cžc phÇn tö cij b»ng tĂŠng (hiÖu) cña cžc phÇn tö aij v” bij (vĂ­i mĂ€i i, j). A + B = C VĂ­i cij = aij + bij. A - B = C VĂ­i cij = aij - bij. PhÐp cĂ©ng, trĂ” ma trËn cĂŁ cžc tÝnh chÊt giĂšng phÐp cĂ©ng sĂš thĂčc. b/ TÝch cña hai ma trËn : TÝch cña ma trËn A (kÝch th−íc m x n) vĂ­i ma trËn B (kÝch th−íc n x p) l” ma trËn C cĂŁ kÝch th−íc m x p. VÝ dĂŽ : cho hai ma trËn :

1 2 3 1 2A = 4 5 6 v” B = 3 4

7 8 9 5 6 Ta cĂŁ :

TS. PhÂčm §šng Ph−íc

Page 12: Robot cong nghiep

Robot c«ng nghiÖp 11

1.1+2.3+3.5 1.2+2.4+3.6 22 28 C = A.B = 4.1+5.3+6.5 4.2+5.4+6.6 = 49 64

7.1+8.3+9.5 7.2+8.4+9.6 76 100 PhÐp nh©n hai ma trËn kh«ng cĂŁ tÝnh giao hožn, nghÜa l” : A . B ≠ B . A Ma trËn Ÿn vÞ I (Indentity Matrix) giao hožn ¼−üc vĂ­i bÊt kĂș ma trËn n”o : I.A = A.I PhÐp nh©n ma trËn tu©n theo cžc qui tŸc sau : 1. (k.A).B = k.(A.B) = A.(k.B) 2. A.(B.C) = (A.B).C 3. (A + B).C = A.C + B.C 4. C.(A + B) = C.A + C.B c/ Ma trËn nghÞch ٦o cña ma trËn thuÇn nhÊt :

MĂ©t ma trËn thuÇn nhÊt l” ma trËn 4 x 4 cĂŁ dÂčng :

nx Ox ax px

T = ny Oy ay py

nz Oz az pz

0 0 0 1

Ma trËn nghÞch ٦o cña T kĂœ hiÖu l” T-1 :

nx ny nz -p.nT-1 = Ox Oy Oz -p.O (2-1)

ax ay az -p.a 0 0 0 1

Trong Ÿã p.n l” tÝch v« h−íng cña vectÂŹ p v” n. nghÜa l” : p.n = pxnx + pyny + pznz

t−¬ng tĂč : p.O = pxOx + pyOy + pzOz

v” p.a = pxax + pyay + pzaz

VÝ dĂŽ : t×m ma trËn nghÞch ٦o cña ma trËn biÕn ŸÊi thuÇn nhÊt :

0 0 1 1H = 0 1 0 2

-1 0 0 3 0 0 0 1

Gi¶i : žp dÎng c«ng thÞc (2-1), ta cã :

0 0 -1 3H-1 = 0 1 0 -2

1 0 0 -1 0 0 0 1

ChĂłng ta kiÓm chĂžng r»ng Ÿ©y chÝnh l” ma trËn nghÞch ٦o b»ng cžc nh©n ma trËn H vĂ­i H-1 :

0 0 1 1 0 0 -1 3 1 0 0 0 0 1 0 2 0 1 0 -2 = 0 1 0 0 -1 0 0 3 1 0 0 -1 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 1

TS. PhÂčm §šng Ph−íc

Page 13: Robot cong nghiep

Robot c«ng nghiÖp 12

Ph−¬ng phžp tÝnh ma trËn nghÞch ٦o nÇy nhanh hÂŹn nhiÒu so vĂ­i ph−¬ng phžp chung; tuy nhiÂȘn nĂŁ kh«ng žp dĂŽng ¼−üc cho ma trËn 4x4 bÊt kĂș m” kÕt qu¶ chØ Ÿóng vĂ­i ma trËn thuÇn nhÊt. d/ VÕt cña ma trËn : VÕt cña ma trËn vu«ng bËc n l” tĂŠng cžc phÇn tö trÂȘn ¼−ĂȘng chÐo :

Trace(A) hay Tr(A) = ∑ =

n

iiia

1

MĂ©t sĂš tÝnh chÊt quan trĂ€ng cña vÕt ma trËn : 1/ Tr(A) = Tr(AT) 2/ Tr(A+B) = Tr(A) + Tr(B) 3/ Tr(A.B) = Tr(B.A) 4/ Tr(ABCT) = Tr(CBTAT) e/ §Âčo h”m v” tÝch ph©n ma trËn : NÕu cžc phÇn tö cña ma trËn A l” h”m nhiÒu biÕn, th× cžc phÇn tö cña ma trËn ÂźÂčo h”m b»ng ÂźÂčo h”m riÂȘng cña cžc phÇn tö ma trËn A theo biÕn t−¬ng Ăžng.

VÝ dî : cho

⎄⎄⎄⎄

⎊

⎀

⎱⎱⎱⎱

⎣

⎡

=

44434241

34333231

24232221

14131211

aaaaaaaaaaaaaaaa

A

th× : dt

ta

ta

ta

ta

ta

ta

ta

ta

ta

ta

ta

ta

ta

ta

ta

ta

dA

44434241

34333231

24232221

14131211

⎄⎄⎄⎄⎄⎄⎄⎄

⎊

⎀

⎱⎱⎱⎱⎱⎱⎱⎱

⎣

⎡

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=

T−¬ng tĂč, phÐp tÝch ph©n cña ma trËn A l” mĂ©t ma trËn, cĂŁ :

})({)( dttadttA ij∫∫ =

2.3. Cžc phÐp biÕn ŸÊi Cho u l” vectÂŹ ÂźiÓm biÓu diÔn ÂźiÓm cÇn biÕn ŸÊi, h l” vectÂŹ dÉn ¼−üc biÓu diÔn b»ng

mĂ©t ma trËn H gĂ€i l” ma trËn chuyÓn ŸÊi . Ta cĂŁ : v = H.u

v l” vectÂŹ biÓu diÔn ÂźiÓm sau khi Ÿ· biÕn ŸÊi. 2.3.1. PhÐp biÕn ŸÊi tÞnh tiÕn (Translation) : Gi¶ sö cÇn tÞnh tiÕn mĂ©t ÂźiÓm hoÆc mĂ©t vËt thÓ theo vectÂŹ dÉn

r r r rh ai bj ck= + + . Tr−íc

hÕt ta cĂŁ ¼Þnh nghÜa cña ma trËn chuyÓn ŸÊi H :

1 0 0 a H = Trans(a,b,c) = 0 1 0 b (2.2)

0 0 1 c 0 0 0 1

TS. PhÂčm §šng Ph−íc

Page 14: Robot cong nghiep

Robot c«ng nghiÖp 13

GĂ€i u l” vectÂŹ biÓu diÔn ÂźiÓm cÇn tÞnh tiÕn : u = [x y z w]T

Th× v l” vectÂŹ biÓu diÔn ÂźiÓm Ÿ· biÕn ŸÊi tÞnh tiÕn ¼−üc xžc ¼Þnh bĂ«i :

1 0 0 a x x+aw x/w+a v = H.u = 0 1 0 b . y = y+bw = y/w+b

0 0 1 c z z+cw z/w+c 0 0 0 1 w w 1

Nh− vËy b¶n chÊt cña phÐp biÕn ŸÊi tÞnh tiÕn l” phÐp cĂ©ng vectÂŹ giĂ·a vectÂŹ biÓu diÔn

ÂźiÓm cÇn chuyÓn ŸÊi v” vectÂŹ dÉn.

VÝ dî : r r r r

r r ru = 2i + 3j + 2k

h = 4i - 3j + 7kr

Th× 1 0 0 4 2 2+4 6

v = Hu = 0 1 0 -3 . 3 = 3-3 = 0 0 0 1 7 2 2+7 9 0 0 0 1 1 1 1

v” viÕt l” : v = Trans(a,b,c) u

H×nh 2..4: PhÐp biÕn ŸÊi tÞnh tiÕn trong kh«ng gian 2.3.2. PhÐp quay (Rotation) quanh cžc trĂŽc toÂč Ÿé : Gi¶ sö ta cÇn quay mĂ©t ÂźiÓm hoÆc mĂ©t vËt thÓ xung quanh trĂŽc toÂč Ÿé n”o Ÿã vĂ­i gĂŁc quay Ξo, ta lÇn l−üt cĂŁ cžc ma trËn chuyÓn ŸÊi nh− sau :

1 0 0 0 Rot(x, Ξo) = 0 cosΞ -sinΞ 0 (2.3)

0 sinΞ cosΞ 0 0 0 0 1

cosΞ 0 sinΞ 0

Rot(y, Ξo) = 0 1 0 0 (2.4) -sinΞ 0 cosΞ 0 0 0 0 1

z

y

x

h

u

v

46

2

3-32

0

7

9

TS. PhÂčm §šng Ph−íc

Page 15: Robot cong nghiep

Robot c«ng nghiÖp 14

cosΞ -sinΞ 0 0 Rot(z, Ξo) = sinΞ cosΞ 0 0 (2.5)

0 0 1 0 0 0 0 1

VÝ dĂŽ : Cho ÂźiÓm U biÓu diÔn bĂ«i r r r ru = 7i + 3j + 2k quay xung quanh z mĂ©t gĂŁc Ξ = 90o

(h×nh 2.5). Ta cã 0 -1 0 0 7 -3

v= Rot(z, 90o)u = 1 0 0 0 3 = 7 0 0 1 0 2 2 0 0 0 1 1 1

NÕu cho ÂźiÓm Ÿ· biÕn ŸÊi tiÕp tĂŽc quay xung quanh y mĂ©t gĂŁc 90o ta cĂŁ :

0 0 1 0 -3 2 w = Rot(y, 90o)v = 0 1 0 0 7 = 7

-1 0 0 0 2 3 0 0 0 1 1 1

V” cĂŁ thÓ biÓu diÔn : 2

w = Rot(y, 90o). Rot(z, 90o) . u = 7 3 1

ChĂł Ăœ : NÕu ŸÊi thĂž tĂč quay ta sÏ ¼−üc w’≠ w (h×nh 2.6), cĂŽ thÓ : cho U quay quanh y tr−íc 1 gĂŁc 900, ta cĂŁ :

0 0 1 0 7 2

v’ = 0 1 0 0 3 = 3 = Rot(y, 90o).u -1 0 0 0 2 -7 0 0 0 1 1 1

Sau Ÿã cho ÂźiÓm vĂ”a biÕn ŸÊi quay quanh z mĂ©t gĂŁc 900, ta ¼−üc :

0 -1 0 0 2 -3 w’ = 1 0 0 0 3 = 2 = Rot(z, 90o).Rot(y,900)u

0 0 1 0 -7 -7 0 0 0 1 1 1

RĂą r”ng : Rot(y, 90o).Rot(z,900)u ≠ Rot(z,900).Rot(y, 90o)u

y

w

z

u

x

v

x

y

u

v’

w’

z

H×nh 2.5 H×nh 2.6 w = Rot(y, 90o). Rot(z, 90o)u w’= Rot(z, 90o). Rot(y, 90o)u

TS. PhÂčm §šng Ph−íc

Page 16: Robot cong nghiep

Robot c«ng nghiÖp 15

2.3.3. PhÐp quay tĂŠng qužt : Trong mĂŽc trÂȘn, ta vĂ”a nghiÂȘn cĂžu cžc phÐp quay cÂŹ b¶n xung quanh cžc trĂŽc toÂč Ÿé

x,y,z cña hÖ toÂč Ÿé chuÈn O(x,y,z). Trong phÇn nÇy, ta nghiÂȘn cĂžu phÐp quay quanh mĂ©t vectÂŹ k bÊt kĂș mĂ©t gĂŁc Ξ. R”ng buĂ©c duy nhÊt l” vectÂŹ k ph¶i trĂŻng vĂ­i gĂšc cña mĂ©t hÖ toÂč Ÿé xžc ¼Þnh tr−íc. Ta h·y kh¶o sžt mĂ©t hÖ toÂč Ÿé C, gŸn lÂȘn ÂźiÓm tžc Ÿéng cuĂši (b”n tay) cña robot, hÖ C ¼−üc biÓu diÔn bĂ«i :

Cx Cy Cz Co nx Ox az 0

C = ny Oy ay 0 nz Oz az 0 0 0 0 1

Khi gŸn hÖ toÂč Ÿé nÇy lÂȘn b”n tay robot (h×nh 2.7), cžc vectÂŹ Ÿn vÞ ¼−üc biÓu thÞ nh−

sau : a : l” vectÂŹ cĂŁ h−íng tiÕp cËn vĂ­i ŸÚi t−üng (approach); O: l” vectÂŹ cĂŁ h−íng m” theo Ÿã cžc ngĂŁn tay nŸm v”o khi cÇm nŸm ŸÚi t−üng (Occupation); n : VectÂŹ phžp tuyÕn vĂ­i (O,a) (Normal). B©y giĂȘ ta h·y coi vectÂŹ bÊt kĂș k (m” ta cÇn thĂčc hiÖn phÐp quay quanh nĂŁ mĂ©t gĂŁc Ξ) l” mĂ©t trong cžc vectÂŹ Ÿn vÞ cña hÖ C.

ChÂŒng hÂčn : r r r rk = a i + a j + a kx y z

LĂłc Ÿã, phÐp quay Rot(k,Ξ) sÏ trĂ« th”nh phÐp quay Rot(Cz,Ξ). NÕu ta cĂŁ T m« t¶ trong hÖ gĂšc trong Ÿã k l” vectÂŹ bÊt kĂș, th× ta cĂŁ X m« t¶ trong hÖ C

vĂ­i k l” mĂ©t trong cžc vectÂŹ Ÿn vÞ. TĂ” ÂźiÒu kiÖn biÕn ŸÊi thuÇn nhÊt, T v” X cĂŁ liÂȘn hÖ : T = C.X hay X = C -1.T

LĂłc Ÿã cžc phÐp quay d−íi Ÿ©y l” ŸÄng nhÊt :

Rot(k,Ξ) = Rot(Cz,Ξ) hay l” Rot(k,Ξ).T = C.Rot(z,Ξ).X = C.Rot(z,Ξ).C -1.T VËy Rot(k,Ξ) = C.Rot(z,Ξ).C -1 (2.6) Trong Ÿã Rot(z,Ξ) l” phÐp quay cÂŹ b¶n quanh trĂŽc z mĂ©t gĂŁc Ξ, cĂŁ thÓ sö dĂŽng c«ng

thĂžc (2.5) nh− Ÿ· tr×nh b”y. C-1 l” ma trËn nghÞch ٦o cña ma trËn C. Ta cĂŁ :

nx ny nz 0C-1 = Ox Oy Oz 0

ax ay az 0 0 0 0 1

a (Cx)O(Cy)

Co

n (Cz)

H×nh 2.7 : HÖ toÂč Ÿé gŸn trÂȘn kh©u chÊp h”nh cuĂši (b”n tay)

TS. PhÂčm §šng Ph−íc

Page 17: Robot cong nghiep

Robot c«ng nghiÖp 16

Thay cžc ma trËn v”o vÕ ph¶i cña ph−¬ng tr×nh (2.6) :

nx Ox ax 0 cosΞ -sinΞ 0 0 nx ny nz 0

Rot(k,Ξ) = ny Oy ay 0 sinΞ cosΞ 0 0 Ox Oy Oz 0 nz Oz az 0 0 0 1 0 ax ay az 0 0 0 0 1 0 0 0 1 0 0 0 1

Nh©n 3 ma trËn nÇy vĂ­i nhau ta ¼−üc :

nxnxcosΞ - nxOxsinΞ + nxOxsinΞ + OxOxcosΞ + axax

Rot(k,Ξ) = nxnycosΞ - nyOxsinΞ + nxOysinΞ + OxOycosΞ + ayax

nxnzcosΞ - nzOxsinΞ + nxOzsinΞ + OxOzcosΞ + azax

0

nxnycosΞ - nxOysinΞ + nyOxsinΞ + OxOycosΞ + axay

nynycosΞ - nyOysinΞ + nyOysinΞ + OyOycosΞ + ayay

nznycosΞ - nzOysinΞ + nyOzsinΞ + OzOycosΞ + azay

0

nxnzcosΞ - nxOzsinΞ + nzOxsinΞ + OxOzcosΞ + axaz0

nynzcosΞ - nyOzsinΞ + nzOysinΞ + OyOzcosΞ + ayaz0

nznzcosΞ - nzOzsinΞ + nzOzsinΞ + OzOzcosΞ + azaz0

0 1 (2.7)

§Ó Ÿn gi¶n cžch biÓu thÞ ma trËn, ta xÐt cžc mĂši quan hÖ sau : - TÝch v« h−íng cña bÊt kĂș h”ng hay cĂ©t n”o cña C vĂ­i bÊt kĂș h”ng hay cĂ©t n”o khžc ¼Òu b»ng 0 v× cžc vectÂŹ l” trĂčc giao. - TÝch v« h−íng cña bÊt kĂș h”ng hay cĂ©t n”o cña C vĂ­i chÝnh nĂŁ ¼Òu b»ng 1 v× l” vectÂŹ Ÿn vÞ. - VectÂŹ Ÿn vÞ z b»ng tÝch vectÂŹ cña x v” y, hay l” :

r r ra = n x O

Trong Ÿã : ax = nyOz - nzOy

ay = nxOz - nzOx

ax = nxOy - nyOx

Khi cho k trĂŻng vĂ­i mĂ©t trong sĂš cžc vectÂŹ Ÿn vÞ cña C ta Ÿ· chĂ€n : kz = ax ; ky = ay ; kz = az

Ta kĂœ hiÖu VersΞ = 1 - cosΞ (Versin Ξ). BiÓu thĂžc (2.6) ¼−üc rĂłt gĂ€n th”nh :

kxkxversΞ+cosΞ kykxversΞ-kzsinΞ kzkxversΞ+kysinΞ 0 Rot(k,Ξ) = kxkyversΞ+kzsinΞ kykyversΞ+cosΞ kzkyversΞ-kxsinΞ 0 (2.8)

kxkzversΞ+kysinΞ kykzversΞ+kzsinΞ kzkzversΞ+cosΞ 0 0 0 0 1

§©y l” biÓu thĂžc cña phÐp quay tĂŠng qužt quanh mĂ©t vectÂŹ bÊt kĂș k. TĂ” phÐp quay tĂŠng

qužt cĂŁ thÓ suy ra cžc phÐp quay cÂŹ b¶n quanh cžc trĂŽc toÂč Ÿé.

TS. PhÂčm §šng Ph−íc

Page 18: Robot cong nghiep

Robot c«ng nghiÖp 17

2.3.4. B”i tožn ng−üc : t×m gĂŁc quay v” trĂŽc quay t−¬ng ¼−¬ng :

TrÂȘn Ÿ©y ta Ÿ· nghiÂȘn cĂžu cžc b”i tožn thuËn, nghÜa l” chØ ¼Þnh trĂŽc quay v” gĂŁc quay tr−íc- xem xÐt kÕt qu¶ biÕn ŸÊi theo cžc phÐp quay Ÿ· chØ ¼Þnh. Ng−üc lÂči vĂ­i b”i tožn trÂȘn, gi¶ sö ta Ÿ· biÕt kÕt qu¶ cña mĂ©t phÐp biÕn ŸÊi n”o Ÿã, ta ph¶i Âźi t×m trĂŽc quay k v” gĂŁc quay Ξ t−¬ng Ăžng. Gi¶ sö kÕt qu¶ cña phÐp biÕn ŸÊi thuÇn nhÊt R=Rot(k, Ξ), xžc ¼Þnh bĂ«i :

nx Ox ax 0 R = ny Oy ay 0

nz Oz az 0 0 0 0 1

Ta cÇn xžc ¼Þnh trĂŽc quay k v” gĂŁc quay Ξ. Ta Ÿ· biÕt Rot(k, Ξ) ¼−üc ¼Þnh nghÜa bĂ«i ma trËn (2.6) , nÂȘn : nx Ox ax 0 kxkxversΞ+cosΞ kykxversΞ-kzsinΞ kzkxversΞ+kysinΞ 0 ny Oy ay 0 = kxkyversΞ+kzsinΞ kykyversΞ+cosΞ kzkyversΞ-kxsinΞ 0 nz Oz az 0 kxkzversΞ+kysinΞ kykzversΞ+kzsinΞ kzkzversΞ+cosΞ 0 0 0 0 1 0 0 0 1

(2.9) B−íc 1 : Xžc ¼Þnh gĂŁc quay Ξ. * CĂ©ng ¼−ĂȘng chÐo cña hai ma trËn Ă« hai vÕ ta cĂŁ : nx + Oy + az + 1 = versΞ + cosΞ + versΞ + cosΞ + versΞ + cosΞ + 1 k x

2 k y2 kz

2

= (1 - cossΞ)( + + ) + 3cosΞ + 1 k x2 k y

2 kz2

= 1 - cosΞ + 3cosΞ +1 = 2(1+ cosΞ)

⇒ cosΞ = (nx + Oy + az - 1)/2 * TÝnh hiÖu cžc phÇn tö t−¬ng ¼−¬ng cña hai ma trËn, chÂŒng hÂčn : Oz- ay = 2kxsinΞ ax - nz = 2kysinΞ (2.10) ny - Ox = 2kzsinΞ B×nh ph−¬ng hai vÕ cña cžc ph−¬ng tr×nh trÂȘn rĂ„i cĂ€ng lÂči ta cĂŁ :

(Oz- ay)2 + (ax - nz)

2 + (ny - Ox)2 = 4 sin2Ξ

⇒ sinΞ = ±12

(O - a ) + (a - n ) + (n - O ) z y2

x z2

y x2

VĂ­i 0 ≀ Ξ ≀ 1800 :

tgΞ = (O - a ) + (a - n ) + (n - O )

(n + O + a - 1)z y

2x z

2y x

2

x y z

V” trĂŽc k ¼−üc ¼Þnh nghÜa bĂ«i :

k = O a

2sinz y

x

−

Ξ; k =

a n2sinx z

y−ξ

; k = n O

2siny z

x

−

Ξ (2.11)

§Ó Ăœ r»ng vĂ­i cžc c«ng thĂžc (2.8) :

- NÕu Ξ = 00 th× kx, ky, kz cĂŁ dÂčng 00

. LĂłc nÇy ph¶i chuÈn hož k sao cho ⎄ k⎄ = 1

TS. PhÂčm §šng Ph−íc

Page 19: Robot cong nghiep

Robot c«ng nghiÖp 18

- NÕu Ξ = 1800 th× kx, ky, kz cĂŁ dÂčng a ≠ 0

0. LĂłc nÇy k kh«ng xžc ¼Þnh ¼−üc, ta ph¶i

dĂŻng cžch tÝnh khžc cho tr−ĂȘng hĂźp nÇy : XÐt cžc phÇn tö t−¬ng ¼−¬ng cña hai ma trËn (2.9) : nx = k versΞ+cosΞ x

2

Oy = k y2 versΞ+cosΞ

az = kz2 versΞ+cosΞ

TÔ Ÿ©y ta suy ra :

k n

vers

n1- cosx

x x= ±−

= ±−cos cosΞ

ΞΞΞ

k O

vers

O1- cosy

y y= ±−

= ±−cos cosΞ

ΞΞΞ

k a

vers

a1- cosz

z z= ±−

= ±−cos cosΞ

ΞΞΞ

Trong kho¶ng 900 ≀ Ξ ≀ 1800 sinΞ lu«n lu«n d−¬ng DĂča v”o hÖ ph−¬ng tr×nh (2.10) ta thÊy kx, ky, kz lu«n cĂŁ cĂŻng dÊu vĂ­i vÕ trži. Ta dĂŻng h”m Sgn(x) ¼Ó biÓu diÔn quan hÖ “cĂŻng dÊu vĂ­i x”, nh− vËy :

k Sgn(On1- cosx z

x= −−

ay )cosΞΞ

k Sgn(a - n )O1- cosy x z

y=− cosξ

Ξ (2.12)

k Sgn(n Oa1- cosz y x

z= −−

)cosΞΞ

HÖ ph−¬ng tr×nh (2.12) chØ dĂŻng ¼Ó xžc ¼Þnh xem trong cžc kx, ky, kz th”nh phÇn n”o cĂŁ giž trÞ lĂ­n nhÊt. Cžc th”nh phÇn cßn lÂči nÂȘn tÝnh theo th”nh phÇn cĂŁ giž trÞ lĂ­n nhÊt ¼Ó xžc ¼Þnh k ¼−üc thuËn tiÖn. LĂłc Ÿã dĂŻng ph−¬ng phžp cĂ©ng cžc cÆp cßn lÂči cña cžc phÇn tö ŸÚi xĂžng qua ¼−ĂȘng chÐo ma trËn chuyÓn ŸÊi (2.9) : ny + Ox = 2kxkyversΞ = 2kxky(1 - cosΞ)

Oz + ay = 2kykzversΞ = 2kykz(1 - cosΞ) (2.13)

ax + nz = 2kzkxversΞ = 2kzkx(1 - cosΞ)

Gi¶ sö theo hÖ (2.12) ta cĂŁ kx l” lĂ­n nhÊt, lĂłc Ÿã ky, kz sÏ tÝnh theo kx b»ng hÖ (2.13); cĂŽ

thÓ l” : kn O

kyy

x

=+

−x

2 1( cos )Ξ

ka n

kzx

x

=+−

z

2 1( cos )Ξ

VÝ dĂŽ : Cho R = Rot[y,900]Rot[z,900]. H·y xžc ¼Þnh k v” Ξ ¼Ó R = Rot[k,Ξ]. Ta Ÿ· biÕt : 0 0 1 0R = Rot(y,900).Rot(z,900) = 1 0 0 0 0 1 0 0 0 0 0 1

Ta cã cosΞ = (nx + Oy + az - 1) / 2 = (0 + 0 + 0 - 1) / 2 = -1 / 2

TS. PhÂčm §šng Ph−íc

Page 20: Robot cong nghiep

Robot c«ng nghiÖp 19

sinΞ = 12

(O - a ) + (a - n ) + (n - O ) z y2

x z2

y x2

= 12

(1 - 0) + (1 - 0) + (1 - 0) = 32

2 2 2

⇒ tgΞ = − 3 v” Ξ = 1200

Theo (2.12), ta cĂŁ :

kx = ky = kz = +++

=0 1 21 1 2

13

//

VËy : R = Rot(y,900).Rot(z,900) = Rot(k, 1200); víi :

r r r rk 1

3i 1

3j 1

3k= + +

H×nh 2.8 : T×m gĂŁc quay v” trĂŽc quay t−¬ng ¼−¬ng 1/ 3

1/ 3

1/ 3k

O

1200

y

z

x

2.3.5. PhÐp quay Euler :

TrÂȘn thĂčc tÕ, viÖc ¼Þnh h−íng th−ĂȘng l” kÕt qu¶ cña phÐp quay xung quanh cžc trĂŽc x, y, z . PhÐp quay Euler m« t¶ kh¶ nšng ¼Þnh h−íng b»ng cžch :

Quay mĂ©t gĂŁc Ί xung quanh trĂŽc z, Quay tiÕp mĂ©t gĂŁc Ξ xung quanh trĂŽc y mĂ­i, Ÿã l” y’, cuĂši cĂŻng quay mĂ©t gĂŁc ψ quanh trĂŽc z mĂ­i, Ÿã l” z’’ (H×nh 2.9).

H×nh 2.9 : PhÐp quay Euler

x

y

z z’z’’z’’’

y’y’’

y’’’

x’ x’’ x’’’

Ξ

Κ

ΚΚ

Ξ

Ξ

Ί

Ί

Ί

Ta biÓu diÔn phÐp quay Euler b»ng cžch nh©n ba ma trËn quay vĂ­i nhau : Euler (Ί,Ξ,ψ) = Rot(z, Ί) Rot(y, Ξ) Rot(z, ψ) (2.14)

TS. PhÂčm §šng Ph−íc

Page 21: Robot cong nghiep

Robot c«ng nghiÖp 20

NĂŁi chung, kÕt qu¶ cña phÐp quay phĂŽ thuĂ©c chÆt chÎ v”o thĂž tĂč quay, tuy nhiÂȘn , Ă« phÐp quay Euler, nÕu thĂčc hiÖn theo thĂž tĂč ng−üc lÂči, nghÜa l” quay gĂŁc ψ quanh z rĂ„i tiÕp ¼Õn quay gĂŁc Ξ quanh y v” cuĂši cĂŻng quay gĂŁc Ί quanh z cĂČng ¼−a ¼Õn kÕt qu¶ t−¬ng tĂč (XÐt trong cĂŻng hÖ qui chiÕu).

cosΊ -sinΊ 0 0 CosΞcosψ -CosΞ sinψ sinΞ 0 = sinΊ cosΊ 0 0 sinψ cosψ 0 0 0 0 1 0 -sinΞ cosψ sinΞ sinψ CosΞ 0 0 0 0 1 0 0 0 1

cosΊCosΞcosψ - sinΊsinψ -cosΊCosΞsinψ - sinΊcosψ cosΊsinΞ 0 = sinΊCosΞcosψ + cosΊsinψ -sinΊCosΞsinψ + cosΊcosψ sinΊsinΞ 0 -sinΞ cosψ sinΞ sinψ cosΞ 0 0 0 0 1

(2.15)

CosΞ 0 sinΞ 0 cosψ -sinψ 0 0 Euler (Ί,Ξ,ψ) = Rot(z, Ί) 0 1 0 0 sinψ cosψ 0 0

-sinΞ 0 CosΞ 0 0 0 1 0 0 0 0 1 0 0 0 1

2.3.6. PhÐp quay Roll-Pitch-Yaw : MĂ©t phÐp quay ¼Þnh h−íng khžc cĂČng th−ĂȘng ¼−üc sö dĂŽng l” phÐp quay Roll-Pitch v”

Yaw. Ta tâˆ’Ă«ng t−üng, gŸn hÖ toÂč Ÿé xyz lÂȘn th©n mĂ©t con t”u. DĂ€c theo th©n t”u l” trĂŽc z, Roll l” chuyÓn Ÿéng lŸc cña th©n t”u, t−¬ng ¼−¬ng vĂ­i viÖc quay th©n t”u mĂ©t gĂŁc Ί quanh trĂŽc z. Pitch l” sĂč bĂ„ng bÒnh, t−¬ng ¼−¬ng vĂ­i quay mĂ©t gĂŁc Ξ xung quanh trĂŽc y v” Yaw l” sĂč lÖch h−íng, t−¬ng ¼−¬ng vĂ­i phÐp quay mĂ©t gĂŁc ψ xung quanh trĂŽc x (H×nh 2.10)

z

y

x

Th©n t”u

YawΚ

Roll Ί

Pitch Ξ

Cžc phÐp quay žp dĂŽng cho kh©u chÊp h”nh cuĂši cña robot nh− h×nh 2.11. Ta xžc ¼Þnh thĂž tĂč quay v” biÓu diÔn phÐp quay nh− sau :

H×nh 2.10: PhÐp quay Roll-Pitch-Yaw

RPY(Ί,Ξ,ψ)=Rot(z,Ί)Rot(y,Ξ)Rot(x, ψ) (2.16)

Yaw, ψ

y

z

Pitch, Ξ

Roll, Ί

x

H×nh 2.11 : Cžc gĂŁc quay Roll-Pitch v” Yaw cña b”n tay Robot. nghÜa l”, quay mĂ©t gĂŁc ψ quanh trĂŽc x, tiÕp theo l” quay mĂ©t gĂŁc Ξ quanh trĂŽc y v” sau Ÿã quay mĂ©t gĂŁc Ί quanh truc z.

TS. PhÂčm §šng Ph−íc

Page 22: Robot cong nghiep

Robot c«ng nghiÖp 21

ThĂčc hiÖn phÐp nh©n cžc ma trËn quay, cžc chuyÓn vÞ Roll, Pitch v” Yaw ¼−üc biÓu thÞ nh− sau :

cosΞ 0 sinΞ 0 1 0 0 0 0 1 0 0 0 cosψ -sinψ 0

RPY(Ί,Ξ,ψ)=Rot(z,Ί) -sinΞ 0 cosΞ 0 0 sinψ cosψ 0

0 0 0 1 0 0 0 1

cosΊ -sinΊ 0 0 cosΞ sinΞsinψ sinΞcosψ 0 = sinΊ cosΊ 0 0 0 cosψ -sinψ 0 0 0 1 0 -sinΞ cosΞsinψ cosΞ cosψ 0 0 0 0 1 0 0 0 1 cosΊcosΞ cosΊsinΞsinψ - sinΊcosψ cosΊsinΞcosψ + sinΊsinψ 0

= sinΊcosΞ sinΊsinΞsinψ +cosΊcosψ sinΊsinΞcosψ - cosΊsinψ 0 -sinΞ cosΞ sinψ cosΞ cosψ 0 0 0 0 1

(2.17) 2.4. BiÕn ŸÊi hÖ toÂč Ÿé v” mĂši quan hÖ giĂ·a cžc hÖ toÂč Ÿé biÕn ŸÊi : 2.4.1 BiÕn ŸÊi hÖ toÂč Ÿé :

Gi¶ sö cÇn tÞnh tiÕn gĂšc toÂč Ÿé §Ò cžt O(0, 0, 0) theo mĂ©t vectÂŹ dÉn r r r rh = 4i - 3j + 7k (h×nh 2.12) . KÕt qu¶ cña phÐp biÕn ŸÊi l” :

1 0 0 4 0 4

OT = 0 1 0 -3 0 = -3 0 0 1 7 0 7 0 0 0 1 1 1

NghÜa l” gĂšc ban ¼Çu cĂŁ toÂč Ÿé O(0, 0, 0) Ÿ· chuyÓn ŸÊi ¼Õn gĂšc mĂ­i OT

cĂŁ toÂč Ÿé (4, -3, 7) so vĂ­i hÖ toÂč Ÿé cĂČ.

yT

xT

OT

zT z

y

x

O

7

-3

4

H×nh 2.12 : PhÐp biÕn ŸÊi tÞnh tiÕn hÖ toÂč Ÿé

Tuy nhiÂȘn trong phÐp biÕn ŸÊi nÇy cžc trĂŽc toÂč Ÿé cña OT vÉn song song v” ŸÄng h−íng vĂ­i cžc trĂŽc toÂč Ÿé cña O.

TS. PhÂčm §šng Ph−íc

Page 23: Robot cong nghiep

Robot c«ng nghiÖp 22

NÕu ta tiÕp tĂŽc thĂčc hiÖn cžc phÐp biÕn ŸÊi quay :

Rot(y,90o)Rot(z,90o).OT

ta sÏ cĂŁ mĂ©t hÖ toÂč Ÿé ho”n to”n mĂ­i, cĂŽ thÓ tÂči gĂšc toÂč Ÿé mĂ­i (4,-3,7) khi cho hÖ OT quay quanh z mĂ©t gĂŁc 900 (chiÒu quay d−¬ng qui −íc l” ng−üc chiÒu kim ŸÄng hĂ„), ta cĂŁ :

Rot(z,900) Ta tiÕp tĂŽc quay hÖ OT quanh truc y (trĂŽc y cña hÖ toÂč Ÿé gĂšc ) mĂ©t gĂŁc 900, Ta cĂŁ :

Rot(y,900)

y'T

OTx'T

z'T

z"TOT

y''T

x''T

yT

xT

OT

90o

zT

y'T

OT x'T

z'T

90oy

VÝ dĂŽ trÂȘn Ÿ©y ta Ÿ· chĂ€n HÖ tÂčo Ÿé cÂŹ sĂ« l”m hÖ qui chiÕu v” thĂž tĂč thĂčc hiÖn cžc phÐp biÕn ŸÊi l” tĂ” Ph¶i sang Trži. NÕu thĂčc hiÖn cžc phÐp biÕn ŸÊi theo thĂž tĂč ng−üc lÂči tĂ” Trži sang Ph¶i th× hÖ qui chiÕu ¼−üc chĂ€n l” cžc hÖ toÂč Ÿé trung gian. XÐt lÂči vÝ dĂŽ trÂȘn :

Rot(y,90o)Rot(z,90o).OT

yT

xT

OT90o

zT y'T

O'Tz'T

Rot(y,90o) x'T Ta tiÕp tĂŽc quay hÖ O'T quanh truc z (B©y giĂȘ l” trĂŽc z'T cña hÖ toÂč Ÿé mĂ­i) mĂ©t gĂŁc 900 :

z"TO''T

y''T

x''T

y'T

x'Tz'T

O'T90o Rot(z',90o) Nh− vËy kÕt qu¶ cña hai ph−¬ng phžp quay l” giĂšng nhau, nh−ng vÒ Ăœ nghÜa vËt lĂœ th× khžc nhau.

2.4.2. Quan hÖ giĂ·a cžc hÖ toÂč Ÿé biÕn ŸÊi :

Gi¶ sö ta cĂŁ 3 hÖ toÂč Ÿé A, B, C; HÖ B cĂŁ quan hÖ vĂ­i hÖ A qua phÐp biÕn ŸÊi v”

hÖ C cĂŁ quan hÖ vĂ­i hÖ B qua phÐp biÕn ŸÊi . Ta cĂŁ ÂźiÓm P trong hÖ C kĂœ hiÖu P

ABT/

BcT/ C, ta t×m

mĂši quan hÖ cña ÂźiÓm P trong hÖ A, tĂžc l” t×m PA (H×nh 2.13) :

TS. PhÂčm §šng Ph−íc

Page 24: Robot cong nghiep

Robot c«ng nghiÖp 23

H×nh 2.13 : Quan hÖ giĂ·a cžc hÖ toÂč Ÿé biÕn ŸÊi.

pA

pC

zC

xC

yCxB

zB

yBxA

zA

yA

C

B

A

ChĂłng ta cĂŁ thÓ biÕn ŸÊi pC th”nh pB nh− sau : pB = pB

cT/ C, (2.18) Sau Ÿã biÕn ŸÊi pB th”nh pA nh− sau : pA = pA

BT/ B, (2.19) KÕt hĂźp (2.18) v” (2.19) ta cĂŁ : (2.20) cC

BB

AA pTTp =

Qua vÝ dĂŽ trÂȘn ta thÊy cĂŁ thÓ m« t¶ mĂši quan hÖ giĂ·a hÖ toÂč Ÿé gŸn trÂȘn ÂźiÓm tžc Ÿéng

cuĂši vĂ­i hÖ tĂ€a Ÿé cÂŹ b¶n, th«ng qua mĂši quan hÖ cña cžc hÖ toÂč Ÿé trung gian gŸn trÂȘn cžc kh©u cña robot, b»ng ma trËn T nh− h×nh 2.14.

O0

O1

O2O3

T4O4

B”n tay

y

z

x

H×nh 2.14 : HÖ toÂč Ÿé cÂŹ b¶n (base) v” cžc hÖ toÂč Ÿé trung gian cña Robot.

2.5. M« t¶ mĂ©t vËt thÓ : Cžc vËt thÓ l” ŸÚi t−üng l”m viÖc cña robot rÊt Âźa dÂčng v” phong phĂł, tuy nhiÂȘn cĂŁ thÓ

dĂča v”o nhĂ·ng ¼Æc ÂźiÓm h×nh hĂ€c ¼Ó m« t¶ chĂłng. Ta cĂŁ thÓ chia h×nh džng vËt thÓ th”nh 3 nhĂŁm chÝnh sau :

NhĂŁm vËt thÓ trßn xoay (Rotative) NhĂŁm vËt thÓ cĂŁ gĂŁc cÂčnh (Prismatic) NhĂŁm vËt thÓ cĂŁ cÊu trĂłc hĂŠn hĂźp (Kombination)

NhĂŁm vËt thÓ trßn xoay cĂŁ cžc giž trÞ ¼Æc tr−ng l” toÂč Ÿé t©m v” bžn kÝnh mÆt cong.

NhĂŁm vËt thÓ cĂŁ gĂŁc cÂčnh ¼Æc tr−ng b»ng toÂč Ÿé cña cžc ÂźiÓm giĂ­i hÂčn. NhĂŁm cßn lÂči cĂŁ cžc giž trÞ ¼Æc tr−ng hĂŠn hĂźp. Tuy nhiÂȘn, ŸÚi vĂ­i hoÂčt Ÿéng cÇm nŸm ŸÚi t−üng v” quž tr×nh vËn Ÿéng cña robot viÖc m« t¶ vËt thÓ cÇn ph¶i gŸn liÒn vĂ­i cžc phÐp biÕn ŸÊi thuÇn nhÊt. Ta xÐt vÝ dĂŽ sau Ÿ©y : Cho mĂ©t vËt h×nh lšng trĂŽ ¼Æt trong hÖ toÂč Ÿé chuÈn O(xyz) nh− h×nh 2.15.

TS. PhÂčm §šng Ph−íc

Page 25: Robot cong nghiep

Robot c«ng nghiÖp 24

Ta thĂčc hiÖn cžc phÐp biÕn ŸÊi sau : H = Trans(4,0,0)Rot(y,900)Rot(z,900) VĂ­i vÞ trÝ cña vËt thÓ, ta cĂŁ ma trËn toÂč Ÿé cña 6 ÂźiÓm ¼Æc tr−ng m« t¶ nĂŁ l” :

1 -1 -1 1 1 -1 0 0 0 0 4 4 0 0 2 2 0 0 1 1 1 1 1 1

Sau khi thĂčc hiÖn cžc phÐp biÕn ŸÊi : - Quay vËt thÓ quanh trĂŽc z mĂ©t gĂŁc 900 (H×nh 2.16),

- Cho vËt thÓ quay quanh trĂŽc y mĂ©t gĂŁc 900 (H×nh 2.17), - TiÕp tĂŽc tÞnh tiÕn vËt thÓ dĂ€c theo trĂŽc x mĂ©t ÂźoÂčn b»ng 4 Ÿn vÞ (h×nh 2.18) ta xžc ¼Þnh ¼−üc ma trËn toÂč Ÿé cžc ÂźiÓm giĂ­i hÂčn cña vËt thÓ Ă« vÞ trÝ Ÿ· ¼−üc biÕn ŸÊi nh− sau (cžc phÐp quay Ÿ· chĂ€n hÖ qui chiÕu l” hÖ toÂč Ÿé gĂšc) :

0 0 1 4 1 -1 -1 1 1 -1

H = 1 0 0 0 0 0 0 0 4 4 0 1 0 0 0 0 2 2 0 0 0 0 0 1 1 1 1 1 1 1

4 4 6 6 4 4 = 1 -1 -1 1 1 1 0 0 0 0 4 4 1 1 1 1 1 1

-1,4,0,1

-1,0,2,1

-1,0,0,1

1,4,0,1

1,0,2,1

1,0,0,1y

x

H×nh 2.15 : M« t¶ vËt thÓ

x

y

z

O

O

x

y

z

z

H×nh 2.17: Rot (y,900) Rot (z,900) H×nh 2.16 : Rot (z,900)

TS. PhÂčm §šng Ph−íc

Page 26: Robot cong nghiep

Robot c«ng nghiÖp 25

H = Trans(4,0,0)Rot (y,900)Rot (z,900)

O

y

z

x

H×nh 2.18: VÞ trÝ vËt thÓ sau khi biÕn ŸÊi 2.6. KÕt luËn : Cžc phÐp biÕn ŸÊi thuÇn nhÊt dĂŻng ¼Ó miÂȘu t¶ vÞ trÝ v” h−íng cña cžc hÖ toÂč Ÿé trong kh«ng gian. NÕu mĂ©t hÖ toÂč Ÿé ¼−üc gŸn liÒn vĂ­i ŸÚi t−üng th× vÞ trÝ v” h−íng cña chÝnh ŸÚi t−üng cĂČng ¼−üc m« t¶. Khi m« t¶ ŸÚi t−üng A trong mĂši quan hÖ vĂ­i ŸÚi t−üng B b»ng cžc phÐp biÕn ŸÊi thuÇn nhÊt th× ta cĂČng cĂŁ thÓ dĂča v”o Ÿã m« t¶ ng−üc lÂči mĂši quan hÖ cña B ŸÚi vĂ­i ŸÚi t−üng A. MĂ©t chuyÓn vÞ cĂŁ thÓ l” kÕt qu¶ liÂȘn tiÕp cña nhiÒu phÐp biÕn ŸÊi quay v” tÞnh tiÕn. Tuy nhiÂȘn ta cÇn l−u Ăœ ¼Õn thĂž tĂč cña cžc phÐp biÕn ŸÊi, nÕu thay ŸÊi thĂž tĂč thĂčc hiÖn cĂŁ thÓ dÉn ¼Õn cžc kÕt qu¶ khžc nhau.

B”i tËp ch−¬ng II : B”i 1 : Cho ÂźiÓm A biÓu diÔn bĂ«i vectÂŹ ÂźiÓm v=[ 2 4 1 1 ]T. TÞnh tiÕn ÂźiÓm A theo vectÂŹ dÉn h = [ 1 2 1 1 ]T, sau Ÿã tiÕp tĂŽc quay ÂźiÓm Ÿ· biÕn ŸÊi quanh trĂŽc x mĂ©t gĂŁc 900. Xžc ¼Þnh vectÂŹ biÓu diÔn ÂźiÓm A sau hai phÐp biÕn ŸÊi. B”i 2 : ViÕt ma trËn biÕn ŸÊi thuÇn nhÊt biÓu diÔn cžc phÐp biÕn ŸÊi sau : H = Trans(3,7,9)Rot(x,-900)Rot(z,900) B”i 3 : Cho ma trËn biÕn ŸÊi thuÇn nhÊt A, t×m ma trËn nghÞch ٦o A-1 v” kiÓm chĂžng.

0 1 0 -1A = 0 0 -1 2

-1 0 0 0 0 0 0 1

TS. PhÂčm §šng Ph−íc

Page 27: Robot cong nghiep

Robot c«ng nghiÖp 26

B”i 4 : H×nh vÏ 2-19 m« t¶ hÖ toÂč Ÿé {B} Ÿ· ¼−üc

quay Âźi mĂ©t gĂŁc 300 xung quanh trĂŽc zA, tÞnh tiÕn dĂ€c theo trĂŽc xA 4 Ÿn vÞ v” tÞnh tiÕn dĂ€c theo yA 3 Ÿn vÞ.

xByB

{B}

xA

yA

{A}

(a) M« t¶ mĂši qua hÖ cña {B} ŸÚi vĂ­i {A} : ATB ? (b) T×m mĂši quan hÖ ng−üc lÂči BTA ?

H×nh 2.19 : Quan hÖ {A} v” {B}

B”i 5 : Cho k = 13

(1, 1, 1)T, ξ = 900. T×m ma trËn R = Rot(k, ξ).

B”i 6 : Xžc ¼Þnh cžc gĂŁc quay Euler, v” cžc gĂŁc quay RPY khi biÕt ma trËn T6 :

1 0 0 0 T6 = 0 0 1 5

0 -1 0 3 0 0 0 1

B”i 7 : MĂ©t vËt thÓ ¼Æt trong mĂ©t hÖ toÂč Ÿé tham chiÕu ¼−üc xžc ¼Þnh bĂ«i phÐp biÕn ŸÊi :

0 1 0 -1UTP = 0 0 -1 2

-1 0 0 0 0 0 0 1

MĂ©t robot m” hÖ toÂč Ÿé chuÈn cĂŁ liÂȘn hÖ vĂ­i hÖ toÂč Ÿé tham chiÕu bĂ«i phÐp biÕn ŸÊi

1 0 0 1UTR = 0 1 0 5

0 0 1 9 0 0 0 1

ChĂłng ta muĂšn ¼Æt b”n tay cña robot lÂȘn vËt thÓ, Ÿã l” l”m cho hÖ tĂ€a Ÿé gŸn trÂȘn b”n tay trĂŻng vĂ­i hÖ toÂč Ÿé cña vËt thÓ. T×m phÐp biÕn ŸÊi RTH (biÓu diÔn mĂši quan hÖ giĂ·a b”n tay v” hÖ toÂč Ÿé gĂšc cña robot) ¼Ó thĂčc hiÖn ÂźiÒu nĂŁi trÂȘn.

TS. PhÂčm §šng Ph−íc

Page 28: Robot cong nghiep

Robot c«ng nghiÖp 27

Ch−¬ng III

ph−¬ng tr×nh Ÿéng hĂ€c cña robot

(Kinematic Equations) 3.1. DÉn nhËp : BÊt kĂș mĂ©t robot n”o cĂČng cĂŁ thÓ coi l” mĂ©t tËp hĂźp cžc kh©u (links) gŸn liÒn vĂ­i cžc khĂ­p (joints). Ta h·y ¼Æt trÂȘn mçi kh©u cña robot mĂ©t hÖ toÂč Ÿé. Sö dĂŽng cžc phÐp biÕn ŸÊi thuÇn nhÊt cĂŁ thÓ m« t¶ vÞ trÝ t−¬ng ŸÚi v” h−íng giĂ·a cžc hÖ toÂč Ÿé nÇy. Denavit. J. Ÿ· gĂ€i biÕn ŸÊi thuÇn nhÊt m« t¶ quan hÖ giĂ·a mĂ©t kh©u v” mĂ©t kh©u kÕ tiÕp l” mĂ©t ma trËn A. NĂŁi Ÿn gi¶n hÂŹn, mĂ©t ma trËn A l” mĂ©t m« t¶ biÕn ŸÊi thuÇn nhÊt bĂ«i phÐp quay v” phÐp tÞnh tiÕn t−¬ng ŸÚi giĂ·a hÖ toÂč Ÿé cña hai kh©u liÒn nhau. A1 m« t¶ vÞ trÝ v” h−íng cña kh©u ¼Çu tiÂȘn; A2 m« t¶ vÞ trÝ v” h−íng cña kh©u thĂž hai so vĂ­i kh©u thĂž nhÊt. Nh− vËy vÞ trÝ v” h−íng cña kh©u thĂž hai so vĂ­i hÖ toÂč Ÿé gĂšc ¼−üc biÓu diÔn bĂ«i ma trËn :

T2 = A1.A2 CĂČng nh− vËy, A3 m« t¶ kh©u thĂž ba so vĂ­i kh©u thĂž hai v” :

T3 = A1.A2.A3 ; v.v...

CĂČng theo Denavit, tÝch cña cžc ma trËn A ¼−üc gĂ€i l” ma trËn T, th−ĂȘng cĂŁ hai chØ sĂš: trÂȘn v” d−íi. ChØ sĂš trÂȘn chØ hÖ toÂč Ÿé tham chiÕu tĂ­i, bĂĄ qua chØ sĂš trÂȘn nÕu chØ sĂš Ÿã b»ng 0. ChØ sĂš d−íi th−ĂȘng dĂŻng ¼Ó chØ kh©u chÊp h”nh cuĂši. NÕu mĂ©t robot cĂŁ 6 kh©u ta cĂŁ :

T6 = A1.A2.A3.A4.A5.A6 (3.1)

T6 m« t¶ mĂši quan hÖ vÒ h−íng v” vÞ trÝ cña kh©u chÊp h”nh cuĂši ŸÚi vĂ­i hÖ toÂč Ÿé gĂšc. MĂ©t robot 6 kh©u cĂŁ thÓ cĂŁ 6 bËc tĂč do v” cĂŁ thÓ ¼−üc ¼Þnh vÞ trÝ v” ¼Þnh h−íng trong tr−ĂȘng vËn Ÿéng cña nĂŁ (range of motion). Ba bËc tĂč do xžc ¼Þnh vÞ trÝ thuÇn tuĂœ v” ba bËc tĂč do khžc xžc ¼Þnh h−íng mong muĂšn. T6 sÏ l” ma trËn tr×nh b”y c¶ h−íng v” vÞ trÝ cña robot. H×nh 3.1 m« t¶ quan hÖ Ÿã vĂ­i b”n tay mžy. Ta ¼Æt gĂšc toÂč Ÿé cña hÖ m« t¶ tÂči ÂźiÓm giĂ·a cña cžc ngĂŁn tay. GĂšc toÂč Ÿé nÇy ¼−üc m« t¶ bĂ«i vectÂŹ p (xžc ¼Þnh vÞ trÝ cña b”n tay). Ba vectÂŹ Ÿn vÞ m« t¶ h−íng cña b”n tay ¼−üc xžc ¼Þnh nh− sau :

np

a o

H×nh 3.1 : Cžc vectÂŹ ¼Þnh vÞ trÝ v” ¼Þnh h−íng cña b”n tay mžy

TS. PhÂčm §šng Ph−íc

Page 29: Robot cong nghiep

Robot c«ng nghiÖp 28

∗ VectÂŹ cĂŁ h−íng m” theo Ÿã b”n tay sÏ tiÕp cËn ¼Õn ŸÚi t−üng, gĂ€i l” vectÂŹ a (approach).

∗ VectÂŹ cĂŁ h−íng m” theo Ÿã cžc ngĂŁn tay cña b”n tay nŸm v”o nhau khi cÇm nŸm ŸÚi t−üng, gĂ€i l” vectÂŹ o (Occupation).

∗ VectÂŹ cuĂši cĂŻng l” vectÂŹ phžp tuyÕn n (normal), do vËy ta cĂŁ : ax o= n rrr

ChuyÓn vÞ T6 nh− vËy sÏ bao gĂ„m cžc phÇn tö :

nx Ox ax px T6 = ny Oy ay py (3.2)

nz Oz az pz 0 0 0 1

TĂŠng qužt, ma trËn T6 cĂŁ thÓ biÓu diÔn gĂ€n hÂŹn nh− sau :

Ma trËn ¼Þnh h−íng R Vect¬ vÞ trÝ p (3.3)

T6 = 0 0 0 1

Ma trËn R cĂŁ kÝch th−íc 3x3, l” ma trËn trĂčc giao biÓu diÔn h−íng cña b”n kÑp (kh©u

chÊp h”nh cuĂši) ŸÚi vĂ­i hÖ toÂč Ÿé cÂŹ b¶n. ViÖc xžc ¼Þnh h−íng cña kh©u chÊp h”nh cuĂši cßn cĂŁ thÓ thĂčc hiÖn theo phÐp quay Euler hay phÐp quay Roll, Pitch, Yaw. VectÂŹ ÂźiÓm pr cĂŁ kÝch th−íc 3x1, biÓu diÔn mĂši quan hÖ tĂ€a Ÿé vÞ trÝ cña cña gĂšc hÖ tĂ€a Ÿé gŸn trÂȘn kh©u chÊp h”nh cuĂši ŸÚi vĂ­i hÖ toÂč Ÿé cÂŹ b¶n. 3.2. BĂ© th«ng sĂš Denavit-Hartenberg (DH) : MĂ©t robot nhiÒu kh©u cÊu th”nh tĂ” cžc kh©u nĂši tiÕp nhau th«ng qua cžc khĂ­p Ÿéng. GĂšc chuÈn (Base) cña mĂ©t robot l” kh©u sĂš 0 v” kh«ng tÝnh v”o sĂš cžc kh©u. Kh©u 1 nĂši vĂ­i kh©u chuÈn bĂ«i khĂ­p 1 v” kh«ng cĂŁ khĂ­p Ă« ¼Çu mĂłt cña kh©u cuĂši cĂŻng. BÊt kĂș kh©u n”o cĂČng ¼−üc ¼Æc tr−ng bĂ«i hai kÝch th−íc :

§é d”i phžp tuyÕn chung : an . GĂŁc giĂ·a cžc trĂŽc trong mÆt phÂŒng vu«ng gĂŁc vĂ­i an : αn.

a

KhĂ­p n KhĂ­p n+1

αn

Kh©u n

H×nh 3.5 : ChiÒu d”i v” gĂŁc xoŸn cña 1 kh©u. Th«ng th−ĂȘng, ng−ĂȘi ta gĂ€i an l” chiÒu d”i v” αn l” gĂŁc xoŸn cña kh©u (H×nh 3.5). PhĂŠ biÕn l” hai kh©u liÂȘn kÕt vĂ­i nhau Ă« chÝnh trĂŽc cña khĂ­p (H×nh 3.6).

TS. PhÂčm §šng Ph−íc

Page 30: Robot cong nghiep

Robot c«ng nghiÖp 29

Ξn+1

Kh©u n+1

KhĂ­p n-1 KhĂ­p n+1KhĂ­p n

xn

an

zn

On

Kh©u nKh©u n-1

dn zn-1xn-1

Ξn

αn

ΞnΞn-1

Kh©u n-2

H×nh 3.6 : Cžc th«ng sĂš cña kh©u : Ξ, d, a v” α. Mçi trĂŽc sÏ cĂŁ hai phžp tuyÕn vĂ­i nĂŁ, mçi phžp tuyÕn dĂŻng cho mçi kh©u (tr−íc v” sau mĂ©t khĂ­p). VÞ trÝ t−¬ng ŸÚi cña hai kh©u liÂȘn kÕt nh− thÕ ¼−üc xžc ¼Þnh bĂ«i dn l” kho¶ng cžch giĂ·a cžc phžp tuyÕn Âźo dĂ€c theo trĂŽc khĂ­p n v” Ξn l” gĂŁc giĂ·a cžc phžp tuyÕn Âźo trong mÆt phÂŒng vu«ng gĂŁc vĂ­i trĂŽc. dn v” Ξn th−ĂȘng ¼−üc gĂ€i l” kho¶ng cžch v” gĂŁc giĂ·a cžc kh©u. §Ó m« t¶ mĂši quan hÖ giĂ·a cžc kh©u ta gŸn v”o mçi kh©u mĂ©t hÖ toÂč Ÿé. NguyÂȘn tŸc chung ¼Ó gŸn hÖ tĂ€a Ÿé lÂȘn cžc kh©u nh− sau :

+ GĂšc cña hÖ toÂč Ÿé gŸn lÂȘn kh©u thĂž n ¼Æt tÂči giao ÂźiÓm cña phžp tuyÕn an vĂ­i trĂŽc khĂ­p thĂž n+1. Tr−ĂȘng hĂźp hai trĂŽc khĂ­p cŸt nhau, gĂšc toÂč Ÿé sÏ ¼Æt tÂči chÝnh ÂźiÓm cŸt Ÿã. NÕu cžc trĂŽc khĂ­p song song vĂ­i nhau, gĂšc toÂč Ÿé ¼−üc chĂ€n trÂȘn trĂŽc khĂ­p cña kh©u kÕ tiÕp, tÂči ÂźiÓm thÝch hĂźp.

+ TrĂŽc z cña hÖ toÂč Ÿé gŸn lÂȘn kh©u thĂž n ¼Æt dĂ€c theo trĂŽc khĂ­p thĂž n+1. + TrĂŽc x th−ĂȘng ¼−üc ¼Æt dĂ€c theo phžp tuyÕn chung v” h−íng tĂ” khĂ­p n ¼Õn n+1.

Trong tr−ĂȘng hĂźp cžc trĂŽc khĂ­p cŸt nhau th× trĂŽc x chĂ€n theo tÝch vectÂŹ . 1-nn zx z rr

Tr−ĂȘng hĂźp khĂ­p quay th× Ξn l” cžc biÕn khĂ­p, trong tr−ĂȘng hĂźp khĂ­p tÞnh tiÕn th× dn l” biÕn khĂ­p v” an b»ng 0. Cžc th«ng sĂš an, αn, dn v” Ξn ¼−üc gĂ€i l” bĂ© th«ng sĂš DH. VÝ dĂŽ 1 : XÐt mĂ©t tay mžy cĂŁ hai kh©u phÂŒng nh− h×nh 3.7 :

Ξ1

Ξ2

a1 a2

O0z1

z2

x1

y1

y2

O1

O2

z0x0

y0

x2

H×nh 3.7 : Tay mžy cĂŁ hai kh©u phÂŒng (vÞ trÝ bÊt kĂș).

TS. PhÂčm §šng Ph−íc

Page 31: Robot cong nghiep

Robot c«ng nghiÖp 30

Ta gŸn cžc hÖ toÂč Ÿé lÂȘn cžc kh©u nh− h×nh vÏ : trĂŽc z0, z1 v” z2 vu«ng gĂŁc vĂ­i tĂȘ giÊy. HÖ toÂč Ÿé cÂŹ sĂ« l” O0x0y0z0, chiÒu cña x0 h−íng tĂ” O0 ¼Õn O1. Sau khi thiÕt lËp hÖ toÂč Ÿé cÂŹ sĂ«, HÖ toÂč Ÿé o1x1y1z1 cĂŁ h−íng nh− h×nh vÏ, O1 ¼Æt tÂči t©m trĂŽc khĂ­p 2. HÖ toÂč Ÿé O2x2y2x2 cĂŁ gĂšc O2 ¼Æt Ă« ÂźiÓm cuĂši cña kh©u 2.

B¶ng th«ng sĂš Denavit-Hartenbert cña tay mžy nÇy nh− sau :

Kh©u Ξi αiai di

1 Ξ1* 0 a1 0

2 Ξ2* 0 a2 0

Trong Ÿã Ξi l” cžc biÕn khĂ­p (dĂŻng dÊu * ¼Ó kĂœ hiÖu cžc biÕn khĂ­p). VÝ dĂŽ 2 : Xem sÂŹ ŸÄ robot SCARA cĂŁ 4 kh©u nh− h×nh 3.8 : §©y l” robot cĂŁ cÊu h×nh kiÓu RRTR, b”n tay cĂŁ chuyÓn Ÿéng xoay xung quanh trĂŽc

ŸÞng. HÖ toÂč Ÿé gŸn lÂȘn cžc kh©u nh− h×nh vÏ.

H×nh 3.8 : Robot SCARA v” cžc hÖ toÂč Ÿé (vÞ trÝ ban ¼Çu).

O0

Ξ1x0

x1d3

x2

x3

x

z3, z4

Ξ2

Ξ4

O3

O4

z0 z1

z2a1

a2

O1O2

d4

§Úi vĂ­i tay mžy nÇy cžc trĂŽc khĂ­p ¼Òu song song nhau, ¼Ó tiÖn lĂźi tÊt c¶ cžc gĂšc toÂč Ÿé ¼Æt tÂči t©m cžc trĂŽc khĂ­p. TrĂŽc x0 n»m trong mÆt phÂŒng tĂȘ giÊy. Cžc hÖ toÂč Ÿé khžc nh− h×nh vÏ. B¶ng th«ng sĂš DH cña robot SCARA nh− sau :

Kh©u Ξi αi

ai di

1 Ξ1* 0 a1 0

2 Ξ2* 1800 a2 0

3 0 0 0 d3*

4 Ξ4* 0 0 d4

* : C¾c biÕn khíp.

3.3. §Æc tr−ng cña cžc ma trËn A :

TrÂȘn cÂŹ sĂ« cžc hÖ toÂč Ÿé Ÿ· Ên ¼Þnh cho tÊt c¶ cžc kh©u liÂȘn kÕt cña robot, ta cĂŁ thÓ

thiÕt lËp mĂši quan hÖ giĂ·a cžc hÖ toÂč Ÿé nĂši tiÕp nhau (n-1), (n) bĂ«i cžc phÐp quay v” tÞnh tiÕn sau Ÿ©y :

Quay quanh zn-1 mĂ©t gĂŁc Ξn TÞnh tiÕn dĂ€c theo zn-1 mĂ©t kho¶ng dn TÞnh tiÕn dĂ€c theo xn-1 = xn mĂ©t ÂźoÂčn an Quay quanh xn mĂ©t gĂŁc xoŸn αn

TS. PhÂčm §šng Ph−íc

Page 32: Robot cong nghiep

Robot c«ng nghiÖp 31

BĂšn phÐp biÕn ŸÊi thuÇn nhÊt nÇy thÓ hiÖn quan hÖ cña hÖ toÂč Ÿé thuĂ©c kh©u thĂž n so

vĂ­i hÖ toÂč Ÿé thuĂ©c kh©u thĂž n-1 v” tÝch cña chĂłng ¼−üc gĂ€i l” ma trËn A : An = Rot(z,Ξ) Trans(0,0,d) Trans(a,0,0) Rot(x,α) (3.4)

cosΞ -sinΞ 0 0 1 0 0 a 1 0 0 0

An = sinΞ cosΞ 0 0 0 1 0 0 0 cosα -sinα 0 0 0 1 0 0 0 1 d 0 sinα cosα 0 0 0 0 1 0 0 0 1 0 0 0 1

cosΞ -sinΞ cosα sinΞ sinα a cosΞ

An = sinΞ cosΞ cosα -cosΞ sinα a sinΞ (3.5) 0 sinα cosα d 0 0 0 1

§Úi vĂ­i khĂ­p tÞnh tiÕn (a = 0 v” Ξi = 0) th× ma trËn A cĂŁ dÂčng :

1 0 0 0 An = 0 cosα - sinα 0 (3.6)

0 sinα cosα d 0 0 0 1

§Úi vĂ­i mĂ©t kh©u Âźi theo mĂ©t khĂ­p quay th× d, a v” α l” h»ng sĂš. Nh− vËy ma trËn A cña khĂ­p quay l” mĂ©t h”m sĂš cña biÕn khĂ­p Ξ. §Úi vĂ­i mĂ©t kh©u Âźi theo mĂ©t khĂ­p tÞnh tiÕn th× Ξ, α l” h»ng sĂš. Ma trËn A cña khĂ­p tÞnh tiÕn l” mĂ©t h”m sĂš cña biÕn sĂš d. NÕu cžc biÕn sĂš ¼−üc xžc ¼Þnh th× giž trÞ cña cžc ma trËn A theo Ÿã cĂČng ¼−üc xžc ¼Þnh. 3.4. Xžc ¼Þnh T6 theo cžc ma trËn An : Ta Ÿ· biÕt : T6 = A1A2A3A4A5A6

Trong Ÿã T6 ¼−üc miÂȘu t¶ trong hÖ toÂč Ÿé gĂšc (hÖ toÂč Ÿé gŸn vĂ­i kh©u cÂŹ b¶n cĂš ¼Þnh cña robot). NÕu m« t¶ T6 theo cžc hÖ toÂč Ÿé trung gian thĂž n-1 th× :

= 6

1 n T− Aii n=∏

6

X

Z

T6E

A

OR

Trong tr−ĂȘng hĂźp tĂŠng qužt, khi xÐt quan hÖ cña robot vĂ­i cžc thiÕt bÞ khžc, nÕu hÖ toÂč Ÿé cÂŹ b¶n cña robot cĂŁ liÂȘn hÖ vĂ­i mĂ©t hÖ toÂč Ÿé n”o Ÿã bĂ«i phÐp biÕn ŸÊi Z, Kh©u chÊp h”nh cuĂši lÂči cĂŁ gŸn mĂ©t c«ng cĂŽ, cĂŁ quan hÖ vĂ­i vËt thÓ bĂ«i phÐp biÕn ŸÊi E (h×nh 3.9) th× vÞ trÝ v” h−íng cña ÂźiÓm cuĂši cña c«ng cĂŽ, kh¶o sžt Ă« hÖ toÂč Ÿé tham chiÕu m« t¶ bĂ«i X sÏ ¼−üc xžc ¼Þnh bĂ«i :

H×nh 3.9 : VËt thÓ v” Robot

X= Z T6E

TS. PhÂčm §šng Ph−íc

Page 33: Robot cong nghiep

Robot c«ng nghiÖp 32

Quan hÖ nÇy ¼−üc thÓ hiÖn trÂȘn tožn ŸÄ sau :

A1 A2 A3 A4 A5

65T

64T

63T

62T

61T

6T

OR OR

Z E XAO0

H×nh 3.10 : Tožn ŸÄ chuyÓn vÞ cña robot. TĂ” tožn ŸÄ nÇy ta cĂŁ thÓ rĂłt ra : T6 = Z-1 X E-1

(Z-1 v” E-1 l” cžc ma trËn nghÞch ٦o). 3.5. Tr×nh tĂč thiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot : §Ó thiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot, ta tiÕn h”nh theo cžc b−íc sau : 1. ChĂ€n hÖ toÂč Ÿé cÂŹ sĂ«, gŸn cžc hÖ toÂč Ÿé mĂ« rĂ©ng lÂȘn cžc kh©u. ViÖc gŸn hÖ toÂč Ÿé lÂȘn cžc kh©u Ÿãng vai trß rÊt quan trĂ€ng khi xžc lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot, th«ng th−ĂȘng Ÿ©y cĂČng l” b−íc khĂŁ nhÊt. NguyÂȘn tŸc gŸn hÖ toÂč Ÿé lÂȘn cžc kh©u Ÿ· ¼−üc tr×nh b”y mĂ©t cžch tĂŠng qužt trong phÇn 3.5. Trong thĂčc tÕ, cžc trĂŽc khĂ­p cña robot th−ĂȘng song song hoÆc vu«ng gĂŁc vĂ­i nhau, ŸÄng thĂȘi th«ng qua cžc phÐp biÕn ŸÊi cña ma trËn A ta cĂŁ thÓ xžc ¼Þnh cžc hÖ toÂč Ÿé gŸn trÂȘn cžc kh©u cña robot theo tr×nh tĂč sau : + Gi¶ ¼Þnh mĂ©t vÞ trÝ ban ¼Çu(♩) (Home Position) cña robot. + ChĂ€n gĂšc toÂč Ÿé O0, O1, ... + Cžc trĂŽc zn ph¶i chĂ€n cĂŻng ph−¬ng vĂ­i trĂŽc khĂ­p thĂž n+1. + ChĂ€n trĂŽc xn l” trĂŽc quay cña zn th”nh zn+1 v” gĂŁc cña zn vĂ­i zn+1 chÝnh l” αn+1. NÕu zn v” zn+1 song song hoÆc trĂŻng nhau th× ta cĂŁ thÓ cšn cĂž nguyÂȘn tŸc chung hay chĂ€n xn theo xn+1. + Cžc hÖ toÂč Ÿé Oxyz ph¶i tu©n theo qui tŸc b”n tay ph¶i. + Khi gŸn hÖ toÂč Ÿé lÂȘn cžc kh©u, ph¶i tu©n theo cžc phÐp biÕn ŸÊi cña ma trËn An. Ÿã l” bĂšn phÐp biÕn ŸÊi : An = Rot(z,Ξ) Trans(0,0,d) Trans(a,0,0) Rot(x,α). NghÜa l” ta coi hÖ toÂč Ÿé thĂž n+1 l” biÕn ŸÊi cña hÖ toÂč Ÿé thĂž n; cžc phÐp quay v” tÞnh tiÕn cña biÕn ŸÊi nÇy ph¶i l” mĂ©t trong cžc phÐp biÕn ŸÊi cña An, cžc th«ng sĂš DH cĂČng ¼−üc xžc ¼Þnh dĂča v”o cžc phÐp biÕn ŸÊi nÇy. Trong quž tr×nh gŸn hÖ tĂ€a Ÿé lÂȘn cžc kh©u, nÕu xuÊt hiÖn phÐp quay cña trĂŽc zn ŸÚi vĂ­i zn-1 quanh trĂŽc yn-1 th× vÞ trÝ ban ¼Çu cña robot Ÿ· gi¶ ¼Þnh l” kh«ng Ÿóng, ta cÇn chĂ€n lÂči vÞ trÝ ban ¼Çu khžc cho robot. 2. LËp b¶ng th«ng sĂš DH (Denavit Hartenberg). 3. DĂča v”o cžc th«ng sĂš DH xžc ¼Þnh cžc ma trËn An. 4. TÝnh cžc ma trËn T v” viÕt cžc ph−¬ng tr×nh Ÿéng hĂ€c cña robot.

(♩) VÞ trÝ ban ¼Çu l” vÞ trÝ m” cžc biÕn nhËn giž trÞ ban ¼Çu, th−ĂȘng b»ng 0.

TS. PhÂčm §šng Ph−íc

Page 34: Robot cong nghiep

Robot c«ng nghiÖp 33

VÝ dĂŽ sau Ÿ©y tr×nh b”y chi tiÕt cña cžc b−íc khi thiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot :

Cho mĂ©t robot cĂŁ ba kh©u, cÊu h×nh RRT nh− h×nh 3.11. H·y thiÕt lËp hÖ ph−¬ng tr×nh

Ÿéng hĂ€c cña robot. 1. GŸn hÖ toÂč Ÿé lÂȘn cžc kh©u : Ta gi¶ ¼Þnh vÞ trÝ ban ¼Çu v” chĂ€n gĂšc toÂč Ÿé O0 cña robot nh− h×nh 3.12. Cžc trĂŽc z ¼Æt cĂŻng ph−¬ng vĂ­i cžc trĂŽc khĂ­p. Ta thÊy trĂŽc z1 Ÿ· quay t−¬ng ŸÚi mĂ©t gĂŁc 900 so vĂ­i trĂŽc z0, Ÿ©y chÝnh l” phÐp quay quanh trĂŽc x0 mĂ©t gĂŁc α1 (phÐp biÕn ŸÊi Rot(x0,α1) trong biÓu thĂžc tÝnh An). NghÜa l” trĂŽc x0 vu«ng gĂŁc vĂ­i z0 v” z1. Ta chĂ€n chiÒu cña x0 tĂ” trži sang ph¶i th× gĂŁc quay α1=900 (chiÒu d−¬ng ng−üc chiÒu kim ŸÄng hĂ„). §Äng thĂȘi ta cĂČng thÊy gĂšc O1 Ÿ· tÞnh tiÕn mĂ©t ÂźoÂčn dĂ€c theo z0 , so vĂ­i O0, Ÿã chÝnh l” phÐp biÕn ŸÊi Trans(0,0,d1) (tÞnh tiÕn dĂ€c theo z0 mĂ©t ÂźoÂčn d1) ; cžc trĂŽc y0,v” y1 xžc ¼Þnh theo qui tŸc b”n tay ph¶i (H×nh 3.12 ) . TiÕp tĂŽc chĂ€n gĂšc tĂ€a Ÿé O2 ¼Æt trĂŻng vĂ­i O1 v× trĂŽc khĂ­p thĂž ba v” trĂŽc khĂ­p thĂž hai cŸt nhau tÂči O1 (nh− h×nh 3.12). TrĂŽc z2 cĂŻng ph−¬ng vĂ­i trĂŽc khĂ­p thĂž ba, tĂžc l” Ÿ· quay Âźi mĂ©t gĂŁc 900 so vĂ­i z1 quanh trĂŽc y1; phÐp biÕn ŸÊi nÇy kh«ng cĂŁ trong biÓu thĂžc tÝnh An nÂȘn kh«ng dĂŻng ¼−üc, ta cÇn chĂ€n lÂči vÞ trÝ ban ¼Çu cña robot (thay ŸÊi vÞ trÝ cña kh©u thĂž 3) nh− h×nh 3.13.

Theo h×nh 3.13, O2 vÉn ¼−üc ¼Æt trĂŻng vĂ­i O1, trĂŽc z2 cĂŁ ph−¬ng thÂŒng ŸÞng, nghÜa l” ta Ÿ· quay trĂŽc z1 th”nh z2 quanh trĂŽc x1 mĂ©t gĂŁc -900 (tĂžc α2= -900).

§Çu cuĂši cña kh©u thĂž 3 kh«ng cĂŁ khĂ­p, ta ¼Æt O3 tÂči ÂźiÓm giĂ·a cña cžc ngĂŁn tay, v” trĂŽc z3, x3 chĂ€n nh− h×nh vÏ, nh− vËy ta Ÿ· tÞnh tiÕn gĂšc toÂč Ÿé dĂ€c theo z2 mĂ©t ÂźoÂčn d3 (PhÐp biÕn ŸÊi Trans(0,0,d3)), v× Ÿ©y l” kh©u tÞnh tiÕn nÂȘn d3 l” biÕn .

H×nh 3.12 : GŸn cžc hÖ toÂč Ÿé O0 v” O1

y1

x1

y0

z1z2

O1 , O2

O0

z0

Ξ1

Ξ2 d3

x0

d1

Ξ1

Ξ2 d3

H×nh 3.11 : Robot RRT

≡ x2

O3

≡ O2

≡ z2

z3

z0

O0x0

O1

y1

d1

x1

y0

z1

Ξ1

Ξ2

d3

x3

d3

H×nh 3.13 : HÖ toÂč Ÿé gŸn lÂȘn cžc kh©u

TS. PhÂčm §šng Ph−íc

Page 35: Robot cong nghiep

Robot c«ng nghiÖp 34

Nh− vËy viÖc gŸn cžc hÖ toÂč Ÿé lÂȘn cžc kh©u cña robot Ÿ· ho”n th”nh. Th«ng qua cžc ph©n tÝch trÂȘn Ÿ©y, ta cĂŁ thÓ xžc ¼Þnh ¼−üc cžc th«ng sĂš DH cña robot. 2. LËp b¶ng th«ng sĂš DH :

Kh©u Ξi αiai di

1 Ξ1* 90 0 d1

2 Ξi* -90 0 0

3 0 0 0 d3*

3. Xžc ¼Þnh cžc ma trËn A : Ma trËn An cĂŁ dÂčng :

cosΞ -sinΞ cosα sinΞ sinα 0An = sinΞ cosΞ cosα -cosΞ sinα 0

0 sinα cosα d 0 0 0 1

VĂ­i qui −íc viÕt tŸt : C1 = cosΞ1 ; S1 = sinΞ1 ; C2 = cosΞ2 . . .

C1 0 S1 0 A1 = S1 0 -C1 0

0 1 0 d1

0 0 0 1

C2 0 -S2 0 A2 = S2 0 C2 0

0 -1 0 0 0 0 0 1

1 0 0 0

A3 = 0 1 0 0 0 0 1 d3

0 0 0 1 4. TÝnh c¾c ma trËn biÕn ¼éi thuÇn nhÊt T :

+ Ma trËn 2T3 = A3

+ Ma trËn 1T3 = A2. 2T3

C2 0 -S2 0 1 0 0 0 C2 0 -S2 -S2*d3

1T3 = S2 0 C2 0 0 1 0 0 = S2 0 C2 C2*d3 0 -1 0 d2 0 0 1 d3 0 -1 0 0 0 0 0 1 0 0 0 1 0 0 0 1

+ Ma trËn T3 = A1 . 1T3

C1 0 S1 0 C2 0 -S2 -S2*d3

T3 = S1 0 -C1 0 S2 0 C2 C2*d3 0 1 0 d1 0 -1 0 0 0 0 0 1 0 0 0 1

TS. PhÂčm §šng Ph−íc

Page 36: Robot cong nghiep

Robot c«ng nghiÖp 35

C1C2 -S1 -C1S2 -C1S2d3 = S1d2 C1 -S1S2 -S1S2d3 S2 0 C2 C2d3 + d1 0 0 0 1

Ta cĂŁ hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot nh− sau :

nx = C1C2; Ox = -S1; ax = -C1S2; px = -C1S2d3

ny = S1C2; Oy = C1; ay = -S1S2; py = -S1S2d3

nz = S2Oz = 0; az = C2; pz = C2d3 + d1;

(Ta cĂŁ thÓ sÂŹ bĂ© kiÓm tra kÕt qu¶ tÝnh tožn b»ng cžch dĂča v”o toÂč Ÿé vÞ trÝ px,py, pz Ÿ· tÝnh so vĂ­i cžch tÝnh h×nh hĂ€c trÂȘn h×nh vÏ). 3.9. HÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot STANFORD : Stanford l” mĂ©t robot cĂŁ 6 kh©u vĂ­i cÊu h×nh RRT.RRR (Kh©u thĂž 3 chuyÓn Ÿéng tÞnh tiÕn, nšm kh©u cßn lÂči chuyÓn Ÿéng quay). KÕt cÊu cña robot Stanford nh− h×nh 3.14 :

H×nh 3.14 : Robot Stanford

TS. PhÂčm §šng Ph−íc

Page 37: Robot cong nghiep

Robot c«ng nghiÖp 36

TrÂȘn h×nh 3.15 tr×nh b”y m« h×nh cña robot Stanford vĂ­i viÖc gŸn cžc hÖ toÂč Ÿé lÂȘn tĂ”ng kh©u. §Ó Ÿn gi¶n trong khi viÕt cžc ph−¬ng tr×nh Ÿéng hĂ€c cña robot, ta qui −íc cžch viÕt tŸt cžc h”m l−üng gižc nh− sau : C1 = cosΞ1; S1 = sinΞ1; C12 = cos(Ξ1+Ξ2); S12 = sin(Ξ1+Ξ2) S234 = sin (Ξ2+Ξ3+Ξ4) ... . HÖ toÂč Ÿé gŸn lÂȘn cžc kh©u cña robot nh− h×nh 3.15. (Kh©u cuĂši cĂŁ chiÒu d”i v” kho¶ng cžch b»ng kh«ng, ¼Ó cĂŁ thÓ gŸn cžc loÂči c«ng cĂŽ khžc nhau nÂȘn chĂ€n O6≡O5). B¶ng th«ng sĂš DH (Denavit-Hartenberg) cña robot Stanford nh− sau :

Kh©u Ξi αiai di

1 Ξ1* -900 0 0 2 Ξ2* 900 0 d2

3 0 0 0 d3* 4 Ξ4* -900 0 0 5 Ξ5* 900 0 0 6 Ξ6* 0 0 0

(* : Cžc biÕn khĂ­p). Cžc ma trËm A cña robot Stanford ¼−üc xžc ¼Þnh nh− sau :

C1 0 -S1 0 C2 0 S2 0 A1= S1 0 C1 0 A2= S2 0 -C2 0

0 -1 0 0 0 1 0 d2 0 0 0 1 0 0 0 1

1 0 0 0 C4 0 -S4 0

A3= 0 1 0 0 A4= S4 0 C4 0 0 0 1 d3 0 -1 0 0 0 0 0 1 0 0 0 1

C5 0 S5 0 C6 -S6 0 0

A5= S5 0 -C5 0 A6= S6 C6 0 0 0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1

d2 d3

z4z3,z5,z6

z2

O0,O1

xi

x0

z0

z1

H×nh 3.15 : HÖ toÂč Ÿé cña Robot Stanford

O3,O4,O5,O6

x1

O2

TÝch cña cžc ma trËn chuyÓn vÞ A ŸÚi vĂ­i robot Stanford ¼−üc bŸt ¼Çu Ă« kh©u 6 v”

chuyÓn dÇn vÒ gĂšc; theo thĂž tĂč nÇy ta cĂŁ :

TS. PhÂčm §šng Ph−íc

Page 38: Robot cong nghiep

Robot c«ng nghiÖp 37

C6 -S6 0 0 T6

5 = S6 C6 0 0 0 0 1 0 0 0 0 1

C5C6 -C5S6 S5 0

T64 = A5A6 = S5C6 -S5S6 -C5 0

S6 C6 0 0 0 0 0 1

C4C5C6 - S4S6 -C4C5S6-S4C6 C4S5 0

T63 = A4A5A6 = S4C5C6 + C4S6 -S4C5S6 + C4C6 S4S5 0

-S5C6 S5S6 C5 0 0 0 0 1

C4C5C6-S4S6 -C4C5S6 - S4C6 C4S5 0

T62 = A3A4A5A6 = S4C5C + C4S6 -S4C5S6 + C4C6 S4S5 0

-S5C6 S5S6 C5 d3 0 0 0 1

C2(C4C5C6 - S4S6) - S2S5C6 -C2(C4C5S6-S4C6)+S2S5S6

T61 =A2 A3A4A5A6 = S2(C4C5C6 - S4S6) + C2S5C6 -S2(C4C5S6+S4C6)-C2S5S6

S4C5C6 + C4S6 -S4C5S6+C4C6 0 0

C2C4S5 + S2C5 S2d3S2C4S5 - C2C5 -C2d3

S4S5 d20 1

CuĂši cĂŻng : nx Ox ax px

T6 = ny Oy ay py = A1T61

nz Oz az pz 0 0 0 1

§Ó tÝnh T6, ta ph¶i nh©n A1 vĂ­i T6

1 sau Ÿã c©n b»ng cžc phÇn tö cña ma trËn T6 Ă« hai vÕ ta ¼−üc mĂ©t hÖ thĂšng cžc ph−¬ng tr×nh sau :

nx = C1[C2(C4C5C6 - S4S6) - S2S5C6] - S1(S4C5C6 + C4S6) ny = S1[C2(C4C5C6 - S4S6) - S2S5C6] + C1(S4C5C6 + C4S6) nz = -S2(C4C5C6 - S4S6) + C2S5C6Ox = C1[-C2(C4C5S6 + S4C6) + S2S5S6] - S1(-S4C5S6 + C4C6) Oy = S1[-C2(C4C5S6 + S4C6) + S2S5S6] + C1(-S4C5C6 + C4C6) Oz = S2(C4C5S6 + S4C6) + C2S5S6aX = C1(C2C4S5 + S2C5) - S1S4S5ay = S1(C2C4S5 + S2C5) + C1S4S5az = -S2C4S5 + C2C5px = C1S2d3 - S1d2py = S1S2d3 + C1d2pz = C2d3

TS. PhÂčm §šng Ph−íc

Page 39: Robot cong nghiep

Robot c«ng nghiÖp 38

NÕu ta biÕt ¼−üc cžc giž trÞ cña biÕn khĂ­p, th× vÞ trÝ v” h−íng cña b”n tay robot sÏ t×m ¼−üc b»ng cžch xžc ¼Þnh cžc giž trÞ cžc phÇn tö cña T6 theo cžc ph−¬ng tr×nh trÂȘn.

Cžc ph−¬ng tr×nh trÂȘn gĂ€i l” hÖ ph−¬ng tr×nh Ÿéng hĂ€c thuËn cña robot Stanford.

3.10. HÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot ELBOW :

§Ó hiÓu rĂą hÂŹn vÒ cžch thiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot, ta xÐt thÂȘm tr−ĂȘng hĂźp robot Elbow.

Kh©u 1

Kh©u 2 Kh©u 3

Kh©u 4

Kh©u 5

Kh©u 6

H×nh 1.16 : Robot Elbow

Ξ1

Ξ2

Ξ3 Ξ4

Ξ6

z4

z0

a5 = a6 = 0

z2z3 z5,z6

xi

O0,O1 a2

a3 a4Ξ5

O2,O5,O6 O3

O2 z1

H×nh 1.17 : VÞ trÝ ban ¼Çu cña robot Elbow v” cžc hÖ toÂč Ÿé

Bé th«ng sÚ DH cña robot Elbow

Kh©u Ξi* αi

ai di

1 Ξ1900 0 0

2 Ξ20 a2 0

3 Ξ30 a3 0

4 Ξ4-900 a4 0

5 Ξ5900 0 0

6 Ξ60 0 0

(* : cžc biÕn khĂ­p ) Cžc ma trËn A cña robot Elbow ¼−üc xžc ¼Þnh nh− sau :

C1 0 S1 0 C2 -S2 0 C2a2

A1= S1 0 -C1 0 A2= S2 C2 0 S2a2

0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1

TS. PhÂčm §šng Ph−íc

Page 40: Robot cong nghiep

Robot c«ng nghiÖp 39

C3 -S3 0 C3a3 C4 0 -S4 C4a4

A3= S3 C3 0 S3a3 A4= S4 0 C4 S4a4

0 0 1 0 0 -1 0 0 0 0 0 1 0 0 0 1

C5 0 S5 0 C6 -S6 0 0

A5= S5 0 -C5 0 A6= S6 C6 0 0 0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1

Ta xžc ¼Þnh cžc ma trËn T theo cžc hÖ toÂč Ÿé lÇn l−üt tĂ” kh©u cuĂši trĂ« vÒ gĂšc :

C6 -S6 0 0 T6

5 = S6 C6 0 0 0 0 1 0 0 0 0 1

C5C6 -C5S6 S5 0

T64 = A5A6 = S5C6 -S5S6 -C5 0

S6 C6 0 0 0 0 0 1

C4C5C6 - S4S6 -C4C5S6-S4C6 C4S5 C4a4T6

3 = A4A5A6 = S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 S4a4 -S5C6 S5S6 C5 0 0 0 0 1

C34C5C6 - S34S6 -C34C5C6 - S34C6 C34S5 C34a4+C3a3

T62 = A3A4A5A6 = S34C5C6+C34S6 -S34C5S6+C34C6 S34S5 S34a4+S3a3

-S5C6 S5S6 C5 0 0 0 0 1

T6

1 =A2 A3A4A5A6 =

C234C5C6 - S234S6 -C234C5S6 - S234C6 C234S5 C234a4+C23a3+C2a2S234C5C6 + C234S6 -S234C5S6 + C234C6 S234S5 S234a4+S23a3+S2a2

-S5C6 S5S6 C5 0 0 0 0 1

CuĂši cĂŻng : nx Ox ax px

T6 = ny Oy ay py = A1T61

nz Oz az pz 0 0 0 1

§Ó tÝnh T6, ta ph¶i nh©n A1 vĂ­i T6

1 sau Ÿã c©n b»ng cžc phÇn tö cña ma trËn T6 ta ¼−üc mĂ©t hÖ thĂšng cžc ph−¬ng tr×nh sau :

TS. PhÂčm §šng Ph−íc

Page 41: Robot cong nghiep

Robot c«ng nghiÖp 40

nx = C1(C234C5C6- S234S6) - S1S5C6ny = S1(C234C5C6- S234S6) + C1S5C6nz = S234C5C6 + C234S6Ox = -C1(C234C5S6 + S234C6) + S1S5S6Oy = -S1(C234C5S6 + S234C6) - C1S5S6Oz = -S234C5S6 + C234C6aX = C1C234S5 + S1C5ay = S1C234S5 - C1C5az = S234S5px = C1(C234a4 + C23a3 + C2a2) py = S1(C234a4 + C23a3 + C2a2) pz = S234a4 + S23a3 + S2a2

CĂ©t ¼Çu tiÂȘn cña ma trËn T6 cĂŁ thÓ ¼−üc xžc ¼Þnh bĂ«i tÝch vectÂŹ :

r r rn = O x a. 3.11. KÕt luËn :

Trong ch−¬ng nÇy chĂłng ta Ÿ· nghiÂȘn cĂžu viÖc dĂŻng cžc phÐp biÕn ŸÊi thuÇn nhÊt ¼Ó m« t¶ vÞ trÝ v” h−íng cña kh©u chÊp h”nh cuĂši cña robot th«ng qua viÖc xžc lËp cžc hÖ toÂč Ÿé gŸn lÂȘn cžc kh©u v” cžc th«ng sĂš DH. Ph−¬ng phžp nÇy cĂŁ thÓ dĂŻng cho bÊt cĂž robot n”o vĂ­i sĂš kh©u (khĂ­p) tuĂș Ăœ. Trong quž tr×nh xžc lËp cžc hÖ toÂč Ÿé mĂ« rĂ©ng ta cĂČng xžc ¼Þnh ¼−üc vÞ trÝ dĂ”ng cña mçi robot. TuĂș thuĂ©c kÕt cÊu cña robot cĂČng nh− c«ng cĂŽ gŸn lÂȘn kh©u chÊp h”nh cuĂši m” ta cĂŁ thÓ ¼−a cžc th«ng sĂš cña kh©u chÊp h”nh cuĂši v”o ph−¬ng tr×nh Ÿéng hĂ€c hay kh«ng. ViÖc tÝnh tožn cžc ma trËn T ¼Ó thiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot th−ĂȘng tĂšn nhiÒu thĂȘi gian v” dÔ nhÇm lÉn, ta cĂŁ thÓ lËp tr×nh trÂȘn mžy tÝnh ¼Ó tÝnh tožn (Ă« dÂčng kĂœ hiÖu) nh»m nhanh chĂŁng xžc ¼Þnh cžc ma trËn An v” thiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot .

ThiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot l” b−íc rÊt quan trĂ€ng ¼Ó cĂŁ thÓ dĂča v”o Ÿã lËp tr×nh ÂźiÒu khiÓn robot. B”i tožn nÇy th−ĂȘng ¼−üc gĂ€i l” b”i tožn Ÿéng hĂ€c thuËn robot. ViÖc gi¶i hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot ¼−üc gĂ€i l” b”i tožn Ÿéng hĂ€c ng−üc, nh»m xžc ¼Þnh giž trÞ cña cžc biÕn khĂ­p theo cžc th«ng sĂš Ÿ· biÕt cña kh©u chÊp h”nh cuĂši; vÊn ¼Ò nÇy ta sÏ nghiÂȘn cĂžu trong ch−¬ng tiÕp theo.

B”i tËp ch−¬ng III : B”i 1 : Cho ma trËn :

? 0 -1 0T6 = ? 0 0 1

? -1 0 2 ? 0 0 1

l” ma trËn biÓu diÔn h−íng v” vÞ trÝ cña kh©u chÊp h”nh cuĂši. T×m cžc phÇn tö ¼−üc Ÿžnh dÊu ?

B”i 2 : Cho mĂ©t robot cĂŁ 3 kh©u phÂŒng nh− h×nh 3.18, cÊu h×nh RRR. ThiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot.

TS. PhÂčm §šng Ph−íc

Page 42: Robot cong nghiep

Robot c«ng nghiÖp 41

B”i 3 : Cho mĂ©t robot cĂŁ 2 kh©u tÞnh tiÕn nh− h×nh 3.19, cÊu h×nh TT. ThiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot.

H×nh 3.18 : Robot cÊu h×nh RRR H×nh 3.19 : Robot cÊu h×nh TT

B”i 4 : Cho mĂ©t robot cĂŁ 2 kh©u phÂŒng nh− h×nh 3.20, cÊu h×nh RT. ThiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot. B”i 5 : Cho mĂ©t robot cĂŁ 3 kh©u nh− h×nh 3.21, cÊu h×nh RTR. ThiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot.

H×nh 3.20 : Robot cÊu h×nh RT

H×nh 3.21 : Robot cÊu h×nh RTR B”i 6 : Cho mĂ©t robot cĂŁ 3 kh©u nh− h×nh 3.22, cÊu h×nh RRR. ThiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot.

H×nh 3.23 : Robot cÊu h×nh RRRRR

H×nh 3.22 : Robot cÊu h×nh RRR

B”i 7 : Cho mĂ©t robot cĂŁ 5 kh©u nh− h×nh 3.23, cÊu h×nh RRRRR. ThiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot.

TS. PhÂčm §šng Ph−íc

Page 43: Robot cong nghiep

Robot c«ng nghiÖp 42

Ch−¬ng IV

Gi¶i ph−¬ng tr×nh Ÿéng hĂ€c robot hay ph−¬ng tr×nh Ÿéng hĂ€c ng−üc

(Invers Kinematic Equations)

Trong ch−¬ng 3, ta Ÿ· nghiÂȘn cĂžu viÖc thiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot th«ng qua ma trËn T6 b»ng ph−¬ng phžp gŸn cžc hÖ toÂč Ÿé lÂȘn cžc kh©u v” xžc ¼Þnh cžc th«ng sĂš DH. Ta cĂČng Ÿ· xÐt tĂ­i cžc ph−¬ng phžp khžc nhau ¼Ó m« t¶ h−íng cña kh©u chÊp h”nh cuĂši nh− cžc phÐp quay Euler, phÐp quay Roll-Pitch v” Yaw .v.v...Trong ch−¬ng nÇy chĂłng ta sÏ tiÕn h”nh gi¶i hÖ ph−¬ng tr×nh Ÿéng hĂ€c Ÿ· thiÕt lËp Ă« ch−¬ng tr−íc nh»m xžc ¼Þnh cžc biÕn trong bĂ© th«ng sĂš Denavit - Hartenberg khi Ÿ· biÕt ma trËn vectÂŹ cuĂši T6. KÕt qu¶ cña viÖc gi¶i hÖ ph−¬ng tr×nh Ÿéng hĂ€c Ÿãng vai trß hÕt sĂžc quan trĂ€ng trong viÖc ÂźiÒu khiÓn robot. Th«ng th−ĂȘng, ÂźiÒu ta biÕt l” cžc vÞ trÝ v” h−íng m” ta muĂšn robot ph¶i dÞch chuyÓn tĂ­i v” ÂźiÒu ta cÇn biÕt l” mĂši quan hÖ giĂ·a cžc hÖ toÂč Ÿé trung gian ¼Ó phĂši hĂźp tÂčo ra chuyÓn Ÿéng cña robot, hay nĂŁi cžch khžc Ÿã chÝnh l” giž trÞ cña cžc biÕn khĂ­p Ăžng vĂ­i mçi toÂč Ÿé v” h−íng cña kh©u chÊp h”nh cuĂši hoÆc c«ng cĂŽ gŸn lÂȘn kh©u chÊp h”nh cuĂši, muĂšn vËy ta ph¶i gi¶i hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot. ViÖc nhËn ¼−üc lĂȘi gi¶i cña b”i tožn Ÿéng hĂ€c ng−üc l” vÊn ¼Ò khĂŁ m” ta sÏ nghiÂȘn cĂžu trong ch−¬ng nÇy. NhiÖm vĂŽ cña b”i tožn l” xžc ¼Þnh tÖp nghiÖm (Ξ1, Ξ2, ...,Ξ6,di*) khi Ÿ· biÕt h×nh thÓ cña robot th«ng qua vectÂŹ cuĂši T6 (khži niÖm “h×nh thÓ” cña robot bao gĂ„m khži niÖm vÒ vÞ trÝ v” h−íng cña kh©u chÊp h”nh cuĂši : Configuration = Position + Orientation).

CĂČng cÇn l−u Ăœ r»ng, Âźa sĂš cžc robot cĂŁ bĂ© Teach pendant l” thiÕt bÞ dÂčy hĂ€c,

cĂŁ nhiÖm vĂŽ ÂźiÒu khiÓn robot ¼Õn cžc vÞ trÝ mong muĂšn trong Ÿéng tr×nh ¼Çu tiÂȘn (ÂźiÒu khiÓn ÂźiÓm : Point to point ), cžc chuyÓn Ÿéng nÇy sÏ ¼−üc ghi lÂči v”o bĂ© nhĂ­ trung t©m (CPU) cña robot hoÆc mžy tÝnh ÂźiÒu khiÓn robot, sau Ÿã robot cĂŁ thÓ thĂčc hiÖn lÂči Ÿóng cžc Ÿéng tžc Ÿ· ¼−üc hĂ€c. Trong quž tr×nh hoÂčt Ÿéng cña robot, nÕu dÂčng quÜ ÂźÂčo ¼−ĂȘng Âźi kh«ng quan trĂ€ng th× kh«ng cÇn lĂȘi gi¶i cña b”i tožn Ÿéng hĂ€c ng−üc. 4.1. Cžc ÂźiÒu kiÖn cña b”i tožn Ÿéng hĂ€c ng−üc :

TS. PhÂčm §šng Ph−íc

Page 44: Robot cong nghiep

Robot c«ng nghiÖp 43

ViÖc gi¶i b”i tožn Ÿéng hĂ€c ng−üc cña robot cÇn tho¶ m·n cžc ÂźiÒu kiÖn sau : 4.1.1. §iÒu kiÖn tĂ„n tÂči nghiÂȘm : §iÒu kiÖn nÇy nh»m khÂŒng ¼Þnh : CĂŁ Ýt nhÊt mĂ©t tÖp nghiÖm (Ξ1,Ξ2, ...,Ξ6,di*) sao cho robot cĂŁ h×nh thÓ cho tr−íc. (“H×nh thÓ” l” khži niÖm m« t¶ t−ĂȘng minh cña vectÂŹ cuĂši T6 c¶ vÒ vÞ trÝ v” h−íng). 4.1.2. §iÒu kiÖn duy nhÊt cña tÖp nghiÖm : Trong khi xžc ¼Þnh cžc tÖp nghiÖm cÇn ph©n biÖt rĂą hai loÂči nghiÖm : + NghiÖm tožn (Mathematical Solution) : Cžc nghiÖm nÇy tho¶ m·n cžc ph−¬ng tr×nh cho tr−íc cña T6. + NghiÖm vËt lĂœ (Physical Solution) : l” cžc tÖp con cña nghiÖm tožn, phĂŽ thuĂ©c v”o cžc giĂ­i hÂčn vËt lĂœ (giĂ­i hÂčn vÒ gĂŁc quay, kÝch th−íc ...) nh»m xžc ¼Þnh tÖp nghiÖm duy nhÊt. ViÖc gi¶i hÖ ph−¬ng tr×nh Ÿéng hĂ€c cĂŁ thÓ ¼−üc tiÕn h”nh theo hai ph−¬ng phžp cÂŹ b¶n sau : + Ph−¬ng phžp gi¶i tÝch (Analytical Method) : t×m ra cžc c«ng thĂžc hay cžc ph−¬ng tr×nh tožn gi¶i tÝch biÓu thÞ quan hÖ giĂ·a cžc giž trÞ cña kh«ng gian biÕn trĂŽc v” cžc th«ng sĂš khžc cña bĂ© th«ng sĂš DH. + Ph−¬ng phžp sĂš (Numerical Method) : T×m ra cžc giž trÞ cña tÖp nghiÖm b»ng kÕt qu¶ cña mĂ©t quž tr×nh lÆp. 4.2. LĂȘi gi¶i cña phÐp biÕn ŸÊi Euler : Trong ch−¬ng 3 ta Ÿ· nghiÂȘn cĂžu vÒ phÐp biÕn ŸÊi Euler ¼Ó m« t¶ h−íng cña kh©u chÊp h”nh cuĂši : Euler (Ί,Ξ,ψ) = Rot(z, Ί) Rot(y, Ξ) Rot(z, ψ) TÖp nghiÖm muĂšn t×m l” cžc gĂŁc Ί, Ξ, ψ khi Ÿ· biÕt ma trËn biÕn ŸÊi ŸÄng nhÊt T6 (cßn gĂ€i l” ma trËn vectÂŹ cuĂši), NÕu ta cĂŁ cžc giž trÞ sĂš cña cžc phÇn tö trong ma trËn T6 th× cĂŁ thÓ xžc ¼Þnh ¼−üc cžc gĂŁc Euler Ί, Ξ, ψ thÝch hĂźp. Nh− vËy ta cĂŁ : Euler (Ί,Ξ,ψ) = T6 (4-1) VÕ trži cña ph−¬ng tr×nh (4-1) Ÿ· ¼−üc biÓu diÔn b»ng c«ng thĂžc (3-4) , nÂȘn ta cĂŁ :

cosΊCosΞcosψ - sinΊsinψ -cosΊCosΞsinψ - sinΊcosψ cosΊsinΞ 0 sinΊCosΞcosψ + cosΊsinψ -sinΊCosΞsinψ + cosΊcosψ sinΊsinΞ 0 =

-sinΞ cosψ sinΞ sinψ cosΞ 0 0 0 0 1

nx Ox ax px ny Oy ay py (4-2) nz Oz az pz 0 0 0 1

LÇn l−üt cho c©n b»ng cžc phÇn tö t−¬ng Ăžng cña hai ma trËn trong ph−¬ng tr×nh (4-2) ta cĂŁ cžc ph−¬ng tr×nh sau :

TS. PhÂčm §šng Ph−íc

Page 45: Robot cong nghiep

Robot c«ng nghiÖp 44

nx = cosΊCosΞcosψ - sinΊsinψ (4.3) ny = sinΊCosΞcosψ + cosΊsinψ (4-4) nz = -sinΞ cosψ (4-5) Ox = -cosΊCosΞsinψ - sinΊcosψ (4-6) Oy = -sinΊCosΞsinψ + cosΊcosψ (4-7) Oz = sinΞ sinψ (4-8) ax = cosΊsinΞ (4-9) ay = sinΊsinΞ (4-10) az = cosΞ (4-11) Ta thö gi¶i hÖ ph−¬ng tr×nh nÇy ¼Ó t×m Ί, Ξ, ψ nh− sau : TĂ” (4-11) ta cĂŁ Ξ = cos-1(az) (4-12) TĂ” (4-9) ta cĂŁ Ί = cos-1(ax / sinΞ) (4-13) TĂ” (4-5) v” (4-12) ta cĂŁ ψ = cos-1(-nz / sinΞ) (4-14) Trong Ÿã ta Ÿ· dĂŻng kĂœ hiÖu cos-1 thay cho h”m arccos. Nh−ng cžc kÕt qu¶ Ÿ· gi¶i Ă« trÂȘn ch−a dĂŻng ¼−üc v× cžc lĂœ do d−íi Ÿ©y :

+ H”m arccos kh«ng chØ biÓu hiÖn cho mĂ©t gĂŁc ch−a xžc ¼Þnh m” vÒ Ÿé chÝnh xžc nĂŁ lÂči phĂŽ thuĂ©c vžo chÝnh gĂŁc Ÿã, nghÜa l” : cosΞ = cos(-Ξ) : Ξ ch−a ¼−üc xžc ¼Þnh duy nhÊt.

dcos

d = 00,180

ΞΞ

: Ξ xžc ¼Þnh kh«ng chÝnh xžc.

+ Trong lĂȘi gi¶i ŸÚi vĂ­i Ί v” ψ mĂ©t lÇn nĂ·a chĂłng ta lÂči dĂŻng h”m arccos v” chia cho sinΞ, ÂźiÒu nÇy dÉn tĂ­i sĂč mÊt chÝnh xžc khi Ξ cĂŁ giž trÞ l©n cËn 0. + Cžc ph−¬ng tr×nh (4-13) v” (4-14) kh«ng xžc ¼Þnh khi Ξ = 0 hoÆc Ξ = ±1800.

Do vËy chĂłng ta cÇn ph¶i cÈn thËn hÂŹn khi chĂ€n lĂȘi gi¶i. §Ó xžc ¼Þnh cžc gĂŁc khi gi¶i b”i tožn ng−üc cña robot ta ph¶i dĂŻng h”m arctg2 (y,x) (h”m arctang hai biÕn). H”m arctg2 nh»m mĂŽc ¼Ých xžc ¼Þnh ¼−üc gĂŁc thĂčc - duy nhÊt khi xÐt dÊu cña hai biÕn y v” x. H”m sĂš tr¶ vÒ giž trÞ gĂŁc trong kho¶ng -π ≀ Ξ < π.

Ξ

x

y

X- Y- X+ Y-

H×nh 4.1 : H”m arctg2(y,x)

X- Y+ X+ Y+

VÝ dî :

arctg2(-1/-1)= -1350, trong khi arctg2(1/1) = 450

H”m nÇy xžc ¼Þnh ngay c¶ khi x hoÆc y b»ng 0 v” cho kÕt qu¶ Ÿóng. (Trong mĂ©t sĂš ng«n ngĂ· lËp tr×nh nh− Matlab, turbo C++, Maple h”m arctg2(y,x) Ÿ· cĂŁ sÂŒn trong th− viÖn)

TS. PhÂčm §šng Ph−íc

Page 46: Robot cong nghiep

Robot c«ng nghiÖp 45

§Ó cĂŁ thÓ nhËn ¼−üc nhĂ·ng kÕt qu¶ chÝnh xžc cña b”i tožn Euler, ta thĂčc hiÖn

thñ thuËt tožn hĂ€c sau : Nh©n T6 vĂ­i ma trËn quay nghÞch ٦o Rot(z, Ί)-1,ta cĂŁ:

Rot(z, Ί)-1 T6 = Rot(y, Ξ) Rot(z, ψ) (4-15)

VÕ trži cña ph−¬ng tr×nh (4-15) l” mĂ©t h”m sĂš cña ma trËn T v” gĂŁc quay Ί. Ta thĂčc hiÖn phÐp nh©n ma trËn Ă« vÕ ph¶i cña (4-15), t×m ra cžc phÇn tö cña ma trËn cĂŁ giž trÞ b»ng 0 hoÆc b»ng h»ng sĂš, cho cžc phÇn tö nÇy c©n b»ng vĂ­i nhĂ·ng phÇn tö t−¬ng Ăžng cña ma trËn Ă« vÕ trži, cĂŽ thÓ tĂ” (4-15) ta cĂŁ :

cosΊ sinΊ 0 0 nx Ox ax px CosΞcosψ -CosΞ sinψ sinΞ 0 -sinΊ cosΊ 0 0 ny Oy ay py = sinψ cosψ 0 0

0 0 1 0 nz Oz az pz -sinΞ cosψ sinΞ sinψ CosΞ 0 0 0 0 1 0 0 0 1 0 0 0 1

(4-16) TÝch hai ma trËn Ă« vÕ trži cña ph−¬ng tr×nh (4-16) l” mĂ©t ma trËn m” cĂŁ thÓ ¼−üc viÕt gĂ€n lÂči b»ng cžc kĂœ hiÖu sau :

f11(n) f11(O) f11(a) f11(p) f12(n) f12(O) f12(a) f12(p) f13(n) f13(O) f13(a) f13(p)

0 0 0 1 Trong Ÿã : f11 = cosΊ x + sinΊ y (4-17) f12 = -sinΊ x + cosΊ y (4-18) f13 = z (4-19) v” x, y, z l” cžc phÇn tö cña vectÂŹ xžc ¼Þnh bĂ«i cžc dĂ· kiÖn f11, f12, f13, vÝ dĂŽ : f11(n) = cosΊ nx + sinΊ ny

f12(O) = -sinΊ Ox + cosΊ Oy f13(a) = az Nh− vËy ph−¬ng tr×nh (4-16) cĂŁ thÓ ¼−üc viÕt th”nh :

f11(n) f11(O) f11(a) 0 CosΞcosψ -CosΞ sinψ sinΞ 0 f12(n) f12(O) f12(a) 0 = sinψ cosψ 0 0 (4-20) f13(n) f13(O) f13(a) 0 -sinΞ cosψ sinΞ sinψ CosΞ 0

0 0 0 1 0 0 0 1 Trong Ÿã f11, f12, f13 Ÿ· ¼−üc ¼Þnh nghÜa Ă« (4-17), (4-18) v” (4-19). Khi tÝnh tožn vÕ trži, ta chĂł Ăœ r»ng px, py, pz b»ng 0 v× phÐp biÕn ŸÊi Euler chØ to”n phÐp quay kh«ng chĂža mĂ©t phÐp biÕn ŸÊi tÞnh tiÕn n”o, nÂȘn f11(p) = f12(p) = f13(p) = 0. TĂ” ph−¬ng tr×nh (4-20), cho c©n b»ng phÇn tö Ă« h”ng 2 cĂ©t 3 ta cĂŁ :

TS. PhÂčm §šng Ph−íc

Page 47: Robot cong nghiep

Robot c«ng nghiÖp 46

f12(a) = -sinΊ ax + cosΊ ay = 0. (4-21) CĂ©ng hai vÕ vĂ­i sinΊ ax v” chia cho cosΊ ax ta cĂŁ :

tgaa x

ΊΊΊ

= =sincos

y

GĂŁc Ί cĂŁ thÓ xžc ¼Þnh b»ng h”m arctg hai biÕn : Ί = arctg2(ay, ax). Ta cĂČng cĂŁ thÓ gi¶i ph−¬ng tr×nh (4-21) b»ng cžch cĂ©ng hai vÕ vĂ­i -cosΊ ay rĂ„i chia hai vÕ cho -cosΊ ax, triÖt tiÂȘu -ax Ă« vÕ trži v” cosΊ Ă« vÕ ph¶i, ta cĂŁ :

tg-a-a x

ΊΊΊ

= =sincos

y

Trong tr−ĂȘng hĂźp nÇy gĂŁc Ί t×m ¼−üc l” : Ί = arctg2(-ay, -ax).

Nh− vËy ph−¬ng tr×nh (4-21) cĂŁ mĂ©t cÆp nghiÖm cžch nhau 1800 (Ÿ©y l”

nghiÖm tožn) v” ta cĂŁ thÓ viÕt : Ί = arctg2(ay, ax) v” Ί = Ί + 1800. (HiÓu theo cžch viÕt khi lËp tr×nh trÂȘn mžy tÝnh).

NÕu c¶ ax v” ay ¼Òu b»ng 0 th× gĂŁc Ί kh«ng xžc ¼Þnh ¼−üc. §iÒu Ÿã x¶y ra khi b”n tay chØ thÂŒng lÂȘn trÂȘn hoÆc xuĂšng d−íi v” c¶ hai gĂŁc Ί v” ψ t−¬ng Ăžng vĂ­i cĂŻng mĂ©t phÐp quay. §iÒu nÇy ¼−üc coi l” mĂ©t phÐp suy biÕn (degeneracy), trong tr−ĂȘng hĂźp nÇy ta cho Ί = 0. VĂ­i giž trÞ cña Ί nhËn ¼−üc, cžc phÇn tö ma trËn Ă« vÕ bÂȘn trži cña ph−¬ng tr×nh (4-20) sÏ ¼−üc xžc ¼Þnh. TiÕp tĂŽc so sžnh cžc phÇn tö cña hai ma trËn ta cĂŁ : f11(a) = cosΊ ax + sinΊ ay = sinΞ. V” f13(a) = az = cosΞ. VËy Ξ = arctg2(cosΊ ax + sinΊ ay, az) Khi c¶ hai h”m sin v” cos ¼Òu ¼−üc xžc ¼Þnh nh− tr−ĂȘng hĂźp trÂȘn, th× gĂŁc th−ĂȘng ¼−üc xžc ¼Þnh duy nhÊt v” kh«ng x¶y ra tr−ĂȘng hĂźp suy biÕn nh− gĂŁc Ί tr−íc Ÿ©y. CĂČng tĂ” ph−¬ng tr×nh (4-20) ta cĂŁ : f12(n) = -sinΊ nx + cosΊ ny = sinψ f12(O) = -sinΊ Ox + cosΊ Oy = cosψ

TS. PhÂčm §šng Ph−íc

Page 48: Robot cong nghiep

Robot c«ng nghiÖp 47

VËy : ψ = arctg2(-sinΊ nx + cosΊ ny, -sinΊ Ox + cosΊ Oy) TĂŁm lÂči, nÕu cho tr−íc mĂ©t phÐp biÕn ŸÊi ŸÄng nhÊt d−íi dÂčng cžc phÐp quay, ta cĂŁ thÓ xžc ¼Þnh cžc gĂŁc Euler t−¬ng Ăžng l” : Ί = arctg2(ay, ax) v” Ί = Ί + 1800

Ξ = arctg2(cosΊ ax + sinΊ ay, az) ψ = arctg2(-sinΊ nx + cosΊ ny, -sinΊ Ox + cosΊ Oy)

4.3. LĂȘi gi¶i cña phÐp biÕn ŸÊi Roll, Pitch v” Yaw : PhÐp biÕn ŸÊi Roll, Pitch v” Yaw Ÿ· ¼−üc ¼Þnh nghÜa : RPY(Ί,Ξ,ψ)= Rot(z,Ί)Rot(y,Ξ)Rot(x, ψ) ViÖc gi¶i ph−¬ng tr×nh : T6 = RPY(Ί,Ξ,ψ) sÏ xžc ¼Þnh ¼−üc cžc gĂŁc Ί,Ξ v” ψ. Cžch gi¶i ¼−üc tiÕn h”nh t−¬ng tĂč nh− khi thĂčc hiÖn lĂȘi gi¶i cho phÐp quay Euler. Nh©n T6 vĂ­i ma trËn nghÞch ٦o Rot(z, Ί)-1, ta cĂŁ :

Rot(z, Ί)-1T6 = Rot(y,Ξ)Rot(x, ψ) Hay l” :

f11(n) f11(O) f11(a) 0 cosΞ sinΞ sinψ sinΞ cosψ 0 f12(n) f12(O) f12(a) 0 = 0 cosψ -sinψ 0 (4-22) f13(n) f13(O) f13(a) 0 -sinΞ cosΞ sinψ cosΞcosψ 0

0 0 0 1 0 0 0 1 Trong Ÿã : f11 = cosΊ x + sinΊ y f12 = -sinΊ x + cosΊ y f13 = z C©n b»ng phÇn tö Ă« h”ng 2 cĂ©t 1 : f12(n) = 0, ta cĂŁ : -sinΊ x + cosΊ y = 0 Ph−¬ng tr×nh nÇy cho ta hai nghiÖm nh− Ÿ· biÕt : Ί = arctg2(nx, ny) v” Ί = Ί + 1800

TiÕp tĂŽc c©n b»ng cžc phÇn tö t−¬ng Ăžng cña hai ma trËn ta cĂŁ : -sinΞ = nz

cosΞ = cosΊ nx + sinΊ ny

TS. PhÂčm §šng Ph−íc

Page 49: Robot cong nghiep

Robot c«ng nghiÖp 48

do vËy : Ξ = arctg2(-nz, cosΊ nx + sinΊ ny) Ngo”i ra ta cßn cĂŁ : -sinψ = -sinΊ ax + cosΊ ay

cosψ = -sinΊ Ox + cosΊ Oy NÂȘn : ψ = arctg2(sinΊ ax - cosΊ ay, -sinΊ Ox + cosΊ Oy) Nh− vËy ta Ÿ· xžc ¼Þnh ¼−üc cžc gĂŁc quay Roll, Pitch v” Yaw theo cžc phÇn tö cña ma trËn T6. 4.4. Gi¶i b”i tožn Ÿéng hĂ€c ng−üc cña robot Stanford : HÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot Stanford Ÿ· ¼−üc thiÕt lËp trong ch−¬ng III, Ta cĂŁ : T6 = A1A2A3A4A5A6 (4-23) LiÂȘn tĂŽc nh©n (4-23) vĂ­i cžc ma trËn A nghÞch ٦o, ta ¼−üc :

A 1 T1−6 = 1T6 (4-24)

A A 1 T21− 1−

6 = 2T6 (4-25)

A 3 A A 1 T1−2

1− 1−6 = 3T6 (4-26)

A A 3 A A 1 T41− 1−

21− 1−

6 = 4T6 (4-27)

A A A 3 A A 1 T51−

41− 1−

21− 1−

6 = 5T6 (4-28) Cžc phÇn tö Ă« vÕ trži cña cžc ph−¬ng tr×nh nÇy l” h”m sĂš cña cžc phÇn tö T6 v” cžc biÕn khĂ­p cña (n-1) khĂ­p ¼Çu tiÂȘn. Trong khi Ÿã cžc phÇn tö cña ma trËn vÕ bÂȘn ph¶i hoÆc b»ng 0, b»ng h»ng sĂš hoÆc l” h”m sĂš cña cžc biÕn khĂ­p thĂž n ¼Õn khĂ­p thĂž 6. TĂ” mçi ph−¬ng tr×nh ma trËn, cho c©n b»ng cžc phÇn tö t−¬ng Ăžng chĂłng ta nhËn ¼−üc 12 ph−¬ng tr×nh. Mçi ph−¬ng tr×nh cĂŁ cžc phÇn tö cña 4 vectÂŹ n, O, a, p. TĂ” ph−¬ng tr×nh (4-24), ta cĂŁ :

C1 S1 0 0 nx Ox ax px

A T11−

6 = 0 0 -1 0 ny Oy ay py

-S1 C1 0 0 nz Oz az pz 0 0 0 1 0 0 0 1

f11(n) f11(O) f11(a) f11(p) = f12(n) f12(O) f12(a) f12(p) f13(n) f13(O) f13(a) f13(p) 0 0 0 1

TS. PhÂčm §šng Ph−íc

Page 50: Robot cong nghiep

Robot c«ng nghiÖp 49

Trong Ÿã : f11 = C1 x + S1 y f12 = - z f13 = -S1 x + C1 y VÕ bÂȘn ph¶i cña (4-24) l” :

C2(C4C5C6 - S4S6) - S2S5C6 -C2(C4C5S6-S4C6)+S2S5S6 C2C4S5 + S2C5 S2d31T6 = S2(C4C5C6 - S4S6) + C2S5C6 -S2(C4C5S6+S4C6)-C2S5S6 S2C4S5 - C2C5 -C2d3

S4C5C6 + C4S6 -S4C5S6+C4C6 S4S5 d2

0 0 0 1

Cžc phÇn tö cña ma trËn vÕ ph¶i ¼Òu l” h”m sĂš cña Ξ2, d3, Ξ4, Ξ5, Ξ6 ngoÂči trĂ” phÇn tö Ă« h”ng 3 cĂ©t 4, Ÿã l” :

f13(p) = d2hay : -S1px + C1py = d2§Ó gi¶i ph−¬ng tr×nh Ă« dÂčng nÇy ta cĂŁ thÓ thay thÕ bĂ«i cžc h”m l−üng gižc sau

Ÿ©y : px = r cosΊ py = r sinΊ

Trong Ÿã : r = + p px2 + y

2

Ί = arctg2(py, px) ThÕ px v” py v”o ph−¬ng tr×nh -S1px + C1py = d2 ta cĂŁ : sinΊcosΞ1 - cosΊsinΞ1 = d2 / r VĂ­i 0 < d2 / r ≀ 1 Hay l” : sin(Ί - Ξ1) = d2 / r VĂ­i 0 < Ί - Ξ1 < π TĂ” Ÿã ta cĂŁ :

cos(Ί - Ξ1) = ± −1 22( / )d r

Trong Ÿã dÊu trĂ” phĂŻ hĂźp vĂ­i h×nh thÓ vai trži cña robotv” dÊu cĂ€ng phĂŻ hĂźp vĂ­i h×nh thÓ vai ph¶i cña robot. CuĂši cĂŻng :

Ξ1 = arctg2(py, px) - arctg2(d2, ± −1 22( / )d r ) (4-29)

NÕu tÝnh ¼−üc Ξ1 th× vÕ trži cña ph−¬ng tr×nh (4-24) ¼−üc xžc ¼Þnh. Cho c©n b»ng cžc phÇn tö Ă« h”ng 1 cĂ©t 4 v” h”ng 2 cĂ©t 4, ta cĂŁ : S2d3 = C1px + S1py -C2d3 = -pz d3 l” dÞch chuyÓn d”i cña khĂ­p tÞnh tiÕn, d3 > 0, nÂȘn ta cĂŁ : Ξ2 = arctg2(C1px + S1py, pz ) (4-30)

TS. PhÂčm §šng Ph−íc

Page 51: Robot cong nghiep

Robot c«ng nghiÖp 50

TĂ” ph−¬ng tr×nh (4-25) : A A 1 T21− 1−

6 = A 21− 1T6 = 2T6, ta cã :

f21(n) f21(O) f21(a) 0 C4C5C6-S4S6 -C4C5S6 - S4C6 C4S5 0 f22(n) f22(O) f22(a) 0 = S4C5C + C4S6 -S4C5S6 + C4C6 S4S5 0 f23(n) f23(O) f23(a) f23(p) -S5C6 S5S6 C5 d3

0 0 0 1 0 0 0 1 (4-31) Trong Ÿã : f21 = C2(C1 x + S1 y) - S2 z f22 = -S1 x + C1 y f23 = S2(C1 x + S1 y) + C2 z TĂ” c©n b»ng phÇn tö Ă« h”ng 3 cĂ©t 4 ta cĂŁ : d3 = S2(C1 px + S1 py) + C2 pz (4-32) - TĂ” ph−¬ng tr×nh (4-27) ta cĂŁ : A A 4

1−3

1− 2T6 = 4T6

ThĂčc hiÖn phÐp nh©n cžc ma trËn Ă« vÕ trži, v” biÓu diÔn Ă« dÂčng rĂłt gĂ€n nh− sau :

f41(n) f41(O) f41(a) 0 C5C6 -C5S6 S5 0 f42(n) f42(O) f42(a) 0 = S5C6 -S5S6 C5 0 f43(n) f43(O) f43(a) 0 S6 C6 0 0

0 0 0 1 0 0 0 1 Trong Ÿã : f41 = C4[C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y) f42 = -S2(-S1 x + C1 y) - C2 z f43 = -S4[C2(C1 x + S1 y) + S2 z] + C4(-S1 x + C1 y) C©n b»ng phÇn tö h”ng 3, cĂ©t 3 ta ¼−üc mĂ©t h”m sĂš cña Ξ4, Ÿã l” : f43(a) = 0. Hay : -S4[C2(C1 ax + S1 ay) + S2 az] + C4(-S1 ax + C1 ay) = 0 §©y l” ph−¬ng tr×nh l−üng gižc cĂŁ dÂčng : - sinΊ ax + cosΊ ay = 0. Nh− Ÿ· gi¶i trong cžc phÇn tr−íc Ÿ©y, ph−¬ng tr×nh nÇy cĂŁ hai nghiÖm : Ξ4 = arctg2(-S1 ax + C1 ay, C2(C1 ax + S1 ay) + S2 az) (4-33) v” Ξ4 = Ξ4 + 1800 NÕu cžc yÕu tĂš tö sĂš v” mÉu sĂš cña (4-33) tiÕn tĂ­i 0 th× robot rÂŹi v”o t×nh trÂčng suy biÕn nh− tru−ĂȘng hĂźp Ÿ· nĂŁi Ă« mĂŽc 4.2. Ta cĂČng cĂŁ thÓ t×m giž trÞ cña gĂŁc quay Ξ4 b»ng cžch c©n b»ng cžc phÇn tö h”ng 1 cĂ©t 3 v” h”ng 2 cĂ©t 3 cña ph−¬ng tr×nh ma trËn (4-31) , ta cĂŁ : C4S5 = C2(C1 ax + S1 ay) - S2 az

TS. PhÂčm §šng Ph−íc

Page 52: Robot cong nghiep

Robot c«ng nghiÖp 51

S4S5 = -S1 ax + C1 ay Víi ξ5 > 0 ta ¼−üc ξ4 = arctg(-S1 ax + C1 ay, C2(C1 ax + S1 ay) + S2 az) Víi ξ5 < 0 ta ¼−üc ξ4 = ξ4 + 1800

Ÿóng nh− kÕt qu¶ Ÿ· t×m (4-33). Khi S5 = 0, Ξ5 = 0. Robot cĂŁ suy biÕn do c¶ hai trĂŽc cña khĂ­p 4 v” 6 n»m thÂŒng h”ng (z3 ≡ z5). Ă« vÞ trÝ nÇy chØ cĂŁ tĂŠng Ξ4+Ξ6 l” cĂŁ Ăœ nghÜa. Khi Ξ5 = 0, ta cĂŁ thÓ tĂč do chĂ€n mĂ©t giž trÞ cña Ξ4. Th−ĂȘng giž trÞ hiÖn h”nh ¼−üc sö dĂŽng. TĂ” vÕ ph¶i cña ph−¬ng tr×nh A 4 A 1−

31− 2T6 = 4T6 = A5A6 ta cã thÓ cã c¾c

ph−¬ng tr×nh cña S5, C5, S6 v” C6 b»ng cžch c©n b»ng cžc phÇn tö thÝch hĂźp. ChÂŒng hÂčn khi c©n b»ng cžc phÇn tö cña ma trËn h”ng 1 cĂ©t 3 v” h”ng 2 cĂ©t 3 ta cĂŁ : S5 = C4 [C2(C1 ax + S1 ay) - S2 az] + S4(-S1 ax + C1 ay) C5 = S2 (C1 ax + S1 ay) + C2 azTĂ” Ÿã suy ra : Ξ5 = arctg2(C4 [C2(C1 ax + S1 ay) - S2 az] + S4(-S1 ax + C1 ay) , S2 (C1 ax + S1 ay) + C2 az ) (4-34) Cžc ph−¬ng tr×nh cĂŁ liÂȘn quan ¼Õn Ξ6 n»m Ă« cĂ©t 1 cña ph−¬ng tr×nh ma trËn, Ÿã l” cžc th”nh phÇn cña vectÂŹ n cña T6. VectÂŹ nÇy th−ĂȘng kh«ng cĂŁ Ăœ nghÜa trong tÝnh tožn, vÝ nĂŁ lu«n cĂŁ thÓ ¼−üc xžc ¼Þnh b»ng tÝch vectÂŹ cña hai vectÂŹ O v” a nh− Ÿ· nĂŁi tr−íc Ÿ©y ( ). Do Ÿã ta ph¶i t×m cžch khžc ¼Ó xžc ¼Þnh Ξ

r r rn = O x a 6. ThĂčc hiÖn phÐp nh©n cžc ma trËn Ă« vÕ trži cña ph−¬ng tr×nh (4-28) : A 4

51− T = 6

5T = A , biÓu diÔn Ă« dÂčng kĂœ hiÖu ta cĂŁ : 6 6

f51(n) f51(O) 0 0 C6 -S6 0 0 f52(n) f52(O) 0 0 = S6 C6 0 0 (4-35) f53(n) f53(O) 1 0 0 0 1 0

0 0 0 1 0 0 0 1 Trong Ÿã : f51 = C5{ C4 [C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y)} + S5[-S2 (C1 x + S1 y) - C2 z] f52 = -S4 [C2(C1 x + S1 y) - S2 z] + C4[-S1 x + C1 y] f53 = S5{ C4 [C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y)} + C5[S2 (C1 x + S1 y) - C2 z] Cho c©n b»ng cžc phÇn tö Ă« h”ng 1 cĂ©t 2 v” h”ng 2 cĂ©t 2 ta nhËn ¼−üc cžc giž trÞ cña S6 v” C6 : S6 = -C5{C4[C2(C1Ox+S1Oy)-S2Oz] +S4(-S1Ox+C1Oy)} + S5[S2 (C1Ox + S1Oy) + C2Oz] C6 = -S4 [C2(C1Ox + S1Oy)- S2 Oz] + C4[-S1 Ox + C1 Oy] TĂ” Ÿã ta xžc ¼Þnh ¼−üc : Ξ6 = arctg2(S6, C6) (4-36)

TS. PhÂčm §šng Ph−íc

Page 53: Robot cong nghiep

Robot c«ng nghiÖp 52

Cžc biÓu thĂžc (4-29), (4-30), (4-32), (4-33), (4-34) v” (4-36) xžc ¼Þnh tÖp nghiÖm khi gi¶i b”i tožn ng−üc cña robot Stanford. 4.5. Gi¶i b”i tožn Ÿéng hĂ€c ng−üc cña robot ELBOW : §Ó tiÕp tĂŽc l”m quen vĂ­i viÖc gi¶i hÖ ph−¬ng tr×nh Ÿéng hĂ€c, chĂłng ta nghiÂȘn cĂžu phÐp gi¶i b”i tožn Ÿéng hĂ€c ng−üc cña robot Elbow. HÖ ph−¬ng tr×nh Ÿéng hĂ€c thuËn cña robot Elbow Ÿ· d−üc xžc ¼Þnh trong ch−¬ng III. Tr−íc hÕt ta kh¶o sžt ph−¬ng tr×nh :

A 1 T1−

6 = 1T6 = A2A3A4A5A6

T−¬ng tĂč nh− Ÿ· l”m, ta xžc ¼Þnh cžc phÇn tö ma trËn cña hai vÕ nh− sau :

f11(n) f11(O) f11(a) f11(p) f12(n) f12(O) f12(a) f12(p) = f13(n) f13(O) f13(a) f13(p)

0 0 0 1

C234C5C6 - S234S6 -C234C5S6 - S234C6 C234S5 C234a4+C23a3+C2a2

S234C5C6 + C234S6 -S234C5S6 + C234C6 S234S5 S234a4+S23a3+S2a2-S5C6 S5S6 C5 0

0 0 0 1 (4-37) Trong Ÿã : f11 = C1 x + S1 y f12 = z f13 = S1 x + C1 y Ta Ÿ· kĂœ hiÖu : C234 = cos(Ξ2+Ξ3+Ξ4) S234 = sin(Ξ2+Ξ3+Ξ4) Cho c©n b»ng phÇn tö Ă« h”ng 3 cĂ©t 4, ta cĂŁ : S1 px + C1 py = 0 Suy ra : Ξ1 = arctg2(py , px) v” Ξ1 = Ξ1 + 1800 (4-38) Trong tr−ĂȘng hĂźp robot Elbow, ba khĂ­p kÕ tiÕp ¼Òu song song v” kh«ng cĂŁ kÕt qu¶ n”o nhËn ¼−üc tĂ” phÐp nh©n vĂ­i nhĂ·ng ma trËn nghÞch ٦o A-1

i . Cho ¼Õn kh©u thĂž 4 th× phÐp nh©n vĂ­i ma trËn nghÞch ٦o mĂ­i cĂŁ Ăœ nghÜa.

A-14A-1

3A-12

1T6 = 4T6 = A5A6 Khi xžc ¼Þnh cžc phÇn tö ma trËn cña hai vÕ ta ¼−üc :

TS. PhÂčm §šng Ph−íc

Page 54: Robot cong nghiep

Robot c«ng nghiÖp 53

f41(n) f41(O) f41(a) f41(p)-C34a2-C4a3-a4 C5C6 -C5S6 S5 0 f42(n) f42(O) f42(a) 0 = S5C6 -S5S6 -C5 0 f43(n) f43(O) f43(a) f43(p)+S34a2+S4a3 S6 C6 0 0

0 0 0 1 0 0 0 1 (4-39) Trong Ÿã : f41 = C234(C1 x + S1 y) + S234 z f42 = -S1 x + C1 y f43 = -S234(C1 x + S1 y) + C234 z C©n b»ng phÇn tö h”ng 3 cĂ©t 3 ta ¼−üc mĂ©t ph−¬ng tr×nh cho Ξ234 : -S234(C1 ax + S1 ay) + C234 az = 0 Suy ra : Ξ234 = arctg2(az , C1 ax + S1 ay) v” Ξ234 = Ξ234 + 1800 (4-40) B©y giĂȘ ta trĂ« lÂči ph−¬ng tr×nh (4-37). C©n b»ng cžc phÇn tö ma trËn Ă« h”ng 1 cĂ©t 4 v” h”ng 2 cĂ©t 4, ta cĂŁ : C1 px + S1 py = C234a4+C23a3+C2a2 (a) pz = S234a4+S23a3+S2a2 (b) Ta gĂ€i : p’x = C1 px + S1 py - C234a4 (c) p’y = pz - S234a4 (d) §em (a) + (c) v” (b) + (d) ta ¼−üc ; p’x = C23 a3 + C2a2 (e) p’y = S23 a3 + S2a2 (g) B×nh ph−¬ng hai vÕ v” cĂ©ng hai ph−¬ng tr×nh (e) v” (g), ta cĂŁ : p’2

x = (C23 a3 + C2a2)2

p’2y = (S23 a3 + S2a2)2

p’2

x + p’2y = (S 2

23 + C 223 )a2

3 + (S 22 + C 2

2)a22 + 2 a2a3(C23C2 + S23S2 )

Ta cĂŁ C23C2 + S23S2 = cos(Ξ2+Ξ3-Ξ2) = cosΞ3 = C3. NÂȘn suy ra : C3 = (p’2

x + p’2y - a2

3 - a22) / 2a2a3

Trong khi cĂŁ thÓ t×m Ξ3 tĂ” h”m arccos, ta vÉn nÂȘn t×m mĂ©t giž trÞ S3 v” dĂŻng h”m arctg2 nh− th−ĂȘng lÖ :

Ta cĂŁ : S3 = ± −( )1 32C

CÆp nghiÖm Ăžng vĂ­i hai dÊu +,- phĂŻ hĂźp vĂ­i h×nh thÓ cña robot lĂłc n©ng vai lÂȘn v” hÂč vai xuĂšng :

TS. PhÂčm §šng Ph−íc

Page 55: Robot cong nghiep

Robot c«ng nghiÖp 54

Ξ3 = arctg2(S3 , C3) (4-41) §Ó t×m S2 v” C2 ta gi¶i hÖ ph−¬ng tr×nh (e),(g). TĂ” (e) ⇒ (C2C3 - S2S3)a3 + C2a2 = p’x TĂ” (g) ⇒ (S2C3 - C2S3)a3 + S2a2 = p’y Khai triÓn v” rĂłt gĂ€n : (C3a3 + a2)C2 - S3a3.S2 = p’x TĂ” (g) ⇒ S3a3 .C2 + (C3a3 + a2)S2 = p’y Ta cĂŁ :

∆ = C a + a - S a

S a C a + a3 3 2

3 3 3 3 2

3 3

∆ c = p - S a p C a + a

x,

y,

3 3 2

3 3 ∆ s = C a + a p

S a p3 3 2 x

,

3 3 y,

C2 = 233

2233

x33y233C

)a(S)aa(Cp'aS)p'aa(C

++

−+=

∆∆

S2 = ∆∆

S 3 3 2 x 3 3 y

3 3 22

3 32

(C a a )p' S a p'(C a a ) (S a )

=+ +

+ +

Do mÉu sĂš d−¬ng v” b»ng nhau, nÂȘn ta cĂŁ : Ξ2 = arctg2(S2, C2) Ξ2 = arctg2((C3a3 + a2)p’y - S3a3p’x , (C3a3 + a2)p’x + S3a3p’y ) (4-42)

§Õn Ÿ©y Ξ4 ¼−üc xžc ¼Þnh bĂ«i : Ξ4 = Ξ234 - Ξ3 - Ξ2 (4-43) Cžc ph−¬ng tr×nh dĂŻng ¼Ó tÝnh Ξ5 ¼−üc thiÕt lËp tĂ” sĂč c©n b»ng cžc phÇn tö ma trËn h”ng 1 cĂ©t 3 v” h”ng 2 cĂ©t 3 cña ph−¬ng tr×nh 4T6 (4-39) : S5 = C234(C1ax + S1ay) + S234az C5 = S1ax - C1ay Suy ra : Ξ5 = arctg2(C234(C1ax + S1ay) + S234az , S1ax - C1ay) (4-44)

§Ó t×m Ξ6 , ta tiÕp tĂŽc nh©n A-15 vĂ­i

4T6 , ta ¼−üc : A-15 .

4T6 = A6. ViÕt tÝch ma trËn vÕ trži Ă« dÂčng kĂœ hiÖu :

TS. PhÂčm §šng Ph−íc

Page 56: Robot cong nghiep

Robot c«ng nghiÖp 55

f51(n) f51(O) 0 0 C6 -S6 0 0 f52(n) f52(O) 0 0 = S6 C6 0 0

0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1

Trong Ÿã : f51 = C5[C234(C1 x + S1 y) + S234 z] - S5(S1 x + C1 y)

f52 = -S234(C1 x + S1 y) + C234 z Cho c©n b»ng cžc phÇn tö ma trËn t−¬ng Ăžng, ta cĂŁ : S6 = -C5[C234(C1 Ox + S1 Oy) + S234 Oz] - S5(S1 Ox + C1 Oy)

C6 = -S234(C1 Ox + S1 Oy) + C234 OzVËy : Ξ6 = arctg2(S6 , C6) (4-45) Cžc ph−¬ng tr×nh (4-38), (4-41), (4-42), (4-43), (4-44) v” (4-45) xžc ¼Þnh tÖp

nghiÖm khi gi¶i b”i tožn Ÿéng hĂ€c ng−üc cña robot Elbow.

4.6. KÕt luËn : Ph−¬ng phžp gi¶i b”i tožn Ÿéng hĂ€c ng−üc ¼−a ra trong ch−¬ng nÇy sö dĂŽng cžc h”m l−üng gižc tĂč nhiÂȘn. Cžc gĂŁc th−ĂȘng ¼−üc xžc ¼Þnh th«ng qua h”m arctang hai biÕn. Ph−¬ng phžp nÇy ¼−üc ¼−a ra bĂ«i Pieper v” žp dĂŽng tĂšt vĂ­i nhĂ·ng robot Ÿn gi¶n, Th−ĂȘng ta nhËn ¼−üc nghiÖm Ă« dÂčng c«ng thĂžc Ÿn gi¶n. Khi gi¶i b”i tožn Ÿéng hĂ€c ng−üc cĂŁ thÓ x¶y ra hiÖn t−üng “suy biÕn” . Khi cĂŁ nhiÒu hÂŹn mĂ©t tÖp nghiÖm ŸÚi vĂ­i b”i tožn ng−üc ¼Ó xžc ¼Þnh vÞ trÝ v” h−íng cña b”n tay, th× cžnh tay ¼−üc gĂ€i l” suy biÕn. DĂŻng ph−¬ng phžp Pieper, cžc nghiÖm nhËn ¼−üc th−ĂȘng cĂŁ 4 dÂčng c«ng thĂžc, mçi dÂčng cĂŁ mĂ©t Ăœ nghÜa Ÿéng hĂ€c riÂȘng. DÂčng thĂž nhÊt : - sinΊ ax + cosΊ ay = 0 DÂčng nÇy cho ta mĂ©t cÆp nghiÖm cžch nhau 1800, nĂŁ m« t¶ hai h×nh thÓ t−¬ng Ăžng cña robot. NÕu cžc tö sĂš v” mÉu sĂš ¼Òu b»ng kh«ng, robot bÞ suy biÕn, lĂłc Ÿã robot mÊt Âźi mĂ©t bËc tĂč do. DÂčng thĂž hai : -S1px + C1py = d2 DÂčng nÇy cĂČng cho ta cÆp nghiÖm sai khžc nhau 1800, mĂ©t lÇn nĂ·a lÂči tĂ„n tÂči kh¶ nšng suy biÕn khi tö sĂš v” mÉu sĂš b»ng 0. Robot Ă« tr−ĂȘng hĂźp nÇy th−ĂȘng cĂŁ mĂ©t khĂ­p tÞnh tiÕn v” Ÿé d”i tÞnh tiÕn ¼−üc coi l” > 0. DÂčng thĂž ba : C1px + S1py = S2d3 v” dÂčng thĂž t− : - C2d3 = - pz

TS. PhÂčm §šng Ph−íc

Page 57: Robot cong nghiep

Robot c«ng nghiÖp 56

Cžc ph−¬ng th×nh nÇy th−ĂȘng cĂŁ nghiÖm duy nhÊt. Ngo”i cžc dÂčng phĂŠ biÕn, khi robot cĂŁ hai hay nhiÒu khĂ­p song song (VÝ dĂŽ robot Elbow), cžc gĂŁc cña tĂ”ng khĂ­p ph¶i ¼−üc xžc ¼Þnh b»ng cžch gi¶i ŸÄng thĂȘi nhiÒu khĂ­p trong mĂši quan hÖ tĂŠng cžc gĂŁc khĂ­p. T×m ra cžc nghiÖm phĂŻ hĂźp vĂ­i h×nh thÓ cña robot (vÞ trÝ v” h−íng) l” mĂ©t trong nhĂ·ng vÊn ¼Ò khĂŁ khšn nhÊt. HÇu nh− ch−a cĂŁ thuËt tožn chung n”o m” nhĂȘ Ÿã cĂŁ thÓ t×m ra ¼−üc tÖp nghiÖm cho mĂ€i robot. Tuy nhiÂȘn ph−¬ng phžp ¼−a ra trong ch−¬ng nÇy Ÿ· thiÕt lËp ¼−üc cžc nghiÖm sĂš Ă« dÂčng t−ĂȘng minh, trĂčc tiÕp. Khi lËp tr×nh ÂźiÒu khiÓn robot ta ph¶i dĂča v”o cžc giĂ­i hÂčn vËt lĂœ ¼Ó chĂ€n cžc nghiÖm vËt lĂœ, nghÜa l” loÂči trĂ” cžc nghiÖm tožn kh«ng thÝch hĂźp ¼Ó xžc ¼Þnh mĂ©t cÊu h×nh duy nhÊt cña robot.

Ξ1

Ξ2

a1 a2

O0

z1

z2

x1

y1

y2

O1

O2

z0x0

y0

x2

H×nh 4.3 : HÖ toÂč Ÿé v” cžc th«ng sĂš cña robot 2 kh©u phÂŒng

B”i tËp ch−¬ng IV : B”i 1 : Cho mĂ©t vÞ trÝ mong muĂšn cña kh©u chÊp h”nh cuĂši cña robot cĂŁ 3 kh©u phÂŒng nh− h×nh 4.2; DĂŻng ph−¬ng phžp h×nh hĂ€c ¼Ó xžc ¼Þnh cĂŁ bao nhiÂȘu lĂȘi gi¶i cña b”i tožn Ÿéng hĂ€c ng−üc ? NÕu h−íng cña kh©u chÊp h”nh cuĂši cĂČng ¼−üc xžc ¼Þnh, th× cĂŁ bao nhiÂȘu lĂȘi gi¶i ? B”i 2 : DĂŻng ph−¬ng phžp h×nh hĂ€c ¼Ó gi¶i b”i tožn Ÿéng hĂ€c ng−üc (xžc ¼Þnh cžc gĂŁc Ξ1, Ξ2 ) cña robot cĂŁ hai kh©u phÂŒng nh− h×nh 4.3 : H×nh 4.2 : Robot cÊu h×nh RRR

TS. PhÂčm §šng Ph−íc

Page 58: Robot cong nghiep

Robot c«ng nghiÖp 57

B”i 3 : ThiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot SCARA (h×nh 4.4) v” gi¶i b”i tožn Ÿéng hĂ€c ng−üc cña nĂŁ.

O0

Ξ1x

xd3

x

x

x

z3, z4

Ξ2

Ξ4

O3

O4

z0 z1

z2a1

a2

d4

H×nh 4.4 : Robot SCARA

TS. PhÂčm §šng Ph−íc

Page 59: Robot cong nghiep

Robot c«ng nghiÖp 58

Ch−¬ng V

ng«n ngĂ· lËp tr×nh robot (Robot Programming Languages )

5.1. GiĂ­i thiÖu chung vÒ lËp tr×nh ÂźiÒu khiÓn robot : LËp tr×nh ÂźiÒu khiÓn robot thÓ hiÖn mĂši quan hÖ giĂ·a ng−ĂȘi ÂźiÒu khiÓn v” robot c«ng nghiÖp. TÝnh phĂžc tÂčp cña viÖc lËp tr×nh c”ng tšng khi cžc Ăžng dĂŽng c«ng nghiÖp ¼ßi hĂĄi sö dĂŽng ŸÄng thĂȘi nhiÒu robot vĂ­i cžc mžy tĂč Ÿéng kh¶ lËp tr×nh khžc tÂčo nÂȘn hÖ thĂšng s¶n xuÊt tĂč Ÿéng linh hoÂčt.

Robot khžc vĂ­i cžc mžy tĂč Ÿéng cĂš ¼Þnh Ă« tÝnh “linh hoÂčt”, nghÜa l” cĂŁ thÓ lËp

tr×nh ¼−üc (Programmable : kh¶ lËp tr×nh). Kh«ng nhĂ·ng chØ cĂŁ cžc chuyÓn Ÿéng cña robot m” ngay c¶ viÖc sö dĂŽng cžc c¶m biÕn cĂČng nh− nhĂ·ng th«ng tin quan hÖ vĂ­i mžy tĂč Ÿéng khžc trong ph©n xâˆ’Ă«ng cĂČng cĂŁ thÓ lËp tr×nh. Robot cĂŁ thÓ dÔ d”ng thÝch nghi vĂ­i sĂč thay ŸÊi cña nhiÖm vĂŽ s¶n xuÊt b»ng cžch thay ŸÊi ch−¬ng tr×nh ÂźiÒu khiÓn nĂŁ.

Khi xem xÐt vÊn ¼Ò lËp tr×nh cho robot, chĂłng ta nÂȘn nhĂ­ r»ng robot l” mĂ©t th”nh phÇn cña mĂ©t quž tr×nh ¼−üc tĂč Ÿéng hož. ThuËt ngĂ·, workcell ¼−üc dĂŻng ¼Ó m« t¶ mĂ©t tËp hĂźp cžc thiÕt bÞ m” nĂŁ bao gĂ„m mĂ©t hoÆc nhiÒu robot, hÖ thĂšng bšng chuyÒn, cžc cÂŹ cÊu cÊp ph«i v” ŸÄ gž. Ă« mĂžc cao hÂŹn, Workcell cĂŁ thÓ ¼−üc liÂȘn kÕt trong mÂčng l−íi cžc ph©n xâˆ’Ă«ng v× thÕ mžy tÝnh ÂźiÒu khiÓn trung t©m cĂŁ thÓ ÂźiÒu khiÓn to”n bĂ© cžc hoÂčt Ÿéng cña ph©n xâˆ’Ă«ng. V× vËy, viÖc lËp tr×nh ÂźiÒu khiÓn robot trong thĂčc tÕ s¶n xuÊt cÇn ph¶i ¼−üc xem xÐt trong mĂši quan hÖ rĂ©ng hÂŹn. §Ó b−íc ¼Çu l”m quen vĂ­i viÖc lËp tr×nh ÂźiÒu khiÓn robot, ch−¬ng nÇy cĂČng giĂ­i thiÖu tĂŁm tŸt ph−¬ng phžp lËp tr×nh ÂźiÒu khiÓn robot TERGAN-45 th«ng qua ng«n ngĂ· ASPECT cña phÇn mÒm Procomm Plus for Window 5.2. Cžc mĂžc lËp tr×nh ÂźiÒu khiÓn robot : Ng−ĂȘi sö dĂŽng cĂŁ thÓ cĂŁ nhiÒu kiÓu giao diÖn lËp tr×nh ÂźiÒu khiÓn robot. Tr−íc sĂč phžt triÓn nhanh chĂŁng cña cžc loÂči mžy vi tÝnh dĂŻng trong c«ng nghiÖp v” cžc ng«n ngĂ· lËp tr×nh ng”y c”ng cĂŁ nhiÒu tiÖn Ých cao, viÖc lËp tr×nh ÂźiÒu khiÓn robot ng”y c”ng dÔ d”ng v” thuËn tiÖn hÂŹn.

TS. PhÂčm §šng Ph−íc

Page 60: Robot cong nghiep

Robot c«ng nghiÖp 59

5.2.1. LËp tr×nh kiÓu “DÂčy - HĂ€c” : Cžc robot thÕ hÖ ¼Çu tiÂȘn Ÿ· ¼−üc lËp tr×nh b»ng mĂ©t ph−¬ng phžp m” chĂłng ta gĂ€i l” : dÂčy b»ng chØ dÉn (Teach by showing), robot ¼−üc ÂźiÒu khiÓn ¼Ó di chuyÓn ¼Õn cžc ÂźiÓm mong muĂšn v” cžc vÞ trÝ Ÿã ¼−üc ghi lÂči trong bĂ© nhĂ­ cña mžy tÝnh, sau Ÿã cžc dĂ· liÖu sÏ ¼−üc ŸÀc tuÇn tĂč v” robot thĂčc hiÖn lÂči cžc Ÿéng tžc Ÿ· ¼−üc hĂ€c. §Ó dÂčy robot, ng−ĂȘi sö dĂŽng cĂŁ thÓ h−íng dÉn robot b»ng tay hoÆc th«ng qua mĂ©t thiÕt bÞ dÂčy hĂ€c gĂ€i l” Teach pendant. ThiÕt bÞ dÂčy hĂ€c gĂ„m mĂ©t hĂ©p nhĂĄ cÇm tay (teaching box) cĂŁ cžc nĂłt bÊm v” card ÂźiÒu khiÓn m” nĂŁ cho phÐp ÂźiÒu khiÓn cžc khĂ­p cña robot ÂźÂčt ¼−üc cžc giž trÞ mong muĂšn. 5.2.2. DĂŻng cžc ng«n ngĂ· lËp tr×nh : CĂŻng vĂ­i quž tr×nh phžt triÓn ng”y c”ng rÎ hÂŹn v” mÂčnh hÂŹn cña mžy tÝnh,, ch−¬ng tr×nh ÂźiÒu khiÓn robot ¼−üc phžt triÓn theo h−íng viÕt cžc ch−¬ng tr×nh b»ng cžc ng«n ngĂ· lËp tr×nh cña mžy tÝnh. Th−ĂȘng cžc ng«n ngĂ· lËp tr×nh nÇy cĂŁ nhĂ·ng ¼Æc ÂźiÓm m” chĂłng ta cĂŁ thÓ Ăžng dĂŽng ¼Ó viÕt cžc phÇn mÒm hay ch−¬ng tr×nh ÂźiÒu khiÓn robot, v” chĂłng ¼−üc gĂ€i l” “ng«n ngĂ· lËp tr×nh robot”. HÇu hÕt cžc hÖ thĂšng ÂźiÒu khiÓn dĂŻng ng«n ngĂ· lËp tr×nh robot vÉn duy tr× kiÓu giao diÖn Teach pendant (dÂčy- hĂ€c). Ng«n ngĂ· lËp tr×nh robot cĂŁ nhiÒu dÂčng khžc nhau. ChĂłng ta ph©n chĂłng th”nh ba loÂči nh− sau : a) Ng«n ngĂ· robot chuyÂȘn dĂŻng : nhĂ·ng ng«n ngĂ· lËp tr×nh robot nÇy ¼−üc x©y dĂčng b»ng cžch tÂčo ra mĂ©t ng«n ngĂ· mĂ­i ho”n to”n. CĂł phžp (Syntax) v” ngĂ· nghÜa (Semantics) cña cžc ng«n ngĂ· nÇy cÇn ph¶i rÊt Ÿ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” mĂ©t chuyÂȘn gia vÒ lËp tr×nh. VÝ dĂŽ nh− ng«n ngĂ· VAL (VAL 2) ¼−üc dĂŻng ¼Ó ÂźiÒu khiÓn cžc robot c«ng nghiÖp cña h·ng Unimation (Hoa kĂș); hoÆc mĂ©t ng«n ngĂ· robot chuyÂȘn dĂŻng khžc gĂ€i l” AL ¼−üc x©y dĂčng Ă« §Âči hĂ€c Stanford (hoa kĂș)... b) TÂčo ra cžc th− viÖn robot cho mĂ©t ng«n ngĂ· lËp tr×nh cÊp cao Ÿ· cĂŁ sÂŒn : NhĂ·ng ng«n ngĂ· lËp tr×nh robot nÇy ¼−üc x©y dĂčng b»ng cžch dĂča trÂȘn cžc ng«n ngĂ· lËp tr×nh cÊp 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 cžc ch−¬ng tr×nh Pascal ¼Ó ÂźiÒu khiÓn robot, ng−ĂȘi sö dĂŽng sÏ gĂ€i cžc h”m hoÆ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Ön 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 robot 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 viÕt trÂȘn cÂŹ sĂ« cña ng«n ngĂ· Turbo C.

c) TÂčo ra cžc th− viÖn robot cho mĂ©t ng«n ngĂ· hoÆc phÇn mÒm Âźa dĂŽng

(Robot library for a new general - purpose language) : NhĂ·ng ng«n ngĂ· lËp tr×nh robot nÇy ¼−üc x©y dĂčng b»ng cžch sö dĂŽng cžc ng«n ngĂ· hoÆc phÇn mÒm dĂŻng

TS. PhÂčm §šng Ph−íc

Page 61: Robot cong nghiep

Robot c«ng nghiÖp 60

chung cho nhiÒu mĂŽc ¼Ých nh− l” mĂ©t ch−¬ng tr×nh cÂŹ b¶n, sau Ÿã cung cÊp thÂȘm mĂ©t th− viÖn chĂža cžc thñ tĂŽc ¼Æc biÖt dĂŻng cho robot. VÝ dĂŽ nh− ng«n ngĂ· lËp tr×nh robot AML cña h·ng IBM v” RISE cña h·ng Silma, ng«n ngĂ· Aspect cña phÇn mÒm Procomm Plus ...

5.2.3. Ng«n ngĂ· lËp tr×nh theo nhiÖm vĂŽ (Task-level programming language)

MĂžc thĂž ba cña ph−¬ng phžp lËp tr×nh robot l” tÂčo ra cžc ng«n ngĂ· lËp tr×nh theo nhiÖm vĂŽ. NhĂ·ng ng«n ngĂ· nÇy cho phÐp ng−ĂȘi sö dĂŽng ra cžc lÖnh ¼Ó robot thĂčc hiÖn mĂ©t c«ng viÖc mong muĂšn mĂ©t cžch trĂčc tiÕp m” kh«ng cÇn xžc ¼Þnh mĂ©t cžch chi tiÕt cžc hoÂčt Ÿéng cña robot nh− cžc ng«n ngĂ· lËp tr×nh th«ng th−ĂȘng. MĂ©t hÖ thĂšng lËp tr×nh robot theo nhiÖm vĂŽ ph¶i cĂŁ kh¶ nšng thÓ hiÖn nhiÒu c«ng viÖc mĂ©t cžch tĂč Ÿéng. ChÂŒng hÂčn, nÕu mĂ©t chØ thÞ “Grasp the bolt” (cÇm lÊy bulong) ¼−üc tÂčo ra, hÖ thĂšng ph¶i vÂčch ra ¼−üc mĂ©t quÜ ÂźÂčo cña tay mžy m” nĂŁ tržnh ¼−üc sĂč va chÂčm vĂ­i bÊt kĂș ch−íng ngÂči vËt n”o chung quanh, chĂ€n ¼−üc vÞ trÝ tĂšt nhÊt ¼Ó cÇm lÊy bulong mĂ©t cžch tĂč Ÿéng. Ng−üc lÂči, trong ng«n ngĂ· lËp tr×nh robot th«ng th−ĂȘng tÊt c¶ nhĂ·ng sĂč lĂča chĂ€n nÇy ph¶i ¼−üc thĂčc hiÖn bĂ«i ng−ĂȘi lËp tr×nh. Trong thĂčc tÕ, ng«n ngĂ· lËp tr×nh theo nhiÖm vĂŽ ch−a ¼−üc dĂŻng trong s¶n xuÊt, nĂŁ cßn l” mĂ©t lÜnh vĂčc Âźang ¼−üc nghiÂȘn cĂžu. Sau Ÿ©y ta sÏ nghiÂȘn cĂžu mĂ©t phÇn mÒm Âźa dĂŽng dĂŻng truyÒn dĂ· liÖu v” ÂźiÒu khiÓn thiÕt bÞ cĂŁ thÓ dĂŻng ¼Ó ÂźiÒu khiÓn robot. 5.3. GiĂ­i thiÖu tĂŁm tŸt phÇn mÒm Procomm Plus For Windows :

Procomm Plus l” phÇn mÒm dĂŻng ¼Ó truyÒn dĂ· liÖu v” ÂźiÒu khiÓn trĂčc tiÕp cžc

thiÕt bÞ qua cĂŠng COM cña mžy tÝnh cž nh©n. VĂ­i Procomm Plus ta cĂŁ thÓ sö dĂŽng mžy tÝnh nh− mĂ©t Terminal hoÆc thĂčc hiÖn cžc Scrip files viÕt b»ng ng«n ngĂ· lËp tr×nh Aspect.

§Ó chÂčy phÇn mÒm Procomm Plus Ă« chÕ Ÿé Terminal ta cĂŁ thÓ dĂŻng mĂ©t sĂš cžch sau : a) Sö dĂŽng Desktop Windows : Ên Ÿóp chuĂ©t trÂȘn biÓu t−üng cña Procomm Plus terminal Windows. b) TĂ” mĂŽc Run... trong Start cña Windows, gĂą lÖnh : pw3 , chĂ€n OK. c) V”o Start cña Windows, chĂ€n Programs, chĂ€n Procomm Plus 3, chĂ€n Data Terminal...

Menu chÝnh cña Procomm Plus cĂŁ nhiÒu tiÖn Ých, rÊt thuËn tiÖn khi ÂźiÒu khiÓn cžc thiÕt bÞ giao diÖn vĂ­i mžy tÝnh kiÓu RS 232.

Cña sĂŠ chÝnh cña phÇn mÒm Procomm plus Ă« chÕ Ÿé Terminal nh− h×nh 5.1.

TS. PhÂčm §šng Ph−íc

Page 62: Robot cong nghiep

Robot c«ng nghiÖp 61

Thanh c«ng cĂŽ Menu chÝnh

Cöa sĂŠ nhËp - xu©t dĂ· liÖu .

Dßng chĂ€n nhanh kiÓu giao diÖnMeta keys Thanh trÂčng thži

H×nh 5.1 : Cöa sĂŠ chÝnh cña Procomm Plus for Windows, Version 3.0

Menu chÝnh : Cung cÊp cžc tiÖn Ých cÇn thiÕt trong quž tr×nh sö dĂŽng, menu chÝnh cĂŁ cžc menu kÐo xuĂšng (Pulldown) t−¬ng tĂč nh− nhiÒu phÇn mÒm th«ng dĂŽng khžc. NĂ©i dung cña Menu chÝnh cĂŁ thÓ thay ŸÊi ¼−üc theo mĂŽc ¼Ých sö dĂŽng. MĂ©t sĂš nĂ©i dung cña menu chÝnh cĂŁ thÓ dĂŻng trong quž tr×nh ÂźiÒu khiÓn robot nh− sau : Menu Setup : DĂŻng ¼Ó xžc ¼Þnh cÊu h×nh cña Terminal Windows v” chÕ Ÿé giao diÖn giĂ·a mžy tÝnh vĂ­i thiÕt bÞ. Trong menu nÇy cßn cĂŁ thÓ sö dĂŽng mĂŽc con Action Bars ¼Ó chĂ€n file chĂža nĂ©i dung cña thanh c«ng cĂŽ v” cho thÓ hiÖn trÂȘn m”n h×nh. Menu Data : Trong menu nÇy ta cĂŁ thÓ dĂŻng cžc menu con sau : + Clear screen (Alt+C): Xož m”n h×nh nhËp xuÊt dĂ· liÖu; + Reset terminal (Alt+U): Xož m”n h×nh v” bĂ© ¼Öm (buffer) cña Procomm. Menu Scripts : Trong menu nÇy ta cĂŁ thÓ dĂŻng cžc menu con sau : + Start scrips (Alt+.) : ThĂčc hiÖn mĂ©t Aspect scrips file, cĂŁ tÂȘn ¼−üc thÓ hiÖn trÂȘn thanh c«ng cĂŽ. + Run... (Alt+F5) : MĂ« hĂ©p hĂ©i thoÂči Run ASPECT file , chĂłng ta cĂŁ thÓ chĂ€n tÂȘn file, thĂčc hiÖn viÖc dÞch cžc file nguĂ„n tr−íc khi chÂčy ch−¬ng tr×nh. + Compile / Edit... (Alt+F3) : MĂ« hĂ©p hĂ©i thoÂči soÂčn th¶o v” dÞch cžc file nguĂ„n.

TS. PhÂčm §šng Ph−íc

Page 63: Robot cong nghiep

Robot c«ng nghiÖp 62

+ Start recorder... : bŸt ¼Çu tĂč Ÿéng tÂčo ra mĂ©t scrips file b»ng cžc ghi lÂči tÊt c¶ cžc lÖnh thÓ hiÖn trÂȘn m”n h×nh (nhËp tĂ” b”n phÝm). Khi chĂ€n mĂŽc nÇy sÏ xuÊt hiÖn mĂŽc Stop recorder, dĂŻng khi muĂšn kÕt thĂłc viÖc ghi tĂč Ÿéng scrips file.

CĂŁ thÓ chĂ€n mĂŽc nÇy trÂȘn thanh c«ng cĂŽ b»ng cžch Ên chuĂ©t v”o biÓu t−üng . Menu Tools : Trong menu nÇy ta cĂŁ thÓ dĂŻng cžc menu con sau : + Action bar Edition : DĂŻng ¼Ó soÂčn th¶o hay thay ŸÊi nĂ©i dung thanh c«ng cĂŽ cho phĂŻ hĂźp vĂ­i mĂŽc ¼Ých sö dĂŽng. + Aspect Editor : MĂ« cöa sĂŠ ¼Ó soÂčn th¶o script files b»ng ng«n ngĂ· Aspect, chĂłng ta cĂŁ thÓ tÂčo mĂ­i, xem hoÆc söa ŸÊi nĂ©i dung cña mĂ©t file (dÂčng Text). + Dialog Editor : MĂ« cöa sĂŠ soÂčn th¶o hĂ©p hĂ©i thoÂči, cho phÐp ta tÂčo ra cžc hĂ©p hĂ©i thoÂči b»ng ph−¬ng phžp trĂčc quan (Visual). Thanh c«ng cĂŽ (Tool bar) : cĂŁ nhiÒu Icon (biÓu t−üng) giĂłp ng−ĂȘi sö dĂŽng cĂŁ thÓ thĂčc hiÖn nhanh mĂ©t c«ng viÖc b»ng cžch bÊm chuĂ©t trÂȘn biÓu t−üng t−¬ng Ăžng, thay v× ph¶i v”o Menu chÝnh. NĂ©i dung cña thanh c«ng cĂŽ cĂČng cĂŁ thÓ thay ŸÊi dÔ d”ng ¼Ó phĂŻ hĂźp vĂ­i mĂŽc ¼Ých sö dĂŽng (mĂŽc Action bar Edition). Cöa sĂŠ nhËp - xuÊt dĂ· liÖu : l” phÇn m”n h×nh ¼Ó ng−ĂȘi sö dĂŽng nhËp v”o cžc dĂ· liÖu, cžc lÖnh ÂźiÒu khiÓn v” cžc thÓ hiÖn th«ng bžo tr¶ vÒ tĂ” cžc thiÕt bÞ ¼−üc ÂźiÒu khiÓn. Meta Keys : DĂŻng ¼Ó c”i ¼Æt sÂŒn cžc Ăžng dĂŽng th−ĂȘng hay thĂčc hiÖn. NĂ©i dung cña cžc Meta Keys cĂŁ thÓ thay ŸÊi ¼−üc ¼Ó phĂŻ hĂźp vĂ­i tĂ”ng mĂŽc ¼Ých sö dĂŽng. Khi muĂšn thĂčc hiÖn mĂ©t c«ng viÖc Ÿ· gžn cho Meta Key chØ cÇn Ên chuĂ©t v”o Meta key Ÿã. MuĂšn soÂčn th¶o hay thay ŸÊi nhiÖm vĂŽ cña Meta Keys ta thĂčc hiÖn nh− sau : Cžch 1 : Ên phÝm ALT+M . Cžch 2 : ChĂ€n mĂŽc Meta Keys Editor tĂ” Tool menu . Dßng chĂ€n nhanh kiÓu giao diÖn : Cho phÐp ng−ĂȘi sö dĂŽng chĂ€n nhanh kiÓu th«ng sĂš giao diÖn giĂ·a thiÕt bÞ ÂźiÒu khiÓn v” mžy tÝnh nh− : cĂŠng giao diÖn, tĂšc Ÿé truyÒn th«ng tin, kiÓu xö lĂœ dĂ· liÖu ... b»ng cžch Ên nĂłt chuĂ©t trži lÂȘn cžc mĂŽc. 5.4. Ng«n ngĂ· lËp tr×nh ASPECT trong Procomm : 5.4.1. GiĂ­i thiÖu : MĂ©t ASPECT script file l” mĂ©t file dÂčng text ¼−üc tÂčo ra ¼Ó chĂža cžc lÖnh ¼−üc thĂčc hiÖn bĂ«i Procomm Plus. GiĂšng nh− nhiÒu ng«n ngĂ· lËp tr×nh khžc, ASPECT yÂȘu cÇu ph¶i dÞch ch−¬ng tr×nh soÂčn th¶o. MĂ©t script file ch−a dÞch, hay cßn gĂ€i l” file nguĂ„n, cĂŁ Âźu«i l” .was (Windows Aspect Source); cßn mĂ©t script file Ÿ· dÞch cĂŁ Âźu«i l” .wax (Windows Aspect eXecutable). Khi mĂ©t script Ÿ· ¼−üc dÞch, th× cžc dĂ· liÖu v” cžc c©u lÖnh chĂža trong file nguĂ„n sÏ ¼−üc chuyÓn sang m· m” Procomm cĂŁ thÓ ŸÀc v” xö lĂœ mĂ©t cžch nhanh chĂŁng. Sau khi dÞch th× file dÞch (.wax) cĂŁ kÝch th−íc nhĂĄ hÂŹn so vĂ­i file nguĂ„n. TĂŁm lÂči : mĂ©t script file ph¶i ¼−üc dÞch tr−íc khi cĂŁ thÓ thĂčc hiÖn. MĂ©t file Ÿ· ¼−üc dÞch kh«ng thÓ dÞch ng−üc trĂ« lÂči th”nh file nguĂ„n.

TS. PhÂčm §šng Ph−íc

Page 64: Robot cong nghiep

Robot c«ng nghiÖp 63

ChĂłng ta cĂŁ thÓ tÂčo mĂ­i v” soÂčn th¶o file nguĂ„n (.was) b»ng tr×nh ASPECT Editor hay bÊt kĂș mĂ©t tr×nh soÂčn th¶o dÂčng text n”o khžc, nh−ng ph¶i ¼Æt tÂȘn tÖp cĂŁ Âźu«i l” . was. §Ó tÂčo mĂ­i mĂ©t file nguĂ„n hoÆc thay ŸÊi bĂŠ sung nĂ©i dung cña mĂ©t file Ÿ· cĂŁ, tĂ” menu chÝnh cña Procomm, chĂ€n Scripts | Compile/Edit... hoÆc Ên chuĂ©t v”o biÓu

t−üng trÂȘn thanh c«ng cĂŽ. HĂ©p hĂ©i thoÂči dĂŻng ¼Ó soÂčn th¶o v” dÞch cžc script files nh− h×nh 5.3. MuĂšn tÂčo mĂ©t file mĂ­i ta chĂ€n nĂłt lÖnh New; muĂšn söa ŸÊi nĂ©i dung mĂ©t file Ÿ· cĂŁ (tÂȘn file Ÿ· chĂ€n tr−íc trong mĂŽc File name) ta chĂ€n nĂłt lÖnh Edit; muĂšn thožt khĂĄi cña sĂŠ soÂčn th¶o ta chĂ€n nĂłt lÖnh Exit. Khi chĂ€n nĂłt lÖnh New hoÆc Edit, trÂȘn m”n h×nh sÏ xuÊt hiÖn cña sĂŠ soÂčn th¶o ¼Ó ta viÕt hoÆc sö ŸÊi ch−¬ng tr×nh. Sau khi soÂčn th¶o xong, muĂšn ghi v”o ¼Üa ta chĂ€n File | Save hoÆc File | Save as ... Ta cĂČng cĂŁ thÓ chĂ€n biÓu tuĂźng “Ghi v” dÞch” (Save and Compile) trÂȘn thanh c«ng cĂŽ ¼Ó ghi v”o ¼Üa ŸÄng thĂȘi dÞch th”nh file .wax.

H×nh 5.3 : Cöa sĂŠ soÂčn th¶o v” dÞch cžc script file §Ó chÂčy mĂ©t Aspect script file cĂŁ thÓ thĂčc hiÖn b»ng nhiÒu cžch : + ChĂ€n mĂŽc Script trÂȘn Menu chÝnh, tiÕp theo chĂ€n mĂŽc Run... LĂłc nÇy sÏ xuÊt hiÖn hĂ©p hĂ©i thoÂči ¼Ó chĂ€n file muĂšn thĂčc hiÖn. + Ên chuĂ©t trÂȘn mĂŽc Script file cña thanh c«ng cĂŽ, sau Ÿã chĂ€n tÂȘn file muĂšn thĂčc hiÖn. NÕu mĂ©t file Ÿ· chÂčy, tÂȘn vÉn cßn trong mĂŽc Script file, muĂšn chÂčy lÂči

thÝ Ên chuĂ©t v”o biÓu t−üng trÂȘn thanh c«ng cĂŽ. + CĂŁ thÓ chÂčy mĂ©t script file tĂ” cña sĂŠ Compile/Edit ASPECT file (ChĂ€n mĂŽc RUN) (h×nh 5.3).

TS. PhÂčm §šng Ph−íc

Page 65: Robot cong nghiep

Robot c«ng nghiÖp 64

5.4.2. KiÓu dĂ· liÖu v” khai bžo biÕn trong ASPECT : a) KiÓu dĂ· liÖu : ASPECT cung cÊp cžc kiÓu dĂ· liÖu nh− sau : integer (kiÓu nguyÂȘn) : CĂŁ giž trÞ tĂ” -32768 ¼Õn 32767. float (kiÓu sĂš thĂčc) : CĂŁ giž trÞ tĂ” 2.22507385072014e-308 ¼Õn 1.797693134862315e+308. long (kiÓu nguyÂȘn d”i) : CĂŁ giž trÞ tĂ” -2147483648 ¼Õn 2147483647. String (kiÓu chuĂŠi) : CĂŁ thÓ chĂža tĂ” 0 ¼Õn 256 kĂœ tĂč. TÊt c¶ tÂȘn cña cžc phÇn tö trong ASPECT, nh− tÂȘn tĂ” lÖnh, tÂȘn h”m v” thñ tĂŽc, tÂȘn nh·n (label) v” biÕn ... cĂŁ chiÒu d”i kh«ng quž 30 kĂœ tĂč.

b) Cžc loÂči biÕn : Trong ASPECT cĂŁ cžc loÂči biÕn sau :

+ BiÕn hÖ thĂšng : BiÕn hÖ thĂšng l” cžc biÕn “chØ ŸÀc” (read-only) m” ASPECT v” Procomm Plus cĂŁ thÓ Ên ¼Þnh cžc giž trÞ ¼Æc biÖt. VÝ dĂŽ : chĂłng ta kh«ng thÓ thay ŸÊi giž trÞ cña biÕn hÖ thĂšng $ROW m” nĂŁ lu«n lu«n b»ng vÞ trÝ dßng hiÖn tÂči cña con trĂĄ trÂȘn m”n h×nh, ta chØ cĂŁ thÓ ŸÀc giž trÞ cña nĂŁ bÊt kĂș nÂŹi n”o trong ch−¬ng tr×nh v” xö lĂœ khi cÇn thiÕt. BiÕn hÖ thĂšng lu«n cĂŁ dÊu $ Ă« ¼Çu. + BiÕn do ng−ĂȘi dĂŻng ¼Þnh nghÜa , cĂŁ hai loÂči : - BiÕn to”n cĂŽc (Global variables) : BiÕn to”n cĂŽc cĂŁ thÓ ¼−üc ¼Þnh nghÜa Ă« bÊt kĂș nÂŹi n”o trong ch−¬ng tr×nh nh−ng ph¶i Ă« bÂȘn ngo”i cžc khĂši Thñ tĂŽc v” H”m. PhĂŠ biÕn , cžc biÕn to”n cĂŽc th−ĂȘng ¼−üc khai bžo Ă« ¼Çu ch−¬ng tr×nh. BiÕn to”n cĂŽc cĂŁ thÓ ¼−üc tham chiÕu ¼Õn tĂ” bÊt cĂž h”m hay thñ tĂŽc n”o cña ch−¬ng tr×nh, NÕu mĂ©t thñ tĂŽc hoÆc h”m l”m thay ŸÊi giž trÞ cña mĂ©t biÕn to”n cĂŽc th× giž trÞ Ÿã vÉn ¼−üc duy tr× cho ¼Õn khi n”o cĂŁ mĂ©t lÖnh khžc l”m thay ŸÊi giž trÞ cña nĂŁ. - BiÕn ¼Þa ph−¬ng (Local variables) : Kh«ng giĂšng nh− biÕn to”n cĂŽc, biÕn ¼Þa ph−¬ng chØ ¼−üc tham kh¶o ¼Õn trong phÂčm vi cña thñ tĂŽc v” h”m m” nĂŁ ¼−üc ¼Þnh nghÜa. Giž trÞ cña nĂŁ sÏ bÞ xož khi ra khĂĄi thñ tĂŽc v” h”m Ÿã. Ta cĂŁ thÓ ¼Æt tÂȘn cžc biÕn ¼Þa ph−¬ng giĂšng nhau trong cžc thñ tĂŽc v” h”m khžc nhau cña ch−¬ng tr×nh, nh−ng ÂźiÒu Ÿã kh«ng cĂŁ nghÜa l” giž trÞ cña biÕn ¼−üc ghi nhĂ­ giĂ·a cžc thñ tĂŽc hoÆc h”m khžc nhau. + Tham biÕn (Parameter variables): BÊt cĂž thñ tĂŽc n”o, ngoÂči trĂ” ch−¬ng tr×nh chÝnh (Proc main) ¼Òu cĂŁ thÓ khai bžo (¼Þnh nghÜa) ¼Õn 12 tham biÕn. Cžc tham biÕn t−¬ng tĂč nh− cžc biÕn ¼Þa ph−¬ng, nghÜa l” nĂŁ chØ ¼−üc tham chiÕu ¼Õn trong phÂčm vi thñ tĂŽc hoÆc h”m m” nĂŁ ¼−üc ¼Þnh nghÜa, tuy nhiÂȘn khžc vĂ­i biÕn ¼Þa ph−¬ng, cžc tham biÕn nhËn cžc giž trÞ ban ¼Çu mĂ©t cžch tĂč Ÿéng khi cžc thñ tĂŽc hoÆc h”m ¼−üc gĂ€i, cžc gÝa trÞ sö dĂŽng ¼−üc cung cÊp bĂ«i c©u lÖnh gĂ€i. Cžc tham biÕn ph¶i ¼−üc khai bžo Ă« ¼Çu mçi thñ tĂŽc hoÆc h”m, tr−íc bÊt cĂž lÖnh n”o hoÆc cžc biÕn ¼Þa ph−¬ng. MĂ©t tham biÕn ¼−üc khai bžo

TS. PhÂčm §šng Ph−íc

Page 66: Robot cong nghiep

Robot c«ng nghiÖp 65

giĂšng nh− biÕn ¼Þa ph−¬ng. ThĂž tĂč m” cžc tham biÕn ¼−üc ¼Þnh nghÜa xžc ¼Þnh thĂž tĂč m” chĂłng sÏ ¼−üc gĂ€i bĂ«i cžc thñ tĂŽc hoÆc h”m. c) Khai bžo (¼Þnh nghÜa) cžc biÕn : TÊt c¶ cžc loÂči biÕn dĂŻng trong ch−¬ng tr×nh ph¶i ¼−üc khai bžo (¼Þnh nghÜa) tr−íc. NÕu cžc biÕn cĂŁ cĂŻng kiÓu dĂ· liÖu, ta cĂŁ thÓ khai bžo trÂȘn mĂ©t dßng cžch nhau bĂ«i dÊu phÈy ( , ).

VÝ dĂŽ : Integer sokhop, Tong, i = 1 Float Goc Integer A[4][4] Trong vÝ dĂŽ trÂȘn ta khai bžo cžc biÕn : sokhop, Tong, i l” cžc biÕn nguyÂȘn, trong Ÿã biÕn i ¼−üc gžn giž trÞ ban ¼Çu l” 1. Goc l” biÕn thĂčc. A l” biÕn m·ng (array) cĂŁ kÝch th−íc 4x4 , cžc phÇn tö cña m·ng kiÓu nguyÂȘn. Cžch khai bžo tham biÕn trong thñ tĂŽc v” h”m nh− sau : param (kiÓu dĂ· liÖu ) (tÂȘn) [, tÂȘn] . . . VÝ dĂŽ : param Integer X, Y, Z Ch−¬ng tr×nh vÝ dĂŽ : ; Vi du ve khai bao bien. Proc main ; Ch−¬ng tr×nh chÝnh. integer A,B,C ; Khai bžo 3 biÕn nguyÂȘn. integer Tong ; TĂŠng cña 3 sĂš (biÕn nguyÂȘn). A=2, B=4, C=8 ; Gžn giž trÞ cho cžc biÕn. Tong = Sum(A,B,C) ; GĂ€i h”m Sum ¼Ó cĂ©ng cžc sĂš. Usermsg “ Tong = %d.” Tong ; Cho hiÖn tĂŠng cña cžc sĂš lÂȘn m”n h×nh Endproc ; HÕt ch−¬ng tr×nh chÝnh. Func Sum : Integer ; §Þnh nghÜa h”m Sum ¼Ó tÝnh tĂŠng. Param integer X, Y, Z ; Khai bžo cžc tham biÕn kiÓu nguyÂȘn. integer Tong ; Khai bžo biÕn Tong (biÕn ¼Þa ph−¬ng). Tong= X+Y+Z ; TĂŠng cña 3 sĂš. return Tong ; Tr¶ vÒ giž trÞ cña tĂŠng cña 3 sĂš. Endfunc ; hÕt phÇn ¼Þnh nghÜa h”m (Ghi chĂł : dÊu “;” dĂŻng ¼Ó ghi chĂł trong ch−¬ng tr×nh, cžc nĂ©i dung sau dÊu “; “ kh«ng ¼−üc dÞch). 5.4.3. CÊu trĂłc cña ch−¬ng tr×nh : CÊu trĂłc ch−¬ng tr×nh cña mĂ©t ASPECT script file gÇn giĂšng nh− mĂ©t file viÕt b»ng ng«n ngĂ· Pascal, nghÜa l” cĂŁ mĂ©t ch−¬ng tr×nh chÝnh v” cžc thñ tĂŽc hoÆc h”m khžc. Chç khžc nhau cÂŹ b¶n l” ch−¬ng tr×nh chÝnh ¼−üc viÕt tr−íc, ch−¬ng tr×nh chÝnh cĂŁ thÓ gĂ€i ¼Õn cžc h”m hoÆc thñ tĂŽc ¼−üc ¼Þnh nghÜa sau Ÿã. Trong ch−¬ng tr×nh chÝnh kh«ng ¼−üc khai bžo cžc tham biÕn. Khi thĂčc hiÖn ch−¬ng tr×nh, nĂŁ sÏ lÇn l−üt thĂčc hiÖn cžc lÖnh tĂ” dßng ¼Çu tiÂȘn ¼Õn hÕt ch−¬ng tr×nh.

TS. PhÂčm §šng Ph−íc

Page 67: Robot cong nghiep

Robot c«ng nghiÖp 66

Khi kÕt thĂłc mĂ©t h”m hoÆc thñ tĂŽc ¼−üc gĂ€i, nĂŁ tĂč Ÿéng tr¶ vÒ dßng lÖnh tiÕp theo. CÊu trĂłc chung cña mĂ©t ch−¬ng tr×nh nh− sau : ; Dßng ¼Çu tiÂȘn dĂŻng ghi chĂł vÒ nĂ©i dung ch−¬ng tr×nh, dßng nÇy sÏ thÓ hiÖn trong ; hĂ©p hĂ©i thoÂči Compile/Edit ¼Ó ng−ĂȘi sö dĂŽng dÔ nhËn biÕt vÒ nĂ©i dung cña ch−¬ng ; tr×nh. Proc main ; bŸt ¼Çu ch−¬ng tr×nh chÝnh (Khai bžo biÕn) (cžc c©u lÖnh thÓ hiÖn nĂ©i dung ch−¬ng tr×nh) . . . . . Endproc ; hÕt ch−¬ng tr×nh chÝnh. Proc (tÂȘn thñ tĂŽc) ; BŸt ¼Çu mĂ©t thñ tĂŽc (khai bžo cžc tham biÕn nÕu cĂŁ) (khai bžo cžc biÕn ¼Þa ph−¬ng) (cžc c©u lÖnh thÓ hiÖn nĂ©i dung thñ tĂŽc) . . . . . Endproc ; hÕt mĂ©t thñ tĂŽc Func (tÂȘn h”m) ; BŸt ¼Çu mĂ©t h”m (khai bžo cžc tham biÕn nÕu cĂŁ) (khai bžo cžc biÕn ¼Þa ph−¬ng) (cžc c©u lÖnh thÓ hiÖn nĂ©i dung cña h”m) . . . . . return (biÕn) ; tr¶ giž trÞ cña biÕn vÒ thñ tĂŽc gĂ€i Endproc ; kÕt thĂłc h”m 5.4.4. MĂ©t sĂš phÐp tÝnh dĂŻng trong ASPECT : ASPECT sö dĂŽng nhiÒu phÐp tÝnh sĂš hĂ€c v” logic khžc nhau, d−íi Ÿ©y giĂ­i thiÖu mĂ©t sĂš phÐp tÝnh hay dĂŻng : +, -, *, / PhÐp tožn cĂ©ng, trĂ”, nh©n, chia. >, <, >=, <= LĂ­n hÂŹn, nhĂĄ hÂŹn, lĂ­n hÂŹn hoÆc b»ng, nhĂĄ hÂŹn hoÆc b»ng. != Khžc vĂ­i ! NOT && AND || OR ++, -- Tšng hoÆc gi¶m mĂ©t Ÿn vÞ. ?: ThĂčc hiÖn mĂ©t ÂźiÒu kiÖn .v.v... VÝ dĂŽ 1 : Cho A=2, B=4

A+B-- = 6 : A ¼−üc cĂ©ng vĂ­i B tr−íc, v” rĂ„i B gi¶m Âźi 1 (B=3). A+ --B = 5 : Tr−íc tiÂȘn B gi¶m Âźi 1, sau Ÿã cĂ©ng A vĂ­i B. VÝ dĂŽ 2 :

TS. PhÂčm §šng Ph−íc

Page 68: Robot cong nghiep

Robot c«ng nghiÖp 67

Proc main integer A,B,C,D integer Tong A=2, B=4 C=A+B Tong = A+ --B D=(tong < C) ? tong : C ; nÕu Tong < C th× D=Tong, nÕu sai D=C

Usermsg " D = %d , C = %d" D,C Endproc

KÕt qu¶ D = 5 v” C = 6. 5.4.5. MĂ©t sĂš tĂ” lÖnh trong ASPECT hay dĂŻng khi ÂźiÒu khiÓn robot: Ng«n ngĂ· ASPECT cĂŁ hÂŹn 600 tĂ” lÖnh, dĂŻng vĂ­i nhiÒu mĂŽc ¼Ých khžc nhau. PhÇn nÇy chØ giĂ­i thiÖu sÂŹ l−üt mĂ©t sĂš lÖnh hay dĂŻng khi lËp tr×nh ÂźiÒu khiÓn robot. Ng−ĂȘi ŸÀc cĂŁ thÓ sö dĂŽng mĂŽc Help trÂȘn menu cöa sĂŠ soÂčn th¶o ¼Ó biÕt thÂȘm chi tiÕt. * Cžc lÖnh cšn b¶n : call :

GĂ€i mĂ©t thñ tĂŽc hoÆc h”m tĂ” ch−¬ng tr×nh chÝnh hoÆc tĂ” mĂ©t thñ tĂŽc khžc. CĂł phžp : Khi gĂ€i mĂ©t h”m : call <tÂȘn> [WITH <danh sžch tham biÕn>] [INTO <biÕn>] Khi gĂ€i mĂ©t thñ tĂŽc : call <tÂȘn> [WITH <danh sžch tham biÕn>] tÂȘn : tÂȘn thñ tĂŽc hoÆc h”m ¼−üc gĂ€i. Danh sžch tham biÕn : TÂȘn cžc tham biÕn trong thñ tĂŽc hoÆc h”m. INTO <biÕn> : ChØ dĂŻng khi gĂ€i mĂ©t h”m, biÕn sÏ chĂža giž trÞ tr¶ lÂči cña h”m.

case/endcase : C©u lÖnh lĂča chĂ€n, dĂŻng vĂ­i tĂ” lÖnh Switch. CĂł phžp :

switch <biÕn> (string | integer | long) case <giž trÞ so sžnh> (string | integer | long) ... [exitswitch] ; thožt khĂĄi khĂši lÖnh switch kh«ng ... ÂźiÒu kiÖn. [endcase] [default] ; thĂčc hiÖn khi cžc tr−ĂȘng hĂźp so sžnh ... ¼Òu kh«ng Ÿóng. endcase

TS. PhÂčm §šng Ph−íc

Page 69: Robot cong nghiep

Robot c«ng nghiÖp 68

endswitch VÝ dĂŽ : proc main integer Alpha = 2 ; gžn giž trÞ ban ¼Çu cho biÕn Alpha=2. switch Alpha ; t×m giž trÞ cña biÕn sĂš case 0 ; Tr−ĂȘng hĂźp biÕn cĂŁ giž trÞ b»ng 0. usermsg "Alpha = 0" ; XuÊt kÕt qu¶ trÂȘn cöa sĂŠ m”n h×nh. Endcase ; HÕt tr−ĂȘng hĂźp so sžnh thĂž nhÊt. case 1 ; t−¬ng tĂč nh− trÂȘn . . . usermsg "Alpha = 1" endcase case 2 usermsg "Alpha = 2" endcase endswitch ; lu«n Âźi kÌm vĂ­i switch ¼Ó kÕt thĂłc khĂši lÖnh switch. endproc if / endif : C©u lÖnh ÂźiÒu kiÖn.

Có ph¾p : if <¼iÒu kiÖn 1>

... [elseif <¼iÒu kiÖn 2] ... [else] ... endif ; kÕt thóc khùi lÖnh if.

(LÖnh nÇy gÇn giĂšng nh− lÖnh if trong Pascal, kh«ng cĂŁ tĂ” then). while/endwhile :

LÆp lÂči mĂ©t sĂš c©u lÖnh cho ¼Õn khi ÂźiÒu kiÖn kiÓm tra l” sai. VÝ dĂŽ : proc main integer SoLanLap = 0 ; BiÕn nguyÂȘn dĂŻng ¼Ó ¼Õm sĂš lÇn lÆp while (SoLanLap++) < 3 ; Mçi lÇn lÆp biÕn tšng giž trÞ thÂȘm mĂ©t endwhile ; KÕt thĂłc khĂši lÖnh while. usermsg "Toi da lap %d lan" SoLanLap endproc for/endfor : C©u lÖnh lÆp theo mĂ©t sĂš lÇn nhÊt ¼Þnh CĂł phžp : for <biÕn ¼Õm>=<giž trÞ ban ¼Çu> UPTO | DOWNTO <giž trÞcuĂši>

[BY <b−íc>] . [exitfor] ; ChuyÓn ÂźiÒu khiÓn thožt khĂĄi c©u lÖnh lÆp for

. ; ¼Õn dßng lÖnh sau endfor

TS. PhÂčm §šng Ph−íc

Page 70: Robot cong nghiep

Robot c«ng nghiÖp 69

endfor Return : Thožt khĂĄi thñ tĂŽc hoÆc h”m hiÖn tÂči, tiÕp tĂŽc Ă« c©u lÖnh tiÕp theo cña thñ tĂŽc Ÿ· gĂ€i. * Cžc lÖnh khžc : transmit : Göi mĂ©t dßng kĂœ tĂč (lÖnh) ¼Õn cĂŠng Âźang hoÂčt Ÿéng. VÝ dĂŽ : proc main transmit "B-250~C-200~F-240~~P+200” ; ChuyÓn lÖnh ÂźiÒu khiÓn robot TG-45 endproc Pause : TÂčm dĂ”ng thĂčc hiÖn ch−¬ng tr×nh trong mĂ©t sĂš gi©y qui ¼Þnh. CĂł phžp :

pause <sĂš gi©y | FOREVER> VÝ dĂŽ : Pause 5 : tÂčm dĂ”ng thĂčc hiÖn ch−¬ng tr×nh 5 gi©y Pause Forever : DĂ”ng vĂ­i thĂȘi gian kh«ng xžc ¼Þnh. LÖnh Pause cĂŁ thÓ ¼−üc huĂ» bĂĄ khi Ên Ctl-Break. KĂœ tĂč ~ thay cho lÖnh pause vĂ­i giž trÞ dĂ”ng b»ng 0,5 gi©y. VÝ dĂŽ : Transmit “B+200~~E-100” Sau khi truyÒn lÖnh B+200 sÏ tÂčm dĂ”ng 1 gi©y (2 kĂœ tĂč ~) mĂ­i truyÒn tiÕp lÖnh E-100. chdir : Thay ŸÊi ¼−ĂȘng dÉn ¼Õn mĂ©t ĂŠ ¼Üa hoÆc th− mĂŽc khžc. CĂł phžp : chdir <”TÂȘn ¼−ĂȘng dÉn”> VÝ dĂŽ : Chdir “C:\ procom3\Robot” copyfile : Copy mĂ©t file theo ¼−ĂȘng dÉn hoÆc vĂ­i mĂ©t tÂȘn khžc. CĂł phžp : copyfile <”file nguĂ„n”> <”file ¼Ých”> VÝ dĂŽ : copy “C:\ Procom3\ aspect\ robot.was” “C:\ tam\ robot1.txt” delfile : Xož mĂ©t file theo chØ ¼Þnh. CĂł phžp : delfile <”tÂȘn file”> mkdir : TÂčo mĂ©t th− mĂŽc mĂ­i. CĂł phžp : mkdir <”tÂȘn th− muc”> rmdir : Xož mĂ©t th− mĂŽc (trĂšng)

TS. PhÂčm §šng Ph−íc

Page 71: Robot cong nghiep

Robot c«ng nghiÖp 70

CĂł phžp : rmdir <”tÂȘn th− mĂŽc”> rename : §Êi tÂȘn mĂ©t file. CĂł phžp : rename <”tÂȘn file cĂČ”> <”tÂȘn file mĂ­i”> Fopen : MĂ« mĂ©t file ¼Ó ŸÀc hoÆc ghi.

CĂł phžp : fopen <sĂš hiÖu file> <”tÂȘn file”> READ | WRITE | READWRITE | CREATE | APPEND | READAPPEND

Cžc tuĂș chĂ€n : READ : chØ ŸÀc; READWRITE : cĂŁ thÓ ŸÀc v” ghi;

CREATE : TÂčo mĂ­i; APPEND : Ghi tiÕp v”o cuĂši file; READAPPEND : CĂŁ thÓ ŸÀc v” ghi tiÕp v”o cuĂši file.

Fclose : §ãng mĂ©t file Ÿ· mĂ«. CĂł phžp : Fclose <sĂš hiÖu file> fputs : Ghi mĂ©t chuçi kĂœ tĂč lÂȘn file. CĂł phžp : fputs <sĂš hiÖu file> <”chuçi kĂœ tĂč”> VÝ dĂŽ : proc main string Fname = "Vidu.txt" ; TÂȘn file ¼−üc mĂ«. if fopen 0 Fname CREATE ; TÂčo mĂ­i v” mĂ« mĂ©t file cĂŁ tÂȘn “Vidu.txt” fputs 0 "Day la file moi duoc mo !" ; Ghi mĂ©t chuçi lÂȘn file. fclose 0 ; §ãng file Ÿ· ¼−üc tÂčo mĂ­i v” mĂ«. else errormsg "Couldn't open file `"%s`"." Fname endif endproc feof : KiÓm tra ÂźiÒu kiÖn Ÿ· Ă« cuĂši mĂ©t file. CĂł phžp : feof <sĂš hiÖu file> [biÕn nguyÂȘn] [biÕn nguyÂȘn] : cĂŁ giž trÞ 0 nÕu ch−a kÕt thĂłc file, b»ng 1

nÕu Ÿ· kÕt thĂłc file. Fgets : §Àc mĂ©t dßng kĂœ tĂč tĂ” mĂ©t tÖp Ÿ· mĂ« ghi v”o mĂ©t biÕn. CĂł phžp : fgets <sĂš hiÖu file> <tÂȘn biÕn kiÓu string> VÝ dĂŽ : proc main string Fname = "Vidu.txt" ; TÂȘn file cÇn ŸÀc string chuoi ; BiÕn chuoi nhËn giž trÞ ŸÀc tĂ” file. if fopen 0 Fname READ ; MĂ« file chØ ¼Ó ŸÀc (sĂš hiÖu file id=0). while not feof 0 ; LÆp lÂči khi ch−a kÕt thĂłc file. fgets 0 FInput ; §Àc mĂ©t dßng tĂ” file

TS. PhÂčm §šng Ph−íc

Page 72: Robot cong nghiep

Robot c«ng nghiÖp 71

usermsg FInput ; ThÓ hiÖn dßng Ÿ· ŸÀc endwhile fclose 0 ; §ãng file else errormsg "Can't open `"%s`" for input." Fname ; bžo lçi nÕu file kh«ng tĂ„n tÂči. endif endproc usesmsg : thÓ hiÖn mĂ©t dßng th«ng bžo hay kÕt qu¶ trÂȘn cöa sĂŠ. CĂł phžp : usermsg <:dßng th«ng bžo”> [biÕn1, ...] Xem cžc vÝ dĂŽ trÂȘn. termwrites : ViÕt mĂ©t dßng kĂœ tĂč lÂȘn cña sĂŠ nhËp xuÊt dĂ· liÖu.

CĂł phžp : termwrites <biÕn hoÆc “dßng kĂœ tĂč”>

Run : ThĂčc hiÖn mĂ©t ch−¬ng tr×nh bÂȘn ngo”i (Âźu«i COM, EXE hoÆc BAT). CĂł phžp : run <”tÂȘn ch−¬ng tr×nh”>

VÝ dĂŽ : proc main string Prog = "C:\ windows\ pbrush.exe" ; Ch−¬ng tr×nh cÇn thĂčc hiÖn. run Prog ; ThĂčc hiÖn ch−¬ng tr×nh PaintBrush cña Windows. Endproc

Ngo”i cžc tĂ” lÖnh Ÿ· giĂ­i thiÖu trÂȘn, cßn cĂŁ rÊt nhiÒu lÖnh khžc..., ng−ĂȘi sö dĂŽng cĂŁ thÓ tham kh¶o trĂčc tiÕp trong mĂŽc HELP cña cö sĂŠ soÂčn th¶o khi cÇn thiÕt. Ng«n ngĂ· ASPECT kh«ng cĂŁ sÂŒn cžc h”m tožn hĂ€c nh− sin, cos, ... nÂȘn khi muĂšn thĂčc hiÖn cžc tÝnh tožn phĂžc tÂčp ta ph¶i dĂŻng cžc phÇn mÒm khžc. 5.5. LËp tr×nh ÂźiÒu khiÓn robot TERGAN - 45 :

Nh− Ÿ· nĂŁi trÂȘn, ¼Ó ÂźiÒu khiÓn robot TERGAN-45 ta cĂŁ thÓ dĂŻng phÇn mÒm Procomm Plus for Windows ÂźiÒu khiÓn trĂčc tiÕp hoÆc viÕt cžc ch−¬ng tr×nh b»ng ng«n ngĂ· ASPECT.

5.5.1. GiĂ­i thiÖu robot TERGAN 45 (TG-45): TERGAN 45 l” mĂ©t loÂči robot dĂŻng ¼Ó dÂčy hĂ€c do Phžp s¶n xuÊt. §©y l” loÂči robot to”n khĂ­p quay, cĂŁ 4 bËc tĂč do. §i kÌm vĂ­i robot gĂ„m cĂŁ mĂ©t bĂ© nguĂ„n v” mĂ©t m«Ÿun ÂźiÒu khiÓn. M«Ÿun ÂźiÒu khiÓn cho phÐp ÂźiÒu khiÓn robot trÂȘn cžc Terminal hoÆc mžy tÝnh cĂŁ giao diÖn kiÓu RS-232. CÊu h×nh cña robot nh− h×nh 5.2 :

o o

o

Th©n

Vai Cžnh tay

CĂŠ tay

B”n tay

H×nh 5.3 : SÂŹ ŸÄ Ÿéng Robot TG-45

TS. PhÂčm §šng Ph−íc

Page 73: Robot cong nghiep

Robot c«ng nghiÖp 72

Cžc khĂ­p quay cña robot ¼−üc dÉn Ÿéng b»ng cžc Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu cĂŁ gŸn cžc potentionmeter, ngo”i ra ¼Ó Ÿãng mĂ« b”n tay cña robot ng−ĂȘi ta dĂŻng truyÒn Ÿéng vit-me cĂŁ gŸn cö h”nh tr×nh, vËn tĂšc Ÿãng mĂ« cžc ngĂŁn tay cĂŁ thÓ ÂźiÒu chØnh ¼−üc. Cžc gĂŁc quay giĂ­i hÂčn cña cžc kh©u trÂȘn robot l” :

+ ChuyÓn Ÿéng cña th©n 2610. + ChuyÓn Ÿéng cña vai 850. + ChuyÓn Ÿéng cña cžnh tay 2490. + ChuyÓn Ÿéng cña cĂŠ tay 1800.

TĂšc Ÿé truyÒn th«ng tin qua moÂźun ÂźiÒu khiÓn tĂ” 50 ¼Õn 9600 bauds vĂ­i bĂ© vi xö lĂœ 8 bits, Stop bit l” 1 hoÆc 2. §iÖn žp nguĂ„n cung cÊp l” 110V/220V, 50HZ. §iÖn žp ÂźiÒu khiÓn ±12V. TrÂȘn m«Ÿun ÂźiÒu khiÓn cĂŁ thÂȘm cžc ¼Çu v”o v” ra ¼Ó giao diÖn vĂ­i cžc thiÕt bÞ khžc (nh− cžc c¶m biÕn, ÂźiÒu khiÓn bšng t¶i nhĂĄ, ...). MoÂźun ÂźiÒu khiÓn robot TG-45 ¼−üc thiÕt kÕ giao diÖn vĂ­i mžy tÝnh b»ng cžc lÖnh cÂŹ b¶n sau :

B±XXX : §iÒu khiÓn th©n (Base), E±XXX : §iÒu khiÓn vai (Ðpaule), C±XXX: §iÒu khiÓn cžnh tay (Coude), F±XXX : §iÒu khiÓn cĂŠ tay (Poignet), P±XXX : §ãng mĂ« b”n tay (Pince), S±XXX : §iÒu khiÓn cžc tÝn hiÖu ra, I±XXX : §iÒu khiÓn cžc tÝn hiÖu v”o. ChiÒu d”i cña cžc lÖnh ÂźiÒu khiÓn l” 5 kĂœ tĂč m· ASCII. KĂœ hiÖu XXX biÓu

diÔn cžc chĂ· sĂš tĂ” 000 ¼Õn 511. VÝ du :

LÖnh B-200 sÏ ÂźiÒu khiÓn th©n robot quay sang ph¶i mĂ©t gĂŁc : Ξ1 = (261o/2) x 200 / 511 ≈ 51004’

LÖnh C+200 sÏ ÂźiÒu khiÓn cžnh tay robot quay lÂȘn phÝa trÂȘn mĂ©t gĂŁc : Ξ3 = (249o/2) x 200 / 511 ≈ 48043’ (so vĂ­i vai). LÖnh P+200 sÏ Ÿãng b”n tay (dĂŻng khi muĂšn nŸm mĂ©t vËt) , vËn tĂšc Ÿãng mĂ« thay ŸÊi ¼−üc theo giž trÞ tĂ” 001 ¼Õn 511. VÝ dĂŽ P+100 sÏ Ÿãng chËm hÂŹn P+200.

Cžc lÖnh ¼−üc chuyÓn ¼Õn tĂ” mžy tÝnh sÏ ¼−üc m«Ÿun ÂźiÒu khiÓn xö lĂœ sau Ÿã tr¶ lÂči cžc th«ng bžo thĂčc hiÖn (message) trÂȘn m”n h×nh. 5.5.2. §iÒu khiÓn trĂčc tiÕp robot TG-45 nhĂȘ phÇn mÒm Procomm :

TS. PhÂčm §šng Ph−íc

Page 74: Robot cong nghiep

Robot c«ng nghiÖp 73

Ă« chÕ Ÿé TERMINAL cña Procomm Plus ta cĂŁ thÓ ÂźiÒu khiÓn trĂčc tiÕp robot Tergan 45 b»ng cžch gĂą trĂčc tiÕp cžc lÖnh l”m quay cžc khĂ­p cña robot, vÝ dĂŽ : B+200 C-250 E-100 F-250 P+200 Ta cĂŁ thÓ ghi lÂči cžc lÖnh vĂ”a nhËp v”o mĂ©t file ¼Ó thĂčc hiÖn lÂči sau nÇy, nÕu tr−íc khi nhËp cžc lÖnh ta chĂ€n mĂŽc START RECORDER trÂȘn menu hoÆc Icon t−¬ng Ăžng. 5.5.3. ViÕt ch−¬ng tr×nh ÂźiÒu khiÓn robot TERGAN-45 : Ta cĂŁ thÓ ÂźiÒu khiÓn robot Tergan-45 b»ng cžch viÕt cžc ch−¬ng tr×nh b»ng ng«n ngĂ· ASPECT. MĂ©t ch−¬ng tr×nh vÝ dĂŽ Ÿn gi¶n nh− sau : proc main transmit "E-100~B-250~F-180~C-200~B-300~~~P+150~~~” transmit “E+000~C-150~B+300~~C-180~~~~P-200~~~" transmit "E+200~B-400~~~E-000~~~C-300~~~F-080~~~B-450~~~P+150~~~” transmit “C-260~~E+100~~B+300~~~~~~P-200~~" transmit "F+200~C-130~B-350~F-300~~E-180~~B-400~~~P+100~~" transmit "E+200~~B+300~~~~E-100~~~F-230~~~~P-200~~~~” transmit “C-000~F-000~E-000~B-000~P-200~" pause 50 clear termwrites "Da thuc hien xong, xin cho lenh !" endproc

Khi soÂčn th¶o xong ch−¬ng tr×nh ta ph¶i ¼Æt tÂȘn v” ghi v”o ¼Üa, vÝ dĂŽ tÂȘn ch−¬ng tr×nh l” DEMO.WAS. Sau Ÿã ta ph¶i dÞch ch−¬ng tr×nh ¼Ó tÂčo ra file DEMO.WAX lĂłc Ÿã mĂ­i cĂŁ thÓ chÂčy ¼−üc trong Procomm Plus. Tuy nhiÂȘn, nh− Ÿ· giĂ­i thiÖu Ă« trÂȘn, m«Ÿun ÂźiÒu khiÓn robot TG-45 chØ cĂŁ cžc lÖnh Ÿn gi¶n ¼Ó ÂźiÒu khiÓn cžc m«tÂŹ dÉn Ÿéng cžc khĂ­p quay. NÕu chØ ÂźiÒu khiÓn robot b»ng cžc lÖnh Ÿn th× kh«ng thÓ mĂ« rĂ©ng kh¶ nšng l”m viÖc cña robot ¼−üc, hÂŹn nĂ·a viÖc lËp tr×nh cĂČng mÊt nhiÒu c«ng sĂžc v× khĂŁ xžc ¼Þnh ¼−üc cžc toÂč Ÿé m” ta yÂȘu cÇu b”n tay robot ph¶i ÂźÂčt tĂ­i. Do Ÿã viÖc lËp tr×nh ÂźiÒu khiÓn robot ph¶i tÂčo ra cžc chĂžc nšng khžc khi ÂźiÒu khiÓn robot nh− :

1) Ch−¬ng tr×nh cĂŁ thÓ giĂłp ng−ĂȘi sö dĂŽng dÂčy robot hĂ€c m” robot cĂŁ thÓ lÆp lÂči cžc chuyÓn Ÿéng Ÿ· ¼−üc dÂčy-hĂ€c mĂ©t cžch chÝnh xžc.

TS. PhÂčm §šng Ph−íc

Page 75: Robot cong nghiep

Robot c«ng nghiÖp 74

2) ThiÕt kÕ ÂźiÒu khiÓn Ÿéng hĂ€c thuËn : nghÜa l” ch−¬ng tr×nh cho phÐp ng−ĂȘi sö dĂŽng ÂźiÒu khiÓn robot theo giž trÞ cžc gĂŁc quay cña khĂ­p (tÝnh b»ng Ÿé) khi xžc ¼Þnh tr−íc mĂ©t cÊu h×nh n”o Ÿã cña robot.

3) ThiÕt kÕ ÂźiÒu khiÓn Ÿéng hĂ€c ng−üc : nghÜa l” ng−ĂȘi sö dĂŽng ch−¬ng tr×nh

cĂŁ thÓ ÂźiÒu khiÓn robot theo cžc toÂč Ÿé vÞ trÝ v” h−íng cña b”n tay Ÿ· ¼−üc xžc ¼Þnh tr−íc. Khi ta nhËp cžc giž trÞ vÒ toÂč Ÿé v” h−íng cña b”n tay th× ch−¬ng tr×nh tĂč Ÿéng tÝnh tožn cžc gĂŁc quay cña cžc khĂ­p ¼Ó robot chuyÓn Ÿéng ¼Õn vÞ trÝ yÂȘu cÇu vĂ­i h−íng Ÿ· ¼−üc xžc ¼Þnh.

4) ThiÕt kÕ cžc tiÖn Ých khžc nh− : ÂźiÒu khiÓn theo ¼−ĂȘng, tÂčo cžc meta keys,

tÂčo ra cžc trĂź giĂłp cho ng−ĂȘi sö dĂŽng... Cžc nĂ©i dung 2 v” 3 cÇn ph¶i thiÕt lËp hÖ ph−¬ng tr×nh Ÿéng hĂ€c cña robot

TERGAN-45 v” gi¶i hÖ ph−¬ng tr×nh Ÿéng hĂ€c Ÿã. PhÇn tÝnh tožn cĂŁ thÓ viÕt b»ng ng«n ngĂ· Pascal hoÆc C++ m” nĂŁ ¼−üc gĂ€i tĂ” ch−¬ng tr×nh ÂźiÒu khiÓn (dĂŻng lÖnh RUN), ch−¬ng tr×nh ÂźiÒu khiÓn xö lĂœ kÕt qu¶ tÝnh tožn qua cžc file trung gian dÂčng text.

PhÇn mÒm Procomm cung cÊp nhiÒu tiÖn Ých ¼Ó ta cĂŁ thÓ thiÕt kÕ ch−¬ng tr×nh kiÓu trĂčc quan (Visual), giĂłp cho viÖc viÕt ch−¬ng tr×nh v” thao tžc trong quž tr×nh sö dĂŽng ¼−üc dÔ d”ng, thuËn tiÖn hÂŹn. 5.8. KÕt luËn : Trong ch−¬ng nÇy chØ giĂ­i thiÖu mĂ©t cžch tĂŠng qužt vÒ cžc ph−¬ng phžp lËp tr×nh ÂźiÒu khiÓn robot. KhĂŁ cĂŁ thÓ Âźi s©u, cĂŽ thÓ v”o mĂ©t ng«n ngĂ· n”o v× nĂŁ phĂŽ thuĂ©c rÊt nhiÒu v”o loÂči robot ¼−üc sö dĂŽng. PhÇn ng«n ngĂ· ASPECT trong phÇn mÒm Procomm ¼−üc nghiÂȘn cĂžu Ă« trÂȘn l” mĂ©t vÝ dĂŽ vÒ Ăžng dĂŽng cžc phÇn mÒm dĂŻng cho nhiÒu mĂŽc ¼Ých ¼Ó ÂźiÒu khiÓn robot. Trong thĂčc tÕ, tuĂș nhiÖm vĂŽ cĂŽ thÓ cña mçi robot, phĂši hĂźp vĂ­i hoÂčt Ÿéng chung cña cžc mžy mĂŁc thiÕt bÞ khžc m” ta thiÕt kÕ cžc ch−¬ng tr×nh cĂŽ thÓ ¼Ó robot hoÂčt Ÿéng theo nhĂ·ng mĂŽc ¼Ých mong muĂšn.

TS. PhÂčm §šng Ph−íc

Page 76: Robot cong nghiep

Robot c«ng nghiÖp 75

B”i tËp ch−¬ng V : B”i 1 : H·y viÕt mĂ©t Function cña h”m arctg2(y,x) b»ng ng«n ngĂ· Turbo Pascal. B”i 2 : ViÕt mĂ©t ch−¬ng tr×nh (ng«n ngĂ· tuĂș Ăœ) ¼Ó nhËp cžc th«ng sĂš DH v” tĂč Ÿéng xžc lËp cžc ma trËn An (BiÓu hiÖn kÕt qu¶ trÂȘn m”n h×nh v” ghi v”o mĂ©t file dÂčng text). B”i 3 : ViÕt mĂ©t ch−¬ng tr×nh b»ng Turbo Pascal ¼Ó tÝnh tožn Ÿéng hĂ€c nguĂźc (Xžc ¼Þnh cžc gĂŁc quay) cña robot TERGAN-45. DĂ· liÖu nhËp tĂ” b”n phÝm. Ghi kÕt qu¶ v”o mĂ©t file dÂčng text. B”i 4 : ViÕt mĂ©t ch−¬ng tr×nh ÂźiÒu khiÓn robot TERGAN-45 b»ng ng«n ngĂ· ASPECT ¼Ó robot cĂŁ cÊu h×nh nh− sau :

Ξ1 = +300; Ξ2 = -100; Ξ3 = -300; Ξ4 = -250. B”n tay robot nŸm lÂči sau khi di chuyÓn ¼Õn vÞ trÝ yÂȘu cÇu.

B”i 5 : ViÕt mĂ©t ch−¬ng tr×nh b»ng ng«n ngĂ· ASPECT, gĂ€i ch−¬ng tr×nh tÝnh Ÿéng hĂ€c ng−üc viÕt b»ng Turbo Pascal (nh− b”i 3), xö lĂœ kÕt qu¶ tÝnh tožn ¼Ó ÂźiÒu khiÓn robot TERGAN-45 theo toÂč Ÿé vÞ trÝ v” h−íng cña b”n tay.

TS. PhÂčm §šng Ph−íc

Page 77: Robot cong nghiep

ROBOT C«ng nghiÖp 76

Ch−¬ng VI M« phĂĄng robot trÂȘn mžy tÝnh

(Robot Simulation) (PhÇn thĂčc h”nh trÂȘn mžy tÝnh)

6.1. KĂŒ thuËt m« phĂĄng robot :

M« phĂĄng l” mĂ©t kĂŒ thuËt hiÖn ÂźÂči, ¼−üc žp dĂŽng trong nhiÒu lÜnh vĂčc

nghiÂȘn cĂžu v” s¶n xuÊt. Khi nghiÂȘn cĂžu vÒ ÂźiÒu khiÓn robot, ta cĂŁ thÓ thĂčc hiÖn ÂźiÒu khiÓn trĂčc tiÕp

robot hoÆc ÂźiÒu khiÓn m« phĂĄng. §iÒu khiÓn m« phĂĄng l” dĂŻng cžc m« h×nh tÝnh tožn Ÿéng hĂ€c v” Ÿéng lĂčc hĂ€c cña robot kÕt hĂźp vĂ­i cžc ph−¬ng phžp ŸÄ hoÂč trÂȘn mžy vi tÝnh ¼Ó m« t¶ vÒ kÕt cÊu v” hoÂčt Ÿéng cña cžnh tay robot.

NghiÂȘn cĂžu vÒ m« phĂĄng hoÂčt Ÿéng cña robot trÂȘn mžy tÝnh giĂłp cho cžc nh” thiÕt kÕ nhanh chĂŁng lĂča chĂ€n ¼−üc ph−¬ng žn h×nh - Ÿéng hĂ€c cña robot, cĂŁ thÓ kiÓm tra kh¶ nšng hoÂčt Ÿéng cña robot trÂȘn m”n h×nh, kiÓm tra sĂč phĂši hĂźp cña robot vĂ­i cžc thiÕt bÞ khžc trong d©y chuyÒn. §iÒu nÇy rÊt cĂŁ Ăœ nghÜa trong quž tr×nh thiÕt kÕ chÕ tÂčo robot mĂ­i hoÆc bĂš trÝ d©y chuyÒn s¶n xuÊt.

Qua m« phĂĄng ng−ĂȘi thiÕt kÕ cĂŁ thÓ Ÿžnh giž t−¬ng ŸÚi ¼Çy Ÿñ kh¶ nšng l”m viÖc cña ph−¬ng žn thiÕt kÕ m” kh«ng cÇn chÕ thö. NĂŁ cĂČng ¼−üc xem l” ph−¬ng tiÖn ŸÚi thoÂči, hiÖu chØnh thiÕt kÕ theo yÂȘu cÇu Âźa dÂčng cña ng−ĂȘi sö dĂŽng.

Ph−¬ng phžp lËp tr×nh m« phĂĄng cĂČng giĂłp ng−ĂȘi thiÕt kÕ chĂ€n ¼−üc quĂŒ ÂźÂčo c«ng nghÖ hĂźp lĂœ cña robot trong quž tr×nh l”m viÖc vĂ­i mĂ©t ŸÚi t−üng cĂŽ thÓ hay phĂši hĂźp vĂ­i cžc thiÕt bÞ khžc trong mĂ©t c«ng ÂźoÂčn s¶n xuÊt ¼−üc tĂč Ÿéng hož.

HiÖn nay cĂŁ nhiÒu phÇn mÒm c«ng nghiÖp v” cžc phÇn mÒm nghiÂȘn cĂžu

khžc nhau ¼Ó m« phĂĄng robot, phÂčm vi Ăžng dĂŽng v” giž th”nh cña chĂłng cĂČng khžc nhau. Ă« Ÿ©y chĂłng ta nghiÂȘn cĂžu ph−¬ng phžp m« phĂĄng robot dĂŻng phÇn mÒm EASY-ROB.

6.2. GiíÝ thiÖu phÇn mÒm EASY-ROB : EASY-ROB l” c«ng cĂŽ m« phĂĄng robot sö dĂŽng ŸÄ hoÂč trong kh«ng gian 3

chiÒu (3D) v” cžc h×nh ¶nh cĂŁ thÓ hoÂčt Ÿéng ¼−üc. MĂ©t hÖ thĂšng 3D-CAD Ÿn gi¶n cho phÐp tÂčo ra cžc khĂši h×nh hĂ€c cÂŹ b¶n nh− khĂši trĂŽ, khĂši cÇu, khĂši chĂ· nhËt, khĂši tam gižc ... ¼Ó vÏ kÕt cÊu cña robot. Trong EASY-ROB chĂłng ta cĂŁ thÓ dĂŻng chuĂ©t ¼Ó quay hoÆc tÞnh tiÕn robot ¼Õn mĂ©t toÂč Ÿé tuĂș Ăœ. EASY-ROB cĂČng cĂŁ cžc chĂžc nšng phĂŁng to, thu nhĂĄ ŸÚi t−üng vÏ nh− nhiÒu phÇn mÒm thiÕt kÕ khžc... Ch−¬ng tr×nh cho phÐp thiÕt kÕ cžc robot ¼Õn 12 bËc tĂč do. ChuyÓn Ÿéng cña Robot cĂŁ thÓ ¼−üc ÂźiÒu khiÓn theo cžc biÕn khĂ­p hoÆc cžc toÂč Ÿé §Ò-cžt. ChĂłng ta cĂČng cĂŁ thÓ m« t¶ Ÿéng hĂ€c cña robot theo kiÓu DH hoÆc trong hÖ toÂč Ÿé to”n cĂŽc (Universal

TS. PhÂčm §šng Ph−íc

Page 78: Robot cong nghiep

ROBOT C«ng nghiÖp 77

Coordinates). Easy-Rob Ÿ· cĂŁ sÂœn cžc tr×nh ÂźiÒu khiÓn Ÿéng hĂ€c thuËn v” ng−üc cña cžc cÊu h×nh robot th«ng dĂŽng, khi thiÕt kÕ ta chØ cÇn khai bžo kiÓu Ÿéng hĂ€c thÝch hĂźp. Trong tr−ĂȘng hĂźp robot cĂŁ kÕt cÊu ¼Æc biÖt hoÆc cĂŁ cžc kh©u bÞ Ÿéng gŸn vĂ­i cžc chuyÓn Ÿéng cña cžc khĂ­p th× cÇn ph¶i gi¶i b”i tožn Ÿéng hĂ€c ng−üc hoÆc xžc ¼Þnh h”m tožn hĂ€c m« t¶ sĂč phĂŽ thuĂ©c cña kh©u bÞ Ÿéng ŸÚi vĂ­i khĂ­p quay, viÕt ch−¬ng tr×nh xžc ¼Þnh sĂč phĂŽ thuĂ©c Ÿã b»ng ng«n ngĂ· C v” sau Ÿã dĂŻng tËp tin MAKE.EXE trong C ¼Ó dÞch th”nh tËp tin th− viÖn liÂȘn kÕt Ÿéng er_kin.dll (Easy-Rob kinematic Dynamic link library), khi chÂčy ch−¬ng tr×nh, EASY-ROB sÏ liÂȘn kÕt vĂ­i tËp tin nÇy v” thĂčc hiÖn kiÓu Ÿéng hĂ€c Ÿ· ¼−üc khai bžo trong ch−¬ng tr×nh ÂźiÒu khiÓn.

Easy-ROB cĂŁ mĂ©t sĂš cžc lÖnh ÂźiÒu khiÓn riÂȘng, Ch−¬ng tr×nh ¼−üc viÕt theo kiÓu xö lĂœ tuÇn tĂč, tËp tin dÂčng Text, cĂŁ thÓ soÂčn th¶o ch−¬ng tr×nh trong bÊt kĂș tr×nh soÂčn th¶o n”o. Cžc c«ng cĂŽ gŸn trÂȘn kh©u chÊp h”nh cuĂši cĂŁ thÓ thay ŸÊi ¼−üc. ChĂłng ta cĂŁ thÓ viÕt mĂ©t ch−¬ng tr×nh chuyÓn Ÿéng cho mĂ©t robot theo mĂ©t quĂŒ ÂźÂčo mong muĂšn, cĂŁ thÓ kiÓm tra kh¶ nšng v−¬n tĂ­i cña cžnh tay, xžc ¼Þnh vĂŻng l”m viÖc cña robot . . . Robot m« phĂĄng cĂŁ thÓ cÇm nŸm hoÆc th¶ cžc ŸÚi t−üng l”m viÖc. Cžc chuyÓn Ÿéng cña robot cĂŁ thÓ ghi v”o mĂ©t tËp tin v” cĂŁ thÓ thĂčc hiÖn lÂči.

PhÇn mÒm cho phÐp ta xem ¼−üc cžc hÖ toÂč Ÿé Ÿ· gŸn trÂȘn cžc kh©u cña robot, xem ¼−üc quĂŒ ÂźÂčo chuyÓn Ÿéng cña ÂźiÓm cuĂši c«ng cĂŽ gŸn trÂȘn kh©u chÊp h”nh cuĂši. PhÇn mÒm cßn cĂŁ nhiÒu tiÖn Ých khžc nh− : cho phÐp ta lËp tr×nh ÂźiÒu khiÓn robot b»ng ph−¬ng phžp dÂčy hĂ€c, thiÕt kÕ cžc ŸÚi t−üng l”m viÖc cña robot, cĂŁ cžc cöa sĂŠ vÒ toÂč Ÿé v” giž trÞ gĂŁc quay cña cžc khĂ­p tÂči tĂ”ng thĂȘi ÂźiÓm khi robot hoÂčt Ÿéng...

ViÖc sö dĂŽng phÇn mÒm EASY-ROB ¼Ó m« phĂĄng robot giĂłp chĂłng ta hai kh¶ nšng nghiÂȘn cĂžu :

a/ M« phĂĄng lÂči mĂ©t robot Ÿ· cĂŁ v” cžc ŸÚi t−üng l”m viÖc cña nĂŁ. §žnh giž kh¶ nšng l”m viÖc v” mĂžc Ÿé linh hoÂčt cña robot, xžc ¼Þnh cžc th«ng sĂš ÂźiÒu khiÓn, quĂŒ ÂźÂčo chuyÓn Ÿéng ¼Ó dĂŻng trong ÂźiÒu khiÓn thĂčc.

b/ NghiÂȘn cĂžu thiÕt kÕ Ÿéng hĂ€c, cžc kÝch th−íc v” kÕt cÊu cña robot trÂȘn mžy tÝnh ¼Ó cĂŁ thÓ chĂ€n ¼−üc ph−¬ng žn Ÿéng hĂ€c tĂšt nhÊt, ٦m b¶o cho robot ho”n th”nh cžc nhiÖm vĂŽ yÂȘu cÇu.

6.3. T×m hiÓu m”n h×nh EASYÝROB : a- Menu chÝnh : Menu chÝnh cña phÇn mÒm EASY-ROB cung cÊp cžc nĂ©i dung hoÂčt Ÿéng

khžc nhau cña phÇn mÒm. B−íc ¼Çu l”m quen, ta cÇn quan t©m cžc Menu sau : Menu FILE : Xö lĂœ cžc tžc vĂŽ trÂȘn File. Trong Easy-Rob cĂŁ nhiÒu loÂči file ¼−üc qui ¼Þnh bĂ«i phÇn mĂ« rĂ©ng (Âźu«i cña File), vÝ dĂŽ : File cĂŁ dÂčng *.Cel : (Cellfile) ¼Ó m« t¶ kÕt cÊu Robot, c«ng cĂŽ l”m viÖc v” ŸÚi t−üng l”m viÖc cña robot. §©y l” mĂ©t File tĂŠng hĂźp, bao gĂ„m c¶ ch−¬ng tr×nh dĂŻng ¼Ó ÂźiÒu khiÓn robot.

TS. PhÂčm §šng Ph−íc

Page 79: Robot cong nghiep

ROBOT C«ng nghiÖp

TS. PhÂčm §šng Ph−íc

78

121

1

18

Cöa sĂŠ ¼Ó thiÕt kÕ Thanh c«ng cĂŽ

Menu chÝnh Thanh c«ng cĂŽ 17

H×nh 6.1 : M”n h×nh EASY-ROB.

File cĂŁ dÂčng *.Rob : (Robotfile) ¼Ó m« t¶ riÂȘng kÕt cÊu cña mĂ©t robot. File cĂŁ dÂčng *.Bod : (Bodyfile) ¼Ó m« t¶ cžc ŸÚi t−üng l”m viÖc cña robot. File cĂŁ dÂčng *.Tol : (Toolfile) ¼Ó m« t¶ c«ng cĂŽ gŸn trÂȘn kh©u chÊp h”nh cuĂši cña robot.

. . . . .

File cĂŁ dÂčng *.Vie : (Viewfile) ¼Ó xžc ¼Þnh gĂŁc nh×n trong kh«ng gian. File cĂŁ dÂčng *.igp : (Igrip Partfile) l−u trö mĂ©t bĂ© phËn kÕt cÊu. File cĂŁ dÂčng *.Prg : (Programm) Ch−¬ng tr×nh ÂźiÒu khiÓn.

v.v.... Menu Robotics : DĂŻng ¼Ó nhËp cžc th«ng sĂš DH, xžc ¼Þnh vÞ trÝ cña dĂŽng cĂŽ, xžc ¼Þnh vÞ trÝ robot v” cžc th«ng sĂš khžc. Menu 3D-CAD : Cung cÊp cžc c«ng cĂŽ ¼Ó vÏ kÕt cÊu robot trong kh«ng gian 3 chiÒu (3D) cĂČng nh− ¼Ó thiÕt kÕ cžc c«ng cĂŽ, cžc ŸÚi t−üng l”m viÖc. §Ó vÏ ¼−üc kÕt cÊu cña robot, dĂča v”o cžc khĂši h×nh hĂ€c Ÿn gi¶n ta cĂŁ thÓ lŸp ghÐp chĂłng lÂči ¼Ó tÂčo nÂȘn cžc h×nh džng khžc nhau cña robot. b- Cžc thanh c«ng cĂŽ : Cžc nĂłt trÂȘn thanh c«ng cĂŽ dĂŻng ¼Ó thĂčc hiÖn cžc thao tžc nh− cña menu chÝnh (m” kh«ng cÇn v”o menu). Sö dĂŽng cžc nĂłt trÂȘn thanh c«ng cĂŽ cho phÐp ta thao tžc nhanh hÂŹn l” ph¶i v”o menu chÝnh. ChĂžc nšng cña cžc nĂłt chÝnh trÂȘn thanh c«ng cĂŽ nh− sau : Thanh c«ng cĂŽ n»m ngang phÝa trÂȘn, tÝnh tĂ” trži sang ph¶i : 1. BËt tŸt chÕ Ÿé chiÕu sžng cžc ŸÚi t−üng vÏ.

Page 80: Robot cong nghiep

ROBOT C«ng nghiÖp 79

2. ChuyÓn tÊt c¶ cžc ŸÚi t−üng sang dÂčng l−íi. 3. ChuyÓn ŸÚi t−üng dÂčng trĂŽ / khĂši phĂžc tÂčp. 5. ThÓ hiÖn/kh«ng thÓ hiÖn s”n. 6. ThÓ hiÖn s”n Ă« dÂčng l−íi. 7. Reset vÞ trÝ robot trÂȘn m”n h×nh. 8. ChuyÓn ŸÊi cöa sĂŠ khi mĂ« Cellfile hoÆc igip partfile (kÕt hĂźp vĂ­i nĂłt 7). 9. ChÂčy ch−¬ng tr×nh. 10. TÂčm dĂ”ng ch−¬ng tr×nh. 11. TiÕp tĂŽc chÂčy ch−¬ng tr×nh. 12. KÕt thĂłc ch−¬ng tr×nh. 13. ChÂčy ch−¬ng tr×nh theo tĂ”ng b−íc. 14. LÆp lÂči ch−¬ng tr×nh sau khi kÕt thĂłc. 15. 16. Gi¶m v” tšng tĂšc Ÿé ÂźiÒu khiÓn. 17. §žnh giž sai sĂš v” xem cžc giž trÞ Ÿéng hĂ€c. Thanh c«ng cĂŽ n»m ngang phÝa d−íi, tÝnh tĂ” trži sang ph¶i : 1. ThÊy hoÆc kh«ng thÊy kÕt cÊu robot. 2. ThÊy hoÆc kh«ng thÊy dĂŽng cĂŽ. 3. ThÊy hoÆc kh«ng thÊy cžc ŸÚi t−üng l”m viÖc. 4. ThÓ hiÖn/kh«ng thÓ hiÖn hÖ toÂč Ÿé gŸn vĂ­i dĂŽng cĂŽ . 5. ThÓ hiÖn/kh«ng thÓ hiÖn hÖ toÂč Ÿé gŸn trÂȘn cžc kh©u cña robot. 6. ThÓ hiÖn vÞ trÝ ÂźiÒu khiÓn.

7. M« phĂĄng Ÿéng lĂčc hĂ€c. 8. ThÓ hiÖn quÜ ÂźÂčo chuyÓn Ÿéng. 9. Sö dĂŽng cžc giĂ­i hÂčn cña khĂ­p. 10. SoÂčn th¶o ch−¬ng tr×nh v” dÂčy hĂ€c. 12. ThÓ hiÖn hoÆc kh«ng thÓ hiÖn HÖ toÂč Ÿé gŸn trÂȘn ŸÚi t−üng hiÖn thĂȘi. 13. ChuyÓn ¼Õn ŸÚi t−üng tiÕp theo (khi thiÕt kÕ). 14. Xžc ¼Þnh vÞ trÝ tuyÖt ŸÚi cña ŸÚi t−üng hiÖn tÂči. 15. Xžc ¼Þnh vÞ trÝ t−¬ng ŸÚi cña ŸÚi t−üng hiÖn tÂči. 16. Reset vÞ trÝ cña ŸÚi t−üng hiÖn tÂči. 17. Ghi lÂči vÞ trÝ cña ŸÚi t−üng sau khi ÂźiÒu chØnh. 18. §−a robot vÒ vÞ trÝ dĂ”ng (Home position). 19. §iÒu khiÓn robot theo khĂ­p quay. Thanh c«ng cĂŽ thÂŒng ŸÞng (Thao tžc b»ng chuĂ©t) , tÝnh tĂ” trÂȘn xuĂšng : 1. DĂŻng chuĂ©t ¼Ó view, zoom v” Pan. 2.3. §iÒu khiÓn h−íng cña kh©u chÊp h”nh cuĂši b»ng chuĂ©t. 4. §iÒu khiÓn cžc khĂ­p 1,2,3 (DĂŻng cžc phÝm chuĂ©t). 5. Di chuyÓn th©n robot. (hÖ toÂč Ÿé cÂŹ sĂ«) 6. Di chuyÓn cžc ŸÚi t−üng (body) b»ng chuĂ©t. 7. Di chuyÓn tÊt c¶ cžc ŸÚi t−üng b»ng chuĂ©t.

9. ChuyÓn ŸÊi chuyÓn Ÿéng l” quay hoÆc tÞnh tiÕn (DĂŻng khi hiÖu chØnh ŸÚi t−üng vÏ). 11.12. Tšng gi¶m tĂšc Ÿé ÂźiÒu khiÓn b»ng chuĂ©t. 6.4. Thao tžc chuĂ©t : Easy-Rob cho phÐp dĂŻng chuĂ©t vĂ­i nhiÒu chĂžc nšng nh− :

TS. PhÂčm §šng Ph−íc

Page 81: Robot cong nghiep

ROBOT C«ng nghiÖp 80

Khi nĂłt lÖnh sĂš 1 cña thanh c«ng cĂŽ thÂŒng ŸÞng ¼−üc chĂ€n : zoom (PhĂŁng to, thu nhĂĄ) : Ên nĂłt chuĂ©t ph¶i, rÂȘ chuĂ©t lÂȘn xuĂšng theo ph−¬ng thŸng ŸÞng cña m”n h×nh. Pan (thay ŸÊi vÞ trÝ cña ŸÚi t−üng so vĂ­i khung m”n h×nh) : Ên ŸÄng thĂȘi hai nĂłt chuĂ©t ph¶i v” trži, rÂȘ chuĂ©t trÂȘn m”n h×nh. Rotate (quay robot ¼Ó nh×n Ă« cžc gĂŁc Ÿé khžc nhau) : Ên chuĂ©t trži, rÂȘ chuĂ©t. Khi nĂłt lÖnh sĂš 4 cña thanh c«ng cĂŽ thÂŒng ŸÞng ¼−üc chĂ€n : Quay khĂ­p 1: Ên nĂłt chuĂ©t ph¶i, rÂȘ chuĂ©t (nÕu l” khĂ­p tÞnh tiÕn sÏ l”m kh©u chuyÓn Ÿéng tÞnh tiÕn). Quay khĂ­p 2: Ên ŸÄng thĂȘi 2 nĂłt chuĂ©t ph¶i v” trži, rÂȘ chuĂ©t. Quay khĂ­p 3: Ên nĂłt chuĂ©t trži, rÂȘ chuĂ©t. 6.5. GŸn hÖ toÂč Ÿé : MuĂšn xžc ¼Þnh hÖ toÂč Ÿé cña robot tr−íc hÕt ph¶i thĂčc hiÖn b»ng tay cžc c«ng viÖc sau: VÏ sÂŹ ŸÄ Ÿéng robot Ă« vÞ trÝ dĂ”ng, gŸn hÖ toÂč Ÿé cña cžc kh©u lÂȘn h×nh vÏ trÂȘn giÊy, xžc ¼Þnh cžc th«ng sĂš DH. Cžc b−íc tiÕp theo : 1- BËt nĂłt lÖnh sĂš 5 trÂȘn menu ngang, d−íi. 2- V”o menu chÝnh : FILE -> LOAD -> ROBOTFILE chĂ€n DHTempl -> OPEN. 3- V”o menu chÝnh : ROBOTICS -> ROBOTMOTION + KINEMATICS -> KINEMATICS DATA. 4- ChĂ€n Active Join -> Ok -> Activ Joint (1) RZ (hoÆc chĂ€n TZ nÕu l” khĂ­p tÞnh tiÕn) -> Ok -> NhËp cžc th«ng sĂš DH cña kh©u thĂž nhÊt. 5- ChĂ€n Quit -> Ok. V”o lÂči b−íc 4 -> Number Active Joint(1) -> Ok -> Ên Ÿóp chuĂ©t v”o vÖt xanh hoÆc ¼−a con trĂĄ v”o phÇn nhËp dĂ· liÖu (text box) Ên 2 (B©y giĂȘ sĂš kh©u Ÿéng l” 2), nhËp cžc th«ng sĂš DH cho kh©u sĂš 2 ... L”m t−¬ng tĂč cho ¼Õn khi Ÿñ sĂš khĂ­p yÂȘu cÇu. Ta cĂŁ thÓ kiÓm tra cžc sĂš liÖu Ÿ· nhËp b»ng cžch kÝch chuĂ©t v”o menu : ROBOTICS -> ROBOTMOTION + KINEMATICS -> KINEMATICS DATA-> KINEMATIC INFOMATION ¼Ó xem lÂči sĂš kh©u, khĂ­p v” cžc th«ng sĂš DH. NÕu v”o dĂ· liÖu sai ta cĂŁ thÓ hiÖu chØnh lÂči. §Ó thÓ hiÖn hÖ toÂč Ÿé cña robot trÂȘn m”n h×nh (HÖ toÂč Ÿé m”u v”ng), nhĂ­ kÝch chuĂ©t v”o nĂłt sĂš 5 cña thanh c«ng cĂŽ n»m ngang phÝa d−íi.

TS. PhÂčm §šng Ph−íc

Page 82: Robot cong nghiep

ROBOT C«ng nghiÖp 81

6.6. VÏ h×nh džng robot : Sau khi ho”n th”nh viÖc gŸn hÖ toÂč Ÿé cña robot, b−íc tiÕp theo l” vÏ h×nh džng cña nĂŁ. H×nh džng cña robot cĂŁ thÓ ¼−üc m« phĂĄng giĂšng nh− robot thĂčc nhĂȘ c«ng cĂŽ 3D CAD cña EasyRob. Menu 3D-CAD cho phÐp tÂčo ra cžc khĂši h×nh hĂ€c cÂŹ b¶n nh− khĂši trĂŽ, khĂši cÇu, khĂši chĂ· nhËt, khĂši tam gižc ... SĂč phĂši hĂźp hĂźp lĂœ vÒ kÝch th−íc v” vÞ trÝ cña cžc khĂši h×nh hĂ€c nÇy cho phÐp thÓ hiÖn ¼−üc cžc kÕt cÊu khžc nhau cña robot.

Cžc menu kÐo xuĂšng cña Menu 3D-CAD nh− h×nh 6.2, mĂ©t sĂš cžc chĂžc nšng chÝnh nh− sau : + Select group : ChĂ€n nhĂŁm ŸÚi t−üng ¼Ó thiÕt kÕ : 1/Robot group, 2/Tool group hay 3/ Body group. + Select body from group : ChĂ€n cžc bĂ© phËn cña robot Ÿ· vÏ (theo tÂȘn ¼Æt tr−íc) cña nhĂŁm chĂ€n hiÖn h”nh. + Create/Import new 3D body : TÂčo mĂ­i hoÆc nhËp mĂ©t bĂ© phËn Ÿ· cĂŁ sÂŒn. CÇn nhËp cžc th«ng sĂš cÇn thiÕt ¼Ó tÂčo ra ŸÚi t−üng mong muĂšn. + Modify sel. Body_set Jnt_idx : HiÖu chØnh cžc thuĂ©c tÝnh cña bĂ© phËn hiÖn h”nh. + Clone : Copy bĂ© phËn Âźang vÏ th”nh nhiÒu h×nh.

+ Render : BiÓu hiÖn ŸÚi t−üng Ă« dÂčng H×nh 6.2 : Menu 3D-CAD l−íi, dÂčng hĂ©p, . . . + Color : Thay ŸÊi m”u sŸc. + Name : Thay ŸÊi tÂȘn bĂ© phËn Âźang vÏ. + Clear : Xož ŸÚi t−üng (bĂ© phËn) hiÖn h”nh. + Position's : Thay ŸÊi vÞ trÝ cña ŸÚi t−üng (bĂ© phËn) hiÖn h”nh. + 3D CAD Coorsys Visibility : Cho hiÖn hoÆc Èn hÖ tĂ€a Ÿé cña ŸÚi t−üng vÏ. + Next Body in group : ChĂ€n ŸÚi t−üng vÏ tiÕp theo. DĂŻng menu 3D CAD ta lÇn l−üt vÏ tÊt c¶ cžc kh©u cña robot, cĂŁ thÓ dĂŻng cžc m”u sŸc khžc nhau ¼Ó thÓ hiÖn h×nh džng cña robot. L−u Ăœ trong quž tr×nh vÏ, nÕu vÏ sai ph¶i dĂŻng mĂŽc CLEAR ¼Ó xĂŁa Âźi hoÆc dĂŻng mĂŽc MODIFY CEL ¼Ó hiÖu chØnh. Mçi ŸÚi t−üng vÏ ph¶i gŸn vĂ­i mĂ©t kh©u nhÊt ¼Þnh, ¼−üc khai bžo trong mĂŽc SET JOINT INDEX. CĂŁ thÓ dĂŻng thanh c«ng cĂŽ thÂŒng ŸÞng phÝa ph¶i ¼Ó thay ŸÊi vÞ trÝ cña cžc ŸÚi t−üng vÏ cho thÝch hĂźp.

TS. PhÂčm §šng Ph−íc

Page 83: Robot cong nghiep

ROBOT C«ng nghiÖp 82

6.7. LËp tr×nh ÂźiÒu khiÓn robot m« phĂĄng : §Ó lËp tr×nh ÂźiÒu khiÓn robot Ÿ· m« phĂĄng ta dĂŻng ph−¬ng phžp lËp tr×nh kiÓu dÂčy hĂ€c. Sau khi Ÿ· thiÕt kÕ h×nh džng robot, c«ng cĂŽ gŸn trÂȘn kh©u chÊp h”nh cuĂši, cžc ŸÚi t−üng l”m viÖc khžc . . . ta cĂŁ thÓ lËp tr×nh ¼Ó ÂźiÒu khiÓn robot Ÿ· m« phĂĄng. ViÖc lËp tr×nh thĂčc hiÖn theo tr×nh tĂč sau Ÿ©y : NhÊp chuĂ©t v”o nĂłt lÖnh sĂš 10 (Show program window) ¼Ó kÝch hoÂčt cöa sĂŠ lËp tr×nh nh− h×nh 6.3 :

H×nh 6.3 : Cöa sĂŠ lËp tr×nh.

ChĂ€n New ¼Ó ¼Æt tÂȘn cho File ch−¬ng tr×nh. ChĂ€n Append nÕu muĂšn bĂŠ sung mĂ©t ch−¬ng tr×nh Ÿ· cĂŁ trÂȘn ¼Üa. Xžc ¼Þnh vÞ trÝ cžc ÂźiÓm m” dĂŽng cĂŽ ph¶i Âźi qua (dĂŻng chuĂ©t ¼Ó ÂźiÒu khiÓn cžc khĂ­p, dĂŻng menu ŸÞng). CĂž sau mçi lÇn xžc ¼Þnh ¼−üc mĂ©t vÞ trÝ th× Ên nĂłt PTP (ÂźiÒu khiÓn ÂźiÓm) hoÆc LIN (ÂźiÒu khiÓn ¼−ĂȘng) hoÆc VIA (diÓm trung gian dÉn h−íng khi ÂźiÒu khiÓn ¼−ĂȘng cong), CIRC (ÂźiÒu khiÓn theo ¼−ĂȘng cong). L”m liÂȘn tĂŽc cho tÊt c¶ cžc ÂźiÓm ¼Ó cĂŁ mĂ©t ch−¬ng tr×nh ho”n thiÖn. Sau khi kÕt thĂłc viÖc dÂčy robot hĂ€c, Ên nĂłt Close trÂȘn Program Window ¼Ó kÕt thĂłc. §Ó hiÖu chØnh v” bĂŠ sung cžc lÖnh ÂźiÒu khiÓn khžc v”o ch−¬ng tr×nh, Ên chuĂ©t v”o nĂłt EDIT, DĂŻng cžc lÖnh cña EasyRob nh− d−íi Ÿ©y ¼Ó ho”n thiÖn ch−¬ng tr×nh. ERPL - EASY-ROB-Program Language Ghi chĂł : - §n vÞ chiÒu d”i l” MÐt [m], GĂŁc l” Ÿé [deg] hoÆc [%] - §n vÞ cña tĂšc Ÿé l” [m/s] - VÞ trÝ v” h−íng cña hÖ tĂ€a Ÿé gŸn trÂȘn kh©u chÊp h”nh cuĂši ¼−üc xžc ¼Þnh gĂ„m : X, Y v” Z : chØ tĂ€a Ÿé vÞ trÝ, A, B v” C chØ gĂŁc h−íng. H−íng cña kh©u chÊp h”nh cuĂši xžc ¼Þnh theo cžc gĂŁc ABC l”: Rot (A,B,C) = Rot(X,A) * Rot(Y,B) * Rot(Z,C) CÊu trĂłc chung cña ch−¬ng tr×nh, M« t¶ cĂł phžp mĂ©t sĂš lÖnh hay dĂŻng : PROGRAMFILE : BŸt ¼Çu ch−¬ng tr×nh

TS. PhÂčm §šng Ph−íc

Page 84: Robot cong nghiep

ROBOT C«ng nghiÖp 83

ENDPROGRAMFILE or END : KÕt thĂłc ch−¬ng tr×nh. CALL fct_name : GĂ€i mĂ©t h”m cĂŁ tÂȘn fct_name(), Ÿ· ¼−üc ¼Þnh nghÜa trong ch−¬ng tr×nh. CALL FILE filename : GĂ€i mĂ©t File ch−¬ng tr×nh cĂŁ tÂȘn filename, File ph¶i cĂŁ cung cÊu trĂłc nh− ch−¬ng tr×nh chÝnh. FCT fct_name() : BŸt ¼Çu §Þnh nghÜa mĂ©t h”m cĂŁ tÂȘn fct_name(). ENDFCT : KÕt thĂłc ¼Þnh nghÜa mĂ©t function. ! Cžc ghi chĂł trong ch−¬ng tr×nh. TOOL X Y Z A B C [m,deg] : §Þnh tĂ€a Ÿé ÂźiÓm cuĂši cña dĂŽng so so vĂ­i kh©u chÊp h”nh cuĂši. PTP X Y Z A B C [m,deg] : Di chuyÓn robot ¼Õn ÂźiÓm mĂ­i (tĂ€a Ÿé tuyÖt ŸÚi). §iÒu khiÓn ÂźiÓm. PTP_REL dX dY dZ dA dB dC [m,deg] : Di chuyÓn robot ¼Õn ÂźiÓm mĂ­i (tĂ€a Ÿé t−¬ng ŸÚi). §iÒu khiÓn ÂźiÓm. LIN X Y Z A B C [m,deg] : Di chuyÓn robot ¼Õn ÂźiÓm mĂ­i (tĂ€a Ÿé tuyÖt ŸÚi). §iÒu khiÓn ¼−ĂȘng. LIN_REL dX dY dZ dA dB dC [m,deg] : Di chuyÓn robot ¼Õn ÂźiÓm mĂ­i (tĂ€a Ÿé t−¬ng ŸÚi). §iÒu khiÓn ¼−ĂȘng. CIRC X Y Z A B C [X2 Y2 Z2] [m,deg] : Di chuyÓn robot ¼Õn ÂźiÓm mĂ­i (tĂ€a Ÿé tuyÖt ŸÚi). §iÒu khiÓn ¼−ĂȘng cong. [X2 Y2 Z2] - §iÓm trung gian (3 ÂźiÓm ¼Ó xžc ¼Þnh mĂ©t cung trßn). CIRC_REL dX dY dZ dA dB dC [dX2 dY2 dZ2] [m,deg] : Di chuyÓn robot ¼Õn ÂźiÓm mĂ­i (tĂ€a Ÿé t−¬ng ŸÚi). §iÒu khiÓn ¼−ĂȘng cong. WAIT x [sec] : Robot dĂ”ng hoÂčt Ÿéng trong x gi©y. ERC TRACK ON,OFF : ThÓ hiÖn hoÆc kh«ng thÓ hiÖn quĂŒ ÂźÂčo chuyÓn Ÿéng. ERC LOAD TOOL filename : GĂ€i mĂ©t Tool file (*.tol) ERC LOAD VIEW filename : GĂ€i mĂ©t View file (*.vie) ERC LOAD ROBOT filename Loads a Robot file (*.rob) ERC LOAD BODY filename Loads a Body file (*.bod) ERC LOAD TAGS filename Loads a Tag file (*.tag) ERC GRAB BODY ’bodyname’ : DĂŽng cĂŽ cÇm lÊy mĂ©t vËt thÓ (body) cĂŁ tÂȘn Bodyname. ERC GRAB BODY_GRP : DĂŽng cĂŽ cÇm lÊy mĂ©t nhĂŁm vËt thÓ (Body_Grp). ERC RELEASE BODY ’bodyname’ : DĂŽng cĂŽ th¶ (bu«ng) mĂ©t vËt thÓ (body) cĂŁ tÂȘn Bodyname. ERC RELEASE BODY_GRP DĂŽng cĂŽ th¶ (bu«ng) mĂ©t nhĂŁm vËt thÓ (Body_Grp). ERC ROBOT_BASE XYZ ABC [m,deg] : Di chuyÓn gĂšc tĂ€a Ÿé cÂŹ b¶n cña robot ¼Õn vÞ trÝ mĂ­i. v.v... Cßn rÊt nhiÒu cžc lÖnh khžc cña Easy-Rob, cĂŁ thÓ tham kh¶o trÂȘn Website: http ://www. easy-rob.com.

=================================================

TS. PhÂčm §šng Ph−íc

Page 85: Robot cong nghiep

Robot c«ng nghiÖp 84

ch−¬ng VII

§éng lĂčc hĂ€c Robot (Dynamic of Robot)

7.1. NhiÖm vĂŽ v” ph−¬ng phžp ph©n tÝch Ÿéng lĂčc hĂ€c robot NghiÂȘn cĂžu Ÿéng lĂčc hĂ€c robot l” c«ng viÖc cÇn thiÕt khi ph©n tÝch cĂČng nh− tĂŠng hĂźp quž tr×nh ÂźiÒu khiÓn chuyÓn Ÿéng. ViÖc nghiÂȘn cĂžu Ÿéng lĂčc hĂ€c robot th−ĂȘng gi¶i quyÕt hai nhiÖm vĂŽ sau Ÿ©y : 1/ Xžc ¼Þnh momen v” lĂčc Ÿéng xuÊt hiÖn trong quž tr×nh chuyÓn Ÿéng. Khi Ÿã qui luËt biÕn ŸÊi cña biÕn khĂ­p qi(t) coi nh− Ÿ· biÕt. ViÖc tÝnh tožn lĂčc trong cÂŹ cÊu tay mžy l” rÊt cÇn thiÕt ¼Ó chĂ€n c«ng suÊt Ÿéng cÂŹ, kiÓm tra Ÿé bÒn, Ÿé cĂžng vĂ·ng, ٦m b¶o Ÿé tin cËy cña robot. 2/ Xžc ¼Þnh cžc sai sĂš Ÿéng tĂžc l” sai lÖch so vĂ­i qui luËt chuyÓn Ÿéng theo ch−¬ng tr×nh. LĂłc nÇy cÇn kh¶o sžt Ph−¬ng tr×nh chuyÓn Ÿéng cña robot cĂŁ tÝnh ¼Õn ¼Æc tÝnh Ÿéng lĂčc cña Ÿéng cÂŹ v” cžc kh©u. CĂŁ nhiÒu ph−¬ng phžp nghiÂȘn cĂžu Ÿéng lĂčc hĂ€c robot, nh−ng th−ĂȘng gÆp hÂŹn c¶ l” ph−¬ng phžp cÂŹ hĂ€c Lagrange, cĂŽ thÓ l” dĂŻng ph−¬ng tr×nh Lagrange - Euler. §Úi vĂ­i cžc kh©u khĂ­p cña robot, vĂ­i cžc nguĂ„n Ÿéng lĂčc v” kÂȘnh ÂźiÒu khiÓn riÂȘng biÖt, kh«ng thÓ bĂĄ qua cžc hiÖu Ăžng trĂ€ng tr−ĂȘng (gravity effect), qužn tÝnh (initial), t−¬ng hĂŠ (Coriolis), ly t©m (centripetal)... m” nhĂ·ng khÝa cÂčnh nÇy ch−a ¼−üc xÐt ¼Çy Ÿñ trong cÂŹ hĂ€c cĂŠ ÂźiÓn; CÂŹ hĂ€c Lagrange nghiÂȘn cĂžu cžc vÊn ¼Ò nÂȘu trÂȘn nh− mĂ©t hÖ thĂšng khÐp kÝn nÂȘn Ÿ©y l” nguyÂȘn lĂœ cÂŹ hĂ€c thÝch hĂźp ŸÚi vĂ­i cžc b”i tožn Ÿéng lĂčc hĂ€c robot. 7.2. CÂŹ hĂ€c Lagrange vĂ­i cžc vÊn ¼Ò Ÿéng lĂčc cña robot. H”m Lagrange cña mĂ©t hÖ thĂšng nšng l−üng ¼−üc ¼Þnh nghÜa : L = K - P (7.1) Trong Ÿã : K l” tĂŠng Ÿéng nšng cña hÖ thĂšng P l” tĂŠng thÕ nšng K v” P ¼Òu l” nhĂ·ng ÂźÂči l−üng v« h−íng nÂȘn cĂŁ thÓ chĂ€n bÊt cĂž hÖ toÂč Ÿé thÝch hĂźp n”o ¼Ó b”i tožn ¼−üc Ÿn gi¶n. §Úi vĂ­i mĂ©t robot cĂŁ n kh©u, ta cĂŁ :

v” K Kii

n= ∑

=1P Pi

i

n= ∑

=1

Ă« Ÿ©y, Ki v” Pi l” Ÿéng nšng v” thÕ nšng cña kh©u thĂž i xÐt trong hÖ toÂč Ÿé chĂ€n.Ta biÕt mçi ÂźÂči l−üng Ki v” Pi l” mĂ©t h”m sĂš phĂŽ thuĂ©c nhiÒu biÕn sĂš: Ki = K(qi, ) v” Piq& i = P(qi, ) &q i

VĂ­i qi l” toÂč Ÿé suy rĂ©ng cña khĂ­p thĂž i. NÕu khĂ­p thĂž i l” khĂ­p quay th× qi l” gĂŁc quay Ξi, nÕu l” khĂ­p tÞnh tiÕn th× qi l” Ÿé d”i tÞnh tiÕn di. Ta ¼Þnh nghÜa : LĂčc tžc dĂŽng lÂȘn kh©u thĂž i (i=1, 2,..., n) vĂ­i quan niÖm l” lĂčc tĂŠng qužt (Generalized forces), nĂŁ cĂŁ thÓ l” mĂ©t lĂčc hoÆc mĂ©t momen (phĂŽ thuĂ©c v”o biÕn khĂ­p qi l” tÞnh tiÕn hoÆc quay), ¼−üc xžc ¼Þnh bĂ«i:

Fi = −ddt

Lq

Lqi i

∂∂

∂∂&

(7.2)

TS. PhÂčm §šng Ph−íc

Page 86: Robot cong nghiep

Robot c«ng nghiÖp 85

Ph−¬ng tr×nh nÇy ¼−üc gĂ€i l” ph−¬ng tr×nh Lagrange-Euler, hay th−ĂȘng ¼−üc gĂ€i tŸt l” ph−¬ng tr×nh Lagrange. 7.3. VÝ dĂŽ žp dĂŽng : XÐt mĂ©t robot cĂŁ hai kh©u nh− h×nh vÏ, Cžc kh©u cĂŁ chiÒu d”i l” d1 v” d2 vĂ­i cžc khĂši l−üng t−¬ng Ăžng m1 v” m2 qui ŸÊi vÒ ¼Çu mĂłt cña kh©u. Robot ¼−üc ¼Æt thÂŒng ŸÞng chÞu gia tĂšc trĂ€ng tr−ĂȘng g. Cžc khĂ­p chuyÓn Ÿéng quay vĂ­i cžc biÕn khĂ­p Ξ1 v” Ξ2. TÝnh lĂčc tĂŠng qužt.

Qua vÝ dĂŽ nÇy, chØ vĂ­i mĂ©t mĂši liÂȘn kÕt hai kh©u, cžc vÊn ¼Ò ¼Æt ra ¼Òu Ÿ· cĂŁ mÆt trong quž tr×nh nghiÂȘn cĂžu Ÿéng lĂčc hĂ€c, v” do Ÿã, vÝ dĂŽ nÂȘu trÂȘn cĂŁ thÓ mĂ« rĂ©ng ¼Ó žp dĂŽng trong nhĂ·ng tr−ĂȘng hĂźp phĂžc tÂčp hÂŹn. §Úi vĂ­i kh©u 1 :

m2

m1

Ξ2

Ξ1

g = 9,81m/s2

y2

y1

x2x1O0

z

x

y

K m v m d1 1 12

1 12

121

212

= = &Ξ (7.3)

P1 = -m1gd1cosΞ1 (7.4) §Úi vĂ­i kh©u 2 : VÒ toÂč Ÿé : x2 = d1sinΞ1 + d2sin(Ξ1 + Ξ2) y2 = -d1cosΞ1 - d2cos(Ξ1 + Ξ2) ChiÒu cao thÕ nšng : h = d1cosΞ1 + d2cos(Ξ1 + Ξ2)

VÒ mÆt vËn tùc : v x y22

22

22= +& &

VĂ­i & cos( ) & cos( )( & & )x ddt

x d d2 2 1 1 1 2 1 2 1 2= = + + +Ξ Ξ Ξ Ξ Ξ Ξ

& sin( ) & sin( )( & & )y ddt

y d d2 2 1 1 1 2 1 2 1= = + + +Ξ Ξ Ξ Ξ Ξ Ξ2

[ ]v d d d d22

12

12

22

12

1 2 22

1 2 2 12

1 22 2= + + + + +& ( & & & & ) cos( )( & & & )Ξ Ξ Ξ Ξ Ξ Ξ Ξ Ξ Ξ

§éng nšng v” thÕ nšng sÏ l” :

[ ]K m v m d d d d2 2 22

2 12

12

22

12

1 2 22

1 2 2 12

1 212

12

2 2= = + + + + +& ( & & & & ) cos( )( & & & )Ξ Ξ Ξ Ξ Ξ Ξ Ξ Ξ Ξ (7.5)

(7.6) [ ]P m g d d2 2 1 1 2 1= − + +cos( ) cos( )Ξ Ξ 2Ξ 7.4. H”m Lagrange v” lĂčc tĂŠng qužt : žp dĂŽng h”m Lagrange cho vÝ dĂŽ trÂȘn, ta cĂŁ : L = (K1 + K2) - (P1 + P2)

L m m d m d m d d= + + + + + +12

12

21 2 12

12

2 22

12

1 2 22

2 1 2 2 12

1 2( ) & ( & & & & ) cos ( & & & )Ξ Ξ Ξ Ξ Ξ Ξ Ξ Ξ +Ξ

+ + + +( ) cos cos(m m gd m gd1 2 1 1 2 2 1 2 )Ξ Ξ Ξ (7.7)

Khi tÝnh lĂčc tĂŠng qužt, cžc biÕn cña hÖ : q1 = Ξ1 v” q2 = Ξ2. §Úi vĂ­i kh©u 1 : ∂∂

∂∂ξ

Ξ Ξ Ξ Ξ Ξ ΞLq

L m m d m d m d d m d d& &

( ) & ( & & ) cos & cos &

1 11 2 1

21 2 2

21 2 2 1 2 2 1 2 1 2 22= = + + + + + Ξ2

TS. PhÂčm §šng Ph−íc

Page 87: Robot cong nghiep

Robot c«ng nghiÖp 86

ddt

L m m d m d m d d m d d∂∂ξ

Ξ Ξ Ξ Ξ Ξ Ξ Ξ&

( ) && (&& && ) sin & & cos &&

11 2 1

21 2 2

21 2 2 1 2 2 2 1 2 1 2 2 12 2= + + + − + ξ −

− + m d d m d d2 1 2 2 22

2 1 2 2 2sin & cos &&ξ ξ ξ ξ∂∂

∂∂ξ

Ξ ΞLq

L m m gd m gd1 1

1 2 1 1 2 2 1 2= = − + − +( ) sin sin( ξ )

VËy :

F ddt

L L m m d m d m d d

m d m d d m d d m d dm m gd m gd

11 1

1 2 12

2 22

2 1 2 2 1

2 22

2 1 2 2 2 2 1 2 2 2 1 2 1 2 2 22

1 2 1 1 2 2 1 2

2

2

= − = + + + +

+ + − −+ + + +

∂∂ξ

∂∂ξ

Ξ Ξ

Ξ Ξ Ξ Ξ Ξ Ξ ΞΞ Ξ Ξ

&[( ) cos ]&&

[ cos ]&& sin & & sin &

( ) sin sin( )+ (7.8)

MuĂšn cho kh©u 1 quay ¼−üc mĂ©t gĂŁc Ξ1 th× Ÿéng cÂŹ ph¶i tÂčo ra mĂ©t lĂčc tĂŠng qužt ≄ F1. LĂčc tĂŠng qužt nÇy cĂŁ ¼Æc tÝnh phi tuyÕn, l” hĂźp tžc dĂŽng cña nhiÒu yÕu tĂš (non linear and cuppling). T−¬ng tĂč, ¼Ó tÝnh lĂčc tĂŠng qužt cña kh©u thĂž hai , ta cĂŁ :

∂∂ξ

Ξ Ξ ΞL m d m d m d d&

& & cos &

22 2

21 2 2

22 2 1 2 2= + + Ξ1

ddt

L m d m d m d d m d d∂∂ξ

Ξ Ξ Ξ Ξ Ξ Ξ&

&& && cos && sin & &

22 2

21 2 2

22 2 1 2 2 1 2 1 2 2 1= + + − ξ2

v” )sin()sin()sin( 21222

122122122122

ξξξξξξξ∂ξ∂

+−−−−= gdmddmddmL &&&

VËy :

)sin()sin(

]cos[

21222

12212

222212212

222

222

ΞΞΞΞ

ξξξ∂ξ∂

ξ∂∂

++−

++=−=

gdmddm

dmddmdmLLdtdF

&

&&&&& (7.9)

§Ó ph©n tÝch Ăœ nghÜa cžc th”nh phÇn trong biÓu thĂžc tÝnh lĂčc tĂŠng qužt, ta viÕt lÂči

c¾c biÓu thþc F1, F2 nh− sau :

F D D D D D D D1 11 1 12 2 111 12

122 22

112 1 2 121 1 2 1= + + + + + +&& && & & & & & &Ξ Ξ Ξ Ξ Ξ Ξ Ξ Ξ

F D D D D D D D2 12 1 22 2 211 12

222 22

212 1 2 221 1 2 2= + + + + + +&& && & & & & & &Ξ Ξ Ξ Ξ Ξ Ξ Ξ Ξ HiÖu Ăžng HiÖu Ăžng HiÖu Ăžng HiÖu Ăžng qužn tÝnh ly t©m t−¬ng hĂŠ trĂ€ng tr−ĂȘng Effective inertias Centripetal effect Coriolis effect Gravity

(Trong Ÿã : D111 = 0; D222 = 0; D112 = D121 = D212 = D221 =-m2d1d2sinΞ2 ...)

Trong cžc biÓu thĂžc trÂȘn, cžc hÖ sĂš dÂčng Dii hoÆc thÓ hiÖn hiÖu Ăžng qužn tÝnh tÂči

khĂ­p i hoÆc j g©y ra bĂ«i gia tĂšc tÂči khĂ­p i hoÆc j. Cžc sĂš hÂčng cĂŁ dÂčng

ijD2

ijjD jΞ& l” lĂčc ly t©m

tžc Ÿéng lÂȘn khĂ­p i g©y ra bĂ«i vËn tĂšc tÂči khĂ­p j. SĂš hÂčng dÂčng l” lĂčc

Cariolis tžc Ÿéng lÂȘn khĂ­p thĂž i g©y ra do vËn tĂšc tÂči khĂ­p j v” k. SĂš hÂčng cĂŁ dÂčng Djkkj ΞΞΞΞ &&&&

ikjijk DD +

i l” lĂčc trĂ€ng tr−ĂȘng tžc Ÿéng lÂȘn khĂ­p i.

TS. PhÂčm §šng Ph−íc

Page 88: Robot cong nghiep

Robot c«ng nghiÖp 87

7.5. Ph−¬ng tr×nh Ÿéng lĂčc hĂ€c robot : XÐt kh©u thĂž i cña mĂ©t robot cĂŁ n kh©u. TÝnh lĂčc tĂŠng qužt Fi cña kh©u thĂž i vĂ­i khĂši l−üng vi ph©n cña nĂŁ l” dm. LĂčc tĂŠng qužt Fi Ÿãng vai trß rÊt quan trĂ€ng khi x©y dĂčng sÂŹ ŸÄ khĂši ¼Ó thiÕt lËp h”m ÂźiÒu khiÓn cho robot cĂŁ n bËc tĂč do. 7. 5. 1. VËn tĂšc cña mĂ©t ÂźiÓm trÂȘn robot : MĂ©t ÂźiÓm trÂȘn kh©u thĂž i ¼−üc m« t¶ trong hÖ toÂč Ÿé cÂŹ b¶n l” : r = Ti.

ir (7.10) Trong Ÿã : ir l” toÂč Ÿé cña ÂźiÓm xÐt ŸÚi vĂ­i kh©u thĂž i, ir kh«ng thay ŸÊi theo thĂȘi gian. Ti l” ma trËn chuyÓn ŸÊi tĂ” kh©u thĂž i vÒ hÖ toÂč Ÿé gĂšc : Ti = A1A2...Ai. Nh− vËy r l” mĂ©t h”m cña thĂȘi gian t. TĂšc Ÿé cña vi khĂši l−üng dm ¼−üc tÝnh bĂ«i c«ng thĂžc :

&r drdt

ddt

T rTq

qii i

jj

ij

i= = =⎛

⎝⎜⎜

⎞

⎠⎟⎟=

∑∂∂1

& r (7.11)

Khi tÝnh b×nh ph−¬ng cña vËn tĂšc nÇy ta cĂŁ :

(7.12) &.& ( & , & , & ) (& & )r r r x y z Tr r ro o oT= =∑ 2

z

x

y

i r dm

Kh©u i

O0

Ti

r

H×nh 7.1. Kh¶o sžt tĂšc Ÿé cña vi khĂši l−üng dm. VĂ­i rT l” chuyÓn vÞ vectÂŹ v” Tr l” viÕt tŸt cña Trace (vÕt cña ma trËn) :

Trace

a a aa a a

a a a a

a

n

n

n n nn

iii

n

11 12 1

21 22 2

1 2 11

1

...

...... ... ... ...

⎡

⎣

⎱⎱⎱⎱

⎀

⎊

⎄⎄⎄⎄

==∑

Hay :

[ ]⎄⎄⎄

⎊

⎀

⎱⎱⎱

⎣

⎡

⎄⎄⎄

⎊

⎀

⎱⎱⎱

⎣

⎡

2

2

2

yx

= zyx . zz

yx

Do vËy

& (&.& ) ( . . .r Tr r r Tr ddt

T r ddt

T rTi

iiT i T2 = = )

TS. PhÂčm §šng Ph−íc

Page 89: Robot cong nghiep

Robot c«ng nghiÖp 88

=⎡

⎣⎱⎱

⎀

⎊⎄⎄==

∑∑TrTq

q rTq

q ri

jj

i iT

kk

i T

k

i

j

i ∂∂

∂∂

& . &11

⎄⎄⎊

⎀

⎱⎱⎣

⎡= ∑∑

= =

i

jkj

k

TiTii

j

ii

k

qqqTrr

qTTr

1 1

. &&∂∂

∂∂

(7.13)

7. 5. 2. TÝnh Ÿéng nšng cña vi khĂši l−üng dm. KĂœ hiÖu Ki l” Ÿéng nšng cña kh©u thĂž i. dKi l” Ÿéng nšng cña vi khĂši l−üng dm ¼Æt tÂči vÞ trÝ ir trÂȘn kh©u thĂž i.

dK TrTq

r rTq

q qik

i i

j

i i T iT

kj k

j

i=

⎡

⎣⎱⎱

⎀

⎊⎄⎄==

∑∑12 11

∂∂

∂∂

. & & dm

=⎡

⎣⎱⎱

⎀

⎊⎄⎄==

∑∑12 11

TrTq

r dm rTq

q qk

i i

j

i i T iT

kj k

j

i ∂∂

∂∂

( . . ). & & (7.14)

V” do Ÿã Ÿéng nšng cña kh©u thĂž i sÏ l” :

⎄⎄⎊

⎀

⎱⎱⎣

⎡== ∑ ∫∑∫

= =

i

jkj

k

Ti

Khau

Tii

j

ii

ki qq

qTdmrr

qTTrdKK

1 i 1

)..(21

&&∂∂

∂∂

i Khau

(7.15)

§Æt gĂ€i l” ma trËn gi¶ qužn tÝnh (Pseudo inertia matrix). ∫=i

Tii rr.Khau

i dmJ

Ăœ nghÜa "gi¶ qužn tÝnh" ¼−üc sö dĂŽng v× khi thiÕt lËp ¼Çy Ÿñ cžc phÇn tö cña ma trËn Ji ta cĂŁ thÓ liÂȘn hÖ vĂ­i cžc khži niÖm "m«men qužn tÝnh Ÿéc cĂčc" v” tr×nh b”y cžc phÇn tö cña Ji giĂšng nh− cžc phÇn tö cña m«men qužn tÝnh Ÿéc cĂčc. Ta xÐt mĂši quan hÖ nÇy nh− sau : Theo ¼Þnh nghÜa ta cĂŁ :

= J∫=i

Tii rr.Khau

i dmJ i = (7.16)

⎄⎄⎄⎄⎄

⎊

⎀

⎱⎱⎱⎱⎱

⎣

⎡

∫∫∫∫∫∫∫∫∫∫∫∫∫∫∫∫

dmzdmydmxdm

zdmdmzzdmyzdmx

ydmzdmydmyydmx

xdmzdmxydmxdmx

iii

iiiiii

iiiiii

iiiiii

2

2

2

B©y giĂȘ ta nhŸc lÂči m«men qužn tÝnh Ÿéc cĂčc cña mĂ©t vËt thÓ bÊt kĂș nh− h×nh vÏ.

z

y x ω Theo ¼Þnh nghÜa ta cĂŁ :

∫ += dmzyxx )(I 22

∫ += dmzxyy )I 22

∫ += dmyxzz )(I 22 H×nh 7.2 : M«men qužn tÝnh Ÿéc cĂčc

V” v× : )(21)(

21)(

21x 2222222 yxzxzy +++++−=

VËy : ; .v.v
 2/)I I I( zzyyxx2 ++−=∫ dmx

Ngo”i ra ta cßn cĂŁ :

; ; ∫= xydmxyI ∫= yzdmyzI ∫= xzdmxzI

; ; ∫= xdmmx ∫= ydmmy ∫= zdmmz

TS. PhÂčm §šng Ph−íc

Page 90: Robot cong nghiep

Robot c«ng nghiÖp 89

§Úi chiÕu vĂ­i ma trËn gi¶ qužn tÝnh Ji, ta cĂŁ thÓ tr×nh b”y Ji nh− sau :

⎄⎄⎄⎄⎄⎄⎄

⎊

⎀

⎱⎱⎱⎱⎱⎱⎱

⎣

⎡

−+

+−

++−

=

mmzmymx

mz2

IIIII

myI2

IIII

mxII2

III

jzzyyxx

yzyz

zyzzyyxx

xy

zxyxzzyyxx

i (7.17)

Nh− vËy Ăœ nghÜa biÓu tr−ng cña Ji Ÿ· rĂą.

VËy ta cĂŁ : ⎄⎄⎊

⎀

⎱⎱⎣

⎡= ∑∑

= =

i

jkj

k

Ti

ij

ii

ki qq

qTJ

qTTrK

1 121

&&∂∂

∂∂

(7.18)

CuĂši cĂŻng, §éng nšng cña mĂ©t robot cĂŁ n kh©u ¼−üc tÝnh :

(7.19) ∑=

=n

iiKK

1

7. 5. 3. TÝnh thÕ nšng cña robot : ThÕ nšng cña kh©u i cĂŁ khĂši l−üng mi, trĂ€ng t©m ¼−üc xžc ¼Þnh bĂ«i vectÂŹ ri (vectÂŹ biÓu diÔn trĂ€ng t©m cña kh©u i trong hÖ toÂč Ÿé cÂŹ b¶n) l” :

Pi = -mi. g. ri = -mi. g. Ti iri (7.20)

Trong Ÿã, vectÂŹ gia tĂšc trĂ€ng tr−ĂȘng g ¼−üc biÓu diÔn d−íi dÂčng mĂ©t ma trËn cĂ©t :

⎄⎄⎄⎄

⎊

⎀

⎱⎱⎱⎱

⎣

⎡

−=

⎄⎄⎄⎄

⎊

⎀

⎱⎱⎱⎱

⎣

⎡

=

08,9

00

0z

y

x

ggg

g

ThÕ nšng cña to”n cÂŹ cÊu robot n kh©u Ÿéng sÏ l” :

∑=

−=n

ii

iii rgTmP

1

(7.21)

7. 5.4. H”m Lagrange : Sau khi xžc ¼Þnh Ÿéng nšng v” thÕ nšng cña to”n cÂŹ cÊu, ta cĂŁ h”m Lagrange cña robot cĂŁ n bËc tĂč do :

∑∑∑∑== = =

+⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

=n

ii

iiikj

n

i

i

j

i

k k

Ti

ij

i rgTmqqqTJ

qTTraceL

11 1 1 21

21

&& (7.22)

ChĂłng ta chĂł Ăœ r»ng, trong h”m Lagrange vÉn ch−a ¼Ò cËp ¼Õn ¶nh hâˆ’Ă«ng cña nguĂ„n truyÒn Ÿéng (gĂ„m cžc phÇn tÜnh (stator) v” phÇn Ÿéng (Rotor) cña Ÿéng cÂŹ ÂźiÖn). 7. 5. 5. Ph−¬ng tr×nh Ÿéng lĂčc hĂ€c robot : Ta Ÿ· biÕt lĂčc tĂŠng qužt ¼Æt lÂȘn kh©u thĂž i cña robot cĂŁ n kh©u (Ph−¬ng tr×nh Lagrange - Euler) :

Fi = −ddt

Lq

Lqi i

∂∂

∂∂&

(7.23)

Sau khi thiÕt lËp h”m Lagrange, vĂ­i p = 1... n, ta tÝnh ¼−üc :

TS. PhÂčm §šng Ph−íc

Page 91: Robot cong nghiep

Robot c«ng nghiÖp 90

(p l” chØ sĂš lÇn l−üt lÊy theo j v” k)

j

n

i

i

j p

Ti

ij

ik

n

i

i

k k

Ti

ip

i

p

qqTJ

qTTrq

qTJ

qTTr

qL

&&& ∑∑∑∑

= == =⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

+⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

=∂∂

1 11 1 21

21

(7.24)

Thay ŸÊi chØ sĂš gi¶ j th”nh k trong sĂš hÂčng thĂž hai ,v” ¼Ó Ăœ r»ng :

⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

=⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

=⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

j

Ti

ip

i

T

p

Ti

ij

i

p

Ti

ij

i

qTJ

qTTr

qTJ

qTTr

qTJ

qTTr (7.25)

ta cĂŁ : k

n

i

i

k p

Ti

ik

i

p

qqTJ

qTTr

qL

&& ∑∑

= =⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

=∂∂

1 1

(7.26)

CĂČng ¼Ó Ăœ r»ng : trong Ti(q1, q2, . . . , qi), vĂ­i qi l” cžc biÕn khĂ­p cña i khĂ­p ¼Çu tiÂȘn. Do

vËy, nÕu i < p th× 0=∂∂

p

i

qT

.

CuĂši cĂŻng ta cĂŁ : k

n

pi

i

k p

Ti

ik

i

p

qqTJ

qTTr

qL

&& ∑∑

= =⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

=∂∂

1

(7.27)

LÊy vi ph©n theo thĂȘi gian t cña ph−¬ng tr×nh trÂȘn :

k

n

pi

i

k p

Ti

ik

i

p

qqTJ

qTTr

dtd

qL

dtd

&&

1

∑∑= =

⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

=∂∂

+⎄⎄⎊

⎀

⎱⎱⎣

⎡

∂∂

∂∂∂

+⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

= ∑∑∑∑∑= = == =

mqqqTJ

qqTTrq

qTJ

qTTr k

n

pi

i

k

i

m p

Ti

imk

ik

n

pi

i

k p

Ti

ik

i &&&& 1 1

2

1

mqqqTJ

qqTTr k

n

pi

i

k

i

m k

Ti

imp

i &&∑∑∑= = = ⎄

⎄⎊

⎀

⎱⎱⎣

⎡

∂∂

∂∂∂

+1 1

2

(7.28)

(BiÕn ŸÊi theo chĂł Ăœ (7.25)) SĂš hÂčng cuĂši cña ph−¬ng tr×nh Lagrange Euler l” :

+⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂∂

=∂∂ ∑∑∑

= = =k

n

pi

i

j

i

k k

Ti

ipj

i

p

qqqTJ

qqTTr

qL

&& j1 1

2

21

ii

n

pi p

iik

n

i

i

jj

i

k j

Ti

ipk

i rqTgmqq

qTJ

qqTTr ∑∑∑∑

== = = ∂∂

+⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂∂

+ &&1 1 1

2

21 (7.29)

CuĂši cĂŻng ta cĂŁ lĂčc tĂŠng qužt cña kh©u p : pp

p qL

qL

dtdF

∂∂

∂∂

−=&

Thay thÕ cžc chØ sĂš p v” i th”nh i v” j, ta sÏ cĂŁ :

jj

n

ij i

jjk

n

ij

j

k

j

m i

Tj

jmk

jk

n

ij

j

k i

Tj

jk

ji r

qT

gmmqqqT

Jqq

TTrq

qT

JqT

TrF ∑∑∑∑∑∑== = == = ∂

∂−⎄

⎊

⎀⎹⎣

⎡

∂∂

∂∂∂

+⎟⎟⎠

⎞⎜⎜⎝

⎛

∂∂

∂∂

= &&&& 1 1

2

1

(7.30) VĂ­i mĂ©t robot cĂŁ n bËc tĂč do th× :

TS. PhÂčm §šng Ph−íc

Page 92: Robot cong nghiep

Robot c«ng nghiÖp 91

q = [q1, q2, . . . ,qn]T

q = & [ ]n21 q , ... ,q ,q &&& T

v” F = F[F1, F2, . . . , Fn]T

§Ó cho gĂ€n, ta biÓu diÔn : )(),()( qGqqqCqqJF ++= &&&& (7.31) Trong Ÿã : J thÓ hiÖn tžc dĂŽng cña qužn tÝnh, l” mĂ©t ma trËn ŸÚi xĂžng (n x n); C thÓ hiÖn tžc dĂŽng cña lĂčc ly t©m v” Cariolis, l” mĂ©t vectÂŹ (n x 1); G thÓ hiÖn tžc dĂŽng cña lĂčc trĂ€ng tr−ĂȘng, cĂČng l” mĂ©t vectÂŹ (n x 1). §©y l” ph−¬ng tr×nh Ÿéng lĂčc hĂ€c cña robot. NÕu thÂȘm v”o ph−¬ng tr×nh trÂȘn cžc tžc dĂŽng khžc nh− : FEX ¼Æc tr−ng cho cžc ngoÂči lĂčc tžc dĂŽng lÂȘn trĂŽc, V ¼Æc tr−ng cho hiÖu Ăžng ma sžt, ta cĂŁ :

EXFqVqGqqqCqqJF ++++= )()(),()( &&&&& (7.32)

TS. PhÂčm §šng Ph−íc

Page 93: Robot cong nghiep

robot c«ng nghiÖp 99

Ch−¬ng 9

TruyÒn Ÿéng v” ÂźiÒu khiÓn robot. 9.1. TruyÒn Ÿéng ÂźiÖn trong robot: TruyÒn Ÿéng ÂźiÖn ¼−üc dĂŻng khž nhiÒu trong kĂŒ thuËt robot, v× cĂŁ nhiÒu −u ÂźiÓm nh− l” ÂźiÒu khiÓn Ÿn gi¶n kh«ng ph¶i dĂŻng cžc bĂ© biÕn ŸÊi phĂŽ, kh«ng g©y bÈn m«i tr−ĂȘng, cžc loÂči Ÿéng cÂŹ ÂźiÖn hiÖn ÂźÂči cĂŁ thÓ lŸp trĂčc tiÕp trÂȘn cžc khĂ­p quay... Tuy nhiÂȘn so vĂ­i truyÒn Ÿéng thuĂ» lĂčc hoÆc thuĂ» khÝ th× truyÒn Ÿéng ÂźiÖn cĂŁ c«ng suÊt thÊp v” th«ng th−ĂȘng ph¶i cÇn thÂȘm hĂ©p gi¶m tĂšc v× th−ĂȘng cžc kh©u cña robot chuyÓn Ÿéng vĂ­i tĂšc Ÿé thÊp. Trong kĂŒ thuËt robot, vÒ nguyÂȘn tŸc cĂŁ thÓ dĂŻng Ÿéng cÂŹ ÂźiÖn cžc loÂči khžc nhau, nh−ng trong thĂčc tÕ chØ cĂŁ hai loÂči ¼−üc dĂŻng nhiÒu hÂŹn c¶. §ã l” Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu v” Ÿéng cÂŹ b−íc. Ng”y nay, do nhĂ·ng th”nh tĂču mĂ­i trong nghiÂȘn cĂžu ÂźiÒu khiÓn Ÿéng cÂŹ ÂźiÖn xoay chiÒu, nÂȘn cĂČng cĂŁ xu h−íng chuyÓn sang sö dĂŽng Ÿéng cÂŹ ÂźiÖn xoay chiÒu ¼Ó tržnh ph¶i trang bÞ thÂȘm bĂ© nguĂ„n ÂźiÖn mĂ©t chiÒu. Ngo”i ra, loÂči Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu kh«ng chĂŠi gĂŁp (DC brushless motor) cĂČng bŸt ¼Çu ¼−üc Ăžng dĂŽng v”o kĂŒ thuËt robot. 9.1.1. §éng cÂŹ ÂźiÖn mĂ©t chiÒu : §éng cÂŹ ÂźiÖn mĂ©t chiÒu gĂ„m cĂŁ hai phÇn :

+ Stato cĂš ¼Þnh vĂ­i cžc cuĂ©n d©y cĂŁ dßng ÂźiÖn c¶m hoÆc dĂŻng nam ch©m vÜnh cĂ·u. PhÇn nÇy cßn ¼−üc gĂ€i l” phÇn c¶m. PhÇn c¶m tÂčo nÂȘn tĂ” th«ng trong khe hĂ« kh«ng khÝ.

+ Roto vĂ­i cžc thanh dÉn. Khi cĂŁ dßng ÂźiÖn mĂ©t chiÒu chÂčy qua v” vĂ­i dßng tĂ” th«ng xžc ¼Þnh, roto sÏ quay. PhÇn nÇy gĂ€i l” phÇn Ăžng. TuĂș cžch ¼Êu d©y giĂ·a phÇn c¶m so vĂ­i phÇn Ăžng, ta cĂŁ nhĂ·ng loÂči Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu khžc nhau : + §éng cÂŹ kÝch tĂ” nĂši tiÕp (H×nh 9.1.a); + §éng cÂŹ kÝch tĂ” song song (H×nh 9.1.b); + §éng cÂŹ kÝch tĂ” hĂŠn hĂźp (H×nh 9.1.c).

a/ b/ c/ H×nh 9.1. Cžc loÂči Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu.

TS. PhÂčm §šng Ph−íc

Page 94: Robot cong nghiep

robot c«ng nghiÖp 100

Cžc th«ng sĂš chñ yÕu quyÕt ¼Þnh tÝnh nšng l”m viÖc cña Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu l” : U : §iÖn žp cung cÊp cho phÇn Ăžng; I : C−ĂȘng Ÿé dßng ÂźiÖn cña phÇn Ăžng; r : §iÖn trĂ« trong cña phÇn Ăžng; Ί : TĂ” th«ng; E : SĂžc ph¶n ÂźiÖn Ÿéng phÇn Ăžng. Cžc quan hÖ cÂŹ b¶n cña Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu l” : E = U - rI = knΊ k l” hÖ sĂš phĂŽ thuĂ©c v”o ¼Æc tÝnh cña d©y cuĂšn v” sĂš thanh dÉn cña phÇn Ăžng.

SĂš vßng quay cña Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu : Ω−

=k

IrUn

M«men Ÿéng C xžc ¼Þnh tĂ” ph−¬ng tr×nh c©n b»ng c«ng suÊt : EI = 2πnC

Hay : π

Ik2C Ί=

MuĂšn ÂźiÒu chØnh tĂšc Ÿé Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu cĂŁ thÓ thĂčc hiÖn b»ng cžch : - Thay ŸÊi tĂ” th«ng Ί, th«ng qua viÖc ÂźiÒu chØnh ÂźiÖn žp dßng kÝch tĂ”. Trong tr−ĂȘng hĂźp giĂ· nguyÂȘn ÂźiÖn žp phÇn Ăžng U, tšng tĂšc Ÿé tĂ” 0 ¼Õn tĂšc Ÿé ¼Þnh mĂžc, th× c«ng suÊt kh«ng ŸÊi cßn momen gi¶m theo tĂšc Ÿé. - §iÒu chØnh ÂźiÖn žp phÇn Ăžng. Trong tr−ĂȘng hĂźp tĂ” th«ng kh«ng ŸÊi, khi tšng tĂšc Ÿé tĂ” 0 ¼Õn tĂšc Ÿé ¼Þnh mĂžc th× m«men sÏ kh«ng ŸÊi, cßn c«ng suÊt tšng theo tĂšc Ÿé. MuĂšn ٦o chiÒu quay cña Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu cÇn thay ŸÊi hoÆc chiÒu cña tĂ” th«ng (tĂžc chiÒu cña dßng ÂźiÖn kÝch tĂ”) hoÆc thay ŸÊi chiÒu dßng ÂźiÖn phÇn Ăžng. 9.1.2. §éng cÂŹ b−íc : NguyÂȘn tŸc hoÂčt Ÿéng : TrÂȘn h×nh 9.2 l” sÂŹ ŸÄ Ÿéng cÂŹ b−íc loÂči Ÿn gi¶n nhÊt dĂŻng nam ch©m vÜnh cöu gĂ„m stato cĂŁ 4 cĂčc v” roto cĂŁ 2 cĂčc.

α αN

NÎČ ÎČ ÎČ' ÎČ' S N

SS

α' α'

H×nh 9.2 : SÂŹ ŸÄ nguyÂȘn lĂœ hoÂčt Ÿéng cña Ÿéng cÂŹ b−íc.

TS. PhÂčm §šng Ph−íc

Page 95: Robot cong nghiep

robot c«ng nghiÖp 101

NÕu cÊp ÂźiÖn cho cuĂ©n d©y αα' th× roto sÏ dĂ”ng Ă« vÞ trÝ m” dßng tĂ” qua cuĂ©n d©y l” lĂ­n nhÊt. NÕu cÊp ÂźiÖn cho cuĂ©n d©y ÎČÎČ' th× roto sÏ quay Âźi ±900 (PhĂŽ thuĂ©c chiÒu dßng ÂźiÖn cÊp v”o). Khi ŸÄng thĂȘi cÊp ÂźiÖn cho c¶ 2 cuĂ©n d©y α v” ÎČ th× roto sÏ dĂ”ng Ă« vÞ trÝ giĂ·a 00 v” 900, v” nÕu dßng ÂźiÖn v”o 2 cuĂ©n d©y ho”n to”n nh− nhau th× roto sÏ dĂ”ng Ă« vÞ trÝ 450. Nh− vËy vÞ trÝ cña roto phĂŽ thuĂ©c v”o sĂš cĂčc ¼−üc cÊp ÂźiÖn trÂȘn stato v” chiÒu cña dßng ÂźiÖn cÊp v”o. TrÂȘn Ÿ©y l” sÂŹ ŸÄ nguyÂȘn lĂœ cña Ÿéng cÂŹ b−íc loÂči cĂŁ Ýt cĂčc v” dĂŻng nam ch©m vÜnh cöu. TrÂȘn cÂŹ sĂ« Ÿã ta cĂŁ thÓ t×m hiÓu cžc loÂči Ÿéng cÂŹ cĂŁ nhiÒu cĂčc v” dĂŻng nam ch©m ÂźiÖn cĂŁ tĂ” tÝnh thay ŸÊi. Nh− vËy tuĂș theo cžch cÊp ÂźiÖn cho cžc cuĂ©n d©y trÂȘn stato ta cĂŁ thÓ ÂźiÒu khiÓn cžc vÞ trÝ dĂ”ng cña roto. ViÖc cÊp ÂźiÖn cho cžc cuĂ©n d©y cĂŁ thÓ sĂš hož, cho nÂȘn cĂŁ thÓ nĂŁi Ÿéng cÂŹ b−íc l” loÂči Ÿéng cÂŹ ÂźiÖn chuyÓn cžc tÝn hiÖu sĂš ¼Çu v”o th”nh chuyÓn Ÿéng cÂŹ hĂ€c tĂ”ng nÊc Ă« ¼Çu ra. Šu nh−üc ÂźiÓm : + Khi dĂŻng Ÿéng cÂŹ b−íc kh«ng cÇn mÂčch ph¶n hĂ„i cho c¶ ÂźiÒu khiÓn vÞ trÝ v” vËn tĂšc. + ThÝch hĂźp vĂ­i cžc thiÕt bÞ ÂźiÒu khiÓn sĂš. VĂ­i kh¶ nšng ÂźiÒu khiÓn sĂš trĂčc tiÕp, Ÿéng cÂŹ b−íc trĂ« th”nh th«ng dĂŽng trong cžc thiÕt bÞ cÂŹ ÂźiÖn tö hiÖn ÂźÂči. Tuy nhiÂȘn phÂčm vi Ăžng dĂŽng Ÿéng cÂŹ b−íc l” Ă« vĂŻng c«ng suÊt nhĂĄ v” trung b×nh. ViÖc nghiÂȘn cĂžu n©ng cao c«ng suÊt Ÿéng cÂŹ b−íc Âźang l” vÊn ¼Ò rÊt ¼−üc quan t©m hiÖn nay. Ngo”i ra, nĂŁi chung hiÖu suÊt cña Ÿéng cÂŹ b−íc thÊp hÂŹn cžc loÂči Ÿéng cÂŹ khžc. Cžc th«ng sĂš chñ yÕu cña Ÿéng cÂŹ b−íc : GĂŁc quay : §éng cÂŹ b−íc quay mĂ©t gĂŁc xžc ¼Þnh Ăžng vĂ­i mçi xung kÝch thÝch. GĂŁc b−íc Ξ c”ng nhĂĄ th× Ÿé ph©n gi¶i vÞ trÝ c”ng cao. SĂš b−íc s l” mĂ©t th«ng sĂš quan trĂ€ng :

Ξ3600

=s

TĂšc Ÿé quay v” tÇn sĂš xung : TĂšc Ÿé quay cña Ÿéng cÂŹ b−íc phĂŽ thuĂ©c v”o sĂš b−íc trong mĂ©t gi©y. §Úi vĂ­i hÇu hÕt cžc Ÿéng cÂŹ b−íc, sĂš xung cÊp cho Ÿéng cÂŹ b»ng sĂš b−íc (tÝnh theo phĂłt) nÂȘn tĂšc Ÿé cĂŁ thÓ tÝnh theo tÇn sĂš xung f. TĂšc Ÿé quay cña Ÿéng cÂŹ b−íc tÝnh theo c«ng thĂžc sau :

sfn 60

= (f : b−íc/phót)/(s : b−íc /vßng)

Tong Ÿã : n - tĂšc Ÿé quay (vßng/phĂłt) f - tÇn sĂš xung (Hz) s - SĂš b−íc trong mĂ©t vßng quay.

TS. PhÂčm §šng Ph−íc

Page 96: Robot cong nghiep

robot c«ng nghiÖp 102

Ngo”i ra cßn cžc th«ng sĂš quan trĂ€ng khžc nh− Ÿé chÝnh xžc vÞ trÝ, momen v” qužn tÝnh cña Ÿéng cÂŹ... Cžc loÂči Ÿéng cÂŹ b−íc : TuĂș theo kiÓu cña roto, Ÿéng cÂŹ b−íc ¼−üc chia th”nh cžc loÂči sau : + §éng cÂŹ b−íc kiÓu tĂ” trĂ« biÕn ŸÊi (VR : Variable Resistance) + §éng cÂŹ b−íc nam ch©m vÜnh cĂ·u (PM : Permanent Magnet ) + §éng cÂŹ b−íc kiÓu lai (Hybrid)

TuĂș theo sĂš cuĂ©n d©y Ÿéc lËp trÂȘn stato Ÿéng cÂŹ b−íc ¼−üc chia th”nh cžc loÂči : 2 pha, 3 pha hoÆc 4 pha.

Roto Ÿéng cÂŹ b−íc cĂŁ nhiÒu cĂčc (cßn gĂ€i l” ršng). SĂš cĂčc cña roto phĂši hĂźp vĂ­i sĂš cĂčc cña stato xžc ¼Þnh giž trÞ gĂŁc b−íc Ξ. GĂŁc b−íc lĂ­n nhÊt l” 900 Ăžng vĂ­i Ÿéng cÂŹ cĂŁ sĂš b−íc s = 4 b−íc/vßng. PhÇn lĂ­n nhĂ·ng Ÿéng cÂŹ b−íc hiÖn nay cĂŁ sĂš b−íc s = 200, nÂȘn Ξ = 1,80.

SĂš b−íc c”ng lĂ­n Ÿé ph©n gi¶i c”ng cao v” ¼Þnh vÞ c”ng chÝnh xžc. Nh−ng trong thĂčc tÕ, kh«ng thÓ tšng sĂš b−íc lÂȘn quž cao. Tuy nhiÂȘn cĂŁ thÓ dĂŻng c«ng nghÖ tÂčo b−íc nhĂĄ ¼Ó chia b−íc th”nh 2 nĂ·a b−íc (nh− h×nh b/ 9.2) hoÆc tĂ” 10 ¼Õn 125 b−íc nhĂĄ. C«ng nghÖ tÂčo b−íc nhĂĄ cßn gĂ€i l” tÂčo vi b−íc, chØ Ÿn gi¶n l” mĂ« rĂ©ng ph−¬ng phžp nĂŁi trÂȘn cho nhiÒu vÞ trÝ trung gian b»ng cžch cung cÊp nhĂ·ng giž trÞ dßng khžc nhau cho mçi cuĂ©n d©y. §éng cÂŹ ¼−üc tÂčo b−íc nhĂĄ cĂŁ Ÿé ph©n gi¶i tinh hÂŹn nhiÒu. VÝ dĂŽ, nÕu ph©n 125 b−íc nhĂĄ trong mĂ©t b−íc ¼Çy, vĂ­i 200 b−íc/vßng th× Ÿé ph©n gi¶i cña Ÿéng cÂŹ l” 125 x 200 = 25.000 b−íc nhĂĄ/ vßng. 9.2. TruyÒn Ÿéng khÝ nÐn v” thuĂ» lĂčc : Ngo”i truyÒn Ÿéng ÂźiÖn, trong kĂŒ thuËt robot cßn th−ĂȘng dĂŻng cžc loÂči truyÒn Ÿéng khÝ nÐn hoÆc thuĂ» lĂčc. 9.2.1. TruyÒn dÉn Ÿéng khÝ nÐn : DĂŻng khÝ nÐn trong hÖ truyÒn Ÿéng robot nhiÒu thuËn lĂźi nh− : Do cžc ph©n xĂ«ng c«ng nghiÖp th−ĂȘng cĂŁ mÂčng lĂ­i khÝ nÐn chung, nÂȘn Ÿn gi¶n hož ¼−üc phÇn thiÕt bÞ nguĂ„n Ÿéng lĂčc cho robot. HÖ truyÒn dÉn khÝ nÐn tÂŹng ŸÚi gĂ€n nhÑ, dÔ sö dĂŽng, dÔ ٦o chiÒu, ... Tuy nhiÂȘn hÖ truyÒn dÉn khÝ nÐn cĂČng cĂŁ nhiÒu nh−üc ÂźiÓm nh− : do tÝnh nÐn ¼−üc cña chÊt khÝ nÂȘn chuyÓn Ÿéng th−ĂȘng kÌm theo dao Ÿéng, dĂ”ng kh«ng chÝnh xžc, ngo”i ra cßn cÇn trang bÞ thÂȘm cžc thiÕt bÞ phun dÇu b«i trÂŹn, lĂ€c bĂŽi, gi¶m tiÕng Ă„n ... 9.2.2. TruyÒn dÉn Ÿéng thuĂ» lĂčc : HÖ truyÒn dÉn thuĂ» lĂčc cĂŁ nhĂ·ng −u ÂźiÓm nh− : T¶i trĂ€ng lĂ­n, qužn tÝnh bÐ, dÔ thay ŸÊi chuyÓn Ÿéng, dÔ ÂźiÒu khiÓn tĂč Ÿéng. Tuy nhiÂȘn chĂłng cĂČng cĂŁ nhĂ·ng nh−üc ÂźiÓm nh− : HÖ thuĂ» lĂčc lu«n ¼ßi hĂĄi bĂ© nguĂ„n, bao gĂ„m thĂŻng dÇu, bÂŹm thuĂ» lĂčc, thiÕt bÞ lĂ€c, b×nh tÝch dÇu, cžc

TS. PhÂčm §šng Ph−íc

Page 97: Robot cong nghiep

robot c«ng nghiÖp 103

loÂči van ÂźiÒu chØnh, ¼−ĂȘng Ăšng ... l”m hÖ truyÒn Ÿéng cho robot khž cĂ„ng kÒnh so vĂ­i truyÒn Ÿéng khÝ nÐn v” truyÒn Ÿéng ÂźiÖn. Nh×n chung, hÖ truyÒn dÉn thuĂ» lĂčc vÉn ¼−üc sö dĂŽng khž phĂŠ biÕn trong robot, nhÊt l” trong tr−ĂȘng hĂźp t¶i nÆng. Cžc phÇn tö trong hÖ truyÒn Ÿéng b»ng khÝ nÐn v” thuĂ» lĂčc Ÿ· ¼−üc tiÂȘu chuÈn hož. Cžc tÝnh tožn thiÕt kÕ hÖ truyÒn dÉn khÝ nÐn v” thuĂ» lĂčc Ÿ· ¼−üc nghiÂȘn cĂžu trong cžc gižo tr×nh riÂȘng. 9.3. Cžc ph−¬ng phžp ÂźiÒu khiÓn Robot : NhiÖm vĂŽ quan trĂ€ng ¼Çu tiÂȘn cña viÖc ÂźiÒu khiÓn robot l” b¶o ٦m cho ÂźiÓm tžc Ÿéng cuĂši E (End-effector) cña tay mžy dÞch chuyÓn bžm theo mĂ©t quĂŒ ÂźÂčo ¼Þnh tr−íc. Kh«ng nhĂ·ng thÕ, hÖ toÂč Ÿé gŸn trÂȘn kh©u chÊp h”nh cuĂši cßn ph¶i ٦m b¶o h−íng trong quž tr×nh di chuyÓn. Gi¶i b”i tožn ng−üc ph−¬ng tr×nh Ÿéng hĂ€c ta cĂŁ thÓ gi¶i quyÕt vÒ mÆt Ÿéng hĂ€c yÂȘu cÇu trÂȘn. §ã cĂČng l” nĂ©i dung cÂŹ b¶n ¼Ó x©y dĂčng ch−¬ng tr×nh ÂźiÒu khiÓn vÞ trÝ cho robot. Tuy nhiÂȘn viÖc gi¶i b”i tožn nÇy ch−a xÐt tĂ­i ÂźiÒu kiÖn thĂčc tÕ khi robot l”m viÖc, nh− l” cžc tžc Ÿéng cña momen lĂčc, ma sžt ... TuĂș theo yÂȘu cÇu n©ng cao chÊt l−üng ÂźiÒu khiÓn (Ÿé chÝnh xžc) m” ta cÇn tÝnh ¼Õn ¶nh hâˆ’Ă«ng cña cžc yÕu tĂš trÂȘn, v” theo Ÿã, ph−¬ng phžp ÂźiÒu khiÓn cĂČng trĂ« nÂȘn Âźa dÂčng v” phong phĂł hÂŹn.

9.3.1. §iÒu khiÓn tØ lÖ sai lÖch (PE : Propotional Error): NguyÂȘn tŸc cÂŹ b¶n cña ph−¬ng phžp nÇy rÊt dÔ hiÓu; Ÿã l” l”m cho hÖ

thĂšng thay ŸÊi theo chiÒu huĂ­ng cĂŁ sai lÖch nhĂĄ nhÊt. H”m sai lÖch cĂŁ thÓ l” Δ = Ξd - Ξ(t), Ă« Ÿ©y Ξd l” gĂŁc quay mong muĂšn v” Ξ(t) l” giž trÞ quay thĂčc tÕ cña biÕn khĂ­p, ta sÏ gĂ€i Ξd l” "gĂŁc ¼Æt". Khi Δ = 0 th× khĂ­p ÂźÂčt ¼−üc vÞ trÝ mong muĂšn. NÕu Δ < 0, th× khĂ­p Ÿ· di chuyÓn quž mĂžc v” cÇn chuyÓn Ÿéng ng−üc lÂči. Nh− vËy, kiÓu ÂźiÒu khiÓn chuyÓn Ÿéng nÇy l” lu«n cĂŁ chiÒu h−íng l”m cho sai lÖch Δ xÊp xØ zero.

BÂȘn cÂčnh Ÿã, chĂłng ta cĂČng cÇn quan t©m ¼Õn phÇn Ÿé lĂ­n, nghÜa l”, chĂłng ta kh«ng nhĂ·ng cÇn biÕt "l”m cho Ÿéng cÂŹ chuyÓn Ÿéng b»ng cžch n”o?" m” cßn cÇn biÕt "cÇn cung cÊp cho Ÿéng cÂŹ mĂ©t nšng l−üng (m«men Ÿéng) l” bao nhiÂȘu?". §Ó tr¶ lĂȘi c©u hĂĄi nÇy mĂ©t lÇn nĂ·a, chĂłng ta cĂŁ thÓ dĂŻng tÝn hiÖu sai sĂš Δ = Ξd - Ξ. ChĂłng ta h·y žp dĂŽng mĂ©t tÝn hiÖu ÂźiÒu khiÓn m” nĂŁ tØ lÖ vĂ­i Δ :

F = Kp(Ξd - Ξ(t)) (9.1) Qui luËt nÇy xžc ¼Þnh mĂ©t hÖ ÂźiÒu khiÓn ph¶n hĂ„i v” ¼−üc gĂ€i l” hÖ ÂźiÒu

khiÓn tØ lÖ sai lÖch.

TS. PhÂčm §šng Ph−íc

Page 98: Robot cong nghiep

robot c«ng nghiÖp 104

9.3.2. §iÒu khiÓn tØ lÖ - ÂźÂčo h”m (PD : Propotional Derivative): Ph−¬ng phžp ÂźiÓu khiÓn tØ lÖ sai lÖch cßn nhiÒu nh−üc ÂźiÓm nh− : HÖ dao

Ÿéng lĂ­n khi ma sžt nhĂĄ (t×nh trÂčng v−üt quž) v” Ă« trÂčng thži tÜnh, khi Δ → 0 th× momen cĂČng gÇn b»ng kh«ng, nÂȘn kh«ng giĂ· ¼−üc vÞ trÝ d−íi tžc dĂŽng cña t¶i.

§Ó khŸc phĂŽc ÂźiÒu trÂȘn, cĂŁ thÓ chĂ€n ph−¬ng phžp ÂźiÒu khiÓn tØ lÖ - ÂźÂčo h”m (PD), vĂ­i lĂčc tĂŠng qužt :

(9.2) (t)ΞKΔKF dp

&+=

Trong Ÿã : Δ - sai sĂš vÞ trÝ cña khĂ­p Ÿéng. Δ = Ξd - Ξ(t). - Th”nh phÇn ÂźÂčo h”m - vËn tĂšc gĂŁc. (t)Ξ& Ke - HÖ sĂš tØ lÖ sai lÖch vÞ trÝ.

Kd - HÖ sĂš tØ lÖ vËn tĂšc. 9.3.3. §iÒu khiÓn tØ lÖ - tÝch ph©n - ÂźÂčo h”m (PID : Propotional Integral Derivative): HÖ thĂšng vĂ­i cÊu trĂłc luËt ÂźiÒu khiÓn PD vÉn cßn mĂ©t sĂš nh−üc ÂźiÓm,

kh«ng phĂŻ hĂźp vĂ­i mĂ©t sĂš loÂči robot. MĂ©t hÖ thĂšng ÂźiÒu khiÓn khžc cĂŁ bĂŠ sung thÂȘm tÝn hiÖu tĂšc Ÿé ¼Æt v” sai lÖch tĂšc Ÿé Δ = tžc Ÿéng v”o kh©u khuyÕch ÂźÂči K

dξ& & (t)ξξd&& −

d. Ph−¬ng tr×nh lĂčc tžc Ÿéng lÂȘn khĂ­p Ÿéng cĂŁ dÂčng :

(9.3) ∫++=t

0ide Δ(t)dtKΔKΔKF &

VĂ­i Δ - sai sĂš tĂšc Ÿé. = . & Δ& (t)ΞΞd&& −

Nh− vËy, tuĂș theo cÊu trĂłc Ÿ· lĂča chĂ€n cña bĂ© ÂźiÒu khiÓn, ta Âźem ŸÚi

chiÕu cžc ph−¬ng tr×nh(9.1), (9.2) hoÆc (9.3) vĂ­i ph−¬ng tr×nh Lagrange - Euler, 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 nÇy cña hÖ ÂźiÒu khiÓn, cÇn xžc ¼Þnh cžc hÖ sĂš tØ lÖ Ke, Kd, Ki ¼Ó hÖ hoÂčt Ÿéng ĂŠn ¼Þnh. 9.3.4. H”m truyÒn chuyÓn Ÿéng cña mçi khĂ­p Ÿéng : NĂ©i dung phÇn nÇy tr×nh b”y ph−¬ng phžp x©y dĂčng h”m truyÒn ŸÚi vĂ­i tr−ĂȘng hĂźp chuyÓn Ÿéng mĂ©t bËc tĂč do, mçi khĂ­p th−ĂȘng ¼−üc ÂźiÒu khiÓn b»ng mĂ©t hÖ truyÒn Ÿéng riÂȘng. PhĂŠ biÕn hÂŹn c¶ l” Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu. XÐt sÂŹ ŸÄ truyÒn Ÿéng cña Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu vĂ­i tÝn hiÖu v”o l” ÂźiÖn žp Ua ¼Æt v”o phÇn Ăžng, tÝn hiÖu ra l” gĂŁc quay Ξm cña trĂŽc Ÿéng cÂŹ; Ÿéng cÂŹ kiÓu kÝch tĂ” Ÿéc lËp.

TS. PhÂčm §šng Ph−íc

Page 99: Robot cong nghiep

robot c«ng nghiÖp 105

_

+

Ua(t)

La Ra

Mm

Ξm

Lf

Uf

+

_

Rt

+

_eb(t)

ia(t)

Jm

H×nh 9.3. SÂŹ ŸÄ Ÿéng cÂŹ ÂźiÖn mĂ©t chiÒu. Trong thĂčc tÕ, trĂŽc Ÿéng cÂŹ ¼−üc nĂši vĂ­i hĂ©p gi¶m tĂšc rĂ„i tĂ­i trĂŽc phĂŽ t¶i nh− h×nh 9.4. GĂ€i n l” tØ sĂš truyÒn, ΞL l” gĂŁc quay cña trĂŽc phĂŽ t¶i, ta cĂŁ : ΞL(t) = n Ξm(t) (9.4) )(Ξ n (t)Ξ mL t&& =

)(Ξ n (t)Ξ mL t&&&& = ΞL

JL

ML

Mm

Jm

fm

Ξm= ΞL/n

fL

H×nh 9.4. SÂŹ ŸÄ Ÿéng cÂŹ ÂźiÖn cĂŻng phĂŽ t¶i.

M«men trÂȘn trĂŽc Ÿéng cÂŹ b»ng tĂŠng momen cÇn ¼Ó Ÿéng cÂŹ quay, cĂ©ng vĂ­i m«men phĂŽ t¶i quy vÒ trĂŽc Ÿéng cÂŹ.

(t) M (t) M M(t) *Lm += (9.5)

KĂœ hiÖu : Jm : M«men qužn tÝnh cña Ÿéng cÂŹ. JL : Momen qužn tÝnh phĂŽ t¶i. Ta cĂŁ :

(t)f (t)J (t)M mmmmm ΞΞ &&& += (9.6)

(t)f (t)J (t)M LLLLL ΞΞ &&& += (9.7) Trong Ÿã fm v” fL l” hÖ sĂš c¶n cña Ÿéng cÂŹ v” cña phĂŽ t¶i. Theo ¼Þnh luËt b¶o tĂ„n nšng l−üng, c«ng do phĂŽ t¶i sinh ra, tÝnh trÂȘn trĂŽc phĂŽ t¶i l” MLΞL ph¶i b»ng c«ng quy vÒ trĂŽc Ÿéng cÂŹ . TĂ” Ÿã ta cĂŁ : mΞM*

L

TS. PhÂčm §šng Ph−íc

Page 100: Robot cong nghiep

robot c«ng nghiÖp 106

(t)nM(t)Ξ

(t)(t)ΞM(t)M Lm

LL*L == (9.8)

Thay (9.1) v” (9.4) v”o c«ng thĂžc trÂȘn : [ ](t)Ξf (t)ΞJn(t)M mLmL

2*L

&&& += (9.9) Thay (9.3) v” (9.6) v”o (9.2) ta cã :

(t)Ξ)fnf (t)Ξ)JnJ(M(t) mL2

mmL2

m&&& ++=

Hay : (9.10) (t)Ξ f (t)ΞJM(t) mm&&& +=

VĂ­i : J = Jm + n2JL : M«men qužn tÝnh tĂŠng hiÖu dĂŽng. f = fm + n2fL : HÖ sĂš ma sžt tĂŠng hiÖu dĂŽng. M«men 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 quay v” vËn tĂšc gĂŁc, ta cĂŁ : M(t) = Kaia(t) (9.11) VĂ­i ia : C−ĂȘng Ÿé dßng ÂźiÖn phÇn Ăžng. Ka : HÖ sĂš tØ lÖ m«men. žp dĂŽng ¼Þnh luËt Kirchhoff cho mÂčch ÂźiÖn phÇn Ăžng :

(t)edt

(t)diL(t)iR(t)U ba

aaaa ++= (9.12)

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ÂŹ. (9.13) (t)Ξ K (t)e mbb

&= Kb : hÖ sĂš tØ lÖ cña sĂžc ph¶n ÂźiÖn Ÿéng. Sö dĂŽng phÐp biÕn ŸÊi Laplace, tĂ” (9.12) ta cĂŁ :

aa

mbaa sLR

(s)ΞsK - (s)U(s)I+

= (9.14)

TÔ (9.10) v” (9.11) ta cã : M(s) = s2JΞm(s) + sfΞm(s) = KaIa(s)

⇒ sfJs(s)IK(s)ξ 2

aam +

= (9.15)

Thay (9.14) v”o (9.15) :

⎄⎊

⎀⎹⎣

⎡++

=)sLsf)(RJ(s

)(ΞsK-(s)UK(s)Ξaa

2mba

am

s

⇒ a

aa2

m

mba

K)sLsf)(RJ(s

(s)Ξ)(ΞsK-(s)U ++=

s

TS. PhÂčm §šng Ph−íc

Page 101: Robot cong nghiep

robot c«ng nghiÖp 107

a

baaa2

m

a

KKsK)sLsf)(RJ(s

(s)Ξ(s)U +++

=

Hay : [ ]baaa

a

a

m

KK)sLf)(R(sJsK

(s)U(s)Ξ

+++= (9.16)

§©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 Ua). V× hÖ thĂšng gĂ„m cĂŁ Ÿéng cÂŹ v” phĂŽ t¶i nÂȘn tÝn hiÖu ra thĂčc tÕ l” gĂŁc quay cña trĂŽc phĂŽ t¶i ΞL, do Ÿã h”m truyÒn chuyÓn Ÿéng 1 bËc tĂč do cña tay mžy l” :

[ ]baaa

a

a

L

KKf))(sJsL(RsnK

(s)U(s)Ξ

+++= (9.17)

v” ta cĂŁ sÂŹ ŸÄ khĂši t−¬ng Ăžng vĂ­i h”m truyÒn trÂȘn l” :

1 sLa+ Ra

1 sJ + f

Ka1 s

n

Kb

_ +

ξL(s) Ua(s) ∑

H×nh 9.5 : SÂŹ ŸÄ khĂši h”m truyÒn chuyÓn Ÿéng mĂ©t bËc tĂč do.

Trong c«ng thĂžc (9.17) cĂŁ thÓ bĂĄ qua th”nh phÇn ÂźiÖn c¶m phÇn Ăžng La, v× nĂŁ th−ĂȘng quž nhĂĄ so vĂ­i cžc nh©n tĂš ¶nh hĂ«ng cÂŹ khÝ khžc. NÂȘn :

)KKfRJs(sRnK

(s)U(s)Ξ

baaa

a

a

L

++= (9.18)

9.3.6. §iÒu khiÓn vÞ trÝ mçi khĂ­p Ÿéng :

MĂŽc ¼Ých cña ÂźiÒu khiÓn vÞ trÝ l” l”m sao cho Ÿéng cÂŹ chuyÓn dÞch khĂ­p Ÿéng Âźi mĂ©t gĂŁc b»ng gĂŁc quay Ÿ· tÝnh tožn ¼Ó ٦m b¶o quĂŒ ÂźÂčo Ÿ· chĂ€n tr−íc (ch−¬ng 8). ViÖc ÂźiÒu khiÓn ¼−üc thĂčc hiÖn nh− sau : Theo tÝn hiÖu sai lÖch giĂ·a giž trÞ thĂčc tÕ v” giž trÞ tÝnh tožn cña vÞ trÝ gĂŁc m” ÂźiÒu chØnh ÂźiÖn žp Ua(t) ¼Æt v”o Ÿéng cÂŹ. NĂŁi cžch khžc, ¼Ó ÂźiÒu khiÓn Ÿéng cÂŹ theo quĂŒ ÂźÂčo mong muĂšn ph¶i ¼Æt v”o Ÿéng cÂŹ mĂ©t ÂźiÖn žp tØ lÖ thuËn vĂ­i Ÿé sai lÖch gĂŁc quay cña khĂ­p Ÿéng.

n(t))Ξ(t)Ξ~(K

ne(t)K

(t)U LLppa

−== (9.19)

Trong Ÿã Kp : hÖ sĂš truyÒn tÝn hiÖu ph¶n hĂ„i vÞ trÝ.

TS. PhÂčm §šng Ph−íc

Page 102: Robot cong nghiep

robot c«ng nghiÖp 108

e(t) = (t)Ξ(t)Ξ~ LL − : Ÿé sai lÖch gĂŁc quay.

Giž trÞ gĂŁc quay tĂžc thĂȘi : (t)Ξ~L ¼−üc Âźo b»ng c¶m biÕn quang hĂ€c hoÆc chiÕt žp. BiÕn ŸÊi Laplace ph−¬ng tr×nh (9.18) :

nE(s)K

n(s))Ξ(s)Ξ~(K

(s)U pLLpa =

−= (9.20)

Thay (9.20) v”o ph−¬ng tr×nh (9.18) :

G(s))KKfRJs(sR

KKE(s)

(s)Ξ

baaa

paL =++

= (9.21)

Sau khi biÕn ŸÊi ÂźÂči sĂš ta cĂŁ h”m truyÒn :

=+++

=+

=babaaa

2pa

L

L

KK)KKfs(RJRsKK

G(s)1G(s)

(s)Ξ~(s)Ξ

JRKKs

JR)KKf(Rs

JR/KK

a

ba

a

baa2

apa

++

+ (9.22)

Ph−¬ng tr×nh (9.22) cho thÊy r»ng hÖ ÂźiÒu khiÓn tØ lÖ cña mĂ©t khĂ­p Ÿéng l” mĂ©t hÖ bËc hai, nĂŁ sÏ lu«n ĂŠn ¼Þnh nÕu cžc hÖ sĂš cña cña ph−¬ng tr×nh bËc hai l” nhĂ·ng sĂš d−¬ng. §Ó n©ng cao ¼Æc tÝnh Ÿéng lĂčc hĂ€c v” gi¶m sai sĂš trÂčng thži ĂŠn ¼Þnh cña hÖ ng−ĂȘi ta cĂŁ thÓ tšng hÖ sĂš ph¶n hĂ„i vÞ trÝ Kp v” kÕt hĂźp l”m gi¶m dao Ÿéng trong hÖ b»ng cžch thÂȘm v”o th”nh phÇn ÂźÂčo h”m cña sai sĂš vÞ trÝ. VĂ­i viÖc thÂȘm ph¶n hĂ„i nÇy, ÂźiÖn žp ¼Æt lÂȘn Ÿéng cÂŹ sÏ tØ lÖ tuyÕn tÝnh vĂ­i sai sĂš vÞ trÝ v” ÂźÂčo h”m cña nĂŁ :

n

(t)eKe(t)Kn

(t))Ξ(t)Ξ~(K(t))Ξ(t)Ξ~(K(t)U vpLLvLLp

a

&&& +=

−+−= (9.23)

Trong Ÿã Kv l” hÖ sĂš ph¶n hĂ„i cña sai sĂš vÒ vËn tĂšc. VĂ­i ph¶n hĂ„i nÂȘu trÂȘn, hÖ thĂšng trĂ« th”nh khÐp kÝn v” cĂŁ h”m truyÒn nh−

thÓ hiÖn trÂȘn sÂŹ ŸÄ khĂši h×nh (9.6). §©y l” ph−¬ng phžp ÂźiÒu khiÓn tØ lÖ - §Âčo h”m.

1 sLa+ Ra

1 sJ + f

Ka1 s

n

Kb

∑Ua(s) ξL(s)

+ _

1 n

Kp+ sKv ∑

_

ΞL(s)

H×nh 9.6 : SÂŹ ŸÄ khĂši ÂźiÒu khiÓn chuyÓn dÞch mĂ©t khĂ­p Ÿéng cĂŁ liÂȘn hÖ ph¶n hĂ„i. TS. PhÂčm §šng Ph−íc

Page 103: Robot cong nghiep

robot c«ng nghiÖp 109

BiÕn ŸÊi Laplace ph−¬ng tr×nh (9.23) v” thay Ua(s) v”o (9.21) ta cĂŁ :

G(s))KKfRJs(sR

KKsKK)KKfRJs(sR

)sK(KKE(s)

(s)Ξ

baaa

pva

baaa

vpaL =++

+=

+++

= a (9.24)

TÔ Ÿã ta cã :

pavabaaa

2pva

L

L

KK)KKKKfs(RJRs)Ks(KK

G(s)1G(s)

(s)Ξ~(s)Ξ

+++++

=+

= (9.25)

TS. PhÂčm §šng Ph−íc

Page 104: Robot cong nghiep

Robot c«ng nghiÖp

92

Ch−¬ng VIII

ThiÕt kÕ quÜ ÂźÂčo robot. (Trajectory Planing)

Trong cžc Ăžng dĂŽng c«ng nghiÖp cña robot, ta th−ĂȘng gÆp hai tr−ĂȘng hĂźp sau :

Tr−ĂȘng hĂźp 1 : Kh©u chÊp h”nh cuĂši cña robot chØ cÇn ÂźÂčt ¼−üc vÞ trÝ v” h−íng tÂči cžc ÂźiÓm nĂłt (ÂźiÓm tĂča : Knot point). §©y chÝnh l” ph−¬ng phžp ÂźiÒu khiÓn ÂźiÓm (PTP). TÂči Ÿã, b”n tay robot thĂčc hiÖn cžc thao tžc cÇm nŸm ŸÚi t−üng hoÆc bu«ng nh¶ ŸÚi t−üng. §©y l” tr−ĂȘng hĂźp cña cžc robot thĂčc hiÖn c«ng viÖc vËn chuyÓn v” trao ŸÊi ph«i liÖu trong mĂ©t hÖ thĂšng tĂč Ÿéng linh hoÂčt robot hož. B”n tay robot kh«ng trĂčc tiÕp tham gia v”o cžc nguyÂȘn c«ng c«ng nghÖ nh− h”n, cŸt kim loÂči ... Cžc ÂźiÓm nĂłt l” mĂŽc tiÂȘu quan trĂ€ng nhÊt, cßn dÂčng ¼−ĂȘng Âźi tĂ­i cžc ÂźiÓm nĂłt l” vÊn ¼Ò thĂž yÕu. Trong tr−ĂȘng hĂźp nÇy Robot th−ĂȘng ¼−üc lËp tr×nh b»ng ph−¬ng phžp dÂčy hĂ€c (Teach and playback mode). Trong tr−ĂȘng hĂźp nÇy kh«ng cÇn tÝnh tožn ph−¬ng tr×nh Ÿéng hĂ€c hoÆc Ÿéng hĂ€c ng−üc robot, chuyÓn Ÿéng mong muĂšn ¼−üc ghi lÂči nh− mĂ©t tËp hĂźp cžc gĂŁc khĂ­p (thĂčc tÕ l” tËp hĂźp cžc giž trÞ m· hož cña biÕn khĂ­p) ¼Ó robot thĂčc hiÖn lÂči (Playback) khi l”m viÖc.

Tr−ĂȘng hĂźp 2 : Kh©u chÊp h”nh cuĂši cña robot ph¶i xžc ¼Þnh ¼−ĂȘng Âźi qua cžc ÂźiÓm nĂłt theo thĂȘi gian thĂčc. §ã l” tr−ĂȘng hĂźp cžc tay mžy trĂčc tiÕp thĂčc hiÖn cžc nguyÂȘn c«ng c«ng nghÖ nh− sÂŹn, h”n, cŸt kim loÂči ... VÊn ¼Ò thiÕt kÕ quĂŒ ÂźÂčo cho cžc robot trong tr−ĂȘng hĂźp nÇy l” rÊt quan trĂ€ng. NĂŁ quyÕt ¼Þnh trĂčc tiÕp chÊt l−üng thĂčc hiÖn cžc nguyÂȘn c«ng c«ng nghÖ m” robot ٦m nhËn. Trong ch−¬ng nÇy, chĂłng ta ¼Ò cËp ¼Õn b”i tožn thiÕt kÕ quĂŒ ÂźÂčo vĂ­i mĂ©t sĂš quĂŒ ÂźÂčo ÂźiÓn h×nh. Cžc quĂŒ ÂźÂčo nÇy kh«ng chØ cĂŁ Ăœ nghÜa trong tr−ĂȘng hĂźp Ăžng dĂŽng thĂž hai m” nĂŁ bao h”m mĂ©t Ăœ nghÜa chung cho mĂ€i robot, v× ngay c¶ tr−ĂȘng hĂźp Ÿn gi¶n nh− cžc robot thuĂ©c Ăžng dĂŽng thĂž nhÊt cĂČng thĂčc hiÖn nhĂ·ng chuyÓn Ÿéng quĂŒ ÂźÂčo cÂŹ b¶n m” chĂłng ta sÏ nghiÂȘn cĂžu d−íi Ÿ©y. 8.1. Cžc khži niÖm vÒ quĂŒ ÂźÂčo robot : §Ó xžc ¼Þnh ¼−üc ¼−ĂȘng Âźi mong muĂšn cña robot theo thĂȘi gian, quĂŒ ÂźÂčo cĂŁ thÓ ¼−üc tÝnh tožn thiÕt kÕ trong mĂ©t hÖ toÂč Ÿé truyÒn thĂšng Oxyz (Cartesian Space) hoÆc thiÕt kÕ trong kh«ng gian biÕn khĂ­p (kh«ng gian tr−ĂȘng vectÂŹ cžc toÂč Ÿé suy rĂ©ng cña robot), chÂŒng hÂčn vĂ­i robot 6 bËc tĂč do th× . [ TX ,.,,, 654321 ΞΞΞΞΞΞ= ] ThiÕt kÕ quĂŒ ÂźÂčo Ă« Ÿ©y ¼−üc hiÓu l” xžc ¼Þnh qui luËt chuyÓn Ÿéng cña cžc biÕn khĂ­p ¼Ó ÂźiÒu khiÓn chuyÓn Ÿéng cña tĂ”ng khĂ­p v” tĂŠng hĂźp th”nh chuyÓn Ÿéng chung cña robot theo mĂ©t quĂŒ ÂźÂčo Ÿ· ¼−üc xžc ¼Þnh.

TS. PhÂčm §šng Ph−íc

Page 105: Robot cong nghiep

Robot c«ng nghiÖp

93

QuĂŒ ÂźÂčo cÇn thiÕt kÕ nhÊt thiÕt ph¶i Âźi qua mĂ©t sĂš ÂźiÓm nĂłt cho tr−íc (Ýt nhÊt l” ÂźiÓm ¼Çu v” ÂźiÓm cuĂši). Ngo”i cžc ÂźiÓm nĂłt chÝnh, ta cßn cĂŁ thÓ chĂ€n thÂȘm cžc ÂźiÓm nĂłt phĂŽ gĂ€i l” ÂźiÓm dÉn h−íng (via point) ¼Ó tržnh cžc ch−íng ngÂči vËt.

Khi thiÕt kÕ quĂŒ ÂźÂčo trong kh«ng gian biÕn khĂ­p, tÂči mçi ÂźiÓm nĂłt ph¶i xžc ¼Þnh giž trÞ cña cžc biÕn khĂ­p b»ng ph−¬ng phžp tÝnh tožn Ÿéng hĂ€c ng−üc. ThĂȘi gian yÂȘu cÇu cña mçi ÂźoÂčn quĂŒ ÂźÂčo (giĂ·a 2 ÂźiÓm nĂłt) l” giĂšng nhau cho tÊt c¶ cžc khĂ­p v× vËy yÂȘu cÇu tÊt c¶ cžc khĂ­p ph¶i ÂźÂčt ¼Õn ÂźiÓm nĂłt ŸÄng thĂȘi. Ngo”i viÖc yÂȘu cÇu thĂȘi gian ph¶i giĂšng nhau cho cžc khĂ­p, viÖc xžc ¼Þnh cžc h”m quĂŒ ÂźÂčo cña mçi biÕn khĂ­p kh«ng phĂŽ thuĂ©c v”o cžc h”m cña cžc khĂ­p khžc. V× vËy viÖc thiÕt kÕ quĂŒ ÂźÂčo trong kh«ng gian biÕn khĂ­p Ÿn gi¶n v” dÔ tÝnh tožn hÂŹn khi m« t¶ trong hÖ toÂč Ÿé §Òcžc.

QuĂŒ ÂźÂčo thiÕt kÕ ph¶i ٦m b¶o cžc ÂźiÒu kiÖn liÂȘn tĂŽc (continous conditions) bao gĂ„m : + LiÂȘn tĂŽc vÒ vÞ trÝ (Position) + LiÂȘn tĂŽc vÒ tĂšc Ÿé (Velocity) + LiÂȘn tĂŽc vÒ gia tĂšc (Acceleration).

qi(t2)...

x(t)

t

xo

xf-1

x1 x2

xf

tf tf-1t2 t1 to

C¾c ¼iÓm nót

H×nh 8.1. TÝnh liÂȘn tĂŽc cña quĂŒ ÂźÂčo robot. §Ó thiÕt kÕ quĂŒ ÂźÂčo robot, ng−ĂȘi ta th−ĂȘng dĂŻng ph−¬ng phžp xÊp xØ cžc Âźa thĂžc bËc n, cžc quÜ ÂźÂčo th−ĂȘng gÆp l” : + QuÜ ÂźÂčo CS (Cubic Segment) : T−¬ng ¼−¬ng Âźa thĂžc bËc 3; + QuĂŒ ÂźÂčo LS (linear Segment) : T−¬ng ¼−¬ng Âźa thĂžc bËc 1; + QuĂŒ ÂźÂčo LSPB (Linear Segment with Parabolic Blend) : PhĂši hĂźp Âźa thĂžc bËc 2 vĂ­i Âźa thĂžc bËc 1.

§oÂčn thÂŒngq0 q2

q1

§−ĂȘng cong bËc 2

qf

H×nh 8.2 : QuĂŒ ÂźÂčo LSPB

TS. PhÂčm §šng Ph−íc

Page 106: Robot cong nghiep

Robot c«ng nghiÖp

94

+ QuĂŒ ÂźÂčo BBPB (Bang Bang Parabolic Blend) : l” tr−ĂȘng hĂźp ¼Æc biÖt cña quĂŒ ÂźÂčo LSPB khi ÂźoÂčn tuyÕn tÝnh thu vÒ b»ng 0 v” xuÊt hiÖn ÂźiÓm uĂšn.

H×nh 8.2 : QuĂŒ ÂźÂčo BBPB NÕu cho tr−íc nhiÒu ÂźiÓm nĂłt, ta cĂŁ thÓ žp dĂŽng nhiÒu dÂčng quĂŒ ÂźÂčo cÂŹ b¶n khžc nhau cho mĂ©t biÕn khĂ­p. 8.2. QuĂŒ ÂźÂčo Âźa thĂžc bËc 3 :

Khi thiÕt kÕ quĂŒ ÂźÂčo robot theo Âźa thĂžc bËc 3 qua cžc ÂźiÓm nĂłt, mçi ÂźoÂčn quĂŒ ÂźÂčo giĂ·a hai ÂźiÓm nĂłt sÏ ¼−üc biÓu diÔn b»ng mĂ©t ph−¬ng tr×nh bËc 3 riÂȘng biÖt. QuĂŒ ÂźÂčo Âź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 hai tÂči cžc ÂźiÓm nĂłt. TÂči thĂȘi ÂźiÓm tk ≀ t ≀ tk+1, quĂŒ ÂźÂčo xÊp xØ Âźa thĂžc bËc 3 cña biÕn khĂ­p thĂž i l” qi(t) cĂŁ dÂčng :

qi(t) = ai + bi(t - tk) + ci(t - tk)2 + di(t - tk)

3 (8.1)

Víi cžc r”ng buéc : qi(tk) = qk v” kki q )(tq && = qi(tk+1) = qk+1 v” 1k1ki q )(tq ++ = &&

TĂ” (8.1) ta thÊy : t = tk → ai = qk (8.2)

q0

qf

BËc 3

tk+1tk

qk

qk+1

t

qi(t)

LÊy ÂźÂčo h”m cña (8.1) theo t, ta cĂŁ : 2

kikiii )t(t3d)t(t2cb(t)q −+−+=&

TÂči : t = tk → (8.3) ki q b &= TÂči t = ti+1 ta cĂŁ hai tham sĂš :

2k

k1kkk1ki ÎŽt

ήt )qq(2)q3(qc ++ +−−=

&& (8.4)

3k

k1kkk1ki ÎŽt

)q 2(qήt )qq(d −−+= ++ &&

(8.5)

Trong Ÿã : k1kk t t ÎŽt −= + Cžc ph−¬ng tr×nh (8.4) v” (8.5) nhËn ¼−üc khi gi¶i (8.1) ... (8.3).

TÝnh liÂȘn tĂŽc cña vËn tĂšc l” sĂč ٦m b¶o cho quĂŒ ÂźÂčo kh«ng gÊp khĂłc, giËt cĂŽc, g©y sĂšc trong quž tr×nh hoÂčt Ÿéng cña robot. VËn tĂšc v” gia tĂšc tÂči ÂźiÓm cuĂši cña mĂ©t ÂźoÂčn ¼−ĂȘng cong bËc 3 chÝnh b»ng vËn tĂšc v” gia tĂšc cña ÂźoÂčn cong bËc 3 tiÕp theo. CÇn chĂł Ăœ r»ng khi thiÕt kÕ quĂŒ ÂźÂčo trong kh«ng gian §Ò cžt, ¼Ó ÂźiÒu khiÓn ¼−üc robot, Ă« mçi thĂȘi ÂźiÓm ¼Òu ph¶i t×m ¼−üc nghiÖm cña b”i tožn Ÿéng hĂ€c ng−üc. V× vËy yÂȘu cÇu "n·o bĂ©" cña robot (mžy tÝnh) ph¶i thĂčc hiÖn

TS. PhÂčm §šng Ph−íc

Page 107: Robot cong nghiep

Robot c«ng nghiÖp

95

mĂ©t khĂši l−üng cžc phÐp tÝnh khĂŠng lĂ„ trong mĂ©t kho¶ng thĂȘi gian rÊt ngŸn (v”i chĂŽc microgi©y) ¼Ó ٦m b¶o thĂȘi gian thĂčc khi robot hoÂčt Ÿéng. NÕu ta kh«ng t×m cžch c¶i biÕn thiÕt kÕ quĂŒ ÂźÂčo th× rÊt khĂŁ ٦m b¶o yÂȘu cÇu nÇy. * VÝ dĂŽ vÒ thiÕt kÕ quĂŒ ÂźÂčo CS: ThiÕt kÕ quĂŒ ÂźÂčo CS (Path with Cubic segment) cña khĂ­p thĂž i Âźi qua hai ÂźiÓm nĂłt cĂŁ giž trÞ q0 v” qf. VĂ­i cžc r”ng buĂ©c 0 ; 00 == fqq && .

TĂ” cžc c«ng thĂžc (8.2) . . . (8.5) ta xžc ¼Þnh cžc hÖ sĂš cña Âźa thĂžc bËc 3 nh− sau : ai = q0 ; bi = 0;

20f

0fi )t(t

)q3(qc−−

= V” 30f

0fi )t(t

)q2(q-d−−

=

Do vËy quĂŒ ÂźÂčo qi(t) cĂŁ dÂčng nh− sau : 3

030f

0f202

0f

0f0i )(

)t(t)q2(q)(

)t(t)q3(qq (t)q tttt −

−−

−−−−

+=

VËn tĂšc l” : 203

0f

0f02

0f

0fi )(

)t(t)q6(q)(

)t(t)q6(q (t)q tttt −

−−

−−−−

=&

V” gia tÚc l” : )()t(t

)q12(q)t(t

)q6(q (t)q 030f

0f2

0f

0fi tt −

−−

−−−

=&&

Trong vÝ dĂŽ trÂȘn, gi¶ sö thĂȘi gian t0 = 0 v” tf = 1 gi©y, th× : qi(t) = q0 + 3(qf - q0) t

2 - 2(qf - q0) t3

20f

0f

)t(t)q6(q

−−

−

tTÚc Ÿé

QuĂŒ ÂźÂčo

tf

t

tf

t

tf

Gia tĂšc

(t)q&

(t)q&&20f

0f

)t(t)q6(q

−−

0qq f0 == &&

t0

t0

t0

O

q0

q(t)

qf

H×nh 8.3. ThiÕt kÕ quĂŒ ÂźÂčo CS

TS. PhÂčm §šng Ph−íc

Page 108: Robot cong nghiep

Robot c«ng nghiÖp

96

TĂ” cžc ph−¬ng tr×nh quĂŒ ÂźÂčo, ph−¬ng tr×nh vËn tĂšc v” ph−¬ng tr×nh gia tĂšc ta x©y dĂčng ¼−üc cžc biÓu ŸÄ ¼Æc tÝnh chuyÓn Ÿéng cña khĂ­p thĂž i trÂȘn ÂźoÂčn quĂŒ ÂźÂčo thiÕt kÕ.

8.3. QuĂŒ ÂźÂčo tuyÕn tÝnh vĂ­i cung Ă« hai ¼Çu l” parabol (LSPB) : Khi yÂȘu cÇu c«ng cĂŽ gŸn trÂȘn kh©u chÊp h”nh cuĂši cña robot chuyÓn Ÿéng vĂ­i vËn tĂšc ¼Òu ¼Æn, ta dĂŻng quĂŒ ÂźÂčo LSPB.

qi(t)

tf tf - tbtf/2tb

v = constant

Parabol O t0

Parabol

t

(q0+qf)/2

H×nh 8.3. QuĂŒ ÂźÂčo LSPB. Cžc ÂźiÒu kiÖn liÂȘn tĂŽc cña quĂŒ ÂźÂčo nÇy thÓ hiÖn Ă« :

q(to) = q0 ; q(tf) = qf; v” 0 )(tq )(q f0 == && t v” ÂźiÒu kiÖn c«ng nghÖ l” v = constant. QuĂŒ ÂźÂčo ¼−üc chia l”m 3 ÂźoÂčn : a/ Trong ÂźoÂčn 1 : 0 ≀ t ≀ tb quĂŒ ÂźÂčo Parabol cĂŁ dÂčng : qi(t) = α + ÎČt + Îłt2 (8.6) Khi t = 0 th× α = q(t0) = q0 (8.7)

LÊy ÂźÂčo h”m (8.6) : t2ÎłÎČ(t)q +=& (8.8) Khi t = 0 th× 0 )(tq ÎČ o == &

TÂči thĂȘi ÂźiÓm tb ta cÇn cĂŁ vËn tĂšc b»ng h»ng sĂš vËn tĂšc cho tr−íc v : NÂȘn khi t = tb Îł = v/2tb

§Æt v/tb = a ⇒ Îł = a/2 v” quĂŒ ÂźÂčo cĂŁ dÂčng : qi(t) = q0 + at2/2 (0 ≀ t ≀ tb) (8.9)

b/ Trong ÂźoÂčn 2 : [tb, (tf-tb)] quĂŒ ÂźÂčo tuyÕn tÝnh cĂŁ dÂčng : qi(t) = α0 + vt

Do tÝnh ¼ùi xþng : 2

)q(q)2t(q f0f +

=

Suy ra 2tvα

2)q(q f

0f0 +=

+

VËy 2

)vtq(qα ff00

−+=

Ph−¬ng tr×nh quĂŒ ÂźÂčo tuyÕn tÝnh sÏ l” :

TS. PhÂčm §šng Ph−íc

Page 109: Robot cong nghiep

Robot c«ng nghiÖp

97

vt2

vtqq(t)q f0fi +

−+= (8.10)

TĂ” ÂźiÒu kiÖn liÂȘn tĂŽc vÒ vÞ trÝ, tÂči thĂȘi ÂźiÓm tb ta cĂŁ :

bf0f

2b

0 vt2

vtqq2

atq +−+

=+

RĂłt ra :

v

vtqqt ff0b

+−=

VĂ­i ÂźiÒu kiÖn tĂ„n tÂči : 0 < tb ≀ tf/2, dÉn ¼Õn :

v)q2(qt

vqq 0f

f0f −

≀<−

§iÒu nÇy xžc ¼Þnh vËn tĂšc ph¶i n»m giĂ·a cžc giĂ­i hÂčn trÂȘn, nÕu kh«ng chuyÓn Ÿéng sÏ kh«ng thĂčc hiÖn ¼−üc. VÒ mÆt vËt lĂœ : NÕu tf > (qf - q0) / v v” tf ≀ 2(qf - q0) / v th× : v > (qf - q0) / tf v” v ≀ 2(qf - q0) / tf. NghÜa l” tgΞ < v ≀ tg2Ξ. c/ Trong ÂźoÂčn 3 : (tf - tb) ≀ t ≀ tf quĂŒ ÂźÂčo Parabol cĂŁ dÂčng :

2f

2f

fi t2atat

2atq(t)q −+−= (8.11)

TĂ” cžc ph−¬ng tr×nh (8.9)...(8.11) ta x©y dĂčng ¼Æc tÝnh chuyÓn Ÿéng theo quĂŒ ÂźÂčo LSPB cña khĂ­p qi nh− sau : (t) iq&

( iq&& t)

Ξ

tft0q0

qf

t

t

t

qi(t); q

v = const

tf

tf

tf

tf-tb

tf-tb

tf-tb

tb

tb

tb

t0

t0

t0 q0

(t)iq(t);i &&&

qf

H×nh 8.4 : §Æc tÝnh quĂŒ ÂźÂčo LSPB

TS. PhÂčm §šng Ph−íc

Page 110: Robot cong nghiep

Robot c«ng nghiÖp

98

8.4. QuĂŒ ÂźÂčo Bang Bang Parabolic blend (BBPB) : Nh− Ÿ· tr×nh b”y Ă« trÂȘn, Ÿ©y l” tr−ĂȘng hĂźp ¼Æc biÖt cña quĂŒ ÂźÂčo LSPB khi ÂźoÂčn tuyÕn tÝnh thu vÒ 0.

VĂ­i : 0 ≀ t ≀ 2tf qi(t) = q0 +

2at2

v” vĂ­i 2tf ≀ t ≀ tf qi(t) = 2q0 - qf +2a

2at-t

aqq 2

0f −

§Ä thÞ ¼Æc tÝnh cña quĂŒ ÂźÂčo nÇy nh− sau :

(t)iq&&

(t)iq&Vmax

t

t

t

tf

tf

tf

tf/2

tf/2

tf/2

t0

t0

t0

qf

q0

qi(t)

H×nh 8.5. §Æc tÝnh quĂŒ ÂźÂčo BBPB

=======================

TS. PhÂčm §šng Ph−íc


Proyecto robot

Proyecto robot

Robot-Aided Neurorehabilitation: A Novel Robot for Ankle Rehabilitation

Robot-Aided Neurorehabilitation: A Novel Robot for Ankle Rehabilitation

BỘ CONG THÆŻÆ NG

BỘ CONG THÆŻÆ NG

CONG TY BAU GIA HqP DANH HOANG GIA CONG HOA xA ...

CONG TY BAU GIA HqP DANH HOANG GIA CONG HOA xA ...

Cam nang doanh nghiep EVFTA va nganh rau qua - WTO

Cam nang doanh nghiep EVFTA va nganh rau qua - WTO

Cong Nghe Khai Thac

Cong Nghe Khai Thac

KHAI QUAT VE NEN CONG NGHIEP MANGA CUA NHAT OAN*

KHAI QUAT VE NEN CONG NGHIEP MANGA CUA NHAT OAN*

Robot programming

Robot programming

Trier] khai thyc hien ha try va ph& trien Doanh nghiep

Trier] khai thyc hien ha try va ph& trien Doanh nghiep

cong van 1458.pdf

cong van 1458.pdf

Nghiep Vu Che Bien Mon An

Nghiep Vu Che Bien Mon An

Tong dai hanh chinh cong Da Nang

Tong dai hanh chinh cong Da Nang

The robot musician ‘wabot-2’ (waseda robot-2)

The robot musician ‘wabot-2’ (waseda robot-2)

tai chinh doanh nghiep

tai chinh doanh nghiep

Robot - Robo Challenge

Robot - Robo Challenge

Self-balancing robot

Self-balancing robot

CONG BO - Sở xĂąy dá»±ng phĂș thọ

CONG BO - Sở xĂąy dá»±ng phĂș thọ

Bai tap nhom KT206 tai chinh doanh nghiep (1)

Bai tap nhom KT206 tai chinh doanh nghiep (1)

Robot Actuators

Robot Actuators

Cong Nghe 7 chuan

Cong Nghe 7 chuan