ĐẠI HỌC BÁCH KHOA HÀ NỘI

TRƯỜ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
TS. Nguyễn Thị Vân Anh

Khoa: Tự động hoá công nghiệp

Nhóm 17

Nguyễn Mạnh Hiếu 20191835

Lê Đình Nhàn 20191997
Trần Minh Quang 20192047

HÀ NỘI, 7/2023
 MỤC LỤC

CHƯƠNG 1. THIẾT KẾ BỘ ĐIỀU KHIỂN VỊ TRÍ PID CHO HAI TRỤC.1

1.1 Thiết kế mạch vòng điều khiển vị trí..........................................................1
1.1.1 Chọn thông số cho hàm truyền....................................................1
1.1.2 Lựa chọn bộ điều khiển...............................................................1
CHƯƠNG 2. TĂNG TỐC, GIẢM TỐC TRƯỚC NỘI SUY...........................3
2.1 Tính toán tăng tốc, giảm tốc.......................................................................3
2.2 Nội suy đường thẳng..................................................................................4
2.2.1 Nội suy thô..................................................................................6
2.2.2 Nội suy tinh.................................................................................8
2.2.3 Mô phỏng điều khiển quỹ đạo nội suy đường thẳng...................9
2.3 Nội suy đường tròn...................................................................................12
2.3.1 Nội suy thô................................................................................13
2.4 Nội suy tinh..............................................................................................16
2.4.1 Mô phỏng điều khiển quỹ đạo nội suy đường tròn....................17
CHƯƠNG 3. MÔ PHỎNG DI CHUYỂN THEO HÌNH CHỮ......................20
3.1 Mô phỏng di chuyển theo quỹ đạo chữ “P”..............................................20
3.1.1 Thiết kế quỹ đạo........................................................................20
3.1.2 Mô phỏng...................................................................................20
3.2 Mô phỏng di chuyển theo quỹ đạo chữ “O”.............................................23
CHƯƠNG 4. TĂNG TỐC VÀ GIẢM TỐC SAU NỘI SUY..........................26
4.1 Tăng tốc giảm tốc sau nội suy..................................................................26
4.1.1 Cấu trúc điều khiển....................................................................26
4.1.2 Bộ lọc số....................................................................................26
4.2 Tăng tốc giảm tốc cho đường thẳng.........................................................27
4.2.1 Tính toán thông số.....................................................................27
4.2.2 Kết quả mô phỏng.....................................................................28
4.3 Tăng tốc giảm tốc cho đường tròn............................................................31
4.3.1 Tính toán thông số.....................................................................31
4.3.2 Kết quả mô phỏng.....................................................................34
TÀI LIỆU THAM KHẢO..................................................................................38
PHỤ LỤC............................................................................................................39
 CHƯƠNG 1. THIẾT KẾ BỘ ĐIỀU KHIỂN VỊ TRÍ PID CHO HAI TRỤC

1.1 Thiết kế mạch vòng điều khiển vị trí

1.1.1 Chọn thông số cho hàm truyền
Vì 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, Y là khâu quán tính bậc nhất
=> Hàm truyền với đầu vào điện áp, đầu ra là vị trí sẽ là khâu tích phân quán tính
bậc nhất.
Ta có:
Vx K1 X K1
+ Với trục X: = => =G1 ( s)=
U x T 1 s +1 Ux s (T 1 s+1)
Vy K2 Y K2
+ Với trục Y: = => =G2 (s )=
U y T 2 s+ 1 Uy s (T 2 s+1)
Chọn thông số cho hàm truyền:
Hệ số khuếch đại: K 1=5 ; K 2=7
Hằng số thời giàn: T 1=0.04 ; T 2=0.03
1.1.2 Lựa chọn bộ điều khiển
Trong đối tượng có khâu tích phân nên ta sử dụng bộ điều khiển PD

Trục x

Trục y

Hình 1.1 Mô hình hoá bộ điều khiển

Sử dụng công cụ tuning của Matlab ta chọn được các bộ thông số dưới đây:
Đặc điểm Trục x Trục y
P 4.37879677683833 4.084064424305
D 0.108545544825106 0.084035639357702
N 45.1777162549826 68.416742180801
Thời gian quá độ (s) 0.0672 0.0522
Độ quá điều chỉnh (%) 1.23 0

1
Sai lệch tĩnh (%) 0 0

Hình 1.2 Đáp ứng đầu ra trục x

Hình 1.3 Điện áp điều khiển trục x

Hình 1.4 Đáp ứng đầu ra trục y

2
Hình 1.5 Điện áp điều khiển trục y

3
 CHƯƠNG 2. TĂNG TỐC, GIẢM TỐC TRƯỚC NỘI SUY

2.1 Tính toán tăng tốc, giảm tốc

Việc tính toán tăng giảm tốc trước nội suy cho việc xác định lượng đặt điều
khiển chính xác hơn với tốc độ thực tế của máy công cụ , cho đặc tính đáp ứng
điều khiển mượt mà hơn.
Với các thông số cơ bản:
• Chiều dài quỹ đạo: L = √( X s− X e )2 +(Y s−Y e )2
• 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 thô: T ipo
• Chu kỳ nội suy tinh: T pos
• Tốc độ tối đa cần đạt: V max
- Quãng đường cho quá trình tăng giảm tốc:
2 2
V max V max
L L= +
2A 2D
- Kiểm tra quỹ đạo là dạng Normal block hay short block:
2 2
V max V max
+ −L< 0
2A 2D
Nếu bất phương trình trên đúng thì là dạng Normal Block và sai thì là Short
Block. Vì yêu cầu bài toán là tăng, giảm tốc dạng hình thang nên ta chọn thông
số sao cho bất phương trình trên là đúng.
- Ta tính toán thời gian cho quá trình tăng, giảm tốc:

Normal block Short block

V max V max '
T A= T A=
A A
V max V max '
T D= T D=
D D
L−LL V max ' .(T A−T D )
T C= T C=
V max 2

√
L
V max ' = 1 1
+
2A 2D

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

Các thông số đầu vào của tính toán tăng giảm tốc:
• Chọn 2 điểm A(6,7) và B(10,10)
• Chiều dài quỹ đạo: L = √( X A− X B )2 +(Y A −Y B)2 =5(mm)

4
 • Gia tốc tăng tốc cho phép: A = 100 (mm/ s2)
