Luận án Nghiên cứu tổng hợpđiều khiểnhệ thống truyền động bám cho các đối tượng chuyển động chậm

BỘ GIÁO DỤC ĐÀO TẠO BỘ QUỐC PHÒNG 
 HỌC VIỆN KỸ THUẬT QUÂN SỰ 
TRẦN ĐỨC CHUYỂN 
NGHIÊN CỨU TỔNG HỢP ĐIỀU KHIỂN HỆ THỐNG TRUYỀN ĐỘNG BÁM 
CHO CÁC ĐỐI TƯỢNG CHUYỂN ĐỘNG CHẬM 
LUẬN ÁN TIẾN SỸ KỸ THUẬT 
HÀ NỘI - NĂM 2016 
BỘ GIÁO DỤC ĐÀO TẠO BỘ QUỐC PHÒNG 
HỌC VIỆN KỸ THUẬT QUÂN SỰ 
TRẦN ĐỨC CHUYỂN 
NGHIÊN CỨU TỔNG HỢP ĐIỀU KHIỂN HỆ THỐNG TRUYỀN ĐỘNG BÁM 
CHO CÁC ĐỐI TƯỢNG CHUYỂN ĐỘNG CHẬM 
Chuyên ngành: Kỹ thuật Điều khiển và Tự động hóa 
 Mã số: 62.52.02.16 
LUẬN ÁN TIẾN SỸ KỸ THUẬT 
NGƯỜI HƯỚNG DẪN KHOA HỌC: 
 1. PGS. TS ĐÀO HOA VIỆT 
 2. TS NGUYỄN THANH TIÊN 
HÀ NỘI - NĂM 2016 
 i 
LỜI CAM ĐOAN 
Tôi cam đoan đây là công trình nghiên cứu của tôi dưới sự hướng dẫn 
của PGS. TS Đào Hoa Việt và TS. Nguyễn Thanh Tiên. Các số liệu, kết quả 
nêu trong luận án là trung thực và chưa từng được ai công bố trong bất kỳ 
công trình nào. 
 Tác giả 
 Trần Đức Chuyển 
 ii 
LỜI CẢM ƠN 
Trước hết, tôi xin bày tỏ lòng biết ơn chân thành đến thầy giáo hướng 
dẫn khoa học, PGS.TS Đào Hoa Việt và TS Nguyễn Thanh Tiên, đã hướng 
dẫn, vạch ra những nội dung cần giải quyết, sau đó kiểm tra kết quả nghiên 
cứu, giúp đỡ và khuyến khích tôi hoàn thành luận án này. 
Tôi cũng xin chân thành cảm ơn các nhà khoa học và tập thể cán bộ giáo 
viên Bộ môn Kỹ thuật điện / Khoa Kỹ thuật điều khiển đã quan tâm đóng góp 
ý kiến giúp tôi hoàn thiện nội dung nghiên cứu. 
Tôi chân thành cảm ơn các đồng nghiệp trong Khoa Điện / Trường ĐH 
Kinh tế Kỹ thuật Công nghiệp / Bộ Công Thương, đã tạo điều kiện giúp đỡ về 
mặt khối lượng công việc để tôi có thời gian tập trung thực hiện luận án. 
Cuối cùng, tôi xin chân thành cảm ơn gia đình, bạn bè và đồng nghiệp đã 
luôn thông cảm, động viên, khuyến khích giúp tôi có thêm nghị lực để hoàn 
thành nội dung luận án. 
 iii 
MỤC LỤC 
Trang 
LỜI CAM ĐOAN ........................................................................................... i 
LỜI CẢM ƠN ................................................................................................ ii 
MỤC LỤC .................................................................................................... iii 
DANH MỤC CÁC CHỮ VIẾT TẮT VÀ KÝ HIỆU..................................... vi 
DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ ........................................................ xi 
MỞ ĐẦU ....................................................................................................... 1 
Chương 1. TỔNG QUAN VỀ HỆ THỐNG TRUYỀN ĐỘNG BÁM LÀM 
VIỆC Ở CHẾ ĐỘ CHẬM .............................................................................. 5 
 1.1. Khái quát về một số hệ thống truyền động bám cho các đối tượng có chế 
độ chuyển động chậm..................................................................................... 5 
 1.1.1. Khái quát chung về hệ truyền động bám ......................................... 5 
 1.1.2. Chế độ chậm trong một số hệ thống truyền động bám phức tạp ..... 6 
1.2. Xây dựng mô hình hệ truyền động bám làm việc ở chế độ chậm ............. 9 
 1.2.1. Xây dựng mô hình phần cơ hệ thống truyền động........................... 9 
 1.2.2. Mô hình động học đối tượng điều khiển phi tuyến với động cơ chấp 
