Hệ thống phân loại sản phẩm sử dụng robot delta kết hợp xử lí ảnh

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ ĐỒ ÁN TỐT NGHIỆP NGÀNH: KỸ THUẬT CƠ ĐIỆN TỬ ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO HỆ THỐNG PHÂN LOẠI SẢN PHẨM SỬ DỤNG ROBOT DELTA KẾT HỢP XỬ LÝ ẢNH Người hướng dẫn: Người duyệt: Sinh viên thực hiện: TS LÊ HOÀI NAM TS VÕ NHƯ THÀNH HÀ HỒNG NHẬT TRƯƠNG XUÂN HẢI Số thẻ sinh viên : 101140193 101140138 Lớp: 14CDT2 14CDT1 Đà Nẵng, 6/2019 ĐẠI HỌC ĐÀ NẴNG CỘNG HỊA XÃ HƠI CHỦ NGHĨA VIỆT NAM TRƯỜNG ĐẠI HỌC BÁCH KHOA Độc lập - Tự - Hạnh phúc KHOA CƠ KHÍ NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP TT Họ tên sinh viên Số thẻ SV Lớp Ngành Hà Hồng Nhật 101140193 14CDT2 Cơ Điện Tử Trương Xuân Hải 101140138 14CDT1 Cơ Điện Tử Tên đề tài đồ án: Hệ thống phân loại sản phẩm sử dụng Robot Delta kết hợp xử lí ảnh Đề tài thuộc diện: ☒ Có ký kết thỏa thuận sở hữu trí tuệ kết thực Các số liệu liệu ban đầu: - Vùng làm việc robot Delta; Các kích thước thơng số robot; Các vật liệu: nhơm ngun khối, inox, nhơm định hình Nội dung phần thuyết minh tính tốn: a Phần chung: TT Họ tên sinh viên Hà Hồng Nhật Trương Xuân Hải Nội dung Nghiên cứu tổng quan hệ thống ứng dụng Robot Delta cơng nghiệp; Tìm hiểu động học Robot Delta; Xây dựng hệ thống khí cho đề tài; Thiết kế hệ thống điện tủ điện; Xây dựng vẽ cho hệ thống b Phần riêng: TT Họ tên sinh viên Hà Hồng Nhật Nội dung Tìm hiểu cách thức hoạt động động driver; Lập trình điều khiển cho Robot Delta; Tìm hiểu ứng dụng máy tính nhúng vào đề tài Trương Xuân Hải Thiết kế mạch điện cho hệ thống; Thiết kế in 3D liên quan đến đề tài; Thiết kế lập trình phần xử lí ảnh vùng làm việc cho hệ thống Các vẽ, đồ thị (ghi rõ loại kích thước vẽ): a Phần chung: TT Họ tên sinh viên Nội dung Hà Hồng Nhật Bản vẽ sơ đồ động Trương Xuân Hải Poster hệ thống b Phần riêng: TT Họ tên sinh viên Nội dung Hà Hồng Nhật Bản vẽ sơ đồ tủ điện sơ đồ kết nối dây Trương Xuân Hải Bản vẽ khí lắp ghép Họ tên người hướng dẫn: T.S Lê Hoài Nam Phần/ Nội dung: Hướng dẫn từ lý thuyết đến hoàn thành đề tài Ngày giao nhiệm vụ đồ án: 19/1/2019 Ngày hoàn thành đồ án: 19/5/2019 Đà Nẵng, ngày 19 tháng năm 2019 Trưởng Bộ môn Người hướng dẫn CAM ĐOAN - Chúng em xin cam đoan đồ án tuân thủ tốt quy định liêm học - thuật; Khơng bịa đặt, đưa thơng tin sai lệch so với nguồn trích dẫn; - Khơng ngụy tạo số liệu q trình khảo sát, thí nghiệm, thực hành, thực tập hoạt động học thuật khác; - Khơng sử dụng hình thức gian dối việc trình bày, thể hoạt động học thuật kết từ trình học thuật mình; - Khơng đạo văn, sử dụng từ ngữ, cách diễn đạt người khác thể mình, trình bày, chép, dịch đoạn, nêu ý tưởng người khác mà - khơng trích dẫn; Khơng tự đạo văn, sử dụng lại thông tin nghiên cứu mà khơng có trình dẫn phân mảnh thơng tin kết nghiên cứu để cơng bố nhiều ấn phẩm i MỤC LỤC Tóm tắt Nhiệm vụ đồ án Lời cam đoan liêm học thuật i Mục lục ii Danh sách bảng biểu, hình vẽ sơ đồ v Danh sách cụm từ viết tắt vii Lời nói đầu…………………………………………………………………………… Chương Tổng quan đề tài 1.1 Giới thiệu chung 1.2 Giới thiệu robot Delta 1.2.1 Phân loại 1.2.2 Phân tích ưu điểm nhược điểm 1.3 Mục tiêu đề Chương Cơ sở lí thuyết 2.1 Cấu trúc 2.2 Bài toán động học 2.2.1 Xây dựng phương trình động học 11 2.2.2 Bậc tự 13 2.2.3 Động học nghịch 14 2.2.4 Động học thuận 16 2.3 Khảo sát vùng làm việc 21 2.4 Xứ lí ảnh 27 2.4.1 Khái niệm xứ lí ảnh 27 2.4.2 Các vấn đề xử lí ảnh 27 2.4.3 Khử nhiễu 29 2.4.4 Mơ hình màu HSV (Hue, Staturation, Value) 31 Chương Thiết kế 33 3.1 Sơ đồ tổng quan 33 ii 3.1.1 Sơ đồ tổng quát hệ thống 33 3.1.2 Lưu đồ thuật toán 34 3.2 Thiết kế hệ thống khí 35 3.2.1 Tấm đế cố định 36 3.2.2 Tấm đế di động 37 3.2.3 Cánh tay 37 3.2.4 Cánh tay 37 3.2.5 Khớp cầu 37 3.2.6 Hộp giảm tốc 37 3.3 Thiết kế chọn động 38 3.3.1 Tính toán 38 3.3.2 Động Easy Servo Motor 39 3.4 Thiết kế hệ thống điện – điện tử 41 3.4.1 Thiết kế hệ thống tủ điện 41 3.4.2 Thiết kế mạch điện tử 44 3.4.3 Bảng tính tốc độ chu kì xung 50 3.4.4 Mạch điều khiển 52 3.5 Thiết kế phần lập trình cho vi điều khiển 55 3.5.1 Lưu đồ thuật toán điều khiển 55 3.5.2 Điều khiển động có gia tốc 55 3.5.3 Phương pháp điều khiển động có gia tốc 56 3.6 Thiết kế hệ thống khí nén 59 3.7 Thiết kế phần xử lí ảnh 60 3.7.1 Lưu đồ thuật toán 60 3.7.2 Xứ lí giao tiếp qua Webcam 60 3.7.3 Chuyển ảnh màu dạng RGB sang HVS 61 3.7.4 Loại bỏ thành phần nhiễu 61 3.7.5 Xác định tọa độ âm, góc nghiêng vật 62 3.7.6 Chuyển đổi toạ độ khung ảnh sang tọa độ mặt phẳng thực tế 62 Chương Đánh giá kết luận 64 4.1 Kết 64 4.2 Đánh giá 66 4.3 Kết luận 67 iii 4.4 Hướng phát triển 67 Tài liệu tham khảo………………………………………………………………… 74 Phụ lục………………………………………………………………………………75 iv DANH SÁCH CÁC BẢNG, HÌNH VẼ Hình 1.1 Robot chuỗi hãng Kuka Hình Hãng Fanuc Hình Hãng ABB Hình Hãng Omron Hình Robot Delta Hình Robot Delta ứng dụng dây chuyền đóng gói sản phẩm Hình Robot Delta hãng FANUC ứng dụng lắp ráp khí Hình Kiểu khớp xoay Hình Kiểu khớp trượt Hình 10 Tốc độ Robot Hình 11 Mục tiêu hướng đến đề tài tương lai Hình Robot kiểu khớp quay Hình 2 Thơng số hình học Hình Tám nhiệm phương trình động học ngược 15 Hình Sơ đồ động học thuận 17 Hình Vùng làm việc có dạng ring torus (với 𝐿 = 1244mm, 𝑙 = 5244mm) 23 Hình Vùng làm việc có dạng horn torus (với 𝐿 = 𝑙 = 884mm) 23 Hình Vùng làm việc có dạng spindle torus (với 𝐿 = 524mm, 𝑙 = 1244mm) 24 Hình Vùng làm việc cánh tay 24 Hình Mười vùng làm việc 26 Hình 10 Phương án 26 Hình 11 Q trình xử lí ảnh 27 Hình 12 Ảnh tĩnh 28 Hình 13 Chuỗi ảnh động 28 Hình 14 Kết sau lọc 30 Hình 15 Kết sau lọc 31 Hình 16 Lọc giả trung vị 31 Hình 17 Mơ hình màu khơng gian HVS 32 v Hình Sơ đồ tổng quan hệ thống 33 Hình Lưu đồ thuật tốn 34 Hình 3 Sơ đồ hệ thống thiết kế khí 35 Hình Thiết kế lắp ráp phần mềm Solidwork 36 Hình Mơ vùng làm việc Robot 36 Hình Thiết kế gia cơng 36 Hình Hộp giảm tốc 37 Hình Động easy servo motor 39 Hình Sơ đồ kí hiệu kết nối dây 40 Hình 10 Sơ đồ hệ thống điện- điện tử 41 Hình 11 Contactor 42 Hình 12 Sóng hài 43 Hình 13 Tác dụng lọc 3P 1P 43 Hình 14 Sơ đồ kết nối 44 Hình 15 Cách đấu nguồn cho Driver 44 Hình 16 Cách đấu nguồn thực tế 45 Hình 17 Kết nối với động 45 Hình 18 Sơ đồ kết nối với vi điều khiển 46 Hình 19 Sơ đồ kết nối chân với tín hiệu điều khiển 47 Hình 20 Cáp kết nối điều khiển 47 Hình 21 Giao diện phần mềm 48 Hình 22 Bảng điều khiển Driver 48 Hình 23 Arduino Mega 2560 52 Hình 24 Sơ đồ chân board mega 2560 53 Hình 25 Lưu đồ thuật toán điều khiển 55 Hình 26 Mối quan hệ tốc độ Momen động ES- MH 23480 56 Hình 27 Mối quan hệ tốc độ momen 56 Hình 28 Tạo xung với tốc độ không đổiv 57 Hình 29 Mối quan hệ gia tốc, vận tốc, vị trí 57 Hình 30 Đặc tính tốc độ xung điều khiển 58 Hình 31 Hệ thống khí nén 59 Hình 32 Lưu đồ thuật tốn xử lí ảnh 60 vi Hình 33 Hình ảnh gốc chụp từ Camera 61 Hình 34 Ảnh tọa độ, màu sắc vật 62 Hình 35 Chuyển đổi hệ tọa độ 63 Hình Hồn thiện sản phẩm 64 Hình Giao diện điều khiển 65 Hình Giao diện mơ tính tốn động học 66 Hình 4 Thiết kế tủ sơ đồ dây 66 vii Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh CHƯƠNG ĐÁNH GIÁ VÀ KẾT LUẬN 4.1 Kết Sau nhiều tháng liên tục hoàn thiện nâng cấp sản phẩm Nhóm đạt được số thành công định với tiến độ đặt trước Về mặt tổng thể: Hình Hồn thiện sản phẩm Các thơng số kĩ thuật Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 64 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh Stt Đặc tính Kí hiệu Thơng số Chiều cao tổng 1800 mm Chiều rộng tổng 1000 mm Cánh tay L 200 mm Cánh tay l 296 mm Tấm đế cố định sB 364 mm Tấm đế di động sP 173 mm Vùng làm việc S 300x300 mm Bảng Thơng số Về giao diện điều khiển: Trong giao diện điều khiển robot Delta theo kiểu dạy học tự động xử lí ảnh bắt vật Hình Giao diện điều khiển Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 65 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh Về giao diện mơ phỏng: Hình Giao diện mơ tính tốn động học Thiết kế tủ điện Có đầy đủ thành phần bảo vệ nguồn bảo vệ driver cho động Các thiết bị điện bố trí cách hợp lí Có thành phần lọc nhiễu cho Driver động Hình 4 Thiết kế tủ sơ đồ dây 4.2 Đánh giá Phần khí Robot Delta chuyển động tốt, gặp cố Kiểu dáng tương đối chắn song bên cạnh nhiểu điểm cần khắc phục liên kết cánh tay đế Phần xử lí ảnh Đã đưa xử lí ảnh vào đồ án thành cơng Camera bắt vật tương đối xác trả vị trí Nhưng bên cạnh cần nhiều vấn đề cần nâng cấp, cải tiến xài camera theo chuẩn công nghiệp cần nâng cấp vùng xử lí ảnh rộng Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 66 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh Phần mô Đã mô Robot Delta phần mềm Matlab Giải toán động học vẽ mơ hình mơ Song cần nâng cấp độ xác tốn Phần điện tử Các thành phần bảo vệ nguồn driver cho động lắp đặt Cần nâng cấp mạch điều khiển cho Robot để khử nhiễu trình hoạt động 4.3 Kết luận Sau nhiều tháng liên tục tìm tịi phát triển nhóm tác giả hồn thành tiến độ đặt ra, đáp ứng yêu cầu nhiệm vụ thiết kế đồ án Cùng với việc áp dụng nghiên cứu mặt lí thuyết kết hợp với thiết kế thực tiễn nhóm có nhiều kinh nghiệm việc thiết kế Robot nói chung Robot Delta nói riêng Về mặt lí thuyết tìm hiểu quy trình thiết kế Robot việc Tìm hiểu động học Robot Tìm hiểu động lực học Robot Xác định động học xác định vùng làm việc Robot Từ vùng làm việc suy ngược kích thước Robot (Cánh tay trên, cánh tay dưới, đế di động, đế cố định) Tính chọn động từ động chọn Driver điều khiển cho động Tìm hiểu, sử dụng phầm mềm đại ứng dụng nhiều việc thiết kế lập trình Robot (Matlab, Visual, Solidworks…) Về mặt thiết kế thực tiễn kinh nghiệm quý giá trường Điều khiển động Servo Motor theo dạng chuẩn công nghiệp Cách sử dụng máy tính nhúng Gặp vấn đề khí sai số, dung sai quan trọng lắp ghép Nó ảnh hưởng trực tiếp việc di chuyển Robot Biết cách thiết kế tủ điện theo dạng chuẩn công nghiệp Biết hệ thống khí quan trọng độ cứng vững Robot Một lần nhóm tác giả xin chân thành cảm ơn sâu sắc tới thầy TS Lê Hồi Nam, thầy khoa cơng ty điện tử R&P hỗ trợ nhóm thời gian khó khăn vừa qua để nhóm hồn thành đồ án tốt Xin chân thành cảm ơn! 4.4 Hướng phát triển Tuy có số thành cơng định song để đề tài vào cơng nghiệp Nhóm cần cố gắng nhiều tương lai Có mục tiêu mà nhóm đề tương lai: Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 67 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh Mục tiêu ngắn hạn: - Ổn định phần khí cách chắn chắn - Nghiên cứu sâu vào phần động lực học Robot Delta - Thiết kế phần băng tải vào hệ thống Áp dụng hệ thống nhúng vào dây chuyền Mục tiêu dài hạn: - Thiết kế Robot Delta hàn mạch điện tử - Đa dạng hóa kiểu dáng Robot Delta từ bậc tự lên thành bậc Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 68 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh - TÀI LIỆU THAM KHẢO [1] Dangzang, 2010 Parallel Robotic Machine Tool [2] R Clavel, 1988, “A Fast Robot with Parallel Geometry”, Proc Int Symposium on Industrial Robots, pp 91-100 [3] R Clavel, 1990, “Device for the Movement and Positioning of an Element in Space”, US Patent 4976582 A [4] [Hoai Nam LE and Xuan Hoang LE] Geometrical design of a RUU type Delta robot based on the predescribed Workspace, 4th International Conference on Green Technology and Sustainable Development (GTSD), HCMUTE (2018) [5] [Hoai Nam LE and Xuan Hoang LE] Bài toán động học, động lực học phương pháp thiết kế hình học cho Robot Delta kiểu ba khớp quay [6] [Nguyễn Vĩnh An] “So sánh số phương pháp phát biên”, Tạp chí Khoa học ĐHQGHN: Khoa học Tự nhiên Công nghệ, Tập 31, Số (2015) 1-7 [7] https://www.robots.com [8] http://www.ia.omron.com [9] http://www.irobotnews.com [10] bit AVR Microcontroler of ATMEL [11] Žlajpah, Leon "Simulation in robotics." Mathematics and Computers in Simulation 79.4 (2008): 879-897 [12] X.-J Liu, J Wang, H Zheng, “Workspace atlases for the computer aided design of the Delta robot”, Proc IMECHE part C: J Mech Engrg Sci., vol 217, pp 861869, 2003 Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 69 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh PHỤ LỤC Code động học viết ngôn ngữ C sử dụng cho vi điều khiển #include /* // THONG SO DUNG CHO CANH TAY NHUA const float e = 150; // end effector // chu y day la canh cua tam giac// diem noi diem giua gia canh const float f = 364.41; // base const float re = 415; const float rf = 150; */ const float e = 173.2; // end effector // chu y day la canh cua tam giac// diem noi diem giua gia canh const float f = 364.41; // base const float re = 296; const float rf = 200; // trigonometric constants const float sqrt3 = sqrt(3.0); const float pi = 3.141592653; // PI const float sin120 = sqrt3/2.0; const float cos120 = -0.5; const float tan60 = sqrt3; const float sin30 = 0.5; const float tan30 = 1/sqrt3; int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) { float t = (f-e)*tan30/2; float dtr = pi/(float)180.0; theta1 *= dtr; theta2 *= dtr; theta3 *= dtr; Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 70 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh float y1 = -(t + rf*cos(theta1)); float z1 = -rf*sin(theta1); float y2 = (t + rf*cos(theta2))*sin30; float x2 = y2*tan60; float z2 = -rf*sin(theta2); float y3 = (t + rf*cos(theta3))*sin30; float x3 = -y3*tan60; float z3 = -rf*sin(theta3); float dnm = (y2-y1)*x3-(y3-y1)*x2; float w1 = y1*y1 + z1*z1; float w2 = x2*x2 + y2*y2 + z2*z2; float w3 = x3*x3 + y3*y3 + z3*z3; // x = (a1*z + b1)/dnm float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1); float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0; // y = (a2*z + b2)/dnm; float a2 = -(z2-z1)*x3+(z3-z1)*x2; float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0; // a*z^2 + b*z + c = float a = a1*a1 + a2*a2 + dnm*dnm; float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm); float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 re*re); // discriminant float d = b*b - (float)4.0*a*c; if (d < 0) return -1; // non-existing point z0 = -(float)0.5*(b+sqrt(d))/a; x0 = (a1*z0 + b1)/dnm; y0 = (a2*z0 + b2)/dnm; Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 71 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh return 0; } // inverse kinematics // helper functions, calculates angle theta1 (for YZ-pane) int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) { float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 y0 -= 0.5 * 0.57735 * e; // shift center to edge // z = a + b*y float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re y1*y1)/(2*z0); float b = (y1-y0)/z0; // discriminant float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); if (d < 0) return -1; // non-existing point float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point float zj = a + b*yj; theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0); return 0; } // inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3) // returned status: 0=OK, -1=non-existing position int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) { theta1 = theta2 = theta3 = 0; int status = delta_calcAngleYZ(x0, y0, z0, theta1); if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2); // rotate coords to +120 deg if (status == 0) status = delta_calcAngleYZ(x0*cos120 y0*sin120, y0*cos120+x0*sin120, z0, theta3); // rotate coords to 120 deg return status; } Code phương trình động học nghịch viết Matlab để mơ Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 72 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh function calculatinginverse_Callback(hObject, eventdata, handles) % hObject handle to calculatinginverse (see GCBO) % eventdata of MATLAB reserved - to be defined in a future version % handles GUIDATA) structure with handles and user data (see global sBm ; sBm = str2double(get(handles.sB,'string')); global Lm ; Lm = str2double(get(handles.L,'string')); %rfm = str2double(get(handles.L,'string')); global lm ; lm = str2double(get(handles.l,'string')); %rem = str2double(get(handles.l,'string')); global sPm; sPm = str2double(get(handles.sP,'string')); global xim ; xim = str2double(get(handles.xi,'string')); global yim ; yim = str2double(get(handles.yi,'string')); global zim ; zim = str2double(get(handles.zi,'string')); y1=-sBm/(2*sqrt(3)); k=sPm/(2*sqrt(3)); yim1=yim-k; %shift a=(xim^2 + yim1^2 + zim^2 +Lm^2 - lm^2 - y1^2)/(2*zim); b=(y1-yim1)/zim; d=-(a+b*y1)^2+Lm*(b^2*Lm+Lm); if(dy1) theta1=theta1+180 end theta11= round(theta1) ; s=0; end set(handles.angle1i,'string',num2str(theta11)) %tinh goc theta2 xim2 = xim*cos(120*pi/180) + yim*sin(120*pi/180); yim2= yim*cos(120*pi/180)-xim*sin(120*pi/180); zim2= zim ; yim2=yim2-k; %shift a2=(xim2^2 + yim2^2 + zim2^2 +Lm^2 - lm^2 y1^2)/(2*zim2); b2=(y1-yim2)/zim2; d2=-(a2+b2*y1)^2+Lm*(b2^2*Lm+Lm); if(d2y1) theta2=theta2+180 end Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 74 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh theta22= round(theta2); s=0; end set(handles.angle2i,'string',num2str(theta22)) %tih goc theta xim3= xim*cos(120*pi/180) - yim*sin(120*pi/180) yim3 = yim*cos(120*pi/180)+xim*sin(120*pi/180) zim3 = zim ; yim3=yim3-k; %shift a3=(xim3^2 + yim3^2 + zim3^2 +Lm^2 - lm^2 y1^2)/(2*zim3); b3=(y1-yim3)/zim3; d3=-(a3+b3*y1)^2+Lm*(b3^2*Lm+Lm); if(dy1) theta3=theta3+180 end theta33= round(theta3); s=0; end set(handles.angle3i,'string',num2str(theta33)) Code phương trình động học thuận viết Matlab để mô global sBm ; sBm = str2double(get(handles.sB,'string')); Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 75 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh global Lm ; Lm = str2double(get(handles.L,'string')); %rfm = str2double(get(handles.L,'string')); global lm ; lm = str2double(get(handles.l,'string')); %rem = str2double(get(handles.l,'string')); global sPm; sPm = str2double(get(handles.sP,'string')); %em = str2double(get(handles.sP,'string')); global angle1fm ; angle1fm = str2double(get(handles.angle1f,'string')) ; global angle2fm ; angle2fm = str2double(get(handles.angle2f,'string')) ; global angle3fm ; angle3fm = str2double(get(handles.angle3f,'string')) ; k=(sBm-sPm)/(2*sqrt(3)); A1v=[0;-k-Lm*cos(angle1fm*pi/180);Lm*sin(angle1fm*pi/180)]; %x1=0->projsPmction on yz plansPm A2v=[(k+Lm*cos(angle2fm*pi/180))*cos(30*pi/180); (k+Lm*cos(angle2fm*pi/180))*sin(30*pi/180); -Lm*sin(angle2fm*pi/180)]; A3v=[(-k-Lm*cos(angle3fm*pi/180))*cos(30*pi/180); (k+Lm*cos(angle3fm*pi/180))*sin(30*pi/180); -Lm*sin(angle3fm*pi/180)]; %proportional constants sBmor sPmnd sPmsBmsBmsPmctor position w1=A1v(1)^2 + A1v(2)^2 + A1v(3)^2; w2=A2v(1)^2 + A2v(2)^2 + A2v(3)^2; w3=A3v(1)^2 + A3v(2)^2 + A3v(3)^2; %calculatsPm Ji positions x1=A1v(1); x2=A2v(1); x3=A3v(1); Sinh viên thực hiện: Hà Hồng Nhật, Trương Xuân Hải GVHD: T.S Lê Hoài Nam 76 Hệ thống phân loại sản phẩm sử dụng robot Delta kết hợp với xử lí ảnh y1=A1v(2); y2=A2v(2); y3=A3v(2); z1=A1v(3); z2=A2v(3); z3=A3v(3); d=(y2-y1)*x3-(y3-y1)*x2; a1=(1/d)*((z2-z1)*(y3-y1)-(z3-z1)*(y2-y1)); b1=(-1/(2*d))*((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1)); a2=(-1/d)*((z2-z1)*x3-(z3-z1)*x2); b2=(1/(2*d))*((w2-w1)*x3-(w3-w1)*x2); %% numsPmrics %quadratic sPmquation to solvsPm->choossPm smallsPmr nsPmgativsPm valusPm! a=(a1^2+a2^2+1); b=2*(a1*b1+a2*(b2-y1)-z1); c=(b1^2+(b2-y1)^2 +z1^2-lm^2); syms z ; z=solve(a*z^2+b*z+c==0,z); nghiemz1=z(1) ; nghiemz2=z(2) ; if( nghiemz1>= nghiemz2 && nghiemz2