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 khoa đào tạo chất lượng cao ngành công nghệ kỹ thuật cơ điện tử

Đồ á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 DANH MỤC CÁC HÌNH ẢNH Hình 1.1: Robot chân R.S Mosher Hình 2.1: Cấu trúc robot song song 11 Hình 2.2: Robot Puma 12 Hình 2.3: Cơ cấu song song Gough 14 Hình 2.4: Robot Delta ứng dụng cơng nghệ thực phẩm 14 Hình 2.5: Cơ cấu song song Stewart 15 Hình 2.6: Sản phẩm Persival École National d'Elquitation (Pháp) 15 Hình 2.7: Bộ mô xe đạp Viện KAIST sản phẩm Caren Motek 16 Hình 2.8: Sản phẩm SuriScope vận hành, Đại học Humboldt (Berlin, Đức) 16 Hình 2.9: Robot CRIGOS dùng để phẫu thuật tái tạo xương 17 Hình 2.10: Robot song song kiểu Stewart Hình 2.11: Robot Delta 17 Hình 2.12: Robot song song phẳng bậc tự với cấu trúc chân RRR 21 Hình 2.13: Cấu trúc chấp hành song song 3PRP 22 Hình 2.14: Cấu trúc chấp hành kiểu cầu 3RRR 22 Hình 3.1: Robot song song Quattro không gian RUS 27 Hình 3.2: Các góc khớp vector cánh tay robot 29 Hình 3.3: Vector tọa độ cánh tay nhìn từ phía 30 Hình 3.4: Các góc khớp cánh tay robot 33 Hình 3.5: Hệ tọa độ đầu công tác 39 Hình 3.6: Các thơng số hình học cánh tay 42 Hình 3.7: Ba dạng hình xuyến 42 Hình 3.8: Vùng làm việc Robot Quattro hệ tọa độ Oxyz 44 Hình 3.9: Vùng làm việc Robot Quattro hệ tọa độ Oxy 45 Hình 4.1: Vị trí cánh tay điểm home 48 Hình 4.2: Vị trí cánh tay điểm 48 Hình 4.3: Sơ đồ mô kiểm nghiệm động học 49 Hình 4.4: Quỹ đạo đường tròn với R=50 49 Hình 4.5: Tín hiệu x, y, z với quỹ đạo hình trịn R=50 50 Hình 4.6: Tín hiệu góc theta với hình trịn R=50 50 Hình 4.7: Tín hiệu sai số vị trí 51 Hình 4.8: Mơ hình Matlab Simulink robot 52 Hình 4.9: Khối động học nghịch 52 xv Đồ án tốt nghiệp Khoa đào tạo chất lượng cao Hình 4.10: Khối Simcape Multibody 53 Hình 4.11: Mơ hình robot chuyển từ Solidwork sang Matlab 54 Hình 4.12: Khối động học thuận 54 Hình 4.13: Khối quy hoạch đường thẳng 55 Hình 4.14: Khối quy hoạch quỹ đạo hình tam giác 55 Hình 4.15: Khối quy hoạch quỹ đạo hình trịn 56 Hình 4.16: Plot XY đường thẳng 56 Hình 4.17: Animation chuyển động đường thẳng robot 57 Hình 4.18: Tín hiệu ngõ vào giá trị góc theta quy hoạch theo đường thẳng 57 Hình 4.19: Tín hiệu ngõ giá trị góc theta quy hoạch theo đường thẳng 58 Hình 4.20: Tín hiệu sai số góc theta quy hoạch theo đường thẳng 58 Hình 4.21: Tín hiệu ngõ vào, vị trí quy hoạch theo đường thẳng 59 Hình 4.22: Plot XY hình tam giác 59 Hình 4.23: Animation chuyển động tam giác robot 60 Hình 4.24: Tín hiệu ngõ vào giá trị góc theta quy hoạch theo hình tam giác 60 Hình 4.25: Tín hiệu ngõ giá trị góc theta quy hoạch theo hình tam giác 61 Hình 4.26: Tín hiệu sai số góc theta quy hoạch theo hình tam giác 61 Hình 4.27: Tín hiệu ngõ vào, vị trí quy hoạch theo hình tam giác 62 Hình 4.28: Plot XY hình trịn 62 Hình 4.29: Tín hiệu ngõ vào giá trị góc theta quy hoạch theo hình trịn 63 Hình 4.30: Tín hiệu ngõ giá trị góc theta quy hoạch theo hình trịn 63 Hình 4.31: Tín hiệu sai số góc theta quy hoạch theo hình trịn 64 Hình 4.32: Tín hiệu ngõ vào, vị trí quy hoạch theo hình trịn 64 Hình 5.1: Mơ hình robot Quattro Inventor 65 Hình 5.2: Mơ hình 3D khung robot Inventor 66 Hình 5.3: Tấm kê góc 67 Hình 5.4: Mơ hình 3D đế robot Inventor 67 Hình 5.5: Mơ hình 3D bệ đỡ động Inventor 68 Hình 5.6: Mơ hình 3D trục Inventor 69 Hình 5.7: Mơ hình 3D mặt bích Inventor 69 Hình 5.8: Mơ hình 3D khớp Inventor 70 Hình 5.9: Mơ hình 3D cấu nối khớp 70 Hình 5.10: Mơ hình 3D đế robot Inventor 71 xvi Đồ án tốt nghiệp Khoa đào tạo chất lượng cao Hình 5.11: Động HC-HFS13 72 Hình 5.12: Thơng só kích thước động HC-HFS13 73 Hình 5.13: Hộp giảm tốc KSBL 44-10-P1 73 Hình 5.14: Thơng số kích thước hộp giảm tốc 75 Hình 5.15: Driver Mitsubishi MR-J2S-10A 76 Hình 5.16: Thơng só kích thước MR-J2S-10A 77 Hình 5.17: Arduino Nano V3.0 78 Hình 5.18: Nguồn tổ ong 12V 5A 79 Hình 5.19: Nhơm định hình 30x30 80 Hình 5.20: MCB Schneider EZ9F34210 81 Hình 5.21: Contactor Schneider LC1D09M7 82 Hình 5.22: Lọc nhiễu nguồn MXB-1210-33 83 Hình 6.1: Lưu đồ giải thuật tổng quan trình nghiên cứu 85 Hình 6.2: Lưu đồ tổng quan hoạt động robot Quattro 87 Hình 6.3: Lưu đồ giải thuật điều khiện động 88 Hình 6.4: Datasheet hướng dẫn kết nối 90 Hình 6.5: Datasheet hướng dẫn kết nối 91 Hình 6.6: Sơ đồ kết nối phần cứng điều khiển 92 Hình 6.7:Mạch in board điều khiển robot Quattro 92 Hình 6.8: Sơ đồ kết nối Board điều khiển 93 Hình 6.9 :Layout tủ điều khiển CAD 94 Hình 6.10: Bố trí thiết bị tủ điều khiển 95 Hình 6.11: Sơ đồ mạch điều khiển CAD 96 Hình 6.12: Kết nối dây UART 97 Hình 6.13: Giao diện điều khiển robot Quattro 100 xvii Đồ án tốt nghiệp Khoa đào tạo chất lượng cao CHƯƠNG : TỔNG QUAN 1.1 Đặt vấn đề: Trong trình cơng nghiệp hóa đại hóa nước ta, việc nghiên cứu thiết kế chế tạo robot cơng nghiệp để ứng dụng vào sản xuất có ý nghĩa quan trọng, đặc biệt giai đoạn hội nhập phát triển công nghệ Việc tự động hóa q trình sản xuất với có mặt robot làm tăng khả mềm dẻo hệ thống sản xuất, tăng chất lượng sản phẩm đặc biệt làm giảm giá thành sản phẩm để tăng tính cạnh tranh Ngồi ra, robot cịn có tính quan trọng khác làm việc liên tục ổn định thay cho người môi trường khắc nghiệt là: mơi trường nhiều khói bụi, mơi trường độc hại hóa chất, mơi trường nhiệt độ cao, … Hiện nay, giới có nhiều nghiên cứu ứng dụng cánh tay robot vào trình sản xuất cánh tay robot hàn nhà máy sản xuất ô tô, robot lắp máy, robot đào đường hầm, robot cấp phôi máy gia công chi tiết khí, …Tuy nhiên Việt Nam việc nghiên cứu chế tạo robot tập trung chủ yếu robot (hay gọi tay máy) nối tiếp có kết cấu vịng hở Với nhu cầu ngày cao, nhà máy có xu hướng ứng dụng robot nhiều mục đích hỗ trợ thực thi nhiệm vụ sản xuất Không vậy, nhiều doanh nghiệp, robot trở thành trợ thủ đắc lực q trình đại hóa sản xuất Do đó, việc đưa cải tiến, hệ, mẫu mã robot khác để đáp ứng nhu cầu cơng việc quan trọng đội ngũ nghiên cứu chế tạo robot Với mong muốn tìm hiểu sâu lĩnh vực cánh tay Robot để ứng dụng vào thực tế sống Chính đề tài “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” thực 1.2 Tính cấp thiết đề tài: Cùng với q trình phát triển robot cơng nghiệp, việc ứng dụng robot sản xuất cần thiết Tuy nhiên, loại robot nối tiếp sử dụng nhiều lĩnh vực bộc lộ nhiều nhược điểm tính linh hoạt thấp, tốc độ xử lý khả Đồ án tốt nghiệp Khoa đào tạo chất lượng cao TÀI LIỆU THAM KHẢO Tài liệu tiếng Việt: [1] PGS.TS Nguyễn Trường Thịnh, “Giáo trình kỹ thuật robot” Nhà xuất Đại học quốc gia TP Hồ Chí Minh, 498 trang [2] Huỳnh Thái Hồng (2006), “Giáo trình Hệ thống điều khiển tự động”, Nhà xuất Đại học quốc gia TP Hồ Chí Minh, 421 trang [3] Trịnh Chất, Lê Văn Uyển, “Tính tốn hệ thống dẫn động khí tập 1”, Nhà xuất giáo dục Việt Nam [4] Nguyễn Văn Khang Nguyễn Thành Cơng (2012), “Về hai phương pháp giải tốn động lực học ngược robot song song”, Hội nghị toàn quốc lần điện tử VCM 2012, tr 574-583 [5] Nguyễn Văn Khang Lương Anh Tuấn (2013), "Tính tốn so sánh vài phương pháp số giải toán động học ngược robot song song dư dẫn động", Tạp chí Tin học Điều khiển học T 29, S 1, tr 3-15 [6] Phạm Văn Bạch Ngọc, Nguyễn Dũng Nguyễn Văn Hội (2015), Thuật toán điều khiển động lực học ngược thích nghi cho robot song song ba bậc tự tịnh tiến kiểu Delta robot, Hội nghị học kỹ thuật toàn quốc, Đà Nẵng, tr 434441 [7] Nguyễn Đức Sang (2014), Tính tốn động lực học điều khiển robot song song không gian Delta, Đồ án tốt nghiệp đại học, Trường Đại học Bách khoa Hà Nội Tài liệu tiếng Anh: [8] G Eason, B Noble, and I N Sneddon, “On certain integrals of LipschitzHankel type involving products of Bessel functions,” Phil Trans Roy Soc London, vol A247, pp 529–551, April 1955 (references) [9] J Clerk Maxwell, A Treatise on Electricity and Magnetism, 3rd ed., vol Oxford: Clarendon, 1892, pp.68–73 [10] S Jacobs and C P Bean, “Fine particles, thin films and exchange anisotropy,” in Magnetism, vol III, G T Rado and H Suhl, Eds New York: Academic, 1963, pp 271–350 [11] R Nicole, “Title of paper with only first word capitalized,” J Name Stand Abbrev, in press 103 Đồ á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