You are on page 1of 82

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

ti: Thit k b iu khin trt cho tay my Robot 2 bc t


do v m phng trn Matlab Simulink.

CHNG I: TNG QUAN V ROBOT CNG NGHIP

I.1.Robot cng nghip:


I.1.1. S ra i ca Robot cng nghip :
Thut ng Robot ln u tin xut hin nm 1922 trong tc phm
Rosums Universal Robot ca Karal Capek. Theo ting Sc th Robot l
ngi lm tp dch. Trong tc phm ny nhn vt Rosum v con trai ng
to ra nhng chic my gn ging nh con ngi hu h con ngi.
Hn 20 nm sau, c m vin tng ca Karel Capek bt u
hin thc. Ngay sau chin tranh th gii ln th 2, M xut hin
nhng tay my chp hnh iu khin t xa, trong cc phng th nghim
phng x. Nm 1959, Devol v Engelber ch to Robot cng nghip u
tin ti cng ty Unimation.
Nm 1967 Nht Bn mi nhp chic Robot cng nghip u tin t
cng ty AMF ca M. n nm 1990 c hn 40 cng ty ca Nht, trong
c nhng cng ty khng l nh Hitachi, Mitsubishi v Honda a ra th
trng nhiu loi Robot ni ting.
T nhng nm 70, vic nghin cu nng cao tnh nng ca robot
ch nhiu n s lp t thm cc cm bin ngoi tn hiu nhn bit
mi trng lm vic. Ti trng i hc tng hp Stanford, ngi ta to
ra loi Robot lp rp t ng iu khin bng vi tnh trn c s x l thng
tin t cc cm bin lc v th gic. Vo thi gian ny cng ty IBM ch
to Robot c cc cm bin xc gic v cm bin lc iu khin bng my
vi tnh lp rp cc my in gm 20 cm chi tit .
Nhng nm 90 do p dng rng ri cc tin b khoa hc v vi x l
v cng ngh thng tin, s lng Robot cng nghip tng nhanh, gi
thnh gim i r rt, tnh nng c nhiu bc tin vt bc. Nh vy
Robot cng nghip c v tr quan trng trong cc dy truyn sn xut
1

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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 c cu ng hc, ng lc hc, lp trnh qu o, 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 c ch
my mc tng t con ngi, c kh nng s dng cng c lao ng
thc hin cc cng vic thay cho con ngi, thm ch c th tnh ton hay
c kh nng hnh ng theo ch.
Trong thc tin k thut, khi nim Robot hin i c hiu kh
rng, m theo Robot l tt c cc h thng k thut c kh nng cm
nhn v x l thng tin cm nhn c, sau a ra hnh x thch
hp. Theo cch hiu ny, cc h thng xe t hnh, hay thm ch mt thit
b xy dng c trang b cm bin thch hp nh Camera, cng c gi l
Robot. Cc khi nim nh Hexapod, Parallel Robot, Tripod, Gait Biped,
Manipulator Robocar hay Mobile Robot nhm ch vo cc h thng 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). Cng
ngh tay my (Handling Technology) l cng ngh ca dng thit b k
thut c kh nng thc hin cc chuyn ng theo nhiu trc trong khng
gian, tng t nh con ngi.
V c bn c th phn thit b tay my (hnh 1.1) thnh 2 loi chnh :
iu khin (K) theo chng trnh hay K thng minh :

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Handling
Equipments

iu khin

iu khin theo

thng minh

chng trnh

Manipulators,
Telemanipulators

Chng trnh

Chng trnh

cng

linh hot

My bc d,
xp t

Robot cng
nghip

Hnh 1.1 : Phn loi thit b tay my


+ Loi K theo chng trnh gm 2 h:
Chng trnh cng : Cc thit b bc d, xp t c chng trnh
hot ng c nh. Ta hay gp h ny trong cc h thng kho hin i.
Chng c rt t trc chuyn ng v ch thu thp thng tin v qung ng
qua cc tip im hnh trnh. Ta khng th K chng theo mt qu o
mong mun.
Chng trnh linh hot : L h Robot m ngi s dng c kh
nng thay i chng trnh K chng tu theo i tng cng tc. Ta hay
gp chng trong cc cng on nh hn, sn hay lp rp ca cng nghip
t. 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 kh nng lp li cc chuyn ng ca tay ngi. Bn cht l dng thit
3

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

b h tr cho s kho lo, cho tr tu, cho h thng gic quan (Complex
Sensorics) v kinh nghim ca ngi s dng. Hay c s dng trong cc
nhim v cn 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 s dng h thng Camera quan st mi trng s dng.
Theo tiu chun chu u EN775 v VDI 2860 ca c c th
hiu Robot cng nghip l mt Automat s dng vn nng to chuyn
ng nhiu trc, c kh nng lp trnh linh hot cc chui chuyn ng v
qung ng (gc) to nn chuyn ng theo qu o. Chng c th
c trang b thm cc ngn (Grippe), dng c hay cc cng c gia cng
v c th thc hin cc nhim v ca i tay (Handling) hay cc nhim v
gia cng khc
Nh vy, RBCN khc cc loi tay my cn li 2 im chnh l s
dng vn nng v kh nng 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 sut
dy 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 t nhng u im c bn ca Robot l :
- Robot c th thc hin mt quy trnh thao tc hp l bng hoc hn
ngi th lnh ngh mt cch n nh trong sut thi gian di lm vic. Do
Robot gip nng cao cht lng v kh nng cnh tranh ca sn phm.
- Kh nng gim gi thnh sn phm do ng dng Robot l v gim
c ng k chi 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 t sn xut c rt nhiu ni ngi
lao ng phi lm vic trong mi trng nhim, m t, nng nc. Thm
4

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

ch rt c hi n sc kho v tnh mng nh mi 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, xin
c nu ra mt s lnh vc ch yu :
- K ngh c
- Gia cng p lc
- Cc qu trnh hn v nhit luyn
- Cng ngh gia cng lp rp
- Phun sn, vn chuyn hng ho (Robocar)
I.2.3. Cc xu th ng dng Robot trong tng lai :
- Robot ngy cng thay th nhiu lao ng
- Robot ngy cng tr ln chuyn dng
- Robot ngy cng m nhn c nhiu loi cng vic lp rp
- Robot di ng ngy cng tr ln ph bin
- Robot ngy cng tr ln tinh khn
I.2.4. Tnh hnh tip cn v ng dng Robot cng nghip Vit
Nam :
Trong giai on trc nm 1990, hu nh trong nc hon ton cha
du nhp v k thut Robot, thm ch cha nhn c nhiu thng tin k
thut v lnh vc ny. Tuy vy, vi mc tiu ch yu l tip cn lnh vc
mi m ny 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 t nm 1990 cc ngnh cng nghip trong nc
bt u i mi. Nhiu c s nhp ngoi nhiu loi Robot cng nghip
phc v cc cng vic nh: tho lp dng c, lp rp linh kin in t, hn
v t xe my, phun ph cc b mt

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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 ch to v lp rp Robot.
Nhng nm gn y, Trung tm nghin cu k thut T ng ha,
Trng i hc Bch Khoa H Ni, nghin cu thit k mt kiu Robot
mi l Robot RP. Robot RP thuc loi Robot phng sinh (bt chc c cu
tay ngi). Hin nay ch to 2 mu: Robot RPS-406 dng phun men
v Robot RPS-4102 dng trong cng ngh b mt.
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 th vin cc m hnh ca
Robot trn my tnh
I.3.Cu trc ca Robot cng nghip:
I.3.1.Cc b phn cu thnh Robot cng nghip :
Trn hnh 1.2 gii thiu cc b phn ch yu ca Robot cng nghip:
Tay my gm cc b phn: 1 t c nh hoc gn lin vi xe di
ng 2, thn 3, cnh tay trn 4, cnh tay di 5, bn kp 6.

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Hnh 1.2: Cc b phn cu thnh Robot cng nghip


H thng truyn dn ng c th l c kh, thu kh hoc in kh: l
b phn ch yu to nn s chuyn dch cc khp ng.
H thng iu khin m bo s hot 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 v hot 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 t do v cc to suy rng :
I.3.2.1.Bc t do :
Robot cng nghip l loi thit b t ng nhiu cng dng. C cu
tay my ca chng phi c cu to sao cho bn kp gi vt kp theo mt
hng nht nh no v di chuyn d dng trong vng lm vic. Mun
vy c cu tay my phi t c mt s bc t do chuyn ng.
Thng thng cc khu ca c cu 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 s bc t do :
5

W= 6n - i p i

(1.1)

vi

n : s khu ng
Pi : s khp loi i

V d: Tay my c 2 khp quay nh hnh v 1.3 :

S khu ng n = 2
Khp quay l khp loi 5 .

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Do W = 6.2 ( 5.1 + 5.1) = 2 bc t do

Hnh 1.3: Tay my 2 khp quay


I.3.2.2. To suy rng :
Cc cu hnh khc nhau ca c cu 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 tr ban 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 (to suy rng) ca c
cu tay my v biu th bng :

q =
i

+ (1 i )Si

(1.2)
vi

1,i vi khp quay


=

i
0,i vi khp tnh tin

- dch chuyn gc ca cc khp quay

- dch chuyn tnh tin ca cc khp tnh tin

I.3.3.Nhim v lp trnh iu khin Robot:


I.3.3.1. nh v v 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 c thao 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 l
im tc ng cui.

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Hnh 1.4: nh v v 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 . V tr ca im E c xc nh bng 3 to xE, yE, zE trong h
trc to c nh. Cn hng tc ng ca khu cui c th xc nh
bng 3 trc xn,yn, zn gn lin vi khu cui ti im E, hoc bng 3 thng
s gc , , no .
I.3.3.2. Lp trnh iu khin Robot cng nghip :
Trn hnh 1.5 m t 1 s lp trnh iu khin Robot cng nghip.
Khi robot nhn nhim v thc hin mt quy trnh cng ngh no , v d
im tc ng cui E phi bm theo mt hnh trnh cho trc. Qu o
hnh trnh ny thng cho bit trong h to cc x0, y0, z0 c nh.
mi v tr m im E i qua xc nh bng 3 to c nh xE, yE, zE v 3
thng s gc nh hng , , . T cc thng s trong h to cc
tnh ton cc gi tr bin khp qi tng ng vi mi thi im t. l ni
dung ca bi ton ng hc ngc s trnh by trong chng II.

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Qu o trong h to
cc
(xE,yE,zE,,,)

My tnh

Qu o trong h to
cc
(xE,yE,zE,,,)

q1

Chng trnh iu khin

H tr ng chp hnh

ROBOT

q2

Cc gin Bin i
qi(t)

H tr ng chp hnh

Hnh 1.5: S lp trnh iu khin

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


I.4.1.Bin i to dng Ma trn:
I.4.1.1. Vector im v to thun nht :
Vector im (point vector) dng m t v tr ca im trong khng
gian 3 chiu.
Trong khng gian 3 chiu, mt im M c th c biu din bng
nhiu vector trong cc h to (coordinate frame) khc nhau:
Trong h to oixiyizi im M xc nh bng vector ri :

= (r xi, r yi, r zi )

(1.3)

