Ch ng III. ph ng tr nh éng häc cña robot (Kinematic Equations)

Transcription

1 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. A 1 m«t vþ trý vµ h íng cña kh u Çu tiªn; A 2 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 : T 2 = A 1.A 2 Còng nh vëy, A 3 m«t kh u thø ba so víi kh u thø hai vµ : T 3 = A 1.A 2.A 3 ; 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ã : T 6 = A 1.A 2.A 3.A 4.A 5.A 6 (3.1) T 6 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. T 6 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 : p a o n H nh 3.1 : C c vect Þnh vþ trý vµ Þnh h íng cña bµn tay m y

2 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ã : r r r n = o x a ChuyÓn vþ T 6 nh vëy sï bao gåm c c phçn tö : n x O x a x p x T 6 = n y O y a y p y (3.2) n z O z a z p z Tæng qu t, ma trën T 6 cã thó bióu diôn gän h n nh sau : T 6 = Ma trën Þnh h íng R Vect vþ trý p (3.3) 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 p r 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 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 : a n. Gãc gi a c c trôc trong mæt ph¼ng vu«ng gãc víi a n : α n. Khíp n Khíp n+1 Kh u n a α 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 a n 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).

3 Robot c«ng nghiöp 29 Khíp n-1 Khíp n Khíp n+1 θ n-1 Kh u n-1 θ n Kh u n θ n+1 Kh u n+1 Kh u n-2 a n α n z n d n z n-1 xn-1 O n x n θ n 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 d n 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. d n 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 a n 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. r r Trong tr êng hîp c c trôc khíp c¾t nhau th trôc x chän theo tých vect. z x n z n-1 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 d n lµ biõn khíp vµ a n b»ng 0. C c th«ng sè a n, α n, d n 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 : y 2 O 2 x 2 y 1 θ 2 z 2 y 0 x 1 a 1 a 2 O 0 z 1 x 0 z 0 H nh 3.7 : Tay m y cã hai kh u ph¼ng (vþ trý bêt kú). O 1

4 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 z 0, z 1 vµ z 2 vu«ng gãc víi tê giêy. HÖ to¹ é c së lµ O 0 x 0 y 0 z 0, chiòu cña x 0 h íng tõ O 0 Õn O 1. Sau khi thiõt lëp hö to¹ é c së, HÖ to¹ é o 1 x 1 y 1 z 1 cã h íng nh h nh vï, O 1 Æt t¹i t m trôc khíp 2. HÖ to¹ é O 2 x 2 y 2 x 2 cã gèc O 2 Æ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 α i a i d i 1 0 a θ 2 0 a 2 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ï. O 0 x 0 z 0 z 1 a 1 θ 2 x 2 O 1 x 1 O 2 d 3 z 2 x 3 z 3, z 4 H nh 3.8 : Robot SCARA vµ c c hö to¹ é (vþ trý ban Çu). è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 x 0 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 : a 2 O 3 O 4 θ 4 d 4 x Kh u θ i α i a i d i 1 0 a θ a d 3 4 θ d 4 : C c biõn khíp Æ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 z n-1 mét gãc θ n TÞnh tiõn däc theo z n-1 mét kho ng d n TÞnh tiõn däc theo x n-1 = x n mét o¹n a n Quay quanh x n mét gãc xo¾n α n

5 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 : A n = Rot(z,θ) Trans(0,0,d) Trans(a,0,0) Rot(x,α) (3.4) cosθ -sinθ a A n = sinθ cosθ cosα -sinα d 0 sinα cosα 0 cosθ -sinθ cosα sinθ sinα a cosθ A n = sinθ cosθ cosα -cosθ sinα a sinθ (3.5) 0 sinα cosα d èi víi khíp tþnh tiõn (a = 0 vµ θ i = 0) th ma trën A cã d¹ng : A n = 0 cosα - sinα 0 (3.6) 0 sinα cosα d è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 X c Þnh T 6 theo c c ma trën A n : Ta biõt : T 6 = A 1 A 2 A 3 A 4 A 5 A 6 Trong ã T 6 î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 T 6 theo c c hö to¹ é trung gian thø n-1 th : n 1 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 : X= Z T 6 E T 6 = 6 i= n A i Z T 6 O R X E A H nh 3.9 : VËt thó vµ Robot

6 Robot c«ng nghiöp 32 Quan hö nçy îc thó hiön trªn to n å sau : O R O 0 Z A 1 A 2 A 3 A 4 A 5 E A X 1 T 6 2 T 6 3 T 6 4 T 6 5 T 6 O R T 6 H nh 3.10 : To n å chuyón vþ cña robot. Tõ to n å nçy ta cã thó rót ra : T 6 = Z -1 X E -1 (Z -1 vµ E -1 lµ c c ma trën nghþch o) 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¹ é O 0, O 1,... + C c trôc z n ph i chän cïng ph ng víi trôc khíp thø n+1. + Chän trôc x n lµ trôc quay cña z n thµnh z n+1 vµ gãc cña z n víi z n+1 chýnh lµ α n+1. NÕu z n vµ z n+1 song song hoæc trïng nhau th ta cã thó c n cø nguyªn t¾c chung hay chän x n theo x n+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 A n. ã lµ bèn phðp biõn æi : A n = 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 A n, 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 z n èi víi z n-1 quanh trôc y n-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 A n. 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.