• Gia tốc giảm tốc cho phép: D = 100 (mm/ s2 ¿
• Chu kỳ nội suy thô: T ipo= 5 (ms)
• Chu kỳ nội suy tinh: T pos= 1 (ms)
• Tốc độ tối đa cần đạt: V max = 20 (mm/s)

Theo yêu cầu bài tập lớn ta chỉ quan tâm đến quá trình tăng giảm tốc hình thang
(tức L L ≤ L) , ta có:
V max
• Thời gian tăng tốc: T A= =0.2 s
A
V
• Thời gian giảm tốc: T D = max =0.2 s
D
L−LL
• Thời gian vận tốc không đổi: T C = =0.05 s
V max

5
2.2.1 Nội suy thô

Hình 2.6 Lưu đồ nội suy thô

2.2.1.1. Quá trình tăng tốc
- Chuyển động nhanh dần đều, vận tốc sẽ tăng từ 0 đến V max với gia tốc A.
- Vận tốc sau mỗi chu kỳ nội suy là: V ( i+1 )=V (i)+ A .
- Quãng đường đi được sau mỗi chu kỳ nội suy là: ∆ L A =V (i). T ipo
xe −x s
- Độ dịch chuyển trên trục X: ∆ X (i)=∆ L A . cos(φ)=∆ L A .
L
y −y
- Độ dịch chuyển trên trục Y: ∆ Y (i)=∆ L A .sin (φ)=∆ L A . e s
L
- Từ đó ta có tọa độ của dao qua mỗi chu kỳ nội suy là:
X (i+1)= X (i)+ ∆ X (i)
Y (i+1)=Y (i)+ ∆ Y (i)

Trong đó: V(i) là vận tốc của khoảng thời gian thứ i và V(0) = 0
L(i) là độ dịch chuyển của lần lấy mẫu thứ I
2 2
V i+1 −V i
Li =
2A
TA
- Số lần nội suy: N A = =40
T ipo

6
 2.2.1.2. Quá trình vận tốc không đổi
- Chuyển động đều với vận tốc F, vận tốc sau mỗi chu kỳ không đổi.
TC
N C= =10
T ipo
- Tương tự như quá trình tăng tốc trên ta xác định được:
∆ Lc =V max .T ipo =0.1 mm
x e −x s
∆ X=∆ L c . cos(φ)=∆ Lc . =0.08 mm
L
ye− y s
∆ Y =∆ Lc .sin (φ)=∆ Lc . =0.06 mm
L
2.2.1.3. Quá trình giảm tốc
- Chuyển động chậm dần đều, vận tốc sẽ giảm từ V max về 0 với gia tốc D.
- Tương tự quá trình tăng tốc, ta xác định được:
V (i+1)=V (i)−D .T ipo
∆ L D=V (i).T ipo
x e −x s
∆ X (i)=∆ LD . cos (φ)=∆ LD .
L
ye− ys
∆ Y (i)=∆ LD . sin( φ)=∆ L D .
L
TD
N D= =40
T ipo
2.2.1.4. Mô phỏng MATLAB nội suy thô
Sau khi tiến hành mô phỏng trong Matlab khi nội suy thô đường thẳng từ
điểm A(6,7) đến B(10,10) ta thu được các đồ thị sau:

Hình 2.7 Kết quả mô phỏng nội suy thô đường thẳng

7
 Hình 2.8 Kết quả nội suy vận tốc

2.2.2 Nội suy tinh

Khi chu kì nội suy lớn hơn chu kì trích mẫu bộ điều khiển vị trí, cần tạo ra
các đầu vào lượn đặt vị trí tại các thời điểm trích mẫu trong chu kì nội suy.
Sau quá trình nội suy thô, ta thu được các giá trị sai lệch giữa thời điểm i
và i+1 theo 2 trục là 2 ma trận ∆ X và ∆ Y
Gọi tổng số điểm nội suy là N S =N A + N C + N D
Do chu kì điều khiển (nội suy tinh) T pos nhỏ hơn chu kì nội suy thô T ipo 5
lần nên sau khi nội suy tinh, kích thước của 2 ma trận mới ∆ Xnew , ∆ Ynew sẽ tăng
gấp 5 lần so với 2 ma trận cũ ∆ X và ∆ Y
Các phương pháp nội suy tinh:
• Phương pháp tuyến tính
Chu kỳ nội suy thô 5ms, chu kỳ nội suy tinh 1ms => N = 5
Ta chia đều các ∆ X và ∆ Y thành 5 lần ứng với chu kì trích mẫu T pos = 1ms
∆ Xi
a ( j )= , i≤ j≤ i+T ipo
N
• Phương pháp trung bình
a ( j+2 ) +a ( j+ 1 )+ a ( j ) +a ( j−1 ) +a( j−2)
b ( j )=
5
a ( j+3 ) + a ( j+2 ) +a ( j+1 ) + a ( j ) +a ( j−1 )
b ' ( j )=
5
b ( j ) +b ' ( j)
b ' ' ( j )=
2
Sau khi mô phỏng nội suy tinh đường thẳng bằng MATLAB ta được các đồ thị
nội suy sau:

8
Hình 2.9 Kết quả mô phỏng nội suy tinh đường thẳng

9
2.2.3 Mô phỏng điều khiển quỹ đạo nội suy đường thẳng
Sơ đồ mô phỏng trên Simulink:

Hình 2.10 Sơ đồ mô phỏng điều khiển nội suy vị trí

Sau khi mô phỏng với bộ điều khiển PD ta thu được quỹ đạo di chuyển từ A(6,7)
đến B(10,10) và tiến hành so sánh với lượng đặt ban đầu:
* Kết quả mô phỏng:

Hình 2.11 So sánh quỹ đạo sau điều khiển với quỹ đạo sau nội suy

10
Hình 2.12 So sánh quỹ đạo trục X sau điều khiển với quỹ đạo nội suy

Hình 2.13 So sánh quỹ đạo trục Y sau điều khiển với quỹ đạo nội suy

Hình 2.14 Mô phỏng quỹ đạo sau bộ điều khiển

11
 Hình 2.15 Quỹ đạo nội suy và quỹ đạo sau điều khiển sau khi phóng to

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

Vì yêu cầu bài toán là tăng giảm tốc dạng hình thang => Normal Block
- Từ đó ta chọn các thông số:
 Nội suy từ điểm A(6;1) đến điểm B(2;5) tâm C(2;1) và R = 4
π
 Góc quay: L = θ= 2

 Gia tốc tăng tốc: A = 40 (rad/s2)

 Gia tốc giảm tốc: D = 40 (rad/s2)
 Tốc độ ăn dao: F = 5 (rad/s)
 Chu kì nội suy thô: T ipo = 5ms
 Chu kì nội suy tinh: T pos= 1ms

- Tính toán thời gian tăng, giảm tốc:

F
T A= (Thời gian tăng tốc)
A

12
 2 2
F F
F L− −
T D = (Thời gian giảm tốc) 2 A 2 D (Thời gian tốc độ không
D T C=
F
đổi)
2.3.1 Nội suy thô

- Tính toán thời tăng, giảm tốc và vận tốc ko đổi:
F 5
T A= = =0.125 s s
A 40
2 2
F F
φ− −
2 A 2D s
T c= =0.185
F
F 5
T D = = =0.125 s
D 40
A

X(i+1)=Rcos(θi +1)=Rcos(θi +α )=Rcosθi cosα -Rsinθi sinα =cosα X(i) - sinα

Y(i)=AX(i)-BY(i)
Y(i+1)=Rsin(θi +1)=Rsin(θi +α )=Rsinθi cosα +Rcosθi sinα =cosα Y(i) - sinα
X(i)=AY(i)+BX(i)
A = cosα ; B = sinα (α là dịch của 1 lần nội suy)
- Từ đó ta được sai lệch theo 2 trục như trên hình.
- Xác định sai lệch bán kính và sai lệch cung:
 Sai lệch bán kính:

 Sai lệch cung:

13
 2.3.1.1. 1.1. Quá trình tăng tốc
T A 0.125
N A= = =25
T ipo 0.005
- Ta có tốc độ quay sau mỗi chu kì nội suy: F(i+1) = F(i) + A.T ipo
2 2
F (i) −F (i−1)
=> θi=
2A

2.3.1.2. Quá trình tốc độ không đổi

T C 0.185
N C= = =37
T ipo 0.005
θi=F . T ipo= 5.0.005=0.025rad

2.3.1.3. Quá trình giảm tốc

T D 0.125
N D= = =25
T ipo 0.005
- Ta có: F i+1=F i−D .T ipo (i=0, 1, 2,…, N D)
2 2
F i−1 −F i
θi = ;
2D
rad
- Với vận tốc ban đầu F (i=0)= F= 5 ( )
s

14
 2.3.1.4. Mô phỏng nội suy thô đường tròn bằng MATLAB

Hình 2.16 Đồ thị mô phỏng quỹ đạo vị trí nội suy thô theo đường tròn

Hình 2.17 Đồ thị nội suy tốc độ

2.4 Nội suy tinh

Do chu kỳ nội suy T ipo lớn hơn chu kỳ trích mẫu T pos của bộ điều khiển vị trí
nên ta cần chia nhỏ các giá trị chu kỳ trích mẫu nội suy ra theo chu kỳ trích mẫu
điều khiển và xác định các giá trị ở giữa chu kỳ trích mẫu nội suy. Ta có: Nt =
T ipo
=5
T pos
Tương tự như nội suy tinh với đường thẳng ta có:
• Phương pháp tuyến tính
15
 - Ta chia đều các ∆ X và ∆ Y thành 5 lần ứng với chu kì trích mẫu T pos =
1ms
∆ Xi
a ( j )= , i≤ j≤ i+T ipo
N
• Phương pháp trung bình
a ( j+2 ) +a ( j+ 1 )+ a ( j ) +a ( j−1 ) +a( j−2)
b ( j )=
5
a ( j+3 ) + a ( j+2 ) +a ( j+1 ) + a ( j ) +a ( j−1 )
b ' ( j )=
5
b ( j ) +b ' ( j)
b ' ' ( j )=
2
Áp dụng phương pháp nội suy tuyến tính đã trình bày ở trên bằng cách chia nhỏ
chu kỳ nội suy và các giá trị góc nội suy tương ứng ta thu được kết quả nội suy
tinh cho đường tròn như sau:

Hình 2.18 Đồ thị mô phỏng quỹ đạo vị trí nội suy tinh đường tròn

16
 Hình 2.19 So sánh kết quả nội suy thô và nội suy tinh

2.4.1 Mô phỏng điều khiển quỹ đạo nội suy đường tròn
- Ta chọn bộ điều khiển PD tương tự với nội suy quỹ đạo đường thẳng
- Kết quả mô phỏng:

17
Hình 2.20 So sánh quỹ đạo trục X sau điều khiển với quỹ đạo nội suy

Hình 2.21 So sánh quỹ đạo trục Y sau điều khiển với quỹ đạo nội suy

18
 Hình 2.22 Mô phỏng quỹ đạo sau bộ điều khiển

Hình 2.23 Quỹ đạo nội suy và sau bộ điều khiển theo trục X khi phóng to

19
 CHƯƠNG 3. MÔ PHỎNG DI CHUYỂN THEO HÌNH CHỮ

3.1 Mô phỏng di chuyển theo quỹ đạo chữ “P”

3.1.1 Thiết kế quỹ đạo
Để tạo quỹ đạo hình chữ P ta chia quỹ đạo làm 4 đoạn
Bước 1: Nội suy đường thẳng từ A(10,20) tới B(15,20)
Bước 2: Nội suy nửa đường tròn từ B(15,20) tới C(15,35) với tâm T(15; 27,5)
Bước 3: Nội suy đường thẳng từ C(15,35) tới D(10,35)
Bước 4: Nội suy đường thẳng từ D(10,35) tới E(10,5)
3.1.2 Mô phỏng
Thông số chương trình matlab
%Tham so ham truyen
Kx=12; Tx=0.05;
Ky=21; Ty=0.02;
Kpx=10; Kdx=0.232;
Kpy=10; Kdy=0.102;
%Tham so
F=5; %toc do an dao 50mm/s
A=2; %gia toc tang toc cho phep
D=2; %gia toc giam toc cho phep
T_ipo=5e-3; %chu ky noi suy tho
T_pos=1e-3; %chu ky dieu khien vi tri
N=T_ipo/T_pos;%ty so
%Toa do cac diem
xA=10; yA=20; %Toa do diem A(10,20)
xB=15; yB=20; %Toa do diem B(15,20)
xT=15; yT=27.5; %Toa do diem T(15,27.5)
xC=15; yC=35; %Toa do diem C(15,35)
xD=10; yD=35; %Toa do diem D(10,35)
xE=10; yE=5; %Toa do diem E(10,5)
R=yT-yB; %Ban kinh cung tron BC

20
- Tăng tốc và giảm tốc

Hình 3.24 Nội suy tăng tốc và giảm tốc

- Simulink

Hình 3.25 Quỹ đạo hình chữ P

- Đồ thị vị trí và tốc độ theo từng trục:

21
 Hình 3.26 Đáp ứng vị trí trục X

Hình 3.27 Đáp ứng vận tốc trục X

Hình 3.28 Đáp ứng vị trí trục Y

22
 Hình 3.29 Đáp ứng vận tốc trục Y

3.2 Mô phỏng di chuyển theo quỹ đạo chữ “O”

Để tạo quỹ đạo hình chữ O ta chia quỹ đạo làm 4 đoạn
Bước 1: Nội suy đường thẳng từ A(100,0) tới B(100,500)
Bước 2: Nội suy nửa đường tròn từ B(100,500) tới C(300,500) với tâm I1(200;
500)
Bước 3: Nội suy đường thẳng từ C(300,500) tới D(300,0)
Bước 4: Nội suy nửa đường tròn từ D(300,0) tới A(100,0) với tâm I2(200;0)
Thông số chương trình matlab:

%% Toa do cua cac diem can ve

A=[100 0];
B=[100 500];
C=[300 500];
D=[300 0];
I1=[200 500];
I2=[200 0];
%% Thong so dieu khien
Tns=5e-3;
Tdk= 1e-3;
kT= Tns/Tdk;
F= 60;
23
A= 30;
D= 30;

Đồ thị kết quả nội suy quỹ đạo:

Hình 3.30 Nội suy chữ O

Đồ thị đáp ứng quỹ đạo kết quả mô phỏng Simulink:

24
 Ket qua Simulink
600

X 100 X 299.9989
Y 500.0739 Y 500.0007
500

400
Y axis (mm)

300

200

100

X 100 X 300
Y 0 Y -0.098704
0

100 150 200 250 300

X axis (mm)

Hình 3.31 Đáp ứng quỹ đạo

Đồ thị vị trí và tốc độ theo từng trục:

Hình 3.32 Đáp ứng vị trí theo trục X

25
Hình 3.33 Đáp ứng vận tốc theo trục X

Hình 3.34 Đáp ứng vị trí theo trục Y

Hình 3.35 Đáp ứng vận tốc theo trục Y

26
 CHƯƠNG 4. TĂNG TỐC VÀ GIẢM TỐC SAU NỘI SUY

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

4.1.1 Cấu trúc điều khiển

Hình 4.36 Lưu đồ thuật toán tăng tốc, giảm tốc sau nội
suy
Để thực hiện quy trình tăng tốc, giảm tốc sau nội suy ADCAI, ta cần phải
tính toán khoảng dịch chuyển trên các trục x và y trong các khoảng thời gian nội
suy. Thực hiện điều khiển tăng tốc, giảm tốc độc lập cho từng trục sau đó thực
hiện phép nội suy tinh để tăng độ chính xác của giá trị dịch chuyển.

4.1.2 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ọc
số 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ăng
tốc và giảm tốc.
Ta có: Y(z)=X(z).H(z)
Biến đổi Z ngược cả 2 vế ta thu được: y[n]=x[n]*h[n]

27
 Phép tích chập giữa x[n] và h[n] được tính theo công thức:
i + j=n
y ( n )= ∑ x ( i ) . h( j)
i, j

Để bộ lọc số vẫn đảm bảo tổng của tín hiệu đầu vào (x[n]) giống như tổng
của tín hiệu đầu ra (y[n]) thì ta sẽ có điều kiện: ∑ h(k )=1

Theo dạng quỹ đạo hình thang thì bộ lọc số sẽ có hàm truyền:
−m
1 1−z
H ( z )= .
m 1−z −1

1
V L0 ( k )= ( V ( k )−V i ( k−m) ) + V L 0 ( k−1 )
m i

Input Digital Filter Output

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.
Do 1 bước sẽ tương ứng với 1 chu kì nội suy T ns, suy ra m bước sẽ tương
ứng thời gian là m .T ns và đó cũng chính là thời gian trong quá trình tăng tốc T A
TA F
Suy ra: m .T ns=T A → m= (với T A= )
T ns A

4.2 Tăng tốc giảm tốc cho đường thẳng

4.2.1 Tính toán thông số
4.2.1.1. Lựa chọn bài toán
Di chuyển từ điểm A tới điểm B theo quỹ đạo là 1 đường thẳng
Thông số cần sử dụng cho quá trình nội suy:
Tọa độ điểm bắt đầu A(20;30)
Tọa độ điểm kết thúc N(70;90)
Tốc độ ăn dao F = 10 (mm/s)
Gia tốc tăng/giảm tốc cho phép A = D = 8 (mm/s2)
Chu kỳ nội suy Tipo = 5 (ms)
4.2.1.2. Nội suy thô
- Chiều dài đoạn thẳng L =√ ( x B −x A ) + ( y B − y A ) =78.1025
2 2

x B−x A y −y
- Cosφ= , Sinφ= B A (φ là góc lệch giữa AB và trục Ox)
L L

28
 - Thời gian di chuyển: T = L/F=7.8103
- Số bước nội suy: N = floor(T/Tipo) + 1=1563
- Khoảng dịch chuyển: ΔL=F.Tipo=0.05 = 0.05
- Khoảng dịch chuyển trục X: Δx=ΔL.sin φ = 0.032
- Khoảng dịch chuyển trục Y: Δy=ΔL.cos φ = 0.0384

4.2.1.3. Tăng tốc giảm tốc sử dụng bộ lọc số

₋ Thời gian tăng tốc: Tacc = F/A
₋ Số bước tăng tốc: m = floor(Tacc/Tipo)+1
₋ Sử dụng bộ lọc số kiểu tuyến tính (dạng hình thang)
1
V L0 ( k )= ( V ( k )−V i ( k−m) ) + V L 0 ( k−1 )
m i
Ta có:
1
Δx 0 ( k )= ( Δx ( k )−Δx ( k −m ) ) + Δx 0 ( k −1 )
m
1
Δy 0 ( k )= ( Δy ( k )−Δy ( k−m ) ) + Δy 0 ( k−1 )
m
- Vận tốc trục x: Vx= Δx 0 /Tipo
- Vận tốc trục y: Vy= Δy 0 /Tipo
- Vận tốc di chuyển: V =√ Vx2 +Vy 2
- Quỹ đạo theo trục x: x0(k+1) = x0(k) + Δx 0 (k )
- Quỹ đạo theo trục y: y0(k+1) = y0(k) + Δy 0 (k )

4.2.2 Kết quả mô phỏng

Hình 4.37 x

29
 Hình 4.38 Nội suy vận tốc trên trục

Hình 4.39 Nội suy vận tốc trên hai trục

30
Hình 4.40 Quỹ đạo dịch chuyển

Hình 4.41 Quỹ đạo theo trục x

Hình 4.42 Quỹ đạo theo trục y

31
 Hình 4.43 Điện áp điều khiển

4.3 Tăng tốc giảm tốc cho đường tròn

4.3.1 Tính toán thông số
4.3.1.1. Lựa chọn bài toán
Ta cần di chuyển từ điểm A tới điểm B theo quỹ đạo là 1 cung tròn có tâm
I.
Một số thông số ta cần sử dụng cho quá trình nội suy:
- Tọa độ điểm bắt đầu M(7;17)
- Tọa độ điểm kết thúc N(6;18)
- Bán kính cung tròn R = 11
- Tốc độ ăn dao F = 10 (mm/s)
- Gia tốc tăng/giảm tốc cho phép A = D = 8 (mm/s2)
- Chu kỳ nội suy T_ns=5(ms)
- Chiều quay ngược chiều kim đồng hồ: dir = 1 (cùng chiều dir = - 1)
- Góc quay nhỏ hơn π: goc = 1 (góc quay lớn hơn π goc = - 1)
- BLU = 0.01
4.3.1.2. Tìm tâm đường tròn
Tính khoảng cách giữa hai điểm A, B
√ 2 2
d= ( x B−x A ) + ( y B− y A ) =15.5563

Toạ độ trung điểm M của AB: M ( x +2 x ; y +2 y )

B A B A

x B− x A y B− y A
Vecto chỉ phương đơn vị: n=( ; )
d d
y − y x −x
Vecto pháp tuyến đơn vị: n 1=( B A ; B A )
d d

√
2
Khoảng cách từ tâm tới đoạn thẳng AB: h= R 2− d
4
Toạ độ tâm có thể tính theo: c=m+ z ×h × n1
32
 x B+ x A yB− y A yB+ y A x B−x A
a= + z ×h ,b= + z ×h
2 d 2 d

4.3.1.3. Mapping về 2 trục, nội suy thô

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

X ( i+1 )=cos ( alpha ) × X (i ) −sin ( alpha ) × Y ( i )
Y ( i+ 1 )=cos ( alpha ) ×Y ( i ) +sin ( alpha ) × X ( i )
Với A=cos ( alpha ) , B=sin ⁡( alpha), ta có:
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 )
Với chiều quay ngược chiều kim đồng hồ:
X ( i+1 )=DX (i ) + X ( i )
Y ( i+ 1 )=DY ( i ) +Y ( i )
Với chiều quay cùng chiều kim đồng hồ:
X ( i+1 )=DX (i ) −X ( i )
Y ( i+ 1 )=DY ( i )−Y ( i )

