Nhoms7 Cuoiki

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN ĐIỆN
CHUYÊN NGÀNH KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA
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
Thành viên: MSSV

Đỗ Đức Thắng 20174202
Nguyễn Văn Hiếu 20173866
Phạm Thế Thiện 20174235
Hà Nội, 6/2021
Mục lục
Mục Lục
PHẦN I: ĐỀ BÀI......................................................................................................3
PHẦN II: BÀI LÀM.................................................................................................4
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
1
Đề bài
PHẦN I: ĐỀ 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ều
khiể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ậc
nhất:
Vx k1
Trục x: =
Ux T 1 s+1
Vy k2
Trục y: =
Uy T 2 s+ 1
Với k1≠ k2, T1≠ T2 tự chọn, và T1, T2 nhỏ (< 0.5).
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í cho từ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đk | ≤ 10V
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ốc dạ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 tùy
chọn.
- 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(x0,y0) 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, amax và vmax tự chọn.
Chia tiếp 5ms nội suy theo 2 phương pháp tuyến tính và trung bình.
3
Đề bài
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.
4
Nội suy đường thẳng
PHẦN II: BÀI LÀM
1. Thiết kế bộ điều khiển PID vị trí cho từng trục

* Hai hàm truyền của đối tượng:
k1 4
- Trục x: G1(s)¿ =
T 1 +1 0.02 s+1
k2 6
- Trục y: G2(s)¿ =
T 2 +1 0.01 s+1
* Mô phỏng trên matlab:
+ Mục tiêu thiết kế:
 Đáp ứng càng nhanh càng tốt

 |Udk| <=10
 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:
7
Sử dụng PID tuning trong matlab sau đó hiệu chỉnh để đạt được kết quả tốt hơn.
- Trục x:
- Trục y:
Kết quả thu đc

Trục X
Hình 2: Đáp ứng trục X
8
Nhận xét:
- Trục x: 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
+ |Udk|max ≈ 9.398 < 10
+ thời gian xác lập ≈ 0.11s
+ Độ quá điều chỉnh ≈ 1.3%
9
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
+ |Udk|max ≈ 9.679 < 10
10
+ thời gian xác lập ≈ 0.06s
+ Độ quá điều chỉnh ≈ 1.2%
2. Nội suy
2.1 Nội suy đường thẳng
2.1.1 Công thức tính toán nội suy đường thẳng
Hình 4: Nội suy đường thẳng

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/ s2)
Gia tốc giảm tốc cho phép: D=20(mm/ s2)
Chu kỳ nội suy: τ =5 ( ms )
Tốc độ max: F= v max=20 (mm/ s)
F 2 F2
+ =20< L=50 ⟹ Normal block
2 A 2D
Các bước thực hiện cho bài toán
11
a) Gia tốc,giảm tốc và nội suy thô

 Profile vận tốc hình thang
F
Thời gian tăng tốc Ta= A =1(s)
F
Thời gian giảm tốc Td= D =1 ( s )
F2 F2
L− −
2A 2D
Thời gian vận tốc không đổi Tc= =1.5( s)
F
2.1.2 Nội suy thô

 Các bước thực hiện
12
LAB= √ (x B −x A )2+( y B − y A )2=√ (30−70)2+(50−80)2 =50 (mm)

 Gia tốc
TA
NA¿ τ
=200
x e −x s ye− ys
i Vi(mm/s) Li(mm/s) Δx =Δ L Δ y =Δ L
L L
0 0 0.00025 0.0002 0.00015
1 0.1 0.00075 0.0006 0.00045
2 0.2 0.00125 0.001 0.00075
3 0.3 0.00175 0.0014 0.00105
4 0.4 0.00225 0.0018 0.00135
… … … … …
… … … … …
19 19.9 0.09975 0.0798 0.05985
9
13
20 20.0 -- -- --
0
 Giảm tốc
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)
Li là độ dịch chuyển của lần lấy mẫu thứ i
TD
N D= =200
τ
x e −x s ye− ys
i Vi(mm/s) Li(mm/s) Δx =Δ L Δ y =Δ L
L L
0 20 0.09975 0.0798 0.05985
1 19.9 0.09925 0.0794 0.05955
2 19.8 0.09875 0.079 0.05925
3 19.7 0.09825 0.0786 0.05895
4 19.6 0.09775 0.0782 0.05865
.. … … … …
.. … … … …
19 0.1 0.00025 0.0002 0.00015
9
20 0 -- -- --
0
2.1.3 Nội suy tinh
Chu kì nội suy 5ms, chu kì điều khiển 1ms → N=5
 Phương pháp:
