Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
iu khin thch nghi dng b quan st trt cho cc khp ca tay my
cng nghip c cng vng thp
Adaptive control using sliding mode observer for the industrial manipulator
joints with low stiffess
V Thu H Bi Quc Khnh
Trng H KT KT Cng Nghip Trng HBK H Ni
e-Mail: vothuha1979@gmail.com e-Mail: khanhbq@mail.hut.edu.vn
Vit Phng
Trng HBK H Ni
e-Mail: vietphuongdo14@gmail.com


Tm tt
B iu khin thch nghi dng b quan st trt cho
cc khp ca tay my cng nghip c cng vng
thp l b iu khin nh gi li cu trc chp nhn
iu khin phi tuyn, tham s bt nh v cng
vng thp ca khp ni vi s nh hng mmen
qun tnh ti ln hn mmen qun tnh ng c v s
nh hng ca lc ty tm tc ng n khp ni. B
iu khin dng b quan st trt SMO gim bc
quan st c lng cc thnh phn phi tuyn qua cc
bin trng thi nh v tr ti, tc ti u im ca b
quan st trt l quan st c lng nhanh cc bin
trng thi v nhanh chng hi t sau a vo b
phn hi trng thi LQR m bo cht lng iu
khin bm h thng. Kt qu m phng trn phn
mm Matlab/Simmechanic v thc nghim vi mt
khp c cng vng thp cho Robot Almega 16 cho
thy h truyn ng chuyn ng mt khp ni c
cng vng thp p ng c yu cu iu khin:
m bo h chuyn ng khng dao ng, p ng v
tr ca ti t bm st v tr ti thc vi sai s l nh.
Qua c th c rt kinh nghim cho thit k h
truyn ng khp ni c cng vng thp.
Abstract:
The adaptive controller using the sliding mode
observer for the industrial manipulator joints with low
stiffness is to rerate the structure of nonlinear control,
uncertainty parameter of the joints with low stiffness
linked with the influence of inertia load larger than
inertia motor and the influence of the centripetal
forces affecting coupling. The controller using the
minimum-order sliding mode observer (SMO) to
observe and estimate nonlinear parameters through
the state variables such as load position, load speed .
The advantages of the sliding mode observer is to
quickly observe and estimate the state variables ,
convergence fast and then put into the state feedback
controller LQR to maintain control quality tracking
the system. The simulation using
Matlab/Simmechanic software and experiment results
with a low stiffness joint for the Robot Almega 16
indicates the transmission couplings with low
stiffness has met the desired control: ensure the
system is non-oscillation,the response of the reference
load position tracking closely to the load position so
error is small. Which can draw on experience to
design transmission coupling stiffness is low.
K hiu
K hiu n v ngha
A, B, C ma trn ca m hnh
f
u

hm phi tuyn
tn hiu iu khin u vo
L ma trn quan st
Luenberger
K
s
ma trn phn hi trng thi
ca b quan st trt

f(x)
hm phi tuyn c lng
s
s
i

vect cc mt trt
cc im cc mong mun
J
M

J
L

K
CV
M

B
M
B
L

F
d
m
l
T
m
kgm
2

kgm
2

Nm/rad
rad
rad





Nm
mmen qun tnh ca roto
ng c.
mmen qun tnh ca roto
ti
h s c cu ni khp
v tr ng c
v tr ti
h s ma st nht ca C
h s ma st nht ca ti
thnh phn lc ma st
khi lng ca thanh ni
chiu di ca thanh ni
mmen in t ca ng
c
Ch vit tt
SMO b quan st trt - Sliding mode
observer SMO
LQR b iu khin phn hi trng thi LQR -
Linear Quadratic Regulator
TMCN tay my cng nghip



1. t vn
Trong h truyn ng cc khp ni sau ca TMCN c

