Robot c«ng nghiÖp 84

ch−¬ng VII
§éng lùc häc Robot
(Dynamic of Robot)

7.1. NhiÖm vô vµ ph−¬ng ph¸p ph©n tÝch ®éng lùc häc robot

Nghiªn cøu ®éng lùc häc robot lµ c«ng viÖc cÇn thiÕt khi ph©n tÝch còng nh− tæng
hîp qu¸ tr×nh ®iÒu khiÓn chuyÓn ®éng. ViÖc nghiªn cøu ®éng lùc häc robot th−êng gi¶i
quyÕt hai nhiÖm vô sau ®©y :
1/ X¸c ®Þnh momen vµ lùc ®éng xuÊt hiÖn trong qu¸ tr×nh chuyÓn ®éng. Khi ®ã qui
luËt biÕn ®æi cña biÕn khíp qi(t) coi nh− ®· biÕt.
ViÖc tÝnh to¸n lùc trong c¬ cÊu tay m¸y lµ rÊt cÇn thiÕt ®Ó chän c«ng suÊt ®éng c¬,
kiÓm tra ®é bÒn, ®é cøng v÷ng, ®¶m b¶o ®é tin cËy cña robot.
2/ X¸c ®Þnh c¸c sai sè ®éng tøc lµ sai lÖch so víi qui luËt chuyÓn ®éng theo ch−¬ng
tr×nh. Lóc nÇy cÇn kh¶o s¸t Ph−¬ng tr×nh chuyÓn ®éng cña robot cã tÝnh ®Õn ®Æc tÝnh ®éng
lùc cña ®éng c¬ vµ c¸c kh©u.
Cã nhiÒu ph−¬ng ph¸p nghiªn cøu ®éng lùc häc robot, nh−ng th−êng gÆp h¬n c¶ lµ
ph−¬ng ph¸p c¬ häc Lagrange, cô thÓ lµ dïng ph−¬ng tr×nh Lagrange - Euler. §èi víi c¸c
kh©u khíp cña robot, víi c¸c nguån ®éng lùc vµ kªnh ®iÒu khiÓn riªng biÖt, kh«ng thÓ bá
qua c¸c hiÖu øng träng tr−êng (gravity effect), qu¸n tÝnh (initial), t−¬ng hæ (Coriolis), ly
t©m (centripetal)... mµ nh÷ng khÝa c¹nh nÇy ch−a ®−îc xÐt ®Çy ®ñ trong c¬ häc cæ ®iÓn; C¬
häc Lagrange nghiªn cøu c¸c vÊn ®Ò nªu trªn nh− mét hÖ thèng khÐp kÝn nªn ®©y lµ nguyªn
lý c¬ häc thÝch hîp ®èi víi c¸c bµi to¸n ®éng lùc häc robot.

7.2. C¬ häc Lagrange víi c¸c vÊn ®Ò ®éng lùc cña robot.

Hµm Lagrange cña mét hÖ thèng n¨ng l−îng ®−îc ®Þnh nghÜa :
L=K-P (7.1)
Trong ®ã : K lµ tæng ®éng n¨ng cña hÖ thèng
P lµ tæng thÕ n¨ng
K vµ P ®Òu lµ nh÷ng ®¹i l−îng v« h−íng nªn cã thÓ chän bÊt cø hÖ to¹ ®é thÝch hîp
nµo ®Ó bµi to¸n ®−îc ®¬n gi¶n. §èi víi mét robot cã n kh©u, ta cã :
n n
K = ∑ Ki vµ P = ∑ Pi
i =1 i =1
ë ®©y, Ki vµ Pi lµ ®éng n¨ng vµ thÕ n¨ng cña kh©u thø i xÐt trong hÖ to¹ ®é chän.Ta
biÕt mçi ®¹i l−îng Ki vµ Pi lµ mét hµm sè phô thuéc nhiÒu biÕn sè:
Ki = K(qi, q& i ) vµ Pi = P(qi, q& i )
Víi qi lµ to¹ ®é suy réng cña khíp thø i. NÕu khíp thø i lµ khíp quay th× qi lµ gãc
quay θi, nÕu lµ khíp tÞnh tiÕn th× qi lµ ®é dµi tÞnh tiÕn di.
Ta ®Þnh nghÜa : Lùc t¸c dông lªn kh©u thø i (i=1, 2,..., n) víi quan niÖm lµ lùc tæng
qu¸t (Generalized forces), nã cã thÓ lµ mét lùc hoÆc mét momen (phô thuéc vµo biÕn khíp
qi lµ tÞnh tiÕn hoÆc quay), ®−îc x¸c ®Þnh bëi:
d ∂L ∂L
Fi = − (7.2)
dt ∂q& i ∂q i

