De Cuong Thi Robot Cong Nghiep

7/31/2019 De Cuong Thi Robot Cong Nghiep

1/114

BI TP LN: RBT CNG NGHIP

Trang:1LP : CTK8LC1

GVHD: Trn Xun Tin

Cu hi n tp ch-ng i

Cu 1: nh ngha robot cng nghip? Nu cc ng dng carobot cng nghip?

Hin nay c nhiu nh ngha v Robot, c th im qua mt

s nh ngha nh sau :nh ngha theo tiu chun AFNOR (Php) :

Robot cng nghip l mt c cu chuyn ng t ng c th

lp trnh, lp li cc chng trnh, tng hp cc chng

trnh t ra trn cc trc to ; c kh nng nh v,

nh hng, di chuyn cc i tng vt cht : chi tit,

dao c, g lp . . . theo nhng hnh trnh thay i

chng trnh ho nhm thc hin cc nhim v cng ngh

khc nhau.

nh ngha theo RIA (Robot institute of America) :

Robot l mt tay my vn nng c th lp li cc chng

trnh c thit k di chuyn vt liu, chi tit, dng

c hoc cc thit b chuyn dng thng qua cc chng

trnh chuyn ng c th thay i hon thnh cc nhim

v khc nhau.

nh ngha theo OCT 25686-85 (Nga) :Robot cng nghip l mt my t ng, c t c nh

hoc di ng c, lin kt gia mt tay my v mt hthng iu khin theo chng trnh, c th lp trnh li

hon thnh cc chc nng vn ng v iu khin trong

qu trnh sn xut.

C th ni Robot cng nghip l mt my t ng linh hot

thay th tng phn hoc ton b cc hot ng c bp v

hot ng tr tu ca con ngi trong nhiu kh nng thch

nghi khc nhau.

Robot cng nghip c kh nng chng trnh ho linh hottrn nhiu trc chuyn ng, biu th cho s bc t do ca

chng. Robot cng nghip c trang b nhng bn tay my

hoc cc c cu chp hnh, gii quyt nhng nhim v xc

nh trong cc qu trnh cng ngh : hoc trc tip tham

gia thc hin cc nguyn cng (sn, hn, phun ph, rt kim


2/114


Trang:2LP : CTK8LC1

GVHD: Trn Xun Tin

loi vo khun c, lp rp my . . .) hoc phc v cc

qu trnh cng ngh (tho lp chi tit gia cng, dao c,

g . . .) vi nhng thao tc cm nm, vn chuyn v

trao i cc i tng vi cc trm cng ngh, trong mt

h thng my t ng linh hot, -c gi l H thng tng linh hot robot ho cho php thch ng nhanh v thao

tc n gin khi nhim v sn xut thay i.

Cc ng dng ca robot cng nghip:

T khi mi ra i robot cng nghip c p dng trong

nhiu lnh vc di gc thay th sc ngi. Nh vy cc

dy chuyn sn xut c t chc li, nng sut v hiu

qu sn xut tng ln r rt.

Mc tiu ng dng robot cng nghip nhm gp phn nng

cao nng sut dy chuyn cng ngh, gim gi thnh, nng

cao cht lng v kh nng cnh tranh ca sn phm ng

thi ci thin iu kin lao ng. t c cc mc tiu

trn l nh vo nhng kh nng to ln ca robot nh : lm

vic khng bit mt mi, rt d dng chuyn ngh mt cch

thnh tho, chu c phng x v cc mi trng lm vic

c hi, nhit cao, cm thy -c c t tr-ng v

nghe -c c siu m ... Robot -c dng thay th con

ngi trong cc trng hp trn hoc thc hin cc cngvic tuy khng nng nhc nhng n iu, d gy mt mi,

nhm ln.

Trong ngnh c kh, robot c s dng nhiu trong cng

ngh c, cng ngh hn, ct kim loi, sn, phun ph kim

loi, tho lp vn chuyn phi, lp rp sn phm . . .

Ngy nay xut hin nhiu dy chuyn sn xut t

ng gm cc my CNC vi Robot cng nghip, cc dy chuyn

t mc t ng ho cao, mc linh hot cao . . . y cc my v robot c iu khin bng cng mt h

thng chng trnh.

Ngoi cc phn xng, nh my, k thut robot cng c s

dng trong vic khai thc thm lc a v i dng, trong

y hc, s dng trong quc phng, trong chinh phc v tr,


3/114


Trang:3LP : CTK8LC1

GVHD: Trn Xun Tin

trong cng nghip nguyn t, trong cc lnh vc x hi . .

.

R rng l kh nng lm vic ca robot trong mt s iu

kin vt hn kh nng ca con ngi; do n l phng

tin hu hiu t ng ho, nng cao nng sut lao ng,gim nh cho con ngi nhng cng vic nng nhc v c

hi. Nhc im ln nht ca robot l cha linh hot nh

con ngi, trong dy chuyn t ng, nu c mt robot b

hng c th lm ngng hot ng ca c dy chuyn, cho nn

robot vn lun hot ng di s gim st ca con ngi.

Cc ng dng c th ca robot:

ng dng robot trong cng nghip: Robot gp vt liu

hn im, phun sn, phc v my cng c lm khun trong

cng nghip nha.

ng dng robot trong phng th nghim: dng thc

hin cc cng vic th cng, thc hin cc cng vic lp

i lp li.

ng dng robot trong thao tc cn khuch i lc: Dng

trong nhng khu vc nguy him(phng x ) bc d hng ha,

vt liu, phi c trng lng ln cng knh trong cc

nghnh cng nghip nng

ng dng robot trong nng nghip: Robot ct lng cu,

m x tht heoTrong lnh vc ny robot m nhim c ngay

cc cng vic mang tnh lp li, nhiu thao tc i hi s

phi hp tay ngh cao v s lanh li ca i mt.

ng dng robot trong khng gian: Tay my c ch to

nhm tng cng kh nng bc d hng ha tip t, lp ghp

vi cc trm khng gian khc nhau. Cc xe t hnh trang b

tay my linh hot, cc robot cng dng chung trong cc

trm khng gian, bo tr v tinh, xy dng trong khnggian

ng dng robot trong tu ln: Pht trin cc tu ln

khng ngi li trong cng tc kim tra, d tm, bo tr

thm him di y i dng.


4/114


Trang:4LP : CTK8LC1

GVHD: Trn Xun Tin

ng dng robot trong gio dc: Robot c s dng lm

phng tin ging dy trong cc chng trnh gio dc.

Robot c kt hp vi ngn ng LOGO ging dy v nhn

thc my tnh, to ra phng hc robot.

ng dng robot trong h tr ngi tn tt: thng quaxung no iu khin cc tay my lm nhng thao tc mong

mun, nhng b phn thay th c th.

ng dng robot trong sinh hot v gii tr: Cc loi

chi tr em, robot htoong minh s dng trong nh v vn

phng

Cu 2: Nu khi nim min lm vic, bc t do? C nhngloi khp no? Bc t do ca mi loi?

Khi nim min lm vic ca robot:

Min lm vic (hay vng lm vic, trng cng tc,khng gian cng tc) ca robot l ton b th tch c

qut bi khu chp hnh cui khi robot thc hin tt c

cc chuyn ng c th. Min lm vic b rng buc bi ccthng s hnh hc ca robot cng nh cc rng buc c hc

ca cc khp; v d, mt khp quay c chuyn ng nh hn

mt gc 360

0

. Ngi ta thng dng hai hnh chiu m tmin lm vic ca mt robot.


5/114


Trang:5LP : CTK8LC1

GVHD: Trn Xun Tin

Bc t do ca robot (DOF : Degrees Of Freedom) :

Bc t do l s kh nng chuyn ng ca mt c

cu (chuyn ng quay hoc tnh tin) trong khng gian

hot ng. dch chuyn c mt vt th trong khng

gian, c cu chp hnh ca robot phi t c mt s

bc t do. Ni chung c h ca robot l mt c cu h,

do bc t do ca n c th tnh theo cng thc :

5

1

.6i

ipinw

y: w - S bc t do ca tay my.n - S khp ca tay my;


6/114


Trang:6LP : CTK8LC1

GVHD: Trn Xun Tin

pi- S khp loi i (i = 1,2,. . .,5 : S bc t do b

hn ch hay loi khp).

i vi cc c cu c cc khu c ni vi nhau bng

khp quay hoc tnh tin (khp ng loi 5) th s bc t

do bng vi s khu ng . i vi c cu h, s bc t dobng tng s bc t do ca cc khp ng.

nh v v nh hng khu chp hnh cui mt cch

tu trong khng gian 3 chiu robot cn c 6 bc t do,

trong 3 bc t do nh v v 3 bc t do nh

hng. Mt s cng vic n gin nng h, sp xp... c

th yu cu s bc t do t hn. Cc robot hn, sn...

thng yu cu 6 bc t do. Trong mt s trng hp cn s

kho lo, linh hot hoc khi cn phi ti u ho qu

o,... ngi ta dng robot vi s bc t do ln hn 6.

Cc bc chuyn ng c phn ra bc chuyn ng c bn

(chuyn ng nh v) v bc chuyn ng b xung (chuyn

ng nh hng).

Chuyn ng c bn thng l chuyn ng tnh tin

hoc l chuyn ng quay, mi bc chuyn ng c ngun dn

ng ring.

Chuyn ng b xung thng l cc chuyn ng quay

nhm tng kh nng linh hot, m bo cho khu chp hnhcui hon thnh nhim v.

Khp v bc t do ca mi loi:

Ty theo cu trc, mi khp hn ch mt s chuyn ng

gia 2 khu. B mt tip xc ca mi khu ti khp gi l

mt thnh phn khp. Hai thnh phn khp to thnh mt

khp ng. Khp ng c th phn thnh khp thp v khp

cao ty thuc vo dng tip xc.

Khp thp : nu hai thnh phn tip xc l mt.Khp cao : nu hai thnh phn tip xc l im hoc

ng.

C 6 loi khp thp v hai loi khp cao c bn thng

dng trong cc c cu my v cc robot l:


7/114


Trang:7LP : CTK8LC1

GVHD: Trn Xun Tin

Khp quay (Revolute Joint - R): khp li chuyn ng

quay ca khu ny i vi khu khc quanh 1 trc quay.

Ngha l khp quay hn ch 5 kh nng chuyn ng tng

i gia hai thnh phn khp v c 1 bc t do. Khp quay

thng c gi l khp bn l.Khp lng tr (Prismatic Joint - P): Cho php hai khu

trt trn nhau theo 1 trc. Do khp lng tr hn ch 5

kh nng chuyn ng tng i gia hai khu v c 1 bc

t do. Ngi ta cn gi khp lng tr l khp tnh tin

(Join - T).

Khp tr (Cylindrical Joint - C): Cho php 2 chuyn ng

c lp gm 1 chuyn ng quay quanh trc v chuyn ng

tnh tin dc trc quay. Do khp tr hn ch 4 kh nng

chuyn ng gia 2 khu v c 2 bc t do.

Khp ren (Helical Joint - H): Cho php chuyn ng quay

quanh trc ng thi tnh tin theo trc quay. Tuy nhin

chuyn ng tnh tin ph thuc vo chuyn ng quay bi

bc ca ren vt. Do khp ren hn ch 5 kh nng chuyn

ng tng i gia hai khu v cn lai 1 bc t do.

Khp cu (Spherical Joint - S): Cho php thc hin chuyn

ng quay gia 2 thnh phn khp quanh tm cu theo tt c

cc hng , nhng khng c chuyn ng tnh tin gia 2thnh phn khp ny. Do khp cu hn ch 3 kh nng

chuyn ng v cn li 3 bc t do.

Khp phng (Plane Joint - E): Cho 2 kh nng chuyn ng

tnh tin theo 2 trc trong mt phng tip xc v mt kh

nng quay quanh trc vung gc vi mt phng tip xc. Do

khp cu hn ch 3 kh nng chuyn ng v cn li 3

bc t do.

Khp bnh rng (Geat Pair - G): Cho hai bnh rng n khpvi nhau cc mt rng tip xc y nhau chng thng trt

trn nhau. Do khp bnh rng phng hn ch 4 kh nng

chuyn ng tng i gia 2 thnh phn khp cn li 2 bc

t do.


8/114


Trang:8LP : CTK8LC1

GVHD: Trn Xun Tin

Khp cam phng (Cam Pi - Cp): Tng t nh khp bnh rng

hai thnh phn khp lun tip xc vi nhau. Do , khp

cam phng c 2 bc t do.

Cu 3: Cng thc tnh bc t do ca tay my? Tnh bc tdo ca nhng tay my sau:

Cng thc tnh bc t do ca tay my:

5

1

.6i

ipinw

Trong : w - S bc t do ca tay my.

n - S khp ca tay my;

pi- S khp loi i (i = 1,2,. . .,5 : S bc t do b

hn ch hay loi khp).Tnh bc t do ca nhng tay my sau:

Hnh a Hnh b Hnh c

Hnh a: S khp n = 4

i 1 2 3 4 5pi 0 0 0 2 2

Vy: S bc t do ca tay my l:

ADCT:

5

1

.6i

ipinw

