xây dựng bộ điều khiển cho khớp 1

LỜI NÓI ĐẦU
Trong công ngiệp hóa hiện, đại hóa đất nước vấn đề tự động hóa sản xuất có
vai trò đặc biệt quan trong .
Mục tiêu của kỹ thuật robot trong công nghiệplà nâng cao năng suất dây
chuyền công nghệ, nâng caao chất lượng và khả năng cạnh tranh của sản phẩm,
đồng thời cải thiện điều kiện lao động. Robot công nghiệp là một bộ phận cấu
thành không thể thiếu trong việc tạo ra những thống tự động sản xuất.
Ngày nay robot công nghiệp được dùng rộng rãi ở nhiều lĩnh vực sản xuất,
trong đó có Robot SCARA . Đây là một kiểu tay máy có cấu tạo đặc biệt được
sử dụng nhiều trong các công việc lắp ráp các tải trọng nhỏ theo phương thẳng
đứng.
Do đó nhóm của chúng em đã chọn loại robot này để làm đề tài tiểu luận của
nhóm.Trong quá trình làm bài, nhóm chúng em đã nghiên cứu, tìm hiểu và tham
khảo nhiều tài liệu khác nhau.Tuy nhiên, bài làm của nhóm sẽ có những chỗ
thiếu xót và không hợp lí. Nhóm chúng em rất mong nhận được sự thông cảm
của thầy. Chúng em xin chân thành cảm ơn!

1


CHƯƠNG I: CHỌN TAY MÁY VÀ TÍNH VẬN TỐC
1.

Chọn tay máy:

 Chọn tay máy scara 4 khâu như hình bên dưới.
Đây là robot có cấu hình kiểu RRTR, bàn tay có chuyển động xoay quanh trục
đứng. Hệ tọa độ gắng lên các khâu như hình vẽ.

2



Bảng thông số DH:
Thông số

ai

di

αi

1

a1

0

0

2

a2

0

180

3

0

d3

0

4

0

d4

0

i

khâu
1

2

0
4

2. Tính vận tốc tay máy:


Từ bảng DH ta có:
cos θ
1


 sin θ1
A0 = 
1
 0
 0


− sin θ
1
cos θ
1
0
0

0 a cos θ 
1
1
0 a sin θ 
1
1
1
0


0
1


cos θ

2

 sin θ 2
A1 = 
2
 0
 0


sin θ
2
− cos θ
2
0
0

0 a cos θ 
2
2
0 a sin θ 
2
2
−1
0


0
1


1

0
A2 = 
3
0
0


0 0
1 0

0

0


0 1 d 
3
0 0 1 

3


cos θ
4

 sin θ 4
A3 = 
4

 0
 0


cos θ
1

sin
θ

1
A0 = A0 . A1 = 
2
1 2
0

 0

cos θ
12

sin
θ

12
=
 0
 0

cos θ

12

sin
θ

12
A0 = A0 . A 2 = 
3
2 3
0

 0

cos θ
12

sin
θ

12
=
0

 0

cos θ
12

sin
θ


12
A 0 = A 0 . A3 = 
4
3 4
0

 0


− sin θ
4
cos θ
4
0
0

− sin θ
1
cos θ
1
0
0

0 0

0 0

1 d 
4

0 1 

0 a cos θ  cos θ
1
1 
2
0 a sin θ   sin θ
1
1 .
2
1
0
0

 0
0
1


sin θ
12
− cosθ
12
0
0
sin θ
12
− cosθ
12
0

0
sin θ
12
− cos θ
12
0
0
sin θ
12
− cos θ
12
0
0

sin θ
2
− cos θ
2
0
0

0 a cos θ 
2
2
0 a sin θ 
2
2
−1
0



0
1


a cosθ + a cosθ 
2
12 1
1
0 a sin θ + a sin θ 
2
12 1
1
−1
0


0
1

0

a cosθ + a cosθ  1
2
12
1
1 
0 a sin θ + a sin θ  0
2
12

1
1 .
−1
0
 0
 0
0
1

0

0 0
1 0

0

0


0 1 d 
3
0 0 1 

