Nghiên cứu thiết kế và giải thuật điều khiển tay máy theo vết một đường cong không gian

ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH TRƯỜNG ĐẠI HỌC BÁCH KHOA [ \ LÊ ĐÌNH NGUYÊN NGHIÊN CỨU THIẾT KẾ VÀ GIẢI THUẬT ĐIỀU KHIỂN TAY MÁY THEO VẾT MỘT ĐƯỜNG CONG KHƠNG GIAN Chun ngành : Cơng Nghệ Chế Tạo Máy LUẬN VĂN THẠC SĨ Tp Hồ Chí Minh, tháng 11 năm 2009 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH Cán hướng dẫn khoa học : TS TRẦN THIÊN PHÚC Cán chấm nhận xét : Cán chấm nhận xét : Luận văn thạc sĩ bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SĨ TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày tháng năm 2009 TRƯỜNG ĐẠI HỌC BÁCH KHOA CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA CƠ KHÍ Độc Lập - Tự Do - Hạnh Phúc -oOo Tp HCM, ngày 30 tháng 11 năm 2009 NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên : LÊ ĐÌNH NGUYÊN Phái Ngày, tháng, năm sinh: 06/08/1978 : Nam Nơi sinh: Nha Trang Chuyên ngành : Công nghệ chế tạo máy MSHV : 00407222 - TÊN ĐỀ TÀI: NGHIÊN CỨU THIẾT KẾ VÀ GIẢI THUẬT ĐIỀU KHIỂN TAY MÁY THEO VẾT MỘT ĐƯỜNG CONG KHƠNG GIAN - NHIỆM VỤ LUẬN VĂN: - Tìm hiểu nghiên cứu kết cấu tay máy bậc tự (robot PUMA 560) - Tìm hiểu nghiên cứu phương pháp điều khiển tay máy bậc tự - Thiết kế điều khiển PID điều khiển thích nghi cho tay máy bậc tư - Mô điều khiển PID điều khiển thích nghi - NGÀY GIAO NHIỆM VỤ : 02/02/2009 - NGÀY HOÀN THÀNH NHIỆM VỤ : 30/11/2009 - HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: TS TRẦN THIÊN PHÚC Nội dung đề cương Luận văn thạc sĩ Hội Đồng Chuyên Ngành thơng qua CÁN BỘ HƯỚNG DẪN CHỦ NHIỆM BỘ MƠN QUẢN LÝ CHUYÊN NGÀNH TS TRẦN THIÊN PHÚC PGS.TS PHẠM NGỌC TUẤN LỜI CÁM ƠN Em xin chân thành cám ơn thầy Trần Thiên Phúc tận tình quan tâm, giúp đỡ hướng dẫn em hoàn thành tốt luận văn Xin chân thành cám ơn Thầy Cơ Phịng đào tạo sau đại học, Thầy Cô Bộ môn Chế tạo máy, Khoa Cơ khí, truyền đạt kiến thức kinh nghiệm nghiên cứu khoa học, tạo điều kiện giúp đỡ em hoàn thành luận văn Chân thành cám ơn gia đình, bạn bè, đồng nghiệp tạo điều kiện thuận lợi, hỗ trợ, động viên, giúp đỡ tơi hồn thành luận văn tiến độ đạt mục tiêu đề Vì thời gian thực đề tài không nhiều, kiến thức thân cịn hạn chế khơng tránh khỏi thiếu sót Kính mong q thầy cơ, bạn bè bảo, đóng góp ý kiến để luận văn hồn thiện Tp.HCM, ngày 30 tháng 11 năm 2009 Học viên Lê Đình Ngun TĨM TẮT LUẬN VĂN ‫ﻬ*ﻬ‬ Luận văn trình bày việc mơ hình hóa động học phương pháp thiết kế điều khiển cho tay máy bậc tự Bộ điều khiển sử dụng dựa giải thuật điều khiển PID điều khiển thích nghi nhằm nâng cao đặc tính bám theo quỹ đạo tay máy Với việc sử dụng điều khiển này, tay máy bám theo quỹ đạo không gian Trong luận văn tác giả trình bày điều khiển dựa ràng buộc động học để điều khiển động học tay máy bậc tự Hiệu điều khiển thể thông qua kết mô i MỤC LỤC LỜI CÁM ƠN .4 TÓM TẮT LUẬN VĂN .5 CHƯƠNG 1: MỞ ĐẦU 1.1 Lý chọn đề tài 1.2 Tính cấp thiết đề tài .6 1.3 Mục tiêu luận văn 1.4 Phương pháp nghiên cứu 1.5 Ý nghĩa khoa học luận văn 1.6 Ý nghĩa thực tiễn luận văn .7 CHƯƠNG 2: TỔNG QUAN .8 2.1 Giới thiệu .8 2.2 Tình hình nghiên cứu ứng dụng nước 14 2.3 Tình hình nghiên cứu ứng dụng Việt Nam 15 CHƯƠNG 3: MƠ HÌNH ĐỘNG HỌC TAY MÁY BẬC TỰ DO 18 3.1 Động học vị trí 18 3.1.1 Động học thuận 18 3.1.2 Động học ngược robot 22 3.1.2.1 Động học ngược vị trí 24 3.1.2.2 Động học ngược hướng 26 3.2 Động học vận tốc ma trận Jacobian .29 3.2.1 Ma trận Jacobian hình học .29 3.2.2 Ma trận Jacobian giải tích 34 CHƯƠNG 4: THIẾT KẾ BỘ ĐIỀU KHIỂN PID 36 4.1 Cơ sở lý thuyết 36 4.2 Thiết kế điều khiển PID 39 4.3 Kết mô .41 ii CHƯƠNG 5: THIẾT KẾ BỘ ĐIỀU KHIỂN THÍCH NGHI 49 5.1 Thiết kế điều khiển thích nghi .49 5.2 Kết mô .54 KẾT LUẬN .62 TÀI LIỆU THAM KHẢO .63 PHỤ LỤC 65 LÝ LỊCH TRÍCH NGANG .83 CHƯƠNG 1: MỞ ĐẦU 1.1 Lý chọn đề tài: Với phát triển không ngừng khoa học công nghệ dẫn đến phát sinh yêu cầu hệ thống sản xuất, mơi trường làm việc Robot có độ phức tạp ngày tăng Yêu cầu thực tiễn đặt Robot phải hoạt động điều kiện phức tạp, với yêu cầu chất lượng điều khiển cao.Vì việc nghiên cứu điều khiển Robot có ý nghĩa quan trọng 1.2 Tính cấp thiết đề tài: Hiện có loại Robot nghiên cứu phát triển rộng rãi, Robot cố định (còn gọi tay máy) Robot di động Với phát triển không ngừng máy tính phần mềm mơ phỏng, tính tốn hỗ trợ việc nghiên cứu giứp nâng cao khả điều khiển Robot vấn đề thực tiễn cấp thiết 1.3 Mục tiêu luận văn: Đề tài ”Nghiên cứu thiết kế giải thuật điều khiển tay máy bậc tự (robot puma 560) theo vết đường cong khơng gian” nhằm mục tiêu: + Tìm hiểu nghiên cứu kết cấu tay máy bậc tự (robot PUMA 560) + Tìm hiểu nghiên cứu phương pháp điều khiển tay máy bậc tự Phương pháp điều khiển kinh điển ( điều khiển PID) Phương pháp điều khiển đại (điều khiển thích nghi) + Thiết kế điều khiển PID điều khiển thích nghi cho tay máy bậc tư + Mô điều khiển PID bơ điều khiển thích nghi 1.4 Phương pháp nghiên cứu: Để hoàn thành luận văn, người thực phải sử dụng phương pháp sau: + Phương pháp thu thập tài liệu + Phương pháp xử lý tài liệu + Ứng dụng phép tốn để mơ hình hố tay máy + Ứng dụng nguyên lý điều khiển để xây dựng điều khiển + Phương pháp thực mô 1.5 Ý nghĩa khoa học luận văn: Đề tài nghiên cứu việc ứng dụng giải thuật điều khiển PID điều khiển thích nghi lĩnh vực robot lĩnh vực mới, cần tìm hiểu Đề tài hoàn thành sở cho việc nghiên cứu động lực học sau tài liệu tham khảo có giá trị lĩnh vực Robot 1.6 Ý nghĩa thực tiễn luận văn: Lĩnh vực Robot quan tâm xong nghiên cứu ứng dụng vào thực tiễn cịn Luận văn đóng góp nhỏ vào số nghiên cứu Robot Nghiên cứu có tính thực tiễn cao ứng dụng vào thiết kế chế tạo tay máy sau Với điều khiển bậc tự tay máy chuyển động nhẹ nhàng thực cơng việc xác theo quỹ đạo mong muốn, đặc biệt sử dụng dây chuyền lắp ráp gia cơng địi hỏi tính xác độ khó cao CHƯƠNG II: TỔNG QUAN 2.1 Giới thiệu: Việc ứng dụng rô bốt rộng rãi nhà máy công xưởng giới hình ảnh người máy biết nghe biết nói người làm việc nhà công xưởng, hay hình ảnh người máy phim khoa học viễn tưởng, mà hình ảnh hàng trăm, nghìn cánh tay máy với sáu bậc tự vươn cánh tay người khổng lồ làm thay người Người ta gọi chung rô bốt rơ bốt cơng nghiệp Hình 2.1 - Rơ bốt công nghiệp 70 qe(4,1) =0; else qe(4,1) = atan2(-s1*R(1,3)+c1*R(2,3),(c1*c2*c3c1*s2*s3)*R(1,3)+(s1*c2*c3-s1*s2*s3)*R(2,3)+(-s2*c3c2*s3)*R(3,3));% theta4 end qe(6,1) = atan2(((c1*c2*s3+c1*s2*c3)*R(1,2)+(s1*c2*s3+s1*s2*c3)*R(2,2)+(s2*s3+c2*c3)*R(3,2)), ((c1*c2*s3+c1*s2*c3)*R(1,1)+(s1*c2*s3+s1*s2*c3)*R(2,1)+(s2*s3+c2*c3)*R(3,1))); %tinh goc theta6 qe; qo = qe*180/pi; Đoạn chương trình tính ma trận chuyển đổi dang biểu thức symbolic: % Chuong trinh tinh toan ma tran o dang bieu thuc Doan chuong trinh tinh ma % tran T03, T36, T06 Doan chuong trinh tinh ma tran R03, R36, R06 c1 la % cos(theta1), s1 la sin(theta1) tuong tu la sin cos cua cac goc theta2, % theta3, theta4,theta5, theta6 R la ma tran xoay Euler % clc; clear; syms c1 s1 c2 s2 c3 s3 c4 s4 c5 s5 c6 s6 d2 a2 a3 d4 d6; syms r11 r12 r13 r21 r22 r23 r31 r32 r33; T01=[c1 -s1 0; s1 c1 0;0 -1 0;0 0 1] T12=[c2 -s2 a2*c2;s2 c2 a2*s2;0 d2;0 0 1] T23=[c3 -s3 a3*c3;s3 c3 a3*s3;0 -1 0;0 0 1] T34=[c4 s4 0;s4 -c4 0;0 d4;0 0 1] T45=[c5 -s5 0;s5 c5 0;0 -1 0;0 0 1] 71 T56=[c6 -s6 0;s6 c6 0;0 d6;0 0 1] T03=T01*T12*T23 %T03 la Ma tran vi tri va huong cua khau thu doi voi khau co so R03=T03(1:3,1:3) %R03 la Ma tran huong cua khau thu doi voi khau co so R03T=R03.' %R03T la Ma tran ngich dao cua ma tran R03 T36=T34*T45*T56 %T36 la Ma tran vi tri va huong cua khau thu doi voi khau thu R36=T36(1:3,1:3) %R36 la Ma tran huong cua khau thu doi voi khau thu T06=T01*T12*T23*T34*T45*T56 % T06 la Ma tran vi tri va huong cua khau thu doi voi he toa co so % -R =[ r11 r12 r13; r21 r22 r23;r31 r32 r33]; A =R03T*R; % A la ma tran bang tich cua ma tran R03T*R, A = T36 dung de giai goc theta 4, 5, Đoạn chương trình chuyển đổi từ mm sang mét function qfb = mm_to_m(qve) %Chuong trinh doi x y z tu mm sang m % Chuyen vi tri cua vecto tu Visual Nastran sang Simulink qfb(1,1) = qve(4,1)/1000; qfb(2,1) = qve(5,1)/1000; qfb(3,1) = qve(6,1)/1000; qfb(4,1) = qve(1,1); qfb(5,1) = qve(2,1); qfb(6,1) = qve(3,1); Đoạn chương trình tính ma trận Jacobian dạng biểu thức % Chuong trinh tinh Jacobian % Ngay tao 19/05/09 72 % Cac gia tri a2 a3 d2 d4 d6 thay bang gia tri theo cau hinh cua robot % Puma 560 clc; clear; syms q1 q2 q3 q4 q5 q6 a2 a3 d2 d4 d6; % -% Cac ma tran cua cac khau ->6 % -T00 =[0;0;1]; T01 =[cos(q1) -sin(q1) 0;sin(q1) cos(q1) 0;0 -1 0;0 0 1]; T12 =[cos(q2) -sin(q2) a2*cos(q2);sin(q2) cos(q2) a2*sin(q2);0 d2;0 0 1]; T23 =[cos(q3) sin(q3) a3*cos(q3);sin(q3) -cos(q3) a3*sin(q3);0 0;0 0 1]; T34 =[cos(q4) -sin(q4) 0;sin(q4) cos(q4) 0;0 -1 d4;0 0 1]; T45 =[cos(q5) sin(q5) 0;sin(q5) -cos(q5) 0;0 0;0 0 1]; T56 =[cos(q6) -sin(q6) 0;sin(q6) cos(q6) 0;0 d6;0 0 1]; % -% Cac ma tran chuyen vi cua cac khau ->6 so voi he toa co so % -T01 T02 =T01*T12 T03 =T01*T12*T23 T04 =T01*T12*T23*T34 T05 =T01*T12*T23*T34*T45 T06 =T01*T12*T23*T34*T45*T56 73 % -% Cac toa x , y ,z % -x = T06(1,4) y = T06(2,4) z = T06(3,4) % -% Dao ham x theo cac goc theta % -J11 = diff(x,q1); J12 = diff(x,q2); J13 = diff(x,q3); J14 = diff(x,q4); J15 = diff(x,q5); J16 = diff(x,q6); % -% Dao ham y theo cac goc theta % -J21 = diff(y,q1); J22 = diff(y,q2); J23 = diff(y,q3); J24 = diff(y,q4); J25 = diff(y,q5); J26 = diff(y,q6); % -% Dao ham z theo cac goc theta % -J31 = diff(z,q1); J32 = diff(z,q2); J33 = diff(z,q3); J34 = diff(z,q4); J35 = diff(z,q5); 74 J36 = diff(z,q6); % -J1 =[J11 J12 J13 J14 J15 J16;J21 J22 J23 J24 J25 J26;J31 J32 J33 J34 J35 J36]; J2 =[T00 T01(1:3,3) T02(1:3,3) T03(1:3,3) T04(1:3,3) T05(1:3,3)]; J =[J1;J2] w = det(J); Đoạn chương trình luật điều khiển thích nghi: function e = luatdk(q) % Luat dieu khien thich nghi % q(1,1) >>q(6,1) la sai so % q(7,1) >>q(12,1) la dao ham cua quy dao % q(13,1) >>q(18,1) la he so k k(1,1) = q(13,1); k(2,1) = q(14,1); k(3,1) = q(15,1); k(4,1) = q(16,1); k(5,1) = q(17,1); k(6,1) = q(18,1); e(1,1) = q(1,1)*k(1,1)+ q(7,1); e(2,1) = q(2,1)*k(2,1)+ q(8,1); e(3,1) = q(3,1)*k(3,1)+ q(9,1); e(4,1) = sin(q(4,1)*pi/180)/k(4,1)+ q(10,1); e(5,1) = sin(q(5,1)*pi/180)/k(5,1)+ q(11,1); e(6,1) = sin(q(6,1)*pi/180)/k(6,1)+ q(12,1); Đoạn chương trình tính ma trận Jacobian giải tích nghịch đảo function Ja_1 = jacobi_analytical_inv_1711(qj) % Chuong trinh tao ham ma tran Jacobian analytical ngich dao theo cac bien khop theo % radian % Ngay 16/06/09 75 % Nhap vao gia tri cua cau hinh robot PUMA a2 = 0.4; % Gia tri cua a2 tinh theo met theo cau hinh ROBOT a3 = 0;% Gia tri cua a3 tinh theo met theo cau hinh ROBOT d2 = 0.2;% Gia tri cua d2 tinh theo met theo cau hinh ROBOT d4 = 0.37;% Gia tri cua d4 tinh theo met theo cau hinh ROBOT d6 = 0.11;% Gia tri cua d6 tinh theo met theo cau hinh ROBOT q1 = qj(1,1); q2 = qj(2,1); q3 = qj(3,1); q4 = qj(4,1); q5 = qj(5,1); q6 = qj(6,1); Jt =[(((sin(q1)*cos(q2)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*cos(q4)cos(q1)*sin(q4))*sin(q5)(sin(q1)*cos(q2)*sin(q3)+sin(q1)*sin(q2)*cos(q3))*cos(q5))*d6 +(-sin(q1)*cos(q2)*sin(q3)-sin(q1)*sin(q2)*cos(q3))*d4sin(q1)*cos(q2)*a3*cos(q3)+sin(q1)*sin(q2)*a3*sin(q3)sin(q1)*a2*cos(q2)-cos(q1)*d2,((-cos(q1)*cos(q2)*sin(q3)cos(q1)*sin(q2)*cos(q3))*cos(q4)*sin(q5)(cos(q1)*sin(q2)*sin(q3)cos(q1)*cos(q2)*cos(q3))*cos(q5))*d6+(cos(q1)*cos(q2)*cos(q3) -cos(q1)*sin(q2)*sin(q3))*d4-cos(q1)*sin(q2)*a3*cos(q3)cos(q1)*cos(q2)*a3*sin(q3)-cos(q1)*a2*sin(q2),((cos(q1)*cos(q2)*sin(q3)cos(q1)*sin(q2)*cos(q3))*cos(q4)*sin(q5)(cos(q1)*sin(q2)*sin(q3)cos(q1)*cos(q2)*cos(q3))*cos(q5))*d6+(cos(q1)*cos(q2)*cos(q3) -cos(q1)*sin(q2)*sin(q3))*d4-cos(q1)*cos(q2)*a3*sin(q3)cos(q1)*sin(q2)*a3*cos(q3),(-(cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*sin(q4)sin(q1)*cos(q4))*sin(q5)*d6,(((cos(q1)*cos(q2)*cos(q3)- 76 cos(q1)*sin(q2)*sin(q3))*cos(q4)-sin(q1)*sin(q4))*cos(q5)+(cos(q1)*cos(q2)*sin(q3)cos(q1)*sin(q2)*cos(q3))*sin(q5))*d6,0; (((cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3))*cos(q4)sin(q1)*sin(q4))*sin(q5)-(-cos(q1)*cos(q2)*sin(q3)cos(q1)*sin(q2)*cos(q3))*cos(q5))*d6+(cos(q1)*cos(q2)*sin(q3) +cos(q1)*sin(q2)*cos(q3))*d4+cos(q1)*cos(q2)*a3*cos(q3)cos(q1)*sin(q2)*a3*sin(q3)+cos(q1)*a2*cos(q2)-sin(q1)*d2,((sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*cos(q4)*sin(q5)-(sin(q1)*cos(q2)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*cos(q5))*d6+ (sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*d4sin(q1)*sin(q2)*a3*cos(q3)-sin(q1)*cos(q2)*a3*sin(q3)sin(q1)*a2*sin(q2), ((-sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*cos(q4)*sin(q5)-(sin(q1)*cos(q2)*cos(q3)+sin(q1)*sin(q2)*sin(q3))*cos(q5))*d6+ (sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*d4sin(q1)*cos(q2)*a3*sin(q3)-sin(q1)*sin(q2)*a3*cos(q3), ((sin(q1)*cos(q2)*cos(q3)sin(q1)*sin(q2)*sin(q3))*sin(q4)+cos(q1)*cos(q4))*sin(q5)*d6, (((sin(q1)*cos(q2)*cos(q3)sin(q1)*sin(q2)*sin(q3))*cos(q4)+cos(q1)*sin(q4))*cos(q5)+(sin(q1)*cos(q2)*sin(q3)-sin(q1)*sin(q2)*cos(q3))*sin(q5))*d6, 0; 0,((sin(q2)*sin(q3)-cos(q2)*cos(q3))*cos(q4)*sin(q5)(cos(q2)*sin(q3)+sin(q2)*cos(q3))*cos(q5))*d6+(sin(q2)*cos(q3)-cos(q2)*sin(q3))*d4cos(q2)*a3*cos(q3)+sin(q2)*a3*sin(q3)a2*cos(q2),((sin(q2)*sin(q3)cos(q2)*cos(q3))*cos(q4)*sin(q5)(cos(q2)*sin(q3)+sin(q2)*cos(q3))*cos(q5))*d6+(sin(q2)*cos(q3)-cos(q2)*sin(q3))*d4+sin(q2)*a3*sin(q3)- 77 cos(q2)*a3*cos(q3),-(-sin(q2)*cos(q3)cos(q2)*sin(q3))*sin(q4)*sin(q5)*d6,((-sin(q2)*cos(q3)cos(q2)*sin(q3))*cos(q4)*cos(q5)+(sin(q2)*sin(q3)cos(q2)*cos(q3))*sin(q5))*d6,0; 0,-sin(q1),sin(q1),cos(q1)*cos(q2)*sin(q3)+cos(q1)*sin(q2)*cos(q3), (cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3))*sin(q4)sin(q1)*cos(q4),((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*cos(q4)-sin(q1)*sin(q4))*sin(q5)-(cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*cos(q5); 0, cos(q1), cos(q1), sin(q1)*cos(q2)*sin(q3)+sin(q1)*sin(q2)*cos(q3), (sin(q1)*cos(q2)*cos(q3)sin(q1)*sin(q2)*sin(q3))*sin(q4)+cos(q1)*cos(q4), ((sin(q1)*cos(q2)*cos(q3)sin(q1)*sin(q2)*sin(q3))*cos(q4)+cos(q1)*sin(q4))*sin(q5)-(sin(q1)*cos(q2)*sin(q3)-sin(q1)*sin(q2)*cos(q3))*cos(q5); 1,0, 0, -sin(q2)*sin(q3)+cos(q2)*cos(q3), -(sin(q2)*cos(q3)-cos(q2)*sin(q3))*sin(q4), (-sin(q2)*cos(q3)cos(q2)*sin(q3))*cos(q4)*sin(q5)-(sin(q2)*sin(q3)cos(q2)*cos(q3))*cos(q5)]; %function Euler = matran_Euler(qeuler) % Ham tao ma tran Euler tu cac goc Euler % Tao ma tran Ja(q) = [I,O;0,B]*Jt alpha = qj(10,1)*pi/180; beta = qj(11,1)*pi/180; gamma = qj(12,1)*pi/180; B = [cos(gamma)*sin(beta),sin(gamma),0;sin(gamma)*sin(beta),cos(gamma),0;cos(beta),0,1] ; %theo tai lieu cua Spong W Mark B_1 = inv(B); Euler = [1, 0, 0, 0, 0, 0; 78 0, 1, 0, 0, 0, 0; 0, 0, 1, 0, 0, 0; 0, 0, 0, B_1(1,1),B_1(1,2),B_1(1,3); 0, 0, 0, B_1(2,1),B_1(2,2),B_1(2,3); 0, 0, 0, B_1(3,1),B_1(3,2),B_1(3,3)]; Ja = Euler*Jt; Ja_1 = inv(Ja); qj; Đoạn chương trình tạo quỹ đạo đường cong khơng gian Đecác %Chuong trinh tao duong cong 3D goc phan tu thu nhat khong gian %lam viec robot Puma clc; clear; t = 0:0.1:10; x = 0.4+0.1*cos(0.157*t); y = 0.4+0.1*sin(0.157*t); z = 0.01*t; alpha = 30*cos(0*t); beta = 30*cos(0*t); gamma = 30*cos(0*t); u =[t;x;y;z;alpha;beta;gamma]; a =u.' % Doan tinh van toc vx = -0.0157*sin(0.157*t); vy = 0.0157*cos(0.157*t); vz = 0.01*cos(0*t); w = [t;vx;vy;vz;0*t;0*t;0*t]; b = w.' figure(7); clf; 79 plot3((0.4+0.1*cos(0.157*t)),(0.4+0.1*sin(0.157*t)),0.01*t,'L ineWidth',2) grid on Đoạn chương trình tạo quỹ đạo đường thẳng không gian Đềcác %Chuong trinh tao duong thang xien clc; clear; t = 0:0.1:9; x = 0.7-0.05*t; y = 0.2+0.05*t; z = 0.01*t; alpha = 30*cos(0*t); beta = 30*cos(0*t); gamma = 30*cos(0*t); u =[t;x;y;z;alpha;beta;gamma]; a =u.' vx = -0.05*cos(0*t); vy = 0.05*cos(0*t); vz = 0.01*cos(0*t); w = [t;vx;vy;vz;0*t;0*t;0*t]; b = w.' figure(7); clf; plot3((0.7-0.05*t),(0.2+0.05*t),0.01*t,'LineWidth',2); grid on Đoạn chương trình vẽ đồ thị đáp ứng Robot %% Plot x reference figure(1); clf; out1 = ScopeData; plot(out1(:,1),out1(:,2),'k ','LineWidth',2); title(' Toa x','FontSize',12); 80 xlabel('Time [s]','FontSize', 12); grid on; hold on; %% Plot x work figure(1); plot(out1(:,1),out1(:,3));%,'b-','LineWidth',2 %grid on; legend('Toa X_w tay may','Toa X_r tham chieu',1); % -%% Plot y reference figure(2); clf; out2 = ScopeData1; plot(out2(:,1),out2(:,2),'k ','LineWidth',2); title(' Toa y','FontSize',12); xlabel('Time [s]','FontSize', 12); grid on; hold on; %% Plot y work figure(2); plot(out2(:,1),out2(:,3));%,'b-','LineWidth',2 legend('Toa Y_w tay may','Toa Y_r tham chieu',4); % -%% Plot Z reference figure(3); clf; out3 = ScopeData2; plot(out3(:,1),out3(:,2),'k ','LineWidth',2); title(' Toa z','FontSize',12); xlabel('Time [s]','FontSize', 12); grid on; hold on; 81 %% Plot Z work figure(3); plot(out3(:,1),out3(:,3));%,'b-','LineWidth',2 %title('Amature Current [A]','FontSize',12); %xlabel('Time [s]','FontSize', 12); %grid on; legend('Toa Z_w tay may','Toa Z_r tham chieu',1); % -% -%% Plot EULER_Phi reference figure(4); clf; out4 = ScopeData3; plot(out4(:,1),out4(:,2),'k ','LineWidth',2); title(' Goc Euler Phi','FontSize',12); xlabel('Time [s]','FontSize', 12); grid on; hold on; %% Plot y work figure(4); plot(out4(:,1),out4(:,3));%,'b-','LineWidth',2 legend('Goc EULER Phi_w tay may','Goc EULER Phi_r tham chieu',4); % -%% Plot EULER_THETA reference figure(5); clf; out5 = ScopeData4; plot(out5(:,1),out5(:,2),'k ','LineWidth',2); title(' Goc Euler Theta','FontSize',12); xlabel('Time [s]','FontSize', 12); grid on; 82 hold on; %% Plot EULER_THETA work figure(5); plot(out5(:,1),out5(:,3));%,'b-','LineWidth',2 legend('Goc EULER Theta_w tay may','Goc EULER Theta_r tham chieu',4); % -%% Plot EULER_SI reference figure(6); clf; out6 = ScopeData5; plot(out6(:,1),out6(:,2),'k ','LineWidth',2); title(' Goc Euler Si','FontSize',12); xlabel('Time [s]','FontSize', 12); grid on; hold on; %% Plot EULER_SI work figure(6); plot(out6(:,1),out6(:,3));%,'b-','LineWidth',2 legend('Goc EULER Si_w tay may','Goc EULER Si_r tham chieu',4); hold off; LÝ LỊCH TRÍCH NGANG Họ tên học viên : LÊ ĐÌNH NGUYÊN Ngày, tháng, năm sinh: 06/08/1978 Nơi sinh : Nha Trang Địa liên lạc : 264/32 Lê Quang Định, quận Bình Thạnh, TPHCM Q TRÌNH ĐÀO TẠO 1996 – 2002 Sinh viên trường Đại học Bách Khoa_ Tp Hồ Chí Minh 2007 – Học viên cao học trường Đại học Bách Khoa Tp Hồ Chí Minh Q TRÌNH CƠNG TÁC 2002 – 2007 Làm việc Xí nghiệp in bao bì Khatoco – Thành phố Nha Trang 2007 – Làm việc Công ty TNHH xây dựng STC – TPHCM 62 KẾT LUẬN 6.1 Tổng kết kết thực đề tài: Mô hình hóa động học tay máy bậc tự hồn chỉnh Từ mơ hình động học tay máy tác giả đưa hai phương án điều khiển khác điều khiển PID điều khiển thích nghi để điều khiển tay máy bám theo đường cong không gian Hiệu phương án điều khiển thể thông qua kết mô 6.2 Kiến nghị nghiên cứu tiếp theo: Qua kết qua mô cho thấy điều khiển PID có hạn chế dùng để điều khiển robot bậc tự bám theo quỹ đạo, điều khiển thích nghi đáp ứng tốt thay đổi quỹ đạo công tác Trong luân văn chưa đề cập việc chọn lựa hệ số điều chỉnh k cho luật điều khiển thích nghi tối ưu, việc điều chỉnh thông số điều khiển PID để robot hoạt động ổn định thay đổi quỹ đạo cần bám theo vết Để đáp ứng tốt yêu cầu độ xác, độ ổn định, tính linh hoạt, đa năng… robot cần sử dụng kỹ thuật điều khiển thông minh logic mờ, điều khiển bền vững, mạng thần kinh, giải thuật di truyền… Những vấn đề nêu nên nghiên cứu đề tài sau để phát triển đề tài trở thành đề tài hoàn chỉnh trở thành sở lý luận cho việc thiết kế chế tạo robot phục vụ xã hội ... chế tạo máy MSHV : 00407222 - TÊN ĐỀ TÀI: NGHIÊN CỨU THIẾT KẾ VÀ GIẢI THUẬT ĐIỀU KHIỂN TAY MÁY THEO VẾT MỘT ĐƯỜNG CONG KHÔNG GIAN - NHIỆM VỤ LUẬN VĂN: - Tìm hiểu nghiên cứu kết cấu tay máy bậc... pháp điều khiển kinh điển ( điều khiển PID) Phương pháp điều khiển đại (điều khiển thích nghi) + Thiết kế điều khiển PID điều khiển thích nghi cho tay máy bậc tư + Mô điều khiển PID bơ điều khiển. .. tay máy bậc tự (robot puma 560) theo vết đường cong không gian? ?? nhằm mục tiêu: + Tìm hiểu nghiên cứu kết cấu tay máy bậc tự (robot PUMA 560) + Tìm hiểu nghiên cứu phương pháp điều khiển tay máy