Robot cng nghip

84

chng VII ng lc hc Robot (Dynamic of Robot)

7.1. Nhim v v phng php phn tch ng lc hc robot Nghin cu ng lc hc robot l cng vic cn thit khi phn tch cng nh tng hp qu trnh iu khin chuyn ng. Vic nghin cu ng lc hc robot thng gii quyt hai nhim v sau y : 1/ Xc nh momen v lc ng xut hin trong qu trnh chuyn ng. Khi qui lut bin i ca bin khp qi(t) coi nh bit. Vic tnh ton lc trong c cu tay my l rt cn thit chn cng sut ng c, kim tra bn, cng vng, m bo tin cy ca robot. 2/ Xc nh cc sai s ng tc l sai lch so vi qui lut chuyn ng theo chng trnh. Lc ny cn kho st Phng trnh chuyn ng ca robot c tnh n c tnh ng lc ca ng c v cc khu. C nhiu phng php nghin cu ng lc hc robot, nhng thng gp hn c l phng php c hc Lagrange, c th l dng phng trnh Lagrange - Euler. i vi cc khu khp ca robot, vi cc ngun ng lc v knh iu khin ring bit, khng th b qua cc hiu ng trng trng (gravity effect), qun tnh (initial), tng h (Coriolis), ly tm (centripetal)... m nhng kha cnh ny cha c xt y trong c hc c in; C hc Lagrange nghin cu cc vn nu trn nh mt h thng khp kn nn y l nguyn l c hc thch hp i vi cc bi ton ng lc hc robot. 7.2. C hc Lagrange vi cc vn ng lc ca robot. Hm Lagrange ca mt h thng nng lng c nh ngha : L=K-P (7.1) Trong : K l tng ng nng ca h thng P l tng th nng K v P u l nhng i lng v hng nn c th chn bt c h to thch hp no bi ton c n gin. i vi mt robot c n khu, ta c :
K = Ki
i =1 n

P = Pi
i =1

y, Ki v Pi l ng nng v th nng ca khu th i xt trong h to chn.Ta bit mi i lng Ki v Pi l mt hm s ph thuc nhiu bin s: & & Ki = K(qi, q i ) v Pi = P(qi, q i ) Vi qi l to suy rng ca khp th i. Nu khp th i l khp quay th qi l gc quay i, nu l khp tnh tin th qi l di tnh tin di. Ta nh ngha : Lc tc dng ln khu th i (i=1, 2,..., n) vi quan nim l lc tng qut (Generalized forces), n c th l mt lc hoc mt momen (ph thuc vo bin khp qi l tnh tin hoc quay), c xc nh bi: d L L Fi = (7.2) & dt q i q i

TS. Phm ng Phc

Robot cng nghip

85

Phng trnh ny c gi l phng trnh Lagrange-Euler, hay thng c gi tt l phng trnh Lagrange.
7.3. V d p dng :

Xt mt robot c hai khu nh hnh v, Cc khu c chiu di l d1 v d2 vi cc khi lng tng ng m1 v m2 qui i v u mt ca khu. Robot c t thng ng chu gia tc trng trng g. Cc khp chuyn ng quay vi cc bin khp 1 v 2. Tnh lc tng qut. Qua v d ny, ch vi mt mi lin kt hai y khu, cc vn t ra u c mt g = 9,81m/s2 trong qu trnh nghin cu ng lc hc, v do , v d nu trn c th m rng p dng trong nhng trng hp phc tp O0 x x1 x2 hn. i vi khu 1 : 1 1 2 2&2 K 1 = m1 v 1 = m1 d 1 1 (7.3) z 2 2 1 P1 = -m1gd1cos1 (7.4) m1 y1 i vi khu 2 : V to : x2 = d1sin1 + d2sin(1 + 2) 2 y2 m2 y2 = -d1cos1 - d2cos(1 + 2) Chiu cao th nng : h = d1cos1 + d2cos(1 + 2) 2 2 2 & & v2 = x2 + y2 V mt vn tc : d & & & & Vi x 2 = x 2 = d 1 cos(1 ) 1 + d 2 cos(1 + 2 )( 1 + 2 ) dt d & & & & y 2 = y 2 = d 1 sin(1 )1 + d 2 sin(1 + 2 )(1 + 2 ) dt 2&2 &2 & & &2 &2 & & v 2 = d 1 1 + d 2 ( 1 + 21 2 + 2 ) + 2d 1d 2 cos( 2 )( 1 + 1 2 ) 2 2