0 a cos θ + a cos θ 
2
12
1
1
0 a sin θ + a sin θ 
2

12
1
1
−1
−d

3

0
1

0 a cos θ + a cos θ  cos θ
2
12
1
1 
4
0 a sin θ + a sin θ   sin θ
2
12
1
1 .
4
−1
−d
0

3
 0
0

1


− sin θ
4
cos θ
4
0
0

4

0 0

0 0

1 d 
4
0 1 


cos(θ − θ )
4 12

 sin(θ 4 − θ12 )
=
0


0





sin(θ − θ )
0 a cos θ + a cos θ 
4 12
2
12
1
1
− cos(θ − θ ) 0 a sin θ + a sin θ 
4 12
2
12
1
1
0
−1
−d −d

3
4

0
0
1


Vận tốc tay gắp:

[

x = J J
1
2

J

3

 γ 
 1
γ 2 
J . 
4 d 
 3
γ 
 4 

]

Trong đó:
z0 p0 
4
J =
1  0

z


 (0)
p1(0) 
 z1
4
J =

2
(
0
)
 z1


J

J

3

4

 z (0) 
= 2 
 0 


 (0)
p 3(0) 
 z3
4

=

(
0
)
 z3


Với:

5


0 cos θ
1
  
1
(
0
)
0
z
= R .0 =  sin θ
1   
1
1
0

  

− sin θ 0 0 0
1
   
cosθ
0.0 = 0
1
   
0
1 1 1
cos θ
sin θ
0  0  0 
12
12

   
z 2(0) = R 0 .z 2(2) =  sin θ
− cos θ
0 .0 =  0 
2
12
12

   
0
0
− 1 1 − 1

cos θ
12


3
(
0
)
3
(
3
)
0
z
= R .z
=  sin θ
3
12

 0

sin θ
12
− cos θ
12
0

0  0   0 
   
0 .0 =  0 
 





− 1 1 − 1

Ta có:
p 3(0) = R 0 .r 3(3)
3 4
4
p1(0) = R 0 .r1(1) + R 0 .r 2(2) + p 3(0)
1 2
2 3
4
4
p 0 = R 0 .r 0 + p1(0)
4
0 1
4

Với :
1

R 0 .r 0 = 0
0 1 
0

0 0 a cos θ  a cos θ 
1  1
1
 1

1 0. a sin θ  =  a sin θ 
1
1  1
1

0 1  0   0 

− sin θ 0 a cos θ  a cos θ 
cos θ
1
1  2
2  2
12 

1
(
1
)
R 0 .r
=  sin θ
cos θ
0. a sin θ  =  a sin θ 
1 2
1
1
2
2  2
12 



0
1 
0
0
 

 0

6


0  0   0 

cos θ
12

R 0 .r 2(2) =  sin θ
2 2
12

 0

sin θ
12
− cos θ
12
0

cos θ
12


R 0 .r 3(3) =  sin θ
3 4
12

 0

sin θ 12

0  0   0 

− cos θ
12
0

0. 0  =  0 

















0 . 0  =  0 
− 1 d   − d 
 3  3
















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

Suy ra:
 a cos θ 
1
 1

3
(
3
)
p
=  a sin θ 
1
1
4

0


a cos θ   0   0   a cos θ 
12  
12 
 
  2
 2
1
(
0
)





p
=  a sin θ  + 0 + 0 = a sin θ 

2
12  
12 
4
 
  2

0
−
d
−
d
−
d
−
d






  3 
4  3
4 
a cos θ  a cos θ  a cos θ + a cos θ 
1  2
12   1
1 2
12 

 1
0
p =  a sin θ  +  a sin θ  =  a sin θ + a sin θ 
4  1
1  2
12   1
1 2
12 
0
−
d
−
d
−
d
−
d





 
3
4 
3
4


Từ đó ta tìm được:



i
0


0


 a cos θ + a cos θ
1 2
12
J = 1
1







j
0
0
a sin θ + a sin θ
1
1 2
12
0
0

1


k
0
1

 − a sin θ − a sin θ 
1 2
12 
  1
  a cos θ + a cos θ
1 2
12 
  1
0
−d −d  

3
4 0 = 

0
 

 

