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

2 LỜI MỞ ĐẦUTrong thời đại phát triển nhanh như ngày nay, máy móc dần thay thế con người trong các công việc mang tính thủ công, các doanh nghiệp cũng đầu tư về máy móc nhiều để hạn chế

ĐẠI HỌC BÁCH KHOA HÀ NỘITrường Điện – Điện Tử

Trần Văn Vĩnh 20192177 Nguyễn Công Đức 20191760 Đoàn Minh Quang 20192040

2

LỜI MỞ ĐẦU

Trong thời đại phát triển nhanh như ngày nay, máy móc dần thay thế con người trong các công việc mang tính thủ công, các doanh nghiệp cũng đầu tư về máy móc nhiều để hạn chế về mặt số lượng nhân công, vì tính hiệu quả trong công việc của chúng cao hơn Một trong những ứng dụng phổ biến trong công nghiệp đấy là máy CNC, được sử dụng rất phổ biến để gia công các chi tiết phức tạp như hoa văn, họa tiết … hay có thể kể một số chức năng của máy CNC như:

Được sử dụng để điều khiển các loại máy công cụ khác

Lập trình các thông số nhanh chóng giúp cho các máy hoạt động một cách tự động

Bù được sai số cho sai số khi thực hiện gia công Máy có chức năng chẩn sai hỗ sợ quá trình sửa chữa

Thông qua môn học này, chúng em đã có những kiến thức cơ bản về việc vận hành máy CNC, cũng như hiểu biết hơn cách vận hành của chúng và ứng dụng thực tế của chúng ra sao

Chúng em xin chân thành cảm ơn thầy Dương Minh Đức, nhờ những bài giảng và sự chỉ bảo nhiệt tình của thầy mà chúng em đã hiểu biết hơn về môn học này, sau đây là báo cáo về bài tập lớn của nhóm chúng em, báo cáo gồm có 5 phần, tương ứng với những yêu cầu mà thầy đưa ra để làm BTL này và phần cuối là code thực hiện trên Matlab của nhóm chúng em:

Chương 1: Thiết kế bộ điều khiển vị trí Chương 2: Tăng giảm tốc trước khi nội suy Chương 3: Thiết kế quỹ đạo chữ P và chữ C Chương 4: Tăng giảm tốc sau khi nội suy Chương 5: Phụ lục

Báo cáo BTL này của chúng em là sự cố gắng và nỗ lực của cả nhóm, về mặt kiến thức và hiểu biết của chúng em còn hạn chế nên báo cáo này sẽ còn nhiều sai sót, mong Thầy có nhận xét và đưa ra góp ý để chúng em sửa sai và tiến bộ hơn trong tương lai

Chúng em xin chân thành cảm ơn Thầy

MỤC LỤC

Mục lục ……….……….……… 3

CHƯƠNG 1: THIẾT KẾ BỘ ĐIỀU KHIỂN VỊ TRÍ ……… 4

1.1 Lựa chọn hàm truyền……… 4

1.2 Thiết kế bộ điều khiển ……… 4

CHƯƠNG 2: TĂNG GIẢM TỐC TRƯỚC NỘI SUY – ADCBI ………… 6

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

2.1.1 Nội suy thô ……… 8

2.1.2 Nội suy tinh ……… 11

2.2 Nội suy theo quỹ đạo hình tròn ……… 13

2.2.1 Tăng tốc và giảm tốc ……… 13

2.2.2 Nội suy thô ……… 14

2.2.3 Nội suy tinh ……… 15

CHƯƠNG 3: NỘI SUY CHỮ ……… 18

3.1 Nội suy chữ P ……… 18

3.2 Nội suy chữ C ……… 20

CHƯƠNG 4: TĂNG GIẢM TỐC SAU NỘI SUY – ADCAI ……… 23

4.1 Nội suy đường thẳng ……… 23

4.1.1 Nội suy thô ……… 23

