Robot cng nghip

27

Chng III phng trnh ng hc ca robot (Kinematic Equations)

3.1. Dn nhp : Bt k mt robot no cng c th coi l mt tp hp cc khu (links) gn lin vi cc khp (joints). Ta hy t trn mi khu ca robot mt h to . S dng cc php bin i thun nht c th m t v tr tng i v hng gia cc h to ny. Denavit. J. gi bin i thun nht m t quan h gia mt khu v mt khu k tip l mt ma trn A. Ni n gin hn, mt ma trn A l mt m t bin i thun nht bi php quay v php tnh tin tng i gia h to ca hai khu lin nhau. A1 m t v tr v hng ca khu u tin; A2 m t v tr v hng ca khu th hai so vi khu th nht. Nh vy v tr v hng ca khu th hai so vi h to gc c biu din bi ma trn : T2 = A1.A2 Cng nh vy, A3 m t khu th ba so vi khu th hai v : T3 = A1.A2.A3 ; v.v... Cng theo Denavit, tch ca cc ma trn A c gi l ma trn T, thng c hai ch s: trn v di. Ch s trn ch h to tham chiu ti, b qua ch s trn nu ch s bng 0. Ch s di thng dng ch khu chp hnh cui. Nu mt robot c 6 khu ta c : T6 = A1.A2.A3.A4.A5.A6 (3.1)

T6 m t mi quan h v hng v v tr ca khu chp hnh cui i vi h to gc. Mt robot 6 khu c th c 6 bc t do v c th c nh v tr v nh hng trong trng vn ng ca n (range of motion). Ba bc t do xc nh v tr thun tu v ba bc t do khc xc nh hng mong mun. T6 s l ma trn trnh by c hng v v tr ca robot. Hnh 3.1 m t quan h vi bn tay my. Ta t gc to ca h m t ti im gia ca cc ngn tay. Gc to ny c m t bi vect p (xc nh v tr ca bn tay). Ba vect n v m t hng ca bn tay c xc nh nh sau :

n a o

Hnh 3.1 : Cc vect nh v tr v nh hng ca bn tay my

TS. Phm ng Phc

Robot cng nghip

28

Vect c hng m theo bn tay s tip cn n i tng, gi l vect a (approach). Vect c hng m theo cc ngn tay ca bn tay nm vo nhau khi cm nm i tng, gi l vect o (Occupation). Vect cui cng l vect php tuyn n (normal), do vy ta c :

r r r n=oxa
Chuyn v T6 nh vy s bao gm cc phn t : T6 = nx ny nz 0 Ox Oy Oz 0 ax ay az 0 px py pz 1 (3.2)

Tng qut, ma trn T6 c th biu din gn hn nh sau : T6 = Ma trn nh hng R 0 0 0 Vect v tr p 1 (3.3)

Ma trn R c kch thc 3x3, l ma trn trc giao biu din hng ca bn kp (khu chp hnh cui) i vi h to c bn. Vic xc nh hng ca khu chp hnh cui cn c th thc hin theo php quay Euler hay php quay Roll, Pitch, Yaw. Vect im p c kch thc 3x1, biu din mi quan h ta v tr ca ca gc h ta gn trn khu chp hnh cui i vi h to c bn. 3.2. B thng s Denavit-Hartenberg (DH) : Mt robot nhiu khu cu thnh t cc khu ni tip nhau thng qua cc khp ng. Gc chun (Base) ca mt robot l khu s 0 v khng tnh vo s cc khu. Khu 1 ni vi khu chun bi khp 1 v khng c khp u mt ca khu cui cng. Bt k khu no cng c c trng bi hai kch thc : di php tuyn chung : an . Gc gia cc trc trong mt phng vung gc vi an : n. Khp n Khu n n a Hnh 3.5 : Chiu di v gc xon ca 1 khu. Thng thng, ngi ta gi an l chiu di v n l gc xon ca khu (Hnh 3.5). Ph bin l hai khu lin kt vi nhau chnh trc ca khp (Hnh 3.6). Khp n+1

TS. Phm ng Phc

Robot cng nghip

29

Khp n-1

Khp n

Khp n+1

n-1
Khu n-1

n
Khu n