TS. Ph¹m §¨ng Ph−íc

Robot c«ng nghiÖp 85

Ph−¬ng tr×nh nÇy ®−îc gäi lµ ph−¬ng tr×nh Lagrange-Euler, hay th−êng ®−îc gäi t¾t
lµ ph−¬ng tr×nh Lagrange.

7.3. VÝ dô ¸p dông :

XÐt mét robot cã hai kh©u nh− h×nh vÏ, C¸c kh©u cã chiÒu dµi lµ d1 vµ d2 víi c¸c
khèi l−îng t−¬ng øng m1 vµ m2 qui ®æi vÒ ®Çu mót cña kh©u. Robot ®−îc ®Æt th¼ng ®øng
chÞu gia tèc träng tr−êng g. C¸c khíp chuyÓn ®éng quay víi c¸c biÕn khíp θ1 vµ θ2. TÝnh
lùc tæng qu¸t.
Qua vÝ dô nÇy, chØ víi mét mèi liªn kÕt hai
y kh©u, c¸c vÊn ®Ò ®Æt ra ®Òu ®· cã mÆt
g = 9,81m/s2 trong qu¸ tr×nh nghiªn cøu ®éng lùc häc,
vµ do ®ã, vÝ dô nªu trªn cã thÓ më réng ®Ó
¸p dông trong nh÷ng tr−êng hîp phøc t¹p
O0 x1 x2 x h¬n. §èi víi kh©u 1 :
1 1
z K1 = m1 v 12 = m1d 12 θ& 12 (7.3)
2 2
θ1 P1 = -m1gd1cosθ1 (7.4)
y1 m1 §èi víi kh©u 2 :
VÒ to¹ ®é :
θ2 x2 = d1sinθ1 + d2sin(θ1 + θ2)
y2 m2
y2 = -d1cosθ1 - d2cos(θ1 + θ2)
ChiÒu cao thÕ n¨ng :
h = d1cosθ1 + d2cos(θ1 + θ2)
VÒ mÆt vËn tèc : v 2 = x& 2 + y& 2
2 2 2

d
Víi 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
[ ]
v 2 = d 12 θ& 12 + d 22 (θ& 12 + 2θ& 1θ& 2 + θ& 22 ) + 2d 1d 2 cos(θ 2 )(θ& 12 + θ& 1θ& 2 )
2

§éng n¨ng vµ thÕ n¨ng sÏ lµ :

1 1
[
K 2 = m 2 v 22 = m 2 d 12 θ& 12 + d 22 (θ& 12 + 2θ& 1θ& 2 + θ& 22 ) + 2d 1d 2 cos(θ 2 )(θ& 12 + θ& 1θ& 2 )
2 2
] (7.5)

P2 = − m 2 g[d 1 cos(θ1 ) + d 2 cos(θ 1 + θ 2 )] (7.6)

7.4. Hµm Lagrange vµ lùc tæng qu¸t :

¸p dông hµm Lagrange cho vÝ dô trªn, ta cã :
L = (K1 + K2) - (P1 + P2)
1 1
L = ( m1 + m 2 )d 12 θ& 12 + m 2 d 22 (θ& 12 + 2θ& 1θ& 2 + θ& 22 ) + m 2 d 1d 2 cos θ 2 (θ& 12 + θ& 1θ& 2 ) +
2 2
+ ( m1 + m 2 )gd 1 cos θ1 + m 2 gd 2 cos(θ1 + θ 2 ) (7.7)
Khi tÝnh lùc tæng qu¸t, c¸c biÕn cña hÖ : q1 = θ1 vµ q2 = θ2.
§èi víi kh©u 1 :
∂L ∂L
= = ( m1 + m 2 )d 12 θ& 1 + m 2 d 22 (θ& 1 + θ& 2 ) + 2 m 2 d 1d 2 cos θ 2 θ& 1 + m 2 d 1d 2 cos θ 2 θ& 2
∂q& 1 ∂θ1&

