BÀI TẬP LỚN KỸ THUẬT ROBOT ĐỀ 18 Câu 1: a) Xây dựng hệ tọa độ cho các thanh nối Từ sơ đồ cánh tay ta gắn các hệ trục tọa độ như sau: • Vị trí gốc tọa độ: - Khâu số 0 thân Robot là khâu cố định có gốc tọa độ O được đặt tại khớp 1 - Khung tọa độ 1 có gốc O1 được gắn vào khớp thứ 2 - Khung tọa độ 2 có gốc O2 được gắn vào khớp thứ 3 - Khung tọa độ 3 có gốc O3 được gắn vào khâu tác động cuối • Chiều của các khung tọa độ : - Khung tọa độ số 0 có trục Z0 trùng với trục của khớp 1, trục X0 được chọn vuông góc với mặt giấy - Khung tọa độ số 1 có trục Z1 vuông góc với mặt phẳng giấy và trục X1 trùng với trúc của khớp thứ 2 - Khung tọa độ số 2 có trục Z2 trùng với trục của khớp số 3 và trục X2 được chọn vuông góc với mặt giấy - Khung tọa độ số 3 có trục Z3 trùng với phương của trục Z2 và trục X3 vuông góc với mặt giấy - Trục Y0, Y1, Y2, Y3 được xác định theo quay tắc bàn tay phải. Từ đó ta xây dựng được hệ trục tọa độ như sau: 1 o 0 1 0 2 03 K 1 K2 K 3 X0 Y0 Z0 x1 y1 z1 x2 z2 x3 z3 b) Xác định ma trận T biểu diễn hệ tọa độ tay robot Từ hình vẽ trên ta lập được bảng D-H các tham số sau: KTĐ a i K1 0 0 K2 0 K3 0 0 0 Vị trí của hai khâu liền kề nhau được mô tả bởi một ma trận biến đổi đồng nhất i-1 A i. Ma trận 0 A 1 mô tả quan hệ giữa khâu 1 và khâu 0 (khâu cố gắn với thân Robot), ma trận 1 A 2 mô tả quan hệ giữa khâu 2 và khâu 1, ma trận 2 A 3 mô tả quan hệ giữa khâu 3 và khâu 2. Dạng tổng quát của ma trận i-1 A i như sau: 2 i-1 A i = . . . . . . 0 0 0 0 1 C i S i C i S i S i ai C i S i C i C i C i S i ai S i S i C i di θ θ α θ α θ θ θ α θ α θ α α −     −         Áp dụng với các tham số bảng D-H ta có: 0 A 1 = 1 1 1 1 0 0 0 0 0 1 0 0 0 0 0 1 C S S C θ θ θ θ     −         1 A 2 = 2 2 2 2 2 0 0 0 0 0 1 0 0 0 0 1 C S S C a θ θ θ θ −         −     2 A 3 = 3 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1 d             Từ đó ta thu được ma trận T như sau: T= 0 A 1 . 1 A 2 . 2 A 3 = 1 2 1 1 2 2 1 3 1 2 1 2 1 1 2 2 1 3 1 2 2 2 3 2 . . . . . . . . 0 . 0 0 0 1 C C S C S a S d C C S C C S S a C d S S S C d C θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ − − −     − − −         c) Giải thích ý nghĩa của ma trận T: Ma trận T có dạng tổng quát như sau: 3 T= 0 0 0 1 x x x x y y y y z z z z n o a p n o a p n o a p             Trong đó: - Vector n , o , a xác định hướng của khung tọa độ tay so với khung tọa độ gốc cố định. - Vector p biểu diễn vị trí của khung tọa độ tay so với khung tọa độ gốc. Qua ma trận T người ta có thể phân tích sự hoạt động và lập trình điều khiển cho Robot. Ma trận T có ý nghĩa rất lớn trong bài toán động học thuận và bài toán động học ngược: - Động học thuận: khi biết giá trị của các biến khớp thay đổi theo thời gian thì vị trí và hướng của tay Robot sẽ hoàn toàn xác định tại mọi thời điểm. - Động học ngược: khi biết hướng và vị trí của điểm tác động cuối ta hoàn toàn có thể xác định được giá trị của các biến khớp từ việc giải hệ phương trình động học T trên. d) Xác định vị trí tay Robot trong hệ tọa độ gốc khi =30 o , , d 3 =0.1m Khi robot quay một góc =30 o ; , d 3 =0.1m thì ta có ma trận T như sau: T= o o o o o o o o o o o o o o o o o o C30 .C30 -S30 C30 .S30 0,3.S30 0,1.C30 .C30 S30 .C30 C30 S30 .S30 0,3.C30 0,1.S30 .S30 S30 0 C30 0,1.C30 0 0 0 1 o   − −   − − −         = 3 1 3 0,3 4 2 4 4 3 3 1 0,6. 3 0,1 4 2 4 4 1 3 0,1. 3 0 2 2 2 0 0 0 1   − −       + − −               Vậy vị trí của gốc tọa độ gắn lên tay có vị trí như sau: 4 p = 0,3 4 0,6. 3 0,1 4 0,1. 3 2 1       +   −             Câu 2: a) Xác định momen khớp quay và lực tổng ở các khớp tịnh tiến khi Robot ở cuối hành trình chuyển động Ta có mô hình của Robot như sau: Để xây dựng được các phương trình động lực học của Robot θ-rđơn giản chúng ta giả thuyết: - Khối lượng m 1 của xilanh tập trung tại điểm cuối của xilanh tức là điểm A - Khối lượng m 2 của pittong tập trung ở bàn tay Robot tức là tập trung tại điểm B - Mômen quán tính ở khớp J i = 0 5 Trình tự xây dựng phương trình động lực học như sau:  Xác định hàm lagrange Phương trình lagrange của một cơ cấu được xác định như sau: Trong đó: - K: Tổng động năng của hệ thống - P: Tổng thế năng của hệ thống  Với khớp quay ta có phương trình động lực học như sau: M = d L L dt θ θ ∂ ∂   −  ÷ ∂ ∂   &  Với khớp tịnh tiến ta có phương trình động lực học như sau: F= d L L dt r r ∂ ∂   −  ÷ ∂ ∂   & • Xác định động năng của hệ thống Động năng của hệ thống: K = K 1 + K 2 Trong đó: K 1 : động năng của khớp 1 K 2 : động năng của khớp 2 Theo hình vẽ ta có: - Động năng của khớp 1: Vị trí của điểm A trong hệ tọa độ như sau: X 1 = r 1 . cos θ Y 1 = r 1. sin θ Đạo hàm hai vế theo thời gian thu được thành phần vận tốc theo các trục: 1 1 .sin .X r θ θ = − & & 1 1 .cos .Y r θ θ = & & Vận tốc điểm A: 2 2 2 2 2 1 1 1 1 v x y r θ = + = & & &  Động năng của khớp 1: 6 2 2 1 1 1 1 2 K m r θ = & - Động năng của khớp 2: Vị trí của điểm B trong hệ tọa độ như sau: 2 cosx r θ = 2 siny r θ = 7 Vận tốc của điểm B theo các trục như sau: 2 sin . cos .x r r θ θ θ = − + & & & 2 cos . sin .y r r θ θ θ = + & & & Vận tốc của B: 2 2 2 2 2 2 2 2 2 v x y r r θ = + = + & & & &  Động năng của khớp 2: 2 2 2 2 2 1 ( ) 2 K m r r θ = + & & • Xác định thế năng của hệ thống - Thế năng của khớp 1: 1 1 1 sinP m gr θ = - Thế năng của khớp 2: 1 2 sinP m gr θ = • Hàm Lagrange Từ các tính toán trên ta có hàm Lagrange như sau: 1 2 1 2 2 2 2 2 2 1 1 2 1 1 2 ( ) 1 1 ( ) ( sin sin ) 2 2 L K K P P m r m r r m gr m gr θ θ θ θ = + − + = + + − + & & &  Tính Mô men khớp quay (khớp 1) Mô men quán tính được tính theo công thức: d L L M dt θ θ ∂ ∂   = −  ÷ ∂ ∂   & Ta có: 2 2 1 1 2 2 2 1 1 2 2 2 L m r m r d L m r m r m rr dt θ θ θ θ θ θ θ ∂ = + ∂ ∂   = + +  ÷ ∂   & & & && && & & & Và 1 1 2 cos cos L m gr m gr θ θ θ ∂ = − − ∂ Từ đó mô men khớp quay như sau: 8 2 2 1 1 2 2 1 1 2 2 2 1 1 2 2 1 1 2 2 ( ) cos ( ) 2 ( ) cos M m r m r m rr m r m r g m r m r m rr m r m r g θ θ θ θ θ θ θ = + + + + = + + + + && && && & && & & 9  Tính lực khớp tịnh tiến (khớp 2) Lực tác động lên khớp tịnh tiến đưuọc tính như sau: d L L F dt r r ∂ ∂   = −  ÷ ∂ ∂   & Trong đó: 2 2 2 2 2 sin L m r r d L m r dt r L m r m g r θ θ ∂ = ∂ ∂   =  ÷ ∂   ∂ = − ∂ & & && & & Vậy lực cần tìm là: 2 2 2 2 sinF m r m r m g θ θ = − + & &&  Tính toán tham số cụ thể Robot θ - r có hai bậc tự do gồm khớp 1 là khớp quay, khớp thứ 2 là khớp tịnh tiến. Giả thuyết chuyển động của các khớp này là chuyển động đều. Đồng thời thời gian để khớp quay quay hết quỹ đạo yêu cầu bằng thời gian mà khớp tịnh tiến thực hiện hết hành trình làm việc của mình. Thời gian để cơ cấu tịnh tiến chuyển động từ r 1 = 0.5(m) tới vị trí r max =1.0(m) với vận tốc r = 0.125(m/s) là: max 1 1.0 0.5 4( ) 0.125 r r t s r − − = = = Mặt khác cơ cấu Robot lại quay từ 0 0 với tốc độ 16 π θ = & và thời gian khớp quay đúng bằng thời gian khớp tịnh tiến thực hiện hết hành trình của mình Vậy ở cuối hành trình thì khớp đã quay một góc: 4 . ( ) 16 4 t rad π π θ θ = = = & 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: 10 [...]... dsd+Kp*errorTheta+Kd*errordTheta; M=H*U+V+G; 23 = 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 24 ... (khop 2) grid % ve goc quay khop 1 plot(At1,Aq1) grid % do 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 = 4; %Dat thoi gian di chuyen cua tay Robot m1 = 2.5 ; %Khoi luong thanh 1 m2 = 2.5 ; %Khoi luong thanh 2 r1 = 0.5; %Chieu dai thanh noi 1 %===Vi tri cua tay ban dau` va` cuoi... %3.4f\n',t,qd(1),q(1),qd(2),q(2),M(1),M(2)); end fclose(file1); 21 + 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 do goc khop quay x21 = X0(3); %Gia tri r x22 = X0(4); %Toc do khop tinh tien % %Cac thong so cua Robot m1 = 2.5 ; %Khoi luong thanh 1 m2... TRINH CHINH %======================================= % khai bao gia tri bo dieu khien % goi ham RobotThetaR [At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR() % ======================================== % do thi tin hieu thu duoc %========================================= % ve mo men va luc cua robot tren cung do thi plot(At1,AM1,'r') % mo mem khop quay (khop 1) hold on plot(At1,AM2) %... 65  0, 7.1  14 và thời gian quá độ tqd = 1s 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 của hai khớp giống nhau Suy ra các hệ số bộ điều khiển: 8 0  Kd =   0 8  0  32, 65 Kp =  32, 65  0  c) Mô 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(x 0 , y0 ) đến vị... - 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 20 = 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 do thi %Tinh toan gia tri... x21 H12 = 0; H21 = 0; H22 = m2; 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 do 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... 0 , 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: 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 : 15 - Đầu tiên là quá trình khởi động có dạng bậc 2 Quá trình chuyển động đều có dạng là đường thẳng bậc 1 Quá trình hãm có dạng đường cong bậc 2 Ta giả thuyết & q0 = 0  & qc = 0 mặt khác ta có :  tc qc + q 0... thành phần phi tuyến của phương trình động lực học của Robot và phân ly đặc tính động học của các thanh nối.Kết quả nhận được một hệ thống tuyến tính từ đó dễ dàng sử dụng các phương pháp thiết kế kinh điển của hệ thống tuyến tính Phương trình luật điều khiển có dang: & M dk = K P ε + Κ D ε (24) Với: ε = qd − q KP = KD = là vector sai số vị trí của khớp robot; ( K p1 , K p 2 ) diag diag - ma trận đường... tc2 qc − q0 t1 = − − & & 2 4 q1 (2 − 7) Biểu thức (2-7) có nghĩa khi và chỉ khi : tc2 qc − q0 − & & 4 q1 ⇒ 4 qc − q0 & & & & < q1 ≤ q1cp tc2 (2 − 8) Giới hạn của gia tốc phụ thuộc vào độ bền cơ khí của Robot Như vậy quỹ đạo thiết kế như sau: 16 1  & & 0 − t1 : q(t ) = q0 + q1t 2  2  t1  & & t1 − t2 : q(t ) = q0 + q1t1 (t − ) 2  1  2 & & t2 − t3 : q(t ) = qc − 2 q1t1 (t − tc )  Trong đó: & . BÀI TẬP LỚN KỸ THUẬT ROBOT ĐỀ 18 Câu 1: a) Xây dựng hệ tọa độ cho các thanh nối Từ sơ đồ cánh tay ta gắn các hệ trục tọa độ như sau: • Vị trí gốc tọa độ: - Khâu số 0 thân Robot là. ta có thể phân tích sự hoạt động và lập trình điều khiển cho Robot. Ma trận T có ý nghĩa rất lớn trong bài toán động học thuận và bài toán động học ngược: - Động học thuận: khi biết giá trị của. khi Robot ở cuối hành trình chuyển động Ta có mô hình của Robot như sau: Để xây dựng được các phương trình động lực học của Robot θ-rđơn giản chúng ta giả thuyết: - Khối lượng m 1 của xilanh tập