ng nng v th nng s l : 1 1 2 2&2 &2 & & &2 &2 & & K 2 = m 2 v 2 = m 2 d 1 1 + d 2 ( 1 + 21 2 + 2 ) + 2d 1d 2 cos( 2 )( 1 + 1 2 ) 2 2 2 P2 = m 2 g[d 1 cos(1 ) + d 2 cos( 1 + 2 )]

(7.5) (7.6)

7.4. Hm Lagrange v lc tng qut : p dng hm Lagrange cho v d trn, ta c : L = (K1 + K2) - (P1 + P2) 1 1 2&2 &2 & & & &2 & & L = ( m1 + m 2 )d 1 1 + m 2 d 2 (1 + 21 2 + 2 ) + m 2 d 1d 2 cos 2 ( 1 + 1 2 ) + 2 2 2 2 + ( m1 + m 2 )gd 1 cos 1 + m 2 gd 2 cos(1 + 2 ) (7.7) Khi tnh lc tng qut, cc bin ca h : q1 = 1 v q2 = 2. i vi khu 1 : L L 2& & & & & = = ( m1 + m 2 )d 1 1 + m 2 d 2 ( 1 + 2 ) + 2 m 2 d 1d 2 cos 2 1 + m 2 d 1d 2 cos 2 2 2 & & q 1 1

TS. Phm ng Phc

Robot cng nghip

86

d L 2 && 2 & & && = ( m1 + m 2 )d 1 1 + m 2 d 2 (&&1 + && 2 ) 2 m 2 d 1d 2 sin 2 2 1 + 2 m 2 d 1d 2 cos 2 1 & dt 1 & && m d d sin 2 + m d d cos

L L = = ( m1 + m 2 )gd 1 sin 1 m 2 gd 2 sin(1 + 2 ) q 1 1

Vy :

2 1 2

2 2

2 1 2

2 2

L d L 2 && = [( m1 + m 2 )d 1 + m 2 d 2 + 2 m 2 d 1d 2 cos 2 ] 1 + 2 & dt 1 1 2 && & & &2 (7.8) +[ m 2 d 2 + m 2 d 1d 2 cos 2 ] 2 2 m 2 d 1d 2 sin 2 2 1 m 2 d 1d 2 sin 2 2 + + ( m1 + m 2 )gd 1 sin 1 + m 2 gd 2 sin( 1 + 2 ) Mun cho khu 1 quay c mt gc 1 th ng c phi to ra mt lc tng qut F1. Lc tng qut ny c c tnh phi tuyn, l hp tc dng ca nhiu yu t (non linear and cuppling). Tng t, tnh lc tng qut ca khu th hai , ta c : L & & & = m 2 d 2 1 + m 2 d 2 2 + m 2 d 1d 2 cos 2 1 2 2 &
F1 = d L && & & && = m 2 d 2 && 1 + m 2 d 2 2 + m 2 d 1d 2 cos 2 1 m 2 d 1d 2 sin 2 1 2 2 2 & dt 2
v Vy :
2

L = m2 d1d 2 sin( 2 )&1&2 m2 d1d 2 sin( 2 )&12 m2 gd 2 sin(1 + 2 ) 2

d L L 2 2& && & & = [m2 d 2 + m2 d1d 2 cos 2 ]1 + m2 d 2 2 dt 2 2 m d d sin( )& 2 + m gd sin( + ) F2 =
2 1 2 2 1 2 2 1 2

(7.9)

phn tch ngha cc thnh phn trong biu thc tnh lc tng qut, ta vit li cc biu thc F1, F2 nh sau :
&& &2 & & & & & F1 = D11&& 1 + D12 2 + D111 1 + D122 2 + D112 1 2 + D121 1 2 + D1 2 && &2 & & & & & F2 = D12 &&1 + D 22 2 + D 211 1 + D 222 2 + D 212 1 2 + D 221 1 2 + D 2 2 Hiu ng Hiu ng Hiu ng Hiu ng qun tnh ly tm tng h trng trng Effective inertias Centripetal effect Coriolis effect Gravity

(Trong : D111 = 0; D222 = 0; D112 = D121 = D212 = D221 =-m2d1d2sin2 ...) Trong cc biu thc trn, cc h s dng Dii hoc D ij th hin hiu ng qun tnh ti khp i hoc j gy ra bi gia tc ti khp i hoc j. Cc s hng c dng D & 2 l lc ly tm
ijj
j

tc ng ln khp i gy ra bi vn tc ti khp j. S hng dng D ijk& j&k + D ikj&k& j l lc Cariolis tc ng ln khp th i gy ra do vn tc ti khp j v k. S hng c dng Di l lc trng trng tc ng ln khp i.

