BCTNLVTNTLVN

n tt nghip Thit k b iu khin trt cho Robot 2 bc t do
TRNG .
KHOA
----------
BO CO TT NGHIP
ti:
Thit k b iu khin trt cho tay my Robot 2
bc t do v m phng trn Matlab Simulink
SV: Hong c Thng - Phan Thanh Hi
1
Mc lc
Li ni u....................................................................................................5
CHNG I: TNG QUAN V ROBOT CNG NGHIP.........................6
I.1.Robot cng nghip:..............................................................................6
I.1.1. S ra i ca Robot cng nghip : ..............................................6
I.1.2.Phn loi tay my Robot cng nghip:.........................................7
I.2. ng dng ca Robot cng nghip : ...................................................9
I.2.1.Mc tiu ng dng Robot cng nghip : ......................................9
I.2.2.Cc lnh vc ng dng Robot cng nghip : ..............................10
I.2.3. Cc xu th ng dng Robot trong tng lai :.............................10
I.2.4. Tnh hnh tip cn v ng dng Robot cng nghip Vit Nam :
.............................................................................................................10
I.3.Cu trc ca Robot cng nghip:.......................................................11
I.3.1.Cc b phn cu thnh Robot cng nghip : ..............................11
I.3.2.Bc t do v cc to suy rng : .............................................12
I.3.2.1.Bc t do : ...........................................................................12
I.3.2.2. To suy rng : ...............................................................13
I.3.3.Nhim v lp trnh iu khin Robot: ........................................13
I.3.3.1. nh v v nh hng ti im tc ng cui : .............13
I.3.3.2. Lp trnh iu khin Robot cng nghip : ..........................14
I.4. Cc php bin i ton hc cho Robot : ..........................................15
I.4.1.Bin i to dng Ma trn:....................................................15
I.4.1.1. Vector im v to thun nht : ....................................15
I.4.1.3.Bin i Ma trn dng to thun nht:............................18
I.4.1.4. ngha hnh hc ca Ma trn thun nht:..........................19
I.4.2.Cc php bin i c bn:...........................................................21
I.4.2.1.Php bin i tnh tin:........................................................22
I.4.2.2. Php quay quanh cc trc to :.......................................22
CHNG II: H PHNG TRNH NG HC V NG LC HC
CA ROBOT CNG NGHIP:.................................................................23
II.1. H phng trnh ng hc Robot : .................................................23
II.1.1. t vn : ..............................................................................23
II.1.2. Xc nh trng thi ca Robot tai im tc ng cui : ...........24
2
II.1.3. M hnh ng hc : ..................................................................25
II.1.3.1. Ma trn quan h : ..............................................................25
II.1.3.2. B thng s DH : ..............................................................25
II.1.3.3. Thit lp h to : ..........................................................27
II.1.3.4. M hnh bin i : .............................................................28
II.1.3.5. Phng trnh ng hc : ...................................................29
II.2. Tng hp chuyn ng Robot : ......................................................29
II.2.1. Nhim v : ...............................................................................29
II.2.2. Bi ton ng hc ngc : .......................................................30
II.2.3. Cc phng php gii bi ton ng hc ngc : ...................30
II.3. ng lc hc Robot:........................................................................31
II.3.1.Nhim v v phng php phn tch ng lc hc Robot:......31
II.3.2.Vn tc v gia tc:.....................................................................32
II.3.3. ng nng tay my:..................................................................35
II.3.4 .Th nng tay my: ...................................................................37
II.3.5.M hnh ng lc hc tay my: ................................................38
II.3.6. ng lc hc ca c cu tay my 2 khu:................................39
CHNG III : THIT K B IU KHIN TRT CHO TAY MY
ROBOT 2 BC T DO..............................................................................44
III.1.H phi tuyn :..................................................................................44
III.1.1.H phi tuyn l g ? ..................................................................44
III.1.2.M hnh trng thi v qu o trng thi ca H phi tuyn: ...46
III.1.2.1.M hnh trng thi: ...........................................................46
III.1.2.2.Qu o trng thi :...........................................................48
III.1.3. im cn bng v im dng ca h thng:...........................49
III.1.3.1. im cn bng:.................................................................49
III.1.3.2.im dng ca h :...........................................................49
III.1.3.3 Tnh n nh ti mt im cn bng: ................................50
III.1.4 Tiu chun n inh Lyapunov :................................................51
III.1.4.1.Tiu chun Lyapunov:.......................................................52
III.1.4.2.Tiu chun Lyapunov phc v thit k b iu khin: ....55
III.2.Bc tng i ca h phi tuyn:......................................................56
III.3.Tnh ng hoc khng:......................................................................58
III.4.Thit k b iu khin trt cho tay my:......................................61
III.4.1.iu khin trt:......................................................................61
III.4.1.1.Trng hp bc tng i ca h bng bc ca h p=n:. .62
III.4.1.2. Trng hp bc tng i ca h p<n.............................63
III.4.2. Thit k b iu khin trt cho tay my n bc t do:............64
III.4.3. ng dng iu khin trt cho tay my Robot 2 bc t do:. .68
III.4.3.1. Phng trnh ng lc hc tay my hai bc t do ton
khp quay:.......................................................................................68
III.4.3.2. M hnh ng lc hc tay my hai bc t do:..................69
III.4.3.3. Thit k b iu khin trt cho tay my 2 bc t do:.....70
3
III.4.3.4. Tnh ton gi tr t i cho tay my hai bc t do:...........72
CHNG IV: M PHNG QU TRNH CHUYN NG CA
ROBOT DNG B IU KHIN TRT TRN NN MATLAB AND
SIMULINK: ...............................................................................................74
IV.1. Tng quan v Matlab-Simulink: ...................................................74
IV.2. Cc thao tc thc hin m phng: .................................................75
Kt lun ......................................................................................................84
Ti liu tham kho :.....................................................................................84
4
Li ni u
Trong s nghip cng nghip ho, hin i ho t nc vn t
ng ho sn xut c vai tr c bit quan trng.
Mc tiu ng dng k thut Robot trong cng nghip nhm nng cao
nng sut dy chuyn cng ngh, nng cao cht lng v kh nng cnh
tranh ca sn phm, ng thi ci thin iu kin lao ng. S cnh tranh
hng ho t ra mt vn thi s l lm sao h thng t ng ho sn
xut phi c tnh linh hot nhm p ng vi s bin ng thng xuyn
ca th trng hng ho. Robot cng nghip l b phn cu thnh khng
th thiu trong h thng sn xut t ng linh hot .
Gn na th k c mt trong sn xut. Robot cng nghip c mt
lch s pht trin hp dn. Ngy nay, Robot cng nghip c dng rng
ri nhiu lnh vc sn sut. iu xut pht t nhng u im c bn
ca cc loi Robot c la chn v c kt qua bao nhiu nm ng
dng nhiu nc.
nc ta, trc nhng nm 1990 hu nh cha du nhp v k thut
Robot. T nm 1990 nhiu c s cng nghip bt u nhp ngoi nhiu
loi Robot phc v cc vic nh tho lp dng c cho cc trung tm CNC,
lp rp cc linh kin in t, hn v xe t, xe my v phun ph b mt
C nhng ni bt u thit k ch to v lp rp Robot. C th ni,
Robot v ang gp phn rt ln vo s nghip cng nghip ho, hin i
ho t nc. Vi nhng ngha to ln ca Robot cng nghip, chc
chn ngnh cng nghip ch to v ng dng Robot s pht trin rt mnh
trong tng lai.
Trong lnh vc Robot hin nay, phn C kh (Robot Mechanics), h
thng iu khin (Robot control) v h thng Lp trnh (Programming
system) c coi l cc thnh phn c lp v c cc nh sn xut cho
bn c lp. V vy, vi nhng kin thc hc v c s hng dn ca
thy gio Th.S L Huy Tng, chng ti nghin cu ti: Thit k b
iu khin trt cho tay my Robot 2 bc t do v m phng trn
Matlab Simulink.
5
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
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
6
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 :
7
Handling
Equipments
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
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
8
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
9
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
10
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.
11
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
i
1
W= 6n - i
p
(1.1)
vi n : s khu ng
P
i
: 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 .
12
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 :
(1 )
S i i i i
i
q

