Luận án Nghiên cứu xây dựng thuật toán điều khiển dự báo theo mô hình cho đối tượng phi tuyến liên tục

BỘ GIÁO DỤC VÀ ðÀO TẠO 
ðẠI HỌC THÁI NGUYÊN 
NGUYỄN THỊ MAI HƯƠNG 
NGHIÊN CỨU XÂY DỰNG THUẬT TOÁN ðIỀU KHIỂN 
DỰ BÁO THEO MÔ HÌNH CHO ðỐI TƯỢNG 
PHI TUYẾN LIÊN TỤC 
LUẬN ÁN TIẾN SĨ KỸ THUẬT 
THÁI NGUYÊN – NĂM 2016 
BỘ GIÁO DỤC VÀ ðÀO TẠO 
ðẠI HỌC THÁI NGUYÊN 
NGUYỄN THỊ MAI HƯƠNG 
NGHIÊN CỨU XÂY DỰNG THUẬT TOÁN ðIỀU KHIỂN 
DỰ BÁO THEO MÔ HÌNH CHO ðỐI TƯỢNG 
PHI TUYẾN LIÊN TỤC 
 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: 
PGS.TS Lại Khắc Lãi 
 THÁI NGUYÊN – NĂM 2016 
ii 
LỜI CẢM ƠN 
Trong quá trình làm luận án, tôi ñã nhận ñược rất nhiều góp ý về chuyên 
môn cũng như sự ủng hộ về các công tác tổ chức của tập thể cán bộ hướng 
dẫn, của các nhà khoa học, của các bạn ñồng nghiệp. Tôi xin ñược gửi tới họ 
lời cảm ơn sâu sắc. 
Tôi xin bày tỏ lòng cảm ơn ñến tập thể cán bộ hướng dẫn ñã tâm huyết 
hướng dẫn tôi trong suốt thời gian qua. 
Tôi cũng xin chân thành cảm ơn các ñồng nghiệp, tập thể các nhà khoa 
học trường ðại học Kỹ thuật Công nghiệp, của bộ môn ðiều khiển tự ñộng 
trường ðại học Bách khoa Hà Nội, ñã có những ý kiến ñóng góp quý báu, các 
Phòng ban của Trường ðại học Kỹ thuật Công nghiệp ñã tạo ñiều kiện thuận 
lợi cho tôi trong suốt quá trình thực hiện ñề tài luận án. 
 Thái Nguyên, ngày tháng 01 năm 2016 
 Tác giả luận án 
 Nguyễn Thị Mai Hương 
