Bài tập lớn robot công nghiệp
BÁCH KHOA HÀ NỘI BÀI TẬP LỚN ROBOT CÔNG NGHIỆP BÀI TẬP LƠN ROBOT CÔNG NGHIỆP TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI ============ BỘ MÔN TỰ ĐỘNG HÓA ĐỀ BÀI TOÁN Cho robot có cấu hình vẽ a b c d Xây dựng hệ tọa độ cho nối Xác định ma trận T biểu diễn hệ tọa độ tay robot Giải thích ý nghĩa ma trận T Xác định vị trí ta Robot hệ tọa độ gốc θ1=300 , d2 = 0,2m , d3 = 0,1m Hình 1.1 kí hiệu minh họa cấu Robot BÀI TOÁN Cho Robot θ– r có r1 = 0,5 m ; m1 = m2 = 1Kg Khớp tịnh tiến chuyển động với tốc độ r = 0,1 m/s từ r1 đến rmax = m Robot quay từ góc ban đầu 00 đến góc 600 a Hãy xác định mô mem 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 b Thiết kế điều khiển “mô mem tính toán ” cho Robot c Mô hệ thống BÀI TOÁN TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP 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 minh họa cấu Robot 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 thứ - Khung tọa độ có gốc O2 gắn vào khớp thứ - Khung tọa độ có gốc O3 gắn vào khâu tác động cuối • Chiều khung tọa độ : - Khung tọa độ số có trục Z0 trùng với trục khớp 1, trục X0 chọn vuông góc với mặt giấy - Khung tọa độ số có trục Z trùng với trục khớp số trục X chọn vuông góc với mặt giấy - Khung tọa độ số có trục Z trùng với trục khớp số trục X chọn vuông góc với mặt giấy Khung tọa độ số có trục Z3 trùng với phương trục Z2 trục X3 vuông góc với mặt giấy - Trục Y0, Y1, Y2, Y3 xác định theo quay tắc bàn tay phải Từ ta xây dựng hệ trục tọa độ sau: TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP 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 Từ hình 1.2 lập bảng D-H tham số khâu sau: KTĐ K1 K2 K3 θi θ1 0 di d2 d3 αi - 900 0 Vị trí hai khâu liền kề mô tả ma trận biến đổi đồng ii-1 Ai Ma trận 0A1 mô tả quan hệ khâu khâu (khâu cố định gắn với thân Robot ) , Ma trận 1A2 mô tả quan hệ khâu khâu 1, Ma trận 2A3 mô tả quan hệ khâu khâu Cấu hình khâu mộtt Robot thu cách nhân số thích hợp ma trận i-1Ai lại với Dạng tổng quát ma trận i-1Ai sau: Cθi Sθ i −1 Ai = i − Sθi Cα i CθiCα i Sα i Sθ i Sα i −Cθ i Sα i Cα i Cθ i Sθ i (1.2) di Áp dụng (1.2) để tính toán ma trân i-1Ai với tham số cho bảng D-H sau: TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP Cθ1 − Sθ1 Sθ Cθ1 A1 = 0 1 0 A2 = 0 −1 0 1 0 A3 = 0 0 0 0 0 1 0 d2 1 0 0 d3 1 Từ thu ma trận T sau: T = A11 A2 A3 Cθ1 Sθ = − Sθ1 Cθ1 Sθ = 0 − Sθ1 Cθ1 −1 0 Cθ1 0 0 1 0 0 0 −1 0 0 1 d 0 0 0 0 0 d3 1 −d3 Sθ1 d 3Cθ1 d2 c Giải thích ý nghĩa ma trận T Ma trận T có dạng tổng quát sau: nX n T = y nz 0 oX oy oz aX ay az pX p y pz Trong đó: - Véc tơ n , o, a xác định hướng khung tọa độ tay so với khung tọa độ cố định , - Véc tơ p biểu diễn vị trí khung tọa độ tay so với khung tọa độ gốc • 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 TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP • - Ma trận T có ý nghĩa lớn toá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ị ví hướng tay Robot 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 hoàn toàn xác định giá trị biến khớp từ việc giải hệ phương trình động học T d Xác định vị trí Robot hệ tọa độ gốc �1=300 , d2 = 0,2[m] , d3 = 0,1 [m] Khi robot quay góc θ1 = 300 chuyển động tịnh tiến đoạn d 2= 0,2m d3 = 0,1 m ta có ma trận T có giá trị sau: C 300 S 300 T = = 2 − S 300 C 300 −1 0 0 − −1 0 0 −0,1.S 300 0,1.C 300 0, −1 20 3 20 Vậy vị trí gốc tọa độ gắn lên tay có vị trí sau: −1 20 uv p = 20 BÀI TOÁN TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP a Xác định mô mem khớp quay lực tổng khớp tịnh tiến Robot cuối hành trình chuyển động Ta có mô hình Robot θ– r sau : θ X Y Y2 r B Y1 A r1 X1 X2 Hình 2.1 mô hình robot θ-r Để xây dựng phương trình động lực học Robot θ-r để đơn giản giả thuyết: - Khối lượng m1 xilanh tập trung điểm cuối xilanh tức điểm A - Khối lượng m2 pittong tập trung bàn tay Robot tức tập trung điểm B - Mô mem quán tính khớp Ji = TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP Trình tự xây dựng phương trình động lực học saU: Xác định hàm lagrange Phương trình lagrange cấu xác định sau: L=K–P Trong : - K : Tổng động hệ thống - P : Tổng hệ thống Với khớp quay ta có phương trình động lực học sau: Với khớp tịnh tiến ta có phương trình động lực học sau: • Xác định động hệ thống Động hệ thống : K = K1 + K2 Trong đó: TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP K1 – động khớp K2 – động khớp Theo hình 2.1 ta có: - Động khớp 1: Vị trí điểm A hệ tọa độ sau: X = r1.cosθ Y1 = r1.sinθ Đạo hàm hai vế theo thời gian thu thành phần vận tốc theo trục: • • X = − r1.sin θ θ • • Y1 = r1.cosθ θ Vận tốc điểm A: Động khớp 1: - Động khớp 2: Vị trí điểm B hệ tọa độ sau: Vận tốc điểm B theo trục sau: Vận tốc B : Động khớp 2: • Xác định hệ thống - Thế khớp 1: - • Thế khớp 2: Hàm lagrange Từ tính toán ta có hàm lagrange sau: TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP Tính Mô men khớp quay (khớp 1) Mô men quán tính tính theo công thức : Ta có : Và Từ mô men khớp quay sau: Tính lực khớp tịnh tiến (khớp Lực tác động lên khớp tịnh tiến tính sau: Trong đó: Vậy lực cần tìm : Tím toán với tham số cụ thể Robot θ-r có hai bậc tự gồm khớp khớp quay, khớp thứ hai khớp tịnh tiến Giả thuyết chuyển động khớp chuyển động Đồng thời thời gian để khớp quay quay hết quỹ đạo yêu cầu thời gian mà khớp tịnh tiến thực hết hành trình làm việc Thời gian để cấu tịnh tiến chuyển động từ r = 0,5 [m] tới vị trí r max = [m] với vận tốc r = 0,1 [m/s] là: t= rmax − r1 − 0,5 = = [s] r 0,1 Mặt khác, cấu Robot lại quay từ 00 đến 600 cuối hành trình khớp quay góc θ = 600 = π/3 [rad] thời gian khớp quay quay thời gian TRƯƠNG VĂN TRUNG Page 10 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP khớp tịnh tiến thực hết hành trình nên ta có vận tốc góc khớp quay tính sau: Tiếp theo ta có khớp quay khớp tịnh tiến chuyển động với vận tốc không đổi nên: Mặt khác cuối hành trình chuyển động : r = rmax = [m] θ = π/3 thay vào công thức xây dựng - Mô mem khơp quay: - Lực tổng khớ tịnh tiến: b Thiết kế điều khiển “Mô mem tính toán” cho Robot Phương trình động lực học cấu Robot θ-r trình bày viết lại sau: & + 2m rr& & M = (m1.r12 + m2 r )θ& θ + (m1r1 + m2 r ) gcθ & − m rθ&2 + m gSθ F= m r& 2 Hay viết dạng ma trận: (2-1) Trong đó: M M = F - mô men lực khớp m1r12 + m2 r H = TRƯƠNG VĂN TRUNG 0 m2 - ma trận mô men lực tỉ lệ với gia tốc Page 11 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP 2m rr& θ& V = 2 & − m2 rθ - ma trận mô men lực nhớt (m r + m2 r ) gCθ G= 11 m2 gSθ -ma trận mô men lực trọng lực Nguyên lý thiết kế điều khiển mô men tính toán :khử thành phần phi tuyến phương trình động lực học Robot phân ly đặc tính động học nối.Kết nhận hệ thống tuyến tính từ dễ dàng sử dụng phương pháp thiết kế kinh điển hệ thống tuyến tính Từ ý tưởng ta có cấu trúc điều khiển : ĐK Tuyến tính U ĐK R phi tuyến Hình 2.2 cấu trúc điều khiển Để làm điều chọn phương trình mô tả điều khiển mô men tính toán sau: (2-2) Từ (2-1) (2-2) ta có: (2-3) Thiết kế luật điều khiển PD cho Robot ta có: (2-4) Từ (2-3) (2-4) có : TRƯƠNG VĂN TRUNG Page 12 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP Laplace hóa hai vế phương trình thu phương trình sau: p2 + Kd p + K p = (2 − 5) Mong muốn hệ thống có đáp ứng khâu giao động bặc có dạng sau : p + 2ξωn p + ωn2 = Trong đó: ξ -hệ số suy giảm ωn = ξ t qd -tần số giao động Cân phương trình (2-5) (2-6) thu được: K di = 2ξωn = 2ξ = ξ tqd tqd K pi = ωn2 = ξt qd ÷ ÷ Hình 2.3 sơ đồ cấu trúc hệ thống điều khiển mô men tính toán TRƯƠNG VĂN TRUNG Page 13 (2 − 6) BÀI TẬP LƠN ROBOT CÔNG NGHIỆP ξ = 0, Chọn chất lượng điều khiển có hệ số suy giảm 1s thu tham số điều khiển khớp i sau: K di = thời gian độ tqd = =8 K pi = ÷ = 32, 65 0, 7.1 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 giống Suy hệ số điều khiển: 8 Kd = 0 32, 65 Kp = 32, 65 c Mô hệ thống • Thiết kế quỹ đạo chuyển động cho hai khớp dạng – – Để đả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 toán quỹ đạo sau: TRƯƠNG VĂN TRUNG Page 14 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP Hình 2.4 quỹ đạo 2-1-2 Khi Robot di chuyển từ A tới B xảy trình : - Đầu tiên trình khởi động có dạng bậc - Quá trình chuyển động có dạng đường thẳng bậc - Quá trình hãm có dạng đường cong bậc Ta giả thuyết TRƯƠNG VĂN TRUNG q&0 = q&c = mặt khác ta có : Page 15 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP tc qc + q tm = ; qm = 2 qm − q1 & t = q&1 = q& 1 tm − t1 & q1 = q0 + q& 1t1 2 & & ⇒ q& 1.t1 − q1.t c t1 + qc − q0 = tc tc2 qc − q0 → t1 = m − & q& t1 < Từ đồ thị thấy t1 = tc Do : tc t q − q0 − c − c & q& (2 − 7) Biểu thức (2-7) có nghĩa : tc2 qc − q0 − & q& ⇒ qc − q0 & & & < q& ≤ q1cp tc2 (2 − 8) Giới hạn gia tốc phụ thuộc vào độ bền khí Robot Như quỹ đạo thiết kế sau: & − t : q ( t ) = q + q& 1t t1 & ) t1 − t2 : q (t ) = q0 + q& 1t1 (t − & 1t1 (t − t c ) t2 − t3 : q (t ) = qc − q& Trong đó: TRƯƠNG VĂN TRUNG Page 16 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP & q& - Chọn nằm khoảng theo biểu thức (2-8) & q& Biết , tc, qc, q0 ta tính t1 theo (2-7) Kết mô • Mô men lực khớp Sai lệch biến khớp 12 0.02 mo men khop 10 luc khop -0.02 -0.04 -0.06 -0.08 -0.1 khop tinh tien -2 0.5 1.5 2.5 3.5 4.5 -0.12 khop quay 0.5 Góc quay θ khớp 1.5 2.5 3.5 4.5 4.5 Độ dịch chuyển r khớp 1.6 1.2 1.4 1.1 1.2 1 0.9 0.8 0.8 0.6 0.7 0.4 0.6 0.2 0 0.5 1.5 2.5 3.5 4.5 0.5 0.5 1.5 2.5 3.5 Hình 2.5 tín hiệu thu từ mô 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 TRƯƠNG VĂN TRUNG Page 17 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP • Chương trình mô Chương trình %====================================== % 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) 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 = 5; m1 = ; m2 = ; %Dat thoi gian di chuyen cua tay Robot %Khoi luong %Khoi luong TRƯƠNG VĂN TRUNG Page 18 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP r1 = 0.5; %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 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 = 0; %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); %dqd1 la van %toc cua khop 1, ddq1 la gia toc khop [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)); 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); TRƯƠNG VĂN TRUNG Page 19 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP V1 = [v1;v2]; H11 = m1*r1^2 + m2*X0(3)^2; H12 = 0; H21 = 0; H22 = m2; H = [H11, H12; H21, H22]; %r = x21 %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 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 = ; %Khoi luong m2 = ; %Khoi luong r1 = 0.5; %Chieu dai noi % - TRƯƠNG VĂN TRUNG Page 20 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP 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); %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 x11 = x21 = x12 = x22 = gan x11 x21 x12 x22 dung + Tk + Tk + Tk + Tk phuong trinh vi phan * x11p; * x21p; * x12p; * 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) if tt1) & (tt2)&(t[...]... + G; U = dsd+Kp*errorTheta+Kd*errordTheta; M=H*U+V+G; TRƯƠNG VĂN TRUNG Page 22 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP Tài liệu tham khảo: 1 TS Nguyễn Mạnh Tiến Điều Khiển Robot Công Nghiệp Nhà xuất bản Khoa Học và Kỹ Thuật Hà Nội 2006 2 TS Nguyễn Mạnh Tiến Bài Giảng Robot Công Nghiệp 3 GS.TSKH Nguyễn Thiện Phúc Robot Công Nghiệp Nhà xuất bản Khoa Học và Kỹ Thuật Hà Nội 2006 TRƯƠNG VĂN TRUNG Page 23 ... dich chuyen cua khop 2 tinh tien plot(At1,Aq2) grid Xây dựng các hàm + hàm RobotThetaR function[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR(Kp,Kd) tc = 5; m1 = 1 ; m2 = 1 ; %Dat thoi gian di chuyen cua tay Robot %Khoi luong thanh 1 %Khoi luong thanh 2 TRƯƠNG VĂN TRUNG Page 18 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP r1 = 0.5; %Chieu dai thanh noi 1 %===Vi tri cua tay ban dau` va` cuoi... phỏng 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 cùng là B(xc , yc) trong khoảng thời gian tc (s) ta có thể tính toán quỹ đạo như sau: TRƯƠNG VĂN TRUNG Page 14 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP Hình 2.4 quỹ đạo 2-1-2 Khi nó Robot di chuyển từ A tới B xảy ra 3 quá trình : - Đầu tiên là quá trình khởi động có... v2 = -m2*X0(4)*X0(1)*X0(1); TRƯƠNG VĂN TRUNG Page 19 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP V1 = [v1;v2]; H11 = m1*r1^2 + m2*X0(3)^2; H12 = 0; H21 = 0; H22 = m2; H = [H11, H12; H21, H22]; %r = x21 %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 do thi... điểm dừng cuối của mình TRƯƠNG VĂN TRUNG Page 17 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP • Chương trình mô phỏng Chương trình chính %====================================== % CHUONG TRINH CHINH %======================================= % khai bao gia tri bo dieu khien Kp = 32.65 Kd = 8 % goi ham RobotThetaR [At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR(Kp,Kd) % ========================================... khiển Để làm được điều đó chọn phương trình mô tả bộ điều khiển mô men tính toán như sau: (2-2) Từ (2-1) và (2-2) ta có: (2-3) Thiết kế luật điều khiển PD cho Robot ta có: (2-4) Từ (2-3) và (2-4) có : TRƯƠNG VĂN TRUNG Page 12 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP Laplace hóa hai vế của phương trình thu được phương trình sau: p2 + Kd p + K p = 0 (2 − 5) Mong muốn hệ thống có đáp ứng như khâu giao động bặc 2 có... 2.3 sơ đồ cấu trúc hệ thống điều khiển mô men tính toán TRƯƠNG VĂN TRUNG Page 13 (2 − 6) BÀI TẬP LƠN ROBOT CÔNG NGHIỆP ξ = 0, 7 Chọn chất lượng điều khiển có hệ số suy giảm 1s thu được tham số bộ điều khiển của khớp i như sau: K di = và thời gian quá độ tqd = 8 =8 1 2 4 K pi = ÷ = 32, 65 0, 7.1 Trong bài này bộ điều khiển thiết kế cho hai khớp là giống nhau nên các thông số bộ điều khiển...BÀI TẬP LƠN ROBOT CÔNG NGHIỆP khớp tịnh tiến thực hiện hết hành trình của mình nên ta có vận tốc góc của khớp quay tính như sau: Tiếp theo ta có khớp quay và khớp tịnh tiến chuyển động đều với vận tốc không đổi nên: Mặt khác ở cuối hành trình chuyển động thì : r = rmax = 1 [m] và θ = π/3 thay vào các công thức đã xây dựng ở trên được - Mô mem của khơp... x12p; * 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) if tt1) & (tt2)&(t ... Page 22 BÀI TẬP LƠN ROBOT CÔNG NGHIỆP Tài liệu tham khảo: TS Nguyễn Mạnh Tiến Điều Khiển Robot Công Nghiệp Nhà xuất Khoa Học Kỹ Thuật Hà Nội 2006 TS Nguyễn Mạnh Tiến Bài Giảng Robot Công Nghiệp. .. BÀI TOÁN TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP a Xác định mô mem khớp quay lực tổng khớp tịnh tiến Robot cuối hành trình chuyển động Ta có mô hình Robot θ– r sau... Robot c Mô hệ thống BÀI TOÁN TRƯƠNG VĂN TRUNG Page BÀI TẬP LƠN ROBOT CÔNG NGHIỆP 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 minh họa cấu Robot Từ sơ đồ cánh