TS. Ph¹m §¨ng Ph−íc

Robot c«ng nghiÖp 86

d ∂L && + m d 2 (&& && & & &&

= ( m1 + m 2 )d 12 θ 2 2 θ 1 + θ 2 ) − 2 m 2 d 1d 2 sin θ 2 θ 2 θ 1 + 2 m 2 d 1d 2 cos θ 2 θ 1 −
&
dt ∂θ1
1

− m d d sin θ θ& 2 + m d d cos θ θ

2 1 2 2 2 2 1 2 2 2
&&
∂L ∂L
= = − ( m1 + m 2 )gd 1 sin θ1 − m 2 gd 2 sin(θ1 + θ 2 )
∂q 1 ∂θ1
VËy :
d ∂L ∂L && +
F1 = − = [( m1 + m 2 )d 12 + m 2 d 22 + 2 m 2 d 1d 2 cos θ 2 ]θ
&
dt ∂θ 1 ∂θ 1
1

&& − 2 m d d sin θ θ& θ& − m d d sin θ θ& 2 +

+[ m 2 d 22 + m 2 d 1d 2 cos θ 2 ]θ (7.8)
2 2 1 2 2 2 1 2 1 2 2 2
+ ( m1 + m 2 )gd 1 sin θ 1 + m 2 gd 2 sin(θ 1 + θ 2 )
Muèn cho kh©u 1 quay ®−îc mét gãc θ1 th× ®éng c¬ ph¶i t¹o ra mét lùc tæng qu¸t ≥
F1. Lùc tæng qu¸t nÇy cã ®Æc tÝnh phi tuyÕn, lµ hîp t¸c dông cña nhiÒu yÕu tè (non linear
and cuppling).
T−¬ng tù, ®Ó tÝnh lùc tæng qu¸t cña kh©u thø hai , ta cã :
∂L
= m 2 d 22 θ& 1 + m 2 d 22 θ& 2 + m 2 d 1d 2 cos θ 2 θ& 1
∂θ& 2
d ∂L
= m 2 d 22 &&
θ1 + m 2 d 22 θ &&
&& + m d d cos θ θ & &
2 1 − m 2 d 1d 2 sin θ 2 θ 1θ 2
&
dt ∂θ 2
2 2 1 2

∂L
vµ = − m2 d1d 2 sin(θ 2 )θ&1θ&2 − − m2 d1d 2 sin(θ 2 )θ&12 − m2 gd 2 sin(θ1 + θ 2 )
∂θ 2
VËy :
d ∂L ∂L
F2 = & − = [m2 d 22 + m2 d1d 2 cosθ 2 ]θ&&1 + m2 d 22θ&&2
dt ∂θ 2 ∂θ 2 (7.9)
− m d d sin(θ )θ& 2 + m gd sin(θ + θ )
2 1 2 2 1 2 2 1 2

§Ó ph©n tÝch ý nghÜa c¸c thµnh phÇn trong biÓu thøc tÝnh lùc tæng qu¸t, ta viÕt l¹i
c¸c biÓu thøc F1, F2 nh− sau :

F1 = D11&&
θ1 + D12 θ&& + D θ& 2 + D θ& 2 + D θ& θ& + D θ& θ& + D
2 111 1 122 2 112 1 2 121 1 2 1
&& && & & & & & &
F2 = D12 θ1 + D 22 θ 2 + D 211θ 1 + D 222 θ 2 + D 212 θ1θ 2 + D 221θ 1θ 2 + D 2
2 2

HiÖu øng HiÖu øng HiÖu øng HiÖu øng

qu¸n tÝnh ly t©m t−¬ng hæ träng tr−êng
Effective inertias Centripetal effect Coriolis effect Gravity

(Trong ®ã : D111 = 0; D222 = 0; D112 = D121 = D212 = D221 =-m2d1d2sinθ2 ...)

Trong c¸c biÓu thøc trªn, c¸c hÖ sè d¹ng Dii hoÆc D ij thÓ hiÖn hiÖu øng qu¸n tÝnh t¹i
khíp i hoÆc j g©y ra bëi gia tèc t¹i khíp i hoÆc j. C¸c sè h¹ng cã d¹ng D θ& 2 lµ lùc ly t©m
ijj j