+)Phương pháp tuyến tính
+)Phương pháp trung bình
14
Hình 5: chu kì trích mẫu nội suy và chu kì điều khiển
 Trục X
Hình 6: Tính toán nội suy tuyến tính trục X
 Trục Y
15
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
16
Hình 9: đồ thị trục X
Hình 10: đồ thị trục Y
17
Hình 11: Profile vận tốc
18
Nội suy đường tròn
2.2 Nội suy đường tròn

2.2.1 Xây dựng công thức nội suy đường tròn
Hình 12: Lý thuyết tính độ dịch theo góc lệch alpha

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) = Ri.𝒄𝒐𝒔 θi+1 = AX(i)−BY(i)
Y(i+1)= Ri. 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
Fmax = 20mm/s, gia tốc tăng giảm tốc là 20mm/s2, 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
 A ≈ 0.99 B ≈ 0.127 
14
Nội suy thô sai lệch đường tròn

i Xi Yi DXi DYi
0 8 0 -0.1011 1.267
1 7.8988 1.267 -0.3007 1.235
2 7.5981 2.5036 -0.4928 1.1725
3 7.1053 3.6762 -0.672 1.079
4 6.432 4.755 -0.835 0.959
5 5.597 5.71 -0.976 0.814
6 4.621 6.530 -1.093 0.649
7 3.527 7.180 -1.183 0.468
8 2.345 7.648 -1.241 0.275
9 1.103 7.923 -1.269 0.074
… … …
Nội suy tinh đường tròn ta làm tương tự đường thẳng
2.2.2 Mô phỏng
 Kết quả mô phỏng
Hình 13: Nội suy thô và nội suy tinh đường tròn
15
Hình 14: sự khác nhau giữa nội suy thô và nội suy tinh
Hình 15: Đồ thị trục X
16
Hình 16: Đồ thị trục Y
Hình 17: Đồ thị vận tốc
17
Tăng giảm tóc sau nội suy
3. Vẽ Chữ C
3.1 Tính toán lý thuyết
Cách vẽ: vẽ 2 đường tron và 1 đường thẳng

- Nội suy đường tròn từ điểm A60;50) đến điểm B(20;50) tâm
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
20
3.2 Mô phỏng
Hình 19: Nội suy tinh và thô chữ C
Hình 20: Sự khác nhau giữa nội suy tinh và thô
21
22
Hình 23: Đồ thị vận tốc
23
4.Thực hiện tăng tốc/ giảm tốc sau nội suy
4.1 Tổng quan chung
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
n
ứng xung, được xác định bằng công thức ∑ h [ k ] =1và hàm x[n] là giá trị
k=0
đầu vào. Và đầu ra được biểu diễn theo công thức:

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