n+1
Khu n+1

Khu n-2

an dn n zn-1 xn-1

zn On xn

Hnh 3.6 : Cc thng s ca khu : , d, a v . Mi trc s c hai php tuyn vi n, mi php tuyn dng cho mi khu (trc v sau mt khp). V tr tng i ca hai khu lin kt nh th c xc nh bi dn l khong cch gia cc php tuyn o dc theo trc khp n v n l gc gia cc php tuyn o trong mt phng vung gc vi trc. dn v n thng c gi l khong cch v gc gia cc khu. m t mi quan h gia cc khu ta gn vo mi khu mt h to . Nguyn tc chung gn h ta ln cc khu nh sau : + Gc ca h to gn ln khu th n t ti giao im ca php tuyn an vi trc khp th n+1. Trng hp hai trc khp ct nhau, gc to s t ti chnh im ct . Nu cc trc khp song song vi nhau, gc to c chn trn trc khp ca khu k tip, ti im thch hp. + Trc z ca h to gn ln khu th n t dc theo trc khp th n+1. + Trc x thng c t dc theo php tuyn chung v hng t khp n n n+1. r r Trong trng hp cc trc khp ct nhau th trc x chn theo tch vect z n x z n-1 . Trng hp khp quay th n l cc bin khp, trong trng hp khp tnh tin th dn l bin khp v an bng 0. Cc thng s an, n, dn v n c gi l b thng s DH. V d 1 : Xt mt tay my c hai khu phng nh hnh 3.7 : y2 x2
O2

y1 y0
O0

z2 x1 a 2

a1 1 z1
O1

z0

x0

Hnh 3.7 : Tay my c hai khu phng (v tr bt k).

TS. Phm ng Phc

Robot cng nghip

30

Ta gn cc h to ln cc khu nh hnh v : trc z0, z1 v z2 vung gc vi t giy. H to c s l O0x0y0z0, chiu ca x0 hng t O0 n O1. Sau khi thit lp h to c s, H to o1x1y1z1 c hng nh hnh v, O1 t ti tm trc khp 2. H to O2x2y2x2 c gc O2 t im cui ca khu 2. Bng thng s Denavit-Hartenbert ca tay my ny nh sau : Khu 1 2 i 1 * 2 * i 0 0 ai a1 a2 di 0 0

Trong i l cc bin khp (dng du * k hiu cc bin khp). V d 2 : Xem s robot SCARA c 4 khu nh hnh 3.8 : y l robot c cu hnh kiu RRTR, bn tay c chuyn ng xoay xung quanh trc ng. H to gn ln cc khu nh hnh v. a2 z0 z1 1 2 x2 x0 O
0

O1

x1 a1 d3

O2

z2 x 3
O3 O4 4

d4 x

z3 , z4 Hnh 3.8 : Robot SCARA v cc h to (v tr ban u). i vi tay my ny cc trc khp u song song nhau, tin li tt c cc gc to t ti tm cc trc khp. Trc x0 nm trong mt phng t giy. Cc h to khc nh hnh v. Bng thng s DH ca robot SCARA nh sau : Khu 1 2 3 4 i 1 * 2 * 0 4 * i 0 1800 0 0 ai a1 a2 0 0 di 0 0 d3* d4

* : Cc bin khp. 3.3. c trng ca cc ma trn A : Trn c s cc h to n nh cho tt c cc khu lin kt ca robot, ta c th thit lp mi quan h gia cc h to ni tip nhau (n-1), (n) bi cc php quay v tnh tin sau y : Quay quanh zn-1 mt gc n Tnh tin dc theo zn-1 mt khong dn Tnh tin dc theo xn-1 = xn mt on an Quay quanh xn mt gc xon n

TS. Phm ng Phc

Robot cng nghip

31

Bn php bin i thun nht ny th hin quan h ca h to thuc khu th n so vi h to thuc khu th n-1 v tch ca chng c gi l ma trn A : An = Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,) An = cos sin 0 0 cos sin 0 0 -sin cos 0 0 0 0 1 0 0 0 0 1 1 0 0 0 sin sin -cos sin cos 0 0 1 0 0 0 0 1 0 a 0 d 1 1 0 0 0 0 cos sin 0 0 -sin cos 0 (3.4) 0 0 0 1