Thay s: 6)2.52.4(4.6 w


9/114


Trang:9LP : CTK8LC1

GVHD: Trn Xun Tin

Hnh b: S khp n = 4

i 1 2 3 4 5pi 0 0 1 1 2


ADCT:

5

1

.6i

ipinw

Thay s: 7)2.51.41.3(4.6 w

Hnh c: S khp n = 4

i 1 2 3 4 5pi 0 0 0 1 3


ADCT:

5

1

.6i

ipinw

Thay s: 5)3.51.4(4.6 w

Hnh d Hnh eHnh f

Hnh d: S khp n = 4

i 1 2 3 4 5


10/114


Trang:10LP : CTK8LC1

GVHD: Trn Xun Tin

pi 0 0 1 1 2


ADCT:

5

1

.6i

i

pinw

Thay s: 7)2.51.41.3(4.6 w

Hnh e: S khp n = 4

i 1 2 3 4 5pi 0 0 2 0 2


ADCT:

5

1

.6i

ipinw

Thay s: 8)2.52.3(4.6 w

Hnh f: S khp n = 4

i 1 2 3 4 5pi 0 0 1 0 3


ADCT:

5

1

.6i

ipinw

Thay s: 6)3.51.3(4.6 w


11/114



GVHD: Trn Xun Tin

Hnh g Hnh h

Hnh i

Hnh g: S khp n = 3

i 1 2 3 4 5

pi 0 0 0 1 2


ADCT:

5

1

.6i

ipinw

Thay s: 4)2.51.4(3.6 w

Hnh h: S khp n = 3

i 1 2 3 4 5pi 0 0 0 0 3


ADCT:

5

1

.6i

ipinw

Thay s: 33.53.6w

Hnh i: S khp n = 4

i 1 2 3 4 5pi 0 0 0 1 3


12/114



GVHD: Trn Xun Tin


ADCT:

5

1

.6i

ipinw

Thay s: 5)3.51.4(4.6 w

Cu 4: Phn loi robot cng nghip theo h thng dn ng

v khng gian lm vic?

Phn loi theo h thng truyn ng :

C cc dng truyn ng ph bin l :

H truyn ng in : Thng dng cc ng c in 1

chiu (DC : Direct Current) hoc cc ng c bc (step

motor). Loi truyn ng ny d iu khin, tc ng nhanhv chnh xc, kt cu gn, gi thnh khng cao, mmen quay

cao, trng lng gim v p ng nhanh, p dng c k

thut iu khin phc tp. Tuy nhin, bn cht l tc

cao, khe h bnh rng lm gim chnh xc, gy qu

nhit khi qu ti, cn s dng phanh ghim v tr v

khp.

H truyn ng thu lc : c th t c cng sut

cao, p ng nhng iu kin lm vic nng, tc chym, c th kha cng khp ti v tr xc nh (khng nn

c), an ton p sut chy n. Tuy nhin h thng thu

lc thng c kt cu cng knh, chi ph cao, khng thch

hp cho c cu quay vi tc nhanh, cn c ng hi

du, tn ti phi tuyn ln kh x l khi iu khin.

H truyn ng kh nn : c kt cu gn nh hn do

khng cn dn ngc, gi thnh khng cao, kh thi khng

gy nhim mi trng nhng li phi gn lin vi trung

tm ta ra kh nn, gy n kh iu chnh tc , phi s

dng b lc. H ny lm vic vi cng sut trung bnh v

nh, km chnh xc, thng ch thch hp vi cc robot

hot ng theo chng trnh nh sn vi cc thao tc n


13/114



GVHD: Trn Xun Tin

gin nhc ln - t xung (Pick and Place or PTP :

Point To Point).

Phn loi theo dng khng gian lm vic:

Khng gian lm vic ca c cu chp hnh c xc

nh l th tch khng gian tc ng c th vi ti.Thng s dng hai nh ngha v khng gian lm vic.

Th nht l khng gian c th vi ti, th tch khng

gian trong c cu tc ng c th vi ti tng im

theo t nht 1 chiu. Th hai l khng gian linh hot,

th tch khng gian trong c cu tc ng c th

vi ti tng im theo mi chiu c th. Khng gian

linh hot l mt phn ca khng gian c th vi ti.

Mt s robot cu trc ni tip in hnh:

Robot kiu to cc (Robot ta vung gc):

l tay my c 3 chuyn ng c

bn tnh tin theo phng ca

cc trc h to gc (cu

hnh T.T.T). Trng cng tc c

dng khi ch nht. Do kt cu

n gin, loi tay my ny c

cng vng cao, chnh xc

c kh d m bo v vy nthung dng vn chuyn phi

liu, lp rp, hn trong mt phng

Robot kiu to tr : Vng

lm vic ca robot c dng hnh

tr rng. Thng khp th nhtchuyn ng quay, cc khp cn

li chuyn ng tnh tin hoc

c mt trc tnh tin v 2 trc

quay. V d robot 3 bc t do,

cu hnh R.T.T nh hnh v. C


14/114



GVHD: Trn Xun Tin

nhiu robot kiu to tr nh : robot Versatran ca hng

AMF (Hoa K).

Robot kiu to cu : Vng lm vic ca robot c dnghnh cu, thng cng vng ca loi robot ny thp hn

so vi hai loi trn. V d robot 3 bc t do, cu hnh

R.R.R hoc R.R.T lm vic theo kiu to cu (hnh).

Robot kiu ta gc (H ta phng sinh) : y l kiu

robot c dng nhiu hn c. Ba chuyn ng u tin lcc chuyn ng quay, trc quay th nht vung gc vi haitrc kia. Cc chuyn ng nh hng khc cng l ccchuyn ng quay. Vng lm vic ca tay my ny gn gingmt phn khi cu. Tt c cc khu u nm trong mt phngthng ng nn cc tnh ton c bn l bi ton phng. uim ni bt ca cc loi robot hot ng theo h to gc l gn nh, tc l c vng lm vic tng i ln sovi kch c ca bn thn robot, linh hot cao.Cc robot hot ng theo h to gc nh : Robot PUMA

ca hng Unimation - Nokia (Hoa K - Phn Lan), IRb-6,IRb-60 (Thu in), Toshiba, Mitsubishi, Mazak (Nht Bn).V.V...V d mt robot hot ng theo h to gc (H to phng sinh), c cu hnh RRR.RRR :


15/114



GVHD: Trn Xun Tin

Robot kiu SCARA : Robot SCARA

ra i vo nm 1979 ti trng

i hc Yamanashi (Nht Bn) l

mt kiu robot mi nhm p ng

s a dng ca cc qu trnh sn

xut. Tn gi SCARA l vit tt

ca "Selective Compliant

Articulated Robot Arm" : Tay my

mm do tu . Loi robot nythng dng trong cng vic lp

rp nn SCARA i khi c gii

thch l t vit tt ca "Selective Compliance Assembly

Robot Arm". Ba khp u tin ca kiu Robot ny c cu

hnh R.R.T, cc trc khp u theo phng thng ng. S

ca robot SCARA nh hnh. Vng lm vic ca tay my

SCARA l mt phn ca hnh tr rng.

CU HI N TP CHNG 2Cu 1:Trnh by cng thc ma trn cosin chhng? Nu v chng minh nhng tnh

cht c bn ca ma trn cosin chhng.Cng thc ma trn cosin chhng:


16/114



GVHD: Trn Xun Tin

Cho vt rn B v h quy chiu R0 = )0(3)0(2)0(1 ,, eee

trong )0(

3

)0(

2

)0(

1,, eee

lba vc t n v trn cc trc Ox0, Oy0, Oz0 . Ta gn cht vt rn vo mt h quy chiu

R1 = )1(3)1(2)1(1 ,, eee

vi)1(

3

)1(

2

)1(

1,, eee

l ba vc t n v trn cc trc Ax, Ay,Az (Hnh).

Ma trn vung cp ba:

)1(

3

)0(

3

)1(

2

)0(

3

)1(

1

)0(

3

)1(

3

)0(

2

)1(

2

)0(

2

)1(

1

)0(

2

)1(

3

)0(

1

)1(

2

)0(

1

)1(

1

)0(

1

...

...

...

eeeeee

eeeeee

eeeeee

A

c gi l ma trn cosin chhng ca vt rn B i vi h quy chiu R0.

Nu ta a vo k hiu: ).cos(.)1()0()1()0(

jijiij eeeea

, (i,j = 1,2,3)

th ma trn cosin chhng c dng:

333231

232221

131211

aaa

aaa

aaa

A

Tnh ngha trn, trong h quy chiu R0 ta c cc h thc hin h:)0(331

)0(

221

)0(

1111... eaeaeae

)0(

332

)0(

222

)0(

1122... eaeaeae

)0(

333

)0(

223

)0(

1133... eaeaeae

Nu ta k hiu ie l ma trn ct gm cc phn t ca vc t ie

trong h qui chiu R0.


17/114



GVHD: Trn Xun Tin

Ta c:

31

21

11

1

a

a

a

e

,

32

22

12

2

a

a

a

e

,

33

23

13

1

a

a

a

e

Th ma trn cosin chhng c dng:

321eeeA

Ma trn cosin chhng A cn c gi l ma trn quay ca vt rn.

Nhng tnh cht c bn ca ma trn cosin chhng:nh l 1:Ma trn cosin chhng l ma trn trc giao.Chng minh:Tcc phng trnh :

Ta suy ra:

0

11

21

31

(1)

1

R

a

a

a

e

, 012

22

32

(1)

2

R

a

a

a

e

,0

13

23

33

(1)

3

R

a

a

a

e

T ma trn cosin chhng c th vit di dng :

A= 0 (1)1R e 0 (1)2R e 0 (1)3R e (1.12)Ta thy ma trn cosin chhng Al ma trn c ba ct l ba vec t trc chun. do Al mt ma trn trc giao. T tnh cht ca ma trn trc giao ta suy ra A-1 =AT. Do , tcng thc (1.11) ta c th bin i ngc :

u(1)=A-1.u(0)=AT.u(0)

nh l 2: nh thc ca ma trn csin chhng bng 1, detA= 1.Chng minh: theo biu thc tnh tch hn hp ca ba vct

1 1 1

2 2 2

3 3 3

.( )

a b c

a b c a b c

a b c

=

| |

V cng thc (1.12) ta suy ra:

detA= (1) (1) (1)1 2 3, ,e e e = 2 3 1, ;Do R1 l h quy chiu thun nn ta c:


18/114



GVHD: Trn Xun Tin

(1) (1) (1)

2 3 1e e e

Th(2) vo (1) ta c :

detA= 2(1) (1)

1. 1e e

inh l 3: Ma trn csin chhng c t nht mt tr s ring bng 1Chng minh: trc ht ta chng minh cc tr ring ca ma trn csin chhng A u ctr tuyt i l 1. Ga s l tr ring ca ma trn A, u l vc t ringtng ng. Theobi ton tr s ring ta c:

A. u = u T ta suy ra :

2

( ).( ) ( ) .( ) . . .T T T T

Au Au Au Au u A Au u u u .

Vy : . 1Au u u

By gita chng minh trong ba tr ring ca ma trn A c mt tr ring l 1 .Theo trn ta c || || || . Do A l ma trn vung cp ba vi cc phn t lcc s thc nn phng trnh c trng ca n l phng trnh i s bc ba vi h sthc : Do cc tr s ring ca ma trn A c th c hai khnng: l cc s thc . c mt tr s ring thc v mt cp tr ring lin hp.Ga s

l tr ring ca A. Theo (3) ta thy c hai khnng xy ra hoc

hoc

. Nu , nh l c chng minh. Nu , ta c :detA= Ta thy c hai khnng xy ra: hoc l hai s thc , hoc l hai s phclin hp. Nu v l hai s phc lin hp ( ta c h thc : 2 22 Hthc (6) mu thun vi h thc (5) . Vy phi l hai s thc , do 2 3 1, nn ta suy ra:Hoc 2 1 , 3 1 hoc 2 1 , 3 1

Nh th ma trn csin chhng A lun c mt tr ring bng 1.Cu 2: Trnh by nhng ma trn quay c bn, cc gc euler, cc gc Roll- Pitch-Yaw.Trnh by nhng ma trn quay c bn:Trc ht ta quy c hng quay dng quanh mt trc l hng quay ngc chiu kimng hnh hnh v:


19/114



GVHD: Trn Xun Tin

Cc php quay quanh trc x,y,z ca h tovung gc Oxyz c l php quay cbn. Ta tm ra ma trn quay ca php quay quanh trc mt gc . Hnh:

Theo cng thc nh ngha ma trn csin chhng:

)1(

3

)0(

3

)1(

2

)0(

3

)1(

1

)0(

3

)1(

3

)0(

2

)1(

2

)0(

2

)1(

1

)0(

2

)1(

3

)0(

1

)1(

2

)0(

1

)1(

1

)0(

1

...

...

...

eeeeee

eeeeee

eeeeee

A

Ta ddng xc nh c ma trn csin chhng:

cossin0

sincos0

001

xA

Ma trn trn c gi l ma trn quay caphp quay c bn quanh trc x0. Bng cchtng tta xc nh c cc ma trn quay c bn quanh cc trc y0 v z0.


20/114



GVHD: Trn Xun Tin

cos0sin

010

sin0cos

yA

100

0cossin

0sincos

zA

Cc gc Euler:

V tr ca vt rn B quay xung quanh im O cnh xc nh bi v tr ca h quy chiung Oxyz (gn cht vo vt rn B) i vi h quy chiu cnh Ox0y0z0 .Ga s giaoca mt phng quy chiu ) v mt phng Oxy (mt phng ng ) l ng OK. Trc OKny c gi l ng nt .Ta a cc k hiu sau:-Gc gia trc Ox0 v OK l ,-Gc gia trc Oz0 v Oz l ,-Gc gia trc OK v Ox l

Ba gc , , c gi l ba gc Euler. S dng cc gc Euler ta c th quay h quychiu Ox0y0z0 sang h quy chiu Oxyz bng php quay nh sau:


21/114



GVHD: Trn Xun Tin

Quay h quy chiu R0 = Ox0y0z0 quanh trc Oz0 mt gc trc Ox0 chuyn ti ngOK. Vi php quay ny, h quy chiu Ox0y0x0 chuyn sang h quy chiu R1 = Ox1y1z1vi Oz0 Oz1.

Quay h quy chiu R1 = Ox1y1z1 quanh trc Ox1 OKmt gc trc Oz1 chuyn titrc Oz2 Oz h quy chiu Ox1y1z1 chuyn thnh h quy chiu Ox2y2z2 vi Ox1 Ox2 OK.Quay h quy chiu R2 = Ox2y2z2 quanh trc Oz2 Oz mt gc trc Ox2 OKchuyn ti trc Ox.Vi php quay ny, h quy chiu Ox2y2x2 chuyn sang h quy chiuOxyzvi Oz2 Oz.

Nh th , bng ba php quay Euler quanh trc Oz0 mt gc , quanh trc OK mt gc ,quanh trc Oz mt gc , h quy chiu Ox0y0z0 chuyn sang h quy chiu Oxyz. Th tcc php quay l z-x-z. cc ma trn ng vi cc php quay Euler c dng:

Cc gc Roll - PitchYawCc php quay Roll - PitchYaw ln lt quanh cc trc zyx. Ta c th quay hquy chiu cnh Ox0y0z0 sang h quy chiu ng Oxyz bng ba php quay Roll - Pitch

Yaw nh hnh:

z2 z

z1 z

y 2

y1

x

x 2

z0 z1

x1 K

x0

y0

y1

x1 x 2

z0 z

1

y 0

y1


22/114



GVHD: Trn Xun Tin

Quay h Ox0y0z0 quanh trc Oz0 mt gc ty (gc Roll). H Ox0y0z0 chuyn thnh hOx1y1z1 vi Oz0 Oz1.Quay h Ox1y1z1 quanh trc Oy1 mt gc ty (gc Pitch) sao cho trc Ox1 chuyn

n trc Ox. H Ox1y1z1 chuyn thnh h Ox2y2z2 vi Oy1 Oy2.Quay h Ox2y2z2 quanh trc Ox2 Ox mt gc ty (gc Yaw) sao cho trc Oz2chuyn n trc Oz. H Ox2y2z2 chuyn thnh h Oxyz.Cc ma trn quay tng ng vi cc php quay Roll, Pitch, v Yaw c dng nh sau:

z

x

y

Rol l

Pitch

Y aw

z0 z1

x1

x 0

y 0

y1

z1 z0

x 2 xx1

z2

y1

x 2 x

z

y 2

z2

O O O

y


23/114



GVHD: Trn Xun Tin

Cu 3: Trnh by v chng minh cc nh l v vn tc gc v gia tc gc ca vt rn.

nh v vn tc c ca vt rnXc nh vn tc gc ca vt rn qua cc vc tn v ca h quy chiu gn lin vovt rn.nh l 1: Trong c svung gc R1 = { , , }gn lin vo vt rn B,

vn tc gc ca vt rn B i vi h quy chiu R0 c xc nh bi cng thc :

0 0 00

(1) (1) (1)

(1) (1) (1) (1) (1) (1)2 3 1

1 3 2 1 3 2. . .

R R RR B de de de

e e e e e edt dt dt

Chng minh:Trong h qui chiu R1 ={ , , } vc t vn tc gc ca vt rn B c th vitdi dng:

0 0 0 0(1) (1) (1) (1) (1) (1)

1 1 2 2 3 3( . ). ( . ). ( . ).R B R B R B R B

e e e e e e (1)Do , , l ba vc t n v trn ba trc ca h ta vung gc nn ta ccng thc quen bit:

=

.

,

=

.

,

=

.

(2)

Th cc biu thc 2 vo 1 ta c :0 0 0 0(1) (1) (1) (1) (1) (1) (1) (1) (1)

3 1 1 2 2 31 3 1[ .( ]. [ .( ]. [ .( ].

R B R B R B R B

e e e e e e e e e (3)Da vo tnh cht ca tch hn hp ba vc t

.( ) .( ) .( )a b c b c a c a b V nh ngha vn tc gc ca vt rn , h thc (3) c dng

0 0 0 0(1) (1) (1) (1) (1) (1) (1) (1) (1)

3 2 1 1 3 2 2 1 3[ .( )]. [ .( )]. [ .( )].R B R B R B R B

e e e e e e e e e

0 0 0(1) (1) (1)

(1) (1) (1) (1) (1) (1)2 3 1

3 1 2 2 2 3. . . . . .

R R Rde de d e

e e e e e edt dt dt

(4)

Do tch v hng ca hai vc t c tnh cht giao hon, t (4) ta suy ra cng thc

0 0 00

(1) (1) (1)

(1) (1) (1) (1) (1) (1)2 3 1

1 3 2 1 3 2. . .

R R RR B d e d e d e

e e e e e edt dt dt


24/114



GVHD: Trn Xun Tin

nh l c chng minh.

Xc nh vn tc c ca vt rn t a trn csn ch hnnh l 2: cho A l ma trn cosin chhng ca vt rn B trong h quy chiuR0={x0y0z0}. Khi ton t sng ca vc t vn tc gc ca vt rn trong h quy chiu

cnh R0 c xc nh bi cng thc: Trong : 0 0 0

0 0 0

(0), ,

TR R RB B B

B x y z , = 0R dAdt

Chng minh: chn P l mt im ty thuc vt rn B. Ta c:

pp Ar r u

(1)

o hm phng trnh (1) theo thi gian trong h quy chiu c nh 0 ta c:

0 0

R Rp A

p p A pd r d r u v v udt dt

(2)

Trong 0R B l vn tc gc ca vt rn.Nu s dng k hiu ma trn th phng trnh (2) c dng (4)Cn phng trnh (1) c dng: T cng thc u(0) =Au(1) ta c

(5)

Trong l ma trn ct gm c ba thnh phn l hnh chiu ca vct pu trong hOx0y0z0 cn

(1)

pu l ma trn ct gm ba thnh phn l cc hnh chiu ca vct pu trongh Ax1y1z1 .Th (5) vo (4) ta c: (6)o hm phng trnh (6) theo thi gian trong h 0 ta c:

(0) (0) (1)Ap pAv v u (7)

Do ma trn cosin ch hng la ma trn trc giao nn t h thc (5) ta suy ra(1) 1 (0) (0)

T

p p pu A u A u (8)

Th (8) vo (7) ta c(0) (0) (0)

AAT

p pAv v u (9)

o snh (9) vi (3) ta suy ra cng thc


25/114



GVHD: Trn Xun Tin

a tc c ca vt rn

nh nha a tc c ca vt rn v h ta 0 t n vect

hu

0R B

bn h the th an trn h quy chu 0 ca vc t vn tcc 0

0 0

( )

RR B R Bd

dt nh trn h quy chu 1n n v vt nu vct vn tc c ca vt rn

c bu dn d dn 0 (1) (1) (1)1 1 1

1 2 31 2 3. . .

R B

e e e

Th biu thc tnh gia tc gc ca vt rn c dng nh sau:

0 (1) (1) (1)(1) (1) (1)1 2 31 2 3

R B

e e e hn nh theo nh ngha gia tc gc ta c :

0 0

0 0 (1) (1) (1)1 1 11 2 31 2 3

( ) ( . . . )

R RR B R Bd d

e e edt dt

0 0 0

(1) (1) (1)

(1) (1) (1) 1 2 31 1 1(1) (1) (1)1 2 31 2 3 1 2 3

R R Rde de de

e e edt dt dt

(1)

Do0

(1)

(1)R Bii

dee

dt (i 1,2,3) nn ta c

0 0 0

0

(1) (1) (1)

(1) (1) (1)1 2 31 1 1 1 1 11 2 31 2 3 1 2 3

( . . . )

R R RR Bd e de d e

e e edt dt dt

0 0

0R B R B

(2)Th (2) vo (1) ta c

0 (1) (1) (1)(1) (1) (1)1 2 31 2 3

R B

e e e

Cu 4:Trnh by cng thc cng vn tc gc v cng thc cng gia tc gc ca vtrn.

Cng thc cng vn tc gc:


26/114



GVHD: Trn Xun Tin

Xt mt vct ty ca vt rn B. Vt rn B chuyn n i vi h quychiu , h quy chiu li chuyn n i vi h quy chiu . The nh nhavn tc gc ta c cng thc:

Hnh 1

0

0

RR Bd c

cdt

(1)

1

1

RR Bd c

cdt

(2)

Mt khc t cng thc:0 1

0 1

R RR Rd c d c

adt dt

(.)

Ta c phng trnh:0 1

0 1

R RR Rd c d c

cdt dt

(3)

Th cc h thc (1) v (2) vo phng trnh (3) c0 0 01 1 1 1( )

R R RR R R RB B Bc c c c (4)T (4) suy ra

0 0 1 1R R R RB B (3.1)Cng thc (3.1) l cng thc cng vn tc vt rn.


27/114



GVHD: Trn Xun Tin

Hnh 2Chuyn ng ca vt rn B i vi h quy chiu 1 c gi l chuyn ng tngi, chuyn ng ca vt rn B i vi h quy chiu 0 c gi l chuyn ng tuyt

i. Chuyn ng ca h quy chiu ng 1 i vi h quy chiu cnh 0 c gi

l chuyn ng theo. K hiu 0R B

a , (vn tc gc tuyt i ca vt rn B),

1R B

r , (vn tc gc tng i ca vt rn B), 0 1R R e ,(vn tc gc theo).

Khi biu thc (3.1) c dng

a r e (3.2)

nh l 2.4. Ti mi thi im, vn tc gc tuyt i ca vt rn B bng tng hnh hcvn tc gc tng i v vn tc gc theo ca n.Ap dng lin tip cng thc (3.1) i vi n+1 h quy chiu nh hnh 2 ta c

0 0 11 1 2 ... n n n BR R R R RR R R RB

(3.3)

Cng thc cng vn tc gc:o hm theo thi gian cng thc (3.1) h quy chiu 0 ta c

0 0 0

0 0 1 1( ) ( ) ( )

R R RR R R RB Bd d d

dt dt dt (1)

Ch n cng thc (.) ta c0

0 0 01 1 1 1( )

RR R RR R R RB B Bd

dt (2)

Tnh ngha chuyn ng tuyt i, tng i v theo ca vt rn suy ra


28/114



GVHD: Trn Xun Tin

0R B

a , 1

R B

r , 0 1

R R

e (3)

Th(3) vo (2) ta c

a e r e r (3.4)

nh l 2.5. mi thi im gia tc gc tuyt i ca vt rn B bng tng hnh hc giatc gc tng i, gia tc gc theo, v gia tc gc ph p e r ca vt rn.

Gia tc ph p e r cn c gi l gia tc gc Resal.

Cu 5:Xc nh vn tc, gia tc mt im bt k thuc vt rn.Vn tc 1 m bt k thuc vt rn:

xt chuyn ng ca vt rn B trong h quy chiu 0 0 0 0R ox y z , ta ly im A bt

k thuc vt rn lm im nh v(im cc). Dng h quy chiu 1 1 1 1R Ax y z

gn linvo vt rn B.

V tr ca im P c xc nh bi vc t sau

P A Pr r u (1)

o hm 2 v theo thi gian ca phng trnh (1) trong h quy chiu o ta c0 0 0R R R

P A Pd r d r du

dt dt dt

(2)Theo nh ngha vn tc gc ca vt rn ta c

0R

R BPP P

d uu u

dt (3)

Th(3) vo phng trnh (2) ta c


29/114



GVHD: Trn Xun Tin

0 0R R

P AP

d r d r u

dt dt ==> P A Pv v u (4.1)

Chiu phng trnh vc t (4.1) ln h quy chiu Ro ta c:(0) (0) (0) (0)

P A Pv v u (4.2)Chiu phng trnh vc t (4.1) ln h quy chiu 1 ta c(1) (1) (1) (1)

P A Pv v u (4.3)

Ch rng:(0) (1)

P Pu Au

Gia tc 1 m bt k thuc vt rn:o hm theo thi gian biu thc (4.1) trong h quy chiu ota c

0 0 00R R RR

P A PP

d v d v d udu

dt dt dt dt

T suy ra

).~.(~.~PPAP

uuaa (4.4)Chiu phng trnh (4.4) ln h quy chiu o ta c phng trnh ma trn

)0()0()0()0()0()0(

)0()0()0()0()0()0()0(

).~.~~(

).~.(~.~

PAP

PPAP

uaa

uuaa

(4.5)

Chiuphng trnh vc t (4.4) ln h quy chiu R1

)1()1()1()1()1()1(

)1()1()1()1()1()1()1(

).~.~~(

).~.(~.~

PAP

PPAP

uaa

uuaa

(4.6)

Cu 6:Cho c cu nh hnh v.


30/114



GVHD: Trn Xun Tin

Da vo cc h ta gn ln cc khu, cc thng skch thc v gc quay , hytnh:

Cc ma trn cosin chhng gia cc h ta lin tip (R0 R1 R2 R3 R4 R5): A1, A2,A3,A2,A3,A4,A5:

100

0cossin

0sincos

11

11

1

A

;

100

0cossin

0sincos

22

22

2

A

100

0cossin

0sincos

33

33

3

A

;

100

0cossin

0sincos

44

44

4

A

100

0cossin

0sincos

55

55

5

A

Vn tc gc, gia tc gc cc khu trong h ta R0(O0x0y0z0)Gi i l vn tc gc ca khu th i.

p dng cng thc: V cng thc cng vn tc gc ta c: BRRRRRRRBR n ...3221100 Vn tc gc khu 1:

Ta c: Vi:


31/114



GVHD: Trn Xun Tin

100

0cossin

0sincos

11

11

1

A

=

000

0sincos

0cossin

11

11

.

100

0cossin

0sincos

11

11

1

T

A

= Tng t ta c:Vn tc gc khu 2:

=

Vn tc gc khu 3:

=

Vn tc gc khu 4:

=


32/114



GVHD: Trn Xun Tin

Vn tc gc khu 5:

Do R5 tnh tin so vi R0 nn ma trn cosin chhng ca h ta R5 so vi R0 l:

100

010

001

05A

;

=

;

Gia tc gc cc khu:

Vn tc, gia tc ti trng tm cc khu(trn t c m chnh gia ca cc khu)trong h ta R0(O0x0y0z0):

Vn tc cc gc ta :

vO1 =

; vO2 =

vO3 =


33/114



GVHD: Trn Xun Tin

vO4=

vO5 = Vn tc ti trng tm cc khu:

vG1 =

; vG2 =

vG3 =

vG4 =

[ 0 ]vG5 =


34/114



GVHD: Trn Xun Tin

[ 0]

Gia tc ti trng tm cc khu:

aG1 =

aG2 = ( ) ( ) ( ) ( ) ( ) ( ) aG3 =

[

.l2.S(2.1 + 2.2 + 3) - (

.l3.S(2.1 + 2.2 + 3))/2 -

.l1.S(2.1 + 2) -

.l2.C(2.1 + 2.2 + 3) - (.l3.C(2.1 + 2.2 + 3))/2 - .l1.C(2.1 + 2) -2. .l2.C(2.1 + 2.2 + 3) - 2. .l2.C(2.1 + 2.2 + 3) - .l3.C(2.1 +2.2 + 3) - .l3.C(2.1 + 2.2 + 3) - 2. .l1.C(2.1 + 2)][ .l2.C(2.1 + 2.2 + 3) + (.l3.C(2.1 + 2.2 + 3))/2 + .l1.C(2.1 + 2) -.l2.S(2.1 + 2.2 + 3) - (.l3.S(2.1 + 2.2 + 3))/2 - .l1.S(2.1 + 2) -2. .l2.S(2.1 + 2.2 + 3) - 2. .l2.S(2.1 + 2.2 + 3) - .l3.S(2.1 +2.2 + 3) -

.l3.S(2.1 + 2.2 + 3) - 2.

.l1.S(2.1 + 2)]

[ 0]

aG4 =

[- .l2S(2. + 2. + ) - (.l3.C(2. + 2. + + ))/2 - (.l4.C(2. +2. + + ))/2 - .l1.S(2. + ) - .l2.C(2. + 2. + ) - .l1.C(2. + ) - (.l3.S( + + + ))/2 - (.l4.S( + + + ))/2 -


35/114



GVHD: Trn Xun Tin

. .l3.C( + + + ) - . .l3.C( + + + ) - . .l3.C(+ + + ) - . .l4.C( + + + ) - . .l4.C( + + +) - . .l4.C( + + + - 2. . .l2.C(2. + 2. + ) -2.

.

.l2.C(2.

+ 2.

+

) - 2.

.

.l2.C(2.

+ 2.

+

)]

.l2C(2. + 2. + )+ .l1C(2. + )-(.l3.S(2. + 2. + + ))/2 -(.l4.S(2. + 2. + + ))/2 - .l2.S(2. + 2. + ) - .l1.S(2. + )+( l3.C(2. + 2. + + ))/2 + ( l4.C(2. + 2. + +))/2 - . .l3.S( + + + ) - . .l3.S( + + + ) -. .l3.S( + + + ) - . .l4.S( + + + ) - . .l4.S(+ + + ) - . .l4.S( + + + - 2. . .l2.S(2. + 2. + )- 2.

.

.l2.S(2.

+ 2.

+

) - 2.

.

.l2.S(2.

+ 2.

+

)]

[ 0]

aG5 =

[- .l1S(2. + 2. + ) - (.l3.C(2. + 2. + + ))/2 - .l1.S(2. + ) - .l2.C(2. + 2. + ) - .l1.C(2. + ) - .l4.S( + + + ) -(.l3.S( + + +2 ))/2 - . .l4.C( + + + ) - . .l4.C( + + + ) - . .l4.C( + + + ) - . .l4.C( + + + ) -

.

.l3.C(2.

+ 2.

+

+

) -

.

.l3.C(2.

+ 2.

+

+

) -

. .l3.C(2. + 2. + + ) - 2. . .l2.C(2. + 2. + ) -2. . .l2.C(2. + 2. + ) - 2. . .l1.C(2. + 2. + )][ .l2S(2. + 2. + ) + .l1.C(2. + ) - (.l3.S(2. + 2. + + ))/2

- .l2.S(2. + 2. + ) - .l1.S(2. + ) + .l4.C( + + + ) +(.l3.C( + + +2 ))/2 - . .l4.S( + + + ) - . .l4.S( + + + ) - . .l4.S( + + + ) - . .l4.S( + + + ) -

.

.l3.S(2.

+ 2.

+

+

) -

.

.l3.S(2.

+ 2.

+

+

) -

. .l3.S(2. + 2. + + ) - 2. . .l2.S(2. + 2. + ) -2. . .l2.S(2. + 2. + ) - 2. . .l1.S(2. + 2. + )][ 0]


36/114



GVHD: Trn Xun Tin

BI TP CHNG III V IV

L

1

L2

L3

x1z1

x0

z0Z3

X3

Z2

X2


37/114



GVHD: Trn Xun Tin

H2

di ai i Khp1 1 L1 0 90 R2 2 0 L2 -90 R3 3 0 L3 0 R

Bng D-H:

Vit cc ma trn H1, H2, H3

;

Vit phng trnh th hin v tr im chp hnh cui

Vit biu thc vn tc, gia tc tuyt i ca im chp hnh cui

aE =


38/114



GVHD: Trn Xun Tin

. - . - . + - . - . . - . - . - . - . - . - 2. . . + 2. . . - . -

.

.

-

.

+ 2.

.

.

+ 2.

.

.

+

2. . . . - . - . - . - . + . - . - .L3.S1.S3 - . - . - . - 2. . . - 2. . . + . - . - . - 2. . . . - 2. . . +2. . .

.

-

.

-

.

-

.

+

.

-

.

-

2. . . ; ;

;

;

Vit biu thc vn tc gc, gia tc gc a phngv tuyt i ca tng khu

; ;

;

;

;


39/114



GVHD: Trn Xun Tin

Xy dng v tnh ton cc phn t trong ma trn Jacobi:

JR=

[

]


40/114



GVHD: Trn Xun Tin

H3Bng D-H:

di ai i Khp

1 1 0 L1 90 R2 2 0 L2 0 R3 3 0 L3 0 R



41/114



GVHD: Trn Xun Tin

D3 =

[ C1.C2.C3 - C1.S2.S3, - C1.C2.S3 - C1.C3.S2, S1, l1.C1 + l2.C1.C2 + l3.C1.C2.C3 -

l3.C1.S2.S3][ C2.C3.S1 - S1.S2.S3, - C2.S1.S3 - C3.S1.S2, -C1, l1.S1 + l2.C2.S1 + l3.C2.C3.S1 -l3.S1.S2.S3][ C2.S3 + C3.S2, C2.C3 - S2.S3, 0,l2.S2 + l3.C2.S3 + l3.C3.S2][ 0, 0, 0,1]



vE =

- .( + . + . - ) - .( . + . + . ) -.( . + . ).( . + . +. - . . ) - .( + . . +. . ) - . . . + . . ).( .cos(2 + 3) + . ) + . .cos(2 + 3)


42/114



GVHD: Trn Xun Tin

aE =

. . . . - . . - . . . - .. - . . . - . . . . - . . . . - . . . . - . . . . - . . + . . . . + . . . . + 2. . . . . . - .l3. . . - . . . - . . . . - . . . . - . . . . +. . . . - 2. .. . . . + 2. . . . . +2. . . . . . .+ 2. . . . . . + 2. . . . . . + 2. .. . . .

.

.

-

.

.

-

.

.

.

-

.

.

.

+

.

.

.

-

.

.

.

-

. . . . - . . . . - . . . . + . . . . + . . . . + . . . . - 2. . . . . . + . . . . -. . . . - . . . . - . . . . - . . . - . . . . - 2. . . . . . - 2. . .l3. . . -2. . . . . . - 2. . . l3. . . - 2. . . . . . + 2. .. . . . . . - .l3.sin(2 + 3) - . .sin(2 + 3) - . + . .cos(2 + 3) +

.

.cos(2 + 3) - 2.

.

.

.sin(2 + 3)


43/114



GVHD: Trn Xun Tin

Vit biu thc vn tc gc, gia tc gc a phng v tuyt i ca tng khu


44/114



GVHD: Trn Xun Tin

( ) ( )

Xy dng v tnh ton cc phn t trong ma trn Jacobi

JR=

[

]


45/114



GVHD: Trn Xun Tin

H4

ng hc thun:

Tnh bc tdo ca tay my.

S khp n = 3

i 1 2 3 4 5

pi 0 0 0 0 3

Vy s bc t do ca tay my l:ADCT:

5

1

.6i

ipinw

Trong : w s bc t do ca tay my

ns khp ca tay my.

pis khp loi i (i 1,2,,5 s khp hay s bc t do b hn ch )Thay s: 33.53.6 w (bc t do)

Xy dng cc h ta cho cc khu:


46/114



GVHD: Trn Xun Tin

Bng D-H:

di ai i Khp1 1 0 l1 0 R2 2 0 l2 0 R3 3 0 l3 0 R

Vt cc a trn H1 H2 H3

=

D3 =

[

(

-

) -

(

+

), -

(

+

) -

(

-

), 0,

+ ( - ) - ( + ) + - ][( + ) + ( - ), ( - ) - ( + ), 0, + ( + ) + ( - ) + - ][ 0 0 1 0][ 0 0 0 1]

Vt phn trnh th hn v tr chp hnh cu

X0

Z0

X1

Z1

X2

Z2

X3

Z3

L3

L2

L1


47/114



GVHD: Trn Xun Tin

rE = + ( - ) - ( + ) + -

+

(

+

) +

(

-

) +

+ -

0Vt bu thc vn tc a tc tuyt ca chp hnh cu

vE =- ( + + ) - ( + ) - )( + + ) + ( + ) +

0

aE =[ -

-

-

-

) -

-

-

-

- - - - - 2 - 2 -2 - 2 ][ - - + + + - - - - + + - 2 -2 - 2 - 2 ][ 0]

; ;

Vt bu thc vn tc c a tc c a phn v tuyt ca tn hu


48/114



GVHD: Trn Xun Tin

; ;

ng hc ngc

Xy dn v tnh tn cc phn t trn a trn Jacb

; JR=

[

]

H5Bng D-H:

di ai i Khp1 1 0 l1 0 R2 2 0 l2 -90 R3 3 0 l3 0 R


49/114



GVHD: Trn Xun Tin





50/114



GVHD: Trn Xun Tin

aE =2. . . - . - - . - . - . - . - . - . - 2 - . - . - . . - 2

-

.

.

+ 2

.

. - . - . - . + . . - + . . -. - . - . . - 2. . + . . + . . - . . - 2. . - 2. . . . - 2. . .(- - . )



51/114



GVHD: Trn Xun Tin



52/114



GVHD: Trn Xun Tin

JR=

[

]

H6

di ai i Khp1 1 0 L1 -90 R2 2 0 L2 0 R3 3 0 L3 0 R

Bng D-H:

DH =

[ q1, 0, l1, -pi/2][ q2, 0, l2, 0]


53/114



GVHD: Trn Xun Tin

[ q3, 0, l3, 0]


H1 =

[ C1, 0, -S1, l1.C1][ S1, 0, C1, l1.S1][ 0, -1, 0, 0][ 0, 0, 0, 1]

H2 =

[ C2, -S2, 0, l2.C2][ S2, C2, 0, l2.S2][ 0, 0, 1, 0][ 0, 0, 0, 1]

H3 =

[ C3, -S3, 0, l3.C3][ S3, C3, 0, l3.S3][ 0, 0, 1, 0][ 0, 0, 0, 1]

D3 =

[ C1.C2.C3 - C1.S2.S3, - C1.C2.S3 - C1.C3.S2, -S1, l1.C1 + l2.C1.C2 + l3.C1.C2.C3 -l3.C1.S2.S3][ C2.C3.S1 - S1.S2.S3, - C2.S1.S3 - C3.S1.S2, C1, l1.S1 + l2.C2.S1 + l3.C2.C3.S1 -

l3.S1.S2.S3][ - C2.S3 - C3.S2, S2.S3 - C2.C3, 0, -l2.S2 - l3.C2.S3 - l3.C3.S2][ 0, 0, 0,1]



54/114



GVHD: Trn Xun Tin

rE =

l1.C1 + l2.C1.C2 + l3.C1.C2.C3 - l3.C1.S2.S3l1.S1 + l2.C2.S1 + l3.C2.C3.S1 - l3.S1.S2.S3

- l2.S2 - l3.C2.S3 - l3.C3.S2


vE =

- dq1.(l1.S1 + l2.C2.S1 + l3.C2.C3.S1 - l3.S1.S2.S3) - dq2.(l2.C1.S2 + l3.C1.C2.S3 +l3.C1.C3.S2) - dq3.(l3.C1.C2.S3 + l3.C1.C3.S2)

dq1.(l1.C1 + l2.C1.C2 + l3.C1.C2.C3 - l3.C1.S2.S3) - dq2.(l2.S1.S2 + l3.C2.S1.S3 +

l3.C3.S1.S2) - dq3.(l3.C2.S1.S3 + l3.C3.S1.S2)

- dq2.(l3.cos(q2 + q3) + l2.C2) - dq3.l3.cos(q2 + q3)

aE =

dq1^2.l3.C1.S2.S3 - dq1^2.l1.C1 - dq1^2.l2.C1.C2 - dq2^2.l2.C1.C2 - ddq1.l2.C2.S1 -ddq2.l2.C1.S2 - dq1^2.l3.C1.C2.C3 - dq2^2.l3.C1.C2.C3 - dq3^2.l3.C1.C2.C3 -ddq1.l1.S1 + dq2^2.l3.C1.S2.S3 + dq3^2.l3.C1.S2.S3 + 2.dq1.dq2.l2.S1.S2 -ddq1.l3.C2.C3.S1 - ddq2.l3.C1.C2.S3 - ddq2.l3.C1.C3.S2 - ddq3.l3.C1.C2.S3 -ddq3.l3.C1.C3.S2 + ddq1.l3.S1.S2.S3 - 2.dq2.dq3.l3.C1.C2.C3 +2.dq1.dq2.l3.C2.S1.S3 + 2.dq1.dq2.l3.C3.S1.S2 + 2.dq1.dq3.l3.C2.S1.S3 +2.dq1.dq3.l3.C3.S1.S2 + 2.dq2.dq3.l3.C1.S2.S3ddq1.l1.C1 - dq1^2.l1.S1 - dq1^2.l2.C2.S1 - dq2^2.l2.C2.S1 + ddq1.l2.C1.C2 -ddq2.l2.S1.S2 - dq1^2.l3.C2.C3.S1 - dq2^2.l3.C2.C3.S1 - dq3^2.l3.C2.C3.S1 +dq1^2.l3.S1.S2.S3 + dq2^2.l3.S1.S2.S3 + dq3^2.l3.S1.S2.S3 - 2.dq1.dq2.l2.C1.S2 +ddq1.l3.C1.C2.C3 - ddq1.l3.C1.S2.S3 - ddq2.l3.C2.S1.S3 - ddq2.l3.C3.S1.S2 -ddq3.l3.C2.S1.S3 - ddq3.l3.C3.S1.S2 - 2.dq1.dq2.l3.C1.C2.S3 - 2.dq1.dq2.l3.C1.C3.S2

- 2.dq1.dq3.l3.C1.C2.S3 - 2.dq1.dq3.l3.C1.C3.S2 - 2.dq2.dq3.l3.C2.C3.S1 +2.dq2.dq3.l3.S1.S2.S3

dq2^2.l3.sin(q2 + q3) - ddq2.l2.C2 + dq3^2.l3.sin(q2 + q3) + dq2^2.l2.S2 -ddq2.l3.cos(q2 + q3) - ddq3.l3.cos(q2 + q3) + 2.dq2.dq3.l3.sin(q2 + q3)


55/114



GVHD: Trn Xun Tin

A1 =

[ C1, 0, -S1][ S1, 0, C1][ 0, -1, 0]

A2 =

[ C2, -S2, 0][ S2, C2, 0][ 0, 0, 1]

A3 =

[ C3, -S3, 0][ S3, C3, 0][ 0, 0, 1]

W10 =

[ 0, -dq1, 0][ dq1, 0, 0][ 0, 0, 0]

W21 =

[ 0, -dq2, 0][ dq2, 0, 0][ 0, 0, 0]

W32 =

[ 0, -dq3, 0][ dq3, 0, 0][ 0, 0, 0]


56/114



GVHD: Trn Xun Tin


w10 =

00

dq1

w21 =

00

dq2

w32 =

00

dq3

w1 =

00

dq1

w2 =

-dq2.S1dq2.C1dq1

w3 =


57/114



GVHD: Trn Xun Tin

- dq2.S1 - dq3.S1dq2.C1 + dq3.C1

dq1

a1 =

00

ddq1

a2 =

- ddq2.S1 - dq1.dq2.C1ddq2.C1 - dq1.dq2.S1ddq1

a3 =

- dq1.(dq2.C1 + dq3.C1) - ddq2.S1 - ddq3.S1ddq2.C1 - dq1.(dq2.S1 + dq3.S1) + ddq3.C1

ddq1


JT =

[ -S1.(l1 + l3.cos(q2 + q3) + l2.C2), -C1.(l3.sin(q2 + q3) + l2.S2), -l3.sin(q2 + q3).C1][ C1.(l1 + l3.cos(q2 + q3) + l2.C2), -S1.(l3.sin(q2 + q3) + l2.S2), -l3.sin(q2 + q3).S1][ 0, - l3.cos(q2 + q3) - l2.C2, -l3.cos(q2 + q3)]

JR =

[ 0, -S1, -S1][ 0, C1, C1][ 1, 0, 0]


58/114



GVHD: Trn Xun Tin

Jb =

[ -S1.(l1 + l3.cos(q2 + q3) + l2.C2), -C1.(l3.sin(q2 + q3) + l2.S2), -l3.sin(q2 + q3).C1][ C1.(l1 + l3.cos(q2 + q3) + l2.C2), -S1.(l3.sin(q2 + q3) + l2.S2), -l3.sin(q2 + q3).S1][ 0, - l3.cos(q2 + q3) - l2.C2, -l3.cos(q2 + q3)][ 0, -S1, -S1][ 0, C1, C1][ 1, 0, 0]

H7

Bng D-H: di ai i Khp

1 1 L1 0 90 R2

20 L2 0 R

3 3 0 L3 0 R



59/114



GVHD: Trn Xun Tin

H1 =

; H2 =

H3 =

D3 =

[

]


rE=


vE=

[


60/114



GVHD: Trn Xun Tin

aE=

[

A1 = ; A2 =

; A3 =

=

; =

; =



61/114



GVHD: Trn Xun Tin

= ; =

; =

; =

; =

=

=

; =

=


JT=

[ ]

; JR =


62/114



GVHD: Trn Xun Tin

H8Bng D-H:

di ai i Khp1 1 L1 0 90 R2 2 0 L2 -90 R3 3 0 L3 0 R

Vit cc ma trn H1, H2, H3 :

[c1.c2.c3 - s1.s3, - c3.s1 - c1.c2.s3, -c1.s2, l2.c1.c2 - l3.s1.s3 + l3.c1.c2.c3]D3=[ c1.s3 + c2.c3.s1,c1.c3 - c2.s1.s3, -s1.s2, l2.c2.s1 + l3.c1.s3 + l3.c2.c3.s1 ]

[ c3.s2, -s2.s3, c2, l1 + l2.s2 + l3.c3.s2 ][ 0, 0, 0, 1 ]

Vit phng trnh th hin vtr im chp hnh cui


63/114



GVHD: Trn Xun Tin

Vit biu thc vn tc, gia tc tuyt i ca im chp hnh cuivE =- .( + . ) - .( +. ) -.( . + . . ).( . - . ) - .( . + . ) +. . + . . )

.(

.

+

) -

.

.

aE =

. . . - . . . - . . . 2 + . . . - . . . - . . . - . . . - . . . - . . . . - . . + . . . . + . . . . + 2. . . . . . - .l3. . . - . . . -. . . . - . . . . - . . . . + . . . . -2. .. . . . + 2. . . . . + 2. . . . . . .+2.

.

.

.

.

.

+ 2.

.

.

.

.

.

+ 2.

.

.

.

.

.

. . - . . - . . . - . . . + . . . - . . . -. . . . - . . . . - . . . . + . . . . + . . . . + . . . . - 2. . . . . . + . . . . -. . . . - . . . . - . . . . - . . . - . . . . - 2. . . . . . - 2. . .l3. . . -2. . . . . . - 2. . . l3. . . - 2. . . . . . + 2. .. . . .

.

.

-

.l3.sin(2 + 3) -

.

.sin(2 + 3) -

.

+

.

.cos(2 + 3) +

. .cos(2 + 3) - 2. . . .sin(2 + 3)


64/114



GVHD: Trn Xun Tin

;

;


; ;

; ;

;

;


JR=

[

]


65/114



GVHD: Trn Xun Tin

H9

Bng D-H:

di ai i Khp

1 1 L1 0 0 R2 2 0 L2 0 R3 3 0 L3 0 R


L1

L2

L3

1

2 3

z 0

x 0

z1

x1

z 2

x 2

z 3

x 3


66/114



GVHD: Trn Xun Tin



- 2 - 2 ( )1 2 2

2 2 2 2 2- ( )- ( )- ( - )- ( )2 12 1 2 3 123 1 2 3 3 123 1 2 3 2 12 1 2

2 2 2- ( ( )- (

1 3 123 2 12 2

12 3 123 1 2 1

1 3 123 2 3 3 12 1

3

3

3 2

)

l Cl C l S l C l S

l C l S l C S a l

l C

E

2 2 ) ( )-2 -2 ( )

2 3 2 12 1 2 1 2 2 12 3 123 1 2 1 3 2 3

0

l C l S l S


67/114



GVHD: Trn Xun Tin



68/114



GVHD: Trn Xun Tin


[

]


69/114



GVHD: Trn Xun Tin

H10

Bng D-H:

di ai i Khp1 1 0 L1 -90 R2 2 0 L2 -90 R3 3 0 L3 0 R


; ;



70/114



GVHD: Trn Xun Tin


( )

aE =

[

.l3.C1.S3

.l1.C1 -

.l2.C12 -

.l2.C12 -

.l3.S13 -

.l3.S13 -

.l2.C2.S1 - .l2.C1.S2 - .l1.S1 + .l3.C3.S1 - ..l3.C123 - .l3.C123 - .l3.C123 + 2. . .l3.C13 + 2. . .l2.S12 - .l3.C23.S1 - .l3.C13.S2 -.l3.C12.S3 + 2. . .l3.C3.S12 + 2. . .l3.C2.S13 + 2. . .l3.C1.S23][.l1.C1 - .l1.S1 - .l2.C2.S1 - .l2.C2.S1 + .l3.C1.S3 + .l3.C1.S3 +.l2.C12 - .l3.C13 - .l2.S12 + .l3.S13 - .l3.C23.S1 - ..l3.C23.S1 - ..l3.C23.S1 - 2. . .l2.C1.S2 + 2. . .l3.C3.S1 + .l3.C123 - .l3.C3.S12 -.l3.C2.S13 - 2. . .l3.C13.S2 - 2. . .l3.C12.S3 + 2. . .l3.S123][ .l2.S2 - .l2.C2 + .l3.C3.S2 + .l3.C3.S2 - .l3.C23 + .l3.S23 +2. . .l3.C2.S3]

; ;

;

;

;


; ;


71/114



GVHD: Trn Xun Tin

;

;

;

; - .( .C1 - .S12) - .S1 - .C1.S2 - ..C12 ][ .C1 - .( .S1 + .C1.S2) - .S12 - ..C2.S1][ - .C2 + ..S2 ]


[


72/114



GVHD: Trn Xun Tin

H11 di ai i Khp

1 1 0 0 -90 R2 -90 L1 L2 -90 P3 3 0 L3 0 R

Bng D-H:


L1

L2

L3

x0

z1

x1

z0

Z2

X2

Z3

X3


73/114



GVHD: Trn Xun Tin




aE =. - . - . - 2. . . - . - . +. + . + 2. . . . - 2. . . - . - . + . + . - . + . + 2. . .

.(

.

+

.

)

; ;


74/114



GVHD: Trn Xun Tin

;

;


; ;

; ;

;

;

;

;


; JR=

[

]


75/114



GVHD: Trn Xun Tin

l1

l3

1

3

o2

o1

o0

o3

Z2

Z3

X3

Z1

Z0

X0

X1X2

Z1

l2

H12

Bng D-H:

di ai i Khp1 1 0 0 0 R2 L1 L2 -90 P3 3 0 L3 0 R



76/114



GVHD: Trn Xun Tin



[

1 3

1 3

1l 3

]

aE =

1 .l2.C1 -2

1

.l2.S1 -

2

1

.l3.C3.S1 -

2

3

.l3.C3.S1 + 1 .l3.C1.C3 -

3

.l3.S13 - 2. 1 .

3 .l3.C1.S3


77/114



GVHD: Trn Xun Tin

1 .l2.S1 +2

1

.l2.C1 +

2

1

.l3.C1.C3 +

2

3

.l3.C13 + 1 .l3.C3.S1 +

3

.l3.C1.S3 - 2. 1 .

3 .l3.S13. l3.S3.2

3

+ 1l - 3 .l3.C3

A3 =

=

Vitbiu thc vn tc gc, gia tc gc a phng v tuyt i ca tng khu


78/114



GVHD: Trn Xun Tin


79/114



GVHD: Trn Xun Tin

1


;

[

]

H13Bng D-H:

di ai i Khp1 1 0 L1 0 R2 L2 0 90 P3 3 0 L3 0 R


80/114



GVHD: Trn Xun Tin


;

;

Vit phng trnh thhin v tr im chp hnh cui


aE =

. - . - . + . - . + + . +2. . . . - . - . - . - . - . +2. . . . . . + .


81/114



GVHD: Trn Xun Tin

;

;

; ;


;

;

; ;

;

;


; JR=

[

]


82/114



GVHD: Trn Xun Tin

H14

Bng D-H: di ai i Khp

1 1 0 L1 0 R2 2 0 0 -90 R3 L2 L3 0 P

Vit cc ma trn H1, H2, H3:

Vit phng trnh th hin v tr im chp hnh cui:


83/114



GVHD: Trn Xun Tin

Vit biu thc vn tc, gia tc tuyt i ca im chp hnh cui:

aE =[ - - - - + - -2 -2 - - - - - 2 - 2 ][ + - - - - - + +

- 2

- 2

-

-

- 2

- 2

]

[ 0 ]


84/114



GVHD: Trn Xun Tin

Vit biu thc vn tc gc, gia tc gc a phng v tuyt i ca tng khu:


85/114


86/114



GVHD: Trn Xun Tin

S khp n = 3

i 1 2 3 4 5pi 0 0 0 0 3

Vy s bc t do ca tay my l:ADCT:

5

1

.6i

ipinw

Trong : w s bc t do ca tay myns khp ca tay my.pis khp loi i (i 1,2,,5 s khp hay s bc t do b hn ch )

Thay s: 33.53.6 w (bc t do)

Xy dng cc h ta cho cc khu

Lp bng DH:

di ai i Khp1 1 0 0 -90 R2 l1 l2 0 P3 3 0 l3 0 R

Vt cc a trnH1, H2, H3

X0, X1

Z1

X2

Z2

Z0

L

1

L2 L3

Z3

X3


87/114



GVHD: Trn Xun Tin

=

Vt phn trnh th hn v tr chp hnh cu

Vt bu thc vn tc a tc tuyt ca chp hnh cu

aE =[ l1 - l1 - l2 - l2 - 2 - l3 - l3 -

l3

-

l3

+ 2

]

[

+

l2

- 2

-

l1

-

l1

-

l2

-

l3

-

l3

+

l3 - l3 - 2 2l3 ][l3( - )]


88/114



GVHD: Trn Xun Tin

Vt bu thc vn tc c a tc c a phn v tuyt ca tn hu

ng hc ngc:

Xy dn v tnh tn cc phn t trn a trn Jacb

JR=

[

]


89/114



GVHD: Trn Xun Tin

H16Bng D-H:

di ai i Khp1 1 0 L1 -90 R2 2 0 0 90 R3 L2+L3 0 0 P



90/114



GVHD: Trn Xun Tin



( ) aE =

.( . - . ) - .( + ) - .( .( + )+ . + . ) - .( . . - . + . . )+

.

+

.

)

.( . + . - . . )) + .( + ) + .( . + . ) + .( . - .( + . )) + . . .( +)) + . + . . - .( . + . )) - . . - .


