Robot SCARA ra đời vào năm 1979, đâ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.
Trang 1TRƯỜNG ĐẠI HỌC BÁCH KHOA TPHCM
KHOA ĐIỆN-ĐIỆN TỬ
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG
TIỂU LUẬN MÔN HỌC ROBOT CÔNG NGHIỆP VÀ NGƯỜI MÁY
MOMENT TÍNH
Học viên: Nguyễn Minh Hồ Lớp: ĐKHKT K14
MSHV: 01503347
TPHCM 12/2004
Trang 21 Giới thiệu về Robot Scara
Robot SCARA ra đời vào năm 1979, đâ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
Robot SCARA bao gồm hai khớp nối và hai cánh tay Trên hai khớp nối sẽ gắn hai động cơ để cung cấp moment τ1 ,τ2 điều khiển hai cánh tay l1,l2 như hình vẽ
1 θ
2 θ 1
l
2
l
Hình 1: Hình chiếu bằng Robot Scara
2 Mô hình động học của Robot SCARA
Phương trình động học của Robot SCARA như sau:
) 1 ( )
, ( )
(q q +C q q. q. = τ
M
Trong đó q1, q2là góc quay của hai trục động cơ DC và θ1 ,θ2 là góc quay của hai
khớp xoay của robot Trong đó:
+ + +
−
+
− +
2 3 2 2
1 2
1
2 1 2
1
2 1 1
)) (
cos(
)) (
cos(
)
(
ml I I q
q k l ml
q q k l ml ml
I q
M
e
e
+
+
=
2
1 2 1 2
1
2 2 1 2
1 1
.
)) (
sin(
)) (
sin(
)
,
(
V q
q q k k l ml
q q q k k l ml V
q
q
C
e e
e e
Quan hệ giữa [q1 q2] và [θ1θ2] như sau:
) 2 ( 0
2
1 2
1
−
=
q
q k k
k
e e
e
θ
θ
Các thông số được cho như sau:
Trang 31 1 8Kg.m
I = : Moment quán tính của cánh tay 1
2
2 0 041Kg.m
I = : Moment quán tính của cánh tay 2
2
I = : Moment quán tính của tải trọng
Kg
m= 10 : Khối lượng tải
m
l1 = 0 4 : Chiều dài của cánh tay 1
m
l2 = 0 35 : Chiều dài của cánh tay 2
5
.
0
=
e
k : Hệ số truyền động của khớp nối 2
s m Kg
1 = : Hệ số ma sát của khớp nối 1
s m Kg
2 = : Hệ số ma sát của khớp nối 2
Tín hiệu vào: Moment τ1 ,τ2 của 2 động cơ DC(hoặc điện áp)
Tín hiệu ra: Góc θ1 ,θ2 của 2 cánh tay robot SCARA
3 Hệ phương trình biến trạng thái mô tả robot
Biến đổi phương trình (1) và (2) ta được:
2
2 1
2
2 2 3 2
2 1 1
2 21 1
2 2 3 2
2
2 21
1 1
2 2 3 2
2 2
21
2 2 3 2
2 1
2 2
2 1 2
1
) )(
(
) (
) (
) (
Al lm ml
I I ml I
Al ml ml
I I
qA Vl ml qV ml I I qB kl ml ml I I q AB kl lm q
e e
− + + +
+ +
+
+
− +
+
− +
+
−
−
2
2 1
2
2 2 3 2
2 1 1
2
2 1 1 1 21
2
2
2 1 1 1
1 21
2 2
2 2
2 1 2
2 1
21
2 1 1
2
) )(
(
) (
) (
) (
Al lm ml
I I ml I
ml I Al ml
q V ml I qA Vl ml q AB ll m qB kl ml ml I
q
e
− + + +
+ +
+ +
−
−
− +
−
(3)
Với A= cos(k e(q1+q2)) B= sin(k e(q1+q2))
Đặt các biến trạng thái như sau:
Trang 4.
4
3
.
2
3
1
.
2
1
.
1
1
) 4 (
q
x
x
q
x
q
x
x
q
x
=
=
=
=
=
=
Từ (3) và (4) ta được hệ phương trình biến trạng thái mô tả robot sau:
2
2 1
2
2 2 3 2
2 1 1
2
2 1 1 1 2
4 2
2 1 1 2 1 2
2 4
2 2
2 1
2
2 2 2
2 1 1
4
.
4
3
.
2
2 2
2 1
2
2 2 3 2
2 1 1
2 2 1 1
2 2 3 2
4 2 2 1 2 1
2 2 3 2
2 4 2 1
2 2 3 2
2 2
2 2
2 1 2
2
.
2
1
.
) )(
(
) (
) (
) (
)5( )
)(
(
) (
) (
) (
Al lm ml I I ml I
ml I Al ml
xV ml I Ax Vl ml ABx ll m Bx kl ml ml I
x
x
x
Al lm ml I I ml I
Al ml ml
I I
Ax Vl ml xV ml I I Bx kl ml ml I I ABx kl lm
x
x
x
e
e e
− + + +
+ +
+ +
−
−
− +
−
=
=
− + + +
+ +
+
+
− +
+
− +
+
−
−
=
=
τ τ
τ τ
4 Quỹ đạo đầu mút cánh tay Robot SCARA
Mối liên hệ giữa các khớp nối 1 và 2 có toạ độ góc [q1 q2]T với các cánh tay có tọa độ góc [ ]T
2
1θ
θ , được mô tả bởi phương trình (2)
−
=
2
1 2
q
q k k
k
e e
e
θ
θ
Tọa độ đầu mút của cánh tay robot SCARA được xác định bởi phương trình sau:
) 6 ( ) sin(
sin
) cos(
cos
2 1 2 1
1
2 1 2 1
1
θ θ θ
θ θ θ
+ +
=
+ +
=
l l
y
l l
x
Trang 5Các phương trình trên áp dụng cho bài tốn điều khiển theo khơng gian khớp nối Cho phép xác định cặp gĩc [ ]T
2
1θ
θ từ tọa độ (x,y) cho trước của đầu mút cánh tay robot SCARA trong khơng gian hoạt động
Trong quá trình điều khiển robot, ứng với một vị trí (x,y) trong khơng gian hoạt động cĩ thể cĩ nhiều cặp gĩc trong khơng gian khớp nối Do đĩ, ta phải chọn lựa một cặp nghiệm phù hợp với yêu cầu điều khiển Cĩ nhiều cách chọn lựa tuỳ theo đối tượng và mục đích:
- Tổng quãng đường dịch chuyển của các khớp ngắn nhất
- Giảm bớt ảnh hưởng tương tác giữa các khớp
- Năng lượng tiêu thụ thấp nhất
- Giới hạn động học của hệ thống
Robot SCARA với hai khớp nối ứng với mỗi tọa độ (x,y) của đầu mút cánh tay chúng ta sẽ cĩ hai nghiệm cho các cặp gĩc Do đĩ, ta sẽ chọn cặp gĩc sao cho tổng biến thiên của cặp gĩc từ vị trí k-1 đến vị trí k là nhỏ nhất
( ( ) ( 1 ) ( ) ( 1 )) ( 7 )
min θ1 k − θ1 k− + θ2 k − θ2 k−
Giới hạn khơng gian hoạt động của hệ thống thơng qua cặp gĩc [q1 q2]T
) 8 (
2
1
π
θ
π
π
θ
π
≤
≤
−
≤
≤
−
5 Điều khiển robot SCARA dùng moment tính(computed torque)
Trong phương pháp moment tính, ta phải xây dựng hai vịng điều khiển Vịng hồi tiếp trong được xây dựng dựa trên động lực học của hệ thống để bù tất cả các thành phần phi tuyến của hệ thống Vịng hồi tiếp ngồi xây dựng dựa trên sai lệch giữa tín hiệu ra và tín hiệu đặt Chức năng của vịng hồi tiếp trong là tuyến tính hố mối liên
hệ giữa tín hiệu đặt và tín hiệu ra trong khi vịng hồi tiếp ngồi làm ổn định hố hệ thống
5.1 Cấu trúc của bộ điều khiển moment tính
Bộ điều khiển PD
Chiến lược
Hệ thống tuyến tính hoá Vòng điều khiển trong
Vòng điều khiển ngoài
Hình 2: Sơ đồ hệ thống điều khiển dùng moment tính
5.1.1 Hồi tiếp tuyến tính hố
Mơ hình robot SCARA được cho bởi (1) như sau:
τ
= + . .
) , ( )
M
Trang 6Với
+ + +
−
+
− +
2 3 2 2
1 2
1
2 1 2
1
2 1 1
)) (
cos(
)) (
cos(
)
(
ml I I q
q k l ml
q q k l ml ml
I q
M
e
e
+
+
=
2
1 2 1 2
1
2 2 1 2
1 1
.
)) (
sin(
)) (
sin(
)
,
(
V q
q q k k l ml
q q q k k l ml V
q
q
C
e e
e e
Để tuyến tính hố và phân ly, ta chọn luật điều khiển τ gồm hai thành phần như sau:
) 9 (
N
τ
Với
) 11 (
) 10 (
q
M
q C q
M
d
L
n
N
=
+
=
τ
τ
SCARA ROBOT
) , (q q. N
+ +
N
τ
L
τ
q
Hình 3: Sơ đồ cấu trúc hồi tiếp tuyến tính hố
+
−
+
−
=
0 ))
( cos(
)) (
cos(
0
2 1 2
1
2 1 2
1
q q k l ml
q q k l ml M
e
e n
+ +
+
2 3 1
2 1 1
0
0
ml I I
ml I
M d
+
+
=
2
1 2 1 2
1
2 2 1 2
1 1
.
)) (
sin(
)) (
sin(
)
,
(
V q
q q k k l ml
q q q k k l ml V
q
q
C
e e
e e
5.1.2 Điều khiển PD
Đặt y =q với y là tín hiệu vào mới, ta có luật điều khiển được mô tả như sau:
) 12 (
y
M d
L =
τ
Mục đích điều khiển là tín hiệu ra q (t)bám theo quỹ đạo thiết kế q d (t)
Ta có: r=q d+K p q d +K d q.d ( 13 )
Chọn luật điều khiển
) 14 (
.
r q K q
K
Trang 7Thay (13) vào (14) ta được
q
q
e
K
e
K
e
d
p d
−
=
+
Giả thuyết Kp và Kd là các ma trận dương xác định, ổn định tiệm cận
Từ (10) và (11) ta được
+ +
−
+ +
−
=
2 1
2 1 2
2 1
2 1
2 2
2 1 1
1 2
2 1 2
1
q B k l ml q V q A l ml
q B k l ml q V q A l ml
e
e N
N
τ
τ
)) (
sin(
)) (
+ + +
+
+ + +
=
) )(
(
) )(
(
2
2 2
2 2 3 2
1
1 1
2 1 1
2
1
e K e K q ml I I
e K e K q ml I
p d
d
p d
d L
L
τ
τ
.
.
.
q
q
e
q
q
e
d
d
−
=
−
=
PD
Chiến lược
) , (q q. N
+ +
N
τ
L
τ
q
Hình 4: Sơ đồ khối hệ thống điều khiển dùng moment tính
5.2 Thực hiện mơ phỏng hệ thống điều khiển robot SCARA dùng moment tính 5.2.1 Xây dựng sơ đồ mơ phỏng
Xây dựng mơ hình robot SCARA theo phương trình (1) trên Simulink như sau:
Trang 8Hình 5: Mô hình robot SCARA
Hình 6: Sơ đồ điều khiển dùng moment tính
Trang 9Hình 7: Sơ đồ khối phần hồi tiếp tuyến tính hố
Hình 8: Sơ đồ khối bộ điều khiển PD
5.2.2 Kết quả mô phỏng
5.2.2.1 Đáp ứng bước:
Trang 100 20 40 60 0
0.2
0.4
0.6
0.8
1
Tin hieu qd1-q1
0 0.2 0.4 0.6 0.8
1
Tin hieu qd2-q2
Hình 9: Đáp ứng bước của q1 và q2
Hình 10: Moment của 2 động cơ khi tín hiệu vào là hàm bước
Nhận xét: Đáp ứng bước không có vọt lố, loại bỏ được sự ảnh hưởng lẩn nhau
giữa hai khớp nối
5.2.2.2 Bám theo quỹ đạo hình sin:
Trang 110 10 20 -1
-0.5
0 0.5
1
qd1-q1
-1 -0.5 0 0.5 1
qd2-q2
q1 qd1
q2 qd2
Hình 11: Đáp ứng ngõ ra q1 và q2 khi tín hiệu vào là hình sin
Hình 12: Moment của 2 động cơ DC 1 và DC2 khi tín hiệu vào là sin
Nhận xét: Ngỏ ra của hệ thống bám theo tín hiệu vào hình sin rất tốt,gần như
không có sai số,hệ phân ly tốt các khớp không bị ảnh hưởng lẩn nhau