v cng im M trong h to ojxjyjzj c m t bi vector rj :

= (r xj , r yj , r zj )

(1.4)

10

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

K hiu ( )T l biu th php chuyn v (Transportation) vector hng


thnh vector ct.

zi

zj

ri

rj

yi

Oi

yj

xj
xi
Hnh 1.6: Biu din 1 im trong khng gian
Vector

= (r x, r y, r z ) trong khng gian 3 chiu, nu c b sung

thm mt thnh phn th 4 v th hin bng 1 vector m rng :

r% = ( r x, r y r z , )

(1.5)

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


nht (homogeneous coordinate).
n gin c th b qua k hiu ( ) i vi vector m rng (1.5)
Cc to thc ca vector m rng ny vn l:

r x

r y

r z

(1.6)

Khng phi duy nht c mt cch biu din vector trong khng gian
ta thun nht, m n ph thuc vo gi tr ca . Nu ly = 1 th
cc ta biu din bng to c thc. Trong trng hp ny vector m
rng c vit l:
r = (r x, r y , r z )

(1.7)

Nu ly 1 th cc to biu din gp ln to thc, nn


c th gi l h s t l. Khi cn biu din s thay i to km theo
th c s bin dng t l th dng 1.

11

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

I.4.1.2.Quay h to dng Ma trn 3x3:

Trc ht thit lp quan h gia 2 h to XYZ v UVW chuyn


ng quay tng i vi nhau khi gc O ca 2 h vn trng nhau (hnh 1.7)
Z
W

M
V

U
X
Hnh 1.7: Cc h to
Gi (ix, jy, kz) v (iu, jv, kw) l cc vector n v ch phng 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 h to OUVW bng vector:


ruvw = ( ru,rv,rw)T

(1.9)

Nh vy :
r = ruvw= ruiu + rvjv + rwkw
r = rxyz= rxix + ryjy + rzkz

(1.10)

T ta c

r
r
r

.
r
=
=
+
+
j y j yi u r u j y j v r v j y k w r w
y

= k z.r = k zi u r u + k z j v r v + k z k wr w
z

= i x.r = i xi u r u + i x j v r v + i x k wr w

(1.11)

Hay vit di dng ma trn:

12

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

r x i xi u
=
r y j yi u

r z k zi u

i j
j j
k j
x

ik
jk
kk
x

y
z


ru
.
w rv

rw
w

(1.12)

Gi R l Ma trn quay (rotation) 3x3 vi cc phn t l tch v


hng 2 vector ch phng cc trc tng ng ca 2 h to OXYZ v
OUVW.
Vy (1.12) c vit li l:

r
r

= R.r uvw

= R 1.r xyz
uvw
xyz

(1.13)

I.4.1.3.Bin i Ma trn dng to thun nht:

By gi thit lp quan h gia 2 h to : h to ojxjyjzj sang h


to mi oixiyizi. Chng khng nhng quay tng i vi nhau m tnh
tin c gc to : gc oj xc nh trong h xiyizi bng vector p:
p=(a,-b,-c,1)T

(1.14)

Gi s v tr ca im M trong h to xjyjzj c xc nh bng


vector rj:
rj = (xjyjzj,1)T

(1.15)

v trong h to xiyizi im M c xc nh bng vector ri:


ri = (xiyizi,1)T

(1.16)

T hnh (1.8) c th d dng thit lp mi quan h gia cc to :

x = x + at
y = y cos z sin bt
z = y sin + z cos ct
t = t =1
i

(1.17)

13

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do


xj
oj

zj

yj
xi
c
yi

oi
b
zi

Hnh 1.8: Cc h to
Sp xp cc h s ng vi xj,yj,zj v tj thnh mt ma trn:

0
1
0 cos
=
T ij 0 sin

0
0

0
sin
cos
0

a
b

(1.18)

v vit phng trnh bin i to nh sau:


ri = Tij rj

(1.19)

Ma trn Tij biu th bng ma trn 4x4 nh phng trnh (1.18) v gi


l ma trn thun nht. N dng bin i vector m rng t h to
thun nht ny sang h to thun 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 :

0
0
1
0 cos -sin
Tij =
0 sin cos

0
0
0

-b
c

(1.20)

14

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Hoc vit rt gn l:

Rij
0

Tij =

(1.21)

Trong :

Rij - ma trn quay 3x3


p ma trn 3x1 biu th 3 to ca im gc h to 0j trong h
to oi, xi, yi, zi
1x3 ma trn khng
1x1 ma trn n v
Nh vy ma trn thun nht 4x4 l ma trn 3x3 m rng, thm ma
trn 3x1 biu th s chuyn dch gc to v phn t a44 biu th h s t
l.
D dng nhn thy ma trn Rij chnh l ma trn quay 3x3, nu suy t
ma trn quay trong (1.12) sang trng hp hnh 1.8 ta c:
cos(x i , xj ) cos(x i , yj ) cos(x i , z j )

Rij = a ij = cos(yi , xj ) cos(yi , yj ) cos(yi , z j )


cos(z , x ) cos(z , y ) cos(z , z )
i
j
i
j
i
j

(1.22)

v cc gc cosin ch phng ny u lin h n gc (hnh 1.8).


Nu ch v quan h gia 2 cp trc,v d, cos(xi,yj) = cos(yi, xj)
y d dng nhn c biu thc:

Rij = Rij-1 = RijT

(1.23)

M t tng qut hn nu mt im M no c xc nh trong h


to thun nht UVW bng vect m rng ruvw , th trong h to thun
nht XYZ im xc nh bng vector m rng rxyz:
Rxyz = T.ruvw

(1.24)

15

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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


dng sau:

nx
n
T= y
nz

sx a x
sy a y
sz a z
0

px
p y
pz

(1.25)

R p
hoc T =

0 1

