Tải bản đầy đủ
CHƯƠNG II: BÀI TOÁN ĐỘNG LỰC HỌC

CHƯƠNG II: BÀI TOÁN ĐỘNG LỰC HỌC

Tải bản đầy đủ

 S 2θ
12

1
2

Vậy m2 d 3  − Sθ12 Cθ12
12
0



• i=4:

(
(

C θ 4 − θ12
0
R =  S θ − θ
4
4 12

0


0
12 12 
C 2θ
0

12
0
1


− Sθ

) S (θ 4 − θ12 )
) − C (θ 4 − θ12 )
0

(

(
(

0
C θ −θ
T  4 12

0


0  ⇒  R  = S θ −θ
4 12
 4

− 1
0

)


S2 θ −θ
4
12

1
2

Vậy I 4 = m 4 d 4  − S θ 4 − θ12 C θ 4 − θ12
12
0



(

)(

)

(

)(

) S (θ 4 − θ12 )
) − C (θ 4 − θ12 )

S θ −θ C θ −θ
4
12
4
12
2
C θ −θ
4
12
0

(

)

)

0

0
0 
− 1

0

0

1


2. Tính vận tốc khối tâm của từng khâu:
j
j  Z j −1 × Pci
J =
Vi  Z
j −1





j   Z
Jωi =  j −1
Z 0 = 0




khớp quay
khớp tịnh tiến
khớp quay
khớp tịnh tiến

• Khâu 1:

1
 1

a Cθ   a Cθ 

1
1
1
1
 1 0 0 2
2
1
 1



P 0 = R 0r 0 + P1 = R 0r 0 = 0 1 0  a Sθ  =  a Sθ 
c1
01
c1
01
2 1 1
2 1 1
 0 0 1  0   0 
 




 


10


 1


 − 2 a1Sθ1 
 1  1

 J   =  a Cθ 
 V1  2 1 1 

0


⇒




0

 
1

J  =0

ω1

1


• Khâu 2:
P 0 = R 0 r 0 + P1
c2
01
c2
1
 1

a Cθ   a Cθ 

1
1
2
12
2
Cθ1 − Sθ1 0  2
 1

1


1
0
1
P = R r =  Sθ

0   a Sθ  =  a Sθ 
c2
1 2
1
1
 2 1 1   2 2 12 
 0
0
1  0  
0


 

1


a1Cθ1 + 2 a2 Cθ12 


1
⇒ P 0 =  a Sθ + a Sθ 
c2  1
2 2 12 
0




0 Cθ1 − Sθ1 0 0 0
0
Z = R 0 =  Sθ

0 0 = 0
1
1
1
1
0
1 1 1
1  0

1



− a1Sθ1 − 2 a2 Sθ12 



1
 J (1) =  a Cθ1 + a Cθ 
2 2 12 
 V2  1

0


⇒





0 
 
1  

 
J = 0 

ω1

1


11




J (2)
 V2









 1

− 2 a2 Sθ12 
 1

=  a Cθ 
 2 2 12 
0




0
(2)  
J
= 0
ω2  
1

• Khâu 3:

0 
 (1)
 
 J V 3 = Z 0 = 0

1

 J (1) = 0
 ω 3

0
 (3)
J
= Z 0 =  0 
 V 3

− 1

 J (3) = 0
 ω 3

• Khâu 4:

Cθ12
3
0
3
4
P = R r + P =  Sθ
C4
3 4
C4
12
 0



12
− Cθ
12
0

0  0   0 
  

0   0  =  0 
− 1  d   − d 
 4  4


0 
  4   
 J V 4 = 0 

0
⇒
  4   0 
J   =  0 
 ω4  

− 1

12

Cθ
12
( 2)
0
2
3
P
= R r + P =  Sθ
C4
23
C4
12
 0


12
− Cθ
12
0

0  0   0  
0
  
 

0  0  +  0  = 
0
− 1 d  − d  − d + d
 3  4   3 4

(

)








0
 (3)  
 J V 4 = 0
0

⇒
0

(
3
)
J
=  0 
ω
4

− 1


1

a Cθ 

0
Cθ − Sθ 0  2 2 2  
1
1

