Luận án Nghiên cứu hệ điều khiển truyền động điện phi tuyến chứa nhiều động cơ có liên hệ ràng buộc ứng dụng trong điều khiển quadror

NGHIÊN CỨU HỆ ĐIỀU KHIỂN TRUYỀN ĐỘNG ĐIỆN PHI 
TUYẾN CHỨA NHIỀU ĐỘNG CƠ CÓ LIÊN HỆ RÀNG 
BUỘC ỨNG DỤNG TRONG ĐIỀU KHIỂN QUADROR 
LUẬN ÁN TIẾN SĨ KỸ THUẬT 
HÀ NỘI - 2018 
BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG 
HỌC VIỆN KỸ THUẬT QUÂN SỰ 
NGHIÊN CỨU HỆ ĐIỀU KHIỂN TRUYỀN ĐỘNG ĐIỆN PHI 
TUYẾN CHỨA NHIỀU ĐỘNG CƠ CÓ LIÊN HỆ RÀNG BUỘC 
ỨNG DỤNG TRONG ĐIỀU KHIỂN QUADROR 
Chuyên ngành: Kỹ thuật điều khiển và tự động hóa 
Mã số: 9.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 HOÀNG QUANG CHÍNH 
HÀ NỘI - 2018 
BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG 
HỌC VIỆN KỸ THUẬT QUÂN SỰ 
LỜI CAM ĐOAN 
Tôi Hoàng Văn Huy cam đoan luận án là công trình nghiên cứu của bản 
thân tôi dưới sự hướng dẫn của PGS. TS Đào Hoa Việt và TS Hoàng Quang 
Chính. Các số liệu, kết quả nêu trong luận án là trung thực và chưa từng được 
công bố trong bất kỳ công trình nào khác.  
  Nghiên cứu sinh 
Hoàng Văn Huy 
LỜI CẢM ƠN 
Trước  hết  tôi  xin  bày  tỏ  lòng  kính  trọng  và  biết  ơn  sâu  sắc  nhất  tới: 
PGS. TS Đào Hoa Việt và TS Hoàng Quang Chính là hai người thầy đã trực 
tiếp hướng dẫn và chỉ bảo tận tình, giúp đỡ tôi trong suốt quá trình học tập, 
nghiên cứu và thực hiện luận án. 
Tôi gửi lời cảm ơn chân thành tới các thầy giáo, cô giáo của Bộ môn 
Cơ điện tử và Robot đặc biệt, Khoa Hàng không Vũ trụ, Học viện Kỹ thuật 
Quân sự đã chia sẻ, giúp đỡ, động viên và tạo mọi điều kiện tốt nhất để tôi có 
thể hoàn thành luận án này. 
Tôi xin trân trọng cảm ơn Ban giám hiệu, các anh chị em đồng nghiệp 
của phòng Đào  tạo Trường Cao đẳng Công nghiệp Thực phẩm đã cử  tôi đi 
học và tạo mọi điều kiện thuận lợi trong suốt quá trình học tập và thực hiện 
luận án.  
Cuối cùng, tôi vô cùng cảm ơn bố mẹ, vợ con, người  thân và bạn bè, 
những người đã luôn bên tôi chia sẻ, động viên và giúp đỡ tôi trong cuộc sống 
và học tập để hoàn thành luận án này. 
  Hà Nội, ngày tháng năm 2018 
