BÁO cáo bài tập lớn hệ THỐNG điều KHIỂN máy CNC một hệ thống CNC gồm 2 trục x và y được điều khiển bởi 2 động cơ

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 Ix 0, y 0 bán kính R tùy chọn.. Xác định quỹ đạo chuyển động theo thời gian với profile vận t

BÁO CÁO BÀI TẬP LỚN

HỆ THỐNG ĐIỀU KHIỂN MÁY CNC

Giảng Viên hướng dẫn: TS Dương Minh Đức

Nhóm sinh viên thực hiện: Nhóm 7

1 Thiết kế bộ điều khiển PID vị trí cho từng trục 4

2 Nội suy 11

2.1 Nội suy đường thẳng 11

2.1.1 Công thức tính toán nội suy đường thẳng 11

2.1.2 Nội suy thô ……… 12

2.1.3 Nội suy tinh……… ………13

2.1.2 Kết quả mô phỏng 16

2.2 Nội suy đường tròn 18

2.2.1 Xây dựng công thức nội suy đường tròn 19

2.2.2 Mô phỏng 20

3 Vẽ Chữ C 20

3.1 Tính toán lý thuyết 20

3.2 Mô phỏng 21

4 Thực hiện tăng tốc/ giảm tốc sau nội suy………

23 4.1 Tổng quan chung 24

4.2 Nội suy đoạn thẳng……….25

4.3 Nội suy đường tròn ……… 27

5 CODE MATLAB……… 33

- 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.

 Sai lệch tĩnh =0, độ quá điều chỉnh <5%

 dùng phương pháp tối ưu độ lớn để thiết kế bộ điều khiển

Sơ đồ cho trục X, Y:

Hình 1 : Sơ đồ điều khiển vị trí 2 trục X, Y

* Xác định thông số của bộ PID:

Sử dụng PID tuning trong matlab sau đó hiệu chỉnh để đạt được kết quả tốt hơn.

+ thời gian xác lập ≈ 0.11s + Độ quá điều chỉnh ≈ 1.3%

Trục Y

Hình 3: Đáp ứng trục Y

- Trục y: bộ điều khiển đã đạt yêu cầu bài toán với các chỉ tiêu:

+ Sai lệch tĩnh ≈ 0

Giả sử điểm A(70;80), B(30;50) Xây dựng Profile tốc độ cho chuyển động thẳng A(30;50) - B(70;80) Chiều dài: L= √ (x B −x A ) 2 +( y B − y A ) 2 = √ (30−70) 2 +(50−80) 2 =50 (mm)

Gia tốc tang tốc cho phép: A=20(mm/ s 2 ) Gia tốc giảm tốc cho phép: D=20(mm/ s 2 ) Chu kỳ nội suy: τ =5 (ms)

Các bước thực hiện cho bài toán

Hình 4: Nội suy đường thẳng

Trong đó: V i là vận tốc của khoảng thời gian thứ i và V o =V m =20(mm/ s)

L i là độ dịch chuyển của lần lấy mẫu thứ i

2.1.3 Nội suy tinh

 Phương pháp:

+)Phương pháp tuyến tính +)Phương pháp trung bình

Hình 5: chu kì trích mẫu nội suy và chu kì điều khiển

Hình 7: Tính toán nội suy tuyến tính trục Y

2.1.2Kết quả mô phỏng

-Nội suy thô và tinh đoạn thẳng AB

Hình 8: Nội suy thô và tinh đoạn AB

Hình 9: đồ thị trục X

Hình 10: đồ thị trục Y

Hình 11: Profile vận tốc

Từ hình vẽ, ta xây dựng được công thức:

Cosθ i+1 = Acosθ i −Bsinθ i

Sin θ i+1 = Asinθ i + Bcosθ i

Đặt A = cos(𝜶), B = sin(𝜶) Với θ i+1 = 𝜽 i + 𝜶

=>

Y(i+1) = R i .𝒄𝒐𝒔 θ i+1 = AX(i)−BY(i) Y(i+1)= R i sin θ i+1 = AY(i) + BX(i) Suy ra:

DX(i) = X(i+1)−X(i)=(A−1)X(i)−BY(i) DY(i) = Y(i+1)−Y(i)=(A−1)Y(i) +BX(i)

Ta sẽ tìm góc lệch alpha nhờ vận tốc góc mỗi chu kì tính toán.

Lựa chọn đường tròn có tâm I (20, 20), bán kính R=20 Điểm bắt đầu nội suy là A(2,11), điểm kết thúc nội suy là B ( 8,11) Nội suy nửa đường tròn, nửa còn lại làm tương tự Chọn

F max = 20mm/s, gia tốc tăng giảm tốc là 20mm/s 2 , góc quay π , chu kì nội suy 5ms.

Áp dụng thuật toán Euler ta xác định được góc   4 /  R 

2 / 5 

Hình 12: Lý thuyết tính độ dịch theo góc lệch alpha

Nội suy thô sai lệch đường tròn

Hình 14: sự khác nhau giữa nội suy thô và nội suy tinh

Hình 16: Đồ thị trục Y

Hình 17: Đồ thị vận tốc

O1(40,50) bán kính R=20.

- Nội suy đường thẳng từ điểm B(20;50) đến điểm C(20;20)

- Nội suy đường tròn từ điểm C(20;20) đến điểm D(60;20) tâm O2(40;20) bán kính R=20.

Quy trình thiết kế tăng tốc giảm tốc nội suy như phần đường thẳng và đường tròn

Hình 18: Quỹ đạo chữ C

3.2 Mô phỏng