Thuật toán Euler

Ta có:
A=1; B=anpha với góc anpha rất bé
Với một bước dịch chuyển là một BLU = 0.01
4 × BLU
alpha= (rad )
π ×R

N=floor ( 2 ×alpha
π
)+1
4.3.1.4. Tăng tốc giảm tốc sử dụng bộ lọc số
₋ Thời gian tăng tốc: Tacc = F/A

33
 ₋ Số bước tăng tốc: m = floor(Tacc/Tipo)+1
₋ Sử dụng bộ lọc số kiểu tuyến tính (dạng hình thang)
1
V L0 ( k )= ( V ( k )−V i ( k−m) ) + V L 0 ( k−1 )
m i
Ta có:
1
Δx 0 ( k )= ( Δx ( k )−Δx ( k −m ) ) + Δx 0 ( k −1 )
m
1
Δy 0 ( k )= ( Δy ( k )−Δy ( k−m ) ) + Δy 0 ( k−1 )
m
- Vận tốc trục x: Vx= Δx 0 /Tipo
- Vận tốc trục y: Vy= Δy 0 /Tipo
- Vận tốc di chuyển: V =√ Vx2 +Vy 2
- Quỹ đạo theo trục x: x0(k+1) = x0(k) + Δx 0 (k )
- Quỹ đạo theo trục y: y0(k+1) = y0(k) + Δy 0 (k )