iii 
MỤC LỤC 
LỜI CAM ðOAN i 
LỜI CẢM ƠN ii 
MỤC LỤC iii 
MỞ ðẦU 1 
1. Giới thiệu ....................................................................................................... 1 
2. Tính cấp thiết của luận án ............................................................................... 2 
3. Mục tiêu của luận án ...................................................................................... 4 
4. ðối tượng, phạm vi và phương pháp nghiên cứu ............................................ 4 
5. Ý nghĩa khoa học và thực tiễn ........................................................................ 5 
5.1. Ý nghĩa khoa học ................................................................................. 5 
5.2. Ý nghĩa thực tiễn ................................................................................. 5 
6. Bố cục luận án ................................................................................................ 6 
CHƯƠNG 1 8 
TỔNG QUAN VỀ ðIỀU KHIỂN DỰ BÁO CHO HỆ PHI TUYẾN 8 
1.1. Tổng quan các công trình nghiên cứu về ñiều khiển dự báo hệ phi 
tuyến trên thế giới ....................................................................................... 9 
1.2. Các phương pháp quy hoạch phi tuyến ...................................................... 18 
1.2.2. Bài toán tối ưu hóa phi tuyến bị ràng buộc gồm: Kỹ thuật 
hàm phạt và hàm chặn, Phương pháp SQP [3], [5[ và GA [2] ........... 19 
1.3. Các phương pháp ñiều khiển tối ưu ........................................................... 19 
1.4. Các công trình nghiên cứu về ñiều khiển dự báo hệ phi tuyến 
trong nước ................................................................................................ 20 
1.5. Những vấn ñề cần tiếp tục nghiên cứu về ñiều khiển dự báo cho 
hệ phi tuyến và hướng nghiên cứu của luận án ......................................... 21 
1.6. Kết luận chương 1 ..................................................................................... 23 
iv 
CHƯƠNG 2 24 
ðIỀU KHIỂN DỰ BÁO HỆ PHI TUYẾN TRÊN NỀN CÁC 
PHƯƠNG PHÁP QUY HOẠCH PHI TUYẾN 24 
2.1. Nguyên lý làm việc của ñiều khiển dự báo phi tuyến ................................. 24 
2.1.1. Cấu trúc bộ ñiều khiển dự báo ........................................................ 26 
2.1.2. Kỹ thuật cài ñặt bộ ñiều khiển dự báo trên nền các phương 
pháp quy hoạch phi tuyến ................................................................. 29 
2.2. Áp dụng vào ñiều khiển dự báo lớp hệ song tuyến..................................... 31 
2.2.1. Thuật toán ñiều khiển dự báo phi tuyến cho hệ song tuyến ............. 32 
2.2.2. ðKDB trên nền tối ưu hóa theo sai lệch tín hiệu ñiều khiển ............ 36 
2.3. Kết luận chương 2 ..................................................................................... 42 
CHƯƠNG 3 43 
ðỀ XUẤT MỘT PHƯƠNG PHÁP MỚI ðỂ ðIỀU KHIỂN DỰ BÁO 
HỆ PHI TUYẾN LIÊN TỤC TRÊN NỀN BIẾN PHÂN 43 
3.1. Nội dung cơ bản của phương pháp biến phân ............................................ 44 
3.1.1. Nguyên lý biến phân ....................................................................... 45 
3.1.2. Bộ ñiều khiển LQR (Linear Quadratic Regulator) .......................... 46 
3.1.3. ðiều kiện ñủ cho tính ổn ñịnh của hệ LQR ..................................... 46 
3.1.4. Áp dụng nguyên tắc ñiều khiển LQR ñể ñiều khiển tối ưu hệ 
tuyến tính bám ổn ñịnh theo giá trị ñầu ra cho trước ......................... 47 
3.2. Phương pháp ñề xuất ñể ñiều khiển dự báo với cửa sổ dự báo vô 
hạn cho hệ song tuyến liên tục không dừng, bám theo ñược giá trị 
ñầu ra cho trước ........................................................................................ 49 
3.2.1. Tư tưởng chính của phương pháp ................................................... 49 
3.2.2. Xây dựng thuật toán ñiều khiển ...................................................... 51 
3.2.3. Khả năng xử lý ñiều kiện ràng buộc ................................................ 53 
3.2.4. Chứng minh tính bám ổn ñịnh của phương pháp ñược ñề xuất ....... 54 
3.2.5. Khả năng áp dụng cho hệ phi tuyến affine không dừng................... 56 
v 
CHƯƠNG 4 58 
THỰC NGHIỆM KIỂM CHỨNG CHẤT LƯỢNG PHƯƠNG 
PHÁP ðà ðỀ XUẤT TRÊN ðỐI TƯỢNG TRMS 58 
4.1. Mô hình toán của hệ TRMS ....................................................................... 58 
4.1.1. Mô tả vật lý hệ TRMS .................................................................... 58 
4.1.2. Mô hình tựa Newton ....................................................................... 59 
4.2. Thiết kế bộ ñiều khiển dự báo trên nền quy hoạch phi tuyến ..................... 64 
4.2.1. Thiết kế và cài ñặt bộ ñiều khiển dự báo cho hệ TRMS .................. 64 
4.2.2. Mô phỏng trên MatLab ................................................................... 65 
4.3. Thiết kế bộ ñiều khiển dự báo trên nền biến phân (phương pháp 
ñiều khiển ñược luận án ñề xuất) .............................................................. 69 
4.3.1. Thiết kế và cài ñặt bộ ñiều khiển .................................................... 69 
4.3.2. Mô phỏng trên MatLab và so sánh, ñánh giá chất lượng ................. 70 
4.4. Thí nghiệm trên mô hình vật lý của hệ TRMS ........................................... 74 
4.4.1. Cài ñặt bộ quan sát Kalman ............................................................ 75 
4.4.2. Các kết quả thực nghiệm ................................................................. 82 
4.5. Kết luận chương 4 .................................................................................... 90 
DANH MỤC CÔNG TRÌNH ðà CÔNG BỐ LIÊN QUAN ðẾN ðỀ 
TÀI ........................................................................................................... 92 
TÀI LIỆU THAM KHẢO 93 
Tiếng Việt ................................................................................................ 93 
Tiếng Anh ................................................................................................ 93 
PHỤ LỤC ...................................................................................................... 102 
vi 
DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT 
Các kí hiệu: 
Ký hiệu Diễn giải nội dung ñầy ñủ 
pN Miền (phạm vi) dự báo 
cN Miền (phạm vi) ñiều khiển 
( )tl m Chiều dài của phần ñuôi của cánh tay ñòn (m ) 
( )ml m Chiều dài của phần chính của cánh tay ñòn (m ) 
( )bl m Chiều dài cánh tay ñòn ñối trọng (m ) 
( )cbl m Khoảng giữa cánh tay ñòn ñối trọng và khớp (bộ nối) (m ) 
/ s ( )ms tr m Bán kính của hộp bảo vệ cánh quạt chính/ñuôi 
( )trm kg Khối lượng của ñộng cơ một chiều ñuôi (kg ) 
( )mrm kg Khối lượng của ñộng cơ một chiều chính (kg ) 
( )cbm kg Khối lượng của ñối trọng (kg ) 
( )tm kg Khối lượng của phần ñuôi của cánh tay ñòn (kg ) 
( )mm kg Khối lượng phần chính của cánh tay ñòn (kg ) 
( )bm kg Khối lượng của cánh tay ñòn ñối trọng (kg ) 
( )tsm kg Khối lượng của lưới chắn ñuôi (kg ) 
( )msm kg Khối lượng của lưới chắn chính (kg ) 
gk Hệ số con quay 
vii 
/ ( )av hR Ω ðiện trở phần ứng của ðCMC cánh quạt chính/ñuôi (Ω ) 
/ ( )av hL mH ðiện cảm phần ứng của ðCMC cánh quạt chính/ñuôi (H ) 
( )ak Nm Aϕ Từ thông 
2
/ ( )mr trJ gcm Mômen quán tính của ðCMC chính/ñuôi ( 2kgm s ) 
2
/ ( )mr trB kgm s Hệ số ma sát nhớt của ðCMC chính và ðCMC ñuôi 
( 2kgm s ) 
v hF Hàm phi tuyến của lực khí ñộng học từ cánh quạt chính 
và cánh quạt ñuôi (N ) 
g Gia tốc trọng trường ( 2m s ) 
vJ Mômen quán tính của trục ngang (trục hoành) ( 2kgm ) 
, ,
/fric v fric hM M Mômen của lực ma sát trong mặt phẳng thẳng ñứng/ mặt 
phẳng ngang 
, , , ,
, , ,
ah v fhp fhn fvp
fvn th v v m
k k k k
k k k k
 Các hệ số dương (Nm AWb ) 
v hω Vận tốc góc của cánh quạt chính và cánh quạt ñuôi 
(rad s ) 
/h vΩ Vận tốc góc của cánh tay ñòn TRMS trong mặt phẳng 
ngang/ mặt phẳng thẳng ñứng (rad s ) 
v hU ðiện áp ðCMC cánh quạt chính/ñuôi (V ) 
av hE Sức ñiện ñộng của ðCMC cánh quạt chính/ñuôi (V ) 
viii 
av hi Dòng ñiện phần ứng của ðCMC cánh quạt chính/ñuôi (A ) 
v hϕ Từ thông của ðCMC cánh quạt chính/ñuôi (Wb ) 
ev hM Mômen ñiện từ của ðCMC cánh quạt chính/ñuôi (Nm ) 
lv hM Mômen tải của ðCMC cánh quạt chính/ñuôi (Nm ) 
,γ γm t Các hệ số biến dạng của chiều dài cánh tay ñòn chính và ñuôi 
vS
Vận tốc góc của cánh tay ñòn TRMS trong mặt phẳng 
thẳng ñứng mà không bị ảnh hưởng bởi cánh quạt 
ñuôi (rad s ) 
hS
Vận tốc góc của cánh tay ñòn TRMS trong mặt phẳng 
ngang mà không bị ảnh hưởng bởi cánh quạt chính 
(rad s ) 
ˆ ( )k i k+y ðầu ra dự báo ở thời ñiểm thứ k i+ so với thời ñiểm thứ k 
( )k i k+u Tín hiệu ñiều khiển ở thời ñiểm thứ k i+ so với thời ñiểm 
thứ k 
refy Tín hiệu ñặt hoặc ñầu ra quá trình 
kx Vector của n giá trị trạng thái của hệ tính tại thời ñiểm 
t kT= 
ku Vector của m n≤ giá trị tín hiệu ñiều khiển (tín hiệu ñầu vào) 
ky Vector của r m≤ giá trị tín hiệu ñáp ứng (tín hiệu ñầu ra) 
ix 
k i+e Sai lệch 
T Chu kỳ trích mẫu tín hiệu 
( )J U Hàm mục tiêu 
*
U Nghiệm của bài toán tối ưu 
iq Trọng số sai lệch 
jr Trọng số ñiều khiển 
Q Ma trận trọng số sai lệch 
R Ma trận trọng số ñiều khiển 
k∆u Sai lệch tín hiệu ñiều khiển 
Θ Ma trận có tất cả các phần tử ñều bằng 0 
I Ma trận ñơn vị 
δ Sai lệch giữa tham số trạng thái hiện thời và tham số trạng 
thái xác lập 
ρ Sai lệch giữa tín hiệu ñiều khiển hiện thời và tín hiệu ñiều 
khiển xác lập 
( )s U Hàm phạt 
x 
Các chữ viết tắt: 
ANFIS Adaptive Neural Fuzzy Inference System 
BB Branch and Bound 
BFO Bacterial Foraging Optimization 
ðCMC ðộng cơ một chiều 
ðKDB ðiều khiển dự báo 
DMC Dynamical Matrix Control 
EKF Extended Kalman Filter 
FSMC Fuzzy Sliding Mode Control 
GA Genetic Algorithm 
GPC Generalized Predictive Control 
IIO Increment Input Output models 
IO Direct Input Output models 
IOM Input Output Models 
LP Linear programming 
LQG Linear Quadratic Gausian 
LQR Linear Quadratic Regulator 
LRPC Long-Range Predictive Control 
LTI Linear time - invariant 
xi 
MIMO Multiple Input Multiple Output 
MPC Model Prediction Control 
MPCS Thuật toán MPC 
NMPC Nonlinear Model Prediction Control 
NNs Neural Networks 
PIDAFC PID Active force control 
QP Quadratic Programing 
RHC Receding horizon control 
SISO Single Input Single Output 
SQP Sequential Quadratic Programing 
TRMS Twin rotor MIMO system 
UKF Unscented Kalman Filter 
xii 
 DANH MỤC CÁC HÌNH ẢNH, ðỒ THỊ 
Hình 2.1. Cấu trúc cơ bản của một hệ thống ñiều khiển dự báo 37 
Hình 2.2. Sơ ñồ khối của MPC ñể ñiều khiển hệ song tuyến 44 
Hình 3.1: Hệ kín với bộ ñiều khiển phản hồi trạng thái tối ưu LQR 60 
Hình 3.2: Mô tả tư tưởng của phương pháp 63 
Hình 3.3. ðiều khiển dự báo hệ phi tuyến liên tục với cửa sổ dự báo 
vô hạn 
68 
Hình 4.1. Cấu hình vật lý của hệ TRMS 72 
Hình 4.2. Cấu trúc bộ ðKDB áp dụng cho thuật toán SQP 79 
Hình 4.3. ðáp ứng của góc chao dọc khi tín hiệu ñặt là xung vuông 79 
Hình 4.4. ðáp ứng của góc ñảo lái khi tín hiệu ñặt là xung vuông 80 
Hình 4.5. ðáp ứng của góc chao dọc khi tín hiệu ñặt là substep 80 
Hình 4.6. ðáp ứng của góc ñảo lái khi tín hiệu ñặt là substep 81 
Hình 4.7. Sơ ñồ cấu trúc bộ ðKDB phản hồi trạng thái ñể tín hiệu ra 
bám theo tín hiệu ñầu ra mẫu cho hệ TRMS 
83 
Hình 4.8. ðáp ứng ñầu ra góc ñảo lái khi tín hiệu ñặt là xung vuông 84 
Hình 4.9. ðáp ứng ñầu ra góc chao dọc khi tín hiệu ñặt là xung vuông 84 
Hình 4.10. ðáp ứng ñầu ra góc ñảo lái khi tín hiệu ñặt là substep 84 
Hình 4.11. ðáp ứng ñầu ra góc chao dọc khi tín hiệu ñặt là substep 85 
Hình 4.12. Lưu ñồ của phương pháp quan sát Kalman mở rộng 89 
Hình 4.13. Sơ ñồ mô phỏng kiểm tra bộ quan sát trạng thái 92 
xiii 
Hình 4.14. ðáp ứng ñầu ra bộ quan sát trạng thái so với ñáp ứng ñầu ra 
của mô hình của biến trạng thái thứ nhất (
h
Ω ) 
92 
Hình 4.15. ðáp ứng ñầu ra bộ quan sát trạng thái so với ñáp ứng ñầu ra 
của mô hình của biến trạng thái thứ hai (
h
S ) 
93 
Hình 4.16. ðáp ứng ñầu ra bộ quan sát trạng thái so với ñáp ứng ñầu ra 
của mô hình của biến trạng thái thứ ba (
h
α ) 
93 
Hình 4.17. ðáp ứng ñầu ra bộ quan sát trạng thái so với ñáp ứng ñầu ra 
của mô hình của biến trạng thái thứ tư ( vΩ ) 
94 
Hình 4.18. ðáp ứng ñầu ra bộ quan sát trạng thái so với ñáp ứng ñầu ra 
của mô hình của biến trạng thái thứ tư năm ( vS ) 
94 
Hình 4.19. ðáp ứng ñầu ra bộ quan sát trạng thái so với ñáp ứng ñầu ra 
của mô hình của biến trạng thái thứ tư sáu (
vα ) 
95 
Hình 4.20. Hình ảnh thí nghiệm ñiều khiển hệ thống TRMS 96 
Hình 4.21. Bộ ñiều khiển dSPACE1103 98 
Hình 4.22. Phần mềm giám sát và ñiều khiển ControlDesk 99 
Hình 4.23. ðáp ứng ñầu ra của góc chao dọc khi sử dụng bộ ñiều khiển 
dự báo tối ưu hóa trên nền qui hoạch phi tuyến 
101 
Hình 4.24. ðáp ứng ñầu ra của góc ñảo lái khi sử dụng bộ ñiều khiển dự 
báo tối ưu hóa trên nền qui hoạch phi tuyến 
101 
Hình 4.25. ðáp ứng ñầu ra của góc chao dọc khi sử dụng bộ ñiều khiển 
dự báo bám ổn ñịnh theo tín hiệu mẫu ở ñầu ra 
102 
Hình 4.26. ðáp ứng ñầu ra của góc ñảo lái khi sử dụng bộ ñiều khiển 
dự báo bám ổn ñịnh theo tín hiệu mẫu ở ñầu ra 
102 
1 
MỞ ðẦU 
1. Giới thiệu 
 ðiều khiển dự báo dựa trên mô hình (Model Predictive Control - 
MPC), hay còn thường ñược gọi ngắn gọn là ñiều khiển dự báo, ra ñời vào 
cuối thập niên 70 và ñầu thập niên 80 của thế kỉ trước, là một xu hướng ñiều 
khiển ñược ưa chuộng. Trong hai thập kỷ trở lại ñây, ñiều khiển dự báo ñã có 
những bước phát triển rất ñáng kể, ñóng góp khá nhiều các phương pháp về 
mặt học thuật cũng như ñẩy mạnh khả năng ứng dụng của MPC trong thực tế, 
 ... +av0))-
0.5*dah^2*H*sin(2*(av+av0)))/Jv; 
 end 
 sys=[dwh;dSh;dah;dwv;dSv;dav]; 
function sys = mdlOutputs(t,x,u) 
sys = x; 
3. Code chương trình EKF cài ñặt bộ ñiều khiển dự báo cho ñối tượng TRMS 
chạy thực nghiệm. 
function Xe=mdlKalman(Uhp,Uvp,ah,av) 
%================= Cac hang so ================= 
lt=2.82e-1; % Chieu dai cua phan canh tay don duoi 
lm=2.46e-1; % Chieu dai cua phan canh tay don chinh 
Jv=5.26e-2; % Mo men quan tinh cua truc ngang 
D=5.52877e-2; 
E=5.85764e-3; 
F=5.85077e-3; 
123 
g=9.81; % Gia toc trong truong 
A=9.82347e-2; 
B=1.135659e-1; 
C=2.21788e-2; 
H=4.94734e-2; 
Jtr=3.1432e-5; % Mo men quan tinh cua dong co duoi/tai 
Rah=8; 
kthp=5e-8; % Th=kthp*sign(wh)*(wh)^2 
kthn=4.41e-8; % Th=kthn*sign(wh)*(wh)^2 
Btr=2.3e-5; % He so ma sat nhot cua rotor duoi 
Jmr=2.0098e-4; % Mo men quan tinh cua dong co chinh/tai 
Rav=8; 
ktvp=5.6e-7; % Th=ktvp*sign(wv)*(wv)^2 
ktvn=5.1e-7; % Th=ktvn*sign(wv)*(wv)^2 
Bmr=2.97e-5; % He so ma sat nhot cua rotor chinh 
av0=-6.048e-1; % Goc chao doc ban dau 
kfhp=2.1377e-6; % Fh(wh)=kfhp*sign(wh)*(wh)^2 
kfhn=1.9056e-6; % Fh(wh)=kfhn*sign(wh)*(wh)^2 
kfvp=1.9506e-5; % Fv(wv)=kfvp*sign(wv)*(wv)^2 
kfvn=1.1012e-5; % Fv(wv)=kfvn*sign(wv)*(wv)^2 
kvfh=4.91e-3; % Ma sat nhot theo phuong ngang 
kvfv=5.47e-3; % Ma sat nhot theo phuong thang dung 
kcfh=3.96e-5; 
kcfv=1.5e-4; 
kchp=5.60e-3; % He so ma sat (cable force coefficient) 
kchn=5.60e-3; % He so ma sat (cable force coefficient) 
km=0.00023; % Anh huong cua rotor chinh len goc theo phuong ngang 
kt=0.000026; % Anh huong cua rotor duoi len goc theo phuong thang dung 
TC=0.0202; % He so mo men xoan (mo men xoan khong doi) 
124 
k1=8.5; % He so giao dien theo phuong thang dung (doc) 
k2=6.5; % He so giao dien ngang 
kg=0.2; 
Ts=0.2; 
nx=6; 
nu=2; 
ny=2; 
%================= Cac bien so ================ 
persistent Xp Pk 
if isempty(Xp) 
 Xp=zeros(nx,1); 
end 
if isempty(Pk) 
 Pk=zeros(nx,nx); 
end 
ahp=Xp(3,1); 
avp=Xp(6,1); 
whp=Xp(1,1); 
Shp=Xp(2,1); 
wvp=Xp(4,1); 
Svp=Xp(5,1); 
Q=eye(nx,nx)*0.01; 
R=eye(ny,ny)*0.01; 
As=zeros(nx,nx); 
Bs=zeros(nx,nu); 
Cs=[0 0 1 0 0 0;0 0 0 0 0 1]; 
if whp>=0 
 whn=whp+Ts*(TC*(k2*Uhp-TC*whp)/Rah-Btr*whp-kthp*whp^2)/Jtr; 
 Shn=Shp+Ts*(lt*kfhp*whp^2*cos(avp+av0)-kvfh*Shp-... 
125 
 kchp*ahp)/(D*(cos(avp+av0))^2+E*(sin(avp+av0))^2+F)-... 
 Ts*kvfh*km*wvp*cos(avp+av0)/(D*(cos(avp+av0))^2+E*... 
 (sin(avp+av0))^2+F)^2; 
 else 
 whn=whp+Ts*(TC*(k2*Uhp-TC*whp)/Rah-Btr*whp+kthn*whp^2)/Jtr; 
 Shn=Shp+Ts*(-lt*kfhn*whp^2*cos(avp+av0)-kvfh*Shp-... 
 kchp*ahp)/(D*(cos(avp+av0))^2+E*(sin(avp+av0))^2+F)-... 
 Ts*kvfh*km*wvp*cos(avp+av0)/(D*(cos(avp+av0))^2+E*... 
 (sin(avp+av0))^2+F)^2; 
end 
if wvp>=0 
 wvn=wvp+Ts*(TC*(k1*Uvp-TC*wvp)/Rav-Bmr*wvp-
ktvp*wvp^2)/Jmr; 
 Svn=Svp+Ts*(lm*kfvp*wvp^2-kvfv*Svp-kvfv*kt*whp/Jv+... 
 g*((A-B)*cos(avp+av0)-C*sin(avp+av0)))/Jv; 
 else 
 wvn=wvp+Ts*(TC*(k1*Uvp-TC*wvp)/Rav-
Bmr*wvp+ktvn*wvp^2)/Jmr; 
 Svn=Svp+Ts*(-lm*kfvn*wvp^2-kvfv*Svp-kvfv*kt*whp/Jv+... 
 g*((A-B)*cos(avp+av0)-C*sin(avp+av0)))/Jv; 
 end 
ahn=ahp+Ts*Shp+Ts*km*wvp*cos(avp+av0)/... 
 (D*(cos(avp+av0))^2+E*(sin(avp+av0))^2+F); 
avn=avp+Ts*Svp+Ts*kt*whp/Jv; 
DEN=D*(cos(avp+av0))^2+E*(sin(avp+av0))^2+F; 
if whp>=0 
 As(1,1)=1-Ts*(Btr+kthp*whp+TC^2/Rah)/Jtr; 
 As(2,1)=Ts*lt*kfhp*whp*cos(avp+av0)/DEN; 
else 
 As(1,1)=1-Ts*(Btr-kthn*whp+TC^2/Rah)/Jtr; 
126 
 As(2,1)=-Ts*lt*kfhn*whp*cos(avp+av0)/DEN; 
end 
As(2,2)=1-Ts*kvfh/DEN; 
if ahp>=0 
 As(2,3)=-Ts*kchp/DEN; 
else 
 As(2,3)=-Ts*kchn/DEN; 
end 
As(2,4)=-Ts*kvfh*km*cos(avp+av0)/DEN^2; 
As(3,2)=Ts; 
As(3,3)=1; 
As(3,4)=Ts*km*cos(avp+av0)/DEN; 
if wvp>=0 
 As(4,4)=1-Ts*(Bmr+ktvp*wvp+TC^2/Rav)/Jmr; 
 As(5,4)=Ts*lm*kfvp*wvp/Jv; 
else 
 As(4,4)=1-Ts*(Btr-ktvn*wvp+TC^2/Rav)/Jmr; 
 As(5,4)=-Ts*lm*kfvn*wvp/Jv; 
end 
As(5,1)=-Ts*kvfv*kt/(Jv^2); 
As(5,5)=1-Ts*kvfv/Jv; 
if avp==0 
 As(5,6)=Ts*g*((A-B)*cos(avp+av0)-C*sin(avp+av0))/Jv; 
else 
 As(5,6)=Ts*g*((A-B)*cos(avp+av0)-C*sin(avp+av0))/(Jv*avp); 
end 
As(6,1)=Ts*kt/Jv; 
As(6,5)=Ts; 
As(6,6)=1; 
127 
Bs(1,1)=Ts*TC*k2/(Jtr*Rah); 
Bs(4,2)=Ts*TC*k1/(Jmr*Rav); 
% ============Buoc du bao 
Xp(3,1)=ahn; 
Xp(6,1)=avn; 
Xp(1,1)=whn; 
Xp(2,1)=Shn; 
Xp(4,1)=wvn; 
Xp(5,1)=Svn; 
Pk=As*Pk*As'+Q; 
% ================== Buoc cap nhap 
Lk=Pk*Cs'*inv(Cs*Pk*Cs'+R); 
Xp=Xp+Lk*([ah;av]-Cs*Xp); 
Pk=(eye(nx,nx)-Lk*Cs)*Pk; 
Xe=Xp; 
function OUTU =UKF(INU) 
Uhp=INU(1); %Dau vao doi tuong (bien duoc dieu khien) 
Uvp=INU(2); %Dau vao doi tuong (bien duoc dieu khien) 
ah=INU(3); %Dau ra doi tuong (bien duoc dieu khien) 
av=INU(4); %Dau ra doi tuong (bien duoc dieu khien) 
ahp=INU(5); %Dau ra doi tuong truoc do 
avp=INU(6); %Dau ra doi tuong truoc do 
whp=INU(7); %Trang thai duoc uoc luong truoc do 
Shp=INU(8); %Trang thai duoc uoc luong truoc do 
wvp=INU(9); %Trang thai duoc uoc luong truoc do 
Svp=INU(10); %Trang thai duoc uoc luong truoc do 
nx=6; %So bien trang thai 
ny=2; %So bien dau vao 
ny=2; %So bien dau ra 
128 
Pkpv=INU(13:12+nx*nx); %Dang vector cua P(k-1|k-1) 
Pkp=zeros(nx,nx); %Dang matran cua P(k-1|k-1) 
for i=1:nx 
 Pkp(:,i)=Pkvp([1:nx]+(i-1)*nx,1); 