1
P1 = R 0 r1 + P 2 =  Sθ

0  a Sθ  + 
0
C4
1 2
C4
1
1
2 2 2 
 0
0
1 
0
  − d 3 + d 4



 1


− 2 a2 Sθ12 

 1

 J ( 2) =  a Cθ 
 V 4  2 2 12 
0



⇒




 0
 ( 2)  
 J ω 4 =  0

1


(

)

1
a Cθ
  2 2 12
 1
 =  a 2 Sθ12
 2
  − d 3 + d 4


(

1
 1
 

a Cθ   a C θ + a C θ 

 a1Cθ1   2 2 12   1 1 2 2 12 
1
1
 =  a Sθ + a Sθ 
P 0 = R 0 r 0 + P1 =  a Sθ  +  a Sθ
C4
01
C4
1 1
 2 2 12   1 1 2 2 12 
 0   − (d + d )  − (d + d ) 
3
4
3
4

 


13

)










1



− a1Cθ1 − 2 a 2 Cθ12 



1
 J (1) =  a Sθ + a Sθ

 V 4  1 1 2 2 12 
0



⇒




 0
 (1)  
 J ω 4 =  0

1


3. Vận tốc khối tâm của tay máy:
 x 
 VC10 
 1
 •
 y 
 •   − a Sθ
0
0
0
 θ 
V 0 
θ1   2 1 1
 1 
 C1 
 •   1 a Cθ
0 0 0  • 
 z 


1
1
0
 2
 θ 
θ
 VC1 


2
0
0
0
0

  •2 
 x  = J C1  •  =

 
ω 0 
0
0 0 0  d3 
d  
 C1 
 •3  
 • 
 y 
0
0
0
0



 θ 
θ
ωC10 
 
 4  
1
0 0 0  4 
 z 

ω 0 
 C1 
 x 
 VC 02 
1
1

 y 
 •   − a Sθ − a Sθ
− a Sθ
V 0 
2 1 12
 θ   1 1 2 2 12
1
 C2 
1
1
   a Cθ + a Cθ
a Cθ
 z 
 •   1 1 2 2 12
1 12
0
2
V
θ
 C2 
 2 
0
0
 x =J
 • =
C
2
ω 0 
d  
0
0
 C2 
 3 
 y 
• 
0
0
ωC02 
θ 4  
 z 
1
1

ω 0 
 C 2 



0 0


0 0
0
0
0
0



0

0

0
0

14

 x 
 VC 30 
 y 
 •  0 0 0 0 
V 0 
θ  

 C3 
 1  0 0 0 0 
 z 
• 

 VC 03 
θ 2  1 1 − 1 0
=
 x =J
C 3  •  0 0 0 0
ω 0 


d
 C3 
 3  0 0 0 0 
 y 
• 

ωC 03 
θ 4  0 0 0 0
 z 
ω 0 
 C 3 
 x 
 VC 04 
1

 y 
 •  − a1Sθ1 − a2 Sθ12
2
V 0 
θ  
1
1
C
4


   a Cθ + a Cθ
 z 
 •   1 1 2 2 12
 VC 04 
θ 2  
0
 x =J
 • =
C
4
ω 0 
0
d  
 C4 
 3 
 y 
0
• 
ωC04 
θ
 4  
1
 z 

ω 0 
 C 4 

1
− a Sθ
2 1 12
1
a Cθ
2 1 12
0

0
0
0

0

0

0

0

1

−1





0

0


0


0

− (d + d )
3 4 

0

4. Ma trận quán tính cơ cấu chấp hành:
Thay J,I vào phương trình:

n
T I J T 
M = ∑  J T mi J T + J ω
i i ωi 
Vi
i = 1 Vi

Ta được:

Ta có:

M =  J T m J T + J T I1J T  +  J T m J T + J T I 2 J T  +
ω1 ω1  V 2 2 V 2 ω 2 ω 2 
 V1 1 V1
 T
m J T + J T I3 J T  +  J T m J T + J T I 4 J T 
J
V
3
3 V 3 ω3 ω 3   V 4 4 V 4 ω 4 ω 4 

=M +M +M +M
1
2
3
4

15

1 2

0 0 0
 a
4 1

 0

 T