(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 to im
gc ca h to UVW biu din trong h XYZ.
Nu 2 gc to trng nhau th cc thnh phn ca ma trn 3x1
ny u l 0. Khi xt trng hp:

ruvw = (1,0,0,1)T
tc l rxyz = iu
th d dng nhn thy ct th nht hoc vect n ca ma trn (1.25)
chnh l cc to ca vect ch phng trc OU biu din trong h to
XYZ.
Tng t khi xt cc trng hp

ruvw = (0,1,0,1)T
v ruvw = (0,0,1,1)T
cng i n nhn xt ct th 2 (hoc vect s) ng vi cc to ca
vect ch phng trc OV v ct th 3 (hoc vect a) ng vi cc to
vector ch phng trc OW.
Nh vy, ma trn thun nht T 4x4 hon ton xc nh v tr v nh
hng ca h to UVW so vi h to XYZ. l ngha hnh hc
ca ma trn thun nht 4x4.
I.4.2.Cc php bin i c bn:

16

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

I.4.2.1.Php bin i tnh tin:

T (1.18) hoc (1.25), biu th ma trn thun nht khi ch c bin i


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

0
T =
0
0

0 0
1 0
0 1
0 0

p
p
p

y
= T p ( p x, p y , p z )
z
1
x

(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:

u = ( x, y, z )T
v p l vector ch hng v di cn dch chuyn
p=

( p x , p y , p z )T

th v l vector biu din im to trong khng gian c tnh


tin ti:

v = T p ( v = T p ( p x , p y , p z )T u

(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 to UVW quay quanh trc OX mt gc


no . Trong trng hp ny

0
1
0 cos
R ( x, ) =
0 sin

0
0

= iu :

0
sin
cos
0

0
0

(1.29)

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


17

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

cos 0 sin 0
0
1
0
0

R ( y , ) =
sin 0 cos 0

0
0
1
0

(1.30)

v trng hp quay quanh trc OZ mt gc :

cos
sin
R ( z , ) =
0

sin
cos
0
0

0 0
0 0

1 0

0 1

(1.31)

Ct th 4 ca cc ma trn 4x4 trn c 3 phn t u bng 0 v y


khng c s tnh tin. Cc ma trn ny c gi l cc ma trn quay
(rotation) c bn. Cc ma trn quay khc c th xy dng t cc ma trn c
bn ny.

CHNG II: H PHNG TRNH NG HC V NG LC HC


CA ROBOT CNG NGHIP:

II.1. H phng 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 th thao tc linh
hot c cu chp hnh ca n phi c cu to sao cho im mt ca khu
cui cng m bo d dng di chuyn theo mt qu o no , ng thi

18

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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 v tr v hng ca E trong khng gian ta gn vo n mt h
to ng th n v gn vi mi khu ng mt h to khc, cn gn
lin vi gi mt h to c nh. nh s k hiu cc h ny t 0 n
n bt u t gi c nh. Khi kho st chuyn ng ca Robot cn bit
nh v v nh hng ti im tc ng cui trong mi thi im. Cc
li gii ca bi ton ny c xc nh t nhng phng trnh ng hc
ca Robot. Cc phng trnh ny l m hnh ng hc ca Robot. Chng
c xy dng trn c s thit lp cc mi quan h gia cc h to ng
ni trn so vi h to c nh.
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 s nh v v nh hng ti im tc ng cui .
Nh cp phn I.4.1.4 biu th s nh v v nh hng
bng ma trn trng thi cui TE :

nx
n
TE = y
nz

sx a x
ny a y
sz a z
0

px
py
pz

(2.1)

Trong cc phn t ca ma trn 3x1 l to px , py, pz ca im


tc ng cui E. Mi ct ca ma trn quay 3x3 l mt vect n v ch
phng mt trc ca h to ng NSA (chnh l UVW) biu din trong
to c nh XYZ.
H to gn lin vi bn kp ca Robot c cc vect n v ch
phng cc trc nh sau :
a - vector c hng tip cn (approach) vi i tc .

19

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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


n - vector php tuyn (normal).
II.1.3. M hnh ng hc :
II.1.3.1. Ma trn quan h :

Chn h to c nh gn lin vi gi v cc h to gn vi
tng khu ng. K hiu cc h to ny t 0 n n, k t gi c nh tr
i.
Mt im bt k no trong khng gian c xc nh trong h to
th i bng bn knh ri v trong h to c nh x0, y0, z0 c xc nh
bng bn knh vector r0 :
r0 = A1A2Airi
hoc

r0 = Tiri

vi

Ti = A1A2Ai , i= 1, 2, n

(2.2)
(2.3)
(2.4)

Trong ma trn A1 m t v tr hng ca khu u tin; ma trn


A2 m t v tr v hng ca khu th 2 so vi khu u; ma trn Ai m t
v tr v hng ca khu th i so vi khu th i-1.
Nh vy, tch ca cc ma trn Ai l ma trn Ti m t v tr v hng
ca khu th i so vi gi tr c nh. Thng k hiu ma trn T vi 2 ch s:
trn v di. Ch s di ch khu ang xt cn ch s trn ch to
c dng i chiu. V d, biu thc (2.4) c th vit li l :

Ti = 0 Ti = A1 1Ti
vi

Ti = A2 A3 ...Ai

(2.5)
(2.6)

l ma trn m t v tr v hng ca khu th i so vi khu th nht.


Trong k hiu thng b qua ch s trn nu ch s bng 0.
Denavit & Hartenberg xut dng ma trn thun nht 4x4 m t
quan h gia 2 khu lin tip trong c cu khng gian .
II.1.3.2. B thng s DH :

20

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Di y trnh by cch xy dng cc h to ng 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 h to i vi 2 khu ng lin tip

Trc ht xc nh b thng s c bn gia 2 trc quay ca khp


ng i+1 v i :
ai l di ng vung gc chung gia 2 trc khp ng i+1 v i .
i l gc cho gia 2 trc khp ng i+1 v i .
di l khong cch o dc trc khp ng i t ng 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.
i l gc gia 2 ng vung gc chung ni trn.
B thng s ny c gi l b thng s Denavit Hartenberg (DH).
Bin khp (joint variable):
Nu khp ng i l khp quay th bin khp l i
21

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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 ai c xem l bng 0.
II.1.3.3. Thit lp h to :

Gc ca h to gn lin vi khu th i (gi l h to th i) 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 to l im bt k
trn trc khp ng i+1.
Trc zi ca h to th i 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 xi trng 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 nh hnh 2.2.

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


22

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Gn cc h to vi cc khu nh hnh v :
- Trc z0 , z1 v z2 vung gc vi mt t giy.
- H to c nh l o0x0y0z0 chiu x0 hng t o0 n o1.
- H to o1x1y1z1 c gc o1 t ti tm trc khp ng 2.
- H to o2x2y2z2 c gc o2 t ti tm trc khp ng cui
khu 2.
Bng thng s DH ca tay my ny nh sau :

Khu

ai

di

a1

a2

*
1
*
2

II.1.3.4. M hnh bin i :

Trn c s xy dng cc h to vi 2 khu ng lin tip nh


trn trnh by. C th thit lp mi quan h gia 2 h to lin tip
theo 4 php bin i :
+ Quay quanh trc z1-1 gc i .
+ Tnh tin dc trc zi-1 mt on di .
+ Tnh tin dc trc xi-1 ( trng vi xi) mt on ai .
+ Quay quanh trc xi mt gc i .
Bn php bin i ny c biu th bng tch cc ma trn thun
nht sau
Ai = R(z,i).Tp(0,0,di).Tp(ai,0,0).R(x,i)

(2.7)

23

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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:

C i S i C i S i S i

C i S i
Ai = S i C iC i
0
Si
C i

0
0
0

i i

aC
aS
d
i

(2.8)

Trong khp tnh tin : a = 0 .


II.1.3.5. Phng trnh ng hc :

Vi Robot c n khu, ma trn m t v tr v hng im cui E ca


tay my c miu t :
Tn = A1A2An

(2.9)

Mt khc, h to ti im tc ng cui ny c m t bng


ma trn TE. V vy hin nhin l:
TE = Tn

(2.10)

Tc l ta c :

nx
n
Tn = y
nz

sx a x
ny a y
sz a z
0

px
p y
pz

(2.11)

Phng trnh (2.11) l phng trnh ng hc c bn 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 qi l to suy rng hoc l bin khp.

24

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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 th xem qu o chuyn ng l tp hp lin tip cc v tr khc
nhau ca bn kp. Ti mi v tr trn qu o cn xc nh b thng s cc
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


c s ch yu xy dng chng trnh iu khin chuyn ng ca Robot
bm theo qu o cho trc.
Xut pht t phng trnh ng hc c bn (2.11) ta c :

nx
n
Tn = A1A2An = y
nz

sx a x
ny a y
sz a z
0

px
p y
pz

(2.12)

Cc ma trn Ai l hm ca cc bin khp qi. Vector nh v bn kp


p = (px,py,pz)T cng l hm ca qi. Cc vector n, s, a l cc vector n v
ch phng cc trc ca h to gn lin vi bn kp biu din trong h
to c nh XYZ. Cc vector ny vung gc vi nhau tng i mt nn
trong 9 thnh phn ca chng ch tn ti c lp ch c 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 t tng 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 h phng trnh ng hc ca Robot c


n bc t do.
V tri ca phng trnh (2.12) theo cc k hiu nh (2.4)-(2.6) c
th vit li nh sau:

25

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

= T i. T n
i

(2.13)
1

Nhn 2 v ca (2.13) vi T i ta c:
Ai-1...A2-1A1-1 Tn =

(2.14)

Kt hp (2.12) ta c:

nx

-1
-1
-1 n y
=Ai ...A2 A1
nz

sx
sy

ax
ay

sz
0

az
0

px
p y
pz

(2.15)

vi i=1,...,n-1
ng vi mi gi tr ca i, khi so snh cc phn t tng ng ca 2
ma 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 v v 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 v sau y :
+ Nhim v th nht 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 v th hai l xc nh cc sai s ng. Lc ny phi kho
st cc phng trnh chuyn ng ca c cu tay my ng thi xem xt
cc c tnh ng lc ca ng c truyn ng.
C nhiu phng php nghin cu ng lc hc Robot nhng
thng dng hn c l phng php Lagrange bc 2 v khi kt hp vi m
hnh ng lc hc kiu DH (Denavit-Hartenberg) ta s c cc phng
trnh ng lc hc dng vector ma trn, rt gn nh v thun tin cho
vic nghin cu gii tch v tnh ton trn my tnh.

26

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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


phng trnh Lagrange bc 2:

d L L
= F Mi

dt q& i q i

,i=1,...,n

(2.16)

Trong :
L- hm Lagrange

L=K-P

(2.17)

K, P- ng nng v th nng ca c h.
FMi - ng lc, hnh thnh trong khp ng th i khi thc hin
chuyn ng.
qi - bin khp (to suy rng)

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


i

ng thi khi m t v tr gia 2 h to th i v i-1 dng ma


trn thun nht Ai hoc vit y hn l i 1 Ai . Dng ma trn ny c th
m t v tr trng thi trong h to th i-1 ca mt im bt k thuc h
to th i.
Cc bin khp qi l b cc thng s dch chuyn ca cc khp ng
ca Robot. V tr trng thi ca im tc ng cui ca Robot hon ton
c xc nh bi b bin khp qi ny.
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 h to i, xc nh bng vc t m rng i ri :
i

ri = ( xi , yi , zi , 1 )

T,

(2.18)

K hiu i ri 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 0 ri th c ngha l
im M cho bit trong h to i, nhng c biu th trong h to x0,
y0, z0, tc l trong h to c bn.
27

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Nh trc y, dng ma trn

i 1

Ai m t v tr tng i gia h

to th i i vi h to i-1 v ma trn 0 Ai m t quan h gia h


to th i v h to c bn.
Vy quan h gia 0 ri v

Ma trn

ri c th biu th nh sau :

ri = 0 Ai i ri

(2.19)

Ai = 0 A1 1 A2 i 1 Ai

(2.20)

vi

i 1

i 1

Ai c t biu thc (2.8):

Ci Ci Si

S
Ci Ci
i 1
Ai = i
0
Si

0
0

SiSi
a i Ci

Si Ci a i Si
Ci
di

0
1

(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 :
Ci Ci Si

S
Ci Ci
i 1
Ai = i
0
Si

0
0

Si Si
Si Ci
C i
0

0
di

(2.22)

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


l bin khp.
Cc phn t khc khng ca ma trn 0 Ai u l hm ca j, dj, j v
aj

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

c th ca tay my. Do vy cc phn t ny l hm ca bin khp qi ni


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

chung

Vi phn biu thc (2.19) vi lu rng cc vect

ri l khng i

vi h to th i v gi thit rng cc khu ca tay my l vt rn tuyt


i, ta c:
Vi Vi =

d 0
d
( ri ) = ( 0Ai i ri ) =
dt
dt

28

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

& 1 A ... i 1 A i r + 0 A 1 A
& ... i 1 A i r + ... + 0 A ...
= A
i
2
i i
i
2
i i
i
0

i 0 Ai i
Vi =
ri
j =1 q j

& i i r + 0 A i r&
A
i
i i

(2.23)

o hm ca ma trn

i 1

i 1

Ai i vi bin khp qi c th d dng xc

nh theo cng thc sau :


d i-1 Ai
= Dii 1 Ai
dqi

(2.24)

Trong i vi khp quay :


0 1
1 0
Di =
0 0

0 0

0 0
0 0
0 0

0 0

(2.25)

v i vi khp tnh tin :


0
0
Di =
0

0 0 0
0 0 0
0 0 1

0 0 0

(2.26)

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


0 Ai

( A1 A2 ... A j 1 A j ... Ai 1 Ai )
=
q j
q j

(2.27)

Trong cc ma trn bn v phi ch c Aj ph thuc vo qj, do theo


(2.24) ta c :
dA j
0 Ai
= A1 A2 ... A j 1
... Ai 1 Ai
q j
dq j

(2.28)

vi Dj tnh theo (2.25) hoc (2.26) :


0 Ai
= A1 A2 ... A j 1 D j ... Ai 1 Ai
q j

(2.29a)

29

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Phng trnh (2.29a) m t s thay i v tr cc im ca khu th i


gy nn bi s dch chuyn ca khp ng th j .
K hiu v tri ca (2.29a) l Uij v n gin ho cch vit (2.29a)
nh sau :
0 A D j 1 A , j i
U ij = j 1 j i
0, j > i

(2.29b)

vy cng thc (2.23) c th vit li l:

Vi = U ijq& j i ri
j =1

(2.30)

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

a=

i
i
2 Ai
dVi i 0 Ai
&
&
=
+
q
q
i ri
&&s
q
s k
dt s =1 qs
s =1 k =1 qs qk

(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:

1
2

1
2

T
dKi = ( x&12 + y&12 + z&12 )dm = Tr ( VV
i i )dm

(2.32)

Trong Tr l vt ca ma trn :
TrA =

a
i =1

ii

Ta c :
T
i

1 i

1
i
dKi= Tr Uip q& p ri U ir q& r ri dm
2 p =1
r =1

1 i i i 1 T T
Tr U ip ri ri U ir q& p q& r dm
2 p=1 r =1

30

n tt nghip
=

Thit k b iu khin trt cho Robot 2 bc t do

1 i i
Tr U ip ( i ri dmi r T i )U Tir q& p q& r
2 p=1 r =1

(2.33)

Nh ni trn ma trn Uij biu th s thay i v tr ca cc im


thuc khu i gy nn bi s dch chuyn ca khp ng j. Ma trn ny u
nh nhau ti mi thi im thuc khu i v khng ph thuc vo s phn
b khi lng trn khu i, tc l khng ph thuc vo dm. Cng vy, o
hm ca bin khp qi theo thi gian khng ph thuc vo dm. Do vy ta c:
i i
1
K i dK i = Tr U ip ( i rii riT dm ) U irT q& p q& r

2 p=1 r =1

(2.34)

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


xi2 dm

x y dm
Ji = i rii riT dm = i i
xi zi dm
x dm
i

x y dm
y dm
z y dm
y dm
i

2
i

x z dm
z y dm
z dm
z dm
i i

2
i
i

x dm
y dm .
z dm
dm
i

(2.35)

Nu dng Tenso qun tnh Iij:


I ij = ij xk2 xi x j dm,

(2.36)

Vi cc ch s i, j, k ly ln lt bng cc gi tr xi , yi, zi, l cc


trc ca h to i, v ij l k hiu Cronecke, th ma trn Ji c th biu th
dng sau:
I +I +I

xx yy zz
Ixy
Ixz
mi xi
2

Ixx Iyy +Izz

Ixy
Iyz
mi yi
2

Ji =

Ixx +Iyy +Izz

Ixz
Iyz
mi zi

mi xi
mi yi
mi zi
mi

(2.37)

31

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do


1

r i = x i , yi ,zi ,1 - bn knh vector biu din trng tm

ca khu th i trong h to i. Cng thc (2.37) vit thnh :


k 2 +k 2 + k 2
i11 i 22 i 33
2

ki12
Ji =

ki13

xi

y K ijk =
i

2
ki12

2
ki13

2
ki222 +ki233
ki11
2

ki223

ki223

2
ki11
+ki222 ki233
2

yi

zi

xi

yi

zi

(2.38)

I jk v j = 1,2,3 ; k = 1, 2, 3
mi


r i = x i , y i , z i ,1

- bn knh vc t biu din trng tm ca

khu th i trong h to i .
Vy, ng nng ca ton c cu tay my bng tng i s ng nng
ca cc khu ng :

1n i i
1n i i
T
T
&
&
&
&
K = Ki = Tr UipJi Uir qp qr = Tr ( Uip JiUir ) qp qr (2.39)
2 i=1 p=1 r=1
i=1

2 i=1 p=1 r=1

Lu rng, cc ma trn Ji (i=1,2,3,n) ch ph thuc vo s phn


b khi lng ca khu i trong h to i m khng ph thuc vo v tr v
vn tc. V th cn tnh ma trn Ji ch 1 ln.
II.3.4 .Th nng tay my:

Th nng Pi ca khu i:
Pi = - mi.g. 0 ri = -mi.g. (0 Ai 1ri )

(2.40)

i=1,2,,n
Trong

32

n tt nghip
i

Thit k b iu khin trt cho Robot 2 bc t do

ri , 0 ri - bn knh vec t biu din trng tm ca khu i trong h

to c bn .
g - vector gia tc trng trng, g = ( 0, 0 , -g, 0)
( gia tc trng trng g = 9,8062 m/s2 )
Th nng ca ton c cu n khu ng :
n

i =1

i =1

P = Pi = mi g 0 A i r i .

(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):
d L L

= FMi , I = 1,2, ,n
dt q qi

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 :

FMi = Tr ( U jk J jU Tji ) &&


q k + Tr ( U jkm J jU Tji ) q& k q& m m jgU ji r j
n

j=1 k =1

j=1 k =1 m =1

i=1,2,,n

j=1

(2.42)

Biu thc (2.42) c th vit gn li nh sau :


n

FMi = Dik &&


q k + h ikm q& k q& m + ci
j=1

(2.43)

k =1 m =1

Hoc l di dng ma trn :


&& + h(q,q)
& + c(q)
FMi = D(q)q

(2.44)

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

33

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

FM (t ) = [FM 1 (t ), FM 2 (t ),..., FMn (t )]

(2.45)

q(t) vector (nx1) bin khp :


q(t ) = [q1 (t ), q2 (t ),..., qn (t )]

(2.46)

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


q(t)

& = [ q& 1 (t),q& 2 (t),...,q& n (t)]


q(t)

(2.47)

&&
q(t) - vect (nx1) gia tc bin khp :
&&
&&2 (t),...,q
&&n (t)]
q(t) = [&&
q1 (t),q

(2.48)

D(q) Ma trn (nxn) , c cc phn t Dik sau y :

Tr (U
n

Dik =

j = max(i , k )

jk

J jU Tji

i,k =1,2, . n

(2.49)

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

& = (h1 ,h 2 ,...,h n )T


h(q,q)
n

h i = h ikm q& k q& m


k =1 m =1

hikm =

Tr (U
n

j = max(i , k , m )

ikm

J jU Tji

i=1,2,,n

(2.50)

i,k,m = 1,2,,n

(2.51)

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


c(q) = (c1 , c2 ,..., cn )

ci = mi gU ji j r j
j =1

(2.52)

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

Trong phn ny dn ra v d minh ha xy dng m hnh ng


lc hc ca c cu tay my 2 khu ton khp (hnh 2.3) .
Nh ch trn hnh 2.3 cc trc Zi u trng phng vi cc trc
khp quay ng. Khi lng ca cc khu tng ng l m1, m2; B thng
s DH ca tay my ghi trong bng sau :

34

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Khu

Ai

di

Khp

i*

i*

Hnh 2.3: C cu tay my 2 khu


Cc ma trn
C1 S1
S C
1
0
A1 = 1
0 0

0 0

i 1

Ai ( i =1,2) s l :

0 lC1
0 lS1
1 0

0 1

C12
S
0
A2 = 0A2 1 A = 12
0

S12
C12

0
0

C2
S
1
A2 = 2
0

S2
C2

0
0

0 lC2
0 lS 2
1 0

0 1

0 l (C12 + C1 )
0 l ( S12 + S1 )

1
0

0
1

vn nh trc 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 :

35

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

0 1
1 0
0A1
0
U11 =
= D1 A1 =
0 0
1

0 0

S1 0 lC1 S1
C1 0 lS1 C1
=
0 1 0 0

0 0 1 0

0 0 C1
0 0 S1
0 0 0

0 0 0

C1 0 lS1
S1 0 + lC1
0
0
0

0
0
0

Tng t, i vi U12, v U22 :


0 1
1 0
A2
= D2 0 A2 =
U 21 =
0 0
1

0 0
0

C2
S
A2 0
U 22 =
= A1 D2 1 A2 = 1
0
2

0
0

0 0 C12
0 0 S12
0 0 0

0 0 0

S12

0 l (C12 + C1 ) S12
0 l ( S12 + S1 ) C12
=
0
1
0

0
0
0

C12
0
0

S1 0 lC1 0 1
C1 0 lS1 1 0
0 1 0 0 0

0 0 1 0 0

0 0 C2
0 0 S 2
0 0 0

0 0 0

S2
C2
0
0

C12
S12
0
0

0 lC2 S12
0 lS 2 C12
=
1 0 0

0 0 0

0 l ( S12 + S1 )
0 l (C12 + C1 )

0
0

0
0

C12
S12
0
0

0 lS12
0 lC12
0
0

0
0

Theo (2.37) v gi thit c cc thnh phn mmen ly tm qun tnh


u bng 0 , ta c :
1 / 3m1l 2

0
J1 =

2
1 / 2m1l

0 0 1 / 2m1l 2

0 0
0
,

0 0
0

0 0
m1

1 / 3m2l 2

0
J2 =

2
1 / 2m2l

0 0 1 / 2m2l 2

0 0
0
.

0 0
0

0 0
m2

Trn c s (2.49), ta c :

T
D11 = T2 U11 J1U11T + T2 U 21 J 2U 21

S12

C
+ T2 12
0
0

C12
S12
0
0

S1

C
= T2 1
0
0

C1 0 lS1 1 / 3m1l 2

S1 0 lC1
0
0
0
0
0

0
0
0 1 / 2m1l 2

0 l ( S12 + C1 ) 1 / 3m2l 2

0 l (C12 + C1 )
0

0
0
0

2
0
0
1 / 2m2l

0 0 1 / 2m1l 2

0 0
0
U T +
11
0 0
0

m1
0 0

0 0 1 / 2m2l 2

0 0
0
U T = 1 / 3m l 2 + 4 / 3m l 2 + m C l 2
1
2
2 2
21
0 0
0

m2
0 0

36

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

T
D12 = D21 = Tr U 22 J 2U 21

S12

C
= T2 12
0
0

C12
S12
0
0

0 lS12 1 / 3m2l 2

0 lC12
0
0
0
0

0
0 1 / 2m2l 2

0
0
0
0

0 1 / 2m2l 2

0
0
U T
21
0
0

m2
0

= m2l 2 (1 / 6 + 1 / 2 + 1 / 2C2 ) = 1 / 3m2l 2 + 1 / 2m2l 2 .

T
D22 = Tr U 22 J 2U 22

S12

C
= T2 12
0
0

C12
S12
0
0

0 lS12 1 / 3m2l 2

0 lC12
0
0
0
0

0
0 1 / 2m2l 2

0 0 1 / 2m2l 2

0 0
0
U T
22
0 0
0

0 0
m2

= 1 / 3m2l 2 S122 + 1 / 3m2l 2C122 = 1 / 3m2l 2 .

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


2

h1 = h1km & k & m = h111 & 1 & 2 + h121 & 1 & 2 + h122 & 22 + h122 & 22
k =1 m =1

V theo (2.51) tnh cc h s hikm, ri thay vo phng trnh trn, ta


c:
h1 = -1/2m2S2l2 & 22 - m2S2l2 & 1 & 2 .
Tng t i vi i = 2
2

h 2 = h 2km & k & m = h 211 & 12 + h 212 & 1 & 2 + h 221 & 2 & 1 + h 222 & 22 = 1/ 2m 2S2l2 & 12
k =1 m =1

Nh vy :
2

2 &
2 & &
1/
2m
S
l

2 m 2S2 l 1 2

2
2
H(, & ) =
.
2

1/ 2m 2S2l2 & 1

Trn c s (2.52), ta c :

2
c1 = m1gU111 r1 + m 2gU 21
r2 =

37

n tt nghip

S1
C
= m(0, g ,0,0) 1
0

Thit k b iu khin trt cho Robot 2 bc t do

C1 0 lS1 1 / 2
S12
C

S1 0 lC1 0
m2 (0, g ,0,0) 12
0
0
0
0 0

0
0
0 1
0

C12
S12
0
0

0 l ( S12 + S1 ) 1 / 2
0 l (C12 + C1 ) 0
=
0
0
0

0
0
1

= 1 / 2m1 glC1 + 1 / 2m2 glC12 + 1 / 2m2 glC1 ;


S12
C
c2 = m2 (0, g ,0,0) 12
0

C12
S12
0
0

0 lS12 1 / 2
0 lC12 0
=
0
0 0

0
0 1

= m2 (1 / 2 glC12 glC12 )

Vy vector trng trng s l:


c 1 / 2m1 glC1 + 1 / 2m2 glC12 + m2 glC1
c( ) = 1 =
.
1 / 2m2 glC12

c 2

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


khu dng sau :
Fm(t) = D()(& )(t) + h(, & ) + c()
FM 1 1 / 3m1l 2 + 4 / 3m2l + m2C2l 1 / 3m2l 2 + 1 / 2m2l 2C2

F =
2
2
1 / 3m2l 2
M 2 1 / 3m2l + 1 / 2m2l C2

2 &
2
1/ 2m 2S2l 2 m 2S2l & 1 & 2
1 / 2m2 glC1 1 / 2m2 glC12 + m2 glC1
.

+
2
1 / 2m2 gl 2C12

1/ 2m 2S2l2 & 1

38

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

39

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

CHNG III : THIT K B IU KHIN TRT CHO TAY MY


ROBOT 2 BC T DO

III.1.H phi tuyn :


III.1.1.H phi tuyn l g ?

nh ngha c r rng mt i tng hay h thng nh th no


c gi l phi tuyn trc tin ta nn nh ngha li h tuyn 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

u1 (t )

u (t ) = M

u (t )
r
y1 (t )

v s tn hiu u ra thnh y(t ) = M


th m hnh h thng c

y (t )
s
quan tm y l m hnh ton hc m t quan h gia vect tn hiu vo
u(t ) v tn hiu ra y(t ) , tc l m t nh x T : u(t ) a y(t ) .

nh x ny (Thng cn gi l ton t - operator) vit li nh sau :

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

nu nh x T tho mn :

T(a1u1 (t )) + T(a 2 u2 (t )) = a1T(u1 (t )) + a 2 T(u2 (t )) ,

(3.1)

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


(3.1) ca h tuyn tnh, trong iu khin, cn c gi l nguyn l xp
chng.
V d : Xt 1 h gm 1 l xo c v 1 vt khi lng m lm 1 v d.
Vt s chuyn ng trn trc nm ngang di tc ng ca lc F (hnh 3.1).

40

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do


..

ms
cs

F
m
s

Hnh 3.1: V d v mt i tng tuyn tnh


Nu F c xem nh l tn hiu vo v qung ng s m vt i
c l tn hiu ra (p ng ca h) th theo cc tin c hc 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 nh v F2 = m &&
s ca chuyn ng. Vi
nguyn l cn bng lc ta c nh x T : F (t ) a s(t ) m t quan h vo / ra
ca h :

ms&& + cs = F .

(3.2a)

Gi s rng di tc ng ca lc F1 h c p ng s1 v ca F2 th
t :

ms&&1 + cs1 = F1
ms&&2 + cs2 = F2
c ngay c

m(a1 &&s1 + a 2 &&s2 ) + c(a1s1 + a 2 s2 ) = a1 F1 + a 2 F2 ,


trong a1 , a2 l nhng s thc tu . Ni cch khc di tc ng
ca lc a1 F1 + a 2 F2
vt s i c mt qung ng l a1s1 + a 2 s2 . Bi vy T tho mn
(3.1) v do trong trng hp |s| tng i nh v lc cn ca l xo c
xc nh gn ng bng cng thc F1 = cs th h thng l xo + vt l mt h
tuyn tnh .

41

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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


v l 2 hng s, m trong thc t ngi ta vn s dng, th quan h vo /
ra ca h s l :

ms&& + cs + s3 =F

(3.2b)

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

m(a1 &&s1 + a 2 &&s2 ) + c(a1s1 + a 2 s2 ) + (a1s13 + a 2 s23 ) a1 F1 + a 2 F2


m(a1 &&s1 + a 2 &&s2 ) + c(a1s1 + a 2 s2 ) + (a1s13 + a 2 s23 ) a1 F1 + a 2 F2
Ni cch khc, di tc ng ca lc a1 F1 + a 2 F th qung ng
ca vt i c khng phi l a1s1 + a 2 s2 . Vy trng hp ny h c 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, h thng phi tuyn c xy dng t


quan h vo ra qua vic thm cc bin x1(t), x2 (t), , xn(t) gi l bin
trng thi, sao cho quan h gia vector tn hiu ra y(t) vi n bin ny v tn
hiu vo u(t) ch cn li thun tu l mt quan h i s. Nhng bin trng
thi ny, v mt ngha vt l, l nhng i lng m s thay i ca n s
quyt nh tnh cht ng hc ca i tng.
V d 1 : T m hnh (3.1) ca i tng l xo + vt, nu thm bin
trng thi x1 = s , x2 = s& s c:
x1
= (1 0). x + 0.F
x2

s = (1 0)

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


trnh vi phn bao gm :

x&1 = x 2
Suy ra t nh ngha v x1 , x2 v :
42

n tt nghip

x& 2 =

Thit k b iu khin trt cho Robot 2 bc t do

c
1
x1 + F
m
m

thu c bng cch thay trc tip x1, x2 vo phng trnh (3.1).
Vit chung hai phng trnh vi phn trn li vi nhau s c :

x. 1 0 1 0
x+ F
x = . = c
1

0
x2 m
m
.

Ni chung, mt h phi tuyn SISO c quan h vo ra gia tn hiu


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

y( n ) = f ( y( n1) ,..., y, y, u)
Trong k hiu y(k) ch o hm bc k ca y(t), tc l

y( k ) =

dky
,
dt k

th vi cc bin trng thi c nh ngha nh sau :

x1 = y, x2 = y&, x3 = &&
y,..., xn = y( n1)
h s c m hnh hai phn: phn cc phng trnh vi phn bc nht

x&1 = x2
M
x& n 1 = xn

x& n = f ( xn ,..., x2 , x1 , u)
v phng trnh i s : Y=x1
Tng qut ln th mt h phi tuyn, sau khi nh ngha cc bin trng
thi x1(t) , x2(t) , , xn(t), s m t bi :
M hnh trng thi tng minh autonom

x = f ( x , u )

y = g( x , u )

x1 (t )

trong x(t ) = M

x (t )
n

43

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

M hnh trng thi tng minh khng autonom

x = f ( x , u , t )
y = g( x, u, t )

hoc m hnh trng thi khng tng minh

f ( x, x, u, t ) = 0
g( x, u, y, t ) = 0
III.1.2.2.Qu o trng thi :

T phng trnh trng thi m t h thng vi mt tn hiu u vo

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


cho trc ta s c c nghim x(t ) m t s thay i trng thi h thng
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) nh mt
th ph thuc tham s t c mi tn ch chiu tng ca t ta c mt qu o
trng thi (hnh 3.2a). Tp tt c cc qu o trng thi ng vi mt tn
hiu u vo u(t ) c nh nhng vi nhng im trng thi ban u x0
khc nhau c gi l h cc qu o trng thi (hnh 3.2b) .

Hnh 3.2a. Qu o trmg thi ca h c 3 bin trng thi

x1

44

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

x2
Hnh 3.2b. H cc qu o trng thi ca h c 2 bin trng thi
III.1.3. im cn bng v im dng ca h thng:
III.1.3.1. im cn bng:

nh ngha 1: Mt im trng thi

c gi l im cn bng

(equilibrium point) nu nh khi ang im trng thi

v khng c mt

tc ng no t bn ngoi th h s nm nguyn ti .
Cn c theo nh ngha nh vy th im cn bng

ca h thng

phi l nghim ca phng trnh:

dx
= f ( x, u, t ) = 0
u= 0
dt

Nh vy im cn bng l im m h thng s nm yn ti , tc
l trng thi ca n s khng b thay i (

dx
= 0 ) khi khng c s tc ng
dt

t bn ngoi ( u = 0 ).
III.1.3.2.im dng ca h :

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


thng nu nh h ang im trng thi xd v vi tc ng u(t ) = ud c
nh, khng i cho trc, th h s nm nguyn ti .
R rng l im dng theo nh ngha va nu s l nghim ca :

dx
= f ( x, ud , t ) u=u = f ( x, ud , t ) = 0
dt
d

45

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

trong ud l cho trc.


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

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


cn bng xe nu nh c mt tc ng tc thi (chng hn nh nhiu tc
thi) nh bt h ra khi xe v a ti im x0 thuc mt ln cn no
ca xe th sau h c kh nng t quay v c im cn bng xe ban
u.
Theo nh ngha trn th ta c th nhn bit c h c n nh hay
khng ti mt im cn bng thng qua dng h cc ng qu o trng
thi ca n. Nu h n nh ti mt im cn bng xe no th mi ng
qu o trng thi x(t ) xut pht t mt im x0 thuc ln cn ca xe u
phi kt thc ti xe
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 h phi tuyn ch c ngha khi i cng


vi im cn bng xe . C th h s n nh ti im cn bng ny, song li
khng n nh im cn bng khc. iu ny cng khc so vi khi nim
n nh h tuyn tnh. V h tuyn tnh thng ch c mt im cn bng
l gc to ( xe = 0 ) nn khi h n nh ti 0 , ngi ta cng ni thm lun
mt cch ngn gn l h n nh .

46

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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


im cn bng xe nn cng c th mc d h n nh ti im cn bng xe
song vi mt ln cn qu nh th s khng c nh ngha s dng. Ni cch
khc, v mt ng dng, n c xem nh khng n nh. Bi vy, i vi
h phi tuyn, vic xc nh xem h c n nh ti im cn bng xe hay
khng l cha m cn phi ch ra min n nh ca n ti xe , tc l phi
ch ra c ln cn O ca xe sao cho h c kh nng t quay v c xe t
bt k mt im x0 no thuc O (hnh 3.3). Min n nh O cng ln th
tnh n nh ca h ti xe cng tt .
Nhim v u tin ca b iu khin l phi gi cho h thng n
nh. Nu nh ban u i tng khng n nh, tc l khi c nhiu t bn
ngoi tc ng a n ra khi im lm vic v n khng c kh nng t
quay v th b iu khin phi to ra tn hiu iu khin dn i tng quay
tr v im 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 h thng l tnh n nh. Ti sao li
nh vy ? T khi nim v tnh n nh ca h thng ti mt im cn
bng c nu trong nh ngha 3 ta thy r nu mt h qu nhy cm
vi tc ng nhiu n ni ch mt tc ng tc thi khng mong mun rt
nh lm cho h b bt ra khi im cn bng (hoc im lm vic) m
sau h khng c kh nng t tm v im cn bng ban u th cht
lng ca h khng th gi l tt c.
Bi vy, kim tra tnh n nh ca h (ti mt im cn bng) cng
nh min n nh O tng ng phi l cng vic u tin ta phi tin hnh
khi phn tch h thng. Tiu chun Lyapunov l mt cng c hu ch gip
ta thc hin c iu .
nh ngha 4 : Mt h thng c m hnh khng kch thch :
~
dx
= f ( x, u, t ) u=0 = f ( x, t )
dt

(3.3)
47

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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

x(0) = x0 tho mn : x0 < => x(t ) < , t 0.


b) n nh tim cn 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

x(0) = x0 tho mn :
lim
x(t ) = 0 .
t
III.1.4.1.Tiu chun Lyapunov:

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


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

dx
= f ( x1 , x2 )
dt

x
vi x = 1
x2

(3.4)

H trn c gi thit l cn bng ti gc to 0 .

Hnh 3.4 : Minh ha khi nim n nh Lyapunov:


Hnh 3.4 minh ho cho nh ngha 4 v tnh 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 h cc ng cong khp kn

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


khng ta ch cn kim tra xem qu do pha x(t ) , tc l nghim ca (3.4) i
48

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

t im trng thi u x0 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 t ngoi vo trong hay khng ( hnh 3.5).
- Nu x(t ) khng ct bt c mt ng cong h v no theo chiu t
trong ra ngoi th h s n nh ti 0 .
- Nu x(t ) ct mi ng cong h v theo chiu t ngoi vo trong
th h s n nh tim cn ti 0 .

Hnh 3.5: Mt gi v vic kim tra tnh n nh ca h ti O


R rng l cn v qu o pha x(t ) ca h khng ct bt c
mt ng cong khp kn thuc h v theo chiu t trong ra ngoi l ti
im ct , tip tuyn ca x(t ) phi to vi vector v , c nh ngha l
vector vung gc vi ng cong theo hng t trong ra ngoi, mt gc

khng nh hn 900. Ni cch khc, h s n nh ti 0 nu nh c c


iu kin:
0 v .

dx
dx
.cos = Tv
dt
dt

(3.5)

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


Vn cn li l lm th no c c cc ng cong v sao cho vic
kim tra iu kin (1.48) c thun tin. Cu tr li l s dng hm xc
nh dng V( x )c nh ngha nh sau :

49

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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


c gi l hm xc nh dng nu :
a) V(0, t ) = 0
b) Tn ti hai hm mt bin, dng 1 ( a ) v 1 (b) lin tc, n iu
tng vi 1 (0) = 2 (0) = 0 sao cho :
0 < 1 ( x ) V( x, t ) 2 ( x )

vi mi x 0

(3.6)

Hm V( x, t ) s xc nh dng trong ton b khng gian trng thi


nu cn c :

lim
(a ) = =>
a 1

lim V( x, t ) = .
x

nh l 1 : H phi tuyn (c th khng autonom) cn bng ti gc to


v khi khng b kch thch th c m t bi hnh :

dx
= f ( x, t )
dt

(3.7)

s n nh Lyapunov ti 0 vi min n nh O nu :
a) Trong O tn ti mt hm xc nh dng V(0, t ) .
b) o hm ca n tnh theo m hnh (1.51) c gi tr khng dng
trong O, tc l :

dV V V
f ( x, t ) 0 vi mi x O .
=
+
dt t x

(3.8)

nh l 2: H phi tuyn (c th khng autonom) cn bng ti gc


to v khi khng b kch thch th c m t bi m hnh.

dx
= f ( x, t )
dt

(3.9)

s n 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 ) .
50

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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


vi x 0 , tc l :

dV V V
f ( x, t ) < 0 vi mi x O v x 0 .
=
+
dt t x

(3.10)

III.1.4.2.Tiu chun Lyapunov phc v thit k b iu khin:

Ngoi vic kim tra tnh n nh, tiu chun Lyapunov cn c s


dng thit k b iu khin n nh i tng phi tuyn. Chng hn i
tng c m hnh :

dx
= f ( x, u)
dt
v c iu khin bng b iu khin phn hi trng thi r( x)

dx
= f ( x, u)
dt

r ( x)
Hnh 3.6: ng dng tiu chun Lyapunov thit k b iu khin
Vy h kn khi khng b kch thch ( = 0 ) s c m hnh :

dx
= f ( x,r( x))
dt
Gi V( x) l hm xc nh dng thch hp, khi h kn n nh
tim cn vi min n nh l O th b iu khin cn tm r( x) phi tho
mn :

Lf V =
V

V
f ( x,r( x)) < 0 vi mi x 0 , x O
x

V
f ( x,r( x)) =0ch khi x = 0
x

(3.11a)
(3.11b)

III.2.Bc tng i ca h phi tuyn:

51

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Bc tng i ca h SISO:

d tip cn ti khi nim bc tng i ta xt trng hp c


bit vi i tng tuyn tnh, m t bng hm truyn t hp thc cht
(strickly proper):
m

G(s) =

b + b + ... + b s
a + a s + ... + a s
0

(3.12)

Khi bc tng i c hiu l hiu r = (n-m) 1


Gi s rng i tng trn, bn cnh hm truyn t (3.12) cn c
m hnh tng ng trong khng gian trng thi :

d x

= A x+ b u
dt

y = cT x

x R n ,A R nxn ,b R nx1,c R 1xn

(3.13)

Vy th do
G(s)= cT(sI-A)-1 b
Ta c :
r

Lm s G(s)=
s

b
a

lim sr [ c (sI A) 1 b ] =
s

Lm
s

k =0

c Ak b
s

k +1 r

b
a

b
a

m
n

m
n

Hn na
Lm
s

k +1 r

= 0 khi k > r-1

nn chui trn tr thnh tng ca hu hn r phn t u tin


T

c Akb
Lm k +1r = bm
s
k =0 s
an
r 1

52

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

T y, v tri bng gi tr hu hn th cn v l :
T
c Ak b =

= 0

khi 0 k r - 2

(3.14)

khi k = r - 1

Ni cch khc, bc tng i r = n-m cn c th c xc nh trc


tip t m hnh trng thi (3.13) ca h theo cng thc (3.14).
Chuyn sang h phi tuyn v vi s gi ca cng thc tnh (3.14),
khi nim bc tng i ca h ALI c 1 tn hiu vo, mt tn hiu ra, c
nh ngha nh sau :
nh ngha 6: Cho h SISO vi cu trc ALI :

d x

= f (x) + h(x)u
dt
y = g(x)

(3.15)

Bc tng i ti im trng thi x ca h l s t nhin r m


trong ln cn x tho mn :

= 0 khi 0 k r-2
Lh L fhg(x) =
0 khi k = r-1

(3.16)

C th thy c ngay rng vi f(x)= Ax , H(x)= b , g(x)=cTx , hai


cng thc (3.14) v (3.16) s ng nht, v :

h
f

g(x)= cTATxLh L f g(x)=cTAkb


k

V d : Xt h Val der Pol c m hnh trng thi nh sau :


khi th do :
Lhg(x)=

g
0
h(x) = [ 1 0 ] = 0
x
1

LhLfg(x) =

(Lf g)
g
h(x) f h(x)
x
x x

53

n tt nghip
=

Thit k b iu khin trt cho Robot 2 bc t do

0
0

x2
1
0
=
(
)
(1 0 )


1 =1 0
2
x

ax 2 (1 bx1 ) x1 1

Bc tng i ca h bng 2 ( ti mi x ).
Tuy nhin, cng cn phi rng h phi tuyn (3.15) c th c bc
tng i khc nhau nhng im trng thi khc nhau. Ngoi ra, khc vi
h tuyn tnh, khng phi bt c im trng thi x no trong khng gian
trng thi, h phi tuyn phng c bc tng i. Chng hn, h s khng c
bc tng i ti im trng thi x0 m trong ln cn ca n c :
Lhg( x )0,LhLfg( x ) 0,,LhLfhg( x ) 0 ,
III.3.Tnh ng hoc khng:

Rt nhiu khi nim s dng trong h phi tuyn c chuyn th t


h tuyn tnh, chng hn khi nim bc tng i, h th ng, cng
nh vy l tnh ng hc khng (zero dynamic). Do , d tip cn ti
khi nim ny, ta nn bt u t h tuyn tnh.
Xt h phi tuyn SISO c m hnh trng thi :
d x

= f (x) + h(x)u
dt
y = g(x )

(3.17)

Tnh ng hc khng (zero dynamic ) ca h (3.17) c nh ngha


nh sau :
nh ngha 7 : Nu h (3.17) c t nht mt im trng thi u x 0

0 v ng vi n l tn hiu iu khin u0(t) sao cho tn hiu u ra y(t)

ng nht bng khng th h c gi l c tnh ng hc khng (zero


dynamic).
Ta c th thy c l h c tnh ng hc khng th cn thit
phi c g(x0) = 0. Gi s rng h (3.17) c bc tng i l r, tc l :

54

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

= 0
LhLfk g(x) =
0

nu 0 k r-2
nu k= r -1

(3.18)

Khi , vi php i trc to vi phi :

g(x)

z1
M
M
r 2


Lg(x)
z r 1
f

r 1

z z r = m(x) = Lg(x)
f

z r +1


m r +1(x)
M

z n
M

m
n (x)

vi Lhmk ( x) = 0 , k=r+1 , , n

h (1.18) cho s c a v dng chun

z2
z1

M
M

zr
d z d z r 1


= z r = a(z) + b(z)u , y = z1
dt dt

z r +1 c1(z)

z n
c n r (z)

(3.19)

Trong
1

A (z) = L rf g( m (z) ) , b (z) =Lh L rf1 g( m (z) ) , ci (z) = Lh mr+1(


1
m (z) )

s dng k hiu :

c1(z)
z r +1

z r +1

z = v i = M , = M v c(z) = M

zn
z n

c n r (z)
55

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

th m hnh (3.19) c vit thnh

z2
z1

M
M

dz d

zr
= z r 1 =

dt dt a(, ) + b(, )u
zr


c(
,
n)

, y = z1

(3.20)

Gi s rng h (3.17) c tnh ng hc khng ng vi trng thi u


x0 0 v tn hiu iu khin u0(t) thch hp. Vy th t y(t) = z1(t) = 0 ta suy
ra c :
z1(t) = = zr(t) =0
v do l = 0. iu ny dn n :
a(0, )

a( 0 , ) + b( 0 , )u0 = 0 u0 (t) = -

b(0, )

(3.21a)

d
dt

= c(0, )

(3.21b)

cng l hai phng trnh phn tch tnh ng hc khng ca h


(3.17) thng qua m hnh tng ng (3.20) ca n. iu kin c
phng trnh (3.21b) l h (3.17) phi c bc tng i r nh hn n (r < n).
T =0 cng nh php bin i trc to (3.18) v 2 phng trnh
(3.21) ta thy, ch ng hc khng, qu o trng thi x(t) phi tho
mn : g( x ) =Lfg( x )== Lrf1 g( x ) = 0 .
Ni cch khc x(t) ca ng hc khng s ch nm trong a tp (hnh
3.7)
K = { x Rn|g( x )=Lfg( x )= = Lrf1 g( x ) = 0 }

(3.22)

56

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

x (t )

Hnh 3.7: Qu o trng thi ca H phi tuyn, khi ang ch


ng hc khng, lun nm trong a tp K.

Tuy rng nm trong a tp K, song vic qu o x(t) ch ng


hc khng (ng vi tn hiu iu khin u0(t) thch hp) c tin v gc to
0 hay khng th cha c m bo v iu ny khng c quyt nh
bi h phi tuyn (3.17) c n nh hay khng. N ch c th tin v 0 n
nh h (3.21b) l n nh tim cn Lyapunov, tc l phi tn ti 1 hm xc
nh dng Q() sao cho :

Q
c(0, ) < 0 khi 0

III.4.Thit k b iu khin trt cho tay my:


III.4.1.iu khin trt:

H phi tuyn c m hnh


x& = f ( x, u ) = f ( x) + g ( x)u

y = h( x )

(3.23)

Trong y l tn hiu u ra, u l tn hiu u vo, x = [x1, x2, ..,


xn] l vector trng thi ca h, f(x) = [f1(x), f2(x), ..., fn(x)]T, g(x) = [g1(x),
g2(x), ..., gn(x)]T
T

57

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

H phi tuyn c bc tng i l p nu:


d p h( x )
= L pf h( x) + L g L pf 1 h( x)u

p
dt

L L p 1 h( x) 0; L Li h( x) = 0, i = 1,2,3,..., p 2
g f
g f

(3.24)

S iu khin:
y r , y& r ,..., y r( n )

y, y& ,..., y ( n )

SMC

x& = f ( x) + g ( x)u

y = h( x )

III.4.1.1.Trng hp bc tng i ca h bng bc ca h p=n:

c th thit k c b iu khin th h (3.23) phi tn ti mt


trt. H (3.23) c mt trt S khi tho mn:
n 1

S = e + i e ( i )

(3.25)

i =1

A( S ) = 1 + 1 S + ... + n 1 S ( n 1)

l a thc Hurwitz c:
lim e(t ) = 0
t

S(0) = 0

(3.26)
(3.27)

iu kin (3.23) trt v im cn bng l phi tho mn iu


kin trt. iu kin trt c xy dng trn c s m bo h kn n
nh tim cn, c ngha l cho h trong hnh trn tn ti 1 hm Lyapunov.
Gi s h c hm Lyapunov c dng sau:
V ( x, t ) =

1 2
S
2

(3.28)

l hm xc nh dng. o hm ca n c dng sau:

58

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

dV
= SS&
dt

(3.29)

H (3.23) n nh tim cn khi (3.29) l hm c du xc nh m:


S > 0, S& < 0
SS& < 0
S < 0, S& > 0

(3.30)

Nh vy S& phi tri du vi S, do vy ta c:


S& = Kh(S )

(3.31)

h(S) cng du vi S do vy tho mn iu kin trt ta c th


chn hm h(S) c cc dng sau: hm du Sig(S), hm bo ho
Saturation(S), hm h(S)=Tan(S)
Theo (3.25) ta c:
n

S& = e& + i 1e (i ) = e& + 1e&& + ... + n e ( n ) = Kh( S )

(3.32)

i =2

Ta c:
e ( n ) = y r( n ) ( Lnf h( x) + Lg Lnf1 h( x)u )

(3.33)

Do vy: e& + 1e&& + ... + n y r( n ) n ( Lnf h( x) + Lg Lnf1h( x)u ) = Kh( S )

(3.34)

Tn hiu iu khin tm c:
u (t ) =

Kh( S ) + e& + 1 e&& + ... + n y r( n ) n Lnf h( x)

n L g Lnf1 h( x)

(3.35)

III.4.1.2. Trng hp bc tng i ca h p<n

H (3.23) phi tho mn ng hc khng.


Xy dng mt trt :

p 1

S = e + i e ( i )

(3.36)

i =1

A( s) = 1 + 1 S + ... + p 1 S ( p 1) l a thc Hurwitz, c lim e(t ) = 0 (3.37)


t

S (0) = 0 , mt trt phi i qua gc to v tho mn iu kin trt.

59

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Hon ton tng t nh trong trng hp trn, ta xy dng hm


Lyapunov c dng sau:
V =

1 2
S xc nh dng
2

V& = SS& xc nh m

Ta c:
S& = Kh( S )
S& = e& + 1e&& + ... + p 1e ( p )
e = y r (t ) y (t )

(3.38)

e ( p ) = y r( p ) (t ) y ( p ) (t )
y ( p ) (t ) = Lpf h( x ) + Lg L(fp 1) h ( x )u
S& = e& + 1e&& + ... + p 1 y r( p ) (t ) p 1 ( Lpf h ( x ) + Lg L(fp 1) h( x )u ) = Kh( S )

Tn hiu iu khin:
u(t ) =

Kh( S ) + e& + 1e&& + ... + p 1 ( y r( p ) Lpf h( x ))

(3.39)

p 1 Lg L(fp 1) h( S )

III.4.2. Thit k b iu khin trt cho tay my n bc t do:

M hnh ng lc hc ca tay my:


(3.40)

= H ( q ) q&& + h ( q, q& )

vi H(q) l ma trn qun tnh xc nh dng, i xng.


Chng ta gi s rng cc gi tr c lng H ( q ) v h( q, q& ) quan h
vi gi tr thc H (q ) v h ( q, q& ) bi bt ng thc sau:
H 1 ( q ) H ( q) ( q )

(3.41)

v h( q, q& ) h( q, q& ) hmax ( q, q& )

(3.42)

vi (q) v hmax ( q, q& ) l nhng hm bit.


Vit li biu thc ng lc hc di dng:
q&& = f ( q, q& ) + B ( q )

(3.43)

60

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Vi f ( q, q& ) = H 1 ( q)h( q, q& )

(3.44)

B ( q ) = H 1 ( q )

(3.45)

Nhim v ca iu khin l tm m men thch hp sao cho vector


v tr q ca tay my bm theo qu o mong mun qd.
Chng ta nh ngha sai lch trng thi e v mt trt nh sau:
(3.46)

e = qd q

(3.47)

S = Ce + e&; C = C T > 0

R rng rng S=0 th q(t ) qq (t ) . Qu thc vi S=0 ta c th vit li


nh sau:
S = 0 Ce + e& = 0 e& = Ce

Nh vy h thng n nh tim cn nu c e = 0 v theo iu kin


bm q(t ) qq (t ) s c m bo.
Do vy vn iu khin l phi tm m men thch hp sao cho
vector trng thi ca h thng c th bm c trn mt trt. Hay phi tm
tha mn iu kin trt. iu kin trt c th xc nh theo tiu chun
Lyapunov.
Chng ta nh ngha hm Lyapunov nh sau:
V =

1 T
S S>0
2

(3.48)

o hm ca (3.48) c dng:
V& = S T S&

(3.49)

Nh vy, nu V& < 0 th vi V 0 dn ti S 0 v e 0

Do vy, iu kin ca iu kin trt l:


S T S& < 0

(3.50)

61

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Khi iu kin trt m bo cho h kn 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 th tha mn theo :
S T S& S 0; > 0; S =

S
i =1

(3.51)

2
i

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


hn T0 :
T0 =

1
S ( q(0))
2

(3.52)

Biu thc trn c chng minh nh sau:


T (29) ta c:
S T S&

S

(3.53)
1
2

Thay V& = S T S& v S = (2V ) vo (3.53) sau tch phn hai v vi


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

V&
( 2V )

1
2

dt =

[ ]

1
1
V2
2

treach

=
0

S ( q(0))
S ( q(0))
t reach t reach
= T0 (3.54)
2
2

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


trt.
Ly o hm biu thc (3.47) ta c:
S& = Ce& + q&& q&&d

(3.55)

Thay biu thc (3.39) vo ta c:


S& = Ce& + f ( q, q& ) + B ( q ) q&&d

(3.56)

Do tn hiu iu khin c dng


= B 1 [ eq KSgn( s )]

(3.57)

62

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

vi:
eq = q&& Ce& f ( q, q& )

(3.58)

Sgn( s ) = [Sgn( s1 ), Sgn( s2 ),..., Sgn( sn )]

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 s khng r, nhiu,
Trong trng hp c lng chnh xc B = B, f = f th iu kin
trt c vit li nh sau:
S T S& = S T KSgn( s ) S

(3.59)
(3.60)

Nu chn K I ; >
m

v S T S& = S = Si
i =1

S
i =1

2
i

= S S

(3.61)

th ch trt 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 s c lng l
khng nh th vic chn K l khng n gin nh biu thc trn.
Trong trng hp S& cho di dng:
S& = q&&d + Ce& + f ( q, q& ) + B( q ) B 1 eq B( q) B 1 KSgn( s )

(3.62)

t f = f + ( f f ); R = B ( q ) B 1 dn ti:
S& = ( R I ) eq + ( f f ) RKSgn( s ) (3.63)

T y, iu kin trt l:

S T S& = S T ( R I ) eq + ( f f& ) RKSgn( s ) S T Sgn( s ) (3.64)

Do vy, nu chn K :

S T RKSgn( s ) S T ( R I ) eq + ( f f ) + S T Sgn( s )

(3.65)

63

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

th iu kin trt nh trn SS& < 0 c tha mn v iu kin


trt t c.
R 1 = B B 1 ( q )

(3.66)

T biu thc (3.41) v (3.42) ta c bt ng thc:


R 1 = B B 1 ( q )

(3.67)

R 1 ( f f ) = B (h h) B hmax

(3.68)

T y, ta c th chn ma trn K tho mn iu kin trt nh sau:


K (1 ) I eq + B hmax + I

(3.69)

III.4.3. ng dng iu khin trt cho tay my Robot 2 bc t


do:
III.4.3.1. Phng trnh ng lc hc tay my hai bc t do ton
khp quay:

B thng s tay my:

m1 = m2 = 1 kg
l1 = l2 = 1 m

Phng trnh ng lc hc:


F1 h11
F = h
2 21

h12 &&1 c11


+
h22 &&2 c 21

c12 &1 g1
+
c 22 &2 g 2

(3.70)

Trong F1, F2 l lc c to ra cc khp ng, ma trn H l ma


trn xc nh dng v i xng, ma trn C l ma trn lc ly tm, G l ma
trn lc trng trng.

Gi tr ca cc ma trn khi thay gi tr c xc nh nh sau:


- Ma trn H:
h11 = 5 + 3 cos 2
h12 = h21 = 1 + 3 / 2 cos 2

(3.71)

h22 = 1

64

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

- Ma trn C:
c11 = 3&2 sin 2
c = 3 / 2& sin
12

(3.72)

c 21 = 3&1 sin 2
c 22 = 0

- Ma trn G:
g1 = 15 cos 1 15 cos(1 + 2 )
g 2 = 15 cos(1 + 2 )

(3.73)

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

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


cc khp tay my:
Khp 1:
x11 = 1

x12 = x&11 = &1


&
&&
x12 = 1

(3.74)

Khp 2:
x 21 = 2

x 22 = x& 21 = &2
&
&&
x 22 = 2

(3.75)

Tn hiu vo u:
u1 = F1

u 2 = F2

(3.76)

T biu thc (3.70) ta c:


&&1
c
F
1 1
1 11
&& = H H
1
F2
c 21

g
c12 &1
1 1
& H

c 22 2
g 2

(3.77)

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


Tnh H-1 v kt hp tt c cc phng trnh trn v thay vo (3.77)
tnh c:
65

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Khp 1

x& = x
12
11
1
3

( u1 + x22 (2 x12 + x22 ) sin x21 15(cos x11 cos x21 ))


x& 12 =
2
4 9 4 cos x21
2

3
3 2
(1 + cos x21 )( u2 x12 sin x21 15 cos( x11 + x21 ))

2
2

(3.78)

Khp 2:

x& = x
21 22
(3.79)
1
3
3

&
=

+
+
+

+
x
x
u
x
x
x
x
x
x
(
(1
cos
)(
(2
)sin
15(cos
cos
)))
22
21
1
22
12
22
21
11
21
4 9 4cos2 x21
2
2

3 2
+(5 + 3cos x21 )(u2 x12 sin x21 15cos( x11 + x21 )))

