Nâng cao chất lượng điều khiển cho robot Scara, chương 16
lượt xem 62
download
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...
Bình luận(0) Đăng nhập để gửi bình luận!
Nội dung Text: 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 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
- 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
- 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
- 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
- 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
- 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
- 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
- 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
- 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
- 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
- % 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
- 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
- % 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
-
Giải pháp nâng cao chất lượng dịch vụ - Công nghệ VoIP: Phần 2
150 p | 149 | 24
-
Nghiên cứu điều khiển hệ thống nâng hạ gầu của máy xúc thủy lực và việc nâng cao chất lượng điều chỉnh bằng cơ cấu servo
4 p | 123 | 18
-
Nâng cao chất lượng điều khiển ổn định hệ thống điện bằng bộ điều khiển thiết kế theo lý thuyết tối ưu RH
6 p | 109 | 10
-
Nâng cao chất lượng điều khiển robot Scara 4 bậc tự do
6 p | 117 | 10
-
Nâng cao chất lượng điều khiển hệ thống cân băng định lượng bằng bộ điều khiển mờ thích nghi
4 p | 75 | 10
-
Nghiên cứu nâng cao chất lượng điều khiển chính xác cho động cơ PMSM ứng dụng trong sản xuất công nghiệp trên cơ sở phương pháp điều khiển trượt mờ thích nghi
13 p | 62 | 6
-
Nâng cao chất lượng hệ điều khiển chuyển động động cơ một chiều không chốt than bằng chiến lược điều khiển đơn giản
6 p | 87 | 5
-
Kết hợp bộ điều khiển LQR và CBF nâng cao chất lượng điều khiển cho robot cân bằng trên bóng (Ballbot)
3 p | 9 | 5
-
Về một giải pháp nâng cao chất lượng điều khiển hệ thống có khâu trễ dùng bộ điều khiển ngoại suy và luật điều khiển PID số
4 p | 64 | 5
-
Nâng cao chất lượng điều khiển hệ thống van điều khiển – bình mức bằng bù mờ PID
5 p | 24 | 4
-
Giải pháp nâng cao chất lượng điều khiển động cơ điện một chiều sử dụng DSP
7 p | 80 | 4
-
Nâng cao chất lượng điều khiển hệ truyền động điện gió trên cơ sở bộ điều khiển thông minh sử dụng máy phát điện BLDCG
9 p | 17 | 3
-
Nghiên cứu giải pháp nâng cao chất lượng điều khiển trực tiếp công suất chỉnh lưu tích cực trong hệ thống truyền động điện điều khiển trực tiếp momen
15 p | 7 | 3
-
Nâng cao chất lượng điều khiển cho Robot hai bánh tự cân bằng
10 p | 44 | 3
-
Nâng cao chất lượng hệ truyền động bám góc sử dụng động cơ đồng bộ kích thích vĩnh cửu công suất nhỏ bằng phương pháp điều khiển thích nghi modal kết hợp với bù các đặc tính tĩnh
8 p | 32 | 1
-
Nâng cao chất lượng hệ thống định vị động tàu dịch vụ dưới tác động không mong muốn với giải thuật Fuzzy Takagi-Sugeno
4 p | 56 | 1
-
Nâng cao chất lượng điều khiển cơ cấu Gimbal hai trục bằng bộ điều khiển mờ Fuzzy-PID
10 p | 7 | 1
-
Nâng cao chất lượng điều khiển ổn định hệ thống điện bằng bộ điều khiển thiết kế theo lý thuyết tối ưu
6 p | 102 | 0
Chịu trách nhiệm nội dung:
Nguyễn Công Hà - Giám đốc Công ty TNHH TÀI LIỆU TRỰC TUYẾN VI NA
LIÊN HỆ
Địa chỉ: P402, 54A Nơ Trang Long, Phường 14, Q.Bình Thạnh, TP.HCM
Hotline: 093 303 0098
Email: support@tailieu.vn