T
T
T
0
0
0
M =  J m J + J I1J  = m 
+
1  V 1 1 V 1 ω1 ω1
1
0
0 0 0

 0
0 0 0



1 0 0 0  1 m a 2 0 0 0

 

3 1 1


0
0
0
0
1

0
0 0 0
=
m a2 

12 1 1 0 0 0 0  0
0 0 0

0 0 0 0 


0
0 0 0

M =  J T m J T + J T I 2 J T  =
2  V 2 2 V 2 ω2 ω2 
 2 1 2

1 2 1
a + a a Cθ
0 0
 a + a + a a Cθ
 1

3 2 1 2 2 3 2 2 1 2 2
 1

1 2
1 2
a
0 0
m  a1a2Cθ 2 + a2
2 2

3
3 2


0
0
0
0



0
0
0 0

1
1 −1 0


1
1 −1 0

M =  J T m J T + J T I 3 J T  = m 
3  V 3 3 V 3 ω3 ω 3 
3 −1 −1 1 0


0
0 0 0


16

M =  J T m J T + J T I 4 J T  =
4  V 4 4 V 4 ω4 ω4 
 2 1 2
1 2
a + a a Cθ
 a + a + a a Cθ
2
1
2
2
 1
4
4 2 1 2 2
 1
1 2
1 2
a
m  a1a2Cθ 2 + a2
4 2
4
4 2

0
0




0
1

0



0 0
0
0
0



0 +

0
0


1
−1
− (d + d )
3
4 


1
1
−1
− (d + d )
1

3
4 =
m d 2

1

1
1
(
d
+
d
)
12 4 4 
3
4 

2
− (d + d ) − (d + d ) (d + d ) (d + d ) 
3
4
3
4
3
4
3
4 

 2 1 2
1 2 1 2
1 2
1 2
 a1 + a 2 + a1a 2 Cθ 2 + d 4   a 2 + a1a 2 Cθ 2 + d 4  − d 4
4

12
12
12
 4

 1



1
1 2
1
1
 1 a2 +
  a a Cθ + a 2 + d 2 
d 
− d2



1
2
2
2
2 4
4
4
4
12
12
12

4

m   2
4
1
1
1 2

− d2
− d2
d
4
4
4

12
12
12

1
1
1 2

− d 2d
− d 2d
d d
4
4
4
12
12
12




1



d 2d 
4 

12

1
− d 2d 
4 
12
1 2 
d d 
4 
12
1 2 2
d d 
4
12


với : d= (d3 + d 4 )
Vậy:

M =M

1

+M +M +M
2
3
4

M
 11
M
=  21
M
 31
M
 41

M
12
M
22
M
32
M
42

M
13
M
23
M
33
M
43

M 
14 
M 
24 
M 
34 
M 
44 

Trong đó:
M = 1 m1a12 + m2 (a12 + 1 a22 + a1a2Cθ 2 ) + m3 + m4  a12 + 1 a22 + a1a2Cθ 2 + 1 d 42 
11 3
4
4
12


M

1 2 1

1

1
= m  a + a a Cθ  + m + m  a 2 + a a Cθ + d 2 
12
2 3 2 2 1 2 2 
3
4  4 2 1 2 2 12 4 
17

1

M = −m3 − m4d42
13
12

M = − 1 m4d 42d
14 12
1


1 
1
M = m  a a Cθ + a 2  + m + m  1 a1a2Cθ 2 + 1 a22 + d 42 
21
2 2 1 2 2 3 2 
3
4 2
4
12

M

1

1
1
= m a 2 + m + m  a 2 + d 2 
22 3 2 2
3
4  4 2 12 4 

1
= −m − m d 2
23
3 12 4 4
1
M = − m d 2d
24
12 4 4
1
M = −m − m d 2
31
3 12 4 4
1
M = −m − m d 2
32
3 12 4 4
1
M = m + m d2
33
3 12 4 4
1
M = m d 2d
34 12 4 4
1
M = − m d 2d
41
12 4 4
1
M = − m d 2d
42
12 4 4
1
M = m d 2d
43 12 4 4
1
M = m d 2d 2
44 12 4 4
M

5.