t¸c ®éng lªn khíp i g©y ra bëi vËn tèc t¹i khíp j. Sè h¹ng d¹ng D ijkθ& jθ&k + D ikjθ&kθ& j lµ lùc
Cariolis t¸c ®éng lªn khíp thø i g©y ra do vËn tèc t¹i khíp j vµ k. Sè h¹ng cã d¹ng Di lµ lùc
träng tr−êng t¸c ®éng lªn khíp i.

TS. Ph¹m §¨ng Ph−íc

Robot c«ng nghiÖp 87

7.5. Ph−¬ng tr×nh ®éng lùc häc robot :

XÐt kh©u thø i cña mét robot cã n kh©u. TÝnh lùc tæng qu¸t Fi cña kh©u thø i víi
khèi l−îng vi ph©n cña nã lµ dm. Lùc tæng qu¸t Fi ®ãng vai trß rÊt quan träng khi x©y dùng
s¬ ®å khèi ®Ó thiÕt lËp hµm ®iÒu khiÓn cho robot cã n bËc tù do.

7. 5. 1. VËn tèc cña mét ®iÓm trªn robot :

Mét ®iÓm trªn kh©u thø i ®−îc m« t¶ trong hÖ to¹ ®é c¬ b¶n lµ :
r = Ti. ir (7.10)
Trong ®ã : r lµ to¹ ®é cña ®iÓm xÐt ®èi víi kh©u thø i, ir kh«ng thay ®æi theo thêi
i

gian. Ti lµ ma trËn chuyÓn ®æi tõ kh©u thø i vÒ hÖ to¹ ®é gèc : Ti = A1A2...Ai. Nh− vËy r lµ
mét hµm cña thêi gian t.
Tèc ®é cña vi khèi l−îng dm ®−îc tÝnh bëi c«ng thøc :
dr d i ⎛ i ∂T ⎞
&r = = Ti r = ⎜⎜ ∑ i q& j ⎟⎟ i r (7.11)
dt dt ⎝ j=1∂q j ⎠

Khi tÝnh b×nh ph−¬ng cña vËn tèc nÇy ta cã :

&r. &r = ∑ r 2 ( x& o , y& o , z& o ) = Tr ( &r &r T ) (7.12)
y
Kh©u i
i
r
dm

Ti
r

O0 x
z

H×nh 7.1. Kh¶o s¸t tèc ®é cña vi khèi l−îng dm.

Víi rT lµ chuyÓn vÞ vect¬ vµ Tr lµ viÕt t¾t cña Trace (vÕt cña ma trËn) :
⎡ a 11 a 12 ... a 1n ⎤
⎢a a 22 ... a 2 n ⎥ n
Trace ⎢ ⎥ = ∑a
21
⎢ ... ... ... ... ⎥ i =1 ii
⎢ ⎥
⎣a n1 a n 2 a 11 a nn ⎦
Hay :
⎡ x⎤ ⎡x 2 ⎤
⎢ y ⎥ . [x y z ] = ⎢ ⎥
⎢ y2 ⎥
⎢ ⎥
⎢⎣ z ⎥⎦ ⎢ z 2
⎥
⎣ ⎦
Do vËy
d d
&r 2 = Tr ( &r. &r T ) = Tr ( Ti .i r. Ti T .i r T )
dt dt

TS. Ph¹m §¨ng Ph−íc

Robot c«ng nghiÖp 88

⎡ i ∂T i ∂T
T ⎤
= Tr ⎢ ∑ i q& j i r. ∑ i q& k i r T ⎥
⎢⎣ j=1∂q j k =1 ∂q k ⎥⎦
⎡ i i ∂Ti i i T ∂Ti T
⎤
= Tr ⎢∑∑ rr . q& j q& k ⎥ (7.13)
⎣⎢ j =1 k =1 ∂q j ∂qk ⎦⎥

7. 5. 2. TÝnh ®éng n¨ng cña vi khèi l−îng dm.

