bài tập lớn hệ thống điều khiển máy cnc

Đề bàiMộ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.. 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ốcdạng

ĐẠI HỌC BÁCH KHOA HÀ NỘITRƯỜNG ĐIỆN – ĐIỆN TỬ

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 Chữ ký của GVHD

Hà Nội, 11/7

MỤC LỤC

BÀI 1: THIẾT KẾ BỘ ĐIỀU KHIỂN VỊ TRÍ CHO TỪNG TRỤC X, Y 5

BÀI 2: TĂNG GIẢM TỐC TRƯỚC NỘI SUY (ADCBI) 8

2.1Nội suy theo quỹ đạo đường thẳng 10

2.2Nội suy theo quỹ đạo đường tròn 15

BÀI 3: THIẾT KẾ QUỸ ĐẠO VẼ CHỮ P, S 24

3.1Chữ P 24

3.2Chữ S 25

BÀI 4: TĂNG GIẢM TỐC SAU NỘI SUY (ADCAI) 26

4.1Bộ lọc số 26

4.2Nội suy đường thẳng 28

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

Danh mục hình ảnh:

Hình 1: Mô hình bộ điều khiển vị trí cho trục X 5

Hình 2: Thông số cho bộ điều khiển PD của trục X 5

Hình 3: Đáp ứng của trục X 6

Hình 4: Tín hiệu điều khiển U cho trục X 6

Hình 5: Mô hình bộ điều khiển vị trí cho trục Y 7

Hình 6: Thông số cho bộ điều khiển PD của trục Y 7

Hình 7: Đáp ứng của trục Y 7

Hình 8: Tín hiệu điều khiển U cho trục Y 8

Hình 9: Cấu trúc nội suy ADCBI 8

Hình 10: Hai dạng tốc độ 9

Hình 11: Kiểm tra dạng quỹ đạo 9

Hình 12: Lựa chọn dạng nội suy 10

Hình 13: Normal block và Short block 11

Hình 14: Nội suy thô dường thẳng 11

Hình 15: Nội suy tuyến tính 12

Hình 16: Sơ đồ khối trong simulink 13

Hình 17: Đồ thị sau khi nội suy thô (đường thẳng) 13

Hình 18: Đồ thị sau khi nội suy tinh (đường thẳng) 14

Hình 19: Đồ thị vận tốc 2 trục X, Y 14

Hình 20: Vị trí trục X, Y 15

Hình 21: Quỹ đạo sau khi đi qua bộ điều khiển 15

Hình 22: Các trường hợp lựa chọn tâm 16

Hình 23: Normal block (circle) 17

Hình 24: Short block (circle) 18

Hình 25: Phương pháp trích mẫu dữ liệu 19

Hình 26: Nội suy theo phương pháp tuyến tính 20

Hình 27: Quỹ đạo sau khi nội suy tinh 21

Hình 28: Quỹ đạo sau khi nội suy thô có so sánh với nội suy tinh 21

Hình 36: Minh họa bộ lọc dạng linear 27

Hình 37: Quỹ đạo nội suy thô có bộ lọc (thẳng) 28

Hình 38: Quỹ đạo nội suy tinh có bộ lọc (thẳng) 28

Đề 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ềukhiể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ậcnhất

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í chotừng trục thỏa mãn các yêu cầu sau

- Độ quá điều chỉnh ≤ 5%- Sai lệch tĩnh ≤ 0,1%- Thời gian xác lập nhỏ nhất- | U | ≤ 10Vđk

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ốcdạ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, chiều quay nhập bởi người dùng.

- Thực hiện cả nội suy thô và nội suy tinh

- Vận tốc tối đa, gia tốc tối đa cũng nhập bởi người dùng- 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 2 chữ trong cácchữ sau {B, C, D, G, O, P, Q, R, S, U}.

4 Trình bày và nêu ví dụ minh họa (thông số nhập bởi người dùng) về nộisuy thô đường thẳng và đường tròn với kiểu tăng tốc/giảm tốc dạng hình

di chuyển cho từng trục, sau đó dùng bộ lọc số để thực hiện tăng tốc/giảmtốc.

BÀI 1: THIẾT KẾ BỘ ĐIỀU KHIỂN VỊ TRÍ CHO TỪNG TRỤC X, Y

 Hai động cơ có hàm truyền:Trục x: �

(�) = �1( � )

= � 1

1� �1( )

1++++++ 1�

1+0.08∗∗∗ ∗