4.3.2 Kết quả mô phỏng

Hình 4.44 Vận tốc theo trục x

34
 Hình 4.45 Vận tốc theo trục y

Hình 4.46 Vận tốc dịch chuyển

Hình 4.47 Quỹ đạo dịch chuyển

35
Hình 4.48 Quỹ đạo theo trục x

Hình 4.49 Quỹ đạo theo trục y

Hình 4.50 Điện áp điều khiển

36
 TÀI LIỆU THAM KHẢO
[1] Theory and Design of CNC Systems - Suk-Hwan Suh • Seong-Kyoon Kang
Dae-Hyuk Chung • Ian Stroud.
https://math.stackexchange.com/questions/27535/how-to-find-center-of-an-arc-
given-start-point-end-point-radius-and-ar

37
 PHỤ LỤC

ADCAI đường thẳng

clear
xA=20; yA=30; %Toạ độ điểm bắt đầu
xB=70; yB=90; %Toạ độ điểm kết thúc
F=10; %Tốc độ ăn dao
A=8; %Gia tốc tăng tốc
D=8; %Gia tốc giảm tốc
Tipo=0.005; %Chu kỳ nội suy thô

%Nội suy thô

L=sqrt((xB-xA)^2+(yB-yA)^2); %Quãng đường di chuyển
T=L/F; %Thời gian đi hết quãng đường
N=floor(T/Tipo)+1;
F=L/(N*Tipo);
delta_L=F*Tipo; %Khoảng cách giữa các điểm
for i=1:N
delta_x(i)=delta_L*(xB-xA)/L;
delta_y(i)=delta_L*(yB-yA)/L;