Hình 19: Nội suy tinh và thô chữ C

Hình 21: Đồ thị trục X

Hình 22: Đồ thị trục Y

Hình 23: Đồ thị vận tốc

Phương pháp tăng/giảm tốc sau nội suy sử dụng bộ lọc số với phép tính tích chập

Bộ lọc số dựa trên lí thuyết phép tính tích chập giữa hàm h[n] là hàm đáp ứng xung, được xác định bằng công thức ∑ k=0 n h [ k ]=1 và hàm x[n] là giá trị đầu vào Và đầu ra được biểu diễn theo công thức:

f[n] =x[n] *h[n] = ∑ k =1 n x [ k ]∗h[n−k]

Áp dụng với profile vận tốc dạng tuyến tính, ta có bộ lọc số dạng rời rạc theo công thức:

H L (z) = m 1 1−z 1−z −m −1 (*) Sau đó ta dùng phương pháp Time-Shifting, ta biến đổi công thức * dưới dạng sai phân:

AX (k) = m 1 ( i (k) – V i (k-m)) + AX(k-1)

Với m thỏa mãn r=mT, r là thời gian tăng tốc max, T là chu kì nội suy.

4.2 Đường thẳng

Nội suy thô đoạn thẳng AB với A(x s , y s ) , B( x e , y e ), => độ dài AB = L,

thẳng và Ox là góc θ

Tính toán nội suy

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 F max 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=AL Với T là chu kì nội suy

 Tăng tốc/ giảm tốc sau nội suy

 Ví dụ minh họa Chọn A(30,50), B(90,80), => AB = L = 67.082 (mm) Với mỗi T cyc = 5ms đi được một đoạn ∆ L

Chọn a = d = 20mm/s 2 , F max = 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

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, a ht max = d ht 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

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

DX(i) = X(i+1)−X(i) = (A−1)X(i)−BY(i) DY(i) = Y(i+1)−Y(i) = (A−1)Y(i) +BX(i)

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=20mm Đ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

giảm tốc là 20mm/s 2 , góc quay 2π , chu kì nội suy 5ms.

DX(i) = X(i+1)−X(i) = (A−1)X(i)−BY(i) DY(i) = Y(i+1)−Y(i) = (A−1)Y(i) +BX(i) 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 25: Đồ thị trục X

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 29: Đầu ra trục X

Hình 30: Đầu ra trục Y

4 CODE MATLAB Nội suy đường thẳng

%noi suy duong thang

clear clc

%ma tran bo nho

nA = 0:1:(Ta/Tipo); %them gia tri 0 the hien toc do ban dau la 0

hold on

axis([-10 100, -10 100]) axis equal

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

X = xA + cumsum(dL_t)*cos;

Y = yA + cumsum(dL_t)*sin;

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 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' )

Nội suy đường tròn:

%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_OA = sqrt(vecOA(1)^2 + vecOA(2)^2);

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

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)

ylabel( 'V (m/s)' ) title( 'do thi toc do' )

%noi suy tinh su dung phuong phap tuyen tinh

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

xA = 60; yA = 50; %diem bat dau

xB = 20; yB = 50; %diem ket thuc

xO1 = 40; yO1 = 50; %tam cung tron

R1 = R;

xC = xB; yC = yB-30; %diem bat dau

xD = xA; yD = yA-30; %diem ket thuc

xO2 = xO1; yO2 = yO1-30; %tam cung tron

R2 = R;

%tinh do dai quang duong

vecO1A = [xA-xO1 yA-yO1];

vecO1B = [xB-xO1 yB-yO1];

L_O1A = sqrt(vecO1A(1)^2 + vecO1A(2)^2);

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_O2C = sqrt(vecO2C(1)^2 + vecO2C(2)^2);

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

Ta = F/A;

Td = F/D;

Tc = (L_AB - F^2/(2*D) - F^2/(2*A))/F;

T_AB = Td + Ta + Tc;

nA = 0:1:(Ta/Tipo); %them gia tri 0 the hien toc do ban dau la 0

X_CD(i) = xO2 + (X_CD(i-1) - xO2)*cos(angle(i)) + (yO2 - 1))*sin(angle(i));

Y_CD(i) = yO2 - (yO2 - Y_CD(i-1))*cos(angle(i)) + (X_CD(i-1) - xO2)*sin(angle(i));

nA = 0:1:(Ta/Tipo); %them gia tri 0 the hien toc do ban dau la 0

stairs(X_BC, Y_BC, 'linewidth' , 1.5) hold on

axis equal

xlabel( 'X' ) ylabel( 'Y' ) title( 'Noi suy tho chu C' )

%noi suy tinh cung AB

%noi suy tinh cung CD

%noi suy tinh doan BC

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_AB', angle_AB*R/Tipo, 'linewidth' , 1.5) hold on

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' )

% Import Data to Simulink

T = T_AB + T_BC + T_CD;

X = [X_t_AB, X_t_BC, X_t_CD];

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

axis([-1 65, -1 85]) xlabel( 't(s)' )

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

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)

S(i) = S(i-1) + (P(i) - Q(i))/n;

hold on

axis([-10 100, -10 100]) axis equal

xlabel( 'X' ) ylabel( 'Y' ) title( 'Noi suy tho doan thang AB' )

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_OA = sqrt(vecOA(1)^2 + vecOA(2)^2);

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

%ap dung cong thuc S(k) = S(k ?1) + ?X(k)??X(k ?n)

% ?Xo(k) = S(k)/n

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)

S(i) = S(i-1) + (P(i) - Q(i))/n;

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'];

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' )

%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' )