Thiết Kế Cánh Tay Robot 2 Bậc Tự Do

8/9/2019 Thit K Cnh Tay Robot 2 Bc T Do

1/82


2/82

n tt nghip Thit kbiu khin trt cho Robot 2 bc tdo

2

hin i. Ngy nay, chuyn ngnh khoa hc nghin cu v Robot

Robotics tr thnh mt lnh vc rng trong khoa hc, bao gm cc

vn cu trc ccu ng hc, ng lc hc, lp trnh quo, cm bin

tn hiu, iu khin chuyn ng v.v

I.1.2.Phn loi tay my Robot cng nghip:

Ngy nay, khi ni n Robot thng ta hay hnh dung ra mt cch

my mc tng tcon ngi, c khnng sdng cng c lao ng

thc hin cc cng vic thay cho con ngi, thm ch c thtnh ton hay

c khnng hnh ng theo ch.

Trong thc tin k thut, khi nim Robot hin i c hiu kh

rng, m theo Robot l tt ccc hthng kthut c khnng cm

nhn v x l thng tin cm nhn c, sau a ra hnh x thch

hp. Theo cch hiu ny, cc hthng xe thnh, hay thm ch mt thit

bxy dng c trang bcm bin thch hp nhCamera, cng c gi l

Robot. Cc khi nim nh Hexapod, Parallel Robot, Tripod, Gait Biped,

Manipulator Robocar hay Mobile Robot nhm chvo cc hthng Robot

khng cn gn lin vi cc hnh dung ban u ca con ngi.

Trong ni dung n ch nhm vo i tng Robot cng nghip

(RBCN), thc cht l mt thit b tay my (Handling Equipment). Cngngh tay my (Handling Technology) l cng ngh ca dng thit b k

thutc khnng thc hin cc chuyn ng theo nhiu trc trong khng

gian, tng tnhcon ngi.

Vcbn c thphn thit btay my (hnh 1.1) thnh 2 loi chnh :

iu khin (K) theo chng trnh hay K thng minh :


3/82


3

Handling

Equipments

Hnh 1.1 :Phn loi thit btay my

+ Loi K theo chng trnh gm 2 h:

Chng trnh cng: Cc thit bbc d, xp t c chng trnh

hot ng cnh. Ta hay gp h ny trong cc h thng kho hin i.

Chng c rt t trc chuyn ng v chthu thp thng tin vqung ng

qua cc tip im hnh trnh. Ta khng thK chng theo mt quo

mong mun.

Chng trnh linh hot : L hRobot m ngi sdng c kh

nng thay i chng trnh K chng tutheo i tng cng tc. Ta hay

gp chng trong cc cng on nhhn, sn hay lp rp ca cng nghipt. Trong hnh 1.1 ta gi l Robot cng nghip.

+ Loi K thng minh c 2 kiu chnh :

Manipulator: L loi tay my c K trc tip bi con ngi,

c khnng lp li cc chuyn ng ca tay ngi. Bn cht l dng thit

iu khin

thng minh

iu khin theo

chng trnh

Chng trnh

cng

Chng trnh

linh hot

My bc d,

xp t

Robot cng

nghip

Manipulators,

Telemanipulators


4/82


4

b h tr cho skho lo, cho tr tu, cho h thng gic quan (Complex

Sensorics) v kinh nghim ca ngi sdng. Hay c sdng trong cc

nhim vcn chuyn ng phc hp c tnh chnh xc cao, hay mi trng

nguy him cho sc kho, mi trng kh tip cn v.v...

Telemanipulator: L loi Manipulator c iu khin t xa v

ngi K phi sdng hthng Camera quan st mi trng sdng.

Theo tiu chun chu u EN775 v VDI 2860 ca c c th

hiu Robot cng nghip l mt Automat sdng vn nng to chuyn

ng nhiu trc, c khnng lp trnh linh hot cc chui chuyn ng v

qung ng (gc) to nn chuyn ng theo qu o. Chng c th

c trang bthm cc ngn (Grippe), dng chay cc cng cgia cng

v c ththc hin cc nhim vca i tay (Handling) hay cc nhim vgia cng khc

Nhvy, RBCN khc cc loi tay my cn li 2 im chnh l s

dng vn nng vkhnng lp trnh linh hot.

I.2. ng dng ca Robot cng nghip :

I.2.1.Mc tiu ng dng Robot cng nghip :

Mc tiu ng dng Robot cng nghip nhm nng cao nng sutdy truyn 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. iu

xut pht tnhng u im cbn ca Robot l :

-Robot c ththc hin mt quy trnh thao tc hp l bng hoc hn

ngi thlnh nghmt cch n nh trong sut thi gian di lm vic. Do

Robot gip nng cao cht lng v khnng cnh tranh ca sn phm.

-Khnng gim gi thnh sn phm do ng dng Robot l v gim

c ng kchi ph cho ngi lao ng.

-Robot gip tng nng sut dy chuyn cng ngh.

-Robot gip ci thin iu kin lao ng. l u im ni bt nht

m chng ta cn quan tm. Trong thc tsn xut c rt nhiu ni ngi

lao ng phi lm vic trong mi trng nhim, m t, nng nc. Thm


5/82


5

ch rt c hi n sc khov tnh mng nhmi trng ho cht, in t,

phng x

I.2.2.Cc lnh vc ng dng Robot cng nghip :

Robot cng nghip c ng dng rt rng ri trong sn xut, xinc nu ra mt slnh vc chyu :

-Knghc

-Gia cng p lc

-Cc qu trnh hn v nhit luyn

-Cng nghgia cng lp rp

-

Phun sn, vn chuyn hng ho (Robocar)I.2.3. Cc xu thng dng Robot trong tng lai :

- Robot ngy cng thay thnhiu lao ng

- Robot ngy cng trln chuyn dng

- Robot ngy cng m nhn c nhiu loi cng vic lp rp

- Robot di ng ngy cng trln phbin

- Robot ngy cng trln tinh khn

I.2.4. Tnh hnh tip cn v ng dng Robot cng nghip Vit

Nam :

Trong giai on trc nm 1990, hu nhtrong nc hon ton cha

du nhp vk thut Robot, thm ch cha nhn c nhiu thng tin k

thut v lnh vc ny. Tuy vy, vi mc tiu chyu l tip cn lnh vc

mi mny trong nc c trin khai cc ti nghin cu khoa hc cp

nh nc: ti 58.01.03 v 52B.03.01.

Giai on tip theo tnm 1990 cc ngnh cng nghip trong nc

bt u i mi. Nhiu cs nhp ngoi nhiu loi Robot cng nghip

phc vcc cng vic nh: tho lp dng c, lp rp linh kin in t, hn

vt xe my, phun phcc bmt


6/82


6

Mt s kin ng ch l thng 4 nm 1998, nh my

Rorze/Robotech bc vo hot ng khu cng nghip Nomura Hi

Phng. y l nh my u tin Vit Nam chto v lp rp Robot.

Nhng nm gn y, Trung tm nghin cu k thut Tng ha,

Trng i hc Bch Khoa H Ni, nghin cu thit kmt kiu Robot

mi l Robot RP. Robot RP thuc loi Robot phng sinh (bt chc ccu

tay ngi). Hin nay chto 2 mu: Robot RPS-406 dng phun men

v Robot RPS-4102 dng trong cng nghbmt.

Ngoi ra Trung tm cn ch to cc loi Robot khc nh: Robot

SCA mini dng dy hc, Robocar cng nghip phc v phn xng,

Robocar ch thp cho ngi tn tt Bn cnh cn xy dng cc

thut ton mi iu khin Robot, xy dng thvin cc m hnh caRobot trn my tnh

I.3.Cu trc ca Robot cng nghip:

I.3.1.Cc bphn cu thnh Robot cng nghip :

Trn hnh 1.2 gii thiu cc bphn chyu ca Robot cng nghip:

Tay mygm cc bphn: 1 t cnh hoc gn lin vi xe di

ng 2, thn 3, cnh tay trn 4, cnh tay di 5, bn kp 6.


7/82


7

Hnh 1.2: Cc bphn cu thnh Robot cng nghip

Hthng truyn dn ngc thl ckh, thukh hoc in kh: l

bphn chyu to nn schuyn dch cc khp ng.Hthng iu khinm bo shot ng ca Robot theo cc thng

tin t trc hoc nhn bit trong qu trnh lm vic.

H thng cm bin tn hiu thc hin vic nhn bit v bin i

thng tin vhot ng ca bn thn Robot (cm bin ni tn hiu) v ca

mi trng, i tng m Robot phc v(cm bin ngoi tn hiu).

I.3.2.Bc tdo v cc tosuy rng :

I.3.2.1.Bc tdo :

Robot cng nghip l loi thit btng nhiu cng dng. Ccu

tay my ca chng phi c cu to sao cho bn kp givt kp theo mt

hng nht nh no v di chuyn ddng trong vng lm vic. Mun

vy ccu tay my phi t c mt sbc tdochuyn ng.

Thng thng cc khu ca ccu tay my c ni ghp vi nhau

bng cc khp quay hoc khp tnh tin. Gi chung chng l khp ng.

Cc khp quay hoc khp tnh tin u thuc khp ng hc loi 5.

Cng thc tnh sbc tdo :

5

i1

W= 6n - ip (1.1)

vi n : skhu ng

Pi : skhp loi i

V d: Tay my c 2 khp quay nhhnh v1.3 :

Skhu ng n = 2

Khp quay l khp loi 5 .


8/82


8

Do W = 6.2 ( 5.1 + 5.1) = 2 bc tdo

Hnh 1.3: Tay my 2 khp quay

I.3.2.2. Tosuy rng :

Cc cu hnh khc nhau ca ccu tay my trong tng thi im xc

nh bng cc dch chuyn gc hoc dch chuyn di ca cc khp

quay hoc khp tnh tin.

Cc dch chuyn tc thi , so vi gi trban u no ly lm

mc tnh ton, c gi l cc to suy rng (generalized joint

coordinates). y ta gi chng l cc bin khp(tosuy rng) ca c

cu tay my v biu thbng :

(1 )Si i i iiq = +

(1.2)

vi

=

1,i vi khp quay

0,i vi khp tnh tini

i - dch chuyn gc ca cc khp quay

Si - dch chuyn tnh tin ca cc khp tnh tin

I.3.3.Nhim vlp trnh iu khin Robot:

I.3.3.1. nh vv nh hng ti im tc ng cui :

Khu cui cng ca tay my thng l bn kp (gripper) hoc l

khu gn lin vi dng cthao tc (tool). im mt ca khu cui cng l

im ng quan tm nht v l im tc ng ca Robot ln i tc v

c gi l im tc ng cui (end-effector). Trn hnh 1.4 im E lim tc ng cui.


9/82


9

Hnh 1.4:nh vv nh hng ti im tc ng cui

Chnh ti im tc ng cui E ny cn quan tm khng nhng v

