Nghiên cứu, thiết kế, chế tạo robot song song bốn bậc tự do điều khiển bằng raspberry kết hợp xử lí ảnh ứng dụng trong dây chuyền sản xuất khoa đào tạo chất lượng cao ngành công nghệ kỹ thuật cơ điện t

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT CƠ ĐIỆN TỬ NGHIÊN CỨU, THIẾT KẾ, CHẾ TẠO ROBOT SONG SONG BỐN BẬC TỰ DO ĐIỀU KHIỂN BẰNG RASPBERRY KẾT HỢP XỬ LÍ ẢNH ỨNG DỤNG TRONG DÂY CHUYỀN SẢN XUẤT GVHD: ThS NGUYỄN MINH TRIẾT SVTH: ĐINH KIÊN CƯỜNG ĐÀO NGỌC SANG LÊ HẢI ĐĂNG SKL008110 Tp Hồ Chí Minh, tháng 07/2021 TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP NGHIÊN CỨU, THIẾT KẾ, CHẾ TẠO ROBOT SONG SONG BỐN BẬC TỰ DO ĐIỀU KHIỂN BẰNG RASPBERRY KẾT HỢP XỬ LÍ ẢNH ỨNG DỤNG TRONG DÂY CHUYỀN SẢN XUẤT SVTH ĐINH KIÊN CƯỜNG ĐÀO NGỌC SANG LÊ HẢI ĐĂNG MSSV 17146091 17146179 17146103 NGÀNH: CÔNG NGHỆ KĨ THUẬT CƠ ĐIỆN TỬ GVHD: THS NGUYỄN MINH TRIẾT KHÓA: 2017-2021 Tp Hồ Chí Minh, tháng năm 2021 TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HCM KHOA ĐÀO TẠO CHẤT LƯỢNG CAO CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự – Hạnh phúc NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Giảng viên hướng dẫn: Th.S Nguyễn Minh Triết Sinh viên: Đinh Kiên Cường MSSV:17146091 Lớp:17146CL1B ĐT:0375979242 Đào Ngọc Sang MSSV:17146179 Lớp:17146CL1B ĐT:0888271727 Lê Hải Đăng MSSV:17146103 Lớp:17146CL1B ĐT:0868042212 Tên đề tài: Nghiên cứu, thiết kế, chế tạo robot song song bốn bậc tự điều khiển raspberry kết hợp xử lí ảnh ứng dụng dây chuyền sản xuất Các số liệu, tài liệu ban đầu: - Kích thước dự kiến: 660x660x800 mm (Dài, rộng, cao), khối lượng tổng thể: 50 Kg - Miền làm việc: 0.266 m3 cách tâm đế 0.432 m (dựa kết động học thuận) - Khối lượng tối đa mà End Effector tải được: Kg Nội dung đồ án: - Tính tốn động học, động lực học, vùng làm việc, quy hoạch quỹ đạo Robot - Kiểm nghiệm động học, viết chương trình mơ quỹ đạo chuyển động Robot MATLAB Simulink kết hợp với Simscape Multibody - Thi cơng mơ hình thực tế Robot dựa mơ hình Inventor thiết kế mơ - Lập trình điều khiển chuyển động Robot Raspberry kết hợp camera xử lí ảnh Các sản phẩm dự kiến: - Mơ hình thực nghiệm robot song song Quattro bậc tự - Chương trình điều khiển - Quyển báo cáo đồ án tốt nghiệp Ngày giao đồ án: Ngày nộp đồ án: Ngơn ngữ trình bày: TRƯỞNG KHOA (Ký, ghi rõ họ tên) Bản báo cáo: Tiếng Anh  Tiếng Việt x Trình bày bảo vệ: Tiếng Anh  Tiếng Việt x TRƯỞNG NGÀNH GIẢNG VIÊN HƯỚNG DẪN (Ký, ghi rõ họ tên) (Ký, ghi rõ họ tên) i TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HCM KHOA ĐÀO TẠO CHẤT LƯỢNG CAO CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự – Hạnh phúc PHIẾU NHẬN XÉT ĐỒ ÁN TỐT NGHIỆP (Dành cho giảng viên hướng dẫn) Họ tên sinh viên: Đinh Kiên Cường MSSV: 17146091 Họ tên sinh viên: Đào Ngọc Sang MSSV: 17146179 Họ tên sinh viên: Lê Hải Đăng MSSV: 17146103 Tên đề tài: Nghiên cứu, thiết kế, chế tạo robot song song bốn bậc tự điều khiển raspberry kết hợp xử lí ảnh ứng dụng dây chuyền sản xuất Ngành đào tạo: Công nghệ kỹ thuật điện tử - Khóa 2017-2021 Họ tên GV hướng dẫn: Nguyễn Minh Triết 0184 Ý KIẾN NHẬN XÉT Nhận xét tinh thần, thái độ làm việc sinh viên Các sinh viên có tinh thần làm việc tốt: Cơng việc nhóm triển khai định kỳ có báo cáo tiến độ với GVHD Thái độ làm việc cầu thị giải vấn đề cách hiệu Đơi nhóm có khó khăn thiết kế mạch điện lập trình, nhiên nhóm cố gắng giải vấn đề đạt kết tốt Nhận xét kết thực ĐATN 2.1.Kết cấu, cách thức trình bày ĐATN: Kết cấu, cách thức trình bày ĐATN tuân thủ theo qui định Khoa 2.2 Nội dung đồ án: (Cơ sở lý luận, tính thực tiễn khả ứng dụng đồ án, hướng nghiên cứu tiếp tục phát triển) Nội dung đồ án gồm chương (Chương giới thiệu tổng quan đề tài Chương trình bày sở lí thuyết mơ ngun lí hoạt động Chương nghiên cứu phân tích tính toán động học robot; Chương kiểm nghiệm động học mô MATLAB Simulink Thiết kế thi cơng mơ hình thể Chương Chương trình bày giải thuật điều khiển Robot Cuối cùng, chương đưa kết luận phương hướng phát triển mơ hình xây dựng.) Phần khí mạch điện thiết kế, chế tạo để làm mơ hình thực nghiệm cho giải thuật điều khiển Tuy nhiên, tình hình dịch Covid 19, phần thực nghiệm chưa triển khai nhiều kế hoạch ban đầu 2.3 Kết đạt được: Mơ hình máy hoạt động theo u cầu thiết kế ii Thuyết minh đề tài Các vẽ khí điện 2.4 Những tồn (nếu có): Do thời gian có hạn nên nhóm chưa có thiết bị đo để kiểm nghiệm độ chính xác robot Từ chưa đánh giá độ chính xác robot chưa đưa thuật toán điều khiển nâng cao vào robot Đánh giá: Điểm tối đa Mục đánh giá TT Hình thức kết cấu ĐATN 30 Điểm đạt 30 Đúng format với đầy đủ hình thức nội dung mục 10 10 Mục tiêu, nhiệm vụ, tổng quan đề tài 10 10 Tính cấp thiết đề tài 10 10 Nội dung ĐATN 50 50 Khả ứng dụng kiến thức toán học, khoa học kỹ thuật, khoa học xã hội… 5 Khả thực hiện/phân tích/tổng hợp/đánh giá 10 10 Khả thiết kế chế tạo hệ thống, thành phần, quy trình đáp ứng yêu cầu đưa với ràng buộc thực tế 15 15 Khả cải tiến phát triển 15 15 5 Khả sử dụng công cụ kỹ thuật, phần mềm chuyên ngành… Đánh giá khả ứng dụng đề tài 10 4 Sản phẩm cụ thể ĐATN 10 10 100 94 Tổng điểm Kết luận:  Được phép bảo vệ  Không phép bảo vệ TP.HCM, ngày tháng năm 2021 Giảng viên hướng dẫn ((Ký, ghi rõ họ tên) iii KHOA ĐÀO TẠO CHẤT LƯỢNG CAO NGÀNH: CNKT CƠ ĐIỆN TỬ CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh Phúc ******* PHIẾU NHẬN XÉT ĐỒ ÁN TỐT NGHIỆP (Dành cho giảng viên phản biện) Họ tên sinh viên: Đinh Kiên Cường MSSV: 17146091 Hội đồng: Họ tên sinh viên: Đào Ngọc Sang MSSV: 17146179 Hội đồng: Họ tên sinh viên: Lê Hải Đăng MSSV: 17146103 Hội đồng: Tên đề tài: Nghiên cứu, thiết kế, chế tạo robot song song bốn bậc tự điều khiển raspberry kết hợp xử lí ảnh ứng dụng dây chuyền sản xuất Ngành đào tạo: Công nghệ ky thuật Cơ điện tử Họ tên GV hướng dẫn: Th.S Nguyễn Minh Triết Ý KIẾN NHẬN XÉT Về nội dung đề tài & khối lượng thực hiện: Ưu điểm: Khuyết điểm: iv Điểm đánh giá cụ thể ĐIỂM TỐI ĐA TT MỤC ĐÁNH GIÁ Hình thức kết cấu luận án Đúng format với đầy đủ hình thức nội dung mục Tính tổng quan, mục tiêu nhiệm vụ nghiên cứu Tính cấp thiết đề tài Phương pháp nghiên cứu Nội dung nghiên cứu Khả ứng dụng kiến thức toán học, khoa học kỹ thuật, khoa học xã hội,… để giải vấn đề Khả phân tích/tổng hợp  Khả thực thiết kế chế tạo hệ thống, máy móc, thiết bị,…(đối với đề tài theo hướng công nghệ)  Khả thực nghiên cứu, đề xuất phương pháp quy trình,… có tính sáng tạo, đáp ứng yêu cầu đưa với ràng buộc thực tế (đối với đề tài theo hướng nghiên cứu) Khả cải tiến phát triển đề tài Khả sử dụng công cụ kỹ thuật, phần mềm chuyên ngành,… Điểm thưởng Các ĐATN có tiêu chí sau công thêm 10 điểm: - Thuyết minh ĐATN viết tiếng Anh - ĐATN báo cáo tiếng Anh - Kết ĐATN viết báo khoa học (Hội nghị, tạp chí chuyên ngành, …) - ĐATN chuyển giao cho cơng ty (có giấy xác nhận công ty) Tổng điểm (*) Nếu > 100 qui đổi thành 100 điểm Tổng điểm quy đổi (hệ 10) (*) Nếu > 10 qui đổi thành 10 điểm ĐIỂM ĐẠT ĐƯỢC 20 5 5 80 10 50 10 10 10 100 10 Câu hỏi phản biện (nếu có): Kết luận:  Được phép bảo vệ  Không phép bảo vệ Tp Hồ Chí Minh, ngày tháng năm 20… Giáo viên phản biện (Ký & ghi rõ họ tên) v Đồ án tốt nghiệp Khoa đào tạo chất lượng cao LỜI CẢM ƠN Đất nước ta trình phát triển, nghiên cứu khoa học kĩ thuật đóng vai trị quan trọng cơng cơng nghiệp hóa, đại hóa đất nước Việc áp dụng khoa học kĩ thuật chính làm tăng suất lao động, thay sức lao động người, lao động cách có hiệu nhất, bảo đảm an tồn cho người lao động trình làm việc Để tạo tảng tốt cho bước phát triển tương lai, cần đầu tư, nghiên cứu, giáo dục, phát triển khoa học kĩ thuật cách nghiêm túc từ giai đoạn nhà trường Bước đầu vào nghiên cứu khoa học, nghiên cứu đề tài thực tế Chúng em gặp khơng ít khó khăn vốn kiến thức, hiểu biết, kỹ nhiều hạn chế, tình hình dịch bệnh COVID 19 diễn phức tạp địa bàn thành phố Hồ Chí Minh Song với hướng dẫn dạy tận tình Thạc sĩ Nguyễn Minh Triết, quan tâm, thông tin kịp thời tình hình dịch bệnh từ phía nhà trường Chúng em hồn thành đồ án mơn học mục tiêu ban đầu đặt Để có kết này, Thầy trò chúng em đồng hành làm việc cách nghiêm túc suốt tháng qua Xin trân trọng cảm ơn Thầy nhiều Kiến thức vô tận Chúng em chắn cịn nhiều điều thiếu sót Rất mong nhận nhận xét, đóng góp ý kiến tích cực từ q thầy để đề tài tiếp tục hoàn thiện Cuối cùng, chúng em chân thành gửi lời cảm ơn đến tồn thể thầy cơ, chúc q thầy khoa Đào tạo chất lượng cao nói riêng toàn thể cán bộ, viên chức nhà trường dồi sức khỏe, công tác thật tốt, tiếp tục thực sứ mệnh cao quý dẫn dắt hệ tương lai mai sau “Vinh dự, Tự hào sinh viên Đại Học Sư Phạm Kỹ Thuật TP Hồ Chí Minh” Nhóm thực đề tài xin chân thành cảm ơn! Nhóm thực hiền đề tài Đinh Kiên Cường – Đào Ngọc Sang – Lê Hải Đăng vi Đồ án tốt nghiệp Khoa đào tạo chất lượng cao TÓM TẮT ĐỒ ÁN Dựa vào kiến thức học trường điều khiển tự động tìm hiểu ngành cơng nghiệp robot Nghiên cứu robot lĩnh vực quan tâm Các loại robot tự động, bán tự động hay cao cấp robot với trí tuệ nhân tạo ngày phổ biến phát triển rộng rãi nơi Nắm bắt xu hướng nghiên cứu ngành điều khiển tự động, đề tài đồ án tốt nghiệp liên quan đến robot đề xuất Tuy nhiên, kiến thức chuyên ngành hạn chế mà lĩnh vực robot phức tạp tân tiến, nên đề tài tập trung hướng nghiên cứu để điều khiển xác robot Quattro song song bậc tự do, đồng thời kiểm nghiệm mơ hình thực tế Với đề tài đồ án tốt nghiệp “Nghiên cứu, thiết kế chế tạo Robot song song bậc tự điều khiển Raspberry kết hợp với xử lí ảnh ứng dụng dây chuyền sản xuất ”, nhóm thực nội dung sau: Nghiên cứu thiết kế mơ hình Tính toán động học, động lực học quy hoạch quỹ đạo Mô kiểm nghiệm động học phần mềm Matlab R2019a Thi công lắp ráp mô hình thực nghiệm Thiết kế xây dựng hệ thống điện cho robot hoạt động Điều khiển xác vị trí robot Quattro song song bậc tự quy hoạch chuyển động điểm tới điểm Xây dựng thuật tốn xử lí ảnh để phân loại sản phẩm theo màu sắc cho ứng dụng gắp đặt vật Sau mơ hình nhiệm vụ mơ hồn thành, đề tài áp dụng phương pháp điều khiển nghiên cứu lên mơ hình thực Báo cáo đồ án gồm chương: Đầu tiên, Chương giới thiệu tổng quan đề tài Cơ sở lí thuyết mơ ngun lí hoạt động nói đến Chương Chương nghiên cứu phân tích tính tốn động học robot; Kiểm nghiệm động học mơ MATLAB Simulink trình bày Chương Thiết kế thi công mô hình thể Chương Sau việc điều khiển Robot giải thích rõ Chương Cuối cùng, Chương đưa kết luận phương hướng phát triển mô hình xây dựng vii Đồ án tốt nghiệp Khoa đào tạo chất lượng cao ABSTRACT Based on knowledge studied on automatic control theories and robotics during the undergraduate course, we found that research on robotics is one of the most interested areas today So far, automatic robot, semi-automatic robot or robots with artificial intelligence have been increasingly popular Catching the trend of the automatic control research, we decided to choose a robot-related graduation project Our specialized knowledge is however limited, and the field of robots is very complex and advanced So, our project has mainly focused on intensive algorithms for precise control of Quattro parallel robot DOF In order to finish the project titled: "Research, design and manufacture Quattro parallel robot controlled by Raspberry Pi and image processing in the production line applications", we have implemented the following contents: Research and design Calculation of kinematics, dynamics and trajectory planning Simulation and kinetic analyse on Matlab R2019a software Executing and assembling the realistic model Create electrical systems for robots’s operation Quattro robot DOF control for desired position and point-to-point motion planning Implementing an image processing algorithm to classify products based on color for the Pick & Place application After completing the model and simulation tasks, we proceed to apply the researched control strategy to the realistic model This paper is organized as follows: In Section I, an overview of the topic is presented The theoretical basis of the model as well as the operating principle is mentioned in Section II Section III is shown the robot analysis and kinematic calculation Section IV is detailed experiment results and simulations using MATLAB Simulink Next, the design and construction of the model will be demonstrated in Section After that, the robot control will be described in Chapter Finnaly, the conclusion and development direction of the produced model will be detailed in Chapter viii Đồ án tốt nghiệp Khoa đào tạo chất lượng cao [12] Y Yorozu, M Hirano, K Oka, and Y Tagawa, “Electron spectroscopy studies on magneto-optical media and plastic substrate interface,” IEEE Transl J Magn Japan, vol 2, pp 740–741, August 1987 [Digests 9th Annual Conf Magnetics Japan, p 301, 1982] [13] M Young, The Technical Writer’s Handbook Mill Valley, CA: University Science, 1989 [14] V E Gough and S G Whitehall (1962), Universal Type Test Machine, Proceedings of 9th International Congress FISITA pp 117-137 [15] D Stewart (1965), A platform with six degrees of freedom, Proceedings of the Institution of Mechanical Engineers, pp 371-386 [16] I.A Bonev (2001), Delta parallel robot – The story of success, http://www Parallelmic.org/Reviews/ Reviews002.html, (20th December-2014) [17] J Brinker and B Corves (2015), A Survey on Parallel Robots with Deltalike Architecture, Proceedings of the 14th IFToMM World Congress, Taipei, Taiwan, pp 407-414 [18] D Stewart (1965), A platform with six degrees of freedom, Proceedings of the Institution of Mechanical Engineers, pp 371-386 [19] K M Lee and D K Shah (1988), "Dynamic analysis of a three-degreesof freedom in-parallel actuated manipulator", IEEE Journal of Robotics and Automation 4, pp 361-367 [20] Sokolov and P Xirouchakis (2007), "Dynamics analysis of a 3-DOF parallel manipulator with RPS joint structure", Mechanism and Machine Theory 42, pp 541-55 [21] L.W Tsai (1999), Robot Analysis the Mechanics of Serial and Parallel Manipulators, John Wiley and Sons [22] K.K Ahn and H T C Nguyen, “Intelligent switching control of a pneumatic muscle robot arm using learning vector quantization neural network,” Mechatronics, vol 17, pp 255–262, 2007 [23] Lewis FL, Abdallah CT, Dawson DM Control of robot manipulator New York: Macmillan; 1993 104 Đồ án tốt nghiệp Khoa đào tạo chất lượng cao [24] S Yasui, “Stochastic functional fourier series, volterra series, and nonlinear systems analysis,” IEEE Trans Automatic Control, vol 24, no 2, pp 230-242, 1979 [25] S Abid, F Fnaiech and M Najim, “A fast feedforward training algorithm using a modified form of the standard backpropagation algorithm,” IEEE Trans Neural Network, vol 12, no 2, pp 424-430, 2001 [26] N Q Lin, D M Xuan, and D X Ba, “Advanced control design for a highprecision heating furnace using combination of PI/Neural network,” Journal of Technical Education Science, no 55, pp 25-31, 2020 105 Đồ án tốt nghiệp Khoa đào tạo chất lượng cao PHỤ LỤC Phụ lục 1: Từ phương trình (3.20) ta đặt: a1  2( X A   rb ) L2 b1  2(Z A  da ) L2 c1  X A2  YA2  Z A2  L22  L12  (ra  rb )2  d a2  X A (ra  rb )  2Z Ad a Phương trình trở thành  a1 cos1  b1 sin 1  c1 2 Điều kiện a1  b1  c1 a12  b12 Chia vế phương trình (*) cho  a1 cos1  a b 2  a1 Vì   a b2  Nên đặt b1 a b 2   b1     a b2   a1 a12  b12  sin 1 ;  sin 1 cos1  cos 1 sin 1   sin(1  1 )  sin 1  c1 a  b12 2   1   b1 a12  b12  cos 1 c1 a12  b12 c1 a12  b12 106 Đồ án tốt nghiệp Khoa đào tạo chất lượng cao c1   1  1  arcsin a12  b12    c1 1  1    arcsin a12  b12  c1  1  arcsin  1  2 a  b 1   c1  1 (loai ) 1    arcsin 2 a  b  1 Với 1  tan a1 b1 Tương tự cách giải phương trình (3.25), ta đặt: a2  2(ra  rb  YA ) L2 b2  2(da  Z A ) L2 c2  X A2  YA2  Z A2  L22  L12  (ra  rb )2  d a2  2YA (ra  rb )  2Z Ad a   arcsin c2 a2  b2 Với   tan 2  2 a2 b2 Đối với phương trình (3.30), ta đặt: a3  2(ra  rb  X A ) L2 b3  2(da  Z A ) L2 c3  X A2  YA2  Z A2  L22  L12  (ra  rb )2  d a2  X A (ra  rb )  2Z Ad a 107 Đồ án tốt nghiệp   arcsin Với Khoa đào tạo chất lượng cao c3 a3  b3   tan  3 a3 b3 Đối với phương trình (3.35), ta đặt: a4  2(ra  rb  YA ) L2 b4  2(Z A  da ) L2 c4  X A2  YA2  Z A2  L22  L12  (ra  rb )2  d a2  2YA (ra  rb )  2Z Ad a   arcsin Với c4 a4  b4   tan 2  4 a4 b4 Phụ lục 2: Code khối động học thuận: function [X,Y,Z] = FK(the11,the12,the13,the14) l2=0.181; %link noi voi dc l1=0.498; %thanh cacbon ra=0.1265; % khoang cach tu truc goc cua tam de tren den truc dong co theo phuong x da=0.035; % khoang cach tu de tren toi truc dong co theo phuong z rb = 0.055; %% J1 x1 = (ra+l2*cos(the11))-rb; %y1 = 0; z1 = (l2*sin(the11)+da); %% J2 %x2 = 0; 108 Đồ án tốt nghiệp Khoa đào tạo chất lượng cao y2 = (ra+l2*cos(the12))-rb; z2 = (l2*sin(the12)+da); %% J3 x3 = -(ra+l2*cos(the13))+rb; %y3 = 0; z3 = (l2*sin(the13)+da); %% J4 %x4 = 0; y4 = -(ra+l2*cos(the14))+rb; z4 = (l2*sin(the14)+da); %% tinh a1=(z1-z3)/(x3-x1); b1=(x3^2+z3^2-x1^2-z1^2)/(2*(x3-x1)); a2=(z2-z4)/(y4-y2); b2=(y4^2+z4^2-y2^2-z2^2)/(2*(y4-y2)); A=a1^2+a2^2+1; B=2*(a1*(b1-x1)+a2*b2-z1); C=(b1-x1)^2+b2^2+z1^2-l1^2; denta=B^2-4*A*C; D=sqrt(denta); z=((-B+D)/(2*A)); x=(a1*z+b1); y=(a2*z+b2); Z=z*1000; Y=y*1000; X=x*1000; Code khối động học nghịch: function [theta11,theta12,theta13,theta14]= IK(x,y,z) L1=498; L2=181; d=35; %da ra=126.5; %Aa rb = 55; A = - rb; %% a1=2*L2*(x-A); 109 Đồ án tốt nghiệp Khoa đào tạo chất lượng cao b1=2*L2*(z-d); c1=x^2+y^2+z^2+L2^2-L1^2+A^2+d^2-2*x*A-2*z*d; alpha1=atan2(a1,b1); %link-6 theta11=asin(c1/(sqrt(a1^2+b1^2)))-alpha1; %theta31=acos(y/L1) %theta21= acos((x-L2*cos(theta11)-A)/L1*sin(theta31))-theta11 %% 90 a2=-2*L2*(A-y); b2=-2*L2*(d-z); c2=x^2+y^2+z^2+L2^2-L1^2+A^2+d^2-2*y*A-2*z*d; alpha2=atan2(a2,b2); %link2 theta12=asin(c2/(sqrt(a2^2+b2^2)))-alpha2; %theta32=acos(x/-L1) %theta22=acos((y-A-cos(theta12)*L2)/L1*sin(theta32))-theta12 %% 180 a3=-2*L2*(x+A); b3=-2*L2*(d-z); c3=x^2+y^2+z^2+L2^2-L1^2+A^2+d^2+2*x*A-2*z*d; alpha3=atan2(a3,b3); %link3 theta13=asin(c3/(sqrt(a3^2+b3^2)))-alpha3; %theta33=acos(y/-L1) %theta23=acos((x+L2*cos(theta13)+A)/-L1*sin(theta33))-theta13 %% 270 a4=-(2*y+2*A)*L2; b4=2*(z-d)*L2; c4=x^2+y^2+z^2+L2^2-L1^2+A^2+d^2+2*y*A-2*z*d; alpha4=atan2(a4,b4); %link4 theta14=asin(c4/(sqrt(a4^2+b4^2)))-alpha4; %theta34=acos(x/L1) %theta24=acos((y+L2*cos(theta14)+A)/-L1*sin(theta34))-theta14 Code khối line: function [Px,Py,Pz]=Line(t,data,i) 110 Đồ án tốt nghiệp Khoa đào tạo chất lượng cao Px1 = data(i,1); Py1 = data(i,2); Pz1 = data(i,3); Px2 = data(i,4); Py2 = data(i,5); Pz2 = data(i,6); tf = data(i,7); Px = Px1 + (6*(Px2-Px1)*t^5)/tf^5 - (15*(Px2-Px1)*t^4)/tf^4 + (10*(Px2 - Px1)*t^3)/tf^3; Py = Py1 + (6*(Py2-Py1)*t^5)/tf^5 - (15*(Py2-Py1)*t^4)/tf^4 + (10*(Py2 - Py1)*t^3)/tf^3; Pz = Pz1 + (6*(Pz2-Pz1)*t^5)/tf^5 - (15*(Pz2-Pz1)*t^4)/tf^4 + (10*(Pz2 - Pz1)*t^3)/tf^3; Code khối Triangle : function [Px,Py,Pz]= Triangle(t,data) %diem thu Px1 = data(1,1); Py1 = data(1,2); Pz1 = data(1,3); %diem thu Px2 = data(2,1); Py2 = data(2,2); Pz2 = data(2,3); %diem thu Px3 = data(3,1); Py3 = data(3,2); Pz3 = data(3,3); %qui hoach tf=4; tf1=8; if (t4 && t