III.4.3.3. Thit k b iu khin trt cho tay my 2 bc t do:


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

T phng 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:
S1 = e1 + 1e&1
S 2 = e2 + 2 e&2

(3.80)

y:
e1 = x d 1 x r1

(3.81)

e2 = x d 2 x r 2

1, 2 l nhng s thc dng.


iu kin xy ra ch trt cho h trn:
V& = SS& < 0

(3.82)

Xy dng b iu khin:

T (3.78) v (3.79) nu t:

66

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

x&11 = x12

x&12 = f1 ( x) + g1 ( x, u )

(3.83)

v:
x& 21 = x 22

x& 22 = f 2 ( x) + g 2 ( x, u )

(3.84)

Ta c:
S&1 = e&1 + 1&e&1 = e&1 + 1 &x&1d 1 &x&11 = e&1 + 1 &x&1d 1 ( f1 ( x) + g1 ( x, u ))
(3.85)
= (e&1 + 1 &x&1d 1 f1 ( x)) 1 g1 ( x, u ) = K1 h( S1 )
g 1 ( x, u ) =

K1 h( S1 ) + (e&1 + 1 &x&1d 1 f1 ( x))

Tng t ta cng c:
g 2 ( x, u ) =

K 2 h( S 2 ) + (e&2 + 2 &x&2 d 2 f 2 ( x))