Ký hiÖu Ki lµ ®éng n¨ng cña kh©u thø i. dKi lµ ®éng n¨ng cña vi khèi l−îng dm ®Æt
t¹i vÞ trÝ ir trªn kh©u thø i.
1 ⎡ i i ∂Ti i i T ∂Ti ⎤
T
dK i = Tr ⎢ ∑ ∑ rr . q& j q& k ⎥ dm
2 ⎢⎣ j=1k =1 ∂q j ∂q k ⎥⎦
1 ⎡ i i ∂Ti i ∂T T ⎤
= Tr ⎢ ∑ ∑ ( r. dm.i r T ). i q& jq& k ⎥ (7.14)
2 ⎢⎣ j=1k =1 ∂q j ∂q k ⎥⎦
Vµ do ®ã ®éng n¨ng cña kh©u thø i sÏ lµ :
1 ⎡ i i ∂Ti ∂Ti T ⎤
K i = ∫ dK = Tr ⎢∑ ∑ ( ∫ r. r dm ).
i i T
q& j q& k ⎥ (7.15)
Khau i
2 ⎣⎢ j =1 k =1 ∂q j Khau i ∂q k ⎦⎥
Ji = ∫
i
§Æt r.i r T dm gäi lµ ma trËn gi¶ qu¸n tÝnh (Pseudo inertia matrix).
Khau i

ý nghÜa "gi¶ qu¸n tÝnh" ®−îc sö dông v× khi thiÕt lËp ®Çy ®ñ c¸c phÇn tö cña ma trËn Ji ta
cã thÓ liªn hÖ víi c¸c kh¸i niÖm "m«men qu¸n tÝnh ®éc cùc" vµ tr×nh bµy c¸c phÇn tö cña Ji
gièng nh− c¸c phÇn tö cña m«men qu¸n tÝnh ®éc cùc. Ta xÐt mèi quan hÖ nÇy nh− sau :
Theo ®Þnh nghÜa ta cã :
⎡ i x 2 dm x ydm ∫ i x i zdm ∫ i xdm ⎤
∫ ∫
i i

⎢ i i ⎥
J i = ∫ i r.i r T dm = Ji = ⎢ ∫ i i ∫ ∫ ∫
⎢ ⎥
i 2 i i i
x ydm y dm y zdm ydm
⎥ (7.16)
⎢∫ ∫ ∫ ∫
i i i 2 i
Khau i x zdm y zdm z dm zdm ⎥
⎢ i xdm
⎣ ∫ ∫ ydm ∫ zdm ∫ dm ⎥⎦
i i

B©y giê ta nh¾c l¹i m«men qu¸n tÝnh ®éc cùc cña
mét vËt thÓ bÊt kú nh− h×nh vÏ.
y x
ω Theo ®Þnh nghÜa ta cã :
I xx = ∫ ( y 2 + z 2 )dm
z
I yy = ∫ x 2 + z 2 )dm
H×nh 7.2 : M«men qu¸n tÝnh ®éc cùc I zz = ∫ ( x 2 + y 2 )dm
1 1 1
Vµ v× : x2 = − ( y2 + z2 ) + (x2 + z2 ) + (x2 + y2 )
2 2 2
∫ x dm =( −I xx + I yy + I zz ) / 2 ; .v.v…
2
VËy :
Ngoµi ra ta cßn cã :
I xy = ∫ xydm ; I yz = ∫ yzdm ; I xz = ∫ xzdm
mx = ∫ xdm ; my = ∫ ydm ; mz = ∫ zdm

TS. Ph¹m §¨ng Ph−íc

Robot c«ng nghiÖp 89

§èi chiÕu víi ma trËn gi¶ qu¸n tÝnh Ji, ta cã thÓ tr×nh bµy Ji nh− sau :
⎡ − I xx + I yy + I zz ⎤
⎢ 2
I yx I zx mx ⎥
⎢ I xx − I yy + I zz ⎥
⎢ I xy I zy my⎥
ji = ⎢ 2 ⎥ (7.17)
⎢ I xx + I yy − I zz ⎥
⎢ I yz I yz mz ⎥
⎢ 2
⎣ mx my mz m ⎥⎦
Nh− vËy ý nghÜa biÓu tr−ng cña Ji ®· râ.
1 ⎡ i i ∂Ti ∂Ti ⎤
T
VËy ta cã : K i = Tr ⎢∑∑ Ji q& j q& k ⎥ (7.18)
2 ⎣⎢ j =1 k =1 ∂q j ∂q k ⎦⎥
Cuèi cïng, §éng n¨ng cña mét robot cã n kh©u ®−îc tÝnh :
n
K = ∑ Ki (7.19)
i =1

