Thiết kế robot Scara

BÀI TẬP LỚN MÔN : RÔ BỐT CÔNG NGHIỆP ĐỀ TÀI : TÌNH TOÁN VÀ THIẾT KẾ RÔ BỐT SCARA Giáo viên hướng dẫn : KHỔNG MINH Sinh viên thực : Nhóm 16 Lớp : PHAN VĂN HUYNH Mã sv : 0741020238 ĐINH HÙNG SƠN Mã sv : 0741020194 LÊ BÁ KHÁNH Mã sv : 0741020245 ĐH Cơ điện tử – k7 • Mục lục Chương I : Tĩnh học động học tay máy 1.1 Tình toán kích thước khâu tay máy 1.2 Động học thuận tay máy 1.3 Động học ngược tay máy 1.4 Xác định quy luật chuyển động khớp Chương II : Động lực học tay máy 2.1 Phân tích lực tác động lên tay máy 2.2 Tính động 2.3 Tính 2.4 Thiết lập phương trình vi phân chuyển động 2.5 Xác định quy luật biến thiên momen lực khớp A NỘI DUNG Chương I Tĩnh học động học tay máy • Bảng thông số bản: t(s) 4,5 mp(g) 145 pg1(độ) 0,001 zp1(mm) 325 pg2(độ) 0,001 zp2(mm) 315 pg3(mm) 0,1 xp1(mm) 775 pg4(độ) 0,025 xp2(mm) 575 xp(mm) 35 yp1(mm) 675 yp(mm) 35 yp2(mm) 625 zp(mm) 35 anpha(độ) 180 Chú ý : Thời gian gắp phôi từ vị trí sang vị trí : t(s) Độ phân giải điều khiển khớp là: pg1, pg2, pg3, pg4 Phôi có hình hộp chữ nhật với kích thước xp, yp, zp khối lượng mp zp1, zp2 khoảng cách từ tay kẹp tới băng tải 1, xp1,yp1 tọa độ điểm đặt phôi băng tải xp2,yp2 tọa độ điểm đặt phôi băng tải 1.1 Tính toán kích thước khâu tay máy yp1 Robot xp1 xp2 yp2 α Ta có: xp1 + yp1 = 775 + 675 = 1450 = 1027 mm xp2 + yp2 = 575 + 625 = 1200 = Ta có điều kiện : 849 849 mm + 1027 ; = 325 mm Vậy ta chọn : Giới hạn chuyển động khâu : Khâu : -90 Khâu : -120 Khâu : Khâu : -90 +90 +120 350 mm +90 Hình 1: hình vẽ minh họa robot scara bậc tự + Bán kính công tác tay máy: Rmax = 980 mm + Chọn l1 =560 mm; l2 = 420 mm; d3max = 350mm + Chiều dài , robot: = 500 mm = 150 mm + Vậy giới hạn chuyển động cuả khâu: - Khâu 1: , - Khâu 2: -900≤θ1≤ +900 -1200≤θ2≤ +1200 , , , - Khâu 3: ≤ d3≤ 350 mm - Khâu 4: -90 ≤θ1≤ +90 , 1.2 Động học thuận tay máy , , , , Hình 2: Hệ tọa độ gắn khâu robot • Xây dựng bảng thông số động học DH BẢNG THÔNG SỐ DENAVIT - HARTENBERG (DH) Khâu di 0 qi i 0 180 0 Viết ma trận Ta quy ước: = = ; ; = = ; = ; = ; ; = ; = = ; = Phương trình động học rô bốt công nghiệp = Tính toán: = = = = = = = Vậy = (1.1) Mặt khác theo ký hiệu tổng quát: = • Ta có n, o, a, p vectơ định vị, vectơ định hướng, vectơ tới vectơ vị trí để biểu diễn hướng vị trí tay máy không gian làm việc Từ ma trận ta có hệ phương trình động học thuận tay máy robot: = = Suy ra: = = = =- =0 = = ==- =0 =0 =0 = -1 Hệ phương trình xác định vị trí điểm tác động cuối sau: x= = y= = z= = Với : thay vào hệ ta x= = 420 y= = 420 z= = 500 + 560 + 560 1.3 Động học ngược tay máy Ma trận mô tả vị trí hướng khâu thứ tư so với giá cố định = Ta có : Khi ; = (I) Ta có : Ta có : - phương trình viết lại sau: (3) (4) -từ phương trình (3): (5) -từ (3,4,5) ta có : - Theo ma trận ta có: = = - Vậy ta có hệ phương trình động học ngược robot là: (6) 1.4 Xác định quy luật chuyển đông của khớp Quỹ đạo khớp robot biểu diễn theo quỹ đạo bậc : - Phương trình vị trí : q(t) = a3t3 + a2t2+ a1t + a0 - Phương trình vận tốc : (t) = 3a3t2 + 2a2t + a1 - Phương trình giatốc : (t) = 6a3t + 2a2 Như để xác định quy luật chuyển động khớp, thông số giả định a 3, a2, a1, a0 cần phải xác định Để xác định thông số này, cần có điều kiện đầu, thường vị trí đầu qAvà vị trí cuối qB, vận tốc đầu A vận tốc cuối B Từ hệ phương trình động học ngược (II) ta tính vị trí đầu khớp: - Tại vị trí đầu: tọa độ điểm tác động đầu A(775, 575 , -325) Khi q1A = 27,9 , q2A = 20,27 , q3A = 475 , q4A = Tại vị trí cuối: tọa độ điểm tác động cuối B(675, 625, -315) Khi q1B = 47,9 , q2B = 40,77 , q3B = 465, q4B = 180 Chọn vận tốc đầu vận tốc cuối khớp bằng 0: 1A = 1B = 2A = 2B = 3A = 3B = 4A = 4B =0 Lúc hệ số giả định xác định từ hệ phương trình: 10 % tinh goc quay q1,q2 tai vi tri dau,cuoi c2A = (px1^2+py1^2-l1^2-l2^2)/(2*l1*l2); s2A = sqrt(1-c2A^2); q2A = atan(s2A/c2A); q1A = (atan(py1/px1)-atan((l2*s2A)/(l1+l2*c2A))); c2B = (px2^2+py2^2-l1^2-l2^2)/(2*l1*l2); s2B = sqrt(1-c2B^2); q2B = atan(s2B/c2B); q1B = ((pi+atan(py2/px2))-atan((l2*s2B)/(l1+l2*c2B))); format short %xac dinh quy luat chuyen dong cac khop a = [0 0 s^3 s^2 s 0010 3*s^2 2*s 0]; b1 = [q1A;q1B;qd1A;qd1B]; b2 = [q2A;q2B;qd2A;qd2B]; b3 = [q3A;q3B;qd3A;qd3B]; b4 = [q4A;q4B;qd4A;qd4B]; x1 = a^(-1)*b1; x2 = a^(-1)*b2; x3 = a^(-1)*b3; x4 = a^(-1)*b4; t = linspace(0,s,e); y1 = x1'; q1 = polyval(y1,t); yd1 = polyder(y1); qd1 = polyval(yd1,t); ydd1 = polyder(yd1); qdd1 = polyval(ydd1,t); y2 = x2'; q2 = polyval(y2,t); yd2 = polyder(y2); qd2 = polyval(yd2,t); ydd2 = polyder(yd2); qdd2 = polyval(ydd2,t); y3 = x3'; q3 = polyval(y3,t); yd3 = polyder(y3); qd3 = polyval(yd3,t); ydd3 = polyder(yd3); qdd3 = polyval(ydd3,t); y4 = x4'; q4 = polyval(y4,t); yd4 = polyder(y4); qd4 = polyval(yd4,t); 13 ydd4 = polyder(yd4); qdd4 = polyval(ydd4,t); format rat %doi voi khop tinh tien q3 plot(t,q3,'g') figure plot(t,qd3,'g') figure plot(t,qdd3,'g') figure %doi voi cac lhop quay q1, q2, q4 plot(t,q1,'r',t,q2,'y',t,q4,'k') title('do thi vi tri') xlabel(' truc x(s)') ylabel('truc y(rad)') grid on figure plot(t,qd1,'r',t,qd2,'y',t,qd4,'k') title('do thi van toc') xlabel(' truc x(s)') ylabel('truc y(rad/s)') grid on figure plot(t,qdd1,'r',t,qdd2,'y',t,qdd4,'k') title('do thi gia toc') xlabel(' truc x(s)') ylabel('truc y(rad/s^2)') grid on %xac dinh vi tri diem tac dong cuoi q=q1+q2; figure x0=0;y0=0;z0=0;plot3(x0,y0,z0,'o') grid on x00 = zeros(1,300); y00 = zeros(1,300); z00 = zeros(1,300); x11 = l1.*cos(q1);y11 = l1.*sin(q1);z11= zeros(1,300); x22 = (l1.*cos(q1)+l2.*cos(q));y22 = (l1.*sin(q1)+l2.*sin(q));z22=zeros(1,300); x33 = (l1.*cos(q1)+l2.*cos(q));y33 = (l1.*sin(q1)+l2.*sin(q));z33=-q3; hold on for i=1:5:300 P11=[x00(i) x11(i)]; P21=[y00(i) y11(i)]; P31=[z00(i) z11(i)]; plot3(P11,P21,P31,'-o') title('khong gian lam viec') hold on P12=[x11(i) x22(i)]; 14 P22=[y11(i) y22(i)]; P32=[z11(i) z22(i)]; plot3(P12,P22,P32,'-*') title('khong gian lam viec') hold on P13=[x22(i) x33(i)]; P23=[y22(i) y33(i)]; P33=[z22(i) z33(i)]; plot3(P13,P23,P33,'-+') title('khong gian lam viec') xlabel('truc x') ylabel('truc y') zlabel('truc z') end 15 16 Chương II Động lực học tay máy 2.1 Phân tích lực tác động lên tay máy , , , , , , , , , 17 • Động lực học robot Scara - Bảng thông số robot Scara bậc tự do: Khớp Khớp Khớp Khớp Biến Chiều dài Khối lượng Vận tốc Chiều dài tâm khối 0 * Giả sử khối lượng nằm đầu nối, ta có chiều dài tâm khối chiều dài nối: ; ; Hệ quy chiếu gắn với trục tọa độ ( ) khớp thứ Khi mặt phẳng ( ) mặt phẳng đẳng 2.2,3Tính toán động cho khớp *) Khớp 1: Trong đó: , , hình chiếu nối số lên trục x, y, z 18 → → Trong đó: mô men quán tính khớp =0 = + → *) Khớp 2: → → 19 → =0 = + → *) Khớp 3: → → → = 20 → *) Khớp 4: 2.4 phương trinh vi phân chuyển động Như phương trình động lực học hệ thống biểu diễn bằng phương trình sau: +2 .cos ) cos ] + + - sin - sin cos ) + + sin Trong đó: 21 : khối lượng tải nối với khớp : khối lượng khớp ; với : momen quán tính khớp : momen quán tải nối với khớp Với = momen động khớp quay 1,2,4 động lực đặt lên khớp tịnh tiến Nếu chỉ xét momen động với khớp quay viết gọn lại ba phương trình động lực học (2.61), (2.62),(2.64) để tiện cho trình tính toán sau: = Hay : 22 = Các thành phần phương trình động lực học xác định: = cos - sin = = 2.5 Matlap mômen lực q1A = 0; q1B =pi; qd1A = 0; qd1B = 0; q2A = pi/9; q2B = 2*pi/9; qd2A = 0; qd2B = 0; q3A = 0; q3B = 0; qd3A = 0; qd3B = 0; q4A = 0; q4B = pi; qd4A = 0; qd4B = 0; s = 4,5; e=450; m1=9.5;m2=7;m3=3.5;m40=2.5;mt=1;J1=19;J2=14;J3=0;J40=5;Jt=2;l1=0.56;l2=0.42 ;l3=0.35;l4=0.15;g=10;r1=0.23;r2=0.630;r3=0.87;r4=0.87; 23 %/////////////////////////////////////////////////////////////// a = [0 0 s^3 s^2 s 0010 3*s^2 2*s 0]; b1 = [q1A;q1B;qd1A;qd1B]; b2 = [q2A;q2B;qd2A;qd2B]; b3 = [q3A;q3B;qd3A;qd3B]; b4 = [q4A;q4B;qd4A;qd4B]; x1 = a^(-1)*b1; x2 = a^(-1)*b2; x3 = a^(-1)*b3; x4 = a^(-1)*b4; t = linspace(0,s,e); y1 = x1'; q1 = polyval(y1,t); yd1 = polyder(y1); qd1 = polyval(yd1,t); ydd1 = polyder(yd1); qdd1 = polyval(ydd1,t); y2 = x2'; q2 = polyval(y2,t); yd2 = polyder(y2); qd2 = polyval(yd2,t); ydd2 = polyder(yd2); qdd2 = polyval(ydd2,t); y3 = x3'; q3 = polyval(y3,t); yd3 = polyder(y3); qd3 = polyval(yd3,t); ydd3 = polyder(yd3); qdd3 = polyval(ydd3,t); y4 = x4'; q4= polyval(y4,t); yd4 = polyder(y4); qd4= polyval(yd4,t); ydd4 = polyder(yd4); qdd4 = polyval(ydd4,t); q12=q1+q2; q123=q1+q2+q3; q1234=q1+q2+q3; x1 = 0.23*cos(q1); y1 = 0.23*sin(q1); x2 = (0.23*cos(q1)+0.63*cos(q12)); y2 = (0.23*sin(q1)+0.63*sin(q12)); x3 = (0.23*cos(q1)+0.63*cos(q12)+0.87*cos(q123)); 24 y3 = (0.23*sin(q1)+0.63*sin(q12)+0.87*sin(q123)); x4 = (0.23*cos(q1)+0.63*cos(q12)+0.87*cos(q123)+0.87*cos(q1234)); y4 = (0.23*sin(q1)+0.63*sin(q12)+0.87*sin(q123)+0.87*sin(q1234)); plot(x1,y1) hold on plot(x2,y2) hold on plot(x3,y3) hold on plot(x4,y4) title('vi tri diem tac dong cuoi') grid on m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; m4=m40+mt; J124=J1+J2+J4; J24=J2+J4; J4=J40+Jt; M1=(m1234*l1^2+m123*l2^2+J124+2*m234*l1*l2*cosq2)*dq1+ ((m234*l2^2+J24+m234*l1*l2*cosq2)*ddq2+J4*dq4-m234*l1*l2*sinq2*dq2^22*m234*l1*l2*sinq2*q1^2); figure plot(M1,r) grid on title('momen dong co 1') M2=(m234*l2^2+J24+m234*l1*l2*cosq2)*ddq1+J4*dq4+ ((m234*l2^2+J24)*ddq2+J4*ddq4+(m234*l1*l2*sinq2*dq1^2)); figure plot(M2,r) grid on title('momen dong co 2') M3=m34*ddq3-m34*g; figure plot(M3,r) grid on title('momen dong co 3') M4=J4*ddq1+J4*ddq2+J4*ddq4; figure plot(M4,r) grid on 25 title('momen dong co 4') 26 27 [...]... lam viec') xlabel('truc x') ylabel('truc y') zlabel('truc z') end 15 16 Chương II Động lực học tay máy 2.1 Phân tích các lực tác động lên tay máy , , , , , , , , , 17 • Động lực học robot Scara - Bảng thông số của robot Scara 4 bậc tự do: Khớp 1 Khớp 2 Khớp 3 Khớp 4 Biến Chiều dài Khối lượng Vận tốc Chiều dài tâm khối 0 0 * Giả sử khối lượng nằm ở đầu các thanh nối, ta có chiều dài tâm khối chính là... luật chuyển động của biến khớp 3 là : Đối với biến khớp 4 ta có: , , , , , Thay vào hệ (III) ta được : Vậy quy luật chuyển động của biến khớp 4 là : * Sử dụng Matlab mô phỏng việc điều khiển các khớp robot : +) Code matlab : qd1A = 0; qd1B = 0;% gia toc khop 1 qd2A = 0; qd2B = 0;% gia toc khop 2 q3A = 475; q3B = 465; qd3A = 0; qd3B = 0;%vi tri, gia toc khop 3 q4A = 0; q4B = (pi); qd4A = 0; qd4B = 0;%vi ... máy 2.1 Phân tích lực tác động lên tay máy , , , , , , , , , 17 • Động lực học robot Scara - Bảng thông số robot Scara bậc tự do: Khớp Khớp Khớp Khớp Biến Chiều dài Khối lượng Vận tốc Chiều dài... 350 mm +90 Hình 1: hình vẽ minh họa robot scara bậc tự + Bán kính công tác tay máy: Rmax = 980 mm + Chọn l1 =560 mm; l2 = 420 mm; d3max = 350mm + Chiều dài , robot: = 500 mm = 150 mm + Vậy giới... trận ta có: = = - Vậy ta có hệ phương trình động học ngược robot là: (6) 1.4 Xác định quy luật chuyển đông của khớp Quỹ đạo khớp robot biểu diễn theo quỹ đạo bậc : - Phương trình vị trí :