đồ án Điều khiển robot

Đề 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. Hình 2. Đồ thị tốc độ. 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 0 0.25 0.75 1.0 t(s) V (m/s) 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 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) 2 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 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. 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 3 = 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 : ( ) ( ) ( ) ( ) 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: 4 = 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 đó: ( ) ì = =ì 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 5 =+ +=+ 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 : [ ] ++= ++++++= )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 : 6 [ ] ++= ++= ++++++= )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ó: +++= ++= )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 +++= 7 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); l1*cos(d1)+l2*cos(d1+d2),l2*cos(d1+d2)] % Ma tran Jacobien 8 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: maxy21 maxx21 Vb Va = = Xét khoảng thời gian từ 0,75ữ1s, robot di chuyển theo quỹ đạo bậc 2: 9 += ++= 31323 3031 2 323 a0,75)(t2a(t)x a0,75)(ta0,75)(ta(t)x += ++= 31323 3031 2 323 b0,75)(t2b(t)y b0,75)(tb0,75)(tb(t)y Dựa vào các điều kiện biên có: 0(1)y V(0,75)y 0(1)x V(0,75)x 3 ymax3 3 xmax3 = = = = maxy32 maxy31 maxx32 maxx31 V2b Vb V2a Va = = = = Xét khoảng thời gian sau 1 giây ta có: 0,4(1)y 0(1)x 3 3 = = 0,4b0,25V0,25V2 0a0,25V0,25V2 30ymax 2 ymax 30xmax 2 xmax =+ì+ì =+ì+ì Mặt khác để đảm bảo tính liên tục của quỹ đạo ta có: = = (0,75)x(0,75)x (0,25)x(0,25)x 23 12 +== +== xmax20230 xmax120 0,5Va(0,75)xa 0,125V0,4(0,25)xa = = (0,75)y(0,75)y (0,25)y(0,25)y 23 12 +== == ymax20230 ymax120 0,5Va(0,75)yb 0,125V(0,25)yb Vậy ta có: ( ) 00,5V0,125V0,40,25V0,25V2 xmaxxmaxxmax 2 xmax =+++ì+ì ( ) 0,40,5V0,125V0,25V0,25V2 ymaxymaxymax 2 ymax =++ì+ì sm5333,0V xmax = sm5333,0V ymax = Thay vào các hệ số của phơng trình quỹ đạo chuyển động ở trên ta đợc ph- ơng trình quỹ đạo chuyển động của robot nh sau: ( ) ( ) ( ) ( ) + + + = 0 0,06660,75t0,53330,75t1,0666 3333,00,25t0,5333 0,41,0666t tx 2 2 1t 1t0,75 0,75t0,25 0,25t0 > 10 [...]... g1 = m1 glg 1c1 + m2 g ( l1c1 + l g 2c12 ) g 2 = m2 gl g 2c12 b Xây dựng luật điều khiển Các hệ thống điều khiển có phản hồi nhằm tạo ra khả năng chống nhiễu tốt Nhng khi cần bám sát quỹ đạo với tốc độ và gia tốc lớn thì chúng không đáp ứng đợc Cơ chế điều khiển có bù cho phép giảm sai số quỹ đạo Việc xác định tín hiệu điều khiển sao cho hệ thống ổn định xung quanh điểm cân bằng dựa trên phơng pháp ổn... = qT ( H ( q ) 2C ( q ,q ) ) q qT Fq + qT ( M g ( q ) K p ) 2 Nhìn vào phơng trình trên ta chọn luật điều khiển nh sau: u = g ( q ) + K p K D q Trong đó KD là số xác định dơng, tơng ứng với phần bù phi tuyến thành phần trọng trờng với bộ điều khiển PD 18 Câu 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 radian đến 1 radian Mô phỏng bằng SIMULINK... chơng trình với số điểm vẽ N=150 ta đợc: >>vkhop(150); 15 Hình 5 Đồ thị vận tốc chuyển động của khớp Câu 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: 16 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 h12... với 200 điểm quỹ đạo đợc thể hiện trên hình vẽ 3 >> cdtay(200) Hình 3 Quỹ đạo chuyển động của tay robot Để xác định tốc độ và vị trí các khớp, cần tính đợc các giá trị x,y rời rạc của tay Robot sau đó thông qua phơng trình động học ngợc xác định góc quay 1 và 2 Biểu diễn 1 và 2 trên đồ thị thời gian, ta đợc đồ thị biểu diễn vị trí của khớp Đạo hàm chuyển động của từng khớp theo thời gian ta sẽ đợc dạng... end %for plot(time,[theta1;theta2]); grid; xlabel('t(s)'); ylabel('Goc(do)'); Chạy chơng trình với số điểm là 150 ta đợc: >> cdkhop(150) 13 Hình 4 Quỹ đạo chuyển động của các khớp Chơng trình tính toán và vẽ đồ thị vận tốc chuyển động của khớp: function vkhop=vkhop(N) %N so diem quy dao can ve l1=0.4; l2=0.3; for i=0:N t=i/N; time(i+1)=t; if t1 Chơng trình MATLAB sau thể hiện quỹ đạo chuyển động của tay robot theo vị trí và tốc độ: Quỹ đạo chuyển động của tay robot: function cdtay=cdtay(N) %N so diem quy dao can ve for i=0:N t=i/N; time(i+1)=t; if t . 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. luật điều khiển Các hệ thống điều khiển có phản hồi nhằm tạo ra khả năng chống nhiễu tốt. Nhng khi cần bám sát quỹ đạo với tốc độ và gia tốc lớn thì chúng không đáp ứng đợc. Cơ chế điều khiển. luật điều khiển nh sau: ( ) qKKqgu Dp += Trong đó K D là số xác định dơng, tơng ứng với phần bù phi tuyến thành phần trọng trờng với bộ điều khiển PD. 18 Câu 5: Mô phỏng hệ thống với bộ điều khiển