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

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 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 cđa 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 cđa 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 cña 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 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Ư gi÷a 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 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 chuyển độ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 khí RBCN 17 H×nh 1.3 Hai thÝ dơ vỊ bµn tay robot 20 Hình 1.4 Sơ đồ khối hệ trun ®é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 muốn 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 chun vị 68 Hình 4.12 Sơ đồ Simulink mô hệ thống điều khiển ma trận Jaccobian chuyển 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 cđa ®iĨm t¸c ®éng ci 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 cuối 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 cđa ®iĨm t¸c ®éng ci 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 cuối 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 cđa ®iĨm t¸c ®éng ci 107 Hình 5.14 Tọa độ z điểm tác động cuối 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 hiểu biết xác chuyển đổ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 cđa ng­êi Ngµy nµ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 ®Ịu 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 vµ 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ó 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 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 nh­ 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