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

TIỂU LUẬN CUỐI KỲ

Hệ thống điều khiển máy CNC

Nhóm sinh viên thực hiện (Nhóm 5): Trần Anh Dũng - 20173773

Lý Ngọc Thuận - 20174249

Hoàng Anh Tú - 20174316

Giảng viên hướng dẫn: TS. Dương Minh Đức

Bộ môn: Tự động hóa công nghiệp
Viện: Viện Điện

HÀ NỘI, 5/2021
1
 MỤC LỤC

1. YÊU CẦU BÀI TOÁN VÀ LỰA CHỌN THAM SỐ ........................................................... 4

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

2.1. Các bước thiết kế bộ điều khiển ............................................................................................. 5

2.2. Thiết kế bộ điều khiển ............................................................................................................. 5

2.2.1 Trục x .............................................................................................................................................................. 5
2.2.2 Trục y .............................................................................................................................................................. 7

3. NỘI SUY ĐƯỜNG THẲNG ................................................................................................... 9

3.1. Cấu trúc bộ điều khiển trung tâm .......................................................................................... 9

3.2. Look Ahead .............................................................................................................................. 9

3.3. Tính toán thời gian tăng tốc, giảm tốc, vận tốc không đổi ................................................... 9

3.4. Nội suy vận tốc ....................................................................................................................... 10

3.5. Nội suy lệnh đặt cho đường thẳng (trích mẫu dữ liệu) ...................................................... 11

3.6. Nội suy tinh............................................................................................................................. 13

3.6.1. Nội suy tuyến tính .............................................................................................................................................. 14
3.6.2. Nội suy trung bình ............................................................................................................................................. 15
3.6.3. Quỹ đạo sau nội suy tinh ................................................................................................................................... 17

3.7. Thực hiện điều khiển vị trí.................................................................................................... 18

4. NỘI SUY ĐƯỜNG TRÒN .................................................................................................... 23

4.1. Tính toán thời gian tăng tốc, giảm tốc, vận tốc không đổi ................................................. 23

4.2. Nội suy vận tốc ....................................................................................................................... 23

4.3. Nội suy sai lệch đặt cho đường tròn ..................................................................................... 24

4.4. Nội suy tinh ............................................................................................................................. 26

5. THỰC HIỆN VẼ CHỮ “C” .................................................................................................. 33

6. NỘI SUY THÔ QUỸ ĐẠO VỚI TĂNG/GIẢM TỐC SAU NỘI SUY ............................. 39

6.1. Nội suy tuyến tính .................................................................................................................. 39

2
6.2. Nội suy đường tròn ................................................................................................................ 42

7. PHỤ LỤC................................................................................................................................ 47

7.1. Chương trình nội suy đường thẳng (m-file) ........................................................................ 47

7.2. Chương trình nội suy đường tròn (m-file) ........................................................................... 49

7.3. Chương trình nội suy chữ C (m-file) .................................................................................... 53

7.4. Nội suy thô đường thẳng với kiểu tăng/giảm tốc sau nội suy dạng hình thang................ 60

7.5. Nội suy thô đường tròn với kiểu tăng/giảm tốc sau nội suy dạng hình thang .................. 61

7.6. Chương trình điều khiển trên simulink ............................................................................... 62

3
 1. YÊU CẦU BÀI TOÁN VÀ LỰA CHỌN THAM SỐ

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.
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
Nhóm em lựa chọn thông số 2 hàm truyền như sau:
𝑉𝑥 2
• Trục x: =
𝑈𝑥 0.03𝑠+1
𝑉𝑦 3
• Trục y: =
𝑈𝑦 0.05𝑠+1

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

Các thông số cụ thể sẽ trình bày cụ thể trong các phần tiếp theo.

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

Với thông số 2 hàm truyền động cơ:

𝑉𝑥 2
• Trục x: =
𝑈𝑥 0.03𝑠+1
𝑉𝑦 3
• Trục y: =
𝑈𝑦 0.05𝑠+1

Chọn phương pháp thiết kế bộ điều khiển vị trí:

Vì 2 hàm truyền trên đều có khâu tích phân trong hàm truyền vì thế sẽ loại bỏ được
sai lệch tĩnh. Bởi vậy, bộ điều khiển vị trí được chọn sẽ là bộ PD với hàm truyền:
𝐾𝑐 (𝑠) = 𝐾𝑝 (1 + 𝑇𝑑 . 𝑠)
2.1. Các bước thiết kế bộ điều khiển
- B1: Đặt giá trị Setpoint là tín hiệu Step, biên độ là 1
𝑈𝑑𝑘𝑚𝑎𝑥
- B2: Đặt giá trị 𝐾𝑝 = , 𝐾𝑑 =0
𝑆𝑒𝑡𝑝𝑜𝑖𝑛𝑡

- B3: Quan sát đáp ứng đầu ra của 2 trục và quan sát 𝑈𝑑𝑘 , sau khi 𝑈𝑑𝑘 thỏa mãn giá
trị cho phép thì tinh chỉnh hệ số 𝐾𝑑 của bộ điều khiển đạt thỏa mãn yêu cầu về thời
gian xác lập và độ quá điều chỉnh

Hình 2. 1. Mô hình mô phỏng Matlab Simulink

2.2. Thiết kế bộ điều khiển

2.2.1 Trục x
Sau khi thực hiện các bước như trên, ta thu được bộ điều khiển vị trí động cơ theo
trục x với các thông số: 𝐾𝑝 = 10, 𝑇𝑑 = 0.005.

5
 Ta thu được kết quả mô phỏng:

Hình 2. 2. Đáp ứng vị trí của bộ điều khiển theo trục x

Nhận xét:
▪ Đáp ứng đầu ra của động cơ theo trục x bám sát giá trị đặt.
▪ Có sai lệch tĩnh xấp xỉ 0 từ thời điểm t = 0.45s
▪ Thời gian xác lập: 0,23s
▪ Độ quá điều chỉnh: 4.1% < 5%

6
 Hình 2. 3. Điện áp điều khiển trục x

Nhận xét: Điện áp điều khiển < 10V

→ Từ những nhận xét trên ta kết luận: Bộ điều khiển vị trí theo trục x thỏa mãn yêu
cầu đề bài
2.2.2 Trục y
Sau khi thực hiện các bước như trên, ta thu được bộ điều khiển vị trí động cơ theo
trục x với các thông số: 𝐾𝑝 = 10, 𝑇𝑑 = 0.025.
Ta thu được kết quả mô phỏng:

7
 Hình 2. 4. Đáp ứng vị trí của bộ điều khiển theo trục y

Nhận xét: ▪ Đáp ứng đầu ra của động cơ theo trục y bám sát giá trị đặt.
▪ Có sai lệch tĩnh xấp xỉ 0 từ thời điểm t = 0.45s
▪ Thời gian xác lập: 0,24s
▪ Độ quá điều chỉnh: 4% < 5%

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

Nhận xét: Điện áp điều khiển < 10V

→ Từ những nhận xét trên ta kết luận: Bộ điều khiển vị trí theo trục x thỏa mãn yêu
cầu đề bài
8
 3. NỘI SUY ĐƯỜNG THẲNG
Trong phần nội suy đường thẳng, nhóm em xin chọn nội suy từ điểm A(10,10) đến
điểm B(50,40).
3.1. Cấu trúc bộ điều khiển trung tâm
Sử dụng cấu trúc ADCBI: thực hiện tăng tốc/giảm tốc trước khi nội suy.

3.2. Look Ahead

• Quỹ đạo chuyển động được phân chia thành các khối.
• Tính toán tốc độ tại điểm bắt đầu và điểm kết thúc của một khối dựa vào các
khối được thực hiện sau nó.

Xác định dạng profile vận tốc là dạng normal block (dạng hình thang) gồm 3 quá
trình: tăng tốc, vận tốc không đổi, giảm tốc.
3.3. Tính toán thời gian tăng tốc, giảm tốc, vận tốc không đổi

Thông số profile vận tốc:

• Chiều dài: L= 50 (mm), đi từ A(10,10) đến B(50,40)

• Gia tốc tăng tốc cho phép : A=2 (mm/s2)
9
 • Gia tốc giảm tốc cho phép: D=2 (mm/s2)
• Chu kỳ nội suy: =5 (ms)
• Tốc độ ăn dao: F=5 (mm/s)

Với profile vận tốc dạng normal block ta có công thức tính như sau:
Tăng tốc Giảm tốc Vận tốc không đổi
Tính toán thời gian 𝐹 𝐹 𝐹2 𝐹2
𝑇𝐴 = = 2.5(𝑠) 𝑇𝐷 = = 2.5(𝑠) 𝐿− −
tăng tốc 𝑇𝐴 , giảm 𝐴 𝐷 𝑇𝐶 = 2𝐴 2𝐷
tốc 𝑇𝐷 , vận tốc 𝐹
= 7.5(s)
không đổi 𝑇𝐶

Nội suy 𝑉𝐴,𝑖+1 = 𝑉𝐴,𝑖 + 𝜏𝐴 𝑉𝐷,𝑖+1 = 𝑉𝐷,𝑖 − 𝜏𝐷 𝑉𝐶 = 𝐹

3.4. Nội suy vận tốc

Sau quá trình tính toán nội suy ta thu được đồ thị vận tốc như sau:

Tăng tốc Giảm tốc

10
3.5. Nội suy lệnh đặt cho đường thẳng (trích mẫu dữ liệu)
Sử dụng phương pháp trích mẫu dữ liệu như sau:

Ta có: 𝐿 = √(𝑥𝑒 − 𝑥𝑠 )2 + (𝑦𝑒 − 𝑦𝑠 )2 = √(𝑥𝐵 − 𝑥𝐴 )2 + (𝑦𝐵 − 𝑦𝐴 )2

= √(50 − 10)2 + (40 − 10)2 =50 (mm)

❖ Tăng tốc
Từ công thức nội suy tăng tốc: 𝑉𝐴,𝑖+1 = 𝑉𝐴,𝑖 + 𝜏𝐴, ta thu được bảng kết quả:
i 𝑽𝑨,𝒊 ∆𝑳𝒊 = 𝑽 ∗ 𝑻𝒊𝒑𝒐 𝒙𝒆 − 𝒙𝒔 X 𝒚𝒆 − 𝒚𝒔 Y
∆𝒙 = ∆𝑳 ∆𝒚 = ∆𝑳
𝑳 𝑳
(mm/s) (× 𝟏𝟎−𝟐 mm) (mm) (mm)
(× 𝟏𝟎−𝟐 mm) (× 𝟏𝟎−𝟐 mm)
0 0 0 0 10 0 10

1 0.01 0.005 0.004 10 0.003 10

2 0.02 0.01 0.008 10.0001 0.006 10.0001

-- -- -- -- -- -- --

499 4.99 2.49 1.99 14.99 1.49 13.7425

500 5 2.5 2 15.01 1.5 13.7575

11
❖ Vận tốc không đổi
Do vận tốc không đổi, ta thu được bảng kết quả:
i 𝑽𝑪,𝒊 ∆𝑳𝒊 = 𝑽 ∗ 𝑻𝒊𝒑𝒐 𝒙𝒆 − 𝒙𝒔 X 𝒚𝒆 − 𝒚𝒔 Y
∆𝒙 = ∆𝑳 ∆𝒚 = ∆𝑳
𝑳 𝑳
(mm/s) (× 𝟏𝟎−𝟐 mm) (mm) (mm)
(× 𝟏𝟎−𝟐 mm) (× 𝟏𝟎−𝟐 mm)
0 5 2.5 2 15.01 1.5 13.7575

1 5 2.5 2 15.03 1.5 13.7725

2 5 2.5 2 15.05 1.5 13.7875

-- -- -- -- -- -- --

1499 5 2.5 2 44.99 1.5 36.2425

1500 5 2.5 2 45.01 1.5 36.2575