(3.86)

Theo (3.77) ta c:
g 1 ( x, u )
1
g ( x, u ) = H u
2

c
f 1 ( x)
1 11
f ( x ) = H c
21
2

(3.87)

c12 x12
g
H 1 1

c 22 x 22
g 2

(3.88)

Ch : g1 g1 ( x, u )
T (3.58) ta c c:
K1 h( S1 ) + e&1 + 1 &x&1d

f 1 ( x)

u1
1

u = H K h( S ) + e& + &x&
2
2
2 2d
2
2
f 2 ( x)

(3.89)

Thay (3.88) vo (3.89) ta c c b iu khin:

67

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

K 1 h( S1 ) + e&1 + 1 &x&1d

u1
1

H
=
u
K
h
(
S
)
e&2 + 2 &x&2 d
+
2
2
2

c11
+
c 21

c12 x12 g1
+
(3.90)
c 22 x 22 g 2

III.4.3.4. Tnh ton gi tr t i cho tay my hai bc t do:

tnh ton gi tr t cho tay my hai bc t do chng ta cn gii


bi ton ng hc ngc, t tnh ton gi tr t cho cc khp:
C12
S
0
A2 = 0 A1 1 A2 = 12
0

S12
C12
0
0

0 l (C12 + C1 )
0 l ( S12 + S1 )