7. 5. 3. TÝnh thÕ n¨ng cña robot :

ThÕ n¨ng cña kh©u i cã khèi l−îng mi, träng t©m ®−îc x¸c ®Þnh bëi vect¬ ri (vect¬
biÓu diÔn träng t©m cña kh©u i trong hÖ to¹ ®é c¬ b¶n) lµ :
Pi = -mi. g. ri = -mi. g. Ti iri (7.20)
Trong ®ã, vect¬ gia tèc träng tr−êng g ®−îc biÓu diÔn d−íi d¹ng mét ma trËn cét :
⎡gx ⎤ ⎡ 0 ⎤
⎢g ⎥ ⎢ 0 ⎥
g = ⎢ y⎥ = ⎢ ⎥
⎢ g z ⎥ ⎢ − 9,8⎥
⎢ ⎥ ⎢ ⎥
⎣0⎦ ⎣ 0 ⎦
ThÕ n¨ng cña toµn c¬ cÊu robot n kh©u ®éng sÏ lµ :
n
P = − ∑ mi gTi i ri (7.21)
i =1

7. 5.4. Hµm Lagrange :

Sau khi x¸c ®Þnh ®éng n¨ng vµ thÕ n¨ng cña toµn c¬ cÊu, ta cã hµm Lagrange cña
robot cã n bËc tù do :

1 n i i ⎛ ∂Ti ∂Ti T ⎞ n
L= ∑∑∑ Trace ⎜ J ⎟q& j q& k + 1 ∑ mi gTi i ri (7.22)
2 i =1 j =1 k =1 ⎜ ∂q i ∂q ⎟ 2 i =1
⎝ j k ⎠

Chóng ta chó ý r»ng, trong hµm Lagrange vÉn ch−a ®Ò cËp ®Õn ¶nh h−ëng cña
nguån truyÒn ®éng (gåm c¸c phÇn tÜnh (stator) vµ phÇn ®éng (Rotor) cña ®éng c¬ ®iÖn).

7. 5. 5. Ph−¬ng tr×nh ®éng lùc häc robot :

Ta ®· biÕt lùc tæng qu¸t ®Æt lªn kh©u thø i cña robot cã n kh©u (Ph−¬ng tr×nh
Lagrange - Euler) :
d ∂L ∂L
Fi = − (7.23)
dt ∂q& i ∂q i
Sau khi thiÕt lËp hµm Lagrange, víi p = 1... n, ta tÝnh ®−îc :

TS. Ph¹m §¨ng Ph−íc

Robot c«ng nghiÖp 90

(p lµ chØ sè lÇn l−ît lÊy theo j vµ k)

∂L 1 n i ⎛ ∂Ti ∂Ti T ⎞⎟ 1 n i ⎛ ∂Ti ∂Ti T ⎞
= ∑∑ Tr ⎜ Ji q& k + ∑∑ Tr⎜ Ji ⎟q& j (7.24)
∂q& p 2 i =1 k =1 ⎜⎝ ∂q p ∂qk ⎟⎠ 2 i =1 j =1 ⎜⎝ ∂q j ∂q p ⎟⎠
Thay ®æi chØ sè gi¶ j thµnh k trong sè h¹ng thø hai ,vµ ®Ó ý r»ng :
T
⎛ ∂Ti ∂Ti T ⎞⎟ ⎛ ∂Ti ∂Ti T ⎞⎟ ⎛ ∂Ti ∂Ti T ⎞⎟
Tr ⎜ J = Tr ⎜ J = Tr ⎜ J (7.25)
⎜ ∂q i ∂q ⎟ ⎜ ∂q i ∂q ⎟ ⎜ ∂q i ∂q ⎟
⎝ j p ⎠ ⎝ j p ⎠ ⎝ p j ⎠