❖ Giảm tốc
Từ công thức nội suy giảm tốc: 𝑉𝐷,𝑖+1 = 𝑉𝐷,𝑖 − 𝜏𝐷, ta thu được bảng kết quả:
i 𝑽𝑫,𝒊 ∆𝑳𝒊 = 𝑽 ∗ 𝑻𝒊𝒑𝒐 𝒙𝒆 − 𝒙𝒔 X 𝒚𝒆 − 𝒚𝒔 Y
∆𝒙 = ∆𝑳 ∆𝒚 = ∆𝑳
𝑳 𝑳
(mm/s) (× 𝟏𝟎−𝟐 mm) (mm) (mm)
(× 𝟏𝟎−𝟐 mm) (× 𝟏𝟎−𝟐 mm)
0 5 2.5 2 45.01 1.5 36.2575

1 4.99 2.495 1.996 45.03 1.497 36.2725

2 4.98 2.49 1.99 45.0499 1.49 36.2874

-- -- -- -- -- -- --

499 0.01 0.005 0.004 50 0.003 40

500 0 0 0 50 0 40

12
 Sau khi nội suy thu được quỹ đạo đi từ: A(10,10) đến B(50,40) như sau:

Tăng tốc

Vận tốc không đổi Giảm tốc

3.6. Nội suy tinh

𝑇
Do chu kỳ trích mẫu nội suy lớn hơn chu kỳ trích mẫu bộ điều khiển vị trí ( 𝑛𝑠 = 5)
𝑇đ𝑘
nên 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. Cụ thể là chia nhỏ khoảng dịch
chuyển ∆𝐿.

13
3.6.1. Nội suy tuyến tính

Áp dụng công thức:

𝑝(𝑖)
𝑎 (𝑗 ) = , 𝑖 ≤ 𝑗 < 𝑖 + 𝑡𝑖𝑝𝑜
𝑁
Với 𝑎(𝑗) là khoảng dịch chuyển thứ j thu được khi nội suy tuyến tính, 𝑝(𝑖) là khoảng
𝑇
dịch chuyển tổng ∆𝐿 thứ i, 𝑡𝑖𝑝𝑜 là thời gian lặp lại của phép nội suy thô, 𝑁 = 𝑛𝑠 = 5
𝑇đ𝑘
Từ đó ta thu được dạng đồ thị khoảng dịch chuyển của phương pháp tuyến tính (đồ
thị dưới) so với đồ thị nội suy thô (đồ thị trên) như sau:
• Tăng tốc:

• Vận tốc không đổi:

14
 3.6.2. Nội suy trung bình