1
0

0
1

(3.91)

theo cch bin i to ta c c:


x0
x2
y

0 = 0 A2 y 2
z0
z2


1
1

(3.92)

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


chuyn ng ca tm bn kp do vy t (3.91) v (3.92) ta c:
x0 = l (C12 + C1 )

y 0 = l ( S12 + S1 )
x02 + y 02
= 2 + 2 cos( 2 )
l2
x 2 + y 02 2l 2
0
= cos( 2 )
2l 2
x 2 + y 02 2l 2
2 = arccos 0
2l 2

(3.93)

(3.94)

T (3.93) ta c:
( x 0 lC1 ) 2 = (lC12 ) 2
x 0 lC1 = lC12

( y 0 lS1 ) 2 = (lS12 ) 2
y 0 lS1 = lS12

(3.95)

68

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

x 02 + y 02 2l ( x 0 C1 + y 0 S1 ) = 0

x 02 + y 02
2l

x0
x 02 + y 02

C1 +

y0
x 02 + y 02

(3.96)

S1

t
x0

cos( ) =
2
x0 + y 02

y0
sin( ) =

x02 + y 02

(3.97)

Chn 1 > ta c:
x2 + y2
0
1 = arccos( 0
+

2l

(3.98)

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


theo mt qu o c nh trc v c xc nh bi:
x = x(t )

y = y (t )
z = z (t ) = 0

th gi tr t cho cc khp phi l:

x2 + y2
0
1 = arccos 0
+

2l

x02 + y 02 2l 2

2 = arccos
2l 2

(3.99)

69

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

CHNG IV: M PHNG QU TRNH CHUYN NG CA ROBOT


DNG B IU KHIN TRT TRN NN MATLAB AND SIMULINK:

IV.1. Tng quan v Matlab-Simulink:

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


Tn ca b chng trnh chnh l t vit tt ca t Matrix 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 s hm ton, cc chc nng
xut nhp cng nh cc kh nng 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 s dng cn. Simulink l mt
Toolbox c vai tr c bit quan trng: vai tr ca mt b cng c mnh
phc v m hnh ho v m phng cc h thng k thut - Vt l, trn c s
s cu trc dng khi.
Giao din ha trn mn hnh ca Simulink cho php th hin h
thng di dng s tn hiu vi cc khi chc nng quen thuc.
Simulink cung cp cho ngi dng mt th vin rt phong ph, c sn vi
s lng ln cc khi chc nng cho cc h tuyn tnh, phi tuyn v gin
on. Hn th ngi s dng c th to 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 th khi
ng qu trnh m phng. Trong cc qu trnh m phng ta c th trch tn
hiu hin ti v tr bt k ca s cu trc v hin th c tnh ca tn hiu
trn mn hnh. Hn th na, nu c nhu cu ta cn c th ct gi cc
c tnh vo mi trng nh. Vic nhp hoc thay i tham s ca tt c
cc khi cng c th thc hin c rt n gin bng cch nhp trc tip
hay thng qua matlab. kho st h thng, ta c th s dng thm cc
Toolbox nh Signal Processing (x l tn hiu), Optimization (ti u) hay
Control System (h thng iu khin).

70

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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

Khp 1:

x& = x
12
11
1
3

+
u
x22 (2 x12 + x22 )sin x21 15(cos x11 cos x21 ))
(
x&12 =
1
4 9 4 cos2 x21
2

3
3 2
(1 + cos x21 )(u2 x12 sin x21 15cos( x11 + x21 ))

2
2
Khp 2:

x& = x
22
21
1
3
3

((1 + cos x21 )(u1 + x22 (2 x12 + x22 )sin x21 15(cos x11 cos x21 ))) +
x& 22 =
2
4 9 4 cos x21
2
2

3 2
+(5 + 3cos x21 )(u2 x12 sin x21 15cos( x11 + x21 )))

