Robot c«ng nghiÖp
84
ch−¬ng VII §éng lùc häc Robot (Dynamic of Robot)
1/ X¸c ®Þnh momen vµ lùc ®éng xuÊt hiÖn trong qu¸ tr×nh chuyÓn ®éng. Khi ®ã qui
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¬,
L = K - P
(7.1)
Hµm Lagrange cña mét hÖ thèng n¨ng l−îng ®−îc ®Þnh nghÜa : 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
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 : luËt biÕn ®æi cña biÕn khíp qi(t) coi nh− ®· biÕt. 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. nµo ®Ó bµi to¸n ®−îc ®¬n gi¶n. §èi víi mét robot cã n kh©u, ta cã :
P
K
vµ
Pi
K i
n = ∑ =1 i
n = ∑ 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
) vµ P
)
Ki = K(qi,
i = P(qi,
biÕt mçi ®¹i l−îng Ki vµ Pi lµ mét hµm sè phô thuéc nhiÒu biÕn sè: 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
&q i
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 dt
L ∂ q ∂
i
i
TS. Ph¹m §¨ng Ph−íc
− (7.2) Fi = L ∂ q ∂ &
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.
y g = 9,81m/s2
x O0 x1 x2 Qua vÝ dô nÇy, chØ víi mét mèi liªn kÕt hai kh©u, c¸c vÊn ®Ò ®Æt ra ®Òu ®· cã mÆt 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 h¬n. §èi víi kh©u 1 :
1
2 &θ 1
2 m d 1 1
2 m v 1 1
θ1
= = (7.3) K z
(7.4) m1 y1
θ2 y2 m2 x2 = d1sinθ1 + d2sin(θ1 + θ2) y2 = -d1cosθ1 - d2cos(θ1 + θ2)
2 2
2 x +& 2
v = VÒ mÆt vËn tèc : 1 1 2 2 P1 = -m1gd1cosθ1 §èi víi kh©u 2 : VÒ to¹ ®é : ChiÒu cao thÕ n¨ng : h = d1cosθ1 + d2cos(θ1 + θ2) 2 y & 2
2
1
1
1
2
1
2
2
x d cos( d cos( = + θ + θ + = )( & θ Víi ) & θ θ 1 & ) θ 2 x &
2
1
1
1
2
1
2
2
y d sin( d sin( = + θ + θ + = )( & θ ) & θ θ 1 & ) θ 2 y &
2 1
2 1
2 2
2 1
2 & ) θ 2
2 1
2
& & ) θ θ 1 2
2 2
d 2 2 cos( v + + + + θ + = θ & ( & θ )( & θ θ θ & & 1 2 d d 1 2
]
d dt d dt [ d §éng n¨ng vµ thÕ n¨ng sÏ lµ :
2
2 2
2 1
2 1
2 2
2 1
2 & ) θ 2
2 1
2
& & ) θ θ 1 2
2 2 K d cos( = = + + + + + θ θ & ( & θ )( & θ (7.5) m v 2 θ θ & & 1 2 d d 1 2
1 2 cos(
[ m d 2 )
d cos( ) 1 2 = − θ θ + +
] (7.6)
2
1
1
2θ
1
[ m g d 2
]
P 2
¸p dông hµm Lagrange cho vÝ dô trªn, ta cã :
7.4. Hµm Lagrange vµ lùc tæng qu¸t : L = (K1 + K2) - (P1 + P2)
1
2
2 1
2 1
2 2
2 1
2 & ) θ 2
& & ) θ θ 1 2
2 1
2
1
1
1
2 )
L ( m m d ) 2 cos = + + + + + + + θ & ( & θ m d 2 θ θ & & 1 2 m d d 2 1 2 ( & θ θ 2 1 2 cos( cos ( ) + θ θ θ 1 2 + (7.7) + m gd 2
m m gd + 2 1 Khi tÝnh lùc tæng qu¸t, c¸c biÕn cña hÖ : q1 = θ1 vµ q2 = θ2.
+
+
+
+
+
=
=
θ
θ &
1
2
2 1
1
2 2
1
& ) θ 2
θ θ & 2 1
θ 2 &
2
1
1
TS. Ph¹m §¨ng Ph−íc
m m d ) 2 cos cos ( ( & θ m d 2 m d d 2 1 2 m d d 2 1 2 §èi víi kh©u 1 : L L ∂ ∂ q ∂ ∂θ & &
Robot c«ng nghiÖp
86
=
+
+
+
−
+
θ
θ &&
1
2
2 1
1
2 2
1
&& ) θ 2
θ θ θ & & 2 2 1
θ − && 2 1
1
m m d ( ) 2 sin 2 cos (&& θ m d 2 m d d 2 1 2 m d d 2 1 2 d dt L ∂ ∂θ &
+
2 & θ θ 2 2
&& θ θ 2 2
sin cos − m d d 2 1 2 m d d 2 1 2
θ
θ
= −
+
−
+
=
2
1
2
1
1
θ ) 2
1
1
1
m m gd ( ) sin sin( m gd 2 L ∂ q ∂ L ∂ ∂θ
VËy :
=
−
=
+
+
+
+
1
2
2 1
2 2
1
1
1
[( m m d ) 2 cos F 1 m d 2 m d d 2 1 2 ]&& θ θ 2 d dt L ∂ ∂θ L ∂ ∂θ &
−
+
& & θ θ θ 2 1 2
& θ θ 2
2 2
cos sin (7.8) m d d 2 1 2
θ
θ
1
2
2
1
1
2
2 [ m d + + 2 2 m m gd ( + + 1 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).
m d d 2 1 2 sin ) sin ) m d d 2 2 1 2 sin( θ + ]&& θ θ − 2 2 m gd + 2
T−¬ng tù, ®Ó tÝnh lùc tæng qu¸t cña kh©u thø hai , ta cã :
θ
=
+
+
θ &
θ &
2 2
1
2 2
2
θ1 &
2
2
cos m d 2 m d 2 m d d 2 1 2 L ∂ ∂θ &
2 2
1
2 2
2
2
cos sin = + + − θ && θ && m d 2 m d 2 m d d 2 1 2 θ θ && 2 1 m d d 2 1 2 θ θ & & 2 1 θ 2
ddm
sin(
ddm
sin(
gdm
sin(
)
−=
) && θθθ
−−
) & θθ
−
+ θθ
2
1
2
1
2
2
2
1
2
2
2 1
2
2
2
1
2
vµ d L ∂ dt ∂θ & L ∂ ∂θ
F
[
cos
dm
=
−
=
ddmdm +
] && θθ
+
&& θ
2
2
1
2
2
2 2
1
2
2
2 2
2
d dt
L ∂ & θ∂
L ∂ ∂θ
VËy :
2
2
sin(
gdm
sin(
)
−
) & θθ
+ θθ
+
1
2
2
2
2
2
1
2 1
ddm 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
(7.9)
c¸c biÓu thøc F1, F2 nh− sau :
D D D D D D + + + + + +
1 D
&& θ 12 2 && θ 22 2
2 & θ 111 1 2 & θ 211 1
2 & θ 122 2 2 & θ 222 2
& & θ θ 121 1 2 & & D θ θ 221 1 2
2
D D D = + + + +
& & θ θ 112 1 2 & & D D θ θ 212 1 2 HiÖu øng HiÖu øng ly t©m
+ + HiÖu øng t−¬ng hæ träng tr−êng
&& F D θ = 11 1 1 && F θ 12 1 2 HiÖu øng qu¸n tÝnh
ijD
2
thÓ hiÖn hiÖu øng qu¸n tÝnh t¹i 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
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
ijk
ijjD jθ& lµ lùc ly t©m && && θθ θθ lµ lùc 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 ikj j k k j 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 D i lµ lùc träng tr−êng t¸c ®éng lªn khíp i.
TS. Ph¹m §¨ng Ph−íc
D D +
Robot c«ng nghiÖp
87
(7.10) r = Ti. ir
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µ : Trong ®ã : ir lµ to¹ ®é cña ®iÓm xÐt ®èi víi kh©u thø i, ir kh«ng thay ®æi theo thê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.
=
=
=
i r
i T r i
j
j
⎞ ⎟⎟ ⎠
⎛ ⎜⎜ ⎝
∑ 2
(7.11) &r q & Tèc ®é cña vi khèi l−îng dm ®−îc tÝnh bëi c«ng thøc : d dt dr dt T ∂ i i ∑ q ∂1 j =
=
T (& & ) Tr r r
Khi tÝnh b×nh ph−¬ng cña vËn tèc nÇy ta cã : = (7.12) r x y z o r r &.&
Kh©u i
i r
dm
Ti
r
x
O0
z
H×nh 7.1. Kh¶o s¸t tèc ®é cña vi khèi l−îng dm.
11
12
1 n
=
( & , & , & ) o o y
ii
n a ∑ i 1 =
Trace
a a n 2 ... a
a a 22 ... a
... ... ... a
1 n
n
2
11
nn
⎤ ⎥ ⎥ ⎥ ⎥ ⎦
⎡ ⎢ ⎢ ⎢ ⎢ ⎣
Hay :
2
x
x
2
y
]
2
y z
z
⎡ ⎢ ⎢ ⎢ ⎣
⎡ ⎤ ⎢ ⎥ [ = zyx . ⎢ ⎥ ⎢ ⎥ ⎦ ⎣
⎤ ⎥ ⎥ ⎥ ⎦
Do vËy
Tr r r
Tr
(
.
T i T r .
2 =
=
)
i T r . i
T i
r &
T (&.& )
d dt
d dt
TS. Ph¹m §¨ng Ph−íc
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 a 21 ... a
Robot c«ng nghiÖp
88
T
.
i T r
=
k
i q r & j
q &
i ∑ 1 k =
T i ∂ i q ∂
T ∂ i q ∂
j
k
T
i
i
i
Ti rr
(7.13)
k
k
j
1 =
1 =
k
j
⎡ ∑Tr ⎢ ⎢ 1 j = ⎣ ⎡ = ∑∑ ⎢ ⎢ ⎣
⎤ ⎥ ⎥ ⎦ ⎤ ⎥ ⎥ ⎦
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.
T
i
Tr . qq && j T ∂ i q ∂ T ∂ i q ∂
i T r r
dm
i
i i ∑∑ k 1 1 j ==
j
k
T
i
i T
dK Tr . = q q & & j k 1 2 T ∂ i q ∂ T ∂ i q ∂ ⎡ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎦
=
(7.14)
i i ∑∑ k 1 1 j ==
j
k
T
i
i
i
Tr ( r dm r . . ). q q & & j k 1 2 T ∂ i q ∂ T ∂ i q ∂ ⎡ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎦
(7.15)
i
k
Tr K ( ). = = qq && j
∑
∑
∫
∫
j
k
1 =
1 =
k
j
Ti dmrr . Khau i
Khau
Vµ do ®ã ®éng n¨ng cña kh©u thø i sÏ lµ : ⎡ ⎢ ⎢ ⎣
i
Ti rr.
1 2 T ∂ i q ∂ T ∂ i q ∂ dK i ⎤ ⎥ ⎥ ⎦
gäi lµ ma trËn gi¶ qu¸n tÝnh (Pseudo inertia matrix).
§Æt
i
J dm
∫=
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
i
i
i
i
i
2 dmx
ydmx
zdmx
xdm
i
i
i
i
i
i
ydmx
2 dmy
zdmy
ydm
i
Ti rr.
= J
(7.16)
i =
i
i
i
i
i
i
i
J dm
∫=
zdmx
zdmy
2 dmz
zdm
Khau i
i
i
i
xdm
ydm
zdm
dm
∫ ∫ ∫ ∫
∫ ∫ ∫ ∫
∫ ∫ ∫ ∫
∫ ∫ ∫ ∫
⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦
⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣
2
y
y x ω
xx
2
x
2 ) dmz
yy
2
2
H×nh 7.2 : M«men qu¸n tÝnh ®éc cùc
x
I + = z I + =
zz
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Ï. Theo ®Þnh nghÜa ta cã : 2 ∫ ( ) dmz ∫ ∫
2
2
2
2
2
2
2
z
x
z
y
y
x
) dmy ( I + =
Vµ v× :
2 dmx
x ) ( ( ) ) ( −= + + + + +
VËy :
yy
xx
zz
∫
Ngoµi ra ta cßn cã :
;
;
xyI
yzI
xzI
I 2/) 1 2 I( −= 1 2 I + + 1 2 ; .v.v…
;
;
∫= xydm ∫= xdm
∫= yzdm ∫= ydm
∫= xzdm ∫= zdm
TS. Ph¹m §¨ng Ph−íc
mx my mz
Robot c«ng nghiÖp
§èi chiÕu víi ma trËn gi¶ qu¸n tÝnh Ji, ta cã thÓ tr×nh bµy Ji nh− sau :
89
xx
yy
zz
yx
zx
xx
zz
zy
xy
(7.17)
yy 2
xx
zz
yz
yz
yy 2 mz
I I I − + + I I mx 2 I I I − + I I my = j i I I I + − I I mz
m ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣
T
i
i
K
Tr
J
mx my Nh− vËy ý nghÜa biÓu tr−ng cña Ji ®· râ.
VËy ta cã :
(7.18)
i
i
k
qq && j
∑∑
T ∂ i q ∂
T ∂ i q ∂
j
k
j
k 1 1 = = Cuèi cïng, §éng n¨ng cña mét robot cã n kh©u ®−îc tÝnh :
n
= 1 2 ⎡ ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎦
(7.19)
iK
K =
∑
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
x
y
g = = −
(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 : ⎡ ⎢ ⎢ ⎢ ⎢ ⎣
ThÕ n¨ng cña toµn c¬ cÊu robot n kh©u ®éng sÏ lµ :
n
i
g g g z 0 0 ⎤ ⎥ 0 ⎥ 8,9 ⎥ ⎥ 0 ⎦ ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎣
(7.21)
P −= gTm i i r i
∑
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 :
T
n
i
i
n
(7.22)
i
k
i rgTm i
i
i
L Trace J + =
∑∑∑
∑
i
j
k
i
1 =
1 =
1 =
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
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
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). Lagrange - Euler) :
1 2 1 2 T ∂ i q ∂ T ∂ i q ∂ ⎞ ⎟ qq && ⎟ j ⎠ ⎛ ⎜ ⎜ ⎝
(7.23)
Fi =
i
i
TS. Ph¹m §¨ng Ph−íc
− L ∂ q ∂ L ∂ q ∂ & d dt Sau khi thiÕt lËp hµm Lagrange, víi p = 1... n, ta tÝnh ®−îc :
Robot c«ng nghiÖp
(p lµ chØ sè lÇn l−ît lÊy theo j vµ k)
T
T
n
i
n
i
90
(7.24)
i
j
i
k
Tr J Tr J + =
∑∑
∑∑
i
j
i
k
1 =
1 =
1 =
1 =
j
p
p
k
p Thay ®æi chØ sè gi¶ j thµnh k trong sè h¹ng thø hai ,vµ ®Ó ý r»ng :
T
T
T
T
Tr
J
Tr
J
Tr
J
=
=
(7.25)
i
i
i
T ∂ i q ∂
T ∂ i q ∂
T ∂ i q ∂
T ∂ i q ∂
T ∂ i q ∂
T ∂ i q ∂
j
p
j
p
p
j
⎛ ⎜ ⎜ ⎝
⎞ ⎟ ⎟ ⎠
⎛ ⎜ ⎜ ⎝
⎞ ⎟ ⎟ ⎠
⎛ ⎜ ⎜ ⎝
⎞ ⎟ ⎟ ⎠
T
n
i
1 2 1 2 T ∂ i q ∂ T ∂ i q ∂ T ∂ i q ∂ T ∂ i q ∂ L ∂ q ∂ & ⎞ ⎟ q & ⎟ ⎠ ⎞ ⎟ q & ⎟ ⎠ ⎛ ⎜ ⎜ ⎝ ⎛ ⎜ ⎜ ⎝
(7.26)
ta cã :
i
k
Tr J =
∑∑
i
k
1 =
1 =
p
k
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
0=
vËy, nÕu i < p th×
.
T ∂ i q ∂
p
T
n
i
T ∂ i q ∂ T ∂ i q ∂ L ∂ q ∂ & ⎞ ⎟ q & ⎟ ⎠ ⎛ ⎜ ⎜ ⎝
Cuèi cïng ta cã :
(7.27)
i
k
Tr J =
∑∑
k
pi =
1 =
p
k
T ∂ i q ∂ T ∂ i q ∂ ⎛ ⎜ ⎜ ⎝
T
n
i
⎞ L ∂ ⎟ q & ⎟ q ∂ & ⎠ p LÊy vi ph©n theo thêi gian t cña ph−¬ng tr×nh trªn :
i
k
Tr J =
∑∑
k
pi =
1 =
p
k
p
T
T
2
n
i
i
n
i
d dt d dt T ∂ i q ∂ T ∂ i q ∂ L ∂ q ∂ & ⎞ ⎟ q & ⎟ ⎠ ⎛ ⎜ ⎜ ⎝
i
i
Tr J Tr J + + =
∑∑
∑∑∑
k
m
k
pi =
1 =
1 =
1 =
m
p
k
p
pi =
T
2
n
i
i
T ∂ i q ∂ T ∂ i q ∂ T ∂ i q ∂ T ∂ i qq ∂∂ k ⎡ ⎢ ⎢ ⎣ ⎤ mqq ⎥ && k ⎥ ⎦ ⎞ ⎟ q && ⎟ k ⎠ ⎛ ⎜ ⎜ ⎝
(7.28)
i
Tr J +
∑∑∑
k
m
pi =
1 =
1 =
m
k
Sè h¹ng cuèi cña ph−¬ng tr×nh Lagrange Euler lµ :
(BiÕn ®æi theo chó ý (7.25))
T
2
n
i
i
T ∂ i q ∂ T ∂ i qq ∂∂ p ⎡ ⎢ ⎢ ⎣ ⎤ mqq ⎥ && k ⎥ ⎦
i
k
Tr J + =
∑∑∑
j
k
pi =
1 =
1 =
p
p
k
T
2
n
i
i
i
k
i
1 2 L ∂ q ∂ T ∂ i q ∂ T ∂ i qq ∂∂ j ⎞ ⎟ qq && j ⎟ ⎠ ⎛ ⎜ ⎜ ⎝ n Tr J + + (7.29) r i gm i
∑∑∑
∑
i
j
k
1 =
1 =
1 =
pi =
p
p
j
F
=
−
Cuèi cïng ta cã lùc tæng qu¸t cña kh©u p :
p
d dt
L ∂ q ∂
p
p
L ∂ q ∂ &
Thay thÕ c¸c chØ sè p vµ i thµnh i vµ j, ta sÏ cã :
2
j
j
j
n
n
n
1 2 T ∂ i q ∂ T ∂ i q ∂ T ∂ i qq ∂∂ k ⎞ ⎟ qq && ⎟ j ⎠ ⎛ ⎜ ⎜ ⎝
j
j
j
j
j
j
T ∂ T ∂ T ∂ T ∂ T ∂ Tr J Tr J r + = − gm j F i
∑∑
∑∑∑
∑
j
i
k
m
j
i
k
=
1 =
1 =
T j q ∂ i
j qq ∂∂ k
m
T j q ∂ i
k
(7.30)
i =
j =
1 =
Víi mét robot cã n bËc tù do th× :
TS. Ph¹m §¨ng Ph−íc
q ∂ q ∂ i ⎡ ⎢ ⎣ ⎤ mqq ⎥ && k ⎦ ⎛ ⎜ ⎜ ⎝ ⎞ ⎟ q && ⎟ k ⎠
Robot c«ng nghiÖp
T
1
vµ
q = [q1, q2, . . . ,qn]T [ ]n q = q , ... ,q ,q & & & & 2 F = F[F1, F2, . . . , Fn]T
(7.31)
)(
qGqqqCqqJF )(
=
+
+
),( &&
&&
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
§Ó cho gän, ta biÓu diÔn : Trong ®ã : 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ã :
(7.32)
)(
)(
+
=
+
+
+
EXF
qVqGqqqCqqJF ),( )( &
&&
&&
TS. Ph¹m §¨ng Ph−íc
91
