Chng 5: ng lc hc robot v ng dng trong iu khin

64
Chng 5
NG LC HC ROBOT V NG DNG
TRONG IU KHIN

5.1. Mc ch v phng php kho st ng lc hc robot
Vi nhng mc ch thit k v iu khin, cn thit phi c mt m hnh
ton hc m t ng lc hc ca h thng. V th, chng ny ta s xc lp
phng trnh chuyn ng ca tay my di dng phng trnh vi phn. Phng
php p dng y l xy dng phng trnh chuyn ng ca c h da trn
quan h nng lng, xut pht t nguyn l bo ton v chuyn ha nng lng
trn c s xc lp quan h gia ng nng v th nng ca c h tay my, sau
s dng phng trnh vi phn ca chuyn ng trn c h vi cc i lng
tham gia vo phng trnh gm lc, qun tnh v nng lng.
Vic nghin cu ng lc hc Robot thng gii quyt hai nhim v sau :
1. Xc nh momen v lc ng trong qu trnh chuyn ng. Khi qui lut
bin i ca bin khp q
i
(t) xem nh bit.
Vic tnh ton lc cng nh momen trong c cu tay my l nhim v tt
yu trong vic la chn cng sut ng c, tnh ton kim tra bn, cng
vng, m bo tin cy cho Robot.
2. Xc nh cc sai s ng, tc l sai s xut hin so vi qui lut chuyn ng
trong chng trnh.
C nhiu phng php nghin cu ng lc hc Robot, nhng nhiu hn
c l phng php c hc Lagrange, c th l phng trnh Lagrange-Euler.
Trong phm vi ni dung ca mn hc ny, chng ta tm hiu nhim v th
nht, t to c s cho vic lp trnh v iu khin robot.
5.2. ng lc hc robot vi phng trnh Euler-Lagrange.
Hm Lagrange ca mt h thng nng lng c nh ngha :
L= K P
Trong : K l tng ng nng ca c h
L l tng th nng ca c h
K v P u l nhng i lng v hng, nn c th chn bt k h ta
no gi bi ton n gin.
Xt mt Robot c n khu th :
Chng 5: ng lc hc robot v ng dng trong iu khin
65

=
=
n
i
i
K K
1
v
=
=
n
i
i
P P
1
(2.1)
Trong , K
i
v P
i
l ng nng v th nng ca khu th i xt trong h ta
chn. l cc i lng ph thuc vo nhiu bin s :

( )
i i i
q q K K , =
v
( )
i i i
q q P P , =
(2.2)
Vi q
i
l ta suy rng ca khp th i.
nh ngha : Lc (hay momen) tng qut tc dng ln khu th i c xc
nh bi phng trnh Lagrange :

q q c
c

c
c
=
L L
dt
d
F

5.3. Kho st bi ton ng lc hc ca tay my nhiu bc t do
Phng trnh chuyn ng Lagrange thit lp cho mt c h c cho bi:

q q
=
c
c

c
c L L
dt
d

(2.3)
Trong q l vect biu din cc to suy rng ca cc khu ca Tay
my q
i
, t l vect biu din cc lc suy rng ca cc khu ca tay my v hm
Lagrange l s chnh lch gia ng nng v th nng ca c h :
P K L = (2.4)
a. V d 1.
Ta xt v d xy dng phng trnh chuyn ng ca tay my hai khu phng
lin kt bng khp bn l.
Trong v d ny, ta p dng cc kt qu ca bi ton ng hc c kho
st phn trc. xy dng bi ton ng lc hc, ta kho st c h vi gi
thit rng khi lng ca khu c tp trung cc khp. Ma trn bin khp l:
| |
T
q
2 1
u u =
(2.5)
v ma trn biu din ca lc suy rng c th hin:
| |
T
2 1
t t t =
(2.6)
vi
2 1
,t t l cc m men c cho bi cc c cu tc ng (chng hn l m
men pht ng ca cc ng c in).

Chng 5: ng lc hc robot v ng dng trong iu khin
66







Hnh 5.1: Tay my hai khu bn l
- Biu thc ng nng v th nng
Vi khu 1, ta c biu thc ca ng nng v th nng tng ng l:

2
1
2
1 1
2
1
1
u

a m K =
(2.7)

1 1 1 1
sinu ga m P =
(2.8)
Vi khu 2 ta c:
) cos( cos
2 1 2 1 1 2
u u u + + = a a x
(2.9)

) sin( sin
2 1 2 1 1 2
u u u + + = a a y

(2.10)
) sin( ) ( sin
2 1 2 1 2 1 1 1 2
u u u u u u + + =

a a x
(2.11)
) cos( ) ( cos
2 1 2 1 2 1 1 1 2
u u u u u u + + + =

a a y
(2.12)
Bnh phng vn tc l :

2 2 1
2
1 2 1
2
2 1
2
2
2
1
2
1
2
2
2
2
2
2
cos ) ( 2 ) ( u u u u u u u

+ + + + = + = a a a a y x v
(2.13)
Do vy ng nng ca khu 2 l:

2 2 1
2
1 2 1 2
2
2 1
2
2 2
2
1
2
1
2
1 2
2
1
2
2 2
2
1
2
cos ) ( ) ( u u u u u u u

+ + + + = = a a m a m a m v m K
(2.14)
Th nng cho khu 2 l:

)] sin( sin [
2 1 2 1 1 2 2 2 2
u u u + + = = a a g m gy m P

(2.15)

y (x
2
,y
2
)

m
2

a
2
u
2

g

a
1
u
1
m
1

x
0

Chng 5: ng lc hc robot v ng dng trong iu khin
67
- Phng trnh Lagrange
Hm Lagrange cho Tay my ny l:

) sin( sin ) ( cos ) (
) ( ) (
2 1 2 2 1 1 2 1 2 2 1
2
1 2 1 2
2
2 1
2
2 2 2
1
2
1
2
1 2 1 2
1
2 1 2 1
u u u u u u u
u u u
+ + + +
+ + + + = + = =
ga m ga m m a a m
a m a m m P P K K P K L


(2.16
)
Ta cn xc nh cc biu thc :
) cos( sin ) (
sin cos ) (
cos ) (
) cos( cos ) (
cos ) 2 ( cos ) 2 ( ) ( ) (
cos ) 2 ( ) ( ) (
2 1 2 2 2 2 1
2
1 2 1 2
2
2 2 1 2 1 2 2 1 2 1 2 2 1
2
2 2
2
2 1 2 1 2 2 1
2
2 2
2
2 1 2 2 1 1 2 1
1
2
2
2 2 1 2 1 2 2 2 1 2 1 2 2 1
2
2 2 1
2
1 2 1
1
2 2 1 2 1 2 2 1
2
2 2 1
2
1 2 1
1
u u u u u u
u
u u u u u u u
u
u u u u
u
u u u
u
u u u u u u u u u u
u
u u u u u u
u
+ + =
c
c
+ + =
c
c
+ + =
c
c
+ + =
c
c
+ + + + + + =
c
c
+ + + + + =
c
c
ga m a a m
L
a a m a a m a m
L
dt
d
a a m a m
L
ga m ga m m
L
a a m a a m a m a m m
L
dt
d
a a m a m a m m
L

Cui cng, phng trnh chuyn ng ca c h tay my c cho bi h
hai phng trnh vi phn:
) ( ) (
) 2 ( ] [
] 2 ) [(
2 1 2 2 1 1 2 1
2
2
2 2 1 2 1 2 2 2 2 1 2
2
2 2
1 2 2 1
2
2 2
2
1 2 1 1
+ + + +
+ + + +
+ + + + =
cos ga m cos ga m m
sin a a m cos a a m a m
cos m m a m a m m



) (
] [
2 1 2 2
2
2
1 2 1 2 2
2
2 2 1 2 2 1 2
2
2 2 2
+ +
+ + + + =
cos ga m
sin a a m a m cos a a m a m


- Biu din phng trnh chuyn ng ca c h Tay my di dng ma trn
Di dng ma trn, phng trnh chuyn ng hay phng trnh ng lc
hc Tay my di dng ma trn c th vit nh sau:
(2.18)
Chng 5: ng lc hc robot v ng dng trong iu khin
68
(

=
(

+
+ + +
+
(

+
+
+
(

+
+ + + +
2
1
2 1 2 2
2 1 2 2 1 1 2 1
2
2
1 2 1 2
2
2
2 2 1 2 1 2
2
1
2
2 2 2 2 1 2
2
2 2
2 2 1 2
2
2 2 2 2 1
2
2 2
2
1 2 1

) (
) ( ) (

) 2 (

2 ) (
cos ga m
cos ga m cos ga m m
sin a a m
sin a a m
a m cos a a m a m
cos a a m a m cos m m a m a m m

Ta tm c biu thc ng lc hc tay my di dng chun, c biu
din chung di dng sau :

) q ( ) q , q ( q ) q ( = + + G V M


(2.20)
M(q) l ma trn qun tnh,
) q , q ( V
l vect lc Coriolis hoc/v lc hng
tm v G(q) l vect trng lc.
Vi biu thc trn M(q) l ma trn i xng.

b. V d 2.
Xy dng Phng trnh ng lc hc ca robot hai bc t do cu hnh RT.

d2

Hnh 5.3. Cu hnh ca Robot 2 bc t do RP
Xut pht t phng php ng lc hc cho h c hc tng qut
Phng trnh chuyn ng Lagrange thit lp cho mt c h c cho bi:

2
u
0
O
(2.19)
Chng 5: ng lc hc robot v ng dng trong iu khin
69

q q
=
c
c

c
c L L
dt
d

(2.1)
Trong q l vect biu din cc to suy rng ca cc khu ca Tay
my q
i
, t l vect biu din cc lc suy rng ca cc khu ca tay my v hm
Lagrange l s chnh lch gia ng nng v th nng ca c h, vi:
P K L = (2.2)
Tng t v d 1, ta kho st c h vi gi thit rng khi lng ca khu
c tp trung cc khp. Ma trn bin khp l:
| |
T
d q
2 1
u =
(2.3)
v ma trn biu din ca lc suy rng c th hin:
| |
T
2 1
t t t = (2.4)
vi
2 1
,t t l cc m men c cho bi cc c cu tc ng (chng hn l
m men pht ng ca cc ng c in).
- Biu thc ng nng v th nng
x
y
m2(x2,y2)
m1(x1,y1)
d2
l1

Hnh 5.4. To ca cc khu trn Robot
+ Vi khu 1 chuyn ng quay, ta c biu thc ca ng nng v th nng
tng ng l:
2
1
2
1 1 2
1
1
u

l m K =
(2.5)
1 1 1 1
sinu gl m P =
(2.6)
+ Vi khu 2 chuyn ng tnh tin, ta c:
1 2 2
cosu d x =
(2.7)

1 2 2
sinu d y =

(2.8)
1
u
Chng 5: ng lc hc robot v ng dng trong iu khin
70

1 1 2 1 2 2
sin cos u u u

d d x =

(2.9)

1 1 2 1 2 2
cos sin u u u

d d y + =

(2.10)
Bnh phng vn tc l :

2
2
2
1
2
2
2
2
2
2
2
2
d d y x v

+ = + = u

(2.11)
Do vy ng nng ca khu 2 l:

2
2 2
2
1
2
2 2
2
2 2 2
2
1
2
1
2
1
d m d m v m K

+ = = u

(2.12)
Th nng cho khu 2 l:

1 2 2 2 2 2
sinu gd m gy m P = =

(2.13)
- Phng trnh Lagrange
Hm Lagrange cho Tay my ny l:
1 2 2 1 1 1
2
2 2
2
1
2
2 2
2
1
2
1 1 2 1 2 1
sin sin
2
1
2
1
2
1
u u u u gd m gl m d m d m l m P P K K P K L + + = + = =


Vy : 1 2 2 1 1
2
2 2
2
1
2
2 2
2
1 1
sin ) (
2
1
) (
2
1
u u g d m l m d m d m l m L + + + =


(2.14)
Nhng hng thc cn tnh c th hin nh di y:
Chng 5: ng lc hc robot v ng dng trong iu khin
71
1 2
2
1 2 2
2
2 2
2
2 2
2
1 2 2 1 1
1
1
2
2 1 2 2 2 1
2
1 1
1
1
2
2 2
2
1 1
1
sin
cos ) (
) 2 (
) (
u u
u
u
u u u
u
u
u
g m d m
d
L
d m
d
L
dt
d
d m
d
L
g d m l m
L
d d d m l m
L
dt
d
d m l m
L
=
c
c
=
c
c
=
c
c
+ =
c
c
+ + =
c
c
+ =
c
c

Cui cng, phng trnh chuyn ng ca c h Tay my c cho bi h
hai phng trnh vi phn:

q q
=
c
c

c
c L L
dt
d

1 2 2 1 1 1
2
2 1 2 2 2 1
2
1 1
1 1
1
cos ) ( ) 2 ( u u u u
u u
t g d m l m d d d m l m
L L
dt
d
+ + + + =
c
c

c
c
=

Vy :
1 2 2 1 1 2 2 1 2 1
2
2 2
2
1 1 1
cos ) ( 2 ) ( u u u t g d m l m d d m d m l m + + + + ==


1 2
2
1 2 2 2 2
2 2
2
sinu u t g m d m d m
d
L
d
L
dt
d
+ =
c
c

c
c
=

Vy :
1 2
2
1 2 2 2 2 2
sinu u t g m d m d m + =


- Biu din phng trnh chuyn ng ca c h Tay my di dng ma trn
Di dng ma trn, phng trnh chuyn ng hay phng trnh ng lc
hc tay my c th vit nh sau:
1 2 2 1 1 2 2 1 2 1
2
2 2
2
1 1 1
cos ) ( 2 ) ( u u u t g d m l m d d m d m l m + + + + ==


1 2
2
1 2 2 2 2
2 2
2
sinu u t g m d m d m
d
L
d
L
dt
d
+ =
c
c

c
c
=

=
(

+
+
(

+
(

+
2
1
1 2
1 2 2 1 1
2
1 2 2
2 1 2 2
2
1
2
2
2 2
2
1 1

sin
cos ) (

d 2
d 0
0
g m
g d m l m
d m
d m
m
d m l m
u
u u

(2.15)
)
Chng 5: ng lc hc robot v ng dng trong iu khin
72
5.4. Phng trnh ng lc hc tay my.
5.4.1. Tng qut.
Chng ta ch ra cc v d ng dng phng trnh Lagrange tnh ton
nhng phng trnh ng lc hc ca cc Tay my. Trong cc v d trn v
ng lc hc ta nhn thy biu thc kt qu c dng:

) q ( ) q q, ( q ) q ( = + + G V M

vi q l bin khp, l vect lc hoc m men suy rng.
nhn c phng trnh ng lc hc ca tay my ta bt u t vic xc
nh ng nng v th nng ca c h, xy dng hm Lagrange, sau a cc
hng thc vo phng trnh Lagrange, thu gn ta s nhn c phng trnh
chuyn ng ca c h Tay my.
xy dng m hnh ng lc hc tay my bng cch s dng phng
trnh Lagrange loi II, ta cn phi bit cc thng s sau y:
- Khi lng cng nh ta ca khi tm ca cc khu,
- Vn tc ca im bt k trn Tay my thit k,
- Cc thng s v ma st ng, ma st tnh gia cc khu, khp v tc ng
nhiu nu c.
Do trong thc t, hot ng ca Tay my lun b nh hng bi cc lc ma
st v nhiu, nn ta s khi qut m hnh ng lc hc Tay my va nhn c
nh sau:
) q ( ) q ( ) q , q ( q ) q (
d
= + + + + G F V M


vi q v t c nh ngha trn. M(q) l ma trn qun tnh, ) q , q ( V l vect
lc Coriolis/hng tm v G(q) l vect trng lc nh phn tch trn.
phng trnh khi qut trn, ta cng thm lc ma st vo , vi:

d v
F F F + = q ) q (


trong F
v
l ma trn h s ca ma st tnh v F
d
l ma st ng. Ta s a
thm lng nhiu t
d
vo phng trnh, i lng ny gip m t phn b cho
trng hp m hnh ng lc hc c sai st m ta cha lng ht trong qu trnh
xy dng m hnh ton.
Vic xc nh lc ma st rt kh khn, cch m t nh vy c chp nhn.
Hu ht nhng tr lc no chng li chuyn ng u c cc nh nghin cu
m t trong m hnh ng lc hc Tay my theo cch nh trn.
Phng trnh ng lc hc Tay my cng c biu din di dng:

t t = + +
d
q q N q q M ) , ( ) (

Chng 5: ng lc hc robot v ng dng trong iu khin
73
:
) ( ) ( ) , ( ) , ( q G q F q q V q q N + + =

biu din cho c cc i lng phi tuyn.
5.4.2. Ma trn qun tnh
Ma trn qun tnh M(q) n x n c cc thnh phn c nh ngha bi biu
thc:

= (
(

c
c
c
c
=
n
i
k
T
i
i
j
i
jk
q
T
I
q
T
trace q m
1
) (

-
j i
q T c c /
m t s thay i v tr ca im thuc khu th i gy nn bi s
chuyn dch ca khu th j.
- I
i
l ma trn qun tnh gi ca khu i v c xc nh di dng khai trin
nh sau:
(
(
(
(
(

= =
} } } }
} } } }
} } } }
} } } }
}
dm dm z dm y dm x
dm z dm z dm yz dm xz
dm y dm zy dm y dm xy
dm x dm zx dm yx dm x
dm r r I
T
i
i
i
i
i
2
2
2