Ta c m hnh Simulink ca Robot 2 bc t do:

71

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Khi Subsytem:

y ta s dng cc khi:
+ intergrator: khi tch phn vi cc tham s ca 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:
.

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])))/(42.25*cos(u[3])*cos(u[3]))
.

V: 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]))

72

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

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


ca qu trnh m phng theo thi gian. Nu m ca s Scope sn t trc
khi bt u m phng ta c th theo di trc tip din bin ca tn hiu.
Ta s dng ngun tn hiu u1, u2 l 1(t)
Cc hm x12, v x22 l :
=
(u[5]+1.5*u[4]*sin(u[3])*(2*u[2]+u[4])-15*(cos(u[1])X12
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 v tr th t trn
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 th cc ng c tnh ca cc bin trng
thi x11, x12, x21,x22 l :

73

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

M phng dng hm iu khin:

T cc cng thc:
K 1 h( S1 ) + e&1 + 1 &x&1d

u1
1

=
H
u
+
(
)
K
h
S
e&2 + 2 &x&2 d
2
2
2

c11
+
c 21

c12 x12 g1
+
c 22 x 22 g 2

- Ma trn H:
h11 = 5 + 3 cos 2
h12 = h21 = 1 + 3 / 2 cos 2
h22 = 1

- Ma trn C:
c11 = 3&2 sin 2
c = 3 / 2& sin
12

c 21 = 3&1 sin 2
c 22 = 0

74

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

- Ma trn G:
g1 = 15 cos 1 15 cos(1 + 2 )
g 2 = 15 cos(1 + 2 )

Ta c:
U1=h11

K1h( S1 ) + e&1 + 1 &&


x1d
K h( S ) + e&2 + 2 &&
x2 d
+h12 2 2
1
2

+c11x12+c12x22+15cos(x11)
-15sos(x11+x21)
U2=h21

K1h( S1 ) + e&1 + 1 &&


x1d
K h( S ) + e&2 + 2 &&
x2 d
+h22 2 2
+c21x12+c22x22
1
2

+15cos(x11+x21)
Chuyn v dng hm ca s Simulink:
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]=

K1h( S1 ) + e&1 + 1 &&


x1d
1

U[6]=

K2 h( S2 ) + e&2 + 2 &&
x2 d
2

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 tr upper v gi tr lower.
t ta c:
u[5]=

K1
1 .
H(S1)+ e1 + &x&1d ( Khi subsytem)
1
1

75

n tt nghip
u[6]=

Thit k b iu khin trt cho Robot 2 bc t do

K2
1 .
H(S2)+
e2 + &x&2 d ( Khi subsytem2)
2
2

76

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

77

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

Ta chn khong thi gian m phng l t 0 ->30 s , tc gi tr stop


time = 30 trong Congiguration Parameters.
Khi chay s Simulink ta c cc kt qu ng c tuyn ca 2
hm iu khin U1, U2, v sai s e&1 , e&2 l:
Ta c cc gi tr t xd1 v xd2 l:
T cc cng thc:
x0 = l (C12 + C1 )

y 0 = l ( S12 + S1 )
x0

cos( ) =
x02 + y 02

y0
sin( ) =
2

x0 + y 02

78

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

x2 + y2
0
1 = arccos 0
+

2l

x02 + y 02 2l 2

2 = arccos
2l 2

=>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)

