Mô hình yêu cầu cần có 1 cánh tay robot 6 trục và 2 băng tải dùng để di chuyển sản phẩm. Ngoài ra, yêu cầu thêm hệ thống khung đỡ để bố trí tủ điện bên dưới mô hình. Vì vậy, phần cơ khí sẽ bao gồm các phần sau:
- 1 cánh tay robot 6 trục và bệ đỡ
- 2 băng tải và khung đỡ
- Khung gắn camera và nam châm
- Tấm đỡ mô hình
- Khung đỡ mô hình và đặt tủ điện Sơ đồ bố trí
Hình 3.2 Sơ đồ bố trí mô hình Bản vẽ thiết kế và các phạm vi hoạt động
Hình 3.3 Thông số robot
Hình 3.4 Thông số robot
Hình 3.5 Thông số khung và băng tải
Ta dựa vào thông số bản vẽ của nhà sản xuất và vẽ lại trên Solidwork
Hình 3.6 Bản vẽ đế và khớp nối 1
Hình 3.7 Bản vẽ khớp nối 2 và 3
Hình 3.8 Bản vẽ khớp nối 4,5 và 6
- Kết nối các khớp lại:
Hình 3.9 Bản vẽ robot hoàn chỉnh 3.2.2 Phần điện
Phần điện của mô hình có các khối chính là: khối nguồn, khối xử lý trung tâm (PLC), khối điều khiển động cơ servo, khối giao tiếp HMI, khối giao tiếp camera.
Sơ đồ khối hệ thống
Hình 3.10 Sơ đồ khối hệ thống
Chức năng từng khối trong hệ thống như sau:
Khối PLC: là nơi nhận tín hiệu từ khối camera và khối HMI, tính toán xử lý các thuật toán được người dùng lập trình, hiển thị các giá trị điều khiển thông qua giao tiếp với khối HMI cho người dùng dễ dàng quản lý.
Khối chuyển động robot: là nơi nhận tín hiệu điều khiển từ khối xử lý trung tâm và xuất tín hiệu để điều khiển các động cơ servo được lắp trên robot và băng tải.
Khối HMI: có chức năng giao tiếp trực tiếp với khối PLC, khiến cho việc giao tiếp điều khiển với người vận hành trở nên dễ dàng hơn.
Khối thu thập, xử lý ảnh: thu thập, tính toán hình ảnh của vật và truyền dữ liệu làm dữ liệu đầu vào cho khối PLC.
Khối gắp vật: dùng để gắp hoặc hút sản phẩm. Có thể dùng tay kẹp xi- lanh hay nam châm điện, …
Lựa chọn thiết bị
Từ cơ sở lý thuyết và dựa trên kinh nghiệm điều khiển tích lúy được, nhóm chúng tôi lựa chọn các thiết bị điều khiển của hãng Mitsubishi và camera của hãng Cognex.
- Khối PLC: trạm PLC bao gồm base Q35DB để truyền nhận tín hiệu giữa các module và CPU, bộ nguồn 24V Q61P để cấp nguồn cho các mạch điều khiển, CPU xử lý tín hiệu chính Q04UDEHCPU, CPU thiết kế điều khiển chuyển động Q173DSCPU, module điều khiển tín hiệu ngõ ra số QY42P, module giao tiếp với HMI QJ71C24N-R2.
Main base Q35DB: Tính năng Loại base Số module mở rộng Hỗ trợ nguồn dự phòng Kích LxWxH 26 Nguồn điều khiển Q61P:
Tính năng
Điện áp ngõ vào Tần số
Công suất Hiệu suất
Dòng điện ngõ ra tối đa Chức năng bảo vệ Kích thước LxWxH Q04UDEHCPU: Tính năng Số I/O tích hợp sẵn Chu kỳ quét Bộ nhớ chương trình Bộ nhớ nội Số lần nạp vào RAM Số lần nạp vào SRAM Cổng giao tiếp Tốc độ truyền dữ liệu Các thiết bị nội
Chiều dài cáp kết nối Kích thước LxWxH
Q173DSCPU:
Tính năng
Số trục điều khiển tối đa
Chu kỳ vận hành Cổng giao tiếp Module tín hiệu mở rộng Thông số 32 trục 0.44ms USB, RS-232, Enthernet Q172DLX Hình ảnh QY42P: Tính năng Loại kết nối Số ngõ ra Điện áp hoạt động Dòng điện ngõ ra tối đa Độ sụt áp tối đa Thời gian đáp ứng Cầu chì bảo vệ Mức độ bảo vệ Chức năng bảo vệ Cáp kết nối Kích thước LxWxH 28
QJ71C24N-R2:
Tính năng
Số cổng giao tiếp Cổng giao tiếp
Phương thức giao tiếp
Tốc độ truyền tối đa
Chức năng phát hiện lỗi
Cáp kết nối
Kích thước LxWxH
Thông số 2 cổng RS-232
Truyền dữ liệu song song
115200 bps Có sẵn Com DB9
89.3x27.4x98mm
Hình ảnh
- Khối chuyển động robot:
Nhóm sử dụng driver J4 của Mitsubishi để điều khiển các khớp của robot.
Hình 3.11 Driver amplifier J4
- Khối thu thập, xử lý ảnh:
Lợi dụng ưu thế mạnh mẽ về các tính năng xử lý ảnh của camera Cognex, nhóm đã sử dụng một trong các sản phẩm của hãng đó là Camera In-sight Micro 1100 (ISM 1100).
Thông số cơ bản của camera:
+ Độ phân giải: 480x640 (pixel)
+ Bit Depth: 256 mức xám
+ Frame per second: tối đa 60 FPS
+ Power: Power over Ethernet (PoE)
Hình 3.12 Camera ISM-1100
- Khối gắp vật:
Để tránh gây khó khăn về thiết kế phần khí nén, và các cơ cấu tay gắp, nhóm chọn nam châm điện 24vdc để làm cơ cấu gắp vật với ưu điểm đơn giản, gọn nhẹ và thiết kế nhanh chóng.
Hình 3.13 Nam châm điện
- Khối HMI:
Chọn HMI loại cầm tay thuộc dòng GOT1000 của hãng Mitsubishi. HMI GT1155HS – QSBD có đầy đủ tính năng của như dòng GT1000, hơn nữa kích thước nhỏ gọn dạng tay cầm, tích hợp thêm nút nhấn tiếp điểm và khóa liên động, phù hợp cho các ứng dụng điều khiển máy móc từ xa ở khoảng cách an toàn.
- Kết nối mạch điều khiển:
Hinh 3.14 HMI GT1155HS-QSBD
Hình 3.15 Sơ đồ điều khiển tổng quát
3.2.3 Thi công hệ thống
Hình 3.16 Thi công phần cơ khí
Hình 3.17 Thi công phần điện
3.3 Mô phỏng động học trên Matlab- Giao diện GUI (graphical user interface) - Giao diện GUI (graphical user interface)
Hình 3.18 Giao diện GUI - Giao diện mô phỏng trên Matlab:
Hình 3.19 Mô phỏng trên Matlab
Chương 4 Mô tả bài toán
4.1 Mô tả hệ thống
Mô hình bao gồm 2 phần chính:
- Các thiết bị điều khiển: Q04UDEHCPU, Q173DS, QH42P, bộ điều khiển động cơ servo MR-J4W3-444B, MR-J4W2-22B, MR-J4W2-44B và các thiết bị khác như CB, rơ-le, bộ lọc nhiễu điện áp, nguồn 24V, … được lắp đặt trong tủ điện. Riêng camera Cognex IS1400 và nam châm được gắn trên khớp cuối của robot.
- Các động cơ HG-KR43 (trục 1), HG-KR43B (trục 2), HG-KR23B (trục 3), HF- KP23B (trục 4), HF-KP053 (trục 5), HF-KP053 (trục 6) được đặt tại các khớp xoay của robot. Động cơ HF-KP13B (trục 7) được đặt vào băng tải cùng cơ cấu chuyển động của băng tải được đặt trên mặt phẳng nhôm nằm ngang. 4.2 Mô tả hoạt động của mô hình
Mô hình làm việc dựa trên 3 chế độ chính đó là chế độ làm việc bình thường, chế độ jog mode, chế độ positioning mode.
Ở chế độ làm việc bình thường: người điều khiển nhập giá trị tốc độ cho băng tải, sau đó nhấn START CONVEYOR để băng tải hoạt động. Nhấn GOTO CAMERA để chạy đến vị trí camera có thể quét được vật trên băng tải. Tiến hành thả vật trên băng tải, hệ thống sẽ tự động gắp vật và sắp xếp qua băng tải còn lại.
Trong quá trình hoạt động, nếu người điều khiển nhấn STOP bên cạnh màn hình HMI hoạt nhấn EMC-STOP thì hệ thống sẽ dừng hoạt động. Khi hệ thống dừng, muốn hoạt động lại, nhấn RESTART hệ thống sẽ hoàn thành chu kỳ đang làm và chạy đến vị trí camera để đợi.
Ở chế độ jog mode: người điều khiển thiết lập tốc độ ban đầu cho các trục, sau đó nhấn giữ JOG+ nếu muốn động cơ quay thuận và JOG- nếu muốn động cơ quay ngược. Chế độ jog mode được thiết lập cho cả 7 trục.
Ở chế độ positioning mode: người điều khiển nhập vị trí theo tọa độ Descartes (x; y; z), sau đó nhấn GOTO XYZ khâu cuối cùng của cánh tay robot sẽ chạy đến vị trí vừa thiết lập.
Lưu ý:
-Phải giới hạn lại hành trình của từng trục vì giới hạn của cơ cấu từng khớp xoay của robot do nhà sản xuất quy định. Nếu nhập quá giới hạn của không gian làm việc, hệ thống sẽ không hoạt động và báo lỗi trên màn hình HMI.
4.3 Tính toán phương trình động học4.3.1 Tính động học thuận 4.3.1 Tính động học thuận
Xem robot là một chuỗi các khâu và các khớp nối tiếp nhau. Các khớp này có thể là khớp trượt (tịnh tiến) hay khớp trụ (xoay) và chúng có thể sắp xếp theo bất kỳ thứ tự nào và có thể nằm trong bất kỳ mặt phẳng nào. Các khâu cũng có chiều dài bất kỳ kể cả bằng
1. Vì vậy, bất kỳ một tập hợp các khâu, các khớp đều có thể tạo thành một cấu hình robot và chúng ta có thể giải quyết các vấn đề đối với nó thông qua việc mô hình hóa.
Để làm được việc này, cần phải gắn một hệ trục tham chiếu đến mỗi khớp và sau đó xác định sự chuyển vị từ khớp này đến khớp kế tiếp. Nếu kết hợp tất cả các chuyển vị từ bệ đến khớp thứ nhất, từ khớp thứ nhất đến khớp thứ hai và cứ tiếp tục cho đến khi đến khớp cuối cùng chúng ta sẽ có ma trận chuyển vị tổng cộng. Ở phần dưới đây, chúng ta sẽ xác định giải thuật cụ thể dựa vào biểu diễn D-H để gắn hệ trục tham chiếu lên mỗi khớp, sau đó xác định chuyển vị giữa hai hệ trục kế tiếp nhau. Cuối cùng sẽ có được ma trận chuyển vị của robot.
Quy tắc gắn hệ trục tham chiếu lên robot:
- Bắt đầu đánh số từ 0 đến n (với n là số khâu của robot) và bệ robot sẽ được đánh số 0.
- Hệ trục tọa độ của bệ gắn song song hoặc trùng với hệ trục tham chiếu. Gốc của hệ trục tọa độ của bệ gắn trùng với gắn trùng với gốc của khớp 1 với giả thiết là trục khớp 1 vuông góc với mặt phẳng xy.
- Hệ trục tọa độ gắn trên khâu của robot được gắn tại khớp xa hơn so với bệ của mỗi khâu. Vì vậy, hệ trục tọa độ số 1 sẽ gắn tại khớp số 2 (khớp này nối giữa khâu 1 và khâu 2).
- Gốc của hệ trục tọa độ đạt tại giao điểm của đường vuông góc chung của các trục khớp với nhau. Nếu các trục khớp song song với nhau thì vị trí của gốc hệ trục tọa độ được lựa chọn sao cho khoảng cách giữa các khâu bằng 0 hoặc nhỏ nhất nếu có một khoảng dịch chuyển giữa các khâu. Nếu các khớp giao nhau thì gốc tọa độ sẽ được đặt tại điểm giao nhau của các trục.
- Trục z đặt trùng với trục khớp. Nếu khớp tịnh tiến thì hướng trục z sẽ là hướng di chuyển đi xa khỏi khớp. Nếu là khớp xoay, hướng trục z xác định là hướng dương theo hướng quay trục z (theo người thiết kế) theo quy tắc nắm tay phải hoặc theo hướng lựa chọn sao cho góc xoắn là nhỏ nhất.
- Trục x sẽ song song với đường vuông góc chung giữa các trục khớp của khâu. Trong trường hợp các trục khớp song song với nhau, trục x sẽ trùng với đường tâm của khâu. Nếu các trục giao nhau không có đường vuông góc chung, trục x sẽ là tích có hướng của 2 vector trục z của khớp trước và trục z của khớp hiện tại.
- Trục y chúng ta sẽ xác định dựa trên quy tắc tam diện thuận sau khi đã có trục x và trục z. Biểu diễn D-H không cần thiết sử dụng trục y nên chúng ta có thể gắn hoặc không gắn trục y lên robot đối với robot có cấu trúc phức tạp.
Bảng tham số Denavit – Hartenberg (DH) tổng quát l d 1 2 . . . n Trong đó:
− : là khoảng cách giữa giao điểm trục
hay còn được gọi là khoảng cách 2 trục
vàvới gốc tọa độ hệ trục thứ i dọc theo trục và .
- : là khoảng cách từ gốc tọa độ thứ i-1 tới giao điểm của trục và dọc theo trục hay còn được gọi là khoảng cách 2 trục và .
- : góc quay từ trục tới xung quanh trục .
- : góc quay của trục tới xung quanh trụctheo quy tắc nắm tay phải.
Hình 4.1Thông số cơ cấurobot
Hình 4.2 Gán tọa độ cho các khớp
35
1 2 3 4 5 6 n -1= 0 4 Ma trận chuyển vị từ hệ trục số 0 đến hệ trục số 6 là: 0=0 .1 .2 .3 .4 .5
Tọa độ vị trí của cơ cấu gắp cuối là:
= 75 - 270
= 75 - 270
= 270 + 90
4.3.2 Tính động học nghịch
Chọn vị trí như trên hình 4.3 làm vị trí ban đầu của robot. Tại vị trí này, các giá trị góc quay của các trục là bằng 0. Quy ước chiều chuyển động của vật như hình 4.3. Nếu quay theo chiều dương, góc sẽ tăng dần từ 0.000000 đến 359.999990 và ngược lại góc sẽ giảm từ 359.999990 về 0.000000 tuân theo quy ước của bộ điều khiển Q173DSCPU khi chọn đơn vị điều khiển là “degree”.
Ta chọn cách tính động học theo phương pháp hình học và trường hợp robot hoạt động trong mặt phẳng thẳng đứng vuông góc với mặt phẳng Oxy, góc quay của trục 3 không vượt quá 900.
Hình 4.5 Hình vẽ tính động học nghịch
Hình 4.3 Khớp xoay
Hình 4.4 Độ dài cơ cấu robot
Tìm góc quay của trục 1: = 2 Tìm góc quay của trục 3: Ta có: = = = − − = − − = − − . ( )
= + − 75 − 80. (− ) = − + = − + = − + . ( ) = − 335 + 80. sin ( ) ⇒ Ta có: cos( ) = ⇒ sin( ) = ⇒ = 2 ⇒ =180 − − Tìm góc quay của trục 2: Ta có: ( ) = 2 cos ( ( )) =+ ⇒ sin( ( )) = ( )) ⇒ ( ) = 2 cos( ( )) ⇒ = 90 − ( ) − ( )sin( Tìm góc quay của trục 5:
Góc của trục 5 được chọn phụ thuộc vào yêu cầu điều khiển. Góc là góc hợp bởi khớp thứ 5 so với phương nằm ngang, nó định hướng cho cơ cấu gắp cuối.
Ta có: = − −
Tìm góc quay của trục 4:
Vì trong trường hợp robot chỉ hoạt động trong mặt phẳng vuông góc với mặt phẳng Oxy nên nếu trục 5 có góc quay khác 0, góc quay của trục 4 sẽ chọn bằng 0. Ngược lại, nếu góc quay trục 5 bằng 0, góc quay trục 4 có thể chọn giá trị bất kỳ sao cho kết hợp với góc quay trục 6 để đạt được yêu cầu thực tế được đề ra.
0 ℎ ≠ 0
=
ℎ = 0 à ∈
Tìm góc quay của trục 6:
Góc quay trục 6 được chọn bất kỳ sao cho kết hợp với góc quay trục 4 để đạt được yêu cầu thực tế được đề ra.
∈
4.4 Các giải thuật được áp dụng
4.4.1 Giải thuật chạy nội suy đồng thời 6 trục
Trong các tập lệnh hỗ trợ bên trong module Q173DSCPU, ta có thể điều khiển chạy nội suy tối đa 4 trục và có thể điều khiển lên đến 32 trục cùng bắt đầu đồng thời nhưng không thể điều khiển để chúng cùng kết thúc đồng thời. Vì vậy, dựa trên 2 chức năng chạy nội suy và chạy đồng mà module Q173DSCPU hỗ trợ, ta sẽ đưa ra hướng giải quyết cho bài toán nội suy nhiều hơn 4 trục, cụ thể ở đây ta sẽ giải quyết bài toán nội suy 6 trục.
Có 3 cách điều khiển nội suy:
- Nội suy theo vector tốc độ: dựa vào tốc độ đặt trước, bộ điều khiển sẽ tính toán tốc độ các trục còn lại tỉ lệ với quãng đường dịch chuyển của chúng, đảm bảo cho các trục bắt đầu và kết thúc đồng thời.
- Nội suy theo trục có quãng đường di chuyển lớn nhất: bộ điều khiển sẽ tính toán để tìm ra trục nào có quãng đường di chuyển lớn nhất, tính thời gian của trục có chuyển động dài nhất này để làm dữ liệu đầu vào suy ra vận tốc các trục còn lại dựa trên quãng đường di chuyển độc lập của chúng. Đảm bảo cho các trục bắt đầu và kết thúc đồng thời.
- Nội suy theo trục được chọn làm trục tham chiếu: ta sẽ chọn trước tốc độ và trục làm trục tham chiếu. Bộ điều khiển sẽ tính toán thời gian chuyển động của trục tham chiếu để làm dữ liệu đầu vào suy ra vận tốc các trục còn lại dựa trên quãng đường di chuyển độc lập của chúng. Đảm bảo cho các trục bắt đầu và kết thúc đồng thời.
Trong cả 3 trường hợp điều khiển trên, chúng ta sẽ giải quyết chúng trong tọa độ Descartes và hệ tọa độ tuyệt đối. Ta chọn chức năng nội suy 4 trục 1, 2, 3, 5 và chức năng bắt đầu đồng thời 3 chương trình chạy là 4 trục nội suy với trục 4 và trục 6.