+
(1.2)
vi
'
1,i v i kh p quay
0,i v i kh p tnh tin
i
i
- dch chuyn gc ca cc khp quay
Si
- 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.
Qu o trong h to
cc
(x
E
,y
E
,z
E
,,,)
Qu o trong h to
cc
(x
E
,y
E
,z
E
,,,)
13
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 x
E
, y
E
, z
E
trong h
trc to c nh. Cn hng tc ng ca khu cui c th xc nh
bng 3 trc x
n
,y
n
, z
n
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 x
0
, y
0
, z
0
c nh.
mi v tr m im E i qua xc nh bng 3 to c nh x
E
, y
E
, z
E
v 3
thng s gc nh hng
, ,
. T cc thng s trong h to cc
tnh ton cc gi tr bin khp q
i
tng ng vi mi thi im t. l ni
dung ca bi ton ng hc ngc s trnh by trong chng II.
Qu o trong h to
cc
(x
E
,y
E
,z
E
,,,)
Qu o trong h to
cc
(x
E
,y
E
,z
E
,,,)
14
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 o
i
x
i
y
i
z
i
im M xc nh bng vector r
i
:
i
( , ,
)
r
T
xi yi zi r r r
(1.3)
v cng im M trong h to o
j
x
j
y
j
z
j
c m t bi vector r
j
:
j
( , ,
)
r
T
xj yj zj r r r
(1.4)
Qu o trong h to
cc
(x
E
,y
E
,z
E
,,,)
Qu o trong h to
cc
(x
E
,y
E
,z
E
,,,)
Chng trnh iu khin
H tr ng chp hnh
H tr ng chp hnh ROBOT
My tnh
q
1
q
2
Cc gin Bin i
q
i
(t)
15
K hiu ( )
T
l biu th php chuyn v (Transportation) vector hng
thnh vector ct.
Hnh 1.6: Biu din 1 im trong khng gian
Vector ( , ,
)
r
T
x y z r r r
trong khng gian 3 chiu, nu c b sung
thm mt thnh phn th 4 v th hin bng 1 vector m rng :
( , , )
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 th b qua k hiu ( ) i vi vector m rng (1.5)
Cc to thc ca vector m rng ny vn l:
x
x
r
r

y
y
r
r

z
z
r
r

(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:
( , ,
)
T
x y z
r
r r r
(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.
16
y
j
x
i
x
j
z
j
r
j
r
i
y
i
O
i
M
z
i
I.4.1.2.Q u ay 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)

Hnh 1.7: Cc h to
Gi (i
x
, j
y
, k
z
) v (i
u
, j
v
, k
w
) 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:
r
xyz
=( r
x
,r
y
,r
z
)
T
(1.8)
cn trong h to OUVW bng vector:
r
uvw
= ( r
u
,r
v
,r
w
)
T
(1.9)
Nh vy :
r = r
uvw
= r
u
i
u
+ r
v
j
v
+ r
w
k
w
r = r
xyz
= r
x
i
x
+ r
y
j
y
+ r
z
k
z
(1.10)
T ta c
.
.
.
x u v w x x u x x w
v
y u v w u w
y y y v y
z u v w z z u z z w
v
r
r
r
j
i i i i i k r r r r
j j j j j
i k r r r r
j
k k i k k k r r r r
+ +
+ +
)
+ +

(1.11)
Hay vit di dng ma trn:
17
U
Y
V
M
W
Z
X
.
x u x x w
v x u
y v u w
y y v y
z w
z u z z w
v
j
i i i i k
r r
j j j j
i k r r
r r
j
k i k k k
_
_ _

, ,
,
(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:
1
.
.
xyz uvw
uvw xyz
R
R
r r
r r
(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 o
j
x
j
y
j
z
j
sang h
to mi o
i
x
i
y
i
z
i
. Chng khng nhng quay tng i vi nhau m tnh
tin c gc to : gc o
j
xc nh trong h x
i
y
i
z
i
bng vector p:
p=(a,-b,-c,1)
T
(1.14)
Gi s v tr ca im M trong h to x
j
y
j
z
j
c xc nh bng
vector r
j
:
r
j
= (x
j
y
j
z
j
,1)
T
(1.15)
v trong h to x
i
y
i
z
i
im M c xc nh bng vector r
i
:
r
i
= (x
i
y
i
z
i
,1)
T
(1.16)
T hnh (1.8) c th d dng thit lp mi quan h gia cc to :
cos sin
sin cos
1
i j j
j j
i j
i j j
j
i j
a
b
c
x x t
y y
t z
y
t z z
t t

+

)
+

(1.17)
18

Hnh 1.8: Cc h to
Sp xp cc h s ng vi x
j
,y
j
,z
j
v t
j
thnh mt ma trn:
1 0 0
0 cos sin
0 sin cos
0 0 0 1
ij
a
b
c
T

1
1

1
1
1
]
(1.18)
v vit phng trnh bin i to nh sau:
r
i
= T
ij
r
j
(1.19)
Ma trn T
ij
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 :

_

,
ij
1 0 0
0 os -sin -b
0 sin os
0 0 0 1
a
c
T
c c
(1.20)
y
j
z
i
x
i
y
i
c
o
i
b
a
z
j
o
j
x
j
19
Hoc vit rt gn l:
ij
ij
0 1
R p
T
_

,
(1.21)
Trong :
ij
R
- ma trn quay 3x3
p ma trn 3x1 biu th 3 to ca im gc h to 0
j
trong h
to o
i
, x
i
, y
i
, z
i

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 a
44
biu th h s t
l.
D dng nhn thy ma trn
ij
R
chnh l ma trn quay 3x3, nu suy t
ma trn quay trong (1.12) sang trng hp hnh 1.8 ta c:
_

1
]

,
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 ch phng ny u lin h n gc
(hnh 1.8).
Nu ch v quan h gia 2 cp trc,v d, cos(x
i
,y
j
) = cos(y
i
, x
j
)
y d dng nhn c biu thc:
-1 T
ij ij ij
R R R
(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 r
uvw
, th trong h to thun
nht XYZ im xc nh bng vector m rng r
xyz
:
R
xyz
= T.r
uvw
(1.24)
20
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)
hoc
0 1
R p
T
_