tr n chim trong khng gian lm vic m c hng tc ng ca khu

cui . Vtr ca im E c xc nh bng 3 toxE, yE, zEtrong h

trc to cnh. Cn hng tc ng ca khu cui c th xc nh

bng 3 trc xn,yn, zngn lin vi khu cui ti im E, hoc bng 3 thng

sgc ,, no .

I.3.3.2. Lp trnh iu khin Robot cng nghip :

Trn hnh 1.5 m t1 slp trnh iu khin Robot cng nghip.

Khi robot nhn nhim vthc hin mt quy trnh cng nghno , v d

im tc ng cui E phi bm theo mt hnh trnh cho trc. Quo

hnh trnh ny thng cho bit trong htocc x0, y0, z0cnh.

mi vtr m im E i qua xc nh bng 3 tocnh xE, yE, zE v 3

thng sgc nh hng ,, . Tcc thng strong htocc

tnh ton cc gi trbin khp qitng ng vi mi thi im t. l ni

dung ca bi ton ng hc ngc strnh by trong chng II.


10/82


10

Hnh 1.5:Slp trnh iu khin

I.4. Cc php bin i ton hc cho Robot :

I.4.1.Bin i todng Ma trn:I.4.1.1. Vector im v tothun nht :

Vector im (point vector) dng m tvtr ca im trong khng

gian 3 chiu.

Trong khng gian 3 chiu, mt im M c thc biu din bng

nhiu vector trong cc hto(coordinate frame) khc nhau:

Trong htooixiyiziim M xc nh bng vector ri:

i ( , , )rT

xi yi zir r r= (1.3)

v cng im M trong htoojxjyjzjc m tbi vector rj:

j ( , , )rT

xj yj zjr r r= (1.4)

Quo trong h tocc(xE,yE,zE,,,)

Quo trong h tocc(xE,yE,zE,,,)

Chng trnh iu khin

Htrng chp hnh

Htrng chp hnhROBOT

My tnh

q1

q2

Cc gin Bin iqi(t)


11/82


11

K hiu ( )Tl biu thphp chuyn v(Transportation) vector hng

thnh vector ct.

Hnh 1.6:Biu din 1 im trong khng gian

Vector ( , , )rT

x y zr r r= trong khng gian 3 chiu, nu c bsung

thm mt thnh phn th4 v thhin bng 1 vector mrng :

( , , )x y z

r r r r =% (1.5)

th l cch biu din vector im trong khng gian to thun

nht (homogeneous coordinate).

n gin c thbqua k hiu ( ) i vi vector mrng (1.5)

Cc tothc ca vector mrng ny vn l:

x

x

rr

= y

y

rr

= z

z

rr

= (1.6)

Khng phi duy nht c mt cch biu din vector trong khng gian

ta thun nht, m n ph thuc vo gi trca . Nu ly = 1 th

cc ta biu din bng toc thc. Trong trng hp ny vector m

rng c vit l:

( , , )T

x y zr r r r= (1.7)

Nu ly 1 th cc tobiu din gp ln tothc, nn

c thgi lhstl. Khi cn biu din sthay i tokm theo

th c sbin dng tlth dng 1.

yj

xi

xj

zj

rjri

yiOi

Mzi


12/82


12

I.4.1.2.Quay htodng Ma trn 3x3:

Trc ht thit lp quan hgia 2 htoXYZ v UVW chuyn

ng quay tng i vi nhau khi gc O ca 2 hvn trng nhau (hnh 1.7)

Hnh 1.7:Cc hto

Gi (ix, jy, kz) v (iu, jv, kw) l cc vector n vchphng cc trc

OXYZ v OUVW tng ng.

Mt im M no c biu din trong h to OXYZ bng

vector:

rxyz=( rx,ry,rz)T (1.8)

cn trong htoOUVW bng vector:ruvw= ( ru,rv,rw)

T (1.9)

Nhvy :

r = ruvw= ruiu+ rvjv+ rwkw

r = rxyz= rxix+ ryjy+ rzkz (1.10)

T ta c

.

.

.

x u v wx x u x x wv

y u v wu wy y y v y

z u v wz z u z z wv

r

r

r

ji i i i i k r r r r

j j j j ji kr r r r

jk k i k k k r r r r

= = + + = = + +

= = + +

(1.11)

Hay vit di dng ma trn:

U

YV

MW

Z

X


13/82


13

.

x u x x wvx u

y vu wy y v y

z wz u z z wv

ji i i i k r rj j j ji kr r

r rjk i k k k

=

(1.12)

Gi R l Ma trn quay (rotation) 3x3 vi cc phn t l tch vhng 2 vector chphng cc trc tng ng ca 2 htoOXYZ v

OUVW.

Vy (1.12) c vit li l:

1

.

.

xyz uvw

uvw xyz

R

R

r r

r r

=

= (1.13)

I.4.1.3.Bin i Ma trn dng tothun nht:

By githit lp quan hgia 2 hto: htoojxjyjzjsang h

tomi oixiyizi. Chng khng nhng quay tng i vi nhau m tnh

tin cgc to: gc ojxc nh trong hxiyizibng vector p:

p=(a,-b,-c,1)T (1.14)

Gi sv tr ca im M trong h toxjyjzjc xc nh bng

vector rj:

rj= (xjyjzj,1)T (1.15)

v trong htoxiyiziim M c xc nh bng vector ri:

ri= (xiyizi,1)T (1.16)

Thnh (1.8) c thddng thit lp mi quan hgia cc to:

cos sin

sin cos

1

i j j

j ji j

i j jj

i j

a

b

c

x x t

y ytz

y tz z

t t

= +

= = + = =

(1.17)


14/82


14

Hnh 1.8: Cc hto

Sp xp cc hsng vi xj,yj,zjv tjthnh mt ma trn:

1 0 00 cos sin

0 sin cos

0 0 0 1

ij

a

b

cT

=

(1.18)

v vit phng trnh bin i tonhsau:

ri= Tijrj (1.19)

Ma trn Tijbiu thbng ma trn 4x4 nhphng trnh (1.18) v gil ma trn thun nht. N dng bin i vector m rng th to

thun nht ny sang htothun nht kia.

I.4.1.4. ngha hnh hc ca Ma trn thun nht:

T (3.19) nhn thy ma trn thun nht 4x4 l mt ma trn gm 4

khi :

=

ij

1 0 0

0 os -sin -b

0 sin os

0 0 0 1

a

cT

c c (1.20)

yj

zi

xi

yi

c

oib

a

zjoj

xj


15/82


15

Hoc vit rt gn l:

ij

ij0 1

R pT

=

(1.21)

Trong :

ijR - ma trn quay 3x3

p ma trn 3x1 biu th3 toca im gc hto0jtrong h

tooi, xi, yi, zi

1x3 ma trn khng

1x1 ma trn n v

Nhvy ma trn thun nht 4x4 l ma trn 3x3 m rng, thm matrn 3x1 biu thschuyn dch gc tov phn ta44biu thhst

l.

Ddng nhn thy ma trnij

R chnh l ma trn quay 3x3, nu suy t

ma trn quay trong (1.12) sang trng hp hnh 1.8 ta c:

= =

i i i

ij ij i i i

i i i

cos(x , ) cos(x , ) cos(x , )

cos(y , ) cos(y , ) cos(y , )

cos(z , ) cos(z , ) cos(z , )

j j j

j j j

j j j

x y z

R a x y z

x y z

(1.22)

v cc gc cosin chphng ny u lin hn gc (hnh 1.8).

Nu ch vquan hgia 2 cp trc,v d, cos(xi,yj) = cos(yi, xj)

y ddng nhn c biu thc:

-1 T

ij ij ijR R R= = (1.23)

M ttng qut hn nu mt im M no c xc nh trong htothun nht UVW bng vectmrng ruvw, th trong htothun

nht XYZ im xc nh bng vector mrng rxyz:

Rxyz = T.ruvw (1.24)


16/82


16

Trong T l ma trn thun nht 4x4, c th vit khai trin

dng sau:

0 0 0 1

x x x x

y y y y

z z z z

n s a p

n s a p

T n s a p

=

(1.25)

hoc0 1

R pT

=

(1.26)

Ta tm hiu ngha hnh hc ca ma trn T. Nh trnh by khi

phn tch cc khi ca ma trn 4x4, ma trn 3x1 tng ng vi toim

gc ca htoUVW biu din trong hXYZ.Nu 2 gc totrng nhau th cc thnh phn ca ma trn 3x1

ny u l 0. Khi xt trng hp:

w (1,0,0,1)T

uvr =

tc l rxyz= iu

th ddng nhn thy ct thnht hoc vectnca ma trn (1.25)

chnh l cc toca vectchphng trc OU biu din trong hto

XYZ.

Tng tkhi xt cc trng hp

w (0,1,0,1)T

uvr =

vw

(0,0,1,1)Tuv

r =

cng i n nhn xt ct th2 (hoc vects) ng vi cc toca

vectchphng trc OV v ct th3 (hoc vecta) ng vi cc to

vector chphng trc OW.

Nhvy, ma trn thun nht T 4x4 hon ton xc nh vtr v nh

hng ca h toUVW so vi h toXYZ. l ngha hnh hc

ca ma trn thun nht 4x4.

I.4.2.Cc php bin i cbn:


17/82


17

I.4.2.1.Php bin i tnh tin:

T(1.18) hoc (1.25), biu thma trn thun nht khi chc bin i

tnh tin m khng c quay ( 0= ), ta c:

1 0 00 1 0

0 0 1

0 0 0 1

x

y

z

T

pp

p

=

= ( , , )p x y zp p pT (1.27)

l ma trn bin i tnh tin (Tranlation)

Gi u l vector biu din mt im trong khng gian cn dch

chuyn tnh tin:

( , , )Tu x y z =

vpl vector chhng v di cn dch chuyn

( , , )Tx y z

p p p p=

th vl vector biu din im totrong khng gian c tnh

tin ti:

v (pT= v ( , , ) uT

p x y zp p pT= (1.28)

I.4.2.2. Php quay quanh cc trc to:

T ma trn quay 3x3 trong biu thc (1.12) ta xy dng ma trn

( , )R x cho trng hp h toUVW quay quanh trc OX mt gc

no . Trong trng hp nyx ui i= :

1 0 0 0

0 cos sin 0( , )

0 sin cos 0

0 0 0 1

R x

=

(1.29)

Tng ng cho trng hp quay quanh trc OY mt gc :


18/82


18

cos 0 sin 0

0 1 0 0( , )

sin 0 cos 0

0 0 0 1

R y

=

(1.30)

v trng hp quay quanh trc OZ mt gc :

cos sin 0 0

sin cos 0 0( , )

0 0 1 0

0 0 0 1

R z

=

(1.31)

Ct th4 ca cc ma trn 4x4 trn c 3 phn tu bng 0 v y

khng c s tnh tin. Cc ma trn ny c gi l cc ma trn quay(rotation) cbn. Cc ma trn quay khc c thxy dng tcc ma trn c

bn ny.

CHNG II: HPHNG TRNH NG HC V NG LC HC

CA ROBOT CNG NGHIP:

II.1. Hphng trnh ng hc Robot :

II.1.1. t vn :

C cu chp hnh ca Robot thng l mt c cu h gm mt

chui cc khu (link) ni vi nhau bng cc khp (joints). Cc khp ng

ny l khp quay (R) hoc khp tnh tin (T). Robot c ththao tc linh

hot ccu chp hnh ca n phi c cu to sao cho im mt ca khu

cui cng m bo ddng di chuyn theo mt quo no , ng thi


19/82


19

khu ny c mt hng nht nh theo yu cu. Khu cui cng ny

thng l bn kp (griper), im mt ca n chnh l im tc ng cui

E (end-effector).

xt vtr v hng ca E trong khng gian ta gn vo n mt h

tong thn v gn vi mi khu ng mt htokhc, cn gn

lin vi gi mt htocnh. nh sk hiu cc hny t0 n

n bt u t gi cnh. Khi kho st chuyn ng ca Robot cn bit

nh vv nh hng ti im tc ng cui trong mi thi im. Cc

li gii ca bi ton ny c xc nh tnhng phng trnh ng hc

ca Robot. Cc phng trnh ny l m hnh ng hc ca Robot. Chng

c xy dng trn csthit lp cc mi quan hgia cc htong

ni trn so vi htocnh.II.1.2. Xc nh trng thi ca Robot tai im tc ng cui :

Trng thi ca Robot ti im tc ng cui hon ton xc nh

bng snh vv nh hng ti im tc ng cui .

Nh cp phn I.4.1.4 biu thsnh vv nh hng

bng ma trn trng thi cui TE:

= 0 0 0 1

x x x x

y y y y

E

z z z z

n s a p

n n a p T

n s a p (2.1)

Trong cc phn tca ma trn 3x1 l topx , py, pzca im

tc ng cui E. Mi ct ca ma trn quay 3x3 l mt vectn v ch

phng mt trc ca htong NSA (chnh l UVW) biu din trong

tocnh XYZ.

H togn lin vi bn kp ca Robot c cc vectn v ch

phng cc trc nhsau :

a - vector c hng tip cn (approach) vi i tc .


20/82


20

s - vector c hng ng trt (sliding) ng mbn kp .

n - vector php tuyn (normal).

II.1.3. M hnh ng hc :

II.1.3.1. Ma trn quan h:

Chn htocnh gn lin vi gi v cc htogn vi

tng khu ng. K hiu cc htony t0 n n, ktgi cnh tr

i.

Mt im bt k no trong khng gian c xc nh trong hto

thi bng bn knh riv trong htocnh x0, y0, z0c xc nh

bng bn knh vector r0 :

r0= A1A2Airi (2.2)

hoc r0= Tiri (2.3)

vi Ti= A1A2Ai, i= 1, 2, n (2.4)

Trong ma trn A1m tvtr hng ca khu u tin; ma trn

A2m tvtr v hng ca khu th2 so vi khu u; ma trn Aim t

vtr v hng ca khu thi so vi khu thi-1.

Nhvy, tch ca cc ma trn Ail ma trn Tim tvtr v hngca khu thi so vi gi trcnh. Thng k hiu ma trn T vi 2 chs:

trn v di. Chsdi chkhu ang xt cn ch s trn ch to

c dng i chiu. V d, biu thc (2.4) c thvit li l :

0 1

1i i iT T A T = = (2.5)

vi 12 3

...i i

T A A A= (2.6)

l ma trn m tvtr v hng ca khu thi so vi khu thnht.Trong k hiu thng bqua chstrn nu chs bng 0.

Denavit & Hartenberg xut dng ma trn thun nht 4x4 m t

quan hgia 2 khu lin tip trong ccu khng gian .

II.1.3.2. Bthng sDH :


21/82


21

Di y trnh by cch xy dng cc h tong i vi 2 khu

ng lin tip i v i+1. Hnh di y l trng hp 2 khp ng lin tip

l 2 khp quay.

Hnh 2.1: Cc htoi vi 2 khu ng lin tip

Trc ht xc nh b thng s c bn gia 2 trc quay ca khpng i+1 v i :

ail di ng vung gc chung gia 2 trc khp ng i+1 v i .

il gc cho gia 2 trc khp ng i+1 v i .

dil khong cch o dc trc khp ng i tng vung gc chung

gia trc khp ng i+1 v trc khp ng i ti ng vung gc

chung gia khp ng i v trc khp ng i -1.

il gc gia 2 ng vung gc chung ni trn.

Bthng sny c gi l bthng sDenavit Hartenberg (DH).

Bin khp(joint variable):

Nu khp ng i l khp quay th bin khp l i


22/82


22

Nu khp ng i l khp tnh tin th bin khp l di

k hiu thm bin khp dng thm du * v trong trng hp

khp tnh tin th aic xem l bng 0.

II.1.3.3. Thit lp hto:Gc ca htogn lin vi khu thi (gi l htothi) t

ti giao im gia ng vung gc chung (ai) v trc khp ng i+1.

Trng hp 2 trc giao nhau th gc h to ly trng vi giao

im . Nu 2 trc song song vi nhau th chn gc tol im bt k

trn trc khp ng i+1.

Trc zica htothi nm dc theo trc khp ng i+1.

Trc xi ca h to th i nm dc theo ng vung gc chung

hng t khp ng i n khp ng i+1. Trng hp 2 trc giao nhau,

hng trc xitrng vi hng vector tch zi x zi-1, tc l vung gc vi mt

phng cha zi, zi-1.

V d: Xt tay my c 2 khu phng nhhnh 2.2.

Hnh 2.2:Tay my 2 khu phng (vtr bt k)


23/82


23

Gn cc htovi cc khu nhhnh v:

- Trc z0, z1v z2vung gc vi mt tgiy.

- Htocnh l o0x0y0z0chiu x0hng to0n o1.

- Htoo1x1y1z1c gc o1t ti tm trc khp ng 2.

- H too2x2y2z2 c gc o2t ti tm trc khp ng cui

khu 2.

Bng thng sDH ca tay my ny nhsau :

Khu i i ai di

1 *1 0 a1 0

2*

2 0 a2 0

II.1.3.4. M hnh bin i :

Trn cs xy dng cc htovi 2 khu ng lin tip nh

trn trnh by. C th thit lp mi quan hgia 2 h to lin tiptheo 4 php bin i :

+ Quay quanh trc z1-1gc i.

+ Tnh tin dc trc zi-1mt on di.

+ Tnh tin dc trc xi-1( trng vi xi) mt on ai.

+ Quay quanh trc ximt gc i.

Bn php bin i ny c biu th bng tch cc ma trn thunnht sau

Ai= R(z,i).Tp(0,0,di).Tp(ai,0,0).R(x,i) (2.7)


24/82


24

Cc ma trn v phi phng trnh (2.7) tnh theo cc cng thc

(1.27),(1.29),(1.31). Sau khi thc hin php nhn cc ma trn ni trn, ta

c:

=

0

0 0 0 1

ii i i i i i

ii i i i i i

i

ii i

A

C S C S S C a

S C C C S S a

S C d (2.8)

Trong khp tnh tin : a = 0 .

II.1.3.5. Phng trnh ng hc :

Vi Robot c n khu, ma trn m tvtr v hng im cui E catay my c miu t:

Tn= A1A2An (2.9)

Mt khc, hto ti im tc ng cui ny c m tbng

ma trn TE. V vy hin nhin l:

TE= Tn (2.10)

Tc l ta c :

0 0 0 1

x x x x

y y y y

n

z z z z

n s a p

n n a p T

n s a p

=

(2.11)

Phng trnh (2.11) l phng trnh ng hc cbn ca Robot.

II.2. Tng hp chuyn ng Robot :

II.2.1. Nhim v:

Nhim v tng hp chuyn ng bao gm vic xc nh cc b li

gii qi(t), (i = 1,..., n), vi qil tosuy rng hoc l bin khp.


25/82


25

Bit quy lut chuyn ng ca bn kp, cn xc nh quy lut thay

i cc bin khp tng ng. l ni dung chnh ca vic tng hp qu

o chuyn ng Robot.

C thxem quo chuyn ng l tp hp lin tip cc vtr khc

nhau ca bn kp. Ti mi vtr trn quo cn xc nh bthng scc

bin khp qi. l ni dung ca bi ton ng hc ngc (inverse

kinematics problem) ca Robot.

II.2.2. Bi ton ng hc ngc :

Bi ton ng hc ngc c c bit quan tm v li gii ca n l

cschyu xy dng chng trnh iu khin chuyn ng ca Robot

bm theo quo cho trc.

Xut pht tphng trnh ng hc cbn (2.11) ta c :

Tn= A1A2An=

0 0 0 1

x x x x

y y y y

z z z z

n s a p

n n a p

n s a p

(2.12)

Cc ma trn Ail hm ca cc bin khp qi. Vector nh vbn kp

p= (px,py,pz)T

cng l hm ca qi. Cc vector n, s, al cc vector n vchphng cc trc ca htogn lin vi bn kp biu din trong h

tocnh XYZ. Cc vector ny vung gc vi nhau tng i mt nn

trong 9 thnh phn ca chng chtn ti c lp chc 3 thnh phn. Hai

ma trn v phi v v tri ca phng trnh (2.12) u l cc ma trn

thun nht 4x4. So snh cc phn ttng ng ca 2 ma trn trn ta c 6

phng trnh c lp vi cc n qi(i = 1, 2,...,n).

II.2.3. Cc phng php gii bi ton ng hc ngc :

Trng hp tng qut ta xt hphng trnh ng hc ca Robot c

n bc tdo.

V tri ca phng trnh (2.12) theo cc k hiu nh (2.4)-(2.6) c

thvit li nhsau:


26/82


26

.in i nT T T= (2.13)

Nhn 2 vca (2.13) vi 1iT ta c:

Ai-1...A2

-1A1-1Tn = Tn

i (2.14)

Kt hp (2.12) ta c:

n

iT =Ai-1...A2

-1A1-1

1000zzzz

yyyy

xxxx

pasn

pasn

pasn

(2.15)

vi i=1,...,n-1

ng vi mi gi trca i, khi so snh cc phn ttng ng ca 2ma trn biu thc (2.15) ta c 6 phng trnh tn ti c lp xc nh

bin khp qi.

II.3. ng lc hc Robot:

II.3.1.Nhim vv phng php phn tch ng lc hc Robot:

Nghin cu ng lc hc Robot l giai on cn thit trong vic

phn tch cng nh tng hp qu trnh iu khin chuyn ng. Trong

nghin cu ng lc hc Robot thng gii quyt 2 nhim vsau y :

+ Nhim vthnht l xc nh momen v lc ng xut hin trong

qu trnh chuyn ng. Khi quy lut bin i ca bin khp qi(t) xem

nh bit.

+ Nhim vthhai l xc nh cc sai sng. Lc ny phi kho

st cc phng trnh chuyn ng ca ccu tay my ng thi xem xt

cc c tnh ng lc ca ng ctruyn ng.

C nhiu phng php nghin cu ng lc hc Robot nhng

thng dng hn cl phng php Lagrange bc 2 v khi kt hp vi m

hnh ng lc hc kiu DH (Denavit-Hartenberg) ta sc cc phng

trnh ng lc hc dng vector ma trn, rt gn nhv thun tin cho

vic nghin cu gii tch v tnh ton trn my tnh.


27/82


27

Cc phng trnh ng lc hc Robot c thit lp da trn cs

phng trnh Lagrange bc 2:

Mi

i i

d L LF

dt q q

=

&

,i=1,...,n (2.16)

Trong :

L- hm Lagrange L = K - P (2.17)

K, P- ng nng v thnng ca ch.

FMi - ng lc, hnh thnh trong khp ng th i khi thc hin

chuyn ng.

qi- bin khp (tosuy rng)

iq& - o hm bc nht ca bin khp theo thi gian.

ng thi khi m tvtr gia 2 htothi v i-1 dng ma

trn thun nht Aihoc vit y hn l 1i

i . Dng ma trn ny c th

m tvtr trng thi trong htothi-1 ca mt im bt k thuc h

tothi.

Cc bin khp qil bcc thng sdch chuyn ca cc khp ngca Robot. V tr trng thi ca im tc ng cui ca Robot hon ton

c xc nh bi bbin khp qiny.

II.3.2.Vn tc v gia tc:

xy dng m hnh ng lc hc Robot dng phng trnh

Lagrange bc 2, cn bit vn tc ca im bt k trn tay my.

im M no trong htoi, xc nh bng vc tmrng iir:

i

ir= ( xi, yi , zi , 1 )T , (2.18)

K hiui

ir c ngha l im M cho bit trong h to i v c

biu th cng trong h to i. Cn khi dng k hiu ir0 th c ngha l

im M cho bit trong htoi, nhng c biu thtrong htox0,

y0, z0, tc l trong htocbn.


28/82


28

Nhtrc y, dng ma trn ii A1 m tvtr tng i gia h

tothi i vi htoi-1 v ma trn iA0 m tquan hgia h

tothi v htocbn.

Vy quan hgia ir0

v ii

r

1

c thbiu thnhsau :

ir0 = iA

0 iir (2.19)

vi iA0 = 1

0A 21A i

i A1 (2.20)

Ma trn ii A1 c tbiu thc (2.8):

i

i A1 =

1000

S0

aS

aSS

i

ii

iii

ii

iiiii

iiii

dC

SCS

SC

CCCC

(2.21)

Biu thc (2.21) l vit cho trng hp khp quay i, cn nu khp

ng l khp tnh tin th ai=0 v t(2.21) ta c :

i

i A1 =

1000

S0

0S

0SS

i

i

ii

ii

iiii

iii

dC

CS

SC

CCC

(2.22)

i vi khp quay th i l bin khp v i vi khp tnh tin th di

l bin khp.

Cc phn tkhc khng ca ma trniA

0 u l hm ca j, dj, j v

aj (j = 1 ,2 ,, i). Trong i, j li l thng sxc nh bng cu trc

c thca tay my. Do vy cc phn tny l hm ca bin khp qini

chung (qi j i vi khp quay v qidii vi khp tnh tin).

Vi phn biu thc (2.19) vi lu rng cc vect iir l khng ivi htothi v githit rng cc khu ca tay my l vt rn tuyt

i, ta c:

iV0 Vi= )()( 00 i

i

ii rAdt

dr

dt

d= =


29/82


29

=1

0 1 1 0 1 1 0 0

2 2... ... ... ...

i

i i i i i i i

i i i i i i i i i i A A A r A A A r A A r A r

+ + + +& & & &

i

ii

j j

ii r

q

AV

==1

00 (2.23)

o hm ca ma trn ii A1 i vi bin khp qic thddng xc

nh theo cng thc sau :

i

i

i

i

i ADdq

A 11-id = (2.24)

Trong i vi khp quay :

Di=

0000

00000001

0010

(2.25)

v i vi khp tnh tin :

Di=

0000

1000

0000

0000

(2.26)

Trong trng hp i = 1, 2,,n ta c :

)......( 11210

iijj

jj

i AAAAAAqq

A

=

(2.27)

Trong cc ma trn bn vphi chc Aj phthuc vo qj, do theo

(2.24) ta c :

ii

j

j

j

j

i AAdq

dAAAA

q

A1121

0...... =

(2.28)

vi Djtnh theo (2.25) hoc (2.26) :

iijj

j

i AADAAAq

A1121

0

...... = (2.29a)


30/82


30

Phng trnh (2.29a) m tsthay i vtr cc im ca khu thi

gy nn bi sdch chuyn ca khp ng thj .

K hiu v tri ca (2.29a) l Uijv n gin ho cch vit (2.29a)

nhsau :

0 1

1 ,

0,

=

>

j

j j i

ij

D A j iU

j i (2.29b)

vy cng thc (2.23) c thvit li l:

Vi= ij1

ii

j ij

q rU=

& (2.30)

Tip theo, tbiu thc (2.23) xc nh gia tc:

0 2

1 1 1

i i iii i i

s k iss s k

s s k

dV A Aa q q r

dt q q q q

= = =

= = +

& &&& (2.31)

II.3.3. ng nng tay my:

K hiu Ki l ng nng ca khu i ( i =1,2, , n) v dKi l ng

nng ca mt cht im khi lng dm thuc khu i:

2 2 21 1 11 1( ) ( )

2 2T

i i idK x y z dm Tr VV dm = + + =& & & (2.32)

Trong Tr l vt ca ma trn :

TrA = =

n

i

iia1

Ta c :

dKi=

i1

ir1 r 1

1

2

Ti

i

pip i r i pTr U q r U q r dm = =

& &

=i i

i 1 T T

p rip i i ir p 1 r 1

1Tr U r r U q q dm

2 = =

& &


31/82


31

=i i

i i T T

p rip i i ir p 1 r 1

1Tr U ( rdm r )U q q

2 = =

& & (2.33)

Nh ni trn ma trn Uijbiu thsthay i vtr ca cc im

thuc khu i gy nn bi sdch chuyn ca khp ng j. Ma trn ny unhnhau ti mi thi im thuc khu i v khng phthuc vo sphn

bkhi lng trn khu i, tc l khng phthuc vo dm. Cng vy, o

hm ca bin khp qitheo thi gian khng phthuc vo dm. Do vy ta c:

( )i i

i i T T

ri i ip i i ir pp 1 r 1

1K dK Tr U r r dm U q q

2 = = =

& & (2.34)

Phn trong ngoc n l ma trn qun tnh Jica khu i:

Ji=

=

dmdmzdmydmx

dmzdmzdmyzdmzx

dmydmyzdmydmyx

dmxdmzxdmyxdmx

dmrr

iii

iiiiii

iiiiii

iiiiii

T

i

i

i

i

2

2

2

. (2.35)

Nu dng Tenso qun tnh Iij:

,2ijij dmxxxIk

jik

= (2.36)

Vi cc chsi, j, k ly ln lt bng cc gi trxi , yi, zi, l cc

trc ca htoi, v ijl k hiu Cronecke, th ma trn Ji c thbiu th

dng sau:

xx yy zzixy xz i

xx yy zzxy yz i i

i

xx yy zzixz yz i

i ii i i ii

I I II I m x

2I I I

I I m y2J

I I II I m z2

m x m y m z m

+ + + =

+ +

(2.37)


32/82


32

yT1

i i iir x ,y ,z ,1

=

- bn knh vector biu din trng tm

ca khu thi trong htoi. Cng thc (2.37) vit thnh :

2 2 2 2 2i11 i22 i33ii12 i13

2 2 22 2i11 i 22 i33i12 i23 i

i 2 2 22 2 i11 i22 i33

ii13 i 23

i ii

k k k k k x2

k k kk k y

2Jk k k

k k z2

x y z 1

+ + + = +

(2.38)

y jkijki

K I

m

= v j = 1,2,3 ; k = 1, 2, 3

T

iiii

i

zyxr

=

1,,, - bn knh vc t biu din trng tm ca

khu thi trong htoi .

Vy, ng nng ca ton ccu tay my bng tng i sng nng

ca cc khu ng :

( )n n i i n i iT Tii ip i ir p r ip ir p r i 1 i 1 p 1r 1 i 1 p 1r 1

1 1K K Tr U J U q q Tr U U q q2 2

J= = = = = = =

= = = & & & & (2.39)

Lu rng, cc ma trn Ji(i=1,2,3,n) chph thuc vo sphn

bkhi lng ca khu i trong htoi m khng phthuc vo vtr v

vn tc. V thcn tnh ma trn Jich1 ln.

II.3.4 .Thnng tay my:

Thnng Pica khu i:Pi= - mi.g. ir

0 = -mi.g. ( )ii rA 10 (2.40)

i=1,2,,n

Trong


33/82


33

iir, ir

0 - bn knh vec tbiu din trng tm ca khu i trong h

tocbn .

g - vector gia tc trng trng, g = ( 0, 0 , -g, 0)

( gia tc trng trng g = 9,8062 m/s2

)Thnng ca ton ccu n khu ng :

( )n n i0

ii i ii 1 i 1

P P m g A r = =

= = . (2.41)

II.3.5.M hnh ng lc hc tay my:

xy dng m hnh ng lc hc tay my dng phng trnh

Lagrange bc 2 (phng trnh 2.16):

Mi

i

Fq

L

q

L

dt

d=

, I = 1,2, ,n

Phng trnh trn cho biu thc tnh ng lc FMi. l lc hoc m

men to nn bi ngun ng lc khp ng i thc hin chuyn ng

khu i.

Thay (2.39), (2.41) vo (2.16), cui cng ta c :

( ) ( )j j jn n n jT T

jMi r jk j ji jkm j ji j jik k mj 1 k 1 j 1 k 1 m 1 j 1

F T U J U q Tr U J U q q m gU r = = = = = =

= + && & &

i=1,2,,n (2.42)

Biu thc (2.42) c thvit gn li nhsau :

n n n

Mi ik k ikm k m ij 1 k 1 m 1

F D q h q q c= = =

= + + && & & (2.43)

Hoc l di dng ma trn :

MiF D(q)q h(q,q) c(q)= + +&& & (2.44)

Trong :

FM(t) vector (nx1) lc ng, to nn n khp ng :


34/82


34

[ ]TMnMMM tFtFtFtF )(),...,(),()( 21= (2.45)

q(t) vector (nx1) bin khp :

[ ]Tn tqtqtqtq )(),...,(),()( 21= (2.46)

q(t)& - vec t(nx1) tc thay i bin khp :

[ ]T

1 2 nq(t) q (t), q (t),...,q (t)=& & & & (2.47)

q(t)&& - vect(nx1) gia tc bin khp :

[ ]T

1 2 nq(t) q (t), q (t),...,q (t)=&& && && && (2.48)

D(q) Ma trn (nxn) , c cc phn tDiksau y :

( )=

=n

kij

T

jijjkik UJUTrD),max(

i,k =1,2, . n (2.49)

H(q, q& ) vect(nx1) lc ly tm v Coriolit

T1 2 nh(q,q) (h ,h ,...,h )=&

n n

i ikm k mk 1m 1

h h q q= =

= & & i=1,2,,n (2.50)

( )=

=n

mkij

T

jijikmikm UJUTrh),,max(

i,k,m = 1,2,,n (2.51)

C(q) vec t(nx1) lc trng trng.

( )Tncccqc ,...,,)( 21= =

=

n

j

j

j

jiii rgUmc1

(2.52)

II.3.6. ng lc hc ca ccu tay my 2 khu:

Trong phn ny dn ra v dminh ha xy dng m hnh nglc hc ca ccu tay my 2 khu ton khp (hnh 2.3) .

Nh chtrn hnh 2.3 cc trc Ziu trng phng vi cc trc

khp quay ng. Khi lng ca cc khu tng ng l m1, m2; Bthng

sDH ca tay my ghi trong bng sau :


35/82


35

Khu i i Ai di Khp

1 i* 0 1 0 R

2 i* 0 1 0 R

Hnh 2.3: Ccu tay my 2 khu

Cc ma trn ii A1 ( i =1,2) sl :

=

1000

0100

0

0

111

111

10 lSCS

lCSC

A

=

1000

0100

0

0

222

222

21 lSCS

lCSC

A

+

+

==

1000

0100

)(0

)(0

1121212

1121212

12

02

0 SSlCS

CClSC

AAA

vn nhtrc y dng cc k hiu sau :

Ci = cos i ; S i= sin i ; Cij= cos( i + j ) ; Sij= sin( i + j ) .

Theo (2.30), ta c :


36/82


36

+

=

==

=

0000

0000

0

0

1000

0100

0

0

0000

0000

0001

0010

111

111

111

111

10

11

10

11

lCSC

lSCS

lSCS

lCSC

ADA

U

Tng t, i vi U12, v U22:

+

+

=

+

+

==

=

0000

0000

)(0

)(0

0000

0100

)(0

)(0

0000

0000

0001

0010

1121212

1121212

1121212

1121212

20

21

20

21

CClSC

SSlCS

SSlCS

CClSC

ADA

U

=

==

=

00000000

0

0

00000100

0

0

00000000

0001

0010

10000100

0

0

121212

121212

222

222

111

112

21

210

2

20

22

lCSC

lSCS

lSCS

lCSC

lSCS

lCSC

ADAA

U

Theo (2.37) v githit ccc thnh phn mmen ly tm qun tnh

u bng 0 , ta c :

=

12

1

21

21

1

002/1

0000

0000

2/1003/1

mlm

lmlm

J ,

=

22

2

22

22

2

002/1

0000

0000

2/1003/1

mlm

lmlm

J .

Trn cs(2.49), ta c :

( ) ( ) +

=+= TTT U

mlm

lmlm

lCSC

lSCS

TUJUTUJUTD 11

12

1

21

21

111

111

221221211111211

002/1

0000

0000

2/1003/1

0000

0000

0

0

222

22

2121

22

2

2

2

2

2

1121212

1121212

2 3/43/1

002/1

0000

0000

2/1003/1

0000

0000

)(0

)(0

lCmlmlmU

mlm

lmlm

CClSC

CSlCS

T T ++=

+

+

+


37/82


37

( )

=== TT U

mlm

lmlm

lCSC

lSCS

TUJUTrDD 21

22

2

22

22

121212

121212

2212222112

002/1

0000

0000

2/1003/1

0000

0000

0

0

)2/12/16/1( 22

2 Clm ++= =2

22

2 2/13/1 lmlm + .

( )

== TT U

mlm

lmlm

lCSC

lSCS

TUJUTrD 22

22

2

22

22

121212

121212

22222222

002/1

0000

0000

2/1003/1

0000

0000

0

0

22

212

22

212

22 3/13/13/1 lmClmSlm =+= .

Tnh cc shng trong biu thc (2.50) i vi i = 1, ta c :

2 22 2

1 1km k m 111 1 2 121 1 2 122 2 122 2k 1 m 1

h h h h h h= =

= = + + + & & & & & & & &

V theo (2.51) tnh cc hshikm, ri thay vo phng trnh trn, ta

c:

h1= -1/2m2S2l2 2

2& - m2S2l2 1 2 & & .

Tng ti vi i = 2

2 22 2 2 2

2 2km k m 211 1 212 1 2 221 2 1 222 2 2 2 1k 1 m 1

h h h h h h 1/ 2m S l= =

= = + + + = & & & & & & & & &

Nhvy :

22 2

2 1 22 2 2 2

22

12 2

1/ 2m S l m S lH( , )

1/ 2m S l

=

& & &&

&

.

Trn cs(2.52), ta c :

1 21 21 1 11 2 21c m gU r m gU r

= + =


38/82


38

=

+

+

=

1

0

0

2/1

0000

0000

)(0

)(0

)0,0,,0(

1

0

0

2/1

0000

0000

0

0

)0,0,,0( 1121212

1121212

2111

111

CClSC

SSlCS

gmlCSC

lSCS

gm

1212211 2/12/12/1 glCmglCmglCm ++= ;

c2

=

1

0

0

2/1

0000

0000

0

0

)0,0,,0( 121212

121212

2

lCSC

lSCS

gm =

)2/1( 12122 glCglCm =

Vy vector trng trng sl:

++=

=

122

1212211

2

1

2/1

2/12/1)(

glCm

glCmglCmglCm

c

cc .

Cui cng ta c phng trinh ng lc hc ca c cu tay my 2

khu dng sau :

Fm(t) = D( )( )(t) h( , ) c( ) + + & &

+

+++=

2

222

22

2

22

22

22222

1

2

1

3/12/13/1

2/13/13/43/1

lmClmlm

ClmlmlCmlmlm

F

F

M

M +

22 2

2 1 22 2 2 2

22

12 2

1/ 2m S l m S l

1/ 2m S l

& & &

&

+

+

122

2

1212212

2/1

2/12/1

Cglm

glCmglCmglCm .


39/82


39


40/82


40

CHNG III : THIT KBIU KHIN TRT CHO TAY MY

ROBOT 2 BC TDO

III.1.Hphi tuyn :

III.1.1.Hphi tuyn l g ?

nh ngha c r rng mt i tng hay hthng nhthno

c gi l phi tuyn trc tin ta nn nh ngha li htuyn tnh.

Xt mt h thng MIMO, vit tt ca nhiu vo / nhiu ra (Multi

Inputs Multi Outputs) vi r tn hiu vo u1(t), u2(t), , ur(t) v s tn hiu

ra y1(t) , y2(t) , , ys(t) . Nu vit chung r tn hiu u vo thnh vect

=

M

1( )

( )

( )r

u t

u t

u t

v s tn hiu u ra thnh

=

M

1( )

( )

( )s

y t

y t

y t

th m hnh h thng c

quan tm y l m hnh ton hc m tquan hgia vecttn hiu vo

( )u t v tn hiu ra

( )t , tc l m tnh xT :

a( ) ( )u t y t .

nh xny (Thng cn gi l ton t- operator) vit li nhsau :

=( ) ( ( ))t T u t .

nu nh xT thomn :

+ = +1 1 2 2 1 1 2 2

( ( )) ( ( )) ( ( )) ( ( ))T a u t T a u t a T u t a T u t , (3.1)

trong a1 v a2R, th h c ni l tuyn tnh. Tnh cht

(3.1) ca h tuyn tnh, trong iu khin, cn c gi l nguyn l xpchng.

V d: Xt 1 hgm 1 l xo c v 1 vt khi lng m lm 1 v d.

Vt schuyn ng trn trc nm ngang di tc ng ca lc F (hnh 3.1).


41/82


41

Hnh 3.1: V dvmt i tng tuyn tnh

Nu F c xem nhl tn hiu vo v qung ng sm vt i

c l tn hiu ra (p ng ca h) th theo cc tin chc ca Newton,

tc ng vo vt v ngc hng vi F c hai lc cn bng: Lc cn ca l

xo F1=cs

trong trng hp |s| tng i nhv F2= m&&sca chuyn ng. Vinguyn l cn bng lc ta c nh xT : a( ) ( )F t s t m tquan hvo / ra

ca h:

+ =&&ms cs F . (3.2a)

Gisrng di tc ng ca lc F1hc p ng s1v ca F2th

t:

+ =&&1 1 1

ms cs F

+ =&&2 2 2

ms cs F

c ngay c

+ + + = +&& &&1 1 2 2 1 1 2 2 1 1 2 2

( ) ( )m a s a s c a s a s a F a F ,

trong a1, a2l nhng sthc tu . Ni cch khc di tc ng

ca lc +1 1 2 2

a F a F

vt si c mt qung ng l +1 1 2 2

a s a s . Bi vy T thomn

(3.1) v do trong trng hp |s| tng i nhv lc cn ca l xo c

xc nh gn ng bng cng thc F1= cs th hthng l xo + vt l mt h

tuyn tnh .

m..

s Fcs


42/82


42

Ngc li, nu lc cn l xo li c tnh theo F1= cs + s3, vi c

v l 2 hng s, m trong thc tngi ta vn sdng, th quan hvo /

ra ca hsl :

+ +&& 3=Fms cs s (3.2b)

v khi (3.2b) khng cn thomn nguyn l xp chng (3.1)

+ + + + + +&& && 3 31 1 2 2 1 1 2 2 1 1 2 2 1 1 2 2

( ) ( ) ( )m a s a s c a s a s a s a s a F a F

+ + + + + +&& && 3 31 1 2 2 1 1 2 2 1 1 2 2 1 1 2 2

( ) ( ) ( )m a s a s c a s a s a s a s a F a F

Ni cch khc, di tc ng ca lc +1 1 2

a F a F th qung ng

ca vt i c khng phi l +1 1 2 2

a s a s . Vy trng hp ny hc tnh

phi tuyn.III.1.2.M hnh trng thi v qu o trng thi ca H phi

tuyn:

III.1.2.1.M hnh trng thi:

M hnh ng ca i tng, hthng phi tuyn c xy dng t

quan hvo ra qua vic thm cc bin x1(t), x2(t), , xn(t) gi l bin

trng thi, sao cho quan hgia vector tn hiu ra y(t) vi n bin ny v tn

hiu vo u(t) chcn li thun tu l mt quan hi s. Nhng bin trngthi ny, vmt ngha vt l, l nhng i lng m sthay i ca n s

quyt nh tnh cht ng hc ca i tng.

V d1 : Tm hnh (3.1) ca i tng l xo + vt, nu thm bin

trng thi x1= s , x2= &s sc:

1

2

(1 0) (1 0). 0.x

s x Fx

= = +

v y l mt phng trnh i s. Ngoi ra cn c phn cc phng

trnh vi phn bao gm :

=&1 2

x x

Suy ra tnh ngha vx1, x2 v :


43/82


43

= +&2 1

1cx x F

m m

thu c bng cch thay trc tip x1, x2vo phng trnh (3.1).

Vit chung hai phng trnh vi phn trn li vi nhau sc :.

. 1

.

2

0 1 0

10

xx x Fc

x m m

= = +

Ni chung, mt hphi tuyn SISO c quan hvo ra gia tn hiu

vo u(t) v ra y(t) dng:

.( ) ( 1)( ,..., , , )n ny f y y y u=

Trong k hiu y(k)cho hm bc k ca y(t), tc l

( )

k

k

k

d yy

dt= ,

th vi cc bin trng thi c nh ngha nhsau :

= = = =& && ( 1)1 2 3

, , ,..., nn

x y x y x y x y

hsc m hnh hai phn: phn cc phng trnh vi phn bc nht

=

=

=

&

M

&

&

1 2

1

2 1( ,..., , , )

n n

n n

x x

x x

x f x x x u

v phng trnh i s: Y=x1

Tng qut ln th mt hphi tuyn, sau khi nh ngha cc bin trng

thi x1(t) , x2(t) , , xn(t), sm tbi :

- M hnh trng thi tng minh autonom

.

( , )

( , )

x f x u

g x u

=

= trong

1( )

( )

( )n

x t

x t

x t

=

M


44/82


44

- M hnh trng thi tng minh khng autonom

.

( , , )

( , , )

x f x u t

g x u t

=

=,

- hoc m hnh trng thi khng tng minh

.

( , , , ) 0

( , , , ) 0

f x x u t

g x u y t

=

=

III.1.2.2.Quo trng thi :

Tphng trnh trng thi m ththng vi mt tn hiu u vo

( )u t xc nh cho trc v vi mt im trng thi ban u 0 (0)x x= cng

cho trc ta sc c nghim ( )x t m tsthay i trng thi hthng

theo thi gian di tc ng ca kch thch ( )u t cho. Biu din ( )x t

trong khng gian n chiu Rn(cn gi l khng gian trng thi) nhmt

thphthuc tham st c mi tn chchiu tng ca t ta c mt quo

trng thi (hnh 3.2a). Tp tt c cc quo trng thi ng vi mt tn

hiu u vo ( )u t cnh nhng vi nhng im trng thi ban u 0x

khc nhau c gi l hcc quo trng thi(hnh 3.2b) .

Hnh 3.2a. Quo trmg thi ca hc 3 bin trng thi

1x


45/82


45

Hnh 3.2b.Hcc quo trng thi ca hc 2 bin trng thi

III.1.3. im cn bng v im dng ca hthng:

III.1.3.1. im cn bng:

nh ngha 1: Mt im trng thiex c gi l im cn bng

(equilibrium point) nu nhkhi ang im trng thiex v khng c mt

tc ng no tbn ngoi th hsnm nguyn ti .

Cn ctheo nh ngha nhvy th im cn bngex ca hthng

phi l nghim ca phng trnh:

0( , , 0)

u

d xf x u t

dt == =

Nhvy im cn bng l im m h thng snm yn ti , tc

l trng thi ca n skhng bthay i ( 0d x

dt= ) khi khng c stc ng

tbn ngoi ( 0u= ).

III.1.3.2.im dng ca h:

nh ngha 2: Mt im trng thi dx c gi l im dng ca h

thng nu nhhang im trng thi dx v vi tc ng ( ) du t u= c

nh, khng i cho trc, th hsnm nguyn ti .

R rng l im dng theo nh ngha va nu sl nghim ca :

( , , ) ( , , ) 0d

d du u

d xf x u t f x u t

dt == = =

2x


46/82


46

trong du l cho trc.

III.1.3.3 Tnh n nh ti mt im cn bng:

nh ngha 3 : Mt hthng c gi l n nh (tim cn) ti im

cn bng ex nu nhc mt tc ng tc thi (chng hn nhnhiu tcthi) nh bt hra khi ex v a ti im 0x thuc mt ln cn no

ca ex th sau hc khnng tquay vc im cn bng ex ban

u.

Theo nh ngha trn th ta c thnhn bit c hc n nh hay

khng ti mt im cn bng thng qua dng hcc ng quo trng

thi ca n. Nu hn nh ti mt im cn bng ex no th mi ng

quo trng thi ( )x t xut pht tmt im 0x thuc ln cn ca ex uphi kt thc ti ex

a) Min n nh O b)