0
 

 


1
 

7





i
0


0

a cos θ
12
J = 2
2







k
0
1


j
0
0
a sin θ
2
12
0
0
1

 − a sin θ 
12 
  2
  a cos θ
12 
  2
0
−d −d  

3
4 0 = 

0
 

 

0

 

 

1
 

0
 
0
 
−1
J =  
3
0
 
0
 
 0 



i
0
0

0
J =
4







Vậy:


j
0
0
0
0
0
1


k  0
0   
1  0
  
− d  0
4 =  
 0
 0
  
 1 
  

− a sin θ − a sin θ

1 2
12
 1
 a1 cos θ1 + a2 cos θ12

0
x = 
0


0


1

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

0 0

0 0   γ1 
 

−1 0  γ 2 
. 
0 0  γ 3 
 
0 0  γ 4 

0 −1

Nhận xét: Dựa vào các thông số trong ma trận biểu diễn vận tốc thì tay máy có
các thành phần vận tốc

có giá trị khác không,

bằng

không. Vậy tay máy có 3 chuyển động tịnh tiến và một chuyển động quay quanh
trục OZ.
8


CHƯƠNG II: BÀI TOÁN ĐỘNG LỰC HỌC
1. Tính các ma trận quán tính




Giả sử các khâu có tiết diện nhỏ
 0 0 0

1

i
2
Ta có Ii = mi a i 0 1 0
12


0 1 1
Ma trận quán tính khâu I viết trong hệ tọa độ tuyệt đối:
T
I = R 0 I i  R 0 
i
i i i 

• i=1

Cθ1 − Sθ1 0
 Cθ1 Sθ1 0
T
R 0 =  Sθ
Cθ
0 ⇒  R 0  = − Sθ Cθ 0
1
1
1
1
1
 1
 0
 0
0

1
0 1

Vậy

 S 2θ
− Sθ C θ 0 
1
1 1 

1
2
2

I = m a − Sθ Cθ
C θ
0

1 12 1 1 
1 1
1
0
0
1




• i=2:

0
Cθ
Sθ
0
Cθ12 Sθ12
T  12
12


0
0
R =  Sθ
− Cθ
0  ⇒  R  =  Sθ
− Cθ
0 
2
12
12
2
12
12
 
 0
 0
0
− 1
0
− 1

 S 2θ
12

1
2

Vậy: I 2 = m2 a 2  − Sθ12 Cθ12
12
0



• i=3:

Cθ12
0
R =  Sθ
3
12
 0

Sθ
12
− Cθ
12
0

− Sθ

Cθ

12 12
C 2θ
12
0

0
Cθ
T  12

0


0  ⇒  R  =  Sθ
12
 3
 0
− 1

0

0

1

Sθ
12
− Cθ
12
0

0
0 
− 1

9


 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

Cθ
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θ
Cθ
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θ
Cθ
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

Sθ

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

Sθ
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θ
Cθ
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


 1
1 2
1
1 2 1 2  • •

τ + F =  m  a a C θ + a  + m + m  a a Cθ + a + d   θ
2
4  2 1 2 2 4 2 12 4  1
2  2 2 1 2 2 3 2  3
••

••

1

1
1
1


+  m a 2 + m + m  a 2 + d 2  θ + − m − m d 2  d
3
4  4 2 12 4  2  3 12 4 4  3
3 2 2
•2 3
• •
1
 •• 1
2
−  m d (d + d )θ + a a Sθ (m + m )θ 1 + a a Sθ (m + m )θ θ
4
4 1 2
4  4 2 1 2 2 2
12 4 4 3
4 1 2 2 2

−

• •
1

1
1
m d 2 θ θ + m ga Cθ + m ga Cθ
4
4
2
2
12
2
4
12
2
2 4 2 12

• i=3:



1

 •• 

1

 ••



1

 ••

τ + F = − m − m d 2  θ + − m − m d 2  θ + m + m d 2  d
3 3  3 12 4 4  1  3 12 4 4  2  3 12 4 4  3
• •
• •
• •
1
1
1
 •• 1