∂L n i ⎛ ∂Ti ∂Ti T ⎞⎟
ta cã : = ∑∑ Tr ⎜ Ji q& k (7.26)
∂q& p i =1 k =1 ⎜⎝ ∂qk ∂q p ⎟⎠
Còng ®Ó ý r»ng : trong Ti(q1, q2, . . . , qi), víi qi lµ c¸c biÕn khíp cña i khíp ®Çu tiªn. Do
∂Ti
vËy, nÕu i < p th× = 0.
∂q p
∂L n i ⎛ ∂T ∂T T ⎞
Cuèi cïng ta cã : = ∑∑ Tr ⎜ i J i i ⎟q& k (7.27)
∂q& p i = p k =1 ⎜⎝ ∂qk ∂q p ⎟
⎠
LÊy vi ph©n theo thêi gian t cña ph−¬ng tr×nh trªn :

d ∂L d n i ⎛ ∂T ∂T T ⎞
= ∑∑ Tr⎜ i J i i ⎟ q& k
dt ∂q& p dt i = p k =1 ⎜⎝ ∂qk ∂q p ⎟
⎠

⎛ ∂T ∂T T ⎞ ⎡ 2 T ⎤
⎟ q&&k + ∑∑∑ Tr ⎢ ∂ Ti J i ∂Ti ⎥ q& k q& m +
n i n i i
= ∑∑ Tr ⎜ i J i i
⎜ ∂q ∂q p ⎟
i = p k =1 ⎝ k ⎠ i = p k =1 m =1 ⎣⎢ ∂qk ∂qm ∂q p ⎦⎥

n i i ⎡ ∂ 2Ti ∂T T ⎤
+ ∑∑∑ Tr ⎢ J i i ⎥ q& k q& m (7.28)
i = p k =1 m =1 ⎢⎣ ∂q p ∂qm ∂qk ⎥⎦
(BiÕn ®æi theo chó ý (7.25))
Sè h¹ng cuèi cña ph−¬ng tr×nh Lagrange Euler lµ :

∂L 1 n i i ⎛ ∂ 2Ti ∂T T ⎞
= ∑∑∑ Tr⎜ Ji i ⎟ q& j q& k +
∂q p 2 i = p j =1 k =1 ⎜⎝ ∂q j ∂q p ∂qk ⎟
⎠
⎛ ∂ 2Ti ∂Ti T ⎞
1 n i i ⎟ q& j q& k + ∑ mi g ∂Ti i ri
n
+ ∑∑∑ Tr ⎜
2 i =1 j =1 k =1 ⎜⎝ ∂qk ∂q p
J i
∂q j ⎟ ∂q p
(7.29)
⎠ i= p

d ∂L ∂L
Cuèi cïng ta cã lùc tæng qu¸t cña kh©u p : Fp = −
dt ∂q& p ∂q p

Thay thÕ c¸c chØ sè p vµ i thµnh i vµ j, ta sÏ cã :

n j ⎛ ∂T j ⎞
∂T jT n j j ⎡ ∂ 2T j ∂T jT ⎤ n ∂T j j
Fi = ∑∑ Tr ⎜⎜ Jj ⎟ q&&k + ∑∑∑ Tr ⎢
⎟ J ⎥ &
q &
q m − ∑ m g rj
⎝ ∂qk ∂qi ∂ ∂ ∂ ∂
j k j
j =i k =1 ⎠ j = i k = 1 m = 1 ⎣ q k q m q i ⎦ j = i q i

(7.30)
Víi mét robot cã n bËc tù do th× :

TS. Ph¹m §¨ng Ph−íc

Robot c«ng nghiÖp 91

q = [q1, q2, . . . ,qn]T

q& = [q& 1 , q& 2 , ... , q& n ] T
vµ F = F[F1, F2, . . . , Fn]T

§Ó cho gän, ta biÓu diÔn :

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

Trong ®ã :
J thÓ hiÖn t¸c dông cña qu¸n tÝnh, lµ mét ma trËn ®èi xøng (n x n);
C thÓ hiÖn t¸c dông cña lùc ly t©m vµ Cariolis, lµ mét vect¬ (n x 1);
G thÓ hiÖn t¸c dông cña lùc träng tr−êng, còng lµ mét vect¬ (n x 1).
§©y lµ ph−¬ng tr×nh ®éng lùc häc cña robot.

NÕu thªm vµo ph−¬ng tr×nh trªn c¸c t¸c dông kh¸c nh− : FEX ®Æc tr−ng cho c¸c
ngo¹i lùc t¸c dông lªn trôc, V ®Æc tr−ng cho hiÖu øng ma s¸t, ta cã :

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

TS. Ph¹m §¨ng Ph−íc