Hình 4.3 Sơ đồ khối ban đầu sau khi xuất mơ hình 3D của tay máy sang Matlab
75
- Khối Weld: biểu diễn một khớp không bậc tự do. Hai Body đã nối với các phía khác nhau của khối Weld được khóa cứng với nhau mà không thể chuyển động tương đối.
Sau khi đã tính tốn được động học tay máy và có được sơ đồ khối ban đầu như hình 4.3, để kiểm tra bài tốn động học, bước tiếp theo cần thêm cụm khối tác động và các cụm khối hiển thị để có được sơ đồ khối như trong hình 4.4.
Hình 4.4 Sơ đồ khối được sử dụng trong q trình mơ phỏng
❖ Cụm khối tác động gồm có:
- Khối Joint Actuator: tác dụng lực hoặc mô men tạo chuyển động cho một khớp.
- Khối Derivative: tính đạo hàm của dữ liệu đầu vào.
- Khối From - Workspace: có nhiệm vụ lấy dữ liệu từ Matlab Workspace để cung cấp cho mơ hình Simulink. Các số liệu lấy vào phải có dạng được khai báo trong mục Data.
76
❖ Cụm khối hiển thị gồm có:
- Khối Joint Sensor: đo góc, vận tốc góc, gia tốc góc hoặc mô men của một khớp.
- Khối Scope: hiển thị dạng tín hiệu đo được từ khối Joint Sensor trong suốt thời gian mô phỏng.
- Khối Body Sensor: dị tìm vị trí, vận tốc hoặc gia tốc của Body.
4.3.1. Kiểm tra bài toán động học thuận
Để dễ dàng trong việc kiểm tra bài toán động học thuận và nghịch, tiến hành thiết kế giao diện GUI (Graphical User Interface) như hình 4.5.
77
Kiểm tra bài tốn động học thuận theo lưu đồ hình 4.7 a, bằng cách nhập các giá trị θ1, θ2, θ3, θ4 vào các ô Theta1, Theta2, Theta3, Theta4 tương ứng ở mục “Input Joint Angles”. Sau đó nhấn nút “Forward Calculate”. 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 như trong hình 4.6 sẽ xuất hiện, yêu cầu nhập lại các giá trị biến khớp. Nếu các giá trị θ1, θ2, θ3, θ4 đều thỏa thì các giá trị Pxk, Pyk, Pzk trong hệ phương trình (4.7) sẽ được tính tốn và hiển thị tương ứng trong ô giá trị x, y, z ở mục “End Effector Position”. Sau khi đã tính tốn xong tọa độ của khâu tác động cuối bằng bài toán động học thuận, ấn “Simulink” để tiến hành mô phỏng và kiểm tra toạ độ 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ị trong hình 4.4, chứng tỏ các phương trình động học thuận và q trình mơ phỏng động học thuận đúng. Các số liệu tính tốn động học thuận bằng Matlab thơng qua giao diện GUI như trong hình 4.5 được thể hiện trong bảng 4.2.
78
Bảng 4.2 Các số liệu tính tốn động học thuận bằng Matlab.
Trường hợp Giá trị nhập (độ) Giá trị tính tốn được (mm)
Theta1 Theta2 Theta3 Theta4 x y z
T1 0 0 0 0 1530 25 293
T2 0 90 -90 90 830 25 993
T3 30 30 -60 -60 1150,08 692,868 293
a) Kiểm tra bài toán thuận b) Kiểm tra bài tốn nghịch
79
Hình 4.8 Kết quả mô phỏng tay máy ứng với trường hợp T1
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 tố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 q 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