An =

-sin cos cos cos sin 0

a cos a sin d 1

(3.5)

i vi khp tnh tin (a = 0 v i = 0) th ma trn A c dng : An = 1 0 0 0 0 cos sin 0 0 - sin cos 0 0 0 d 1 (3.6)

i vi mt khu i theo mt khp quay th d, a v l hng s. Nh vy ma trn A ca khp quay l mt hm s ca bin khp . i vi mt khu i theo mt khp tnh tin th , l hng s. Ma trn A ca khp tnh tin l mt hm s ca bin s d. Nu cc bin s c xc nh th gi tr ca cc ma trn A theo cng c xc nh. 3.4. Xc nh T6 theo cc ma trn An : Ta bit : T6 = A1A2A3A4A5A6

Trong T6 c miu t trong h to gc (h to gn vi khu c bn c nh ca robot). Nu m t T6 theo cc h to trung gian th n-1 th :

n 1

i=n

Ai

Trong trng hp tng qut, khi xt quan h ca robot vi cc thit b khc, nu h to c bn ca robot c lin h vi mt h to no bi php bin i Z, Khu chp hnh cui li c gn mt cng c, c quan h vi vt th bi php bin i E (hnh 3.9) th v tr v hng ca im cui ca cng c, kho st h to tham chiu m t bi X s c xc nh bi : X= Z T6E

OR Z T6 X E A

Hnh 3.9 : Vt th v Robot

TS. Phm ng Phc

Robot cng nghip

32

Quan h ny c th hin trn ton sau :

Z O0 A1 OR

A2

A3

A4
4 3

A5
5

E A X OR

T6

T6

2 1

T6

T6

T6

T6
Hnh 3.10 : Ton chuyn v ca robot. T ton ny ta c th rt ra : T6 = Z-1 X E-1 -1 -1 (Z v E l cc ma trn nghch o). 3.5. Trnh t thit lp h phng trnh ng hc ca robot : thit lp h phng trnh ng hc ca robot, ta tin hnh theo cc bc sau : 1. Chn h to c s, gn cc h to m rng ln cc khu. Vic gn h to ln cc khu ng vai tr rt quan trng khi xc lp h phng trnh ng hc ca robot, thng thng y cng l bc kh nht. Nguyn tc gn h to ln cc khu c trnh by mt cch tng qut trong phn 3.5. Trong thc t, cc trc khp ca robot thng song song hoc vung gc vi nhau, ng thi thng qua cc php bin i ca ma trn A ta c th xc nh cc h to gn trn cc khu ca robot theo trnh t sau : + Gi nh mt v tr ban u() (Home Position) ca robot. + Chn gc to O0, O1, ... + Cc trc zn phi chn cng phng vi trc khp th n+1. + Chn trc xn l trc quay ca zn thnh zn+1 v gc ca zn vi zn+1 chnh l n+1. Nu zn v zn+1 song song hoc trng nhau th ta c th cn c nguyn tc chung hay chn xn theo xn+1. + Cc h to Oxyz phi tun theo qui tc bn tay phi. + Khi gn h to ln cc khu, phi tun theo cc php bin i ca ma trn An. l bn php bin i : An = Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,). Ngha l ta coi h to th n+1 l bin i ca h to th n; cc php quay v tnh tin ca bin i ny phi l mt trong cc php bin i ca An, cc thng s DH cng c xc nh da vo cc php bin i ny. Trong qu trnh gn h ta ln cc khu, nu xut hin php quay ca trc zn i vi zn-1 quanh trc yn-1 th v tr ban u ca robot gi nh l khng ng, ta cn chn li v tr ban u khc cho robot. 2. Lp bng thng s DH (Denavit Hartenberg). 3. Da vo cc thng s DH xc nh cc ma trn An. 4. Tnh cc ma trn T v vit cc phng trnh ng hc ca robot.
()

V tr ban u l v tr m cc bin nhn gi tr ban u, thng bng 0.

TS. Phm ng Phc

Robot cng nghip

33