hành xoay chiều đồng bộ kích từ nam châm vĩnh cửu .................................. 19 
1.3. Mô hình hệ truyền động bám làm việc ở chế độ chậm và những yếu tố 
ảnh hưởng .................................................................................................... 21 
1.4. Những nghiên cứu trong nước và ngoài nước ........................................ 27 
1.5. Đặt vấn đề nghiên cứu ........................................................................... 30 
1.6. Kết luận chương ................................................................................... 32 
Chương 2. CƠ SỞ TỔNG HỢP ĐIỀU KHIỂN BACKSTEPPING TRƯỢT 
THÍCH NGHI CHO HỆ THỐNG TRUYỀN ĐỘNG BÁM PHI TUYẾN.. .. 34 
2.1. Xây dựng phương pháp tổng hợp bộ quan sát trượt mô men cản ........... 34 
 2.1.1. Phương pháp Backstepping trượt (Backstepping - Sliding mode) .... 34 
 iv 
 2.1.2. Tổng hợp bộ quan sát trượt ............................................................ 38 
 2.1.3. Bộ quan sát trạng thái trong chế độ trượt đánh giá thành phần không 
xác định........................................................................................................ 43 
2.2. Phương pháp Backstepping trượt thích nghi cho hệ thống bám phi tuyến.. 45 
2.3. Kết luận chương ................................................................................... 61 
Chương 3. NGHIÊN CỨU TỔNG HỢP BỘ ĐIỀU KHIỂN HỆ THỐNG 
BÁM PHI TUYẾN ĐỘNG CƠ IPMSM LÀM VIỆC Ở CHẾ ĐỘ CHẬM ... 63 
3.1. Tổng hợp vòng điều khiển tốc độ backstepping trượt thích nghi cho hệ 
truyền động bám làm việc ở chế độ chậm sử dụng động cơ IPMSM ............ 63 
 3.1.1. Xây dựng thuật toán tổng hợp điều khiển backstepping trượt thích 
nghi cho vòng điều khiển tốc độ................................................................... 64 
 3.1.2. Thiết kế bộ quan sát mô men cản.................................................. 71 
3.2. Xây dựng hệ kín cho hệ thống bám theo vòng vị trí sử dụng động cơ 
IPMSM trên cơ sở bộ điều khiển backstepping trượt thích nghi ................... 73 
3.3. Kết luận chương ................................................................................... 78 
Chương 4. MÔ PHỎNG VÀ THỰC NGHIỆM ĐÁNH GIÁ CHẤT LƯỢNG 
ĐIỀU KHIỂN BACKSTEPPING TRƯỢT THÍCH NGHI HỆ THỐNG 
TRUYỀN ĐỘNG BÁM LÀM VIỆC Ở CHẾ ĐỘ CHẬM............................ 79 
4.1. Mô phỏng hệ thống truyền động bám phi tuyến với BĐK tốc độ trên cơ 
sở phương pháp backstepping trượt thích nghi sử dụng động cơ IPMSM .... 80 
4.2. Mô phỏng hệ thống truyền động bám phi tuyến với BĐK vị trí trên cơ sở 
phương pháp backstepping trượt thích nghi sử dụng động cơ IPMSM ......... 90 
 4.2.1. Nghiên cứu mô phỏng hệ thống truyền động bám vị trí trên cơ sở 
phương pháp backstepping trượt thích nghi với bộ điều khiển PI ................. 90 
 4.2.2. Nghiên cứu mô phỏng hệ thống truyền động bám vị trí trên cơ sở 
phương pháp backstepping trượt thích nghi với bộ điều khiển PID ........... 100 
4.3. Khảo xát đánh giá trên mô hình thực nghiệm với động cơ IPMSM ..... 104 
 v 
 4.3.1. Xây dựng mô hình thực nghiệm.................................................. 104 
 4.3.2. Các kết quả thực nghiệm............................................................. 106 
4.4. Kết luận chương ................................................................................. 110 
KẾT LUẬN CHUNG................................................................................. 111 
DANH MỤC CÔNG TRÌNH CỦA TÁC GIẢ .......................................... 114 
TÀI LIỆU THAM KHẢO .......................................................................... 116 
PHỤ LỤC .................................................................................................. 127 
Phụ lục 1: Nghiên cứu mô phỏng, phân tích ảnh hưởng yếu tố phi tuyến và 
thông số biến thiên đến sự làm việc của HTB ở chế độ chậm với bộ điều 
khiển kinh điển PID.................................................................................... 127 
Phụ lục 2: Các sơ đồ mô phỏng trong Matlab - Simulink ........................... 130 
Phụ lục 3: Các tham số mô phỏng và code chương trình lập trình trong Matlab 
Simulink cho BĐK backstepping trượt thích nghi ...................................... 135 
Phụ lục 4: Code chương trình lập trình cho DSP TMS320F28069.............. 137 
 vi 
DANH MỤC CÁC CHỮ VIẾT TẮT VÀ CÁC KÝ HIỆU 
1. Chữ viết tắt Ý Nghĩa 
TĐĐ Truyền động điện 
HTB Hệ thống truyền động bám 
CNC Computer Numerical Control: Máy gia công CNC 
BLDC Brusless DC motor: động cơ một chiều không chổi than 
IPMSM Interior Permanent Magnet Synchronous Motor: Động cơ đồng 
bộ kích từ nam châm vĩnh cửu (có Ld khác Lq) 
HTĐK Hệ thống điều khiển 
LabVIEW Laboratory Virtual Instrusment Engineering Workbench 
DAQ Data Acqusition: phần cứng thu thập dữ liệu và điều khiển 
NI National Instrument 
VSC Variable Structure Control: bộ điều khiển có cấu trúc biến đổi 
SVM Space vector modulation: mô hình không gian véc tơ 
DSP digital signal processing: bộ xử lý tín hiệu số 
DLL Dynamic Link Library: Thư viện liên kết động 
P Bộ điểu khiển tỷ lệ 
PI Bộ điểu khiển tỷ lệ, tích phân 
PD Bộ điểu khiển tỷ lệ, đạo hàm 
PID Bộ điểu khiển tỷ lệ, tích phân và đạo hàm 
DI đạo hàm và tích phân 
PWM Bộ điều chế độ rộng xung 
PC Personal computer: máy tính cá nhân 
PLD Programmable Logic Devices: Vi mạch logic lập trình được 
ASIC Application Specific Intergrated Circuits: các mạch tích hợp 
ứng dụng riêng 
 vii 