7 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 H y thiõt lëp hö ph ng tr nh éng häc cña robot. θ 2 d 3 H nh 3.11 : Robot RRT 1. G¾n hö to¹ é lªn c c kh u : Ta gi Þnh vþ trý ban Çu vµ chän gèc to¹ é O 0 cña robot nh h nh C c trôc z Æt cïng ph ng víi c c trôc khíp. Ta thêy trôc z 1 quay t ng èi mét gãc 90 0 so víi trôc z 0, y chýnh lµ phðp quay quanh trôc x 0 mét gãc α 1 (phðp biõn æi Rot(x 0,α 1 ) trong bióu thøc týnh A n ). NghÜa lµ trôc x 0 vu«ng gãc víi z 0 vµ z 1. Ta chän chiòu cña x 0 tõ tr i sang ph i th gãc quay α 1 =90 0 (chiòu d ng ng îc chiòu kim ång hå). ång thêi ta còng thêy gèc O 1 tþnh tiõn mét o¹n däc theo z 0, so víi O 0, ã chýnh lµ phðp biõn æi Trans(0,0,d 1 ) (tþnh tiõn däc theo z 0 mét o¹n d 1 ) ; c c trôc y 0,vµ y 1 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 é O 2 Æt trïng víi O 1 v trôc khíp thø ba vµ trôc khíp thø hai c¾t nhau t¹i O 1 (nh h nh 3.12). Trôc z 2 cïng ph ng víi trôc khíp thø ba, tøc lµ quay i mét gãc 90 0 so víi z 1 quanh trôc y 1 ; phðp biõn æi nçy kh«ng cã trong bióu thøc týnh A n 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 Theo h nh 3.13, O 2 vén îc Æt trïng víi O 1, trôc z 2 cã ph ng th¼ng øng, nghüa lµ ta quay trôc z 1 thµnh z 2 quanh trôc x 1 mét gãc (tøc α 2 = ). Çu cuèi cña kh u thø 3 kh«ng cã khíp, ta Æt O 3 t¹i ióm gi a cña c c ngãn tay, vµ trôc z 3, x 3 chän nh h nh vï, nh vëy ta tþnh tiõn gèc to¹ é däc theo z 2 mét o¹n d 3 (PhÐp biõn æi Trans(0,0,d 3 )), v y lµ kh u tþnh tiõn nªn d 3 lµ biõn. d 1 y 1 θ 2 d 3 O 1, O 2 H nh 3.12 : G¾n c c hö to¹ é O 0 vµ O 1 x 1 z 1 z 2 d 3 d 1 d 3 O 0 θ 2 O 3 z 0 z 0 z 3 y 1 O 1 y 0 x 3 x 0 z 2 O 2 x 1 z 1 x 2 y 0 O 0 x 0 H nh 3.13 : HÖ to¹ é g¾n lªn c c kh u

8 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 α i a i d i d 1 2 θ i d 3 3. X c Þnh c c ma trën A : Ma trën A n cã d¹ng : cosθ -sinθ cosα sinθ sinα 0 A n = sinθ cosθ cosα -cosθ sinα 0 0 sinα cosα d Víi qui íc viõt t¾t : C 1 = cos ; S 1 = sin ; C 2 = cosθ 2... C 1 0 S 1 0 A 1 = S 1 0 -C d 1 C 2 0 -S 2 0 A 2 = S 2 0 C A 3 = d 3 4. TÝnh c c ma trën biõn æi thuçn nhêt T : + Ma trën 2 T 3 = A 3 + Ma trën 1 T 3 = A 2. 2 T 3 C 2 0 -S C 2 0 -S 2 -S 2 d 3 1 T 3 = S 2 0 C = S 2 0 C 2 C 2 d d d Ma trën T 3 = A 1. 1 T 3 C 1 0 S 1 0 C 2 0 -S 2 -S 2 d 3 T 3 = S 1 0 -C 1 0 S 2 0 C 2 C 2 d d

