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

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

64

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

n

K Ki

P Pi

i 1

i 1

(2.1)

Trong , Ki v Pi 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 :

Ki K qi , qi v Pi Pqi , qi

(2.2)

Vi qi 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 :
F

d L L

dt q q

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

dt q q

(2.3)

Trong q l vect biu din cc to suy rng ca cc khu ca Tay

my qi, 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 :

L K P

(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:
q 1 2

(2.5)
v ma trn biu din ca lc suy rng c th hin:

(2.6)
vi 1 , 2 l cc m men c cho bi cc c cu tc ng (chng hn l m
men pht ng ca cc ng c in).
65

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

(x2,y2)
2

a2

m2
g

a1

m1
x

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 2
K1 1 2 m1a1 1

(2.7)

P1 m1 ga1 sin1
(2.8)
Vi khu 2 ta c:

x2 a1 cos1 a2 cos(1 2 )
(2.9)

y2 a1 sin1 a2 sin(1 2 )
(2.10)
x2 a11 sin1 a2 (1 2 ) sin(1 2 )

(2.11)

y 2 a11 cos1 a2 (1 2 ) cos(1 2 )

(2.12)
Bnh phng vn tc l :

v22 x22 y 22 a1212 a22 (1 2 )2 2a1a2 (12 12 ) cos 2

(2.13)
Do vy ng nng ca khu 2 l:
K2 1 2 m2v22 1 2 m2 a1212 1 2 m2 a 22 (1 2 )2 m2 a1a2 (12 12 ) cos 2
(2.14)
Th nng cho khu 2 l:

P2 m2 gy2 m2 g[a1 sin1 a2 sin(1 2 )]

(2.15)
66

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

Phng trnh Lagrange

Hm Lagrange cho Tay my ny l:
2 2
L K P K1 K 2 P1 P2 1 2 (m1 m2 )a1 1 1 2 m2 a 22 (1 2 ) 2
(2.16
m2 a1a2 (12 12 ) cos 2 (m1 m2 ) ga1 sin 1 m2 ga2 sin(1 2 )

)
Ta cn xc nh cc biu thc :

L
2
(m1 m2 )a1 1 m2 a 22 (1 2 ) m2 a1 a 2 (21 2 ) cos 2
1
d L
2
(m1 m2 )a1 1 m2 a 22 (1 2 ) m2 a1 a 2 (21 2 ) cos 2 m2 a1 a 2 (212 22 ) cos 2
dt 1
L
(m1 m2 ) ga1 cos 1 m2 ga 2 cos(1 2 )
1
L
m2 a 22 (1 2 ) m2 a1 a 21 cos 2

2
d L
m2 a 22 (1 2 ) m2 a1 a 21 cos 2 m2 a1 a 212 sin 2
dt 2
L
m2 a1 a 2 (12 12 ) sin 2 m2 ga 2 cos(1 2 )
2
Cui cng, phng trnh chuyn ng ca c h tay my c cho bi h
hai phng trnh vi phn:

1 [(m1 m2 )a12 m2 a22 2m1m2cos 2 ] 1

[m2 a22 m2 a1a2cos 2 ] 2 m2 a1a2 (2 1 2 22 ) sin 2
(m1 m2 ) ga1cos 1 m2 ga2cos (1 2 )

(2.18)

2 [m2 a22 m2 a1a2cos 2 ]1 m2 a22 2 m2 a1a2 12 sin 2

m2 ga2cos (1 2 )
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:

67

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

(m1 m2 )a12 m2 a22 2m1m2cos 2 m2 a22 m2 a1a2cos 2 1

(2.19)

2
2
m2 a2 m2 a1a2cos 2
m2 a2

2
m2 a1a2 (2 1 2 22 ) sin 2 (m1 m2 ) ga1cos1 m2 ga2cos (1 2 ) 1

2 sin
m
ga
cos
(

)
m
a
a

2
2
1
2
2
2 1 2 1
2

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

M (q)q V (q, q ) G(q)

(2.20)
M(q) l ma trn qun tnh, V (q, q ) 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

O0

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:

68

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

d L L

dt q q

(2.1)

Trong q l vect biu din cc to suy rng ca cc khu ca Tay

my qi, 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:

L K P

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

d2

(2.3)
v ma trn biu din ca lc suy rng c th hin:

(2.4)

vi 1 , 2 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
y

m 2 ( x 2 ,y 2)

l1
m 1(x 1,y 1 )

d2

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
2 2
K1 1 2 m1l1 1
tng ng l:
P1 m1 gl1 sin 1
(2.5)
(2.6)
+ Vi khu 2 chuyn ng tnh tin, ta c:

x2 d 2 cos 1

y2 d 2 sin 1

(2.8)
69

(2.7)

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

x2 d2 cos 1 d 21 sin 1
(2.9)

y 2 d2 sin 1 d 21 cos 1
(2.10)
Bnh phng vn tc l :
2 2
2
v22 x22 y 22 d 2 1 d2

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

K2

1
1
1
2 2
2
m2 v22 m2 d 2 1 m2 d2
2
2
2

(2.12)
Th nng cho khu 2 l:

P2 m2 gy2 m2 gd2 sin 1

(2.13)
Phng trnh Lagrange
Hm Lagrange cho Tay my ny l:

1 2 2 1
1
2 2
2
L K P K1 K 2 P1 P2 m1l1 1 m2 d 2 1 m2 d2 m1 gl1 sin 1 m2 gd 2 sin 1
2
2
2
1
1 2
2
2 2
Vy : L (m1l1 m2 d 2 )1 m2 d 2 (m1l1 m2 d 2 ) g sin 1
2
2
(2.14)
Nhng hng thc cn tnh c th hin nh di y:

70

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

L
2
2

(
m
l

m
d
)1
1
1
2
2
1
d L
2
d 2 )