TS. Phm ng Phc

Robot cng nghip

87

7.5. Phng trnh ng lc hc robot :

Xt khu th i ca mt robot c n khu. Tnh lc tng qut Fi ca khu th i vi khi lng vi phn ca n l dm. Lc tng qut Fi ng vai tr rt quan trng khi xy dng s khi thit lp hm iu khin cho robot c n bc t do.
7. 5. 1. Vn tc ca mt im trn robot : Mt im trn khu th i c m t trong h to c bn l : (7.10) r = Ti. ir i Trong : r l to ca im xt i vi khu th i, ir khng thay i theo thi gian. Ti l ma trn chuyn i t khu th i v h to gc : Ti = A1A2...Ai. Nh vy r l mt hm ca thi gian t. Tc ca vi khi lng dm c tnh bi cng thc : i T dr d i &= r = Ti r = i q j i r (7.11) j=1q & dt dt j

Khi tnh bnh phng ca vn tc ny ta c : &. & = r 2 ( x o , y o , z o ) = Tr ( & & T ) & & & rr rr
y
i

(7.12)

Khu i

dm

Ti r O0 z
Hnh 7.1. Kho st tc ca vi khi lng dm.

Vi rT l chuyn v vect v Tr l vit tt ca Trace (vt ca ma trn) : a 11 a 12 ... a 1n a a 22 ... a 2 n n 21 = a Trace ... ... ... ... i =1 ii a n1 a n 2 a 11 a nn Hay : x 2 x y . [x y z ] = y2 z2 z Do vy d d & 2 = Tr ( &. & T ) = Tr ( Ti .i r. Ti T .i r T ) r rr dt dt

TS. Phm ng Phc

Robot cng nghip

88
T i T i T & & = Tr i q j i r. i q k i r T k =1 q k j=1q j T i i Ti i i T Ti & & rr . q j qk = Tr qk j =1 k =1 q j

(7.13)

7. 5. 2. Tnh ng nng ca vi khi lng dm.

K hiu Ki l ng nng ca khu th i. dKi l ng nng ca vi khi lng dm t ti v tr ir trn khu th i. T 1 i i Ti i i T Ti & & dK i = Tr rr . q j q k dm 2 j=1k =1 q j q k T T 1 i i Ti i & & = Tr (7.14) ( r. dm.i r T ). i q jq k 2 j=1k =1 q j q k V do ng nng ca khu th i s l : Ti T 1 i i Ti i i T & & K i = dK = Tr ( r. r dm ). q j qk (7.15) 2 j =1 k =1 q j Khau i q k Khau i t

Ji =

r.i r T dm gi l ma trn gi qun tnh (Pseudo inertia matrix).

Khau i

ngha "gi qun tnh" c s dng v khi thit lp y cc phn t ca ma trn Ji ta c th lin h vi cc khi nim "mmen qun tnh c cc" v trnh by cc phn t ca Ji ging nh cc phn t ca mmen qun tnh c cc. Ta xt mi quan h ny nh sau : Theo nh ngha ta c : i i i i i i x 2 dm x ydm x zdm xdm i i i 2 i i i x ydm y dm y zdm ydm J i = i r.i r T dm = Ji = i i (7.16) x zdm i y i zdm i z 2 dm i zdm Khau i i i i xdm ydm zdm dm By gi ta nhc li mmen qun tnh c cc ca mt vt th bt k nh hnh v. y x Theo nh ngha ta c : I xx = ( y 2 + z 2 )dm z I yy = x 2 + z 2 )dm Hnh 7.2 : Mmen qun tnh c cc

I zz = ( x 2 + y 2 )dm

V v : Vy :

1 1 1 x2 = ( y2 + z2 ) + (x2 + z2 ) + (x2 + y2 ) 2 2 2 2 x dm =( I xx + I yy + I zz ) / 2 ; .v.v

Ngoi ra ta cn c : I xy = xydm ; I yz = yzdm ; I xz = xzdm

mx = xdm ; my = ydm ;

mz = zdm

TS. Phm ng Phc

Robot cng nghip

89

i chiu vi ma trn gi qun tnh Ji, ta c th trnh by Ji nh sau : I xx + I yy + I zz I yx I zx mx 2 I xx I yy + I zz I xy I zy my ji = 2 I xx + I yy I zz I yz I yz mz 2 mx my mz m Nh vy ngha biu trng ca Ji r. T 1 i i Ti Ti & & Vy ta c : K i = Tr Ji q j qk 2 j =1 k =1 q j q k Cui cng, ng nng ca mt robot c n khu c tnh :