,
(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:
w
(1,0,0,1)
T
uv
r
tc l r
xyz
= i
u

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
w
(0,1,0,1)
T
uv
r
v
w
(0,0,1,1)
T
uv
r
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:
21
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:
1 0 0
0 1 0
0 0 1
0 0 0 1
x
y
z
T
p
p
p
1
1
1
1
1
1
]
=
( , , )
p
x y z
p p p
T (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:
( , , )
T
u x y z
v p l vector ch hng v di cn dch chuyn
( , , )
T
x y z
p
p p p
th v l vector biu din im to trong khng gian c tnh

tin ti:
v (
p T
v ( , , ) u
T
p
x y z
p p p
T
(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
x u i i
:
1 0 0 0
0 cos sin 0
( , )
0 sin cos 0
0 0 0 1
R x

1
1
1
1
]
(1.29)
Tng ng cho trng hp quay quanh trc OY mt gc
:
22
cos 0 sin 0
0 1 0 0
( , )
sin 0 cos 0
0 0 0 1
R y

1
1
1
1
1
]
(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
1
1
1
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
khu ny c mt hng nht nh theo yu cu. Khu cui cng ny
23
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 T
E
:
1
1
1
1
1
]
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 t ca ma trn 3x1 l to p
x
, p
y
, p
z
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 .
s - vector c hng ng trt (sliding) ng m bn kp .
24
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 r
i
v trong h to c nh x
0
, y
0
, z
0
c xc nh
bng bn knh vector r
0
:
r
0
= A
1
A
2
A
i
r
i
(2.2)
hoc r
0
= T
i
r
i
(2.3)
vi T
i
= A
1
A
2
A
i
, i= 1, 2, n (2.4)
Trong ma trn A
1
m t v tr hng ca khu u tin; ma trn
A
2
m t v tr v hng ca khu th 2 so vi khu u; ma trn A
i
m t
v tr v hng ca khu th i so vi khu th i-1.
Nh vy, tch ca cc ma trn A
i
l ma trn T
i
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 :

0 1
1 i i i
T T A T
(2.5)
vi
1
2 3
...
i i
T A A A
(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 :
25
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 :
a
i
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 .
d
i
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
26
Nu khp ng i l khp tnh tin th bin khp l d
i
k hiu thm bin khp dng thm du * v trong trng hp
khp tnh tin th a
i
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 (a
i
) 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 z
i
ca h to th i nm dc theo trc khp ng i+1.
Trc x
i
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 x
i
trng vi hng vector tch z
i
x z
i-1
, tc l vung gc vi mt
phng cha z
i
, z
i-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)
27
Gn cc h to vi cc khu nh hnh v :
- Trc z
0
, z
1
v z
2
vung gc vi mt t giy.
- H to c nh l o
0
x
0
y
0
z
0
chiu x
0
hng t o
0
n o
1
.
- H to o
1
x
1
y
1
z
1
c gc o
1
t ti tm trc khp ng 2.
- H to o
2
x
2
y
2
z
2
c gc o
2
t ti tm trc khp ng cui
khu 2.
Bng thng s DH ca tay my ny nh sau :
Khu
i

i
a
i
d
i
1
*
1
0 a
1
0
2
*
2
0 a
2
0
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 z
1-1
gc
i
.
+ Tnh tin dc trc z
i-1
mt on d
i
.
+ Tnh tin dc trc x
i-1
( trng vi x
i
) mt on a
i
.
+ Quay quanh trc x
i
mt gc
i
.
Bn php bin i ny c biu th bng tch cc ma trn thun
nht sau
A
i
= R(z,
i
).T
p
(0,0,d
i
).T
p
(a
i
,0,0).R(x,
i
) (2.7)
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:
28

1
1
1
1
]
0
0 0 0 1
i i i i i i i
i i i i i i i
i
i i 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 t v tr v hng im cui E ca
tay my c miu t :
T
n
= A
1
A
2
A
n
(2.9)
Mt khc, h to ti im tc ng cui ny c m t bng
ma trn T
E
. V vy hin nhin l:
T
E
= T
n
(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
1
1
1
1
1
]
(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 q
i
(t), (i = 1,..., n), vi q
i
l to suy rng hoc l bin khp.
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
29
bin khp q
i
. 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 :
T
n
= A
1
A
2
A
n
=
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
1
1
1
1
1
]
(2.12)
Cc ma trn A
i
l hm ca cc bin khp q
i
. Vector nh v bn kp
p = (p
x
,p
y
,p
z
)
T
cng l hm ca q
i
. 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 q
i
(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:
.
i
n i n T T T
(2.13)
Nhn 2 v ca (2.13) vi
1
i T
ta c:
A
i
-1
...A
2
-1
A
1
-1
T
n
=
Tn
i
(2.14)
Kt hp (2.12) ta c:
30
n
i
T
=A
i
-1
...A
2
-1
A
1
-1
1
1
1
1
]
1
1 0 0 0
z z z z
y y y y
x x x x
p a s n
p a s n
p a s n
(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 q
i
.
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 q
i
(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.
Cc phng trnh ng lc hc Robot c thit lp da trn c s
phng trnh Lagrange bc 2:
Mi
i i
d L L
F
dt
q q
_

,
&
,i=1,...,n (2.16)
Trong :
31
L- hm Lagrange L = K - P (2.17)
K, P- ng nng v th nng ca c h.
F
Mi
- ng lc, hnh thnh trong khp ng th i khi thc hin
chuyn ng.
q
i
- bin khp (to suy rng)
i
q&- o hm bc nht ca bin khp theo thi gian.
ng thi khi m t v tr gia 2 h to th i v i-1 dng ma
trn thun nht A
i
hoc vit y hn l
1
i
i
A
. 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 q
i
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 q
i
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
i
r
:

i
i
r
= ( x
i
, y
i ,
z
i ,
1 )
T ,
(2.18)
K hiu
i
i
r
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
i
r
0
th c ngha l
im M cho bit trong h to i, nhng c biu th trong h to x
0
,
y
0
, z
0
, tc l trong h to c bn.
Nh trc y, dng ma trn
i
i
A
1
m t v tr tng i gia h
to th i i vi h to i-1 v ma trn
i
A
0
m t quan h gia h
to th i v h to c bn.
Vy quan h gia
i
r
0
v
i
i
r
1
c th biu th nh sau :
32

i
r
0
=
i
A
0

i
i
r
(2.19)
vi
i
A
0
=
1
0
A
2
1
A
i
i
A
1
(2.20)
Ma trn
i
i
A
1
c t biu thc (2.8):
i
i
A
1
=
1
1
1
1
]
1
1 0 0 0
S 0
a S
a S S
i
i i
i i i
i i
i
i i
i i
i
i i
i
d C
S C S
S C
C C
C C

(2.21)
Biu thc (2.21) l vit cho trng hp khp quay i, cn nu khp
ng l khp tnh tin th a
i
=0 v t (2.21) ta c :
i
i
A
1
=
1
1
1
1
]
1
1 0 0 0
S 0
0 S
0 S S
i
i
i i
i i
i i
i i
i i
i
d C
C S
S C
C C
C

(2.22)
i vi khp quay th
i
l bin khp v i vi khp tnh tin th d
i
l bin khp.
Cc phn t khc khng ca ma trn
i
A
0
u l hm ca
j
, d
j
,
j
v
a
j
(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 q
i
ni
chung (q
i

j
i vi khp quay v q
i
d
i
i vi khp tnh tin).
Vi phn biu thc (2.19) vi lu rng cc vect
i
i
r
l khng i
vi h to th i v gi thit rng cc khu ca tay my l vt rn tuyt
i, ta c:
i
V
0
V
i
=
) ( ) (
0 0
i
i
i i
r A
dt
d
r
dt
d
=
=
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
i
i
j j
i
i
r
q
A
V
1
1
]
1

1
0
0
(2.23)
33
o hm ca ma trn
i
i
A
1
i vi bin khp q
i
c th d dng xc
nh theo cng thc sau :
i
i
i
i
i
A D
dq
A
1
1 - i
d

(2.24)
Trong i vi khp quay :
D
i
=
1
1
1
1
]
1

0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 0
(2.25)
v i vi khp tnh tin :
D
i
=
1
1
1
1
]
1
0 0 0 0
1 0 0 0
0 0 0 0
0 0 0 0
(2.26)
Trong trng hp i = 1, 2,,n ta c :

) ... ... (
1 1 2 1
0
i i j j
j j
i
A A A A A A
q q
A

(2.27)
Trong cc ma trn bn v phi ch c A
j
ph thuc vo q
j
, do theo
(2.24) ta c :
i i
j
j
j
j
i
A A
dq
dA
A A A
q
A
1 1 2 1
0
... ...

(2.28)
vi D
j
tnh theo (2.25) hoc (2.26) :
i i j j
j
i
A A D A A A
q
A
1 1 2 1
0
... ...

(2.29a)
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 U
ij
v n gin ho cch vit (2.29a)
nh sau :
34
0 1
1
,
0,
'
>
j
j j i
ij
A D A j i
U
j i
(2.29b)
vy cng thc (2.23) c th vit li l:
V
i
=
ij
1
i
i
j i
j
q r
U
1
1
]
&
(2.30)
Tip theo, t biu thc (2.23) xc nh gia tc:
0 2
1 1 1
i i i
i
i i i
s k i
s
s s k
s s k
dV A A
a q q r
dt q q q
q

1

+ 1

1
]

&&
&& (2.31)
II.3.3. ng nng tay my:
K hiu K
i
l ng nng ca khu i ( i =1,2, , n) v dK
i
l ng
nng ca mt cht im khi lng dm thuc khu i:
2 2 2
1 1 1
1 1
( ) ( )
2 2
T
i i i
dK x y z dm Tr VV dm + + & & & (2.32)
Trong Tr l vt ca ma trn :
TrA =
n
i
ii
a
1
Ta c :
dK
i
=
i
1
ir
1 r 1
1
2
T
i
i
p ip i r i
p
Tr U q r U q r dm

1
1
1
1
]
]

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

1
1
]
& &

=
i i
i i T T
p r ip i i ir
p 1 r 1
1
Tr U ( r dmr )U q q
2

1
1
]
&&
(2.33)
Nh ni trn ma trn U
ij
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
35
b khi lng trn khu i, tc l khng ph thuc vo dm. Cng vy, o
hm ca bin khp q
i
theo thi gian khng ph thuc vo dm. Do vy ta c:
( )
i i
i i T T
r i i ip i i ir p
p 1 r 1
1
K dK Tr U r r dm U q q
2

1

1
]
& & (2.34)
Phn trong ngoc n l ma trn qun tnh J
i
ca khu i:
J
i
=

1
1
1
1
1
]
1
dm dm z dm y dm x
dm z dm z dm y z dm z x
dm y dm y z dm y dm y x
dm x dm z x dm y x dm x
dm r r
i i i
i i i i i i
i i i i i i
i i i i i i
T
i
i
i
i
2
2
2
. (2.35)
Nu dng Tenso qun tnh I
ij
:

,
2
ij ij
dm x x x I
k
j i k
1
]
1
1
]
1

(2.36)
Vi cc ch s i, j, k ly ln lt bng cc gi tr x
i
, y
i
, z
i
, l cc
trc ca h to i, v
ij
l k hiu Cronecke, th ma trn J
i
c th biu th
dng sau:

xx yy zz
i
xy xz i
xx yy zz
xy yz i i
i
xx yy zz
i
xz yz i
i i
i i i i i
I I I
I I m x
2
I I I
I I m y
2
J
I I I
I I m z
2
m x m y m z m
1
+ +
1
1
1
+
1
1
1
+ +
1
1
1
1
]
(2.37)
y
T
1
i i i
i
r x , y , z ,1
_

,
- bn knh vector biu din trng tm
ca khu th i trong h to i. Cng thc (2.37) vit thnh :
36

2 2 2
2 2 i 11 i 22 i 33
i
i 12 i 13
2 2 2
2 2 i 11 i 22 i 33
i 12 i 23 i
i
2 2 2
2 2 i 11 i 22 i 33
i
i 13 i 23
i i
i
k k k
k k x
2
k k k
k k y
2
J
k k k
k k z
2
x y z 1
1
+ +
1
1
1
+
1
1
1
+
1
1
1
1
]
(2.38)
y
jk
ijk
i
K
I
m
v j = 1,2,3 ; k = 1, 2, 3

T
i
i
i i
i
z y x r

,
_

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 :
( )
n n i i n i i
T T
i i ip i ir p r ip ir p r
i 1 i 1 p 1 r 1 i 1 p 1 r 1
1 1
K K Tr U J U q q Tr U U q q
2 2
J

1
1
1

1
1
]
]

& & & &
(2.39)
Lu rng, cc ma trn J
i
(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 J
i
ch 1 ln.
II.3.4 .Th nng tay my:
Th nng P
i
ca khu i:
P
i
= - m
i
.g.
i
r
0
= -m
i
.g.
( )
i i
r A
1 0
(2.40)
i=1,2,,n
Trong

i
i
r
,
i
r
0
- 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)
37
( gia tc trng trng g = 9,8062 m/s
2
)
Th nng ca ton c cu n khu ng :

( )
n n
i
0
i
i i i
i 1 i 1
P P mg 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
F
q
L
q
L
dt
d
,
_
, I = 1,2, ,n
Phng trnh trn cho biu thc tnh ng lc F
Mi
. 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 j n n n
j
T T
j
Mi r jk j ji jkm j ji j ji k k m
j 1 k 1 j 1 k 1m 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 th vit gn li nh sau :
n n n
Mi ik k ikm k m i
j 1 k 1 m 1
F D q h q q c

+ +

&& &&
(2.43)
Hoc l di dng ma trn :
Mi
F D(q)q h(q, q) c(q) + + && &
(2.44)
Trong :
F
M
(t) vector (nx1) lc ng, to nn n khp ng :
[ ]
T
Mn M M M
t F t F t F t F ) ( ),..., ( ), ( ) (
2 1
(2.45)
q(t) vector (nx1) bin khp :
[ ]
T
n
t q t q t q t q ) ( ),..., ( ), ( ) (
2 1
(2.46)
38
q(t) &
- vec t (nx1) tc thay i bin khp :
[ ]
T
1 2 n
q(t) q (t), q (t),..., q (t) & & & & (2.47)
q(t) &&
- vect (nx1) gia tc bin khp :
[ ]
T
1 2 n
q(t) q (t), q (t),..., q (t) && && && && (2.48)
D(q) Ma trn (nxn) , c cc phn t D
ik
sau y :
( )
n
k i j
T
ji j jk ik
U J U Tr D
) , max(
i,k =1,2, . n (2.49)
H(q,
q&
) vect (nx1) lc ly tm v Coriolit
T
1 2 n
h(q, q) (h , h ,..., h ) &
n n
i ikm k m
k 1m 1
h h q q

& &
i=1,2,,n (2.50)
( )
n
m k i j
T
ji j ikm ikm
U J U Tr h
) , , max(
i,k,m = 1,2,,n (2.51)
C(q) vec t (nx1) lc trng trng.
( )
T
n
c c c q c ,..., , ) (
2 1
,
_

n
j
j
j
ji i i
r gU m c
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 Z
i
u trng phng vi cc trc
khp quay ng. Khi lng ca cc khu tng ng l m
1
, m
2
; B thng
s DH ca tay my ghi trong bng sau :
Khu
i

i
A
i
d
i
Khp
1
i
*
0 1 0 R
2
i
*
0 1 0 R
39
Hnh 2.3: C cu tay my 2 khu
Cc ma trn
i
i
A
1
( i =1,2) s l :

1
1
1
1
]
1
1 0 0 0
0 1 0 0
0
0
1 1 1
1 1 1
1
0
lS C S
lC S C
A

1
1
1
1
]
1
1 0 0 0
0 1 0 0
0
0
2 2 2
2 2 2
2
1
lS C S
lC S C
A

1
1
1
1
]
1
+
+

1 0 0 0
0 1 0 0
) ( 0
) ( 0
1 12 12 12
1 12 12 12
1
2
0
2
0
S S l C S
C C l S C
A A A
vn nh trc y dng cc k hiu sau :
C
i
= cos
i
; S
i
= sin
i
; C
ij
= cos(
i
+ j
) ; S
ij
= sin(
i
+ j
) .
Theo (2.30), ta c :
1
1
1
1
]
1
1
1
1
1
]
1

1
1
1
1
]
1
0 0 0 0
0 0 0 0
0
0
1 0 0 0
0 1 0 0
0
0
0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 0
1 1 1
1 1 1
1 1 1
1 1 1
1
0
1
1
1
0
11
lC S C
lS C S
lS C S
lC S C
A D
A
U
Tng t, i vi U
12
, v U
22
:
40

1
1
1
1
]
1
+
+
1
1
1
1
]
1
+
+
1
1
1
1
]
1
0 0 0 0
0 0 0 0
) ( 0
) ( 0
0 0 0 0
0 1 0 0
) ( 0
) ( 0
0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 0
1 12 12 12
1 12 12 12
1 12 12 12
1 12 12 12
2
0
2
1
2
0
21
C C l S C
S S l C S
S S l C S
C C l S C
A D
A
U
1
1
1
1
]
1
1
1
1
1
]
1

1
1
1
1
]
1

1
1
1
1
]
1
0 0 0 0
0 0 0 0
0
0
0 0 0 0
0 1 0 0
0
0
0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 0
1 0 0 0
0 1 0 0
0
0
12 12 12
12 12 12
2 2 2
2 2 2
1 1 1
1 1 2
2
1
2 1
0
2
2
0
22
lC S C
lS C S
lS C S
lC S C
lS C S
lC S C
A D A
A
U
Theo (2.37) v gi thit c cc thnh phn mmen ly tm qun tnh

u bng 0 , ta c :
1
1
1
1
1
]
1
1
2
1
2
1
2
1
1
0 0 2 / 1
0 0 0 0
0 0 0 0
2 / 1 0 0 3 / 1
m l m
l m l m
J
,
1
1
1
1
1
]
1
2
2
2
2
2
2
2
2
0 0 2 / 1
0 0 0 0
0 0 0 0
2 / 1 0 0 3 / 1
m l m
l m l m
J
.
Trn c s (2.49), ta c :
( ) ( ) +
'
1
1
1
1
1
]
1
1
1
1
1
]
1

+
T T T
U
m l m
l m l m
lC S C
lS C S
T U J U T U J U T D
11
1
2
1
2
1
2
1
1 1 1
1 1 1
2 21 2 21 2 11 1 11 2 11
0 0 2 / 1
0 0 0 0
0 0 0 0
2 / 1 0 0 3 / 1
0 0 0 0
0 0 0 0
0
0
2
2 2
2
2
2
1 21
2
2
2
2
2
2
2
1 12 12 12
1 12 12 12
2
3 / 4 3 / 1
0 0 2 / 1
0 0 0 0
0 0 0 0
2 / 1 0 0 3 / 1
0 0 0 0
0 0 0 0
) ( 0
) ( 0
l C m l m l m U
m l m
l m l m
C C l S C
C S l C S
T
T
+ +
'
1
1
1
1
1
]
1
1
1
1
1
]
1
+
+
+
( )
'
1
1
1
1
1
]
1
1
1
1
1
]
1