Do đối tượng đã có sẵn khâu tích phân đã triệt tiêu sai lệch tĩnh, nên ta chỉ cần sử dụng bộ điều khiển PD Ta có sơ đồ mô phỏng mạch điều khiển cho 2 trục:

Hình 1.1: Sơ đồ mô phỏng mạch điều khiển cho 2 trục

Ta sử dụng công cụ PID Tuner của MATLAB để thiết kế bộ điều khiển PD Chọn theo mục tiêu là thời gian xác lập nhỏ nhất nhưng vẫn đảm bảo những yêu cầu đề bài đặt ra, ta chọn được thông số của bộ PD là:

P = 4.55862080976123 D = 0.170655045869507

Với thông số PD trên, ta có đáp ứng của hệ thống: Đáp ứng tục X,Y:

Hình 1.2: Đáp ứng trục X

Hình 1.3: Đáp ứng trục Y Nhận xét:

Độ quá điều chỉnh 4.44% ≤ 5% Sai lệch tĩnh: ~ 0

Thời gian xác lập: 0.186s Udk X/Y =9.788V ≤ 10V Thoả mãn yêu cầu bài toán

6

CHƯƠNG 2: TĂNG GIẢM TỐC TRƯỚC NỘI SUY – ADCBI

Thực hiện tăng giảm tốc trước khi nội suy:

Hình 2.1: cấu trúc nội suy theo ADCBI

Hình 2.2: Hai dạng profile tốc độ - 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- Chu kỳ nội suy Tipo

- Chu kỳ điều khiển Tpos

- Tốc độ ăn dao F

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

2𝐴+𝐹2𝐷≥ 𝐿 2.1 Nội suy theo quỹ đạo đường thẳng Profile tốc độ cho chuyển động thẳng

- Chiều dài L = 50 mm đi từ điểm A(20;30) đến điểm B (60;60) - Gia tốc tăng tốc cho phép A= 4 mm/s2

- Gia tốc giảm tốc cho phép D = 4 mm/s2

- Chu kì nội suy Tipo = 5 ms- Chu kỳ điều khiển Tpos = 1 ms- Tốc độ ăn dao F = 10 mm/s Tăng và giảm tốc:

𝐹2𝐴+

8 Tốc dộ không đổi:

Tc= − − = 2.5(𝑠) VC(i) =F Giảm tốc:

TD = = 2.5 (𝑠) VD(i+1) = VD(i) - Tipo D

Hình 2.3: Profile vận tốc 2.1.1 Nội suy thô

L= 50 mm; XS = 20; Xe = 60; YS= 30; Ye= 60

cos θ = = 0.8; sin θ = = 0.6; Tăng tốc

VA(i+1) = VA(i) + Tipo A ( i =0,1,2,…, NA ); VA(0) =0;

N= = 500;

Độ dịch chuyển nội suy:

Δ𝐿𝑖 = 𝑉𝐴(𝑖) 𝑇𝑖𝑝𝑜 Δ𝑋𝑖 = Δ𝐿𝑖 𝑐𝑜𝑠𝜃 Δ𝑌𝑖 = Δ𝐿𝑖 𝑠𝑖𝑛𝜃

Tốc độ không đổi

𝑉𝐶(𝑖+1) = 𝑉𝐶(𝑖) = 𝐹 = 10(𝑚𝑚 𝑠/ ) ,( i=0,1,2,….Nc ) 𝑁 = = 500; Độ dịch chuyển trong mỗi chu kì nội suy

Δ𝐿𝑖 = 𝑉𝐴(𝑖) 𝑇𝑖𝑝𝑜 Δ𝑋𝑖 = Δ 𝐿𝑖𝑐𝑜𝑠𝜃Δ𝑌𝑖 = Δ 𝐿𝑖𝑠𝑖𝑛𝜃

Giảm tốc

𝑉𝐷(𝑖+1) = 𝑉𝐷(𝑖) - 𝑇𝑖𝑝𝑜 𝐷 ( 𝑖 = 0,1,2, … 𝑁𝐷) 𝑉𝐷(0) = 𝐹 = 10 (𝑚𝑚/𝑠);