9 Robot c«ng nghiöp 35 C 1 C 2 -S 1 -C 1 S 2 -C 1 S 2 d 3 = S 1 d 2 C 1 -S 1 S 2 -S 1 S 2 d 3 S 2 0 C 2 C 2 d 3 + d 1 Ta cã hö ph ng tr nh éng häc cña robot nh sau : n x = C 1 C 2 ; O x = -S 1 ; a x = -C 1 S 2 ; p x = -C 1 S 2 d 3 n y = S 1 C 2 ; O y = C 1 ; a y = -S 1 S 2 ; p y = -S 1 S 2 d 3 n z = S 2 O z = 0; a z = C 2 ; p z = C 2 d 3 + d 1 ; (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ý p x,p y, p z týnh so víi c ch týnh h nh häc trªn h nh vï) 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

10 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 : z 3,z 5,z 6 x i O 3,O 4, O 5,O 6 z 4 C 1 = cos ; S 1 = sin ; C 12 = cos( +θ 2 ); S 12 = sin( +θ 2 ) S 234 = sin (θ 2 +θ 3 +θ 4 ).... HÖ to¹ é g¾n lªn c c kh u cña robot nh h nh (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 O 6 O 5 ). x 0 z 0 O 0,O 1 z 1 d 2 d 3 z 2 O 2 x 1 H nh 3.15 : HÖ to¹ é cña Robot Stanford B ng th«ng sè DH (Denavit-Hartenberg) cña robot Stanford nh sau : Kh u θ i α i a i d i θ d d 3 4 θ θ θ ( : C c biõn khíp). C c ma trëm A cña robot Stanford îc x c Þnh nh sau : C 1 0 -S 1 0 C 2 0 S 2 0 A 1 = S 1 0 C 1 0 A 2 = S 2 0 -C d C 4 0 -S 4 0 A 3 = A 4 = S 4 0 C d C 5 0 S 5 0 C 6 -S A 5 = S 5 0 -C 5 0 A 6 = S 6 C 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ã :

11 Robot c«ng nghiöp 37 C 6 -S T 5 6 = S 6 C C 5 C 6 -C 5 S 6 S 5 0 T 4 6 = A 5 A 6 = S 5 C 6 -S 5 S 6 -C 5 0 S 6 C C 4 C 5 C 6 - S 4 S 6 -C 4 C 5 S 6 -S 4 C 6 C 4 S 5 0 T 3 6 = A 4 A 5 A 6 = S 4 C 5 C 6 + C 4 S 6 -S 4 C 5 S 6 + C 4 C 6 S 4 S 5 0 -S 5 C 6 S 5 S 6 C 5 0 C 4 C 5 C 6 -S 4 S 6 -C 4 C 5 S 6 - S 4 C 6 C 4 S 5 0 T 2 6 = A 3 A 4 A 5 A 6 = S 4 C 5 C + C 4 S 6 -S 4 C 5 S 6 + C 4 C 6 S 4 S 5 0 -S 5 C 6 S 5 S 6 C 5 d 3 C 2 (C 4 C 5 C 6 - S 4 S 6 ) - S 2 S 5 C 6 -C 2 (C 4 C 5 S 6 -S 4 C 6 )+S 2 S 5 S 6 T 1 6 =A 2 A 3 A 4 A 5 A 6 = S 2 (C 4 C 5 C 6 - S 4 S 6 ) + C 2 S 5 C 6 -S 2 (C 4 C 5 S 6 +S 4 C 6 )-C 2 S 5 S 6 S 4 C 5 C 6 + C 4 S 6 -S 4 C 5 S 6 +C 4 C Cuèi cïng : n x O x a x p x T 6 = n y O y a y p y = 1 A 1 T 6 n z O z a z p z C 2 C 4 S 5 + S 2 C 5 S 2 d 3 S 2 C 4 S 5 - C 2 C 5 -C 2 d 3 S 4 S 5 d Ó týnh T 6, ta ph i nh n A 1 víi T 6 1 sau ã c n b»ng c c phçn tö cña ma trën T 6 ë hai võ ta îc mét hö thèng c c ph ng tr nh sau : n x = C 1 [C 2 (C 4 C 5 C 6 - S 4 S 6 ) - S 2 S 5 C 6 ] - S 1 (S 4 C 5 C 6 + C 4 S 6 ) n y = S 1 [C 2 (C 4 C 5 C 6 - S 4 S 6 ) - S 2 S 5 C 6 ] + C 1 (S 4 C 5 C 6 + C 4 S 6 ) n z = -S 2 (C 4 C 5 C 6 - S 4 S 6 ) + C 2 S 5 C 6 O x = C 1 [-C 2 (C 4 C 5 S 6 + S 4 C 6 ) + S 2 S 5 S 6 ] - S 1 (-S 4 C 5 S 6 + C 4 C 6 ) O y = S 1 [-C 2 (C 4 C 5 S 6 + S 4 C 6 ) + S 2 S 5 S 6 ] + C 1 (-S 4 C 5 C 6 + C 4 C 6 ) O z = S 2 (C 4 C 5 S 6 + S 4 C 6 ) + C 2 S 5 S 6 a X = C 1 (C 2 C 4 S 5 + S 2 C 5 ) - S 1 S 4 S 5 a y = S 1 (C 2 C 4 S 5 + S 2 C 5 ) + C 1 S 4 S 5 a z = -S 2 C 4 S 5 + C 2 C 5 p x = C 1 S 2 d 3 - S 1 d 2 p y = S 1 S 2 d 3 + C 1 d 2 p z = C 2 d 3