T T
U
m l m
l m l m
lC S C
lS C S
T U J U Tr D D
21
2
2
2
2
2
2
2
12 12 12
12 12 12
2 21 2 22 21 12
0 0 2 / 1
0 0 0 0
0 0 0 0
2 / 1 0 0 3 / 1
0 0 0 0
0 0 0 0
0
0
) 2 / 1 2 / 1 6 / 1 (
2
2
2
C l m + +
=
2
2
2
2
2 / 1 3 / 1 l m l m +
.
41
( )
'
1
1
1
1
1
]
1
1
1
1
1
]
1

T T
U
m l m
l m l m
lC S C
lS C S
T U J U Tr D
22
2
2
2
2
2
2
2
12 12 12
12 12 12
2 22 2 22 22
0 0 2 / 1
0 0 0 0
0 0 0 0
2 / 1 0 0 3 / 1
0 0 0 0
0 0 0 0
0
0
2
2
2
12
2
2
2
12
2
2
3 / 1 3 / 1 3 / 1 l m C l m S l m +
.
Tnh cc s hng trong biu thc (2.50) i vi i = 1, ta c :
2 2
2 2
1 1km k m 111 1 2 121 1 2 122 2 122 2
k 1m 1
h h h h h h

+ + +

& & && && & &

V theo (2.51) tnh cc h s h
ikm
, ri thay vo phng trnh trn, ta
c:
h
1
= -1/2m
2
S
2
l
2
2
2
& - m
2
S
2
l
2

1 2

&& .
Tng t i vi i = 2
2 2
2 2 2 2
2 2km k m 211 1 212 1 2 221 2 1 222 2 2 2 1
k 1m 1
h h h h h h 1/ 2m S l

+ + +

& & & && && & &

Nh vy :

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

1

1
1
]
& &&
&
&
.
Trn c s (2.52), ta c :

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

,

1
1
1
1
]
1
1
1
1
1
]
1
+
+

1
1
1
1
]
1
1
1
1
1
]
1

1
0
0
2 / 1
0 0 0 0
0 0 0 0
) ( 0
) ( 0
) 0 , 0 , , 0 (
1
0
0
2 / 1
0 0 0 0
0 0 0 0
0
0
) 0 , 0 , , 0 (
1 12 12 12
1 12 12 12
2
1 1 1
1 1 1
C C l S C
S S l C S
g m
lC S C
lS C S
g m
1 2 12 2 1 1
2 / 1 2 / 1 2 / 1 glC m glC m glC m + +
;
42
c
2
1
1
1
1
]
1
1
1
1
1
]
1

1
0
0
2 / 1
0 0 0 0
0 0 0 0
0
0
) 0 , 0 , , 0 (
12 12 12
12 12 12
2
lC S C
lS C S
g m
=

) 2 / 1 (
12 12 2
glC glC m
Vy vector trng trng s l:

1
]
1
+ +
1
]
1
12 2
1 2 12 2 1 1
2
1
2 / 1
2 / 1 2 / 1
) (
glC m
glC m glC m glC m
c
c
c
.
Cui cng ta c phng trinh ng lc hc ca c cu tay my 2
khu dng sau :
F
m
(t) =
D( )( )(t) h( , ) c( ) + +
& &
1
]
1
+
+ + +
1
]
1
2
2 2
2
2
2
2
2
2
2
2
2 2 2 2
2
1
2
1
3 / 1 2 / 1 3 / 1
2 / 1 3 / 1 3 / 4 3 / 1
l m C l m l m
C l m l m l C m l m l m
F
F
M
M
+
2
2 2
2 1 2
2 2 2 2
2
2
1
2 2
1/ 2m S l m S l
1/ 2m S l
1

1
1
1
]
& &&
&
+
1
]
1
+
12
2
2
1 2 12 2 1 2
2 / 1
2 / 1 2 / 1
C gl m
glC m glC m glC m
.
43
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 u
1
(t), u
2
(t), , u
r
(t) v s tn hiu
ra y
1
(t) , y
2
(t) , , y
s
(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 t quan h gia vect tn hiu vo
( ) u t
v tn hiu ra
( ) y t
, tc l m t nh x T :
a ( ) ( ) u t y t
.
nh x ny (Thng cn gi l ton t - operator) vit li nh sau :
( ) ( ( )) y t T ut
.
nu nh x T tho mn :

+ +
1 1 2 2 1 1 2 2
( ( )) ( ( )) ( ( )) ( ( )) T au t T au t aT u t a T u t
, (3.1)
trong a
1
v a
2
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).
44
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 F
1
=cs
trong trng hp |s| tng i nh v F
2
= m&& sca chuyn ng. Vi
nguyn l cn bng lc ta c nh x T :
a ( ) ( ) F t s t
m t quan h vo / ra
ca h :
+ && ms cs F . (3.2a)
Gi s rng di tc ng ca lc F
1
h c p ng s
1
v ca F
2
th 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
( ) ( ) ma s a s c as as aF a F
,
trong a
1
, a
2
l nhng s thc tu . Ni cch khc di tc ng
ca lc
+
1 1 2 2
aF a F
vt s i c mt qung ng l
+
1 1 2 2
as as
. 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 F
1
= cs th h thng l xo + vt l mt h
tuyn tnh .
45
s
m
m
..
s
F
cs
Ngc li, nu lc cn l xo li c tnh theo F
1
= cs +
s
3
, 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 :
+ + &&
3
=F ms cs s (3.2b)
v khi (3.2b) khng cn tho mn nguyn l xp chng (3.1)
+ + + + + + && &&
3 3
1 1 2 2 1 1 2 2 1 1 2 2 1 1 2 2
( ) ( ) ( ) ma s a s c as as as as aF a F
+ + + + + + && &&
3 3
1 1 2 2 1 1 2 2 1 1 2 2 1 1 2 2
( ) ( ) ( ) ma s a s c as as as as aF a F
Ni cch khc, di tc ng ca lc
+
1 1 2
aF a F
th qung ng
ca vt i c khng phi l
+
1 1 2 2
as as
. 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 x
1
(t), x
2
(t), , x
n
(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 x
1
= s , x
2
= & s s c:
1
2
(1 0) (1 0). 0.
x
s x F
x

_
+

,
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 t nh ngha v x
1
, x
2
v :
46

+ &
2 1
1 c
x x F
m m
thu c bng cch thay trc tip x
1
, x
2
vo phng trnh (3.1).
Vit chung hai phng trnh vi phn trn li vi nhau s c :
.
.
1
.
2
0 1 0
1
0
x
x x F
c
x
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:

.
( ) ( 1)
( ,..., , , )
n n
y f y y y u
Trong k hiu y
(k)
ch o hm bc k ca y(t), tc l
( )
k
k
k
d y
y
dt
,
th vi cc bin trng thi c nh ngha nh sau :
& &&
( 1)
1 2 3
, , ,...,
n
n
x y x y x y x y
h s c 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=x
1
Tng qut ln th mt h phi tuyn, sau khi nh ngha cc bin trng
thi x
1
(t) , x
2
(t) , , x
n
(t), s m t bi :
- M hnh trng thi tng minh autonom
.
( , )
( , )
x f x u
y g x u

trong
1
( )
( )
( )
n
x t
x t
x t

,
M
47
- M hnh trng thi tng minh khng autonom
.
( , , )
( , , )
x f x ut
y g x ut

,
- hoc m hnh trng thi khng tng minh
.
( , , , ) 0
( , , , ) 0
f x x ut
g x u y t

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
0
(0) x x
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 R
n
(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
0
x

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
48
1
x
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
e
x c gi l im cn bng
(equilibrium point) nu nh khi ang im trng thi
e
x 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
e
x ca h thng
phi l nghim ca phng trnh:
0
( , , 0
)
u
dx
f x ut
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 (
0
dx
dt

) khi khng c s tc ng
t bn ngoi (
0 u
).
III.1.3.2.im dng ca h :
nh ngha 2: Mt im trng thi
d
x
c gi l im dng ca h
thng nu nh h ang im trng thi
d
x
v vi tc ng
( )
d
u t u
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 :
( , , ) ( , , ) 0
d
d d
u u
dx
f x u t f x u t
dt

49
2
x
trong
d
u 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
e
x
nu nh c mt tc ng tc thi (chng hn nh nhiu tc
thi) nh bt h ra khi
e
x
v a ti im
0
x
thuc mt ln cn no
ca
e
x
th sau h c kh nng t quay v c im cn bng
e
x
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
e
x
no th mi ng
qu o trng thi ( ) x t xut pht t mt im
0
x
thuc ln cn ca
e
x
u
phi kt thc ti
e
x
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
e
x
. 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 (
e
x
=
0
) nn khi h n nh ti
0
, ngi ta cng ni thm lun
mt cch ngn gn l h n nh .
50
e
x
e
x
Ngoi ra, do khi nim n nh h phi tuyn b gn vi ln cn
im cn bng
e
x
nn cng c th mc d h n nh ti im cn bng
e
x

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
e
x
hay
khng l cha m cn phi ch ra min n nh ca n ti
e
x
, tc l phi
ch ra c ln cn O ca
e
x
sao cho h c kh nng t quay v c
e
x
t
bt k mt im
0
x
no thuc O (hnh 3.3). Min n nh O cng ln th
tnh n nh ca h ti
e
x
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 :
51
~
0
( , , ) ( , )
u
dx
f x ut f x t
dt

(3.3)
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
tho mn :
0
( ) , 0. x x t t < > <
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
0
(0) x x
tho mn :
lim ( ) 0
t
x 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 :

1 2
( , )
dx
f x x
dt

vi
1
2
x
x
x
_

,
(3.4)
H trn c gi thit l cn bng ti gc to
0
.
Hnh 3.4 : Minh ha khi nim n nh Lyapunov:
52
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
t im trng thi u
0
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 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 90
0
. Ni cch khc, h s n nh ti
0
nu nh c c
iu kin:
T
v
0 . . os =
v
dx dx
c
dt dt

(3.5)
53
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 :
nh ngha 5 : Mt hm thc nhiu bin, c th khng dng
(0, ) V t
,
c gi l hm xc nh dng nu :
a)
(0, ) 0 V t
b) Tn ti hai hm mt bin, dng
1
( ) a
v
1
( ) b
lin tc, n iu
tng vi
1 2
(0) (0) 0
sao cho :
1 2
0 ( ) ( , ) ( ) x V x t x <
vi mi
0 x
(3.6)
Hm
( , ) V x t
s xc nh dng trong ton b khng gian trng thi
nu cn c :
1
lim ( )
a
a