𝑁 = = 500

Độ dịch chuyển trong mỗi chu kỳ nội suy

Δ𝐿𝑖 = 𝑉𝐴(𝑖) 𝑇𝑖𝑝𝑜 Δ𝑋𝑖 = Δ 𝐿𝑖 𝑐𝑜𝑠𝜃 Δ𝑌𝑖 = Δ 𝐿𝑖 𝑠𝑖𝑛𝜃

10 Hình 2.4: Quỹ đạo nội suy thô

Hình 2.5: ΔX nội suy thô

2.1.2 Nội suy tinh

Ta có chu kì nội suy (𝑇𝑖𝑝𝑜 = 5 𝑚𝑠) lớn hơn chu kì điều khiển (𝑇𝑝𝑜𝑠 = 1 𝑚𝑠) nên cần chia nhỏ các giá trị độ dịch chuyển Δ𝑋𝑖, Δ theo chu kì điều 𝑌𝑖khiển Tạo ra các đầu vào lượng đặt tại các thời điểm phát tín hiệu điều khiểnNội suy tuyến tính

𝑁 =𝑇𝑇 = 5 𝑎 𝑗( ) = ( ), ≤ ≤ + 𝑖 𝑗 𝑖 𝑁

Với 𝑎(𝑗) là lượng đặt cho bộ điều khiển vị trí tại chu kỳ điều khiển thứ j; 𝑝(𝑖) là khoảng dịch chuyển tính được cho chu kì nội suy thứ i

Hình 2.6: Quỹ đạo nội suy tuyến tính

12 Hình 2.7: ΔX nội suy tuyến tính

HÌnh 2.8: Đáp ứng trục X, trục Y

Hình 2.9: So sánh quỹ đạo 2.2 Nội suy theo quỹ đạo hình tròn

Thông số lựa chọn

- Từ điểm A(40;5) đến điểm B(0;25) tâm đường tròn I(15;5) - Gia tốc tăng tốc cho phép: A = 4 mm/s 2

- Gia tốc góc tăng tốc cho phép: 𝑎𝐴 = = 0.16𝑟𝑎𝑑/𝑠2

- Gia tốc giảm tốc cho phép: D = 4 mm/s2

- Gia tốc góc giảm tốc cho phép: 𝑎𝐷 = = 0.16 𝑟𝑎𝑑/𝑠2 - Chu kì nội suy: 𝑇𝑖𝑝𝑜 = 5 ms

- Chu kì điều khiển: 𝑇𝑝𝑜𝑠 = 1 ms - Tốc độ ăn dao (tốc độ dài): F = 10 mm/s - Vận tốc góc tối đa 𝜔𝑚𝑎𝑥 = = 0.4 𝑟𝑎𝑑 𝑠/ - Góc nội suy: 𝜑

- Nội suy ngược chiều kim đồng hồ 2.2.1 Tăng tốc và giảm tốc

Cung tròn cần di chuyển = arccos𝜑 (| |.|. | ) = 2.2143(𝑟𝑎𝑑)Thuật toán Euler

Cos(𝛼) = 𝐴 = 1, sin(𝛼) = 𝐵 = 𝛼

Nội suy cung tròn 𝜑 = 𝜋 (𝑟𝑎𝑑), 1 bước dịch chuyển cơ bản (BLU) = 10-2 mm 𝐸𝑅 = 𝜑

𝛼 1 + 𝛼 − 1 𝑅

14

2𝛼𝑅 Để 𝐸𝑅 ≤ 1 𝐵𝐿𝑈:

𝛼 = 2

𝜑𝑅 1𝐵𝐿𝑈 = 0.03613 10 (𝑟𝑎𝑑) Với 𝛼 = 0 03613 × 10 (-2 𝑟𝑎𝑑), số chu kỳ nội suy thô cần thiết là:

𝑛 = 𝑐𝑒𝑖𝑙 𝜑𝛼 = 6129 Đưa qua bộ lọc

𝛼𝐿0(𝑖) = (𝛼(𝑖) - 𝛼(𝑖 𝑚 - )) + 𝛼𝐿0(𝑖 – 1) 2.2.2 Nội suy thô

Δ𝑋 = +1 - = 𝑋𝑖 𝑋𝑖 𝑅𝑠𝑖𝑛(𝜃𝑖+1) = 𝑅𝑠𝑖𝑛(𝜃𝑖 + 𝛼) = 𝑅𝑠𝑖𝑛(𝜃𝑖)𝑐𝑜𝑠 𝛼( ) + 𝑅𝑐𝑜𝑠(𝜃𝑖)𝑠𝑖𝑛( )𝛼 = ( - 𝑋𝑖 𝑋𝐴)𝑐𝑜𝑠(𝛼) + (𝑌𝑇 𝑌𝑖)𝑠𝑖𝑛(𝛼) - = ( - 𝑋𝑖 𝑋𝐴)𝐴 + (𝑌𝑇 𝑌𝑖)𝐵 -

→ + 1 - = ( - 𝑋𝑖 𝑋𝑖 𝑋𝑖 𝑋𝐴)(𝐴 - 1) + (𝑌𝑇 𝑌𝑖)𝐵 - Δ𝑌 = 𝑌𝑇 𝑌𝑖 - + 1 = 𝑅𝑐𝑜𝑠(𝜃𝑖+1) = 𝑅𝑐𝑜𝑠(𝜃𝑖 + 𝛼) = 𝑅𝑐𝑜𝑠(𝜃𝑖)𝑐𝑜𝑠(𝛼) - 𝑅𝑠𝑖𝑛(𝜃𝑖)𝑠𝑖𝑛(𝛼 )

= (𝑌𝑇 𝑌𝑖)𝑐𝑜𝑠(𝛼 - ) - ( - 𝑋𝑖 𝑋𝑇)𝑠𝑖𝑛(𝛼) = (𝑌𝑇 𝑌𝑖)𝐴 - - ( - 𝑋𝑖 𝑋𝑇)𝐵

→ + 1 - = (𝑌𝑖 𝑌𝑖 𝑌𝑇 𝑌𝑖 - )(1 - ) + ( - 𝐴 𝑋𝑖 𝑋𝑇)𝐵 Với thuậ toán Euler: A=1; B= 𝛼

Δ𝑋 = 𝑋𝑖+1 - = ( - 𝑋𝑖 𝑌𝑇 𝑌𝑖)𝛼 𝐿𝑂( )𝑖 Δ𝑌 = 𝑌𝑖+1 - = ( - 𝑌𝑖 𝑋𝑖 𝑋𝑇)𝛼 𝐿𝑂 𝑖( )

Hình 2.10: Độ dịch chuyển theo trục Y nội suy thô

Hình 2.11: Quỹ đạo nội suy thô 2.2.3 Nội suy tinh

Vì chu kỳ nội suy thô lớn hơn chu kỳ điều khiển nên sẽ có những chũ kỳ điều khiển vị trí không có lượng đặt Do đó ta thực hiện nội suy tinh để chia tạo giá trị đặt cho từng chu kỳ điều khiển vị trí nằm trong từng chu kỳ nội suy thô tương tự với nội suy đường thẳng

16 \

Nội suy tuyến tính

Hình 2.12: Độ dịch chuyển theo trục Y nội suy tuyến tính

Hình 2.13: Quỹ đạo nội suy tuyến tính

Hình 2.14: So sánh nội suy thô, nội suy tinh

Hình 2.15: Đáp ứng trục X và Y

18

CHƯƠNG 3: NỘI SUY CHỮ

3.1 Nội suy chữ P

Hình 3.1: Ý tưởng chữ P Nội suy quỹ đạo chữ P với các thông số:

o Nửa cung tròn AB với tọa độ A(20,60), B(20,100) Bán kính R = 20mm, tọa độ tâm O(20,80) Góc quay 𝜃 = 𝜋, quay ngược chiều kim đồng hồo Đoạn thẳng BC với tọa độ B(20,100), C(20,20) Độ dài 𝐿 = 80𝑚𝑚 Trình tự thực hiện:

o Thực hiện nội suy cung tròn AB theo chiều ngược chiều kim đồng hồ o Thực hiện nội suy đường thẳng BC

Kết quả mô phỏng:

Hình 3.2: Quỹ đạo chữ P

Hình 3.3: Vận tốc chữ P

41

dY_thoAB = zeros(1,n+m+1); dY_thoAB(1)=0;

for i=1:n+m

dX_thoAB(1,i)=(yO-Y_thoAB(i))*alpha_LO(i); dY_thoAB(1,i)=(X_thoAB(i)-xO)*alpha_LO(i); X_thoAB(1,i+1)=X_thoAB(1,i)+dX_thoAB(1,i); Y_thoAB(1,i+1)=Y_thoAB(1,i)+dY_thoAB(1,i);

for i=1:n+m+1 for j=1:N

dX_ttAB(N*i-N+j)=dX_thoAB(i)/N; dY_ttAB(N*i-N+j)=dY_thoAB(i)/N; end

X_ttAB=[]; X_ttAB(1)=xA; Y_ttAB=[]; Y_ttAB(1)=yA;

for i=2:(n+m+1)*N

X_ttAB(i)=X_ttAB(i-1)+dX_ttAB(i); Y_ttAB(i)=Y_ttAB(i-1)+dY_ttAB(i);

V_AB=alpha_LO*R/T_ipo;

%%Vẽ đường thẳng BC

L_BC=sqrt((xC-xB)^2+(yC-yB)^2); cos_phi_BC=(xC-xB)/L_BC; sin_phi_BC=(yC-yB)/L_BC;

if(F^2/(2*A)+F^2/(2*D)<L_BC) % Normal block

TA_BC=F/A; TD_BC=F/D;

TC_BC=L_BC/F-F/(2*A)-F/(2*D);

else % Short block

F=sqrt(2*L_BC*A*D/(A+D)); %F'

TA_BC=F/A; TD_BC=F/D; TC_BC=0;

nA_BC=ceil(TA_BC/T_ipo); %Số chu kì nội suy tăng tốc

nC_BC=ceil(TC_BC/T_ipo); %Số chu kì nội suy tốc độ không đổi

nD_BC=ceil(TD_BC/T_ipo); %Số chu kì nội suy giảm tốc

n_BC=nA_BC+nC_BC+nD_BC;

% Vận tốc

V_BC=zeros(1,n_BC+1); V_BC(1,1)=0; dL_BC=zeros(1,n_BC+1); t_ipo_BC=zeros(1,n_BC+1); t_ipo_BC(1,1)=t_ipo_AB(1,n+m+1);

for i=1:n_BC

t_ipo_BC(1,i+1)=t_ipo_BC(1,1)+T_ipo*i;

endfor i=1:nA_BC

V_BC(1,i+1)=V_BC(1,i)+A*T_ipo;

for i=nA_BC:nA_BC+nC_BC V_BC(1,i+1)=F;

for i=nA_BC+nC_BC+1:n_BC V_BC(1,i+1)=V_BC(1,i)-D*T_ipo;

43 % Nội suy thô

dL_BC=V_BC*T_ipo; dX_thoBC=dL_BC*cos_phi_BC; dY_thoBC=dL_BC*sin_phi_BC; X_thoBC=zeros(1,n_BC+1); Y_thoBC=zeros(1,n_BC+1); X_thoBC(1,1)=xB; Y_thoBC(1,1)=yB;

for i=2:n_BC+1

X_thoBC(1,i)=X_thoBC(1,i-1)+dX_thoBC(1,i); Y_thoBC(1,i)=Y_thoBC(1,i-1)+dY_thoBC(1,i);