Tính giá trị: Vi , Gi :
4 4  ∂M1 j 1 ∂M jk
Vi = ∑ ∑ 


q
2 ∂qi

j =1k =1
k

 • •

 q qi
 k


18

•2
• •
• •
∗V1 = ( 1 m2 a1a2 Sθ 2 + m4 a1a2 Sθ 2 )θ 2 + a1a2 Sθ 2 (m2 + m4 )θ1θ 2 − 1 m4 d 42 θ 4 d3

2

1

12

•2

3





1





∗V2 = a1a2 Sθ 2 (m2 + m4 )θ 1 + a1a2 Sθ 2 (m2 + m4 )θ1θ 2 − m4 d 42 θ 2 θ 4
2
4
12

• •
• •
• •
1
1
1
m d2θ θ + m d 2θ θ − m d2 d θ
12 4 4 1 4 12 4 4 2 4 12 4 4 3 4
• 2
1
− m d 2 (d + d ) θ
4 4
12 4 4 3
• •
• •
• •
1
1
1
∗V4 = − m4 d 42 θ1 d3 − m4 d 42 θ 2 d3 + m4 d 42 θ 4 d 3
24
24
12
Tính Gi :

∗V3 =

4
Gi = ∑ m j g T J (i)
Vi
j =1
∗ G = m g T J (1) + m g T J (1) + m g T J (1) + m g T J (1) =
1
1
2
3
4
V1
V2
V3
V4
1
1
1
m ga Cθ + m g (a Cθ + a Cθ ) + m g (a Sθ + a Sθ )
2 1 1 2 2 12
4 1 1 2 2 12
2 1 1 1
∗ G = m g T J (2) + m g T J (2) + m g T J (2) + m g T J (2) =
2
1
2
3
4
V1
V2
V3
V4
1
1
m ga Cθ + m ga Cθ
2 2 2 12 2 4 2 12
∗G = 0
3
∗G = 0
4

6. Phương trình chuyển động Lagrang
 Lực tổng quát:

Q = τ + J T Fe − f r

Nếu bỏ qua lực ma sát tại khớp, f r = 0
Jacobi cho khâu tác động cuối:

19

− a sin θ − a sin θ
1 2
12
 1
 a1 cosθ1 + a2 cosθ12

0
J = 
0


0


1

− a sin θ
2
12
a cosθ
2
12
0
0
0
1

− a sin θ − a sin θ
1 2
12
 1
− a sin θ

2
12
JT F = 
0


0


0 0

0 0

−1 0 
0 0 
0 0

0 − 1

a cosθ + a cosθ
1
1 2
12
a cosθ
2
12
0
0

0

0 0

0 0 0
−1 0 0
0

0 0

 Fxn 


1   F yn   F 1 



1   Fzn   F 2 



0   M xn   F 3 
− 1  M yn   F 4 


 M zn 

 Q  τ   F 1 
 1 =  1 + 

Q2  τ 2   F 2 

 Hệ phương trình động học lagrang:

4
∑ M ij q j + Vi + Gi = τ i + Fi
j =1

(i=1…4)

• i=1:


 ••





1
1
1
1
τ1 + F =  m a 2 + m (a 2 + a 2 + a a Cθ ) + m + m  a 2 + a 2 + a a Cθ + d 2  θ
2 1 4 2 1 2 2
3
4  1 4 2 1 2 2 12 4   1
1 3 1 1

 •• 
 ••
1
1
1
1
1
+ m2  a22 + a1a2Cθ 2  + m3 + m4  a22 + a1a2Cθ 2 + d 42 θ −  m3 + m4 d 42   d
  3
  3
4
 2  

2
12
12
••
•2
1

m d 2 (d + d ) θ + ( m a a Sθ + m a a Sθ )θ 2 +
4 1 2 2
4  4 2 2 1 2 2
12 4 4 3

1

−

• •
• •
1
1
1
a1a2 Sθ 2 (m2 + m4 )θ θ − m4 d 42 θ d + m1ga1Cθ1 + m2 g (a1Cθ1 + a2Cθ12 )
1 2 12
4 3 2
2
1
+ m4 g (a1Sθ1 + a2 Sθ12 )
2

• i=2:
20