=>
lim ( , )
x
V x t

.
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
(0, ) V t
.
b) o hm ca n tnh theo m hnh (1.51) c gi tr khng dng
trong O, tc l :
( , ) 0
dV V V
f x t
dt t x

+

vi mi
x O
. (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.
54
( , )
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
.
b) o hm ca n tnh theo mt hnh (1.51) c gi tr m trong O
vi
0 x
, tc l :
( , ) 0
dV V V
f x t
dt t x

+ <

vi mi
x O
v
0 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

u
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 :
( ,r( ))
dx
f x 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 :
55
( , )
dx
f x u
dt
( ) r x
( ,r( )) 0
f
V
L V f x x
x
<
vi mi
0 x
,
x O
(3.11a)
V
( ,r( ))
V
f x x
x
=0ch khi
0 x
(3.11b)
III.2.Bc tng i ca h phi tuyn:
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):
G(s) =
s a a a
s b b b
n
n
m
m
s + + +
+ + +
...
...
1 0
1 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 :
T
d x
Ax bu
dt
y x
c
'

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

(3.13)
Vy th do
G(s)= c
T
(sI-A)
-1 b
Ta c :
s
Lm
s
r
G(s)=
a
b
n
m

s
lim
s
r
[
T
1
(sI A) b
c

] =
a
b
n
m

s
Lm
T
k
k 1 r
k 0
b
c
A
s
=
a
b
n
m
Hn na
56

s
Lm

s
r k +1
1
= 0 khi k > r-1
nn chui trn tr thnh tng ca hu hn r phn t u tin
s
Lm
T
k
r 1
k 1 r
k 0
b
c
A
s
=
a
b
n
m

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

1 - r k khi 0
2 - r k 0 khi 0
(3.14)
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 :
L
h
h
f
0 khi 0 k r-2
g(x)
0 khi k r-1
L

'

(3.16)
C th thy c ngay rng vi f(x)= Ax , H(x)= b , g(x)=c
T
x , hai
cng thc (3.14) v (3.16) s ng nht, v :
L
h
f
g(x)= c
T
A
T
xL
h L
k
f
g(x)=c
T
A
k
b
V d : Xt h Val der Pol c m hnh trng thi nh sau :
khi th do :
57
L
h
g(x)=
g
h(x)
x
[
0 1
]
1
]
1
1
0
= 0
L
h
L
f
g(x) =
f
(L g) g
h(x) f h(x)
x x x
_

,
=
( ) ( )
2
2
2 1 1
0 0
1 0 1 0
ax (1 bx ) x 1 1 x
x
1
_ _ _
1

, , ,
]
=1 0
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 x
0
m trong ln cn ca n c :
L
h
g(
x
)0,L
h
L
f
g(
x
) 0,,L
h
L
f
h
g(
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 :

dx
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 u
0
(t) sao cho tn hiu u ra y(t)
58
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(x
0
) = 0. Gi s rng h (3.17) c bc tng i l r, tc l :
L
h
L
f
k
g(x) =
0 nu 0 k r-2
0 nu k=r -1

'
(3.18)
Khi , vi php i trc to vi phi :
1
r 2
f
r 1
r 1
r
f
r 1
r 1
n
n
g(x)
g(x)
z m(x)
g(x)
(x)
(x)
z
L
z
z
L
z
m
z
m
+
+
1
1
1
1
1
1
1
1
1
1
1
1

1
1
1
1
1
1
1
1
1
1
]
1
1
]
M
M
M
M
vi L
h
m
k
) (
x
= 0 , k=r+1 , , n
h (1.18) cho s c a v dng chun
2
1
r
r 1
r
1
r 1
n
n r
z
dz
d
a(z) b(z)u
dt dt
(z)
c (z)
z
z
z
z
c
z
z
1
1
1
1
1
1
1
1
1
1
+

1
1
1
1
1
1
1
1
1
1
]
1
]
M
M
M
M
, y = z
1
(3.19)
Trong
A
(z)
= L
r
f g(
1
(z)
m
) , b
(z)
=L
h
L
1 r
f g(
1
(z)
m
) , c
i
(z)
= L
h
m
r+1
(
1
(z)
m
)
59
s dng k hiu :
z
=
1
1
1
1
]
v i
=
1
1
1
]
1
+
n
r
z
z
1
,
r 1
n
z
z
+
1
1
1
1
]
M
v
1
n r
(z)
c(z)
(z)
c
c
1
1
1
1
1
]
M

th m hnh (3.19) c vit thnh
2
1
r
r 1
r
dz
d
dt dt a( , ) b( , )u
c( , n)
z
z
z
z
z
1
1
1
1
1
1
1
1

1
1
+
1
1
1
1
1
1
]
]
M
M
, y = z
1
(3.20)
Gi s rng h (3.17) c tnh ng hc khng ng vi trng thi u
x
0
0 v tn hiu iu khin u
0
(t) thch hp. Vy th t y(t) = z
1
(t) = 0 ta suy
ra c :
z
1
(t) = = z
r
(t) =0
v do l = 0. iu ny dn n :
a(
0
,
) + b(
0
,
)u
0
= 0 u
0
(t) = -
a(0, )
b(0, )
(3.21a)

d
c(0, )
dt

