ĐIỀU KHIỂN ROBOT SCARA DÙNG MOMENT TÍNH

Tiểu luận môn học: Robot công nghiệp và người máy
TRƯỜ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
Đề tài: ĐIỀU KHIỂN ROBOT SCARA DÙNG
MOMENT TÍNH
Học viên: Nguyễn Minh Hồ
Lớp: ĐKHKT K14
MSHV: 01503347
TPHCM 12/2004
Học viên: Nguyễn Minh Hoà Lớp: ĐKHKT K14 Trang 1
Tiểu luận môn học: Robot công nghiệp và người máy
1. 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
21
,
ττ
điều khiển hai cánh tay l
1
,l
2
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(),()(
....
τ
=+
qqqCqqM
Trong đó
21
,qq
là góc quay của hai trục động cơ DC và
21
,
θθ
là góc quay của hai
khớp xoay của robot. Trong đó:






+++−

+−+
=
2
2322121
2121
2
11
))(cos(
))(cos(
)(
mlIIqqklml
qqklmlmlI
qM
e
e








+
+
=
2
.
12121
.

221211
.
))(sin(
))(sin(
),(
Vqqqkklml
qqqkklmlV
qqC
ee
ee
Quan hệ giữa
[ ]
21
qq
và
[ ]
21
θθ
như sau:
)2(
0
2
1
2
1













−
=






q
q
kk
k
ee
e
θ
θ
Các thông số được cho như sau:
Học viên: Nguyễn Minh Hoà Lớp: ĐKHKT K14 Trang 2
Tiểu luận môn học: Robot công nghiệp và người máy
2
1
.8.1 mKgI
=

: Moment quán tính của cánh tay 1
2
2
.041.0 mKgI
=
: Moment quán tính của cánh tay 2
2
3
.134.0 mKgI
=
: Moment quán tính của tải trọng
Kgm 10
=
: Khối lượng tải
ml 4.0
1
=
: Chiều dài của cánh tay 1
ml 35.0
2
=
: 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
smKgV /.5
2
1

=
: Hệ số ma sát của khớp nối 1
smKgV /.3
2
2
=
: Hệ số ma sát của khớp nối 2
Tín hiệu vào: Moment
21
,
ττ
của 2 động cơ DC(hoặc điện áp)
Tín hiệu ra: Góc
21
,
θθ
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:
[ ]
22
2
2
1
22
232
2
11
2211
2

232
2
.
221
.
11
2
232
2
2
..
21
2
232
2
1
..
2
2
2
1
2
1
..
))((
)(
)()(
AllmmlIImlI
AlmlmlII
qAVlmlqVmlIIqBklmlmlIIqABkllm

q
ee
−+++








+++
+−++−++−−
=
ττ
[ ]
22
2
2
1
22
232
2
11
2
2
11121
2
.
2

2
11
1
.
121
2
2
..
2
2
2
1
2
2
1
..
21
2
11
2
..
))((
)(
)()(
AllmmlIImlI
mlIAlml
qVmlIqAVlmlqABllmqBklmlmlI
q
e
−+++









++
++−−−+−
=
ττ
(3)
Với
))(sin())(cos(
2121
qqkBqqkA
ee
+=+=
Đặt các biến trạng thái như sau:
Học viên: Nguyễn Minh Hoà Lớp: ĐKHKT K14 Trang 3
Tiểu luận môn học: Robot công nghiệp và người máy
2
.
4
3
.
23
1
.

2
1
.
11
)4(
qxx
qx
qxx
qx
==
=
==
=
Từ (3) và (4) ta được hệ phương trình biến trạng thái mô tả robot sau:
[ ]
[ ]
22
2
2
1
22
232
2
11
2
2
11121
42
2
112121

2
4
2
2
2
1
22
221
2
11
4
.
4
3
.
22
2
2
1
22
232
2
11
2211
2
232
422121
2
232
2

421
2
232
2
2
2
2
2
1
2
2
.
2
1
.
))((
)(
)()(
)5(
))((
)(
)()(
AllmmlIImlI
mlIAlml
xVmlIAxVlmlABxllmBxklmlmlI
x
xx
AllmmlIImlI
AlmlmlII
AxVlmlxVmlIIBxklmlmlIIABxkllm

x
xx
e
ee
−+++








++
++−−−+−
=
=
−+++








+++
+−++−++−−
=
=

ττ
ττ
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
[ ]
T
qq
21
với các cánh tay có
tọa độ góc
[ ]
T
21
θθ
, được mô tả bởi phương trình (2)












−
=







2
1
2
1
0
q
q
kk
k
ee
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
21211
21211
θθθ
θθθ
++=
++=
lly
llx
Học viên: Nguyễn Minh Hoà Lớp: ĐKHKT K14 Trang 4

Tiểu luận mơn học: Robot cơng nghiệp và người máy
Cá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
21
θθ
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 q 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 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 qng đườ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.
( )
)7()1()()1()(min
2211
−−+−−
kkkk
θθθθ
Giới hạn khơng gian hoạt động của hệ thống thơng qua cặp góc
[ ]
T
qq

21
)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
điều khiển
Hồi tiếp tuyến
tính hoá
SCARA
ROBOT
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:

τ
=+
....
),()( qqqCqqM
Học viên: Nguyễn Minh Hồ Lớp: ĐKHKT K14 Trang 5