V d sau y trnh by chi tit ca cc bc khi thit lp h phng trnh ng hc ca robot : Cho mt robot c ba khu, cu hnh RRT nh hnh 3.11. Hy thit lp h phng trnh ng hc ca robot. 2 d3

Hnh 3.11 : Robot RRT 1. Gn h to ln cc khu : Ta gi nh v tr ban u v chn gc to O0 ca robot nh hnh 3.12. Cc trc z t cng phng vi cc trc khp. Ta thy trc z1 quay tng i mt gc 90 so vi trc z0, y chnh l php quay quanh trc x0 mt gc 1 (php bin i Rot(x0,1) trong biu thc tnh An). Ngha l trc x0 vung gc vi z0 v z1. Ta chn chiu ca x0 t tri sang phi th gc quay 1=900 (chiu dng ngc chiu kim ng h). ng thi ta cng thy gc O1 tnh tin mt on dc theo z0 , so vi O0, chnh l php bin i Trans(0,0,d1) (tnh tin dc theo z0 mt on d1) ; cc trc y0,v y1 xc nh theo qui tc bn tay phi (Hnh 3.12 ) .
0

2 z1 d1
O0

y1 O1 , O2

d3

x1 z2 1
z0 y0

x0

Hnh 3.12 : Gn cc h to O0 v O1 z3
O3

Tip tc chn gc ta O2 t trng vi O1 v trc khp th ba v trc khp th hai ct nhau ti O1 (nh hnh 3.12). Trc z2 cng phng vi trc khp th ba, tc l quay i mt gc 900 so vi z1 quanh trc y1; php bin i ny khng c trong biu thc tnh An nn khng dng c, ta cn chn li v tr ban u ca robot (thay i v tr ca khu th 3) nh hnh 3.13. Theo hnh 3.13, O2 vn c t trng vi O1, trc z2 c phng thng ng, ngha l ta quay trc z1 thnh z2 quanh trc x1 mt gc -900 (tc 2= -900). u cui ca khu th 3 khng c khp, ta t O3 ti im gia ca cc ngn tay, v trc z3, x3 chn nh hnh v, nh vy ta tnh tin gc to dc theo z2 mt on d3 (Php bin i Trans(0,0,d3)), v y l khu tnh tin nn d3 l bin . TS. Phm ng Phc

x3

d3 d3 2 z1 d1
z0 O0 y1 z2 O1 O2

x1 x2 1
y0

x0

Hnh 3.13 : H to gn ln cc khu

Robot cng nghip

34

Nh vy vic gn cc h to ln cc khu ca robot hon thnh. Thng qua cc phn tch trn y, ta c th xc nh c cc thng s DH ca robot. 2. Lp bng thng s DH : Khu 1 2 3 i 1* i* 0 i 90 -90 0 ai 0 0 0 di d1 0 d3 *

3. Xc nh cc ma trn A : Ma trn An c dng : cos -sin cos sin sin An = sin cos cos -cos sin 0 sin cos 0 0 0 Vi qui c vit tt : C1 = cos1 ; S1 = sin1 ; C2 = cos2 . . . A1 = C1 S1 0 0 C2 S2 0 0 1 0 0 0 0 0 1 0 0 0 -1 0 0 1 0 0 S1 -C1 0 0 -S2 C2 0 0 0 0 1 0 0 0 d1 1 0 0 0 1 0 0 d3 1

0 0 d 1

A2 =

A3 =

4. Tnh cc ma trn bin i thun nht T : + Ma trn 2T3 = A3 + Ma trn 1T3 = A2. 2T3
1

T3 = +

C2 0 -S2 0 S2 0 C2 0 0 -1 0 d2 0 0 0 1 Ma trn T3 = A1 . 1T3 T3 = C1 S1 0 0 0 0 1 0

1 0 0 0 S1 -C1 0 0

0 1 0 0

0 0 1 0 0 0 d1 1

0 0 d3 1 C2 S2 0 0

C2 S2 0 0 0 0 -1 0 -S2 C2 0 0

0 0 -1 0

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

-S2*d3 C2*d3 0 1

TS. Phm ng Phc

Robot cng nghip

35

C1C2 S1d2 S2 0

-S1 C1 0 0

-C1S2 -S1S2 C2 0

-C1S2d3 -S1S2d3 C2d3 + d1 1

