Đang tải... (xem toàn văn)
Mô tả tổng quan về robot 6 bậc tự do. Mô hình hoá Robot theo phương pháp Euler Lagrange. Thực hiện mô hình hoá Robot IRB 2400 trên nền MatlabSimulink và công cụ Simmechanics. Thiết kế điều khiển động lực học Robot 6 bậc tự do sử dụng bộ điều khiển PD phi tuyến
B GIO DC V O TO Trường đại học Bách Khoa hµ néi **** LUẬN VĂN THẠC SỸ KHOA HỌC ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT BẬC TỰ DO NGÀNH: ĐIỀU KHIỂN TỰ ĐỘNG MÃ SỐ: LÊ VĂN CHINH Người hướng dẫn khoa học: GS.TSKH NGUYỄN PHÙNG QUANG HÀ NỘI 2008 Tr˱ͥng ÐHBK +jN͡i Lu̵n vãn cao K͕c LỜỎ ỌẢM ÕN Em xin chân thành cảm õn PGS TSKH Nguyễn Phùng Quang, ngýời thҫy ðã tận tình hýớng dẫn em thӵc hiӋn ðề tài này; ðã ðӏnh hѭӟng, FKӍEҧo JL~p ðӥem ÿӇKRjn WKjnh Eҧn luұn vãn Qjy Yjtạo cõ hội cho em ðýợc tìm hiểu robot IRB2400; hỗ trợ tài liệu tham khảo hữu ích, nghiêm khắc nhýng công giúp ðỡ em khắc phục mặt thiếu sót hạn chế luұn án Em xin chân thành cảm õn Fic anh, FKӏ cán Trung tâm Công nghệ cao ðã Wҥo ðiӅu kiӋn JL~p em Wum hiӇu thông số robot cần thiết cho luұn án FNJng nhý JL~p ðӥtrong suӕt TXiWUunh thӵc hiӋn luұn in Wҥi Trung Tâm Em xin gửi lời cảm õn sâu sắc tới thҫy cô Bộ môn Ðiều khiển tự ðộng, khoa Ðiện, Trýờng Ðại học Bách Khoa Hà Nội ðã quan tâm dìu dắt chúng em suӕt TXiWUunh Kӑc tұp Wҥi trѭӡng +jn͡i, ngày 14 tháng nãm 2008 +ӑc viên Lê Vãn Chinh Tr˱ͥng ÐHBK +jN͡i Lu̵n vãn cao K͕c MỤỌ ỆỤỌ Trang Ký hiệu chữ viết tắt Danh Pөc Fic Eҧng YjKunh YӁ MӢĈҪU CHÝÕNG TӘNG QUAN 1.1 Giӟi thiӋu chung 1.2 Sõ lѭӧc vӅRobot bұc tӵdo IRB2400 11 1.2.1 C̭u WU~c Fͯa Robot IRB2400 1.2.2 &ic thông s͙ð̿c Wtnh NͿthu̵t cõ E̫n 1.2.3 7tnh nãng KR̩t ð͡ng 1.3 0өc ðtch YjSKҥm vi Fӫa luұn in CHÝÕNG 2: MƠ HÌNH HỐ ROBOT BẬC TỰ DO THEO PHÝÕNG PHÁP EULER – LAGRANGE 2.1 Mô Kunh Kya theo phýõng SKip Euler-Largrange 2.1.1 &ic SKpp biӃn ðәi thuҫn nhҩt 2.1.1.1 H͏W͕a ð͡thu̯n nh̭t 2.1.1.2 &ic SKpp bi͇n ð͝i thu̯n nh̭t 2.2 Mô Kunh ðӝng Kӑc robot 2.2.1 Bộ thơng số Denavit – Hartenberg 2.2.2 Trình tự thiết lập mơ hình ðộng học Denavit – Hartenberg 2.2.3 Ðộng học ngýợc Robot 2.3 Mơ hình ðӝng lӵc Kӑc robot 2.3.1 Cõ học Lagrange 2.3.2 Phýõng trình ðộng lực học 2.3.2.1 Vận tốc Yjgia t͙c ðiểm robot hệ tọa ðộ cõ 2.3.2.2 Ðộng nãng tay máy 2.3.2.3 Thế nãng tay máy 2.3.2.4 Mô hình ðộng lực học robot 2.3.2.5 Các ðặc ðiểm mơ hình ðộng lực học Robot 2.4 Mơ hình hố Robot bұc tӵdo IRB2400 2.4.1 Xác ðịnh thông số Robot IRB 2400 2.4.1.1 Cấu trúc hình học ðõn giản hóa khâu 12 13 14 15 17 17 17 17 18 19 19 21 22 23 23 24 24 26 27 27 29 30 30 30 Tr˱ͥng ÐHBK +jN͡i Lu̵n vãn cao K͕c Trang 2.4.1.2 Khối lýợng khâu 2.4.1.3 Ma trận quán tính vị trí trọng tâm khâu 2.4.2 Mơ hình ðộng học (Kinematic) 2.4.2.1 Giải tốn ðộng học thuận (forward kinematics) 2.4.2.2 Giải toán ðộng học ngýợc (inverse kinematics) 2.4.3 Mơ hình ðộng lực học (Dynamic) 2.4.3.1 Các ma trận Uij 2.4.3.2 Các ma trận Uijk 2.4.3.3 Ma trận quan hệ D 2.4.3.4 Vector lực ly tâm lực Coriolis 2.4.3.5 Vector lực trọng trýờng c CHÝÕNG 3: ÐIỀU KHIỂN ÐỘNG LỰC HỌC NGÝỢC ROBOT SӰ'ӨNG BӜ ÐIӄU KHIӆN PID PHI TUYӂN 3.1 Tổng quan ðiều khiển chuyӇn ðӝng Robot 3.2 Mӝt sӕphýõng pháp ðiều khiển ðiӇn Kunh: 3.2.1 Phýõng pháp ðiều khiển ðộc lập có phản hồi 3.2.2 Phýõng pháp ðiều khiển phản hồi phân ly phi tuyến 3.2.3 Phýõng pháp ðiều khiển thích nghi theo mơ hình mẫu 3.2.4 Phýõng pháp ðiều khiển thích nghi theo sai lệch 3.2.5 Phýõng pháp ðiều khiển trýợt 3.3 Phýõng pháp ðiều khiển ðộng lực học ngýợc 3.3.1 Nội dung phýõng pháp 3.3.2 Thiết kế vịng ngồi với ðiều khiển PD tuyến tính 3.3.3 Thiết kế vịng ngồi với ðiều khiển PID tuyến tính 3.2.4 Một số vấn ðề khó khãn biện phác khắc phục thực tế việc ðiều khiển robot theo phýõng pháp ðiều khiển ðộng lực học ngýợc 3.4 Bộ ðiӅu khiӇn PID phi tuyến 3.4.1 Giới thiệu chung 3.4.2 Các loại PID phi tuyến (NPID Type) 3.4.3 Các phýõng pháp tổng hợp PID phi tuyến 3.4.4 Một số ðặc tính khuyếch ðại phi tuyến ðiển hình 3.4.5 Xét tính ổn ðịnh với ðiều khiển PD phi tuyến 3.5 Thiết kế thuật toán ðiều khiển ðộng lực học ngýợc robot với ðiӅu khiӇn 33 34 41 42 43 51 52 52 54 55 60 61 61 62 62 64 65 65 66 68 68 71 72 74 74 74 75 75 76 78 79 Tr˱ͥng ÐHBK +jN͡i Lu̵n vãn cao K͕c Trang PD phi tuyến 3.5.1 Ðiều khiển Robot IRB 2400 theo phýõng pháp ðộng lực học ngýợc sử dụng 79 PD tuyến tính 3.5.2 Ðiều khiển Robot IRB 2400 theo phýõng pháp ðộng lực học ngýợc sử dụng 81 PD phi tuyến 3.6 Kết luận CHÝÕNG 4: THIẾT KẾ QUỸ ÐẠO CHUYỂN ÐỘNG ROBOT 4.1 Giới thiệu chung 4.2 Tham số hố phýõng trình chuyển ðộng 4.2.1 Quỹ ðạo ða thức bậc 4.2.2 Quỹ ðạo ða thức bậc cao 4.3 Thiết kế quỹ ðạo chuyển ðộng chuẩn cho Robot IRB2400 CHÝÕNG 5: MÔ HÌNH ÐIỀU KHIỂN ROBOT TRÊN NӄN MATLAB& SIMULINK VÀ MƠ PHỎNG ÐỒ HOẠ VIRTUAL REALITY 5.1 Mơ hình Robot SimMechanic Simulink 82 84 84 87 87 88 89 94 94 5.1.1 Giới thiệu sõ lѭӧc phần mềm Matlab/Simulink, Simmechanics Yjcông Fөmô SKӓng thӵc Wҥi ҧo Virtual Reality Toolbox 94 5.1.2 Giӟi thiӋu vӅSimmechanics Toolbox 94 5.2 Mô SKӓng hӋthӕng cõ ðӝng Kӑc Robot IRB 2400 bҵng Simmechanics Toolbox 98 5.3 Mô Kunh bӝðiӅu khiӇn 99 5.4 Mô ðồ Kӑa môi trѭӡng thӵcWҥi ҧo Robot IRB 2400 bҵng Virtual Reality Toolbox: 99 5.4.1 Giới thiệu chung Virtual Reality (VR) Toolbox 99 5.4.2 Xây dựng mô ðồ họa cho robot IRB2400 5.5 Mô Kunh ghép nối hệ thống ðiều khiển với hệ thống cõ ðộng học 5.6 KӃt TXҧmô SKӓng KẾT LUẬN TÀI LIỆU THAM KHẢO 102 104 105 111 113 PHỤ LỤC: 3KөOөc A: &ic mô Kunh hӋthӕng ðiӅu khiӇn Robot IRB2400Yjthông sӕmô Kunh 3KөOөc B: &ic chýõng WUunh SKөc Yөmô SKӓng Robot IRB2400 Tr˱ͥng ÐHBK +jN͡i Lu̵n vãn cao K͕c Ký hiệu chữ viết tắt Ký hiệu Di͍n JL̫i Ma trận tịnh tiến Tp R(x,á), R(y,ö), R(z,è) Các ma trận quay quanh trục cõ góc týõng ứng Ma trận quan hệ khâu thứ i so với hệ tọa ðộ cố ðịnh Ti Ðộ dài ðýờng vng góc hai trục khớp ðộng i+1 i Góc chéo hai trục khớp ðộng i+1 i Khoảng cách ðo dọc trục khớp ðộng i từ ðýờng vuông góc chung di trục khớp ðộng i+1 trục khớp ðộng i tới ðýờng vng góc chung khớp ðộng i trục khớp ðộng i-1 èi Góc hai ðýờng vng góc chung trục khớp ðộng i i+1 Ma trận quán tính khâu thứ i so với hệ tọa ðộ thứ i Ji Ma trận quán tính khâu thứ i so với trọng tâm khâu thứ i JCi Vector tọa ðộ ðiểm cho hệ tọa ðộ thứ j ðýợc biểu thị i rj hệ tọa ðộ thứ i Vector tọa ðộ ðiểm cho hệ tọa ðộ thứ i ðýợc biểu thị ri hay 0ri hệ tọa ðộ cố ðịnh Vi hay 0Vi Vector vận tốc ðiểm cho hệ tọa ðộ thứ i ðýợc biểu thị hệ tọa ðộ cố ðịnh Vector gia tốc ðiểm cho hệ tọa ðộ thứ i ðýợc biểu thị hay 0ai hệ tọa ðộ cố ðịnh Ma trận mơ tả vị trí hýớng khâu thứ j so với khâu thứ i i Aj Ai hay Ti i-1 Ai Ma trận mơ tả vị trí hýớng khâu thứ i so với khâu thứ i-1 Ma trận quan hệ mô tả vị trí hýớng khâu thứ i so với hệ tọa ðộ cố ðịnh L K P FMi Hàm Lagarange Ðộng nãng cõ hệ Thế nãng cõ hệ Ðộng lực (mơmen lực) hình thành khớp ðộng i Tr˱ͥng ÐHBK +jN͡i qi qi Lu̵n vãn cao K͕c Biến khớp (góc, khoảng cách) Ðạo hàm bậc biến khớp theo thời gian (vận tốc góc, vận tốc thẳng) Vi, Vector vận tốc, gia tốc khâu i so với hệ tọa ðộ cố ðịnh không gian tọa ðộ Uij Uijk g D h c TE IC Ðạo hàm riêng bậc Ai theo qj Ðạo hàm riêng bậc hai Ai theo qj qk Vector gia tốc trọng trýờng Ma trận quan hệ (ma trận quán tính mơmen qn tính) Vector lực ly tâm lực Coriolit Vector lực trọng trýờng Ma trận tọa ðộ ðiểm tác ðộng cuối Tensor quán tính (so với trọng tâm) Chữ viết tắt DOF Degree Of Free RPY Roll – Pitch – Yaw DH Denavit – Hartenberg EE End-Effector VR Virtual Reality VRML Virtual Reality Modeling Language MIMO Multi Input Multi Output SISO Single Input Single Output PID Proportional Integral Derivative PD Proportional Derivative NPD Nonlinear Proportional Derivative NDT Nonlinear Decoupling Theory OCS Operational Space Control GCT Geometric Control Theory LEL Locally Exactly externally Linearized and decoupled ABB Asea Brown Boveri (tập ðồn ða quốc gia hoạt ðộng lĩnh vực cơng nghệ nãng lýợng tự ðộng hóa) Tr˱ͥng ÐHBK +jN͡i Lu̵n vãn cao K͕c Danh Pөc Fic Eҧng Stt N͡i dung Trang %̫ng 1.1-%̫ng thông s͙NͿthu̵t Fͯa Robot IRB 2400 14 %̫ng 2.1-%̫ng thông s͙DH Fͯa Robot IRB 2400 34 B̫ng 2.2-&ic ma tr̵n Uij 52 %̫ng 2.3: &ic ma tr̵n Uijk 53 %̫ng 5-1: &ic chýõng WUunh/mô Kunh SKͭc Yͭthc hi͏n vi͏c mô SK͗ng 105 Danh Pөc Fic Eҧn YӁ, ðӗWKӏ Stt N͡i dung Trang +unh 1.1: Sõ ð͛c̭u W̩o chung Fͯa Robot IRB2400 12 10 11 12 13 14 15 16 17 18 19 +unh 1.2: Sõ ð͛kh͙i Fͯa mô Kunh Robot IRB2400 Hình 2.1 – Các thơng số khâu : è, d, a Hình 2.2.Khảo sát tốc ðộ vi khối lýợng dm Hình 2.3 .tch th˱ͣc Fͯa Robot IRB2400 Hình 2.4 Hình dạng ðõn giản hóa ðế Hình 2.5 Hình dạng ðõn giản hóa khâu thứ Hình 2.6 Hình dạng ðõn giản hóa khâu thứ hai Hình 2.7 Hình dạng ðõn giản hóa khâu thứ ba Hình 2.8 Hình dạng ðõn giản hóa khâu thứ tý Hình 2.9 Hình dạng ðõn giản hóa khâu thứ nãm Hình 2.10 Hình dạng ðõn giản hóa khâu thứ sáu Hình 2.11 Gắn hệ trục tọa ðộ theo mơ hình DH Hình 2.12 Minh họa tay máy khớp quay tách biệt Hình 2.13 Trýờng hợp ðặc biệt thứ hai (giải ðộng học ngýợc) Hình 3.1 : Sõ ðồ cấu trúc hệ thống ðiều khiển ðộc lập có phản hồi Hình 3-2: Sõ ðồ cấu trúc ðiều khiển thích nghi theo mơ hình mẫu Hình 3-2: Sõ ðồ cấu trúc ðiều khiển thích nghi theo sai l͏ch Hình 3-4: Sõ ðồ nguyên lý ðiều khiển trýợt 13 20 24 30 31 31 31 32 32 33 33 34 45 49 63 65 66 67 Tr˱ͥng ÐHBK +jN͡i Stt 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 Lu̵n vãn cao K͕c N͡i dung Hình 3-5 Sõ ðồ ðiều khiển ðộng lực học ngýợc mô tả theo vịng vịng ngồi Hình 3-6 Sõ ðồ cấu trúc ðiều khiển ðộng lực học ngýợc Hình 3.7 Sõ ðồ cấu trúc ðiều khiển ðộng lực học ngýợc với ðiều khiển PID Hình 3.8 Sõ ðồ Cấu trúc ðiều khiển PD tuyến tính Hình 3.9 Cấu trúc hệ thống lắp thêm ðiều khiển PD Hình 3.10 Sõ ðồ hệ thống sử dụng ðiều khiển PID phi tuyến +unh 4-1: '̩ng TXͿð̩o chuy͋n ð͡ng chu̱n Hình 4.2 Sõ ðồ khối TXiWUunh tạo giá trị ðặt góc Hình 5.1 Các thý viện khối SimMechanics toolbox Hình 5.2 Thý viện khối Bodies Hình 5.3 Thý viện khối Joint Hình 5.4 Thý viện khối Sensors & Actuators Hình 5.5 Thý viện khối Utilities Hình 5.6 Khối Joint Stiction Actuator Hình 5.7 Mơ hình SimMechanics robot Trang 70 71 73 79 80 81 90 93 95 96 96 97 97 97 98 35 Hình 5.8 Mơ hình b͡ði͉u khi͋n PD phi tuy͇n 99 36 Hình 5.9 Cấu hình khối VR Sink 102 37 38 39 40 41 42 43 Hình 5.10 Mơ Kunh thc W̩i ̫o Robot IRB2400 Hình 5.11 Sõ ð͛mơ hình h͏th͙ng ði͉u khi͋n Hình 5.12 4XͿð̩o chuy͋n ð͡ng chu̱n +unh 5.13: K͇t TX̫mô SK͗ng vͣi W̫i m=0 +unh 5.14: K͇t TX̫mô SK͗ng vͣi W̫i ð͓nh mͱc m=7kg +unh 5.15: K͇t TX̫mô SK͗ng Fy[pt tͣi ma Vit W̩i khͣp +unh 5.16: K͇t TX̫mô SK͗ng Jyc khͣp bi͇n ð͝i nhanh Kunh sin 103 104 105 106 107 107 108 Tr˱ͥng ÐHBK +jN͡i Lu̵n vãn cao K͕c CHÝÕNG I TӘNG QUAN 1.1 GIӞI THIӊU CHUNG &ng vӟi sӵSKit triӇn không ngӯng Fӫa khoa Kӑc YjNӻthuұt, Fic Piy Pyc YjthiӃt Eӏtrong công nghiӋp QJjy Fjng SKҧi ðҥt nãng suҩt cao, Ojm viӋc FKtnh [ic YjhiӋu TXҧ cao Mһt NKic nhu cҫu tӵðӝng Kya cho Fic TXiWUunh rӡi Uҥc QJjy Fjng cao thay thӃ cho sӭc lӵc Fӫa ngѭӡi ðһc biӋt Oj nhӳng công viӋc nãng QKӑc, môi trѭӡng Ojm viӋc nguy hiӇm, FyWtnh lһp Oҥi, vv…&ic TXiWUunh Qjy FythӇkӇÿӃn nhý: TXiWUunh Qҥp phôi liӋu, lҳp Uip Eҧng Pҥch, sõn, Kjn, … VjRobot ðӡi nhҵm WKӓa Pmn Fic yêu cҫu Qjy HiӋn Robot ðang ðѭӧc nghiên cӭu YjSKit triӇn không ngӯng Fҧi tiӃn ÿӇðip ӭng ðѭӧc yêu cҫu Vҧn xuҩt tӵ ðӝng Kya cao thӃ giӟi vӟi Pөc ðtch Oj tãng ðӝ thông minh, linh KRҥt, NKҧnãng WKtch nghi vӟi môi trѭӡng, ðӝFKtnh [ic cao, Wic ðӝng nhanh YjNKҧnãng giao diӋn phӕi hӧp Wic nghiӋp mӝt TXiWUunh hoһc mӝt công viӋc Qjo ðy Robot Ojðӕi tѭӧng phӭc Wҥp, FyWtnh phi tuyӃn Pҥnh &ic công WUunh nghiên cӭu ðiӅu khiӇn Robot tұp chung FKӫyӃu theo hai hѭӟng: Thӭnhҩt, sӱGөng Fic mô Kunh Fyðһc Wtnh phi tuyӃn ÿӇðõn JLҧn viӋc phân Wtch YjthiӃt kӃ Thӭhai, ÿӅra Fic thuұt WRin ðiӅu khiӇn mӟi nhҵm nâng cao chҩt lѭӧng ðiӅu khiӇn Robot Ĉһc ðiӇm Fӫa Robot Oj: V͉ð͡ng K͕c:9ӏWUtKunh Kӑc cӫa cҩu WU~c Fic khӟp nӕi Gҥng hӣbiӇu diӉn bӣi Kjm phi tuyӃn phӭc Wҥp V͉ð͡ng lc K͕c: ChuyӇn ðӝng Fӫa mӛi khӟp nӕi ҧnh hѭӣng Wic ðӝng qua Oҥi bӣi Fic khӟp NKic 7ҧi WUӑng ban ðҫu Wҥi Fic khӟp nӕi SKөthuӝc Yjo tý thӃ(WUҥng WKii) Fӫa Finh tay robot Ҧnh hѭӣng Fӫa Fic mômen TXin Wtnh ðӕi ngүu Yjmômen TXin Wtnh hѭӟng tâm (Coriolis and Centrifugal) Ojðing kӇkhi robot KRҥt ðӝng ӣtӕc ðӝcao Khâu 6: CS2 B F CS1 CS11 Base Body6 Follower Posi tion Torque Body Sensor Revolute Orientation Angle Joint Actuator Continuous Angle Rate Angle Joint Sensor Continuous Angle Velocity Joi nt Initial Condi tion %ҧng thông sӕxây dӵng mô Kunh: Khӟp 7ӑa ðӝ CS1 [0 0] W 7ӑa ðӝ CS2 [0 0.0435] W 7ӑa ðӝCG [0 0] Adj [0 0] Adj [0 0] Adj [0 0] Adj [0 0] Adj [0.855 0] CS1 [0 0.47] CS1 [0 0.4] CS1 [0 0.065] CS1 [0 0.29] CS1 [-0.4725 0] CS2 [0 -0.235] CS2 [0 -0.265] CS2 [0 -0.0325] CS2 [0 -0.145] CS2 [0 0.36] Mômen TXin Wtnh K lѭӧng (kg) 35 diag([0.702 0.5164 0.6736]) diag([ 0.3204 0.3155 0.0278]) 10 diag([0.7785 0.7785 0.0845]) diag([0.2171 0.1676 0.0571]) diag([0.0029 0.0029 0.0029]) 0.5 diag([0.0037 0.0038 0.000486]) A6 Mô Kunh kh͙i W̩o ma Vit Wƭnh Yjma Vit nhͣt khͣp: CS1 B Base CS2 F Follower Bar1 Angle C ontinuous Angle External Actuation Revolute T orque R ate Joint Sensor Kinetic Friction Continuous Angle Foward Stiction Lim it Joint Initial Condition Kv1 Kv2 Angle Velocity Static Friction R ev erse Stiction Lim it Sign Joint Stiction Actuator 0.1 Product Product1 Kd 0.1 Kv PL.6 A7 Mô Kunh kh͙i W̩o TXͿð̩o chu̱n bi͇n ð͝i nhanh Kunh sin: 6 Scope Angle Sine Wave Constant du/dt 6 Derivati ve Velo Constant1 du/dt Derivati ve1 Acc Constant2 PHỤ LỤC B Các chýõng trình hệ thống ðiều khiển Chýõng WUunh Trang B1 Tạo quỹ ðạo chuyển ðộng chuẩn PL.8 B2 7tnh hӋsӕkhuyӃch ðҥi phi tuyӃn PL.12 B3 7tnh mômen PL.13 PL.7 B1 Chýõng WUunh tạo quỹ ðạo chuyển ðộng chuẩn: function g=trajectory_ref(u) %=============================================================== %Xay dung quy dao chuyen dong chuan cho khop cua Robot IRB2400 %=============================================================== %Cac tham so cua ham %Soc=[x0, y0, z0]: Toa diem dau %Des=[xc, yc, zc]: Toa diem cuoi %Vol0=[v01, v02, v03, v04, v05, v06]: Van toc goc khop ban dau %Volf=[vf1, vf2, vf3, vf4, vf5, vf6]: Van toc goc khop tai diem cuoi %Thong so cua Robot: a, d %tf: Thoi gian chuyen dong % -x0=u(1); y0=u(2); z0=u(3); xf=u(4); yf=u(5); zf=u(6); v01=u(7); v02=u(8); v03=u(9); v04=u(10); v05=u(11); v06=u(12); vf1=u(13); vf2=u(14); vf3=u(15); vf4=u(16); vf5=u(17); vf6=u(18); nx=u(19); ny=u(20); nz=u(21); sx=u(22); sy=u(23); sz=u(24); ax=u(25); ay=u(26); az=u(27); tf=u(28); t=u(29); %Dieu kien ban dau: d1=0.435;d4=0.4;d6=0.29;a2=0.855;ta=1.5; %=============================================================== %CHUONG TRINH CHINH %=============================================================== %Tinh cac goc quay cua tung khop tai thoi diem dau va diem cuoi % -[q01, q02, q03, g04, g05,g06]=invIRB(x0,y0,z0,nx,ny,nz,sx,sy,sz,ax,ay,az,d1,d4,d6,a2); [qf1, qf2, qf3, gf4, gf5, gf6]=invIRB(xf,yf,zf,nx,ny,nz,sx,sy,sz,ax,ay,az,d1,d4,d6,a2); % PL.8 %Khoi tao cac he so cua quy dao chuan khop dinh vi % -h1=Init(q01,qf1,ta,tf); h2=Init(q02,qf2,ta,tf); h3=Init(q03,qf3,ta,tf); % -gp1=Quydaochuan(t,h1,ta,tf); gp2=Quydaochuan(t,h2,ta,tf); gp3=Quydaochuan(t,h3,ta,tf); % -%Tinh khop dinh huong theo khop dinh vi dam bao huong cong cu khong thay doi moi truong lam viec %% -o4=atan2((ax*sin(gp1)+ay*cos(gp1)),((ax*cos(gp1)+ay*sin(gp1))*cos(gp2+gp3)+az*sin (gp2+gp3))); num5=(ax*sin(gp1)-ay*cos(gp1))/sin(o4); den5=(-ax*cos(gp1)-ay*sin(gp1))*sin(gp2+gp3)+az*cos(gp2+gp3); o5=atan2(num5,den5); num6= sin(o4)*((nx*cos(gp1)+ny*sin(gp1))*cos(gp2+gp3)+nz*sin(gp2+gp3))+cos(o4 )*(-nx*sin(gp1)+ny*cos(gp1)); den6= sin(o4)*((sx*cos(gp1)+sy*sin(gp1))*cos(gp2+gp3)+sz*sin(gp2+gp3))+cos(o4 )*(-sx*sin(gp1)+sy*cos(gp1)); o6=atan2(num6,den6); gp=[gp1; gp2; gp3; o4; o5; o6]; goo=[q01 q02 q03 g04 g05 g06]'; v00=[v01 v02 v03 v04 v05 v06]'; g=[gp;goo]; %g=gp; %=============================================================== %KET THUC CHUONG TRINH CHINH %=============================================================== %=============================================================== %Ham dong hoc nguoc tinh cac gia tri tuong ung cua cac goc khop dau va cuoi: q0,qf %=============================================================== function [q1, q2, q3, g4, g5, g6]=invIRB(u1,u2,u3,nx,ny,nz,sx,sy,sz,ax,ay,az,d1,d4,d6,a2) px=u1; py=u2; pz=u3; q=[nx sx ax;ny sy ay;nz sz az]; p=[px;py;pz]; b6=[0 d6]'; %Tinh cac goc dinh vi q1=atan2((ay*d6-py),(ax*d6-px)); c=p-q*b6; PL.9 e=a2^2+d4^2-c(1)^2-c(2)^2-(c(3)-d1)^2; d=-2*a2*d4; x3=(-d-(d^2-e^2)^(1/2))/e; q3=2*atan(x3); k=(ax*d6-px)*cos(q1); k2=az*d6-pz*d1; a11=a2-d4*sin(q3); a12=-d4*cos(q3); delta=a11^2+a12^2; den2=(a11*(c(1)*cos(q1)+c(2)*sin(q1))-a12*(c(3)-d1))/delta; num2=(a12*(c(1)*cos(q1)+c(2)*sin(q1))+a11*(c(3)-d1))/delta; q2=atan2(num2,den2); %Tinh cac goc dinh huong g4=atan2((ax*sin(q1)+ay*cos(q1)),((ax*cos(q1)+ay*sin(q1))*cos(q2+q3)+az*sin(q2+q3 ))); num5=(ax*sin(q1)-ay*cos(q1))/sin(g4); den5=(-ax*cos(q1)-ay*sin(q1))*sin(q2+q3)+az*cos(q2+q3); g5=atan2(num5,den5); num6=sin(g4)*((nx*cos(q1)+ny*sin(q1))*cos(q2+q3)+nz*sin(q2+q3))+cos(g4)*(nx*sin(q1)+ny*cos(q1)); den6=sin(g4)*((sx*cos(q1)+sy*sin(q1))*cos(q2+q3)+sz*sin(q2+q3))+cos(g4)*(sx*sin(q1)+sy*cos(q1)); g6=atan2(num6,den6); %=============================================================== %Xay dung quy dao chuyen dong chuan %=============================================================== %=============================================================== function quydao=Quydaochuan(t,h,ta,td) %gsi_ac=a0ac+a1ac*t+a2ac*t^2+a3ac*t^3+a4ac*t^4 a0ac=h(1); a3ac=h(2); a4ac=h(3); a0cd=h(4); a1cd=h(5); a0df=h(6); a1df=h(7); a3df=h(8); a4df=h(9); qcuoi=h(10); if td0)&(td=0)&(ttd/2)&(ttd quydao=qcuoi; else quydao=-1; %Error end else %Neu td>(4*ta), quy dao gom giai doan: tang, chay deu, giam toc if (t>=0)&(t=2*ta)&(t(td-2*ta))&(ttd quydao=qcuoi; else quydao=-1; %Error end end %=============================================================== %Tinh cac he so cua quy dao chuan ung voi tung khop %=============================================================== function heso=Init(qdau,qcuoi,ta,td) if (td>0)&(td(4*ta) %Quy dao doan ac: a0ac=qdau; a3ac=(qcuoi-qdau)/(4*ta^2*(td-2*ta)); a4ac=-(qcuoi-qdau)/(16*ta^3*(td-2*ta)); %Quy dao doan cd: a0cd=qdau+(qcuoi-qdau)*(ta/(td-2*ta)); a1cd=(qcuoi-qdau)/(td-2*ta); %Quy dao doan df: a0df=qcuoi-(qcuoi-qdau)*ta/(td-2*ta); a1df=(qcuoi-qdau)/(td-2*ta); a4df=-(3*qcuoi-3*a0df-4*a1df*ta)/(16*ta^4); a3df=-(a1df+32*a4df*ta^3)/(12*ta^2); % -end %=============================================================== heso=[a0ac,a3ac,a4ac,a0cd,a1cd,a0df,a1df,a3df,a4df,qcuoi]; %=============================================================== PL.11 B2 Chýõng WUunh ttnh h͏s͙khuy͇ch ð̩i phi tuy͇n: function [y1,y2] = N_gain(e1,e2,e3,e4,e5,e6,e1d,e2d,e3d,e4d,e5d,e6d) e=[e1 e2 e3 e4 e5 e6]'; ed=[e1d e2d e3d e4d e5d e6d]'; y1=zeros(6,1); y2=zeros(6,1); sech1=zeros(6,1); sech2=zeros(6,1); k0=[1 1 1 ]'; k1=[2 2 2 1]'; k2=[1000 1000 1000 1000 1000 1000]; for i=1:1:6 sech1(i)=sech(k2(i)*e(i)); sech2(i)=sech(k2(i)*ed(i)); y1(i)=(k0(i)+k1(i)*(1-sech1(i)))*e(i); y2(i)=(k0(i)+k1(i)*(1-sech2(i)))*ed(i); end PL.12 B3 Chýõng WUunh ð͡ng lc K͕c ng˱ͫc ttnh mômen: function T = inv_dyn(u) q1=u(1);q2=u(2);q3=u(3);q4=u(4);q5=u(5);q6=u(6); q1d=u(7);q2d=u(8);q3d=u(9);q4d=u(10);q5d=u(11);q6d=u(12); u1=u(13);u2=u(14);u3=u(15);u4=u(16);u5=u(17);u6=u(18); % Khoi tao gia tri ban dau a2=0.855;d1=0.435;d4=0.4;d6=0.29; m0 = 324.5; m1 = 35; m2 = 5;m3 = 10; m4 = 4; m5 = 1; m6=0.5; J1=[0.2439 0 0.2725 0 -0.075*m1 r1=[0 -0.075 1]'; J2=[0.7601 0 -0.4454 0 -0.4275*m2 0; -0.075*m1; 0.4297 0; m1]; 0 0.4733 0 ; -0.4275*m2; 0; m2]; r2=[0 -0.4275 1]'; J3=[0.0423 0 0.0423 0 0 r3=[0 -0.235 1]'; J4=[0.0038 0 0 0.7362 -0.235*m3 0.1638 -0.265*m4 0; 0; -0.235*m3; m3 ]; 0; -0.265*m4; 0.0533 0; m4]; r4=[0 -0.265 1]'; J5=[0.0014 0 0 0; 0.0014 0; 0.0014 -0.0325*m5; -0.0325*m5 m5]; r5=[0 -0.0325 1]'; J6=[0.000293 0 0 0; 0.000193 0; 0.0140195 -0.145*m6; -0.145*m6 m6]; r6=[0 -0.145 1]'; g=[0 -9.81 1]; Dr = [0 -1 0; 0 0; 0 0; 0 0]; % end definitions PL.13 A1=[ cos(q1) sin(q1) 0; sin(q1) -cos(q1) 0; d1; 0 1]; A2=[ cos(q2) -sin(q2) a2*cos(q2); sin(q2) cos(q2) a2*sin(q2); 0 0; 0 1]; A3=[ cos(q3) -sin(q3) 0; sin(q3) cos(q3) 0; -1 0; 0 1]; A4=[ cos(q4) sin(q4) 0; sin(q4) -cos(q4) 0; d4; 0 1]; A5=[ cos(q5) -sin(q5) 0; sin(q5) cos(q5) 0; -1 0; 0 1]; A6=[ cos(q6) -sin(q6) 0; sin(q6) cos(q6) 0; 0 d6; 0 1]; % -U11 = Dr*A1; U21 = Dr*A1*A2; U22 = A1*Dr*A2; U31 = U21*A3; U32 = U22*A3; U33 = A1*A2*Dr*A3; U41 U42 U43 U44 = = = = U31*A4; U32*A4; U33*A4; A1*A2*A3*Dr*A4; U51 U52 U53 U54 U55 = = = = = U41*A5; U42*A5; U43*A5; U44*A5; A1*A2*A3*A4*Dr*A5; U61 = U51*A6; U62 = U52*A6; U63 = U53*A6; U64 = U54*A6; U65 = U55*A6; U66 = A1*A2*A3*A4*A5*Dr*A6; % D11 = trace(U11*J1*U11')+trace(U21*J2*U21')+trace(U31*J3*U31')+ trace(U41*J4*U41')+trace(U51*J5*U51')+trace(U61*J6*U61'); D12 = trace(U22*J2*U21')+trace(U32*J3*U31')+trace(U42*J4*U41')+ trace(U52*J5*U51') + trace(U62*J6*U61'); D13 = trace(U33*J3*U31')+trace(U43*J4*U41')+trace(U53*J5*U51')+trace(U63*J6*U 61'); D14 = trace(U44*J4*U41')+trace(U54*J5*U51')+trace(U64*J6*U61'); PL.14 D15 = trace(U55*J5*U51')+trace(U65*J6*U61'); D16 = trace(U66*J6*U61'); D22 = trace(U22*J2*U22')+trace(U32*J3*U32')+trace(U42*J4*U42')+ trace(U52*J5*U52')+trace(U62*J6*U62'); D23 = trace(U33*J3*U32')+trace(U43*J4*U42')+trace(U53*J5*U52')+trace(U63*J6*U 62'); D24 = trace(U44*J4*U42')+trace(U54*J5*U52')+trace(U64*J6*U62'); D25 = trace(U55*J5*U52')+trace(U65*J6*U62'); D26 = trace(U66*J6*U62'); D33 = trace(U33*J3*U33')+trace(U43*J4*U43')+trace(U53*J5*U53')+trace(U63*J6*U 63'); D34 = trace(U44*J4*U43')+trace(U54*J5*U53')+trace(U64*J6*U63'); D35 = trace(U55*J5*U53')+trace(U65*J6*U63'); D36 = trace(U66*J6*U63'); D44 = trace(U44*J4*U44')+trace(U54*J5*U54')+trace(U64*J6*U64'); D45 = trace(U55*J5*U54')+trace(U65*J6*U64'); D46 = trace(U66*J6*U64'); D55 = trace(U55*J5*U55')+trace(U65*J6*U65'); D56 = trace(U66*J6*U65'); D66 = trace(U66*J6*U66'); D = [D11 D12 D13 D14 D15 D16; D12 D22 D23 D24 D25 D26; D13 D23 D33 D34 D35 D36; D14 D24 D34 D44 D45 D46; D15 D25 D35 D45 D55 D56; D16 D26 D36 D46 D56 D66]; % -c1 = -m1*g*U11*r1 -m2*g*U21*r2 -m3*g*U31*r3 -m4*g*U41*r4 -m5*g*U51*r5 m6*g*U61*r6; c2 = -m2*g*U22*r2 -m3*g*U32*r3 -m4*g*U42*r4 -m5*g*U52*r5 -m6*g*U62*r6; c3 = -m3*g*U33*r3 -m4*g*U43*r4 -m5*g*U53*r5 -m6*g*U63*r6; c4 = -m4*g*U44*r4 -m5*g*U54*r5 -m6*g*U64*r6; c5 = -m5*g*U55*r5 -m6*g*U65*r6; c6 = -m6*g*U66*r6; c = [c1 c2 c3 c4 c5 c6]'; % -U111 = Dr*Dr*A1; U211 = U111*A2; U212 = Dr*A1*Dr*A2; U222 = A1*Dr*Dr*A2; U311 U312 U313 U322 U323 U333 = = = = = = U211*A3; U212*A3; Dr*A1*A2*Dr*A3; U222*A3; A1*Dr*A2*Dr*A3; A1*A2*Dr*Dr*A3; U411 U412 U413 U414 U422 U423 U424 = = = = = = = U311*A4; U312*A4; U313*A4; Dr*A1*A2*A3*Dr*A4; U322*A4; U323*A4; A1*Dr*A2*A3*Dr*A4; PL.15 U433 = U333*A4; U434 = A1*A2*Dr*A3*Dr*A4; U444 = A1*A2*A3*Dr*Dr*A4; U511 U512 U513 U514 U515 U522 U523 U524 U525 U533 U534 U535 U544 U545 U555 = = = = = = = = = = = = = = = U411*A5; U412*A5; U413*A5; U414*A5; Dr*A1*A2*A3*A4*Dr*A5; U422*A5; U423*A5; U424*A5; A1*Dr*A2*A3*A4*Dr*A5; U433*A5; U434*A5; A1*A2*Dr*A3*A4*Dr*A5; U444*A5; A1*A2*A3*Dr*A4*Dr*A5; A1*A2*A3*A4*Dr*Dr*A5; U611 = U511*A6; U612 = U512*A6; U613 = U513*A6; U614 = U514*A6; U615 = U515*A6; U616 = Dr*A1*A2*A3*A4*A5*Dr*A6; U622 = U522*A6; U623 = U523*A6; U624 = U524*A6; U625 = U525*A6; U626 = A1*Dr*A2*A3*A4*A5*Dr*A6; U633 = U533*A6; U634 = U534*A6; U635 = U535*A6; U636 = A1*A2*Dr*A3*A4*A5*Dr*A6; U644 = U544*A6; U645 = U545*A6; U646 = A1*A2*A3*Dr*A4*A5*Dr*A6; U655 = U555*A6; U656 = A1*A2*A3*A4*Dr*A5*Dr*A6; U666 = A1*A2*A3*A4*A5*Dr*Dr*A6; % -% h111 = trace(U111*J1*U11')+trace(U211*J2*U21')+trace(U311*J3*U31')+ trace(U411*J4*U41')+trace(U511*J5*U51')+trace(U611*J6*U61'); h112 = trace(U212*J2*U21')+trace(U312*J3*U31')+ trace(U412*J4*U41')+trace(U512*J5*U51')+trace(U612*J6*U61'); h113 = trace(U313*J3*U31')+trace(U413*J4*U41')+trace(U513*J5*U51')+trace(U613* J6*U61'); h114 = trace(U414*J4*U41')+trace(U514*J5*U51')+trace(U614*J6*U61'); h115 = trace(U515*J5*U51')+trace(U615*J6*U61'); h116 = trace(U616*J6*U61'); h122 = trace(U222*J2*U21')+trace(U322*J3*U31')+ trace(U422*J4*U41')+trace(U522*J5*U51')+trace(U622*J6*U61'); h123 = trace(U323*J3*U31')+trace(U423*J4*U41')+trace(U523*J5*U51')+trace(U623* J6*U61'); h124 = trace(U424*J4*U41')+trace(U524*J5*U51')+trace(U624*J6*U61'); PL.16 h125 = trace(U525*J5*U51')+trace(U625*J6*U61'); h126 = trace(U626*J6*U61'); h133 = trace(U333*J3*U31')+trace(U433*J4*U41')+trace(U533*J5*U51')+trace(U633* J6*U61'); h134 = trace(U434*J4*U41')+trace(U534*J5*U51')+trace(U634*J6*U61'); h135 = trace(U535*J5*U51')+trace(U635*J6*U61'); h136 = trace(U636*J6*U61'); h144 = trace(U444*J4*U41')+trace(U544*J5*U51')+trace(U644*J6*U61'); h145 = trace(U545*J5*U51')+trace(U645*J6*U61'); h146 = trace(U646*J6*U61'); h155 = trace(U555*J5*U51')+trace(U655*J6*U61'); h156 = trace(U656*J6*U61'); h166 = trace(U666*J6*U61'); % h211 = trace(U211*J2*U22')+trace(U311*J3*U32')+trace(U411*J4*U42')+trace(U511* J5*U52')+trace(U611*J6*U62'); h212 = trace(U212*J2*U22')+trace(U312*J3*U32')+ trace(U412*J4*U42')+trace(U512*J5*U52')+trace(U612*J6*U62'); h213 = trace(U313*J3*U32')+trace(U413*J4*U42')+trace(U513*J5*U52')+trace(U613* J6*U62'); h214 = trace(U414*J4*U42')+trace(U514*J5*U52')+trace(U614*J6*U62'); h215 = trace(U515*J5*U52')+trace(U615*J6*U62'); h216 = trace(U616*J6*U62'); h222 = trace(U222*J2*U22')+trace(U322*J3*U32')+ trace(U422*J4*U42')+trace(U522*J5*U52')+trace(U622*J6*U62'); h223 = trace(U323*J3*U32')+trace(U423*J4*U42')+trace(U523*J5*U52')+trace(U623* J6*U62'); h224 = trace(U424*J4*U42')+trace(U524*J5*U52')+trace(U624*J6*U62'); h225 = trace(U525*J5*U52')+trace(U625*J6*U62'); h226 = trace(U626*J6*U62'); h233 = trace(U333*J3*U32')+trace(U433*J4*U42')+trace(U533*J5*U52')+trace(U633* J6*U62'); h234 = trace(U434*J4*U42')+trace(U534*J5*U52')+trace(U634*J6*U62'); h235 = trace(U535*J5*U52')+trace(U635*J6*U62'); h236 = trace(U636*J6*U62'); h244 = trace(U444*J4*U42')+trace(U544*J5*U52')+trace(U644*J6*U62'); h245 = trace(U545*J5*U52')+trace(U645*J6*U62'); h246 = trace(U646*J6*U62'); h255 = trace(U555*J5*U52')+trace(U655*J6*U62'); h256 = trace(U656*J6*U62'); h266 = trace(U666*J6*U62'); % h311 = trace(U311*J3*U33')+trace(U411*J4*U43')+trace(U511*J5*U53')+trace(U611* J6*U63'); h312 = trace(U312*J3*U33')+trace(U412*J4*U43')+trace(U512*J5*U53')+trace(U612* J6*U63'); h313 = trace(U313*J3*U33')+trace(U413*J4*U43')+trace(U513*J5*U53')+trace(U613* J6*U63'); h314 = trace(U414*J4*U43')+trace(U514*J5*U53')+trace(U614*J6*U63'); h315 = trace(U515*J5*U53')+trace(U615*J6*U63'); h316 = trace(U616*J6*U63'); PL.17 h322 = trace(U322*J3*U33')+trace(U422*J4*U43')+trace(U522*J5*U53')+trace(U622* J6*U63'); h323 = trace(U323*J3*U33')+trace(U423*J4*U43')+trace(U523*J5*U53')+trace(U623* J6*U63'); h324 = trace(U424*J4*U43')+trace(U524*J5*U53')+trace(U624*J6*U63'); h325 = trace(U525*J5*U53')+trace(U625*J6*U63'); h326 = trace(U626*J6*U63'); h333 = trace(U333*J3*U33')+trace(U433*J4*U43')+trace(U533*J5*U53')+trace(U633* J6*U63'); h334 = trace(U434*J4*U43')+trace(U534*J5*U53')+trace(U634*J6*U63'); h335 = trace(U535*J5*U53')+trace(U635*J6*U63'); h336 = trace(U636*J6*U63'); h344 = trace(U444*J4*U43')+trace(U544*J5*U53')+trace(U644*J6*U63'); h345 = trace(U545*J5*U53')+trace(U645*J6*U63'); h346 = trace(U646*J6*U63'); h355 = trace(U555*J5*U53')+trace(U655*J6*U63'); h356 = trace(U656*J6*U63'); h366 = trace(U666*J6*U63'); % h411 = trace(U411*J4*U44')+trace(U511*J5*U54')+trace(U611*J6*U64'); h412 = trace(U412*J4*U44')+trace(U512*J5*U54')+trace(U612*J6*U64'); h413 = trace(U413*J4*U44')+trace(U513*J5*U54')+trace(U613*J6*U64'); h414 = trace(U414*J4*U44')+trace(U514*J5*U54')+trace(U614*J6*U64'); h415 = trace(U515*J5*U54')+trace(U615*J6*U64'); h416 = trace(U616*J6*U64'); h422 = trace(U422*J4*U44')+trace(U522*J5*U54')+trace(U622*J6*U64'); h423 = trace(U423*J4*U44')+trace(U523*J5*U54')+trace(U623*J6*U64'); h424 = trace(U424*J4*U44')+trace(U524*J5*U54')+trace(U624*J6*U64'); h425 = trace(U525*J5*U54')+trace(U625*J6*U64'); h426 = trace(U626*J6*U64'); h433 = trace(U433*J4*U44')+trace(U533*J5*U54')+trace(U633*J6*U64'); h434 = trace(U434*J4*U44')+trace(U534*J5*U54')+trace(U634*J6*U64'); h435 = trace(U535*J5*U54')+trace(U635*J6*U64'); h436 = trace(U636*J6*U64'); h444 = trace(U444*J4*U44')+trace(U544*J5*U54')+trace(U644*J6*U64'); h445 = trace(U545*J5*U54')+trace(U645*J6*U64'); h446 = trace(U646*J6*U64'); h455 = trace(U555*J5*U54')+trace(U655*J6*U64'); h456 = trace(U656*J6*U64'); h466 = trace(U666*J6*U64'); % h511 = trace(U511*J5*U55')+trace(U611*J6*U65'); h512 = trace(U512*J5*U55')+trace(U612*J6*U65'); h513 = trace(U513*J5*U55')+trace(U613*J6*U65'); h514 = trace(U514*J5*U55')+trace(U614*J6*U65'); h515 = trace(U515*J5*U55')+trace(U615*J6*U65'); h516 = trace(U616*J6*U65'); h522 = trace(U522*J5*U55')+trace(U622*J6*U65'); h523 = trace(U523*J5*U55')+trace(U623*J6*U65'); h524 = trace(U524*J5*U55')+trace(U624*J6*U65'); h525 = trace(U525*J5*U55')+trace(U625*J6*U65'); h526 = trace(U626*J6*U65'); h533 = trace(U533*J5*U55')+trace(U633*J6*U65'); h534 = trace(U534*J5*U55')+trace(U634*J6*U65'); h535 = trace(U535*J5*U55')+trace(U635*J6*U65'); h536 = trace(U636*J6*U65'); h544 = trace(U544*J5*U55')+trace(U644*J6*U65'); PL.18 h545 h546 h555 h556 h566 % h611 h612 h613 h614 h615 h616 h622 h623 h624 h625 h626 h633 h634 h635 h636 h644 h645 h646 h655 h656 h666 = = = = = trace(U545*J5*U55')+trace(U645*J6*U65'); trace(U646*J6*U65'); trace(U555*J5*U55')+trace(U655*J6*U65'); trace(U656*J6*U65'); trace(U666*J6*U65'); = = = = = = = = = = = = = = = = = = = = = trace(U611*J6*U66'); trace(U612*J6*U66'); trace(U613*J6*U66'); trace(U614*J6*U66'); trace(U615*J6*U66'); trace(U616*J6*U66'); trace(U622*J6*U66'); trace(U623*J6*U66'); trace(U624*J6*U66'); trace(U625*J6*U66'); trace(U626*J6*U66'); trace(U633*J6*U66'); trace(U634*J6*U66'); trace(U635*J6*U66'); trace(U636*J6*U66'); trace(U644*J6*U66'); trace(U645*J6*U66'); trace(U646*J6*U66'); trace(U655*J6*U66'); trace(U656*J6*U66'); trace(U666*J6*U66'); H1v=[h111 h112 h113 h114 h115 h116; h112 h122 h123 h124 h125 h126; h113 h123 h133 h134 h135 h136; h114 h124 h134 h144 h145 h146; h115 h125 h135 h145 h155 h156; h116 h126 h136 h146 h156 h166]; H2v=[h211 h212 h213 h214 h215 h216; h212 h222 h223 h224 h225 h226; h213 h223 h233 h234 h235 h236; h214 h224 h234 h244 h245 h246; h215 h225 h235 h245 h255 h256; h216 h226 h236 h246 h256 h266]; H3v=[h311 h312 h313 h314 h315 h316; h312 h322 h323 h324 h325 h326; h313 h323 h333 h334 h335 h336; h314 h324 h334 h344 h345 h346; h315 h325 h335 h345 h355 h356; h316 h326 h336 h346 h356 h366]; H4v=[h411 h412 h413 h414 h415 h416; h412 h422 h423 h424 h425 h426; h413 h423 h433 h434 h435 h436; h414 h424 h434 h444 h445 h446; h415 h425 h435 h445 h455 h456; h416 h426 h436 h446 h456 h466]; H5v=[h511 h512 h513 h514 h515 h516; h512 h522 h523 h524 h525 h526; h513 h523 h533 h534 h535 h536; h514 h524 h534 h544 h545 h546; h515 h525 h535 h545 h555 h556; h516 h526 h536 h546 h556 h566]; H6v=[h611 h612 h613 h614 h615 h616; h612 h622 h623 h624 h625 h626; PL.19 h613 h623 h633 h634 h635 h636; h614 h624 h634 h644 h645 h646; h615 h625 h635 h645 h655 h656; h616 h626 h636 h646 h656 h666]; qd = [q1d q2d q3d q4d q5d q6d]'; u = [u1 u2 u3 u4 u5 u6]'; h = [qd'*H1v*qd; qd'*H2v*qd; qd'*H3v*qd; qd'*H4v*qd; qd'*H5v*qd; qd'*H6v*qd]; T = D*u+h+c; end PL.20 ... với ðiều khiển PD phi tuyến 3.5 Thiết kế thuật toán ðiều khiển ðộng lực học ngýợc robot với ðiӅu khiӇn 33 34 41 42 43 51 52 52 54 55 60 61 61 62 62 64 65 65 66 68 68 71 72 74 74 74 75 75 76 78 79... cao K͕c xC x ( q11a6 q12b6 q13b6 ? ?6 ) y y (q a q b q b ) 21 22 6 23 6 C zC z ( q31a6 q32b6 ? ?6 q33b6? ?6 ) (2 .63 ) Q (b +Q 3b +Q d u3 )... 0142125 - Thành phần tensor quán tính theo trục y khâu 6: I yC 1 2 m6a (ax6 cz62 ) m6b (a'x6 cz62 ) 12 12 ðó: ax6 = 0, 065 m, ax6’ = 0, 063 m Sau thay số ta tính ðýợc: IyC = 0,0038 Mơmen qn