Thực hiện mô phỏng hệ thống di chuyển theo quỹ đạo hình chữ O hoặc C Nội suy đường tròn tâm Ix0,y0 bán kính R tùy chọn.. Trình bày và nêu ví dụ minh họa thông số tùy chọn về nội suy thô
ĐỀ BÀI
Một hệ thống CNC gồm 2 trục X và Y được điều khiển bởi 2 động cơ có bộ driver điều khiển tốc độ kèm theo Giả sử hàm truyền với đầu vào là điện áp điều khiển, đầu ra là tốc độ của 2 hệ truyền động 2 trục X và Y là khâu quán tính bậc nhất :
Trục y: Uy Vy = T 2 k s+ 2 1 Với k 1 ≠ k 2, T 1 ≠ T 2 tự chọn, và T 1 , T 2 nhỏ (< 0.5).
1 Tự chọn thông số 02 hàm truyền trên và thiết kế bộ điều khiển vị trí cho từng trục thỏa mãn các yêu cầu sau
- Thời gian xác lập nhỏ nhất
2 Thực hiện nội suy đường thẳng và đường tròn với kiểu tăng tốc/giảm tốc dạng hình thang (tăng tốc/giảm tốc trước nội suy) Điểm đầu, điểm cuối, bán kính tùy chọn
- Thực hiện cả nội suy thô và nội suy tinh
- Tự chọn vận tốc tối đa, gia tốc tối đa
- Chu kỳ nội suy thô là 5ms, nội suy tinh là 1ms
3 Thực hiện mô phỏng hệ thống di chuyển theo quỹ đạo hình chữ O hoặc C Nội suy đường tròn tâm I(x 0, y 0 ) bán kính R tùy chọn chu kỳ nội suy 5ms, chu kì điều khiển 1ms Yêu cầu gia tốc giảm tốc trước khi nội suy Xác định quỹ đạo chuyển động theo thời gian với profile vận tốc hình thang, a max và v max tự chọn.
Chia tiếp 5ms nội suy theo 2 phương pháp tuyến tính và trung bình.
4 Trình bày và nêu ví dụ minh họa (thông số tùy chọn) về nội suy thô đường thẳng và đường tròn với kiểu tăng tốc/giảm tốc dạng hình thang (tăng tốc/giảm tốc sau nội suy) Gợi ý: Từ quỹ đạo tính ra các bước di chuyển cho từng trục, sau đó dùng bộ lọc số để thực hiện tăng tốc/giảm tốc
5 Ghi chú: Mỗi nhóm sinh viên chuẩn bị một quyển báo cáo, có kèm mã chương trình mô phỏng File code chương trình mô phỏng cũng phải nộp cho giáo viên.
BÀI LÀM
Thiết kế bộ điều khiển PID vị trí cho từng trục
* Hai hàm truyền của đối tượng:
Đáp ứng càng nhanh càng tốt
Sai lệch tĩnh =0, độ quá điều chỉnh độ dài AB = L, với chu kỳ nội suy T Vận tốc ăn dao lớn nhất Fmax góc hợp bởi đoạn thẳng và Ox là góc θ
Vì quá trình nội suy các trục diễn ra trước nên ta coi vận tốc ăn dao trên quỹ đạo là không đổi và bằng vận tốc max Fmax Nên trong các chu kỳ nội suy, đoạn dịch chuyển AL là bằng nhau và bằng L[k] = Fmax*T=ALVới T là chu kì nội suy
Mỗi chu kì nội suy, dao sẽ đi được 1 khoảng ∆ L , tương ứng trên trục Ox sẽ đi được 1 khoảng ∆ x = ∆ L cos θ và trên trục Oy đi được 1 khoảng ∆ y =
Từ đó ta có các đoạn dịch chuyển ax, ay trong các chu kì nội suy
Tăng tốc/ giảm tốc sau nội suy
Với mỗi Tcyc = 5ms đi được một đoạn ∆ L
Chọn a = d = 20mm/s 2 , Fmax = v max = 20mm/s
Nội suy trong khoảng thời gian 3.354s, m = 3.354 0.005 = 670.8 sấp sỉ 671
Kết quả của phép nội suy thô (phép tích chập sử dụng bộ lọc số) được thể hiện dưới đây: k AL[k] AL(k-200) S(k) AL0(k)
Về cơ bản, các tăng tốc/ giảm tốc sau nội suy đường tròn giống với nội suy đường thẳng Điểm khác là ∆ x , ∆ y thay đổi và ảnh hưởng bởi giá trị xuất phát cũ. x[k] = m L = ∆ L = √ DX(i ) 2 + DY (i ) 2 // bỏVận tốc góc ω = F R = 20 20 (rad/s), ω max = 1rad/s, aht max = dht max = 1 rad/s 2
Vì quá trình nội suy các trục diễn ra trước nên ta coi vận tốc ăn dao trên quỹ đạo là không đổi và bằng vận tốc max Wmax Nên trong các chu kỳ nội suy, góc dịch chuyển Aalpha là bằng nhau và bằng alpha[k] = Wmax*T, T là chu kì nội suy
Và theo công thức tính đoạn dịch chuyển từng chu kì nội suy
Ta sẽ tìm ra Dx, Dy trong các chu kì nội suy
Sau đó ta thực hiện tăng tốc/ giảm tốc bằng công thức sai phân của bộ lọc số bên trên, ta thu được các giá trị Dx, DY thực tế để đưa vào bộ điều khiển vị trí.
Lựa chọn đường tròn có tâm I (20, 20), bán kính R mm Điểm bắt đầu nội suy là A(20, 00), điểm kết thúc nội suy cũng tại A Nội suy nửa đường tròn, nửa còn lại làm tương tự Chọn Fmax = 20mm/s, gia tốc tăng giảm tốc là 20mm/s 2 , góc quay 2 π , chu kì nội suy 5ms. với dL(k) = 10 x 0.005 = 0.05 rad
Kết quả của phép nội suy thô (phép tích chập sử dụng bộ lọc số) được thể hiện dưới đây:
Hình 24: Nội suy thô đoan AB
Hình 27: Đồ thị vận tốc đoạn thẳng
Hình 28: Nội suy thô sau tăng giảm tốc đường tròn
Hình 31: Đồ thị tốc độ
%noi suy duong thang clear clc
Tdk = 1e-3; xA = 30; yA = 50; xB = 70; yB = 80;
L = sqrt((xB-xA)^2+(yB-yA)^2); cos =(xB-xA)/L; sin =(yB-yA)/L;
%ma tran bo nho nA = 0:1:(Ta/Tipo); %them gia tri 0 the hien toc do ban dau la 0 nT = 1:1:(T/Tipo); nC = 1:1:(Tc/Tipo); nD = 1:1:(Td/Tipo);
%noi suy tho dL = V; for i=1:(length(V)-1) dL(i) = ((V(i)+V(i+1))/2)*Tipo; end
%do thi noi suy tho duong thang figure(1) stairs(X, Y, 'linewidth', 1.5) grid on hold on axis([-10 100, -10 100]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho duong thang AB')
%do thi toc do figure(4) time = Tipo*(1:(fix(T/Tipo)+1)); plot (time', dL/Tipo, 'linewidth', 1.5) grid on axis([-1 5, -1 30]) xlabel('t (s)') ylabel('V (m/s)') title('Do thi toc do')
%noi suy tinh su dung phuong phap tuyen tinh
N = Tipo/Tdk; dL_t = dL(1)/5; for i = 1:length(dL) if i == 1 for j = 1:N-1 dL_t = [dL_t, dL(i)/N]; end else for j = 1:N dL_t = [dL_t, dL(i)/N]; end end end
Y = yA + cumsum(dL_t)*sin; figure(1) stairs(X, Y, 'linewidth', 1.5) grid on hold on axis([-10 100, -10 100]) ylabel('Y') title('Noi suy tinh duong thang AB')
% Import Data to Simulink time = Tipo*(1:(fix(T/Tipo)+1)*5); % Chu ki trich mau 1ms in_x = [time', X']; in_y = [time', Y'];
%dau ra X figure(2) plot(time', X', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('X') title('dau ra X')
%dau ra Y figure(3) plot(time', Y', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('Y') title('dau ra Y')
%noi suy duong tron clear clc
%thong so duong tron xA = 2; yA = 11; %diem bat dau xB = 8; yB = 11; %diem ket thuc xO = 20; yO = 20; %tam duong tron
R = 20; vecOA = [xA-xO yA-yO]; vecOB = [xB-xO yB-yO];
L_OB = sqrt(vecOB(1)^2 + vecOB(2)^2); angleAOB = acos(dot(vecOA, vecOB)/(L_OA*L_OB));
%L = R * angleAOB; %do dai cung tron
%ma tran bo nho nA = 0:1:(Ta/Tipo); %them gia tri 0 the hien toc do ban dau la 0 nC = 1:1:ceil(Tc/Tipo); nD = 1:1:(Td/Tipo); nT = 1:1:ceil(T/Tipo);
%tinh goc di chuyen trong noi Tipo angle = W*Tipo;
X(i) = xO + (X(i-1) - xO)*cos(angle(i)) + (yO - Y(i-1))*sin(angle(i)); Y(i) = yO - (yO - Y(i-1))*cos(angle(i)) + (X(i-1) - xO)*sin(angle(i)); end figure(1) stairs(X, Y, 'linewidth', 1.5) grid on hold on axis([-10 50, -10 50]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho duong tron')
%dau ra X time = Tipo*(1:(fix(T/Tipo)+2)); figure(2) plot(time', X', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('X') title('dau ra X')
%dau ra Y figure(3) plot(time', Y', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('Y') title('dau ra Y')
%do thi toc do figure(4) plot(time', angle*R/Tipo, 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('V (m/s)') title('do thi toc do')
%noi suy tinh su dung phuong phap tuyen tinh
Y_t(t) = yA; for i = 1:length(angle_t) if i == 1 for j = 2:N t = t+1;
X_t(t) = xO + (X_t(t-1) - xO)*cos(angle_t(i)) + (yO - Y_t(t-
Y_t(t) = yO - (yO - Y_t(t-1))*cos(angle_t(i)) + (X_t(t-1) - xO)*sin(angle_t(i)); end else
X_t(t) = xO + (X_t(t-1) - xO)*cos(angle_t(i)) + (yO - Y_t(t-
Y_t(t) = yO - (yO - Y_t(t-1))*cos(angle_t(i)) + (X_t(t-1) - xO)*sin(angle_t(i)); end end end figure(1) stairs(X_t, Y_t, 'linewidth', 1.5) grid on hold on axis([-10 50, -10 50]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho va noi suy tinh duong tron')
% Import Data to Simulink time = Tipo*(1:(fix(T/Tipo)+2)*5); % Chu ki trich mau 1ms in_x = [time', X_t']; in_y = [time', Y_t'];
%noi suy chu C clear clc
%cung AB xA = 60; yA = 50; %diem bat dau xB = 20; yB = 50; %diem ket thuc xO1 = 40; yO1 = 50; %tam cung tron
%cung CD xC = xB; yC = yB-30; %diem bat dau xD = xA; yD = yA-30; %diem ket thuc xO2 = xO1; yO2 = yO1-30;%tam cung tron
%tinh do dai quang duong vecO1A = [xA-xO1 yA-yO1]; vecO1B = [xB-xO1 yB-yO1];
L_O1B = sqrt(vecO1B(1)^2 + vecO1B(2)^2); angleAO1B = acos(dot(vecO1A, vecO1B)/(L_O1A*L_O1B));
L_AB = R * angleAO1B; %do dai cung tron AB vecO2C = [xC-xO2 yC-yO2]; vecO2D = [xD-xO2 yD-yO2];
L_O2D = sqrt(vecO2D(1)^2 + vecO2D(2)^2); angleCO2D = acos(dot(vecO2C, vecO2D)/(L_O2C*L_O2D));
L_CD = R * angleCO2D; %do dai cung tron CD
L_BC = sqrt((xB-xC)^2+(yB-yC)^2); %do dai doan BC
%noi suy tho cung AB
%ma tran bo nho nA = 0:1:(Ta/Tipo); %them gia tri 0 the hien toc do ban dau la 0 nC = 1:1:ceil(Tc/Tipo); nD = 1:1:(Td/Tipo); nT = 1:1:ceil(T_AB/Tipo);
%tinh goc di chuyen trong noi Tipo angle = W*Tipo; angle_AB = angle;
Y_AB(1) = yA; for i = 2:length(angle)
X_AB(i) = xO1 + (X_AB(i-1) - xO1)*cos(angle(i)) + (yO1 - Y_AB(i-
Y_AB(i) = yO1 - (yO1 - Y_AB(i-1))*cos(angle(i)) + (X_AB(i-1) - xO1)*sin(angle(i)); end
% noi suy tho cung CD
%ma tran bo nho nA = 0:1:(Ta/Tipo); %them gia tri 0 the hien toc do ban dau la 0 nC = 1:1:ceil(Tc/Tipo); nD = 1:1:(Td/Tipo); nT = 1:1:ceil(T_CD/Tipo);
%tinh goc di chuyen trong noi Tipo angle = W*Tipo; angle_CD = angle;
Y_CD(1) = yC; for i = 2:length(angle)
X_CD(i) = xO2 + (X_CD(i-1) - xO2)*cos(angle(i)) + (yO2 - Y_CD(i-
Y_CD(i) = yO2 - (yO2 - Y_CD(i-1))*cos(angle(i)) + (X_CD(i-1) - xO2)*sin(angle(i)); end
%noi suy tho doan thang BC c =(xC-xB)/L_BC; s =(yC-yB)/L_BC;
%ma tran bo nho nA = 0:1:(Ta/Tipo); %them gia tri 0 the hien toc do ban dau la 0 nT = 1:1:(T_BC/Tipo); nC = 1:1:(Tc/Tipo); nD = 1:1:(Td/Tipo);
V = [Va, Vc, Vd]; dL_BC = V; for i=1:(length(V)-1) dL_BC(i) = ((V(i)+V(i+1))/2)*Tipo; end
X_BC = xB + cumsum(dL_BC)*c;
Y_BC = yB + cumsum(dL_BC)*s;
% ve noi suy tho chu C figure(2) stairs(X_AB, Y_AB, 'linewidth', 1.5) hold on stairs(X_BC, Y_BC, 'linewidth', 1.5) hold on stairs(X_CD, Y_CD, 'linewidth', 1.5) grid on hold on axis([-10 100, -10 100]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho chu C')
%noi suy tinh cung AB
Y_t_AB(t) = yA; for i = 1:length(angle_t) if i == 1 for j = 2:N t = t+1;
X_t_AB(t) = xO1 + (X_t_AB(t-1) - xO1)*cos(angle_t(i)) + (yO1 - Y_t_AB(t-1))*sin(angle_t(i));
Y_t_AB(t) = yO1 - (yO1 - Y_t_AB(t-1))*cos(angle_t(i)) + (X_t_AB(t-
1) - xO1)*sin(angle_t(i)); end else for j = 1:N t = t+1;
X_t_AB(t) = xO1 + (X_t_AB(t-1) - xO1)*cos(angle_t(i)) + (yO1 - Y_t_AB(t-1))*sin(angle_t(i));
Y_t_AB(t) = yO1 - (yO1 - Y_t_AB(t-1))*cos(angle_t(i)) + (X_t_AB(t-
1) - xO1)*sin(angle_t(i)); end end
%noi suy tinh cung CD angle_t = angle/N; t=1;
Y_t_CD(t) = yC; for i = 1:length(angle_t) if i == 1 for j = 2:N t = t+1;
X_t_CD(t) = xO2 + (X_t_CD(t-1) - xO2)*cos(angle_t(i)) + (yO2 - Y_t_CD(t-1))*sin(angle_t(i));
Y_t_CD(t) = yO2 - (yO2 - Y_t_CD(t-1))*cos(angle_t(i)) + (X_t_CD(t-
1) - xO2)*sin(angle_t(i)); end else for j = 1:N t = t+1;
X_t_CD(t) = xO2 + (X_t_CD(t-1) - xO2)*cos(angle_t(i)) + (yO2 - Y_t_CD(t-1))*sin(angle_t(i));
Y_t_CD(t) = yO2 - (yO2 - Y_t_CD(t-1))*cos(angle_t(i)) + (X_t_CD(t-
1) - xO2)*sin(angle_t(i)); end end end
%noi suy tinh doan BC dL_t_BC = dL_BC(1)/N; for i = 1:length(dL_BC) if i == 1 for j = 1:N-1 dL_t_BC = [dL_t_BC, dL_BC(i)/N]; end else for j = 1:N dL_t_BC = [dL_t_BC, dL_BC(i)/N]; end end endX_t_BC = xB + cumsum(dL_t_BC)*c;
Y_t_BC = yB + cumsum(dL_t_BC)*s;
%ve noi suy tinh va tho hinh chu C figure(2) stairs(X_t_AB, Y_t_AB, 'linewidth', 1.5) hold on stairs(X_t_BC, Y_t_BC, 'linewidth', 1.5) hold on stairs(X_t_CD, Y_t_CD, 'linewidth', 1.5) grid on hold on axis([-10 100, -10 100]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho va tinh chu C')
%ve van toc an dao figure(3) time_AB = Tipo*(1:(fix(T_AB/Tipo)+2)); time_BC = max(time_AB) + (Tipo*(1:(fix(T_BC/Tipo)+1))); time_CD = max(time_BC) + (Tipo*(1:(fix(T_CD/Tipo)+2))); plot(time_CD', angle_CD*R/Tipo, 'linewidth', 1.5) hold on plot(time_BC', dL_BC/Tipo, 'linewidth', 1.5) hold on grid on axis([-1 12, -1 30]) xlabel('t(s)') ylabel('V(mm/s)') title('do thi van toc')
Y = [Y_t_AB, Y_t_BC, Y_t_CD]; time = Tipo*(1:(fix(T/Tipo)+5)*5); % Chu ki trich mau 1ms in_x = [time', X']; in_y = [time', Y'];
%ve tin hieu suat ra tu simulink
%dau ra X figure(1) plot(time', X', 'linewidth', 1.5) grid on axis([-1 65, -1 85]) xlabel('t(s)') ylabel('X') title('dau ra X')
%dau ra Y figure(4) plot(time', Y', 'linewidth', 1.5) grid on axis([-1 65, -1 85]) xlabel('t(s)') ylabel('Y') title('dau ra Y')
Tăng/Giảm tốc sau nội suy đường thẳng
%tang giam toc sau noi suy doan thang clear clc
Tdk = 1e-3; xA = 30; yA = 50; xB = 90; yB = 80;
L = sqrt((xB-xA)^2+(yB-yA)^2); cos =(xB-xA)/L; sin =(yB-yA)/L;
Ta = F/A; nT = 1:1:(ceil(T/Tipo)); k = zeros(1, ceil(Ta/Tipo)); n = length(k); dL = L/max(nT); for i=2:length(nT) dL = [dL, L/max(nT)]; end
P = [dL, k]; %dong vai tro la ?X(k)
Q = [k,dL]; %dong vai tro la ?X(k ?n)
S = P/n; %dong vai tro la ?Xo(k) for i=2:length(P)
%do thi noi suy tho doan thang figure(1) stairs(X, Y, 'linewidth', 1.5) grid on hold on axis([-10 100, -10 100]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho doan thang AB')
% Import Data to Simulink time = Tipo*(1:(ceil(T/Tipo)+n)); % Chu ki trich mau 1ms in_x = [time', X']; in_y = [time', Y'];
%do thi bieu thi toc do figure(2) plot(time', S/Tipo, 'linewidth', 1.5) grid on hold on axis([-1 5, -1 30]) xlabel('X') ylabel('Y') title('do thi bieu thi toc do')
%dau ra X figure(3) plot(time', X', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('X') title('dau ra X')
%dau ra Y figure(4) plot(time', Y', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('Y') title('dau ra Y')
Tăng/Giảm tốc sau nội suy đường tròn:
%tang giam toc sau noi suy duong tron clear clc
%thong so duong tron xA = 20; yA = 00; %diem bat dau xB = 40; yB = 20; %diem ket thuc xO = 20; yO = 20; %tam duong tron
R = 20; vecOA = [xA-xO yA-yO]; vecOB = [xB-xO yB-yO];
L_OB = sqrt(vecOB(1)^2 + vecOB(2)^2); angleAOB = acos(dot(vecOA, vecOB)/(L_OA*L_OB));
%L = R * angleAOB; %do dai cung tron
T = L/F; nT = 1:1:(ceil(T/Tipo)); k = zeros(1, ceil(Ta/Tipo)); n = length(k);
W = F/R; angle = W*Tipo; for i=1:length(nT) angle = [angle, W*Tipo]; end
P = [angle, k]; %dong vai tro la ?X(k)
Q = [k, angle]; %dong vai tro la ?X(k ?n)
S = P/n; %dong vai tro la ?Xo(k) for i=2:length(P)
X(i) = xO + (X(i-1) - xO)*cos(S(i)) + (yO - Y(i-1))*sin(S(i));
Y(i) = yO - (yO - Y(i-1))*cos(S(i)) + (X(i-1) - xO)*sin(S(i)); end figure(1) stairs(X, Y, 'linewidth', 1.5) grid on hold on axis([-10 50, -10 50]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho duong tron')
%Import Data to Simulink time = Tipo*(1:(ceil(T/Tipo)+n+1)); % Chu ki trich mau 1ms in_x = [time', X'];
%dau ra X figure(2) plot(time', X', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('X') title('dau ra X')
%dau ra Y figure(3) plot(time', Y', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('Y') title('dau ra Y')
%do thi bieu thi toc do dai figure(4) plot(time', (S/Tipo)*R, 'linewidth', 1.5) grid on hold on axis([-1 10, -1 30]) xlabel('time') ylabel('V') title('do thi bieu thi toc do dai')