Do quãng đường là đạo hàm của vận tốc nên ta luôn có khâu tích phân trong hàmtruyền hệ thống giúp loại bỏ sai lệch tĩnh Do đó ta lựa chọn bộ điều khiển PD choứng dụng điều khiển vị trí này.

Do có khâu tích phân nên sai lệch tĩnh sẽ bằng 0 với dạng đầu vào là hàm step.1.1 Xác định bộ điều khiển vị trí cho trục X:

 Hàm truyền bộ điều khiển PD cho trục X:��� � = ��� + � ���

Hình 1: Mô hình bộ điều khiển vị trí cho trục X

Sử dụng chức năng tuning trong matlab dò tìm các thông số cho bộ PD sau đó hiệuchỉnh lại để đạt được yêu cầu đề bài về sai lệch.

Hình 2: Thông số cho bộ điều khiển PD của trục X

 Kết quả mô phỏng:

 Đáp ứng của trục X:

 Tín hiệu điều khiển U:

 Nhận xét:

Hình 4: Tín hiệu điều khiển U cho trục X

 Độ quá điều chỉnh: 1.92% Thời gian xác lập: 0.06s Sai lệch tĩnh: xấp xỉ 0 Tín hiệu Udkmax: 7.8<10V1.2 Xác định bộ điều khiển vị trí cho trục Y: Hàm truyền bộ điều khiển PD cho trục Y:

���� = � + � ��� ��

Sử dụng chức năng tuning trong matlab dò tìm các thông số cho bộ PD sau đó hiệu chỉnh lại để đạt được yêu cầu đề bài về sai lệch.

Hình 6: Thông số cho bộ điều khiển PD của trục Y

 Kết quả mô phỏng: Đáp ứng của trục Y :

 Tín hiệu điều khiển U:

Hình 7: Đáp ứng của trục Y

interpreterPart program

Mapping to each axisRough intrpola琀椀on

Fine intrpola琀椀onPosi琀椀on control Nhận xét:

 Độ quá điều chỉnh: 5.31% Thời gian xác lập: 0.277s Sai lệch tĩnh: xấp xỉ 0 Tín hiệu Udkmax: 8.4<10V

BÀI 2: TĂNG GIẢM TỐC TRƯỚC NỘI SUY (ADCBI)

- Chiều dài: L

- Gia tốc tăng tốc cho phép : A- Gia tốc giảm tốc cho phép: D

- Tốc độ ăn dao: F

- Kiểm tra quỹ đạo là dạng nào bằng công thức:�2

Hình 11: Kiểm tra dạng quỹ đạo

Bước 2: Thực hiện nội suy thô

Bước 3: chiếu xuống các trục tọa độ và thực hiện nội suy tinh2.1 Nội suy theo quỹ đạo đường thẳng

�� Bước 1: Thực hiện khối tăng/giảm tốc Kiểm tra normal block và short block

Bước 2: Thực hiện nội suy thô

