Hoạch định quỹ đạo của robot là xác định quy luật chuyển động các biến khớp để điều khiển chuyển động từng khớp và tổng hợp chung thành chuyển động của cả robot theo một quỹ đạo xác định.
Quỹ đạo cần thiết kế đảm bảo phải đi qua những điểm nút chính (Đơn giản nhất là điểm đầu và điểm cuối). Ngoài các điểm nút chính, thông thường còn có thêm những điểm nút trung gian (khi liên quan đến vật cản, hoặc tránh xa những điểm kì dị,…).
Quỹ đạo cần thiết kế phải tạo nên những chuyển động êm, nhẹ nhàng tránh tạo nên những rung động, va đập. Do đó, thông thường quỹ đạo cần phải đảm bảo tính liên tục về vị trí, vận tốc và cả gia tốc.
Để thiết kế quỹ đạo robot, người ta thường dùng phương pháp xấp xỉ các đa thức bậc n, các quỹ đạo thường gặp là:
- Quỹ đạo CS (Cubic Segment): Tương đương đa thức bậc 3. - Quỹ đạo LS (Linear Segment): Tương đương đa thức bậc 1.
- Quỹ đạo LSPB (Linear Segment with Parabolic Blend): Phối hợp đa thức bậc 2 với đa thức bậc 1.
- Quỹ đạo BBPB (Bang Bang Parabolic Blend): là trường hợp đặc biệt của quỹ đạo LSPB khi đoạn tuyến tính thu về bằng 0 và xuất hiện điểm uốn.
Trong nghiên cứu này, nếu áp dụng lập quĩ đạo đa thức bậc 3 [1], vận tốc và vị trí tương đối liên tục tại điểm đầu và điểm cuối nơi ta xét trong khi đó gia tốc thì không liên tục. Do đó, để cải thiện sự liên tục với gia tốc, ta sử dụng đa thức bậc 5 để biểu diễn quĩ đạo. Phương trình tổng quát bậc 5: 2 3 4 5 0 1 2 3 4 5 t P a a t a t a t a t a t (3.51) 2 3 4 1 2 2 3 3 4 4 5 5 t P a a t a t a t a t (3.52) 2 3 2 3 4 5 2 6 12 20 t P a a t a t a t (3.53) Với: +Pt: Là vị trí của vật đang xét +Pt: Là vận tốc của điểm đang xét +Pt: Là gia tốc của điểm đang xét + t0: thời điểm bắt đầu
+ tf: Thời điểm sau khi di chuyển đến điểm mong muốn Các giá trị vị trí, vận tốc và gia tốc ở biên là:
2 3 4 5 0 0 1 0 2 0 3 0 4 0 5 0 q a a t a t a t a t a t (3.54) 2 3 4 0 1 2 2 03 3 0 4 4 0 5 5 0 v a a t a t a t a t (3.55) 2 3 0 2 26 3 0 12 4 0 20 5 0 a a a t a t a t (3.56)
2 3 4 5 0 1 2 3 4 5 f f f f f f q a a t a t a t a t a t (3.57) 2 3 4 1 2 2 3 3 4 4 5 5 f f f f f v a a t a t a t a t (3.58) 2 3 2 3 4 5 2 6 12 20 f f f f a a a t a t a t (3.59) Với:
+q0là vị trí ban đầu +v0là vận tốc ban đầu +a0là gia tốc ban đầu +qf là vị trí cuối +vf là vận tốc cuối +af là gia tốc cuối
Để cho chuyển động mượt hơn ta thêm 2 điều kiện là gia tốc đầu và gia tốc cuối bằng 0. Thay vào (3.55), (3.56), (3.58), (3.59), và giải phương trình ta được:
0 5 5 6( ) f f q q a t (3.60) 0 5 4 15( ) f f q q a t (3.61) 0 3 3 10( ) f f q q a t (3.62) 0 0 f a a (3.63) 0 0 a q (3.64)
CHƯƠNG 4 : KIỂM NGHIỆM, MÔ PHỎNG ĐỘNG HỌC VÀ ROBOT SONG SONG QUATTRO KHÔNG GIAN 4.1.Kiểm nghiệm kết quả động học trên Matlab Simulink:
4.1.1.Kiểm nghiệm động học nghịch:
Chúng tôi tiến hành kiểm nghiệm kết quả động học nghịch đã tính toán trên Matlab Simulink. Ở đây chúng tôi nhập các phương trình động học nghịch đã tính được vào khổi Inverse Kinematic. Với thông số đầu vào là tọa độ điểm đầu cuối ( , , )x y z và đầu ra là tọa độ 4 góc theta của 4 cánh tay.
Hình 4.1: Vị trí các cánh tay tại điểm home
Trong trường hợp này chúng tôi nhập tọa độ vị trí điểm home ( , , )x y z = (0,0, 464.3) . Vị trí của các cánh tay được thể hiện ở Hình 4.1 , kết quả thu được của các góc 11, 12, 13, 14lần lượt là 0o
Trường hợp này chúng tôi nhập tọa độ vị trí điểm bất kì theo trục z với ( , , )x y z
= (0,0,662,5) . Vị trí của các cánh tay được thể hiện ở Hình 4.2, kết quả thu được của các góc 11, 12, 13, 14lần lượt là 60o.
Các tọa độ nêu trên chúng tôi sử dụng kích thước mô hình trên Inventor cũng như kích thước thực thế của mô hình nhằm đem lại tính chính xác về thực tế.
4.1.2.Kiểm nghiệm động học thuận:
Để kiểm nghiệm kết quả động học thuận, cũng như kết hợp kiểm tra tính chính xác của động học nghịch theo một cách khác. Ta tiến hành so sánh tín hiệu vị trí đầu vào của khối động học nghịch và tín hiệu đầu ra của khối động học thuận. Nếu kết quả là như nhau thì có thể chứng minh được kết quá phép tính động học là đúng.
Trong trường hợp này, chúng tôi cho tin hiệu đặt vào khối động học nghịch là một quỹ đạo đường tròn có tọa độ ( , )x y thay đổi theo thời gian, tọa độ z500(mm) và bán kính đường tròn 50 (mm).
Hình 4.3: Sơ đồ mô phỏng kiểm nghiệm động học
Quỹ đạo chuyển động của đầu công tác robot trong trường hợp đầu tiên được thể hiện trong hình Hình 4.4 với hệ tọa độ Oxy:
Sự biến thiên giá trị tọa độ ( , , )x y z được thể hiện trong hình Hình 4.5:
Hình 4.5: Tín hiệu x, y, z với quỹ đạo hình tròn R=50
Sự biến thiên giá trị các góc theta của 4 cánh tay để vẽ nên quỹ đạo hình tròn R=50 (mm) được thể hiện trong hình Hình 4.6 , ở đây các giá trị góc theta sẽ là đầu ra của khối động học nghịch, sau khi tính toán kết quả đó sẽ được đưa vào khối động học thuận để tính tọa độ điểm EE của robot.
Cuối cùng, để kiểm tra độ chính xác trong tính toán các phương trình động học. Chúng ta sẽ kiểm tra sai số giữa tín hiệu đầu đặt vào khối động học nghịch và tín hiệu đầu ra của khối động học thuận. Kết quả được biểu diễn trong hình Hình 4.7.
Hình 4.7: Tín hiệu sai số vị trí
Hình 4.8: Mô hình Matlab Simulink của robot
4.2.1.Khối động học nghịch:
Inverse Kinematic là khối động học nghịch. Dùng để chuyển đổi giá trị đầu vào là tọa độ đầu vào mà Robot muốn đến thành các giá trị góc 11, 12, 13, 14 của bốn cánh tay. Giá trị đầu vào ( , , )x y z được lấy từ khối quy hoạch quỹ đạo.
4.2.2.Khối Simcape Multibody
Simcape Multibody là khối dùng để mô phỏng chuyển động thực tế của các cánh tay Robot bằng mô hình Solidwork theo thời gian. Với đầu vào là các góc 11, 12, 13, 14
đã được tính toán từ khối động học nghịch.
Hình 4.10: Khối Simcape Multibody
Trong khối Simcape Multibody là các khớp và thông số mô hình robot đã được chuyển đổi từ Solidwork sang Matlab Simulink.
Hình 4.11: Mô hình robot chuyển từ Solidwork sang Matlab
4.2.3.Khối động học thuận:
Forward Kinematic là khối động học thuận. Dùng để tính toán chuyển đổi giá trị góc 11, 12, 13, 14đã tính từ khối động học nghịch sang tọa độ (x, y, z) và đưa vào khối scope để vẽ đồ thị hình dạng chuyển động.
4.2.4.Khối quy hoạch quỹ đạo:
Hình 4.13: Khối quy hoạch đường thẳng
Khối Line dùng để quy hoạch quỹ đạo điểm-điểm trên một đường thẳng. Với đầu vào là ma trận tọa độ điểm đầu vào điểm cuối, được chạy theo miền thời gian Tf. Qua đó sẽ trả ra 1 chuỗi các giá trị (Px, Py, Py).
Khối Triangle dùng để quy hoạch quỹ đạo điểm-điểm hình tam giác. Với đầu vào là ma trận tọa độ 3 đỉnh của tam giác, được chạy theo miền thời gian Tf. Qua đó sẽ trả ra 1 chuỗi các giá trị (Px, Py, Py).
Hình 4.15: Khối quy hoạch quỹ đạo hình tròn
Khối Cirle dùng để quy hoạch quỹ đạo hình tròn. Với đầu vào là tọa độ tâm hình tròn (x0, y0, z0) và bán kính hình tròn R, được chạy theo miền thời gian Tf. Qua đó sẽ trả ra 1 chuỗi các giá trị (Px, Py, Py).
4.2.5.Kết quả mô phỏng chuyển động của robot trên Matlab Simulink:
Chạy quỹ đạo đường thẳng từ điểm ( 40, 40,500) đến điểm (50,50,500) trong thời gian 10 giây.
Hình 4.17: Animation chuyển động đường thẳng của robot
Hình 4.21: Tín hiệu ngõ vào, ra của vị trí quy hoạch theo đường thẳng
Chạy quỹ đạo hình tam giác với tọa độ 3 đỉnh lần lượt là ( 100, 100,500) , (100,100,500) , (0,0,600) trong thời gian 12 giây.
Hình 4.23: Animation chuyển động tam giác của robot
:
Hình 4.27: Tín hiệu ngõ vào, ra của vị trí quy hoạch theo hình tam giác
Chạy quỹ đạo tròn với tọa độ tâm (0,0,500) với bán kính R=150 (mm) với tần số f=0.2 (Hz)
Hình 4.29: Tín hiệu ngõ vào giá trị góc theta quy hoạch theo hình tròn
Hình 4.31: Tín hiệu sai số góc theta quy hoạch theo hình tròn
Hình 4.32: Tín hiệu ngõ vào, ra của vị trí quy hoạch theo hình tròn
CHƯƠNG 5 : THIẾT KẾ VÀ THI CÔNG MÔ HÌNH 5.1. Chế tạo mô hình Robot Quattro 4 bậc tự do:
Sau khi nghiên cứu và lên ý tưởng, nhóm chúng em đã chế tạo mô hình Robot Quattro như hình. Trong đó phần khung chịu lực của robot được làm nhôm định hình Tấm phủ phía trên và chịu lực được làm thép C45. Phần đỡ động cơ (Top base) được mica nhằm giảm khối lượng cho phần tấm phủ, nhưng vẫn đảm bảo độ chắc chắn cho robot. Phần khớp 1 được làm bằng kim loại nhôm. Khớp 2 được làm bằng thanh cacbon và kết nối với khớp 1 và điểm đầu cuối nhờ các mắt trâu. Điểm đầu cuối (EE) được gia công bằng nhôm và được gắn nam châm điện để thực hiện nhiệm vụ gắp và thả vật.
5.1.1.Phần khung robot:
Như đã đề cập robot song song có kết cấu vòng kín, các khớp, động cơ, điểm đầu cuối chịu ràng buộc lẫn nhau nên khi robot vận hành sẽ xuất hiện rung lắc. Vì vậy phần khung robot phải đảm bảo sự chắc chắn, nên nhóm đã sử dụng nhôm định hình 30x30 mm cùng các tấm kê góc và kê góc vuông nhôm. Vừa đảm bảo độ chắc chắn vừa đem lại sự thẩm mĩ cho robot.
Hình 5.2: Mô hình 3D khung robot trên Inventor
Kích thước:
- Chiều dài khung: 660 mm - Chiều rộng khung: 660 mm
- Chiều cao khung: 760 mm
Hình 5.3: Tấm kê góc
5.1.2.Tấm đế trên robot:
Là chi tiết dùng để kết nối phần khung của robot với phần đỡ động cơ. Do phải chịu tải lớn lên được làm bằng thép C45 để đảm bảo độ bền cũng như độ cứng vững.
Kích thước:
- Chiều dài: 660 mm - Chiều rộng: 660 mm - Chiều dày: 5 mm
5.1.3.Bệ đỡ động cơ:
Đúng với tên gọi, dùng để liên kết các động cơ với tấm đế trên. Được gia công bằng các tấm mica lắp ghép với nhau một cách đồng đều, nhằm làm giảm khối lượng của robot mà vẫn giữ được độ vững chắc của cơ cấu. Chi tiết này được nhắc đến ở chương 3 với tên gọi: “Bàn máy cố định tâm O”, kích thước của chi tiết này rất quan trọng và phải được gia công chính xác do ảnh hưởng trực tiếp đến tính toán kết quả động học của robot. Một sự sai lệch nhỏ vài mm cũng ảnh hưởng rất lớn đến hoạt động của robot.
Hình 5.5: Mô hình 3D bệ đỡ động cơ trên Inventor
5.1.4.Khớp 1 (khớp chủ động):
Được gia công phay bằng vật liệu nhôm nhằm giảm bớt khối lượng và được tối ưu về mặt kích thước. Điều này sẽ làm tăng tính linh hoạt của robot khi hoạt động cũng
như đáp ứng được vùng làm việc của robot. Đây là “cánh tay” của robot, được kết nối với trục động cơ bằng mặt bích.
Hình 5.6: Mô hình 3D trục 1 trên Inventor
5.1.5.Khớp 2 (khớp bị động):
Được làm bằng thanh cacbon rỗng với chiều dài. Hai đầu được gắn với chi tiết mắt trâu nhằm thực hiện chuyển động “xoay và kéo theo” của khớp thứ nhất. Cứ 2 thanh cacbon ở khớp bị động này sẽ được gắn với khớp chủ động tạo nên cơ cấu hình bình hành thông qua cơ cấu nối khớp.
Hình 5.8: Mô hình 3D khớp 2 trên Inventor Hình 5.9: Mô hình 3D cơ cấu nối khớp
5.1.6.Đầu công tác – Đế dưới robot (EE):
Được nhắc đến ở chương 3 với tên gọi “Bàn máy động tâm A”, nối với 4 cặp thanh cacbon của khớp bị động bằng mắt trâu, tổng hợp chuyển động của các thanh này thành một chuyển động duy nhất. Được gia công phay bằng nhôm và được gắn module nam châm điện ở tâm để thực hiện ứng dụng gắp thả vật.
Hình 5.10: Mô hình 3D đế dưới robot trên Inventor
5.2.Thông số kỹ thuật của Robot:
Bảng 5.1: Bảng thông số chung của robot
Dài [mm] Kích thước tổng thể: Rộng [mm] Cao [mm] 660 660 765 Khối lượng tổng thế [kg] 35 Kích thước khớp 1 [mm] 181 Khối lượng khớp 1 [kg] 0.125 Kích thước khớp 2 [mm] 498 Khối lượng khớp 2 [kg] 0.093 Dài [mm]
Kích thước tủ điện: Rộng [mm] Cao [mm]
600 300 800
5.3.Lựa chọn thiết bị:
Xét theo yêu cầu cơ cấu đặt ra Kết hợp tra cứu catalogue của các nhà sản xuất đề xuất chúng tôi chọn một số loại thiết bị dưới đây:
5.3.1.Động cơ AC Servo Mitsubishi HC-KFS13:
Hình 5.11: Động cơ HC-HFS13
Bảng 5.2: Thông số kĩ thuật của động cơ HC-HFS13
Thông số Giá trị Đơn vị
Tốc độ định mức 3000 rpm
Tôc độ tối đa 4500 rpm
Dòng định mức 0.71 A
Dòng tối đa 22 A
Điện áp 220 V
Công suất 100 W
Mô-men xoắn cực đại 0.95 N.m
Hình 5.12: Thông só kích thước động cơ HC-HFS13
5.3.2.Hộp giảm tốc KSBL 44-10-P1:
Ở mỗi động cơ được lắp thêm một hộp giảm tốc với tỉ số truyền 1/10 để tăng momen chịu tải.
Bảng 5.3: Thông số kĩ thuật của hộp giảm tốc
Thông số Giá trị Đơn vị
Tốc độ đầu vào định mức
3000 rpm
Tốc độ đầu vào max 8000 rpm
Mô men xoắn đầu ra định mức
14 Nm
Mô men cực đại 42 Nm
Mô men quán tính 0.09 Kg.cm2
Độ cứng xoắn 3 Nm/arcmin
Tải xuyên tâm tối đa 760 N
Tải dọc trục 380 N
Hình 5.14: Thông số kích thước hộp giảm tốc
5.3.3.Driver Mitsubishi MR-J2S-10A:
Để điều khiển xuất xung cho 4 động cơ Mitsubishi HC-KFS13 của robot, nhóm sử dụng 4 bộ driver cùng thương hiệu.
Hình 5.15: Driver Mitsubishi MR-J2S-10A Bảng 5.4: Thông số kĩ thuật MR-J2S-10A
Thông số Giá trị Đơn vị
Khối lượng 0.7 Kg
Điện áp 3 pha (200-230V)
1 pha (230V) VAC
Dòng điện 10 A
Tần số 50/60 Hz
Biến thiên điện áp cho phép
3 pha cho phép: 170-253
1 pha cho phép: 207-253 VAC
Biến thiên tần số ±5 %
Công suất (Động cơ sử
dụng trong đồ án) 100 W
Đầu vào điện áp Torque
(Chế độ Torque) 0 đến ±8 VDC
Tốc độ giới hạn (Chế độ Torque)
0 đến ±10
VDC
Hình 5.16: Thông só kích thước MR-J2S-10A
5.3.4.Arduino Nano V3.0 ATmega328P:
Board Arduino Nano là một trong những phiên bản nhỏ gọn của board Arduino. Arduino Nano có đầy đủ các chức năng và chương trình có trên Arduino Uno do cùng sử dụng MCU ATmega328P. Nhờ việc sử dụng IC dán của ATmega328P thay vì IC chân cắm nên Arduino Nano có thêm 2 chân Analog so với Arduino Uno.
Hình 5.17: Arduino Nano V3.0
Bảng 5.5: Thông số kĩ thuật Adruino nano v3.0
Thông số Giá trị Đơn vị
Vi điều khiển ATmega328
Điện áp hoạt động 5 V
Chân Digital I/O 14
Chân PWM Digital I/O 6
Chân đầu vào Analog 8