Nghiên cứu thiết kế tham số hoá, cân bằng và điều khiển tay máy 4 bậc tự do

ĐẠI HỌC QUỐC GIA TP HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA CHÂU THÁI NGUYÊN NGHIÊN CỨU THIẾT KẾ THAM SỐ HÓA, CÂN BẰNG VÀ ĐIỀU KHIỂN TAY MÁY BẬC TỰ DO STUDY ON PARAMETRIZED DESIGN, BALANCE AND CONTROL OF DOF MANIPULATOR Chuyên ngành: Kỹ thuật Cơ Điện Tử Mã số: 8520114 LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, tháng 01 năm 2023 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA – ĐHQG - HCM Cán hướng dẫn khoa học: PGS TS Nguyễn Tấn Tiến Cán chấm nhận xét 1: PGS TS Phạm Huy Hoàng Cán chấm nhận xét 2: PGS TS Nguyễn Hùng Luận văn thạc sĩ bảo vệ Trường Đại học Bách Khoa, ĐHQG Thành phố Hồ Chí Minh ngày 13 tháng 01 năm 2023 Thành phần Hội đồng đánh giá luận văn thạc sĩ gồm: PGS TS Nguyễn Quốc Chí - Chủ tịch hội đồng TS Phạm Phương Tùng - Thư ký PGS TS Phạm Huy Hoàng - Cán phản biện PGS TS Nguyễn Hùng - Cán phản biện TS Đoàn Thế Thảo - Ủy viên Xác nhận Chủ tịch Hội đồng đánh giá LV Trưởng Khoa quản lý chuyên ngành sau luận văn sửa chữa (nếu có) CHỦ TỊCH HỘI ĐỒNG TRƯỞNG KHOA CƠ KHÍ ĐẠI HỌC QUỐC GIA TP.HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự - Hạnh phúc NHIỆM VỤ LUẬN VĂN THẠC SỸ Họ tên học viên: CHÂU THÁI NGUYÊN Ngày tháng năm sinh: 17/09/1997 Chuyên ngành: MSHV: 2170096 Nơi sinh: Đồng Nai Kỹ thuật Cơ điện tử Mã số: 8520114 I TÊN ĐỀ TÀI NGHIÊN CỨU THIẾT KẾ THAM SỐ HÓA, CÂN BẰNG VÀ ĐIỀU KHIỂN TAY MÁY BẬC TỰ DO (Study on Parametrized Design, Balance and Control of DOF Manipulator) II NHIỆM VỤ VÀ NỘI DUNG - Tìm hiểu tổng quan tổng quan tay máy công nghiệp: thiết kế, cân bằng, điều khiển, ứng dụng, … từ đặt đầu cho thiết kế - Đề xuất lựa chọn phương án thiết kế khả thi - Thiết kế tay máy (cơ, điện, điều khiển, …) - Mô với tham số khác nhau, đánh giá thiết kế III NGÀY GIAO NHIỆM VỤ: 05/09/2022 V NGÀY HOÀN THÀNH NHIỆM VỤ: 26/12/2022 IV CÁN BỘ HƯỚNG DẪN: PGS.TS NGUYỄN TẤN TIẾN Tp HCM ngày 05 tháng 09 năm 2022 CÁN BỘ HƯỚNG DẪN (Họ tên chữ ký) CHỦ NHIỆM BỘ MÔN ĐÀO TẠO (Họ tên chữ ký) Nguyễn Tấn Tiến TRƯỞNG KHOA (Họ tên chữ ký) LỜI CẢM ƠN “It always seems impossible until it has been done”, câu nói tiếng Nelson Mandela, xác tinh thần “vượt khó” tơi cảm nhận suốt trình thực luận văn tốt nghiệp hướng dẫn Thầy, PGS TS Nguyễn Tấn Tiến – giám đốc Phịng thí nghiệm Trọng điểm Điều khiển số Kỹ thuật hệ thống (DCSELAB) Tôi xin chân thành cảm ơn Thầy tận tình giúp đỡ cho tơi nhiều lời khun từ ngày đầu đến lúc hoàn thành luận văn Được làm việc với thầy may mắn, giúp tơi nhìn nhận thân cách thực tế, bước cải thiện điểm yếu trở thành người tốt chuyên môn Xin cảm ơn tất tập thể anh, chị người bạn DCESLAB hỗ trợ, giúp đỡ nhiều suốt thời gian làm việc vừa qua Tôi thật biết ơn gia đình, người thân ln động lực, ln đứng phía sau đồng hành tơi đường học tập, nghiên cứu Dù cố gắng, hạn chế khả lý luận giới hạn kiến thức, kính mong góp ý dẫn q Thầy, Cơ qua giúp luận văn tơi hoàn thiện Xin chân thành cảm ơn! Học viên Châu Thái Ngun i TĨM TẮT Luận văn trình bày nghiên cứu thiết kế tham số hóa, đề xuất cấu cân trình bày giải thuật điều khiển trượt để điều khiển tay máy bậc tự có cấu hình kết hợp cấu hình nối tiếp cấu hình bình bình hành Để đảm bảo tính chắn thiết kế trước trình hoạt động, vùng làm việc động lực học tay máy tính tốn trình bày Tiếp theo đó, đề tài ứng dụng trực tiếp kết trình bày vào tốn tay máy kiểm định bình chữa cháy Tính chắn hiệu đề xuất xác nhận thơng qua việc phân tích kết mô phần mềm Matlab ABSTRACT The thesis illustrates study on parametrized design, proposes balancing mechanisms and presents a sliding mode algorithm to control D.O.F manipulator with the configuration combined of serial and parallelogram To ensure the robustness of the design before manufacturing and during operations, manipulator workspace analysis and dynamic modelling are also computed and presented Next, the thesis applies theoretical results to solve the problem of Fire Extinguisher Testing Manipulator The effectiveness of proposal is verified through the analysis of simulation results on Matlab ii LỜI CAM ĐOAN Tôi xin cam đoan nội dung luận văn tốt nghiệp tơi thực thời gian qua Những số liệu, kết nghiên cứu trình bày trung thực, thực DCSELAB, Đại học Bách Khoa, Đại học Quốc gia Thành phố Hồ Chí Minh không chép nguồn khác Trong nội dung luận văn, nguồn tài liệu tham khảo trích dẫn trích dẫn thích rõ ràng Tơi xin hồn tồn chịu trách nhiệm trước môn, khoa nhà trường cam đoan TPHCM, ngày 26 tháng 12 năm 2022 Học viên Châu Thái Nguyên iii MỤC LỤC LỜI CẢM ƠN i TÓM TẮT ii LỜI CAM ĐOAN iii MỤC LỤC iv DANH MỤC HÌNH ẢNH .vii DANH MỤC BẢNG BIỂU ix Chương MỞ ĐẦU 1.1 Lý chọn đề tài: 1.2 Đặt vấn đề: 1.3 Mục tiêu luận văn: 1.4 Cấu trúc luận văn: 1.5 Những kết nghiên cứu ban đầu: Chương TỔNG QUAN 2.1 Các dạng tay máy công nghiệp: 2.1.1 Tay máy với cấu hình liên tục: 2.1.2 Tay máy với cấu hình song song: 2.1.3 Tay máy cấu hình SCARA: 2.1.4 Tay máy cấu hình hình bình hành: 2.2 Vấn đề cân cho tay máy: 12 2.2.1 Cân lực lắc ngang cho tay máy: 12 2.2.2 Cân moment lắc ngang: 13 iv 2.2.3 Cân trọng lực tay máy: 13 2.3 Tổng quan điều khiển tay máy: 19 2.3.1 Phương pháp điều khiển tuyến tính hóa (Linear Control Technique): 19 2.3.2 Phương pháp điều khiển ổn định (Robust Force Control): 19 2.4 Ứng dụng tay máy xếp dỡ hàng hóa 20 2.5 Tìm hiểu tham số hóa thơng số tay máy: 21 Chương LỰA CHỌN PHƯƠNG ÁN 22 3.1 Lựa chọn cấu hình tay máy: 22 3.2 Lựa chọn cấu cân cho tay máy 23 3.3 Lựa chọn truyền động 24 3.4 Lựa chọn động cho tay máy: 27 3.5 Lựa chọn điều khiển 28 Chương THIẾT KẾ VÀ MƠ HÌNH HĨA 29 4.1 Lựa chọn thông số cho tay máy: 29 4.2 Động học thuận tay máy: 32 4.3 Động học nghịch tay máy: 38 4.4 Xác định vùng làm việc tay máy: 42 4.5 Tính tốn cân tay máy: 45 4.5.1 Cân khâu số 3: 46 4.5.2 Cân khâu số 2: 47 Chương THIẾT KẾ BỘ ĐIỀU KHIỂN TAY MÁY 52 5.1 Động lực học tay máy: 52 5.1.1 Phân tích lực tác dụng lên khâu tay máy: 52 v 5.1.2 Tính tốn biến phục vụ động lực học tay máy: 54 5.1.2.1 Vị trí khối tâm khâu: 54 5.1.2.2 Vận tốc góc khâu: 56 5.1.2.3 Vận tốc dài khâu tay máy: 58 5.1.2.4 Tensor quán tính khâu: 60 5.1.2.5 Ước tính khối lượng khâu: 63 5.1.3 Động lực học tay máy sử dụng phương pháp Lagrange: 65 5.2 Thiết kế điều khiển trượt tay máy: 67 Chương MÔ PHỎNG VÀ ĐÁNH GIÁ 71 6.1 Bài toán tay máy kiểm tra bình chữa cháy: 71 6.2 Kết mô bám quỹ đạo: 77 6.3 Đánh giá kết mô phỏng: 82 6.3.1 Đánh giá bám quỹ đạo tay máy: 82 6.3.2 Đánh giá cấu cân bằng: 84 6.4 Bàn luận kết thiết kế tham số hóa: 84 6.5 Kết luận định hướng nghiên cứu: 86 DANH MỤC CÁC CƠNG TRÌNH KHOA HỌC 87 DANH MỤC TÀI LIỆU THAM KHẢO 108 PHỤ LỤC 111 PL1 Chi tiết ma trận động lực học tay máy 111 PHẦN LÝ LỊCH TRÍCH NGANG 117 vi DANH MỤC HÌNH ẢNH Hình 2.1 Sơ đồ nguyên lý tay máy cấu hình liên tục Hình 2.2 Tay máy Mitsubishi RV-20FR [2] Hình 2.3 Tay máy Fanuc M-20iB/25 Hình 2.4 Sơ đồ nguyên lý tay máy cấu hình hình bình hành, 3RRR Hình 2.5 Tay máy cấu hình song song ABB FlexPicker [5] Hình 2.6 Tay máy cấu hình song song Fanuc M-2iA/3A [6] Hình 2.7 Sơ đồ nguyên lý tay máy cấu hình SCARA Hình 2.8 Tay máy cấu hình SCARA RH-3FHR3515 [7] Hình 2.9 Tay máy cấu hình SCARA ABB IRB 910SC [8] Hình 2.10 Sơ đồ nguyên lý cấu hình tay máy hình bình hành Hình 2.11 Tay máy cấu hình hình bình hành Yaskawa MPL500ii [9] 10 Hình 2.12 Tay máy cấu hình hình bình hành Fanuc M410iC/185 [10] 11 Hình 2.13 Đối trọng động cơ, gắn trực tiếp lên khâu 14 Hình 2.14 Đối trọng tải ngồi, gắn lên cấu phụ 14 Hình 2.15 Cân sử dụng lị xo, gắn trực tiếp lên khâu 15 Hình 2.16 Tay máy sử dụng cấu cân trọng lực: đối trọng lò xo 16 Hình 2.17 Tay máy MPL500ii sơ đồ nguyên lý 17 Hình 2.18 Tay máy Fanuc M410ic – 500 sơ đồ nguyên lý 18 Hình 2.19 Ứng dụng gắp thả sản phẩm vào pallet tay máy [14] 20 Hình 2.20 Ứng dụng kiểm định bình chữa cháy [17] 21 Hình 3.1 Sơ đồ nguyên lý tay máy bậc tự 23 Hình 3.2 Sơ đồ nguyên lý cấu hình tay máy hình bình hành cấu cân 24 Hình 3.3 Cấu trúc bên tay máy Mitsubishi RV-4FR [19] 25 Hình 3.4 Cấu trúc bên tay máy Mitsubishi RV-100TFH [20] 25 Hình 3.5 Cấu trúc điều khiển Robust Force control, gồm Sliding mode [13] 28 vii CÁC CƠNG TRÌNH KHOA HỌC 103 CÁC CƠNG TRÌNH KHOA HỌC 104 CÁC CƠNG TRÌNH KHOA HỌC 105 CÁC CƠNG TRÌNH KHOA HỌC 106 CÁC CƠNG TRÌNH KHOA HỌC 107 TÀI LIỆU THAM KHẢO DANH MỤC TÀI LIỆU THAM KHẢO [1] Inc.com Internet: https://www.inc.com/encyclopedia/robotics.html [Accessed: 21Dec-2022] [2] “RV-FR Series Standard Specifications Manual”, Mitsubishi Electric Corporation, 2017-2018 [3] “M-20iB/25,” Fanucamerica.com Internet: https://www.fanucamerica.com/products/robots/series/m-20/m-20ib-25 [Accessed: 21-Dec-2022] [4] W Khalil and E Dombre, “Introduction to geometric and kinematic modeling of parallel robots,” in Modeling, Identification and Control of Robots, Elsevier, 2002, pp 171–190 [5] “IRB 360,” Robotics Internet: https://new.abb.com/products/robotics/industrialrobots/irb-360 [Accessed: 21-Dec-2022] [6] “FANUC M-2iA/3A delta robot,” Fanucamerica.com Internet: https://www.fanucamerica.com/products/robots/series/m-2ia-delta-robots/m-2ia3a [Accessed: 21-Dec-2022] [7] “RH-FRH Series Standard Specifications Manual”, Mitsubishi Electric Corporation, 2017-2018 [8] “Irb 910sc,” Robotics Internet: https://new.abb.com/products/robotics/industrialrobots/irb-910sc [Accessed: 21-Dec-2022] [9] “Motoman MPL500 – Yaskawa Việt Nam.” Internet: https://yaskawavietnam.vn/product/motoman-mpl500/ (accessed Dec 21, 2022) 108 TÀI LIỆU THAM KHẢO [10] “M-410iC/185,” Fanuc.eu Internet: https://www.fanuc.eu/at/en/robots/robotfilter-page/m-410-series/m-410ic-185 [Accessed: 21-Dec-2022] [11] V Arakelian and S Briot, Balancing of linkages and robot manipulators: Advanced methods with illustrative examples Cham, Switzerland: Springer International Publishing, 2016 [12] V Arakelian, Ed., Gravity Compensation in Robotics, 1st ed Cham, Switzerland: Springer Nature, 2022 [13] G Zeng and A Hemami, “An overview of robot force control,” Robotica, vol 15, no 5, pp 473–482, 1997 [14] “Robotic Palletizer - Haver & Boecker USA,” Haver & Boecker USA, 04-Aug2020 Internet: https://haverusa.com/products/palletizing/robotic-palletizer/ [Accessed: 21-Dec-2022] [15] C Tan et al., “Fire Fighting Mobile Robot: State of the Art and Recent Development,” Australian journal of basic and applied sciences, vol 7, pp 220– 230, 2013 [16] P Sonsale, Computer Engineering, Padmabhooshan Vasantdada Patil Institute of Technology/Pune University, India, R Gawas, S Pise, and A Kaldate, “Intelligent Fire Extinguisher System,” IOSR J Comput Eng., vol 16, no 1, pp 59–61, 2014 [17] N Italiya, “Class A fire test (Kanex brand),” 24-May-2014 Internet: https://www.youtube.com/watch?v=MWbCG68oFS4 [Accessed: 21-Dec-2022] 109 TÀI LIỆU THAM KHẢO [18] A Klimchik, E Magid, S Caro, K Waiyakan, and A Pashkevich, “Stiffness of serial and quasi-serial manipulators: comparison analysis,” MATEC Web Conf., vol 75, p 02003, 2016 [19] “RV-4F/7F/13F/20F Series Instruction Manual: Robot Arm Setup and Maintenance”, Mitsubishi Electric Corporation, 2012-2013 [20] “RV-100TFH-Q/150TFH-Q/ RV-100TFHL-Q/150TFHL-Q Series Instruction Manual: Robot Arm Setup and Maintenance”, Mitsubishi Electric Corporation, 2012-2013 [21] R N Jazar, Theory of applied robotics: Kinematics, dynamics, and control (2nd edition), 2nd ed New York, NY: Springer, 2010 [22] W M Silver, “On the equivalence of Lagrangian and Newton-Euler dynamics for manipulators,” Int J Rob Res., vol 1, no 2, pp 60–70, 1982 [23] Herman Hoifodt, “Dynamic Modeling and Simulation of Robot Manipulator: The Newton-Euler Formulation”, Master Thesis, Norwegian University of Science and Technology, 2011 [24] A Nikoobin, M Moradi, and A Esmaili, “Optimal spring balancing of robot manipulators in point-to-point motion,” Robotica, vol 31, no 4, pp 611–621, 2013 [25] M Safeea, P Neto, and R Bearee, “Robot dynamics: A recursive algorithm for efficient calculation of Christoffel symbols,” Mech Mach Theory, vol 142, no 103589, p 103589, 2019 [26] T R Kurfess, Robotics and Automation Handbook London, England: CRC Press, 2005 110 PHỤ LỤC PHỤ LỤC PL1 Chi tiết ma trận động lực học tay máy - Ma trận Gravitational 𝐺 (𝑞) = [𝐺11; 𝐺21; 𝐺31; 𝐺41] ma trận 4𝑥1, đó: 𝐺11 = 0; 𝐺21 = (𝑎2 ∗ 𝑔 ∗ 𝑚2 ∗ 𝑐𝑜𝑠(𝑡2))/2 − (𝑘𝑠𝑝𝑟𝑖𝑛𝑔 ∗ 𝑙_𝑔𝑠𝑚^2 ∗ 𝑠𝑖𝑛(𝑡2))/4 + 𝑎2 ∗ 𝑔 ∗ 𝑚3 ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎2 ∗ 𝑔 ∗ 𝑚4 ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎2 ∗ 𝑔 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎2 ∗ 𝑔 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑐𝑜𝑠(𝑡2); 𝐺31 = 𝑔 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ (𝑎3 ∗ 𝑐𝑜𝑠(𝑡3) + 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡3) − 𝑦𝑙 ∗ 𝑠𝑖𝑛(𝑡3)) + (𝑎3 ∗ 𝑔 ∗ 𝑚3 ∗ 𝑐𝑜𝑠(𝑡3))/2 + 𝑎3 ∗ 𝑔 ∗ 𝑚4 ∗ 𝑐𝑜𝑠(𝑡3) + 𝑎3 ∗ 𝑔 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡3) − 𝑔 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡3); 𝐺41 = (𝑎5 ∗ 𝑔 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡4))/2; - Ma trận Inertia 𝑀 (𝑞) = [𝑀11 𝑀12 𝑀13 𝑀14; 𝑀21 𝑀22 𝑀23 𝑀24; 𝑀31 𝑀32 𝑀33 𝑀34; 𝑀41 𝑀42 𝑀43 𝑀44], đó: 𝑀11 = 𝐼𝑦1 + 𝐼𝑦2 ∗ 𝑐𝑜𝑠(𝑡2)^2 + 𝐼𝑦3 ∗ 𝑐𝑜𝑠(𝑡3)^2 + 𝐼𝑦4 ∗ 𝑐𝑜𝑠(𝑡4)^2 + 𝐼𝑥2 ∗ 𝑠𝑖𝑛(𝑡2)^2 + 𝐼𝑥3 ∗ 𝑠𝑖𝑛(𝑡3)^2 + 𝐼𝑥4 ∗ 𝑠𝑖𝑛(𝑡4)^2 + (𝑚5 ∗ 𝑐𝑜𝑠(𝑡1)^2 ∗ (2 ∗ 𝑎1 + ∗ 𝑎4 + ∗ 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎3 ∗ 𝑐𝑜𝑠(𝑡3) + 𝑎5 ∗ 𝑐𝑜𝑠(𝑡4))^2)/4 + (𝑚4 ∗ 𝑐𝑜𝑠(𝑡1)^2 ∗ (2 ∗ 𝑎1 + 𝑎4 + ∗ 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎3 ∗ 𝑐𝑜𝑠(𝑡3))^2)/4 + (𝑚2 ∗ 𝑐𝑜𝑠(𝑡1)^2 ∗ (2 ∗ 𝑎1 + 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2))^2)/4 + (𝑚5 ∗ 𝑠𝑖𝑛(𝑡1)^2 ∗ (2 ∗ 𝑎1 + ∗ 𝑎4 + ∗ 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎3 ∗ 𝑐𝑜𝑠(𝑡3) + 𝑎5 ∗ 𝑐𝑜𝑠(𝑡4))^2)/4 + (𝑚4 ∗ 𝑠𝑖𝑛(𝑡1)^2 ∗ (2 ∗ 𝑎1 + 𝑎4 + ∗ 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎3 ∗ 𝑐𝑜𝑠(𝑡3))^2)/4 + (𝑚2 ∗ 𝑠𝑖𝑛(𝑡1)^2 ∗ (2 ∗ 𝑎1 + 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2))^2)/4 + 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡1)^2 ∗ (𝑎1 + 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) − 𝑙𝑐 ∗ 𝑐𝑜𝑠(𝑡3))^2 + 𝑚𝑐𝑤 ∗ 𝑠𝑖𝑛(𝑡1)^2 ∗ (𝑎1 + 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) − 𝑙𝑐 ∗ 𝑐𝑜𝑠(𝑡3))^2 + (𝑚3 ∗ 𝑐𝑜𝑠(𝑡1)^2 ∗ (2 ∗ 𝑎1 + ∗ 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎3 ∗ 𝑐𝑜𝑠(𝑡3))^2)/4 + (𝑚3 ∗ 𝑠𝑖𝑛(𝑡1)^2 ∗ (2 ∗ 𝑎1 + ∗ 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎3 ∗ 𝑐𝑜𝑠(𝑡3))^2)/4 + 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑐𝑜𝑠(𝑡1)^2 ∗ (𝑎1 + 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎3 ∗ 𝑐𝑜𝑠(𝑡3) + 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡3) − 𝑦𝑙 ∗ 𝑠𝑖𝑛(𝑡3))^2 + (𝑎1^2 ∗ 𝑚1 ∗ 𝑐𝑜𝑠(𝑡1)^2)/4 + 𝑚𝑙𝑜𝑎𝑑 ∗ 111 PHỤ LỤC 𝑠𝑖𝑛(𝑡1)^2 ∗ (𝑎1 + 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎3 ∗ 𝑐𝑜𝑠(𝑡3) + 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡3) − 𝑦𝑙 ∗ 𝑠𝑖𝑛(𝑡3))^2 + (𝑎1^2 ∗ ∗ 𝑠𝑖𝑛(𝑡1)^2)/4; 𝑀12 = 𝑀21 = 0; 𝑀13 = 𝑀31 = 0; 𝑀14 = 𝑀41 = 0; 𝑀22 = 𝐼𝑧2 + (𝑎2^2 ∗ 𝑚2)/4 + 𝑎2^2 ∗ 𝑚3 + 𝑎2^2 ∗ 𝑚4 + 𝑎2^2 ∗ 𝑚5 + (𝑎2^2 ∗ 𝑚𝑐𝑤)/2 + 𝑎2^2 ∗ 𝑚𝑙𝑜𝑎𝑑 − (𝑎2^2 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(2 ∗ 𝑡2))/2; 𝑀23 = 𝑀32 = (𝑎2 ∗ (𝑎3 ∗ 𝑚3 ∗ 𝑐𝑜𝑠(𝑡2 − 𝑡3) + ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑐𝑜𝑠(𝑡2 − 𝑡3) + ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡2 − 𝑡3) + ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑐𝑜𝑠(𝑡2 − 𝑡3) − 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡2 − 𝑡3) + ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡2 − 𝑡3) + ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡2 + 𝑡3)))/2; 𝑀24 = 𝑀42 = (𝑎2 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡2 − 𝑡4))/2; 𝑀33 = 𝐼𝑧3 + (𝑎3^2 ∗ 𝑚3)/4 + 𝑎3^2 ∗ 𝑚4 + 𝑎3^2 ∗ 𝑚5 + 𝑎3^2 ∗ 𝑚𝑙𝑜𝑎𝑑 + 𝑙𝑐^2 ∗ 𝑚𝑐𝑤 + 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙^2 + 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙^2 + ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙; 𝑀34 = 𝑀43 = (𝑎3 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡3 − 𝑡4))/2; 𝑀44 = (𝑚5 ∗ 𝑎5^2)/4 + 𝐼𝑧4; - Ma trận Coriolis/ centrifugal 𝐶 (𝑞̇ , 𝑞) = [𝐶11 𝐶12 𝐶13 𝐶14; 𝐶21 𝐶22 𝐶23 𝐶24; 𝐶31 𝐶32 𝐶33 𝐶34; 𝐶41 𝐶42 𝐶43 𝐶44], đó: 𝐶11 = (𝐼𝑥2 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡2))/2 + (𝐼𝑥3 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝐼𝑥4 ∗ 𝑡4_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡4))/2 − (𝐼𝑦2 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡2))/2 − (𝐼𝑦3 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − (𝐼𝑦4 ∗ 𝑡4_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡4))/2 + 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑦𝑙 + 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑥𝑙 ∗ 𝑦𝑙 − (𝑎2^2 ∗ 𝑚2 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡2))/8 − (𝑎2^2 ∗ 𝑚3 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡2))/2 − (𝑎2^2 ∗ 𝑚4 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡2))/2 − (𝑎2^2 ∗ 𝑚5 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡2))/2 − (𝑎3^2 ∗ 𝑚3 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/8 − (𝑎3^2 ∗ 𝑚4 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − (𝑎3^2 ∗ 𝑚5 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − (𝑎5^2 ∗ 𝑚5 ∗ 𝑡4_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡4))/8 − (𝑎2^2 ∗ 𝑚𝑐𝑤 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡2))/2 − (𝑎2^2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡2))/2 − (𝑎3^2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − (𝑙𝑐^2 ∗ 𝑚𝑐𝑤 ∗ 𝑡3_𝑑 ∗ 112 PHỤ LỤC 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − (𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑥𝑙^2 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑦𝑙^2 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − 𝑎1 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑥𝑙 ∗ 𝑠𝑖𝑛(𝑡3) − ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡3)^2 − 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑥𝑙 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3) − ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑥𝑙 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡3)^2 − (𝑎1 ∗ 𝑎2 ∗ 𝑚2 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(𝑡2))/2 − 𝑎1 ∗ 𝑎2 ∗ 𝑚3 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(𝑡2) − 𝑎1 ∗ 𝑎2 ∗ 𝑚4 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(𝑡2) − 𝑎1 ∗ 𝑎2 ∗ 𝑚5 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(𝑡2) − (𝑎1 ∗ 𝑎3 ∗ 𝑚3 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(𝑡3))/2 − 𝑎1 ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(𝑡3) − (𝑎2 ∗ 𝑎4 ∗ 𝑚4 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(𝑡2))/2 − 𝑎1 ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(𝑡3) − 𝑎2 ∗ 𝑎4 ∗ 𝑚5 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(𝑡2) − (𝑎3 ∗ 𝑎4 ∗ 𝑚4 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(𝑡3))/2 − 𝑎3 ∗ 𝑎4 ∗ 𝑚5 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(𝑡3) − (𝑎1 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡4_𝑑 ∗ 𝑠𝑖𝑛(𝑡4))/2 − (𝑎4 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡4_𝑑 ∗ 𝑠𝑖𝑛(𝑡4))/2 − 𝑎1 ∗ 𝑎2 ∗ 𝑚𝑐𝑤 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(𝑡2) − 𝑎1 ∗ 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(𝑡2) − 𝑎1 ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(𝑡3) − 𝑎1 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡3) + 𝑎1 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(𝑡3) − (𝑎2 ∗ 𝑎3 ∗ 𝑚3 ∗ 𝑡2_𝑑 ∗ 𝑐𝑜𝑠(𝑡3) ∗ 𝑠𝑖𝑛(𝑡2))/2 − (𝑎2 ∗ 𝑎3 ∗ 𝑚3 ∗ 𝑡3_𝑑 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3))/2 − 𝑎2 ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑡2_𝑑 ∗ 𝑐𝑜𝑠(𝑡3) ∗ 𝑠𝑖𝑛(𝑡2) − 𝑎2 ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑡3_𝑑 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) − 𝑎2 ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑡2_𝑑 ∗ 𝑐𝑜𝑠(𝑡3) ∗ 𝑠𝑖𝑛(𝑡2) − 𝑎2 ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑡3_𝑑 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) − (𝑎2 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡2_𝑑 ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡2))/2 − (𝑎2 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡4_𝑑 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡4))/2 − (𝑎3 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡3_𝑑 ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡3))/2 − (𝑎3 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡4_𝑑 ∗ 𝑐𝑜𝑠(𝑡3) ∗ 𝑠𝑖𝑛(𝑡4))/2 − 𝑎2 ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡2_𝑑 ∗ 𝑐𝑜𝑠(𝑡3) ∗ 𝑠𝑖𝑛(𝑡2) − 𝑎2 ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) − 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑐𝑜𝑠(𝑡3) + 𝑎2 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑡2_𝑑 ∗ 𝑐𝑜𝑠(𝑡3) ∗ 𝑠𝑖𝑛(𝑡2) + 𝑎2 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑡3_𝑑 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) − 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡2_𝑑 ∗ 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡3) ∗ 𝑠𝑖𝑛(𝑡2) − 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡3_𝑑 ∗ 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑡2_𝑑 ∗ 𝑦𝑙 ∗ 𝑠𝑖𝑛(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3); 𝐶12 = −(𝑡1_𝑑 ∗ 𝑠𝑖𝑛(𝑡2) ∗ (4 ∗ 𝐼𝑦2 ∗ 𝑐𝑜𝑠(𝑡2) − ∗ 𝐼𝑥2 ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎2^2 ∗ 𝑚2 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚3 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚4 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚2 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚3 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚4 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚5 + ∗ 𝑎2 ∗ 𝑎4 ∗ 𝑚4 + ∗ 𝑎2 ∗ 𝑎4 ∗ 𝑚5 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚𝑐𝑤 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 + 113 PHỤ LỤC ∗ 𝑎2 ∗ 𝑎3 ∗ 𝑚3 ∗ 𝑐𝑜𝑠(𝑡3) + ∗ 𝑎2 ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑐𝑜𝑠(𝑡3) + ∗ 𝑎2 ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡3) + ∗ 𝑎2 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡4) + ∗ 𝑎2 ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑐𝑜𝑠(𝑡3) − ∗ 𝑎2 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡3) + ∗ 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡3) − ∗ 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑠𝑖𝑛(𝑡3)))/4; 𝐶13 = −𝑡1_𝑑 ∗ ((𝐼𝑦3 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − (𝐼𝑥3 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 − 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑦𝑙 + (𝑎3^2 ∗ 𝑚3 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/8 + (𝑎3^2 ∗ 𝑚4 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑎3^2 ∗ 𝑚5 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑎3^2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑙𝑐^2 ∗ 𝑚𝑐𝑤 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙^2 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − (𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙^2 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑎1 ∗ 𝑎3 ∗ 𝑚3 ∗ 𝑠𝑖𝑛(𝑡3))/2 + 𝑎1 ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎1 ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑠𝑖𝑛(𝑡3) + (𝑎3 ∗ 𝑎4 ∗ 𝑚4 ∗ 𝑠𝑖𝑛(𝑡3))/2 + 𝑎3 ∗ 𝑎4 ∗ 𝑚5 ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎1 ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎1 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡3) − 𝑎1 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎1 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑠𝑖𝑛(𝑡3) + ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡3)^2 + 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3) + ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡3)^2 + (𝑎2 ∗ 𝑎3 ∗ 𝑚3 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3))/2 + 𝑎2 ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎2 ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) + (𝑎3 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡3))/2 + 𝑎2 ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑐𝑜𝑠(𝑡3) − 𝑎2 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3)); 𝐶14 = −(𝑡1_𝑑 ∗ 𝑠𝑖𝑛(𝑡4) ∗ (4 ∗ 𝐼𝑦4 ∗ 𝑐𝑜𝑠(𝑡4) − ∗ 𝐼𝑥4 ∗ 𝑐𝑜𝑠(𝑡4) + 𝑎5^2 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡4) + ∗ 𝑎1 ∗ 𝑎5 ∗ 𝑚5 + ∗ 𝑎4 ∗ 𝑎5 ∗ 𝑚5 + ∗ 𝑎2 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎3 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡3)))/4; 𝐶21 = (𝑡1_𝑑 ∗ 𝑠𝑖𝑛(𝑡2) ∗ (4 ∗ 𝐼𝑦2 ∗ 𝑐𝑜𝑠(𝑡2) − ∗ 𝐼𝑥2 ∗ 𝑐𝑜𝑠(𝑡2) + 𝑎2^2 ∗ 𝑚2 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚3 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚4 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎2^2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚2 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚3 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚4 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚5 + ∗ 𝑎2 ∗ 𝑎4 ∗ 𝑚4 + ∗ 𝑎2 ∗ 𝑎4 ∗ 𝑚5 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚𝑐𝑤 + ∗ 𝑎1 ∗ 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 + ∗ 𝑎2 ∗ 𝑎3 ∗ 𝑚3 ∗ 𝑐𝑜𝑠(𝑡3) + ∗ 𝑎2 ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑐𝑜𝑠(𝑡3) + ∗ 𝑎2 ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡3) + ∗ 𝑎2 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡4) + ∗ 𝑎2 ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑐𝑜𝑠(𝑡3) − ∗ 114 PHỤ LỤC 𝑎2 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡3) + ∗ 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡3) − ∗ 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑠𝑖𝑛(𝑡3)))/4; 𝐶22 = (𝑎2^2 ∗ 𝑚𝑐𝑤 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡2))/2; 𝐶23 = (𝑎2 ∗ 𝑡3_𝑑 ∗ (𝑎3 ∗ 𝑚3 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) − ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡2 − 𝑡3) − 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) − 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑠𝑖𝑛(𝑡2 + 𝑡3)))/2; 𝐶24 = (𝑎2 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡4_𝑑 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡4))/2; 𝐶31 = 𝑡1_𝑑 ∗ ((𝐼𝑦3 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − (𝐼𝑥3 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 − 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑦𝑙 + (𝑎3^2 ∗ 𝑚3 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/8 + (𝑎3^2 ∗ 𝑚4 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑎3^2 ∗ 𝑚5 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑎3^2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑙𝑐^2 ∗ 𝑚𝑐𝑤 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙^2 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 − (𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙^2 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3))/2 + (𝑎1 ∗ 𝑎3 ∗ 𝑚3 ∗ 𝑠𝑖𝑛(𝑡3))/2 + 𝑎1 ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎1 ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑠𝑖𝑛(𝑡3) + (𝑎3 ∗ 𝑎4 ∗ 𝑚4 ∗ 𝑠𝑖𝑛(𝑡3))/2 + 𝑎3 ∗ 𝑎4 ∗ 𝑚5 ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎1 ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎1 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡3) − 𝑎1 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎1 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑠𝑖𝑛(𝑡3) + ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡3)^2 + 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑠𝑖𝑛(2 ∗ 𝑡3) + ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡3)^2 + (𝑎2 ∗ 𝑎3 ∗ 𝑚3 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3))/2 + 𝑎2 ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎2 ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) + (𝑎3 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡3))/2 + 𝑎2 ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑐𝑜𝑠(𝑡3) − 𝑎2 ∗ 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3) + 𝑎2 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3)); 𝐶32 = −(𝑎2 ∗ 𝑡2_𝑑 ∗ (𝑎3 ∗ 𝑚3 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + ∗ 𝑎3 ∗ 𝑚4 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + ∗ 𝑎3 ∗ 𝑚5 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + ∗ 𝑎3 ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) − ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑦𝑙 ∗ 𝑐𝑜𝑠(𝑡2 − 𝑡3) − 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + ∗ 𝑚𝑙𝑜𝑎𝑑 ∗ 𝑥𝑙 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡3) + 𝑙𝑐 ∗ 𝑚𝑐𝑤 ∗ 𝑠𝑖𝑛(𝑡2 + 𝑡3)))/2; 𝐶33 = 0; 𝐶34 = (𝑎3 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡4_𝑑 ∗ 𝑠𝑖𝑛(𝑡3 − 𝑡4))/2; 115 PHỤ LỤC 𝐶41 = (𝑡1_𝑑 ∗ 𝑠𝑖𝑛(𝑡4) ∗ (4 ∗ 𝐼𝑦4 ∗ 𝑐𝑜𝑠(𝑡4) − ∗ 𝐼𝑥4 ∗ 𝑐𝑜𝑠(𝑡4) + 𝑎5^2 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡4) + ∗ 𝑎1 ∗ 𝑎5 ∗ 𝑚5 + ∗ 𝑎4 ∗ 𝑎5 ∗ 𝑚5 + ∗ 𝑎2 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡2) + ∗ 𝑎3 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑐𝑜𝑠(𝑡3)))/4; 𝐶42 = −(𝑎2 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡2_𝑑 ∗ 𝑠𝑖𝑛(𝑡2 − 𝑡4))/2; 𝐶43 = −(𝑎3 ∗ 𝑎5 ∗ 𝑚5 ∗ 𝑡3_𝑑 ∗ 𝑠𝑖𝑛(𝑡3 − 𝑡4))/2; 𝐶44 = 0; 116 LÝ LỊCH TRÍCH NGANG PHẦN LÝ LỊCH TRÍCH NGANG Họ tên: Châu Thái Nguyên Ngày, tháng, năm sinh: 17/09/1997 Nơi sinh: Xuân Lộc, Đồng Nai Địa chỉ: Số 75, Đường 7, Ấp 5, Xuân Tâm, Xuân Lộc, Đồng Nai QUÁ TRÌNH ĐÀO TẠO: - 2015 – 2020: Chuyên ngành Cơ Điện Tử, chương trình PFIEV, Đại học Bách Khoa - 2021 – nay: Cao học chuyên ngành Cơ Điện Tử, Đại học Bách Khoa, Đại học Quốc gia Thành phố Hồ Chí Minh Q TRÌNH CƠNG TÁC: - 10/2020 – 6/2022: Cơng ty Emage Development - 6/2022 – nay: Phịng thí nghiệm Trọng điểm Điều khiển số Kỹ thuật hệ thống 117