end 
% Cac tham so TRMS 
lts=2.82e-1; % Chieu dai cua phan canh tay don duoi 
lm=2.46e-1; % Chieu dai cua phan canh tay don chinh 
Jv=5.26e-2; % Mo men quan tinh cua truc ngang 
D=5.52877e-2; 
E=5.85764e-3; 
F=5.85077e-3; 
g=9.81 % Gia toc trong truong 
A=9.82347e-2; 
B=1.135659e-1; 
C=2.21788e-2; 
H=4.94734e-2; 
Jtr=3.1432e-5; % Mo men quan tinh cua dong co duoi/tai 
Rah=8; % Dien tro phan ung 
Lah=8.6e-4; % Dien cam phan ung 
kthp=5e-8; % Th=kthp*sign(wh)*(wh)^2 
kthn=4.41e-8; % Th=kthn*sign(wh)*(wh)^2 
Btr=2.3e-5; % He so ma sat nhot cua rotor duoi 
Jmr=2.0098e-4; % Mo men quan tinh cua dong co chinh/tai 
Rav=8; % Dien tro phan ung 
Lav=8.6e-4; % Dien cam phan ung 
ktvp=5.6e-7; % Th=ktvp*sign(wv)*(wv)^2 
ktvn=5.1e-7; % Th=ktvn*sign(wv)*(wv)^2 
Bmr=2.97e-5; % He so ma sat nhot cua rotor chinh 
129 
av0=-6.048e-1; % Goc chao doc ban dau 
kfhp=2.1377e-6; % Fh(wh)=kfhp*sign(wh)*(wh)^2 
kfhn=1.9056e-6; % Fh(wh)=kfhn*sign(wh)*(wh)^2 
kfvp=1.9506e-5; % Fv(wv)=kfvp*sign(wv)*(wv)^2 
kfvn=1.1012e-5; % Fv(wv)=kfvn*sign(wv)*(wv)^2 
kvfh=4.91e-3; % Ma sat nhot theo phuong ngang 
kvfv=5.48e-3; % Ma sat nhot theo phuong thang dung 
kchp=5.60e-3; % He so ma sat (cable force coefficient) 
km=0.00023; % Anh huong cua rotor chinh len goc theo phuong ngang 
kt=0.000026; % Anh huong cua rotor duoi len goc theo phuong thang dung 
TC=0.0202; % He so mo men xoan (mo men xoan khong doi) 
k1=8.5; % He so giao dien theo phuong thang dung (doc) 
k2=6.5; % He so giao dien ngang 
Ts=0.2; % Thoi gian lay mau 
% Gia tri trung binh va hiep phuong sai 
Ew=[1 0.1 0.032 1 0.1 0.032] % Vector trung binh cua nhieu qua trinh 
Ev=ones(ny,1)*0.032; % Vector trung binh cua nhieu do duoc 
Q=eye(nx,nx)*0.4; % Ma tran hiep phuong sai cua nhieu qua trinh 
R=eye(ny,ny)*0.4; % Ma tran hiep phuong sai cua nhieu do duoc 
% Cac tham so UKF 
a1=0.001; % Tham so alpha cua UKF 
bet=2; % Tham so beta cua UKF 
kai=0; % Tham so kai cua UKF 
lam=a1^2*(2*nx+kai)-2*nx; %Tham so lamda cua UKF 
% Trang thai tang (trang thai duoc bo sung) va hiep phuong sai 
Xkp=[whp;Shp;ahp;wvp;Svp;avp]; % x(k-1|k-1) 
Xkpa=zeros(2*nx,1); % xa(k-1|k-1) 
Xkpa(1:nx,1)=Xkp; 
Xkpa(nx+1:2*nx)=Ew; 
130 
Pkpa=zeros(2*nx,2*nx); % Pa(k-1|k-1) 
Pkpa(1+nx:2*nx,1+nx:2*nx)=Q; 
% Dang 4*nx+1 diem sigma 
Ksip=zeros(2*nx,4*nx+1); % Xi(k-1|k-1) 
Ksip(:,1)=Xkpa; 
% Tim can bac hai cua ma tran Pkpa bang viec cheo hoa gia tri rieng va vec to 
rieng 
VPkp=zeros(nx,nx); 
DPkp=zeros(nx,nx); 
[VPkp,DPkp]=myeig(Pkp); 
DPkpsqrt=sqrt(abs(DPkp)); 
YPkp=VPkp*DPkpsqrt*inv(VPkp); 
YPkpa=[YPkp zeros(nx,nx); zeros(nx,nx) sqrt(Q)]; 
%======================================================
============== 
Pkpasqr=sqrt(2*nx+lam)*YPkpa; 
for i=1:2*nx 
 Ksip(:,i+1)=Xkpa+Pkpasqr(:,i); 
 Ksip(:,i+2*nx+1)=Xkpa-Pkpasqr(:,i); 
end 
% Su lan truyen diem sigma thong qua cac phuong trinh khong gian trang thai 
% roi rac phi tuyen cua TRMS 
Ksi=zeros(nx,4*nx+1); 
for i=4*nx+1 
 whp=Ksip(1,i); 
 Shp=Ksip(2,i); 
 ahp=Ksip(3,i); 
 wvp=Ksip(4,i); 
 Svp=Ksip(5,i); 
 avp=Ksip(6,i); 