end

%Tăng tốc giảm tốc

if((F^2/(2*A)+F^2/(2*D))<L)
Tacc=F/A;
Tdec=F/D;
k=1;
else
F=sqrt(L*(1/(2*A)+1/(2*D)));
Tacc=F/A;
Tdec=F/D;
k=0;
end

% Sử dụng bộ lọc
m=floor(Tacc/Tipo)+1;
delta_x0=zeros(1, N+m);
delta_y0=zeros(1, N+m);

38
%denta_x=[delta_x, zeros(1,m)];
%denta_y=[delta_y, zeros(1,m)];

delta_x0(1)=delta_x(1)/m;
delta_y0(1)=delta_y(1)/m;
for k=2:m
delta_x0(k)=delta_x(k)/m+delta_x0(k-1);
delta_y0(k)=delta_y(k)/m+delta_y0(k-1);
end
for k = m+1: N
delta_x0(k)=(delta_x(k)-delta_x(k-m))/m + delta_x0(k-
1);
delta_y0(k)=(delta_y(k)-delta_y(k-m))/m + delta_y0(k-
1);
end
for k = (N+1): (N + m)
delta_x0(k)=-delta_x(k-m)/m+delta_x0(k-1);
delta_y0(k)=-delta_y(k-m)/m+delta_y0(k-1);
end

for i=1:N+m
vx(i)=delta_x0(i)/Tipo;
vy(i)=delta_y0(i)/Tipo;
v(i)=sqrt(vx(i)^2+vy(i)^2);
end
for i=1:N+m
x0(1)=xA;
x0(i+1)=x0(i)+delta_x0(i);
y0(1)=yA;
y0(i+1)=y0(i)+delta_y0(i);
end

%Đầu vào mô phỏng simulink

trajct.time = [];
sum=[x0;y0]';
trajct.signals.values = sum;
trajct.signals.dimensions =2;

%Đồ thị quỹ đạo

39
Tk=Tipo*(N+m);
tt=[0:Tipo:Tk];
Th=Tipo*(N+m-1);
tn=[0:Tipo:Th];

figure;
x_point=[xA xB];
y_point=[yA yB];
plot(x0,y0,'r','LineWidth',2)
hold on;
title('Nôi suy trên 2 truc');
axis([0 100 0 100]);
axis on
axis equal
grid on
xlabel('X axis(mm)')
ylabel('Y axis(mm)')