Hnh 3.3. a)im cn bng n nh

b)im cn bng khng n nh

Ch rng tnh n nh ca hphi tuyn chc ngha khi i cng

vi im cn bng ex . C thhsn nh ti im cn bng ny, song likhng n nh im cn bng khc. iu ny cng khc so vi khi nim

n nh htuyn tnh. V htuyn tnh thng chc mt im cn bng

l gc to( ex = 0 ) nn khi hn nh ti 0 , ngi ta cng ni thm lun

mt cch ngn gn l hn nh .

ex

ex


47/82


47

Ngoi ra, do khi nim n nh h phi tuyn b gn vi ln cn

im cn bng ex nn cng c thmc d hn nh ti im cn bng ex

song vi mt ln cn qu nhth skhng c nh ngha sdng. Ni cch

khc, vmt ng dng, n c xem nhkhng n nh. Bi vy, i vi

hphi tuyn, vic xc nh xem h c n nh ti im cn bng ex haykhng l cha m cn phi chra min n nh ca n ti ex , tc l phi

chra c ln cn O ca ex sao cho hc khnng tquay vc ex t

bt k mt im 0x no thuc O (hnh 3.3). Min n nh O cng ln th

tnh n nh ca hti ex cng tt .

Nhim vu tin ca biu khin l phi gi cho h thng n