y cc gi tr c tnh trn khu th i. y l ma trn hng s v xc
nh gi tr mt ln cho mi khu. Ma trn ny ph thuc vo dng hnh hc v
s phn b khi lng ca khu i. Trong cc thnh phn qun tnh c phn
bit nh sau:
M men qun tnh:
}
}
}
+ =
+ =
+ =
dm y x I
dm z x I
dm z y I
zz
yy
xx
) (
) (
) (
2 2
2 2
2 2

M men qun tnh ly tm:
}
}
}
=
=
=
dm yz I
dm xz I
dm xy I
yz
xz
xy

Chng 5: ng lc hc robot v ng dng trong iu khin
74
M men qun tnh bc nht:
}
}
}
=
=
=
dm z z m
dm y y m
dm x x m


vi m l tng khi lng khu i, v:
| |
T
i
i
z y x r 1 =


l bn knh vect biu din trng tm khu th i trong h ta i.
Ta c th vit :
(
(
(
(
(
(
(
(

+
+
+ +
=
m z m y m x m
z m
I I I
I I
y m I
I I I
I
x m I I
I I I
I
zz yy xx
yz xz
yz
zz yy xx
xy
xz xy
zz yy xx
i
2
2
2

Vi
j i
q T c c / = 0, j>i ta c th vit ngn gn hn :

= (
(

c
c
c
c
=
n
k j i
k
T
i
i
j
jk
q
T
I
q
T
trace q m
) , max(
) (

y l mt ma trn i xng dng
5.4.3. Vect coriolis/hng tm

q
K
q M q q M q
q
q q M q q V
T
c
c
=
c
c
=

) ) ( ( ) ( ) , (
2
1

Cc thnh phn ca vect Coriolis/hng tm c xc nh nh sau:
j i
j i
ijk
q q v q q V

=
,
) , (


(
(

c
c

c
c
+
c
c
=
k
ij
j
ki
i
kj
ijk
q
m
q
m
q
m
v
2
1

5.4.4.Vect trng lc:
Ta c
Chng 5: ng lc hc robot v ng dng trong iu khin
75

4
1
e I q T g
q
q
q P
q G
i
n
i
i
T

=
c
c
=
c
c
=
)) ( (
) (
) (

e
4
= (0, 0, 0, 1)
T , ta suy ra c:

=
c
c
=
n
i
i i
T
e I q T g
q
q G
1
4
)) ( ( ) (


n ..., 2 , 1 j , e I
q
T
) g I ( ) q ( G
n
1 i
4 i
i T
n
=
c
c
=

=

y tht s ta c vect G(q) l:
(
(
(
(
(
(
(
(

c
c

c
c

c
c

=
=
=
n
n i
i
n
i T
n
i
i
i T
n
i
i
i T
e I
q
T
g
e I
q
T
g
e I
q
T
g
q G
4
2
4
2
1
4
1

) (

n y ta kho st bi ton ng lc hc Tay my t thu c
cc gi tr lc hay m men suy rng trn mi khp trong qu trnh hot ng ca
robot. Da trn nhng thng s ny ta s a ra nhng gii php thit k kt cu
cng nh iu khin robot tt hn. Bi b iu khin s n gin v c hiu qu
hn nu nhng c tnh ng lc hc bit ca Tay my c kt hp cht ch
ngay t trong giai on thit k.
5.5. ng dng bi ton ng lc hc m t i tng robot trong iu
khin.
Sau khi thc hin tnh ton bi ton ng lc hc robot, chng ta c th s
dng trc tip cc m hnh ton thu c xy dng i tng trong vic m
phng v a ra cc tng trong vn iu khin.
Tt nhin, vic xc nh cc thng s ca robot l rt kh khn, v vy
chng ta ch xy dng i tng robot c tnh cht m phng thc hin cc
gii thut iu khin. V trong thc t, cc thng s ca m hnh ng lc hc
tay my chu nh hng ca rt nhiu cc yu t nh : chnh xc trong gia
cng c kh, nh hng ca cc tc nhn c tnh cht nh nhiu, cc sai s m
hnh khi thc hin tnh ton...
Trong mc ny, bng cc phn mm h tr m phng (Visual C, Visual
Basic, Matlab, ...) chng ta thc hin m hnh ha cc robot t cc phng trnh
Chng 5: ng lc hc robot v ng dng trong iu khin
76
ng hc v ng lc hc. T c s ny c th thc hin thit k v ch to cc
robot thc thi cc mc tiu ra.
Chng ta s thc hin vic m hnh ha cc i tng robot tm hiu
cc chng trc :
a. Xy dng m hnh m phng iu khin v tr ca robot Puma, da vo cc
phng trnh ng hc tm c chng 4.

Hnh 5.6. M phng robot Puma theo v tr

Hnh 5.7. M phng qu o ca robot Puma.
b. Xy dng m hnh ton cho robot hai bc t do cu hnh RT.
Do tnh cht phc tp trong iu khin, vn ca nhng nh nghin cu l
lm sao c th tm gii thut iu khin cho robot khi m tt c cc khu t thit
Chng 5: ng lc hc robot v ng dng trong iu khin
77
k n thi cng u gp nhiu kh khn. Mt cng c rt hu hiu c a ra
l m hnh ton ca robot, nn tng ca m hnh ton l bi ton ng lc hc
c xt n. Mc chnh xc , chnh lch sai s m hnh... ph thuc
nhiu vo qu trnh tnh ton ng lc hc, trong khng loi tr cc kh nng
nh hng ca nhiu v cc vn khc lin quan n ng lc hc c h.
Chng ta quay li v d 5.2, t bi ton ng lc hc xy dng cho robot
hai bc t do, cu hnh RT thu c m hnh ton ca i tng robot.
Xt trn lnh vc iu khin, h robot l cc h phi tuyn, chnh v vy vic
iu khin v s dng cc gii thut phi tun theo cc nguyn tc iu khin h
phi tuyn.
Xy dng m hnh robot RT trong matlab :
U1
U2
Theta
d
(Di en ap di eu khi en motor khop 1)
(Di en ap di eu khi en motor khop 2)
(Goc quay khop 1)
(Do dai ti nh ti en d khop 2)
ROBOT_2DOF

Hnh 5.8. M hnh ton robot 2 bc t do RT
m phng thnh cng, chng ta cn chn cc thng s ca robot thch
hp. Cc thng s ny c th thu thp s liu hay la chn theo cc ti liu
c nghin cu.
2
d2
1
theta
1
s
theta_dot
f(u)
theta_2dot
1
s
theta_
1
s
d_dot
f(u)
d_2dot
1
s
d
2
u2
1
u1

Hnh 5.9. M hnh ton t phng trnh ng lc hc robot.