1. Trang chủ
  2. » Kỹ Thuật - Công Nghệ

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

13 249 0

Đang tải... (xem toàn văn)

Tài liệu hạn chế xem trước, để xem đầy đủ mời bạn chọn Tải xuống

THÔNG TIN TÀI LIỆU

Thông tin cơ bản

Định dạng
Số trang 13
Dung lượng 1,12 MB

Nội dung

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 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 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); 4 a0cd=h(4);a1cd=h(5); a0df=h(6);a1df=h(7);a3df=h(8);a4df=h(9);qcuoi=h(10); if td<=0 td=0; quydao=-1; %Error elseif (td>0)&&(td<=4*ta) %Gom 2 giai doan: gia toc, giam toc if (t>=0)&&(t<=td/2) %Gia toc quydao=a0ac+a3ac*t^3+a4ac*t^4; %Gia toc elseif (t>td/2)&&(t<=td) %Giam toc quydao=a0df+a1df*(t-td/2)+a3df*(t-td/2).^3+a4df*(t-td/2).^4; %Giam toc elseif t>td 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) quydao=a0ac+a3ac*t^3+a4ac*t^4;%Gia toc elseif (t>=2*ta)&&(t<=(td-2*ta)) 5 quydao=a0cd+a1cd*(t-2*ta);%Chay deu elseif (t>(td-2*ta))&&(t<=td) quydao=a0df+a1df*(t-td+2*ta)+a3df*(t-td+2*ta).^3+a4df*(t- td+2*ta).^4;%Giam toc elseif t>td 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<=4*ta) %Quy dao doan ac:(thay ta=td/4) a0ac=qdau; a3ac=(qcuoi-qdau)/(td^3/8); a4ac=-(qcuoi-qdau)/(td^4/8); %Quy dao doan cd: a0cd=0; 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 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; 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]; 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); 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); [...]...% 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)); . 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. 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)); . KK=sqrt(1-nx^2); 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);

Ngày đăng: 02/07/2014, 22:21

TỪ KHÓA LIÊN QUAN

TÀI LIỆU CÙNG NGƯỜI DÙNG

TÀI LIỆU LIÊN QUAN

w