Thiết kế bộ điều khiển phi tuyến cho cánh tay robot sáu bậc tự do trong thực nghiệm

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 CƠNG TRÌNH NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN THIẾT KẾ BỘ ĐIỀU KHIỂN PHI TUYẾN CHO CÁNH TAY ROBOT SÁU BẬC TỰ DO TRONG THỰC NGHIỆM S K C 0 9 MÃ SỐ: SV2022-27 CHỦ NHIỆM ĐỀ TÀI: TRẦN MINH PHÚC SKC008040 Tp Hồ Chí Minh, tháng 11/2022 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM BÁO CÁO TỔNG KẾT ĐỀ TÀI NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN THIẾT KẾ BỘ ĐIỀU KHIỂN PHI TUYẾN CHO CÁNH TAY ROBOT SÁU BẬC TỰ DO TRONG THỰC NGHIỆM SV2022-27 Thuộc nhóm ngành khoa học: Kỹ thuật SV thực hiện: Trần Minh Phúc Nam, Nữ: Nam Dân tộc: Kinh Lớp, khoa: 18151B – Khoa Điện - Điện tử Năm thứ: / Số năm đào tạo: Ngành học: Công nghệ kỹ thuật điều khiển tự động hóa Người hướng dẫn: TS Trần Đức Thiện TP Hồ Chí Minh, 11/2022 MỤC LỤC MỤC LỤC i DANH MỤC BẢNG BIỂU iii DANH MỤC HÌNH ẢNH iv DANH MỤC CÁC TỪ VIẾT TẮT VÀ TIẾNG ANH vi THÔNG TIN KẾT QUẢ NGHIÊN CỨU CỦA ĐỀ TÀI viii MỞ ĐẦU xi Tổng quan tình hình nghiên cứu xi Lý chọn đề tài xi Mục tiêu đề tài xii Phương pháp nghiên cứu xii Đối tượng phạm vi nghiên cứu xii CHƯƠNG CƠ SỞ LÝ THUYẾT 1.1 Tổng quan cánh tay máy công nghiệp 1.1.1 Định nghĩa cánh tay máy công nghiệp 1.1.2 Phân loại cánh tay máy công nghiệp 1.1.3 Thành phần cánh tay máy công nghiệp 1.2 Động học cánh tay máy 1.2.1 Gắn hệ toạ độ lên khâu cánh tay máy 1.2.2 Động học thuận 1.2.3 Động học nghịch 10 1.3 Vận tốc động lực học cánh tay máy .11 1.3.1 Vận tốc góc, vận tốc dài 11 1.3.2 Ma trận Jacobian 11 1.3.3 Động lực học 12 1.4 Quy hoạch quỹ đạo cánh tay máy .14 1.5 Thuyết ổn định Lyapunov 16 1.6 Một số phương pháp điều khiển áp dụng cho cánh tay máy 17 I 1.6.1 Bộ điều khiển trượt 17 1.6.2 Bộ điều khiển trượt thích nghi .18 1.7 Công cụ đánh giá điều khiển 19 CHƯƠNG KẾT QUẢ NGHIÊN CỨU .20 2.1 Mô tả phần cứng thực nghiệm hệ thống 20 2.1.1 Yêu cầu thiết kế 20 2.1.2 Sơ đố khối phần cứng hệ thống 20 2.1.3 Lựa chọn thiết bị phần cứng 21 2.1.4 Sơ đồ dây 35 2.2 Tính tốn động học cánh tay máy .37 2.2.1 Đặt hệ trục toạ độ 37 2.2.2 Động học thuận 37 2.2.3 Động học nghịch 39 2.3 Tính toán vận tốc động lực học cánh tay máy 42 2.3.1 Tính tốn vận tốc góc vận tốc dài 42 2.3.2 Tính tốn ma trận Jacobian 45 2.3.3 Tính tốn động lực học cánh tay máy 46 2.4 Thiết kế điều khiển phi tuyến áp dụng cho hệ cánh tay máy 49 2.4.1 Bộ điều khiển trượt 49 2.4.2 Bộ điều khiển trượt thích nghi .51 CHƯƠNG ĐÁNH GIÁ KẾT QUẢ 54 3.1 Mô kiểm chứng điều khiển 54 3.2 Thực nghiệm kiểm chứng điều khiển 58 KẾT LUẬN VÀ KIẾN NGHỊ xiv Kết luận xiv Kiến nghị xiv TÀI LIỆU THAM KHẢO .xv PHỤ LỤC .xvi MINH CHỨNG SẢN PHẨM ĐỀ TÀI xxiii II DANH MỤC BẢNG BIỂU Bảng 1-1 Bảng phân loại cánh tay máy cơng nghiệp theo cấu trúc khí .1 Bảng 1-2 Bảng thông số Denavit – Hartenberg Bảng 1-3 Phương pháp quy hoạch quỹ đạo không gian làm việc không gian khớp 14 Bảng 2-1 Thông số kỹ thuật Denso VS-6556 21 Bảng 2-2 So sánh điều khiển trung tâm phổ biến 22 Bảng 2-3 Thông số kỹ thuật ADVANTECH 610H 24 Bảng 2-4 Thông số kỹ thuật PCI-6713 25 Bảng 2-5 Thông số kỹ thuật SCB-68A 26 Bảng 2-6 Thông số kỹ thuật PCI-QUAD04 26 Bảng 2-7 Thông số kỹ thuật AC Servo 28 Bảng 2-8 Thông số kỹ thuật MR-J3-A 29 Bảng 2-9 Thông số kỹ thuật Mitsubishi NF30-CS 31 Bảng 2-10 Thông số kỹ thuật Woonyoung WYFS20T1AD 32 Bảng 2-11 Thông số kỹ thuật Mitsubishi SD-Q11 DC24V 32 Bảng 2-12 Thông số kỹ thuật Mitsubishi CP30-BA AC/DC 33 Bảng 2-13 Thông số kỹ thuật Omron S8PS – 05024CD .34 Bảng 2-14 Thông số kỹ thuật Fuji Electric AR22V0L 34 Bảng 2-15 Thông số kỹ thuật Fuji Electric AR22PR 35 Bảng 2-16 Bảng Denavit-Hartenberg cánh tay máy bậc tự 37 Bảng 2-17 Bảng thông số vật lý cánh tay máy bậc tự 46 Bảng 3-1 Bảng thông số mô điều khiển mô .54 Bảng 3-2 Bảng thông số thực nghiệm điều khiển 58 Bảng 3-3 Bảng đánh giá sai số quỹ đạo điều khiển thực nghiệm 61 Bảng 3-4 Bảng đánh giá sai số quỹ đạo điều khiển thực nghiệm 65 III DANH MỤC HÌNH ẢNH Hình 1.1 Cấu trúc hệ thống điều khiển vòng hở Hình 1.2 Cấu trúc hệ thống điều khiển vịng kín Hình 1.3 Miêu tả bảng tham số Denavit Hartenberg .6 Hình 1.4 Bài toán động học thuận Hình 1.5 Bài tốn động học nghịch .10 Hình 1.6 a) Ổn định Lyapunov b) Ổn định tiệm cận Lyapunov 17 Hình 1.7 Cấu trúc điều khiển trượt 17 Hình 1.8 Giai đoạn hoạt động điều khiển trượt 18 Hình 1.9 Hiện tưởng chuyển mạch tần số cao 18 Hình 1.10 Sơ đồ khối điều khiển trượt thích nghi 19 Hình 2.1 Sơ đồ khối phần cứng hệ thống 21 Hình 2.2 Cánh tay máy Denso VS-6556 .21 Hình 2.3 Máy tính công nghiệp ADVANTECH 610H 24 Hình 2.4 Bo mạch xuất ngõ tương tự PCI-6713 25 Hình 2.5 Khối kết nối I/O SCB-68A .26 Hình 2.6 Bo mạch xử lý tín hiệu encoder PCI-QUAD04 .26 Hình 2.7 Động AC Servo Mitsubishi .28 Hình 2.8 Mơ-đun điều khiển động MR-J3-A 29 Hình 2.9 Aptomat khối Mitsubishi NF30-CS .31 Hình 2.10 Bộ lọc nguồn Woonyoung WYFS20T1AD 32 Hình 2.11 Cơng tắc tơ Mitsubishi SD-Q11 DC24V 32 Hình 2.12 Aptomat Mitsubishi CP30-BA AC/DC 33 Hình 2.13 Bộ nguồn Omron S8PS – 05024CD .34 Hình 2.14 Nút nhấn khẩn cấp Fuji Electric AR22V0L 34 Hình 2.15 Cơng tắc Fuji Electric AR22PR 35 Hình 2.16 Sơ đồ khối dây tổng quát hệ thống 36 Hình 2.20 Tủ điện điều khiển hệ thống 36 Hình 2.21 Tổng quan phần cứng hệ thống 36 IV Hình 2.22 a) Cánh tay máy bậc tự sử dụng hệ thống; b) Đặt hệ trục toạ độ cánh tay máy bậc tự .37 Hình 3.1 Đáp ứng ngõ khớp a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp .55 Hình 3.2 Sai số ngõ khớp a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp 56 Hình 3.3 Tín hiệu điều khiển khớp a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp 57 Hình 3.4 Đáp ứng ngõ a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp thực nghiệm quỹ đạo 59 Hình 3.5 Sai số ngõ a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp thực nghiệm quỹ đạo 60 Hình 3.6 Tín hiệu điều khiển a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp thực nghiệm quỹ đạo 61 Hình 3.7 Đáp ứng ngõ a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp thực nghiệm quỹ đạo 63 Hình 3.8 Sai số ngõ a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp thực nghiệm quỹ đạo 64 Hình 3.9 Tín hiệu điều khiển a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp thực nghiệm quỹ đạo 65 V DANH MỤC CÁC TỪ VIẾT TẮT VÀ TIẾNG ANH Từ viết tắt Tiếng Anh Tiếng Việt AC Alternating Current Điện xoay chiều DC Direct Current Điện chiều PLC Programmable Logic Controller Bộ điều khiển logic lập trình PID Proportional Integral Derivative Bộ điều khiển vi tích phân tỉ lệ PD Proportional Derivative Bộ điều khiển vi phân tỉ lệ I/O Input/Output Đầu vào/đầu ISO International Organization for Standardization Tổ chức tiêu chuẩn hoá quốc tế SMC Sliding mode control Điều khiển trượt MIMO Multiple input multiple output Nhiều đầu vào nhiều đầu 3D 3-Dimension chiều TP Trajectory planning Quy hoạch quỹ đạo SC Simscape FK Forward kinematics Động học thuận IK Inverse kinematics Động học nghịch FD Forward dynamics Động học lực thuận ID Inverse dynamics Động học lực nghịch AO Analog output Đầu tín hiệu tương tự DI Digital input Đầu vào tín hiệu số MCCB Moulded Case Circuit Breaker Cầu dao khối NF Noise Filter Bộ lọc nguồn VI C Contactor Công tắc tơ CB Circuit Breaker Cầu dao Viscous friction Ma sát nhớt Coulomb friction Ma sát tĩnh Emergency stop button Nút nhấn khẩn cấp Switch Công tắc Coriolis/centrifugal Coriolis/ly tâm Analog Output Ngõ tương tự Encoder Input Đầu vào tín hiệu từ mã hoá quay VII BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM THÔNG TIN KẾT QUẢ NGHIÊN CỨU CỦA ĐỀ TÀI Thông tin chung: - Tên đề tài: Thiết kế điều khiển phi tuyến cho cánh tay robot sáu bậc tự thực nghiệm - Chủ nhiệm đề tài: Trần Minh Phúc Mã số SV: 18151223 - Lớp: 181511B Khoa: Điện – điện tử - Thành viên đề tài: Stt Họ tên MSSV Lớp Khoa Hà Thanh Bình 18151157 181511B Điện – điện tử Tống Hải Ninh 18151212 181511C Điện – điện tử Nguyễn Minh Hoàng 19151127 19151CL1A Chất lượng cao - Người hướng dẫn: TS Trần Đức Thiện Mục tiêu đề tài: Mục tiêu chung: Tính tốn thiết kế điều khiển phi tuyến cho cánh tay máy sáu bậc tự Mục tiêu cụ thể: - Lập danh sách tìm hiểu tổng quan đề tài nghiên cứu liên quan - Xây dựng lại mơ hình cánh táy máy công nghiệp 2D 3D phần mềm chuyên dụng Phân tích, tính tốn động học, quy hoạch quỹ đạo động lực học cho cánh tay máy bậc tự Thiết kế thi công tủ điện cho mơ hình cánh tay máy bậc tự Xây dựng điều khiển phi tuyến điều khiển trượt (SMC) điều khiển trượt thích nghi (ASMC) Sử dụng thuyết ổn định Lyapunov để chứng minh - - tính ổn định hệ thống Áp dụng điều khiển đề xuất mô mơ hình thực nghiệm Đồng thời so sánh tính hiệu so với điều khiển tuyến tính Sử dụng hệ thống thu thập liệu để quan sát, đánh giá chất lượng hiệu suất hoạt động điều khiển - Viết báo cáo tổng kết, báo khoa học Bài báo khoa học cho tạp chí Khoa học Giáo dục Kỹ thuật VIII a) b) c) d) e) f) Hình 3.8 Đường màu xanh nét liền, đường màu đỏ nét liền biểu diễn cho sai số góc khớp tín hiệu điều khiển góc từ điều khiển PD, điều khiển SMC Bảng đánh giá sai số ngõ góc khớp quỹ đạo thực nghiệm thể Bảng 3-4 66 a) b) c) d) e) f) Hình 3.8 Sai số ngõ a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp thực nghiệm quỹ đạo 67 a) b) c) d) e) f) Hình 3.9 Tín hiệu điều khiển a) khớp 1; b) khớp 2; c) khớp 3; d) khớp 4; e) khớp 5; f) khớp thực nghiệm quỹ đạo Bảng 3-4 Bảng đánh giá sai số quỹ đạo điều khiển thực nghiệm Bộ điều khiển PID  PD 1 2 3 4 5 6 0.0209 -0.1492 -0.4088 -0.0074 0.0107 0.0350 68 SMC RMSEPD 0.1234 0.2110 0.4471 0.1882 0.1960 0.2054  SMC 0.0162 0.0095 -0.0049 0.0085 0.0221 0.0244 RMSESMC 0.11 0.0443 0.0424 0.1749 0.1822 0.2134 Nhận xét thể hai quỹ đạo: Khi sử dụng điều khiển PD sai số khớp khớp cao rõ rệt so với điều khiển SMC không bù thành phần phi tuyến hệ thống, thành phần từ ma trận trọng trường Lúc làm giảm độ xác sử dụng điều khiển tuyến tính cho hệ thống Với tính hiệu lên hệ thống phi tuyến phức tạp, sai số đáp ứng rằng, điều khiển SMC giúp hệ thống đáp ứng tốt kể hệ chịu ảnh hưởng từ thành phần không chắn từ thông số mô hình nhiễu đo lường tác động lên hệ Tuy nhiên ảnh hưởng thành phần bền vững có luật điều khiển SMC, làm cho tín hiệu dao động với tần số cao hay gọi tượng “chattering” Để khắc phục tượng nhóm đề xuất thay hàm sign thành phần bền vững điều khiển SMC thành hàm sat, sigmoid hay luật điều khiển thích nghi thay đổi hệ số eta trực tuyến theo hoạt động hệ (đã xem xét phần 3.1) 69 KẾT LUẬN VÀ KIẾN NGHỊ Kết luận Qua trình nghiên cứu thực hiên đề tài nhóm xin tổng hợp lại thành đạt được: - Tìm hiểu tổng quan cánh tay máy công nghiệp - Tính tốn động học động lực học cho cánh tay máy bậc tự - Quy hoạch quỹ đạo đường cho cánh tay máy Thiết kế, xây dựng điều khiển phi tuyến đánh giá chất lượng điều khiển - Thiết kế, thi công tủ điện điều khiển cho mơ hình thực nghiệm - Các thành phần điện xử lý trung tâm hoạt động tốt: + Giá trị trả từ encoder tương đối xác, khả lọc nhiễu tốt + Xuất tín hiệu điều khiển tương tự ngõ tốt, hoạt động ổn định - Xây dựng mơ hình hoạt động thời gian thực theo phương pháp Phần cứng - vịng lặp Kiểm chứng điều khiển mơ hình thực nghiệm, đánh giá chất lượng điều khiển - Viết báo khoa học cho Hội nghị quốc tế ICATSD 2022 tạp chí Trường Đại học Sư phạm Kỹ thuật Thành phố Hồ Chí Minh JTE 2022 Kiến nghị Nhóm xin kiến nghị số hướng phát triển tương lai đề tài sau - - Khảo sát xây dựng nhận dạng thơng số mơ hình theo thời gian thực cho đối tượng Sử dụng vi xử lý mạnh phát triển hệ thống thành bo mạch nhúng để thuận tiện việc điều khiển Nghiên cứu áp dụng điều khiển nâng cao xử lý vấn đề cịn tồn hệ thống qua giúp cải thiện độ xác độ bền vững cho hệ thống Bộ điều khiển mơ hình tiền đề để phát triển thành quan sát mở rộng nhằm mục đích quan sát thành phần lỗi, nhiễu hệ thống Đồng thời, phát triển lên giải thuật kiểm soát lỗi, bù lại thành phần lỗi để hệ thống hoạt động ổn định thời gian dài xiv TÀI LIỆU THAM KHẢO [1] T Đ Thiện, “THIẾT KẾ, MÔ PHỎNG, CHẾ TẠO VÀ ĐIỀU KHIỂN CÁNH TAY ROBOT BẬC TỰ DO,” Tạp Chí Khoa Học Giáo Dục Kỹ Thuật, vol 64, no September, pp 39–47, 2021 [2] P V Dư, “Controlling robot DOF with all revolute joint base on applying neural fuzzy controller,” Tạp chí Nghiên cứu KH&CN quân sự, vol 48, no November 2018, pp 26–34, 2017 [3] N H Lư, “An adaptive interval type-2 fuzzy logic controller for DOF manipulator,” Tạp chí Nghiên cứu KH&CN quân sự, vol 4, no April 2019, pp 31–40, 2019 [4] H Khan, S J Abbasi, K Dad Kallu, and M C Lee, “Robust Control Design of 6-DOF Robot for Nuclear Power Plant Dismantling,” 2019 Int Conf Robot Autom Ind ICRAI 2019, pp 6–12, 2019, doi: 10.1109/ICRAI47710.2019.8967383 [5] M I Ullah, S A Ajwad, R U Islam, U Iqbal, and J Iqbal, “Modeling and computed torque control of a degree of freedom robotic arm,” 2014 Int Conf Robot Emerg Allied Technol Eng iCREATE 2014 - Proc., pp 133–138, 2014, doi: 10.1109/iCREATE.2014.6828353 [6] A T Hasan, A M S Hamouda, N Ismail, and H M A A Al-Assadi, “An adaptive-learning algorithm to solve the inverse kinematics problem of a D.O.F serial robot manipulator,” Adv Eng Softw., vol 37, no 7, pp 432–438, 2006, doi: 10.1016/j.advengsoft.2005.09.010 [7] J F Allan, S Lavoie, S Reiher, and G Lambert, “Kinematic and dynamic analysis of a novel 6-DOF serial manipulator for underground distribution power lines,” IEEE Int Conf Intell Robot Syst., pp 1849–1856, 2011, doi: 10.1109/IROS.2011.6048118 [8] M Vaezi, F C Samavati, H E S Jazeh, and S A A Moosavian, “Singularity analysis of 6DOF Stäubli© TX40 robot,” 2011 IEEE Int Conf Mechatronics Autom ICMA 2011, pp 446–451, 2011, doi: 10.1109/ICMA.2011.5985699 xv PHỤ LỤC Sơ đồ dây mạch điều khiển Phụ lục Sơ đồ mạch điều khiển xvi Sơ đồ dây mạch động lực Phụ lục Sơ đồ mạch động lực xvii Phụ lục Sơ đồ mạch động lực xviii Chương trình máy tính điều khiển %% AUTHORS: % Made by Tran Minh Phuc, Tong Hai Ninh, Ha Thanh Binh % Topic: Nonlinear control for 6DOF Manipulator % Note: Mathematical model function [ddthe,dt] = DYN_6DOF(u,Fe,dthe,the,Fv,Fc,m,L,Pic,I) e1 = the(1); e2 = the(2); e3 = the(3); e4 = the(4); e5 = the(5); e6 = the(6); de1 = dthe(1); de2 = dthe(2); de3 = dthe(3); de4 = dthe(4); de5 = dthe(5); de6 = dthe(6); x1 = Pic(1,1); y1 = Pic(1,2); z1 = Pic(1,3); x2 = Pic(2,1); y2 = Pic(2,2); z2 = Pic(2,3); x3 = Pic(3,1); y3 = Pic(3,2); z3 = Pic(3,3); x4 = Pic(4,1); y4 = Pic(4,2); z4 = Pic(4,3); x5 = Pic(5,1); y5 = Pic(5,2); z5 = Pic(5,3); x6 = Pic(6,1); y6 = Pic(6,2); z6 = Pic(6,3); I1xx = I(1,1); I1yy = I(1,2); I1zz = I(1,3); I1xy = I(1,4); I1yz = I(1,5); I1zx = I(1,6); I2xx = I(2,1); I2yy = I(2,2); I2zz = I(2,3); I2xy = I(2,4); I2yz = I(2,5); I2zx = I(2,6); I3xx = I(3,1); I3yy = I(3,2); I3zz = I(3,3); I3xy = I(3,4); I3yz = I(3,5); I3zx = I(3,6); I4xx = I(4,1); I4yy = I(4,2); I4zz = I(4,3); I4xy = I(4,4); I4yz = I(4,5); I4zx = I(4,6); I5xx = I(5,1); I5yy = I(5,2); I5zz = I(5,3); I5xy = I(5,4); I5yz = I(5,5); I5zx = I(5,6); I6xx = I(6,1); I6yy = I(6,2); I6zz = I(6,3); I6xy = I(6,4); I6yz = I(6,5); I6zx = I(6,6); a1 = L(1); a2 = L(2); a3 = L(3); d1 = L(4); d4 = L(5); d7 = L(6); m1 = m(1); m2 = m(2); m3 = m(3); m4 = m(4); m5 = m(5); m6 = m(6); g = -9.80665; M = [M11 M12 M13 M14 M15 M16; M21 M22 M23 M24 M25 M26; M31 M32 M33 M34 M35 M36; M41 M42 M43 M44 M45 M46; M51 M52 M53 M54 M55 M56; M61 M62 M63 M64 M65 M66]; xix C = [C11 C12 C13 C14 C15 C16; C21 C22 C23 C24 C25 C26; C31 C32 C33 C34 C35 C36; C41 C42 C43 C44 C45 C46; C51 C52 C53 C54 C55 C56; C61 C62 C63 C64 C65 C66]; G = [G1; G2; G3; G4; G5; G6]; %% Dynamics % Fv: Viscous friction % Fc: Coulomb friction % Fe: External Force Fc_ = diag(Fc); Fv_ = diag(Fv); % dt: Friction dt = Fc_*sign(dthe) + Fv_*dthe ddthe = M\(u - C*dthe - G - dt); % Note: PD Controller function u = PD(the,dthe,the_r,dthe_r,Kp,Kd) % Controler parameters Kp_ = diag(Kp); Kd_ = diag(Kd); % Designed controller E = the_r - the; dE = dthe_r - dthe; u = Kp_*E + Kd_*dE; % Note: SMC Controller function [u] = SMC(the,dthe,the_r,dthe_r,ddthe_r,lamda,K,eta,m,L,Pic,I) e1 = the(1); e2 = the(2); e3 = the(3); e4 = the(4); e5 = the(5); e6 = the(6); de1 = dthe(1); de2 = dthe(2); de3 = dthe(3); de4 = dthe(4); de5 = dthe(5); de6 = dthe(6); x1 = Pic(1,1); y1 = Pic(1,2); z1 = Pic(1,3); x2 = Pic(2,1); y2 = Pic(2,2); z2 = Pic(2,3); x3 = Pic(3,1); y3 = Pic(3,2); z3 = Pic(3,3); x4 = Pic(4,1); y4 = Pic(4,2); z4 = Pic(4,3); x5 = Pic(5,1); y5 = Pic(5,2); z5 = Pic(5,3); xx x6 = Pic(6,1); y6 = Pic(6,2); z6 = Pic(6,3); I1xx = I(1,1); I1yy = I(1,2); I1zz = I(1,3); I1xy = I(1,4); I1yz = I(1,5); I1zx = I(1,6); I2xx = I(2,1); I2yy = I(2,2); I2zz = I(2,3); I2xy = I(2,4); I2yz = I(2,5); I2zx = I(2,6); I3xx = I(3,1); I3yy = I(3,2); I3zz = I(3,3); I3xy = I(3,4); I3yz = I(3,5); I3zx = I(3,6); I4xx = I(4,1); I4yy = I(4,2); I4zz = I(4,3); I4xy = I(4,4); I4yz = I(4,5); I4zx = I(4,6); I5xx = I(5,1); I5yy = I(5,2); I5zz = I(5,3); I5xy = I(5,4); I5yz = I(5,5); I5zx = I(5,6); I6xx = I(6,1); I6yy = I(6,2); I6zz = I(6,3); I6xy = I(6,4); I6yz = I(6,5); I6zx = I(6,6); a1 = L(1); a2 = L(2); a3 = L(3); d1 = L(4); d4 = L(5); d7 = L(6); m1 = m(1); m2 = m(2); m3 = m(3); m4 = m(4); m5 = m(5); m6 = m(6); g = -9.80665; M = [M11 M12 M13 M14 M15 M16; M21 M22 M23 M24 M25 M26; M31 M32 M33 M34 M35 M36; M41 M42 M43 M44 M45 M46; M51 M52 M53 M54 M55 M56; M61 M62 M63 M64 M65 M66]; C = [C11 C12 C13 C14 C15 C16; C21 C22 C23 C24 C25 C26; C31 C32 C33 C34 C35 C36; C41 C42 C43 C44 C45 C46; C51 C52 C53 C54 C55 C56; C61 C62 C63 C64 C65 C66]; G = [G1; G2; G3; G4; G5; G6]; %% Sliding mode control: % Controler parameters lamda_ = diag(lamda); K_ = diag(K); eta_ = diag(eta); % Designed controller e = the - the_r; xxi e_d = dthe - dthe_r; s = lamda_*e + e_d; u = -K_*s + C*(dthe_r - lamda_*e) + G + M*(ddthe_r - lamda_*e_d) eta_*sign(s); Phụ lục Chương trình máy tính điều khiển xxii MINH CHỨNG SẢN PHẨM ĐỀ TÀI Thư chấp nhận báo khoa học chấp thuận đăng kỷ yếu Hội nghị quốc tế “The 2nd International Conference on Advanced Technology & Sustainable Development – 2022” (ICATSD 2022) Giấy xác nhận báo khoa học chấp thuận đăng Tạp chí khoa học Đại học Sư phạm Kỹ thuật Thành phố Hồ Chí Minh “The Journal of Technical Education Science” (JTE 2022) xxiii S K L 0