Tải bản đầy đủ
Tính vận tốc khối tâm của từng khâu:

Tính vận tốc khối tâm của từng khâu:

Tải bản đầy đủ


 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