91/114


92/114



GVHD: Trn Xun Tin


JR=

[

]


93/114



GVHD: Trn Xun Tin

H17

Bng D-H:

di ai i Khp1 L1 0 -90 P2 L2 0 90 R3 3 0 L3 0 R



94/114



GVHD: Trn Xun Tin




95/114



GVHD: Trn Xun Tin



96/114



GVHD: Trn Xun Tin



97/114



GVHD: Trn Xun Tin

[

]

H18

Bng D-H:

di ai i Khp1 1 L1 0 -90 R2 L2 0 90 P


98/114



GVHD: Trn Xun Tin

3 3 0 L3 0 R


H1 = ; H2 = ; H3 =

D3 =

[

]


rE = Vit biu thc vn tc, gia tc tuyt i ca im chp hnh cui

vE= l l

aE=

[

l

l

l l


99/114



GVHD: Trn Xun Tin

A1 =

; A2 =

; A3 =

= ; = ; =


=

;

=

;

=

;

= ; =

; =

=

;

=

;

=


JT = JR =


100/114



GVHD: Trn Xun Tin

Jb =

[

]

H19

di ai i Khp1 1 0 0 0 R2

L1 L2 0 P

3 3 0 L3 0 R

Bng D-H:

[ q1, 0, 0, 0]DH= [ 0, l1, l2, 0]

[ q3, 0, l3, 0]


[ c1, -s1, 0, 0]H1= [ s1, c1, 0, 0]

[ 0, 0, 1, 0][ 0, 0, 0, 1]


101/114


102/114



GVHD: Trn Xun Tin

0

A1 =

[ C1, -S1, 0][ S1, C1, 0][ 0, 0, 1]

A2 =

[ 1, 0, 0][ 0, 1, 0][ 0, 0, 1]

A3 =

[ C3, -S3, 0][ S3, C3, 0][ 0, 0, 1]

W10 =

[ 0, -dq1, 0][ dq1, 0, 0][ 0, 0, 0]

W21 =

[ 0, 0, 0][ 0, 0, 0]

[ 0, 0, 0]

W32 =

[ 0, -dq3, 0][ dq3, 0, 0]


103/114



GVHD: Trn Xun Tin

[ 0, 0, 0]


w10 =

00

dq1

w21 =

0

00

w32 =

00

dq3

w1 =

00

dq1

w2 =

00

dq1

w3 =


104/114



GVHD: Trn Xun Tin

00

dq1 + dq3

a1 =

00

ddq1

a2 =

00

ddq1

a3 =

00

ddq1 + ddq3


JT =

[ - l3.sin(q1 + q3) - l2.S1, C1, -l3.sin(q1 + q3)][ l3.cos(q1 + q3) + l2.C1, S1, l3.cos(q1 + q3)][ 0, 0, 0]

JR =

[ 0, 0, 0][ 0, 0, 0][ 1, 0, 1]


105/114



GVHD: Trn Xun Tin

Jb =

[ - l3.sin(q1 + q3) - l2.S1, C1, -l3.sin(q1 + q3)][ l3.cos(q1 + q3) + l2.C1, S1, l3.cos(q1 + q3)][ 0, 0, 0][ 0, 0, 0][ 0, 0, 0][ 1, 0, 1]

H20

Bng D-H:

di ai i Khp

1 L0 L1 -90 P2 2 0 L2 -90 R3 3 0 L3 0 RVit cc ma trn H1, H2, H3

L0

L1

L2

L3

2 3

z 0

x 0

z1

x1z 2

x 2

z 3

x 3


106/114



GVHD: Trn Xun Tin



( )


107/114



GVHD: Trn Xun Tin



108/114



GVHD: Trn Xun Tin

;

;

;

; ;


;

[

]


109/114



GVHD: Trn Xun Tin

H1

di ai i Khp1 1 0 l1 0 R2 2 0 l2 -90 R3 3 0 l3 0 R

Bng D-H:

DH =

[ q1, 0, l1, 0][ q2, 0, l2, -/2][ q3, 0, l3, 0]


H1 =

[ C1, -S1, 0, l1.C1][ S1, C1, 0, l1.S1][ 0, 0, 1, 0][ 0, 0, 0,

H2 =[ C2, 0, -S2, l2.C2][ S2, 0, C2, l2.S2][ 0, -1, 0, 0][ 0, 0, 0, 1]

H3 =

[ C3, -S3, 0, l3.C3][ S3, C3, 0, l3.S3][ 0, 0, 1, 0][ 0, 0, 0, 1]

D3 =


110/114



GVHD: Trn Xun Tin

[ C3.(C1.C2 - S1.S2), -S3.(C1.C2 - S1.S2), - C1.S2 - C2.S1, l1.C1 + l3.C3.(C1.C2 -S1.S2) + l2.C1.C2 - l2.S1.S2][ C3.(C1.S2 + C2.S1), -S3.(C1.S2 + C2.S1), C1.C2 - S1.S2, l1.S1 + l3.C3.(C1.S2 +C2.S1) + l2.C1.S2 + l2.C2.S1][ -S3, -C3, 0,-l3.S3][ 0, 0, 0,1]


rE =

l1.C1 + l3.C3.(C1.C2 - S1.S2) + l2.C1.C2 - l2.S1.S2l1.S1 + l3.C3.(C1.S2 + C2.S1) + l2.C1.S2 + l2.C2.S1-l3.S3


vE =2

1

- 1 .(l2.S12 + l1.S1 + l3.S12.C3) -

2 .(l2.S12 + l3.S12.C3) -

3 .l3.C12.S3

1 .(l2.C12 + l1.C1 + l3.C12.C3) + 2 .(l2.C12 + l3.C12.C3) -

3 .l3.S12.S3.

3

- .l3.C3

aE =

( 3

.l3.s(q1 + q2 - q3))/2 - (2

2

.l3.c(q1 + q2 - q3))/2 - (

2

3

.l3.c(q1 + q2 - q3))/2 -

1 .l1.S1 -

2

1

.l2.C12 -

2

2

.l2.C12 - ( 1 .l3.S123)/2 - (

2 .l3.S123)/2 - (

3

.l3.S123)/2 -2

1

.l1.C1 - (

2

1

.l3.C123)/2 - (

2

2

.l3.C123)/2 - (

2

3

.l3.C123)/2 - (

1 .l3.sin(q1 + q2 - q3))/2 - (

2 .l3.sin(q1 + q2 - q3))/2 - (

2

1

.l3.cos(q1 + q2 -


111/114



GVHD: Trn Xun Tin

q3))/2 - 1 .l2.S12 -

2 .l2.S12 -

1 .

2 .l3.cos(q1 + q2 - q3) +

1 .

3

.l3.cos(q1 + q2 - q3) + 3 .

2 .l3.cos(q1 + q2 - q3) - 2.

1

. 2 .l2.C12 -

1

. 2 .l3.C123 -

1 .

3 .l3.C123 -

3 .

2 .l3.C123

1

.l1.C1 - (2

1

.l3.sin(q1 + q2 - q3))/2 - (

2

2

.l3.sin(q1 + q2 - q3))/2 - (

2

3

.l3.sin(q1 + q2 - q3))/2 -2

1

.l2.S12 -

2

2

.l2.S12 + ( 1 .l3.C123)/2 + (

2

.l3.C123)/2 + ( 3

.l3.C123)/2 -2

1

.l1.S1 + ( 1 .l3.cos(q1 + q2 - q3))/2 + (

2 .l3.cos(q1 + q2 - q3))/2 - (

3 .l3.cos(q1 + q2 - q3))/2 - (

2

1

.l3.S123)/2 - (

2

2

.l3.S123)/2 - (

2

3

.l3.S123)/2 + 1 .l2.C12 +

2 .l2.C12 -

1

. 2

.l3.sin(q1 + q2 - q3) + 1 .

3 .l3.sin(q1 + q2 - q3) +

3 .

2 .l3.sin(q1 + q2 -

q3) - 2. 1 .

2 .l2.S12 -

1 .

2 .l3.S123 -

1 .

3 .l3.S123 -

3 .

2 .l3.S123

l3.( 23

.S3 - 3 .C3)

A1 =

[ C1, -S1, 0][ S1, C1, 0][ 0, 0, 1]

A2 =

[ C2, 0, -S2][ S2, 0, C2][ 0, -1, 0]

A3 =

[ C3, -S3, 0][ S3, C3, 0][ 0, 0, 1]


112/114



GVHD: Trn Xun Tin

W10 =

[ 0, - 1 , 0]

[ 1 , 0, 0]

[ 0, 0, 0]

W21 =

[ 0, - 2 , 0]

[ 2 , 0, 0][ 0, 0, 0]

W32 =

[ 0, - 3 , 0]

[ 3 , 0, 0]

[ 0, 0, 0]


w10 =

00

1

w21 =

00

2

w32 =

0


113/114



GVHD: Trn Xun Tin

0

3

w1 =

00

1

w2 =

00

1 +

2

w3 =

- 3 .(C1.S2 + C2.S1)

3 .(C1.C2 - S1.S2)

1 + 2

a1 =

00

1

a2 =

00


114/114


1 +

2

a3 =

-

3 .S12 -

1 +

3

.C12 - 2 +

3 .C12

3 .C12 -

1 +

3

.S12 - 2 +

3 .S12

1 +

2


JT =

[ - l2.S12 - l1.S1 - l3.S12.C3, -S12.(l2 + l3.C3), -l3.C12.S3][ l2.C12 + l1.C1 + l3.C12.C3, C12.(l2 + l3.C3), -l3.S12.S3][ 0, 0, -l3.C3]

JR =

[ 0, 0, -S12][ 0, 0, C12][ 1 1 0]

Documents

De Cuong Thi Robot Cong Nghiep