% Nội suy tinh

t_pos_BC=zeros(1,(n_BC+1)*N); t_pos_BC(1,1)=t_pos_AB(1,(n+m+1)*N);

for i=1:(n_BC+1)*N-1

t_pos_BC(1,i+1)=t_pos_AB(1,(n+m+1)*N)+T_pos*i;

dX_ttBC=[]; dY_ttBC=[];

for i=1:n_BC+1 for j=1:N

dX_ttBC(1,N*i-N+j)=dX_thoBC(1,i)/N; dY_ttBC(1,N*i-N+j)=dY_thoBC(1,i)/N; end

X_ttBC = []; X_ttBC(1) = X_ttAB(end);

for i = 2:(n_BC+1)*N

X_ttBC(i) = X_ttBC(i-1) + dX_ttBC(i);

Y_ttBC = [];

for i=1:n+m

dX_thoCD(1,i)=(yI-Y_thoCD(i))*alpha_LO(i); dY_thoCD(1,i)=(X_thoCD(i)-xI)*alpha_LO(i); X_thoCD(1,i+1)=X_thoCD(1,i)+dX_thoCD(1,i); Y_thoCD(1,i+1)=Y_thoCD(1,i)+dY_thoCD(1,i);

% Nội suy tuyến tính%Khởi tạo giá trị chu kì điều khiển

t_pos_CD=zeros(1,(n+m+1)*N); t_pos_CD(1)=t_pos_BC(1,(n_BC+1)*N);

for i=1:(n+m+1)*N-1

t_pos_CD(i+1)=t_pos_CD(1)+T_pos*i;

dX_ttCD=[]; dY_ttCD=[];

45

for j=1:N

dX_ttCD(N*i-N+j)=dX_thoCD(i)/N; dY_ttCD(N*i-N+j)=dY_thoCD(i)/N; end

X_ttCD=[]; X_ttCD(1)=xC; Y_ttCD=[]; Y_ttCD(1)=yC;

for i=2:(n+m+1)*N

X_ttCD(i)=X_ttCD(i-1)+dX_ttCD(i); Y_ttCD(i)=Y_ttCD(i-1)+dY_ttCD(i);

stairs(X_ttAB,Y_ttAB,'r','linewidth',2); hold on;

figure(1);

stairs(X_thoBC,Y_thoBC,'b','linewidth',2); hold on;

figure(1);

stairs(X_thoCD,Y_thoCD,'b','linewidth',2); hold on;

stairs(X_ttCD,Y_ttCD,'r','linewidth',2); hold on;

grid on; xlim([0 130]); ylim([0 130]); title(' Quỹ đạo chữ C '); xlabel('X (mm)');

text(91,90,'A(90,90)'); text(31,90,'B(30,90)'); text(31,30,'C(30,30)'); text(91,30,'D(90,30)');

plot(t_pos_BC,X_ttBC); hold on;

plot(t_pos_CD,X_ttCD); hold on;

grid minor; title('Đáp ứng trục X'); xlabel(' Thời gian (s)'); ylabel(' Vị trí (mm)'); subplot(1,2,2); plot(t_pos_AB,Y_ttAB); hold on;

plot(t_pos_BC,Y_ttBC); hold on;

plot(t_pos_CD,Y_ttCD); hold on;

grid minor; title('Đáp ứng trục y'); xlabel(' Thời gian (s)'); ylabel(' Vị trí (mm)');

figure(3); %Do thi van toc

stairs(t_ipo_AB,V_AB,'r','linewidth',2); hold on;

stairs(t_ipo_BC,V_BC,'g','linewidth',2); hold on;

47

hold on; xlim([0 180]); ylim([0 12]); title(' VẬN TỐC C '); xlabel('Thời gian (s)'); ylabel('V (mm/s)');

Bài 4: ADCAI 4.1 ADCAI đường thẳng

