Tìm hiểu, phân tích cũng như tính toán thiết kế robot khắc chữ trên mặt phẳng nghiêng

Tìm hiểu, phân tích cũng như tính toán thiết kế robot khắc chữ trên mặt phẳng nghiêng

TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI MỤC LỤC LỜI MỞ ĐẦU PHẦN 1: PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC 1.1 Số bậc tự cần thiết 1.2 Các phương án thiết kế cấu trúc khâu khớp 1.3 Chọn, thiết kế cấu trúc chọn PHẦN 2: GIẢI BÀI TOÁN ĐỘNG HỌC 10 2.1 Xây dựng cấu trúc, thiết lập hệ phương trình động học robot 10 2.1.1 Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg 11 2.2.1 Xác định vận tốc điểm tác động cuối vận tốc góc khâu thao tác 13 2.2.2 Xây dựng quy luật chuyển động khâu 14 2.3 Giải toán động học ngược 17 2.3.1 Giải phương pháp giải tích 17 2.3.2 Xây dựng quy luật chuyển động khâu thao tác E giải động học ngược 18 PHẦN 3: TÍNH TOÁN LỰC 22 3.1 Phân tích trạng thái tĩnh 22 3.2 Tính toán lực/moment lớn trạng thái tĩnh 24 PHẦN 4: TÍNH TOÁN ĐỘNG LỰC HỌC 25 4.1 Xây dựng cấu trúc động lực học thành phần cần thiết 25 4.2 Thiết lập phương trình Lagrang robot 27 4.3 Động lực học thuận 29 4.4 Động lực học ngược 33 PHẦN 5: THIẾT KẾ HỆ DẪN ĐỘNG ROBOT 36 5.1 Thiết kế hệ dẫn động cho khớp 36 5.2 Tính chọn động 36 PHẦN 6: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 39 6.1 Hệ thống điều khiển phản hồi không bù trọng lực G(q) 39 6.1.1 Luật điều khiển 39 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI 6.1.2 Mô Phỏng điều khiển Matlab 41 6.2 Hệ thống điều khiển phản hồi có bù trọng lực G(q) 46 6.2.1 Luật điều khiển 46 6.2.2 Mô Phỏng điều khiển Matlab 46 Tài Liệu tham khảo 51 Phục Lục 52 1) Code trương trình (nhom6.text) 52 2) Chương trình maple tính động học thuận 64 3) Chương trình maple tính động học ngược 65 4) Chương trình maple tính tĩnh học 66 5) Chương trình maple tính động lực học thuận 67 6) Chương trình maple tính động lực học ngược 71 7) Chương trình matlab động học thuận 71 8) Chương trình matlab động học ngược 71 9) Chương trình matlab điều khiển không bù trọng lực 72 10) Chương trình matlab điều khiển có bù trọng lực 72 11) Chương trình matlab quỹ đạo 73 12) Chương trình matlab phương trình động lực học robot 73 13) Chương trình matlab mô với điều khiển PD không bù trọng lực 74 14) Chương trình matlab mô với điều khiển PD có bù trọng lực 76 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI LỜI MỞ ĐẦU Robot ngành khoa học kỹ thuật liên quan đến vấn đề thiết kế, xây dựng mô hình, điều khiển ứng dụng robot Ngày nay, robot đồng hành với sống thường ngày người phạm vi ứng dụng ngày rộng rãi từ đồ chơi trẻ em, thiết bị văn phòng đến robot công nghiệp robot thăm dò không gian Trong công nghiệp, từ đời robot áp dụng nhiều lĩnh vực góc độ thay sức lao động người Nhờ dây chuyền sản xuất tổ chức lại, suất lao động tăng cao Với nước ta, nước phát triển, đường trở nước công nghiệp tiên tiến Robot lại mảng thiếu công nghiệp đại Thế nên việc đào sâu nghiên cứu robot tiến hành từ sớm Tuy đời muộn robot Việt Nam dần bắt kịp với nước giới Chính báo cáo môn học tính toán thiết kế Robot, chúng em thầy giao cho tìm hiểu, phân tích tính toán thiết kế robot khắc chữ mặt phẳng nghiêng Tuy sai sót, phần động học, tĩnh học, động lực học điều khiển robot trình bày tiểu luận Mong thầy bạn đóng góp ý kiến để nhóm chúng em hoàn thiện tập Nhóm sinh viên thực TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI PHẦN 1: PHÂN TÍCH VÀ LỰA CHỌN CẤU TRÚC 1.1 Số bậc tự cần thiết Nguyên tắc di chuyển mặt phẳng cần bậc tự robot nằm mặt phẳng đó, với trường hợp viết chữ robot nằm mặt phẳng Vì vậy, cần bậc tự để di chuyển không gian Mặt khác, đề không yêu cầu hướng khâu cuối mà tập trung vào điểm tác động cuối cho lên ta cần thiết kế cấu hình robot chuỗi hở bậc tự trở lên 1.2 Các phương án thiết kế cấu trúc khâu khớp Dựa việc phân tích số bậc tự cần thiết Dưới số phương án cấu trúc robot đề (cấu trúc dạng thanh): Hình 1.2.1: Cấu trúc TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 1.2.2: Cấu trúc Hình 1.2.3: Cấu trúc TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 1.2.4: Cấu trúc TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 1.2.5: Cấu trúc 1.3 Chọn, thiết kế cấu trúc chọn Với phương án đưa trên, nhóm chọn cấu trúc làm cấu trúc để khảo sát, tính toán thiết kế Đây robot DOF dạng RRT Do không gian làm việc mặt phẳng nghiêng có kích thước 30x50 cm góc nghiêng mặt phẳng 300 nên robot với tới điểm xa tổng chiều dài (trên mặt phẳng song song với mặt phẳng nằm ngang)  302  502 cos(30)  50 (cm) Từ ta thiết kế: d1 = 0,7 (m); a1 = 0,3 (m); a2 = 0,3 (m); d3 = 0,6 (m) Mô hình tổng thể robot: TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 1.3.1: Mô hình robot khắc chữ bậc RRT Mô hình khâu, khớp: Hình 1.3.2: Khâu (đế) TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 1.3.3: Khâu Hình 1.3.4: Khâu TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 1.3.5: Khâu PHẦN 2: GIẢI BÀI TOÁN ĐỘNG HỌC Ta thiết lập phương trình động học theo phương pháp ma trận Denavit Hartenberg 2.1 Xây dựng cấu trúc, thiết lập hệ phương trình động học robot Z1 q2 X2 a2 a1 O2 X1 X0' d1 q3 Z2 O1 q1 X2 Z0 X3 O3 Z3 Y0 O0 X0 Hình 2.1: Hệ tọa độ robot khắc chữ cấu trúc RRT 10 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI od: od: RETURN(Je) end: # -jr3m:=proc(a:=Vector(3)) local x,y,z,i,b:=matix(),c:=Vector(3); c:=a; for i to c[i]:=subs([diff(q[1](t),t)=x,diff(q[2](t),t)=y,diff(q[3](t),t)=z,diff(q[4](t),t)=k,diff(q[5](t),t)=m,diff(q[6](t),t)=n],c[i]); od: b:=jacobian(c,[x,y,z]); print(b); RETURN(b); end: 2) Chương trình maple tính động học thuận > > > > > > > > > 64 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI > > > > > > 3) Chương trình maple tính động học ngược > > > > > > > > > > > > > 65 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI > > 4) Chương trình maple tính tĩnh học > > > > > > > > > > > > > > > 66 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI > > > > > > > > 5) Chương trình maple tính động lực học thuận > > > > > > > > > > > > 67 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI > > > > 68 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI > > > > > > > > 69 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI 70 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI 6) Chương trình maple tính động lực học ngược > > > > > > 7) Chương trình matlab động học thuận function [s] = DHTh3(q) d1=0.7;a1=0.3;a2=0.3; s1=a1*cos(q(1))+a2*cos(q(2)+q(1)); s2=a1*sin(q(1))+a2*sin(q(2)+q(1)); s3=d1-q(3); s=[s1;s2;s3]; end 8) Chương trình matlab động học ngược function [q] = DHNh3(s) % thong so m1= 5.2; m2= 4.5; m3= 2.1; d1= 0.7; a1=0.3; a2=0.3; g = 9.81; xc1=0.2; xc2=0.15; zc3=0.3; Iy1 = 0.39e-2; Iz1 = 0.39e-2; Iy2 = 0.3375e-1; Iz2 = 0.3375e-1; Ix3 = 0.63e-1; Iz3 = 0.63e-1; %tinh q2 c2=(s(1)^2+s(2)^2-a1^2-a2^2)/(2*a1*a2); s2=sqrt(1-c2^2); q2=atan2(s2,c2); %tinh q1 c1=(a1*s(1)+a2*(s(1)*c2+s(2)*s2))/(s(1)^2+s(2)^2); 71 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI s1=((a1+a2*c2)*s(2)-a2*s2*s(1))/(s(1)^2+s(2)^2); q1=atan2(s1,c1); % tinh q3 q3=d1-s(3); q=[q1;q2;q3]; end 9) Chương trình matlab điều khiển không bù trọng lực function [M,ssq,ssv] = DKh3(qd,vd,q,v) % thong so m1= 5.2; m2= 4.5; m3= 2.1; d1= 0.7; a1=0.3; a2=0.3; g = 9.81; xc1=0.2; xc2=0.15; zc3=0.3; Iy1 = 0.39e-2; Iz1 = 0.39e-2; Iy2 = 0.3375e-1; Iz2 = 0.3375e-1; Ix3 = 0.63e-1; Iz3 = 0.63e-1; % ss ssv1=vd(1)-v(1); ssv2=vd(2)-v(2); ssv3=vd(3)-v(3); ssv=[ssv1;ssv2;ssv3]; ssq1=qd(1)-q(1); ssq2=qd(2)-q(2); ssq3=qd(3)-q(3); ssq=[ssq1;ssq2;ssq3]; % Mo men dieu khien Kp=[500,0,0;0,500,0;0,0,2500]; Kd=[100,0,0;0,100,0;0,0,1500]; M=Kp*ssq+Kd*ssv; end 10) Chương trình matlab điều khiển có bù trọng lực function [M,ssq,ssv] = DKh3(qd,vd,q,v) % thong so m1= 5.2; m2= 4.5; m3= 2.1; d1= 0.7; a1=0.3; a2=0.3; g = 9.81; xc1=0.2; xc2=0.15; zc3=0.3; Iy1 = 0.39e-2; Iz1 = 0.39e-2; Iy2 = 0.3375e-1; Iz2 = 0.3375e-1; Ix3 = 0.63e-1; Iz3 = 0.63e-1; % ss ssv1=vd(1)-v(1); ssv2=vd(2)-v(2); ssv3=vd(3)-v(3); ssv=[ssv1;ssv2;ssv3]; ssq1=qd(1)-q(1); ssq2=qd(2)-q(2); ssq3=qd(3)-q(3); ssq=[ssq1;ssq2;ssq3]; % Mo men dieu khien Kp=[500,0,0;0,500,0;0,0,500]; Kd=[100,0,0;0,100,0;0,0,100]; % G(q) g1=0; g2=0; g3=-m3*g; G=[g1;g2;g3]; M=Kp*ssq+Kd*ssv+G; end 72 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI 11) Chương trình matlab quỹ đạo function [qd,vd] = quidaoh3(q0,qc,tc,t) % qui dao bac a01=q0(1);a11=0;a21=3*(qc(1)-q0(1))/tc^2;a31=-2*(qc(1)-q0(1))/tc^3; qd1=a01+a11*t+a21*t^2+a31*t^3; vd1=a11+2*a21*t+3*a31*t^2; a02=q0(2);a12=0;a22=3*(qc(2)-q0(2))/tc^2;a32=-2*(qc(2)-q0(2))/tc^3; qd2=a02+a12*t+a22*t^2+a32*t^3; vd2=a12+2*a22*t+3*a32*t^2; a03=q0(3);a13=0;a23=3*(qc(3)-q0(3))/tc^2;a33=-2*(qc(3)-q0(3))/tc^3; qd3=a03+a13*t+a23*t^2+a33*t^3; vd3=a13+2*a23*t+3*a33*t^2; qd=[qd1;qd2;qd3]; vd=[vd1;vd2;vd3]; end 12) Chương trình matlab phương trình động lực học robot function [x,v] = Roboth3(M,X0,tk) % thong so m1=5.2;m2=4.5;m3=2.1;d1=0.7;a1=0.3;a2=0.3;g=9.81;xc1=0.2;xc2=0.15;zc3=0.3; Iy1=0.39e-2;Iz1 = 0.39e-2;Iy2 = 0.3375e-1;Iz2 = 0.3375e-1; Ix3 = 0.63e-1; Iz3 = 0.63e-1; %bien dat M01=M(1); M02=M(2); M03=M(3); x11=X0(1);%x11=q1 x12=X0(2);%v1 x21=X0(3);%x21=q2 x22=X0(4);%v2 x31=X0(5);%q3 x32=X0(6);%v3 %ma tran M(q) m11=(-2*m2*a1*xc2+2*m2*a1*a2+2*m3*a2*a1)*cos(x21)+Iz1+Iz22*m2*xc2*a2+a2^2*m3+m2*xc2^2+m2*a2^2+m1*a1^2+m2*a1^2+m3*a1^2+m1*xc1^22*m1*xc1*a1; m12=(-m2*a1*xc2+m2*a1*a2+m3*a2*a1)*cos(x21)+m2*xc2^22*m2*xc2*a2+m2*a2^2+Iz2+a2^2*m3; m13=0; m21=m12; m22=m2*xc2^2-2*m2*xc2*a2+m2*a2^2+Iz2+a2^2*m3; m23=0; m31=0; m32=0; m33=m3; Mq=[m11,m12,m13;m21,m22,m23;m31,m32,m33]; %vector V(q,v) j1=(-2*m2*sin(x21)*a1*a2+2*m2*sin(x21)*xc2*a1-2*m3*sin(x21)*a1*a2)*x22*x12+(m3*sin(x21)*a1*a2+m2*sin(x21)*xc2*a1-m2*sin(x21)*a1*a2)*x22^2; j2=(m3*sin(x21)*a1*a2-m2*sin(x21)*xc2*a1+m2*sin(x21)*a1*a2)*x12^2; j3=0; V=[j1;j2;j3]; % G(q) g1=0; g2=0; g3=-m3*g; G=[g1;g2;g3]; %Minv=inv(Mq); 73 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI a=-inv(Mq)*(V+G)+inv(Mq)*[M01;M02;M03]; % tinh gan dung phuong trinh vi phan x11=x11+tk*x12; x21=x21+tk*x22; x31=x31+tk*x32; x12=x12+tk*a(1); x22=x22+tk*a(2); x32=x32+tk*a(3); X=[x11;x12;x21;x22;x31;x32]; x=[x11;x21;x31]; v=[x12;x22;x32]; end 13) Chương trình matlab mô với điều khiển PD không bù trọng lực % CHUONG TRINH MO PHONG ROBOT tc=2; s0=[0.2;0.1;0.1]; q0=DHNh3(s0); sc=[0.3;0.2;0.4]; qc=DHNh3(sc); %dieu kien dau q=q0;v=[0;0;0];tk=0.0001; X0=[q0(1);v(1);q0(2);v(2);q0(3);v(3)]; nhom06=fopen('nhom6.txt','w'); for t=0:0.0001:tc; s=DHTh3(q); [qd,vd]=quidaoh3(q0,qc,tc,t); [M,ssq,ssv]=DKh3(qd,vd,q,v); [q,v]=Roboth3(M,X0,tk); X0=[q(1);v(1);q(2);v(2);q(3);v(3)]; fprintf(nhom06,'%2.9f %2.9f %2.9f %2.9f %2.9f %2.9f %2.9f %3.9f %3.9f %2.9f %3.9f %3.9f %3.9f %3.9f %2.9f %2.9f %2.9f %2.9f %2.9f %2.9f %2.9f %2.9f\n',t,qd(1),q(1),qd(2),q(2),q(3),qd(3),ssq(1),ssq(2),ssq(3),M(1),M(2),M(3) ,vd(1),v(1),vd(2),v(2),vd(3),v(3),s(1),s(2),s(3)); end fclose(nhom06); fid = fopen('nhom6.txt', 'r'); a = fscanf(fid, '%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g', [22 inf]) fclose(fid); d1=a(1,:);%t d2=a(2,:);%qd1 d3=a(3,:);%q1 d4=a(4,:);%qd2 d5=a(5,:);%q2 d6=a(6,:);%qd3 d7=a(7,:);%q3 d8=a(8,:);%ssq1 d9=a(9,:);%ssq2 d10=a(10,:);%ssq3 d11=a(11,:);%M1 d12=a(12,:);%M2 d13=a(13,:);%M3 d14=a(14,:);%vd1 d15=a(15,:);%v1 d16=a(16,:);%vd2 d17=a(17,:);%v2 d18=a(18,:);%vd3 74 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI d19=a(19,:);%v3 d20=a(20,:);%x d21=a(21,:);%y d22=a(22,:);%z figure(1); plot(d1,d2,'c',d1,d3,'r ','LineWidth',2);xlabel('t(s)');ylabel('qd1 va q1(rad)'); legend('qd1','q1'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(2); plot(d1,d4,'c',d1,d5,'r ','LineWidth',2);xlabel('t(s)');ylabel('qd2 va q2 (rad)'); legend('qd2','q2'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(3); plot(d1,d6,'c',d1,d7,'r ','LineWidth',2);xlabel('t(s)');ylabel('qd3 va q3 (rad)'); legend('qd3','q3'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(4); plot(d1,d8,'b',d1,d9,'k',d1,d10,'r','LineWidth',2);xlabel('t(s)');ylabel('ssq1, ssq2 va ssq3 (rad)'); legend('ssq1','ssq2','ssq3'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(5); plot(d1,d11,'b',d1,d12,'k',d1,d13,'r','LineWidth',2);xlabel('t(s)');ylabel('M1 M2 va M3 (Nm)'); legend('M1','M2','M3'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(6); plot(d1,d14,'c',d1,d15,'r ','LineWidth',2);xlabel('t(s)');ylabel('vd1 va v1 (rad/s)'); legend('vd1','v1'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(7); plot(d1,d16,'c',d1,d17,'r ','LineWidth',2);xlabel('t(s)');ylabel('vd2 va v2 (rad/s)'); legend('vd2','v2'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(8); plot(d1,d18,'c',d1,d19,'r ','LineWidth',2);xlabel('t(s)');ylabel('vd3 va v3 (rad/s)'); legend('vd3','v3'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(9) plot3(d20,d21,d22,'g','LineWidth',2);title('quy dao');xlabel('x(m)');ylabel('y(m)'); legend('quy dao'); set(gcf,'Color',[1.0,1.0,1.0]); 75 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI grid on 14) Chương trình matlab mô với điều khiển PD có bù trọng lực % CHUONG TRINH MO PHONG ROBOT tc=3; s0=[0.2;0.1;0.1]; q0=DHNh3(s0); sc=[0.3;0.2;0.4]; qc=DHNh3(sc); %dieu kien dau q=q0;v=[0;0;0];tk=0.0001; X0=[q0(1);v(1);q0(2);v(2);q0(3);v(3)]; nhom06=fopen('nhom6.txt','w'); for t=0:0.0001:tc; s=DHTh3(q); [qd,vd]=quidaoh3(q0,qc,tc,t); [M,ssq,ssv]=DKGh3(qd,vd,q,v); [q,v]=Roboth3(M,X0,tk); X0=[q(1);v(1);q(2);v(2);q(3);v(3)]; fprintf(nhom06,'%2.9f %2.9f %2.9f %2.9f %2.9f %2.9f %2.9f %3.9f %3.9f %2.9f %3.9f %3.9f %3.9f %3.9f %2.9f %2.9f %2.9f %2.9f %2.9f %2.9f %2.9f %2.9f\n',t,qd(1),q(1),qd(2),q(2),q(3),qd(3),ssq(1),ssq(2),ssq(3),M(1),M(2),M(3) ,vd(1),v(1),vd(2),v(2),vd(3),v(3),s(1),s(2),s(3)); end fclose(nhom06); fid = fopen('nhom6.txt', 'r'); a = fscanf(fid, '%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g', [22 inf]) fclose(fid); d1=a(1,:);%t d2=a(2,:);%qd1 d3=a(3,:);%q1 d4=a(4,:);%qd2 d5=a(5,:);%q2 d6=a(6,:);%qd3 d7=a(7,:);%q3 d8=a(8,:);%ssq1 d9=a(9,:);%ssq2 d10=a(10,:);%ssq3 d11=a(11,:);%M1 d12=a(12,:);%M2 d13=a(13,:);%M3 d14=a(14,:);%vd1 d15=a(15,:);%v1 d16=a(16,:);%vd2 d17=a(17,:);%v2 d18=a(18,:);%vd3 d19=a(19,:);%v3 d20=a(20,:);%x d21=a(21,:);%y d22=a(22,:);%z figure(1); plot(d1,d2,'c',d1,d3,'r ','LineWidth',2);xlabel('t(s)');ylabel('qd1 va q1(rad)'); legend('qd1','q1'); set(gcf,'Color',[1.0,1.0,1.0]); grid on 76 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI figure(2); plot(d1,d4,'c',d1,d5,'r ','LineWidth',2);xlabel('t(s)');ylabel('qd2 va q2 (rad)'); legend('qd2','q2'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(3); plot(d1,d6,'c',d1,d7,'r ','LineWidth',2);xlabel('t(s)');ylabel('qd3 va q3 (rad)'); legend('qd3','q3'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(4); plot(d1,d8,'b',d1,d9,'k',d1,d10,'r','LineWidth',2);xlabel('t(s)');ylabel('ssq1, ssq2 va ssq3 (rad)'); legend('ssq1','ssq2','ssq3'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(5); plot(d1,d11,'b',d1,d12,'k',d1,d13,'r','LineWidth',2);xlabel('t(s)');ylabel('M1 M2 va M3 (Nm)'); legend('M1','M2','M3'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(6); plot(d1,d14,'c',d1,d15,'r ','LineWidth',2);xlabel('t(s)');ylabel('vd1 va v1 (rad/s)'); legend('vd1','v1'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(7); plot(d1,d16,'c',d1,d17,'r ','LineWidth',2);xlabel('t(s)');ylabel('vd2 va v2 (rad/s)'); legend('vd2','v2'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(8); plot(d1,d18,'c',d1,d19,'r ','LineWidth',2);xlabel('t(s)');ylabel('vd3 va v3 (rad/s)'); legend('vd3','v3'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(9) plot3(d20,d21,d22,'g','LineWidth',2);title('quy dao');xlabel('x(m)');ylabel('y(m)'); legend('quy dao'); set(gcf,'Color',[1.0,1.0,1.0]); grid on 77 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI 78 [...]... MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 2.3.5: Đồ thị kiểm nghiệm lại quỹ đạo điểm tác động cuối 21 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI PHẦN 3: TÍNH TOÁN LỰC 3.1 Phân tích trạng thái tĩnh Lực tác động vào khâu cuối coi như bằng không vì robot thiết kế là robot. .. 5: THIẾT KẾ HỆ DẪN ĐỘNG ROBOT 5.1 Thiết kế một hệ dẫn động cho một khớp Để thiết kế hệ dẫn động cho một khớp của robot, ta chọn khớp có lực/moment động cơ lớn nhất Dựa vào phân tích và tính toán ở phần 3 - tính toán lực, ta chọn khớp 3(khớp tịnh tiến) với lực động cơ bằng 20,601(N) để khảo sát và tính chọn hệ dẫn động Trên thực tế, hệ dẫn động robot thường sử dụng HGT bánh răng sóng, xycloit,… Nhưng... maple các kết quả ở dạng đồ thị có trong chương trình như sau: 29 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 4.3.1 Đồ thị q1 Hình 4.3.1 Đồ thị q2 30 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 4.3.1 Đồ thị q3 31 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM... 33 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI F3 = 25,2t – 20,601 Hình 4.4.1: Moment dẫn động khớp 1 34 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 4.4.2: Moment dẫn động khớp 2 Hình 4.4.3: Lực dẫn động khớp 3 35 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH... động cơ cần cho robot cân bằng từ các kết quả tính toán trên: – Mdc1 = 0M10 [3] = 0 (Nm) – Mdc2 = 0M21 [3] = 0 (Nm) – Fdc3 = 0F32 [3] = m3g = 2,1 9,81 = 20,601 (N) 24 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI PHẦN 4: TÍNH TOÁN ĐỘNG LỰC HỌC 4.1 Xây dựng cấu trúc động lực học và các thành phần cần thiết để viết phương trình động lực... điểm tác động cuối là đường xoắn ốc có phương trình như sau:  xE  0, 2  0,1 cos(5t )   y E  0, 2  0,1sin(5t )  z  0,5t  E 18 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI Hình 2.3.1: Quỹ đạo điểm tác động cuối Hình 2.3.2: Đồ thị q1 theo t 19 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG... TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI z E = – 8 cos(2t) cos(3t) + 12 sin(2t) sin(3t)  vE  x 2E  y 2E  z 2E  aE  x 2E  y 2E  z 2E x  0 y  0 z  3sin(3t )  9t.cos(3t )  10sin(5t ) Dùng maple ta vẽ được các đồ thị sau: Hình 2.2.2: Đồ thị ba góc cardan khâu cuối 15 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6... gia tốc điểm cuối Hình 2.2.4: Đồ thị vận tốc góc khâu cuối 16 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI 2.3 Giải bài toán động học ngược 2.3.1 Giải bằng phương pháp giải tích Khi giải bài toán động học thuận người ta xác định được quan hệ dước dạng ma trận như sau: x=f(q) Từ quan hệ đó ta suy ra một cách hình thức: q=f-1(x) trong đó:... vào công thức đã được đã được thiết lập trong bài giảng robot: 22 TIỂU LUẬN MÔN: TÍNH TOÁN THIẾT KẾ ROBOT - NHÓM 6 ĐH Bách Khoa Hà Nội GIÁO VIÊN HƯỚNG DẪN: PGS.TS.PHAN BÙI KHÔI 0 0 0   F32  FE 3  P3 0 0 0 0 0 0   M 32  M E 3  r3 F32  rc 3 P3   0  0    F32   0    m3 g  Ta tính được:    0  0  M   0 32     0    b) Xét khâu 2 (lấy kết quả của khâu 3):   a2C12... q=[q1, q2, q3]T Điểm tác động cuối robot chịu 1 lực FE  [-Fx ,  Fy ,  Fz ]T : 4.3 Động lực học thuận Với hệ phương trình động lực học đã thiết lập, và các số liệu đã thiết kế phần cơ khí và lực tác động khâu cuối coi như không có ta tích phân số hệ phương trình động lực học đó lên với các momen động cơ cho trước: Ở đây ta cho 3 momen dẫn động và các số liệu các tham số như sau :  M 1  70  3q1  