Osc Oscilloscope: máy hiện sóng 
KĐCS Khuếch đại công suất 
QTQĐ Quá trình quá độ 
LF Hàm Lyapunov (Lyapunov Function) 
CLF Hàm điều khiển Lyapunov (Control Lyapunov Function) 
ĐTTSLG Đặc tính tần số logarit 
BĐK Bộ điều khiển 
2. Ký hiệu Ý nghĩa 
; ;
;
L dh
dc c
M M
M M
 Mô men tải, mô men đàn hồi, mô men động cơ, mô men cản [Nm] 
( )BM t Mô men nhiễu [Nm] 
1
W ( )s Hàm truyền đạt của khối lượng thứ nhất 
2
W ( )s Hàm truyền đạt của khối lượng thứ hai 
1
( )A  Đặc tính biên độ tần số pha 
J1 Mô men quán tính của truyền động chấp hành [kgm2] 
J2 Mô men quán tính còn lại của đối tượng bộ truyền cơ và tải [kgm2] 
J Mô men quán tính tổng cộng [kgm2] 
C Hệ số đàn hồi [Nm/rad] 
hdF Lực đàn hồi [N] 
n Tốc độ (vòng/phút) 
1 và 2 Vận tốc góc của động cơ và vận tốc góc trên tải [rad/s] 
'
1 ;... ' ';n j  Véc tơ hồi quy 
;d r Góc [rad] 
sgn; sign Hàm dấu 
q Biến dạng góc 
 viii 
S Biến dạng dài 
i Hàm phi tuyến 
,s rL L Điện cảm stato và rô to [H] 
,d qL L Điện kháng trục-d, điện kháng trục-q của động cơ điện [H] 
,s rR R Điện trở stato và rô to động cơ điện [ ] 
,sd sqi i Dòng điện tọa độ trục d, dòng điện trục q [A] 
,sd squ u Các thành phần của véc tơ điện áp stato trên hệ trục tọa độ dq 
m Từ thông liên kết 
,  Các độ khuếch đại thích nghi 
i Tỷ số truyền hộp đổi tốc 
 Tham số hằng chưa biết 
BL Góc độ rộng khe hở [rad], (backlash angle). 
F Thành phần ước tính xấp xỉ bằng luật thích nghi 
i Sai số đánh giá 
ry Tín hiệu tham số chuẩn [rad] 
,i j Hàm hiệu chỉnh 
iz Sai số hiệu chỉnh 
X Véc tơ trạng thái của hệ thống 
Y Véc tơ đầu ra của hệ thống 
A Ma trận hệ thống 
B Ma trận đầu vào 
V Hàm Lyapunov 
iV Hàm Lyapunov thứ i 
ix Trạng thái thứ i của hệ thống điều khiển 
, ,ic  Các hằng số dương 
 ix 
i Điều khiển ảo 
i Đại lượng hiệu chỉnh bộ quan sát 
i Sai số bộ quan sát 
 Ma trận có kích cỡ thích hợp 
Q Ma trận đối xứng 
R Tập các số thực 
R+ Tập các số thực dương 
C Tập các số phức 
T Hằng số thời gian của một khâu 
T Hằng số thời gian của cả vòng điều chỉnh tốc độ 
3. Chỉ số trên Ý nghĩa 
0 Độ 
1T Chuyển vị 
 Đánh giá 
4. Chỉ số dưới Ý nghĩa 
i Số nguyên 
dh Đàn hồi 
xn Xoắn 
dc Động cơ 
max Cực đại 
min Cực tiểu 
 Tổng 
,d dat Đặt trước 
ms; masat Ma sát 
sd;sq tọa độ trục d, tọa độ trục q 
 x 
DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ 
STT Tên hình vẽ, đồ thị Trang 
1 Hình 1.1. Sơ đồ khối HTB pháo AK230 trên tàu hải quân 7 
2 Hình 1.2. Mô hình động học pháo tàu với mục tiêu di động 
(máy bay) 
8 
3 Hình 1.3. Sơ đồ động học thể hiện mối quan hệ các phần tử 
trong phần cơ 
9 
4 Hình 1.4. Mô hình hai khối lượng có liên hệ đàn hồi 10 
5 Hinh 1.5. Sơ đồ cấu trúc hệ thống hai khối lượng có liên hệ 
đàn hồi 
11 
6 Hình 1.6 a, b. Biến đổi sơ đồ cấu trúc từ hình 1.5 11 
7 Hình 1.7. Sơ đồ cấu trúc phần cơ hệ truyền động 13 
8 Hình 1.8 a), b). Kết quả khảo sát mô phỏng ĐTTSLG của hệ 
thống 2 khối lượng có liên hệ đàn hồi trên Matlab - Simulink 
14 
9 Hình 1.9. Mô hình mô men ma sát phụ thuộc vào tốc độ 18 
10 Hình 1.10. Mô hình điều khiển động cơ IPMSM 19 
11 Hình 1.11. Sơ đồ khối hệ thống truyền động bám vị trí 21 
12 Hình 1.12. a), b) Đặc tính mô men cản của HTB làm việc ở 
chế độ chậm 
22 
13 Hình 2.1. Sơ đồ cấu trúc bộ quan sát trên cơ sở các bước quan 
sát 
41 
14 Hình 2.2. Sơ đồ bộ quan sát trên cơ sở phương pháp phân 
chia chuyển động 
42 
15 Hình 2.3. Sơ đồ khối hệ thống Backstepping trượt thích nghi 45 
16 Hình 2.4. Hệ thống có dạng phản hồi thuần túy 46 
17 Hình 3.1. Hệ thống tính toán bộ điều khiển backstepping trượt 70 
 xi 
thích nghi 
18 Hình 3.2. Sơ đồ khối bộ quan sát mô men tải 71 
19 Hình 3.3. Sơ đồ khối cấu trúc hệ thống bám 72 
20 Hình 3.4. Sơ đồ khối hệ thống truyền động bám điện cơ làm 
việc ở chế độ chậm sử dụng động cơ IPMSM 
73 
21 Hình 3.5. a), b) Biến đổi sơ đồ cấu trúc bộ điều khiển vị trí 75 
22 Hình 4.1. Sơ đồ mô phỏng sự phụ thuộc của mô men ma sát 
vào tốc độ 
79 
23 Hình 4.2. Đặc tính mô men ma sát theo tốc độ 80 
24 Hình 4.3. Mô hình mô phỏng bộ điều khiển backstepping 
trượt thích nghi hệ truyền động phi tuyến có kể đến phần cơ 
của cơ cấu công tác 
81 
25 Hình 4.4. Mô hình mô phỏng bộ điều khiển backstepping 
trượt thích nghi hệ truyền động phi tuyến trên Matlab-
Simulink 
81 
26 Hình 4.5. a) Tốc độ đặt d và tốc độ thực  của động cơ, b) 
tốc độ đặt d và tốc độ thực  của cơ cấu công tác trường 
hợp 1 
82 
27 Hình 4.6. a) sai số bám sát theo tốc độ và b) ˆ;L LM M ở trường 
hợp 1 
82 
28 Hình 4.7. Đáp ứng dòng điện: a) isq, b) isd ở trường hợp 1 83 
29 Hình 4.8. a) Tốc độ đặt d và tốc độ thực  của động cơ, b) 
mô men đặt LM và mô men ước lượng ˆ LM trường hợp 2 
83 
30 Hình 4.9. Sai số bám sát theo tốc độ trường hợp 2 84 
31 Hình 4.10. Đáp ứng dòng điện: a) isq, b) isd trường hợp 2 84 
32 Hình 4.11. a) Tốc độ đặt d và tốc độ thực  của động cơ, b) 85 
 xii 