178
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
c im cng sut nh, mmen qun tnh ng c rt
nh so vi mmen qun tnh ti, kch thc cc khp
ni sau b hn ch cho nn s kt ni t ng c n
ti trong mt khng gian trt hp hnh tr thng
thng ngi ta dng ai hnh thang ni gia ng
c v ti nh H.1, iu ny dn n h hot ng vi
hai khi qun tnh khc nhau l nguyn nhn gy ra
s dao ng trong qu trnh chuyn ng lm gim
bm qu o chnh xc ca h chuyn ng tay my.
ng c Dy ai thang
Ti

H.1 Khp ni dng ai thang ca Robot Almega16
nng cao cht lng cho h truyn ng chuyn
ng bm cho cc khp c cng vng thp cuae
TMCN c rt nhiu lut iu khin c p dng,
trong bi bo ny xut mt phng php iu
khin cho h truyn ng khp ni c cng vng
thp l: iu khin thch nghi dng b quan st
trt cho cc khp ca tay my cng nghip c
cng vng thp. B iu khin ny bao gm b quan
st trt v b iu khin phn hi trng thi ti u
LQR lm vic theo nguyn l tch. Cc thnh phn
phi tuyn dng b SMO gim bc, b SMO gim bc
c nhim v quan st c lng cc bin trng thi
cn b iu khin LQR m bo cht lng iu
khin bm h thng, [3]. Thut ton iu khin c
m phng vi thi gian thc v kim chng bng thc
nghim vi mt khp c cng vng thp cho
Robot Almega 16 nh H.1.
2. H iu khin phn hi trng thi LQR
2.1 Tng hp b quan st trt cho mt khp ni
c cng vng thp
Trong h truyn ng mt khp ni c cng vng
thp nh H.1 gi thit o c v tr ng c qua
enconder cn v tr ti, cng vng ca khp ni
khng o c l cc thnh phn phi tuyn dng b
SMO gim bc quan st c lng qua cc bin
trng thi nh v tr ti.
Phng trnh m t ton hc cho mt khp ni bng
dy ai hnh thang, c xc nh nh sau:
L L d CV L M
M M M M CV L M M
J B mglsin F +K 0
J . +B . - K .( - ) =
L L L L
. + . + + ( ) .( - )
T
=

(1)
Phng trnh m t ton hc cho mt khp ni bng
dy ai hnh thang vit di dng phng trnh trng
thi chun:
(t)= (t) + (t) + f(x,t)+ u(t)
(2)
(t)= (t) + (t) + f(x,t)+ u(t)

x Ax Ax B
x Ax Ax B