nh. Nu nhban u i tng khng n nh, tc l khi c nhiu tbn

ngoi tc ng a n ra khi im lm vic v n khng c khnng tquay vth biu khin phi to ra tn hiu iu khin dn i tng quay

trvim lm vic ban u.

III.1.4 Tiu chun n inh Lyapunov :

Mt trong nhng iu kin, hay tiu chun cht lng u tin m b

iu khin cn phi mang n c cho hthng l tnh n nh. Ti sao li

nh vy ? Tkhi nim v tnh n nh ca h thng ti mt im cn

bng c nu trong nh ngha 3 ta thy r nu mt hqu nhy cmvi tc ng nhiu n ni chmt tc ng tc thi khng mong mun rt

nh lm cho hbbt ra khi im cn bng (hoc im lm vic) m

sau h khng c kh nng t tm vim cn bng ban u th cht

lng ca hkhng thgi l tt c.

Bi vy, kim tra tnh n nh ca h(ti mt im cn bng) cng

nhmin n nh O tng ng phi l cng vic u tin ta phi tin hnh

khi phn tch hthng. Tiu chun Lyapunov l mt cng chu ch gipta thc hin c iu .

nh ngha 4 : Mt hthng c m hnh khng kch thch :

~

0( , , ) ( , )

u

d xf x u t f x t