79

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do


Kt lun

Cc vn c gii quyt :

Cc vn cn tn ti :

Ti liu tham kho :

80

n tt nghip

1.

Thit k b iu khin trt cho Robot 2 bc t do

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

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


2.
ng linh hot. NXB KHKT 1991.
Nguyn Phng Quang: iu khin Robot cng nghip 3.
Nhng vn cn bit. Tp ch T ng ho ngy nay - s thng 4, 5, 6 /
2006.
Nguyn Thin Phc: Robot - Th gii cng ngh cao ca
4.
bn. NXB KHKT 2005.
5.

o Vn Hip: K thut Robot. NXB KHKT 2002.

6.

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

2003.
7.
Nguyn Hng Thi: Xy dng thut ton iu khin v m
phng ng Robot nhiu bc t do. Th vin HBK HN 2002.
L Huy Tng: iu khin trt thch nghi ng cho i
8.
tng phi tuyn c m hnh khng tng minh. Th vin HBK HN
2003.
Nguyn Don Phc, Phan Xun Minh, Hn Thnh Trung: L
9.
thuyt iu khin phi tuyn. NXB KHKT 2003.
10.
Nguyn Don Phc, Phan Xun Minh: H phi tuyn. NXB
KHKT 2000.
11.
Nguyn Thng Ng: L thuyt iu khin t ng hin
i. NXB KHKT 2003.
12.
Nguyn Thng Ng: L thuyt iu khin thng thng
v hin i. NXB KHKT 2002.
Nguyn Don Phc: L thuyt iu khin tuyn tnh.
13.
NXB KHKT 2002.

81

n tt nghip

Thit k b iu khin trt cho Robot 2 bc t do

14. Nguyn Phng Quang: Matlab & Simulink dnh cho k s


iu khin t ng. NXB KHKT 2004.
15.

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

16.

Solomon: Stability of nonlinear control systems. 1965.

Applied Asymptotic Methods in Nonlinear Oscillitions.


17.
Th vin 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 6th 2004.
21. C.Abdallah, D.Dawson, P.Dorato, and M.Jamshidi: Survey of
Robust Control for Rigid Robots .
22. Mark W.Spong: Motion control of Robot Manipulators. The
coordinated Science Laboratory, University of Illinois at UrbanaChampaign, 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.

82

You might also like