(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).
60
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
) =L
f
g(
x
)==
1 r
f
L
g(
x
) = 0 .
Ni cch khc x(t) ca ng hc khng s ch nm trong a tp (hnh
3.7)
K = {
x
R
n
|g(
x
)=L
f
g(
x
)= =
1 r
f
L
g(
x
) = 0 } (3.22)
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 u
0
(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:
61
1
( ) x t

H phi tuyn c m hnh
'
+
) (
) ( ) ( ) , (
x h y
u x g x f u x f x
(3.23)
Trong y l tn hiu u ra, u l tn hiu u vo, x = [x
1
, x
2
, .., x
n
]
T
l vector trng thi ca h, f(x) = [f
1
(x), f
2
(x), ..., f
n
(x)]
T
, g(x) = [g
1
(x), g
2
(x),
..., g
n
(x)]
T

H phi tuyn c bc tng i l p nu:

'
2 ,..., 3 , 2 , 1 , 0 ) ( ; 0 ) (
) ( ) (
) (
1
1
p i x h L L x h L L
u x h L L x h L
dt
x h d
i
f g
p
f g
p
f g
p
f
p
p
(3.24)
S iu khin:
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:

+
1
1
) (
n
i
i
i
e e S
(3.25)
) 1 (
1 1
... 1 ) (

+ + +
n
n
S S S A

l a thc Hurwitz c:

0 ) ( lim
t
t e
(3.26)
S(0) = 0 (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
62
SMC

'
+
) (
) ( ) (
x h y
u x g x f x
) (
,..., ,
n
r r r
y y y
) (
,..., ,
n
y y y
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:
2
2
1
) , ( S t x V
(3.28)
l hm xc nh dng. o hm ca n c dng sau:
S S
dt
dV

(3.29)
H (3.23) n nh tim cn khi (3.29) l hm c du xc nh m:
'
> <
< >
<
0 , 0
0 , 0
0
S S
S S
S S
(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:
) ( ...
) (
1
2
) (
1
S Kh e e e e e S
n
n
n
i
i
i
+ + + +

(3.32)
Ta c:

) ) ( ) ( (
1 ) ( ) (
u x h L L x h L y e
n
f g
n
f
n
r
n
+
(3.33)
Do vy:
) ( ) ) ( ) ( ( ...
1 ) (
1
S Kh u x h L L x h L y e e
n
f g
n
f n
n
r n
+ + + +

(3.34)
Tn hiu iu khin tm c:
) (
) ( ... ) (
) (
1
) (
1
x h L L
x h L y e e S Kh
t u
n
f g n
n
f n
n
r n
+ + + +

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

+
1
1
) (
p
i
i
i
e e S
(3.36)
) 1 (
1 1
... 1 ) (

+ + +
p
p
S S s A
l a thc Hurwitz, c
0 ) ( lim
t
t e
(3.37)
0 ) 0 ( S
, mt trt phi i qua gc to v tho mn iu kin trt.
Hon ton tng t nh trong trng hp trn, ta xy dng hm
Lyapunov c dng sau:
2
2
1
S V
xc nh dng
S S

V xc nh m
Ta c:
) ( ) ) ( ) ( ( ) ( ...
) ( ) ( ) (
) ( ) (
) ( ) (
...
) (
) 1 (
1
) (
1 1
) 1 ( ) (
) ( ) ( ) (
) (
1 1
S Kh u x h L L x h L t y e e S
u x h L L x h L t y
t y t y e
t y t y e
e e e S
S Kh S
p
f g
p
f p
p
r p
p
f g
p
f
p
p p
r
p
r
p
p
+ + + +
+

+ + +

(3.38)
Tn hiu iu khin:
) (
)) ( ( ... ) (
) (
) 1 (
1
) (
1 1
S h L L
x h L y e e S Kh
t u
p
f g p
p
f
p
r p
+ + + +

(3.39)
III.4.2. Thit k b iu khin trt cho tay my n bc t do:
M hnh ng lc hc ca tay my:
) , ( ) ( q q h q q H +
(3.40)
vi H(q) l ma trn qun tnh xc nh dng, i xng.
Chng ta gi s rng cc gi tr c lng ) (
q H v ) , (
q q h
quan h
vi gi tr thc
) (q H
v
) , ( q q h
bi bt ng thc sau:
) ( ) ( ) (
1
q q H q H
(3.41)
v
) , ( ) , (
) , (
max
q q h q q h q q h
(3.42)
64
vi
) (q
v
) , (
max
q q h
l nhng hm bit.
Vit li biu thc ng lc hc di dng:
) ( ) , ( q B q q f q +
(3.43)
Vi
) , ( ) ( ) , (
1
q q h q H q q f

(3.44)
) ( ) (
1
q H q B

(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 q
d
.
Chng ta nh ngha sai lch trng thi e v mt trt nh sau:
q q e
d

(3.46)
0 ; > +
T
C C e Ce S
(3.47)
R rng rng S=0 th
) ( ) ( t q t q
q
. Qu thc vi S=0 ta c th vit li

nh sau:
Ce e e Ce S + 0 0
Nh vy h thng n nh tim cn nu c e = 0 v theo iu kin
bm
) ( ) ( t q t q
q
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:
0
2
1
> S S V
T
(3.48)
o hm ca (3.48) c dng:
S S V
T

(3.49)
Nh vy, nu 0 < V
th vi
0 V
dn ti
0 S
v
0 e

65
Do vy, iu kin ca iu kin trt l:
0 < S S
T

(3.50)
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 :
>
n
i
i
T
S S S S S
1
2
; 0 ; 0
(3.51)
Tip , mt phng trt S=0 s t c vi thi gian gii hn nh
hn T
0
:
)) 0 ( (
2
1
0
q S T
(3.52)
Biu thc trn c chng minh nh sau:
T (29) ta c:

S
S S
T

(3.53)
Thay S S V
T

v
2
1
) 2 ( V S vo (3.53) sau tch phn hai v vi
t=0t
reach
, S(q(t
reach
))=0 ta c:
[ ]
0
0 0
2
)) 0 ( (
2
)) 0 ( (
2
1
) 2 (
2
1
2
1
T
q S
t t
q S
V dt
V
V
reach reach
t t
reach reach

(3.54)
By gi chng ta tm u vo b iu khin tha mn iu kin
trt.
Ly o hm biu thc (3.47) ta c:
d
q q e C S
+
(3.55)
Thay biu thc (3.39) vo ta c:
d
q q B q q f e C S
+ + ) ( ) , (
(3.56)
Do tn hiu iu khin c dng
66
[ ] ) (
1
s KSgn B
eq

(3.57)
vi:
[ ]
T
n
eq
s Sgn s Sgn s Sgn s Sgn
q q f e C q
) ( ),..., ( ), ( ) (
) , (
2 1

(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 s khng r, nhiu,
Trong trng hp c lng chnh xc f f B B
th iu kin
trt c vit li nh sau:
S s KSgn S S S
T T
) (
(3.59)
Nu chn
> ; I K
(3.60)
v
S S S S S S S
m
i
i
m
i
i
T

1
2
1
(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:
) (
) (
) ( ) , (
1 1
s KSgn B q B B q B q q f e C q S
eq d

+ + +
(3.62)
t
1
) ( );

+ B q B R f f f f dn ti:
) ( )
( ) ( s RKSgn f f I R S
eq
+
(3.63)
T y, iu kin trt l:
{ } ) ( ) ( ) ( ) ( s Sgn S s RKSgn f f I R S S S
T
eq
T T
+

(3.64)
Do vy, nu chn K :
67
{ } ) ( )
( ) ( ) ( s Sgn S f f I R S s RKSgn S
T
eq
T T
+ +
(3.65)
th iu kin trt nh trn 0 < S S
c tha mn v iu kin
trt t c.
) (
1 1
q B B R

(3.66)
T biu thc (3.41) v (3.42) ta c bt ng thc:
) (
1 1
q B B R

(3.67)
max
1
( h B h h B f f R
(3.68)
T y, ta c th chn ma trn K tho mn iu kin trt nh sau:
I h B I K
eq
+ +
max
) 1 (
(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: m
1
= m
2
= 1 kg
l
1
= l
2
= 1 m
Phng trnh ng lc hc:
1
]
1
+
1
]
1
1
]
1
+
1
]
1
1
]
1
1
]
1
2
1
2
1
22 21
12 11
2
1
22 21
12 11
2
1
g
g
c c
c c
h h
h h
F
F

(3.70)
Trong F
1
, F
2
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:
68
1
cos 2 / 3 1
cos 3 5
22
2 21 12
2 11
+
+
h
h h
h
(3.71)
- Ma trn C:
0
sin 3
sin 2 / 3
sin 3
22
2 1 21
2 2 12
2 2 11

c
c
c
c

(3.72)
- Ma trn G:
) cos( 15
) cos( 15 cos 15
2 1 2
2 1 1 1

+
+
g
g
(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:
'
1 12
1 11 12
1 11
x
x x
x
(3.74)
Khp 2:
'
2 22
2 21 22
2 21
x
x x
x
(3.75)
Tn hiu vo u:
'
2 2
1 1
F u
F u
(3.76)
T biu thc (3.70) ta c:
1
]
1
1
]
1
1
]
1
1
]
1
1
1
]
1

2
1 1
2
1
22 21
12 11 1
2
1 1
1
1
g
g
H
c c
c c
H
F
F
H

(3.77)
69
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:
Khp 1
11 12
12 1 22 12 22 21 11 21 2
21
2
21 2 12 21 11 21
1 3
( (2 )sin 15(cos cos ))
4 94cos 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
+ +
'
+ +
&
&
(3.78)
Khp 2:
21 22
22 21 1 22 12 22 21 11 21 2
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 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:
2 2 2 2
1 1 1 1
e e S
e e S
+
+
(3.80)
y:
2 2 2
1 1 1
r d
r d
x x e
x x e

(3.81)
1
,
2
l nhng s thc dng.
iu kin xy ra ch trt cho h trn:
0 < S S V