12 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 T 6 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 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 2 Kh u 3 Kh u 4 Kh u 1 Kh u 5 Kh u 6 z 0 H nh 1.16 : Robot Elbow z 1 O 0,O 1 a 2 z 4 θ 2 a 3 θ a 5 O 2 4 O 3 O 2,O 5,O 6 z 2 θ 3 z 3 θ 4 x i z 5,z 6 θ 6 a 5 = a 6 = 0 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 a i d i θ 2 0 a θ 3 0 a θ a θ θ ( : c c biõn khíp ) C c ma trën A cña robot Elbow îc x c Þnh nh sau : C 1 0 S 1 0 C 2 -S 2 0 C 2 a 2 A 1 = S 1 0 -C 1 0 A 2 = S 2 C 2 0 S 2 a

13 Robot c«ng nghiöp 39 C 3 -S 3 0 C 3 a 3 C 4 0 -S 4 C 4 a 4 A 3 = S 3 C 3 0 S 3 a 3 A 4 = S 4 0 C 4 S 4 a C 5 0 S 5 0 C 6 -S A 5 = S 5 0 -C 5 0 A 6 = S 6 C 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 : C 6 -S T 5 6 = S 6 C C 5 C 6 -C 5 S 6 S 5 0 T 4 6 = A 5 A 6 = S 5 C 6 -S 5 S 6 -C 5 0 S 6 C6 0 0 C 4 C 5 C 6 - S 4 S 6 -C 4 C 5 S 6 -S 4 C 6 C 4 S 5 C 4 a 4 T 3 6 = A 4 A 5 A 6 = S 4 C 5 C 6 +C 4 S 6 -S 4 C 5 S 6 +C 4 C 6 S 4 S 5 S 4 a 4 -S 5 C 6 S 5 S 6 C 5 0 C 34 C 5 C 6 - S 34 S 6 -C 34 C 5 C 6 - S 34 C 6 C 34 S 5 C 34 a 4 +C 3 a 3 T 2 6 = A 3 A 4 A 5 A 6 = S 34 C 5 C 6 +C 34 S 6 -S 34 C 5 S 6 +C 34 C 6 S 34 S 5 S 34 a 4 +S 3 a 3 -S 5 C 6 S 5 S 6 C 5 0 T 6 1 =A 2 A 3 A 4 A 5 A 6 = C 234 C 5 C 6 - S 234 S 6 -C 234 C 5 S 6 - S 234 C 6 C 234 S 5 C 234 a 4 +C 23 a 3 +C 2 a 2 S 234 C 5 C 6 + C 234 S 6 -S 234 C 5 S 6 + C 234 C 6 S 234 S 5 S 234 a 4 +S 23 a 3 +S 2 a 2 -S 5 C 6 S 5 S 6 C 5 0 Cuèi cïng : n x O x a x p x T 6 = n y O y a y p y 1 = A 1 T 6 n z O z a z p z Ó týnh T 6, ta ph i nh n A 1 víi T 61 sau ã c n b»ng c c phçn tö cña ma trën T 6 ta îc mét hö thèng c c ph ng tr nh sau :

14 Robot c«ng nghiöp 40 n x = C 1 (C 234 C 5 C 6 - S 234 S 6 ) - S 1 S 5 C 6 n y = S 1 (C 234 C 5 C 6 - S 234 S 6 ) + C 1 S 5 C 6 n z = S 234 C 5 C 6 + C 234 S 6 O x = -C 1 (C 234 C 5 S 6 + S 234 C 6 ) + S 1 S 5 S 6 O y = -S 1 (C 234 C 5 S 6 + S 234 C 6 ) - C 1 S 5 S 6 O z = -S 234 C 5 S 6 + C 234 C 6 a X = C 1 C 234 S 5 + S 1 C 5 a y = S 1 C 234 S 5 - C 1 C 5 a z = S 234 S 5 p x = C 1 (C 234 a 4 + C 23 a 3 + C 2 a 2 ) p y = S 1 (C 234 a 4 + C 23 a 3 + C 2 a 2 ) p z = S 234 a 4 + S 23 a 3 + S 2 a 2 Cét Çu tiªn cña ma trën T 6 cã thó îc x c Þnh bëi tých vect : r r r n = Ox a 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 A n 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 :? T 6 =? 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.

15 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.22 : Robot cêu h nh RRR H nh 3.23 : Robot cêu h nh RRRRR 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.