figure;
plot(tn,v,'g','LineWidth',1.2);
hold on;
title('Nôi suy van toc trên 2 truc');
legend({'Van toc'});
axis([0 10 0 12]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('velocity (mm/s)');

figure;
plot(tn,vx,'g','LineWidth',1.2);
hold on;
title('Noi suy van toc trên truc X');
legend({'Van toc truc X'});
axis([0 10 0 12]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('velocity X(mm/s)');

40
figure;
plot(tn,vy,'g','LineWidth',1.2);
hold on;
title('Noi suy van toc trên truc Y');
legend({'Van toc truc Y'});
axis([0 10 0 20]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('velocity Y(mm/s)');

figure;
plot(tt,x0,'b','LineWidth',1.2);
hold on;
title('Noi suy truc X ');
legend({'Truc X'});
axis([0 10 0 80]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('Truc X');

figure;
plot(tt,y0,'b','LineWidth',1.2);
hold on;
title('Noi suy truc Y ');
legend({'Truc Y'});
axis([0 10 20 100]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('Truc Y');

ADCAI đường tròn

clear, clc;
xA=17; yA=7; %Toạ độ điểm bắt đầu
xB=6; yB=18; %Toạ độ điểm kết thúc
R=11; %Bán kính đường tròn
F=10; %Tốc độ ăn dao (rad/s)

41
A=8; %Gia tốc tăng tốc
D=8; %Gia tốc giảm tốc
Tipo=0.005; %Chu kỳ nội suy thô
BLU=0.01;
dir = 1; % Hướng quay: 1 ngược chiều kim đồng hồ, -1 cùng
chiều kim đồng hồ
goc = 1; % Góc quay: 1 với góc quay nhỏ hơn 180 độ, -1 với
góc quay lớn hơn 180 độ
Tacc=F/A;
J1=floor(Tacc/Tipo)+1;

%Tìm tâm của đường tròn

z=dir*goc;
%z=-2
d=sqrt((xB-xA)^2+(yB-yA)^2);
M=[(xA+xB)/2, (yA+yB)/2];
n=[(xB-xA)/d, (yB-yA)/d];
n1=[-(yB-yA)/d, (xB-xA)/d];
h=sqrt(R^2-d^2/4);
x0=(xA+xB)/2-z*h*(yB-yA)/d; %Toạ độ tâm x của đường
tròn
y0=(yA+yB)/2+z*h*(xB-xA)/d; %Toạ độ tâm y của đường
tròn

if goc==1
sigma=abs(acos((xA-x0)*(xB-x0)+(yA-y0)*(yB-y0)));
else
sigma=2*pi-abs(acos((xA-x0)*(xB-x0)+(yA-y0)*(yB-y0)));
end

anpha=(2*BLU)/(sigma*R); % Tính góc anpha Euler

N=floor(sigma/anpha)+1;
anpha=sigma/N;
T=sigma*R/F;

%Nội suy thô

X = [xA 0];
Y = [yA 0];
dx0=zeros(1,N+J1);
dy0=zeros(1,N+J1);
for i = 1:N

42
 dx0(i)=(cos(anpha)-1)*(X(i)-x0)-sin(anpha)*(Y(i)-y0);
dy0(i)=(cos(anpha)-1)*(Y(i)-y0)+sin(anpha)*(X(i)-x0);

if(dir==1)
X(i+1) = X(i)+dx0(i);
Y(i+1) = Y(i)+dy0(i);
else
X(i+1) = X(i)-dx0(i);
Y(i+1) = Y(i)-dy0(i);
end
end

%Sử dụng bộ lọc

J1=floor(Tacc/Tipo)+1;
dx=zeros(1, N+J1);
dy=zeros(1, N+J1);

dx(1)=dx0(1)/J1;
dy(1)=dy0(1)/J1;
for k=2:J1
dx(k)=dx0(k)/J1+dx(k-1);
dy(k)=dy0(k)/J1+dy(k-1);
end
for k = J1+1: N
dx(k)=(dx0(k)-dx0(k-J1))/J1 + dx(k-1);
dy(k)=(dy0(k)-dy0(k-J1))/J1 + dy(k-1);
end
for k = (N+1): (N + J1)
dx(k)=-dx0(k-J1)/J1+dx(k-1);
dy(k)=-dy0(k-J1)/J1+dy(k-1);
end

for i=1:N+J1
vx(i)=dx(i)/Tipo;
vy(i)=dy(i)/Tipo;
v(i)=sqrt(vx(i)^2+vy(i)^2);
if(dir==1)
x(1)=xA;
x(i+1)=x(i)+dx(i);
y(1)=yA;

43
 y(i+1)=y(i)+dy(i);
else
x(1)=xA;
x(i+1)=x(i)-dx(i);
y(1)=yA;
y(i+1)=y(i)-dy(i);
end
end

%Đầu vào mô phỏng simulink

trajct.time = [];%iput simulink
sum=[x;y]';
trajct.signals.values = sum;
trajct.signals.dimensions =2;

%Đồ thị quỹ đạo

Tk=Tipo*(N+J1);
tt=[0:Tipo:Tk];
Th=Tipo*(N+J1-1);
tn=[0:Tipo:Th];

figure;
x_point=[xA xB];
y_point=[yA yB];
plot(x,y,'r','LineWidth',2)
hold on;
title('Nôi suy trên 2 truc');
axis([0 20 0 20]);
axis on
axis equal
grid on
xlabel('X axis(mm)')
ylabel('Y axis(mm)')

figure;
plot(tn,v,'g','LineWidth',1.2);
hold on;
title('Nôi suy van toc trên 2 truc');

44
legend({'Van toc'});
axis([0 10 0 4]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('velocity (mm/s)');

figure;
plot(tn,vx,'g','LineWidth',1.2);
hold on;
title('Noi suy van toc trên truc X');
legend({'Van toc truc X'});
axis([0 10 -4 2]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('velocity X(mm/s)');

figure;
plot(tn,vy,'g','LineWidth',1.2);
hold on;
title('Noi suy van toc trên truc Y');
legend({'Van toc truc Y'});
axis([0 10 0 4]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('velocity Y(mm/s)');

figure;
plot(tt,x,'b','LineWidth',1.2);
hold on;
title('Noi suy truc X ');
legend({'Truc X'});
axis([0 10 0 20]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('Truc X');

45
figure;
plot(tt,y,'b','LineWidth',1.2);
hold on;
title('Noi suy truc Y ');
legend({'Truc Y'});
axis([0 10 0 20]);
axis on;
grid on;
xlabel('Time(s)');
ylabel('Truc Y');
ADCBI đường thẳng
F=20; %toc do an dao 10mm/s^2
A=100; %gia toc tang toc cho phep
D=100; %gia toc giam toc cho phep
L=5; %chieu dai
T=5e-3; %chu ky noi suy
Tdk=1e-3;%chu ky dieu khien
Nt=T/Tdk;
I=(F*F)/(2*A)+(F*F)/(2*D)-L;
%%tinh toan thoi gian tang toc giam toc
Ta=F/A;%time acc
Td=F/D;%time dec
Sa=(F*F)/(2*A);
Sd=(F*F)/(2*D);
Tc=(L-Sa-Sd)/F;%time const
xA = 6; yA=7;
%%Noi suy
Va0=0;
Vd0=F;
Na=Ta/T;
Nd=Td/T;
Nc=Tc/T;
Va=[Va0:T*A:F];%Vi+1=Vi+A*T
Vd=[Vd0:-T*D:0];%Vi+1=Vi-A*T
Vc=ones(1,Nc+1)*F;
ta=[0:T:Ta];
td=[Tc+Ta:T:(Tc+Ta+Td)];
tc=[Ta:T:(Ta+Tc)];
%Tinh toan khoang tang toc noi suy tho
deltaL1=Va*T;
deltaX1=deltaL1*0.8;
deltaY1=deltaL1*0.6;
syms i
X1=zeros(1,Na+1);Y1=zeros(1,Na+1);
X1(1,1)=6;Y1(1,1)=7;
for i=2:Na+1
X1(1,i)=X1(1,i-1)+deltaX1(1,i);
Y1(1,i)=Y1(1,i-1)+deltaY1(1,i);
end
%Tinh toan khoang Const noi suy tho
deltaL2=Vc*T;
deltaX2=deltaL2*0.8;
deltaY2=deltaL2*0.6;

46
syms j
X2=zeros(1,Nc+1);Y2=zeros(1,Nc+1);
X2(1,1)=X1(1,Na+1); Y2(1,1)=Y1(1,Na+1);
for j=2:Nc+1
X2(1,j)=X2(1,j-1)+deltaX2(1,j);
Y2(1,j)=Y2(1,j-1)+deltaY2(1,j);
end
%Tinh toan khoang giam toc noi suy tho
deltaL3=Vd*T;
deltaX3=deltaL3*0.8;
deltaY3=deltaL3*0.6;
syms k
X3=zeros(1,Nd);Y3=zeros(1,Nd);
X3(1,1)=X2(1,Nc+1);Y3(1,1)=Y2(1,Nc+1);
for k=2:Nd+1
X3(1,k)=X3(1,k-1)+deltaX3(1,k);
Y3(1,k)=Y3(1,k-1)+deltaY3(1,k);
end
%Tinh toan khoang acc ns tinh theo pp tuyen tinh
syms i j
X1c=zeros(1,Na*Nt+1);X1c(1,Na*Nt+1)=deltaX1(1,Na+1)/Nt;%khoang
tang toc ns tinh truc X
Y1c=zeros(1,Na*Nt+1);Y1c(1,Na*Nt+1)=deltaY1(1,Na+1)/Nt;%khoang
tang toc ns tinh truc Y

for i=1:Na
for j=(Nt*i-Nt+1):(Nt*i)
X1c(1,j)=deltaX1(1,i)/Nt;
Y1c(1,j)=deltaY1(1,i)/Nt;
end
end
syms p
X11=zeros(1,Na*Nt+1);
Y11=zeros(1,Na*Nt+1);
X11(1,1)=6; Y11(1,1)=7;
for p=2:Na*Nt+1
X11(1,p)=X11(1,p-1)+X1c(1,p);
Y11(1,p)=Y11(1,p-1)+Y1c(1,p);
end
%Tinh toan khoang cons ns tinh theo pp tuyen tinh
syms i j
X2c=zeros(1,Nc*Nt+1);X2c(1,Nc*Nt+1)=deltaX2(1,Nc+1)/Nt;
Y2c=zeros(1,Nc*Nt+1);Y2c(1,Nc*Nt+1)=deltaY2(1,Nc+1)/Nt;
for i=1:Nc
for j=(Nt*i-Nt+1):(Nt*i)
X2c(1,j)=deltaX2(1,i)/Nt;
Y2c(1,j)=deltaY2(1,i)/Nt;
end
end
X21=zeros(1,Nc*Nt+1);Y21=zeros(1,Nc*Nt+1);
X21(1,1)=X11(1,Na*Nt+1);Y21(1,1)=Y11(1,Na*Nt+1);
for k=2:Nc*Nt+1
X21(1,k)=X21(1,k-1)+X2c(1,k);
Y21(1,k)=Y21(1,k-1)+Y2c(1,k);
end
%Tinh toan khoang dec ns tinh theo pp tuyen tinh
syms i j%ns tuyen tinh
X3c=zeros(1,Na*Nt+1); X3c(1,1)=deltaX3(1,1)/Nt;

47
Y3c=zeros(1,Na*Nt+1); Y3c(1,1)=deltaY3(1,1)/Nt;
for i=2:Nd
for j=(Nt*(i-1)-Nt+2):(Nt*(i-1)+1)
X3c(1,j)=deltaX3(1,i)/Nt;
Y3c(1,j)=deltaY3(1,i)/Nt;
end
end
syms l
X31=zeros(1,Nd*Nt+1);Y31=zeros(1,Nd*Nt+1);
X31(1,1)=X21(1,Nc*Nt+1);Y31(1,1)=Y21(1,Nc*Nt+1);
for l=2:Nd*Nt+1
X31(1,l)=X31(1,l-1)+X3c(1,l);
Y31(1,l)=Y31(1,l-1)+Y3c(1,l);
end
trajct.time = [];%iput simulink
sum=[X11,X21,X31;Y11,Y21,Y31]';
trajct.signals.values = sum;
trajct.signals.dimensions = 2;

figure;%noi suy 2 diem A,B

x_point=[6 10];
y_point=[7 10];
plot (x_point,y_point,'b o',x_point,y_point,'-
m','LineWidth',1.2)%duong thang qua 2 diem
hold on
plot(X11,Y11,'r',X21,Y21,'g',X31,Y31,'b','LineWidth',1.2)%duong
thang sau khi noi suy
hold on
plot(out.x_st,out.y_st,'--y','LineWidth',1.2)%duong thang sau
khi di qua bdk
hold on
axis([0 10 0 10]);
axis on
grid on
xlabel('X axis(mm)')
ylabel('Y axis(mm)')
legend({'SetPoints','SP','Acc','Cons','Dec','Controllers'},'Loca
tion','northeast','NumColumns',3)
title('Noi suy duong thang')
figure;%chia 3 do thi
subplot(2,1,1);
plot(x_point,y_point,'b
o',X11,Y11,'r',X2,Y2,'g',X31,Y31,'b','LineWidth',1.2)
axis([0 10 0 10]);
axis on
grid on
xlabel('X axis(mm)')
ylabel('Y axis(mm)')
legend({'SetPoints','Acc','Cons','Dec'},'Location','northwest','
NumColumns',3)
title('Noi suy duong thang')
subplot(2,1,2);
plot(x_point,y_point,'b o',out.x_st,out.y_st,'--
y','LineWidth',1.2);
axis([0 10 0 10]);
axis on
grid on
xlabel('X axis(mm)')

48
ylabel('Y axis(mm)')
legend({'SetPoints','Controllers'},'Location','northwest','NumCo
lumns',2)
title('PD Controllers')

figure;%noi suy van toc

[tasz,az]=stairs(ta,Va);[tcz,Vcz]=stairs(tc,Vc);
[tdsz,a1z]=stairs(td,Vd);
plot(tasz,az,'r',tcz,Vcz,'g',tdsz, a1z,'b','LineWidth',1.2)
axis([0 Tc+Ta+Td 0 F+1]);
legend({'Acc','Constant','Dec'},'Location','northeast','NumColum
ns',3)
axis on
grid on
xlabel('Time(s)')
ylabel('Velocity(mm/s)')
title('Noi suy van toc')
figure;
[x1z,y1z]=stairs(X11,Y11);[x2z,y2z]=stairs(X21,Y21);
[x3z,y3z]=stairs(X31,Y31);
plot(x_point,y_point,'b
o',x1z,y1z,'r',x2z,y2z,'g',x3z,y3z,'b','LineWidth',1.2)
axis([0 10 0 10]);
axis on
grid on
xlabel('X axis(mm)')
ylabel('Y axis(mm)')
legend({'SetPoints','Acc','Cons','Dec'},'Location','northwest','
NumColumns',3)
title('Noi suy tinh duong thang')
ADCBI đường tròn
clear sum;
xA=6; yA=1;
F=5; %toc do an dao
A=40; %gia toc tang toc cho phep
D=40; %gia toc giam toc cho phep
L=pi/2; %chieu dai
T=5e-3; %chu ky noi suy
Tdk=1e-3;
Nt=T/Tdk;
I=(F*F)/(2*A)+(F*F)/(2*D)-L;
%%tinh toan thoi gian tang toc giam toc
Ta=F/A;%time acc
Td=F/D;%time dec
Sa=(F*F)/(2*A);
Sd=(F*F)/(2*D);
Tc=(L-Sa-Sd)/F;%time cons
%%Noi suy duong tron
Wa0=0;
Wd0=F;
Na=Ta/T;
Nd=Td/T;
Nc=floor(Tc/T);
Wa=Wa0:T*A:F;%Vi+1=Vi+A*T
Wd=Wd0:-T*D:0;%Vi+1=Vi-A*T
Wc=ones(1,Nc+1)*F;
ta=0:T:Ta;

49
td=Tc+Ta:T:(Tc+Ta+Td);
tc=Ta:T:(Ta+Tc);
tas=0:Tdk:Ta;
tds=Tc+Ta:Tdk:(Tc+Ta+Td);
%Noi suy tho duong tron
X_tam=2;
Y_tam=1;
R=4;
Va=Wa*R;%w=v/r noi suy toc do tang toc
alpha=Wa*T;%noi suy goc alpha khi tang toc
tong=sum(alpha);%tong quang duong tang toc

Vd=Wd*R;%toc do giam toc

alpha2=Wd*T;%noi suy goc alpha khi giam toc
tong2=sum(alpha2);%tong quang duong giam toc

Vc=F*R;%toc do cons
tong1=pi/2-tong-tong2;%quang duong cons
Tc_dtr=tong1/F;%Thoi gian cons
Nc_dtr=round(Tc_dtr/T);% so lan noi suy cons
Wc_dtr=ones(1,Nc_dtr+1)*F;%toc do cons
alpha1=Wc_dtr*T;% noi suy goc alpha cons

%Noi suy tang toc

A_dtr=cos(alpha);%A=cos
B_dtr=sin(alpha);%B=sin
X10=zeros(1,Na+1);
Y10=zeros(1,Na+1);
X10(1,1)=6;Y10(1,1)=1;
syms z
for z=2:Na+1
X10(1,z)=(X10(1,z-1)-X_tam)*A_dtr(1,z-1)-(Y10(1,z-1)-
Y_tam)*B_dtr(1,z-1)+X_tam;
Y10(1,z)=(Y10(1,z-1)-Y_tam)*A_dtr(1,z-1)+(X10(1,z-1)-
X_tam)*B_dtr(1,z-1)+Y_tam;
end
%Noi suy const
A_dtr1=cos(alpha1);
B_dtr1=sin(alpha1);
X11=zeros(1,Nc_dtr+1);Y11=zeros(1,Nc_dtr+1);
X11(1,1)=X10(1,Na+1);Y11(1,1)=Y10(1,Na+1);
syms v
for v=2:Nc_dtr+1
X11(1,v)=(X11(1,v-1)-X_tam)*A_dtr1(1,v-1)-(Y11(1,v-1)-
Y_tam)*B_dtr1(1,v-1)+X_tam;
Y11(1,v)=(Y11(1,v-1)-Y_tam)*A_dtr1(1,v-1)+(X11(1,v-1)-
X_tam)*B_dtr1(1,v-1)+Y_tam;
end
%Noi suy giam toc
A_dtr2=cos(alpha2);
B_dtr2=sin(alpha2);
X12=zeros(1,Nd+1);Y12=zeros(1,Nd+1);
X12_dtr=zeros(1,Nd+1);Y12_dtr=zeros(1,Nd+1);
X12(1,1)=X11(1,Nc_dtr+1); Y12(1,1)=Y11(1,Nc_dtr+1);
syms m
for m=2:Nd+1

50
 X12(1,m)=(X12(1,m-1)-X_tam)*A_dtr2(1,m-1)-(Y12(1,m-1)-
Y_tam)*B_dtr2(1,m-1)+X_tam;
Y12(1,m)=(Y12(1,m-1)-Y_tam)*A_dtr2(1,m-1)+(X12(1,m-1)-
X_tam)*B_dtr2(1,m-1)+Y_tam;
end

%Chia nho goc noi suy tinh acc

syms i j%ns tuyen tinh
a=zeros(1,Na*Nt+1);a(1,Na*Nt+1)=alpha(1,Na+1)/Nt;
for i=1:Na
for j=(Nt*i-Nt+1):(Nt*i)
a(1,j)=alpha(1,i)/Nt;
end
end
%chia nho goc noi suy tinh cons
syms i j k
a1=zeros(1,Nc_dtr*Nt+1);a1(1,Nc_dtr*Nt+1)=alpha1(1,Nc_dtr+1)/Nt;
for i=1:Nc_dtr
for j=(Nt*i-Nt+1):(Nt*i)
a1(1,j)=alpha1(1,i)/Nt;
end
end
%chia nho goc noi suy tinh dec
syms i j%ns tuyen tinh
a2=zeros(1,Nd*Nt+1);a2(1,1)=alpha2(1,1)/Nt;
for i=2:Nd
for j=(Nt*(i-1)-Nt+2):(Nt*(i-1)+1)
a2(1,j)=alpha2(1,i)/Nt;
end
end
%Noi suy tinh acc
A1_dtr=cos(a);%A=cos
B1_dtr=sin(a);%B=sin
X10c=zeros(1,Na*Nt+1);Y10c=zeros(1,Na*Nt+1);
X10c(1,1)=6;Y10c(1,1)=1;
syms z
for z=2:Na*Nt+1
X10c(1,z)=(X10c(1,z-1)-X_tam)*A1_dtr(1,z-1)-(Y10c(1,z-1)-
Y_tam)*B1_dtr(1,z-1)+X_tam;
Y10c(1,z)=(Y10c(1,z-1)-Y_tam)*A1_dtr(1,z-1)+(X10c(1,z-1)-
X_tam)*B1_dtr(1,z-1)+Y_tam;
end
%Noi suy tinh const
A1_dtr1=cos(a1);
B1_dtr1=sin(a1);
X11c=zeros(1,Nc_dtr*Nt+1);Y11c=zeros(1,Nc_dtr*Nt+1);
X11c(1,1)=X10c(1,Na*Nt+1);Y11c(1,1)=Y10c(1,Na*Nt+1);
syms v
for v=2:Nc_dtr*Nt+1
X11c(1,v)=(X11c(1,v-1)-X_tam)*A1_dtr1(1,v-1)-(Y11c(1,v-1)-
Y_tam)*B1_dtr1(1,v-1)+X_tam;
Y11c(1,v)=(Y11c(1,v-1)-Y_tam)*A1_dtr1(1,v-1)+(X11c(1,v-1)-
X_tam)*B1_dtr1(1,v-1)+Y_tam;
end

%Noi suy tinh dec

A1_dtr2=cos(a2);
B1_dtr2=sin(a2);

51
X12c=zeros(1,Nd*Nt+1);Y12c=zeros(1,Nd*Nt+1);
X12c(1,1)=X11c(1,Nc_dtr*Nt+1);Y12c(1,1)=Y11c(1,Nc_dtr*Nt+1);
syms m
for m=2:Nd*Nt+1
X12c(1,m)=(X12c(1,m-1)-X_tam)*A1_dtr2(1,m-1)-(Y12c(1,m-1)-
Y_tam)*B1_dtr2(1,m-1)+X_tam;
Y12c(1,m)=(Y12c(1,m-1)-Y_tam)*A1_dtr2(1,m-1)+(X12c(1,m-1)-
X_tam)*B1_dtr2(1,m-1)+Y_tam;
end
%input simulink
trajct.time = [];
sum=[X10c,X11c,X12c;Y10c,Y11c,Y12c]';
trajct.signals.values = sum;
trajct.signals.dimensions =2;

%V? ?? th?
figure;
plot(ta,Wa,'r',tc,Wc,'g',td,Wd,'b','LineWidth',1.2)
axis([0 0.45 0 6]);
legend({'Acc','Constant','Dec'},'Location','northeast','NumColum
ns',3)
xlabel('Time(s)')
ylabel('Velocity(mm/s)')
title('Velocity Interpolation')
grid on
figure;
[x4z,y4z]=stairs(X10c,Y10c);[x5z,y5z]=stairs(X11c,Y11c);
[x6z,y6z]=stairs(X12c,Y12c);
plot(x4z,y4z,'r',x5z,y5z,'g',x6z,y6z,'b','LineWidth',1.2)
axis([0 6 0 7]);
axis on
grid on
xlabel('X axis(mm)')
ylabel('Y axis(mm)')
legend('Acc','Cons','Dec')
figure;
[x1z,y1z]=stairs(X10,Y10);
plot(x1z,y1z,'b',x4z,y4z,'r','LineWidth',1.2);
xlabel('X axis(mm)')
ylabel('Y axis(mm)')
legend({'Noi suy tho','Noi suy tinh'})
figure;
plot(X10c,Y10c,'r',X11c,Y11c,'g',X12c,Y12c,'b','LineWidth',1.2)%
duong thang sau khi noi suy
hold on
plot(out.x_st,out.y_st,'--r','LineWidth',1.2)%duong tròn sau khi
qua bo dk
hold on
legend({'Input Simulink','OutputSimulink'})

52