(3.82)
70
Xy dng b iu khin:
T (3.78) v (3.79) nu t:
'
) , ( ) (
1 1 12
12 11
u x g x f x
x x
(3.83)
v:
'
) , ( ) (
2 2 22
22 21
u x g x f x
x x
(3.84)
Ta c:
) ( ) , ( )) ( (
)) , ( ) ( (
1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 11 1 1 1 1 1 1 1 1
S h K u x g x f x e
u x g x f x e x x e e e S
d
d d
+
+ + + +

(3.85)
1
1 1 1 1 1 1 1
1
)) ( ( ) (
) , (
x f x e S h K
u x g
d
+ +

Tng t ta cng c:
2
2 2 2 2 2 2 2
2
)) ( ( ) (
) , (
x f x e S h K
u x g
d
+ +

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

1
]
1
(3.87)
1
]
1
1
]
1
1
]
1

1
]
1

2
1 1
22
12
22 21
12 11 1
2
1
) (
) (
g
g
H
x
x
c c
c c
H
x f
x f
(3.88)
Ch :
) , (
1 1
u x g g
T (3.58) ta c c:
1
1
1
1
]
1
+ +
+ +
1
]
1
) (
) (
) (
) (
2
2
2 2 2 2 2
1
1
1 1 1 1 1
2
1
x f
x e S h K
x f
x e S h K
H
u
u
d
d

(3.89)
Thay (3.88) vo (3.89) ta c c b iu khin:
71
1
]
1
+
1
]
1
1
]
1
+
1
1
1
1
]
1
+ +
+ +
1
]
1
2
1
22
12
22 21
12 11
2
2 2 2 2 2
1
1 1 1 1 1
2
1
) (
) (
g
g
x
x
c c
c c
x e S h K
x e S h K
H
u
u
d
d

(3.90)
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:
1
1
1
1
]
1
+
+

1 0 0 0
0 1 0 0
) ( 0
) ( 0
1 12 12 12
1 12 12 12
2
1
1
0
2
0
S S l C S
C C l S C
A A A
(3.91)
theo cch bin i to ta c c:
1
1
1
1
]
1
1
1
1
1
]
1
1 1
2
2
2
2
0
0
0
0
z
y
x
A
z
y
x
(3.92)
Tuy nhin, trong bi ton ny c x
2
, y
2
, z
2
=0 v ta ch quan tm ti
chuyn ng ca tm bn kp do vy t (3.91) v (3.92) ta c:
'
+
+
) (
) (
1 12 0
1 12 0
S S l y
C C l x
(3.93)
,
_
+
+
2
2 2
0
2
0
2
2
2
2 2
0
2
0
2
2
2
0
2
0
2
2
arccos
) cos(
2
2
) cos( 2 2
l
l y x
l
l y x
l
y x
(3.94)
T (3.93) ta c:
'
'

2
12
2
1 0
2
12
2
1 0
12 1 0
12 1 0
) ( ) (
) ( ) (
lS lS y
lC lC x
lS lS y
lC lC x
(3.95)
72
1
2
0
2
0
0
1
2
0
2
0
0
2
0
2
0
1 0 1 0
2
0
2
0
2
0 ) ( 2
S
y x
y
C
y x
x
l
y x
S y C x l y x
+
+
+
+ +
(3.96)
t
'
2
0
2
0
0
2
0
2
0
0
) sin(
) cos(
y x
y
y x
x
(3.97)
Chn
>
1
ta c:
+
,
_
l
y x
2
arccos(
2
0
2
0
1 (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:
'
0 ) (
) (
) (
t z z
t y y
t x x
th gi tr t cho cc khp phi l:
'
,
_
,
_
2
2 2
0
2
0
2
2
0
2
0
1
2
2
arccos
2
arccos
l
l y x
l
y x

(3.99)
73
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).
74
IV.2. Cc thao tc thc hin m phng:
Khp 1:
11 12
12 1 22 12 22 21 11 21 2
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 21 2
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 t do:
75
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:

.
12
x
=(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:
.
22
x
=(-(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]))
76
+ 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 u
1
, u
2
l 1(t)
Cc hm x
12
, v x
22
l :
X
12
= (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])).
X
22
= (-(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] = x
11
, u[2] = x
12
, u[3] = x
21
,

u[4] = x
22
, u[5] = u
1
, u[6] = u
2
.
Sau khi m phng ta c th cc ng c tnh ca cc bin trng
thi x
11
, x
12
, x
21
,x
22
l :
77
M phng dng hm iu khin:
T cc cng thc:
1
]
1
+
1
]
1
1
]
1
+
1
1
1
1
]
1
+ +
+ +
1
]
1
2
1
22
12
22 21
12 11
2
2 2 2 2 2
1
1 1 1 1 1
2
1
) (
) (
g
g
x
x
c c
c c
x e S h K
x e S h K
H
u
u
d
d

- Ma trn H:
1
cos 2 / 3 1
cos 3 5
22
2 21 12
2 11
+
+
h
h h
h
- Ma trn C:
0
sin 3
sin 2 / 3
sin 3
22
2 1 21
2 2 12
2 2 11

c
c
c
c


78
- Ma trn G:
) cos( 15
) cos( 15 cos 15
2 1 2
2 1 1 1

+
+
g
g
Ta c:
U1=h
11
1 1 1 1 1
1
( )
d
K h S e x
+ + & &&
+h
12
2 2 2 2 2
2
( )
d
K h S e x
+ + & &&
+c
11
x
12
+c
12
x
22
+15c
os(x
11
)
-15sos(x
11
+x
21
)
U
2
=h
21
1 1 1 1 1
1
( )
d
K h S e x
+ + & &&
+h
22
2 2 2 2 2
2
( )
d
K h S e x
+ + & &&
+c
21
x
12
+c
22
x
22
+15cos(x
11
+x
21
)
Chuyn v dng hm ca s Simulink:
U
1
=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])
U
2
=(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]=x
11
, u[2]=x
12
, u[3]=x
21
, u[4]=x
22.
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: K
1
=K
2
=500,
1
=
2
=0,156.
Trng hp ny ta dng hm H(S
1
),H(S
2
) l cc hm gii hn u
vo trong khong gi tr upper v gi tr lower.
t ta c:
u[5]=
1
1
K
H(S
1
)+
.
1
1
1
e
+
1d
x &&
( Khi subsytem)
79
u[6]=
2
2
K
H(S
2
)+
.
2
2
1
e
+
2d
x &&
( Khi subsytem2)
80
81
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 U
1
, U
2
, v sai s
1
e&
,
2
e&
l:
Ta c cc gi tr t x
d1
v x
d2
l:
T cc cng thc:
'
+
+
) (
) (
1 12 0
1 12 0
S S l y
C C l x
'
2
0
2
0
0
2
0
2
0
0
) sin(
) cos(
y x
y
y x
x

82
'
,
_
,
_
2
2 2
0
2
0
2
2
0
2
0
1
2
2
arccos
2
arccos
l
l y x
l
y x

=>x
d1
= 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))
x
d2
= acos(((cos(u[1]+u[3])+cos(u[1]))^2+(sin(u[1]+u[3])+sin(u[1]))^2-2)/2)
83
Kt lun
Cc vn c gii quyt :
Cc vn cn tn ti :
Ti liu tham kho :
84
1. Nguyn Thin Phc: Robot cng nghip. NXB KHKT 2004.
2. Nguyn Thin Phc: Ngi my cng nghip v sn xut t
ng linh hot. NXB KHKT 1991.
3. Nguyn Phng Quang: iu khin Robot cng nghip -
Nhng vn cn bit. Tp ch T ng ho ngy nay - s thng 4, 5, 6 /
2006.
4. Nguyn Thin Phc: Robot - Th gii cng ngh cao ca
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.
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
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.
13. Nguyn Don Phc: L thuyt iu khin tuyn tnh.
NXB KHKT 2002.
85
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.
17. Applied Asymptotic Methods in Nonlinear Oscillitions.
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 6
th
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 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.
86

BCTNLVTNTLVN

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

BCTNLVTNTLVN

Uploaded by

Copyright:

Available Formats

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

l h s t l. Khi cn biu din s thay i to km theo

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

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

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

III.1.2.2.Qu o trng thi :

sao cho nghim

sao cho nghim

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

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

. Qu thc vi S=0 ta c th vit li

Nu iu kin trt c th tha mn theo :

SV: Hong c Thng - Phan Thanh Hi

SV: Hong c Thng - Phan Thanh Hi

You might also like