Từ đồ thị khoảng dịch chuyển thu được từ phương pháp tuyến tính, áp dụng công thức:
𝑁 𝑁
−1
∑2 𝑁 𝑎 (𝑗 − 𝑘) ∑2 𝑁 𝑎 (𝑗 − 𝑘)
𝑘=− +1
2
𝑘=−
2 𝑏(𝑗) + 𝑏′(𝑗)
𝑏 (𝑗 ) = , 𝑏′(𝑗) = , 𝑏′′(𝑗) =
𝑁 𝑁 2
𝑇
Với 𝑎 𝑗 − 𝑘 là giá trị tại thời điểm (𝑗 − 𝑘) của phương pháp tuyến tính, 𝑁 = 𝑛𝑠 = 5
( )
𝑇đ𝑘
−2
Ta thu được bảng giá trị một số chu kỳ (đơn vị 𝑎(𝑗 ± 𝑘 ), 𝑏0,1,2 (𝑗) : × 10 (𝑚𝑚)
a(j + a( j + a( j + a( j + a( j +
n j a( j) a( j -1) a( j -2) b( j) b1(j) b2(j)
5) 4) 3) 2) 1)
8.00E-
1 04 0 0 0 0 0 0 0 0 0 0
8.00E- 8.00E-
2 04 04 0 0 0 0 0 0 0 0.00016 0.00008
8.00E- 8.00E- 8.00E-
1
3 04 04 04 0 0 0 0 0 0.00016 0.00032 0.00024
8.00E- 8.00E- 8.00E- 8.00E-
4 04 04 04 04 0 0 0 0 0.00032 0.00048 0.0004
8.00E- 8.00E- 8.00E- 8.00E- 8.00E-
5 04 04 04 04 04 0 0 0 0.00048 0.00064 0.00056
…
2491 0.3992 0.3984 0.3984 0.3984 0.3984 0.3984 0.3976 0.3976 0.39824 0.3984 0.39832
2492 0.3992 0.3992 0.3984 0.3984 0.3984 0.3984 0.3984 0.3976 0.3984 0.39856 0.39848
499
2493 0.3992 0.3992 0.3992 0.3984 0.3984 0.3984 0.3984 0.3984 0.39856 0.39872 0.39864
2494 0.3992 0.3992 0.3992 0.3992 0.3984 0.3984 0.3984 0.3984 0.39872 0.39888 0.3988
15
 2495 0.3992 0.3992 0.3992 0.3992 0.3992 0.3984 0.3984 0.3984 0.39888 0.39904 0.39896
2496 0.4 0.3992 0.3992 0.3992 0.3992 0.3992 0.3984 0.3984 0.39904 0.3992 0.39912
2497 0.4 0.4 0.3992 0.3992 0.3992 0.3992 0.3992 0.3984 0.3992 0.39936 0.39928
500 2498 0.4 0.4 0.4 0.3992 0.3992 0.3992 0.3992 0.3992 0.39936 0.39952 0.39944
2499 0.4 0.4 0.4 0.4 0.3992 0.3992 0.3992 0.3992 0.39952 0.39968 0.3996
2500 0.4 0.4 0.4 0.4 0.4 0.3992 0.3992 0.3992 0.39968 0.39984 0.39976
…
11991 0.08 0.0808 0.0808 0.0808 0.0808 0.0808 0.0816 0.0816 0.08096 0.0808 0.08088
11992 0.08 0.08 0.0808 0.0808 0.0808 0.0808 0.0808 0.0816 0.0808 0.08064 0.08072
2399 11993 0.08 0.08 0.08 0.0808 0.0808 0.0808 0.0808 0.0808 0.08064 0.08048 0.08056
11994 0.08 0.08 0.08 0.08 0.0808 0.0808 0.0808 0.0808 0.08048 0.08032 0.0804
11995 0.08 0.08 0.08 0.08 0.08 0.0808 0.0808 0.0808 0.08032 0.08016 0.08024
11996 0.0792 0.08 0.08 0.08 0.08 0.08 0.0808 0.0808 0.08016 0.08 0.08008
11997 0.0792 0.0792 0.08 0.08 0.08 0.08 0.08 0.0808 0.08 0.07984 0.07992
2400 11998 0.0792 0.0792 0.0792 0.08 0.08 0.08 0.08 0.08 0.07984 0.07968 0.07976
11999 0.0792 0.0792 0.0792 0.0792 0.08 0.08 0.08 0.08 0.07968 0.07952 0.0796
12000 0.0792 0.0792 0.0792 0.0792 0.0792 0.08 0.08 0.08 0.07952 0.07936 0.07944
…

Từ đó thu được kết quả đồ thị khoảng dịch chuyển của phương pháp trung bình (đồ thị
dưới) so với đồ thị phương pháp tuyến tính (đồ thị trên) như sau:

16
3.6.3. Quỹ đạo sau nội suy tinh
Sau khi sử dụng 2 phương pháp trên, ta áp dụng khoảng dịch chuyển đã nội suy tinh
vào nội suy sai lệch đường thẳng. Quỹ đạo thu được như sau:

So sánh với quỹ đạo nội suy thô:

17
3.7. Thực hiện điều khiển vị trí
Sau khi thu được quỹ đạo nội suy cuối cùng, ta sử dụng quỹ đạo đó là đầu vào (setpoint)
cho bộ điều khiển vị trí có mạch vòng điều khiển như sau:

Tuy nhiên, do yêu cầu đề bài, ta sẽ bỏ qua mạch vòng dòng điện và mạch vòng tốc
độ, bộ điều khiển vị trí là bộ PD.
Mạch vòng thiết kế trên simulink:

Ta thu được các kết quả:

Đồ thị quỹ đạo đầu ra:

18
Đồ thị so sánh quỹ đạo vào/ra theo thời gian:

Đồ thị so sánh quỹ đạo vào/ra vận tốc theo thời gian:

19
Đồ thị tín hiệu điều khiển (điện áp):

20
Đồ thị sai số khi điều khiển:

Biểu diễn 3 đường theo yêu cầu bài toán:

21
22
 4. NỘI SUY ĐƯỜNG TRÒN
Trong phần nội suy đường tròn, nhóm em xin chọn nội suy từ điểm A(10,10) đến
𝟏𝟓𝟓
điểm B(50,40) với tâm đường tròn là X(𝟏𝟎, ) và tổng cung tròn di chuyển là
𝟑
𝟕
𝝋 = 𝐚𝐫𝐜𝐜𝐨𝐬 ( )  𝟏. 𝟐𝟖𝟕 𝒓𝒂𝒅
𝟐𝟓
4.1. Tính toán thời gian tăng tốc, giảm tốc, vận tốc không đổi

Thông số profile vận tốc:

𝟕
• Chiều dài di chuyển: 𝜑 = 𝐚𝐫𝐜𝐜𝐨𝐬 ( )  𝟏. 𝟐𝟖𝟕 𝒓𝒂𝒅
𝟐𝟓
1250
• Bán kính đường tròn: R= ≅ 41.67 𝑚𝑚
30
• Gia tốc tăng tốc cho phép : A=2 (mm/s2)
• Gia tốc giảm tốc cho phép: D=2 (mm/s2)
• Chu kỳ nội suy: =5 (ms)
• Tốc độ ăn dao: F=5 (mm/s)

Với profile vận tốc dạng normal block ta có công thức tính như sau:
Tăng tốc Giảm tốc
Nội suy (mm/s) 𝑉𝐴,𝑖+1 = 𝑉𝐴,𝑖 + 𝜏𝐴 𝑉𝐷,𝑖+1 = 𝑉𝐷,𝑖 − 𝜏𝐷
Tốc độ góc nội suy 𝑉𝐴,𝑖 𝑉𝐷,𝑖
𝜔𝐴,𝑖 = 𝜔𝐷,𝑖 =
(rad/s) 𝑅 𝑅
Nội suy góc 𝛼 (𝛼 là góc 𝛼𝐴,𝑖 = 𝜔𝐴,𝑖 ×  𝛼𝐷,𝑖 = 𝜔𝐷,𝑖 × 
giữa 2 điểm cần nội
suy trên cung tròn)
(rad)

Từ bảng trên, ta có thể tính tổng góc 𝜶 trong quá trình tăng tốc và giảm tốc, từ đó
có thể tính toán được chiều dài và thời gian khi tốc độ không đổi:
𝑭 𝟓𝟎
• Tốc độ góc khi vận tốc không đổi: 𝝎𝑪 = = rad/s
𝑹 𝑹
• Chiều dài cung tròn di chuyển: 𝜶𝑪 = 𝝋 − ( 𝜶𝑨 + ∑ 𝜶𝑫 )
∑
𝜶
• Thời gian di chuyển: 𝑻𝑪 = 𝑪
𝝎𝑪
• Nội suy góc 𝜶: 𝜶𝑪,𝒊 = 𝝎𝑪,𝒊 × 

4.2. Nội suy vận tốc

Sau quá trình tính toán nội suy ta thu được đồ thị vận tốc như sau:

23
4.3. Nội suy sai lệch đặt cho đường tròn
Ta có thể nội suy tọa độ X,Y từ hình dưới đây:

Đặt A=cos(𝜶), B=sin(𝜶), từ hình trên ta có thể tính toán:

𝑿𝒊+𝟏 − 𝑿𝟎 = R. 𝒔𝒊𝒏𝜽𝒊+𝟏 = 𝑹.sin(𝜽𝒊 + 𝜶) = 𝑹𝒔𝒊𝒏𝜽𝒊 . 𝒄𝒐𝒔𝜶 + 𝑹𝒄𝒐𝒔𝜽𝒊 . 𝒔𝒊𝒏𝜶
24
 = (𝑿𝒊 − 𝑿𝟎 ). 𝒄𝒐𝒔𝜶 + (𝒀𝒕 − 𝒀𝒊 ). 𝒔𝒊𝒏𝜶
= (𝑿 𝒊 − 𝑿 𝟎 ). 𝑨 + (𝒀 𝒕 − 𝒀 𝒊 ). 𝑩
→ 𝑿𝒊+𝟏 ( ) ( )
= 𝑿𝒊 − 𝑿𝟎 . 𝑨 + 𝒀 𝒕 − 𝒀 𝒊 . 𝑩 + 𝑿𝟎

𝒀𝒕 − 𝒀𝒊+𝟏 = 𝑹. 𝒄𝒐𝒔𝜽𝒊+𝟏 = 𝑹. 𝐜𝐨𝐬(𝜽𝒊 + 𝜶) = 𝑹𝒄𝒐𝒔𝜽𝒊 . 𝒄𝒐𝒔𝜶 − 𝑹𝒔𝒊𝒏𝜽𝒊 . 𝒔𝒊𝒏𝜶

= (𝒀𝒕 − 𝒀𝒊 ). 𝒄𝒐𝒔𝜶 − (𝑿𝒊 − 𝑿𝒕 ). 𝒔𝒊𝒏𝜶
= (𝒀 𝒕 − 𝒀 𝒊 ). 𝑨 − (𝑿 𝒊 − 𝑿 𝒕 ). 𝑩
→ 𝒀𝒊+𝟏 = 𝒀𝒕 − (𝒀𝒕 − 𝒀𝒊 ). 𝑨 + (𝑿𝒊 − 𝑿𝒕 ). 𝑩
Trong đó 𝑿𝒕 , 𝒀𝒕 lần lượt là hoành độ và tung độ của tâm đường tròn X

❖ Tăng tốc
i 𝝎𝑨,𝒊 (rad/s) 𝜶𝒊 (rad) X Y

0 0 0 10 10

1 0.00024 0.0000012 10 10
2 0.00048 0.0000024 10.0001 10

-- -- -- -- --
499 0.1198 0.0005988 16.1896 10.4623

500 0.12 0.0006 16.2142 10.466

❖ Giảm tốc

i 𝝎𝑨,𝒊 (rad/s) 𝜶𝒊 (rad) X Y

0 0.12 0.0006 47.7916 34.1194

1 0.1198 0.0005988 47.8021 34.1421

2 0.1195 0.0005976 47.8126 34.1647

-- -- -- -- --

499 0.00024 0.0000012 49.9930 39.9759

500 0 0 50 39.9899

Sau khi sử dụng MATLAB để thực hiện nội suy đường tròn, ta thu được kết quả
sau đây:

25
 Tăng tốc

Vận tốc không đổi Giảm tốc

4.4. Nội suy tinh

Tương tự như đã nội suy tinh với đường thẳng, tiến hành nội suy tinh quỹ đạo hình
tròn theo hai phương pháp, thu được bảng kết quả một số chu kỳ như sau (đơn vị
𝑎(𝑗 ± 𝑘 ), 𝑏0,1,2 (𝑗) : × 10−2 (𝑚𝑚)
n alpha j a( j + 5) a( j + 4) a( j + 3) a( j + 2) a( j + 1) a( j) a( j -1) a( j -2) b( j) b1(j) b2(j)

1 2.40E-05 0 0 0 0 0 0 0 0 0 0
2 2.40E-05 2.40E-05 0 0 0 0 0 0 0 4.8E-06 2.4E-06
1 0 3 2.40E-05 2.40E-05 2.40E-05 0 0 0 0 0 4.8E-06 9.6E-06 7.2E-06
4 2.40E-05 2.40E-05 2.40E-05 2.40E-05 0 0 0 0 9.6E-06 1.44E-05 0.000012
5 2.40E-05 2.40E-05 2.40E-05 2.40E-05 2.40E-05 0 0 0 1.44E-05 1.92E-05 1.68E-05
…

26
 2491 0.011976 0.012952 0.012952 0.012952 0.012952 0.012952 0.011928 0.011928 0.012747 0.012952 0.01285
2492 0.011976 0.011976 0.012952 0.012952 0.012952 0.012952 0.012952 0.011928 0.012952 0.012757 0.012854
5.98E-
499 2493 0.011976 0.011976 0.011976 0.012952 0.012952 0.012952 0.012952 0.012952 0.012757 0.012562 0.012659
02
2494 0.011976 0.011976 0.011976 0.011976 0.012952 0.012952 0.012952 0.012952 0.012562 0.012366 0.012464
2495 0.011976 0.011976 0.011976 0.011976 0.011976 0.012952 0.012952 0.012952 0.012366 0.012171 0.012269
2496 0.012 0.011976 0.011976 0.011976 0.01976 0.011976 0.012952 0.012952 0.013728 0.013533 0.01363
2497 0.012 0.012 0.011976 0.011976 0.01976 0.011976 0.011976 0.012952 0.013533 0.013538 0.013535
500 0.05988 2498 0.012 0.012 0.012 0.011976 0.01976 0.011976 0.011976 0.011976 0.013538 0.013542 0.01354
2499 0.012 0.012 0.012 0.012 0.01976 0.011976 0.011976 0.011976 0.013542 0.013547 0.013545
2500 0.012 0.012 0.012 0.012 0.012 0.011976 0.011976 0.011976 0.01199 0.011995 0.011993
…

10726 0.011952 0.011976 0.011976 0.011976 0.011976 0.011976 0.012 0.012 0.011986 0.011981 0.011983
10727 0.011952 0.011952 0.011976 0.011976 0.011976 0.011976 0.011976 0.012 0.011981 0.011971 0.011976
2145 0.05988 10728 0.011952 0.011952 0.011952 0.011976 0.011976 0.011976 0.011976 0.011976 0.011971 0.011966 0.011969
10729 0.011952 0.011952 0.011952 0.011952 0.011976 0.011976 0.011976 0.011976 0.011966 0.011962 0.011964
10730 0.011952 0.011952 0.011952 0.011952 0.011952 0.011976 0.011976 0.011976 0.011966 0.011962 0.011964
10731 0.011928 0.011952 0.011952 0.011952 0.011952 0.011952 0.011976 0.011976 0.011957 0.011952 0.011954
10732 0.011928 0.011928 0.011952 0.011952 0.011952 0.011952 0.011952 0.011976 0.011952 0.011947 0.01195
2146 0.5976 10733 0.011928 0.011928 0.011928 0.011952 0.011952 0.011952 0.011952 0.011952 0.011947 0.011942 0.011945
10734 0.011928 0.011928 0.011928 0.011928 0.011952 0.011952 0.011952 0.011952 0.011942 0.011938 0.01194
10735 0.011928 0.011928 0.011928 0.011928 0.011928 0.011952 0.011952 0.011952 0.011938 0.011933 0.011935
…

Đồ thị so sánh độ dịch chuyển quỹ đạo nội suy thô, nội suy tinh tuyến tính và nội suy
tinh trung bình:

27
So sánh với giá trị nội suy thô:

Đưa quỹ đạo sau khi nội suy vào điều khiển thu được

28
Đồ thị quỹ đạo đầu ra:

Đồ thị so sánh quỹ đạo vào/ra theo thời gian:

29
Đồ thị so sánh vận tốc vào/ra theo thời gian:

30
Đồ thị tín hiệu điều khiển:

Đồ thị sai số khi điều khiển:

31
Biểu diễn 3 đường theo yêu cầu bài toán:

32
 5. THỰC HIỆN VẼ CHỮ “C”

Từ những kết quả thu được ở trên, nhóm em sẽ thực hiện vẽ chữ “C” qua 3 bước:
• B1: Thực hiện nội suy đường tròn từ điểm A(20,20) đến điểm B(-20,20) với đường
tròn bán kính R=20mm và góc nội suy 𝝋 = 𝝅
• B2: Thực hiện nội suy đường thằng từ điểm B(-20,20) đến điểm C(-20,-20) với độ
dài di chuyển L=40mm.
• B3: Thực hiện nội suy đường tròn từ điểm C(-20,-20) đến điểm D(20,-20) với đường
tròn bán kính R=20mm và góc nội suy 𝝋 = 𝝅
Cả 3 bước nội suy trên đều thực hiện với chu kì nội suy 𝝉 = 𝟓𝒎𝒔.
Sau khi thực hiện 3 bước nội suy trên, ta thu được kết quả:

Profile vận tốc chia thành 3 quãng lần lượt theo 3 bước ở trên:

33
 Tiến hành nội suy tinh theo phương pháp tuyến tính và trung bình tương tự như trên
ta thu được quỹ đạo cuối cùng:

34
Đưa quỹ đạo sau nội suy vào điều khiển thu được:
Đồ thị quỹ đạo đầu ra:

Đồ thị so sánh quỹ đạo vào/ra theo thời gian:

35
Đồ thị so sánh vận tốc vào/ra theo thời gian:

36
 Đồ thị tín hiệu điều khiển:

Đồ thị sai số khi điều khiển:

37
Biểu diễn 3 đường theo yêu cầu bài toán:

38
 6. NỘI SUY THÔ QUỸ ĐẠO VỚI TĂNG/GIẢM TỐC SAU NỘI SUY
Trong phần này, nhóm em xin chọn nội suy từ điểm A(10,10) đến điểm B(50,40).
6.1. Nội suy tuyến tính
Các thông số lựa chọn:
• Chiều dài: L= 50 (mm), đi từ A(10,10) đến B(50,40)
• Gia tốc tăng tốc cho phép : A=2 (mm/s2)
• Gia tốc giảm tốc cho phép: D=2 (mm/s2)
• Chu kỳ nội suy: 𝜏=5 (ms)
• Tốc độ ăn dao: F=5 (mm/s)
Tiến hành nội suy thô tuyến tính theo lưu đồ sau:

Tính toán:
• 𝑑𝑋 = 𝑥𝑒 − 𝑥𝑠 = 40(mm) %tong dich chuyen truc X
• 𝑑𝑌 = 𝑦𝑒 − 𝑦𝑠 = 30(mm) %tong dich chuyen truc Y
𝐿
• 𝑇 = = 10 (s) %tong thoi gian dich chuyen
𝐹
𝑇
• 𝑁 = 𝑖𝑛𝑡[ ] = 2000 %tong xung dich chuyen
𝜏
𝑑𝑋
• 𝑃𝑢𝑙𝑠𝑒𝑋 = = 0.02(mm) %khoang dich chuyen truc X
𝑁
𝑑𝑌
• 𝑃𝑢𝑙𝑠𝑒𝑌 = = 0.015(mm) %khoang dich chuyen truc Y
𝑁

39
Input pulse train: Impulse response:

Output pulse train:

Áp dụng phương trình:

Cụ thể với khoảng dịch chuyển trục X và trục Y:

1
∆𝑋0 (𝑘) = (∆𝑋 (𝑘 ) − ∆𝑋 (𝑘 − 𝑁𝐴 )) + ∆𝑋0 (𝑘 − 1)
𝑁𝐴
1
∆𝑌0 (𝑘) = (∆𝑌(𝑘 ) − ∆𝑌(𝑘 − 𝑁𝐴 )) + ∆𝑌0 (𝑘 − 1)
𝑁𝐴
Tính toán một số chu kỳ:
Sampling Input pulse: Output of Output pulse: Input pulse: Output of Output pulse:
time: K ∆𝑋(𝑘) buffer ∆𝑋0 (𝑘) ∆𝑌(𝑘) buffer ∆𝑌0 (𝑘)
register: register:
∆𝑋(𝑘 − 𝑁𝐴 ) ∆𝑌(𝑘 − 𝑁𝐴 )
1 0.02 0 4e-05 0.015 0 3e-05
2 0.02 0 8e-05 0.015 0 6e-05
3 0.02 0 1.2e-04 0.015 0 9e-05
4 0.02 0 1.6e-04 0.015 0 1.2e-04
5 0.02 0 2.0e-04 0.015 0 1.5e-04
6 0.02 0 2.4e-04 0.015 0 1.8e-04
7 0.02 0 2.8e-04 0.015 0 2.1e-04
…
497 0.02 0 0.01988 0.015 0 0.01491
498 0.02 0 0.01992 0.015 0 0.01494
499 0.02 0 0.01996 0.015 0 0.01497
500 0.02 0 0.02 0.015 0 0.015
501 0.02 0.02 0.02 0.015 0.015 0.015
502 0.02 0.02 0.02 0.015 0.015 0.015
503 0.02 0.02 0.02 0.015 0.015 0.015
40
 …
1998 0.02 0.02 0.02 0.015 0.015 0.015
1999 0.02 0.02 0.02 0.015 0.015 0.015
2000 0.02 0.02 0.02 0.015 0.015 0.015
2001 0 0.02 0.01996 0 0.015 0.01497
2002 0 0.02 0.01992 0 0.015 0.01494
2003 0 0.02 0.01988 0 0.015 0.01491
…
2499 0 0.02 4e-05 0 0.015 3e-05
2500 0 0.02 0 0 0.015 0
Kết quả:

Zoom và biểu diễn dạng bar graph:

41
Quỹ đạo thu được cuối cùng:

6.2. Nội suy đường tròn

Các thông số lựa chọn:
𝟕
• Tổng cung dịch chuyển: 𝛿 = 𝐚𝐫𝐜𝐜𝐨𝐬 ( )  𝟏. 𝟐𝟖𝟕 𝒓𝒂𝒅
𝟐𝟓
1250
• Bán kính đường tròn: R= ≅ 41.67 𝑚𝑚
30
𝟏𝟓𝟓
• Tâm đường tròn là X(𝟏𝟎, )
𝟑

42
 • Gia tốc tăng tốc cho phép : A=2 (mm/s2)
• Gia tốc giảm tốc cho phép: D=2 (mm/s2)
• Chu kỳ nội suy: =5 (ms)
• Tốc độ ăn dao: F=5 (mm/s)
𝑑𝑋
Từ phần tuyến tính ta có: 𝑃𝑢𝑙𝑠𝑒𝑋 = = 0.02 = ∆𝐿𝑐𝑜𝑠𝜃 = 𝑉𝑐𝑜𝑠𝜃
𝑁
0.02 0.02
→𝑉= = = 5 (mm/s)
𝑐𝑜𝑠𝜃 5.10−3 ×0.8
𝑉
→𝜔= = 0.12 (𝑟𝑎𝑑/𝑠)
𝑅

→ ∆𝛼 = 𝜔 = 0.0006 (𝑟𝑎𝑑)
𝛿
→ 𝑁 = 𝑖𝑛𝑡 [ ] = 2144
∆𝛼

Tương tự như phần tuyến tính ta thu được:

Input pulse train: Impulse response:

Output pulse train:

Áp dụng công thức tương tự như đã làm với phần tuyến tính ta thu được bảng tính toán
một số chu kỳ:
Sampling Input pulse: ∆𝛼(𝑘) Output of buffer register: Output pulse: ∆𝛼0 (𝑘)
time: K ∆𝛼(𝑘 − 𝑁𝐴 )
1 6.0028e-04 0 1.2006e-06
2 6.0028e-04 0 2.4011e-06
3 6.0028e-04 0 3.6017e-06
4 6.0028e-04 0 4.8022e-06
5 6.0028e-04 0 6.0028e-06
6 6.0028e-04 0 7.2034e-06
7 6.0028e-04 0 8.4039e-06
…

43
 497 6.0028e-04 0 5.9668e-04
498 6.0028e-04 0 5.9788e-04
499 6.0028e-04 0 5.9908e-04
500 6.0028e-04 0 6.0028e-04
501 6.0028e-04 6.0028e-04 6.0028e-04
502 6.0028e-04 6.0028e-04 6.0028e-04
503 6.0028e-04 6.0028e-04 6.0028e-04
…
2642 6.0028e-04 6.0028e-04 6.0028e-04
2643 6.0028e-04 6.0028e-04 6.0028e-04
2644 6.0028e-04 6.0028e-04 6.0028e-04
2645 0 6.0028e-04 5.9908e-04
2646 0 6.0028e-04 5.9788e-04
2647 0 6.0028e-04 5.9668e-04
…
3143 0 6.0028e-04 1.2006e-06
3144 0 6.0028e-04 0

Kết quả:

Zoom và biểu diễn dưới dạng bar graph:

44
Quỹ đạo thu được cuối cùng:

So sánh với quỹ đạo đặt:

45
46
 7. PHỤ LỤC

7.1. Chương trình nội suy đường thẳng (m-file)

%%
F=5; %toc do an dao 50mm/s^2
A=2; %gia toc tang toc cho phep
D=2; %gia toc giam toc cho phep
L=50; %chieu dai
T=5e-3; %chu ky noi suy
Tdk=1e-3; %chu ky dieu khien
Nt=T/Tdk;%ty so
%%tinh toan thoi gian tang toc giam toc
Ta=F/A;%time acc
Td=F/D;%time dec
Tc=(L-Sa-Sd)/F;%time cons
Sa=(F*F)/(2*A);
Sd=(F*F)/(2*D);
%%
%%Noi suy tho van toc
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);
tas=0:Tdk:Ta;
tcs=Ta:Tdk:(Ta+Tc);
tds=Tc+Ta:Tdk:(Ta+Tc+Td);
%tinh toan khoang tang toc ns tho
deltaL1=Va*T;
deltaX1=deltaL1*0.8;
deltaY1=deltaL1*0.6;
syms i
X1=zeros(1,Na+1);
Y1=zeros(1,Na+1);
for i=2:Na+1
X1(1,1)=10;
X1(1,i)=X1(1,i-1)+deltaX1(1,i);
Y1(1,1)=10;
Y1(1,i)=Y1(1,i-1)+deltaY1(1,i);
end
%%
%tinh toan khoang cons ns tho
deltaL2=Vc*T;
deltaX2=deltaL2*0.8;
deltaY2=deltaL2*0.6;
syms j
X2=zeros(1,Nc+1);
Y2=zeros(1,Nc+1);
for j=2:Nc+1
X2(1,1)=X1(1,Na+1);
X2(1,j)=X2(1,j-1)+deltaX2(1,j);
Y2(1,1)=Y1(1,Na+1);
Y2(1,j)=Y2(1,j-1)+deltaY2(1,j);
end
%tinh toan khoang giam toc ns tho
deltaL3=Vd*T;
deltaX3=deltaL3*0.8;
deltaY3=deltaL3*0.6;

47
syms k
X3=zeros(1,Nd);
Y3=zeros(1,Nd);
for k=2:Nd+1
X3(1,1)=X2(1,Nc+1);
X3(1,k)=X3(1,k-1)+deltaX3(1,k);
Y3(1,1)=Y2(1,Nc+1);
Y3(1,k)=Y3(1,k-1)+deltaY3(1,k);
end
%tinh toan khoang tang toc ns tinh
syms i j%ns tuyen tinh
aX1=zeros(1,Na*Nt+1);aX1(1,Na*Nt+1)=deltaX1(1,Na+1)/Nt;
aY1=zeros(1,Na*Nt+1);aY1(1,Na*Nt+1)=deltaY1(1,Na+1)/Nt;
for i=1:Na
for j=(Nt*i-Nt+1):(Nt*i)
aX1(1,j)=deltaX1(1,i)/Nt;
aY1(1,j)=deltaY1(1,i)/Nt;
end
end
syms k %ns trung binh
bX1=zeros(1,Na*Nt+1);bY1=zeros(1,Na*Nt+1);
b1X1=zeros(1,Na*Nt+1);b1Y1=zeros(1,Na*Nt+1);
b2X1=zeros(1,Na*Nt+1);b2Y1=zeros(1,Na*Nt+1);
for k=2:Na*Nt-3
bX1(1,k)=(aX1(1,k+3)+aX1(1,k+2)+aX1(1,k+1)+aX1(1,k)+aX1(1,k-1))/Nt;
b1X1(1,k)=(aX1(1,k+4)+aX1(1,k+3)+aX1(1,k+2)+aX1(1,k+1)+aX1(1,k))/Nt;
b2X1(1,k)=(bX1(1,k)+b1X1(1,k))/2;
bY1(1,k)=(aY1(1,k+3)+aY1(1,k+2)+aY1(1,k+1)+aY1(1,k)+aY1(1,k-1))/Nt;
b1Y1(1,k)=(aY1(1,k+4)+aY1(1,k+3)+aY1(1,k+2)+aY1(1,k+1)+aY1(1,k))/Nt;
b2Y1(1,k)=(bY1(1,k)+b1Y1(1,k))/2;
end
for k=Na*Nt-2:Na*Nt+1
bX1(1,k)=((k-Na*Nt+3)*aX1(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*aX1(1,k))/Nt;
b1X1(1,k)=((k-Na*Nt+4)*aX1(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*aX1(1,k))/Nt;
b2X1(1,k)=(bX1(1,k)+b1X1(1,k))/2;
bY1(1,k)=((k-Na*Nt+3)*aY1(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*aY1(1,k))/Nt;
b1Y1(1,k)=((k-Na*Nt+4)*aY1(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*aY1(1,k))/Nt;
b2Y1(1,k)=(bY1(1,k)+b1Y1(1,k))/2;
end
syms l
X11=zeros(1,Na*Nt+1);X12=zeros(1,Na*Nt+1);
Y11=zeros(1,Na*Nt+1);Y12=zeros(1,Na*Nt+1);
for l=2:Na*Nt+1
X11(1,1)=10;X12(1,1)=10;
X11(1,l)=X11(1,l-1)+aX1(1,l);X12(1,l)=X12(1,l-1)+b2X1(1,l);
Y11(1,1)=10;Y12(1,1)=10;
Y11(1,l)=Y11(1,l-1)+aY1(1,l);Y12(1,l)=Y12(1,l-1)+b2Y1(1,l);
end
%tinh toan khoang cons ns tinh
syms i j k
aX2=zeros(1,Nc*Nt+1);aX2(1,Nc*Nt+1)=deltaX2(1,Nc+1)/Nt;
aY2=zeros(1,Nc*Nt+1);aY2(1,Nc*Nt+1)=deltaY2(1,Nc+1)/Nt;
X21=zeros(1,Nc*Nt+1);Y21=zeros(1,Nc*Nt+1);
for i=1:Nc
for j=(Nt*i-Nt+1):(Nt*i)
aX2(1,j)=deltaX2(1,i)/Nt;
aY2(1,j)=deltaY2(1,i)/Nt;
end
end
for k=2:Nc*Nt+1
X21(1,1)=X11(1,Na*Nt+1);
X21(1,k)=X21(1,k-1)+aX2(1,k);
Y21(1,1)=Y11(1,Na*Nt+1);
Y21(1,k)=Y21(1,k-1)+aY2(1,k);
end
%tinh toan khoang giam toc ns tinh
48
syms i j%ns tuyen tinh
aX3=zeros(1,Na*Nt+1); aX3(1,1)=deltaX3(1,1)/Nt;
aY3=zeros(1,Na*Nt+1); aY3(1,1)=deltaY3(1,1)/Nt;
for i=2:Nd
for j=(Nt*(i-1)-Nt+2):(Nt*(i-1)+1)
aX3(1,j)=deltaX3(1,i)/Nt;
aY3(1,j)=deltaY3(1,i)/Nt;
end
end
syms k %ns trung binh
bX3=zeros(1,Nd*Nt+1);bY3=zeros(1,Nd*Nt+1);
b1X3=zeros(1,Nd*Nt+1);b1Y3=zeros(1,Nd*Nt+1);
b2X3=zeros(1,Nd*Nt+1);b2Y3=zeros(1,Nd*Nt+1);
for k=1:4
bX3(1,k)=((5-k)*aX3(1,1)+(k-1)*aX3(1,k))/Nt;
b1X3(1,k)=((6-k)*aX3(1,1)+(k)*aX3(1,k))/Nt;
b2X3(1,k)=(bX3(1,k)+b1X3(1,k))/2;
bY3(1,k)=((5-k)*aY3(1,1)+(k-1)*aY3(1,k))/Nt;
b1Y3(1,k)=((6-k)*aY3(1,1)+(k)*aY3(1,k))/Nt;
b2Y3(1,k)=(bY3(1,k)+b1Y3(1,k))/2;
end
for k=5:Nd*Nt
bX3(1,k)=(aX3(1,k)+aX3(1,k-1)+aX3(1,k-2)+aX3(1,k-3)+aX3(1,k-4))/Nt;
b1X3(1,k)=(aX3(1,k+1)+aX3(1,k)+aX3(1,k-1)+aX3(1,k-2)+aX3(1,k-3))/Nt;
b2X3(1,k)=(bX3(1,k)+b1X3(1,k))/2;
bY3(1,k)=(aY3(1,k)+aY3(1,k-1)+aY3(1,k-2)+aY3(1,k-3)+aY3(1,k-4))/Nt;
b1Y3(1,k)=(aY3(1,k+1)+aY3(1,k)+aY3(1,k-1)+aY3(1,k-2)+aY3(1,k-3))/Nt;
b2Y3(1,k)=(bY3(1,k)+b1Y3(1,k))/2;
end
syms l
X31=zeros(1,Nd*Nt+1);X32=zeros(1,Nd*Nt+1);
Y31=zeros(1,Nd*Nt+1);Y32=zeros(1,Nd*Nt+1);
for l=2:Nd*Nt+1
X31(1,1)=X21(1,Nc*Nt+1);X32(1,1)=X21(1,Nc*Nt+1);
X31(1,l)=X31(1,l-1)+aX3(1,l);X32(1,l)=X32(1,l-1)+b2X3(1,l);
Y31(1,1)=Y21(1,Nc*Nt+1);Y32(1,1)=Y21(1,Nc*Nt+1);
Y31(1,l)=Y31(1,l-1)+aY3(1,l);Y32(1,l)=Y32(1,l-1)+b2Y3(1,l);
end
%%
X41=zeros(1,13500-12503);
Y41=zeros(1,13500-12503);
for t=1:(13500-12503)
X41(1,t)=X31(1,Nd*Nt+1);
Y41(1,t)=Y31(1,Nd*Nt+1);
end
%%
X11s=X11(:, 1:2500);X21s=X21(:, 1:7500);X31s=X31(:, 1:2500);
Y11s=Y11(:, 1:2500);Y21s=Y21(:, 1:7500);Y31s=Y31(:, 1:2500);
wave.time = [];%iput simulink
sum=[X11s,X21s,X31s,X41;Y11s,Y21s,Y31s,Y41]';
wave.signals.values = sum;
wave.signals.dimensions =2;
xsml=out.x_sml; ysml=out.y_sml;
if(wave.time>12503)
wave.signals.value=X32(2501,1);
end
7.2. Chương trình nội suy đường tròn (m-file)
%%
clear sum
F=5; %toc do an dao 50mm/s^2
A=2; %gia toc tang toc cho phep
D=2; %gia toc giam toc cho phep
L=50; %chieu dai
T=5e-3; %chu ky noi suy
Tdk=1e-3;

49
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
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);
tas=0:Tdk:Ta;
tds=Tc+Ta:Tdk:(Tc+Ta+Td);
%noi suy duong tron
X_tam=100/10;
Y_tam=(310/6);
R=1250/30;
Wa=Va/R;%w=v/r toc do tang toc
alpha=Wa*T;%noi suy goc alpha khi tang toc
tong=sum(alpha);%tong quang duong tang toc

Wd=Vd/R;%toc do giam toc

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

Wc=F/R;%toc do cons
tong1=acos(7/25)-tong-tong2;%quang duong cons
Tc_dtr=tong1/Wc;%Thoi gian cons
Nc_dtr=round(Tc_dtr/T);% so lan noi suy cons
Vc_dtr=ones(1,Nc_dtr+1)*F;%toc do cons
tc_dtr=Ta:T:(Ta+Tc_dtr);% ko de lam gi :)
tcs=Ta:Tdk:(Ta+Tc_dtr);
Wc_dtr=Vc_dtr/R;%w=v/r
alpha1=Wc_dtr*T;% noi suy goc alpha cons
td_dtr=Tc_dtr+Ta:T:(Tc_dtr+Ta+Td);

%%
%noi suy tinh acc
syms i j%ns tuyen tinh
a_alpha=zeros(1,Na*Nt+1);a_alpha(1,Na*Nt+1)=alpha(1,Na+1)/Nt;
for i=1:Na
for j=(Nt*i-Nt+1):(Nt*i)
a_alpha(1,j)=alpha(1,i)/Nt;
end
end
syms k %ns trung binh
b_alpha=zeros(1,Na*Nt+1);
b1_alpha=zeros(1,Na*Nt+1);
b2_alpha=zeros(1,Na*Nt+1);
for k=2:Na*Nt-3

b_alpha(1,k)=(a_alpha(1,k+3)+a_alpha(1,k+2)+a_alpha(1,k+1)+a_alpha(1,k)+a_alpha(1,k-
1))/Nt;

b1_alpha(1,k)=(a_alpha(1,k+4)+a_alpha(1,k+3)+a_alpha(1,k+2)+a_alpha(1,k+1)+a_alpha(1,k
))/Nt;
b2_alpha(1,k)=(b_alpha(1,k)+b1_alpha(1,k))/2;
50
end
for k=Na*Nt-2:Na*Nt+1
b_alpha(1,k)=((k-Na*Nt+3)*a_alpha(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*a_alpha(1,k))/Nt;
b1_alpha(1,k)=((k-Na*Nt+4)*a_alpha(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*a_alpha(1,k))/Nt;
b2_alpha(1,k)=(b_alpha(1,k)+b1_alpha(1,k))/2;
end
%noi suy tinh cons
syms i j k
a_alpha1=zeros(1,Nc_dtr*Nt+1);a_alpha1(1,Nc_dtr*Nt+1)=alpha1(1,Nc_dtr+1)/Nt;
for i=1:Nc_dtr
for j=(Nt*i-Nt+1):(Nt*i)
a_alpha1(1,j)=alpha1(1,i)/Nt;
end
end
%noi suy tinh dec
syms i j%ns tuyen tinh
a_alpha2=zeros(1,Nd*Nt+1);a_alpha2(1,1)=alpha2(1,1)/Nt;
for i=2:Nd
for j=(Nt*(i-1)-Nt+2):(Nt*(i-1)+1)
a_alpha2(1,j)=alpha2(1,i)/Nt;
end
end
syms k %ns trung binh
b_alpha2=zeros(1,Nd*Nt+1);
b1_alpha2=zeros(1,Nd*Nt+1);
b2_alpha2=zeros(1,Nd*Nt+1);
for k=1:4
b_alpha2(1,k)=((5-k)*a_alpha2(1,1)+(k-1)*a_alpha2(1,k))/Nt;
b1_alpha2(1,k)=((6-k)*a_alpha2(1,1)+(k)*a_alpha2(1,k))/Nt;
b2_alpha2(1,k)=(b_alpha2(1,k)+b1_alpha2(1,k))/2;
end
for k=5:Nd*Nt
b_alpha2(1,k)=(a_alpha2(1,k)+a_alpha2(1,k-1)+a_alpha2(1,k-2)+a_alpha2(1,k-
3)+a_alpha2(1,k-4))/Nt;
b1_alpha2(1,k)=(a_alpha2(1,k+1)+a_alpha2(1,k)+a_alpha2(1,k-1)+a_alpha2(1,k-
2)+a_alpha2(1,k-3))/Nt;
b2_alpha2(1,k)=(b_alpha2(1,k)+b1_alpha2(1,k))/2;
end
%%
%tinh toan dich chuyen ns tho
%acc
A_dtr=cos(alpha);%A=cos
B_dtr=sin(alpha);%B=sin
X4=zeros(1,Na+1);
Y4=zeros(1,Na+1);
syms z
for z=2:Na+1
X4(1,1)=10;%Hoanh do diem A
Y4(1,1)=10;%Tung do diem A
X4(1,z)=A_dtr(1,z-1)*((X4(1,z-1))-X4(1,1))+B_dtr(1,z-1)*(Y_tam-Y4(1,z-1))+X4(1,1);
Y4(1,z)=Y_tam-A_dtr(1,z-1)*(Y_tam-Y4(1,z-1))+B_dtr(1,z-1)*(X4(1,z-1)-X4(1,1));
end
%cons
A_dtr1=cos(alpha1);
B_dtr1=sin(alpha1);
X5=zeros(1,Nc_dtr+1);
Y5=zeros(1,Nc_dtr+1);
syms v
for v=2:Nc_dtr+1
X5(1,1)=X4(1,Na+1);
Y5(1,1)=Y4(1,Na+1);
X5(1,v)=A_dtr1(1,v-1)*(X5(1,v-1)-X4(1,1))+B_dtr1(1,v-1)*(Y_tam-Y5(1,v-1))+X4(1,1);
Y5(1,v)=Y_tam-A_dtr1(1,v-1)*(Y_tam-Y5(1,v-1))+B_dtr1(1,v-1)*(X5(1,v-1)-X4(1,1));
end
%dec
A_dtr2=cos(alpha2);
51
B_dtr2=sin(alpha2);
X6=zeros(1,Nd+1);
Y6=zeros(1,Nd+1);
X6_dtr=zeros(1,Nd+1);
Y6_dtr=zeros(1,Nd+1);
syms m
for m=2:Nd+1
X6(1,1)=X5(1,Nc_dtr+1);
Y6(1,1)=Y5(1,Nc_dtr+1);
X6(1,m)=A_dtr2(1,m-1)*(X6(1,m-1)-X4(1,1))+B_dtr2(1,m-1)*(Y_tam-Y6(1,m-1))+X4(1,1);
Y6(1,m)=Y_tam-A_dtr2(1,m-1)*(Y_tam-Y6(1,m-1))+B_dtr2(1,m-1)*(X6(1,m-1)-X4(1,1));
end
%%
%tinh toan dich chuyen ns tinh tuyen tinh
%acc
A1_dtr=cos(a_alpha);%A=cos
B1_dtr=sin(a_alpha);%B=sin
X41=zeros(1,Na*Nt+1);
Y41=zeros(1,Na*Nt+1);
syms z
for z=2:Na*Nt+1
X41(1,1)=10;%Hoanh do diem A
Y41(1,1)=10;%Tung do diem A
X41(1,z)=A1_dtr(1,z-1)*((X41(1,z-1))-X41(1,1))+B1_dtr(1,z-1)*(Y_tam-Y41(1,z-
1))+X41(1,1);
Y41(1,z)=Y_tam-A1_dtr(1,z-1)*(Y_tam-Y41(1,z-1))+B1_dtr(1,z-1)*(X41(1,z-1)-
X41(1,1));
end
%cons
A1_dtr1=cos(a_alpha1);
B1_dtr1=sin(a_alpha1);
X51=zeros(1,Nc_dtr*Nt+1);
Y51=zeros(1,Nc_dtr*Nt+1);
syms v
for v=2:Nc_dtr*Nt+1
X51(1,1)=X41(1,Na*Nt+1);
Y51(1,1)=Y41(1,Na*Nt+1);
X51(1,v)=A1_dtr1(1,v-1)*(X51(1,v-1)-X41(1,1))+B1_dtr1(1,v-1)*(Y_tam-Y51(1,v-
1))+X41(1,1);
Y51(1,v)=Y_tam-A1_dtr1(1,v-1)*(Y_tam-Y51(1,v-1))+B1_dtr1(1,v-1)*(X51(1,v-1)-
X41(1,1));
end
%dec
A1_dtr2=cos(a_alpha2);
B1_dtr2=sin(a_alpha2);
X61=zeros(1,Nd*Nt+1);
Y61=zeros(1,Nd*Nt+1);
syms m
for m=2:Nd*Nt+1
X61(1,1)=X51(1,Nc_dtr*Nt+1);
Y61(1,1)=Y51(1,Nc_dtr*Nt+1);
X61(1,m)=A1_dtr2(1,m-1)*(X61(1,m-1)-X41(1,1))+B1_dtr2(1,m-1)*(Y_tam-Y61(1,m-
1))+X41(1,1);
Y61(1,m)=Y_tam-A1_dtr2(1,m-1)*(Y_tam-Y61(1,m-1))+B1_dtr2(1,m-1)*(X61(1,m-1)-
X41(1,1));
end
%%
%tinh toan dich chuyen ns tinh trung binh
%acc
A2_dtr=cos(b2_alpha);%A=cos
B2_dtr=sin(b2_alpha);%B=sin
X42=zeros(1,Na*Nt+1);
Y42=zeros(1,Na*Nt+1);
syms z
for z=2:Na*Nt+1
X42(1,1)=10;%Hoanh do diem A
52
 Y42(1,1)=10;%Tung do diem A
X42(1,z)=A2_dtr(1,z-1)*((X42(1,z-1))-X42(1,1))+B2_dtr(1,z-1)*(Y_tam-Y42(1,z-
1))+X42(1,1);
Y42(1,z)=Y_tam-A2_dtr(1,z-1)*(Y_tam-Y42(1,z-1))+B2_dtr(1,z-1)*(X42(1,z-1)-
X42(1,1));
end
%cons
A2_dtr1=cos(a_alpha1);
B2_dtr1=sin(a_alpha1);
X52=zeros(1,Nc_dtr*Nt+1);
Y52=zeros(1,Nc_dtr*Nt+1);
syms v
for v=2:Nc_dtr*Nt+1
X52(1,1)=X42(1,Na*Nt+1);
Y52(1,1)=Y42(1,Na*Nt+1);
X52(1,v)=A2_dtr1(1,v-1)*(X52(1,v-1)-X42(1,1))+B2_dtr1(1,v-1)*(Y_tam-Y52(1,v-
1))+X42(1,1);
Y52(1,v)=Y_tam-A2_dtr1(1,v-1)*(Y_tam-Y52(1,v-1))+B2_dtr1(1,v-1)*(X52(1,v-1)-
X42(1,1));
end
%dec
A2_dtr2=cos(b2_alpha2);
B2_dtr2=sin(b2_alpha2);
X62=zeros(1,Nd*Nt+1);
Y62=zeros(1,Nd*Nt+1);
syms m
for m=2:Nd*Nt+1
X62(1,1)=X52(1,Nc_dtr*Nt+1);
Y62(1,1)=Y52(1,Nc_dtr*Nt+1);
X62(1,m)=A2_dtr2(1,m-1)*(X62(1,m-1)-X42(1,1))+B2_dtr2(1,m-1)*(Y_tam-Y62(1,m-
1))+X42(1,1);
Y62(1,m)=Y_tam-A2_dtr2(1,m-1)*(Y_tam-Y62(1,m-1))+B2_dtr2(1,m-1)*(X62(1,m-1)-
X42(1,1));
end
%%
X12s=X12(:, 1:2500);X21s=X21(:, 1:5500);X32s=X32(:, 1:2500);
Y12s=Y12(:, 1:2500);Y21s=Y21(:, 1:5500);Y32s=Y32(:, 1:2500);
X42s=X42(:, 1:2500);X52s=X52(:, 1:10060);X62s=X62(:, 1:2500);
Y42s=Y42(:, 1:2500);Y52s=Y52(:, 1:10060);Y62s=Y62(:, 1:2500);
X72s=X72(:, 1:2500);X82s=X82(:, 1:10060);X92s=X92(:, 1:2501);
Y72s=Y72(:, 1:2500);Y82s=Y82(:, 1:10060);Y92s=Y92(:, 1:2501);
wave1.time = [];%iput simulink
sum=[X42s,X52s,X62s,X12s,X21s,X32s,X72s,X82s,X92s;Y42s,Y52s,Y62s,Y12s,Y21s,Y32s,Y72s,Y
82s,Y92s]';
wave1.signals.values = sum;
wave1.signals.dimensions =2;
xsml=x_sml; ysml=y_sml;

7.3. Chương trình nội suy chữ C (m-file)

clear sum
F=5; %toc do an dao 50mm/s^2
A=2; %gia toc tang toc cho phep
D=2; %gia toc giam toc cho phep
L=40; %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
53
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);

%noi suy duong tron

X_tam=0;
Y_tam=20;
R=20;
Wa=Va/R;%w=v/r toc do tang toc
alpha=Wa*T;%noi suy goc alpha khi tang toc
tong=sum(alpha);%tong quang duong tang toc

Wd=Vd/R;%toc do giam toc

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

Wc=F/R;%toc do cons
tong1=pi-tong-tong2;%quang duong cons
Tc_dtr=tong1/Wc;%Thoi gian cons
Nc_dtr=round(Tc_dtr/T);% so lan noi suy cons
Vc_dtr=ones(1,Nc_dtr+1)*F;%toc do cons
tc_dtr=Ta:T:(Ta+Tc_dtr);
Wc_dtr=Vc_dtr/R;%w=v/r
alpha1=Wc_dtr*T;% noi suy goc alpha cons

A_dtr=cos(alpha);%A=cos
B_dtr=sin(alpha);%B=sin
X4=zeros(1,Na+1);
Y4=zeros(1,Na+1);

syms z
for z=2:Na+1
X4(1,1)=20;%Hoanh do diem A
Y4(1,1)=20;%Tung do diem A
X4(1,z)=X4(1,z-1)*A_dtr(1,z-1)-(Y4(1,z-1)-Y_tam)*B_dtr(1,z-1);
Y4(1,z)=(Y4(1,z-1)-Y_tam)*A_dtr(1,z-1)+X4(1,z-1)*B_dtr(1,z-1)+Y_tam;
end

A_dtr1=cos(alpha1);
B_dtr1=sin(alpha1);
X5=zeros(1,Nc_dtr+1);
Y5=zeros(1,Nc_dtr+1);
syms v
for v=2:Nc_dtr+1
X5(1,1)=X4(1,Na+1)*A_dtr1(1,v-1)-(Y4(1,Na+1)-Y_tam)*B_dtr1(1,v-1);
Y5(1,1)=(Y4(1,Na+1)-Y_tam)*A_dtr1(1,v-1)+X4(1,Na+1)*B_dtr1(1,v-1)+Y_tam;
X5(1,v)=X5(1,v-1)*A_dtr1(1,v-1)-(Y5(1,v-1)-Y_tam)*B_dtr1(1,v-1);
Y5(1,v)=(Y5(1,v-1)-Y_tam)*A_dtr1(1,v-1)+X5(1,v-1)*B_dtr1(1,v-1)+Y_tam;
end

A_dtr2=cos(alpha2);
B_dtr2=sin(alpha2);
X6=zeros(1,Nd+1);
Y6=zeros(1,Nd+1);
X6_dtr=zeros(1,Nd+1);
Y6_dtr=zeros(1,Nd+1);
syms m
for m=2:Nd+1
54
 X6(1,1)=X5(1,Nc_dtr+1);
Y6(1,1)=Y5(1,Nc_dtr+1);
X6(1,m)=X6(1,m-1)*A_dtr2(1,m-1)-(Y6(1,m-1)-Y_tam)*B_dtr2(1,m-1);
Y6(1,m)=(Y6(1,m-1)-Y_tam)*A_dtr2(1,m-1)+X6(1,m-1)*B_dtr2(1,m-1)+Y_tam;
end

%%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
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
deltaL1=Va*T;
deltaX1=deltaL1*0;
deltaY1=deltaL1*1;
syms i
X1=zeros(1,Na+1);
Y1=zeros(1,Na+1);
for i=2:Na+1
X1(1,1)=X6(1,Nd+1);
X1(1,i)=X1(1,i-1)-deltaX1(1,i);
Y1(1,1)=Y6(1,Nd+1);
Y1(1,i)=Y1(1,i-1)-deltaY1(1,i);
end

%tinh toan khoang cons

deltaL2=Vc*T;
deltaX2=deltaL2*0;
deltaY2=deltaL2*1;
syms j
X2=zeros(1,Nc+1);
Y2=zeros(1,Nc+1);
for j=2:Nc+1
X2(1,1)=X1(1,Na+1);
X2(1,j)=X2(1,j-1)-deltaX2(1,j);
Y2(1,1)=Y1(1,Na+1);
Y2(1,j)=Y2(1,j-1)-deltaY2(1,j);
end
%tinh toan khoang giam toc
deltaL3=Vd*T;
deltaX3=deltaL3*0;
deltaY3=deltaL3*1;
syms k
X3=zeros(1,Nd);
Y3=zeros(1,Nd);
for k=2:Nd+1
X3(1,1)=X2(1,Nc+1);
X3(1,k)=X3(1,k-1)-deltaX3(1,k);
Y3(1,1)=Y2(1,Nc+1);
Y3(1,k)=Y3(1,k-1)-deltaY3(1,k);
end

%noi suy duong tron

55
X_tam1=0;
Y_tam1=-20;
R=20;
Wa=Va/R;%w=v/r toc do tang toc
alpha=Wa*T;%noi suy goc alpha khi tang toc
tong=sum(alpha);%tong quang duong tang toc

Wd=Vd/R;%toc do giam toc

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

Wc=F/R;%toc do cons
tong1=pi-tong-tong2;%quang duong cons
Tc_dtr=tong1/Wc;%Thoi gian cons
Nc_dtr=round(Tc_dtr/T);% so lan noi suy cons
Vc_dtr=ones(1,Nc_dtr+1)*F;%toc do cons
tc_dtr=[Ta:T:(Ta+Tc_dtr)];% ko de lam gi :)
Wc_dtr=Vc_dtr/R;%w=v/r
alpha1=Wc_dtr*T;% noi suy goc alpha cons
td_dtr=Tc_dtr+Ta:T:(Tc_dtr+Ta+Td);

A_dtr=cos(alpha);%A=cos
B_dtr=sin(alpha);%B=sin
X7=zeros(1,Na+1);
Y7=zeros(1,Na+1);
syms q
for q=2:Na+1
X7(1,1)=X3(1,Nd+1);%Hoanh do diem A
Y7(1,1)=Y3(1,Nd+1);%Tung do diem A
X7(1,q)=X7(1,q-1)*A_dtr(1,q-1)-(Y7(1,q-1)-Y_tam1)*B_dtr(1,q-1);
Y7(1,q)=(Y7(1,q-1)-Y_tam1)*A_dtr(1,q-1)+X7(1,q-1)*B_dtr(1,q-1)+Y_tam1;
end

A_dtr1=cos(alpha1);
B_dtr1=sin(alpha1);
X8=zeros(1,Nc_dtr+1);
Y8=zeros(1,Nc_dtr+1);
syms p
for p=2:Nc_dtr+1
X8(1,1)=X7(1,Na+1);
Y8(1,1)=Y7(1,Na+1);
X8(1,p)=X8(1,p-1)*A_dtr1(1,p-1)-(Y8(1,p-1)-Y_tam1)*B_dtr1(1,p-1);
Y8(1,p)=(Y8(1,p-1)-Y_tam1)*A_dtr1(1,p-1)+X8(1,p-1)*B_dtr1(1,p-1)+Y_tam1;
end

A_dtr2=cos(alpha2);
B_dtr2=sin(alpha2);
X9=zeros(1,Nd+1);
Y9=zeros(1,Nd+1);
for l=2:Nd+1
X9(1,1)=X8(1,Nc_dtr+1)*A_dtr2(1,l-1)-(Y8(1,Nc_dtr+1)-Y_tam1)*B_dtr2(1,l-1);
Y9(1,1)=(Y8(1,Nc_dtr+1)-Y_tam1)*A_dtr2(1,l-1)+X8(1,Nc_dtr+1)*B_dtr2(1,l-1)+Y_tam1;
X9(1,l)=X9(1,l-1)*A_dtr2(1,l-1)-(Y9(1,l-1)-Y_tam1)*B_dtr2(1,l-1);
Y9(1,l)=(Y9(1,l-1)-Y_tam1)*A_dtr2(1,l-1)+X9(1,l-1)*B_dtr2(1,l-1)+Y_tam1;
end
%%
%noi suy tinh duong thang
%tinh toan khoang tang toc ns tinh
syms i j%ns tuyen tinh
aX1=zeros(1,Na*Nt+1);aX1(1,Na*Nt+1)=deltaX1(1,Na+1)/Nt;
aY1=zeros(1,Na*Nt+1);aY1(1,Na*Nt+1)=deltaY1(1,Na+1)/Nt;
for i=1:Na
for j=(Nt*i-Nt+1):(Nt*i)
aX1(1,j)=deltaX1(1,i)/Nt;
aY1(1,j)=deltaY1(1,i)/Nt;
end
56
end
syms k %ns trung binh
bX1=zeros(1,Na*Nt+1);bY1=zeros(1,Na*Nt+1);
b1X1=zeros(1,Na*Nt+1);b1Y1=zeros(1,Na*Nt+1);
b2X1=zeros(1,Na*Nt+1);b2Y1=zeros(1,Na*Nt+1);
for k=2:Na*Nt-3
bX1(1,k)=(aX1(1,k+3)+aX1(1,k+2)+aX1(1,k+1)+aX1(1,k)+aX1(1,k-1))/Nt;
b1X1(1,k)=(aX1(1,k+4)+aX1(1,k+3)+aX1(1,k+2)+aX1(1,k+1)+aX1(1,k))/Nt;
b2X1(1,k)=(bX1(1,k)+b1X1(1,k))/2;
bY1(1,k)=(aY1(1,k+3)+aY1(1,k+2)+aY1(1,k+1)+aY1(1,k)+aY1(1,k-1))/Nt;
b1Y1(1,k)=(aY1(1,k+4)+aY1(1,k+3)+aY1(1,k+2)+aY1(1,k+1)+aY1(1,k))/Nt;
b2Y1(1,k)=(bY1(1,k)+b1Y1(1,k))/2;
end
for k=Na*Nt-2:Na*Nt+1
bX1(1,k)=((k-Na*Nt+3)*aX1(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*aX1(1,k))/Nt;
b1X1(1,k)=((k-Na*Nt+4)*aX1(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*aX1(1,k))/Nt;
b2X1(1,k)=(bX1(1,k)+b1X1(1,k))/2;
bY1(1,k)=((k-Na*Nt+3)*aY1(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*aY1(1,k))/Nt;
b1Y1(1,k)=((k-Na*Nt+4)*aY1(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*aY1(1,k))/Nt;
b2Y1(1,k)=(bY1(1,k)+b1Y1(1,k))/2;
end
syms l
X11=zeros(1,Na*Nt+1);X12=zeros(1,Na*Nt+1);
Y11=zeros(1,Na*Nt+1);Y12=zeros(1,Na*Nt+1);
for l=2:Na*Nt+1
e=0.0319;
X11(1,1)=-20;X12(1,1)=-20;
X11(1,l)=X11(1,l-1)-aX1(1,l);X12(1,l)=X12(1,l-1)-b2X1(1,l);
Y11(1,1)=20+e;Y12(1,1)=20+e;
Y11(1,l)=Y11(1,l-1)-aY1(1,l);Y12(1,l)=Y12(1,l-1)-b2Y1(1,l);
end
%tinh toan khoang cons ns tinh
syms i j k
aX2=zeros(1,Nc*Nt+1);aX2(1,Nc*Nt+1)=deltaX2(1,Nc+1)/Nt;
aY2=zeros(1,Nc*Nt+1);aY2(1,Nc*Nt+1)=deltaY2(1,Nc+1)/Nt;
X21=zeros(1,Nc*Nt+1);Y21=zeros(1,Nc*Nt+1);
for i=1:Nc
for j=(Nt*i-Nt+1):(Nt*i)
aX2(1,j)=deltaX2(1,i)/Nt;
aY2(1,j)=deltaY2(1,i)/Nt;
end
end
for k=2:Nc*Nt+1
X21(1,1)=X11(1,Na*Nt+1);
X21(1,k)=X21(1,k-1)-aX2(1,k);
Y21(1,1)=Y11(1,Na*Nt+1);
Y21(1,k)=Y21(1,k-1)-aY2(1,k);
end
%tinh toan khoang giam toc ns tinh
syms i j%ns tuyen tinh
aX3=zeros(1,Nd*Nt+1); aX3(1,Nd*Nt+1)=deltaX3(1,Nd+1)/Nt;
aY3=zeros(1,Nd*Nt+1); aY3(1,Nd*Nt+1)=deltaY3(1,Nd+1)/Nt;
for i=1:Nd
for j=(Nt*i-Nt+1):(Nt*i)
aX3(1,j)=deltaX3(1,i)/Nt;
aY3(1,j)=deltaY3(1,i)/Nt;
end
end
syms k %ns trung binh
bX3=zeros(1,Nd*Nt+1);bY3=zeros(1,Nd*Nt+1);
b1X3=zeros(1,Nd*Nt+1);b1Y3=zeros(1,Nd*Nt+1);
b2X3=zeros(1,Nd*Nt+1);b2Y3=zeros(1,Nd*Nt+1);
for k=1:4
bX3(1,k)=((5-k)*aX3(1,1)+(k-1)*aX3(1,k))/Nt;
b1X3(1,k)=((6-k)*aX3(1,1)+(k)*aX3(1,k))/Nt;
b2X3(1,k)=(bX3(1,k)+b1X3(1,k))/2;
57
 bY3(1,k)=((5-k)*aY3(1,1)+(k-1)*aY3(1,k))/Nt;
b1Y3(1,k)=((6-k)*aY3(1,1)+(k)*aY3(1,k))/Nt;
b2Y3(1,k)=(bY3(1,k)+b1Y3(1,k))/2;
end
for k=5:Nd*Nt
bX3(1,k)=(aX3(1,k)+aX3(1,k-1)+aX3(1,k-2)+aX3(1,k-3)+aX3(1,k-4))/Nt;
b1X3(1,k)=(aX3(1,k+1)+aX3(1,k)+aX3(1,k-1)+aX3(1,k-2)+aX3(1,k-3))/Nt;
b2X3(1,k)=(bX3(1,k)+b1X3(1,k))/2;
bY3(1,k)=(aY3(1,k)+aY3(1,k-1)+aY3(1,k-2)+aY3(1,k-3)+aY3(1,k-4))/Nt;
b1Y3(1,k)=(aY3(1,k+1)+aY3(1,k)+aY3(1,k-1)+aY3(1,k-2)+aY3(1,k-3))/Nt;
b2Y3(1,k)=(bY3(1,k)+b1Y3(1,k))/2;
end
syms l
X31=zeros(1,Nd*Nt+1);X32=zeros(1,Nd*Nt+1);
Y31=zeros(1,Nd*Nt+1);Y32=zeros(1,Nd*Nt+1);
for l=2:Nd*Nt+1
X31(1,1)=X21(1,Nc*Nt+1);X32(1,1)=X21(1,Nc*Nt+1);
X31(1,l)=X31(1,l-1)-aX3(1,l);X32(1,l)=X32(1,l-1)-b2X3(1,l);
Y31(1,1)=Y21(1,Nc*Nt+1);Y32(1,1)=Y21(1,Nc*Nt+1);
Y31(1,l)=Y31(1,l-1)-aY3(1,l);Y32(1,l)=Y32(1,l-1)-b2Y3(1,l);
end
syms l
%%
%noi suy tinh duong tron
%noi suy tinh acc
syms i j%ns tuyen tinh
a_alpha=zeros(1,Na*Nt+1);a_alpha(1,Na*Nt+1)=alpha(1,Na+1)/Nt;
for i=1:Na
for j=(Nt*i-Nt+1):(Nt*i)
a_alpha(1,j)=alpha(1,i)/Nt;
end
end
syms k %ns trung binh
b_alpha=zeros(1,Na*Nt+1);
b1_alpha=zeros(1,Na*Nt+1);
b2_alpha=zeros(1,Na*Nt+1);
for k=2:Na*Nt-3

b_alpha(1,k)=(a_alpha(1,k+3)+a_alpha(1,k+2)+a_alpha(1,k+1)+a_alpha(1,k)+a_alpha(1,k-
1))/Nt;

b1_alpha(1,k)=(a_alpha(1,k+4)+a_alpha(1,k+3)+a_alpha(1,k+2)+a_alpha(1,k+1)+a_alpha(1,k
))/Nt;
b2_alpha(1,k)=(b_alpha(1,k)+b1_alpha(1,k))/2;
end
for k=Na*Nt-2:Na*Nt+1
b_alpha(1,k)=((k-Na*Nt+3)*a_alpha(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*a_alpha(1,k))/Nt;
b1_alpha(1,k)=((k-Na*Nt+4)*a_alpha(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*a_alpha(1,k))/Nt;
b2_alpha(1,k)=(b_alpha(1,k)+b1_alpha(1,k))/2;
end
%noi suy tinh cons
syms i j k
a_alpha1=zeros(1,Nc_dtr*Nt+1);a_alpha1(1,Nc_dtr*Nt+1)=alpha1(1,Nc_dtr+1)/Nt;
for i=1:Nc_dtr
for j=(Nt*i-Nt+1):(Nt*i)
a_alpha1(1,j)=alpha1(1,i)/Nt;
end
end
%noi suy tinh dec
syms i j%ns tuyen tinh
a_alpha2=zeros(1,Nd*Nt+1);a_alpha2(1,Nd*Nt+1)=alpha2(1,Nd+1)/Nt;
for i=1:Nd
for j=(Nt*i-Nt+1):(Nt*i)
a_alpha2(1,j)=alpha2(1,i)/Nt;
end
end
58
syms k %ns trung binh
b_alpha2=zeros(1,Nd*Nt+1);
b1_alpha2=zeros(1,Nd*Nt+1);
b2_alpha2=zeros(1,Nd*Nt+1);
for k=1:4
b_alpha2(1,k)=((5-k)*a_alpha2(1,1)+(k-1)*a_alpha2(1,k))/Nt;
b1_alpha2(1,k)=((6-k)*a_alpha2(1,1)+(k)*a_alpha2(1,k))/Nt;
b2_alpha2(1,k)=(b_alpha2(1,k)+b1_alpha2(1,k))/2;
end
for k=5:Nd*Nt
b_alpha2(1,k)=(a_alpha2(1,k)+a_alpha2(1,k-1)+a_alpha2(1,k-2)+a_alpha2(1,k-
3)+a_alpha2(1,k-4))/Nt;
b1_alpha2(1,k)=(a_alpha2(1,k+1)+a_alpha2(1,k)+a_alpha2(1,k-1)+a_alpha2(1,k-
2)+a_alpha2(1,k-3))/Nt;
b2_alpha2(1,k)=(b_alpha2(1,k)+b1_alpha2(1,k))/2;
end
%tinh toan dich chuyen ns tinh tuyen tinh
%acc
A1_dtr=cos(a_alpha);%A=cos
B1_dtr=sin(a_alpha);%B=sin
X41=zeros(1,Na*Nt+1);X71=zeros(1,Na*Nt+1);
Y41=zeros(1,Na*Nt+1);Y71=zeros(1,Na*Nt+1);
syms z
for z=2:Na*Nt+1
X41(1,1)=20;X71(1,1)=-20;%Hoanh do diem A
Y41(1,1)=20;Y71(1,1)=-20+e;%Tung do diem A
X41(1,z)=X41(1,z-1)*A1_dtr(1,z-1)-(Y41(1,z-1)-Y_tam)*B1_dtr(1,z-1);
Y41(1,z)=(Y41(1,z-1)-Y_tam)*A1_dtr(1,z-1)+X41(1,z-1)*B1_dtr(1,z-1)+Y_tam;
X71(1,z)=X71(1,z-1)*A1_dtr(1,z-1)-(Y71(1,z-1)-Y_tam1)*B1_dtr(1,z-1);
Y71(1,z)=(Y71(1,z-1)-Y_tam1)*A1_dtr(1,z-1)+X71(1,z-1)*B1_dtr(1,z-1)+Y_tam1;
end
%cons
A1_dtr1=cos(a_alpha1);
B1_dtr1=sin(a_alpha1);
X51=zeros(1,Nc_dtr*Nt+1);X81=zeros(1,Nc_dtr*Nt+1);
Y51=zeros(1,Nc_dtr*Nt+1);Y81=zeros(1,Nc_dtr*Nt+1);
syms v
for v=2:Nc_dtr*Nt+1
X51(1,1)=X41(1,Na*Nt+1);X81(1,1)=X71(1,Na*Nt+1);
Y51(1,1)=Y41(1,Na*Nt+1);Y81(1,1)=Y71(1,Na*Nt+1);
X51(1,v)=X51(1,v-1)*A1_dtr1(1,v-1)-(Y51(1,v-1)-Y_tam)*B1_dtr1(1,v-1);
Y51(1,v)=(Y51(1,v-1)-Y_tam)*A1_dtr1(1,v-1)+X51(1,v-1)*B1_dtr1(1,v-1)+Y_tam;
X81(1,v)=X81(1,v-1)*A1_dtr1(1,v-1)-(Y81(1,v-1)-Y_tam1)*B1_dtr1(1,v-1);
Y81(1,v)=(Y81(1,v-1)-Y_tam1)*A1_dtr1(1,v-1)+X81(1,v-1)*B1_dtr1(1,v-1)+Y_tam1;
end
%dec
A1_dtr2=cos(a_alpha2);
B1_dtr2=sin(a_alpha2);
X61=zeros(1,Nd*Nt+1);X91=zeros(1,Nd*Nt+1);
Y61=zeros(1,Nd*Nt+1);Y91=zeros(1,Nd*Nt+1);
syms m
for m=2:Nd*Nt+1
X61(1,1)=X51(1,Nc_dtr*Nt+1);X91(1,1)=X81(1,Nc_dtr*Nt+1);
Y61(1,1)=Y51(1,Nc_dtr*Nt+1);Y91(1,1)=Y81(1,Nc_dtr*Nt+1);
X61(1,m)=X61(1,m-1)*A1_dtr2(1,m-1)-(Y61(1,m-1)-Y_tam)*B1_dtr2(1,m-1);
Y61(1,m)=(Y61(1,m-1)-Y_tam)*A1_dtr2(1,m-1)+X61(1,m-1)*B1_dtr2(1,m-1)+Y_tam;
X91(1,m)=X91(1,m-1)*A1_dtr2(1,m-1)-(Y91(1,m-1)-Y_tam1)*B1_dtr2(1,m-1);
Y91(1,m)=(Y91(1,m-1)-Y_tam1)*A1_dtr2(1,m-1)+X91(1,m-1)*B1_dtr2(1,m-1)+Y_tam1;
end
%%
%tinh toan dich chuyen ns tinh trung binh
%acc
A2_dtr=cos(b2_alpha);%A=cos
B2_dtr=sin(b2_alpha);%B=sin
X42=zeros(1,Na*Nt+1);X72=zeros(1,Na*Nt+1);
Y42=zeros(1,Na*Nt+1);Y72=zeros(1,Na*Nt+1);
59
syms z
for z=2:Na*Nt+1
X42(1,1)=20;X72(1,1)=-20;%Hoanh do diem A
Y42(1,1)=20;Y72(1,1)=-20+e;%Tung do diem A
X42(1,z)=X42(1,z-1)*A2_dtr(1,z-1)-(Y42(1,z-1)-Y_tam)*B2_dtr(1,z-1);
Y42(1,z)=(Y42(1,z-1)-Y_tam)*A2_dtr(1,z-1)+X42(1,z-1)*B2_dtr(1,z-1)+Y_tam;
X72(1,z)=X72(1,z-1)*A2_dtr(1,z-1)-(Y72(1,z-1)-Y_tam1)*B2_dtr(1,z-1);
Y72(1,z)=(Y72(1,z-1)-Y_tam1)*A2_dtr(1,z-1)+X72(1,z-1)*B2_dtr(1,z-1)+Y_tam1;
end
%cons
A2_dtr1=cos(a_alpha1);
B2_dtr1=sin(a_alpha1);
X52=zeros(1,Nc_dtr*Nt+1);X82=zeros(1,Nc_dtr*Nt+1);
Y52=zeros(1,Nc_dtr*Nt+1);Y82=zeros(1,Nc_dtr*Nt+1);
syms v
for v=2:Nc_dtr*Nt+1
X52(1,1)=X42(1,Na*Nt+1);X82(1,1)=X72(1,Na*Nt+1);
Y52(1,1)=Y42(1,Na*Nt+1);Y82(1,1)=Y72(1,Na*Nt+1);
X52(1,v)=X52(1,v-1)*A2_dtr1(1,v-1)-(Y52(1,v-1)-Y_tam)*B2_dtr1(1,v-1);
Y52(1,v)=(Y52(1,v-1)-Y_tam)*A2_dtr1(1,v-1)+X52(1,v-1)*B2_dtr1(1,v-1)+Y_tam;
X82(1,v)=X82(1,v-1)*A2_dtr1(1,v-1)-(Y82(1,v-1)-Y_tam1)*B2_dtr1(1,v-1);
Y82(1,v)=(Y82(1,v-1)-Y_tam1)*A2_dtr1(1,v-1)+X82(1,v-1)*B2_dtr1(1,v-1)+Y_tam1;
end
%dec
A2_dtr2=cos(b2_alpha2);
B2_dtr2=sin(b2_alpha2);
X62=zeros(1,Nd*Nt+1);X92=zeros(1,Nd*Nt+1);
Y62=zeros(1,Nd*Nt+1);Y92=zeros(1,Nd*Nt+1);
syms m
for m=2:Nd*Nt+1
X62(1,1)=X52(1,Nc_dtr*Nt+1);X92(1,1)=X82(1,Nc_dtr*Nt+1);
Y62(1,1)=Y52(1,Nc_dtr*Nt+1);Y92(1,1)=Y82(1,Nc_dtr*Nt+1);
X62(1,m)=X62(1,m-1)*A2_dtr2(1,m-1)-(Y62(1,m-1)-Y_tam)*B2_dtr2(1,m-1);
Y62(1,m)=(Y62(1,m-1)-Y_tam)*A2_dtr2(1,m-1)+X62(1,m-1)*B2_dtr2(1,m-1)+Y_tam;
X92(1,m)=X92(1,m-1)*A2_dtr2(1,m-1)-(Y92(1,m-1)-Y_tam1)*B2_dtr2(1,m-1);
Y92(1,m)=(Y92(1,m-1)-Y_tam1)*A2_dtr2(1,m-1)+X92(1,m-1)*B2_dtr2(1,m-1)+Y_tam1;
end
%%
X1s=X11(:, 1:2500);X2s=X21(:, 1:5500);X3s=X31(:, 1:2501);
Y1s=Y11(:, 1:2500);Y2s=Y21(:, 1:5500);Y3s=Y31(:, 1:2501);
X4s=X41(:, 1:2500);X5s=X51(:, 1:10060);X6s=X61(:, 1:2501);
Y4s=Y41(:, 1:2500);Y5s=Y51(:, 1:10060);Y6s=Y61(:, 1:2501);
X7s=X71(:, 1:2500);X8s=X81(:, 1:10060);X9s=X91(:, 1:2501);
Y7s=Y71(:, 1:2500);Y8s=Y81(:, 1:10060);Y9s=Y91(:, 1:2501);
X10s=zeros(1,15001-13232);
Y10s=zeros(1,15001-13232);
for t=1:(30001-13232)
X10s(1,t)=X9s(1,Nd*Nt+1);
Y10s(1,t)=Y9s(1,Nd*Nt+1);
end
wave1.time = [];%iput simulink
sum=[X4s,X5s,X6s,X1s,X2s,X3s,X7s,X8s,X9s,X10s;Y4s,Y5s,Y6s,Y1s,Y2s,Y3s,Y7s,Y8s,Y9s,Y10s
]';
wave1.signals.values = sum;
wave1.signals.dimensions =2;
xsml=x_sml; ysml=y_sml;
7.4. Nội suy thô đường thẳng với kiểu tăng/giảm tốc sau nội suy dạng hình
thang
%%thong so
F=5; %toc do an dao 50mm/s^2
A=2; %gia toc tang toc cho phep
D=2; %gia toc giam toc cho phep
L=50; %chieu dai
T=5e-3; %chu ky noi suy
%%tinh toan thoi gian tang toc giam toc
60
Ta=F/A;%time acc
Na=Ta/T;%Pulse acc
Tsum=L/F;%total time
%%tinh toan khoang dich chuyen
dX=L*0.8;
dY=L*0.6;
N=Tsum/T;%Pulse sum
syms i
deltaX=zeros(1,N+1);%khoang dich chuyen truc X
deltaY=zeros(1,N+1);%khoang dich chuyen truc Y
for i=1:N
deltaX(i)=dX/N;
deltaY(i)=dY/N;
end
%%output
XLO=zeros(1,N+Na);
YLO=zeros(1,N+Na);
for i=2:Na%acc
XLO(1)=deltaX(1)/Na;YLO(1)=deltaY(1)/Na;
XLO(i)=deltaX(i)/Na+XLO(i-1);
YLO(i)=deltaY(i)/Na+YLO(i-1);
end
for i=Na+1:N%cons
XLO(i)=(deltaX(i)-deltaX(i-Na))/Na+XLO(i-1);
YLO(i)=(deltaY(i)-deltaY(i-Na))/Na+YLO(i-1);
end
for i=N+1:N+Na%dec
XLO(i)=-deltaX(i-Na)/Na+XLO(i-1);
YLO(i)=-deltaY(i-Na)/Na+YLO(i-1);
end
X=zeros(1,N+Na);
Y=zeros(1,N+Na);
for i=2:N+Na
X(1,1)=10;
X(1,i)=X(1,i-1)+XLO(1,i);
Y(1,1)=10;
Y(1,i)=Y(1,i-1)+YLO(1,i);
End
7.5. Nội suy thô đường tròn với kiểu tăng/giảm tốc sau nội suy dạng hình
thang
%%thong so
F=5; %toc do an dao 50mm/s^2
A=2; %gia toc tang toc cho phep
D=2; %gia toc giam toc cho phep
alpha=1.287;%tong cung dich chuyen
T=5e-3; %chu ky noi suy
X_tam=100/10;%tam duong tron truc X
Y_tam=(310/6);%tam duong tron truc Y
R=1250/30;%ban kinh
%%tinh toan thoi gian tang toc giam toc
Ta=F/A;%time acc
Na=Ta/T;%Pulse acc
Tsum=10.72;%total time
%%tinh toan khoang dich chuyen
N=Tsum/T;%Pulse sum
syms i
d_alpha=zeros(1,N+1);%khoang dich chuyen goc alpha
for i=1:N
d_alpha(i)=alpha/N;
end
%%output
alphaLO=zeros(1,N+Na);
for i=2:Na%acc
alphaLO(1)=d_alpha(1)/Na;
alphaLO(i)=d_alpha(i)/Na+alphaLO(i-1);
61
end
for i=Na+1:N%cons
alphaLO(i)=(d_alpha(i)-d_alpha(i-Na))/Na+alphaLO(i-1);
end
for i=N+1:N+Na%dec
alphaLO(i)=-d_alpha(i-Na)/Na+alphaLO(i-1);
end
A_dtr=cos(alphaLO);%A=cos
B_dtr=sin(alphaLO);%B=sin
X_SP=zeros(1,N);
Y_SP=zeros(1,N);
for i=2:N+Na
X_SP(1,1)=10;%Hoanh do diem A
Y_SP(1,1)=10;%Tung do diem A
X_SP(1,i)=A_dtr(1,i-1)*((X_SP(1,i-1))-X_SP(1,1))+B_dtr(1,i-1)*(Y_tam-Y_SP(1,i-
1))+X_SP(1,1);
Y_SP(1,i)=Y_tam-A_dtr(1,i-1)*(Y_tam-Y_SP(1,i-1))+B_dtr(1,i-1)*(X_SP(1,i-1)-
X_SP(1,1));
end
X=zeros(1,N+Na);
Y=zeros(1,N+Na);
for i=2:N+Na
X(1,1)=10;%Hoanh do diem A
Y(1,1)=10;%Tung do diem A
X(1,i)=A_dtr(1,i-1)*((X(1,i-1))-X(1,1))+B_dtr(1,i-1)*(Y_tam-Y(1,i-1))+X(1,1);
Y(1,i)=Y_tam-A_dtr(1,i-1)*(Y_tam-Y(1,i-1))+B_dtr(1,i-1)*(X(1,i-1)-X(1,1));
end

7.6. Chương trình điều khiển trên simulink

Đường thẳng + Đường tròn:

Chữ C:

62