Nâng cao chất lượng điều khiển cho robot Scara, chương 16

Chia sẻ: Van Teo | Ngày: | Loại File: PDF | Số trang:13

0
102
lượt xem
58
download

Nâng cao chất lượng điều khiển cho robot Scara, chương 16

Mô tả tài liệu
  Download Vui lòng tải xuống để xem tài liệu đầy đủ

Tính toán Quỹ đạo chuyển động chuẩn % Tinh toan Quy dao chuyen dong chuan % Xay dung quy dao chuyen dong chuan cho 3 khop quay cua Robot Serpent % qi0, qic: goc quay dau va cuoi cua khop quay thu i function qdc=qdcdc(u) % Toa do diem dau: DDau=[X0;Y0] X0=u(1); Y0=u(2); % Toa do diem cuoi: DCuoi=[Xc;Yc] Xc=u(3); Yc=u(4); % td: Dat thoi gian chuyen dong td=u(5); % Thoi gian gia toc (hoac giam toc) ta=u(6); % Thong so cua Robot: a1,a2 a1=u(7); %(m) a2=u(8); %(m) t=u(9); % thoi gian thuc nx=u(10); % CHUONG TRINH CHINH % Dong hoc...

Chủ đề:
Lưu

Nội dung Text: Nâng cao chất lượng điều khiển cho robot Scara, chương 16

  1. Chương 16: CÁC CHƯƠNG TRÌNH M FILES (chạy trên nền Matlab/Simulink) 1. Tính toán Quỹ đạo chuyển động chuẩn % Tinh toan Quy dao chuyen dong chuan % Xay dung quy dao chuyen dong chuan cho 3 khop quay cua Robot Serpent % qi0, qic: goc quay dau va cuoi cua khop quay thu i function qdc=qdcdc(u) % Toa do diem dau: DDau=[X0;Y0] X0=u(1); Y0=u(2); % Toa do diem cuoi: DCuoi=[Xc;Yc] Xc=u(3); Yc=u(4); % td: Dat thoi gian chuyen dong td=u(5); % Thoi gian gia toc (hoac giam toc) ta=u(6); % Thong so cua Robot: a1,a2 a1=u(7); %(m) a2=u(8); %(m) t=u(9); % thoi gian thuc 1
  2. nx=u(10); % CHUONG TRINH CHINH % Dong hoc nguoc va tinh cac goc quay cua tung khop tai diem dau va diem cuoi q20=Gocquay2(X0,Y0,a1,a2); q2c=Gocquay2(Xc,Yc,a1,a2); q10=Gocquay1(X0,Y0,q20,a1,a2); q1c=Gocquay1(Xc,Yc,q2c,a1,a2); q40=Gocquay4(q10,q20,nx); q4c=Gocquay4(q1c,q2c,nx); %Khoi tao cac he so cua quy dao chuan cua 3 khop h1=Init(q10,q1c,ta,td); h2=Init(q20,q2c,ta,td); h4=Init(q40,q4c,ta,td); %Quy dao chuyen dong chuan 3 khop quay cua Robot Serpent qdc1= Quydaochuan(t,h1,ta,td); qdc2= Quydaochuan(t,h2,ta,td); qdc4= Quydaochuan(t,h4,ta,td); qdc=[qdc1;qdc2;qdc4]; %KET THUC CHUONG TRINH CHINH %CHUONG TRINH TINH GIA TRI %Tinh cac gia tri tuong ung goc quay 2: q20,q2c 2
  3. function gq2=Gocquay2(x,y,a1,a2) C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); S2=sqrt(1-C2^2); gq2=atan2(S2,C2); %Tinh cac gia tri tuong ung goc quay 1: q10,q1c function gq1=Gocquay1(x,y,q2,a1,a2) C1=((a1+a2*cos(q2))*x+a2*sin(q2)*y)/(x^2+y^2); S1=((a1+a2*cos(q2))*y-a2*sin(q2)*x)/(x^2+y^2); gq1=atan2(S1,C1); %Tinh cac gia tri tuong ung goc quay 4: q40,q4c function gq4=Gocquay4(q1,q2,nx) KK=sqrt(1-nx^2); tetaphu=atan2(KK,nx); gq4=q1+q2-tetaphu; %Xay dung quy dao chuyen dong chuan %qac=a0ac+a3ac*t^3+a4ac*t^4 %qcd=a0cd+a1cd*(t-2ta) %qdf=a0df+a1df*t+a3df*t^3+a4df*t^4 function quydao=Quydaochuan(t,h,ta,td) a0ac=h(1);a3ac=h(2);a4ac=h(3); 3
  4. a0cd=h(4);a1cd=h(5); a0df=h(6);a1df=h(7);a3df=h(8);a4df=h(9);qcuoi=h(10); if td0)&&(td=0)&&(ttd/2)&&(ttd quydao=qcuoi; else quydao=-1; %Error end else %if td>(4*ta) %Gom 3 giai doan: gia toc, chay deu, giam toc if (t>=0)&&(t=2*ta)&&(t
  5. quydao=a0cd+a1cd*(t-2*ta);%Chay deu elseif (t>(td-2*ta))&&(ttd quydao=qcuoi; else quydao=-1;%Error end end %Tinh cac he so cua quy dao chuan ung voi tung khop function heso=Init(qdau,qcuoi,ta,td) a0ac=0;a3ac=0;a4ac=0;a0cd=0;a1cd=0;a0df=0;a1df=0;a3df=0;a4df=0 ; if (td>0)&&(td
  6. a1cd=0; %Quy dao doan df: a0df=qcuoi-(qcuoi-qdau)/2; a1df=(qcuoi-qdau)/(td/2); a4df=-(3*qcuoi-3*a0df-a1df*td)/(td^4/16); a3df=-(a1df+a4df*(td^3/2))/(3*td^2/4); end if td>(4*ta) %Quy dao doan ac: a0ac=qdau; a3ac=(qcuoi-qdau)/(4*ta^2*(td-2*ta)); a4ac=-(qcuoi-qdau)/(16*ta^3*(td-2*ta)); %Quy dao doan cd: a0cd=qdau+(qcuoi-qdau)*ta/(td-2*ta); a1cd=(qcuoi-qdau)/(td-2*ta); %Quy dao doan df: a0df=qcuoi-(qcuoi-qdau)*ta/(td-2*ta); a1df=(qcuoi-qdau)/(td-2*ta); a4df=-(3*qcuoi-3*a0df-4*a1df*ta)/(16*ta^4); a3df=-(a1df+32*a4df*ta^3)/(12*ta^2); end 6
  7. heso=[a0ac,a3ac,a4ac,a0cd,a1cd,a0df,a1df,a3df,a4df,qcuoi]; 2. Tính momem chuẩn % Tinh toan momem chuan function momem=momemchuan(u) q2d=u(1); dq1d=u(2);ddq1d=u(3); dq2d=u(4);ddq2d=u(5); dq4d=u(6);ddq4d=u(7); mt=u(8);Jt=u(9); % Thong so cua robot J1=0.021; J2=0.0034; J3=0.006; J40=0.001; J4=J40+Jt; J124=J1+J2+J4; J24=J2+J4; m1=4; m2=1.5; m3=2; m40=0.6; m4=m40+mt; m1234=m1+m2+m3+m4; 7
  8. m234=m2+m3+m4; m34=m3+m4; l1=0.25; l2=0.15; % Tinh T T=-(m234*l1*l2*sin(q2d)); % Tinh toan cac gia tri Hij H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(q2d); H12=m234*l2^2+J24+m234*l1*l2*cos(q2d); H13=J4; H21=H12; H22=m234*l2^2+J24; H23=J4; H31=J4; H32=J4; H33=J4; DH=J4^2*(2*H12-H11-H22)+J4*(H11*H22-H12^2); % Dau ra % Tinh momem chuan quy dao dat u1m=H11*ddq1d+H12*ddq2d+H13*ddq4d+T*dq2d^2+2*T*dq1d*d q2d; u2m=H21*ddq1d+H22*ddq2d+H23*ddq4d-T*dq1d^2; u4m=H31*ddq1d+H32*ddq2d+H33*ddq4d; momem =[u1m;u2m;u4m]; 8
  9. 3. Tính góc quay ban đầu %Tinh toan goc quay ban dau function init=gocquaybd(u) x=u(1);%m y=u(2);%m a1=u(3);%(m) a2=u(4);%(m) nx=u(5); %q20=Gocquay2(X0,Y0,a1,a2) %q10=Gocquay1(X0,Y0,q20,a1,a2); %q40=Gocquay4(q10,q20,nx); %Tinh cac gia tri tuong ung goc quay 2: q20 C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); S2=sqrt(1-C2^2); q20=atan2(S2,C2); %Tinh cac gia tri tuong ung goc quay 1: q10 C1=((a1+a2*cos(q20))*x+a2*sin(q20)*y)/(x^2+y^2); S1=((a1+a2*cos(q20))*y-a2*sin(q20)*x)/(x^2+y^2); q10=atan2(S1,C1); %Tinh cac gia tri tuong ung goc quay 4: q40 KK=sqrt(1-nx^2); 9
  10. thetaphu=atan2(KK,nx); q40=q10+q20-thetaphu; %Dau ra init=[q10;q20;q40]; 4. Tính góc điều khiển thực % Tinh toan goc dieu khien thuc function ddtheta=tinh_ddtheta(u) % Momem dieu khien u1=u(1); u2=u(2); u4=u(3); % Thong so cua tai mt=u(4); Jt=u(5); % Goc quay theta2 thuc te q1tt=u(6);q2tt=u(7);q4tt=u(8); % Dao ham goc quay thuc te dq1tt=u(9);dq2tt=u(10); % Goc quay ban dau q10=u(11);q20=u(12);q40=u(13); % CHUONG TRINH CHINH % Tinh cac he so heso=tinh_heso(q2tt,dq1tt,dq2tt,mt,Jt); 10
  11. % Tinh dao ham cap hai cua goc quay thuc ddtheta=tinh_ddthetathuc(heso,u1,u2,u4,q10,q20,q40); % Chuong trinh tinh cac he so function HS=tinh_heso(q2tt,dq1tt,dq2tt,mt,Jt) % Thong so cua robot J1=0.021; J2=0.0034; J3=0.006; J40=0.001; J4=J40+Jt; J124=J1+J2+J4; J24=J2+J4; m1=4; m2=1.5; m3=2; m40=0.6; m4=m40+mt; m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; l1=0.25; l2=0.15; % Tinh T T=-(m234*l1*l2*sin(q2tt)); % Tinh toan cac gia tri Hij 11
  12. H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(q2tt); H12=m234*l2^2+J24+m234*l1*l2*cos(q2tt); H13=J4; H21=H12; H22=m234*l2^2+J24; H23=J4; H31=J4; H32=J4; H33=J4; DH=J4^2*(2*H12-H11-H22)+J4*(H11*H22-H12^2); % Cac gia tri dau ra bij b11=DH^-1*(H22*H33-H23*H32); b12=DH^-1*(H13*H32-H12*H33); b13=DH^-1*(H12*H23-H13*H22); b21=DH^-1*(H23*H31-H21*H33); b22=DH^-1*(H11*H33-H13*H31); b23=DH^-1*(H13*H21-H11*H23); b31=DH^-1*(H21*H32-H31*H22); b32=DH^-1*(H12*H31-H11*H32); b33=DH^-1*(H11*H22-H12^2); % Cac gia tri h1,h2 h1=T*dq2tt^2+2*T*dq1tt*dq2tt; h2=-(T*dq1tt^2); 12
  13. % Cac gia tri ai a1=-(b11*h1+b12*h2); a2=-(b21*h1+b22*h2); a4=-(b31*h1+b32*h2); HS =[b11,b12,b13,b21,b22,b23,b31,b32,b33,a1,a2,a4]; % Chuong trinh tinh dao ham cap hai cua goc quay thuc function d2theta=tinh_ddthetathuc(heso,u1,u2,u4,q10,q20,q40) b11=heso(1);b12=heso(2);b13=heso(3); b21=heso(4);b22=heso(5);b23=heso(6); b31=heso(7);b32=heso(8);b33=heso(9); a1=heso(10);a2=heso(11);a4=heso(12); % Tinh dao ham cap hai cua goc quay thuc d2theta1=a1+b11*u1+b12*u2+b13*u4; d2theta2=a2+b21*u1+b22*u2+b23*u4; d2theta4=a4+b31*u1+b32*u2+b33*u4; d2theta=[d2theta1;q10;d2theta2;q20;d2theta4;q40]; 13

CÓ THỂ BẠN MUỐN DOWNLOAD

Đồng bộ tài khoản