Hình 4.9 Kết quả mơ phỏng tay máy ứng với trường hợp T2
80
Hình 4.10 Kết quả mơ phỏng tay máy ứng với trường hợp T3
Kết luận: kết quả mô phỏng thu được như trong hình 4.8, 4.9 và 4.10 hoàn tồn
giống với các giá trị tính tốn trong bảng 4.2, như vậy các phương trình động học thuận và q trình mơ phỏng động học thuận đúng.
4.3.2. Kiểm tra bài tập động toán ngược
Kiểm tra bài tốn động học ngược theo lưu đồ hình 4.7 b, bằng cách nhập tọa độ mà bộ phận công tác muốn tới và hướng của bộ phận công tác vào các ô x, y, z và Theta4 tương ứng ở mục “End Effector Position”. Sau đó nhấn nút “Inverse Calculate”, các giá trị θ1, θ2, θ3, θ4 trong các phương trình (4.13), (4.22) và (4.25) sẽ được tính tốn. Nếu một trong các giá trị θ1, θ2, θ3, θ4 nằm ngoài giới hạn biến khớp được quy định trong bảng 4.3, thơng báo lỗi (hình 4.6) sẽ xuất hiện, u cầu nhập lại x, y, z và Theta4. Nếu các giá trị θ1, θ2, θ3, θ4 đều thỏa thì hiển thị kết quả trong các ơ Theta1, Theta2, Theta3, Theta4 ở mục “Input Joint Angles”. Sau khi đã tính tốn xong các giá trị góc quay của các khâu bằng bài toán động học ngược, ấn “Simulink”
81
để tiến hành mô phỏng và kiểm tra tọa độ của bộ phận công tác trong cụm khối hiển thị trong hình 4.4. Nếu giá trị x, y, z ở mục “End Effector Position” trùng với toạ độ của bộ phận công tác trong cụm khối hiển thị, chứng tỏ các phương trình động học ngược và q trình mơ phỏng động học ngược đúng. Các số liệu tính tốn động học ngược bằng Matlab thơng qua giao diện GUI như trong hình 4.5 được thể hiện trong bảng 4.3. Trong đó:
- Trường hợp N1: nhập giá trị x, y, z giống như giá trị tính tốn được ở trường hợp T2.
- Giá trị nhập vào của trường hợp N2, N3, N4 tương ứng với tọa độ các điểm cao nhất S (725, 1000, 730), gần nhất T (500, 0, 200) và xa nhất U (1300, -400, 200) đã được đề cập ở mục 1.5 chương 1.
Bảng 4.3 Các số liệu tính tốn động học ngược bằng Matlab. Trường Trường
hợp
Giá trị nhập (mm) Giá trị tính tốn được (độ)
x y z Theta4 Theta1 Theta2 Theta3 Theta4
N1 830 25 993 90 0 90 -90 90
N2 725 1000 730 90 52,8981 53,5079 -63,8575 90
N3 500 0 200 0 -2,86598 60,0803 -148,425 0
82
Hình 4.11 Kết quả mơ phỏng tay máy ứng với trường hợp N1
Nhận xét: ở trường hợp N1 (hình 4.11) sau nhập giá trị x, y, z giống như giá trị tính
tốn được ở trường hợp T2 thì sau q trình tính tốn động học ngược thu được các giá trị góc Theta1, Theta2, Theta3 và Theta4 tương tự như các giá trị nhập vào ở trường hợp T2. Như vậy, khi lấy kết quả tính được ở bài toán động học thuận làm giá trị đầu vào của bài tốn động học ngược thì kết quả tính được ở bài tốn động học ngược giống hồn tồn với giá trị đầu vào của bài tốn động học thuận. Điều này chứng tỏ các phương trình của bài tốn động học nghịch đúng.
83
Hình 4.12 Kết quả mơ phỏng tay máy ứng với trường hợp N
84
Hình 4.14 Kết quả mơ phỏng tay máy ứng với trường hợp N4
Nhận xét: từ các hình 4.11 đến 4.14 có thể nhận thấy từ vị trí mong muốn của bộ
phận cơng tác, bài tốn động học nghịch đã tính ra các giá trị biến khớp và với các giá trị biến khớp này, bộ phận công tác của tay máy đã đến được các vị trí mong muốn một cách chính xác như kết quả mơ phỏng đã thể hiện. Ngồi ra từ hình 4.12 đến hình 4.14 cũng cho thấy với thiết kế cơ khí ở chương 3, bộ phận cơng tác của tay máy hồn tồn có thể đi tới các điểm cao nhất S, gần nhất T và xa nhất U theo yêu cầu thiết kế.
Kết luận: với hai nhận xét trên trong mục 4.3.2, có thể kết luận các phương trình
động học nghịch và q trình mơ phỏng động học nghịch đúng, ngồi ra cũng chứng tỏ được thiết kế cơ khí của tay máy ở chương 3 đảm bảo được yêu cầu thiết kế đặt ra ban đầu trong mục 1.5.
85
4.4. Kết luận chương 4
Ở chương 4 đã thực hiện được:
- Giải được bài toán động học thuận và động học ngược để tìm ra mối quan hệ giữa các biến khớp và vị trí của bộ phận công tác.
- Mơ phỏng kiểm tra được tính đúng đắn của bài tốn động học. Kết quả này có vai trị quan trọng trong việc điều khiển tay máy ở chương sau.
86
CHƯƠNG 5: HỆ THỐNG ĐIỆN VÀ ĐIỀU KHIỂN
Với cơ cấu cơ khí của tay máy đã được thiết kế ở chương 3 và các phương trình động học tính tốn được ở chương 4, chương 5 sẽ tiến hành thiết kế hệ thống điện cho toàn bộ tay máy và đưa ra giải thuật điều khiển để điều khiển tay máy gắp và sắp xếp sản phẩm theo đúng yêu cầu.
5.1. Hệ thống điện sử dụng cho tay máy
Ở chương 3 đã tiến hành tính tốn lựa chọn bốn động cơ AC servo tích hợp với hộp giảm tốc Harmonic thuộc hãng HarmonicDrive cho bốn khớp của tay máy. Để cấp nguồn và điều khiển các động cơ, sử dụng bốn driver cùng hãng cho bốn động cơ này là driver HA-800A-6D/E-200 cho hai động cơ ở khớp 1, 2 của tay máy và driver HA800A-3D/E-200 (hình 5.1) cho hai động cơ ở khớp 3, 4 của tay máy. Các thông số cơ bản của driver được thể hiện trong bảng 5.1.
87
Bảng 5.1 Thông số của driver sử dụng trong luận văn
Model HA-800A-6D/E-200 HA-800A-3D/E-200
Input voltage AC220 – 230V
Power frequency 50/60Hz
Driver’s rated current 6A 3A
Driver’s maximum current 19A 9,5A
Trong bảng 5.2 là một số chân trong cổng CN2 của driver được dùng để nối với các chân tương ứng của card điều khiển để điều khiển động cơ.
Bảng 5.2 Các chân tín hiệu điều khiển của driver
Chân số Tín hiệu Ký hiệu
1 or 8 Input signal common IN-COM
2 Servo - On S - ON
17 Servo – On input enable OUT2
18 Alarm OUT3
19 In-position complete OUT4
27 FWD pulse+ FWD+
28 FWD pulse- FWD-
29 REV pulse+ REV+
30 REV pulse- REV-
42 Phase output A+ A+
88
44 Phase output B+ B+
45 Phase output B- B-
46 Phase output Z+ Z+
47 Phase output Z- Z-
❖ Lựa chọn chế độ xung cấp cho driver
Xung điều khiển được lựa chọn để cấp cho driver có dạng Open Collecto (hình 5.2). Theo đó khi xung được cấp vào chân “Forward command pulse signal input” động cơ đang được điều khiển sẽ quay thuận và khi xung được cấp vào chân “Reverse command pulse signal input” động cơ đang được điều khiển sẽ quay nghịch.
89
❖ Card điều khiển
Card PCI - 8134 (hình 5.3) được sử dụng để xuất xung điều khiển cho bốn driver động cơ AC servo trong luận văn. Card PCI - 8134 là card điều khiển chuyển động và có khả năng điều khiển tối đa cho bốn động cơ AC servo. Card gồm có CN1 PIN (External power input) bảng 5.3, CN2 PIN (Main Connector) bảng 5.4, CN3 PIN (Manual pulser input), CN4 PIN (Simultaneous Start/Stop).
Hình 5.3 Card PCI - 8134 [6]
Bảng 5.3 Các chân của cổng CN1
CN1 PIN Ký hiệu Tính năng
1 EXGND Ground
2 EX+24V Cấp nguồn ngoài +24V DC ± 5%
90
Bảng 5.4 Các chân của cổng CN2 được dùng cho một driver
CN2 PIN Ký hiệu Tính năng
4 OUT1- Cấp xung quay thuận cho driver
6 DIR1- Cấp xung quay nghịch cho driver
7 SVON1 Cấp tín hiệu Servo-On
9 ALM1 Nhận tín hiệu Alarm từ driver
10 INP1 Nhận tín hiệu Inposition từ driver
11 RYD1 Nhận tín hiệu Servo-On input enable
12 EXGND Ground
13 EA1+ Nhận tín hiệu encoder A-phase (+)
14 EA1- Nhận tín hiệu encoder A-phase (-)
15 EB1+ Nhận tín hiệu encoder B-phase (+)
16 EB1- Nhận tín hiệu encoder B-phase (-)
17 EZ1+ Nhận tín hiệu encoder Z-phase (+)
91
❖ Lựa chọn chế độ xung cho card điều khiển
Jumpers J1 J8 trên card điều khiển được sử dụng để lựa chọn chế độ xung điều khiển cấp cho driver. Như đã trình bày ở trên, xung điều khiển được lựa chọn để cấp cho driver có dạng Open Collector. Do đó, vị trí các Jumpers được cài đặt như trong hình 5.4.
Hình 5.4 Vị trí cài đặt Jumpers cho chế độ xung Open Collector [6]
❖ Cảm biến xác định sản phẩm và pallet
Như đã trình bày trong mục 1.5 chương 1, có ba cảm biến được sử dụng để phát hiện sản phẩm và pallet. Với các ưu điểm như không tiếp xúc với vật thể cần phát hiện, không bị hao mịn, thời gian đáp ứng nhanh, do đó cảm biến quang điện (loại NPN) PNR02 của hãng Hanyoung (hình 5.5) được lựa chọn sử dụng trong luận văn. Cảm biến được cấp nguồn 24VDC, có dịng điện tối đa ở đầu ra là 100 (mA) và có thời gian đáp ứng tối đa là 3(ms). Tín hiệu ở đầu ra (OUT) của cảm biến được nối với chân L1 của 24VDC Relay (hình 5.5) nhằm điều khiển đóng ngắt relay, cấp tín hiệu cho các chân PEL1, MEL1 và PSD1 của card điều khiển. Các tín hiệu này dùng để báo cho tay máy biết sản phẩm và pallet đã nằm ở đúng vị trí, phục vụ cho quá trình điều khiển tay máy sẽ được trình bày cụ thể ở mục 5.3.
Open Collector Line Driver 1
92
Hình 5.5 Cảm biến quang điện PN-R02 [6]
Hình 5.6 G5V-DPDT 24VDC Relay của hãng Omron [6]
Sau khi đã có được các thành phần trong hệ thống điện của tay máy như: động cơ, driver, card điều khiển và các cảm biến như đã trình bày ở trên, tiến hành kết nối các thành phần này lại thành một hệ thống. Trong hình 5.6 là sơ đồ nối dây cho một driver. Các driver còn lại được kết nối tương tự với card điều khiển.
93
Hình 5.7 Sơ đồ đấu dây cho một driver
❖ Mạch điều khiển xylanh tay kẹp
Trong hình 5.8 là sơ đồ nối dây giữa card điều khiển và mạch khí nén điều khiển cho xylanh chính (9) và xylanh phụ (7) của tay kẹp. Khí nén từ nguồn sẽ qua bộ lọc (1), van điều áp (2), đồng hồ đo áp (3) và bộ tra dầu (4) sau đó sẽ qua van điều khiển 5/2 (5) và bộ tiết lưu (6) đi vào xylanh khí nén tác động kép. Mỗi xylanh đều có hai
94
cảm biến hành trình loại NPN (8) để giới hạn hành trình của pittong. Các tín hiệu cảm biến được đưa đến card điều khiển, từ đó card điều khiển xuất tín hiệu đóng ngắt hai relay 4 và 5 để đóng ngắt hai cuộn solenoid của van 5/2 (5). Ngoài ra để điều khiển tay kẹp bằng tay, tiến hành bật công tắc gạt 3 (CTG3 ON). Khi đó, hai cuộn solenoid của van 5/2 cũng có thể được đóng ngắt bằng tay nhờ hai cơng tắc gạt 1 và 2 (CTG1 và CTG2).
Hình 5.8 Sơ đồ đấu dây giữa card điều khiển với mạch khí nén
Sau khi đã có được hệ thống điện cho tay máy và bộ phận công tác như ở trên, bước tiếp theo tiến hành thiết kế quỹ đạo chuyển động cho tay máy, từ đó làm cơ sở để điều khiển tay máy.
95
5.2. Thiết kế quỹ đạo chuyển động cho tay máy
Để gắp và sắp xếp sản phẩm, bộ phận công tác của tay máy cần di chuyển từ vị trí mong muốn này đến vị trí mong muốn kia với quỹ đạo chuyển động có thể được thiết kế theo không gian làm việc hoặc không gian khớp. Quỹ đạo được thiết kế trong không gian khớp giảm thiểu được các hiệu ứng bất lợi ví dụ như quỹ đạo khơng trơn tru, tuy nhiên đây là một quỹ đạo cong [2]. Điều này gây nguy hiểm cho tay máy do mơi trường làm việc chật hẹp, tay máy có khả năng va chạm với các chướng ngại vật xung quanh. Vì vậy, trong luận văn lựa chọn thiết kế quỹ đạo theo không gian làm việc để có thể kiểm sốt lộ trình của tay máy, tránh các vùng nguy hiểm trong không gian làm việc. Cụ thể, trong luận văn lựa chọn quỹ đạo của bộ phận công tác là các đoạn thẳng Q1Q2, Q2Q3, Q3Q4. Trong đó Q1 là vị trí của sản phẩm cần gắp trên băng tải; Q2 là vị trí sản phẩm được nhấc lên theo phương thẳng đứng trước khi được di chuyển tới pallet. Do đó, trong q trình gắp và sắp xếp sản phẩm, tọa độ và khơng đổi. Ngồi ra, là vị trí chuẩn bị hạ sản phẩm và là vị trí của sản phẩm cần sắp xếp trên pallet.
Theo bài tốn cụ thể ở mục 1.5 chương 1, có tám tọa độ tương ứng với tám vị trí của sản phẩm cần sắp xếp trên pallet. Với hai điểm A ( và B (
96
(5.1)
❖ Trường hợp
Trường hợp này được áp dụng cho quỹ đạo và .
Từ phương trình (5.1) có thể suy ra:
(5.2)
Chọn quỹ đạo thiết kế là đa thức hàm bậc 3 theo thời gian có dạng như sau:
(5.3)
Với thời gian tay máy đi từ A đến B là T (s) và vận tốc tại hai vị trí A và B bằng 0. Từ đó có hệ phương trình sau:
(5.4)
Suy ra:
97
Từ (5.9), (5.10) và (5.12) suy ra:
(5.6)
❖ Trường hợp
Trường hợp này được áp dụng cho quỹ đạo và .
98
5.3. Xây dựng giải thuật điều khiển
Hình 5.9 Lưu đồ giải thuật chính
Tay máy được điều khiển theo lưu đồ giải thuật chính hình 5.9. Trong đó, tay máy gồm hai chế độ điều khiển là chế độ tự động theo lưu đồ hình 5.11 và chế độ điều khiển bằng tay theo lưu đồ hình 5.15. Theo lưu đồ giải thuật chính, sau khi đã cấp nguồn cho toàn bộ tay máy, phải đưa tay máy trở về vị trí Home có tọa độ (430, 25, 230) như hình 5.10. Khi tay máy đã về vị trí Home thì việc chọn chế độ và điều khiển tay máy mới được thực hiện. Trong suốt quá trình tay máy hoạt động, nếu có bất kỳ sự cố nào xảy ra, toàn bộ tay máy sẽ dừng ngay khi nút nhấn Stop được nhấn.
Tay máy đang ở vị trí Home
Chọn chế độ tự động
Chế độ điều khiển bằng tay
Kết thúc Bắt đầu
Chế độ tự động
Đưa tay máy về vị trí Home Đ S Đ S Ngừng thao tác S Đ
99
Hình 5.10 Vị trí Home của tay máy
❖ Chế độ điều khiển tự động
Sau khi tay máy đã được đưa về vị trí Home và được lựa chọn chế độ điều khiển tự động thì tay máy sẽ được điều khiển theo lưu đồ hình 5.11. Để điều khiển tay máy theo chế độ tự động, cần xác định tọa độ 4 vị trí trong chu trình gắp và sắp xếp sản phẩm là Q1, Q2, Q3, Q4 (hình 1.5) dựa vào điều kiện làm việc thực tế của tay máy và nhập các tọa độ này vào chương trình điều khiển. Từ đó, chương trình điều khiển sẽ tính tốn tọa độ các điểm trên quỹ đạo cũng như các giá trị biến khớp tương ứng và từ các giá trị biến khớp này, card điều khiển sẽ xuất xung cho driver điều khiển động cơ.
100
101
Để mơ phỏng q trình điều khiển tay máy, trong luận văn cũng tiến hành thiết kế giao diện GUI như trong hình 5.12 cho cả hai chế độ điều khiển là chế độ tự động và chế độ điều khiển bằng tay. Riêng đối với chế độ tự động, chỉ tiến hành mô phỏng cho một lần gắp nhả sản phẩm. Cụ thể nhập các tọa độ: Q1 (460, 783, 570); Q2 (460, 783,730); Q3 (1080, -227, 200); Q4 (1080, -227, 40) trong chu trình gắp và sắp xếp sản phẩm.
Sau khi nhấn nút “Calculate”, tọa độ các điểm trong quỹ đạoQ2Q1, Q1Q2, Q2Q3, Q3Q4, Q4Q3 và Q3Q2 (được thiết kế trong mục 5.2) sẽ được tính tốn và từ bài tốn động học ngược sẽ tính được các giá trị biến khớp của các điểm trên quỹ đạo. Sau khi đã có các giá trị biến khớp này, nhấn nút “Simulink” để tiến hành mô phỏng. Kết