dt == =

(3.3)


48/82


48

vi mt im cn bng l gc to 0 , c gi l :

a) n nh Lyapunov ti im cn bng 0 nu vi 0> bt k bao

gi cng tn ti ph thuc sao cho nghim ( )x t ca (3.3) vi

0(0)x x= thomn : 0 ( ) , 0.x x t t < => <

b) n nh tim cn Lyapunov ti im cn bng 0 nu vi 0> bt

k bao gicng tn ti phthuc sao cho nghim ( )x t ca (3.3) vi

0(0)x x= thomn :

lim ( ) 0t

x t

= .

III.1.4.1.Tiu chun Lyapunov:

lm quen v tip cn tiu chun Lyapunov ta hy bt u th

bc 2 c m hnh trng thi autonom khi khng bkch thch :

1 2( , )

d xf x x

dt= vi 1

2

xx

x

=

(3.4)

Htrn c githit l cn bng ti gc to 0 .

Hnh 3.4 :Minh ha khi nim n nh Lyapunov:Hnh 3.4 minh hocho nh ngha 4 vtnh n nh Lyapunov ti 0

gi cho ta mt hng kh n gin xt tnh n nh cho h(3.4) ti

0 . Chng hn bng cch no ta c c hcc ng cong khp kn

v bao quanh gc to 0 . Vy th kim tra hc n nh ti 0 hay

khng ta chcn kim tra xem qudo pha ( )x t , tc l nghim ca (3.4) i


49/82


49

tim trng thi u0

x cho trc nhng tu nm trong min bao bi

mt trong cc ng cong khp kn , c ct cc ng cong v ny theo

hng tngoi vo trong hay khng ( hnh 3.5).

- Nu ( )x t khng ct bt cmt ng cong hv no theo chiu ttrong ra ngoi th hsn nh ti 0 .

- Nu ( )x t ct mi ng cong hv theo chiu tngoi vo trong

th hsn nh tim cn ti 0 .

Hnh 3.5:Mt gi vvic kim tra tnh n nh ca hti O

R rng l cn v quo pha ( )x t ca hkhng ct bt c

mt ng cong khp kn thuc h v theo chiu t trong ra ngoi l tiim ct , tip tuyn ca ( )x t phi to vi vectorv

, c nh ngha l

vector vung gc vi ng cong theo hng ttrong ra ngoi, mt gc

khng nhhn 900. Ni cch khc, hsn nh ti 0 nu nhc c

iu kin:

T

v0 . . os =

v

d x d x c

dt dt (3.5)

ti mi giao im ca ( )x t vi cc ng cong thuc hv.

Vn cn li l lm thno c c cc ng cong v sao cho vic

kim tra iu kin (1.48) c thun tin. Cu trli l sdng hm xc

nh dng V( x)c nh ngha nhsau :


50/82


50

nh ngha 5 : Mt hm thc nhiu bin, c thkhng dng (0, )V t ,

c gi l hm xc nh dng nu :

a) (0, ) 0V t =

b) Tn ti hai hm mt bin, dng 1( )a v 1( )b lin tc, n iutng vi

1 2(0) (0) 0 = = sao cho :

1 20 ( ) ( , ) ( )x V x t x < vi mi 0x (3.6)

Hm ( , )V x t sxc nh dng trong ton bkhng gian trng thi

nu cn c :

1lim ( )a

a

= => lim ( , )x

V x t

= .