mô men đặt LM và mô men ước lượng ˆ LM trường hợp 3 
33 Hình 4.12. Sai số bám sát theo tốc độ trường hợp 3 85 
34 Hình 4.13. Đáp ứng dòng điện: a) isq, b) isd trường hợp 3 86 
35 Hình 4.14. a) Tốc độ đặt d và tốc độ thực  của động cơ, b) 
mô men đặt LM và mô men ước lượng ˆ LM trường hợp 4 
86 
36 Hình 4.15. Sai số bám sát theo tốc độ trường hợp 4 86 
37 Hình 4.16. Đáp ứng dòng điện: a) isq, b) isd trường hợp 4 87 
38 Hình 4.17. a) Tốc độ đặt d và tốc độ thực  của động cơ, b) 
mô men đặt LM và mô men ước lượng ˆ LM trường hợp 5 
87 
39 Hình 4.18. Sai số bám sát theo tốc độ trường hợp 5 88 
40 Hình 4.19. Đáp ứng dòng điện: a) isq, b) isd trường hợp 5 88 
41 Hình 4.20. Sơ đồ mô phỏng hệ truyền động bám phi tuyến với 
BĐK vị trí có tính đến yếu tố phi tuyến mô men ma sát và đàn 
hồi sử dụng khâu PI 
89 
42 Hình 4.21. Sơ đồ mô phỏng thực nghiệm nhận dạng khâu bậc 
hai 
90 
43 Hình 4.22. Xây dựng đặc tính vào ra theo tốc độ 90 
44 Hình 4.23. Đáp ứng vào ra BĐK vị trí: a) theo góc, b) bộ 
quan sát mô men tải ˆ;L LM M trường hợp 1 
92 
45 Hình 4.24. Sai số bám sát theo góc trường hợp 1 93 
46 Hình 4.25. Thành phần dòng điện BĐK vị trí: a) isd, b) isq 
trong trường hợp 1 
93 
47 Hình 4.26. Đáp ứng vào ra BĐK vị trí: a) theo góc, b) bộ 
quan sát mô men tải ˆ;L LM M trường hợp 2 
94 
48 Hình 4.27. Sai số bám sát theo gó ... }}'); 
legend('\it\color{black} d_1','\it\color{red} d_1','Location',[.80 .45 
0.07 0.02],'Orientation','Vertical'); 
set(legend, 'Box', 'off'); 
subplot(125); 
plot(t,iqs-0.1,'LineWidth',2.5); 
grid on; 
set(gca,'Fontname','Arial','Fontsize',20,'LineWidth',3,'FontWeight','bold
'); 
axis([0.4 0.8 -2 3]); 
xlabel('Time(sec)'); 
ylabel('{i_{qs}}(A)'); 
%ylabel('{i_{a}}(A)'); 
subplot(126); 
plot(t,idsd,'k:',t,ids,'r','lineWidth',2.5); 
grid on; 
%plot(t,ids,'r-',t,iqs,'k:','lineWidth',2.5); 
set(gca,'Fontname','Arial','Fontsize',20,'LineWidth',3,'FontWeight','bold
'); 
{i_{d}}','Color','r','FontWeight','bold','Fontsize',20); 
% text(0.5,-5, 'Dashed : 
{i_{dsd}}','Color','b','FontWeight','bold','Fontsize',20); 
axis([0.4 0.8 -3 3]); 
%axis([0 1 -20 3]); 
xlabel('Time(sec)'); 
ylabel('i_{ds}(A)'); 
legend('\it\color{black} i_{dsd}','\it\color{red} i_{ds}','Location',[.80 
.25 0.07 0.02],'Orientation','Vertical'); 
set(legend, 'Box', 'off'); 
3.3. Chương trình lập trình code trên S_funtion: 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
function [sys,x0,str,ts] = sch_chuongtrinh(t,x,u,flag) 
 switch flag 
 case 0, 
 [sys,x0,str,ts] = mdlInitializeSizes; 
 case 3, 
 sys = mdlOutputs(t,x,u); 
 case {1,2,4,9} 
 sys = []; 
 otherwise 
 error(['unhandled flag = ',num2str(flag)]); 
end 
sizes.NumInputs = 2;% vsq,vsd 
 137 
sizes.NumSampleTimes = 1;  sys = simsizes(sizes); 
x0 = [];% Khoi tao bien trang thai 
str = []; 
ts = [-1 0]; 
function sys = mdlOutputs(t,x,u) 
Vdc = 110; 
va = 2*Vdc/3; 
vb = va; 
ts = 1/5e3; 
a = abs(u(1))+abs(u(2))/sqrt(3); 
b = abs(u(1))-abs(u(2))/sqrt(3); 
c = 2*abs(u(2))/sqrt(3); 
% for case us_alpha <0 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
if u(2) <0 
 if u(1)<0 
 if b <0 
 s = 5; vr = a; 
 vl = -b; 
: 
 s = 4; vr = b; 
 vl = c; 
 end 
 else 
 if b < 0 
 s = 5; 
 vr = -b; vl = a; 
 else 
 s = 6; 
 vr = c; 
 vl = b; 
 end 
 end 
else 
 if u(1) <0 
 if b<0 
 s = 2; 
 vr = -b; vl = a; 
 else 
 s = 3; 
 vr = c; 
 vl = b; 
 end 
 else 
 if b<0 
 s = 2; 
 vr = a; 
 vl = -b; 
 else 
 s = 1; 
 vr = b; 
 vl = c; 
 end 
 end 
 138 
end 
ta = vr*ts/va; 
tb = vl*ts/vb; 
t0 = ts-ta-tb; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if t0 <0 
 ta = ta*ts/(tb+ta); 
 tb = tb*ts/(tb+ta); 
 t0 = 0; 
end 
y = [ta;tb;t0;s]; 
sys = y; 
 end 
end 
end 
Phụ lục 4: Code chương trình lập trình cho DSP TMS320F28069 
4.1. Chương trình code phần PWM 
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////* 
 PwmSv.cc 
 *//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
#include "Inc.h" 
#define DC_VOLT 310 
#define DC_EFF DC_VOLT * 0.606 * 0.7 
#define PHASE_0 0 
#define PHASE_120 2.094 
#define PHASE_240 4.189 
#define BASE_FREQ 2 
#define PI 3.141592 
#define STEP_ANGLE (BASE_FREQ * 2 * PI) / (PWM_FREQ * xx00) 
Float fSinA,fSinB,fSinC; 
Float fTmpData, fTmp; 
Float fStepAngle; 
 139 
_iq qStepAngle; 
_iq qThetaA; 
_iq qThetaB; 
_iq qThetaC; 
_iq qVan; 
_iq qVbn; 
_iq qVcn; 
Float fVan,fVbn,fVcn; 
Float fTa,fTb,fTc; 
Float fFreq,fVdc; 
pwmSvInfoType pwmSvInfo; 
void pwmSvInit(void) 
{ 
pwmSvInfo.fVm = DC_VOLT; 
pwmSvInfo.fVdcEff = DC_EFF; 
pwmSvInfo.fThetaA = PHASE_0; 
pwmSvInfo.fThetaB = PHASE_120; 
pwmSvInfo.fThetaC = PHASE_240; 
qThetaA = _IQ(PHASE_0); 
qThetaB = _IQ(PHASE_120); 
qThetaC = _IQ(PHASE_240); 
} 
void pwmSvUpdate(pwmSvInfoType *p) 
 140 
{ 
fFreq = _IQtoF(p->Freq); 
fVdc = _IQtoF(p->Vdc); 
qStepAngle = _IQmpy(_IQ(STEP_ANGLE),p->Freq); 
if(qThetaA >= _IQ(2*PI)) 
qThetaA = 0; 
else 
qThetaA += qStepAngle; 
if(qThetaB >= _IQ(2*PI)) 
qThetaB = 0; 
else 
qThetaB += qStepAngle; 
if(qThetaC >= _IQ(2*PI)) 
qThetaC = 0; 
else 
qThetaC += qStepAngle; 
fSinA = _IQtoF(_IQsin(qThetaA)); 
fSinB = _IQtoF(_IQsin(qThetaB)); 
fSinC = _IQtoF(_IQsin(qThetaC)); 
p->fVan = (_IQtoF(p->Vdc) * p->fVm) * _IQtoF(_IQsin(qThetaA)); 
p->fVbn = (_IQtoF(p->Vdc) * p->fVm) * _IQtoF(_IQsin(qThetaB)); 
p->fVcn = (_IQtoF(p->Vdc) * p->fVm) * _IQtoF(_IQsin(qThetaC)); 
qVan = _IQmpy(_IQmpy(p->Vdc,_IQ(p->fVm)),_IQsin(qThetaA)); 
qVbn = _IQmpy(_IQmpy(p->Vdc,_IQ(p->fVm)),_IQsin(qThetaB)); 
 141 
qVcn = _IQmpy(_IQmpy(p->Vdc,_IQ(p->fVm)),_IQsin(qThetaC)); 
fVan = _IQtoF(qVan); 
fVbn = _IQtoF(qVbn); 
fVcn = _IQtoF(qVcn); 
if(p->fVan >= p->fVbn) { 
p->fVmax = p->fVan; 
p->fVmin = p->fVbn; 
 } 
else { 
p->fVmax = p->fVbn; 
p->fVmin = p->fVan; 
 } 
if(p->fVcn > p->fVmax) p->fVmax = p->fVcn; 
else if(p->fVcn fVmin) p->fVmin = p->fVcn; 
#if SET 
fTmpData = p->fVmax - p->fVmin; 
if(fTmpData > p->fVdcEff) { 
fTmp = p->fVdcEff / fTmpData; 
p->fVmax = p->fVmax * fTmp; 
p->fVmin = p->fVmin * fTmp; 
p->fVan = p->fVan * fTmp; 
p->fVbn = p->fVbn * fTmp; 
p->fVcn = p->fVcn * fTmp; 
} 
 142 
p->fVsn = -((p->fVmax + p->fVmin) * 0.5); 
p->fVan_ref = p->fVan + p->fVsn; 
p->fVbn_ref = p->fVbn + p->fVsn; 
p->fVcn_ref = p->fVcn + p->fVsn; 
p->fTa = 0.5 - (p->fVan_ref / p->fVm); 
p->fTb = 0.5 - (p->fVbn_ref / p->fVm); 
p->fTc = 0.5 - (p->fVcn_ref / p->fVm); 
fTa = 0.5 - (p->fVan_ref / p->fVm); 
fTb = 0.5 - (p->fVbn_ref / p->fVm); 
fTc = 0.5 - (p->fVcn_ref / p->fVm); 
} 
End of PwmSv.c 
.. 
4.2. Chương trình code phần chuyển đổi tọa độ alpha beta sang dq 
=============================================================== 
 File name: C.Cs (IQ version) 
 Originator: Digital Control Systems Group 
 Alphascription: chuyen Transformation 
=============================================================== 
 History: 
------------------------------------------------------------------------------------- 
 04-15-2014 Version 3.20 
------------------------------------------------------------------------------------- 
#include "IQmathLib.h" Include header for IQmath library 
Don't forget to set a proper GLOBAL_Q in "IQmathLib.h" file 
#include "park.h" 
void park_calc(PARK *v) 
{ 
 143 
 _iq Cosine,Sine; 
Using look-up IQ sine table 
 Sine = _IQsinPU(v->Angle); 
 Cosine = _IQcosPU(v->Angle); 
 v->Qs = _IQmpy(v->Alpha,Cosine) + _IQmpy(v->Beta,Sine); 
 v->Ds = _IQmpy(v->Alpha,Sine) - _IQmpy(v->Beta,Cosine); 
if (v->Ds > _IQ(0.5)) v->Ds = _IQmpy(v->Ds,_IQ(0.01)); 
if (v->Ds Ds = _IQmpy(v->Ds,_IQ(-0.01)); 
} 
....... 
4.3. Code chương trình tính toán đầu ra 
/******************************************************************** 
* Outputs calculation 
* 
********************************************************************* 
********************************************************************/ 
//Constant 
gamma1 = 0.067; 
gamma2 = 0.01; 
gamma3 = 0.01; 
muyd = 268 ; //muyd > 0 
muyq = 126; 
etad = 0.01; 
etaq = 0.01; 
kq = 30; 
//Main calculation 
F1Estimated = F1Estimated + (- gamma1*(sq*M1 + sd))*Tsamp; 
F2Estimated = F2Estimated + (- gamma2*sq*M2)*Tsamp; 
F3Estimated = F3Estimated + - gamma3*(k1 - M3)*sq; 
idd_k = -lam_mo/(2*(Ldo - Lqo)) - sqrt(lam_mo^2/(4*(Ldo - Lqo)^2) + 
iq^2) 
idd_dev = (idd_k - idd_kminus1)/Tsamp; 
Omegad_devdev = (Omegad_k - 2*Omegad_kminus1 + 
Omegad_kminus2)/Tsamp^2; 
Omegad_dev = (Omegad_k - Omegad_kminus1)/Tsamp; 
vd = Ldo*(idd_dev - f1o - F1Estimated + Ksd*e2_k + 
etad*sd/(Absolute(sd) + muyd)); 
 144 
vq = (Lqo/M2)*(k1*Omegad_dev - k1*(f3o + F3) + Omegad_devdev - 
M1*(f1o + vd/Ldo0) - M2*f2o + M3*f3o - M1*F1Estimated -M2*F2Estimated 
+ M3*F3Estimated - k1*F3Estimated + kq*sq + 
etaq*sq/(Absolute(sq)+muyq)) //trang 78 - delta unknown 
.. 
//For next Loop 
idd_kminus1 = idd_k; 
Omegad_kminus1 = Omegad_k; 
Omegad_kminus2 = Omegad_kminus1; 
/////////////////////////////////////////////////////// 
// Additional Functions 
// 
// 
double Sign(double inVar){ 
 double outVar; 
 if (inVar > 0) outVar = 1; 
 if (inVar < 0) outVar = -1; 
 if (inVar == 0) outVar = 0; 
 return outVar; 
} 
double Absolute(double inVar){ 
 double outVar; 
 if (inVar > 0) outVar = inVar; 
 if (inVar < 0) outVar = -inVar; 
 if (inVar == 0) outVar = 0; 
 return outVar; 
} 
........ 
4.4. Chuong trình lập trình tính toán tốc độ phản hồi từ cảm biến góc 
TI File $Revision: /main/9 $ 
Chuyn $Date: April 21, 2014 15:42:23 $ 
######################################################################## 
FILE: DSP_posspeed.c 
TITLE: Pos/speed measurement using EQEP peripheral 
DESCRIPTION: 
This file includes the EQEP initialization and position and speed caculation 
functions called by DSP_28xxxEqep_posspeed.c. The position and 
speed calculation steps performed by POSSPEED_Calc() at SYSCLKOUT = 150 MHz 
and 100 MHz are described in detail below: 
 145 
 For 150 MHz Operation: 
 --------------------------------------------------------------------------------------------------- 
 1. This program calculates: **theta_mech** 
 theta_mech = QPOSCNT/mech_Scaler = QPOSCNT/xx00, where xx00 is the number of 
 counts in 1 revolution.(xx00/4 = xx00 line/rev. quadrature encoder) 
 2. This program calculates: **theta_elec** 
 theta_elec = (# pole pairs) * theta_mech = 2*QPOSCNT/xx00 for this example 
 3. This program calculates: **SpeedRpm_fr** 
 SpeedRpm_fr = [(x2-x1)/xx00]/T - Equation 1 
 Note (x2-x1) = difference in number of QPOSCNT counts. Dividing (x2-x1) by 
 100 gives position relative to Index in one revolution. 
 If base RPM = xx00 rpm: xx00 rpm = [(x2-x1)/xx00]/10ms - Equation 2 
 = [(x2-x1)/xx00]/(.01s*1 min/60 sec) 
 = [(x2-x1)/xx00]/(1/xx00) min 
max (x2-x1) = xx00 counts, or 1 revolution in 10 ms 
If both sides of Equation 2 are divided by xx00 rpm, then: 
 1 = [(x2-x1)/xx00] rev./[(1/xx00) min * xx00rpm] 
Because (x2-x1) must be <xx00 (max) for 
QPOSCNT increment, 
 (x2-x1)/xx00 < 1 for CW rotation 
 And because (x2-x1) must be >-xx00 for QPOSCNT decrement, 
 (x2-x1)/xx00>-1 for CCW rotation 
 speed_fr = [(x2-x1)/xx00]/[(1/xx00) min * xx00rpm] 
 = (x2-x1)/xx00 - Equation 3 
 146 
To convert speed_fr to RPM, multiply Equation 3 by xx00 rpm 
 SpeedRpm_fr = xx00rpm *(x2-x1)/xx00 - Final Equation 
: 
. 
4. **min rpm ** = selected at 10 rpm based on CCPS prescaler options available (128 is 
greatest) 
5. **SpeedRpm_pr** 
 SpeedRpm_pr = X/(t2-t1) - Equation 4 
 where X = QCAPCTL [UPPS]/xx00 rev. (position relative to Index in 1 revolution) 
If max/base speed = xx00 rpm: xx00 = (32/xx00)/[(t2-t1)/(150MHz/128)] 
 where 32 = QCAPCTL [UPPS] (Unit timeout - once every 32 edges) 
 32/xx00 = position in 1 revolution (position as a fraction of 1 revolution) 
 t2-t1/(150MHz/128), t2-t1= # of QCAPCLK cycles, and 
 1 QCAPCLK cycle = 1/(150MHz/128) 
 = QCPRDLAT 
 So: xx00 rpm = [32(150MHz/128)*60s/min]/[xx00(t2-t1)] 
 t2-t1 = [32(150MHz/128)*60 s/min]/(xx00*xx00rpm) - Equation 5 
 = 94 CAPCLK cycles = maximum (t2-t1) = SpeedScaler 
Divide both sides by (t2-t1), and: 
 1 = 94/(t2-t1) = [32(150MHz/128)*60 s/min]/(xx00*xx00rpm)]/(t2-t1) 
 Because (t2-t1) must be < 94 for QPOSCNT increment: 
 94/(t2-t1) < 1 for CW rotation 
 And because (t2-t1) must be >-94 for QPOSCNT decrement: 
 94/(t2-t1)> -1 for CCW rotation 
 speed_pr = 94/(t2-t1) 
 or [32(150MHz/128)*60 s/min]/(xx00*xx00rpm)]/(t2-t1) - Equation 6 
 147 
To convert speed_pr to RPM: 
Multiply Equation 6 by xx00rpm: 
 SpeedRpm_fr = xx00rpm * [32(150MHz/128)*60 s/min]/[xx00*xx00rpm*(t2-t1)] 
 = [32(150MHz/128)*60 
s/min]/[xx00*(t2-t1)] 
 or [(32/xx00)rev * 60 s/min]/[(t2-t1)(QCPRDLAT)]- Final Equation 
For 100 MHz Operation: 
---------------------- 
The same calculations as above are performed, but with 100 MHz 
instead of 150MHz when calculating SpeedRpm_pr. 
The value for freqScaler_pr becomes: [32*(100MHz/128)*60s/min]/(xx00*xx00rpm) = 63 
More detailed calculation results can be found in the Example_freqcal.xls 
spreadsheet included in the example folder. 
This file contains source for the posspeed module 
######################################################################## 
Original Author: SDchuye 
$TI Release: DSP320F28xxx/DSP320F28xxx C/C++ . 
$Release Date: August 4, 2014 $ 
######################################################################## 
#include "DSP28x_Project.h" Device Headerfile and Examples Include File 
#include "Example_posspeed.h" Example specific Include file 
void POSSPEED_Init(void) 
{ 
#if (CPU_FRQ_150MHZ) 
EQep2Regs.QUPRD=1500xxx; 
Unit Timer for 100Hz at 150 MHz SYSCLKOUT 
#endif 
#if (CPU_FRQ_100MHZ) 
EQep2Regs.QUPRD=1000xxx; Unit Timer for 100Hz at 100 MHz SYSCLKOUT 
#endif 
EQep2Regs.QDECCTL.bit.QSRC=00; QEP quadrature count mode 
EQep2Regs.QEPCTL.bit.FREE_SOFT=2; 
EQep2Regs.QEPCTL.bit.PCRM=00; PCRM=00 mode - QPOSCNT reset on index 
 148 
event 
EQep2Regs.QEPCTL.bit.UTE=1; Unit Timeout Enable 
EQep2Regs.QEPCTL.bit.QCLM=1; Latch on unit time out 
EQep2Regs.QPOSMAX=0x2710; 
EQep2Regs.QEPCTL.bit.QPEN=1; QEP enable 
EQep2Regs.QCAPCTL.bit.UPPS=7; 1/4 for unit position 
EQep2Regs.QCAPCTL.bit.CCPS=5; 1/16 for CAP clock 
EQep2Regs.QCAPCTL.bit.CEN=1; QEP Capture Enable 
} 
void POSSPEED_Calc(POSSPEED *p) 
{ 
 long tmp; 
 unsigned int pos16bval; ,temp1; _iq Tmp1,newp,oldp; 
**** Position calculation - mechanical and electrical motor angle **** 
 p->DirectionQep = EQep2Regs.QEPSTS.bit.QDF; Motor direction: 0=CCW/reverse, 
1=CW/forward 
 pos16bval=(unsigned int)EQep2Regs.QPOSCNT; capture position once per QA/QB 
period 
 p->theta_raw = pos16bval+ p->cal_angle; 
raw theta = current pos. + ang. offset from QA 
The following lines calculate p->theta_mech ~= QPOSCNT/mech_scaler [current cnt/(total 
cnt 
in 1 rev.)] 
 where mech_scaler = 4000 cnts/revolution 
 tmp = (long)((long)p->theta_raw*(long)p->mech_scaler); Q0*Q26 = Q26 
 tmp &= 0x03FFF000; 
 p->theta_mech = (int)(tmp>>11); Q26 -> Q15 
 p->theta_mech &= 0x7FFF; 
 The following lines calculate p->elec_mech 
 p->theta_elec = p->pole_pairs*p->theta_mech; // Q0*Q15 = Q15 
 p->theta_elec &= 0x7FFF; 
Check an index occurrence 
 if (EQep2Regs.QFLG.bit.IEL == 1) 
 { 
 p->index_sync_flag = 0x00F0; 
 EQep2Regs.QCLR.bit.IEL=1; // Clear interrupt flag 
: 
. 
..