131 
 if whp>=0 
 whn=whp+Ts*(TC*(k2*Uhp-TC*whp)/Rah-Btr*whp-kthp*whp^2)/Jtr; 
 Shn=Shp+Ts*(lts*kfhp*whp^2*cos(avp+av0)-kvfh*Shp-kchp*... 
 ahp)/(D*(cos(avp+av0))^2+E*(sin(avp+av0))^2+F)-... 
 Ts*kvfh*km*wvp*cos(avp+av0)/(D*(cos(avp+av0))^2+... 
 E*(sin(avp+av0))^2+F)^2; 
 else 
 whn=whp+Ts*(TC*(k2*Uhp-TC*whp)/Rah-Btr*whp+kthn*whp^2)/Jtr; 
 Shn=Shp+Ts*(-lts*kfhn*whp^2*cos(avp+av0)-kvfh*Shp-kchp*... 
 ahp)/(D*(cos(avp+av0))^2+E*(sin(avp+av0))^2+F)-... 
 Ts*kvfh*km*wvp*cos(avp+av0)/(D*(cos(avp+av0))^2+... 
 E*(sin(avp+av0))^2+F)^2; 
 end 
if wvp>=0 
 wvn=wvp+Ts*(TC*(k1*Uvp-TC*wvp)/Rav-Bmr*wvp-ktvp*wvp^2)/Jmr; 
 Svn=Svp+Ts*(lm*kfvp*wvp^2-kvfv*Svp-kvfv*kt*whp/Jv+... 
 g*((A-B)*cos(avp+av0)-C*sin(avp+av0)))/Jv; 
else 
 wvn=wvp+Ts*(TC*(k1*Uvp-TC*wvp)/Rav-Bmr*wvp+ktvn*wvp^2)/Jmr; 
 Svn=Svp+Ts*(-lm*kfvp*wvp^2-kvfv*Svp-kvfv*kt*whp/Jv+... 
 g*((A-B)*cos(avp+av0)-C*sin(avp+av0)))/Jv; 
end 
ahn=ahp+Ts*Shp+Ts*km*wvp*cos(avp+av0)/(D*(cos(avp+av0))^2+... 
 E*(sin(avp+av0))^2+F); 
avn=avp+Ts*Svp+Ts*kt*whp/Jv; 
Ksi(:,i)=[whn;Shn;ahn;wvn;Svn;avn]+Ksip(nx+1:2*nx,i); % Xi(k|k-1) 
end 
% Trong so cac diem sigma duoc su dung cau truc trang thai du bao va hiep 
% phuong sai 
132 
Ws0=lam/(2*nx+lam); % Trong so cua trang thai du bao 
Wc0=lam/(2*nx+lam)+(1-a1^2+bet); % Trong so cua hiep phuong sai duoc DB 
Wi=1/(2*(2*nx+lam)); % Trong so cua ca hai 
Xk=Ws0*Ksi(:,1)+Wi*sum(Ksi(:,2:4*nx+1),2); % x(k|k-1) 
Pk=zeros(nx,nx); 
for i=1:4*nx 
 Pk=Pk+Wi*((Ksi(:,i+1)-Xk)*(Ksi(:,i+1)-Xk)'); 
end 
Pk=Pk+Wc0*(Ksi(:,1)-Xk)*(Ksi(:,1)-Xk)'; 
% Giai doan Cap nhat cua EKF 
lam=a1^2*(nx+ny+kai)-nx-ny; % Tham so lamda cua UKF 
% Trang thai tang (trang thai duoc bo sung) va hiep phuong sai 
Xka=zeros(nx+ny,1); % xa(k|k-1) 
Xka(1:nx,1)=Xk; 
Xka(nx+1:nx+ny)=Ev; 
Pka=zeros(nx+ny,nx+ny); % Pa(k|k-1) 
Pka(1:nx,1:nx)=Pk; 
Pka(1+nx:nx+ny,1+nx:nx+ny)=R; 
% Dang 4*nx+1 diem sigma 
Ksiy=zeros(nx+ny,2*(nx+ny)+1); % Xi(k|k-1) 
Ksiy(:,1)=Xka; 
% Tim can bac hai cua ma tran Pkpa bang viec cheo hoa gia tri rieng va vec to 
rieng 
VPk=zeros(nx,nx); 
DPk=zeros(nx,nx); 
[VPk,DPk]=myeig(Pk); 
DPksqrt=sqrt(abs(DPk)); 
YPk=VPk*DPksqrt*inv(VPk); 
YPka=[YPk zeros(nx,ny);zeros(ny,nx) sqrt(R)]; 
133 
%======================================================
============ 
Pkasqr=sqrt(nx+ny+lam)*YPka; 
for i=1:(nx+ny) 
 Ksiy(:,i+1)=Xka+Pkasqr(:,i); 
 Ksiy(:,i+nx+ny+1)=Xka-Pkasqr(:,i); 
end 
Cs=[0 0 1 0 0 0;0 0 0 0 0 1]; 
gam=zeros(ny,2*(nx+ny)+1); % Gama(k) 
for i=1:2*(nx+ny)+1 
 gam(:,i)=Cs*Ksiy(1:nx,i)+Ksiy(nx+1:nx+ny,i); 
end 
% Trong so cac diem sigma duoc su dung cau truc trang thai du bao va hiep 
phuong sai 
Ws0=lam/(nx+ny+lam); % Trong so cua trang thai du bao 
Wc0=lam/(nx+ny+lam)+(1-a1^2+bet);% Trong so cua hiep phuong sai duoc 
du bao 
Wi=1/(2*(nx+ny+lam)); % Trong so cua ca hai 
Yh=Ws0*gam(:,1)+Wi*sum(gam(:,2:2*(nx+ny)+1),2); % yhat(k) 
Pzz=zeros(ny,ny); 
Pxz=zeros(nx,ny); 
for i=1:2*(nx+ny) 
 Pzz=Pzz+Wi*(gam(:,i+1)-yh)*(gam(:,i+1)-yh)'; 
 Pxz=Pxz+Wi*(Ksiy(1:nx,i+1)-Xk)*(gam(:,i+1)-yh)'; 
end 
Pzz=Pzz+Wc0*(gam(:,1)-yh)*(gam(:,1)-yh)'; 
Pxz=Pxz+Wc0*(Ksiy(1:nx,1)-Xk)*(gam(:,1)-yh)'; 
Lk=Pxz*inv(Pzz); 
Xkn=Xk+Lk*([ah;av]-yh); 
Pkn=Pk-Lk*Pzz*Lk'; 
134 
% Thay doi ma tran duong cheo Pkn ben trong dang vector (Pknv) 
Pknv=zeros(nx*nx,1); 
for i=1:nx 
 Pknv([1:nx]+(i-1)*nx,1)=Pkn(:,i); 
end 
OUTU=[Xkn;Pknv]; 
function [V,D]=myeig(A) 
L=length(A); 
Q=zeros(L,L); 
V=eye(V,V); 
for i=1:L^3 
 [Q,R]=QRdec(A); 
 A=R*Q; 
 V=V*Q; 
end 
D=zeros(L,L); 
for i=1:L 
 D(i,i)=A(i,i); 
end 
function [Q,R]=QRdec(A) 
L=length(A); 
U=zeros(L,L); 
Q=zeros(L,L); 
if mynorm(A(:,1))~=0 
 Q(:,1)=A(:,1)/mynorm(A(:,1)); 
end 
for i=1:L 
 for j=1:i-1 
135 
 if mynorm(Q(:,j))~=0 
 U(:,i)=U(:,i)-Q(:,j)'*A(:,i)/(Q(:,j)'*Q(:,j))*Q(:,j); 
 end 
 end 
 U(:,i)=U(:,i)+A(:,i); 
 if mynorm(U(:,i))~=0 
 Q(:,i)=U(:,i)/mynorm(U(:,i)); 
 end 