(7.17)

(7.18)

K = Ki
i =1

(7.19)

7. 5. 3. Tnh th nng ca robot :

Th nng ca khu i c khi lng mi, trng tm c xc nh bi vect ri (vect biu din trng tm ca khu i trong h to c bn) l : (7.20) Pi = -mi. g. ri = -mi. g. Ti iri Trong , vect gia tc trng trng g c biu din di dng mt ma trn ct : gx 0 g 0 g = y = g z 9,8 0 0 Th nng ca ton c cu robot n khu ng s l :

P = mi gTi i ri
i =1

(7.21)

7. 5.4. Hm Lagrange : Sau khi xc nh ng nng v th nng ca ton c cu, ta c hm Lagrange ca robot c n bc t do :

L=

T T T 1 n i i Trace i J i i q 2 i =1 j =1 k =1 q k j

n q j qk + 1 mi gTi i ri & & 2 i =1

(7.22)

Chng ta ch rng, trong hm Lagrange vn cha cp n nh hng ca ngun truyn ng (gm cc phn tnh (stator) v phn ng (Rotor) ca ng c in).
7. 5. 5. Phng trnh ng lc hc robot :

Ta bit lc tng qut t ln khu th i ca robot c n khu (Phng trnh Lagrange - Euler) : d L L Fi = (7.23) & dt q i q i Sau khi thit lp hm Lagrange, vi p = 1... n, ta tnh c :
TS. Phm ng Phc

Robot cng nghip

90

(p l ch s ln lt ly theo j v k) T n i Ti Ti T L 1 n i qk + 1 Tr Ti J i Ti q j & & Ji = Tr & 2 i =1 j =1 q j q p q p 2 i =1 k =1 q p qk Thay i ch s gi j thnh k trong s hng th hai ,v rng :
T

(7.24)

T T Ti Ti T = Tr Ti J i Ti = Tr Ti J i Ti (7.25) Tr J q q q i q q j q p p p j j n i Ti Ti T L qk & Ji (7.26) ta c : = Tr & q p i =1 k =1 qk q p Cng rng : trong Ti(q1, q2, . . . , qi), vi qi l cc bin khp ca i khp u tin. Do Ti vy, nu i < p th = 0. q p

Cui cng ta c :

n i T T T L = Tr i J i i & q p i = p k =1 qk q p

qk &

(7.27)

Ly vi phn theo thi gian t ca phng trnh trn :

T T T d L d n i = Tr i J i i & dt q p dt i = p k =1 qk q p
n i T T T = Tr i J i i q q p i = p k =1 k

qk &

T n i i 2 qk + Tr Ti J i Ti qk qm + && & & q p i = p k =1 m =1 qk qm n i i 2Ti T T & & J i i qk qm + Tr qk i = p k =1 m =1 q p qm

(7.28)

(Bin i theo ch (7.25)) S hng cui ca phng trnh Lagrange Euler l :

2Ti T T L 1 n i i Ji i = Tr q p 2 i = p j =1 k =1 q j q p qk +

q j qk + & &
n q j qk + mi g Ti i ri & & q p i= p

2Ti T T 1 n i i Tr Ji i 2 i =1 j =1 k =1 qk q p q j

(7.29)

Cui cng ta c lc tng qut ca khu p :

Fp =

L d L & dt q p q p

Thay th cc ch s p v i thnh i v j, ta s c :
j j n n 2T j T jT T j j qk + Tr && & & Jj qk qm m j g rj qk qm qi qi j =i j =i k =1 m =1 (7.30) Vi mt robot c n bc t do th : j n T j T jT Fi = Tr Jj q qi j =i k =1 k

TS. Phm ng Phc

Robot cng nghip

91

q = [q1, q2, . . . ,qn]T & & & & q = [q 1 , q 2 , ... , q n ] T F = F[F1, F2, . . . , Fn]T

cho gn, ta biu din :

&& & & F = J ( q ) q + C ( q, q ) q + G ( q )

(7.31)

Trong : J th hin tc dng ca qun tnh, l mt ma trn i xng (n x n); C th hin tc dng ca lc ly tm v Cariolis, l mt vect (n x 1); G th hin tc dng ca lc trng trng, cng l mt vect (n x 1). y l phng trnh ng lc hc ca robot. Nu thm vo phng trnh trn cc tc dng khc nh : FEX c trng cho cc ngoi lc tc dng ln trc, V c trng cho hiu ng ma st, ta c :

&& & & & F = J ( q) q + C ( q, q) q + G ( q) + V ( q) + FEX

(7.32)

TS. Phm ng Phc