Điều khiển Robot

Luận Văn: Điều khiển Robot

Đề bàiCho 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.12l2gl1l1gl212x2yyxz1m2m1J2J 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 (l1) 0,4m2 Chiều dài thanh nối1 (l2) 0,3m3 Khối lợng thanh nối 1 (m1) 10Kg4 Khối lợng thanh nối 2 (m2) 5,0Kg5 Momen quán tính khớp 1 quay quanh tâm khối (J1) 0,528 Kgm26 Momen quán tính khớp 2 quay quanh tâm khối (J2) 0,19 Kgm27 Khoảng cách từ khớp 1 đến tâm khối 1 (lg1) 0,25 m8 Khoảng cách từ khớp 2 đến tâm khối 2 (lg2) 0,15 m9 Hằng số momen của động cơ khớp 1, 2 (KM) 0,5 Nm/A10 Điện trở phần ứng (r1,r2)3 11 Momen lớn nhất của động cơ khớp 1 (M1max) 1,2 Nm12 Momen lớn nhất của động cơ khớp 2 (M2max) 0,7 Nm13Tốc độ lớn nhất của động cơ khớp 1, 2 (max)90 rad/s14 Momen quán tính của động cơ (JĐ) 0,0004 Kgm215 Khối lợng tải lớn nhất (mt) 5 Kg16 Tỉ số truyền cho cả hai khớp (i) 12Bài giải2 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ậnBà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:211020n0AATT ==(1)Với:n0T: biểu diễn hệ toạ độ thanh thứ n so với hệ toạ độ gốc.EnT: biểu diễn hệ toạ độ tay (điểm kẹp) so với thanh n.=1000paonpaonpaonTzzzzyyyyxxxxE032l2gl1l1gl212x2yyxz1m2m1J2J iiA1: đợ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 aiiidi12l1l2/2/21200Theo đó ta xác định đợc các ma trận biến đổi A của robot nh sau:=1000dcs0sascccscasscscAiiiiiiiiiiiiiiiiii-1i=10000100sl0cscl0scA1111111110=10000100sl0cscl0scA2222222221( )sins;cosc ==Thay vào (1) ta đợc:4 ( ) ( ) ( )( ) ( ) ( )=++++++++=1000paonpaonpaon 10000100slsl0csclcl0scTzzzzyyyyxãxxx212112121212112121E0(2)Với : ( ) ( )( ) ( )21212121sins 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 E0T. 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(lcoslxT21211T21211T (3) b. Xây dựng phơng trình động học ngợcBà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:=1000paonpaonpaonTzzzzyyyyxãxxxE0Theo (1) ta có:2110E0AAT =Nhân hai vế của (1) với 11A ta có:5 ( )21E0110ATA =ì (4)tơng đơng với:=10000100sl0cscl0sc10000fff0fff0fff22222222333231232221131211(5) trong đó: ( )ì==ì1000paonpaonpaon1000010000cs-l-0sc1000ffffffffffffTAzzzzyyyyxãxxx11111343332312423222114131211E0110++++++++=1000paoncpspcasacosocnsnlspcpsacasocosncnzzzz1y1x1y1x1y1x1y1x1y1xy1xy1x1y1xCân bằng vế trái và phải của (5), ta có:==22242214slfclf =++=+221y1x2211y1xslcpspcllspcp (6) Với ===+yx2222pypx1sc Bình phơng hai vế của hai phơng trình trong (6) và cộng vế ta có:6 212221222l2l)l(l)y(xc++=[ ])l(l)y(x)l(l)y(x2)lly(xtg222122424122222221222++++++++=(7)Đặt : [ ]++=++++++=)l(l)y(xk)l(l)y(x2)lly(xk2221222424122222221221 ta có: )k,atan2(k212=(8)Thay (7) vào (6) ta đợc:1222122112lllxyysxc++=+Giải ra ta có: )k,atan2(kx)atan2(y,311=(9) trong đó: )l(l)y(xk2221223++=. 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(2tana212311(10)Với :[ ]++=++=++++++=)l(l)y(xk)l(l)y(xk)l(l)y(x2)lly(xk222122322212224241222222212217 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(sinlx2122111121221111hay+++++=212122121121221211)cos(l)cos(lcosl)sin(l)sin(lsinlyx(11)Đặt: +++++=)cos(l)cos(lcosl)sin(l)sin(lsinlJ2122121121221211 (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:=yxJ121 (12)Với: (J)det)(sinlsinl)(coslcosl)(sinl)(coslJ21211212112122121++++=( ) ( ) ( ) ( ) ( )1121211212coslsinlsinlcoslJdet+++=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 ta y 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ậnDự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 radianr_d1=d1*2*pi/360;r_d2=d2*2*pi/360;%Tinh toanxy(1)=l1*cos(r_d1)+l2*cos(r_d1+r_d2); % Vi tri tay theo truc xxy(2)=l1*sin(r_d1)+l2*sin(r_d1+r_d2); % Vi tri tay theo truc yxy(3)=0; % Vi tri tay theo truc zb. Chơng trình tính toán động học ngợcDự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ớpDự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 Jacobienv=[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/sd. 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ẳngDự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:+=++=1112110112121at2a(t)xatata(t)x+=++=1112110112121bt2b(t)ybtbtb(t)yCác điều kiện biên gồm:ymaxxmaxV(0,25)y0)0(y0)0(yV(0,25)x0)0(x4,0)0(x====== maxymaxy121110maxxmaxx121110V25,0Vb0b0bV25,0Va0a4,0a======== Xét khoảng thời gian từ 0,25ữ0,75s, robot di chuyển theo quỹ đạo bậc 1:=+=21220212a(t)xa0,25)(ta(t)x=+=21220212b(t)yb0,25)(tb(t)yTừ điều kiện biên suy ra: maxy21maxx21VbVa== Xét khoảng thời gian từ 0,75ữ1s, robot di chuyển theo quỹ đạo bậc 2: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 19 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... N=150 ta đợc: >>vkhop(150); 16 Hình 5 Đồ thị vận tốc chuyển động của khớp 17 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: 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 h22 2 2 h11 = m1lg 1 +... ylabel('s(m)&v(m/s)'); Kết quả thu đợc khi chạy chơng trình với 200 điểm quỹ đạo đợc thể hiện trên hình vẽ 3 >> cdtay(200) 13 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... chuyển động của robot nh sau: 11 1,0666t 2 + 0,4 0,5333( t 0,25 ) + 0 ,3333 x( t ) = 2 1,0666 ( t 0,75 ) 0,5333( t 0,75 ) + 0,0666 0 0 t 0,25 0,25 t 0,75 0,75 t 1 t >1 1,0666t 2 0,5333( t 0,25 ) + 0,0667 y( t ) = 2 1,0666 ( t 0,75 ) + 0 ,5333( t 0,75 ) + 0,3333 0,4 0 t 0,25 0,25 t 0,75 0,75 t 1 t >1 Chơng trình MATLAB sau thể hiện quỹ đạo chuyển động của tay robot theo vị... bằng Simulink Hình 11 Kết quả mô phỏng - đáp ứng đầu 20 Hình 12 Kết quả mô phỏng, sai lệch tĩnh của hệ 21 Tài liệu tham khảo [1] Nguyễn Mạnh Tiến Bài giảng môn Robot cao học năm 2002 [2] Lorenzo Sciavicco, Bruno Sciliano Modeling and Control of Robot Manipulators 22 ... 0,75 ) + 0 ,5333( t 0,75 ) + 0,3333 0,4 0 t 0,25 0,25 t 0,75 0,75 t 1 t >1 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 t0.25)&(t . 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. )+=0sllmsllmsllmq,qC122g122122g12222g12( )122g11211g11clclgmcglmg ++=122g22cglmg =b. Xây dựng luật điều khiểnCá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