trong :
1 m
x
a
x = = bit qua o lng.
| |
T
.
M
2 3 4 L L
x x x (3)
(
=
(

T
b
x =

| |
aa ab
M
a b
0, 1 0 0 ,
1
-
J
= 0, = 0
0
= =
(
(
(
(
(
(
(

A A
B B


CV CV M
M M M
ba bb
CV CV L
L L L
K K B
- - 0
J J J
0 , = 0 0 1
K K B
0 - -
J J J
( (
( (
( (
( ( =
( (
( (
( (

A A
( ) ( )
a b
-1
L d 4
0
f (x,t) = 0, f (x,t)= 0
J - mglsinx - F x
(
(
(
(
(

3

Khai trin chi tit phng trnh (2) nh sau:
CV CV M
2 1 2 3 m
M M M M
4
CV CV d 4 L
1 3 4 3
1 L L L L
K K B 1
x = - x - x x T
J J J J
x = x
K K F (x ) B mgl
x = x - x x sin x -
J J J J J
3
4

+ +

(4)
Cu trc b quan st trt gim bc, [1], [2] xc nh
nh sau:
bb b ba b s

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

b b a b b
x A x + +A x +B Ls K sgn(s
(5)
Trong :
Mt trt s b SMO c xc nh:
ab aa a
2 1 2 2
= = - [ - - ]
= x - x = x - x
b b b b a a
s y - y A x x A x B u
(6)
Hm phi tuyn c lng c xc nh:

( ) ( )
b
-1
L d 4
0

f (x,t)= 0
J -mglsinx - F x
(
(
(
(
(

3
(7)
T cu trc b quan st trt SMO bc [1], [2], [3]
c xc nh:

t t f(x) u
s
x( ) = Ax( ) + +B - Ls - K sgn(s)
(8)

Trin khai chi tit cu trc b SMO cho mt khp ni
bng dy dai thang nh sau:
179
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
CV CV M
1 2 3 m b2
M M M M
2 2
4 b3 3 3
CV CV L
1 3 4 3
1 L L L
d 4
b4 4 4
L
K K B 1

x = - x - x x T + f
J J J J
- L - k

x = x + f - L - k
K K B mgl
x = x - x x sin x
J J J J
F (x )

- + f - L - k
J

+ +

2
b
3 b
4
b
s sgn(s )
s sgn(s )
s sgn(s )

(9)
Trong :
| |
| |
T
3nxn
1 2 3
T
3nxn
s 1 2 3
= L L L R
= k k k R
e
e
L
K

M hnh c bn cu trc b SMO biu din nh H.2

Phng trnh LH cho mt
khp ni bng dy ai thang
(Phng trnh 1)
C
Quan st
Luenberger
C
TM
TM
Tnh hm
b

f (x)
x
d

L
K.sgn(sb)
sb
y
y
x
Cu trc b quan st
trt SMO
X


H. 2 S cu trc c bn ca SMO
2.1.1 nh gi sai lch ca b quan st trt SMO
T (1) v (5) sai lch b SMO c xc nh:
b
bb b s
(t) (t) f - ) =
x b b b
e A e + Ls K sgn(s (10)
trong :
b b

f (x,t)-f (x,t)
b
f =
- cc thng s khng bit ca i
tng phi tuyn l hm b chn trn vi iu kin:
s
b
f

Theo nguyn l n nh Lyapunov, chn hm:
1
V =
2
T
b b
s s
(11)
Ta bit rng: s
b
= 0 th V = 0. Ta phi lm cho V 0 s ,
ngha l:
V = <- <0
T
b b b
s s s (12)
y l iu kin h thng lun n nh tim cn ti
s
b
= 0.
trong : q - hng s dng.
Sai lch trng thi ca h thng lun c a v trn
mt trt s
b
= 0 v gi trn , tc l
b
s = 0 :

ab bb b s
[ (t) f )] 0 = =
b
b x
s A A e + K sgn(
b
s (13)
T :
1
ab s bb
( [ (t)] A

= +
b
b b x
sgn(s ) A K ) C f A e (14)

s bb
t
b
b b x
K sgn(s ) = (f + A e ( ))
(15)
Mt h thng lun c a v trn mt trt s
b

= 0 vi phng trnh (10) th xc nh c
phng trnh ng hc m t sai lch quan st
ca b quan st trt nh sau:
-1
ab ab bb
[I - ( ) ][ + (t)] =
b
x s s b b
e K A K A f A x (16)
2.1.2 Xc nh ma trn Luenberger L
Bng cch t im cc cho ma trn h thng
( )
bb ab
A L.A - t c sai lch mong mun.
Vic xc nh h s L thng qua cn bng
phng trnh c tnh vi phng trnh cha cc
im cc mong mun.
( ) ( )
n
bb ab i
i=0
det sI . s-s = (
[
A - LA
(17)
2.1.3 Xc nh h s ma trn phn hi K
S
b SMO
Vi mt trt s
b
c xc nh:
| |
ab aa 2 1 2 2
u x - x = x - x =
b b a a a
s = A x x A x +B
(18)
Sai lch trn s c lng vi mt trt ca i tng:
| |
2 2
-1 -1
2 2 M CV 3 1 M 2 M M
b2 2 2
CV CV M
1 2 3 m
M M M M
J K (x - x )- B x +J T

+ f
K K B 1
- x - x x T
J J J J
+ +
b
x x
s = x -x =
- L e - k sgn(e )
(19)
Thay (19) vo s
b b b
s s s ta c:
| |
2 2
2
2
-1 -1
M CV 3 1 M 2 M M b2 2 2
CV CV M
1 2 3 m
M M M M

J K (x - x )- B x +J T + f -
K K B 1
- x - x x T
J J J J

| |
|
|
+ +
|
\ .
s
x x
x
x
- L e - k sgn(e )
e
- e
(20)

| |
2
2
2
2
-1
2 M CV 3 1 M 2
-1
M M b2 2
CV CV M
1 2 3 m
M M M M
+J K (x - x )- B x

+J T + f -
K K B 1
- x - x x T
J J J J
>
+ +
x
x
x
x
- e
k sgn(e )
e
L e
(21)
t:
2
2
2
x
x
x
e
= sgn(e )
e
(22)
Thay (20) vo (3.21) ta c:

| |
2 2
2
-1
2 M CV 3 1 M 2
-1
M M b2 2
CV CV M
1 2 3 m
M M M M
+J K (x - x )- B x

+J T + f -
K K B 1
- x - x x T
J J J J
>
+ +
x x
x
k sgn(e ) - sgn(e )
L e
(23)
Chia 2 v (23) cho
2
x
sgn(e ) ta c:

2
-1
2 M CV 3 1 M 2
-1
M M b2 2
CV CV M
1 2 3 m
M M M M
+J K (x - x )- B x
+J T + f -
K K B 1
- x - x x T
J J J J

x
k
L e
> (

+ +
(24)
Khi iu kin (12) c tha mn th trng thi ca h
thng lun c a v trn mt trt s
b
= 0 v gi
180
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
trn , do vy
b
s = 0 , theo tiu chun n nh
Lyapunov th phng trnh (24) s tho mn iu kin
n nh ca tiu chun ny nu h s k
2
k
3
k
4
c
xc nh:
2
-1
2 M CV 3 1 M 2
-1
M M b2 2
CV CV M
1 2 3 m
M M M M
+J K (x - x )- B x
+J T + f -
K K B 1
- x - x x T
J J J J

x
k
L e
> (

+ +
(25)

2
3
3 2
x
f
k =k
e
(26)

2
4
4 2
x
f
k =k
e
(27)
2.1.4 Xc nh thnh phn phi tuynf ca b
quan st trt
Thnh phn sai lch phi tuyn f c xc nh:
| |
b b

f -f
T
b 2 3 4
f = = f f f (28)
Xc nh c thnh phn phi tuyn f ca b SMO
nh sau:
( )
( )
-1
L d 4
-1
L d 4

(29)

J - mglsinx - F x
- J - mglsinx - F x
(

(

2 b2 b2
3 b3 b3
4 b4 b4 3
3
f = f - f = 0
f = f - f = 0
f = f - f =

2.2 Tng hp b iu khin phn hi
trng thi LQR

Flexible Joint
TM_i y
- K SMO
x
qref_i
C
X
m_i
x
y
+
-
y
s

H. 3 S cu trc c bn ca b LQR v SMO

Trong H.3 biu din s cu trc c bn ca b
LQR v SMO. B iu khin ny lm cho h truyn
ng chuyn ng khp ni c cng vng thp n
nh trong cc thnh phn phi tuyn dng b SMO
gim bc quan st c lng qua cc bin trng
thi cn b iu khin phn hi trng thi LQR m
bo cht lng iu khin bm h thng.




2.3 Kt qu m phng
Cc tham s ca mt khp ni c cng vng thp:
Mmen qun tnh ca ng c: J
M
= 0,0004 kgm
2
,
Mmen qun tnh ca ti: J
L
= 0,004kgm
2
,
H s ma st nht ca ti B
L
= 0,4Nms/rad,
H s ma st nht ca ti B
M
= 0.0050 Nms/rad,
H s truyn: i = 1,
cng vng ca khp ni: K
CV
= 4 N/rad.
0 2 4 6 8 10 12 14 16 18 20
0
0.2
0.4
0.6
0.8
1
1.2
Time (s)
J
o
i
n
t

A
n
g
l
e

(
r
a
d
)
L_e

H.4 p ng v tr ti thc
L
v v tr ti
c lng
L_e

0 2 4 6 8 10 12 14 16 18 20
0
0.2
0.4
0.6
0.8
1
1.2
Time (s)
J
o
i
n
t

A
n
g
l
e

(
r
a
d
)
L_ref

H. 5 p ng v tr ti t
L_ref
v v tr ti thc
L

0 5 10 15 20
0
0.005
0.01
0.015
0.02
0.025
0.03
0.035
0.04
Time(s)

H.6 p ng sai lch v tr ti t
L_ref

v v tr ti thc
L



181
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
Nhn xt: T cc c tnh m phng cho thy sai
lch iu khin:
Sai lch ln nht :
e
max
4
9,10
= 100% = 0,09%
1

T cc c tnh m phng cc trng thi thc v c
lng trn chng minh rng b quan st SMO gim
bc thit k l chnh xc cho thy cc ng c
tnh ca tay my c khp ni c K
CV
thp l tt vi
thi gian xc lp nh, gim dao ng, qu iu
chnh nh.
2.4 Kt qu thc nghim
Xy dng m hnh thc nghim ng dng b iu
khin phn hi trng thi LQR dng b quan st trt
SMO cho khp ni ca tay my c cng vng thp
trong min thi gian thc, iu khin bm qu o
ca Robot Almega 16 vi mt khp ni c cng
vng thp l khp 5.

L

L_e

H. 7 p ng v tr ti thc v v tr ti c lng

H. 8 p ng sai lch v tr ti thc v v tr ti c lng

M_ ref
T
M
T

H. 9 p ng mmen t v thc ca ng c

H. 10 p ng sai lch iu khin
3. Kt lun
- Kt qu m phng chng minh thut ton iu
khin thch nghi dng b quan st trt cho cc khp
ca tay my c cng vng thp c a vo thc
nghim trc khp th 5 ca Robot Almega16 m
bo c v tr ti thc bm st v tr ti t vi sai s
< 0,1%
- Kt qu thc nghim bng b iu khin thch nghi
dng b quan st trt cho cc khp ca tay my c
cng vng thp c a vo thc nghim trc
khp th 5 ca Robot Almega16 cho thy h chuyn
ng khng dao ng vi p ng sai lch v tr ti v
v tr ti c lng bng b quan st trt SMO l
nh, nhanh chng hi t. iu khin bm qu o vi
sai lch ln nht:
max
e = 0,09% . B iu khin thch
nghi dng b quan st trt c ci t trong Card
iu khin chuyn ng ds1104 kt hp vi h truyn
ng Servo - Omnuc ca hng Omron. o thc
nghim h truyn ng khp ni c cng vng
thp, Robot Almega 16 chy n nh cho kt qu bm
chnh xc qu o t.
Ti liu tham kho
[1] Jaeyoung Lee.; Tae Jun Ha.; Je Sung Yeonl,
Sanghun Lee and Jong Hyeon Park.: Robust
Nonlinear Observer for Flexible Joint Robot
Manipulators with Only Motor Position
Measurement. Korea 2007
[2] Mohsin Waqar.: Robust nolinear observer for a
non-collocated flexible motin system. Georgia
Institute of Technology., May, 2008
[3] Anh. Ng.Ph.Th. ; Ha. V.Th.: Xy dng thut
ton iu khin thch nghi dng b quan st
trt cho khp ni mm. Tp ch khoa hc cng
ngh cc trng i hc k thut, s 80, nm
2011

182