Điều khiển chuyển động robot gryphon theo phương pháp jacobian xấp xỉ

Điều khiển chuyển động robot gryphon theo phương pháp jacobian xấp xỉ Điều khiển chuyển động robot gryphon theo phương pháp jacobian xấp xỉ Điều khiển chuyển động robot gryphon theo phương pháp jacobian xấp xỉ luận văn tốt nghiệp,luận văn thạc sĩ, luận văn cao học, luận văn đại học, luận án tiến sĩ, đồ án tốt nghiệp luận văn tốt nghiệp,luận văn thạc sĩ, luận văn cao học, luận văn đại học, luận án tiến sĩ, đồ án tốt nghiệp

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - NGƠ VIỆT HỊA ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT GRYPHON THEO PHƯƠNG PHÁP JACOBIAN XẤP XỈ LUẬN VĂN THẠC SĨ KHOA HỌC CHUYÊN NGÀNH: TỰ ĐỘNG HÓA Hà Nội - 2008 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - NGƠ VIỆT HỊA ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT GRYPHON THEO PHƯƠNG PHÁP JACOBIAN XẤP XỈ LUẬN VĂN THẠC SĨ KHOA HỌC CHUYÊN NGÀNH: TỰ ĐỘNG HÓA NGƯỜI HƯỚNG DẪN KHOA HỌC TS NGUYỄN PHẠM THỤC ANH Hà Nội - 2008 mơc lơc danh mơc c¸c b¶ng danh mục hình vẽ, đồ thị Lời nói đầu Ch­¬ng 1: Tỉng quan vỊ robot c«ng nghiƯp .10 1.1 Định nghĩa robot công nghiệp (RBCN) 10 1.2 Tự động hóa robot c«ng nghiƯp 11 1.3 Sơ lược lịch sử phát triển Robot công nghiệp 12 1.4 Các đặc tính robot công nghiệp 15 1.4.1 T¶i träng 15 1.4.2 TÇm víi 15 1.4.3 Độ phân dải không gian 15 1.4.4 Độ xác 16 1.4.5 Độ lặp lại 16 1.4.6 §é nhón 16 1.5 HƯ thèng robot c«ng nghiƯp 16 1.5.1 HÖ thèng chun ®éng robot 16 1.5.1.1 BËc tù cña robot 18 1.5.1.2 Khíp robot 18 1.5.1.3 Cæ tay robot 18 1.5.1.4 Bàn tay robot (cơ cấu tác động cuối) 19 1.5.1.5 Các dạng cấu hình học không gian làm việc RBCN 20 1.5.2 HƯ thèng trun ®éng robot 21 1.5.2.1 Trun ®éng thđy lùc 21 1.5.2.2 Trun ®éng khÝ nÐn 22 1.5.2.3 Trun ®éng ®iƯn 22 1.5.3 HƯ thèng ®iỊu khiĨn robot 23 1.5.4 HƯ thèng c¶m biÕn 24 1.5.4.1 C¶m biÕn néi tuyÕn 24 1.5.4.2 Cảm biến ngoại tuyến 24 1.6 øng dông cđa robot c«ng nghiƯp 24 1.6.1 øng dông robot vËn chun, bèc dì vËt liƯu 25 1.6.2 øng dơng robot lÜnh vùc gia c«ng vËt liƯu 25 1.6.3 øng dơng robot lắp ráp kiểm tra sản phẩm 25 Chương 2: động học vị trí robot gryphon 26 2.1 Giíi thiƯu vÒ robot Gryphon 26 2.1.1 Các thông số động học Robot Gryphon 27 2.1.2 Vïng lµm viƯc cđa Robot Gryphon EC 28 2.2 Các phép biến đổi toạ ®é dïng ma trËn thuÇn nhÊt 29 2.2.1 BiĨu diƠn ma trËn 29 2.2.1.1 BiÓu diễn điểm hay vectơ không gian: 29 2.2.1.2 BiĨu diƠn mét khung täa ®é: 30 2.2.1.3 Biểu diễn đối tượng không gian 31 2.2.2 Các phép biến đổi 32 2.2.2.1 Phép biến đổi tịnh tiến đơn 32 2.2.2.2 PhÐp biÕn ®ỉi quay ®¬n 32 2.2.2.3 PhÐp biĨu diƠn kÕt hỵp 33 2.2.3 PhÐp biến đổi biểu diễn vị trí hướng tay robot so với thân robot 33 2.3 Bài toán ®éng lùc häc thuËn 36 2.3.1 Phương pháp thiết kế khung täa ®é - PhÐp biĨu diƠn Danevit - Hartenberg 36 2.3.1.1 Tham sè cđa nèi vµ khíp 36 2.3.1.2 Nguyên tắc thiết kÕ khung täa ®é 37 2.3.1.3 Quan hệ hai khung tọa độ n-1 vµ n 38 2.3.2 Phương trình động học thuận robot Gryphon 38 2.3.3 Ma trËn Jacobian 41 Chương 3: động lực học robot gryphon .43 3.1 Bài toán động lực học 43 3.2 Phương trình Lagrange 44 3.3 Phương trình động lực häc cña Robot Gryphon 45 3.3.1.Động nối 45 3.3.1.1 Động nối 1: 45 3.3.1.2 Động cña nèi 2: 45 3.3.1.3 Động nối 3: 46 3.3.1.4 Tæng động năng: 46 3.3.2 Phương trình động lực học viết cho c¸c nèi 47 3.3.2.1.Phương trình động lực học viết cho nối 47 3.3.2.2 Phương trình động lực häc viÕt cho nèi 48 3.3.2.3.Phương trình động lực học viết cho nối 49 3.3.2.4 Phương trình động lùc häc tỉng qu¸t cđa robot Gryphon 49 CHƯƠNG 4: ĐIềU KHIểN CHUYểN Động robot Gryphon với động học động lực học xác .52 4.1 Kh¸i qu¸t 52 4.1.1 Phân loại yêu cầu điều khiển chuyển động 52 4.1.1.1 Điều khiển vị trí 52 4.1.1.2 Điều khiển bám quỹ đạo 53 4.1.2 Phân loại hƯ thèng ®iỊu khiĨn chun ®éng 54 4.1.2.1 Phân loại theo không gian điều khiển, ta có hệ thống điều khiển không gian khớp hệ thống điều khiển không gian làm việc 54 4.1.2.2 Phân loại theo mức độ ràng buộc robot, ta có hệ thống điều khiển phân tán hƯ thèng ®iỊu khiĨn tËp trung 55 4.1.2.3 Phân loại theo thay ®ỉi tham sè, ta cã hƯ thèng ®iỊu khiĨn kh«ng thích nghi, hệ thống điều khiển thích nghi hệ thống điều khiển bền vững 56 4.2 Mét sè bé ®iỊu khiĨn 56 4.2.1 HÖ thống điều khiển không gian khớp 56 4.2.1.1 Hệ thống điều khiển phản hồi PD bï träng lùc 57 4.2.1.2 Hệ thống điều khiển mômen tính toán 64 4.2.2 Hệ thống điều khiển không gian lµm viƯc 66 4.2.2.1 HƯ thống điều khiển ma trận Jacobian chuyển vị 66 4.2.2.2 HƯ thèng ®iỊu khiĨn ma trËn Jacobian nghịch đảo 72 4.3 Nhận xét 77 Chương 5: điều khiển chuyển động robot Gryphon theo phương pháp jacobiAn xấp xỉ 81 5.1 Đặt vấn đề 81 5.2 Điều khiển điểm đặt với phương ph¸p Jacobian xÊp xØ 82 5.2.1 Nhắc lại phương trình động lực học phương trình động học tổng quát 82 5.2.2 Điều khiển điểm đặt theo phương pháp Jacobian xấp xỉ 83 5.2.2.1 Bộ điều khiển điểm đặt Jacobian xấp xỉ bï träng lùc 83 5.2.2.2 Bé ®iỊu khiển điểm đặt Jacobian xấp xỉ thích nghi với trọng lực xác 92 5.3 §iỊu khiĨn bám quỹ đạo với phương pháp Jacobian xấp xỉ thích nghi 97 5.3.1 C¬ së lý thuyÕt 97 5.3.2 Thiết kế điều khiển bám quỹ đạo Jacobian xÊp xØ thÝch nghi cho robot Gryphon 101 kÕt luËn 109 tµi liƯu tham kh¶o 110 phô lôc 111 danh mục bảng Bảng 1: Các thông số động học Robot Gryphon 27 B¶ng 2: B¶ng tham sè D-H 39 danh mục hình vẽ, đồ thị Hình 1.1 Robot c«ng nghiƯp IRB - 7600 11 Hình 1.2 Hình dạng c¬ khÝ cđa RBCN 17 H×nh 1.3 Hai thÝ dơ vỊ bµn tay robot 20 Hình 1.4 Sơ đồ khối hệ truyền ®éng thñy lùc 21 H×nh 2.1: Robot Gryphon 26 H×nh 2.2: Các thông số động học 28 Hình2.3: Vùng làm việc chuyển động 28 Hình 2.4 Biểu diễn điểm không gian 29 H×nh 2.5 BiĨu diƠn khung täa ®é B khung täa ®é A 30 H×nh 2.6 Biểu diễn vật thể rắn không gian 31 H×nh 2.7 Khung täa ®é tay khung täa ®é gèc 34 H×nh 2.8 ThiÕt kÕ khung täa ®é nèi 36 Hình 2.9 Khung tọa độ nối 39 Hình 4.1 Tay robot chuyển động từ điểm đầu ®Õn ®iĨm mong mn 53 H×nh 4.2 Tay robot bám theo quỹ đạo chuyển động mong muốn 54 Hình 4.3 Sơ đồ khối hệ thống điều khiển không gian khớp 55 Hình 4.4 Sơ đồ khối hệ thống điều khiển không gian làm việc 55 Hình 4.5 Sơ đồ khối tổng quát hệ thống điều khiển phản hồi 57 Hình 4.6 Sơ đồ mô hệ điều khiểnphản hồi PD bï träng lùc kh«ng gian khíp 61 H×nh 4.7 Gãc quay khíp 62 H×nh 4.8 Gãc quay khíp 62 H×nh 4.9 Gãc quay khíp 63 H×nh 4.10 Sơ đồ khối hệ thống điều khiển mômen tính toán 64 Hình 4.11 Sơ đồ khối hệ thống điều khiển ma trận Jacobian chuyển vị 68 Hình 4.12 Sơ đồ Simulink mô pháng hƯ thèng ®iỊu khiĨn ma trËn Jaccobian chun 69 Hình 4.13 Tọa độ x điểm tác động cuối 70 H×nh 4.14 Tọa độ y điểm tác động cuối 70 Hình 4.15 Tọa độ z điểm tác động cuối 71 Hình 4.16 Sơ đồ khối hệ thống điều khiển ma trận Jacobian nghịch đảo 73 Hình 4.17 Sơ đồ Simulink mô hệ thống điều khiển ma trận Jaccobian nghịch đảo 74 Hình 4.18 Tọa độ x ®iĨm t¸c ®éng ci 75 Hình 4.19 Tọa độ y điểm tác ®éng cuèi 75 Hình 4.20 Tọa độ z điểm tác động cuối 76 H×nh 4.21 Sơ đồ mô hệ điều khiển Jacobian chuyển vị với động học không xác 78 Hình 4.22 Tọa độ x điểm tác ®éng cuèi 79 Hình 4.23 Tọa độ y điểm tác động cuối 79 H×nh 4.24 Tọa độ z điểm tác động cuối 80 Hình 5.1 Sơ đồ mô hệ điều khiển điểm đặt Jacobian xấp xỉ bù trọng lực 89 Hình 5.2 Tọa độ x điểm tác động cuối 90 Hình 5.3 Tọa độ y điểm tác động cuối 90 Hình 5.4 Tọa độ z ®iĨm t¸c ®éng ci 91 Hình 5.5 Sơ đồ mô hệ ®iỊu khiĨn ®iĨm ®Ỉt Jacobian xÊp xØ thÝch nghi 95 Hình 5.6 Tọa độ x điểm tác động cuèi 96 Hình 5.7 Tọa độ y điểm tác động cuối 96 H×nh 5.8 Tọa độ z điểm tác động cuối 97 H×nh 5.9 Sơ đồ mô hệ điều khiển bám quỹ đạo Jacobian xấp xỉ thích nghi 104 Hình 5.10 Sơ đồ cÊu tróc khèi Subsystem1 104 Hình 5.11 Sơ đồ cấu trúc khối Subsystem2 105 H×nh 5.12 Tọa độ x điểm tác động cuối 106 Hình 5.13 Tọa độ y điểm tác động cuối 107 Hình 5.14 Tọa độ z ®iĨm t¸c ®éng ci 107 Lời nói đầu Thật thú vị quan sát hành động với tay người, điều mà ta không cần biết cách xác chuyển động học động lực cánh tay Chúng ta cầm nhắc công cụ hay đối tượng vận hành cách tài tình để thực nhiệm vụ mà sử dụng việc hiểu biết cách xấp xỉ chiều dài, khối lượng, hướng điểm cầm công cụ Với khả cảm nhận đáp ứng lại thay đổi không cần ®Õn sù hiĨu biÕt chÝnh x¸c vỊ sù chun ®ỉi từ cảm nhận sang hành động Điều đà giúp có độ tinh xảo, tinh tế cao với thay đổi trước sống Với phát triển không ngừng khoa học kỹ thuật, máy móc thiết bị công nghiệp ngày phải đáp ứng yêu cầu phức tạp hơn, đạt suất cao làm việc cách xác Trong kỷ 20 bước sang kỷ 21, phát triển công nghệ chế tạo, điều khiển Robot bước tiến mạnh mẽ khoa học kỹ thuật Robot công nghiệp cấu khí lập trình thực công việc có ích cách tự động không cần giúp đỡ trực tiếp người Ngày Robot đà trở thành công cụ thiếu nhà máy, xí nghiệp có mức độ tự động hoá cao Robot đảm nhận công việc khó khăn thay người làm việc môi trường độc hại, nguy hiểm, môi trường phóng xạ hay nhiệt độ cao Hơn nữa, Robot ứng dơng nhiỊu lÜnh vùc khoa häc nh­ y tÕ, sinh học, thăm dò địa chất, thám hiểm không gian đời sống bán hàng, làm việc nhà Trước đây, nghiên cứu điều khiển Robot phần lớn thừa nhận động học Robot biết xác ma trận Jacobian tay máy từ không gian khớp nối đến hệ toạ độ Đêcác phải biết Khi động học xác nhận góc khớp nối mong muốn từ thành phần tác động cuối mong muốn việc giải vấn đề động học ngược Hơn nữa, ma trận Jacobian việc đặt từ vị trí khớp nối đến vị trí thực nhiệm vụ biết xác Giả định dẫn đến số vấn đề việc phát triển luật điều khiển Robot ngày Chúng ta cần biết xác chiều dài khuỷ tay, khớp nối khuỷ ống đối tượng mà Robot cầm, nắm, nhiên, thực tế thông số vật lý nhận cách xác Hơn nữa, Robot cầm, nâng đối tượng hay dụng cụ có độ dài khác nhau, không rõ hướng điểm kẹp, tính động học nói chung thay đổi mà khó để lấy xác Nội dung luận văn sâu tìm hiểu Điều khiển chuyển động Robot theo phương pháp Jacobian xấp xỉ động học động lực học xác Các vấn đề điều khiển chứng minh tính ổn đinh dựa theo tiêu chuẩn Lyapunov.Với việc sử dụng phận thông tin phản hồi Robot vị trí tác dụng, vấn đề ®iỊu khiĨn chØ r»ng ®iĨm t¸c ®éng ci cïng có khả đáp ứng chuyển động mong muốn điều kiện không xác tính động học động lực học Điều cho Robot khả xư lý tinh vi ë cÊp cao víi sù thay đổi không xác định trước không chắn động học động lực học, diều mà tương tự chuyển động với tay vận hành công cụ người Luận văn trình bày thành chương với nội dung chương tóm tắt sau: Chương 1- Tổng quan Robot công nghiệp: Tóm tắt sơ lược trình đời phát triển robot công nghiệp, mối quan hệ robot công nghiệp tự động hoá; Đưa đặc tính robot công nghiệp thành phần hệ thống robot công nghiệp Chương 2- Động häc vÞ trÝ Robot Gryphon: giíi thiƯu vỊ Robot Gryphon thành lập phương trình động học vị trí robot Gryphon, tõ ®ã ®­a ma trËn Jacobian biĨu diƠn mối quan hệ tốc độ tay Robot tốc độ khớp nối Robot Chương - Động lực học Robot Gryphon: Trình bày sở lý thuyết phương trình Lagrange, thành lập phương trình động lực học robot Gryphon; Dùa vµo - 112 - out(3,3)=m3*lg3^2+J3; % phan tu M33 % File “Vg.m” function out = V(u); % ham tinh V(q, q ) global l1 l2 l3 m1 m2 m3; lg1=l1/2; lg2=l2/2; lg3=l3/2; q1=u(1); q2=u(2); q3=u(3); % vi tri khop dq1=u(4); dq2=u(5); dq3=u(6); % toc khop lsq31=-2*m2*lg2^2*sin(q2)*cos(q2)*dq2*dq1; lsq32=2*m3*(lg3*cos(q2+q3) +l2*cos(q2))*(lg3*sin(q2+q3)+l2*sin(q2))*dq1*dq2; lsq33=-2*m3*(lg3*cos(q2+q3) +l2*cos(q2))+l2*cos(q2)*lg3*sin(q2+q3*dq1*dq3; out(1)=lsq31+lsq32+lsq33; % phan tu V1 lsq41=m2*lg2^2*sin(q2)*cos(q2)*dq1^2*; lsq42= -m3*dq1^2*(lg3*cos(q2+q3) +l2*cos(q2))*(lg3*sin(q2+q3)+l2*sin(q2)); lsq43=-m3*lg3*l2*sin(q3)*(2*dq2*dq3+dq3); out(2)=lsq41+lsq42+lsq43; % phan tu V2 lsq51= m3*lg3*sin(q2+q3) *dq1^2*(lg3*cos(q2+q3) +l2*cos(q2)); lsq52= m3*lg3*l2*sin(q3)*^2dq2; out(3)=lsq51+lsq52; % phan tu V3 % File “G.m” function out =G(u); % ham tinh G(q) global l1 l2 l3 m1 m2 m3; lg1=l1/2; lg2=l2/2; lg3=l3/2; - 113 - q1=u(1); q2=u(2); q3=u(3); % vi tri khop out(1)=0; % phan tu G1(q) out(2)=(m2*lg2*cos(q2)+m3*l2*cos(q2)+m3*lg3*cos(q2+q3))*9.8; % G2(q) out(3)=m3*lg3*cos(q2+q3)*9.8; % G3(q) % File “con_para.m” Kp=[200 0; 200 0; 0 200]; % ma tran he so Kp Kd=[25 0; 25 0; 0 25]; % ma tran he so Kd qd=[1;1.5;1.3]; % vecto vi tri dat cua cac khop % File “con_in” function out = con_in(u); % ham tinh momen theo luat dieu khien con_para; q= [u(1);u(2);u(3)]; delq= qd-q; % sai lech vi tri khop deldq=[u(4);u(5);u(6)]; % sai lech toc khop T=Kp*delq-Kd*deldq; % luat dieu khien phan hoi PD khong gian khop out=[T;qd]; % File “dyna.m” function out = dyna(u); % tinh gia toc khop q1=u(4); q2=u(5);q3=u(6); M=M_term(q1,q2,q3); TU=[u(1);u(2);u(3)]; out=M\TU; - 114 - PL2 Các file có đuôi .m kèm với sơ đồ Simulink hình 4.12 để mô hệ thống điều khiển ma trận Jaccobian chuyển vị Các file parasyst.m, “M_term.m”, “Vg.m”, “G.m”, “dyna.m” gièng nh­ PL1 % File “con_para.m” Kp=[200 0; 200 0; 0 200]; Kd=[50 0; 50 0; 0 50]; Xd=[0.3;0.16;0.25]; % vecto dat vi tri cua tay % File “J_term.m” function out= J_term(q1,q2,q3); % ham tinh ma tran Jacobian global l1 l2 l3; out(1,1)= -l2*sin(q1)*cos(q2)-l3*sin(q1)*cos(q2+q3); out(1,2)= -l2*cos(q1)*sin(q2)-l3*cos(q1)*sin(q2+q3); out(1,3)= -l3*cos(q1)*sin(q2+q3); out(2,1)= l2*cos(q1)*cos(q2)+l3*cos(q1)*cos(q2+q3); out(2,2)= l2*sin(q1)*sin(q2)-l3*sin(q1)*sin(q2+q3); out(2,3)= -l3*sin(q1)*sin(q2+q3); out(3,1)= 0; out(3,2)= l2*cos(q2)+l3*cos(q2+q3); out(3,3)= l3* cos(q2+q3); % File “X.m” - 115 - function out= X(u); % ham tinh X tu q qua phuong trinh dong hoc thuan global l1 l2 l3; q1= u(1); q2= u(2); q3= u(3); out(1)=l2 * cos(q1) * cos(q2) + l3* cos(q1) * cos(q2+q3); out(2)=l2 * sin(q1) * cos(q2) + l3* sin(q1) * cos(q2+q3); out(3)= l1+ l2*sin(q2) + l3* sin(q2+q3); % File “Xdot.m” function out= Xdot(u); % ham tinh toc cua tay q1= u(1); q2= u(2); q3= u(3); J= J_term(q1,q2,q3); out= J*[u(4);u(5);u(6)]; % File “con_in.m” function out = con_in(u); % ham tinh momen theo luat dieu khien Jacobian chuyen vi con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; % sai lech vi tri cua tay deldX= [u(4);u(5);u(6)]; % sai lech toc cua tay q1= u(7); q2= u(8); q3= u(9); J= J_term(q1,q2,q3); T= J'*(Kp*delX-Kd*deldX); % luat dieu khien Jacobian chuyen vi out=[T;Xd]; PL3 Các file có đuôi .m kèm với sơ đồ Simulink hình 4.17 để mô hệ thống điều khiển ma trận Jaccobian nghịch đảo - 116 - Tất file giống mơc PL2 trõ file “con_para.m” vµ “con_in.m” % File “con_para.m” Kp=[120 0; 120 0; 0 120]; Kd=[25 0; 25 0; 0 25]; Xd=[0.3;0.16;0.25]; % File “con_in.m” function out = con_in(u); % ham tinh momen theo luat dieu khien Jacobian nghich dao con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; deldX= [u(4);u(5);u(6)]; q1= u(7); q2= u(8); q3= u(9); J= J_term(q1,q2,q3); T= inv(J)*(Kp*delX-Kd*deldX); % luat dieu khien Jacobian nghich dao out=[T;Xd]; PL4 C¸c file có đuôi .m kèm với sơ đồ Simulink hình 4.21 để mô hệ điều khiển Jacobian chuyển vị với động học không xác - 117 - C¸c file gièng nh­ mơc PL2: “con_para.m”, “dyna.m”, “G.m”, “M_term.m”, “Vg.m”, “X.m” C¸c file kh¸c: % File “parasyst.m” global l1 l2 l3 m1 m2 m3 l1e l2e l3e; m1=15; m2=12; m3=8; l1=0.135; l2=0.225; l3=0.225; % cac thong so thuc cua robot l1e=0.137; l2e=0.227; l3e=0.222; % cac thong so duoc (khong chinh xac) % File “Je_term.m” function out= Je_term(q1,q2,q3); % ham tinh ma tran Jacobian theo cac thong so duoc global l1e l2e l3e; out(1,1)= -l2e*sin(q1)*cos(q2)-l3e*sin(q1)*cos(q2+q3); out(1,2)= -l2e*cos(q1)*sin(q2)-l3e*cos(q1)*sin(q2+q3); out(1,3)= -l3e*cos(q1)*sin(q2+q3); out(2,1)= l2e*cos(q1)*cos(q2)+l3e*cos(q1)*cos(q2+q3); out(2,2)= l2e*sin(q1)*sin(q2)-l3e*sin(q1)*sin(q2+q3); out(2,3)= -l3e*sin(q1)*sin(q2+q3); out(3,1)= 0; out(3,2)= l2e*cos(q2)+l3e*cos(q2+q3); out(3,3)= l3e* cos(q2+q3); % File “G3e.m” - 118 - function out =G3e(q1,q2,q3); % ham tinh G(q) theo cac thong so duoc (khong chinh xac) global l1e l2e l3e m1 m2 m3; lg1e=l1e/2; lg2e=l2e/2; lg3e=l3e/2; % khoang cach tu dau noi den tam khoi out(1)=0; % phan tu G1(q) out(2)=(m2*lg2e*cos(q2)+m3*l2e*cos(q2)+m3*lg3e*cos(q2+q3))*9.8; % G2(q) out(3)=m3*lg3e*cos(q2+q3)*9.8; % G3(q) out=[out1;out2;out3]; % File “con_in.m” function out = con_in3(u); % ham tinh momen theo luat dieu khien Jacobian chuyen % vi nhung dong hoc khong chinh xac con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; deldX= [u(4);u(5);u(6)]; q1= u(7); q2= u(8); q3= u(9); Ja= Ja_term(q1,q2,q3); Gee= G3e(q1,q2,q3); T= Je'*(Kp*delX-Kd*deldX)+Gee; out=[T;Xd]; - 119 - Phô lôc chương PL5 Các file có đuôi .m kèm với sơ đồ Simulink hình 5.1 để mô hệ thống điều khiển điểm đặt Jacobian xấp xỉ bù trọng lùc C¸c file gièng nh­ mơc PL4: “dyna.m”, “G.m”, “M_term.m”, “Vg.m”, “X.m”, “Je_term.m” C¸c file kh¸c: % File “parasyst.m” global l1 l2 l3 m1 m2 m3 l1e l2e l3e; m1=15;m2=12;m3=8; l1=0.135;l2=0.225;l3=0.225; l1e=0.137; l2e=0.25; l3e=0.25; % cac thong so dong hoc uoc luong % File “con_para.m” Kp=[3000 0; 3000 0; 0 3000]; Bv=[12 0; 12 0; 0 12]; Xd=[0.33;0.17;0.2]; % File “con_in.m” function out = con_in (u); % tinh momen theo lu©t dieu khien Jacobian xap xi bu luc con_para; s1= u(1); s2= u(2); s3= u(3); % cac ham bao hoa si(ei) s= [s1;s2;s3]; dq= [u(7);u(8);u(9)]; - 120 - q1= u(4); q2= u(5); q3= u(6); Je= Je_term(q1,q2,q3); T= -Je'*Kp*s-Bv*dq; out= [T;Xd]; PL6 Các file có đuôi .m kèm với sơ đồ Simulink hình 5.5 mô hệ thống ®iỊu khiĨn Jacobian xÊp xØ thÝch nghi víi träng lùc xác Tất file giống nh­ mơc PL5 trõ file “con_in.m” Ngoµi cã thêm file I3x4.m De_dot.m % File I3x4.m function out = I3x4(q1,q2,q3); % ham tinh ma tran 3x6 c2= cos(q2); c23= cos(q2+q3); g= 9.8; out= [ 0 0 ; g*c2 g*c23 g*c2 ; 0 g*c23 ]; % File “De_dot.m” function out = De_dot(u); % Tinh dao ham vecto uoc luong cac thong so robot s1= u(1); s2= u(2);s3= u(3); s= [s1;s2;s3]; q1= u(4); q2= u(5); q3= u(6); dq= [u(7);u(8);u(9)]; I= I3x4(q1,q2,q3); Je= Je_term(q1,q2,q3); A= [0.21 0 ; - 121 - 0.21 0 ; 0 0.21 ; 0 0.21]; a= 0.5; out= -A*I'*(dq+a*Je'*s); % File “con_in.m” function out = con_in(u); % tinh momen theo luat dieu khien diem dat % Jacobian xap xi thich nghi con_para; s1= u(1); s2= u(2); s3= u(3); s= [s1;s2;s3]; dq= [u(7);u(8);u(9)]; % vecto toc khop q1= u(4); q2= u(5); q3= u(6); Je= Je_term(q1,q2,q3); I= I3x4(q1,q2,q3); De= [u(10);u(11);u(12);u(13)]; % vecto uoc luong cac thong so robot T= -Je'*Kp*s-Bv*dq+I*De; out=[T;Xd]; PL7 Các file có đuôi .m kèm với sơ đồ Simulink hình 5.9 để mô hệ thống điều khiển bám quỹ đạo Jacobian xấp xỉ thích nghi C¸c file gièng nh­ mơc PL2: “parasyst.m”, “dyna.m”, “G.m”, “M_term.m”, “Vg.m”, “X.m” C¸c file kh¸c: % File “con_para.m” Kp=[2000 0; 2000 0; 0 2000]; - 122 - Kd=[2 0; 0; 0 2]; K = [155 0; 155 0; 0 155]; % File “Je_term.m” function out= Je_term(q1,q2,q3,La1,La2,La3); % ham tinh ma tran Jacobian xap xi out(1,1)= -La2*sin(q1)*cos(q2)-La3*sin(q1)*cos(q2+q3); out(1,2)= -La2*cos(q1)*sin(q2)-La3*cos(q1)*sin(q2+q3); out(1,3)= -La3*cos(q1)*sin(q2+q3); out(2,1)= La2*cos(q1)*cos(q2)+La3*cos(q1)*cos(q2+q3); out(2,2)= La2*sin(q1)*sin(q2)-La3*sin(q1)*sin(q2+q3); out(2,3)= -La3*sin(q1)*sin(q2+q3); out(3,1)= 0; out(3,2)= La2*cos(q2)+La3*cos(q2+q3); out(3,3)=La3* cos(q2+q3); % File “Y_term.m” function out = Y_term(q1,q2,q3,dq1,dq2,dq3); % ham tinh ma tran Y(q, q ) out(1,1)= 0; out(1,2)= -sin(q1)*cos(q2)*dq1+cos(q1)*sin(q2)*dq2; out(1,3)= -sin(q1)*cos(q2+q3)*dq1sin(q2+q3)*cos(q1)*dq2+cos(q1)*sin(q2+q3)*dq3; out(2,1)= 0; out(2,2)= cos(q1)*cos(q2)*dq1 - sin(q1)*sin(q2)*dq2; out(2,3)= cos(q1)*cos(q2+q3)*dq1-sin(q1)*sin(q2+q3)*dq2sin(q2+q3)*sin(q1)*dq3; out(3,1)= 0; - 123 - out(3,2)= cos(q2)*dq2; out(3,3)= cos(q2+q3)*dq2+cos(q2+q3)*dq3; % File “qr_dot.m” function out = qr_dot(u); % ham tinh qr q1= u(1); q2= u(2); q3= u(3); La1= u(7); La2= u(8); La3= u(9); dXr= [u(10);u(11);u(12)]; Je= Je_term3(q1,q2,q3,La1,La2,La3); out= inv(Je)*dXr; % File “Xe_dot.m” function out = Xe_dot(u); % ham tinh xˆ q1= u(1); q2= u(2); q3= u(3); dq1= u(4); dq2= u(5); dq3= u(6); La= [u(7);u(8);u(9)]; Y= Y_term(q1,q2,q3,dq1,dq2,dq3); out= Y*La; % File “Z3x10.m” function out= Z3x18(q1,q2,q3,dq1,dq2,dq3,dqr1,dqr2,dqr3,ddqr1,ddqr2,ddqr3); % ham tinh ma tran Z( q, q, qr , qr ) ∈ R x 18 g= 9.8; c2= cos(q2); c3= cos(q3); c23= cos(q2+q3); s2= sin(q2); s3= sin(q3); s23= sin(q2+q3); out(1,1)= ddqr1; out(1,2)= 0; out(1,3)= 0; out(1,4)= 0; out(1,5)= c2^2*ddqr1-s2*c2*(dq2*dqr1+ dq1*dqr2); - 124 - out(1,6)= 0; out(1,7)= c2^2*ddqr1-s2*c2*(dq2*dqr1+ dq1*dqr2); out(1,8)= 2*c2*c23*ddqr1-c2*s23*(dqr2*dq1+dqr1*dq2+dqr3*dq1) c23*s2*(dq2*dqr1+dq1*dqr2); out(1,9)= 0; out(1,10)= c23^2*ddqr1-c23*s23*(dqr2*dq1+dqr1*dq2+dqr3*dq1); out(2,1)= 0; out(2,2)= ddqr2; out(2,3)= ddqr3+ddqr2; out(2,4)= g*c2; out(2,5)= ddqr2 + c2*s2*dq1*dqr1; out(2,6)= g*c2; out(2,7)= c2*s2*dq1*dqr1; out(2,8)= 2*c3*ddqr2+c3*ddqr3+dqr1*dq1*(c23*s2 + c2*s23)s3*(dqr2*dq3+dqr3*dq2+dq3*dqr3); out(2,9)= g*c23; out(2,10)= ddqr2+ddqr3+c23*s23*dq1*dqr1; out(3,1)= 0; out(3,2)= 0; out(3,3)= ddqr2+ddqr3; out(3,4)= 0; out(3,5)= 0; out(3,6)= 0; out(3,7)= 0; out(3,8)= c3*ddqr2+c2*s23*dq1*dqr1+ s3*dq2*dqr3-; out(3,9)= g*c23; out(3,10)= ddqr2+ddqr3+c23*s23*dqr1*dq1; % File “La_dot.m” function out = La_dot(u); % tinh dao ham vecto cac thong so  %dong hoc uoc luong ( Lˆ) con_para; - 125 - q1= u(1); q2= u(2); q3= u(3); dq1= u(4); dq2= u(5); dq3= u(6); delX= [u(7);u(8);u(9)]; deldX= [u(10);u(11);u(12)]; Y= Y_term(q1,q2,q3,dq1,dq2,dq3); R= [0.1 0; 0.1 0; 0 0.1]; out= R*Y'*(Kd*deldX+Kp*delX); % File “Ua_dot.m”  function out= Ue_dot(u); % tinh vecto Uˆ q1= u(1); q2= u(2); q3= u(3); dq1= u(4); dq2= u(5); dq3= u(6); dqr1= u(7); dqr2= u(8); dqr3= u(9); ddqr1= u(10); ddqr2= u(11); ddqr3= u(12); dq= [dq1;dq2;dq3]; dqr= [dqr1;dqr2;dqr3]; s= dq-dqr; Z= Z3x10(q1,q2,q3,dq1,dq2,dq3,dqr1,dqr2,dqr3,ddqr1,ddqr2,ddqr3); N= [0.1 0 0 0 0 ; 0.1 0 0 0 0 ; 0 0.1 0 0 0 0 ; 0 0.1 0 0 0 ; 0 0 0.1 0 0 0 ; 0 0 0.1 0 0 ; 0 0 0 0.1 0 0 ; 0 0 0 0.1 0 ; 0 0 0 0 0.1 0 ; 0 0 0 0 0.1 ; 0 0 0 0 0 0.1]; - 126 - out= -N*Z'*s; % File “con_in.m” function out = con_in(u); % tinh momen theo luat dieu khien bam quy dao % Jacobian xap xi thich nghi con_para; delX= [u(1);u(2);u(3)]; deldX= [u(4);u(5);u(6)]; La1= u(7); La2= u(8); La3= u(9); sx= [u(10);u(11);u(12)]; q1= u(13); q2= u(14); q3= u(15); dq1= u(16); dq2= u(17); dq3= u(18); dqr1= u(19); dqr2= u(20); dqr3= u(21); ddqr1= u(22); ddqr2= u(23); ddqr3= u(24); Ue= [u(25);u(26);u(27);u(28);u(29);u(30);u(31);u(32);u(33);u(34)]; Je= Je_term3(q1,q2,q3,La1,La2,La3); Z= Z3x10(q1,q2,q3,dq1,dq2,dq3,dqr1,dqr2,dqr3,ddqr1,ddqr2,ddqr3); out= -Je'*(Kd*deldX+Kp*delX)-Je'*K*sx+Z*Ue; ... 5- Điều khiển chuyển động Robot Gryphon theo phương pháp Jacobian xấp xỉ: Đưa vấn đề điều khiển điểm đặt điều khiển bám quỹ đạo theo phương pháp Jacobian xấp xỉ, chứng minh tính ổn định theo tiêu... điều khiển chuyển động robot Gryphon theo phương pháp jacobiAn xấp xỉ 81 5.1 Đặt vÊn ®Ị 81 5.2 Điều khiển điểm đặt với phương pháp Jacobian xấp xỉ 82 5.2.1 Nhắc lại phương. .. làm việc robot chia toán điều khiển robot thành hai loại: điều khiển thô điều khiển tinh Điều khiển thô gọi điều khiển chuyển động, áp dụng cho robot chuyển động tự không gian làm việc robot nghĩa