Ta c h phng trnh ng hc ca robot nh sau : nx = C1C2; Ox = -S1; ax = -C1S2; px = -C1S2d3 ny = S1C2; Oy = C1; ay = -S1S2; py = -S1S2d3 nz = S2 Oz = 0; az = C2; pz = C2d3 + d1;

(Ta c th s b kim tra kt qu tnh ton bng cch da vo to v tr px,py, pz tnh so vi cch tnh hnh hc trn hnh v). 3.9. H phng trnh ng hc ca robot STANFORD : Stanford l mt robot c 6 khu vi cu hnh RRT.RRR (Khu th 3 chuyn ng tnh tin, nm khu cn li chuyn ng quay). Kt cu ca robot Stanford nh hnh 3.14 :

Hnh 3.14 : Robot Stanford

TS. Phm ng Phc

Robot cng nghip

36

Trn hnh 3.15 trnh by m hnh ca robot Stanford vi vic gn cc h to ln tng khu. n gin trong khi vit cc phng trnh ng hc ca robot, ta qui c cch vit tt cc hm lng gic nh sau : C1 = cos1; S1 = sin1; C12 = cos(1+2); S12 = sin(1+2) S234 = sin (2+3+4) ... . H to gn ln cc khu ca robot nh hnh 3.15. (Khu cui c chiu di v khong cch bng khng, c th gn cc loi cng c khc nhau nn chn O6O5). z0

z3,z5,z6 xi d2

O3,O4,O5,O6

z4

d3 z2
O2

O0,O1 z1

x0

x1

Hnh 3.15 : H to ca Robot Stanford

Bng thng s DH (Denavit-Hartenberg) ca robot Stanford nh sau : Khu i i -900 1 1 * 900 2 2 * 3 0 0 -900 4 4 * 900 5 5 * 0 6 6 * (* : Cc bin khp). ai 0 0 0 0 0 0 di 0 d2 d3 * 0 0 0

Cc ma trm A ca robot Stanford c xc nh nh sau : C1 S1 0 0 1 0 0 0 C5 S5 0 0 0 0 -1 0 0 1 0 0 0 0 1 0 -S1 C1 0 0 0 0 1 0 S5 -C5 0 0 0 0 0 1 0 0 d3 1 0 0 0 1 C2 S2 0 0 C4 S4 0 0 C6 S6 0 0 0 0 1 0 S2 -C2 0 0 0 0 -1 0 -S6 C6 0 0 0 0 d2 1 -S4 C4 0 0 0 0 1 0 0 0 0 1 0 0 0 1

A1=

A2=

A3=

A4=

A5=

A6=

Tch ca cc ma trn chuyn v A i vi robot Stanford c bt u khu 6 v chuyn dn v gc; theo th t ny ta c :

TS. Phm ng Phc

Robot cng nghip

37

T6 =

C6 S6 0 0

-S6 C6 0 0 C5C6 S5C6 S6 0

0 0 1 0

0 0 0 1 -C5S6 -S5S6 C6 0 S5 -C5 0 0 0 0 0 1 C4S5 S4S5 C5 0 C4S5 S4S5 C5 0 0 0 0 1 0 0 d3 1

T6 = A5A6 =

T63 = A4A5A6 =

C4C5C6 - S4S6 S4C5C6 + C4S6 -S5C6 0 C4C5C6-S4S6 S4C5C + C4S6 -S5C6 0

-C4C5S6-S4C6 -S4C5S6 + C4C6 S5S6 0 -C4C5S6 - S4C6 -S4C5S6 + C4C6 S5S6 0

T6 = A3A4A5A6 =

T6 =A2 A3A4A5A6 =

C2(C4C5C6 - S4S6) - S2S5C6 S2(C4C5C6 - S4S6) + C2S5C6 S4C5C6 + C4S6 0

-C2(C4C5S6-S4C6)+S2S5S6 -S2(C4C5S6+S4C6)-C2S5S6 -S4C5S6+C4C6 0 C2C4S5 + S2C5 S2C4S5 - C2C5 S4S5 0 S2d3 -C2d3 d2 1