2
+  m d (d + d ) θ + m4 d 42 θ θ + m4 d 42 θ θ − m4 d 42 d θ
1 4 12
2 4 12
3 4
4  4 12
12 4 4 3

−

• 2
1
m d 2 ( d + d )θ
4 4
12 4 4 3

• i=4

 ••  1

 ••
 1
 ••  1
 ••  1
τ + F =  − m d 2 d  θ + − m d 2 d  θ +  m d 2 d  d +  m d 2 d 2  θ
4
4  12 4 4  1  12 4 4  2 12 4 4  2 12 4 4  4

−

• •
• •
• •
1
1
1
m d 2θ d − m d 2θ d + m d 2θ d
24 4 4 1 3 24 4 4 2 3 12 4 4 4 3

21


CHƯƠNG III: XÂY DỰNG BỘ ĐIỀU KHIỂN CHO KHỚP 1.
1. Chọn động cơ.
 Lí do chọn động cơ:
Trong kỹ thuật robot về nguyên tắc có thể dùng động cơ điện các loại khác
nhau, nhưng trong thực tế có hai loại dùng nhiều hơ cả. Đó là động cơ điện
một chiều và động cơ bước.
Về phương diện điều chỉnh tốc độ thò động cơ điện có nhiều ưu điểm hơn
hẳn so với các loại động cơ khác. Đó là khả năng điều chỉnh tốc độ dễ dàng

trong dải rộng, có cấu trúc mạch lực và mạch điều khiển dơn giản.
Do đó, nhóm em đã chọn động cơ điện một chiều để điều khiển cho khớp 1
của tay máy.



Sơ đồ động cơ điện một chiều cùng phụ tải:

22


Sơ đồ động cơ điện một chiều

Trong đó:
Ua : là điện áp đặt vào phần ứng.
Ra, La : điện trở và điện cảm phần ứng.
eb: sức phản điện động của động cơ.
ia : cường độ dòng điện.
Mm, Jm: momem và momen quán tính của động cơ.
ML, JL: momem và momen quán tính của phụ tải.
θm : góc quay của trục động cơ.
θL : góc quay của trục phục tải.
n = θL/ θm: tỉ số truyền của hộp số.
 Công suất động cơ:
Giả sử động cơ dài hạn làm việc cho phụ tải ngắn hạn, có chu kì lặp lại.

23


Động cơ ngắn hạn lặp lại thường được chế tạo chuyên dụng có độ bền cơ khí

cao, quán tính nhỏ (để đảm bảo khởi động và hãm thường xuyên) và khả
năng qua tải lớn (từ 2,5 - 3,5 lần).
Khi chọn động cơ phải đảm bảo
Chọn động cơ có thông số kỹ thuật như sau:
Pđm = 450W
Uđm =24V/36V
Iđm

2,5A

nđm = 4000 v/ph
Ra = 91,4Ω
La =1800 µH
J = 2,3 g.cm2
Mđm= 1,43N.m
η

80%

2. Hàm truyền của hệ thống:
 Momen trên trục động cơ:
M(t) = Mm (t)+ ML(t)
Trong đó:
Mm (t) = Jm
ML (t) = JL


(1)

(t) + fm

(t)

(t) + fL (t)

Theo định luật bảo toàn năng lượng, công do phụ tải sinh ra tính trên trục phụ
tải là ML θL bằng công quy về trục động cơ

θm .

Do đó:

JL

(t) + fL

(t)]

(2)

( với

)

24


 Từ (1) và (2) suy ra:
M(t) = (Jm +
Đặt: J = Jm +

JL ;

JL )

(t) + (fm

(t)

=

J: momen quán tính tổng hiệu dụng.
: hệ số tổng hiệu dụng.
Biến đổi Laplace phương trình trên, ta được:
(*)
 Momen trên trục động cơ phụ thuộc tuyến tính với cường độ dòng điện phần
ứng, không phụ thuộc vào góc quay và vận tốc, ta có:

Ka: hệ số tỷ lệ momen.(Ka =0.2 )
Mạch điện phần ứng:

Trong đó:

: hệ số tỷ lệ với sức phản điện động.( Kb =5,5.10-2 )

Suy ra:
Biến đổi Laplace phương trình trên, ta được:

25