m
l

m
(
2
d
d
1
1
1
2
2
2 1
2
1
dt 1
L
(m1l1 m2 d 2 ) g cos 1
1
L
m2 d2

d 2

(2.15)
)

d L
m2 d2

dt d 2
L
2
m2 d 21 m2 g sin 1
d 2
Cui cng, phng trnh chuyn ng ca c h Tay my c cho bi h
hai phng trnh vi phn:

d L L
2
2

m1l1 1 m2 (2d 2 d21 d 2 1 ) (m1l1 m2 d 2 ) g cos 1

dt 1 1

Vy :

d L L

dt q q

1 (m1l12 m2 d 2 2 )1 2m21d 2 d2 (m1l1 m2 d 2 ) g cos 1

d L L
m d 2 m g sin

m
d
2 2
2 2 1
2
1
dt d2 d 2

Vy :

2 m2 d2 m2 d 212 m2 g sin 1

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 (m1l12 m2d 2 2 )1 2m21d 2d2 (m1l1 m2 d2 ) g cos 1

2

d L L
m d 2 m g sin

m
d
2
2
2 2 1
2
1
dt d2 d 2

m1l12 m2 d 2 2 0 1 2m2 d 21d 2 (m1l1 m2 d 2 ) g cos 1 1

m
g
sin

0
m2 d 2 m2 d 21
2
1
2

71

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

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:

M (q)q V (q, q ) G(q)

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:
M (q)q V (q, q ) F (q ) G(q) d

vi q v c nh ngha trn. M(q) l ma trn qun tnh, V (q, q ) 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:

F (q ) Fv q Fd
trong Fv l ma trn h s ca ma st tnh v Fd l ma st ng. Ta s a
thm lng nhiu 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:

N ( q, q ) d
M ( q)q
72

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

N (q, q ) V (q, q ) F (q ) G(q)

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:

Ti
TiT
m jk ( q ) trace
Ii

q
q k
i 1
j
n

- Ti / q j m t s thay i v tr ca im thuc khu th i gy nn bi s

chuyn dch ca khu th j.
- Ii l ma trn qun tnh gi ca khu i v c xc nh di dng khai trin
nh sau:

x 2 dm

xy dm
I i i ri i riT dm
xz dm
x dm

yx dm
y dm
yz dm
y dm
2

zx dm
zy dm
z dm
z dm
2

x dm
y dm
z dm
dm

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:

I yy ( x 2 z 2 )dm
I zz ( x 2 y 2 )dm

I xx ( y 2 z 2 )dm

M men qun tnh ly tm:

I xz xz dm
I yz yz dm
I xy xy dm

73

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

my y dm
mz z dm
mx x dm

M men qun tnh bc nht:

vi m l tng khi lng khu i, v:

i

ri x

l bn knh vect biu din trng tm khu th i trong h ta i.

Ta c th vit :

I xx I yy I zz

I xy
Ii

I xz

mx

Vi

Ti / q j

I xy

I xz

I xx I yy I zz
2

I yz
I xx I yy I zz

I yz

2
mz

my

mx

my

mz

= 0, j>i ta c th vit ngn gn hn :

T TiT
m jk (q) trace
Ii

i max( j , k )
q j qk
n

y l mt ma trn i xng dng

5.4.3. Vect coriolis/hng tm

1 T
K
V ( q, q ) M ( q )q
( q M ( q )q ) M q
2 q
q
Cc thnh phn ca vect Coriolis/hng tm c xc nh nh sau:

V (q, q ) vijk qi q j
i, j

vijk

1 mkj mki mij

2 qi
q j
q k

5.4.4.Vect trng lc:

Ta c
74

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

G( q )

P( q )
q

( g T Ti ( q ))I i e 4
i 1 q
n

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

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

G ( q )

, j 1,2..., n

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

n T Ti

g q I i e 4
1
in1

T
i
g T
I i e4

G( q ) i 2

q 2

T
T
i
g
I i e4
i n

q n
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
75

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

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
76

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

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

Theta

(Dien ap dieu khien motor khop 1)

(Goc quay khop 1)

U2

d
(Do dai tinh tien d khop 2)

(Dien ap dieu khien motor 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.
1
u1
1
s

f(u)
theta_2dot

theta_dot

1
s

theta_

theta

2
u2
1
s

f(u)
d_2dot

d_dot

1
s

2
d

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

77

d2