Cui cng : T6 = nx ny nz 0 Ox Oy Oz 0 ax ay az 0 px py pz 1 = A1T61

tnh T6, ta phi nhn A1 vi T61 sau cn bng cc phn t ca ma trn T6 hai v ta c mt h thng cc phng trnh sau : nx = C1[C2(C4C5C6 - S4S6) - S2S5C6] - S1(S4C5C6 + C4S6) ny = S1[C2(C4C5C6 - S4S6) - S2S5C6] + C1(S4C5C6 + C4S6) nz = -S2(C4C5C6 - S4S6) + C2S5C6 Ox = C1[-C2(C4C5S6 + S4C6) + S2S5S6] - S1(-S4C5S6 + C4C6) Oy = S1[-C2(C4C5S6 + S4C6) + S2S5S6] + C1(-S4C5C6 + C4C6) Oz = S2(C4C5S6 + S4C6) + C2S5S6 aX = C1(C2C4S5 + S2C5) - S1S4S5 ay = S1(C2C4S5 + S2C5) + C1S4S5 az = -S2C4S5 + C2C5 px = C1S2d3 - S1d2 py = S1S2d3 + C1d2 pz = C2d3

TS. Phm ng Phc

Robot cng nghip

38

Nu ta bit c cc gi tr ca bin khp, th v tr v hng ca bn tay robot s tm c bng cch xc nh cc gi tr cc phn t ca T6 theo cc phng trnh trn. Cc phng trnh trn gi l h phng trnh ng hc thun ca robot Stanford. 3.10. H phng trnh ng hc ca robot ELBOW : hiu r hn v cch thit lp h phng trnh ng hc ca robot, ta xt thm trng hp robot Elbow. Khu 2 Khu 3 Khu 4

Khu 1 Khu 6 Hnh 1.16 : Robot Elbow a2

O2 1

Khu 5

z0
2 O0,O1

z1

a3
O3

z4 a4

z2

O2,O5,O6

z3

4 6

xi z 5, z 6 a5 = a6 = 0

Hnh 1.17 : V tr ban u ca robot Elbow v cc h to B thng s DH ca robot Elbow Khu i * i 1 900 1 2 0 2 3 0 3 4 -900 4 5 900 5 6 0 6 (* : cc bin khp ) ai 0 a2 a3 a4 0 0 di 0 0 0 0 0 0

Cc ma trn A ca robot Elbow c xc nh nh sau : A1= C1 S1 0 0 0 0 1 0 S1 -C1 0 0 0 0 0 1 A2= C2 S2 0 0 -S2 C2 0 0 0 0 1 0 C 2 a2 S 2 a2 0 1

TS. Phm ng Phc

Robot cng nghip

39

A3=

C3 S3 0 0 C5 S5 0 0

-S3 C3 0 0 0 0 1 0

0 0 1 0 S5 -C5 0 0 0 0 0 1

C 3 a3 S 3 a3 0 1

A4=

C4 S4 0 0 C6 S6 0 0

0 0 -1 0 -S6 C6 0 0

-S4 C4 0 0 0 0 1 0

C 4 a4 S 4 a4 0 1 0 0 0 1

A5=

A6=

Ta xc nh cc ma trn T theo cc h to ln lt t khu cui tr v gc : T65 = C6 S6 0 0 -S6 C6 0 0 C5C6 S5C6 S6 0 0 0 1 0 0 0 0 1 -C5S6 -S5S6 C6 0 S5 -C5 0 0 0 0 0 1 -C4C5S6-S4C6 -S4C5S6+C4C6 S5S6 0 -C34C5C6 - S34C6 -S34C5S6+C34C6 S5S6 0 C4S5 S4S5 C5 0 C34S5 S34S5 C5 0 C4a4 S4a4 0 1 C34a4+C3a3 S34a4+S3a3 0 1

T6 = A5A6 =

T63 = A4A5A6 =

C4C5C6 - S4S6 S4C5C6+C4S6 -S5C6 0 C34C5C6 - S34S6 S34C5C6+C34S6 -S5C6 0

T6 = A3A4A5A6 =

T61 =A2 A3A4A5A6 = C234C5C6 - S234S6 S234C5C6 + C234S6 -S5C6 0 Cui cng : T6 = nx ny nz 0 Ox Oy Oz 0 ax ay az 0 px py pz 1 = A1T61 -C234C5S6 - S234C6 -S234C5S6 + C234C6 S5S6 0 C234S5 S234S5 C5 0 C234a4+C23a3+C2a2 S234a4+S23a3+S2a2 0 1

tnh T6, ta phi nhn A1 vi T61 sau cn bng cc phn t ca ma trn T6 ta c mt h thng cc phng trnh sau :

TS. Phm ng Phc

Robot cng nghip

40

nx = C1(C234C5C6- S234S6) - S1S5C6 ny = S1(C234C5C6- S234S6) + C1S5C6 nz = S234C5C6 + C234S6 Ox = -C1(C234C5S6 + S234C6) + S1S5S6 Oy = -S1(C234C5S6 + S234C6) - C1S5S6 Oz = -S234C5S6 + C234C6 aX = C1C234S5 + S1C5 ay = S1C234S5 - C1C5 az = S234S5 px = C1(C234a4 + C23a3 + C2a2) py = S1(C234a4 + C23a3 + C2a2) pz = S234a4 + S23a3 + S2a2 r r r Ct u tin ca ma trn T6 c th c xc nh bi tch vect : n = O x a. 3.11. Kt lun : Trong chng ny chng ta nghin cu vic dng cc php bin i thun nht m t v tr v hng ca khu chp hnh cui ca robot thng qua vic xc lp cc h to gn ln cc khu v cc thng s DH. Phng php ny c th dng cho bt c robot no vi s khu (khp) tu . Trong qu trnh xc lp cc h to m rng ta cng xc nh c v tr dng ca mi robot. Tu thuc kt cu ca robot cng nh cng c gn ln khu chp hnh cui m ta c th a cc thng s ca khu chp hnh cui vo phng trnh ng hc hay khng. Vic tnh ton cc ma trn T thit lp h phng trnh ng hc ca robot thng tn nhiu thi gian v d nhm ln, ta c th lp trnh trn my tnh tnh ton ( dng k hiu) nhm nhanh chng xc nh cc ma trn An v thit lp h phng trnh ng hc ca robot . Thit lp h phng trnh ng hc ca robot l bc rt quan trng c th da vo lp trnh iu khin robot. Bi ton ny thng c gi l bi ton ng hc thun robot. Vic gii h phng trnh ng hc ca robot c gi l bi ton ng hc ngc, nhm xc nh gi tr ca cc bin khp theo cc thng s bit ca khu chp hnh cui; vn ny ta s nghin cu trong chng tip theo.

Bi tp chng III :
Bi 1 : Cho ma trn : T6 = ? ? ? ? 0 -1 0 0 -1 0 0 0 0 1 2 1

l ma trn biu din hng v v tr ca khu chp hnh cui. Tm cc phn t c nh du ? Bi 2 : Cho mt robot c 3 khu phng nh hnh 3.18, cu hnh RRR. Thit lp h phng trnh ng hc ca robot.

TS. Phm ng Phc

Robot cng nghip

41

Bi 3 : Cho mt robot c 2 khu tnh tin nh hnh 3.19, cu hnh TT. Thit lp h phng trnh ng hc ca robot.

Hnh 3.18 : Robot cu hnh RRR

Hnh 3.19 : Robot cu hnh TT

Bi 4 : Cho mt robot c 2 khu phng nh hnh 3.20, cu hnh RT. Thit lp h phng trnh ng hc ca robot. Bi 5 : Cho mt robot c 3 khu nh hnh 3.21, cu hnh RTR. Thit lp h phng trnh ng hc ca robot.

Hnh 3.20 : Robot cu hnh RT

Hnh 3.21 : Robot cu hnh RTR

Bi 6 : Cho mt robot c 3 khu nh hnh 3.22, cu hnh RRR. Thit lp h phng trnh ng hc ca robot.

Hnh 3.22 : Robot cu hnh RRR

Hnh 3.23 : Robot cu hnh RRRRR

Bi 7 : Cho mt robot c 5 khu nh hnh 3.23, cu hnh RRRRR. Thit lp h phng trnh ng hc ca robot.

TS. Phm ng Phc