(Đồ án tốt nghiệp) thiết kế và chế tạo robot delta

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO ROBOT DELTA Người hướng dẫn: Sinh viên thực hiện: TS LÊ HOÀI NAM LÊ TẤN VINH NGUYỄN VIẾT LONG Đà Nẵng, 2019 TÓM TẮT ĐỒ ÁN TỐT NGHIỆP Tên đề tài: Thiết kế chế tạo robot Delta Sinh viên thực hiện: Lê Tấn Vinh Nguyễn Viết Long Giảng viên hướng dẫn: TS Lê Hoài Nam Lớp: 15CDT1 MSSV: 101150197 Lớp: 15CDT1 MSSV: 101150173 Giảng viên duyệt: PGS.TS Trần Xuân Tùy  Nhu cầu thực tế đề tài Ngày nay, robot chiếm ví trí quan trọng dây chuyền sản xuất tự động Với ưu điểm vượt trội tốc độ làm việc robot Delta việc nghiên cứu, thiết kế chế tạo chúng thực nhu cầu cấp thiết Đặc điểm robot Delta với yêu khác vùng làm việc bên cạnh có thơng số khác kích thước robot Trong khn khổ đề tài C C trình bày trình nghiên cứu thiết kế robot từ vùng làm việc yêu cầu đến hoàn thiện sản phẩm cuối cùng, mẫu robot có đủ khả ứng dụng thực tế cơng nghiệp Có thể sử dụng dây chuyền phân loại sản phẩm băng tải động  Nội dung đề tài thực Mơ hình: 1, Bản vẽ: 5, Thuyết minh gồm: 62 trang  Kết đạt Phần lí thuyết: R L T U D  Tìm hiểu chung loại robot, biết ưu nhược điểm loại  Nắm phần động học robot Delta, tính tốn động lực học, vùng làm việc robot Delta  Tính chọn tìm hiểu động cơ, driver điều khiển động cơ, thuật toán điều khiển chúng Phần thiết kế thực tế:  Viết chương trình giải tốn động học, động lực học mô robot Delta  Thiết kế 3D 2D chi tiết robot Delta  Thiết kế tủ điện phận điều khiển trình hoạt động robot Sinh viên thực Sinh viên thực Lê Tấn Vinh Nguyễn Viết Long ĐẠ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 Sinh viên thực hiện: Lê Tấn Vinh Nguyễn Viết Long Lớp: 15CDT1 Lớp: 15CDT1 MSSV: 101150197 MSSV: 101140138 Tên đề tài: Thiết kế chế tạo robot Delta Nội dung a Phần nghiên cứu chung  Nghiên cứu tổng quan robot Delta  Tìm hiểu động học, động lực 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 b Phần nghiên cứu riêng STT Sinh viên thực R L Lê Tấn Vinh C C Nội dung T U  Lập trình mơ chuyển động robot Delta Matlab Solidwork D  Thiết lập vẽ chế tạo chi tiết robot Nguyễn Viết Long Delta  Thiết Kế 3D chi tiết robot Delta  Nghiên cứu phương pháp điều khiển cho robot Ngày giao đồ án: / / Ngày hoàn thành đồ án: / / Giáo viên hướng dẫn: TS Lê Hoài Nam Đà Nẵng, ngày tháng năm 2019 Trưởng Bộ môn Cơ Điện tử Giảng viên hướng dẫn TS Võ Như Thành TS Lê Hoài Nam Thiết kế chế tạo robot Delta Lời nói đầu Trong nghiệp cơng nghiệp hóa đại hóa đất nước, tự động hóa ngày đóng vai trị quan trọng Với tốc độ phát triển không cần lượng lao động khổng lồ mà đòi hỏi có trình độ, chất lượng tay nghề, kỹ thuật lao động thiết bị sản xuất Mức độ phát triển khoa học kỹ thuật ngày cao vấn đề tự động hoá ngày trọng Trong năm gần đây, robot đóng vai trị quan trọng hoạt động kinh tế, môi trường xã hội Nó cột mốc quan trọng đánh dấu phát triển vượt bậc toàn nhân loại Nó giúp cho người lao động an tồn hơn, nâng cao hiệu sản xuất nhằm đẩy mạnh phát triển kinh tế phần cịn làm giảm rác thải ô nhiễm nhà máy công nghiệp Kết hợp xu phát triển thời đại kiến thức quý báu thầy cô truyền đạt qua năm học trường Nhóm tác giả định lựa chọn đề tài “Thiết kế C C R L T chế tạo Robot Delta” nhằm đáp ứng nhu cầu dây chuyển sản suất nhà máy công nghiệp cho hệ thống tối ưu Trong thời gian làm đồ án, bảo tận tình thầy TS Lê Hồi Nam hỗ trợ từ cơng ty Điện tử RP phía sau giúp đỡ to lớn từ gia đình, bạn bè Nhóm tác giả thực cố gắng hoàn thành đề tài cách tốt Tuy nhiên, với kiến thức hạn chế, kinh nghiệm, kĩ thiếu, cố gắng nhiều bên cạnh cịn nhiều thiếu sót cần bổ sung, hồn thiện Mong thầy thơng cảm góp ý để đề tài nhóm hồn thiện hơn, phát triển ứng dụng thời gian U D tới Một lần nhóm xin cảm ơn gia đình, người thân bạn bè động viên, giúp đỡ suốt trình thực đồ án Đặc biệt, nhóm tác giả xin chân thành cảm ơn Thầy, Cô giáo khoa Cơ khí – Trường Đại học Bách khoa – Đại học Đà Nẵng, đặc biệt Thầy trực tiếp hướng dẫn nhóm đề tài tốt nghiệp SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang i Thiết kế chế tạo robot Delta LỜI CAM ĐOAN Chúng em xin cam đoan đồ án tuân thủ đầy đủ 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ừ q trình học thuật 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 Sinh viên thực Sinh viên thực C C Lê Tấn Vinh SVTH: Lê Tấn Vinh, Nguyễn Viết Long R L T U D GVHD: TS Lê Hoài Nam Nguyễn Viết Long Trang ii Thiết kế chế tạo robot Delta MỤC LỤC Tóm tắt đồ án tốt nghiệp Nhiệm vụ đồ án tốt nghiệp Lời nói đầu i Lời cam đoan ii Mục lục iii Danh sach biểu, hình vẽ v 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 Ưu điểm nhược điểm robot Delta 1.3 Mục tiêu đề Chương Cơ sở lí thuyết .7 2.1 Bài toán động học C C R L T 2.1.1 Bậc tự 10 2.1.2.Xây dựng phương trình động học 11 2.2 Động học nghịch 13 2.2.1 Phương pháp giải tích: 13 2.2.2 Phương pháp số Newton-rashson: 15 U D 2.3 Động học thuận 17 2.4 Bài toán động lực học 23 2.4.1 Xác định vận tốc khối tâm vận tốc góc khâu: 23 2.4.2 Xác định phương trình Lagrande dạng nhân tử cho hệ p vật rắn 25 2.4.3 Tính tốn ma trận 26 2.4.4 Giải toán động lực học ngược cách biến đổi tọa độ suy rộng: 30 2.5 Khảo sát vùng làm việc 32 Chương Thiết kế hệ thống .36 3.1 Sơ đồ tổng quan 36 3.1.1 Sơ đồ tổng quát hệ thống 36 3.1.2 Lưu đồ thuật toán 37 3.2 Thiết kế hệ thống khí 37 SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang iii Thiết kế chế tạo robot Delta 3.2.1 Tính chọn thơng số Robot Delta: 39 3.2.2 Tính chọn động 41 3.2.3 Thiết kế khí chi tiết robot Delta 45 a Tấm đế cố định 45 b Gá động 46 c Bạc lót 47 d Tấm đế di động 47 e Cánh tay chủ động 48 f Cánh tay bị động 48 3.3 Thiết kế hệ thống điện – điện tử 50 3.3.1 Thiết kế hệ thống tủ điện 50 a Cách bố trí tủ 50 b Contactor(MS) 51 c Bộ lọc nguồn lọc nhiễu (Noise Filter) 51 3.3.2 Thiết kế mạch điện tử 53 C C R L T a Driver điều khiển động 53 b Mạch điều khiển 54 3.4 Thiết kế hệ thống khí nén 56 Chương Kết nghiên cứu kết luận 58 4.1 Kết nghiên cứu 58 4.2 Đánh giá: 60 4.3 Kết luận 61 U D 4.4 Hướng phát triển 62 Phụ lục SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang iv Thiết kế chế tạo robot Delta DANH SÁCH CÁC BẢNG BIỂU, HÌNH VẼ Hình 1.1 Robot chuỗi hãng Yaskawa 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 kiểu khớp xoay Hình Robot Delta kiểu khớp trượt Hình Tốc độ Robot Hình Mục tiêu hướng đến đề tài tương lai Hình Thơng số hình học Hình Tám nhiệm phương trình động học ngược .14 Hình 2 Sơ đồ động học thuận 18 Hình Các hệ tọa độ địa phương gắn cứng vào khâu chủ động .23 Hình Vùng làm việc có dạng ring torus 32 C C R L T Hình Vùng làm việc có dạng horn torus .33 Hình Vùng làm việc có dạng spindle torus 33 Hình Vùng làm việc cánh tay 34 Hình 10 Mười vùng làm việc 35 Hình Sơ đồ tổng quan hệ thống 36 Hình Lưu đồ thuật tốn .37 Hình 3 Sơ đồ hệ thống thiết kế khí 38 Hình Vùng làm việc chọn 41 U D Hình Mơ hình 3D vùng làm việc 41 Hình Vị trí cánh tay đạt momen tĩnh lớn 42 Hình Đồ thị mơ momen dẫn động z = -0.1 m .43 Hình Đồ thị mô momen dẫn động z = -0.34 m 44 Hình 10 Đồ thị mô momen dẫn động z = -0,45 45 Hình 11 Tấm đế cố định 46 Hình 12 Gá động .46 Hình 13 Bạc lót 47 Hình 14 Tấm đế di động .47 SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang v Thiết kế chế tạo robot Delta Hình 15 Cánh tay chủ động 48 Hình 16 Cánh tay bị động 49 Hình 17 Mơ hình 3D Solid Works robot Delta 49 Hình 18 Sơ đồ hệ thống điện- điện tử 50 Hình 19 Contactor 51 Hình 20 Sóng hài 52 Hình 21 Tác dụng lọc 3P 1P .53 Hình 22 AC Servo Driver MDDKT5540 .53 Hình 23 Sơ đồ chân kết nối Driver MDDKT5540 54 Hình 24 Arduino Mega 2560 55 Hình 25 Sơ đồ chân board mega 2560 55 Hình 26 Hệ thống khí nén 56 Hình Hoàn thiện sản phẩm 58 Hình Giao diện điều khiển 59 Hình Giao diện mô Matlab/Simulink 60 C C R L T U D Bảng Bảng kí hiệu hình học Bảng Vùng IIf .39 Bảng Vùng IIIb 39 Bảng 3 Các thơng số kích thước chọn .39 Bảng Các thơng số kích thước sau hiệu chỉnh 40 Bảng 3.5 Các thông số động AC Servo Panasonic 45 Bảng Thông số kỹ thuật Board Arduino Mega 56 Bảng Thơng số 59 SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang vi Thiết kế chế tạo robot Delta Chương Tổng quan đề tài 1.1 Giới thiệu chung Hiện nay, phân loại sản phẩm công đoạn sử dụng nhiều thực tế sản xuất Khi dùng sức người, công việc địi hỏi tập trung cao có tính lặp lặp lại Mặt khác, có yêu cầu phân loại sản phẩm dựa nhiều đặc tính khác màu sắc, hình dáng, khối lượng hay yêu cầu kĩ thuật nhỏ mà mắt thường khó nhận Điều ảnh hưởng trực tiếp đến suất chất lượng sản phẩm Nhận biết điều ngồi ghế Nhà trường, nhóm bắt tay vào nghiên cứu hệ thống phân loại sản phẩm Đi kèm với dây chuyền phân loại sản phẩm robot cơng nghiệp thay người làm cơng việc mang tính lặp lặp lại nhàm chán với suất vượt trội độ tin cậy cao Robot công nghiệp chia làm hai loại chủ yếu robot chuỗi robot song song C C R L T U D Hình 1.1 Robot chuỗi hãng Yaskawa Robot chuỗi có độ linh hoạt cao, không gian làm việc rộng Tuy nhiên, động dẫn động thường gắn khâu động nên qn tính lớn, độ cứng vững khơng cao Ngược lại, robot song song có khơng gian làm việc bị hạn chế, xuất điểm kì dị làm cho robot thiếu thừa bậc tự lại có điểm mạnh độ cứng vững cao ràng buộc khâu chuỗi động học kín, khớp truyền động cố SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang Thiết kế chế tạo robot Delta Hình 24 Arduino Mega 2560 Sơ đồ chân Arduino mega 2560 C C R L T U D Hình 25 Sơ đồ chân board mega 2560 SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang 55 Thiết kế chế tạo robot Delta Vi điều khiển Điện áp hoạt động Nguồn ngoài(jack dc) Xung clock Số chân digital I/O Số chân PWM Số chan Analog Giao tiếp UART Giao tiếp SPI Giao tiếp I2C Ngắt ATmega2560 5VDC 7-12VDC 16Mhz 54 15 16 Bộ Bộ Bộ 256Kb Flash (8Kb dùng cho Bootloader) SRAM 8Kb EEPROM 4Kb Bảng Thông số kỹ thuật Board Arduino Mega C C R L T 3.4 Thiết kế hệ thống khí nén U D Hình 26 Hệ thống khí nén Sử dụng máy khí nén chứa nhiều tạp chất hóa chất Đặc biệt khơng khí cịn có dầu tổng hợp chứa dung môi hữu cơ, muối khí ăn mịn Chúng gây hư hại làm ăn mòn xilanh Điều làm ảnh hưởng đến q trình hút vật đầu cơng tác SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang 56 Thiết kế chế tạo robot Delta Nếu bơm khí nén chứa nhiều chất thải bột Cacbon Nó dính vào phận tạo chân khơng bên van solenoid chí dính vào cơng tắc áp suất làm giảm hiệu suất lỗi hệ thống khí  Máy nén khí: phận tạo nguồn khí cung cấp cho hệ thống  Bộ phận làm khơ khơng khí: Đây phận quan trọng hệ thống khí nén Bất kể đâu, vùng nhiệt đới hay sa mạc, khơng khí khí chứa lượng nước định Khi ẩm khơng giữ trạng thái chuyển sang trạng thái lỏng làm ảnh hưởng đến trình hoạt động nói chung hoạt động lọc nói riêng  Bộ lọc khơng khí: Sau làm khơ khơng khí, lượng tạp chất lớn vượt qua phận làm khô Chức lọc khơng khí lọc tạp C C chất thải ngồi  Bộ lọc tách khơng khí sau lọc cặn bẩn cịn lại số hóa chất chứa R L T dung mơi hữu cơ, muối, chất ăn mòn… Nhiệm vụ lọc loại bỏ chất hệ khơng khí  Bộ van điều áp: để điều chỉnh áp suất cho hệ thống Van hiển thị hiển thị kim đồng hồ điện tử U D  Van điện từ: để điều khiển luồng hay đóng mở khí  Bộ chuyển đổi dịng khí: Bộ phận chuyển đổi khí nén thành chân khơng  Bộ phận cơng tác: Có nhiều phận công tác hút, tay gắp… SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang 57 Thiết kế chế tạo robot Delta Chương Kết nghiên cứu kết luận 4.1 Kết nghiên cứu Sau thời gian nghiên cứu, thiết kế lắp ráp, nhóm hồn thiện đề tài đạt yêu cầu đặt từ đầu:  Về tổng thể: C C R L T U D Hình Hồn thiện sản phẩm SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang 58 Thiết kế chế tạo robot Delta  Các thơng số kĩ thuật Stt Đặc tính Chiều cao tổng Chiều rộng tổng Cánh tay Cánh tay Tấm đế cố định Tấm đế di động Kí hiệu Vùng làm việc rc x h Bảng Thông số Thơng số 1800 mm 1000 mm 200 mm 280 mm 364,4 mm 86,6 mm L l Sb Sp 225 x 300 mm  Về giao diện điều khiển: C C R L T U D Hình Giao diện điều khiển SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang 59 Thiết kế chế tạo robot Delta  Về giao diện mơ phỏng: C C R L T Hình Giao diện mô Matlab/Simulink  Tủ điện điều khiển: U D  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 4.2 Đánh giá:  Phần khí Robot Delta hoạt động tốt, gặp cố Kiểu dáng thiết kế chắn song bên cạnh cịn nhiểu điểm cần khắc phục khớp cầu mua sẵn bị hạn chế góc quay dẫn đến vùng làm việc thực tế robot bị thu hẹp đáng kể  Phần xử lí ảnh Thuật toán xử lý ảnh đưa vào hợp lý, khiến cho robot hoạt động ổn định Camera bắt vật xác trả tọa độ vật băng tải động Nhưng bên cạnh cần nhiều vấn đề cần nâng cấp, cải tiến sử dụng camera cơng nghiệp để tăng tốc độ chất lượng hình ảnh, giúp q trình xử lý xác hơn, nâng cấp vùng xử lí ảnh rộng để tăng tốc độ làm việc băng tải tốc độ làm việc robot  Phần mô điều khiển SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang 60 Thiết kế chế tạo robot Delta Mô thành công 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ương pháp thuật toán cao cấp  Phần điện tử Các mạch điện tử mạch nguồn công suất thiết kế lắp đặt theo tiêu chuẩn công nghiệp với đầy đủ phận bản, lọc nhiều cho Driver động cơ, relay điều khiển đóng ngắt hệ thống khí nén Nhưng bên cạnh đó, robot điều khiển vi điều khiển Arduino, điều gây số cản trở tốc độ xử lý lệnh chống nhiễu 4.3 Kết luận Sau nhiều tháng liên tục tìm tịi nghiên cứu, thiết kế lắp ráp, nhóm 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 nghiên cứu lý thuyết, tìm hiểu quy trình thiết kế robot C C R L T việc : Xác định vùng làm việc u cầu robot Tìm hiểu giải tốn động học Từ toán động học, xác định thông số robot Từ thông số bản, cánh tay lớn nhỏ kích thước đế, sử dụng phần mềm thiết kế 3D(Solid Works) để thiết kế chi tiết đảm bảo thông số U D Giải toán động lực học robot Từ toán động lực học, sử dụng phần mềm để mô độ bền ứng suất điểm làm việc chi tiết thiết kế, từ có hiệu chỉnh phù hợp Giải toán động học nghịch Sử dụng toán động học nghịch để mơ q trình điều khiển robot hoạt động phần mềm Matlab Chốt phương án thiết kế tiến hành xuất vẽ gia công chi tiết SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang 61 Thiết kế chế tạo robot Delta 4.4 Hướng phát triển Mặc dù đề tài hoàn thiện tương đối yêu cầu đặt ban đầu, hoạt động xác ổn định với tốc độ cao, để đưa vào sử dụng thực tế sản xuất cịn khoảng cách Có mục tiêu mà nhóm đề tương lai:  Mục tiêu ngắn hạn:  Thiết kế tối ưu phần khí chi tiết gia công  Nghiên cứu sâu vào phần động lực học robot Delta  Thay việc sử dụng vi điều khiển Arduino để điểu khiển cách sử dụng máy tính cơng nghiệp, để tích hợp q trình xử lý ảnh điều khiển robot xử lý trung tâm  Áp dụng hệ thống nhúng vào dây chuyền  Mục tiêu dài hạn: C C  Kết hợp thay đầu cấu chấp hành cuối xi lanh kẹp, tay gắp mềm để mở rộng thêm ứng dụng robot Delta R L T  Đa dạng hóa kiểu dáng robot Delta từ bậc tự lên thành nhiều bậc U D SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang 62 Thiết kế chế tạo robot Delta 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]R Williams II, "The Delta Parallel Robot: Kinematics Solutions",Mechanical Engineering, Ohio University, 2016 [6]Lê Xuân Hoàng,TS.Lê Hoài Nam,” 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”, Tạp chí khoa học cơng nghệ Đại học Đà Nẵng,Số 11,2018 C C R L T [7] bit AVR Microcontroler of ATMEL [8] 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 861-869, 2003 U D [9] https://industrial.panasonic.com/content/data/MT/PDF/manual/en/acs/minas-a52_manu_e.pdf SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang 63 Thiết kế chế tạo robot Delta Phụ lục Code động học nghịch Robot Delta viết Matlab:  Hàm tính hệ phương trình function F=hpt(Pa,q,pos) F=zeros(3,1); % cac kich thuoc dong hoc L=Pa(1); l=Pa(2); wb=Pa(3); up=Pa(4); aA=Pa(5); aB=Pa(6); aC=Pa(7); sp = 0.086; wp = (sqrt(3)*sp)/6; a = -up+wb; b = (sp-sqrt(3)*wb)/2; c = wp -wb/2; C C R L T x = pos(1); y =pos(2); z=pos(3); F = [2*L*(y+a)*cos(q(1))+2*z*L*sin(q(1))+x^2+y^2+z^2+a^2+L^2+2*y*a-l^2;… U D -L*(sqrt(3)*(x+b)+y+c)*cos(q(2))+2*z*L*sin(q(2))+x^2+y^2+z^2+b^2+… c^2+L^2+2*x*b+2*y*c-l^2;L*(sqrt(3)*(x-b)-y-c)*cos(q(3))+2*z*L*sin(q(3))+ x^2+y^2+z^2+b^2+c^2+L^2-2*x*b+2*y*c-l^2]; end  Hàm ma trận jacobi q: function F=matrix_jq(Pa,q,X) F=zeros(3,3); % cac kich thuoc dong hoc L=Pa(1); l=Pa(2); wb=Pa(3); up=Pa(4); aA=Pa(5); aB=Pa(6); aC=Pa(7); sp = 2*Pa(4)*cos(pi/6); wp = (sqrt(3)*sp)/6; a = -up+wb; b = (sp-sqrt(3)*wb)/2; Phụ lục Trang Thiết kế chế tạo robot Delta c = wp -wb/2; x = X(1); y = X(2); z = X(3); F =[2*L*(z*cos(q(1))-(y+a)*sin(q(1))), 0; 0, 0, 2*L*(z*cos(q(2))+ ((sqrt(3)/2)*x + y/2 +(sqrt(3)/2)*b +c/2)*sin(q(2))), 0; 0, 0, 2*L*(z*cos(q(3))+ ((-sqrt(3)/2)*x + y/2 +(sqrt(3)/2)*b +c/2)*sin(q(3)))]; End  Hàm ma trận jacobi x: function F=matrix_jx(Pa,q,pos) F=zeros(3,3); % cac kich thuoc dong hoc L=Pa(1); l=Pa(2); wb=Pa(3); up=Pa(4); aA=Pa(5); aB=Pa(6); aC=Pa(7); R L T sp = 2*Pa(4)*cos(pi/6); wp = (sqrt(3)*sp)/6; a = -up+wb; b = (sp-sqrt(3)*wb)/2; c = wp -wb/2; x = pos(1); y =pos(2); C C U D z=pos(3); jx1 =[ 2*x; 2*(x - (sqrt(3)/2)*L*cos(q(2))+b); 2*(x + (sqrt(3)/2)*L*cos(q(2))-b)]; jx2 = [ 2*(y+L*cos(q(1))+a) ; 2*(y-(L/2)*cos(q(2))+c) ; Phụ lục Trang Thiết kế chế tạo robot Delta 2*(y-(L/2)*cos(q(3))+c)]; jx3 = [ 2*(z+L*sin(q(1))); 2*(z+L*sin(q(2))); 2*(z+L*sin(q(3)))]; F = [ jx1 jx2 jx3 ]; End  Hàm ma trận đạo hàm jacobi q: function F=matrix_djq(Pa,q,pos,dq,dx) F=zeros(3,3); % cac kich thuoc dong hoc L=Pa(1); l=Pa(2); wb=Pa(3); up=Pa(4); aA=Pa(5); aB=Pa(6); aC=Pa(7); sp = 0.086; wp = (sqrt(3)*sp)/6; a = -up+wb; C C R L T b = (sp-sqrt(3)*wb)/2; c = wp -wb/2; x = pos(1); y =pos(2); z=pos(3); djq11 = -2*L*(z*sin(q(1))-(y+a)*cos(q(1)))*dq(1)2*L*sin(q(1))*dx(2)+2*L*cos(q(1))*dx(3); U D djq22 = 2*L*(z*sin(q(2))+((sqrt(3)*(x+b))/2+(y+c)/2)*cos(q(2)))*dq(2)+sqrt(3)*L*sin(q(2))*dx( 1)+L*sin(q(2))*dx(2)+2*L*cos(q(2))*dx(3); djq33 = 2*L*(-z*sin(q(3))+((sqrt(3)*(-x+b))/2+(y+c)/2)*cos(q(3)))*dq(3)sqrt(3)*L*sin(q(3))*dx(1)+L*sin(q(3))*dx(2)+2*L*cos(q(3))*dx(3); F =[djq11 0; djq22 0; Phụ lục Trang Thiết kế chế tạo robot Delta 0 djq33]; End  Hàm ma trận đạo hàm jacobi x: function F=matrix_djx(Pa,q,dq,dpos) F=zeros(3,3); % cac kich thuoc dong hoc L=Pa(1); l=Pa(2); wb=Pa(3); up=Pa(4); aA=Pa(5); aB=Pa(6); aC=Pa(7); sp = 0.086; wp = (sqrt(3)*sp)/6; a = -up+wb; b = (sp-sqrt(3)*wb)/2; c = wp -wb/2; dx = dpos(1); dy = dpos(2); dz = dpos(3); F =[ 2*dx 2*dz+L*cos(q(1))*dq(1); R L T C C 2*dx+(sqrt(3)*L*sin(q(2))*dq(2))/2 2*dz+L*cos(q(2))*dq(2); 2*dx-(sqrt(3)*L*sin(q(3))*dq(3))/2 2*dz+L*cos(q(3))*dq(3)]; end  Chương trình động học ngược: U D 2*dy-L*sin(q(1))*dq(1) 2*dy+(L*sin(q(2))*dq(2))/2 2*dy+(L*sin(q(3))*dq(3))/2 function F = IK(Pa,X) % X : vi tri p % Pa : Ma tran kich thuoc robot delta %% Thoi gian thuc hien khao sat T = 1; N =100*T; % So diem chia dt=T/N; % Buoc thoi gian Phụ lục Trang Thiết kế chế tạo robot Delta %% Khai bao kich thuoc ma tran tinh toan t =zeros(1,N+1); t(1)=0; u = zeros(3,1); q =zeros(3,N+1); dq =zeros(3,N+1); ddq=zeros(3,N+1); x=zeros(3,N+1); x(:,1)=[0;0;-0.25];%vi tri goc dx=zeros(3,N+1); dx(:,1)=[0;0;0]; ddx=zeros(3,N+1); ddx(:,1)=[0;0;0]; %% Hieu chinh vecto suy rong tai t=0 q0=[0.6213,0.6213,0.6213]';% xac dinh bang phuong phap ve hinh x0 =[0,0,-0.25]'; dx0 =[0,0,0]'; ddx0=[0,0,0]'; u = [(X(1)-x0(1)), (X(2)-x0(2)), (X(3)-x0(3))]';% vec to chi phuong C C R L T delta = [1;1;1]'; while (abs(delta(3))>10^-8)||(abs(delta(2))>10^-8)||(abs(delta(1))>10^-8) delta =-matrix_jq(Pa,q0,x0)\hpt(Pa,q0,x0); delta =delta'; q0 = q0+delta'; end for j=1:3 U D while abs(q0(j))>2*pi q0(j)=q0(j)-2*pi*sign(q0(j)); end end q(:,1) =q0; dq(:,1) =-inv(matrix_jq(Pa,q0,x0))*matrix_jx(Pa,q0,x0)*dx0; Phụ lục Trang Thiết kế chế tạo robot Delta ddq(:,1)= - inv(matrix_jq(Pa,q0,x0))*(matrix_djq(Pa,q0,x0,dq(:,1),dx0)*dx0+matrix_djx(Pa,q 0,dq(:,1),dx0)*dx0+matrix_jx(Pa,q0,x0)*ddx0); %% Hieu chinh vecto suy rong tai ti for i=2:N+1 ti=(i-1)*dt; t(i)=ti; x(:,i) =[x0(1) + u(1)*ti,x0(2) + u(2)*ti ,x0(3)+ u(3)*ti]'; dx(:,i) =[u(1), u(2), u(3)]'; ddx(:,i)=[0,0,0 ]'; q(:,i)=q(:,i-1)+dq(:,i-1)*dt+(1/2)*ddq(:,i-1)*dt^2; delta = [1;1;1]; while (abs(delta(3))>10^-8)||(abs(delta(2))>10^-8)||(abs(delta(1))>10^-8) delta =-inv(matrix_jq(Pa,q(:,i),x(:,i)))*hpt(Pa,q(:,i),x(:,i)); q(:,i)=q(:,i)+delta; for j=1:3 C C R L T while abs(q(j,i))>2*pi q(j,i)=q(j,i)-2*pi*sign(q(j,i)); end end end dq(:,i) =-inv(matrix_jq(Pa,q(:,i),x(:,i)))*matrix_jx(Pa,q(:,i),x(:,i))*dx(:,i); ddq(:,i)= U D inv(matrix_jq(Pa,q(:,i),x(:,i)))*(matrix_djq(Pa,q(:,i),x(:,i),dq(:,i),dx(:,i))*dq(:,i)+ matrix_djx(Pa,q(:,i),dq(:,i),dx(:,i))*dx(:,i)+matrix_jx(Pa,q(:,i),x(:,i))*ddx(:,i)); end F = q(:,N+1); end Phụ lục Trang - ... động robot Delta Matlab Solidwork D  Thiết lập vẽ chế tạo chi tiết robot Nguyễn Viết Long Delta  Thiết Kế 3D chi tiết robot Delta  Nghiên cứu phương pháp điều khiển cho robot Ngày giao đồ án: ... Trang 38 Thiết kế chế tạo robot Delta 3.2.1 Tính chọn thơng số robot Delta: Với vùng làm việc yêu cầu robot Delta hình trụ có rc = 225 mm, h = 250 mm, z1 = 100 mm Ta tính tốn thông số robot Delta. .. toán 37 3.2 Thiết kế hệ thống khí 37 SVTH: Lê Tấn Vinh, Nguyễn Viết Long GVHD: TS Lê Hoài Nam Trang iii Thiết kế chế tạo robot Delta 3.2.1 Tính chọn thông số Robot Delta: