(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt

(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt(Đề tài NCKH) Nghiên cứu phát triển bộ điều khiển robot bốn chân có khả năng di chuyển linh hoạt

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 NGHIÊN CỨU PHÁT TRIỂN BỘ ĐIỀU KHIỂN ROBOT BỐN CHÂN CÓ KHẢ NĂNG DI CHUYỂN LINH HOẠT SV2021-35 Thuộc nhóm ngành khoa học: Nghiên cứu SV thực hiện: Bùi Mạnh Huy Nam, Nữ: Nam Dân tộc: Kinh Lớp, khoa: 171511A, Điện-Điện Tử Năm thứ: 4/ 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 Đặng Xuân Ba TP Hồ Chí Minh, 10/2021 MỤC LỤC MỞ ĐẦU Tổng quan Lý chọn đề tài Mục tiêu đề tài Phương pháp nghiên cứu Đối tượng Phạm vi nghiên cứu CHƯƠNG 1: CƠ SỞ LÝ THUYẾT 1.1 Một số phần mềm sử dụng hổ trợ thiết kế, gia công robot 1.1.1 Sơ lược Solidworks a Xuất vẽ 3D cắt CNC kim loại b Xuất file in 3D cho chi tiết c Xuất file 2D cho chi tiết để gia công tôn 1.1.2 Giới thiệu MATLAB 10 a Tổng quan cấu trúc liệu ứng dụng MATLAB 11 b Hệ thống Matlab 12 1.2 Thuật toán điều khiển 13 1.2.1 Sơ lược lí thuyết điều khiển tự động 13 a Các khái niệm 13 b Các phần tử hệ thống điều khiển tự động 14 1.2.2 Phân loại hệ thống điều khiển tự động 15 1.3 Lý thuyết điều khiển sử dụng 16 1.3.1 Giới thiệu điều khiển PID 16 CHƯƠNG 2: THIẾT KẾ VÀ THI CƠNG MƠ HÌNH 21 2.1 Lên ý tưởng cho mô hình robot động vật bốn chân 21 2.2 Chế tạo mơ hình robot động vật bốn chân 22 2.2.1 Khớp 23 2.2.2 Khớp 24 2.2.3 Khớp 25 2.2.4 Thông số kĩ thuật loại động 27 CHƯƠNG 3: KHẢO SÁT MƠ HÌNH TỐN VÀ TÍNH ĐỘNG HỌC CHO ROBOT 30 3.1 Bảng Denavit-Hartenberg (DH): 30 3.2 Động học vị trí cho chân robot 31 3.2.1 Động học thuận 31 3.2.2 Động học nghịch 33 3.2.3 Phương trình động lực học 35 CHƯƠNG 4: THIẾT KẾ BỘ ĐIỀU KHIỂN 36 4.1 Bộ điều khiển thông minh 36 CHƯƠNG 5: KẾT QUẢ VÀ ĐÁNH GIÁ 38 5.1 Kết đạt được: 38 5.2 Đánh giá 48 CHƯƠNG 6: KẾT LUẬN VÀ KIẾN NGHỊ 50 6.1 Kết luận 50 6.2 Kiến nghị 50 TÀI LIỆU THAM KHẢO 51 PHỤ LỤC 52 BÀI BÁO HỘI NGHỊ ICSSE 2021 54 DANH MỤC BẢNG BIỂU Bảng Độ ảnh hưởng hệ thống thông số PID: 19 Bảng Bảng DH cho chân robot: 30 Bảng Ưu nhược điểm điều khiển: 37 Bảng Sai số phương X, Y, Z điều khiển squad chân robot: 40 Bảng Sai số phương X, Y, Z điều khiển vẽ robot không theo phương x: 42 Bảng Sai số phương X, Y, Z điều khiển vẽ robot không theo phương y: 43 Bảng Sai số phương X, Y, Z điều khiển squad robot bốn chân: 45 Bảng Sai số phương X, Y, Z điều khiển chân robot di chuyển theo phương x:46 Bảng Sai số phương X, Y, Z điều khiển chân robot di chuyển theo phương y:48 Bảng 10 Các giá trị K1 , K , α, β cho trường hợp khớp 49 DANH MỤC HÌNH ẢNH Hình Cấp độ phần mềm Solidworks Hình Chi tiết thiết kế 3D Solidworks Hình Lưu file định dạng STEP Hình Thiết kế 3D chi tiết cần in Hình Lưu chi tiết với định dạng STL Hình Xác nhận lưu file Hình Thiết kế 3D chi tiết cần cắt laze Hình Chi tiết trải ngang Hình Chi tiết lưu định dạng DXF 10 Hình 10 Icon phần mềm mô Matlab/Simulink 11 Hình 11 Giao diện chức mơ Matlab/Simulink 11 Hình 12 Sơ đồ tồng quát hệ thống điều khiển tự động 14 Hình 13 Sơ đồ điều khiển PID 18 Hình 14 Giải thuật điều khiển robot 20 Hình 15 Mơ hình Robot Cheetal MIT 21 Hình 16 Mơ hình robot Spot hãng Boston Dynamics 21 Hình 17 Mơ hình thực nghiệm Robot động vật bốn chân nhóm 22 Hình 18 Bản vẽ khớp thứ Solidworks 23 Hình 19 Bản vẽ khớp thứ hai phần mềm Solidwords 24 Hình 20 Bản vẽ khớp thứ ba phần mềm Solidwords 25 Hình 21 Mơ hình chân robot thực tế 26 Hình 22 Động JGB-545 27 Hình 23 Động JGB-520 28 Hình 24 Động 5840-31ZY 29 Hình 27 Hệ trục tọa độ robot bậc tự 30 Hình 28 Cơng thức tính ma trận T 32 Hình 29 Giải thuật điều khiển robot 36 Hình 30 Dữ liệu góc khớp 38 Hình 31 Dữ liệu sai số góc khớp 39 Hình 32 Dữ liệu góc khớp 39 Hình 33 Dữ liệu sai số góc khớp 40 Hình 34 Dữ liệu góc khớp 41 Hình 35 Dữ liệu sai số góc khớp 41 Hình 36 Dữ liệu góc khớp 42 Hình 37 Dữ liệu sai số góc khớp 43 Hình 38 Dữ liệu góc khớp 44 Hình 39 Dữ liệu sai số góc khớp 44 Hình 40 Dữ liệu góc khớp 45 Hình 41 Dữ liệu sai số góc khớp 46 Hình 42 Dữ liệu góc khớp 47 Hình 43 Dữ liệu sai số góc khớp 48 DANH MỤC CHỮ VIẾT TẮT DH : Denavit-Hartenberg IIT: Illinois Institute of Technology MIT: Massachusetts Institute of Technology PID: Proportional Integral Derivative 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: Nghiên cứu phát triển điều khiển robot bốn chân có khả di chuyển linh hoạt - Chủ nhiệm đề tài: Bùi Mạnh Huy - Lớp: 171511 Mã số SV: 17151209 Khoa: Điện-Điện Tử - Thành viên đề tài: Stt Họ tên MSSV Lớp Khoa Phạm Tấn Phát 17151240 171511 Điện-Điện Tử Trần Thanh Hải 17151197 171511 Điện-Điện Tử Đào Hùng Vương 17151189 171511 Điện-Điện Tử Phạm Việt Hoàng 17151204 171512 Điện-Điện Tử - Người hướng dẫn: TS Đặng Xuân Ba Mục tiêu đề tài: Nghiên cứu phát triển robot bốn chân có khả di chuyển linh hoạt phục vụ cho ứng dụng tự hành địa hình phức tạp sử dụng điều khiển thơng minh Tính sáng tạo: Robot điều khiển điều khiển tối ưu nhóm tự nghiên cứu, với điều khiển giúp robot di chuyển linh hoạt đáp ứng nhiều yêu cầu thực tế Kết nghiên cứu: Áp dụng thành cơng điều khiển thơng minh lên mơ hình robot chân nhóm, áp dụng lên mơ hình robot bốn chân squad lên xuống Kết trình bày bên có đường dẫn video đính kèm Đóng góp mặt giáo dục đào tạo, kinh tế - xã hội, an ninh, quốc phòng khả áp dụng đề tài: Đề tài cung cấp giá trị mặt học thuật điều khiển thông minh cho học tập nghiên cứu Công bố khoa học SV từ kết nghiên cứu đề tài (ghi rõ tên tạp chí có) nhận xét, đánh giá sở áp dụng kết nghiên cứu (nếu có): Nhóm có đăng báo điều khiển hội nghị ICSSE 2021 Ngày 12 tháng 10 năm 2021 SV chịu trách nhiệm thực đề tài (kí, họ tên) Nhận xét người hướng dẫn đóng góp khoa học SV thực đề tài (phần người hướng dẫn ghi): Ngày 12 tháng 10 năm 2021 Người hướng dẫn (kí, họ tên) MỞ ĐẦU Tổng quan Ngày với phát triển nhanh chóng khoa học giới nói chung Việt Nam nói riêng thúc đẩy nghành công nghiệp robot phát triển lên tầm cao Nó dần sâu vào đời sống người, từ khoa học, y tế, trị nghiên cứu Do vậy, việc chế tạo robot với đầy đủ tính cần thiết để phục vụ đời sống người câu hỏi mà nhà khoa học cần giải đáp Những năm gần xu hướng tự động hóa ngày ưa chuộng, giúp giảm thiểu sức lao động người đồng thời nâng cao suất hiệu kinh tế Ngành robot ứng dụng cho mắt ngày nhiều robot bốn chân có khả làm việc mơi trường khó khăn, độc hại như: Robot Spot hãng Boston Dynamics, Cheetah-cub MIT, HyQReal Moog IIT-Viện Công nghệ Ý… minh chứng cho phát triển vượt bậc ngành công nghiệp Tuy nhiên Việt Nam, loại hình robot chưa phổ biến độ khó tính phức tạp khâu chế tạo điều khiển Đại học Lạc Hồng trường đại học nghiên cứu chế tạo thành công robot bốn chân Tuy nhiên nhiều hạn chế cần hoàn thiện tương lai Một điều khiển thông minh với cấu phần khí chắn hai yếu tố cần thiết để tạo nên robot hoàn hảo Nếu xem phần khí tay chân điều khiển ví não robot, nắm giữ toàn định việc điều khiển robot di chuyển Đầu tư cho điều khiển đầu tư cho phát triển mạnh mẽ robot sau Lý chọn đề tài Xuất phát từ nhu cầu thực tế nghiên cứu điều khiển robot, nhóm nhận thấy, điều khiển PID đáp ứng vài yêu cầu điều khiển tốc độ xử lí chưa nhanh chưa giảm thiểu tối đa sai số điều khiển Phạm vi tần số hoạt động chưa rộng nên nhóm định chọn đề tài này, để tìm điều khiển có khả thích nghi tốt hơn, đáp ứng yêu cầu đặt cho điều khiển Hiện nhóm thực đồ án đề tài robot bốn chân nên hội để nhóm kiểm nghiệm mơ hình nhóm Nếu thành công tiến hành áp dụng rộng rãi cho nghiên cứu sau ngày nâng cấp hoàn thiện tương lai Mục tiêu đề tài Đề tài tập trung vào điều khiển thông minh ứng dụng cho robot bốn chân, dựa điều khiển PID truyền thống Nhóm nâng cấp tinh chỉnh để điều khiển hoạt động cách hiệu việc điều khiển robot Song song với việc nghiên cứu điều khiển nhóm cịn bắt tay vào chế tạo phần cứng cho robot để thử nghiệm điều khiển Do tình hình dịch bệnh diễn biến phức tạp, nhóm khơng thể tập trung lên phịng thí nghiệm để thực áp dụng điều khiển trực tiếp mơ hình, thay vào nhóm tiến hành mơ phần cứng trình điều khiển Phương pháp nghiên cứu Nghiên cứu dựa sở lý thuyết điều khiển PID, sau xây dựng nên điều khiển Các khâu tích phân, vi phân, tỉ lệ thay phương trình tốn với hệ số học tự động giúp tối ưu hóa điều khiển Đối tượng Đối tượng nhóm nghiên cứu lý thuyết điều khiển nhằm nâng cao chất lượng hệ thống áp dụng điều khiển lên mơ hình robot bốn chân Nhóm chọn nghiên cứu điều khiển nhận thấy vai trị to lớn tầm quan trọng Bảng Sai số phương X, Y, Z điều khiển squad robot bốn chân: Sai số xác lập Đơn vị (cm) Phương X [-0.9;1] Phương Y Phương Z [-0.4; 1.1] Áp dụng điều khiển thông minh điều khiển chân robot di chuyển theo phương x Dữ liệu góc theta: Hình 38 Dữ liệu góc khớp 45 Sai số giữ tín hiệu đặt tín hiệu hồi tiếp: Hình 39 Dữ liệu sai số góc khớp Bảng Sai số phương X, Y, Z điều khiển chân robot di chuyển theo phương x: Sai số xác lập Đơn vị (cm) Phương X [-2;0.5] Phương Y Phương Z [-1.4; 1.8] 46 Áp dụng điều khiển thông minh điều khiển chân robot di chuyển theo phương y Dữ liệu góc theta: Hình 40 Dữ liệu góc khớp Sai số giữ tín hiệu đặt tín hiệu hồi tiếp: 47 Hình 41 Dữ liệu sai số góc khớp Bảng Sai số phương X, Y, Z điều khiển chân robot di chuyển theo phương y: Sai số xác lập Đơn vị (cm) Phương X [-1.5;0.3] Phương Y [-1.1;1.6] Phương Z [-0.1; 1.1] Link video mô robot: https://youtu.be/5X5dYFGyEgQ 5.2 Đánh giá Nhìn chung điều khiển hoạt động tốt, giá trị sai số mức nhỏ Do thực mô nên chưa đánh giá hết khả điều khiển, nhiên tảng sau hết dịch nhóm áp dụng lên mơ hình thực tế để tiếp tục điều khiển 48 Bảng 10 Các giá trị K1 , K , α, β cho trường hợp khớp Khớp Khớp Khớp   100 2.56 10.98 0 100 0.639 6.216 100 2.56 10.98 0.7 100 19.945 1.5 100 0.639 6.216 100 2.56 10.98 0.7 100 1.8 19.945 1.5 100 0.639 6.216 100 2.56 10.98 0.7 100 1.8 19.945 100 0.639 6.216 100 2.56 10.98 0.7 100 1.8 19.945 1.5 100 0.639 6.216 100 2.56 10.98 0.7 100 1.8 19.945 1.5 100 0.639 6.216 100 2.56 10.98 0.7 100 K1 K2   K1 1.8 19.945 0 0.639 6.216 1.8 19.945 1.8 K2   K1 K2 Thả rơi tự Squat chân Vẽ elip phương x Vẽ elip phương y Squad bốn chân Robot di chuyển chân phương x Robot di chuyển chân phương y Các thông số K1 , K ,  ,  chọn cách thử sai, ban đầu cho tất trước sau tăng dần lên đến thấy ổn định dừng lại chọn qua thơng số khác Nhóm lấy làm tảng tiếp tục phát triển lên áp dụng lên nhiều mơ hình robot khác sau 49 CHƯƠNG 6: KẾT LUẬN VÀ KIẾN NGHỊ 6.1 Kết luận Với kết trên, nhận thấy rằng, điều khiển hoạt động tương đối tốt, việc chọn hệ số K1 , K ,  ,  tốt kết điều khiển tuyệt vời Phương pháp điều khiển trực tiếp không thông qua khối động học nghịch điều khiển thông minh đem lại kết đáng khả quang, chứng thông qua kết mô video đính kèm Bên cạnh mặt đạt nghiên cứu khoa học lần hạn chế như: chưa thực tối ưu việc chọn trọng số K1 , K ,  ,  Nên phần chưa tối ưu hóa hết khả điều khiển 6.2 Kiến nghị Trong tương lai điều khiển thơng minh áp dụng cho robot động vật để tiết kiệm thời gian khâu giải động học nghịch nâng cao khả di chuyển robot môi trường khác 50 TÀI LIỆU THAM KHẢO [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] [24] [25] [26] [27] Giáo trình lý thuyết điều khiển tự động, TS Huỳnh Thái Hoàng P E Hudson, S A Corr and A M Wilson, "High speed galloping in the cheetah (acinonyx jubatus) and the racing greyhound(canis familiaris): spatio-temporal and kinetic characteristics," the Journal of Experimental Biology, vol 215, no 14, pp 2425–2434, 2012 H Yeom, D X Ba, and J B Bae, “Design Principles and Validation of a Human-sized Quadruped Robot Leg for High Energy Efficiency,” J K Robotics Society, vol.13, no.2, pp.86-91, 2018 D X Ba, H Yeom, and J B Bae, “A Direct Robust Nonsingular Terminal Sliding Mode Controller based on an Adaptive Time-delay Estimator for Servomotor Rigid Robots,” Mechatronics, May 2019 K Sreenath, H.-W Park, I Poulakakis, and J W Grizzle A compliant hybrid zero dynamics controller for stable, efficient and fast bipedal walking on MABEL The International Journal of Robotics Research, 30(9):1170–1193, August 2011 K Sreenath, H.-W Park, I Poulakakis, and J W Grizzle Embedding active force control within the compliant hybrid zero dynamics to achieve stable, fast running on MABEL The International Journal of Robotics Research, 32(3):324–345, March 2013 S Ruthishauser, Cheetah – compliant quadruped robot, Biologically Inspired Robotics Group, EPFL, 2008 M Hutter, C Gehring, M Bloesch, and all, StarlETH: A compliant quadrupedal robot for fast, efficient, and versatile locomotion, 15th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines, 2012 Rémy Siegfried ,Effect of Leg Design on Locomotion Stability for Quadruped Robot, WS 2014-2015, STI-SMT, Semester Project, 05.06.2015 Static Balancing of Wheeled-legged Hexapod Robots,CICATA Instituto Politecnico Nacional – Unidad Queretaro 76090, Mexico, IRCCS Neuromed, 86077 Pozzilli, Italy DIMEG, University of Calabria, 87036 Cosenza, Italy 7, April,2020 K Hashimoto, at el., “Realization by biped leg-wheeled robot of biped walking and wheel-driven locomotion,” in Proceedings of the 2005 IEEE International Conference on Robotics and Automation, Barcelona, Spain, 2005 G Bledt, M J Powell, B Katz, F D Carlo, P W Wensing, and S Kim, “MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot,” 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 2018 P M Wensing, A Wang, S Seok, A Otten, J Lang, and S Kim, “Proprioceptive Actuator Design in the MIT Cheetah: Impact Mitigation and High-Bandwidth Physical Interaction for Dynamic Legged Robots,” IEEE Transactions on Robotics, vol 33, no 3, pp 509-522, 2017 H W Park, S Park, and S Kim, “Variable-speed quadrupedal bounding using impulse planning: Untethered high-speed 3D Running of MIT Cheetah 2,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), Seattle, USA, 2015 K Astrom and K Hagglund, PID Controllers: theory, design and tuning USA: ISA Press, 1995 D X Ba and J B Bae, “A Nonlinear Sliding Mode Controller of Serial Robot Manipulators With Two-Level Gain-Learning Ability,” IEEE Access, vol 8, pp 189224 - 189235, 2020 G Z Tan, Q D Zeng, and W B Li, “Intelligent PID controller based on ant system algorithm and fuzzy inference and its application to bionic artificial leg, ” Journal of Central South University of Technology, vol 11, pp 316-322, 2004 C F Juang and Y C Chang, “Evolutionary-Group-Based Particle-Swarm-Optimized Fuzzy Controller With Application to Mobile-Robot Navigation in Unknown Environments, ” IEEE Trans Fuzzy Systems, vol 19, no 02, pp 379-392, 2011 M Cucientes, D L Moreno, A Bugarin, and S Barro, “Design of a fuzzy controller in mobile roboics using genetic algorithm,” Applied Soft Computing, vol 7, no 2, pp 540-546, 2007 D H Kim and J H Cho, “A Biological Inspired Intelligent PID Controller Tuning for AVR Systems,” International Journal of Control, Automation, and Systems, vol 4, no 5, pp 624 – 636, 2006 M J Neath, A K Swain, U K Madawala, and D J Thrimawithana, “An Optimal PID Controller for a Bidirectional Inductive Power Transfer System Using Multiobjective Genetic Algorithm, ” IEEE Trans Power Electronics, vol 19, no 3, pp 15231531, 2014 T D C Thanh and K K Ahn, “Nonlinear PID control to improve the control performance of axes pneumatic artificial muscle manipulator using neural network,” Mechatronics, 2006 J Ye, “Adaptive control of nonlinear PID-based analog neural networks for a nonholonomic mobile robot,” Neurocomputing, 2008 P Rocco, “Stability of PID control for industrial robot arms,” IEEE Trans Robot Automation, vol 12, no 4, pp 606-614, 1996 D X Ba and J B Bae, “A precise neural-disturbance learning control of constrained robotic manipulators,” IEEE Access, vol 9, pp 50381-50390, 2021 D X Ba, M.S Tran, V P vu, V D Tran, M D Tran, N T Tai, and C D Truong, “A neural-network-based nonlinear controller for robot manipulators with gain-learning ability and output constraints,” In 2021 International Symp Electrical and Electronics Engineering (ISEE), pp 149-153, 2021 J J Craig, “Manipulator dynamics” in Introduction to Robotics: Mechanics and Control, 3rd ed, Pearson Prentice Hall, USA, 2005, pp 165-200 51 PHỤ LỤC l1 = 0.07; l3 = 0.20; l4 = 0.163; th1=q.signals(1).values; th2=q.signals(2).values; th3=q.signals(3).values; Z_fall=Y_fall.signals.values; X_dir = X_direct.signals.values; Y_dir = Y_direct.signals.values; for j = 1:1:length(th1) the1_s=th1(j); the2_s=th2(j); the3_s=th3(j); T_0_1 = [cosd(the1_s), -sind(the1_s), sind(the1_s), cosd(the1_s), , , , , 0, 0, 1, 0, 0; 0; 0; 1]; T_1_2 = [cosd(the2_s), , -sind(the2_s), , 0, 1, 0, 0, l1; 0; 0; 1]; 0, 0, 1, 0, l3; 0; 0; 1]; -sind(the2_s), , -cosd(the2_s), , T_2_3 = [cosd(the3_s), -sind(the3_s), sind(the3_s), cosd(the3_s), , , , , EE1 = T_0_1*T_1_2*T_2_3*[l4;0;0;1]; T_G_O = [0, 0,-1, 0,-1, 0, -1, 0, 0, 0, 0, 0, M H U Y = = = = x_dir(j); Y_dir(j); Z_fall(j); 1]; T_G_O*[0.05;0;-0.10;1]; T_G_O*[0.05;0;0.10;1]; T_G_O*[-0.05;0;0.10;1]; T_G_O*[-0.05;0;-0.10;1]; O = T_G_O*[0;0;0;1]; Ox = T_G_O*[0.10;0;0;1]; Oy = T_G_O*[0;0.10;0;1]; 52 Oz = T_G_O*[0;0;0.10;1]; D = T_G_O*T_0_1*T_1_2; D = D(:,4); F = T_G_O*T_0_1*T_1_2*T_2_3; F = F(:,4); EE = T_G_O*T_0_1*T_1_2*T_2_3*[l4;0;0;1]; plot3( [O(1);Ox(1)], [O(2);Ox(2)], [O(3);Ox(3)],'->r', [O(1);Oy(1)], [O(2);Oy(2)], [O(3);Oy(3)],'-