Bước 3: Chiếu xuống các trục tọa độ và thực hiện nội suy tinh Chọn phương pháp nội suy tuyến tính

)((((

Tốc độ ăn dao: F = 60 (mm/s)Gia tốc cho phép: A = D = 15 (mm/s^2)Chu kỳ nội suy: ���� = 5

 Sơ đồ điều khiển trong Simulink:

 Kết quả:

Hình 16: Sơ đồ khối trong simulink

 Trước khi qua bộ điều khiển:

Hình 19: Đồ thị vận tốc 2 trục X, Y

 Sau khi đi qua bộ điều khiển:

Hình 21: Quỹ đạo sau khi đi qua bộ điều khiển

2.2 Nội suy theo quỹ đạo đường tròn- Từ điểm A(0,0) đến điểm B(400,0)- Bán kính đường tròn: R = 220

- Vận tốc góc ăn dao: � = 360

- Clockwise: { ℎềℎềùℎề ��� đ1: ùùℎềồồù ℎềℎềồồ ℎồ

0: ượ�ℎềợợℎềℎề ℎềℎềℎề

ℎề ��� đồồ ℎồồồ- Shortline: 1: ���� ò{ òòắắ

0: ���� òòăằ ằằ2.2.1 : Lựa chọn tđm

Hình 22: Câc trường hợp lựa chọn tđm

 Trường hợp 1,2:

- Quay thuận: cung bĩ (I2), cung lớn (I1)- Quay ngược: cung bĩ (I1), cung lớn (I2) Trường hợp 3,4:

- Quay thuận: cung bĩ (I1), cung lớn (I2)- Quay ngược: cung bĩ (I2), cung lớn (I1)

2.2.2 Nội suy đường tròn theo phương phâp tuyến tính

 Tăng tốc vă giảm tốc:

 Nội suy thô

Sử dụng phương pháp trích mẫu dữ liệu.

Từ giá trị vận tốc góc , ta tính được giá trị góc ��� là góc giữa 2 điểm nội suy ứng với chu kì nội suy là ���:

Suy ra:

Ta có:

Tương tự

 Nội suy tinh :

→ ��+1 = � + � �+1 ((((− �����)→ ��+1 = � + � �+1 ((((− �����)

��

Sau quá trình nội suy thô, ta thu được các giá trị sai lệch giữa thời điểm ivà i+1 theo 2 trục là 2 ma trận dX và dY

Gọi tổng số điểm nội suy là �� = � + � + �� � �

Do chu kì điều khiển �đ đnhỏ hơn chu kì nội suy �đ �� 5 lần nên sau khi nội

ma trận cũ dX, dY

Ở đây ta sẽ sử dụng phương pháp tuyến tính:��(�)

Hình 26: Nội suy theo phương pháp tuyến tính

Sau khi nội suy tinh, vị trí 2 trục X, Y sẽ được tính lại theo công thức sau:�(� + 1 = �) ( ) + ��� ���(�)

((( + 1) = )((((((( + ��(((( ���(((()

 Kết quả mô phỏng

Hình 28: Quỹ đạo sau khi nội suy thô có so sánh với nội suy tinh

Hình 30: Vận tốc và quỹ đạo trục Y

2.2.3 Nội suy đưởng tròn theo phương pháp euler Áp dụng thuật toán euler:

2 ���∝=

� ��trong đó: ∝: góc dịch trong 1 chu kỳ nội suy

BLU: bước dịch chuyển cơ bảnR: bán kính cung tròn

 Sử dụng bộ lọc dạng linear type:1

� � (((((�) − �� (� − � + �) 0 − 1)(((( Nội suy thô và nội suy tinh tương tự như phương pháp tuyến tính Kết quả mô phỏng

3.1 ChữP:

BÀI 3: THIẾT KẾ QUỸ ĐẠO VẼ CHỮ P, S

- Bước 1: Nội suy đường thẳng từ (0,0) đến (0,100)

- Bước 2: Nội suy cung tròn từ (0,100) đến (0.50) với bán kính 25

3.2 Chữ S:

- Bước 1: Nội suy cung tròn 1 từ (0,0) đến (-50,-50) với bán kính 50- Bước 2: Nội suy cung tròn 2 từ (-50,-50) đến (-100,-100) với bán kính 50

BÀI 4: TĂNG GIẢM TỐC SAU NỘI SUY (ADCAI)

4.1 Bộ lọc số

Hình 35: Bộ lọc số

Với X(z) là tín hiệu nội suy bước di chuyển theo 1 trục, sau khi qua bộ lọcsố H(z) ta sẽ thu được tín hiệu Y(z) có dạng hình thang tương ứng quá trình tăngtốc và giảm tốc.

Ta có : Y(z)=X(z).H(z)

Lấy Laplace ngược cả 2 vế ta thu được y[n]=x[n]*h[n]Phép tích chập giữa x[n] và h[n] được tính theo công thức:

� 1 − � −1

Trên miền thời gian thì h[n] là 1 tín hiệu có chiều dài bằng m m là hệ số lọc của bộ lọc số và cũng chính bằng số bước trong quá trình tăng/giảm tốc.

thời gian là � ��� và đó cũng chính là thời gian trong quá trình tăng tốc ��

(với ��

= � )

�

4.2 Nội suy đường thẳng

Hình 37: Quỹ đạo nội suy thô có bộ lọc (thẳng)

Hình 38: Quỹ đạo nội suy tinh có bộ lọc (thẳng)

Hình 40: Vận tốc và quỹ đạo trục Y có bộ lọc

Hình 42: So sánh bước dịch chuyển Y

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

Hình 43: Quỹ đạo đường tròn có bộ lọc

L = sqrt((X_start-X_end)^2+(Y_start-Y_end)^2); cosphi = (X_end-X_start)/L;

sinphi = (Y_end-Y_start)/L;F = 60; % toc do an dao

A = 15; % gia toc tang toc cho phep

D = 15; % gia toc giam toc cho phep

Va0 = 0; %Van toc khi bat dau tang toc

Vd0 = F; %Van toc khi bat dau giam toc

Tipo = 5e-3;Tdk = 1e-3; V_start=0; V_end = 0;

%Kiem tra quy dao nhap la Normal Block hay Short block

if F^2/(2*A)+F^2/(2*D) < LTacc = (F-V_start)/A;

Tacc = (F-V_start)/A;J1 = ceil(Tacc/Tipo);Tdec = (F-V_end)/D; J3 = ceil(Tdec/Tipo);

%Recaculate max speed

F = 2*L/((J1+J3)*Tipo);a1 = (F-V_start)/(J1*Tipo);a3 = (V_end-F)/(J3*Tipo); J2 = 0;

% Linear Acc/Dec Function%compute the time spent to acc

Nipo = round(J1+J2+J3); %Tong so chu ky noi suy tho

Va = linspace(0,F,J1);%Vi+1 = Vi+a1*T

Vd = linspace(F,0,J3); Vc = ones(1,J2)*F;V = [Va,Vc,Vd];VX = V*cosphi; VY = V*sinphi; dL = V(1)*Tipo;for i = 2:Nipo

dL(end+1) = Tipo*(V(i)+V(i-1))/2;end

dx = dL*cosphi;dy = dL*sinphi;

%Quy dao noi suy tho

X_rough = X_start; Y_rough = Y_start; for i=2:length(dL)

X_rough(end+1) = X_rough(end)+dx(i); Y_rough(end+1) = Y_rough(end)+dy(i);end

X_rough_Acc = X_rough(1:J1); X_rough_Const = X_rough(J1+1:J1+J2); X_rough_Dec = X_rough(J1+J2+1:end); Y_rough_Acc = Y_rough(1:J1); Y_rough_Const = Y_rough(J1+1:J1+J2); Y_rough_Dec = Y_rough(J1+J2+1:end);

plot(X_rough_Acc,Y_rough_Acc, ,X_rough_Const,Y_rough_Const,'y' 'b',X_rough_Dec,Y_ro ugh_Dec,'r' 'LineWidth', ,1)

xlabel('Truc X(mm)')ylabel('Truc Y(mm)')title('Quy dao sau khi noi suy tho') grid on

%Noi suy tinh

kT = Tipo/Tdk;dX_new=[]; dY_new=[]; for i=1:length(dL)

aX=ones(1,kT)*dx(i)/kT; aY=ones(1,kT)*dy(i)/kT;end

dX_new=[dX_new aX]; dY_new=[dY_new aY];

%tinh ra gia tri X, Y (gia tri sau khi noi suy tinh)

+dX_new(i); Yt(end+1)=Yt(end)+dY_new(i);end

%Van toc

VX_A=VX(1:J1); VX_C=VX(J1+1:J1+J2); VX_D=VX(J1+J2+1:end);VY_A=VY(1:J1); VY_C=VY(J1+1:J1+J2); VY_D=VY(J1+J2+1:end);

%Thoi gian noi suy

time_ns=linspace(0,Tsum,Nipo); time_dk=linspace(0,Tsum,kT*Nipo);

%NS tho

time_ns_A=time_ns(1:J1); time_ns_C=time_ns(J1+1:J1+J2); time_ns_D=time_ns(J1+J2+1:end);

time_dk_A=time_dk(1:Na); time_dk_C=time_dk(Na+1:Na+Nc); time_dk_D=time_dk(Na+Nc+1:end);

%do thi hai truc X Y%% figure1

figure subplot(2,1,1);

plot(time_dk_A,Xa, ,time_dk_C,Xc, ,time_dk_D,Xd,'y' 'b' 'r' 'LineWidth', ,1) ylabel('X (mm)')

xlabel('Time (s)') title('Vi tri truc X') grid on

subplot(2,1,2);

plot(time_dk_A,Ya, ,time_dk_C,Yc, ,time_dk_D,Yd,'y' 'b' 'r' 'LineWidth', ,1) ylabel('Y (mm)')

title('Vi tri truc Y') grid on

figure subplot(2,1,1);

plot(time_ns_A,VX_A,'y',time_ns_C,VX_C, ,time_ns_D,VX_D,'b' 'r' 'LineWidth', ,1) ylabel('vX (mm/s)')

xlabel('Time (s)') title('Van toc truc X')grid on

subplot(2,1,2);

plot(time_ns_A,VY_A,'y',time_ns_C,VY_C, ,time_ns_D,VY_D,'b' 'r' 'LineWidth', ,1) ylabel('vY (mm/s)')

xlabel('Time (s)') title('Van toc truc Y')grid on

%% Xuat sang simulink

Tsimulink=Tsum;sp_T.time=[];

sp_T.signals.values=[Xt;Yt]';sp_T.signals.dimensions =2;

xs = 0; ys = 0;%toa do diem bat dau

xe = 40; ye = 0;% toa do diem ket thuc

R = 20;%ban kinh cung tron

A = 30;% gia toc tang toc

D = 30;%gia toc giam toc

F = 60;%toc do an dao

Tipo = 5e-3;% chu ki noi suy

Tpos= 1e-3;% chu ki dieu khien

Kt = 5;% ti so Tns/Tdk

BLU= 1;

clockwise = 1;%1 neu di cung chieu kim dong ho, 0 neu di nguoc chieu kim dong ho

shortline = 0;%1 neu di theo cung tron ngan, 0 neu di theo cung tron dai%% Quy doi mm/s sang degree/s

Ac=(360*A)/(2*pi*R); %chuyen tu mm/s^2 -> deg/s^2

Dc=(360*D)/(2*pi*R); %chuyen tu mm/s -> deg/s%kiem tra dieu kien R

d = sqrt((xe - xs)^2 + (ye - ys)^2); if R>=d/2

y2 = ys-h;if(xe>xs)

if(clockwise==1) if(shortline==1)

xI=x1; yI=y2;else

xI=x1; yI=y1;else end

if(shortline==1) xI=x1; yI=y1;else

xI=x1; yI=y2;end

if(clockwise==1) if(shortline==1)

xI=x1; yI=y1;else

xI=x1; yI=y2;else end

if(shortline==1) xI=x1; yI=y2;else

xI=x1; yI=y1;end

end end

h = sqrt(R^2-(abs(ye-ys)/2)^2); y1 = (ye+ys)/2; y2= y1;x1 = xs+h;x2 = xs-h;if(ye>ys)

if(clockwise==1)if(shortline==1)

xI=x1; yI=y1;else

xI=x2; yI=y1;else

if(shortline==1) xI=x2; yI=y1;else

xI=x1; yI=y1;end

if(clockwise==1) if(shortline==1)

xI=x2; yI=y1;else

endxI=x1; yI=y1;

xI=x1; yI=y1;else

xI=x2; yI=y1;else

M = (((xe^2-xs^2)+(ye^2-ys^2))/(2*(ye-ys)));N = (xe-xs)/(ye-ys);

a = 1 + N^2;

b = -2*xs + 2*N*ys - 2*M*N;c = xs^2 + ys^2 - 2*ys*M + M^2 - R^2; delta = b^2 - 4*a*c;

if (delta == 0)x1 = -b/(2*a);x2 = x1;

elseif (delta > 0)delta = sqrt(delta);x1 = (-b + delta)/ (2*a);x2 = (-b - delta)/ (2*a);else

x1 = (-b/2*a)+(1*sqrt(-delta))/(2*a);x2 = conj (x1);

endy1 = M - x1*N;y2 = M - x2*N;if (xe > xs)

if (clockwise == 0)if (shortline == 1 )

if (y1 > y2)yI = y1; xI = x1;else

yI = y2; xI = x2;end

elseif (y1 > y2)

yI = y2; xI = x2; else

yI = y1; xI = x1;end

if (shortline == 1 ) if (y1 > y2)

yI = y2; xI = x2; else

yI = y1; xI = x1;end

elseif (y1 > y2)

yI = y1; xI = x1;

yI = y2; xI = x2;end

endendelse (xe < xs)

if (clockwise == 0)if (shortline == 1 )

if (y1 > y2)yI = y2; xI = x2; else

yI = y1; xI = x1;end

elseif (y1 > y2)

yI = y1; xI = x1; else

yI = y2; xI = x2;end

if (shortline == 1 ) if (y1 > y2)

yI = y1; xI = x1; else

yI = y2; xI = x2;end

elseif (y1 > y2)

yI = y2; xI = x2; else

yI = y1; xI = x1;

%goc can di chuyen (tu vi tri bat dau den ket thuc)

if shortline==1 % cung tron ngan

phi=abs(phie-phis);else% cung tron dai phi=360-

L = phi;

D_alpha = (2*BLU*180)/(L*pi*R);n = ceil(L/D_alpha);

m = ceil((D_alpha*R)/(Ac*Tipo^2)); alpha_euler = ones(1,n+1)*D_alpha; alpha_LO = zeros(1, n+m+1);