%% Tăng giảm tốc sau khi nội suy - ADCAI %Khai báo tham số

A=4; D=4; F=10; T_ipo=0.005; T_pos=0.001; N=T_ipo/T_pos;

%% Nội suy đường thẳng - ADCAI

xA=20; yA=20; % điểm đầu

xB=60; yB=50; % điểm cuối

L=sqrt((xB-xA)^2+(yB-yA)^2); % chiều dài quãng đường %% NỘI SUY THÔ

T=L/F; n=ceil(T/T_ipo); dX=(xB-xA)/n; dY=(yB-yA)/n; dX_tho=ones(1,n)*dX; dY_tho=ones(1,n)*dY;

%% TĂNG GIẢM TỐC

m=ceil(F/(A*T_ipo)); t_LO = zeros(1,n+m);

for i=1:n+m-1 t_LO(1,i+1)=T_ipo*i;

dX_LO = zeros(1,n+m); dY_LO = zeros(1,n+m); dX_LO(1,1)=1/m*(dX_tho(1,1)-0)+0;

for i=2:m

dX_LO(1,i)=1/m*(dX_tho(1,i)-0)+dX_LO(1,i-1);

endfor i=(m+1):n

dX_LO(1,i)=1/m*(dX_tho(1,i)-dX_tho(1,i-m))+dX_LO(1,i-1);

endfor i=n+1:n+m

dY_LO(1,i)=1/m*(dY_tho(1,i)-dY_tho(1,i-m))+dY_LO(1,i-1);

for i=n+1:n+m

dY_LO(1,i)=1/m*(0-dY_tho(1,i-m))+dY_LO(1,i-1);

X_tho=zeros(1,n+m); Y_tho=zeros(1,n+m); X_tho(1)=xA; Y_tho(1)=yA;

for i=2:n+m

X_tho(i)=X_tho(i-1)+dX_LO(i); Y_tho(i)=Y_tho(i-1)+dY_LO(i);

%% NỘI SUY TUYẾN TÍNH

t_pos=zeros(1,(n+m)*N);

for i=1:(n+m)*N-1 t_pos(1,i+1)=T_pos*i;

X_tt=[]; X_tt(1)=xA; Y_tt=[]; Y_tt(1)=yA;

for i=2:(n+m)*N X_tt(i)=X_tt(i-1)+dX_tt(i); Y_tt(i)=Y_tt(i-1)+dY_tt(i);

%% Simulink

XY_tho.time = []; sum = [X_tho;Y_tho]'; XY_tho.signals.values = sum; XY_tho.signals.dimensions = 2; XY_tinh.time = []; XY_tinh.signals.dimensions = 2;

%% VẼ ĐỒ THỊ% DeltaX sau tăng giảm tốc

figure(1);

stairs(t_LO,dX_LO,'-r','Linewidth',2); hold on;

stairs(t_LO,dY_LO,'-b','Linewidth',2); grid on;

title('\DeltaX va \DeltaY sau tăng giảm tốc '); xlabel('Thời gian (s)');

ylabel('\Delta (mm)'); legend('\DeltaX','\DeltaY')

% Quỹ đạo nội suy thô

figure(3); %Do thi quy dao noi suy tho + tang giam toc

stairs(X_tho,Y_tho,'-r','linewidth',2); grid on

title('Qũy đạo nội suy thô sau tăng giảm tốc '); xlabel('X (mm)');

ylabel('Y (mm)'); hold on;

%DeltaX nội suy thô sau tăng tốc

figure(4); bar(t_LO,dX_LO);

title('\DeltaX nội suy tuyến tính '); xlabel('Thời gian (s)'); ylabel('\DeltaX (mm)');

%So sanh quy dao

title(' So sánh quỹ đạo '); xlabel('X (mm)'); ylabel('Y (mm)');

4.2 Đường tròn

% Khai báo tham s

A=4; F=10; T_ipo=5e-3; T_pos=1e-3; N=T_ipo/T_pos;

