ĐỒ ÁN MÔN HỌC Đề tài ĐIỀU KHIỂN CÁNH TAY ROBOT

TRƯỜNG ĐẠI HỌC GIAO THƠNG VẬN TẢI TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ VIỄN THÔNG ĐỒ ÁN MÔN HỌC Đề tài: ĐIỀU KHIỂN CÁNH TAY ROBOT Ngành: Kỹ thuật điều khiển tự động hóa Chuyên ngành: Tự động hóa cơng nghiệp Giảng viên hướng dẫn : Th.s Trương Ngọc Bảo Nhóm sinh viên thực : Họ tên MSSV Lớp Trần Đình Cung 1751050004 TD17A Phạm Anh Kiệt 1751050016 TD17A Nguyễn Tấn Lập 1751050024 TD17A Nguyễn Văn Phương 1751050036 TD17A Nguyễn Thanh Quy 1551030318 TD17A Lê Sỹ Thức 1751050046 TD17A TP Hồ Chí Minh, 2021 NHẬN XÉT CỦA GIẢNG VIÊN MỤC LỤC MỤC LỤC HÌNH VÀ BẢNG iii Mục lục hình iii Mục lục bảng iv CHƯƠNG TỔNG QUÁT VỀ ROBOT 1.1 Sơ lược phát triển Robot 1.2 Những ứng dụng điển hình Robot 1.3 Phạm vi đề tài 1.4 Nội dung nghiên cứu CHƯƠNG CƠ SỞ LÝ THUYẾT 2.1 Khái niệm ban đầu 2.1.1 Hệ tọa độ 2.1.2 Bậc tự robot (DOF: Degree Of Freedom) 2.1.3 Bài toán động học thuận 2.1.4 Bài toán động học ngược 2.2 Phương trình động học robot 2.2.1 Dẫn nhập 2.2.2 Bộ thông số Denavit – Hartenberg (DH) 2.2.3 Ma trận A khâu 2.2.4 Xác định ma trận biến đổi 10 2.3 Thiết lập hệ phương trình động học robot 11 2.3.1 Trình tự thiết lập hệ phương trình 11 2.3.2 Phương trình động học cánh tay robot đồ án 11 2.3.3 Bài toán động học thuận 15 2.4 Phương trình động học ngược 16 2.4.1 Các điều kiện toán động học ngược 16 2.4.2 Bài toán động học ngược 17 CHƯƠNG NỘI DUNG NGHIÊN CỨU 20 3.1 Giới thiệu phần mềm phần cứng 20 3.1.1 Cánh tay robot EEZY 20 3.1.2 Động servo SG90 20 3.1.3 Mạch Arduino UNO R3 21 3.1.4 Board mở rộng Arduino Nano 22 i 3.1.5 Dây kết nối 23 3.1.6 Mơ hình hồn chỉnh 23 3.2 Giới thiệu phần mềm 24 3.2.1 Phần mềm Matlab 24 3.2.2 Các tính Matlab 24 3.3 Thi công phần cứng 25 3.3.1 Sơ đồ đấu nối thực tế 25 3.3.2 Phần cứng thực tế 26 3.3.3 Giới hạn phần cứng 26 3.4 Thiết kế phần mềm 26 3.4.1 Lưu đồ giải thuật 26 3.4.2 Hàm giải toán động học thuận 27 3.4.3 Hàm giải toán động học ngược 28 3.4.4 Kết nối Matlab với Arduino 28 3.4.5 Điều khiển động 29 3.4.6 Thiết kế giao diện 31 CHƯƠNG KẾT QUẢ NGHIÊN CỨU 33 4.1 Kết đồ án 33 4.2 Hạn chế đồ án 33 TÀI LIỆU THAM KHẢO 34 ii MỤC LỤC HÌNH VÀ BẢNG Mục lục hình Hình 2.1 Các tọa độ suy rộng robot Hình 2.2 Quy tắc bàn tay phải Hình 2.3 Các vector định vị trí hướng bàn tay máy Hình 2.4 Chiều dài góc xoắn khâu Hình 2.5 Các thơng số khâu θ,d,a,α Hình 2.6 Vật thể robot 10 Hình 2.7 Tốn đồ vị trí robot 10 Hình 2.8 Mơ hình 3D cánh tay robot 12 Hình 2.9 Mơ hình đơn giản đặt hệ trục khâu robot 12 Hình 2.10 Quy ước chiều dương tọa độ khâu chấp hành cuối 15 Hình 2.11 Khoảng cách điểm cuối tính tốn thực tế 16 Hình 2.12 Góc quay khâu 17 Hình 3.1 Cánh tay Robot 20 Hình 3.2 Động servo SG90 20 Hình 3.3 Nguyên lý hoạt động động servo SG90 21 Hình 3.4 Mạch Arduino UNO R3 22 Hình 3.5 Board mở rộng Arduino Nano 22 Hình 3.6 Mặt sau Shield Mở Rộng Arduino NaNo 23 Hình 3.7 Dây kết nối 23 Hình 3.8 Mơ hình hồn chỉnh 23 Hình 3.9 Icon Matlab 24 Hình 3.10 Giao diện làm việc Matlab Simulink 25 Hình 3.11 Sơ đồ đấu nối thực tế 25 Hình 3.12 Mơ hình phần cứng thực tế 26 Hình 3.13 Lưu đồ giải thuật phần mềm 27 Hình 3.14 Góc quay trục động 29 Hình 3.15 So sánh trục động khâu hệ trục khâu ban đầu 30 Hình 3.16 So sánh trục động khâu hệ trục khâu ban đầu 30 Hình 3.17 So sánh trục động khâu hệ trục khâu ban đầu 31 Hình 3.18 Giao diện phần mềm điều khiển 32 iii Hình 3.19 Trường hợp đặc biệt động học thuận 33 Mục lục bảng Bảng 2.1 Bảng DH robot 12 Bảng 3.1 Code hàm giải toán động học thuận 27 Bảng 3.2 Code hàm giải toán động học ngược 28 Bảng 3.3 Code kết nối Arduino với Matlab 28 Bảng 3.4 Code điều khiển động theo góc mong muốn 31 iv ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo CHƯƠNG TỔNG QUÁT VỀ ROBOT 1.1 Sơ lược phát triển Robot Nhu cầu nâng cao suất chất lượng sản phẩm ngày đòi hỏi ứng dụng rộng rãi phương tện tự động hóa sản xuất Xu hướng tạo dây chuyền thiết bị tự động có tính linh hoạt cao hình thành Các thiết bị thay dần máy tự động ‘cứng’ đáp ứng việc định thị trường ln địi hỏi thay đổi mặt hàng chủng loại, kích cỡ tính năng… Vì ngày tăng nhanh nhu cầu ứng dụng robot để tạo hệ thống sản xuất tự động linh hoạt Thuật ngữ ‘robot’ lần xuất năm 1922 tác phẩm ‘Rossum’s Universal Robot’ Karel Capek Theo tiếng Séc robot người làm tạp dịch Trong tác phẩm nhân vật Rossum trai ông tạo máy gần giống người để hầu hạ người Hơn 20 năm sau, ước mơ viễn tưởng Karel Capek bắt đầu thực Ngay sau chiến tranh giới thứ 2, Hoa Kỳ xuất tay máy chép hình điều khiển từ xa phịng thí nghiệm vật liệu phóng xạ Vào cuối năm 50, bên cạnh máy chép hình khí đó, xuất loại máy chép hình thủy lực điện từ, tay máy Minitaur I tay máy Handyman Genaral Electric Năm 1954 George C.Devol thiết kế thiết vị có tên ‘cơ cấu lề’ dùng để chuyển hàng theo chương trình Đến năm 1956 Devol cung với Goseph F Engelber, kỹ sư trẻ công nghiệp hàng không, tạo loại robot công nghiệp năm 1559 công ty Unimation Chi đến năm 1975 Công ty Unimation bắt đầu có lợi nhuận từ sản phẩm robot đầu tiền Chiếc robot công nghiệp đưa vào ứng dụng đầu tiên, năm 1961, nhà máy ô tô General Motors Trenton, New Jersey Hoa Kỳ Năm 1967, Nhật Bản nhập robot công nghiệp từ Công ty AMF Đến năm 1990 có 40 Cơng ty Nhật Bản, có Cơng ty khổng lồ Cơng ty Hatachi Công ty Mitsubishi, đưa thị trường quốc tế nhiều loại robot tiếng Từ năm 70 việc nghiên cứu nâng cao tính robot chi nhiều đến lắp đặt thêm cảm biến ngoại tín hiệu để nhận biết mơi trường làm việc Tại trường đại học tổng hợp Ford người ta tạo loại robor lắp ráp tự động điều khiển máy tính sỡ xử lý thơng tin từ cảm biển lực thị giác Vào thời gian Công ty IBM chế tạo loại robot có loại cảm biến xúc giác cảm biến lực, điều khiển máy tính để lắp ráp máy in gồm 20 cụm chi tiết Nhóm trưởng: Nguyễn Thanh Quy ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo Vào giai đoạn nhiều nước khác tiến hành cơng trình nghiên cứu tương tự, tọa loại robot khiển máy vi tính, có lắp đặt thiết bị cảm biến thiết bị ngoại tiếp người – máy Một lĩnh vực mà nhiều phịng thí nghiệm quan tâm việc chế tọa robot tự hành Các cơng trình nghiên cứu tọa robot tự hành theo hướng bắt chước chân người súc vật Các robot cịn chưa có nhiều ứng dụng công nghiệp nhiên loại xe robot (robocar) lại nhanh chóng đưa vào hoạt động hệ thống sản xuất tự động linh hoạt Từ năm 80, vào năm 90, áp dụng rộng rãi tiến kỹ thuật vi xử lý công nghệ thông tin, số lượng robot công nghiệp gia tăng, giá thành giảm rõ rệt, tính có nhiều bước tiến vượt bậc Nhờ robot cơng nghiệp có vị trí quan trọng dây chuyền sản xuất đại Ngày chuyên ngành khoa học robot ‘robotics’ trở thành lĩnh vực rộng khoa học, bao gồm vấn đề cấu trúc động học, lập trình quỹ đạo, cảm biến tín hiệu, điều khiển chuyển động,… Robot công nghiệp hiểu thiết bị tự động linh hoạt, bắt chước chức lao động cơng nghiệp người Nói đến thiết bị tự động linh hoạt nói đến khả thao tác với nhiều bậc tự do, điều khiển trợ động lập trình thay đổi Cịn nói đến bắt chước chức lao động cơng nghiệp người có ý nói đến không hạn chế từ chức lao động chân tay đơn giản đến trí khơn nhân tạo, tùy theo loại hình cơng việc lao động cần đến chức hay khơng Đồng thời nói đến mức độ cần thiết bắt chước người hay khơng 1.2 Những ứng dụng điển hình Robot  Ứng dụng công nghiệp  Lắp đặt vật liệu, hàn điểm phun sơn  Phục vụ máy công cụ, làm khuôn công nghiệp đồ nhựa,…  Ứng dụng phòng thi nghiệm  Dùng để thực công việc thủ công, thực công việc lặp lặp lại  Ứng dụng robo thao tác cần khuyếch đại lực  Dùng khu vực nguy hiểm (nhiễm xạ,….) Nhóm trưởng: Nguyễn Thanh Quy ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo  Dùng bốc dỡ hàng hóa, vật liệu, phơ có trọng lượng lớn cồng kềnh ngành công nghiệp nặng…  Ứng dụng robot nông nghiệp  Robot cát long cừu, mổ xẻ thịt heo,…  Ứng dụng không gian  Tay máy chế tạo nhằm tăng cường khả bốc dỡ hàng hóa tiếp tế, lắp ghép với trạm không gian  Các xe tự hành trang thiết bị tay máy linh hoạt, robot bảo trì vệ tính, xây dựng khơng gian…  Ứng dụng tàu lặn  Phát triển tàu lặn khơng người lái cơng tác kiểm tra, dị tìm thám hiểm đại dương  Ứng dụng giáo dục  Robot sử dụng phương tiện giảng dạy chương trình giáo dục  Robot kết hợp sử dụng với ngôn ngữ LOGO để giảng dạy nhận thức máy tính 1.3 Phạm vi đề tài Đối tượng nghiên cứu đề tài mơ hình cánh tay robot bậc tự sử dụng mạch Arduino để điều khiển Nghiên cứu tổng quan cấu tạo nguyên lý hoạt động cánh tay robot mạch điều khiển Vận dụng kiến thức để vận hành lập trình điều khiển cánh tay robot 1.4 Nội dung nghiên cứu  Nghiên cứu sở khoa học thực tiễn chế điều khiển robot  Nghiên cứu phương trình động học người robot  Nghiên cứu ứng dụng phần gia cơng khí để chế tạo cánh tay robot  Nghiên cứu ứng dụng mạch Arduino để điều khiển cánh tay robot Nhóm trưởng: Nguyễn Thanh Quy ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo CHƯƠNG CƠ SỞ LÝ THUYẾT 2.1 Khái niệm ban đầu 2.1.1 Hệ tọa độ Mỗi robot thường bao gồm nhiều khâu (links) liên kết với qua khớp (joints), tạo thành xích động học xuất phát từ khâu (base) đứng yên  Hệ tọa độ gắn với khâu gọi hệ tọa độ (hay hệ tọa độ chuẩn)  Hệ tọa độ suy rộng hệ tọa độ trung gian khác gắn với khâu động Trong thời điểm hoạt động, tọa độ suy rộng xác định cấu hình robot chuyển dịch dài chuyển dịch góc khớp quay (hình 2.1) Các tọa độ suy rộng cịn gọi biến khớp Hình 2.1 Các tọa độ suy rộng robot Xác định hệ tọa độ gắn với khâu robot phương pháp bàn tay phải:  Dùng tay phải, nắm hai ngón tay út áp út vịng lịng bàn tay, xịe ngón: cái, trỏ theo phương vng góc nhau, chọn ngón phương chiều trục z, ngón trỏ phương, chiều trục x ngón biểu thị phương, chiều trục y (hình 2.2) Hình 2.2 Quy tắc bàn tay phải Nhóm trưởng: Nguyễn Thanh Quy ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo CHƯƠNG NỘI DUNG NGHIÊN CỨU 3.1 Giới thiệu phần mềm phần cứng 3.1.1 Cánh tay robot EEZY Hình 3.1 Cánh tay Robot Khung cánh tay robot EEZY sau lắp ráp hoàn chỉnh (chưa có điều khiển servo) Bộ khung in công nghệ in 3D sử dụng sợi nhựa PLA Bộ cánh tay robot điều khiển servo thông qua Arduino Dùng để chế tạo mơ hình cánh tay robot mô cánh tay công nghiệp 3.1.2 Động servo SG90 Hình 3.2 Động servo SG90 Nhóm trưởng: Nguyễn Thanh Quy 20 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo Cơng dụng động servo đạt góc quay xác khoảng từ 90 độ đến 180 độ Việc điều khiển ứng dụng để lái robot, di chuyển tay máy lên xuống, quay cảm biến để quét khắp phịng… Thơng số kỹ thuật: Điện áp hoạt động: 4.8V(~5V) Khối lượng : 9g Kích thước: 22.2x11.8.32 mm Momen xoắn: 1.8kg/cm Nhiệt độ hoạt động: ºC – 55 ºC Tốc độ hoạt động: 60 độ 0.1 giây Kết nối dây màu đỏ với 5V, dây màu nâu với mass, dây màu cam với chân phát xung vi điều khiển Ở chân xung cấp xung từ 1ms-2ms theo để điều khiển góc quay theo ý muốn Hình 3.3 Nguyên lý hoạt động động servo SG90 3.1.3 Mạch Arduino UNO R3 Arduino Uno bảng mạch vi điều khiển nguồn mở dựa vi điều khiển Microchip ATmega328 phát triển Arduino.cc Bảng mạch trang bị chân đầu vào/ đầu Digital Analog giao tiếp với bảng mạch mở rộng khác Nhóm trưởng: Nguyễn Thanh Quy 21 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo Hình 3.4 Mạch Arduino UNO R3 3.1.4 Board mở rộng Arduino Nano Hình 3.5 Board mở rộng Arduino Nano Board mở rộng Arduino Nano giải pháp mở rộng cho hạn chế phần cứng Arduino Nano Pro mini, cách đưa tất chân IO đồng thời thêm nhiều chân nguồn cấp ra, giúp cho Arduino Nano giao tiếp với module bên dễ dàng Ngoài Arduino Nano Shield dùng cho board Uno R3 Nhằm giúp cho việc kết nối dễ dàng board Arduino Nano với board ngoại vi khác Board mở rộng tương thích với tất version Arduino Nano Board mở rộng chân sau:  chân nguồn cho Servo  14 chân I/O ( loại cắm hỗ trợ servo VCC GND)  3.3V Output Nhóm trưởng: Nguyễn Thanh Quy 22 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo  chân i2C  chân PWM  chân Analog kèm VCC GND  AREF output Hình 3.6 Mặt sau Shield Mở Rộng Arduino NaNo 3.1.5 Dây kết nối Hình 3.7 Dây kết nối 3.1.6 Mơ hình hồn chỉnh Hình 3.8 Mơ hình hồn chỉnh Nhóm trưởng: Nguyễn Thanh Quy 23 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo 3.2 Giới thiệu phần mềm 3.2.1 Phần mềm Matlab MATLAB phần mềm cung cấp mơi trường tính tốn số lập trình, cơng ty MathWorks thiết kế MATLAB cho phép tính tốn số với ma trận, vẽ đồ thị hàm số hay biểu đồ thông tin, thực thuật toán, tạo giao diện người dùng liên kết với chương trình máy tính viết nhiều ngơn ngữ lập trình khác Với thư viện Toolbox, MATLAB cho phép mơ tính tốn, thực nghiệm nhiều mơ hình thực tế kỹ thuật Hình 3.9 Icon Matlab 3.2.2 Các tính Matlab Matlab ngơn ngữ lập trình cao cấp, cho phép tính tốn số phát triển ứng dụng Cung cấp môi trường tương tác để khảo sát, thiết kế giải vấn đề Cung cấp thư viện lớn hàm tốn học cho đại số tuyến tính, thống kê, phân tích Fourier, lọc, tối ưu hóa, tích phân giải phương trình vi phân bình thường Matlab cung cấp đồ thị tích hợp sẵn để hiển thị hình ảnh liệu cơng cụ để tạo đồ thị tùy chỉnh Giao diện lập trình Matlab cung cấp công cụ phát triển để nâng cao khả bảo trì chất lượng mã tối đa hóa hiệu suất Cung cấp cơng cụ để xây dựng ứng dụng với giao diện đồ họa tùy chỉnh Cung cấp hàm để tích hợp thuật toán dựa Matlab với ứng dụng bên ngồi ngơn ngữ khác C, Java, NET Microsoft Excel Matlab sử dụng cơng cụ tính tốn lĩnh vực khoa học kỹ thuật, bao gồm lĩnh vực vật lý, hóa học, tốn học cơng nghệ Matlab sử dụng hầu hết việc:  Xử lý tín hiệu truyền thơng  Xử lý hình ảnh video  Hệ thống điều khiển Nhóm trưởng: Nguyễn Thanh Quy 24 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo  Kiểm tra đo lường  Tính tốn tài  Tính tốn sinh học Hình 3.10 Giao diện làm việc Matlab Simulink 3.3 Thi công phần cứng 3.3.1 Sơ đồ đấu nối thực tế Hình 3.11 Sơ đồ đấu nối thực tế Nhóm trưởng: Nguyễn Thanh Quy 25 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo 3.3.2 Phần cứng thực tế Hình 3.12 Mơ hình phần cứng thực tế 3.3.3 Giới hạn phần cứng Điều khiển thử đo đạc từ robot thực tế, nhóm định giới hạn lại góc điều khiển robot trước thiết kế phần mềm sau: 90o  1  90o  o o 50    130  o o 120  3  70 3.4 Thiết kế phần mềm 3.4.1 Lưu đồ giải thuật Phần mềm hoạt động theo lưu đồ sau: Nhóm trưởng: Nguyễn Thanh Quy 26 ĐAMH: Robot cơng nghiệp GVHD: Th.s Trương Ngọc Bảo Hình 3.13 Lưu đồ giải thuật phần mềm 3.4.2 Hàm giải tốn động học thuận Từ cơng thức (2.7), ta lập trình hàm giải tốn động học thuận sau: Bảng 3.1 Code hàm giải toán động học thuận ForwardKinematic.m function [px, py, pz] = ForwardKinematic(theta1, theta2, theta3) global d1 d2 d3 d4 d5; d1 = 70; d2 = 80; d3 = 80; d4 = 20; d5 = 60; % mm theta1 = theta1 * pi / 180; theta2 = theta2 * pi / 180; theta3 = theta3 * pi / 180; px = round(d3 * cos(theta1) * cos(theta2 + theta3) + d2 * cos(theta1) * cos(theta2), 0) + d5; py = round(d3 * sin(theta1) * cos(theta2 + theta3) + d2 * sin(theta1) * cos(theta2), 0); pz = round(d3 * sin(theta2 + theta3) + d2 * sin(theta2) + d1, 0) - d4; end Nhóm trưởng: Nguyễn Thanh Quy 27 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo 3.4.3 Hàm giải tốn động học ngược Từ cơng thức (2.8), ta lập trình hàm giải tốn động học nguợc sau: Bảng 3.2 Code hàm giải toán động học ngược InverseKinematic.m function [theta1, theta2, theta3] = InverseKinematic(px, py, pz) global d1 d2 d3 d4 d5; d1 = 70; d2 = 80; d3 = 80; d4 = 20; d5 = 60; % mm px = px - d5; pz = pz + d4; theta1 = atan2(py, px); theta3 = -acos((px^2 + py^2 + (pz - d1)^2 - d2^2 d3^2) / (2 * d2 * d3)); theta2 = atan2(pz - d1, px * cos(theta1) + py * sin(theta1)) - atan2(d3 * sin(theta3), d3 * cos(theta3) + d2); theta1 = round(theta1 * 180 / pi, 0); theta2 = round(theta2 * 180 / pi, 0); theta3 = round(theta3 * 180 / pi, 0); end 3.4.4 Kết nối Matlab với Arduino Phần mềm Matlab điều khiển động servo trực tiếp thông qua cổng Serial Port Arduino mà khơng cần phải lập trình Arduino Cho nên để điều khiển được, ta phải thực khai báo phần cứng Arduino để Matlab hiểu, ta sử dụng thư viện ‘Servo’ package ‘Arduino’ mà Matlab cung cấp Bảng 3.3 Code kết nối Arduino với Matlab Kết nối Arduino với Matlab a = arduino('COM8', 'Uno', 'Libraries', 'Servo'); Nhóm trưởng: Nguyễn Thanh Quy 28 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo dc1 = servo(a, 'D4', 'MinPulseDuration', 500*10^-6, 'MaxPulseDuration', 2500*10^-6); dc2 = servo(a, 'D5', 'MinPulseDuration', 500*10^-6, 'MaxPulseDuration', 2500*10^-6); dc3 = servo(a, 'D6', 'MinPulseDuration', 500*10^-6, 'MaxPulseDuration', 2500*10^-6); dc4 = servo(a, 'D7', 'MinPulseDuration', 500*10^-6, 'MaxPulseDuration', 2500*10^-6); Trong đó, MinPulseDuration MaxPulseDuration độ rộng xung nhỏ lớn để cấp cho động tương ứng với góc quay độ 180 độ Tuy nhiên, thử nghiệm nhóm nhận sai số góc quay lớn Thông số mặc định 1000 s 2000  s , nhà sản xuất tn theo thơng số Vì vậy, dựa viết Arduino.vn mathworks.com, nhóm sử dụng thơng số 500  s 2500  s nhận kết xác Bây sai số cịn phụ thuộc vào độ chắn phần cứng 3.4.5 Điều khiển động Động servo MG90 có góc quay từ đến 180 độ tương ứng hình sau: Hình 3.14 Góc quay trục động Giả sử ta đặt trục động trạng thái ban đầu 90 độ, góc quay tăng, trục động quay ngược chiều kim đồng hồ ngược lại Khi ứng dụng vào cánh tay robot, ta phải xét trục động trục z khâu trạng thái ban đầu khác Nhóm trưởng: Nguyễn Thanh Quy 29 ĐAMH: Robot cơng nghiệp GVHD: Th.s Trương Ngọc Bảo Trục động thứ đặt trùng với trục z khâu góc ban đầu chúng trùng (90 độ trục động với độ trục z) Vì thế, ta muốn xoay khâu robot góc 1 , ta cần cộng thêm 90 độ để chuyển từ giới hạn  90o ;90o  thành  0o ; 180o  truyền góc cho động Hình 3.15 So sánh trục động khâu hệ trục khâu ban đầu Trục động thứ hai đặt ngược so với trục z khâu góc ban đầu chúng lại trùng (90 độ trục động với 90 độ trục z) Nhận thấy rằng, ta muốn xoay khâu robot góc  , ta cần truyền góc bù với  cho động Hình 3.16 So sánh trục động khâu hệ trục khâu ban đầu Nhóm trưởng: Nguyễn Thanh Quy 30 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo Trục động thứ ba đặt trùng so với trục z khâu 2, cịn góc ban đầu chúng đối ngược nhờ thơng qua cấu truyền động song song, thực chất góc ban đầu chúng trùng (90 độ trục động với -90 độ trục z) Vì vậy, ta muốn xoay khâu robot góc 3 , ta cần cộng thêm 180 độ để chuyển từ giới hạn  180 o ;0 o  thành  0o ; 180o  truyền góc cho động Hình 3.17 So sánh trục động khâu hệ trục khâu ban đầu Vậy ta lập trình điều khiển động từ góc mong muốn sau: Bảng 3.4 Code điều khiển động theo góc mong muốn Điều khiển động writePosition(dc1, (theta1 + 90) / 180); writePosition(dc2, (180 - theta2) / 180); writePosition(dc3, (180 + theta3) / 180); 3.4.6 Thiết kế giao diện Phần mềm điều khiển robot nhóm có giao diện sau:  FORWARD KINEMATIC: giải tốn động học thuận, có kèm nút Home để đưa Robot vị trí ban đầu Nhóm trưởng: Nguyễn Thanh Quy 31 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo  INVERSE KINEMATIC: giải toán động học ngược, nút Go để điều khiển Robot đến vị trí  PROGRAMMING: lập trình điều khiển Robot theo góc chọn trước thêm vào file txt Hình 3.18 Giao diện phần mềm điều khiển Nhóm trưởng: Nguyễn Thanh Quy 32 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo CHƯƠNG KẾT QUẢ NGHIÊN CỨU 4.1 Kết đồ án Nhóm đáp ứng việc điều khiển động theo góc mong muốn Giải toán động học thuận, động học ngược lập trình robot chạy theo chuỗi vị trí mong muốn cho trước 4.2 Hạn chế đồ án Với chất lượng phần cứng không tốt nên dẫn đến việc cịn sai số tính tốn điều khiển chưa thể mượt mà Trong toán giải động học ngược xuất trường hợp đặc biệt dẫn đến sai kết sau:  Trong tốn động học thuận, góc quay khâu thứ âm tọa độ p y âm ngược lại Tuy nhiên, tồn trường hợp góc quay khâu thứ âm tọa độ p y lại dương ngược lại robot ngửa hết cỡ phía sau (như hình dưới)  Khi đưa tọa độ vào toán động học nghịch, theo cơng thức (2.7) nhóm mặc định tọa độ p y âm góc 1 âm ngược lại nhờ vào hàm arctan biến Vì hàm động học ngược tính nghiệm khác thỏa điều kiện vật lý, từ làm sai kết Hạn chế nhóm khơng tìm cách giải Hình 3.19 Trường hợp đặc biệt động học thuận Nhóm trưởng: Nguyễn Thanh Quy 33 ĐAMH: Robot công nghiệp GVHD: Th.s Trương Ngọc Bảo TÀI LIỆU THAM KHẢO [1] Mathworks Control Servo Motors https://www.mathworks.com/help/supportpkg/arduinoio/examples/control-servomotors.html [2] Nguyễn Đăng Phước Giáo trình Robot công nghiệp [3] Thái Sơn, Arduino VietNam AVR - Điều khiển động SERVO siêu chuẩn với biên độ góc cực nhỏ 2017 http://arduino.vn/tutorial/1423-avr-dieu-khien-dong-coservo-sieu-chuan-voi-bien-do-goc-cuc-nho Nhóm trưởng: Nguyễn Thanh Quy 34 ... cánh tay robot bậc tự sử dụng mạch Arduino để điều khiển Nghiên cứu tổng quan cấu tạo nguyên lý hoạt động cánh tay robot mạch điều khiển Vận dụng kiến thức để vận hành lập trình điều khiển cánh tay. .. chỉnh (chưa có điều khiển servo) Bộ khung in công nghệ in 3D sử dụng sợi nhựa PLA Bộ cánh tay robot điều khiển servo thông qua Arduino Dùng để chế tạo mơ hình cánh tay robot mơ cánh tay công nghiệp... trận T viết phương trình động học robot 2.3.2 Phương trình động học cánh tay robot đồ án Mơ hình 3D cánh tay robot có dạng sau: Nhóm trưởng: Nguyễn Thanh Quy 11 ĐAMH: Robot công nghiệp GVHD: Th.s