nh l 1 : Hphi tuyn (c thkhng autonom) cn bng ti gc to

v khi khng bkch thch th c m tbi hnh :

( , )d x

f x tdt

= (3.7)

sn nh Lyapunov ti 0 vi min n nh O nu :

a) Trong O tn ti mt hm xc nh dng (0, )V t .

b) o hm ca n tnh theo m hnh (1.51) c gi trkhng dng

trong O, tc l :

( , ) 0dV V V

f x tdt t x

= +

vi mi x O . (3.8)

nh l 2: Hphi tuyn (c thkhng autonom) cn bng ti gc

tov khi khng bkch thch th c m tbi m hnh.

( , )d x

f x tdt

= (3.9)

sn nh tim cn Lyapunov ti 0 vi min n nh O nu :

a)Trong O tn ti mt hm xc nh dng ( , )V x t .


51/82


51

b) o hm ca n tnh theo mt hnh (1.51) c gi trm trong O

vi 0x , tc l :

( , ) 0dV V V

f x tdt t x

= + =+= TCCeCeS & (3.47)

R rng rng S=0 th )()( tqtq q . Quthc vi S=0 ta c thvit li

nhsau:

CeeeCeS ==+= && 00

Nhvy hthng n nh tim cn nu c e = 0 v theo iu kin

bm )()( tqtq q sc m bo.

Do vy vn iu khin l phi tm m men thch hp sao cho

vector trng thi ca hthng c thbm c trn mt trt. Hay phi tm

tha mn iu kin trt. iu kin trt c thxc nh theo tiu chun

Lyapunov.

Chng ta nh ngha hm Lyapunov nhsau:

02

1>= SSV T (3.48)

o hm ca (3.48) c dng:

SSV T && = (3.49)

Nhvy, nu 0


62/82


62

Khi iu kin trt m bo cho hkn n nh ton cc, tim cn

v iu kin bm c thc hin mc d m hnh khng chnh xc,

nhiu,

Nu iu kin trt c ththa mn theo :

=

=>n

i

i

T SSSSS1

2;0;0& (3.51)

Tip , mt phng trt S=0 st c vi thi gian gii hn nh

hn T0:

))0((2

10 qST

= (3.52)

Biu thc trn c chng minh nhsau:T(29) ta c:

S

SST & (3.53)

Thay SSV T && = v 21

)2( VS = vo (3.53) sau tch phn hai vvi

t=0treach , S(q(treach))=0 ta c:

[ ] 00 0 2

))0((2

))0((2

1)2(

21

21 TqSttqSVdt

VV

reachreach

t treach reach

=== &

(3.54)

By gi chng ta tm u vo biu khin tha mn iu kin

trt.

Ly o hm biu thc (3.47) ta c:

dqqeCS &&&&&& += (3.55)

Thay biu thc (3.39) vo ta c:

dqqBqqfeCS &&&&& ++= )(),( (3.56)

Do tn hiu iu khin c dng

[ ])( 1 sKSgnB eq= (3.57)


63/82


63

vi:

[ ]Tn

eq

sSgnsSgnsSgnsSgn

qqfeCq

)(),...,(),()(

),(

21=

= &&&& (3.58)

K>0, K l ma trn khuych i nxn.

Ma trn khuych i K phi chn ln iu kin trt c tha

mn mc d c tham skhng r, nhiu,

Trong trng hp c lng chnh xc ffBB == , th iu kin

trt c vit li nhsau:

SsKSgnSSSTT

= )(& (3.59)

Nu chn > ;IK (3.60)

v SSSSSSSm

i

i

m

i

i

T === == 1

2

1

& (3.61)

th chtrt xy ra.

Ta nhn thy rng, u vo iu khin c gin on qua s(t) nh

cho biu thc (3.57). Hin tng chattering xy ra. Bi v trong thc t,

s chuyn i l khng l tng. Trong trng hp sai sc lng l

khng nhth vic chn K l khng n gin nhbiu thc trn.

Trong trng hp S& cho di dng:

)()()(),( 11 sKSgnBqBBqBqqfeCqS eqd +++= &&&&& (3.62)

t 1)();( =+= BqBRffff dn ti:

)()()( sRKSgnffIRS eq += & (3.63)

Ty, iu kin trt l:

{ } )()()()( sSgnSsRKSgnffIRSSS TeqTT += && (3.64)

Do vy, nu chn K :

{ } )()()()( sSgnSffIRSsRKSgnS TeqTT ++ (3.65)


64/82


64

th iu kin trt nh trn 0


65/82


65

- Ma trn C:

0

sin3

sin2/3

sin3

22

2121

2212

2211

=

=

=

=

c

c

c

c

&

&

&

(3.72)

- Ma trn G:

)cos(15

)cos(15cos15

212

2111

+=

+=

g

g (3.73)

III.4.3.2. M hnh ng lc hc tay my hai bc tdo:

Chng ta t cc bin trng thi l tn hiu gc quay v vn tc ca

cc khp tay my:Khp 1:

=

==

=

112

11112

111

&&&

&&

x

xx

x

(3.74)

Khp 2:

=

==

=

222

22122

221

&&&

&&

x

xx

x

(3.75)

Tn hiu vo u:

=

=

22

11

Fu

Fu (3.76)

Tbiu thc (3.70) ta c:

=

2

11

2

1

2221

12111

2

11

1

1

g

gH

cc

ccH

F

FH

&

&

&&

&&

(3.77)

trong H-1l ma trn nghch o ca ma trn H.

Tnh H-1v kt hp tt ccc phng trnh trn v thay vo (3.77)

tnh c:


66/82


66

Khp 1

11 12

12 1 22 12 22 21 11 212

21

2

21 2 12 21 11 21

1 3( (2 )sin 15(cos cos ))

4 9 4 cos 2

3 3(1 cos )( sin 15 cos( ))

2 2

x x

x u x x x x x x x

x u x x x x

= = + +

+ +

&

& (3.78)

Khp 2:

21 22

22 21 1 22 12 22 21 11 212

21

2

21 2 12 21 11 21

1 3 3( (1 cos )( (2 )sin 15(cos cos )))

4 9 4cos 2 2

3(5 3cos )( sin 15cos( )))

2

x x

x x u x x x x x x x

x u x x x x

=

= + + + +

+ + +

&

&

(3.79)

III.4.3.3. Thit kbiu khin trt cho tay my 2 bc tdo:

Xc nh bc tng i cho khp 1 v khp 2:

Tphng trnh trng thi ca cc khp (3.78), (3.79) v biu thc

(3.24) ta c c ngay l trong trng hp ny l p=n hay bc tng i

ca tng khp p=2.

Xy dng mt trt cho tng khp:

2222

1111

eeS

eeS

&

&

+=

+= (3.80)

y:

222

111

rd

rd

xxe

xxe

=

= (3.81)

1, 2l nhng sthc dng.

iu kin xy ra chtrt cho htrn:

0


67/82


67

+=

=

),()( 1112

1211

uxgxfx

xx

&

& (3.83)

v:

+==

),()( 22222221

uxgxfxxx

&& (3.84)

Ta c:

)(),())((

)),()((

111111111

1111111111111111

ShKuxgxfxe

uxgxfxexxeeeS

d

dd

=+=

++=+=+=

&&&

&&&&&&&&&&&&(3.85)

1

11111111

))(()(),(

xfxeShKuxg d

++=

&&&

Tng tta cng c:

2

22222222

))(()(),(

xfxeShKuxg d

++=

&&& (3.86)

Theo (3.77) ta c:

uHuxg

uxg 1

2

1

),(

),( =

(3.87)

=

2

11

22

12

2221

12111

2

1

)()(

g

gH

x

x

cc

ccH

xf

xf (3.88)

Ch : ),(11 uxgg

T(3.58) ta c c:

++

++

=

)()(

)()(

22

22222

1

1

11111

2

1

xfxeShK

xfxeShK

Hu

u

d

d

&&&

&&&

(3.89)

Thay (3.88) vo (3.89) ta c c biu khin:


68/82


68

+

+

++

++

=

2

1

22

12

2221

1211

2

22222

1

11111

2

1

)(

)(

g

g

x

x

cc

cc

xeShK

xeShK

Hu

u

d

d

&&&

&&&

(3.90)

III.4.3.4. Tnh ton gi trt icho tay my hai bc tdo:

tnh ton gi trt cho tay my hai bc tdo chng ta cn gii

bi ton ng hc ngc, t tnh ton gi trt cho cc khp:

+

+

==

1000

0100

)(0

)(0

1121212

1121212

21

10

20 SSlCS

CClSC

AAA (3.91)

theo cch bin i tota c c:

=

112

2

2

20

0

0

0

z

y

x

Az

y

x

(3.92)

Tuy nhin, trong bi ton ny c x2, y2, z2=0 v ta chquan tm ti

chuyn ng ca tm bn kp do vy t(3.91) v (3.92) ta c:

+=+=

)()(

1120

1120

SSlyCClx (3.93)

+=

=+

+=+

2

220

20

2

22

220

20

22

20

20

2

2arccos

)cos(2

2

)cos(22

l

lyx

l

lyx

l

yx

(3.94)

T(3.93) ta c:

=

=

=

=2

122

10

212

210

1210

1210

)()(

)()(

lSlSy

lClCx

lSlSy

lClCx (3.95)


69/82


69

120

20

012

020

020

20

101020

20

2

0)(2

Syx

yC

yx

x

l

yx

SyCxlyx

++

+=

+

=++

(3.96)

t

+=

+=

20

20

0

20

20

0

)sin(

)cos(

yx

y

yx

x

(3.97)

Chn >1 ta c:

+

+

= l

yx

2arccos(

20

20

1 (3.98)

Nhvy, nu yu cu ca bi ton l iu khin tm bn kp i

theo mt quo c nh trc v c xc nh bi:

==

=

=

0)(

)(

)(

tzz

tyy

txx

th gi trt cho cc khp phi l:

+=

+

+=

2

220

20

2

20

20

1

2

2arccos

2arccos

l

lyx

l

yx

(3.99)


70/82


70

CHNG IV: M PHNG QU TRNH CHUYN NG CA ROBOT

DNG BIU KHIN TRT TRN NN MATLAB AND SIMULINK:

IV.1. Tng quan vMatlab-Simulink:

Matlab l mt bchng trnh phn mm ln ca lnh vc ton s.

Tn ca bchng trnh chnh l tvit tt ca tMatrix Laboratory, th

hin nh hng chnh ca chng trnh l cc php tnh vectr v ma trn.

Phn ct li ca chng trnh bao gm mt shm ton, cc chc nng

xut nhp cng nhcc khnng iu khin chu trnh m nh ta c th

dng nn cc Scripts.

Thm vo phn ct li, c th dng cc b cng c Toolbox vi

phm vi chc nng chuyn dng m ngi sdng cn. Simulink l mt

Toolbox c vai tr c bit quan trng: vai tr ca mt bcng cmnh

phc vm hnh ho v m phng cc hthng kthut - Vt l, trn cs

scu trc dng khi.

Giao din ha trn mn hnh ca Simulink cho php thhin h

thng di dng s tn hiu vi cc khi chc nng quen thuc.

Simulink cung cp cho ngi dng mt thvin rt phong ph, c sn vi

slng ln cc khi chc nng cho cc htuyn tnh, phi tuyn v ginon. Hn thngi sdng c thto nn cc khi ring cho mnh.

Sau khi xy dng m hnh ca h thng cn nghin cu, bng

cch ghp cc khi cn thit, thnh s cu trc ca h, ta c thkhi

ng qu trnh m phng. Trong cc qu trnh m phng ta c thtrch tn

hiu hin ti vtr bt k ca scu trc v hin thc tnh ca tn hiu

trn mn hnh. Hn thna, nu c nhu cu ta cn c th ct gicc

c tnh vo mi trng nh. Vic nhp hoc thay i tham sca tt c

cc khi cng c ththc hin c rt n gin bng cch nhp trc tip

hay thng qua matlab. kho st h thng, ta c th sdng thm cc

Toolbox nhSignal Processing (xl tn hiu), Optimization (ti u) hay

Control System (hthng iu khin).


71/82


71

IV.2. Cc thao tc thc hin m phng:

Khp 1:

11 12

12 1 22 12 22 21 11 212

21

2

21 2 12 21 11 21

1 3( (2 )sin 15(cos cos ))

4 9 4cos 2

3 3(1 cos )( sin 15cos( ))

2 2

x x

x u x x x x x x x

x u x x x x

== + +

+ +

&

&

Khp 2:

21 22

22 21 1 22 12 22 21 11 212

21

2

21 2 12 21 11 21

1 3 3( (1 cos )( (2 )sin 15(cos cos )))4 9 4cos 2 2

3(5 3cos )( sin 15cos( )))

2

x x

x x u x x x x x x x

x u x x x x

=

= + + + +

+ + +

&

&

Ta c m hnh Simulink ca Robot 2 bc tdo:


72/82


72

Khi Subsytem:

y ta sdng cc khi:

+ intergrator: khi tch phn vi cc tham sca khi mc nh cho

trc

+ khi mux: chp tn hiu n thnh tn hiu tng hp ca nhiu tn

hiu

+ khi input, ouput: u vo v u ra ca tn hiu.

+ khi hm: biu din 1 hm ton hc khi c tn hiu i vo l cc

bin, tn hiu ra thu c l hm cn biu din:

.

12x =(u[5]+1.5*u[4]*sin(u[3])*(2*u[2]+u[4])-15*(cos(u[1])-cos(u[3]))-

(1+1.5*cos(u[3]))*(u[6]-1.5*u[2]*u[2]*sin(u[3])-15*cos(u[1]+u[3])))/(4-

2.25*cos(u[3])*cos(u[3]))

V:.

22x =(-(1+1.5*cos(u[3]))*(u[5]+1.5*u[4]*sin(u[3])*(2*u[2]+u[4])-

15*(cos(u[1])-cos(u[3])))+(5+3*cos(u[3]))*(u[6]-1.5*u[2]*u[2]*sin(u[3])-

15*cos(u[1]+u[3])))/(4-2.25*cos(u[3])*cos(u[3]))


73/82


73

+ Cc khi scope (thuc th vin con sinks): Hin th cc tn hiu

ca qu trnh m phng theo thi gian. Nu mca sScope sn ttrc

khi bt u m phng ta c ththeo di trc tip din bin ca tn hiu.

Ta sdng ngun tn hiu u1, u2l 1(t)

Cc hm x12, v x22l :

X12 = (u[5]+1.5*u[4]*sin(u[3])*(2*u[2]+u[4])-15*(cos(u[1])-

cos(u[3]))-(1+1.5*cos(u[3]))*(u[6]-1.5*u[2]*u[2]*sin(u[3])-

15*cos(u[1]+u[3])))/(4-2.25*cos(u[3])*cos(u[3])).

X22 = (-(1+1.5*cos(u[3]))*(u[5]+1.5*u[4]*sin(u[3])*(2*u[2]+u[4])-

15*(cos(u[1])-cos(u[3])))+(5+3*cos(u[3]))*(u[6]-1.5*u[2]*u[2]*sin(u[3])-

15*cos(u[1]+u[3])))/(4-2.25*cos(u[3])*cos(u[3]))vi u[1], u[2], u[3], u[4], u[5], u[6] tng ng l cc vtr thttrn

khi Mux. u[1] = x11, u[2] = x12, u[3] = x21,u[4] = x22, u[5] = u1, u[6] = u2.

Sau khi m phng ta c thcc ng c tnh ca cc bin trng

thi x11, x12, x21,x22l :


74/82


74

M phng dng hm iu khin:

Tcc cng thc:

+

+

++

++

=

2

1

22

12

2221

1211

2

22222

1

11111

2

1

)(

)(

g

g

x

x

cc

cc

xeShK

xeShK

Hu

u

d

d

&&&

&&&

- Ma trn H:

1

cos2/31

cos35

22

22112

211

=

+==

+=

h

hh

h

- Ma trn C:

0

sin3

sin2/3

sin3

22

2121

2212

2211

=

=

=

=

c

c

c

c

&

&

&


75/82


75

- Ma trn G:

)cos(15

)cos(15cos15

212

2111

+=

+=

g

g

Ta c:

U1=h11 1 1 1 1 1

1

( )d

K h S e x

+ +& &&+h12 2 2 2 2 2

2

( )d

K h S e x

+ +& &&

+c11x12+c12x22+15cos(x11)

-15sos(x11+x21)

U2=h21 1 1 1 1 1

1

( )d

K h S e x

+ +& &&+h22 2 2 2 2 2

2

( )d

K h S e x

+ +& &&+c21x12+c22x22

+15cos(x11+x21)

Chuyn vdng hm ca sSimulink:

U1=u[5]*(5+cos(u[3]))+(1+1.5*cos(u[3]))*u[6]-3*u[2]*u[3]*sin(u[3])

-1.5*u[3]*u[4]*sin(u[3])+15*cos(u[1])-15*cos(u[1]+u[3])

U2=(1+1.5*cos(u[3]))*u[5]+u[6]-3*u[1]*u[2]*sin(u[3])+15*cos(u[1]+u[3])

vi u[1]=x11, u[2]=x12, u[3]=x21, u[4]=x22.

U[5]= 1 1 1 1 1

1

( )d

K h S e x

+ +& &&

U[6]= 2 2 2 2 2

2

( )d

K h S e x

+ +& &&

Theo cng thc kinh nghim ta chn: K1=K2=500, 1 = 2 =0,156.

Trng hp ny ta dng hm H(S1),H(S2) l cc hm gii hn u

vo trong khong gi trupper v gi trlower.

t ta c:

u[5]= 1

1

K

H(S1)+

.

1

1

1e

+

1dx&& ( Khi subsytem)


76/82


76

u[6]= 2

2

K

H(S2)+

.

2

2

1e

+

2dx&& ( Khi subsytem2)


77/82


77


78/82


79/82


79

+=

+

+=

2

220

20

2

20

20

1

2

2arccos

2arccos

l

lyx

l

yx

=>xd1=acos(sqrt((cos(u[1]+u[3])+cos(u[1]))^2+(sin(u[1]+u[3])+sin(u[1]))^2)/2)+

acos((cos(u[1]+u[3])+cos(u[1]))/sqrt((cos(u[1]+u[3])+cos(u[1]))^2+(sin(u[1])+sin(u[1]

+u[3]))^2))

xd2= acos(((cos(u[1]+u[3])+cos(u[1]))^2+(sin(u[1]+u[3])+sin(u[1]))^2-2)/2)


80/82


81/82


81

1. Nguyn Thin Phc: Robot cng nghip. NXB KHKT 2004.

2. Nguyn Thin Phc: Ngi my cng nghip v sn xut t

ng linhhot. NXB KHKT 1991.3. Nguyn Phng Quang: iu khin Robot cng nghip -

Nhng vn cn bit. Tp ch Tng ho ngy nay - sthng 4, 5, 6 /

2006.

4. Nguyn Thin Phc: Robot - Th gii cng ngh cao ca

bn. NXB KHKT 2005.

5. o Vn Hip: Kthut Robot. NXB KHKT 2002.

6.

inh Gia Tng, TKhnh Lm: Nguyn l my. NXB GD

2003.

7. Nguyn Hng Thi: Xy dng thut ton iu khin v m

phngng Robot nhiu bc tdo. Thvin HBK HN 2002.

8. L Huy Tng: iu khin trt thch nghi ng cho i

tng phi tuyn c m hnh khng tng minh. Th vin HBK HN

2003.

9. Nguyn Don Phc, Phan Xun Minh, Hn Thnh Trung: L

thuytiu khin phi tuyn. NXB KHKT 2003.

10. Nguyn Don Phc, Phan Xun Minh: Hphi tuyn. NXB

KHKT 2000.

11. Nguyn Thng Ng: L thuyt iu khin tng hin

i. NXB KHKT 2003.

12.

Nguyn Thng Ng: L thuyt iu khin thng thngv hini. NXB KHKT 2002.

13. Nguyn Don Phc: L thuyt iu khin tuyn tnh.

NXB KHKT 2002.


82/82


14. Nguyn Phng Quang: Matlab & Simulink dnh cho ks

iu khintng. NXB KHKT 2004.

15. Nguyn Hong Hi: Lp trnh Matlab. NXB KHKT 2003.

16.

Solomon: Stability of nonlinear control systems. 1965.17. Applied Asymptotic Methods in Nonlinear Oscillitions.

Thvin HBK HN 1994.

18. Harry: Nonlinear Modulation Theory. 1971.

19. Jakub Mozaryn, Jerzt E.Kurek: Design of the Sliding Mode

Control for the Puma 560 Robot. Institute of Automatic Control and

Robotics Warsaw university of Technology.

20.

Martin Ansbjerg Kjaer: Sliding Mode Control. Sweden

February 6th2004.

21. C.Abdallah, D.Dawson, P.Dorato, and M.Jamshidi: Survey of

RobustControl for Rigid Robots.

22. Mark W.Spong: Motion control of Robot Manipulators. The

coordinated Science Laboratory, University of Illinois at Urbana-

Champaign, 1308 W. Main St, Urbana, III. 61801 USA.

23. . Andre` Jaritz and Mark W. Spong: An Experimental

Comparison of Robust control Algorithms on a Direct Drive

Manipulator .IEEE Transaction on Control Systems Technology, Vol. 4,

No. 6, November 1996.

24. Austin Blaquie`re: Nonlinear system analysis. Academic

Press New York and London 1966.

Documents

Thiết Kế Cánh Tay Robot 2 Bậc Tự Do