Professional Documents
Culture Documents
n tt nghip
n tt nghip
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
n tt nghip
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
n tt nghip
n tt nghip
W= 6n - i p i
(1.1)
vi
n : s khu ng
Pi : s khp loi i
S khu ng n = 2
Khp quay l khp loi 5 .
n tt nghip
q =
i
+ (1 i )Si
(1.2)
vi
i
0,i vi khp tnh tin
n tt nghip
n tt nghip
Qu o trong h to
cc
(xE,yE,zE,,,)
My tnh
Qu o trong h to
cc
(xE,yE,zE,,,)
q1
H tr ng chp hnh
ROBOT
q2
Cc gin Bin i
qi(t)
H tr ng chp hnh
= (r xi, r yi, r zi )
(1.3)
= (r xj , r yj , r zj )
(1.4)
10
n tt nghip
zi
zj
ri
rj
yi
Oi
yj
xj
xi
Hnh 1.6: Biu din 1 im trong khng gian
Vector
r% = ( r x, r y r z , )
(1.5)
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)
11
n tt nghip
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)
(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)
12
n tt nghip
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)
r
r
= R.r uvw
= R 1.r xyz
uvw
xyz
(1.13)
(1.14)
(1.15)
(1.16)
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
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)
(1.19)
0
0
1
0 cos -sin
Tij =
0 sin cos
0
0
0
-b
c
(1.20)
14
n tt nghip
Hoc vit rt gn l:
Rij
0
Tij =
(1.21)
Trong :
(1.22)
(1.23)
(1.24)
15
n tt nghip
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)
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
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)
u = ( x, y, z )T
v p l vector ch hng v di cn dch chuyn
p=
( p x , p y , p z )T
v = T p ( v = T p ( p x , p y , p z )T u
(1.28)
0
1
0 cos
R ( x, ) =
0 sin
0
0
= iu :
0
sin
cos
0
0
0
(1.29)
n tt nghip
cos 0 sin 0
0
1
0
0
R ( y , ) =
sin 0 cos 0
0
0
1
0
(1.30)
cos
sin
R ( z , ) =
0
sin
cos
0
0
0 0
0 0
1 0
0 1
(1.31)
18
n tt nghip
nx
n
TE = y
nz
sx a x
ny a y
sz a z
0
px
py
pz
(2.1)
19
n tt nghip
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)
Ti = 0 Ti = A1 1Ti
vi
Ti = A2 A3 ...Ai
(2.5)
(2.6)
20
n tt nghip
n tt nghip
n tt nghip
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
(2.7)
23
n tt nghip
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)
(2.9)
(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)
24
n tt nghip
nx
n
Tn = A1A2An = y
nz
sx a x
ny a y
sz a z
0
px
p y
pz
(2.12)
25
n tt nghip
= 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:
26
n tt nghip
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)
ri = ( xi , yi , zi , 1 )
T,
(2.18)
n tt nghip
i 1
Ai m t v tr tng i gia h
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
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)
S
Ci Ci
i 1
Ai = i
0
Si
0
0
Si Si
Si Ci
C i
0
0
di
(2.22)
chung
ri l khng i
d 0
d
( ri ) = ( 0Ai i ri ) =
dt
dt
28
n tt nghip
& 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
(2.24)
0 0
0 0
0 0
0 0
0 0
(2.25)
0 0 0
0 0 0
0 0 1
0 0 0
(2.26)
( A1 A2 ... A j 1 A j ... Ai 1 Ai )
=
q j
q j
(2.27)
(2.28)
(2.29a)
29
n tt nghip
(2.29b)
Vi = U ijq& j i ri
j =1
(2.30)
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)
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
=
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)
2 p=1 r =1
(2.34)
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)
I ij = ij xk2 xi x j dm,
(2.36)
xx yy zz
Ixy
Ixz
mi xi
2
Ixy
Iyz
mi yi
2
Ji =
Ixz
Iyz
mi zi
mi xi
mi yi
mi zi
mi
(2.37)
31
n tt nghip
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
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
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
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)
= FMi , I = 1,2, ,n
dt q qi
j=1 k =1
j=1 k =1 m =1
i=1,2,,n
j=1
(2.42)
(2.43)
k =1 m =1
(2.44)
Trong :
FM(t) vector (nx1) lc ng, to nn n khp ng :
33
n tt nghip
(2.45)
(2.46)
(2.47)
&&
q(t) - vect (nx1) gia tc bin khp :
&&
&&2 (t),...,q
&&n (t)]
q(t) = [&&
q1 (t),q
(2.48)
Tr (U
n
Dik =
j = max(i , k )
jk
J jU Tji
i,k =1,2, . n
(2.49)
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)
ci = mi gU ji j r j
j =1
(2.52)
34
n tt nghip
Khu
Ai
di
Khp
i*
i*
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
35
n tt nghip
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
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
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
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
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
h1 = h1km & k & m = h111 & 1 & 2 + h121 & 1 & 2 + h122 & 22 + h122 & 22
k =1 m =1
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
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
C12
S12
0
0
0 lS12 1 / 2
0 lC12 0
=
0
0 0
0
0 1
= m2 (1 / 2 glC12 glC12 )
c 2
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
39
n tt nghip
u1 (t )
u (t ) = M
u (t )
r
y1 (t )
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 ) .
y(t ) = T(u (t )) .
nu nh x T tho mn :
(3.1)
40
n tt nghip
ms
cs
F
m
s
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
41
n tt nghip
ms&& + cs + s3 =F
(3.2b)
s = (1 0)
x&1 = x 2
Suy ra t nh ngha v x1 , x2 v :
42
n tt nghip
x& 2 =
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
.
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
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
x = f ( x , u , t )
y = g( x, u, t )
f ( x, x, u, t ) = 0
g( x, u, y, t ) = 0
III.1.2.2.Qu o trng thi :
x1
44
n tt nghip
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:
c gi l im cn bng
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
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 :
dx
= f ( x, ud , t ) u=u = f ( x, ud , t ) = 0
dt
d
45
n tt nghip
b)
46
n tt nghip
(3.3)
47
n tt nghip
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 :
lim
x(t ) = 0 .
t
III.1.4.1.Tiu chun Lyapunov:
dx
= f ( x1 , x2 )
dt
x
vi x = 1
x2
(3.4)
n tt nghip
dx
dx
.cos = Tv
dt
dt
(3.5)
49
n tt nghip
vi mi x 0
(3.6)
lim
(a ) = =>
a 1
lim V( x, t ) = .
x
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)
dx
= f ( x, t )
dt
(3.9)
n tt nghip
dV V V
f ( x, t ) < 0 vi mi x O v x 0 .
=
+
dt t x
(3.10)
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)
51
n tt nghip
Bc tng i ca h SISO:
G(s) =
b + b + ... + b s
a + a s + ... + a s
0
(3.12)
d x
= A x+ b u
dt
y = cT x
(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
c Akb
Lm k +1r = bm
s
k =0 s
an
r 1
52
n tt nghip
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
d x
= f (x) + h(x)u
dt
y = g(x)
(3.15)
= 0 khi 0 k r-2
Lh L fhg(x) =
0 khi k = r-1
(3.16)
h
f
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
=
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:
= f (x) + h(x)u
dt
y = g(x )
(3.17)
54
n tt nghip
= 0
LhLfk g(x) =
0
nu 0 k r-2
nu k= r -1
(3.18)
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
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
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
z2
z1
M
M
dz d
zr
= z r 1 =
dt dt a(, ) + b(, )u
zr
c(
,
n)
, y = z1
(3.20)
a( 0 , ) + b( 0 , )u0 = 0 u0 (t) = -
b(0, )
(3.21a)
d
dt
= c(0, )
(3.21b)
(3.22)
56
n tt nghip
x (t )
Q
c(0, ) < 0 khi 0
y = h( x )
(3.23)
57
n tt nghip
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 )
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)
1 2
S
2
(3.28)
58
n tt nghip
dV
= SS&
dt
(3.29)
(3.30)
(3.31)
(3.32)
i =2
Ta c:
e ( n ) = y r( n ) ( Lnf h( x) + Lg Lnf1 h( x)u )
(3.33)
(3.34)
Tn hiu iu khin tm c:
u (t ) =
n L g Lnf1 h( x)
(3.35)
p 1
S = e + i e ( i )
(3.36)
i =1
59
n tt nghip
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 ) =
(3.39)
p 1 Lg L(fp 1) h( S )
= H ( q ) q&& + h ( q, q& )
(3.41)
(3.42)
(3.43)
60
n tt nghip
(3.44)
B ( q ) = H 1 ( q )
(3.45)
e = qd q
(3.47)
S = Ce + e&; C = C T > 0
1 T
S S>0
2
(3.48)
o hm ca (3.48) c dng:
V& = S T S&
(3.49)
(3.50)
61
n tt nghip
S
i =1
(3.51)
2
i
1
S ( q(0))
2
(3.52)
(3.53)
1
2
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
(3.55)
(3.56)
(3.57)
62
n tt nghip
vi:
eq = q&& Ce& f ( q, q& )
(3.58)
(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:
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
(3.66)
(3.67)
R 1 ( f f ) = B (h h) B hmax
(3.68)
(3.69)
m1 = m2 = 1 kg
l1 = l2 = 1 m
c12 &1 g1
+
c 22 &2 g 2
(3.70)
(3.71)
h22 = 1
64
n tt nghip
- 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)
(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)
g
c12 &1
1 1
& H
c 22 2
g 2
(3.77)
n tt nghip
Khp 1
x& = x
12
11
1
3
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 )))
(3.80)
y:
e1 = x d 1 x r1
(3.81)
e2 = x d 2 x r 2
(3.82)
Xy dng b iu khin:
T (3.78) v (3.79) nu t:
66
n tt nghip
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 ) =
Tng t ta cng c:
g 2 ( x, u ) =
(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)
67
n tt nghip
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
S12
C12
0
0
0 l (C12 + C1 )
0 l ( S12 + S1 )
1
0
0
1
(3.91)
(3.92)
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
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)
y = y (t )
z = z (t ) = 0
x2 + y2
0
1 = arccos 0
+
2l
x02 + y 02 2l 2
2 = arccos
2l 2
(3.99)
69
n tt nghip
70
n tt nghip
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 )))
71
n tt nghip
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
73
n tt nghip
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
- Ma trn G:
g1 = 15 cos 1 15 cos(1 + 2 )
g 2 = 15 cos(1 + 2 )
Ta c:
U1=h11
+c11x12+c12x22+15cos(x11)
-15sos(x11+x21)
U2=h21
+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]=
U[6]=
K2 h( S2 ) + e&2 + 2 &&
x2 d
2
K1
1 .
H(S1)+ e1 + &x&1d ( Khi subsytem)
1
1
75
n tt nghip
u[6]=
K2
1 .
H(S2)+
e2 + &x&2 d ( Khi subsytem2)
2
2
76
n tt nghip
77
n tt nghip
y 0 = l ( S12 + S1 )
x0
cos( ) =
x02 + y 02
y0
sin( ) =
2
x0 + y 02
78
n tt nghip
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
Cc vn c gii quyt :
Cc vn cn tn ti :
80
n tt nghip
1.
6.
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
16.
18.
82