Thiết kế bộ điều khiển phi tuyến cho cánh tay máy 6 bậc tự do

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 CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA THIẾT KẾ BỘ ĐIỀU KHIỂN PHI TUYẾN CHO CÁNH TAY MÁY BẬC TỰ DO GVHD: TS TRẦN ĐỨC THIỆN SVTH: TỐNG HẢI NINH HÀ THANH BÌNH TRẦN MINH PHÚC SKL009311 Tp Hồ Chí Minh - 08/2022 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MƠN TỰ ĐỘNG – ĐIỀU KHIỂN KHỐ LUẬN TỐT NGHIỆP ĐỀ TÀI: “THIẾT KẾ BỘ ĐIỀU KHIỂN PHI TUYẾN CHO CÁNH TAY MÁY BẬC TỰ DO” Giảng viên hướng dẫn: TS Trần Đức Thiện Nhóm sinh viên thực hiện: TỐNG HẢI NINH 18151212 HÀ THANH BÌNH 18151157 TRẦN MINH PHÚC 18151223 TP HỒ CHÍ MINH – 08/2022 TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN – ĐIỆN TỬ ĐỘC LẬP – TỰ DO – HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN -o0o TP Hồ Chí Minh, ngày … tháng … năm 2022 NHIỆM VỤ KHOÁ LUẬN TỐT NGHIỆP Họ tên sinh viên 1: Tống Hải Ninh MSSV: 18151212 Họ tên sinh viên 2: Hà Thanh Bình MSSV: 18151157 Họ tên sinh viên 3: Trần Minh Phúc MSSV: 18151223 Chuyên ngành: Công nghệ Kỹ thuật Điều khiển Tự động hố Hệ đào tạo: Chính quy Khoá: 2018 Lớp: 18151 Tên đề tài: Thiết kế điều khiển phi tuyến cho cánh tay máy bậc tự Nội dung đồ án: - 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ự - Thực chuyển đổi mô hình từ Solidworks sang MATLAB Simscape Multibody để mơ hình hố mơ hoạt động hệ thống trước đưa vào thực nghiệm - Kiểm chứng động học động lực học dựa MATLAB Simscape Multibody - 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 tuyến tính vi phân tỉ lệ (PD), áp dụng mơ mơ hình thực nghiệm - Xây dựng điều khiển phi tuyến gồm: điều khiển tính tốn mơ-men (CTC), điều khiển trượt (SMC) 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 - 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 Các sản phẩm dự kiến: - Mơ hình cánh tay máy bậc tự - Tủ điều khiển - Chương trình mơ chương trình thực nghiệm BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN i - Quyển báo cáo khố luận tốt nghiệp Poster tóm tắt đề tài - Video giới thiệu đề tài Ngày giao nhiệm vụ: 07/03/2022 Ngày hoàn thành nhiệm vụ: 26/07/2022 Họ tên cán hướng dẫn: TS Trần Đức Thiện Ngơn ngữ trình bày: Bản báo cáo: Tiếng Anh Tiếng Việt Trình bày bảo vệ: Tiếng Anh Tiếng Việt TP Hồ Chí Minh, ngày … tháng … năm 2022 Giáo viên hướng dẫn (Ký ghi rõ họ tên) BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN ii TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN – ĐIỆN TỬ ĐỘC LẬP – TỰ DO – HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN -o0o TP Hồ Chí Minh, ngày … tháng … năm 2022 LỊCH TRÌNH THỰC HIỆN KHỐ LUẬN TỐT NGHIỆP Họ tên sinh viên 1: Tống Hải Ninh MSSV: 18151212 Họ tên sinh viên 2: Hà Thanh Bình MSSV: 18151157 Họ tên sinh viên 3: Trần Minh Phúc MSSV: 18151223 Tên đề tài: Thiết kế điều khiển phi tuyến cho cánh tay máy bậc tự Ngày bắt đầu (tuần) 07/03/2022 (3 tuần) 28/03/2022 (2 tuần) Nội dung Xác nhận GVHD - Tìm hiều tổng quan đề tài - Tìm hiểu vấn đề liên quan phần cứng phần mềm - Xây dựng lại cấu hình 2D Visio - Xây dựng lại cấu hình 3D Solidworks - Tính tốn động học thuận, động học nghịch 11/04/2022 - Kiểm chứng động học mơ - Tính tốn động lực học quy hoạch quỹ đạo (3 tuần) - Mô quy hoạch quỹ đạo - Thực chuyển đổi mơ hình từ phần mềm CAD Simulink 02/05/2022 (1 tuần) 3D sang MATLAB thông qua Simscape Multibody - Thực kiểm chứng động học động lực học MATLAB Simulink, mơ mơ hình MATLAB Simscape Multibody BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN iii 09/05/2022 - phân tỉ lệ (PD), tính tốn mơ men (3 tuần) 30/05/2022 (CTC), trượt (SMC) - hệ thống - (8 tuần) 27/06/2022 Kiểm chứng tính tốn động học quy hoạch quỹ đạo cánh tay máy (1 tuần) 13/06/2022 Vẽ sơ đồ nguyên lý, sơ đồ mạch điện, mạch động lực, thi công tủ điện cho (1 tuần) 06/06/2022 Thiết kể mô điều khiển vi thực nghiệm - Áp dụng ba điều khiển thực nghiệm - Thu thập liệu đánh giá kết - Viết chỉnh sửa luận văn (5 tuần) TP Hồ Chí Minh, ngày … tháng … năm 2022 Giáo viên hướng dẫn (Ký ghi rõ họ tên) BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN iv TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN – ĐIỆN TỬ ĐỘC LẬP – TỰ DO – HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN -o0o TP Hồ Chí Minh, ngày … tháng … năm 2022 PHIẾU NHẬN XÉT KHOÁ LUẬN TỐT NGHIỆP (Dành cho giáo viên hướng dẫn) Về nội dung đề tài khối lượng thực hiện: Ưu điểm: Nhược điểm: Đề nghị cho bảo vệ: Có, Khơng Đánh giá loại: Điểm:…… (Bằng chữ:…………………….) TP Hồ Chí Minh, ngày … tháng … năm 2022 Giáo viên hướng dẫn (Ký ghi rõ họ tên) BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN v TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN – ĐIỆN TỬ ĐỘC LẬP – TỰ DO – HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN -o0o TP Hồ Chí Minh, ngày … tháng … năm 2022 PHIẾU NHẬN XÉT PHẢN BIỆN KHOÁ LUẬN TỐT NGHIỆP Ngành CNKT Điều khiển Tự động hóa Họ tên sinh viên : MSSV: : MSSV: : MSSV: Tên đề tài : : Giảng viên Hướng dẫn : Giảng viên Phản biện : Hội đồng : Ý KIẾN NHẬN XÉT Nhận xét chung nội dung đề tài Ý kiến kết luận (ghi rõ nội dung cần bổ sung, hiệu chỉnh) Đề nghị: Được bảo vệ:  Bổ sung để bảo vệ:  Không bảo vệ:  Điểm: _Bằng chữ: Câu hỏi phản biện (Giảng viên không cho SV biết trước) TP Hồ Chí Minh, ngày … tháng … năm 2022 Người nhận xét (Ký ghi rõ họ tên) BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN vi Điểm đánh giá đề tài theo rubric sau: Tiêu chí 1: Mức độ thời đề tài, độ khó đề tài (10%) 10 Thực Vấn đề vừa sức/Cần Vấn đề khó/Cần Vấn đề khó/Cần Quá dễ thực thực tế dành thời gian nhiều kiến thức tổng nhiều kiến thức tổng cần nghiên cứu hợp học hợp học Tiên chí 2: Tính ứng dụng đề tài vào thực tiễn (10%) 10 Thực tế bên Thỉnh thoảng có ứng Thực tế bên ngồi Khó ứng dụng Có ứng dụng cần cấp dụng cần thiết Tiêu chí 3: Tính đắn đề tài, phương pháp nghiên cứu hợp lý (10%) 10 Phương pháp nghiên Có phương pháp Phương pháp nghiên Có phương pháp cứu rõ ràng, khoa Không hợp lý nghiên cứu, định cứu rõ ràng, định nghiên cứu học, phù hợp với đề hướng hướng tài Tiêu chí 4: Giải pháp & cơng nghệ, thi công/mô (15%) 12 15 Giải pháp rõ ràng, có Giải pháp rõ ràng, có quy trình thực Giải pháp rõ ràng, có Giải pháp khơng rõ quy trình thực thi cơng/mơ Giải pháp sơ sài thi cơng mơ hình/mơ ràng thi cơng/mơ vận hành được, kết vận hành mơ phỏng/vận hành tốt Tiêu chí 5: Trình bày nội dung đồ án (15%) 12 15 Có đầy đủ cấu trúc nội dung, trình bày Có đầy đủ cấu trúc Nội dung khơng rõ Có đủ cấu trúc, nội hợp lý, khoa học, Báo cáo sơ sài nội dung, trình bày ràng dung logic, rõ ràng, dễ hợp lý, khoa học hiểu, quy định trình bày luận văn Tiêu chí 6: Khả trình bày (10%) 10 Rất tốt – Rõ ràng – Kém Trung bình Khá Tốt Tự tin Tiêu chí 7: Trả lời câu hỏi hội đồng bảo vệ (30%) 12 18 24 30 Trả lời < 20% Trả lời < 40% Trả lời < 60% Trả lời < 80% Trả lời 100% số câu hỏi số câu hỏi số câu hỏi số câu hỏi số câu hỏi Tổng điểm theo thang 10 Điểm TP Hồ Chí Minh, ngày … tháng … năm 2022 Người nhận xét (Ký ghi rõ họ tên) BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN vii TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN – ĐIỆN TỬ ĐỘC LẬP – TỰ DO – HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN -o0o TP Hồ Chí Minh, ngày … tháng … năm 2022 LỜI CAM ĐOAN Chúng xin cam kết đề tài tự thực dựa vào số tài liệu trước khơng chép từ tài liệu hay cơng trình có trước TP Hồ Chí Minh, ngày … tháng … năm 2022 Nhóm Sinh viên thực (Ký ghi rõ họ tên) Tống Hải Ninh – Hà Thanh Bình – Trần Minh Phúc BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN viii CHƯƠNG THỰC NGHIỆM ĐIỀU KHIỂN HỆ THỐNG a) b) c) d) e) f) Hình 5.47 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 BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 101 CHƯƠNG THỰC NGHIỆM ĐIỀU KHIỂN HỆ THỐNG a) b) c) d) e) f) Hình 5.48 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 5-19 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 BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 102 CHƯƠNG THỰC NGHIỆM ĐIỀU KHIỂN HỆ THỐNG CTC SMC RMSEPD 0.1234 0.2110 0.4471 0.1882 0.1960 0.2054  CTC 0.0216 -0.0189 0.0100 0.0188 0.0188 0.0344 RMSECTC 0.1225 0.1281 0.1537 0.2060 0.1990 0.2077  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: Ở quỹ đạo này, ba điều khiển bám tốt so với tín hiệu đặt Sai số từ điều khiển SMC nhỏ so với hai điều khiển lại nhiên cánh tay máy xuất hiện tượng rung dao động tần số cao tín hiệu điều khiển BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 103 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỀN ĐỀ TÀI CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI Ở chương này, nhóm trình bày kết thực dựa mô thực nghiệm Dựa vào kết thực đưa hạn chế mà nhóm cịn thiết sót q trình thực đề tài đồng thời đề xuất hướng phát triển thực cho đề tài tương lai 6.1 Kết luận Hầu hết cánh tay máy công nghiệp điều khiển phương pháp điều khiển tuyến tính PID, PD tính đơn giản dễ áp dụng vào hệ thống phức tạp, đặc biệt cánh tay máy nói chung cánh tay máy bậc tự nói riêng Tuy nhiên cánh tay máy hoạt động môi trường không lý tưởng chịu tải thay đổi có giới hạn định đặc tính phi tuyến cao hệ Với ý tưởng thiết kế điều khiển phi tuyến cho hệ cánh tay máy, đơn giản việc thêm thành phần phi tuyến có chủ đích vào điều khiển tuyến tính để bù ảnh hưởng thành phần không mong muốn cải thiện chất lượng hoạt động hệ thống cách hiệu Dựa vào tuyến tính hóa phản hồi tính tốn mơ-men u cầu hệ, điều khiển tuyến tính hố ngõ thiết kế nhằm cải thiện hệ thống nhờ tính chất triệt tiêu thành phần phi tuyến mạnh mẽ Bộ điều khiển hoạt động tốt thơng số động học cánh tay máy tính tốn xác khơng chịu q nhiều ảnh hưởng từ thành phần khơng chắn bên ngồi Do điều khiển trượt (SMC) đề xuất để kiểm sốt thành phần khơng chắn ảnh hưởng đến hệ nhờ thành phần bền vững điều khiển Việc sử dụng điều khiển trượt giúp tín hiệu ngõ hệ thống đáp ứng nhanh bám theo tín hiệu mong muốn, nhiên ảnh hưởng thành phần chắn khiến cho tín hiệu điều khiển gặp tượng dao động với tần số cao (chattering), việc làm giảm tuổi thọ động cố không mong muốn Để kiểm soát hệ thống cách tối ưu công việc nghiên cứu phát triển thực tương lai Qua trình nghiên cứu thực hiên dề 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ự - Kiểm chứng động học động lực học mơ phương trình vi phân MATLAB Simscape Multibody thông qua MATLAB Simulink 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: BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 104 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỀN ĐỀ TÀI + 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 Tuy thời gian thực có hạn nên đề tài cịn nhiều hạn chế 6.2 Hạn chế đề tài Các mặt hạn chế cịn gặp phải q trình thực đề tài là: - Các thơng số mơ hình thực nghiệm trích xuất từ mơ hình phần mềm CAD tồn lại sai số, dẫn đến chưa tối ưu phần điều khiển thực nghiệm - Phương trình tốn miêu tả hệ thống q dài nên việc nhúng điều khiển thực nghiệm chạy theo thời gian thực vi xử lý tốn nhiều thời gian để xử lý biên dịch chương trình 6.3 Hướng phát triển Qua hạn chế nêu trên, nhóm xin đề xuất 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 BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 105 TÀI LIỆU THAM KHẢO TÀI LIỆU THAM KHẢO [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] I F o Robotics "Industrial Robots." https://ifr.org/industrial-robots (accessed 2019) C V Nam "Tiềm Năng Phát Triển Của Cánh Tay Máy Công Nghiệp." http://www.hanoimoi.com.vn/ban-in/Khoa-hoc/915187/thi-truong-tiem-nangcho-robot-cong-nghiep (accessed 26/08/2021, 2021) Z Vukic, Nonlinear control systems CRC Press, 2003 V C Nguyen, A T Vo, and H J Kang, "A Finite-Time Fault-Tolerant Control Using Non-Singular Fast Terminal Sliding Mode Control and Third-Order Sliding Mode Observer for Robotic Manipulators," IEEE Access, vol 9, pp 31225-31235, 2021, doi: 10.1109/ACCESS.2021.3059897 A T Vo and H.-J Kang, "Adaptive neural integral full-order terminal sliding mode control for an uncertain nonlinear system," IEEE Access, vol 7, pp 4223842246, 2019 M Van, "An enhanced robust fault tolerant control based on an adaptive fuzzy PID-nonsingular fast terminal sliding mode control for uncertain nonlinear systems," IEEE/ASME Transactions on Mechatronics, vol 23, no 3, pp 13621371, 2018 K Lochan, S Suklabaidya, and B K Roy, "Sliding mode and adaptive sliding mode control approaches of two link flexible manipulator," in Proceedings of the 2015 Conference on Advances in Robotics, 2015, pp 1-6 S Beyhan, Z Lendek, R Babuska, M Wisse, and M Alci, "Adaptive fuzzy and sliding-mode control of a robot manipulator with varying payload," Proceedings of the IEEE Conference on Decision and Control, pp 8291-8296, 12/01 2011, doi: 10.1109/CDC.2011.6160802 J Liu and X Wang, Advanced sliding mode control for mechanical systems 2011 W Perruquetti and J P Barbot, Sliding mode control in engineering (Control engineering, no 11) New York: M Dekker, 2002, pp xvii, 409 p S.-H Han, M S Tran, and D.-T Tran, "Adaptive Sliding Mode Control for a Robotic Manipulator with Unknown Friction and Unknown Control Direction," Applied Sciences, vol 11, no 9, 2021, doi: 10.3390/app11093919 C M Ho, D T Tran, and K K Ahn, "Adaptive sliding mode control based nonlinear disturbance observer for active suspension with pneumatic spring," Journal of Sound and Vibration, vol 509, p 116241, 2021/09/29/ 2021, doi: https://doi.org/10.1016/j.jsv.2021.116241 T N Truong, A T Vo, and H J Kang, "A Backstepping Global Fast Terminal Sliding Mode Control for Trajectory Tracking Control of Industrial Robotic Manipulators," IEEE Access, vol 9, pp 31921-31931, 2021, doi: 10.1109/ACCESS.2021.3060115 D T Tran, M S Tran, and N V Hiep, "A Finite -Time Robust Control for a Manipulator with Output Constraints and Unknown Control Directions," in 2020 5th International Conference on Green Technology and Sustainable Development (GTSD), 27-28 Nov 2020 2020, pp 185-191, doi: 10.1109/GTSD50082.2020.9303058 H V Truong, D T Tran, X D To, K K Ahn, and M Jin, "Adaptive Fuzzy Backstepping Sliding Mode Control for a 3-DOF Hydraulic Manipulator with BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 106 TÀI LIỆU THAM KHẢO [16] [17] [18] [19] [20] [21] [22] [23] [24] [25] [26] [27] Nonlinear Disturbance Observer for Large Payload Variation," Applied Sciences, vol 9, no 16, 2019, doi: 10.3390/app9163290 D G Nguyen, D T Tran, and K K Ahn, "Disturbance Observer-Based Chattering-Attenuated Terminal Sliding Mode Control for Nonlinear Systems Subject to Matched and Mismatched Disturbances," Applied Sciences, vol 11, no 17, 2021, doi: 10.3390/app11178158 D.-T Tran, T.-C Do, and K.-K Ahn, "Extended High Gain Observer-Based Sliding Mode Control for an Electro-hydraulic System with a Variant Payload," International Journal of Precision Engineering and Manufacturing, vol 20, no 12, pp 2089-2100, 2019/12/01 2019, doi: 10.1007/s12541-019-00256-0 T X Dinh, T D Thien, T H V Anh, and K K Ahn, "Disturbance Observer Based Finite Time Trajectory Tracking Control for a DOF Hydraulic Manipulator Including Actuator Dynamics," IEEE Access, vol 6, pp 3679836809, 2018, doi: 10.1109/ACCESS.2018.2848240 J J Craig, Introduction to robotics : mechanics and control, 3rd ed Upper Saddle River, N.J.: Pearson/Prentice Hall, 2005, pp viii, 400 p H V Dao, D T Tran, and K K Ahn, "Active Fault Tolerant Control System Design for Hydraulic Manipulator With Internal Leakage Faults Based on Disturbance Observer and Online Adaptive Identification," IEEE Access, vol 9, pp 23850-23862, 2021, doi: 10.1109/ACCESS.2021.3053596 J Allan, S Lavoie, S Reiher, and G Lambert, "Kinematic and dynamic analysis of a novel 6-DOF serial manipulator for underground distribution power lines," in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, 25-30 Sept 2011 2011, pp 1849-1856, doi: 10.1109/IROS.2011.6094462 M Vaezi, F C Samavati, H E S Jazeh, and S A A Moosavian, "Singularity analysis of 6DOF Stäubli© TX40 robot," in 2011 IEEE International Conference on Mechatronics and Automation, 7-10 Aug 2011 2011, pp 446-451, doi: 10.1109/ICMA.2011.5985699 D Guo, Q Feng, and J Cai, "Acceleration-Level Obstacle Avoidance of Redundant Manipulators," IEEE Access, vol 7, pp 183040-183048, 2019, doi: 10.1109/ACCESS.2019.2960399 D M Hung, D T Linh, and D X Ba, "An Intelligent Control Method for Redundant Robotic Manipulators with Output Constraints," in 2021 International Conference on System Science and Engineering (ICSSE), 2021: IEEE, pp 116-121 J G Ziegler and N B Nichols, "Optimum settings for automatic controllers," trans ASME, vol 64, no 11, 1942 E Kreyszig, K Stroud, and G Stephenson, "Advanced engineering mathematics," Integration, vol 9, no 4, 2008 A Mueller, "Modern Robotics: Mechanics, Planning, and Control [Bookshelf]," IEEE Control Systems Magazine, vol 39, no 6, pp 100-102, 2019, doi: 10.1109/MCS.2019.2937265 BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 107 PHỤ LỤC PHỤ LỤC Phụ Lục A: Tính tốn vận tốc dài vận tốc góc ω1 = 01 R ω0 + 1 zˆ = 0 1  ω1 = 01 R ω1 = 0 1  T T υ1 = 01 R ( υ0 + ω × p1 ) =  0 υ1 = 01 R υ1 =  0 T 0 T ω = 21 R ω1 +  2 zˆ = 1 c2 −1 s   ω = 20 R ω =  − s1  c1 1  T υ2 = 21 R ( υ1 + ω1 × p ) = 0 a11  υ2 = 20 R υ2 =  −a11 s1 3 a11 c1 T T 0 T ω3 = 23 R ω + 3 zˆ = 1 c23 −1 s23  + 3  ( ω3 = 03 R ω3 =  − s1  + 3 ) ( c1  + 3 ) T 1  T υ3 = 23 R ( υ2 + ω × p3 ) =  a2 s3 a2 c3 1 ( a1 + a2 s2 )  T  a2 2c1c2 − a11s1 − a21s1s2    υ3 = 03 R υ3 =  a11c1 + a21c1s2 + a2 2c2 s1    −a2 s2   ( ) )  1c23c4 − s4  + 3    ω =  −c4  + 3 − 1c23 s4       − 1s23   (  4c1c23 − 3 s1 −  s1    ω =  2c1 + 3c1 +  4c23 s1    1 −  s23   ( ) ( ( )  − s4 1 ( a1 + a2 s2 ) + d 41c23 + a31s23 − c4 d  + 3 + a2 s3  υ4 =  s4 d  + 3 − a2 s3 − c4 1 ( a1 + a2 s2 ) + d 41c23 + a31s23  a3  + 3 + a2 2c3  ( ( ) ) ( ( BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN ) )  ) 108   PHỤ LỤC υ4 = ( 04 )  (  ) (  )  (  ) = c c ( a ( +  ) + a  c ) − ( c ( ( a + a s ) + d  c + a  s ) − s ( d ( +  ) − a  s ) ) ( c s − s c s ) − ( s ( ( a + a s ) + d  c + a  s ) + c ( d ( +  ) − a  s ) ) ( s s + s c c ) (  ) = ( s ( ( a + a s ) + d  c + a  s ) + c ( d ( +  ) − a  s ) ) ( c s − s c s ) + ( c ( ( a + a s ) + d  c + a  s ) − s ( d ( +  ) − a  s ) ) ( c c + s s s ) + c s ( a ( +  ) + a  c ) 0 T 4 23 23 2 4 1 2 2 23 23 23 23 4 4 2 3 2 23 4 4 23 () 2 23 2 23 23 3 23 4 2 23 4 2 23 2 = − d 4 c23 − d 43c23 − a3 s23 − a33 s23 − a2 s2 ( ( ( ) ) (  s5  − 1s23 − c5 s4  + 3 − 1c23c4  ω5 =  s5 s4  + 3 − 1c23c4 + c5  − 1s23  5 + c4  + 3 + 1c23 s4  ( ( ω5 = ( 05 )  )) ( ) (  ) (  )  0 ) )   T (  ) = − s −  c s −  s −  c +  c s s (  ) =  c + c + c c + s c + s s s (  ) = ( s s − c c c ) ( c ( s ( +  ) −  c c ) − s ( −  s ) ) − ( s c + c c s ) ( s ( s ( +  ) −  c c ) + c ( −  s )) + c s ( + c ( +  ) +  c s ) 5 4 23 23 2 4 23 23 23 5 23 ( ( 23 23 4 23 23 ) ( ( ) 23 23 ( ( )) ( ( 23 ) 23 ) ))  )  s a  +  + a  c − c s  (a + a s ) + d  c + a  s + c d  +  − a  s 2 1 2 23 23 4 2  3  υ5 =  s5 s4 1 ( a1 + a2 s2 ) + d 41c23 + a31 s23 + c4 d  + 3 − a2 s3 + c5 a3  + 3 + a2 c3   c4 1 ( a1 + a2 s2 ) + d 41c23 + a31s23 − s4 d  + 3 − a2 s3  ( ( ) ( ( ( υ5 = ( 05 )  () () () ) (  ) (  )  0 ) ( ( ) ) ) T = a2 c1c2 − a11s1 − a21s1s2 + a3 2c1c2c3 + a33c1c2c3 − d 41c2c3 s1 − d 4 2c1c2 s3 − d 43c1c2 s3 − d 43c1c3 s2 − a31c2 s1s3 − a31c3 s1s2 − a3 2c1s2 s3 − a33c1s2 s3 + d 41s1s2 s3 = a11c1 + a21c1s2 + a2 2c1s1 + d 41c1c2 c3 + a31c1c2 c3 + a31c1c3 s2 + a3 2c2c3 s1 + a33c2 c3 s1 − d 41c1s2 s3 − d 4 2c3 s1s2 − d 43c2 s1s3 − d 43c3 s1s2 − a3 s1s2 s3 − a33 s1s2 s3 = − d 4 c23 − d 43c23 − a3 s23 − a33 s23 − a2 s2 BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 109    PHỤ LỤC ( ( ) ( ( ( ) ) ( ) −s  + c  +  +  c s − c c s  +  −  c c − s  −  s 23 23 23   ω6 =  s6 c5 s4  + 3 − 1c23c4 − s5  − 1s23 − c6 5 + c4  + 3 + 1c23 s4    + s5 s4  + 3 − 1c23c4 + c5  − 1s23  ( ( ( ω6 = ( 06 )  () () () ( ( 6 )2 ( 6 )3  )) ( ( ) ) ( )  =  2c1 + 3c1 + 5c1c4 +  s1c23 +  6c1s4 s5 + 5 s1s4 s23 +  s1c23c5 − 6 s1s23c4 s5 = 1 −  s23 + 5c23 s4 −  s23c5 −  6c23c4 s5 υ6 = ( 66 )  (  ) (  )  6    T =  4c1c23 − 3 s1 − 5c4 s1 −  s1 + 5c1s4 s23 −  s1s4 s5 +  6c1c23c5 − 6c1s23c4 s5 )  ) 6 ( ) ( ) )) T ) ) ) ( ( ) ( ( )) − s ( c ( ( a + a s ) + d  c + a  s ) − s ( d ( +  ) − a  s ) ) (  ) = s ( c ( s ( ( a + a s ) ) + d  c + a  s ) + c ( d ( +  ) − a  s ) − s ( a ( +  ) + a  c ) ) −c ( c ( ( a + a s ) + d  c + a  s ) − s ( d ( +  ) − a  s ) ) (  ) = s ( s ( ( a + a s ) + d  c + a  s ) + c ( d ( +  ) − a  s ) ) + c ( a ( +  ) + a  c ) () 6 ( ( ( ) = −c6 c5 s4 1 ( a1 + a2 s2 ) + d 41c23 + a31 s23 + c4 d  + 3 − a2 s3 − s5 a3  +  + a2 c3 4 6 4 2 23 23 4 2 6 2 23 2 23 23 23 4 2 4 2 4 2 3 2 2 6 υ6 = ( 06 )  () () () 2 23 23 (  ) (  )  0 2 T ( ( ) + ) + d  c c = a21c1c2 − a2 s1s2 − a11s1 + a3c1c23  + 3 − d 41s1c23 − d 4 c1s23 − d 43c1s23 − a31s1s23 = a21s1c2 − a2 c1s2 − a11s1 + a3 s1c23 = − d 4 c23 − d 43c23 − a3 s23 − a33 s23 − a2 s2 0 υ7 = ( 77 )  () 7 ( ( ( 7 ) ( 7 )3  1 23 − d 4 s1s23 − d 43 s1s23 + a31c1s23 T  ( ( ( ) ) = −c6 c5 s4 1 ( a1 + a2 s2 ) + d 41c23 + a31 s23 + c4 d  +  − a2 s3 ( ( ( ( ) ) − s6 c4 1 ( a1 + a2 s2 ) + d 41c23 + a31 s23 − s4 d  + 3 − a2 s3 )) )) − s ( a ( ) +  + a2 c3 ) ))) ) ( ( ( ) ( ( ( ( ) = s ( c ( s ( ( a + a s ) + d  c + a  s ) + c ( d ( +  ) − a  s ) ) − s ( a ( +  ) + a  c ) ) −d c6 5 + c4  +  + 1c23 s4 − s6 c5 s4  +  − 1c23 c4 − s5  − 1 s23 () 7 1 2 23 ( ( 23 ) ( ( 2 ) −c6 c4 1 ( a1 + a2 s2 ) + d 41c23 + a31 s23 − s4 d  + 3 − a2 s3 ( ( ( ) ) 3 2 )) ))) ) ( ) + c ( d ( +  ) − a  s ) ) + c ( a ( +  ) + a  c ) ( ( ( ) −d s6 5 + c4  +  + 1c23 s4 + c6 c5 s4  +  − 1c23 c4 − s5  − 1 s23 () 7 )) ( ( = s5 s4 1 ( a1 + a2 s2 ) + d 41c23 + a31 s23 υ7 = ( 07 )  (  ) (  )  0 4 T BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 110 2 3 2 PHỤ LỤC () = −a11s1 − a21s1s2 + a2 2c1c2 − a31s1s23 + a3 2c1c23 + a33c1c23 − d 41s1c23 − d 4 2c1s23 − d 43c1s23 − d 71c1s4 s5 − d 7 4c4 s1s5 − d 75c5 s1s4 − d 71s1c5c23 − d 7 2c1c5 s23 − d 73c1c5 s23 − d 75c1s5c23 + d 71s1c4 s5 s23 () − d 7 2c1c4 s5c23 − d 73c1c4 s5c23 + d 7 4c1s4 s5 s23 − d 75c1c4c5 s23 = a11c1 + a21c1s2 + a2 s1c2 a31c1s23 + a3 s1c23 + a33 s1c23 + d 41c1c23 − d 4 s1s23 − d 43 s1s23 − d 71s1s4 s5 + d 7 4c4c1s5 + d 75c5c1s4 + d 71c1c5c23 − d 7 s1c5 s23 − d 73 s1c5 s23 − d 75 s1s5c23 − d 71c1c4 s5 s23 () − d 7 s1c4 s5c23 − d 73 s1c4 s5c23 + d 7 s1s4 s5 s23 − d 75 s1c4c5 s23 = −a2 s2 − a3 s23 − a33 s23 + d 4 2c23 − d 43c23 − d 7 2c5c23 − d 73c5c23 + d 75 s5 s23 − d 75c4c5c23 + d 7 2c4 s5 s23 + d 73c4 s5 s23 + d 7 s4 s5c23 Phụ Lục B: Tính tốn động lực học hệ thống https://drive.google.com/drive/folders/14LDG2GsjDFc2HkEq1dt0gwiw091e5L00?usp =sharing Phụ Lục C: Chương trình MATLAB đ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); BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 111 PHỤ LỤC 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]; %% 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: CTC Controller function [u] = CTC(the,dthe,the_r,dthe_r,ddthe_r,Kp,Kv,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); BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 112 PHỤ LỤC I1xy = I(1,4); I1yz = I2xx = I(2,1); I2yy = I2xy = I(2,4); I2yz = I3xx = I(3,1); I3yy = I3xy = I(3,4); I3yz = I4xx = I(4,1); I4yy = I4xy = I(4,4); I4yz = I5xx = I(5,1); I5yy = I5xy = I(5,4); I5yz = I6xx = I(6,1); I6yy = I6xy = I(6,4); I6yz = a1 = L(1); a2 = L(2); m1 = m(1); m2 = m(2); g = -9.80665; I(1,5); I1zx = I(1,6); I(2,2); I2zz = I(2,3); I(2,5); I2zx = I(2,6); I(3,2); I3zz = I(3,3); I(3,5); I3zx = I(3,6); I(4,2); I4zz = I(4,3); I(4,5); I4zx = I(4,6); I(5,2); I5zz = I(5,3); I(5,5); I5zx = I(5,6); I(6,2); I6zz = I(6,3); I(6,5); I6zx = I(6,6); a3 = L(3); d1 = L(4); d4 = L(5); d7 = L(6); m3 = m(3); m4 = m(4); m5 = m(5); m6 = m(6); 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]; %% Computed torque control: % Controler parameters Kp_ = diag(Kp); Kv_ = diag(Kv); % Designed controller E = the_r - the; dE = dthe_r - dthe; u = Kp_*E + Kv_*dE + M*ddthe_r + C*dthe_r + G; % 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); x6 = Pic(6,1); y6 = Pic(6,2); z6 = Pic(6,3); I1xx = I(1,1); I1yy = I(1,2); I1zz = I(1,3); BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 113 PHỤ LỤC I1xy = I(1,4); I1yz = I2xx = I(2,1); I2yy = I2xy = I(2,4); I2yz = I3xx = I(3,1); I3yy = I3xy = I(3,4); I3yz = I4xx = I(4,1); I4yy = I4xy = I(4,4); I4yz = I5xx = I(5,1); I5yy = I5xy = I(5,4); I5yz = I6xx = I(6,1); I6yy = I6xy = I(6,4); I6yz = a1 = L(1); a2 = L(2); m1 = m(1); m2 = m(2); g = -9.80665; I(1,5); I1zx = I(1,6); I(2,2); I2zz = I(2,3); I(2,5); I2zx = I(2,6); I(3,2); I3zz = I(3,3); I(3,5); I3zx = I(3,6); I(4,2); I4zz = I(4,3); I(4,5); I4zx = I(4,6); I(5,2); I5zz = I(5,3); I(5,5); I5zx = I(5,6); I(6,2); I6zz = I(6,3); I(6,5); I6zx = I(6,6); a3 = L(3); d1 = L(4); d4 = L(5); d7 = L(6); m3 = m(3); m4 = m(4); m5 = m(5); m6 = m(6); 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; 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); BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN 114 S K L 0