Cho một cơ cấu robot 2 thanh nối được truyền động bởi động cơ một chiều.
Đề bài Cho một cơ cấu Robot 2 thanh nối đợc truyền động bởi động cơ một chiều. Động cơ một chiều đợc cấp điện từ một bộ khuếch đại điện áp. 1. Xây dựng phơng trình động học thuận và ngợc biểu diễn mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các khớp (Joints). 2. Xây dựng quan hệ giữa tốc độ của các khớp và tốc độ của tay Robot. 3. Viết hàm MATLAB thực hiện các phơng trình khi tay Robot di chuyển từ vị trí [0,4: 0,0 m] đến [0,0: 0,4 m] theo một đờng thẳng. Đồ thị tốc độ đặt trớc của tay Robot dọc theo quỹ đạo cho ở hình 2. 4. Thiết kế bộ điều khiển bù trọng lực cho Robot. 5. Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1 thay đổi nhảy cấp từ 0 đến 1 rad. Hình 1. Cơ cấu động học robot hai thanh nối. 1 2 l 2g l 1 l 1g l 2 1 2 x 2 y y x z 1 m 2 m 1 J 2 J Hình 2. Đồ thị tốc độ. Bảng thông số của Robot: STT Đại lợng Giá trị 1 Chiều dài thanh nối1 (l 1 ) 0,4m 2 Chiều dài thanh nối1 (l 2 ) 0,3m 3 Khối lợng thanh nối 1 (m 1 ) 10Kg 4 Khối lợng thanh nối 2 (m 2 ) 5,0Kg 5 Momen quán tính khớp 1 quay quanh tâm khối (J 1 ) 0,528 Kgm 2 6 Momen quán tính khớp 2 quay quanh tâm khối (J 2 ) 0,19 Kgm 2 7 Khoảng cách từ khớp 1 đến tâm khối 1 (l g1 ) 0,25 m 8 Khoảng cách từ khớp 2 đến tâm khối 2 (l g2 ) 0,15 m 9 Hằng số momen của động cơ khớp 1, 2 (K M ) 0,5 Nm/A 10 Điện trở phần ứng (r 1 ,r 2 ) 3 11 Momen lớn nhất của động cơ khớp 1 (M 1max ) 1,2 Nm 12 Momen lớn nhất của động cơ khớp 2 (M 2max ) 0,7 Nm 13 Tốc độ lớn nhất của động cơ khớp 1, 2 ( max ) 90 rad/s 14 Momen quán tính của động cơ (J Đ ) 0,0004 Kgm 2 15 Khối lợng tải lớn nhất (m t ) 5 Kg 16 Tỉ số truyền cho cả hai khớp (i) 12 Bài giải 2 0 0.25 0.75 1.0 t(s) V (m/s) Câu 1. Xây dựng ph ơng trình động học thuận và ng ợc biểu diễn mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các khớp (Joints) a. Xây dựng phơng trình động học thuận Bài toán động học thuận là bài toán đi tìm các thông số về vị trí và hớng của tay robot so với khung cơ sở (gốc). Thực hiện phép biến đổi đồng nhất dựa trên hệ tọa độ thanh nối (Denavit - Hartenberg) mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các khớp (Joints) đợc xác định nh sau: 2 1 1 0 2 0 n 0 AATT == (1) Với: n 0 T : biểu diễn hệ toạ độ thanh thứ n so với hệ toạ độ gốc. E n T : biểu diễn hệ toạ độ tay (điểm kẹp) so với thanh n. = 1000 paon paon paon T zzzz yyyy xxxx E 0 i i A 1 : đợc định nghĩa là ma trận chuyển đổi đồng nhất, biểu diễn mối quan hệ vị trí của một điểm trong khung i và vị trí của điểm đó trong khung thứ i-1. 3 2 l 2g l 1 l 1g l 2 1 2 x 2 y y x z 1 m 2 m 1 J 2 J Từ mô hình cơ cấu động học của robot ta xây dựng đợc bảng Denavit - Hartenberg nh sau: Thanh a i i i d i 1 2 l 1 l 2 /2 /2 1 2 0 0 Theo đó ta xác định đợc các ma trận biến đổi A của robot nh sau: = 1000 dcs0 sascccs casscsc A iii iiiiiii iiiiiii i -1i = 1000 0100 sl0cs cl0sc A 1111 1111 1 0 = 1000 0100 sl0cs cl0sc A 2222 2222 2 1 ( ) sins;cosc == Thay vào (1) ta đợc: ( ) ( ) ( ) ( ) ( ) ( ) = ++++ ++++ = 1000 paon paon paon 1000 0100 slsl0cs clcl0sc T zzzz yyyy x ã xxx 212112121 212112121 E 0 (2) Với : 4 ( ) ( ) ( ) ( ) 2121 2121 sins cosc +=+ +=+ P là điểm thuộc hệ toạ độ gắn với tay robot, có vị trí đợc xác định bằng vectơ cột thứ t của E 0 T . Từ (2) ta đợc phơng trình động học thuận biểu diễn mối quan hệ giữa hệ tọa độ tay Robot và hệ tọa độ các khớp: = ++= ++= 0z )sin(lsinly )cos(lcoslx T 21211T 21211T (3) b. Xây dựng phơng trình động học ngợc Bài toán động học ngợc là tìm vị trí các khớp khi biết vị trí của tay robot (End-effector). Ma trận biểu diễn vị trí tay robot: = 1000 paon paon paon T zzzz yyyy x ã xxx E 0 Theo (1) ta có: 2 1 1 0 E 0 AAT = Nhân hai vế của (1) với 1 1 A ta có: ( ) 2 1 E 0 1 1 0 ATA =ì (4) tơng đơng với: = 1000 0100 sl0cs cl0sc 1000 0fff 0fff 0fff 2222 2222 333231 232221 131211 (5) trong đó: 5 ( ) ì = =ì 1000 paon paon paon 1000 0100 00cs- l-0sc 1000 ffff ffff ffff TA zzzz yyyy x ã xxx 11 111 34333231 24232221 14131211 E 0 1 1 0 ++++ ++++ = 1000 paon cpspcasacosocnsn lspcpsacasocosncn zzzz 1y1x1y1x1y1x1y1x 1y1xy1xy1x1y1x Cân bằng vế trái và phải của (5), ta có: = = 2224 2214 slf clf =+ +=+ 221y1x 2211y1x slcpsp cllspcp (6) Với = = =+ y x 2 2 2 2 py px 1sc Bình phơng hai vế của hai phơng trình trong (6) và cộng vế ta có: 21 2 2 2 1 22 2 l2l )l(l)y(x c ++ = [ ] )l(l)y(x )l(l)y(x2)lly(x tg 2 2 2 1 22 4 2 4 1 22222 2 2 1 22 2 ++ ++++++ = (7) Đặt : 6 [ ] ++= ++++++= )l(l)y(xk )l(l)y(x2)lly(xk 2 2 2 1 22 2 4 2 4 1 22222 2 2 1 22 1 ta có: )k,atan2(k 212 = (8) Thay (7) vào (6) ta đợc: 1 2 2 2 1 22 11 2l llxy ysxc ++ =+ Giải ra ta có: )k,atan2(kx)atan2(y, 311 = (9) trong đó: )l(l)y(xk 2 2 2 1 22 3 ++= . Vậy phơng trình động học ngợc của hệ đã cho đợc biểu diễn dới dạng = = ),(2tana ),(2tana)x,y(2tana 212 311 (10) Với : [ ] ++= ++= ++++++= )l(l)y(xk )l(l)y(xk )l(l)y(x2)lly(xk 2 2 2 1 22 3 2 2 2 1 22 2 4 2 4 1 22222 2 2 1 22 1 Câu 2: Xây dựng quan hệ giữa tốc độ các khớp và tốc độ của tay Robot Đạo hàm hai vế phơng trình động học thuận theo thời gian ta có: 7 +++= ++= )cos()l(cosly )sin()l(sinlx 21221111 21221111 hay +++ ++ = 2 1 21221211 21221211 )cos(l)cos(lcosl )sin(l)sin(lsinl y x (11) Đặt: +++ ++ = )cos(l)cos(lcosl )sin(l)sin(lsinl J 21221211 21221211 (J: ma trận Jacobien) Phơng trình biểu diễn quan hệ giữa tốc độ các khớp và tốc độ của tay Robot đợc biểu diễn dới dạng: = y x J 1 2 1 (12) Với: (J)det )(sinlsinl)(coslcosl )(sinl)(cosl J 2121121211 212212 1 ++ ++ = ( ) ( ) ( ) ( ) ( ) 1121211212 coslsinlsinlcoslJdet +++= 8 Câu 3. Viết hàm MATLAB thực hiện các ph ơng trình ở câu 1 và 2 và vẽ đ ờng biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí [0,4:0,0] đến [0,0:0,4] theo một đ ờng thẳng. Đồ thị tốc độ của tay Robot đã cho nh hình 2 a. Chơng trình tính toán động học thuận Dựa vào các công thức tính toán động học thuận (3) ta xây dựng đợc hàm tính toán động học thuận cho robot nh sau: function xy=Thuan(l1,l2,d1,d2) % Chuyen doi gia tri goc tu do ra radian r_d1=d1*2*pi/360; r_d2=d2*2*pi/360; %Tinh toan xy(1)=l1*cos(r_d1)+l2*cos(r_d1+r_d2); % Vi tri tay theo truc x xy(2)=l1*sin(r_d1)+l2*sin(r_d1+r_d2); % Vi tri tay theo truc y xy(3)=0; % Vi tri tay theo truc z b. Chơng trình tính toán động học ngợc Dựa vào hệ phơng trình tính động học ngợc (10) ta có hàm tính toán động học ngợc robot nh sau: function d=Nguoc(x,y,l1,l2) k1=sqrt((x^2+y^2+l1^2+l2^2)^2-2*((x^2+y^2)^2+l1^4+l2^4)); k2=(x^2+y^2)-(l1^2+l2^2); k3=(x^2+y^2)+(l1^2-l2^2); d1=atan2(y,x)-atan2(k1,k3); %Goc theta 1 tinh theo radian. d2=atan2(k1,k2); %Goc theta 2 tinh theo radian. d(1)=d1*360/(2*pi) %Goc theta 1 tinh theo do. d(2)=d2*360/(2*pi) %Goc theta 2 tinh theo do. c. Chơng trình tính tốc độ quay khớp Dựa vào phơng trình biểu diễn quan hệ giữa tốc độ các khớp và tốc độ của tay robot ta xây dựng đợc hàm tính toán tốc độ quay của các khớp nh sau: % Chuong trinh tinh toc do quay khop function vd=tocdoquay(vx,vy,d1,d2,l1,l2) % Chuyen doi d1,d2 tu do() sang radian() r_d1=d1*2*pi/360; r_d2=d2*2*pi/360; J=[-l1*sin(d1)-l2*sin(d1+d2),-l2*sin(d1+d2); 9 l1*cos(d1)+l2*cos(d1+d2),l2*cos(d1+d2)] % Ma tran Jacobien v=[vx;vy]; % Vecto van toc theo truc x va y. vd=inv(J)*v; % Van toc goc cua khop 1 va 2 tinh theo radian/s d. Vẽ đờng biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí [0,4:0,0 m] đến [0,0:0,4 m] theo một đờng thẳng Dựa vào dạng đồ thị tốc độ ở hình 2 ta có quỹ đạo chuyển động của tay robot là quỹ đạo 2-1-2. Mặt khác tay Robot di chuyển từ vị trí [0,4: 0,0 m] đến [0,0: 0,4 m] theo một đờng thẳng thực chất là chuyển động tịnh tiến theo cả hai trục x và y. Xét khoảng thời gian từ 0ữ0,25s, tay Robot di chuyển theo quỹ đạo bậc 2: += ++= 11121 1011 2 121 at2a(t)x atata(t)x += ++= 11121 1011 2 121 bt2b(t)y btbtb(t)y Các điều kiện biên gồm: ymax xmax V(0,25)y 0)0(y 0)0(y V(0,25)x 0)0(x 4,0)0(x = = = = = = maxymaxy12 11 10 maxxmaxx12 11 10 V25,0Vb 0b 0b V25,0Va 0a 4,0a == = = == = = Xét khoảng thời gian từ 0,25ữ0,75s, robot di chuyển theo quỹ đạo bậc 1: = += 212 20212 a(t)x a0,25)(ta(t)x = += 212 20212 b(t)y b0,25)(tb(t)y Từ điều kiện biên suy ra: 10 [...]... 0 ,15 2 + 2. 0 ,4.0 ,15 cos 2 ) + 0 ,19 = 2 ,25 55 + 0 ,6 cos 2 2 h 12 = m2 (lg 2 + l1lg 2 c2 ) + J 2 = 5.(0 ,15 2 + 0 ,4.0 ,15.cos2 ) + 0 ,19 = 0 ,3 025 + 0 ,3 cos2 2 h21 = m2 ( lg 2 + l1lg 2 c2 ) + J 2 = 0 ,3 025 + 0 ,3 cos 2 2 h 22 = m2l g 2 + J 2 = 0 ,3 025 m l l s C ( q ,q ) = 2 1 g 2 2 2 m2l1lg 2 s21 m2l1lg 2 s2 (1 + 2 ) 0 g1 = m1 glg 1c1 + m2 g ( l1c1 + lg 2 c 12 ) g 2 = m2 gl g 2 c 12 b Xây... 0,4 2Vxmax ì 0 ,25 2 + Vxmax ì 0 ,25 + a30 = 0 2Vymax ì 0 ,25 2 + Vymax ì 0 ,25 + b30 = 0,4 Mặt khác để đảm bảo tính liên tục của quỹ đạo ta có: x2 (0 ,25 ) = x1(0 ,25 ) x3 (0,75) = x2 (0,75) y2 (0 ,25 ) = y1(0 ,25 ) y3(0,75) = y2 (0,75) a20 = x1(0 ,25 ) = 0,4 + 0, 125 Vxmax a30 = x2 (0,75) = a20 + 0,5Vxmax b20 = y1(0 ,25 ) = 0, 125 Vymax b30 = y2 (0,75) = a20 + 0,5Vymax Vậy ta có: 2Vxmax ì 0 ,25 2 + Vxmax... 4 Thiết kế bộ điều khiển bù trọng lực cho robot a Xây dựng phơng trình động lực học Phơng trình động lực học của hệ thống: H ( q ) q + C ( q , q ) q + Fq + g ( q ) = M Ta cần xác định đợc các hệ số của phơng trình trên Đối với robot Planar ta có (bỏ qua thành phần F): h H ( q ) = 11 h21 h 12 h 22 2 2 h11 = m1l g 1 + J 1 + m2 ( l 12 + lg 2 + 2l1l g 2 c2 ) + J 2 = 10.0 ,25 2 + 0 , 528 + 5.(0 ,4 2. ..a21 =Vx max b21 =V y max Xét khoảng thời gian từ 0,75ữ1s, robot di chuyển theo quỹ đạo bậc 2: x3(t) = a 32 (t 0,75 )2 + a31(t 0,75) + a30 x 3(t) = 2a 32 (t 0,75) + a31 y3(t) = b 32 (t 0,75 )2 + b31(t 0,75) + b30 y3(t) = 2b 32 (t 0,75) + b31 Dựa vào các điều kiện biên có: x3 (0,75) =Vxmax x3 (1) =0 y3 (0,75) =Vymax a31 =Vx max a 32 = Vx max 2 b31 =V y max b 32 = V y max 2 y3 (1)... vx= -2. 13 32* t; y=1.0666*t*t; vy =2. 13 32* t; elseif (t>0 .25 )&(t0.75)&(t