Nghiên cứu sinh 
Hoàng Văn Huy 
MỤC LỤC 
Trang 
MỞ ĐẦU ............................................................................................................ 3 
Chương 1: TỔNG QUAN CÁC HỆ THỐNG ĐIỀU KHIỂN QUADROTOR 
VÀ HỆ TRUYỀN ĐỘNG ĐIỆN CHỨA NHIỀU ĐỘNG CƠ CÓ LIÊN HỆ 
RÀNG BUỘC VỚI NHAU ................................................................................ 8 
1.1. Tổng quan về hệ  truyền động điện chứa nhiều động cơ có  liên hệ  ràng 
buộc với nhau ..................................................................................................... 8 
1.1.1. Sơ đồ chức năng của các hệ thống truyền động điện có liên hệ ràng 
buộc song song theo tác động nhiễu ................................................................ 10 
1.1.2. Sơ đồ  chức năng  của  các  hệ  thống    truyền  động điện  có  liên hệ 
ràng buộc theo nhiễu cơ bản ............................................................................ 11 
1.1.3. Sơ đồ  chức năng  của  các  hệ  thống    truyền  động điện  có  liên hệ 
ràng buộc với nhau theo vị trí của đối tượng điều khiển ................................. 11 
1.2. Sơ đồ khối chức năng hệ thống điều khiển quadrotor .............................. 12 
1.2.1. Một vài vấn đề về đặc điểm điều khiển chuyển động quadrotor ... 12 
1.2.2. Sơ đồ khối hệ thống điều khiển quadrotor ..................................... 15 
1.2.3. Sơ đồ khối mô tả đối tượng điều khiển quadrotor ......................... 17 
1.3. Đánh giá chung về các phương pháp điều khiển quadrotor ...................... 19 
1.3.1. Phương pháp điều khiển PID ......................................................... 20 
1.3.2. Điều khiển tối ưu toàn phương/Gausian - LQR/G ......................... 21 
1.3.3. Điều khiển chế độ trượt (SMC) ...................................................... 22 
1.3.4. Điều khiển Backstepping ............................................................... 22 
1.3.5. Thuật toán điều khiển thích nghi .................................................... 23 
1.3.6. Thuật toán điều khiển bền vững ..................................................... 24 
1.3.7. Thuật toán điều khiển tối ưu .......................................................... 24 
1.3.8. Tuyến tính hóa phản hồi ................................................................. 25 
1.3.9. Điều khiển thông minh (logic mờ và mạng nơron nhân tạo) ......... 26 
1.4. Tổng quan các công trình nghiên cứu trong và ngoài nước ...................... 27 
1.4.1. Các công trình nghiên cứu trong nước ........................................... 27 
1.4.2. Các công trình nghiên cứu của nước ngoài .................................... 28 
1.5. Đặt bài toán nghiên cứu ............................................................................ 34 
1.6. Kết luận chương 1 ..................................................................................... 35 
Chương 2. XÂY DỰNG MÔ HÌNH ĐỐI TƯỢNG ĐIỀU KHIỂN VÀ CẤU 
TRÚC TỔNG QUÁT CỦA HỆ ĐIỀU KHIỂN QUADROTOR KHI KỂ ĐẾN 
ĐỘNG LỰC HỌC ĐỘNG CƠ CHẤP HÀNH ................................................ 37 
2.1. Mô hình động lực học quadrotor ............................................................... 37 
2.1.1. Các giả thiết khi xây dựng mô hình động lực học quadrotor ......... 37 
2.1.2. Xây dựng mô hình động lực học quadrotor ................................... 38 
2.2. Mô hình hệ truyền động chấp hành quay cánh quạt ................................. 44 
2.2.1. Mô hình phần cơ ............................................................................. 44 
2.2.2. Mô hình toán hệ truyền động điện động cơ đồng bộ nam châm vĩnh 
cửu (PMSM) ..................................................................................................... 47 
2.3. Mô hình đối tượng điều khiển của hệ điều khiển quadrotor ..................... 49 
2.4. Xây dựng sơ đồ khối hệ điều khiển quadrotor khi kể đến động học động 
cơ chấp hành ..................................................................................................... 53 
2.5. Đánh giá và nhận xét: ................................................................................ 56 
2.6. Kết luận chương 2 ..................................................................................... 57 
Chương 3: TỔNG HỢP BỘ ĐIỀU KHIỂN ĐỘNG CƠ ĐIỆN ĐỒNG BỘ NAM 
CHÂM VĨNH CỬU QUAY CÁNH QUẠT BẰNG PHƯƠNG PHÁP TRƯỢT 
THÍCH  NGHI  ỨNG  DỤNG  TRONG  HỆ  THỐNG ĐIỀU  KHIỂN 
QUADROTOR ................................................................................................. 59 
3.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 chỉnh tốc độ động cơ PMSM. ................................................... 59 
3.2. Mô phỏng hệ điều khiển tốc độ động cơ điện đồng bộ nam châm vĩnh cửu 
theo phương pháp trượt thích nghi ................................................................... 70 
3.3. Kết luận chương 3 ..................................................................................... 76 
Chương 4: TỔNG HỢP CÁC BỘ ĐIỀU KHIỂN GÓC, ĐIỀU KHIỂN TỐC 
ĐỘ DÀI  VÀ VỊ TRÍ CỦA QUADROTOR KHI KỂ ĐẾN ĐỘNG HỌC CỦA 
ĐỘNG CƠ CHẤP HÀNH ............................................................................... 77 
4.1. Tổng hợp bộ điều khiển và ổn định trạng thái các góc Euler bằng phương 
pháp tuyến tính hóa phản hồi ........................................................................... 77 
4.2. Tổng hợp bộ điều khiển tốc độ dài C2 của quadrotor ............................... 90 
4.3. Tổng hợp bộ điều khiển vị trí C3 ............................................................... 94 
4.4. Tính toán tham số các bộ điều khiển khiển quadrotor khi kể đến động lực 
học động cơ chấp hành ..................................... Error! Bookmark not defined. 
4.4.1. Bảng thông số của quadrotor sử dụng trong tính toán các tham số 
bộ điều khiển và nghiên cứu mô phỏng ........... Error! Bookmark not defined. 
4.4.2. Tính toán các thông số bộ điều khiển ổn định các góc Euler C1Error! Bookmark not defined.
4.4.3. Tính toán các thông số bộ điều khiển tốc độ dài C2 ...................... 99 
4.4.4. Tính toán các thông số bộ điều khiển vị trí .................................... 99 
4.5. Mô phỏng hệ thống điều khiển quadrotor khi sử dụng động cơ điện đồng 
bộ nam châm vĩnh cửu ................................................................................... 104 
4.6. Khảo sát đánh giá trên mô hình thực nghiệm với động cơ điện đồng bộ 
nam châm vĩnh cửu ........................................................................................ 131 
4.6.1. Xây dựng mô hình thực nghiệm ................................................... 131 
4.6.2. Các kết quả thực nghiệm .............................................................. 144 
4.7. Kết luận chương 4 ................................................................................... 147 
KẾT LUẬN CHUNG ..................................................................................... 149 
DANH MỤC CÔNG TRÌNH CỦA TÁC GIẢ .............................................. 152 
TÀI LIỆU THAM KHẢO .............................................................................. 153 
Phụ lục 1: Các sơ đồ chức năng và mô phỏng hệ điều khiển quadrotor ........ 163 
Phụ lục 2: Tham số của động cơ điện đồng bộ nam châm vĩnh cửu ............. 168 
Phụ lục 3: Chương trình lập trình code trên S_funtion .................................. 170 
Phụ lục 4: Chương trình lập trình 3D S_funtion ............................................ 172 
DANH MỤC CÁC CHỮ VIẾT TẮT VÀ CÁC KÝ HIỆU 
1. Chữ viết tắt Ý nghĩa 
ADC  Analog to Digital Converter: Bộ biến đổi tương tự số 
BĐK  Bộ điều khiển 
ĐTĐK  Đối tượng điều khiển 
DOF  Degree Of Freedom: Bậc tự do 
DC  Direct Current: Dòng điện một chiều 
DSP  Digital Signal Processor: Bộ xử lý tín hiệu số 
CLF  Hàm điều khiển Lyapunov (Control Lyapunov Function) 
FBL  Feedback Linearization: Tuyến tính hóa phản hồi 
FLC  Fuzzy Logic Controller: Bộ điều khiển Logic mờ 
GPS  Global Positioning System: Hệ thống định vị toàn cầu 
IR  InfraRed: Hồng ngoại 
IMU  Inertial Measurement Unit: Đơn vị đo lường quán tính 
LQR/G Linear  Quadratic  Regulator/Gausian:  Điều  khiển  tối  ưu  toàn 
phương/Gausian 
LQE  Linear Quadratic Estimator: Ước lượng tuyến tính toàn phương  
MCU  Micro Controller Unit: Bộ vi điều khiển 
MIMO  Multi - Input Multi - Out put: Nhiều đầu vào, nhiều đầu ra 
PD  Bộ điều khiển tỷ lệ, vi phân 
PID  Bộ điều khiển tỷ lệ,tích phân và vi phân 
PMSM  Permanent  Magnet  Synchronous  Motor:  Động  cơ  đồng  bộ  nam 
châm vĩnh cửu 
ROM  Read Only Memory: Bộ nhớ đọc 
RC  Remote Controller: Điều khiển từ xa 
RF  Radio Frequency: Tần số vô tuyến 
SONAR  SOund Navigation And Ranging 
SMC Sliding Mode Control: Điều khiển chế độ trượt 
SDRE  State – Dependent Riccati Equation: Phương  trình  trạng  thái phụ 
thuộc Riccati  
VSC  Variable Structure Control: Bộ điều khiển có cấu trúc biến đổi 
HTTĐĐ  Hệ thống truyền động điện 
2. Ký hiệu Ý nghĩa 
i
   Tốc độ của các cánh quạt  ].[ 1 srad  
i
F   Các lực được tạo ra từ các cánh quạt của quadrotor  ][N  
F1 Lực đẩy của cánh quạt trước của quadrotor  ][N  
F2 Lực đẩy của cánh quạt phải của quadrotor  ][N  
F3 Lực đẩy của cánh quạt sau của quadrotor  ][N  
F4 Lực đẩy của cánh quạt trái của quadrotor  ][N  
Fn Tổng lực cản của các loại nhiễu  ][N  
i
M   Mô men tạo ra từ các cánh quạt  ][ mN  
M Tổng mô men của các góc roll, pitch, yaw [N m] 
M   Mô men của góc roll  ][ mN  
M   Mô men của góc pitch  ][ mN  
M   Mô men của góc yaw  ][ mN  
R   Ma trận quay (roll-pitch-yaw) 
eM   Mô men động cơ  ][ mN  
c
M   Mô men cản của phụ tải  ][ mN  
o
M   Mô men ma sát  ][ mN  
PM
M   Mô men của cánh quạt trên trục động cơ  ][ mN  
P
M   Mô men của cánh quạt  ][ mN  
MP
M   Mô men của động cơ trên trục cánh quạt  ][ mN  
Mn Mô men cản của nhiễu  ][ mN  
r Bán kính của cánh quạt [m] 
b Hệ số lực đẩy [N.s2/rad2] 
d Hệ số lực cản [N.s2/rad2] 
B Hệ số ma sát nhớt 
g Gia tốc trọng trường [m s-2] 
l Khoảng cách từ tâm quadrotor đến tâm của động cơ [m]  
1
U   Lực đẩy thẳng tác động vào hệ tọa độ B gắn với quadrotor  ][N  
2
U   Mô men góc roll tác động vào hệ tọa độ B gắn với quadrotor  
3
U   Mô men góc pitch tác động vào hệ tọa độ B gắn với quadrotor  
4
U   Mô men góc yaw tác động vào hệ tọa độ B gắn với quadrotor  
X   Vị trí chuyển động của khối tâm quadrotor dọc theo trục  Ex  trong 
hệ tọa độ E gắn với trái đất  ][m  
X   Tốc  độ  chuyển  động  của  khối  tâm  quadrotor  dọc  theo  trục  Ex  
trong hệ tọa độ E gắn với trái đất  ][ 1 sm  
X   Gia  tốc  chuyển  động  của  khối  tâm  quadrotor  dọc  theo  trục  Ex  
trong hệ tọa độ E gắn với trái đất  ][ 2 sm  
Y   Vị trí chuyển động của khối tâm quadrotor dọc theo trục  Ey  trong 
hệ tọa độ E gắn với trái đất  ][m  
Y   Tốc  độ  chuyển  động  của  khối  tâm  quadrotor  dọc  theo  trục  Ey  
trong hệ tọa độ E gắn với trái đất  ][ 1 sm  
Y   Gia  tốc  chuyển  động  của  khối  tâm  quadrotor  dọc  theo  trục  Ey  
trong hệ tọa độ E gắn với trái đất  ][ 2 sm  
Z   Vị trí chuyển động của khối tâm quadrotor dọc theo trục  Ez  trong 
hệ tọa độ E gắn với trái đất  ][m  
Z   Tốc  độ  chuyển  động  của  khối  tâm  quadrotor  dọc  theo  trục  Ez  
trong hệ tọa độ E gắn với trái đất  ][ 1 sm  
Z   Gia  tốc  chuyển  động  của  khối  tâm  quadrotor  dọc  theo  trục  Ez  
trong hệ tọa độ B gắn với trái đất  ][ 2 sm  
   Vị  trí góc  (pitch) của quadrotor quanh  trục  By   của hệ  tọa B gắn 
với quadrotor  ][rad  
   Tốc độ góc (pitch) của quadrotor quanh trục  By  của hệ tọa B gắn 
với quadrotor  ][ 1 srad  
   Gia tốc góc (pitch) của quadrotor quanh trục  By  của hệ tọa B gắn 
với quadrotor  ][ 2 srad  
   Vị trí góc (roll) của quadrotor quanh trục 
B
x  của hệ tọa B gắn với 
quadrotor  ][rad  
   Tốc độ góc  (roll)  của quadrotor quanh  trục  Bx   của hệ  tọa B gắn 
với quadrotor  ][ 1 srad  
   Gia  tốc góc  (roll) của quadrotor quanh  trục  Bx   của hệ  tọa B gắn 
với quadrotor  ][ 2 srad  
   Vị trí góc (yaw) của quadrotor quanh trục 
B
z  của hệ tọa B gắn với 
quadrotor  ][rad  
   Tốc độ góc (yaw) của quadrotor quanh trục 
B
z  của hệ tọa B gắn 
với quadrotor  ][ 1 srad  
   Gia  tốc góc (yaw) của quadrotor quanh trục  Bz  của hệ tọa B gắn 
với quadrotor  ][ 2 srad  
r
   Tốc độ động cơ  ][ 1 srad  
r
   Gia tốc của động cơ  ][ 2 srad  
EΓ   Vector vị trí chuyển động của khối tâm quadrotor trong hệ tọa độ 
E gắn với trái đất  ][m  
EΓ   Vector tốc độ chuyển động của khối tâm quadrotor trong hệ tọa độ 
E gắn với trái đất  ][ 1 sm  
EΓ   Vector  gia  tốc  độ  chuyển  động  của  khối  tâm  quadrotor  trong  hệ 
tọa độ E gắn với trái đất  2[ ]ms  
EΘ   Vector vị trí góc của quadrotor trong hệ tọa độ E gắn với trái đất 
][rad  
EΘ   Vector tốc độ góc của quadrotor trong hệ tọa độ E gắn với trái đất 
][ 1 srad  
EΘ   Vector gia tốc ... 5 
B = B_motor + B_Brake; % Equivalent Viscous Friction Coefficient 
lamd = 0.193; % Magnetic Flux (Electrical) [Vsec/rad] 
kpt = 0.001; 
Vdc = 310; % DC-Link Voltage (V) 
fz = 5e3; % PWM Frequency (Hz) 
Tz = 1/fz; % PWM Period (Seconds) 
Vmax = 2/3*Vdc; 
Vmin = -Vmax; 
%kt = (3*P*Lambdam)/4; % Torque constant (Te = kt*iq) 
RPM = 1000;%1500; % Speed Reference RPM, Rated speed = 1850 RPM 
Tstop = 1.01; % Simulation stop time 
Tload = 0.5 ; % Load torque [N.m], Rated torque = 3.874 [N.m] 
kk1=3*P*P/2/J/4*lamd; 
kk2=B/J; 
k3=P/2/J; 
k4=r/Lq; 
k5=lamd/Ls; 
k6=1/Lq; 
k7 = r/Ld; 
k8 = 1/Ld; 
k9 = Lq/Ld; 
k10 = 1/k9; 
k11 = 3*P*P*(Ld-Lq)/8/J; 
k12 = 4/3/P/lamd; 
k13 = 2*k12*J/P; 
k14 = 2*k12*B/P; 
k15 = -(Lq-Ld)/lamd; 
k16 = (Ld-Lq)/Ld; 
%Controller 
k1 = 1250; 
k = 0.001; 
169  
k2 = 0.00002; 
kd = 50; 
ksd = 1e4; 
nuyd = 270; 
ald = 0.01; 
kq = 30; 
ksq = 1e2; 
nuyq = 130; 
alq = 0.01; 
gm1 = 0.067; 
gm2 = 0.01; 
gm3 = 0.01; 
gm1 = 0; 
gm2 = 0; 
gm3 = 0; 
kpt=0.0012 
K1=3*P*P/2/J/4*lamd; 
k2=B/J; 
M0=0.001 
k11 = 3*P*P*(Ld-Lq)/8/J; 
%z0=1; l2=200;l1=(k2+l2)*(k2+l2)/4/k3/z0/z0; 
L1=-10e3;%-9.5 cho toc do cao, -10 cho toc do thap 
L2 = 51e2; 
epsa=0.01; epsb=epsa; epsw=epsa;eps0=0.01; 
170  
Phụ lục 3: Chương trình lập trình code trên S_funtion 
function [sys,x0,str,ts] = svab_Huy(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 
function [sys,x0,str,ts] = mdlInitializeSizes 
sizes = simsizes; 
sizes.NumContStates = 0; 
sizes.NumDiscStates = 0; % Khong co bien trang thai gian doan 
sizes.NumOutputs = 4; % 
sizes.NumInputs = 2;% vsq,vsd 
sizes.DirFeedthrough = 1;% 
sizes.NumSampleTimes = 1; 
sys = simsizes(sizes); 
x0 = [];% Khoi tao bien trang thai 
str = []; 
ts = [-1 0]; 
function sys = mdlOutputs(t,x,u) 
Vdc = 310; 
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; 
 else 
171  
 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 
end 
ta = vr*ts/va; 
tb = vl*ts/vb; 
t0 = ts-ta-tb; 
%overmodulation 
if t0 <0 
 ta = ta*ts/(tb+ta); 
 tb = tb*ts/(tb+ta); 
 t0 = 0; 
end 
y = [ta;tb;t0;s]; 
sys = y; 
172  
Phụ lục 4: Chương trình lập trình 3D S_funtion 
function [sys, x0, str, ts] = sfunxyz(t,x,u,flag,ax,varargin) 
%SFUNXY S-function that acts as an X-Y-Z scope using MATLAB plotting 
functions. 
% This M-file is designed to be used in a Simulink S-function block. 
% It draws a line from the previous input point, which is stored using 
% discrete states, and the current point. It then stores the current 
% point for use in the next invocation. 
% 
% See also SFUNXYS, LORENZS. 
% Copyright 1990-2002 The MathWorks, Inc. 
% $Revision: 1.38 $ 
% Andrew Grace 5-30-91. 
% Revised Wes Wang 4-28-93, 8-17-93, 12-15-93 
% Revised Craig Santos 10-28-96 
% Modified by Giampiero Campa, April 04 
switch flag 
 %%%%%%%%%%%%%%%%%% 
 % Initialization % 
 %%%%%%%%%%%%%%%%%% 
 case 0 
 [sys,x0,str,ts] = mdlInitializeSizes(ax,varargin{:}); 
 warning off; 
 SetBlockCallbacks(gcbh); 
 warning on; 
 %%%%%%%%%% 
 % Update % 
 %%%%%%%%%% 
 case 2 
 sys = mdlUpdate(t,x,u,flag,ax,varargin{:}); 
 %%%%%%%%% 
 % Start % 
 %%%%%%%%% 
 case 'Start' 
 LocalBlockStartFcn 
 %%%%%%%% 
 % Stop % 
 %%%%%%%% 
 case 'Stop' 
 LocalBlockStopFcn 
 %%%%%%%%%%%%%% 
 % NameChange % 
173  
 %%%%%%%%%%%%%% 
 case 'NameChange' 
 LocalBlockNameChangeFcn 
 %%%%%%%%%%%%%%%%%%%%%%%% 
 % CopyBlock, LoadBlock % 
 %%%%%%%%%%%%%%%%%%%%%%%% 
 case { 'CopyBlock', 'LoadBlock' } 
 LocalBlockLoadCopyFcn 
 %%%%%%%%%%%%%%% 
 % DeleteBlock % 
 %%%%%%%%%%%%%%% 
 case 'DeleteBlock' 
 LocalBlockDeleteFcn 
 %%%%%%%%%%%%%%%% 
 % DeleteFigure % 
 %%%%%%%%%%%%%%%% 
 case 'DeleteFigure' 
 LocalFigureDeleteFcn 
 %%%%%%%%%%%%%%%% 
 % Unused flags % 
 %%%%%%%%%%%%%%%% 
 case { 3, 9 } 
 sys = []; 
 %%%%%%%%%%%%%%%%%%%% 
 % Unexpected flags % 
 %%%%%%%%%%%%%%%%%%%% 
 otherwise 
 if ischar(flag), 
 errmsg=sprintf('Unhandled flag: ''%s''', flag); 
 else 
 errmsg=sprintf('Unhandled flag: %d', flag); 
 end 
 error(errmsg); 
end 
% end sfunxy 
% 
%======================================================================== 
% mdlInitializeSizes 
% Return the sizes, initial conditions, and sample times for the S-
function. 
%======================================================================== 
% 
174  
function [sys,x0,str,ts] = mdlInitializeSizes(ax,varargin) 
if length (ax)~=6 
 error(['Axes limits must be defined.']) 
end 
sizes = simsizes; 
sizes.NumContStates = 0; 
sizes.NumDiscStates = 0; 
sizes.NumOutputs = 0; 
sizes.NumInputs = 3*fix(varargin{2}); 
sizes.DirFeedthrough = 0; 
sizes.NumSampleTimes = 1; 
sys = simsizes(sizes); 
x0 = []; 
str = []; 
% 
% initialize the array of sample times, note that in earlier 
% versions of this scope, a sample time was not one of the input 
% arguments, the varargs checks for this and if not present, assigns 
% the sample time to -1 (inherited) 
% 
ts = [varargin{1} 0]; 
% end mdlInitializeSizes 
% 
%======================================================================== 
% mdlUpdate 
% Handle discrete state updates, sample time hits, and major time step 
% requirements. 
%======================================================================== 
% 
function sys=mdlUpdate(t,x,u,flag,ax,varargin) 
% 
% always return empty, there are no states... 
% 
sys = []; 
% 
% Locate the figure window associated with this block. If it's not a 
valid 
% handle (it may have been closed by the user), then return. 
% 
FigHandle=GetSfunXYFigure(gcbh); 
if ~ishandle(FigHandle), 
 return 
end 
175  
nmax=fix(varargin{2}); 
CameraPosition=varargin{3}; 
if varargin{4}, GdSwitch='On'; else GdSwitch='Off'; end 
ud = get(FigHandle,'UserData'); 
if isempty(ud.XData), 
 x_data = [u(1:nmax) u(1:nmax)]; 
 y_data = [u(1*nmax+1:2*nmax) u(1*nmax+1:2*nmax)]; 
 z_data = [u(2*nmax+1:3*nmax) u(2*nmax+1:3*nmax)]; 
else 
 x_data = [ud.XData(:,end) u(1:nmax)]; 
 y_data = [ud.YData(:,end) u(nmax+1:2*nmax)]; 
 z_data = [ud.ZData(:,end) u(2*nmax+1:3*nmax)]; 
end 
% plot the input lines, (use 1+2*(i-1) instead of i to make red the 
second color) 
set(ud.XYAxes,'Visible','on','Xlim', ax(1:2),'Ylim', ax(3:4),'Zlim', 
ax(5:6),'CameraPosition',CameraPosition,'XGrid',GdSwitch,'YGrid',GdSwitch
,'ZGrid',GdSwitch); 
cord=get(ud.XYAxes,'ColorOrder'); 
for i=1:nmax, 
 set(ud.XYLine,'Xdata',x_data(i,:),'Ydata', y_data(i,:),'Zdata', 
z_data(i,:),'LineStyle','-','Color',cord(1+mod(i-1,6),:)); 
end 
set(ud.XYTitle,'String','X Y Z Plot'); 
set(FigHandle,'Color',get(FigHandle,'Color')); 
ud.XData = [ud.XData u(1:nmax)]; 
ud.YData = [ud.YData u(1*nmax+1:2*nmax)]; 
ud.ZData = [ud.ZData u(2*nmax+1:3*nmax)]; 
set(FigHandle,'UserData',ud); 
drawnow 
% end mdlUpdate 
% 
%======================================================================== 
% LocalBlockStartFcn 
% Function that is called when the simulation starts. Initialize the 
% XY Graph scope figure. 
%======================================================================== 
% 
function LocalBlockStartFcn 
% 
% get the figure associated with this block, create a figure if it 
doesn't 
% exist 
% 
FigHandle = GetSfunXYFigure(gcbh); 
if ~ishandle(FigHandle), 
 FigHandle = CreateSfunXYFigure; 
176  
end 
ud = get(FigHandle,'UserData'); 
set(ud.XYLine,'Erasemode','normal'); 
set(ud.XYLine,'XData',[],'YData',[],'ZData',[]); 
set(ud.XYLine,'XData',0,'YData',0,'ZData',0,'Erasemode','none'); 
ud.XData = []; 
ud.YData = []; 
ud.ZData = []; 
set(FigHandle,'UserData',ud); 
% end LocalBlockStartFcn 
% 
%======================================================================== 
% LocalBlockStopFcn 
% At the end of the simulation, set the line's X and Y data to contain 
% the complete set of points that were acquire during the simulation.% 
Recall that during the simulation, the lines are only small segments from 
% the last time step to the current one. 
%======================================================================== 
% 
function LocalBlockStopFcn 
FigHandle=GetSfunXYFigure(gcbh); 
if ishandle(FigHandle), 
 % 
 % Get UserData of the figure. 
 % 
 ud = get(FigHandle,'UserData'); 
 cord=get(ud.XYAxes,'ColorOrder'); 
 for i=1:size(ud.XYAxes,1), 
 set(ud.XYLine,'Xdata',ud.XData(i,:),'Ydata', ud.YData(i,:),'Zdata', 
ud.ZData(i,:),'LineStyle','-','Color',cord(1+mod(i-1,6),:)); 
 end 
end 
% end LocalBlockStopFcn 
% 
%======================================================================== 
% LocalBlockNameChangeFcn 
% Function that handles name changes on the Graph scope block. 
%======================================================================== 
% 
function LocalBlockNameChangeFcn 
% 
% get the figure associated with this block, if it's valid, change 
% the name of the figure 
177  
% 
FigHandle = GetSfunXYFigure(gcbh); 
if ishandle(FigHandle), 
 set(FigHandle,'Name',get_param(gcbh,'Name')); 
end 
% end LocalBlockNameChangeFcn 
% 
%======================================================================== 
% LocalBlockLoadCopyFcn 
% This is the XYGraph block's LoadFcn and CopyFcn. Initialize the 
block's 
% UserData such that a figure is not associated with the block. 
%======================================================================== 
% 
function LocalBlockLoadCopyFcn 
SetSfunXYFigure(gcbh,-1); 
% end LocalBlockLoadCopyFcn 
% 
%======================================================================== 
% LocalBlockDeleteFcn 
% This is the XY Graph block'DeleteFcn. Delete the block's figure 
window, 
% if present, upon deletion of the block. 
%========================================================================
===== 
% 
function LocalBlockDeleteFcn 
% 
% Get the figure handle associated with the block, if it exists, delete 
% the figure. 
% 
FigHandle=GetSfunXYFigure(gcbh); 
if ishandle(FigHandle), 
 delete(FigHandle); 
 SetSfunXYFigure(gcbh,-1); 
end 
% end LocalBlockDeleteFcn 
% 
%========================================================================
===== 
% LocalFigureDeleteFcn 
% This is the XY Graph figure window's DeleteFcn. The figure window is 
% being deleted, update the XY Graph block's UserData to reflect the 
change. 
178  
%======================================================================== 
% 
function LocalFigureDeleteFcn 
% 
% Get the block associated with this figure and set it's figure to -1 
% 
ud=get(gcbf,'UserData'); 
SetSfunXYFigure(ud.Block,-1) 
% end LocalFigureDeleteFcn 
% 
%======================================================================== 
% GetSfunXYFigure 
% Retrieves the figure window associated with this S-function XY Graph 
block 
% from the block's parent subsystem's UserData. 
%======================================================================== 
% 
function FigHandle=GetSfunXYFigure(block) 
if strcmp(get_param(block,'BlockType'),'S-Function'), 
 block=get_param(block,'Parent'); 
end 
FigHandle=get_param(block,'UserData'); 
if isempty(FigHandle), 
 FigHandle=-1; 
end 
% end GetSfunXYFigure 
% 
%======================================================================== 
% SetSfunXYFigure 
% Stores the figure window associated with this S-function XY Graph block 
% in the block's parent subsystem's UserData. 
%======================================================================== 
% 
function SetSfunXYFigure(block,FigHandle) 
if strcmp(get_param(bdroot,'BlockDiagramType'),'model'), 
 if strcmp(get_param(block,'BlockType'),'S-Function'), 
 block=get_param(block,'Parent'); 
 end 
 set_param(block,'UserData',FigHandle); 
end 
% end SetSfunXYFigure 
179  
% 
%======================================================================== 
% CreateSfunXYFigure 
% Creates the figure window associated with this S-function XY Graph 
block. 
%======================================================================== 
% 
function FigHandle=CreateSfunXYFigure 
% 
% the figure doesn't exist, create one 
% 
FigHandle = figure('Units', 'pixel',... 
 'Position', [100 100 400 300],... 
 'Name', get_param(gcbh,'Name'),... 
 'Tag', 'SIMULINK_XYGRAPH_FIGURE',... 
 'NumberTitle', 'off',... 
 'IntegerHandle', 'off',... 
 'Toolbar', 'none',... 
 'Menubar', 'none',... 
 'DeleteFcn', 
'sfunxyz([],[],[],''DeleteFigure'')'); 
% 
% store the block's handle in the figure's UserData 
% 
ud.Block=gcbh; 
% 
% create various objects in the figure 
% 
ud.XYAxes = axes; 
ud.XYLine = plot(0,0,'EraseMode','None'); 
ud.XYXlabel = xlabel('X Axis'); 
ud.XYYlabel = ylabel('Y Axis'); 
ud.XYZlabel = zlabel('Z Axis'); 
ud.XYTitle = get(ud.XYAxes,'Title'); 
ud.XData = []; 
ud.YData = []; 
set(ud.XYAxes,'Visible','off'); 
% 
% Associate the figure with the block, and set the figure's UserData. 
% 
SetSfunXYFigure(gcbh,FigHandle); 
set(FigHandle,'HandleVisibility','callback','UserData',ud); 
% end CreateSfunXYFigure 
% 
%======================================================================== 
% SetBlockCallbacks 
% This sets the callbacks of the block if it is not a reference. 
180  
%======================================================================== 
% 
function SetBlockCallbacks(block) 
% 
% the actual source of the block is the parent subsystem 
% 
block=get_param(block,'Parent'); 
% 
% if the block isn't linked, issue a warning, and then set the callbacks 
% for the block so that it has the proper operation 
% 
if strcmp(get_param(block,'LinkStatus'),'none'), 
 warnmsg=sprintf(['The XY Graph scope ''%s'' should be replaced with a ' 
... 
 'new version from the Simulink block library'],... 
 block); 
 warning(warnmsg); 
 callbacks={ 
 'CopyFcn', 'sfunxyz([],[],[],''CopyBlock'')' ; 
 'DeleteFcn', 'sfunxyz([],[],[],''DeleteBlock'')' ; 
 'LoadFcn', 'sfunxyz([],[],[],''LoadBlock'')' ; 
 'StartFcn', 'sfunxyz([],[],[],''Start'')' ; 
 'StopFcn' 'sfunxyz([],[],[],''Stop'')' 
 'NameChangeFcn', 'sfunxyz([],[],[],''NameChange'')' ; 
 }; 
 for i=1:length(callbacks), 
 if ~strcmp(get_param(block,callbacks{i,1}),callbacks{i,2}), 
 set_param(block,callbacks{i,1},callbacks{i,2}) 
 end 
 end 
end 
% end SetBlockCallbacks 
181  
182