%{} Giả thiết bài toán

xA=10; yA=10; % điểm đầu

xB=50; yB=40; % điểm cuối

xT=10; yT=155/3; R=sqrt((xT-xA)^2+(yT-yA)^2); phi=asin(abs(xB-xT)/R)

%% Noi suy tho

BLU=0.01; alpha=(2*BLU)/(phi*R); n=ceil(phi/alpha); t_ipo=zeros(1,n+1);

for i=1:n t_ipo(i+1)=T_ipo*i;

X_tho = zeros(1,n+1); X_tho(1)=xA; Y_tho = zeros(1,n+1); Y_tho(1)=yA; dX_tho = zeros(1,n+1);

for i=1:n

dX_tho(1,i)=(yT-Y_tho(i))*alpha; X_tho(1,i+1)=X_tho(1,i)+dX_tho(1,i); Y_tho(1,i+1)=Y_tho(1,i)+dY_tho(1,i);

%% Tang giam toc

m=ceil(F/(A*T_ipo)); t_LO = zeros(1,n+m);

for i=1:n+m-1 t_LO(1,i+1)=T_ipo*i;

dX_LO = zeros(1,n+m);

dX_LO(1,1)=1/m*(dX_tho(1,1)-0)+0;

for i=2:m

dX_LO(1,i)=1/m*(dX_tho(1,i)-0)+dX_LO(1,i-1);

endfor i=(m+1):n

dX_LO(1,i)=1/m*(dX_tho(1,i)-dX_tho(1,i-m))+dX_LO(1,i-1);

endfor i=n+1:n+m

dY_LO(1,i)=1/m*(dY_tho(1,i)-dY_tho(1,i-m))+dY_LO(1,i-1);

endfor i=n+1:n+m

dY_LO(1,i)=1/m*(0-dY_tho(1,i-m))+dY_LO(1,i-1);

X_LO=zeros(1,n+m); Y_LO=zeros(1,n+m); X_LO(1)=xA; Y_LO(1)=yA;

for i=2:n+m

X_LO(i)=X_LO(i-1)+dX_LO(i-1); Y_LO(i)=Y_LO(i-1)+dY_LO(i-1);

%% Noi suy tuyen tinh

k_pos=0:(n+m)*N-1; t_pos=zeros(1,(n+m)*N);

for i=1:(n+m)*N-1 t_pos(1,i+1)=T_pos*i;

dX_tt=[]; dY_tt=[];

for i=1:n+m

for j=1:N

dX_tt(N*i-N+j)=dX_LO(i)/N;

%% Noi suy trung binh

dX_tb=zeros(1,(n+m)*N);

dX_tb(1)=(dX_tt(1)+dX_tt(2)+dX_tt(3))/N; dY_tb(1)=(dY_tt(1)+dY_tt(2)+dY_tt(3))/N; dX_tb(2)=(dX_tt(1)+dX_tt(2)+dX_tt(3)+dX_tt(4))/N;

for i=3:(n+m)*N-2

dX_tb(i)=(dX_tt(i-2)+dX_tt(i-1)+dX_tt(i)+dX_tt(i+1)+dX_tt(i+2))/N; dY_tb(i)=(dY_tt(i-2)+dY_tt(i-1)+dY_tt(i)+dY_tt(i+1)+dY_tt(i+2))/N;

1)+dX_tt((n+m)*N))/N;

dX_tb((n+m)*N-1)=(dX_tt((n+m)*N-3)+dX_tt((n+m)*N-2)+dX_tt((n+m)*N-1)+dY_tt((n+m)*N))/N;

dY_tb((n+m)*N-1)=(dY_tt((n+m)*N-3)+dY_tt((n+m)*N-2)+dY_tt((n+m)*N-dX_tb((n+m)*N)=(dX_tt((n+m)*N-2)+dX_tt((n+m)*N-1)+dX_tt((n+m)*N))/N; X_tb=[]; X_tb(1)=xA;