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

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 thucnx=u(10); % CHUONG TRINH CHINH % Dong hoc...
Nội dung trích xuất từ tài liệu:
Nâng cao chất lượng điều khiển cho robot Scara, chương 16 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 RobotSerpent% qi0, qic: goc quay dau va cuoi cua khop quay thu ifunction 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 dongtd=u(5);% Thoi gian gia toc (hoac giam toc)ta=u(6);% Thong so cua Robot: a1,a2a1=u(7); %(m)a2=u(8); %(m)t=u(9); % thoi gian thuc 1nx=u(10);% CHUONG TRINH CHINH% Dong hoc nguoc va tinh cac goc quay cua tung khop tai diem dauva diem cuoiq20=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 khoph1=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 Serpentqdc1= 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 2function 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,q1cfunction 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,q4cfunction 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^4function quydao=Quydaochuan(t,h,ta,td)a0ac=h(1);a3ac=h(2);a4ac=h(3); 3a0cd=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 endelse %if td>(4*ta) %Gom 3 giai doan: gia toc, chay deu, giam toc if (t>=0)&&(t=2*ta)&&(t quydao=a0cd+a1cd*(t-2*ta);%Chay deu elseif (t>(td-2*ta))&&(ttd quydao=qcuoi; else quydao=-1;%Error endend%Tinh cac he so cua quy dao chuan ung voi tung khopfunction 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 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);endif 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 6heso=[a0ac,a3ac,a4ac,a0cd,a1cd,a0df,a1df,a3df,a4df,qcuoi];2. Tính momem chuẩn% Tinh toan momem chuanfunction 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 robotJ1=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; 7m234=m2+m3+m4;m34=m3+m4;l1=0.25; l2=0.15;% Tinh TT=-(m234*l1*l2*sin(q2d));% Tinh toan cac gia tri HijH11=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 datu1m=H11*ddq1d+H12*ddq2d+H13*ddq4d+T*dq2d^2+2*T*dq1d*dq2d;u2m=H21*ddq1d+H22*ddq2d+H23*ddq4d-T*dq1d^2;u4m=H31*ddq1d+H32*ddq2d+H33*ddq4d;momem =[u1m;u2m;u4m]; 83. Tính góc quay ban đầu%Tinh toan goc quay ban daufunction init=gocquaybd(u)x=u(1);%my=u(2);%ma1=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 ...