Đang tải... (xem toàn văn)
Báo cáo bài tập lớn kỹ thuật robot
Báo cáo tập lớn Kỹ thuật Robot BÀI a Xây dựng hệ tọa độ cho nối Ta có sơ đồ cánh tay Robot sau: Hình 1.1 Ký hiệu mơ hình Robot Robot hình 1.1 robot có cấu tay dạng cầu với khớp quay khớp tịnh tiến Hệ tọa độ nối xây dựng theo phương pháp biểu diễn D-H Từ sơ đồ cánh tay,ta gắn hệ trục tọa độ sau: Vị trí gốc tọa độ -Khâu số thân robot khâu cố định có gốc tọa độ O đặt khớp -Khung tọa độ có gốc O1 gắn vào khớp -Khung tọa độ có gốc O2 gắn vào khớp -Khung tọa độ có gốc O3 gắn vào khâu tác động đuôi Chiều khung tọa độ SV thực hiện: Page Báo cáo tập lớn Kỹ thuật Robot -Khung tọa độ số có trục Z0 trùng với trục khớp 1,trục X0 đặt theo phương pháp tuyến chung trục trục -Khung tọa độ số có trục Z trùng với trục khớp 2,trục X đặt đặt theo phương pháp tuyến chung trục trục -Khung tọa độ số có trục Z2 trùng với trục khớp 3,trục X2 đặt theo phương pháp tuyến chung trục trục -Khung tọa độ số có trục Z trùng với trục khớp 4,trục X đặt song song với trục X3 Từ ta xây dựng hệ trục tọa độ sau: Hình 1.2 Vị trí hướng khung tọa độ b Xác định ma trận T biểu diễn hệ tọa độ tay Robot Các khung tọa độ nối Robot thiết kế hình 1.2 Từ đó, ta xác định tham số theo biểu diễn D – H bảng sau : SV thực hiện: Thanh a2 Page 90o o -90 0 0 d3 Báo cáo tập lớn Kỹ thuật Robot Bảng 1.1 : Bảng D – H Robot Vị trí hai khâu liền kề mô tả ma trận biến đổi đồng i 1 Ai Cấu hình Robot Robot thu cách nhân số thích hợp ma trận i 1 Ai lại với Ma trận biểu diễn quan hệ hai khung tọa độ i-1 i i 1 Ai ( 1.2 ) Trong : Ta sử dụng ma trận ( 1.2 ) bảng 1.1 ta xác định ma trận thành phần : i 1 Ai biểu diễn quan hệ hai khung tọa độ hai khớp i-1, i A1 A = 2 = ANhư vậy, ta xác định ma trận T biểu diễn hệ trục tọa độ tay Robot : T3 A1.1 A2 A3 SV thực hiện: Page Báo cáo tập lớn Kỹ thuật Robot = T3 = Trong : c Ý nghĩa ma trận Ma trận T3 có hàng cột Biểu diễn khung tọa độ tay Robot khung tọa chuẩn Biểu diễn vị trí hướng tay Robot cột biểu diễn hướng tay Robot, cột thứ tư biểu diễn vị trí tay Robot + Cột thứ biểu diễn vectơ pháp tuyến n ( normal ) + Cột thứ hai biểu diễn vectơ có hướng mà theo ngón tay bàn tay nắm vào cần nắm đối tượng, gọi vectơ o (occupation) + Cột thứ ba biểu diễn vectơ có hướng mà theo bàn tay tiếp cận với đối tượng, gọi vectơ a ( approach ) Qua ma trận T người ta phần tích hoạt động lập trình điều khiển cho Robot Ma trận T có ý nghĩa lớn tốn động học thuận toán động học ngược: + Động học thuận: biết giá trị biến khớp thay đổi theo thời gian vị trí hướng tay Rotbot hoàn toàn xác định thời điểm + Động học ngược: biết vị trí hướng điểm tác động cuối ta hồn tồn xác định giá trị biến khớp từ việc giải hệ phương trình động học T SV thực hiện: Page Báo cáo tập lớn Kỹ thuật Robot d Vị trí tay Robot hệ tọa độ gốc + θ1 = 30o ; θ2 = 30o ; d3 = 0.3m; a2=0.4m + Vị trí tay Robot xác định ma trận T3 sau: = T3 = = + Vậy vị trí tay robot hệ tọa độ gốc là: p=[0,17 0,1 0,46 1]T SV thực hiện: Page Báo cáo tập lớn Kỹ thuật Robot Bài a, Xác định góc θ robot cuối hành trình chuyển động y r y2 m2 r1 y1 m1 x2 x1 Hình 2.1 Mơ hình Robot θ-r Tính tốn với số liệu sau: m1=m2=3.5 kg, r1=0.4m, rmax=0.8m, = ( rad/s ), =0.125m/s Theo đầu bài, ta có : = ( rad/s ) dθ = dt =0.125m/s => =0.125m/s Thời gian chuyển động khớp quay : SV thực hiện: Page Báo cáo tập lớn Kỹ thuật Robot t===3.2 Góccủa Robot cuối hành trình chuyển động = 3.2=(rad) b, Xác định Momen khớp quay lực tổng hợp khớp tịnh tiến Robot cuối hành trình chuyển động Dạng tổng quát phương trình động lực học: Trong : Mi Fi momen khớp thứ i lực tác dụng lên nối thứ i q biến khớp quay khớp tịnh tiến L hàm Lagrange Ki Pi lần lược động nối thứ i mi khối lượng vận tốc dài nối thứ i Ji momen quán tính nối thứ i trục qua tâm khối nối thứ i khớp quay nối thứ i Ta xác định giá trị Ki Pi với bước sau: Xác định động nối thứ Động nối thứ : K1 = m1 + J1 SV thực hiện: Page Báo cáo tập lớn Kỹ thuật Robot Vì J1 = nên ta có : K1 = m1 Theo hình 2.1, ta có : & �x& �x1 r1 cos � r1 sin � � � & �y1 r1 sin �y&1 r1 cos Do đó: v12 r12 &2 r12&2 K1 m1 (2.1) Thế nối thứ : P1 = m1gr1 sinθ (2.2) Xác định động nối thứ Động nối thứ : K2 = m2 + J2 Vì J1 = nên ta có : K2 = m2 Tương tự nối thứ 1: � �x2 r cos �x&2 r&cos r&sin �� � �y&2 r sin r&cos �y2 r sin Do đó: v22 r&2 r &2 r&2 r 2&2 K m2 SV thực hiện: 2.3 Page Báo cáo tập lớn Kỹ thuật Robot Thế nối thứ : P2 = m2gr sinθ (2.4) Từ (2.1) (2.3), ta có tổng động hai nối : K = K + K2 = ( 2.5 ) Từ (2.2) (2.4), ta có tổng hai nối : P = P1 + P2 = m1gr1 sinθ + m2gr sinθ Do vậy, từ (2.5) (2.6), ta có : Xác định momen khớp quay Robot θ - r Theo phương trình động lực học tổng quát : Đối với khớp quay q1 = θ, ta có : Với : Do : Theo đề bài, ta biết cuối hành trình chuyển động : rmax = 0.8m (rad) Do khớp tịnh tiến chuyển động với tốc độ không đổi SV thực hiện: Page ( 2.6 ) Báo cáo tập lớn Kỹ thuật Robot Do khớp quay chuyển động với tốc độ không đổi Thay số vào M1 , ta thu momen khớp quay Robot θ – r : M1= +2*3.5*0.8*0.125* + (3.5*0.4+3.5*0.8)*9.81*cos =33.47(N.m) Xác định lực tổng khớp tịnh tiến Robot θ - r Theo phương trình động lực học tổng quát : Đối với khớp tịnh tiến q2 = r, ta có : Với : Ta có: => Thay số vào F2 , ta thu lực tổng khớp tịnh tiến Robot cuối hành trình chuyển động : F2= 3.5*0- 3.5*0.8*+ 3.5*9.81*sin= 20.29(N) Vậy momen khớp quay lực tổng khớp tịnh tiến Robot cuối hành trình chuyển động là: M = 33.47 N.m F = 20.29 N c, Xác định điều khiển “ Momen tính tốn” cho Robot SV thực hiện: Page 10 Báo cáo tập lớn Kỹ thuật Robot (2-2) Từ (2-1) (2-2) ta có: = (2-3) Thiết kế luật điều khiển P-D cho Robot ta có: (2-4) Từ (2-3) (2-4) có: + =0 Laplace hóa hai vế phương trình thu phương trình sau: + Kdp + Kp=0 (2-5) Mong muốn hệ thống có đáp ứng khâu dao động bậc có dạng sau: n p+n=0 (2-6) Trong : - Hệ số suy giảm n - Tần số giao động Cân phương trình (2-5) (2-6) thu : Kdi=n== Kpi=n2=)2 SV thực hiện: Page 12 Báo cáo tập lớn Kỹ thuật Robot Hình 2.3 : Sơ đồ cấu trúc hệ thống điều khiển mô men tính tốn Chọn chất lượng điều khiển có hệ số suy giảm =0.7 thời gian độ tqđ=1s thu tham số điều khiển khớp i sau: Kd = Kp = (2=32,65 Trong điều khiển thiết kế cho hai khớp giống nên thông số điều khiển hai khớp Suy hệ số điều khiển: d, Mô hệ thống Thiết kế quỹ đạo chuyển động cho hai khớp dạng 2-1-2 Để đảm bảo tay Robot di chuyển từ vị trí ban đầu A(x0,y0) đến vị trí cuối B(xc,yc) khoảng thời gian tc (s) ta tính tốn quỹ đạo sau: SV thực hiện: Page 13 Báo cáo tập lớn Kỹ thuật Robot Hình 2.4 : Dạng quỹ đạo 2-1-2 Khi Robot di chuyển từ A tới B xảy trình: - Đầu tiên q trình khởi động có dạng bậc - Q trình chuyển động đỳ có dạng đường thẳng bậc - Q trình hãm có dạng đường cong bậc Ta giả thuyết SV thực hiện: q&0 � � q&c � Mặt khác ta có: Page 14 Báo cáo tập lớn Kỹ thuật Robot � tc q q0 tm ; qm c � 2 � qm q1 � & q&1 q& � 1.t1 tm t1 � � & q1 q0 q& � 1t1 & &1.tc t1 qc q0 � q& � 1.t1 q tc tc2 qc q0 � t1 m & q& Từ đồ thị ta nhận thấy t1 < tc /2 (do ta mong muốn chọn vậy) Do ta có: tc tc2 qc q0 t1 & q& (*) tc2 qc q0 & q& Biểu thức có nghĩa hàm dấu lớn không Vậy: > 0 & & & q& 1cp �q1 qc q0 tc2 (**) Giới hạn gia tốc phụ thuộc vào độ bền khí Robot Như thấy cách thiết kế quĩ đạo sau: & nằm khoảng theo biểu thức (**) Chọn q& & , tc, qc, q0 ta tính t1 theo (*) Biết q& Khi quĩ đạo khoảng thời gian sau: SV thực hiện: Page 15 Báo cáo tập lớn Kỹ thuật Robot � & t : q ( t ) q q& 1t � � t1 � & t1 t2 : q(t ) q0 q& ) � 1t1 (t � � & t t : q ( t ) q q& c 1t1 (t tc ) � � Trong : & nằm khoảng theo biểu thức (**) - Chọn q& & ,tc,qc,q0 ta tính t1 theo (*) - Biết q& Kết mô Hình 2.5: Các tín hiệu thu từ mơ SV thực hiện: Page 16 Báo cáo tập lớn Kỹ thuật Robot Nhận xét: Góc quay biến khớp θ r bám sát quỹ đạo 2-1-2 cho trước với sai số nhỏ khớp thực tới điểm dùng cuối %====================================== % CHUONG TRINH CHINH %======================================= % khai bao gia tri bo dieu khien Kp = 32.65 Kd = % goi ham RobotThetaR [At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR(Kp,Kd) % ======================================== % thi tin hieu thu duoc %========================================= % ve mo men va luc cua robot tren cung thi plot(At1,AM1,'r')% mo mem khop quay (khop 1) text (1,11,'mo men khop 1') hold on plot(At1,AM2)% luc cua khop tinh tien (khop 2) SV thực hiện: Page 17 Báo cáo tập lớn Kỹ thuật Robot text(4,11,'luc khop 2') grid % ve sai so goc quay va tinh tien tren cung thi plot(At1,AeTheta,'r') % mo mem khop quay (khop 1) text (2.5,-0.11,' khop quay') hold on plot(At1,Aer) % luc cua khop tinh tien (khop 2) text(2,-0.036,'khop tinh tien') grid % ve goc quay khop plot(At1,Aq1) grid % dich chuyen cua khop tinh tien plot(At1,Aq2) grid Xây dựng hàm + hàm RobotThetaR function[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] =RobotThetaR(Kp,Kd) tc = 3.2;%Dat thoi gian di chuyen cua tay Robot m1 = 3.5 ;%Khoi luong SV thực hiện: Page 18 Báo cáo tập lớn Kỹ thuật Robot m2 = 3.5 ;%Khoi luong r1 = 0.4 ;%Chieu dai noi %=== Vi tri cua tay ban dau` va` cuoi == S0 = [0.5 0]; % r1=0,5 m Sc = [0 1]; % rmax =1 m %Chuyen vi tri ban dau cua tay Robot sang vi tri cac khop q0 = DHN(S0); qc = DHN(Sc); %Khac phuc truong hop chia If (qc (2)==q0(2)); qc (2) = qc(2)-0.0001; end if(qc(1)==q0(1)); qc(1) = qc(1)-0.0001; end %Tinh toan gia toc hai khop tinh tien va quay ddq1 = 1.3*4*abs(qc(1)-q0(1))/tc^2; %Gia toc khop quay ddq2 = 1.3*4*abs(qc(2)-q0(2))/tc^2; %Gia toc khop tinh tien %Xac dinh cac khoang thoi gian chuyen dong tang toc, deu va giam toc cho%cac khop t11 = tc/2 - sqrt((tc^2*ddq1-4*(qc(1)-q0(1)))/ddq1)/2; %Thoi gian tang toc SV thực hiện: Page 19 Báo cáo tập lớn Kỹ thuật Robot t21 = tc - t11; %t21 - t11/2 se la thoi gian chuyen dong deu, tc-t21 se la %thoi gian giam toc ve Tuc la thoi gian tang va giam toc % deu bang t11/2 t12 = tc/2 - sqrt((tc^2*ddq2-4*(qc(2)-q0(2)))/ddq2)/2;%Thoi gian tang toc t22 = tc - t12; Tk = 0.01; %Sau Tk(s) ta se tinh toan cac tham so cua robot %Dieu kien ban dau (So kien) q = q0; dq = [0 ;0]; X0 = [q0(1);dq(1);q0(2);dq(2)];%So kien bien trang thai X file1 = fopen('RobotThetaR.txt','w'); i = ; %Bien dung de dem for t = 0:0.001:tc;i = i+1; At1(i)=t; %Lay thoi gian de ve thi %Tinh toan gia tri dat cho cac khop tung khoang thoi gian chuyen %dong [qd1, dqd1] = quiDaoKhopThetaR(q0(1),qc(1),ddq1,t11,t21,tc,t);%dq d1 la van toc cua khop 1, ddq1 la gia tockhop [qd2, dqd2] = quiDaoKhopThetaR(q0(2),qc(2),ddq2,t12,t22,tc,t);qd = [qd1; qd2]; dqd = [dqd1; dqd2]; G11 = (m1*r1 + m2*X0(3))*9.81*cos(X0(1)); SV thực hiện: Page 20 Báo cáo tập lớn Kỹ thuật Robot G12 = m2*9.81*sin(X0(1)); G1 = [G11;G12]; v1 = 2*m2*X0(3)*X0(4)*X0(2); v2 = -m2*X0(4)*X0(1)*X0(1); V1 = [v1;v2]; H11 = m1*r1^2 + m2*X0(3)^2;%r = x21 H = [H11, H12; H21, H22]; %Tinh momen can thiet de thuc hien chuyen dong [M,fe] = ControllerPD(qd,dqd,q,dq,Kp,Kd,G1,V1,H); %Lay thong so qui dao thuc robot chuyen dong duoc qua M [q, dq] = RobotModel(M,X0,Tk);%Lay cac thong so de ve thi % -qdd1(i)= qd(1); qdd2(i) = qd(2); Aq1(i) = q(1); %Goc quay khop quay Aq2(i) = q(2); %r Adq1(i) = dq(1); %Toc khop quay Adq2(i) = dq(2); %Toc khop tinh tien AM1(i) = M(1); %Mo men khop quay AM2(i) = M(2) ; %Luc truyen dong cho khop tinh tien AeTheta (i) = qd(1) - q(1);%Sai lech goc quay SV thực hiện: Page 21 Báo cáo tập lớn Kỹ thuật Robot Aer(i) = qd(2) - q(2);%Sai lech chuyen dong tinh tien % -X0 = [q(1); dq(1); q(2); dq(2)];%Dat lai so kien cho lan tinh sau%q(1)-> Theta; q(2)->r % plot(At1,q(1)) %Luu du lieu vao file fprintf(file1,'%2.4f%2.4f%2.4f%2.4f%2.4f%3.4f %3.4f\n',t,qd(1),q(1),qd(2),q(2),M(1),M(2)); end fclose(file1); + hàm RobotModel : function[q,dq] = RobotModel(M,X0,Tk) % M01 = M(1);%Mo men dieu khien cho khop quay F02 = M(2);%Luc dieu khien cho khop tinh tien x11 = X0(1);%Goc Theta1 x12 = X0(2);%Toc goc khop quay x21 = X0(3);%Gia tri r x22 = X0(4);%Toc khop tinh tien % %Cac thong so cua Robot m1 = 3.5 ;%Khoi luong m2 = 3.5 ;%Khoi luong SV thực hiện: Page 22 Báo cáo tập lớn Kỹ thuật Robot r1 = 0.4;%Chieu dai noi % C1 = cos(x11); S1 = sin(x11); % %Ma tran quan tinh H11 = m1*r1^2 + m2*x21^2; %r = x21 H12 = 0; H21 = 0; H22 = m2; H = [H11, H12; H21, H22]; %Momen nhot va momen huong tam v1 = 2*m2*x21*x22*x12; v2 = -m2*x22*x12*x12; V = [v1;v2]; %Momen luc G1 = (m1*r1 + m2*x21)*9.81*C1; G2 = m2*9.81*S1; G = [G1;G2]; %Nghich dao ma tran H Hinv = inv(H); SV thực hiện: Page 23 Báo cáo tập lớn Kỹ thuật Robot %Tinh toan gia toc khop tu phuong trinh dong luc hoc dang nguoc dX = -Hinv * (V+G) + Hinv*[M01;F02]; %Phuong trinh trang thai x11p = x12;%Toc khop quay x21p = x22;%Toc khop tinh tien x12p = dX(1);%Gia toc khop quay x22p = dX(2);%Gia toc khop tinh tien %Tinh gan dung phuong trinh vi phan x11 = x11 + Tk * x11p; x21 = x21 + Tk * x21p; x12 = x12 + Tk * x12p; x22 = x22 + Tk * x22p; X = [x11;x12;x21;x22]; %Tinh vi tri tay Robot s = [x21; 0]; q = [x11; x21]; dq = [x12; x22]; hàm quiDaoKhopThetaR: function[q,dq] = quiDaoKhopThetaR(q0,qc,ddq,t1,t2,tc,t) iftt1) & (tt2)&(t