Robot công nghiệp : Độ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
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 2&2
z K 1 = m1 v 1 = m1 d 1 θ 1
2
(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 : v2 = x2 + y2
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
[ 2&2 &2 & & &2 &2 & &
v 2 = d 1 θ1 + d 2 (θ 1 + 2θ1θ 2 + θ 2 ) + 2d 1d 2 cos(θ 2 )(θ 1 + θ 1θ 2 )
2 2 ]
§éng n¨ng vµ thÕ n¨ng sÏ lµ :
1
2
1
2
[
2&2
2
&2 & & &2 &2 & &
K 2 = m 2 v 2 = m 2 d 1 θ1 + d 2 (θ 1 + 2θ1θ 2 + θ 2 ) + 2d 1d 2 cos(θ 2 )(θ 1 + θ 1θ 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 2&2 1 &2 & & & &2 & &
L = ( m1 + m 2 )d 1 θ1 + m 2 d 2 (θ1 + 2θ1θ 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 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 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
∂q 1 ∂θ1
& & 2
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 86
d ∂L
= ( 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 −
2 && 2
θ θ & & &&
&
dt ∂θ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 1 + m 2 d 2 + 2 m 2 d 1d 2 cos θ 2 ]θ 1 +
2
&
dt ∂θ 1 ∂θ 1
2
&& & & &2
+[ 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 +
2
(7.8)
+ ( 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 2 θ1 + m 2 d 2 θ 2 + m 2 d 1d 2 cos θ 2 θ1
∂θ & 2 2
2
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θ
&& && & &
&
dt ∂θ 2
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 && 2& &
F2 = & − ∂θ = [m2 d 2 + m2 d1d 2 cosθ 2 ]θ1 + m2 d 2 θ 2
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 θ 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 &
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
⎜ j=1∂q & ⎟ (7.11)
dt dt ⎝ j ⎠
Khi tÝnh b×nh ph−¬ng cña vËn tèc nÇy ta cã :
&. & = ∑ r 2 ( x o , y o , z o ) = Tr ( & & T )
rr & & & rr (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⎥
⎣ ⎦ ⎢ z2 ⎥
⎣ ⎦
Do vËy
d d
& 2 = Tr ( &. & T ) = Tr ( Ti .i r. Ti T .i r T )
r rr
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 qk ⎥ (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 qk ⎥ (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 ∫ x zdm ∫ xdm ⎤
i i i i i
⎢ i i ⎥
⎢ ∫ x ydm ∫ y dm ∫ y zdm ∫ ydm ⎥
i 2 i i i
J i = ∫ i r.i r T dm = Ji = ⎢ i i ⎥ (7.16)
Khau i ⎢∫ x zdm ∫ i y i zdm ∫ i z 2 dm ∫ i 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 qk ⎥ (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 ⎛ ∂T ∂T T ⎞ n
L= ∑∑∑ Trace⎜ i J i i ⎟q j qk + 1 ∑ mi gTi i ri
& & (7.22)
2 i =1 j =1 k =1 ⎜ ∂q ∂q k ⎟ 2 i =1
⎝ j ⎠
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)
⎛ ∂Ti ∂Ti T ⎞ ⎛ T ⎞
∂L 1 n i ⎟qk + 1 ∑∑ Tr⎜ ∂Ti J i ∂Ti ⎟q j
n i
= ∑∑ Tr ⎜ Ji & & (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 ⎞ ⎛ T ⎞ ⎛ T ⎞
Tr ⎜ J ⎟ = Tr ⎜ ∂Ti J i ∂Ti ⎟ = Tr ⎜ ∂Ti J i ∂Ti ⎟ (7.25)
⎜ ∂q i ∂q ⎟ ⎜ ∂q ∂q p ⎟ ⎜ ∂q ∂q j ⎟
⎝ j p ⎠ ⎝ j ⎠ ⎝ p ⎠
∂L n i ⎛ ∂Ti ∂Ti T ⎞
ta cã : = ∑∑ Tr ⎜ Ji ⎟qk
& (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 ⎟qk
& (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 ⎟ qk
&
dt ∂q p dt i = p k =1 ⎜ ∂qk
& ⎝ ∂q p ⎟
⎠
⎛ ∂T ∂T T ⎞ ⎡ 2 T ⎤
⎟ qk + ∑∑∑ Tr ⎢ ∂ Ti J i ∂Ti ⎥ qk qm +
n i n i i
= ∑∑ Tr ⎜ i J i i && & &
⎜ ∂q ∂q p ⎟ ⎢ ∂qk ∂qm ∂q p ⎥
i = p k =1 ⎝ k ⎠ i = p k =1 m =1 ⎣ ⎦
n i i ⎡ ∂ 2Ti ∂T T ⎤
+ ∑∑∑ Tr ⎢ & &
J i i ⎥ qk qm (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 qk +
& &
∂q p 2 i = p j =1 k =1 ⎜ ∂q j ∂q p
⎝ ∂qk ⎟
⎠
⎛ ∂ 2Ti ∂T T ⎞
1 n i i ⎟ q j qk + ∑ mi g ∂Ti i ri
n
+ ∑∑∑ Tr ⎜
2 i =1 j =1 k =1 ⎜ ∂qk ∂q p
Ji 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 ⎜ ⎜ ∂q Jj ⎟ qk + ∑∑∑ Tr ⎢
⎟
&& Jj ⎥ qk qm − ∑ m j g
& & rj
j =i k =1 ⎝ k ∂qi
⎠ j =i k =1 m =1 ⎣ ∂qk ∂qm ∂qi ⎦ j =i ∂qi
(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
