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

Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (1.74 MB, 31 trang )

Trang 1<div class="page_container" data-page="1">

ĐẠ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 Đồn Minh Quang 20192040

</div>Trang 2<div class="page_container" data-page="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.

</div>Trang 3<div class="page_container" data-page="3">

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

</div>Trang 4<div class="page_container" data-page="4">

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

</div>Trang 5<div class="page_container" data-page="5">

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

</div>Trang 6<div class="page_container" data-page="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

</div>Trang 7<div class="page_container" data-page="7">

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𝐴+

</div>Trang 8<div class="page_container" data-page="8">

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;

</div>Trang 9<div class="page_container" data-page="9">

Độ 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

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

</div>Trang 10<div class="page_container" data-page="10">

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

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

</div>Trang 11<div class="page_container" data-page="11">

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

</div>Trang 12<div class="page_container" data-page="12">

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

</div>Trang 13<div class="page_container" data-page="13">

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 𝑅

</div>Trang 14<div class="page_container" data-page="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 - = ( - 𝑌𝑖 𝑋𝑖 𝑋𝑇)𝛼 𝐿𝑂 𝑖( )

</div>Trang 15<div class="page_container" data-page="15">

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.

</div>Trang 16<div class="page_container" data-page="16">

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

</div>Trang 17<div class="page_container" data-page="17">

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

</div>Trang 18<div class="page_container" data-page="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

</div>Trang 19<div class="page_container" data-page="19">

Kết quả mơ phỏng:

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

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

</div>Trang 20<div class="page_container" data-page="20">

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

</div>Trang 21<div class="page_container" data-page="21">

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;

</div>Trang 22<div class="page_container" data-page="22">

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 = [];

</div>Trang 23<div class="page_container" data-page="23">

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=[];

</div>Trang 24<div class="page_container" data-page="24">

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

</div>Trang 25<div class="page_container" data-page="25">

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;

</div>Trang 26<div class="page_container" data-page="26">

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

</div>Trang 27<div class="page_container" data-page="27">

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

</div>Trang 28<div class="page_container" data-page="28">

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

</div>Trang 29<div class="page_container" data-page="29">

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;

</div>