k =1
Á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:
20
−m
1 1−z
HL(z) = m 1−z−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:
1
AX (k) = m (i(k) – Vi(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(xs, ys) , B( xe, ye), => độ dài AB = L,
với chu kỳ nội suy T. Vận tốc ăn dao lớn nhất F max . góc hợp bởi đoạn
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
21
Mỗi chu kì nội suy, dao sẽ đi được 1 khoảng ∆ L, tương ứng trên trục Ox
sẽ đi được 1 khoảng ∆ x = ∆ L.cosθ và trên trục Oy đi được 1 khoảng ∆ y =
∆ L.sinθ .
Từ đó ta có các đoạn dịch chuyển ax, ay trong các 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 Tcyc = 5ms đi được một đoạn ∆ L
Chọn a = d = 20mm/s2, Fmax = v max = 20mm/s

3.354
Nội suy trong khoảng thời gian 3.354s, m = 0.005 = 670.8 sấp sỉ 671
L 67.082
AL[k] = m = ∆L = 671 = 0.1 mm
 ∆ x = 0.1 .cosθ
 ∆ y = 0.1 .sinθ
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:
k AL[k] AL(k-200) S(k) AL0(k)
1 0.1 0 0.1 0.0005
2 0.1 0 0.2 0.0010
3 0.1 0 0.3 0.0015
4 0.1 0 0.4 0.0020
5 0.1 0 0.5 0.0025
6 0.1 0 0.6 0.0030
7 0.1 0 0.7 0.0035
8 0.1 0 0.8 0.0040
9 0.1 0 0.9 0.0045
10 0.1 0 1.0 0.0050
11 0.1 0 1.1 0.0055
12 0.1 0 1.2 0.0060
13 0.1 0 1.3 0.0065
22
14 0.1 0 1.4 0.0070
15 0.1 0 1.5 0.0075
…. … … … …
198 0.1 0 19.8 0.0990
199 0.1 0 19.9 0.0995
200 0.1 0 20.0 0.1000
201 0.1 0.1 20.0 0.1000
202 0.1 0.1 20.0 0.1000
… … … … …
665 0.1 0.1 20.0 0.1000
666 0.1 0.1 20.0 0.1000
667 0.1 0.1 20.0 0.1000
668 0.1 0.1 20.0 0.1000
669 0.1 0.1 20.0 0.1000
670 0.1 0.1 20.0 0.1000
671 0.1 0.1 20.0 0.1000
672 0 0.1 19.9 0.0995
673 0 0.1 19.8 0.0990
674 0 0.1 19.7 0.0985
675 0 0.1 19.6 0.0980
676 0 0.1 19.5 0.0975
677 0 0.1 19.4 0.0970
678 0 0.1 19.3 0.0965
… … … … …
865 0 0.1 0.6 0.0030
866 0 0.1 0.5 0.0025
867 0 0.1 0.4 0.0020
868 0 0.1 0.3 0.0015
869 0 0.1 0.2 0.0010
870 0 0.1 0.1 0.0005
871 0 0.1 0
Σ 671 671
4.3 Nội suy đường tròn
23
Về cơ bản, các tăng tốc/ giảm tốc sau nội suy đường tròn giống với nội
suy đường thẳng. Điểm khác là ∆ x, ∆ y thay đổi và ảnh hưởng bởi giá trị
xuất phát cũ.

L
x[k] = m = ∆ L = √ DX (i )2+ DY (i)2 // bỏ
F 20
Vận tốc góc ω = R = 20 (rad/s), ωmax = 1rad/s, aht max = dht max = 1 rad/s2
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 W max. Nên trong các chu kỳ
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.
24
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
đườ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/s2, góc quay 2 π , chu kì nội suy 5ms.
với dL(k) = 10 x 0.005 = 0.05 rad
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:
K dL(k) dL(k-201) S(k) dL0(k)
1 0.005 0 0.005 2.5e-5
2 0.005 0 0.010 5.0e-5
3 0.005 0 0.015 7.5e-5
4 0.005 0 0.020 10e-5
5 0.005 0 0.025 12.5e-5
6 0.005 0 0.030 15e-5
7 0.005 0 0.035 17.5e-5
8 0.005 0 0.040 20e-5
9 0.005 0 0.045 22.5e-5
10 0.005 0 0.050 25e-5
11 0.005 0 0.055 27.5e-5
12 0.005 0 0.060 30.e-5
13 0.005 0 0.065 32.5e-5
14 0.005 0 0.070 35e-5
15 0.005 0 0.075 37.5e-5
… … … … …
196 0.005 0 0.980 490e-5
197 0.005 0 0.985 492.5e-5
198 0.005 0 0.990 495e-5
199 0.005 0 0.995 497.5e-5
200 0.005 0 1.000 500e-5
201 0.005 0 1.005 500e-5
202 0.005 0.005 1.005 500e-5
203 0.005 0.005 1.005 500e-5
25
204 0.005 0.005 1.005 500e-5
205 0.005 0.005 1.005 500e-5
… … …
1250 0.005 0.005 1.005 500e-5
1251 0.005 0.005 1.005 500e-5
1252 0.005 0.005 1.005 500e-5
1253 0.005 0.005 1.005 500e-5
1254 0.005 0.005 1.005 500e-5
1255 0.005 0.005 1.005 500e-5
1256 0.005 0.005 1.005 500e-5
1257 0.005 0.005 1.005 500e-5
1258 0 0.005 1.000 500e-5
1259 0 0.005 0.995 497.5e-5
1260 0 0.005 0.990 495e-5
1261 0 0.005 0.985 492.5e-5
1262 0 0.005 0.980 490e-5
…. … … … …
1456 0 0.005 0.010 5e-5
1457 0 0.005 0.005 2.5e-5
1458 0 0.005 0 0
Σ 6.285 6.285
4.4 Kết quả mô phỏng.
Hình 24: Nội suy thô đoan AB
26
27
Hình 27: Đồ thị vận tốc đoạn thẳng
28
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
29
Hình 30: Đầu ra trục Y
Hình 31: Đồ thị tốc độ
30
4 .CODE MATLAB
%noi suy duong thang

clear
clc
A = 20;
D = 20;
F = 20;
Tipo = 5e-3;
Tdk = 1e-3;
xA = 30; yA = 50;
xB = 70; yB = 80;
L = sqrt((xB-xA)^2+(yB-yA)^2);
cos =(xB-xA)/L;
sin =(yB-yA)/L;
Ta = F/A;
Td = F/D;
Tc = (L - F^2/(2*A)-F^2/(2*D))/F;
T = Ta + Td + Tc;
%ma tran bo nho
nA = 0:1:(Ta/Tipo); %them gia tri 0 the hien toc do ban dau la 0
nT = 1:1:(T/Tipo);
nC = 1:1:(Tc/Tipo);
nD = 1:1:(Td/Tipo);
%tinh toc do
Va = 0 + A*nA*Tipo;
Vc = F + 0*nC*Tipo;
Vd = F - D*nD*Tipo;
V = [Va, Vc, Vd];
%noi suy tho
dL = V;
for i=1:(length(V)-1)
dL(i) = ((V(i)+V(i+1))/2)*Tipo;
end
X = xA + cumsum(dL)*cos;
Y = yA + cumsum(dL)*sin;
%do thi noi suy tho duong thang
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 tho duong thang AB')
%do thi toc do
31
figure(4)
time = Tipo*(1:(fix(T/Tipo)+1));
plot (time', dL/Tipo, 'linewidth', 1.5)
grid on
axis([-1 5, -1 30])
xlabel('t (s)')
ylabel('V (m/s)')
title('Do thi toc do')
%noi suy tinh su dung phuong phap tuyen tinh
N = Tipo/Tdk;
dL_t = dL(1)/5;
for i = 1:length(dL)
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)
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:
%noi suy duong tron

clear
32
clc
A = 20;
D = 20;
F = 20;
Tipo = 5e-3;
Tdk = 1e-3;
%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
L = R*2*pi;
Ta = F/A;
Td = F/D;
Tc = (L - F^2/(2*D) - F^2/(2*A))/F;
T = Td + Ta + Tc;
%ma tran bo nho
nC = 1:1:ceil(Tc/Tipo);
nD = 1:1:(Td/Tipo);
nT = 1:1:ceil(T/Tipo);
%tinh toc do dai
Va = 0 + A*nA*Tipo;
Vc = F + 0*nC*Tipo;
Vd = F - D*nD*Tipo;
V = [Va, Vc, Vd];
%tinh to do goc
W = V/R;
%tinh goc di chuyen trong noi Tipo
angle = W*Tipo;
%noi suy tho
X = angle;
Y = angle;
X(1) = xA;
Y(1) = yA;
for i = 2:length(angle)
%if (X(1) >= 20 &&
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)
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)
grid on
xlabel('t(s)')
ylabel('X')
title('dau ra X')
33
%dau ra Y
figure(3)
grid on
xlabel('t(s)')
ylabel('Y')
title('dau ra Y')
%do thi toc do
figure(4)
plot(time', angle*R/Tipo, 'linewidth', 1.5)
grid on
xlabel('t(s)')
ylabel('V (m/s)')
title('do thi toc do')
%noi suy tinh su dung phuong phap tuyen tinh
N = Tipo/Tdk;
angle_t = angle/N;
t=1;
X_t(t) = xA;
Y_t(t) = yA;
for i = 1:length(angle_t)
if i == 1
for j = 2:N
t = t+1;
X_t(t) = xO + (X_t(t-1) - xO)*cos(angle_t(i)) + (yO - Y_t(t-
1))*sin(angle_t(i));
Y_t(t) = yO - (yO - Y_t(t-1))*cos(angle_t(i)) + (X_t(t-1) -
xO)*sin(angle_t(i));
end
else
for j = 1:N
t = t+1;
X_t(t) = xO + (X_t(t-1) - xO)*cos(angle_t(i)) + (yO - Y_t(t-
1))*sin(angle_t(i));
Y_t(t) = yO - (yO - Y_t(t-1))*cos(angle_t(i)) + (X_t(t-1) -
xO)*sin(angle_t(i));
end
end
end
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')
in_x = [time', X_t'];
in_y = [time', Y_t'];
Nội suy chữ C:
%noi suy chu C

clear
clc
A = 20;
34
D = 20;
F = 20;
Tipo = 5e-3;
Tdk = 1e-3;
R = 20;
%thong so chu C
%cung AB
xO1 = 40; yO1 = 50; %tam cung tron
R1 = R;
%cung CD
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;
%ma tran bo nho
nD = 1:1:(Td/Tipo);
nT = 1:1:ceil(T_AB/Tipo);
%tinh toc do dai
Va = 0 + A*nA*Tipo;
Vc = F + 0*nC*Tipo;
Vd = F - D*nD*Tipo;
V = [Va, Vc, Vd];
%tinh to do goc
W = V/R;
angle = W*Tipo;
angle_AB = angle;
%X_AB = angle;
%Y_AB = angle;
X_AB(1) = xA;
Y_AB(1) = yA;
X_AB(i) = xO1 + (X_AB(i-1) - xO1)*cos(angle(i)) + (yO1 - Y_AB(i-
1))*sin(angle(i));
Y_AB(i) = yO1 - (yO1 - Y_AB(i-1))*cos(angle(i)) + (X_AB(i-1) -
xO1)*sin(angle(i));
end
35
% noi suy tho cung CD
Tc = (L_CD - F^2/(2*D) - F^2/(2*A))/F;
T_CD = Td + Ta + Tc;
%ma tran bo nho
nD = 1:1:(Td/Tipo);
nT = 1:1:ceil(T_CD/Tipo);
%tinh toc do dai
Va = 0 + A*nA*Tipo;
Vc = F + 0*nC*Tipo;
Vd = F - D*nD*Tipo;
V = [Va, Vc, Vd];
%tinh to do goc
W = V/R;
angle = W*Tipo;
angle_CD = angle;
%X_CD = angle;
%Y_CD = angle;
X_CD(1) = xC;
Y_CD(1) = yC;
%if (X(1) >= 20 &&
X_CD(i) = xO2 + (X_CD(i-1) - xO2)*cos(angle(i)) + (yO2 - Y_CD(i-
1))*sin(angle(i));
Y_CD(i) = yO2 - (yO2 - Y_CD(i-1))*cos(angle(i)) + (X_CD(i-1) -
xO2)*sin(angle(i));
end
%ve cung CD
%noi suy tho doan thang BC

c =(xC-xB)/L_BC;
s =(yC-yB)/L_BC;
Tc = (L_BC - F^2/(2*A)-F^2/(2*D))/F;
T_BC = Ta + Td + Tc;
%ma tran bo nho
nT = 1:1:(T_BC/Tipo);
nC = 1:1:(Tc/Tipo);
nD = 1:1:(Td/Tipo);
%tinh toc do
Va = 0 + A*nA*Tipo;
Vc = F + 0*nC*Tipo;
Vd = F - D*nD*Tipo;
V = [Va, Vc, Vd];
dL_BC = V;
for i=1:(length(V)-1)
dL_BC(i) = ((V(i)+V(i+1))/2)*Tipo;
end
X_BC = xB + cumsum(dL_BC)*c;
Y_BC = yB + cumsum(dL_BC)*s;
% ve noi suy tho chu C

figure(2)
stairs(X_AB, Y_AB, 'linewidth', 1.5)
hold on
stairs(X_BC, Y_BC, 'linewidth', 1.5)
hold on
stairs(X_CD, Y_CD, 'linewidth', 1.5)
grid on
hold on
36
axis([-10 100, -10 100])
axis equal
xlabel('X')
ylabel('Y')
title('Noi suy tho chu C')
%noi suy tinh cung AB

N = Tipo/Tdk;
angle_t = angle/N;
t=1;
X_t_AB(t) = xA;
Y_t_AB(t) = yA;
if i == 1
for j = 2:N
t = t+1;
X_t_AB(t) = xO1 + (X_t_AB(t-1) - xO1)*cos(angle_t(i)) + (yO1 -
Y_t_AB(t-1))*sin(angle_t(i));
Y_t_AB(t) = yO1 - (yO1 - Y_t_AB(t-1))*cos(angle_t(i)) + (X_t_AB(t-
1) - xO1)*sin(angle_t(i));
end
else
for j = 1:N
t = t+1;
X_t_AB(t) = xO1 + (X_t_AB(t-1) - xO1)*cos(angle_t(i)) + (yO1 -
Y_t_AB(t-1))*sin(angle_t(i));
Y_t_AB(t) = yO1 - (yO1 - Y_t_AB(t-1))*cos(angle_t(i)) + (X_t_AB(t-
end
end
end
%noi suy tinh cung CD

angle_t = angle/N;
t=1;
X_t_CD(t) = xC;
Y_t_CD(t) = yC;
if i == 1
for j = 2:N
t = t+1;
X_t_CD(t) = xO2 + (X_t_CD(t-1) - xO2)*cos(angle_t(i)) + (yO2 -
Y_t_CD(t-1))*sin(angle_t(i));
Y_t_CD(t) = yO2 - (yO2 - Y_t_CD(t-1))*cos(angle_t(i)) + (X_t_CD(t-
end
else
for j = 1:N
t = t+1;
X_t_CD(t) = xO2 + (X_t_CD(t-1) - xO2)*cos(angle_t(i)) + (yO2 -
Y_t_CD(t-1))*sin(angle_t(i));
Y_t_CD(t) = yO2 - (yO2 - Y_t_CD(t-1))*cos(angle_t(i)) + (X_t_CD(t-
end
end
end
%noi suy tinh doan BC

dL_t_BC = dL_BC(1)/N;
for i = 1:length(dL_BC)
if i == 1
for j = 1:N-1
dL_t_BC = [dL_t_BC, dL_BC(i)/N];
37
end
else
for j = 1:N
dL_t_BC = [dL_t_BC, dL_BC(i)/N];
end
end
end
X_t_BC = xB + cumsum(dL_t_BC)*c;
Y_t_BC = yB + cumsum(dL_t_BC)*s;
%ve noi suy tinh va tho hinh chu C

figure(2)
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')
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];
in_x = [time', X'];
in_y = [time', Y'];
%ve tin hieu suat ra tu simulink
%x=out_x.Data;
%y=out_y.Data;
%plot(x,y,'linewidth',1.5)
%xlabel('X')
%ylabel('Y')
%title('simulink chu C')
%dau ra X
figure(1)
grid on
axis([-1 65, -1 85])
xlabel('t(s)')
ylabel('X')
title('dau ra X')
%dau ra Y
38
figure(4)
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
A = 20;
D = 20;
F = 20;
Tipo = 5e-3;
Tdk = 1e-3;
xA = 30; yA = 50;
xB = 90; yB = 80;
L = sqrt((xB-xA)^2+(yB-yA)^2);
cos =(xB-xA)/L;
sin =(yB-yA)/L;
Ta = F/A;
T = L/F;
nT = 1:1:(ceil(T/Tipo));
k = zeros(1, ceil(Ta/Tipo));
n = length(k);
dL = L/max(nT);
for i=2:length(nT)
dL = [dL, L/max(nT)];
end
%ap dung cong thuc S(k) = S(k ?1) + ?X(k)??X(k ?n)
% ?Xo(k) = S(k)/n
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;
end
X = xA + cumsum(S)*cos;
Y = yA + cumsum(S)*sin;
%do thi noi suy tho doan thang
figure(1)
grid on
hold on
axis([-10 100, -10 100])
axis equal
xlabel('X')
ylabel('Y')
title('Noi suy tho doan thang AB')

time = Tipo*(1:(ceil(T/Tipo)+n)); % Chu ki trich mau 1ms
in_x = [time', X'];
in_y = [time', Y'];
39
%do thi bieu thi toc do

figure(2)
plot(time', S/Tipo, 'linewidth', 1.5)
grid on
hold on
axis([-1 5, -1 30])
xlabel('X')
ylabel('Y')
title('do thi bieu thi toc do')
%dau ra X
figure(3)
grid on
xlabel('t(s)')
ylabel('X')
title('dau ra X')
%dau ra Y
figure(4)
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
A = 20;
D = 20;
F = 20;
Tipo = 5e-3;
Tdk = 1e-3;
%thong so duong tron
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
L = R*2*pi;
Ta = F/A;
T = L/F;
nT = 1:1:(ceil(T/Tipo));
k = zeros(1, ceil(Ta/Tipo));
n = length(k);
%tinh to do goc
W = F/R;
angle = W*Tipo;
for i=1:length(nT)
angle = [angle, W*Tipo];
40
end
%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;
end
X = S;
Y = S;
X(1) = xA;
Y(1) = yA;
for i = 2:length(S)
X(i) = xO + (X(i-1) - xO)*cos(S(i)) + (yO - Y(i-1))*sin(S(i));
Y(i) = yO - (yO - Y(i-1))*cos(S(i)) + (X(i-1) - xO)*sin(S(i));
end
figure(1)
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)
grid on
xlabel('t(s)')
ylabel('X')
title('dau ra X')
%dau ra Y
figure(3)
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')
41
23

Nhoms7 Cuoiki

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Nhoms7 Cuoiki

Uploaded by

Copyright:

Available Formats

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

CHUYÊN NGÀNH KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA

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

Thành viên: MSSV

Với k1≠ k2, T1≠ T2 tự chọn, và T1, T2 nhỏ (< 0.5).

PHẦN II: BÀI LÀM

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

* Mô phỏng trên matlab:

+ Mục tiêu thiết kế:

 Đáp ứng càng nhanh càng tốt

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:

Kết quả thu đc

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

+ Sai lệch tĩnh ≈ 0

+ |Udk|max ≈ 9.398 < 10

+ thời gian xác lập ≈ 0.11s

+ Độ quá điều chỉnh ≈ 1.3%

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

+ Sai lệch tĩnh ≈ 0

+ |Udk|max ≈ 9.679 < 10

+ thời gian xác lập ≈ 0.06s

+ Độ quá điều chỉnh ≈ 1.2%

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

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

a) Gia tốc,giảm tốc và nội suy thô

2.1.2 Nội suy thô

LAB= √ (x B −x A )2+( y B − y A )2=√ (30−70)2+(50−80)2 =50 (mm)

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

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

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

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

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

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

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

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

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

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

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

Cách vẽ: vẽ 2 đường tron và 1 đường thẳng

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

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

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

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

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

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

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

4.1 Tổng quan chung

đầu vào. Và đầu ra được biểu diễn theo công thức:

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

4.2 Đường thẳng

Tính toán nội suy

Với T là chu kì nội suy

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

Chọn A(30,50), B(90,80), => AB = L = 67.082 (mm)

Với mỗi Tcyc = 5ms đi được một đoạn ∆ L