end 
R=Q'*A; 
function [Y]=mynorm(X) 
Y=sqrt(sum(X.^2)); 
#define S_FUNCTION_NAME TRMS_Estimation_sf 
#define S_FUNCTION_LEVEL 2 
#include "simstruc.h" 
#include "math.h" 
#include "codegen\lib\mdlKalman\mdlKalman.h" 
static void mdlInitializeSizes(SimStruct *S) 
{ssSetNumSFcnParams(S, 0); 
 if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { 
 return; /* Parameter mismatch will be reported by Simulink */} 
 ssSetNumContStates(S, 0); 
 ssSetNumDiscStates(S, 6); 
 if (!ssSetNumInputPorts(S, 4)) return; 
 ssSetInputPortWidth(S, 0, 1); 
 ssSetInputPortWidth(S, 1, 1); 
 ssSetInputPortWidth(S, 2, 1); 
 ssSetInputPortWidth(S, 3, 1); 
136 
 ssSetInputPortSampleTime(S, 0,CONTINUOUS_SAMPLE_TIME); 
 ssSetInputPortSampleTime(S, 1,CONTINUOUS_SAMPLE_TIME); 
 ssSetInputPortSampleTime(S, 2,CONTINUOUS_SAMPLE_TIME); 
 ssSetInputPortSampleTime(S, 3,CONTINUOUS_SAMPLE_TIME); 
 ssSetInputPortOffsetTime(S, 0, 0.0); 
 ssSetInputPortOffsetTime(S, 1, 0.0); 
 ssSetInputPortOffsetTime(S, 2, 0.0); 
 ssSetInputPortOffsetTime(S, 3, 0.0); 
 ssSetInputPortDirectFeedThrough(S, 0,0); 
 ssSetInputPortDirectFeedThrough(S, 1,0); 
 ssSetInputPortDirectFeedThrough(S, 2,0); 
 ssSetInputPortDirectFeedThrough(S, 3,0); 
 if (!ssSetNumOutputPorts(S,1)) return; 
 ssSetOutputPortWidth(S, 0, 6); 
 ssSetOutputPortSampleTime(S, 0,CONTINUOUS_SAMPLE_TIME); 
 ssSetOutputPortOffsetTime(S, 0,0.0); 
 ssSetNumSampleTimes(S, 2); 
 ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 
 ssSetOptions(S, 
SS_OPTION_EXCEPTION_FREE_CODE|SS_OPTION_PORT_SAMPLE_
TIMES_ASSIGNED); 
static void mdlInitializeSampleTimes(SimStruct *S) 
{ ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME); 
 ssSetOffsetTime(S, 0, 0.0); 
 ssSetSampleTime(S, 1,0.2); 
 ssSetOffsetTime(S, 1, 0.0); 
 ssSetModelReferenceSampleTimeDefaultInheritance(S); } 
#define MDL_INITIALIZE_CONDITIONS 
static void mdlInitializeConditions(SimStruct *S) 
{ int i; 
137 
 real_T *xdisc = ssGetRealDiscStates(S); 
 for (i = 0; i < 6; i++) { *xdisc++ = 0.0; } } 
#define MDL_UPDATE 
void mdlUpdate(SimStruct *S, int_T tid) 
{ real_T *Xe = ssGetRealDiscStates(S); 
 InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,0); 
 InputRealPtrsType uPtrs2 = ssGetInputPortRealSignalPtrs(S,1); 
 InputRealPtrsType uPtrs3 = ssGetInputPortRealSignalPtrs(S,2); 
 InputRealPtrsType uPtrs4 = ssGetInputPortRealSignalPtrs(S,3); 
if (ssIsSampleHit(S, 1, tid)) 
mdlKalman((*uPtrs1[0]),(*uPtrs2[0]),(*uPtrs3[0]),(*uPtrs4[0]),Xe);} 
static void mdlOutputs(SimStruct *S, int_T tid) 
{ int i; 
 real_T *Out = ssGetOutputPortRealSignal(S,0); 
 real_T *Xe = ssGetRealDiscStates(S); 
 for ( i=0;i<6;i++) 
 { 
 *Out++= *Xe++; } 
static void mdlTerminate(SimStruct *S)