(Đồ á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

(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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(Đồ á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

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 Đồ á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 q 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 hoà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 toàn thể thầy cô, chúc quý thầy cô khoa Đào tạo chất lượng cao nói riêng tồ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 tố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ơ hoà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 MỤC LỤC Nhiệm vụ đồ án tốt nghiệp i Trang phiếu nhận xét giáo viên hướng dẫn ii Trang phiếu nhận xét giáo viên phản biện iv Lời cảm ơn vi Tóm tắt vii Mục lục ix Danh mục chữ viết tắt xiii Danh mục bảng biểu xiv Danh mục hình ảnh xv CHƯƠNG : TỔNG QUAN 1.1 Đặt vấn đề: 1.2 Tính cấp thiết đề tài: 1.3 Mục đích, đối tượng phạm vi nghiên cứu: 1.3.1 Mục đích đề tài: 1.3.2 Đối tượng phạm vi nghiên cứu: 1.4 Phương pháp nghiên cứu: 1.5 Ý nghĩa khoa học thực tiễn: 1.6 Các nghiên cứu giới nước liên quan đến đề tài: 1.6.1 Sơ lược lịch sử Robot: 1.6.2 Các nghiên cứu giới: 1.6.3 Các nghiên cứu nước CHƯƠNG : CƠ SỞ LÝ THUYẾT 11 ix Đồ án tốt nghiệp Khoa đào tạo chất lượng cao 2.1 Cơ sở lí thuyết cơng nghệ: 11 2.1.1 Giới thiệu chung Robot song song: 11 2.1.2 So sánh robot chuỗi robot song song: 11 2.1.3 Ứng dụng robot song song: 13 2.1.4 Cấu trúc robot song song: 18 2.1.5 Phân loại robot song song: 20 2.2 Cơ sở lý thuyết khí: 23 2.3 Cơ sở lý thuyết điện điện tử: 24 2.3.1 Giới thiệu Matlab: 24 2.3.2 Matlab Simulink: 25 2.3.3 Thuật tốn xử lí ảnh: 25 CHƯƠNG : NGHIÊN CỨU PHÂN TÍCH, TÍNH TOÁN ĐỘNG HỌC ROBOT QUATTRO 27 3.1 Mơ hình động học robot song song Quattro không gian: 27 3.2 Bài toán động học Robot Quattro: 28 3.2.1 Động học thuận: 28 3.2.2 Động học nghịch: 33 3.3 Bài toán động lực học Robot Quattro: 37 3.4 Vùng làm việc Robot Quattro: 41 3.5 Quy hoạch quỹ đạo: 45 CHƯƠNG : KIỂM NGHIỆM, MÔ PHỎNG ĐỘNG HỌC VÀ ROBOT SONG SONG QUATTRO KHÔNG GIAN 48 4.1 Kiểm nghiệm kết động học Matlab Simulink: 48 4.1.1 Kiểm nghiệm động học nghịch: 48 4.1.2 Kiểm nghiệm động học thuận: 49 4.2 Mô chuyển động robot Matlab Simulink: 51 4.2.1 Khối động học nghịch: 52 x Đồ án tốt nghiệp Khoa đào tạo chất lượng cao 4.2.2 Khối Simcape Multibody 53 4.2.3 Khối động học thuận: 54 4.2.4 Khối quy hoạch quỹ đạo: 55 4.2.5 Kết mô chuyển động robot Matlab Simulink: 56 CHƯƠNG : THIẾT KẾ VÀ THI CƠNG MƠ HÌNH 65 5.1 Chế tạo mơ hình Robot Quattro bậc tự do: 65 5.1.1 Phần khung robot: 66 5.1.2 Tấm đế robot: 67 5.1.3 Bệ đỡ động cơ: 68 5.1.4 Khớp (khớp chủ động): 68 5.1.5 Khớp (khớp bị động): 70 5.1.6 Đầu công tác – Đế robot (EE): 70 5.2 Thông số kỹ thuật Robot: 71 5.3 Lựa chọn thiết bị: 71 5.3.1 Động AC Servo Mitsubishi HC-KFS13: 72 5.3.2 Hộp giảm tốc KSBL 44-10-P1: 73 5.3.3 Driver Mitsubishi MR-J2S-10A: 75 5.3.4 Arduino Nano V3.0 ATmega328P: 77 5.3.5 Nguồn tổ ong 12V 5A: 79 5.3.6 Nhơm định hình 30x30: 80 5.3.7 MCB Schneider EZ9F34210: 80 5.3.8 Contactor Schneider LC1D09M7: 81 5.3.9 Lọc nhiễu nguồn MXB-1210-33: 83 CHƯƠNG : ĐIỀU KHIỂN ROBOT QUATTRO BẬC TỰ DO 84 6.1 Lưu đồ giải thuật Robot Quattro: 84 6.1.1 Lưu đồ tổng quát trình nghiên cứu: 84 xi Đồ án tốt nghiệp Khoa đào tạo chất lượng cao 6.1.2 Lưu đồ tổng quan hoạt động robot: 85 6.1.3 Lưu đồ giải thuật điều khiện động cơ: 87 6.2 Điều khiển mơ hình Robot Quattro bậc tự do: 89 6.2.1 Thiết bị sử dụng: 89 6.2.2 Kết nối phần cứng: 89 6.2.3 Giao thức truyền thông Uart: 97 6.2.4 Phương pháp xác định tọa độ vật sử dụng xử lí ảnh: 98 6.2.5 Giao diện điều khiển: 99 CHƯƠNG : KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 101 7.1 Kết luận: 101 7.2 Phương hướng phát triển: 101 TÀI LIỆU THAM KHẢO 103 PHỤ LỤC 106 xii Đồ án tốt nghiệp Khoa đào tạo chất lượng cao DANH MỤC CÁC TỪ VIẾT TẮT PID: Proportional Integral Derivative EE: End Effector DOF: Degrees Of Freedom SM: Simcape Multibody RSS: Robot song song ĐHN: Động học nghịch ĐHT: Động học thuận ĐLH: Động lực học OL: Open-loop CL: Closed-loop SS: Subsystem IE: Input reference xiii Đồ án tốt nghiệp Khoa đào tạo chất lượng cao DANH MỤC CÁC BẢNG BIỂU Bảng 1.1: Những thành tựu phát triển robot kỉ 20 Bảng 1.2: Các chủng loại robot phát triển nhân rộng toàn giới Bảng 2.1: So sánh robot chuỗi robot song song 13 Bảng 2.2: Phân loại robot song song ứng với số bậc độ liên kết 23 Bảng 5.1: Bảng thông số chung robot 71 Bảng 5.2: Thông số kĩ thuật động HC-HFS13 72 Bảng 5.3: Thông số kĩ thuật hộp giảm tốc 74 Bảng 5.4: Thông số kĩ thuật MR-J2S-10A 76 Bảng 5.5: Thông số kĩ thuật Adruino nano v3.0 78 Bảng 5.6: Thông số kĩ thuật nguồn tổ ong 12V 5A 79 Bảng 5.7: Thông số kĩ thuật MCB EZ9F34210 81 Bảng 5.8: Thông số kĩ thuật Contactor Schneider LC1D09M7 82 Bảng 5.9: Thông số kĩ thuật lọc nhiễu nguồn 83 xiv Đồ á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