TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI LUẬN VĂN THẠC SĨ Nghiên cứu xây dựng mơ hình Robot in 3D bậc tự Trần Bảo Khánh Khanh.TBCB180011@sis.hust.edu.vn Ngành Kỹ thuật điện tử Giảng viên hướng dẫn: TS Trần Thị Thanh Hải Chữ ký GVHD Viện: Cơ khí HÀ NỘI, 10/2020 CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc BẢN XÁC NHẬN CHỈNH SỬA LUẬN VĂN THẠC SĨ Họ tên tác giả luận văn: Trần Bảo Khánh Đề tài luận văn: Nghiên cứu xây dựng mơ hình Robot in 3D bậc tự Chuyên ngành: Kỹ thuật Cơ điện tử Mã số SV: CB180011 Tác giả, Người hướng dẫn khoa học Hội đồng chấm luận văn xác nhận tác giả sửa chữa, bổ sung luận văn theo biên họp Hội đồng ngày 31 tháng 10 năm 2020 với nội dung sau: Sửa lỗi soạn thảo Việt hóa:  Chuyển sang tiếng Việt số thơng số thích Hình 1.5, Bảng 1.1 Bảng 1.2  Sửa số lỗi soạn thảo luận văn Từ hóa chuyên môn:  Thống cách dùng thuật ngữ “Laser” Hoàn thiện nội dung luận văn làm rõ trọng tâm liên quan đề tài:  Ghép chương chương thành chương, tức chương với tên chương “Phương pháp nghiên cứu mô robot in 3D sáu bậc tự do” nhằm tạo liên kết công nghệ in 3D Robot sáu bậc tự Nội dung chương bổ sung phần “Phương pháp nghiên cứu” cho việc mô robot in 3D Chương nhằm tạo liên kết công nghệ in 3D Robot sáu bậc tự  Phương pháp luận tính tốn thiết kế thơng qua tốn động học động lực học đưa Chương  Đưa hình vẽ mơ Robot sáu bậc tự có dụng cụ gắn khâu thao tác lên Chương  Bổ sung thông số kỹ thuật bao gồm tốc độ tầm với Robot phần 2.3.1 Chương  Bổ sung thông số bao gồm tốc độ phương pháp in 3D kim loại phần 1.2.1 Chương  Bổ sung phần kết luận phù hợp với bố cục Kiểm tra hoàn thiện số bảng biểu hình vẽ theo ý kiến hội đồng:  Bổ sung chi tiết cho sơ đồ nguyên lý công nghệ SLA Hình 1.1 nhằm làm rõ chi tiết bước công nghệ in SLA Ngày 13 tháng 11 năm 2020 Giáo viên hướng dẫn TS Trần Thị Thanh Hải CHỦ TỊCH HỘI ĐỒNG PGS TS Phan Bùi Khôi Tác giả luận văn Trần Bảo Khánh ĐỀ TÀI LUẬN VĂN Tên đề tài: Nghiên cứu xây dựng mơ hình robot in 3D bậc tự Tên tiếng anh: Research and design a robot with degrees of freedom integrated with 3D printing Giáo viên hướng dẫn Ký ghi rõ họ tên LỜI CẢM ƠN Đầu tiên, tơi xin bày tỏ lịng biết ơn chân thành tới TS Trần Thị Thanh Hải hộ trợ suốt q trình thực luận văn Bên cạnh tơi xin gửi lời cảm ơn Trường Đại học Bách Khoa Hà Nội tạo điều kiện thuận lợi cho suốt q trình học tập nghiên cứu TĨM TẮT NỘI DUNG LUẬN VĂN Ở Việt Nam, Robot sử dụng nhiều ngành công nghiệp hàn, sơn, lắp ráp… Mặc dù vậy, nhà máy lớn, robot có cố cần mời chuyên gia từ nước thải loại robot cũ mua robot Điều gây lãng phí to lớn nhiều robot có hệ thống khí tốt Chính nhu cầu cấp thiết đặt yêu cầu cần có Robot mang thương hiệu Việt Nam nhằm giúp cho việc bảo dưỡng Robot thuận tiện Chính điều thúc đẩy tơi lựa chọn đề tài “Nghiên cứu xây dựng mơ hình Robot in 3D bậc tự do” tham gia khơi phục robot bị hỏng hồn tồn mạch điện hệ thống điều khiển Mục đích luận văn nhằm thiết kế hệ thống điều khiển robot tích hợp cơng nghệ in 3D Luận văn trình bày phương pháp nghiên cứu, mô Robot bậc tự với dụng cụ gắn khâu thao tác đầu in 3D Việc lập trình mơ điều khiển Robot thơng qua kết tốn động học động lực học Kết luận văn kiểm tra thông qua điều khiển hai động bước cho hai khớp động khớp động Bên cạnh luận văn trình bày phương pháp in phù hợp với Robot bậc tự Xuất phát từ yêu cầu đầu vào bao gồm thông số vị trí ma trận cacdan, tiến hành lập trình điều khiển sáu khớp động robot Các khớp động điều khiển động thông qua điều khiển PLC S7 1200 chương trình điều khiển viết phần mềm Python Ngoài ra, luận văn sử dụng chương trình Matlab Simulink, Solidwork nhằm mô quỹ đạo robot thực tế Bên cạnh đó, từ mơ hình 3D tiến hành xây dựng mơ chuyển động cho cánh tay Robot nhằm khảo sát chuyển động khâu cuối Các nội dung luận văn trình bày ba chương: Chương trình bày tổng quan in 3D kim loại Chương nêu lên vấn đề chung cho ngành công nghệ in 3D nói chung in 3D kim loại nói riêng Bên cạnh nhược điểm ưu điểm phương pháp Chương trình bày phương pháp nghiên cứu mô robot in 3D bậc tự Chương nêu lên phương pháp nghiên cứu, lý thuyết toán động học động lực học Chương nêu lên thuật toán, mạch điện phương pháp điều khiển robot Chương trình bày kết mô điều khiển robot sáu bậc tự với dụng cụ gắn khâu thao tác đầu in 3D Chương cuối đưa kết mô chuyển động khâu tác cuối bảng điều khiển từ yêu cầu thực tế Kết luận văn đạt yêu cầu ban đầu lập trình chương trình điều khiển sáu động thơng qua python PLC di chuyển theo đường thẳng cong cho Robot bậc tự có gắn dụng cụ đầu in 3D Luận văn mô quỹ đạo di chuyển theo đường thẳng cong Robot bậc tự Kết tính tốn, mơ phục vụ cho chế tạo hệ thống điều khiển thực tiễn cho robot sáu bậc tự nghiên cứu sau HỌC VIÊN Ký ghi rõ họ tên LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải MỤC LỤC CHƯƠNG TỔNG QUAN VỀ IN 3D KIM LOẠI 1.1 1.2 In 3D 1.1.1 Tổng quan in 3D 1.1.2 Ưu nhược điểm 1.1.3 Nguyên tắc chung in 3D 1.1.4 Một số vật liệu in 3D 1.1.5 Các phương pháp in 3D In 3D kim loại 11 1.2.1 Các phương pháp in 3D kim loại phổ biến 11 1.2.2 Các vật liệu in 3D kim loại 13 1.3 Đầu in in 3D 14 1.4 Laser dùng in 3D 15 1.4.1 1.5 Một số loại laser thường sử dụng 15 Kết luận 17 CHƯƠNG PHƯƠNG PHÁP NGHIÊN CỨU VÀ MÔ PHỎNG ROBOT IN 3D BẬC TỰ DO 18 2.1 Phương pháp nghiên cứu 18 2.2 Phương pháp luận tính tốn động học động lực học robot 18 2.2.1 Giải toán động học thuận Robot 18 2.2.2 Giải toán động học ngược Robot 23 2.2.3 Giải toán động lực học cho robot 23 2.3 Thiết kế hệ thống điều khiển robot sáu bậc tự với dụng cụ gắn khâu thao tác đầu in 3D 27 2.4 2.3.1 Kết cấu cánh tay robot sáu bậc tự 27 2.3.2 Bài toán động học 29 2.3.3 Bài toán Động Lực học 31 2.3.4 Mô quỹ đạo robot 43 Lập trình điều khiển robot sáu bậc tự 44 2.4.1 Các phân tử điều khiển 44 2.4.2 Mạch điều khiển 45 2.4.3 Lập trình điều khiển 47 LUẬN VĂN THẠC SĨ 2.5 2.6 GVHD: TS Trần T.Thanh Hải Tích hợp điều khiển Robot sáu bậc tự in 3D 50 2.5.1 Lựa chọn phương pháp in 3D 50 2.5.2 Lựa chọn đầu in tích hợp robot 50 2.5.3 Lựa chọn vật liệu in 51 2.5.4 Tích hợp với đầu in 3D 52 Kết luận 53 CHƯƠNG KẾT QUẢ MÔ PHỎNG ĐIỀU KHIỂN ROBOT SÁU BẬC TỰ DO VỚI DỤNG CỤ TRÊN KHÂU THAO TÁC LÀ ĐẦU IN 3D 54 3.1 3.2 Mô điều khiển Robot sáu bậc tự 54 3.1.1 Điều khiển động 54 3.1.2 Mô chuyển động Robot Matlab 55 Kết luận 58 KẾT LUẬN 59 TÀI LIỆU THAM KHẢO 61 PHỤ LỤC 62 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải DANH MỤC HÌNH VẼ Hình 1.1 Sơ đồ cơng ngun lý công nghệ SLA Hình 1.2 Sơ đồ ngun lý cơng nghệ FDM Hình 1.3 Sơ đồ ngun lý cơng nghệ LOM Hình 1.4 Sơ đồ ngun lý cơng nghệ SLS Hình 1.5 Sơ đồ nguyên lý công nghệ SLM 10 Hình 2.1 Các trục tọa độ D-H 20 Hình 2.2 Cánh tay robot ứng dụng 27 Hình 2.3 Hệ tọa độ gắn theo bảng D-H 27 Hình 2.4 Ba dạng profile tốc độ phổ biến 43 Hình 2.5 Mạch điều khiển điện 46 Hình 2.6 Sơ đồ khối thiết bị 46 Hình 2.7 Sơ đồ thuật tốn 47 Hình 2.8 Một đoạn chương trình tính tốn 48 Hình 2.9 Một đoạn chương trình kết nối Python PLC 49 Hình 2.10 Kết chương trình Python 49 Hình 2.11 Kết chương trình TIA Portal 50 Hình 2.12 Robot tích hợp đầu in 52 Hình 3.1 Kết nối chương trình 54 Hình 3.2 Chương trình động học thuận tính tốn giá trị điểm cuối 55 Hình 3.3 Kết điều khiển tìm vị trí sáu góc quay điểm 56 Hình 3.4 Kết điều khiển tìm vị trí sáu góc quay điểm 56 Hình 3.5 Quỹ đạo mơ robot 57 Hình 3.6 Quãng đường di chuyển điểm cuối theo trục x 57 Hình 3.7 Quãng đường di chuyển điểm cuối theo trục y 57 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải DANH MỤC BẢNG Bảng 1.1 Sử dụng Laser hãng sản xuất máy thương mại 15 Bảng 1.2 Độ hấp thụ tia Laser số loại vật liệu 16 Bảng 2.1 Bảng D-H 28 Bảng 2.2 Vị trí khối tâm khâu 31 Bảng 2.3 Momen quán tính khối khâu vị trí trọng tâm Ci 32 Bảng 2.4 Thông số động cho khớp 45 Bảng 2.5 Bảng hợp kim AlSi10Mg 51 Bảng 2.6 Các thơng số tích hợp đầu in khâu cuối 52 Bảng 3.1 Thông số di chuyển 54 Bảng 3.2 Thông số góc quay theo điểm đầu vào 55 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải CHƯƠNG KẾT QUẢ MÔ PHỎNG ĐIỀU KHIỂN ROBOT SÁU BẬC TỰ DO VỚI DỤNG CỤ TRÊN KHÂU THAO TÁC LÀ ĐẦU IN 3D 3.1 Mô điều khiển Robot sáu bậc tự 3.1.1 Điều khiển động Chương trình lập trình điều khiển Robot sáu bậc tự thực theo thuật tốn trình bày kết nối điều khiển thực hai động bước cho hai khớp động khớp động nhằm kiểm chứng kết toán điều khiển Hai động bước kết nối qua Microstep Driver P5C PLC S7-1200 Hình 3.1 Cổng Q0.0 Q0.11 nối với đầu vào cấp tin hiệu chạy thuận ngược chiều động số Cổng Q0.2 Q0.3 nối với đầu vào cấp tín hiệu chạy thuận ngược chiều động số Cổng Q0.4 sử dụng làm đầu phát tín hiệu phanh cho hai động PLC Hình 3.1 Kết nối chương trình Yêu cầu đầu vào robot chạy theo đường thẳng từ điểm (0,0,0) tới điểm (0.5,0.5,0) tới điểm (0.5,0.7,0) với góc cardan (0,0,0) Các thơng số di chuyển trình bày Bảng 3.1 Bảng 3.1 Thông số di chuyển Thống số (mm) (mm) (mm) (rad) (rad) (rad) 54 Điểm bắt đầu 0 0 0 Điểm 0.5 0.5 0 0 Điểm 0.5 0.7 0 0 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải Với yêu cầu đến điểm điểm 2, sáu khớp động cần quay với giá trị Bảng 3.2 Bảng 3.2 Thông số góc quay theo điểm đầu vào Biến khớp Góc quay đến điểm Góc quay đến điểm 0.90781 0.95994 (rad) 0.48973 0.51786 (rad) 0.064801 0.23869 (rad) -2.8855 -2.7664 (rad) 0.43903 0.59674 (rad) -0.55216 -0.61137 (rad) Kết kiểm tra hai động bước cho khớp động khớp động cho thấy động chạy với yêu cầu đầu vào 3.1.2 Mô chuyển động Robot Matlab Robot mô chuyển động Matlab thơng qua phương trình động học viết lại từ Python Hình 3.2 chương trình động học thuận, sử dụng để mô chương trình điều khiển sáu động Việc mơ quỹ đạo điều khiển Matlab nhằm kiểm tra độ tin cậy chương trình với robot thực tế Hình 3.3 Hình 3.4 mơ giao diện điều khiển sáu góc quay sáu biến khớp điểm cuối Robot di chuyển tới điểm điểm + RUN: Chạy chương trình, + Default: Đưa robot chương trình vị trí cũ, + Stop: Dừng chương trình, + INVERSE: Tính tốn góc quay thơng qua vị trí điểm cuối Hình 3.2 Chương trình động học thuận tính tốn giá trị điểm cuối Hình 3.5 mơ chạy tự động theo giá trị yêu cầu đầu vào di chuyển theo hai đường thẳng đường tròn Quỹ đạo điểm cuối robot cần di chuyển liên 55 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải tục, không bị ngắt quãng Yêu cầu đặt khâu cuối cần di chuyển từ điểm (0,0,0) tới (0.5,0.5,0) sau di chuyển tới (0.5,0.7,0) Tại vị trí yêu cầu tiếp tục di chuyển theo cung trịn với đường kính 0.6 có tâm điểm (0.5,0.4,0) Kết mô đưa tương đương cho thấy phương trình động lực học robot xác Nút RUN chạy chương trình Nút Default đưa biến khớp Nút Pause dừng chương trình Nút INVERSE trả giá trị sáu biến khớp đáp ứng u cầu vị trí điểm cuối Hình 3.3 Kết điều khiển tìm vị trí sáu góc quay điểm Thanh điều sáu góc quay sáu biến khớp Thơng số sáu góc quay sáu biến khớp (q1, q2, q3, q4, q5, q6) (q1, q2, q3, q4, q5, q6) Thanh điều khiển vị trí hướng điểm cuối khâu thao tác Thơng số vị trí hướng điểm cuối khâu thao tác (Px, Py, Pz, alpha, beta, gamma) (Px, Py, Pz, alpha, beta, gamma) Hình 3.4 Kết điều khiển tìm vị trí sáu góc quay điểm Cũng cần kiểm tra quãng đường di chuyển robot xác theo phương x,y Hình 3.6 Hình 3.7 Kết đầu cho thấy quỹ đạo đầu vào quỹ đạo đầu điểm cuối trùng khớp với 56 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải Hình 3.5 Quỹ đạo mơ robot Hình 3.6 Qng đường di chuyển điểm cuối theo trục x Hình 3.7 Quãng đường di chuyển điểm cuối theo trục y 57 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải 3.2 Kết luận Chương trình bày kết mơ lập trình điều khiển Robot sáu bậc tự có khâu thao tác cuối gắn đầu in 3D thực theo thuật tốn chương trình điều khiển Chương kết nối điều khiển hai động bước cho hai khớp động khớp động nhằm kiểm chứng kết toán điều khiển khâu thao tác cuối di chuyển theo hai điểm cho trước Chương trình bày kết mơ chuyển động robot sáu bậc tự thông qua phần mềm Matlab Simulink Mục đích việc mơ nhằm kiểm tra độ tin cậy chương trình với robot thực tế 58 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải KẾT LUẬN Việc sản xuất Robot hàng loạt mẻ nước ta cho dù việc ứng dụng Robot sản xuất xuất từ lâu Điều dẫn đến việc lãng phí tài ngun nhiều Robot khơng tái sử dụng hệ thống khí hoạt động tốt Trên sở áp dụng lý thuyết toán động học động lực, ta xây dựng chương trình chuyển động cho Robot sáu bậc tự với dụng cụ gắn khâu thao tác đầu in 3D Có thể thấy rằng, phần quan trọng robot xây dựng hệ thống điều khiển sáu khớp động thông qua chương trình điểu khiển động khớp động qua điều khiển PLC Bên cạnh đó, việc áp dụng phần mềm Matlab cho phép mô quỹ đạo robot cách xác Luận văn trình bày ba chương, chương trình bày tổng quan phương pháp in 3D nói chung in 3D kim loại nói riêng FDM Những cơng nghệ có ưu điểm nhược điểm, tùy vào yêu cầu sản phẩm yêu cầu sử dụng lựa chọn phương pháp phù hợp Chương giới thiệu tổng quát dạng vật liệu in sử dụng ngành công nghiệp in 3D dụng cụ sử dụng phương pháp in phổ biến Chương trình bày phương pháp nghiên cứu cho mô Robot sáu bậc tự với khâu thao tác cuối gắn đầu in 3D Chương trình bày thiết kế hệ thống lập trình điều khiển Robot sáu bậc tự thơng qua việc giải tốn động học động lực học Thiết lập phương trình cho toán động học động lực học dựa vào phương pháp Denavit-Hartenberg Bài toán động học giúp cho việc tìm tọa độ hướng khâu thao tác cuối mà robot Trong tốn động lực học giúp tìm lực momen tác dụng khớp xây dựng quỹ đạo robot Chương trình Visual Studio với ngôn ngữ Python sử dụng để tìm kết góc quay sáu khớp động Robot Kết từ chương trình Visual Studio đưa trực tiếp vào nhớ chương trình TIA Portal để truyền tín hiệu cho hai PLC S7-1200 Chương trình giao tiếp Visual Studio TIA Portal sử dụng SNAP7 Chương trình bày việc lựa chọn phương pháp in FDM, đầu in đồng thau loại vật liệu AlSi10Mg dùng cho việc in 3D kim loại sử dụng phương pháp FDM Việc lựa chọn phương pháp, đầu in vật liệu giúp tối ưu hóa khả Robot sáu bậc tự Chương trình bày kết điều khiển hai động bước cho hai khớp động khớp động nhằm kiểm chứng kết chương trình điều khiển Robot sáu bậc tự có khâu thao tác cuối gắn đầu in 3D Chương trình bày kết mơ quỹ đạo robot sáu bậc tự phần mềm Matlab Kết chương đáp ứng yêu cầu ban đầu với khâu thao tác di chuyển theo hai điểm, chương trình mơ cho thấy phương trình động học, động lực học xác Kết luận văn mô chuyển động Robot sáu bậc tự đồng thời lập trình điều khiển cho Robot sáu bậc tự có khâu thao tác gắn với dụng 59 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải cụ đầu in 3D Mặc dù phạm vi luận văn giải bước đầu điều khiển chuyển động Robot Hướng phát triển luận văn - Kết nối chương trình điều khiển với sáu động robot sáu bậc tự - Lắp động lên Robot - Tối ưu hóa chương trình điều khiển Robot sáu bậc tự 60 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải TÀI LIỆU THAM KHẢO [1] Sharretts Plating Co., Inc; Materials are used for printing; Available: https://www.sharrettsplating.com/blog/materials-used-3d-printing/ [2] Phùng Anh Tiến; In 3D: tương lai, Cục thông tin khoa học công nghệ quốc gia [3] Milan Brandt; Laser Additive Manufacturing, Woodhead Publishing, 2017 [4] Mona Saffarzadeh, M.S., Gregory James Gillispie, M.S., Philip Brown, PhD; " Selectic Laser sintering (SLS) rapid protyping technology: A review of medical applications", April, 2016 [5] M Shellabear, O Nyrhilä; "DMLS – Development History And State Of The Art", Germany [6] Reinhart Poprawe, Christian Hinke, Wilhelm Meiners,Johannes Schrage, Sebastian Bremen and Simon Merkt; "SLM Production Systems: Recent Developments in Process Development,Machine Concepts and Component Design", November, 2015 [7] The Different Types of 3D Printers, Available: https://3dinsider.com/3dprinter-types/ [8] Dinusha Mendis, Mark Lemley and Matthew Rimmer; 3D Printing and Beyondt, Edward Elgar, 2019 [9] Sean Aranda, 3D Printing Failures, 2019 [10] Hyub Lee, Chin Huat Joel Lim, Mun Ji Low, Nicholas Tham,Vadakke Matham Murukeshan, and Young-Jin Kim, " Lasers in Additive Manufacturing: A Review" in International journal of precision engineering and manufacturing-green technology, July, 2017 [11] PGS.TS Tạ Duy Liêm; Máy moc thiết bị công nghệ cao sản xuất khí, "Robot hệ thống cơng nghệ Robot hóa" [12] GS.TSKH Nguyễn Văn Khang; Động lực học hệ nhiều vật Nhà xuất khoa học kỹ thuật, 2017 [13] TS Nguyễn Mạnh Tiến; Điều khiển robot công nghiệp, Nhà xuất Khoa học Kỹ thuật, 2007 [14] Cao Xn Hồng, Lập trình quỹ đạo điều khiển chuyển động robot công nghiệp,Luận văn thạc sĩ điện tử, Đại học Bác Khoa Hà Nội 2011 [15] Python Documentary; Available: https://www.python.org/doc/ [16] Siemens Inc; S7-1200 Programmable controller, 61 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải PHỤ LỤC Chương trình lập trình động học cho Robot Python: import re from ctypes import c_int, c_char_p, byref, sizeof, c_uint16, c_int32, c_byte from ctypes import c_void_p from datetime import datetime import logging import snap7 from snap7.snap7types import S7Object, buffer_type, buffer_size, BlocksList from snap7.snap7types import TS7BlockInfo, param_types, cpu_statuses from snap7.common import check_error, load_library, ipv4 from snap7.snap7exceptions import Snap7Exception from time import sleep import struct # robot from sympy import symbols, cos, sin, pi, simplify, pprint, tan, expand_trig, s qrt, trigsimp, atan2 from sympy.matrices import Matrix import numpy as np # robot logger = logging.getLogger( name ) IP = '192.168.1.10' RACK = SLOT = DB_NUMBER = 100 plc = snap7.client.Client() plc.connect(IP,RACK,SLOT) print(plc.get_cpu_state()) #robot def pose(theta, alpha, a, d): r11, r12 = cos(theta), -sin(theta) r23, r33 = -sin(alpha), cos(alpha) r21 = sin(theta) * cos(alpha) r22 = cos(theta) * cos(alpha) r31 = sin(theta) * sin(alpha) r32 = cos(theta) * sin(alpha) 62 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải y = -d * sin(alpha) z = d * cos(alpha) T = Matrix([ [r11, r12, 0.0, a], [r21, r22, r23, y], [r31, r32, r33, z], [0.0, 0.0, 0.0, 1] ]) T = simplify(T) return T def forward_kin(q1,q2,q3,q4,q5,q6): X = [] Y = [] Z = [] d90 = pi/2 T01 = pose(q1, 0, 0, 0.75) T0g = T01 px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3] X.append(px) Y.append(py) Z.append(pz) T12 = pose(q2 - d90, -d90, 0.35, 0) T0g = T0g* T12 px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3] X.append(px) Y.append(py) Z.append(pz) T23 = pose(q3, 0, 1.25, 0) T0g = T0g* T23 px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3] X.append(px) Y.append(py) Z.append(pz) T34 = pose(q4, -d90, -0.054, 1.5) T0g = T0g* T34 px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3] X.append(px) Y.append(py) Z.append(pz) T45 = pose(q5, d90, 0, 0) T0g = T0g* T45 px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3] X.append(px) Y.append(py) Z.append(pz) T56 = pose(q6, -d90, 0, 0) T0g = T0g* T56 63 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3] X.append(px) Y.append(py) Z.append(pz) T6g = pose(0, 0, 0, 0.303) T0g = T0g* T6g px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3] X.append(px) Y.append(py) Z.append(pz) X = np.reshape(X,(1,7)) Y = np.reshape(Y,(1,7)) Z = np.reshape(Z,(1,7)) return X,Y,Z def cardanAnglesToRotationMatrix(theta) : R_x = np.array([[1, [0, [0, ]) 0, ], math.cos(theta[0]), -math.sin(theta[0]) ], math.sin(theta[0]), math.cos(theta[0]) ] R_y = np.array([[math.cos(theta[1]), [0, [-math.sin(theta[1]), ]) 0, 1, 0, R_z = np.array([[math.cos(theta[2]), [math.sin(theta[2]), [0, ]) -math.sin(theta[2]), math.cos(theta[2]), 0, R = np.dot(R_z, np.dot( R_y, R_x )) return R # Checks if a matrix is a valid rotation matrix def isRotationMatrix(R) : Rt = np.transpose(R) shouldBeIdentity = np.dot(Rt, R) I = np.identity(3, dtype = R.dtype) n = np.linalg.norm(I - shouldBeIdentity) return n < 1e-6 def rotationMatrixTocardanAngles(R) : 64 math.sin(theta[1]) math.cos(theta[1]) 0], 0], 1] ], ], ] LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải assert(isRotationMatrix(R)) sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) singular = sy < 1e-6 if not x = y = z = else : x = y = z = singular : math.atan2(R[2,1] , R[2,2]) math.atan2(-R[2,0], sy) math.atan2(R[1,0], R[0,0]) math.atan2(-R[1,2], R[1,1]) math.atan2(-R[2,0], sy) return np.array([x, y, z]) def get_hypotenuse(a, b): return sqrt(a*a + b*b) def get_cosine_law_angle(a, b, c): cos_gamma = (a*a + b*b - c*c) / (2*a*b) sin_gamma = sqrt(1 - cos_gamma * cos_gamma) gamma = atan2(sin_gamma, cos_gamma) return gamma def get_wrist_center(gripper_point, R0g, dg = 0.303): xu, yu, nx, ny, xw = xu yw = yu zw = zu zu = nz = - dg - dg - dg gripper_point R0g[0, 2], R0g[1, 2], R0g[2, 2] * nx * ny * nz return xw, yw, zw def get_last_three_angles(R): sin_q4 = R[2, 2] cos_q4 = -R[0, 2] sin_q5 = sqrt(R[0, 2]**2 + R[2, 2]**2) 65 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải cos_q5 = R[1, 2] sin_q6 = -R[1, 1] cos_q6 = R[1, 0] q4 = atan2(sin_q4, cos_q4) q5 = atan2(sin_q5, cos_q5) q6 = atan2(sin_q6, cos_q6) return q4, q5, q6 def get_angles(x, y, z, alpha, beta, gamma): gripper_point = x, y, z q1, q2, q3, q4, q5, q6 = symbols('q1:7') alpha, beta, gamma = symbols('alpha beta gamma', real = True) px, py, pz = symbols('px py pz', real = True) R03 = Matrix([ [sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)], [sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3), cos(q1)], [ cos(q2 + q3), -sin(q2 + q3), 0]]) R03T = Matrix([ [sin(q2 + q3)*cos(q1), sin(q1)*sin(q2 + q3), cos(q2 + q3)], [cos(q1)*cos(q2 + q3), sin(q1)*cos(q2 + q3), -sin(q2 + q3)], [ -sin(q1), cos(q1), 0]]) R36 = Matrix([ [-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) sin(q6)*cos(q4)*cos(q5), -sin(q5)*cos(q4)], [ sin(q5)*cos(q6), sin(q5)*sin(q6), cos(q5)], [-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4), sin(q4)*sin(q6)*cos(q5) cos(q4)*cos(q6), sin(q4)*sin(q5)]]) R0u = Matrix([ [1.0*cos(alpha)*cos(beta), 1.0*sin(alpha)*cos(gamma) + sin(beta)*sin(gamma)*cos(alpha), 1.0*sin(alpha)*si n(gamma) + sin(beta)*cos(alpha)*cos(gamma)], [1.0*sin(alpha)*cos(beta), sin(alpha)*sin(beta)*sin(gamma) + 1.0*cos(alph a)*cos(gamma), sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha)], [ 1.0*sin(beta), 1.0*sin(gamma)*cos(beta), 1.0*cos(beta)*cos(gamma)]]) T0g_b = Matrix([ [1.0*sin(alpha)*sin(gamma) + sin(beta)*cos(alpha)*cos(gamma), 1.0*sin(alp ha)*cos(gamma) 1.0*sin(beta)*sin(gamma)*cos(alpha), 1.0*cos(alpha)*cos(beta), px], 66 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải [sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha), 1.0*sin(alpha)*sin(beta)*sin(gamma) 1.0*cos(alpha)*cos(gamma), 1.0*sin(alpha)*cos(beta), py], [ 1.0*cos(beta)*cos(gamma), -1.0*sin(gamma)*cos(beta), 1.0*sin(beta), pz], [ 0, 0, , 1]]) T0g_a = Matrix([ [((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q 1)*cos(q2 + q3))*cos(q6) - (sin(q1)*cos(q4) + sin(q4)*sin(q2 + q3)*cos(q1))*sin(q6), ((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*co s(q2 + q3))*sin(q6) + (sin(q1)*cos(q4) sin(q4)*sin(q2 + q3)*cos(q1))*cos(q6), (sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos (q2 + q3), -0.303*sin(q1)*sin(q4)*sin(q5) + 1.25*sin(q2)*cos(q1) 0.303*sin(q5)*sin(q2 + q3)*cos(q1)*cos(q4) 0.054*sin(q2 + q3)*cos(q1) + 0.303*cos(q1)*cos(q5)*cos(q2 + q3) + 1.5*cos(q1) *cos(q2 + q3) + 0.35*cos(q1)], [ ((sin(q1)*sin(q2 + q3)*cos(q4) sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*cos(q6) (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*sin(q6), ((sin(q1)*sin(q2 + q3)*cos(q4) sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*sin(q6) (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*cos(q6), (sin(q1)*sin(q2 + q3)*cos(q4) sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3), 1.25*sin(q1)*sin(q2 ) - 0.303*sin(q1)*sin(q5)*sin(q2 + q3)*cos(q4) 0.054*sin(q1)*sin(q2 + q3) + 0.303*sin(q1)*cos(q5)*cos(q2 + q3) + 1.5*sin(q1) *cos(q2 + q3) + 0.35*sin(q1) + 0.303*sin(q4)*sin(q5)*cos(q1)], [ (sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*cos(q6) sin(q4)*sin(q6)*cos(q2 + q3), (sin(q5)*sin(q2 + q3) cos(q4)*cos(q5)*cos(q2 + q3))*sin(q6) sin(q4)*cos(q6)*cos(q2 + q3), sin(q5)*cos(q4)*cos(q2 + q3) sin(q2 + q3)*cos(q5), -0.303*sin(q5)*cos(q4)*cos(q2 + q3) 0.303*sin(q2 + q3)*cos(q5) - 1.5*sin(q2 + q3) + 1.25*cos(q2) 0.054*cos(q2 + q3) + 0.75], [ 0, 67 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải 0, 0, 1]]) Rgu_eval = Matrix([[0, 0, 1], [0, 1.00000000000000, 0], [1.00000000000000, 0, 0]]) RguT_eval = Matrix([[0, 0, 1], [0, 1.00000000000000, 0], [1.00000000000000, 0, 0]]) R0u_eval = R0u.evalf(subs = {alpha: alpha, beta: beta, gamma: gamma}) R0g_eval = R0u_eval * RguT_eval wrist_center = get_wrist_center(gripper_point, R0g_eval, dg = 0.303) j1, j2, j3 = get_first_three_angles(wrist_center) R03T_eval = R03T.evalf(subs = {q1: j1.evalf(), q2: j2.evalf(), q3: j3.evalf( )}) R36_eval = R03T_eval * R0g_eval j4, j5, j6 = get_last_three_angles(R36_eval) j1 j2 j3 j4 j5 j6 = = = = = = j1.evalf() j2.evalf() j3.evalf() j4.evalf() j5.evalf() j6.evalf() return j1, j2, j3, j4, j5, j6 plc.db_write(DB_NUMBER, 0, bytearray(struct.pack(">f", q1))) q1_ = plc.db_read(DB_NUMBER, 0, 4) q1_ = struct.unpack('>f', q1_) print("Value = {}".format(q1_)) plc.db_write(DB_NUMBER, 4, bytearray(struct.pack(">f", q2))) plc.db_write(DB_NUMBER, 8, bytearray(struct.pack(">f", q3))) plc.db_write(DB_NUMBER, 12, bytearray(struct.pack(">f", q4))) plc.db_write(DB_NUMBER, 16, bytearray(struct.pack(">f", q5))) plc.db_write(DB_NUMBER, 20, bytearray(struct.pack(">f", q6))) 68 ... “Phương pháp nghiên cứu mô robot in 3D sáu bậc tự do? ?? nhằm tạo liên kết công nghệ in 3D Robot sáu bậc tự Nội dung chương bổ sung phần “Phương pháp nghiên cứu? ?? cho việc mô robot in 3D Chương nhằm... cụ đầu in 3D 17 LUẬN VĂN THẠC SĨ GVHD: TS Trần T.Thanh Hải CHƯƠNG PHƯƠNG PHÁP NGHIÊN CỨU VÀ MÔ PHỎNG ROBOT IN 3D BẬC TỰ DO 2.1 Phương pháp nghiên cứu Robot sáu bậc tự nghiên cứu luận văn robot. .. ĐỀ TÀI LUẬN VĂN Tên đề tài: Nghiên cứu xây dựng mơ hình robot in 3D bậc tự Tên tiếng anh: Research and design a robot with degrees of freedom integrated with 3D printing Giáo viên hướng dẫn Ký