Luận án Tổng hợp thuật toán điều khiển hạ cánh theo chương trình cho máy bay không người lái cỡ nhỏ

BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG 
HỌC VIỆN KỸ THUẬT QUÂN SỰ 
NGÔ VĂN TOÀN 
TỔNG HỢP THUẬT TOÁN ĐIỀU KHIỂN HẠ CÁNH THEO 
CHƯƠNG TRÌNH CHO MÁY BAY KHÔNG NGƯỜI LÁI 
CỠ NHỎ 
LUẬN ÁN TIẾN SĨ KỸ THUẬT 
HÀ NỘI - 2021
BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG 
HỌC VIỆN KỸ THUẬT QUÂN SỰ 
NGÔ VĂN TOÀN 
TỔNG HỢP THUẬT TOÁN ĐIỀU KHIỂN HẠ CÁNH 
THEO CHƯƠNG TRÌNH CHO MÁY BAY KHÔNG NGƯỜI 
LÁI CỠ NHỎ 
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. TS. Lê Thanh Phong 
2. TS. Nguyễn Xuân Căn 
HÀ NỘI – 2021 
i 
 LỜI CAM ĐOAN 
Tôi xin cam đoan đây là công trình nghiên cứu của riêng tôi. Những nội 
dung, số liệu và kết quả đã trình bày trong Luận án là hoàn toàn trung thực và 
chưa có tác giả nào công bố trong bất cứ một công trình nào khác. 
TÁC GIẢ LUẬN ÁN 
Ngô Văn Toàn 
ii 
 LỜI CẢM ƠN 
Tác giả xin bày tỏ lòng biết ơn chân thành đến tập thể cán bộ hướng dẫn, 
các thầy giáo: 
 Đại tá, TS Nguyễn Xuân Căn 
 Đại tá, TS Lê Thanh Phong 
đã giúp đỡ và khuyến khích tôi trong suốt thời gian thực hiện Luận án. 
Tác giả cũng xin chân thành cảm ơn các nhà khoa học, tập thể cán bộ Bộ 
môn Tên Lửa / 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. 
Cuối cùng, tác giả xin cảm ơn gia đình, vợ con, bạn bè và đồng nghiệp 
đã luôn độ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 
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Ị .............................................................. ix 
DANH MỤC CÁC BẢNG BIỂU ...................................................................... xiv 
MỞ ĐẦU ............................................................................................................... 1 
Chương 1 TỔNG QUAN VỀ ĐIỀU KHIỂN HẠ CÁNH UAV ........................... 5 
1.1. Khái quát chung về UAV và các hệ tọa độ sử dụng trong kỹ thuật điều 
khiển UAV.......................................................................................................... 5 
1.1.1. Khái quát chung về UAV ...................................................................... 5 
1.1.2. Các hệ tọa độ sử dụng trong kỹ thuật điều khiển UAV ........................ 8 
1.1.3. Mối quan hệ giữa các hệ tọa độ ........................................................... 11 
1.2. Đặc điểm quá trình hạ cánh và các hệ thống điều khiển hạ cánh UAV .... 15 
1.2.1. Đặc điểm quá trình hạ cánh ................................................................. 15 
1.2.2. Các loại hệ thống điều khiển hạ cánh UAV ........................................ 18 
1.3. Gió và ảnh hưởng của nhiễu động gió đến quá trình hạ cánh của UAV .. 21 
1.4. Tình hình nghiên cứu trong nước và ngoài nước ...................................... 24 
1.4.1. Tình hình nghiên cứu ngoài nước ....................................................... 25 
1.4.2. Tình hình nghiên cứu trong nước ........................................................ 28 
1.4.3. Hướng tiếp cận của Luận án ................................................................ 30 
1.5. Kết luận chương 1 ..................................................................................... 34 
Chương 2 TỐI ƯU QUỸ ĐẠO HẠ CÁNH CHO UAV .................................... 35 
2.1. Cơ sở giải bài toán điều khiển tối ưu quỹ đạo hạ cánh ............................. 35 
2.1.1. Nguyên lý cực đại Pontryagin ............................................................. 35 
2.1.2. Các chỉ tiêu chất lượng ........................................................................ 40 
iv 
2.1.3. Phương pháp giải bài toán biên. .......................................................... 43 
2.2. Ứng dụng thuật toán tối ưu giải bài toán tối ưu quỹ đạo hạ cánh ............. 47 
2.2.1. Thiết lập bài toán tối ưu quỹ đạo hạ cánh UAV ................................. 47 
2.2.2. Giải bài toán tối ưu quỹ đạo hạ cánh của UAV .................................. 51 
2.2.3. Đánh giá bài toán tối ưu quỹ đạo hạ cánh ........................................... 58 
2.3. Kết luận chương 2 ..................................................................................... 65 
Chương 3 TỔNG HỢP BỘ ĐIỀU KHIỂN KÊNH CHUYỂN ĐỘNG DỌC 
CHO UAV CỠ NHỎ TRONG ĐIỀU KIỆN CÓ GIÓ ........................................ 67 
3.1. Mô hình động học chuyển động dọc của UAV ......................................... 67 
3.1.1. Hệ phương trình chuyển động dọc độc lập của UAV ......................... 67 
3.1.2. Tuyến tính hoá hệ phương trình chuyển động dọc của UAV ............. 73 
3.1.3. Xây dựng hàm truyền của UAV .......................................................... 81 
3.2. Thuật toán bám quỹ đạo hạ cánh UAV ..................................................... 85 
3.2.1. Điều khiển tự động ổn định độ cao sử dụng tín hiệu góc chúc ngóc .. 87 
3.2.2. Điều khiển tự động ổn định độ cao sử dụng tín hiệu quá tải đứng ..... 91 
3.2.3. Điều khiển tự động ổn định độ cao kết hợp ........................................ 93 
3.2.4. Xác định các tham số đầu vào trong luật điều khiển .......................... 94 
3.3. Thuật toán điều khiển bám tốc độ của UAV ............................................. 95 
3.4. Kết luận chương 3 ..................................................................................... 96 
Chương 4 MÔ PHỎNG VÀ ĐÁNH GIÁ KẾT QUẢ ........................................ 98 
4.1. Đánh giá bài toán tối ưu quỹ đạo hạ cánh của UAV khi thay đổi điều 
kiện biên ........................................................................................................... 98 
4.1.1. Khi thay đổi điều kiện ban đầu ............................................................ 98 
4.1.2. Khi thay đổi điều kiện cuối ............................................................... 102 
4.2. Đánh giá vòng điều khiển kín kênh chuyển động dọc của UAV ............ 107 
4.2.1. Dữ liệu đầu vào mô phỏng ................................................................ 107 
4.2.2. Lựa chọn các hệ số trong luật điều khiển .......................................... 108 
v 
4.2.3. Kết quả mô phỏng trường hợp không có gió .................................... 112 
4.2.4. Kết quả mô phỏng trường hợp có gió dọc ......................................... 114 
4.2.5. Kết quả mô phỏng trường hợp có gió đứng ...................................... 118 
4.3. Kết luận chương 4 ................................................................................... 121 
KẾT LUẬN VÀ KIẾN NGHỊ........................................................................... 123 
DANH MỤC NHỮNG CÔNG TRÌNH ĐÃ CÔNG BỐ .................................. 126 
TÀI LIỆU THAM KHẢO ................................................................................. 127 
vi 
 DANH MỤC CÁC CHỮ VIẾT TẮT VÀ KÝ HIỆU 
1. Chữ viết tắt: 
UAV máy bay không người lái 
TBB thiết bị bay 
HPTVP hệ phương trình vi phân 
ГРМ đài chuẩn hướng hạ cánh 
КРМ đài chuẩn tầm hạ cánh 
2. Ký hiệu: 
Ooxoyozo hệ tọa độ mặt đất 
Oxgygzg hệ tọa độ mặt đất di động 
Ox1y1z1 hệ tọa độ liên kết 
Oxryrzr hệ tọa độ tốc độ 
Oxkykzk hệ tọa độ quỹ đạo 
 góc chúc ngóc 
𝜓 góc hướng 
 góc nghiêng (cren, roll), góc giữa trục Oz và mặt phẳng 
nằm ngang Oxgzg 
 góc nghiêng quỹ đạo 
 góc tấn (của véc tơ địa tốc kV ) 
 r góc tấn không tốc (của véc tơ không tốc rV ) 
, ,x y z   tốc độ góc của thiết bị bay trong hệ tọa độ liên kết 
z
zm
 đạo hàm hệ số mô men chúc ngóc theo z , với 
a
z z
r
b
V
  
c
zm
 đạo hàm hệ số mô men chúc ngóc theo cánh lái độ cao 
z
m
. đạo hàm hệ số mô men chúc ngóc theo tốc độ góc tấn 
vii 
Mx, My, Mz mô men khí động trong hệ tọa độ liên kết 
Jx, Jy, Jz mô men quán tính của UAV quanh tâm khối 
Yr lực nâng trong hệ tọa độ tốc độ 
Xr lực cản trong hệ tọa độ tốc độ 
 khối lượng riêng không khí 
m khối lượng của UAV 
Vr không tốc (tốc độ tương đối của UAV so với không khí) 
Vk địa tốc (tốc độ tuyệt đối của UAV so với mặt đất) 
Vhc vận tốc hạ cánh của UAV (vận tốc tiếp đất) 
Vmt vận tốc mục tiêu di động 
W vận tốc gió 
Wx vận tốc gió dọc 
Wy vận tốc gió đứng 
ba dây cung khí động trung bình 
S diện tích cánh hiệu dụng của UAV 
T lực kéo động cơ 
c góc lệch cánh lái độ cao 
h góc lệch cánh lái hướng 
ny1 quá tải đứng theo hệ tọa độ liên kết 
nx1 quá tải dọc trục theo hệ tọa độ liên kết 
ny quá tải đứng theo hệ tọa độ tốc độ (quá tải pháp tuyến vận 
tốc) 
nx quá tải dọc theo hệ tọa độ tốc độ (quá tải tiếp tuyến vận 
tốc ) 
H Độ cao chuyến bay 
Hct độ cao theo chương trình 
Ho độ cao bay bằng (bắt đầu vào hạ cánh) 
viii 
ct góc chúc ngóc theo chương trình 
yctn quá tải đứng theo chương trình 
zK hệ số cản dịu kênh dọc 
u(t) tín hiệu điều khiển 
, , ,V x yP P P P các biến đồng trạng thái tương ứng các biến , , ,V x y 
0t thời điểm bắt đầu vào hạ cánh 
ft thời điểm kết thúc hạ cánh (UAV tiếp đất) 
J hàm mục tiêu 
 hàm Hamilton 
L hàm Lagrange 
G hàm Terminant 
s tham số liên tục 
p hàm Laplace 
Lhc quãng đường hạ cánh 
Lhđc quãng đường hạ độ cao 
Lkb quãng đường kéo bằng 
Lhđ quãng đường hãm đà 
l chiều dài đường băng 
l khoảng cách từ đầu đường băng đến vị trí tiếp mong 
muốn 
G trọng lực của UAV 
g gia tốc trọng trường (g = 9,80665 m/s²); 
Cx hệ số lực cản chính diện 
yC hệ số lực nâng 
hdc độ cao của động cơ so với trục dọc UAV 
ix 
 DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ 
Hình 1.1. Một phương án ứng dụng của tổ hợp UAV ...................................... 5 
Hình 1.2. Hệ tọa độ mặt đất và hệ tọa độ chuẩn ............................................... 8 
Hình 1.3. Hệ tọa độ liên kết và hệ tọa độ tốc độ ............................................. 10 
Hình 1.4. Hệ tọa độ quỹ đạo và hệ tọa độ chuẩn ............................................ 11 
Hình 1.5. Mối liên hệ giữa hệ tọa độ liên kết và hệ tọa độ chuẩn .................. 11 
Hình 1.6. Sơ đồ hạ cánh hãm đà ..................................................................... 16 
Hình 1.7. Sơ đồ khối vòng điều khiển kín của UAV ...................................... 19 
Hình 1.8. Các đài chuẩn hướng và tầm hạ cánh .............................................. 20 
Hình 1.9. Ảnh hưởng của gió đứng đến UAV trong mặt phẳng thẳng đứng .. 23 
Hình 1.10. Ảnh hưởng của gió dọc đến UAV trong mặt phẳng thẳng đứng .. 23 
Hình 1.11. Trạng thái của UAV khi vào hạ cánh ............................................ 33 
Hình 2.1. Lưu đồ thuật toán phương pháp giải liên tục theo tham số ............ 57  ... =x(7); 
F(4)=x(8); 
F0=Ftk(x,tf);%[V Tet xo yo P_V P_Teta P_xo P_yo] 
f10=F0(1);f20=F0(2);f30=F0(3);f40=F0(4);f50=F0(5); 
%voi gia tri F1+dF 
x1=x; x1(5)=x(5)+dF; 
F0=Ftk(x1,tf);%[V Tet xo yo P_V P_Teta P_xo P_yo] 
f11=F0(1);f21=F0(2);f31=F0(3);f41=F0(4);f51=F0(5); 
%voi gia tri F2+dF 
x2=x; x2(6)=x(6)+dF; 
F0=Ftk(x2,tf);%[V Tet xo yo P_V P_Teta P_xo P_yo] 
f12=F0(1);f22=F0(2);f32=F0(3);f42=F0(4);f52=F0(5); 
%voi gia tri F3+dF 
x3=x; x3(7)=x(7)+dF; 
F0=Ftk(x3,tf);%[V Tet xo yo P_V P_Teta P_xo P_yo] 
f13=F0(1);f23=F0(2);f33=F0(3);f43=F0(4);f53=F0(5); 
%voi gia tri F4+dF 
x4=x; x4(8)=x(8)+dF; 
F0=Ftk(x4,tf);%[V Tet xo yo P_V P_Teta P_xo P_yo] 
f14=F0(1);f24=F0(2);f34=F0(3);f44=F0(4);f54=F0(5); 
%voi gia tri tf+dF 
x5=x; tf=tf+dF; 
F0=Ftk(x5,tf);%[V Tet xo yo P_V P_Teta P_xo P_yo] 
f15=F0(1);f25=F0(2);f35=F0(3);f45=F0(4);f55=F0(5); 
% Tinh gia tri ma tran J 
ff1(1,1)=(f11-f10)/dF; 
ff1(1,2)=(f12-f10)/dF; 
ff1(1,3)=(f13-f10)/dF; 
ff1(1,4)=(f14-f10)/dF; 
ff1(1,5)=(f15-f10)/dF; 
ff1(2,1)=(f21-f20)/dF; 
ff1(2,2)=(f22-f20)/dF; 
ff1(2,3)=(f23-f20)/dF; 
ff1(2,4)=(f24-f20)/dF; 
ff1(2,5)=(f25-f20)/dF; 
ff1(3,1)=(f31-f30)/dF; 
143 
ff1(3,2)=(f32-f30)/dF; 
ff1(3,3)=(f33-f30)/dF; 
ff1(3,4)=(f34-f30)/dF; 
ff1(3,5)=(f35-f30)/dF; 
ff1(4,1)=(f41-f40)/dF; 
ff1(4,2)=(f42-f40)/dF; 
ff1(4,3)=(f43-f40)/dF; 
ff1(4,4)=(f44-f40)/dF; 
ff1(4,5)=(f45-f40)/dF; 
ff1(5,1)=(f51-f50)/dF; 
ff1(5,2)=(f52-f50)/dF; 
ff1(5,3)=(f53-f50)/dF; 
ff1(5,4)=(f54-f50)/dF; 
ff1(5,5)=(f55-f50)/dF; 
ff=-inv(ff1); %Chu y dau tru 
end 
3.Chương trình tính 
dX
dt
function ff=DXDT(t,f) 
%Ham tinh dao ham rieng cua V,Tet, yo theo thoi gian 
global Re gE m0 Te0 S k1 k2 Xf Yf Tetaf tf; 
global t_Hamilton Hamilton dem_Hamilton nxx nyy ny_min ny_max 
ny_opt_old OFF_Hamilton anfa_fa rho; 
global anfa_min anfa_max VV Vf P; 
ff = zeros(8,1); 
V=f(1); Tet=f(2); x=f(3); y=f(4); 
P_V=f(5); P_Teta=f(6);P_x=f(7); P_y=f(8); 
%Qua tai toi uu 
nx=-P_V*gE*k1^2;%Truong hop dV/dt#0; 
ny=-P_Teta*gE/V*k2^2; 
ff(1)=gE*(nx-sin(Tet)); 
ff(2)=gE/V*(ny-cos(Tet)); 
ff(3)=V*cos(Tet); 
ff(4)=V*sin(Tet); 
ff(5)=P_Teta*gE/V^2*(ny-cos(Tet))-P_x*cos(Tet)-P_y*sin(Tet); 
ff(6)=P_V*gE*cos(Tet)-P_Teta*gE/V*sin(Tet)+P_x*V*sin(Tet)-
P_y*V*cos(Tet); 
ff(7)=0; 
ff(8)=0; 
if OFF_Hamilton==1 
144 
 if t==0 
 dem_Hamilton=1; 
 t_Hamilton(1)=0; 
 Hamilton 
(1)=P_V*ff(1)+P_Teta*ff(2)+P_x*ff(3)+P_y*ff(4)+(0.5*nx^2)/k1^2+1/2*ny
^2/k2^2; 
 nxx(1)=nx; 
 nyy(1)=ny; 
 VV(1)=V; 
 anfa_fa(1)=(2*ny*m0*gE)/(5.9*rho*V^2*S+2*P); 
 end; 
 if (t>=t_Hamilton(dem_Hamilton)+0.0001); 
 dem_Hamilton =dem_Hamilton+1; 
 t_Hamilton(dem_Hamilton)=t; 
 Hamilton 
(dem_Hamilton)=P_V*ff(1)+P_Teta*ff(2)+P_x*ff(3)+P_y*ff(4)+(0.5*nx^2)/
k1^2+1/2*ny^2/k2^2; 
 nxx(dem_Hamilton)=nx; 
 nyy(dem_Hamilton)=ny; 
 VV(dem_Hamilton)=V; 
anfa_fa(dem_Hamilton)=(2*nyy(dem_Hamilton)*m0*gE)/(5.9*rho*VV(dem
_Hamilton)^2*S+2*P); 
 end; 
end; 
end 
4. Chương trình tính Cx 
%Xay dung ham Cx theo M va alpha 
function fCx=fCx(M,alpha) 
 M0=[0.1 0.12 0.14 0.16 0.18 0.20]; 
 alpha0=[0 1 2 3 4 5 6 7]; 
 alpha0=alpha0*pi/180; 
alpha=abs(alpha); 
 Cx0=[ 
0.020974319 0.021388468 0.022630915 0.02470166 0.027600702 
0.031328043 0.035883681 0.041267617 
0.020349917 0.020765163 0.0220109 0.024087128 0.026993847 
0.030731057 0.035298758 0.04069695 
0.019806494 0.020223053 0.02147273 0.023555524 0.026471436 
0.030220466 0.034802614 0.040217879 
145 
0.019317518 0.019735617 0.020989911 0.023080403 0.02600709 
0.029769974 0.034369055 0.039804332 
0.018892091 0.019311965 0.020571587 0.022670956 0.025610072 
0.029388937 0.034007549 0.039465909 
0.018557325 0.018979222 0.020244913 0.022354398 0.025307676 
0.029104749 0.033745616 0.039230277]; 
fCx=interp2(alpha0,M0,Cx0,alpha,M,'spline'); 
end 
5. Chương trình tính Cy 
%Xay dung ham Cx theo M va alpha 
function fCy=fCy(M,alpha) 
 M0=[0.1 0.12 0.14 0.16 0.18 0.20]; 
 alpha0=[0 1 2 3 4 5 6 7]; 
 alpha0=alpha0*pi/180; 
 dau=1; 
 if alpha<0 dau=-1;alpha=dau*alpha;end; 
 Cy0=[ 
0 0.101072915 0.20214583 0.303218744 0.404291659 0.505364574 
0.606437489 0.707510404 
0 0.101206643 0.202413286 0.303619929 0.404826572 0.506033215 
0.607239858 0.708446501 
0 0.101366561 0.202733123 0.304099684 0.405466246 0.506832807 
0.608199369 0.70956593 
0 0.101553686 0.203107371 0.304661057 0.406214742 0.507768428 
0.609322114 0.710875799 
0 0.101769097 0.203538193 0.30530729 0.407076386 0.508845483 
0.610614579 0.712383676 
0 0.102013989 0.204027978 0.306041966 0.408055955 0.510069944 
0.612083933 0.714097921 
]; 
fCy=dau*interp2(alpha0,M0,Cy0,alpha,M,'spline'); 
end 
6. Chương trình tính giá trị hàm Hamilton 
function gg=fHamilton(t,f) 
global Re gE m0 Te0 S k1 k2 gan_zero Xf Yf Tetaf tf; 
global t_Hamilton Hamilton dem_Hamilton nxx nyy ny_min ny_max 
ny_opt_old OFF_Hamilton; 
ff = zeros(8,1); 
%alpha=Teta(t)-Te0; 
V=f(1); Tet=f(2); x=f(3); y=f(4); 
146 
P_V=f(5); P_Teta=f(6);P_x=f(7); P_y=f(8); 
%Qua tai toi uu 
nx=-P_V*gE*k1^2;%Truong hop dV/dt#0; 
ny=-P_Teta*gE/V*k2^2; 
% %---------------- 
ff(1)=gE*(nx-sin(Tet)); 
ff(2)=gE/V*(ny-cos(Tet)); 
ff(3)=V*cos(Tet); 
ff(4)=V*sin(Tet); 
ff(5)=P_Teta*gE/V^2*(ny-cos(Tet))-P_x*cos(Tet)-P_y*sin(Tet); 
ff(6)=P_V*gE*cos(Tet)-P_Teta*gE/V*sin(Tet)+P_x*V*sin(Tet)-
P_y*V*cos(Tet); 
ff(7)=0; 
ff(8)=0; 
gg=P_V*ff(1)+P_Teta*ff(2)+P_x*ff(3)+P_y*ff(4)+gan_zero*1/2*nx^2/
k1^2+1/2*ny^2/k2^2; 
end 
7. Chương trình tính Véc tơ sai số kép 
function [ff]=Ftk(x,tf) 
global Xf Yf Tetaf Vf; 
[TT,F]=ode45(@DXDT,[0,tf],x);%x=[V Teta0 xo yo F1 F2 F3 F4] 
F1=F(:,1); F2=F(:,2);F3=F(:,3);F4=F(:,4); 
F5=F(:,5); F6=F(:,6);F7=F(:,7);F8=F(:,8); 
Vf_tf=F1(length(F1)); 
Tetaf_tf=F2(length(F2)); 
xf_tf=F3(length(F3)); 
yf_tf=F4(length(F4)); 
P1_tf=F5(length(F5)); 
P2_tf=F6(length(F6)); 
P3_tf=F7(length(F7)); 
P4_tf=F8(length(F8)); 
xf=[Vf_tf Tetaf_tf xf_tf yf_tf P1_tf P2_tf P3_tf P4_tf]; 
ff(1)=Vf_tf-Vf; 
ff(2)=10*(Tetaf_tf-Tetaf)*180/pi; 
ff(3)=xf_tf-Xf; 
ff(4)=yf_tf-Yf; 
ff(5)=1000*(fHamilton(tf,xf)-0); 
end 
8. Chương trình tìm bộ giá trị ban đầu để mô đun Véc tơ sai số kép 
cực tiểu 
147 
function [ff]=minFtk(FF) 
global Vo Teta0 Lo Ho P_V tf; 
AA=Ftk([Vo Teta0 Lo Ho FF(1) FF(2) FF(3) FF(4)],tf); 
ff=AA(1)^2+AA(2)^2+AA(3)^2+AA(4)^2+AA(5)^2; 
end 
9. Chương trình giải liên tục theo tham số 
function [ff]=Prodol(x,tf) % Ham giai lien tuc theo tham so de tim bo 
bien dong trang thai ban dau 
F00(1)=x(5); F00(2)=x(6);F00(3)=x(7);F00(4)=x(8); 
for j=1:1:50 
 x5=F00(1); x6=F00(2);x7=F00(3); x8=F00(4); 
 x(5)=x5; x(6)=x6; x(7)=x7; x(8)=x8; 
 dh=0.5; FK0=Ftk(x,tf)'; 
 m=sqrt(FK0(1)^2+FK0(2)^2+FK0(3)^2+FK0(4)^2+FK0(5)^2); 
 if sqrt(FK0(1)^2+FK0(2)^2+FK0(3)^2+FK0(4)^2+FK0(5)^2)<1 
ff=F00; break; end; 
 if sqrt(FK0(1)^2+FK0(2)^2+FK0(3)^2++FK0(4)^2+FK0(5)^2)>1e+6 
ff=F00; break; end; 
 tf0=tf; 
 for i=1:1:(1/dh) 
 x(5)=x5; x(6)=x6; x(7)=x7; x(8)=x8; 
 K1=dFdf(x,tf)*FK0*dh; 
 FF(1)=F00(1)+K1(1); FF(2)=F00(2)+K1(2); FF(3)=F00(3)+K1(3); 
FF(4)=F00(4)+K1(4); tf=tf0+K1(5); 
 x(5)=FF(1); x(6)=FF(2); x(7)=FF(3); x(8)=FF(4); 
 K2=dFdf(x,tf)*FK0*dh/2; 
 FF(1)=F00(1)+K2(1); FF(2)=F00(2)+K2(2); 
FF(3)=F00(3)+K2(3); FF(4)=F00(4)+K2(4); tf=tf0+K2(5); 
 x(5)=FF(1); x(6)=FF(2); x(7)=FF(3); x(8)=FF(4); 
 K3=dFdf(x,tf)*FK0*dh/2; 
 FF(1)=F00(1)+K3(1); FF(2)=F00(2)+K3(2); 
FF(3)=F00(3)+K3(3); FF(4)=F00(4)+K3(4); tf=tf0+K3(5); 
 x(5)=FF(1); x(6)=FF(2); x(7)=FF(3); x(8)=FF(4); 
 K4=dFdf(x,tf)*FK0*dh; 
 tf=tf0+K4(5); 
 F00(1)=F00(1)+(K1(1)+2*K2(1)+2*K3(1)+K4(1))/6; 
 F00(2)=F00(2)+(K1(2)+2*K2(2)+2*K3(2)+K4(2))/6; 
 F00(3)=F00(3)+(K1(3)+2*K2(3)+2*K3(3)+K4(3))/6; 
 F00(4)=F00(4)+(K1(4)+2*K2(4)+2*K3(4)+K4(4))/6; 
 tf=tf0+(K1(5)+2*K2(5)+2*K3(5)+K4(5))/6; 
148 
 x5=F00(1); x6=F00(2);x7=F00(3); x8=F00(4); F00(5)=tf; 
 end; 
end; 
ff=F00; 
end 
II. Chương trình tối ưu quỹ đạo hạ cánh của UAV ( tín hiệu điều 
khiển bị hạn chế):(Chương trình chính và các chương trình con tương tự 
trường hợp tín hiệu điều khiển không bị hạn chế. Chỉ khác phần chương trình 
tính 
dX
dt
 ) 
3. Chương trình tính 
dX
dt
function ff=DXDT(t,f) 
%Ham tinh dao ham rieng cua V,Tet, yo theo thoi gian 
global Re gE m0 Te0 S k1 k2 Xf Yf Tetaf tf; 
global t_Hamilton Hamilton dem_Hamilton nxx nyy ny_min ny_max 
ny_opt_old OFF_Hamilton anfa_fa rho; 
global anfa_min anfa_max VV Vf P; 
ff = zeros(8,1); 
V=f(1); Tet=f(2); x=f(3); y=f(4); 
P_V=f(5); P_Teta=f(6);P_x=f(7); P_y=f(8); 
%Qua tai toi uu 
nx=-P_V*gE*k1^2;%Truong hop dV/dt#0; 
ny=-P_Teta*gE/V*k2^2; 
ny_max=(5.9123*(anfa_max)*rho*Vf^2*S/2+P*sin(anfa_max))/(m0*g
E); 
% %-------------------- 
H1=(P_V*gE*(nx-sin(Tet))+P_Teta*gE/V*(ny-
cos(Tet))+P_x*V*cos(Tet)+P_y*V*sin(Tet)+1/2*nx^2/k1^2+1/2*ny^2/k2^2)
;if(ny>ny_max)||(ny<ny_min) H1=10000000; end; 
ny=ny_min; 
H2=(P_V*gE*(nx-sin(Tet))+P_Teta*gE/V*(ny-
cos(Tet))+P_x*V*cos(Tet)+P_y*V*sin(Tet)+1/2*nx^2/k1^2+1/2*ny^2/k2^2)
;ny=ny_max; 
H3=(P_V*gE*(nx-sin(Tet))+P_Teta*gE/V*(ny-
cos(Tet))+P_x*V*cos(Tet)+P_y*V*sin(Tet)+1/2*nx^2/k1^2+1/2*ny^2/k2^2)
;if H2<H3 ny=ny_min; end; 
if (H1=ny_min)&&(H1<H3) ny=-
P_Teta*gE/V*k2^2; end; 
%------------------- 
149 
ff(1)=gE*(nx-sin(Tet)); 
ff(2)=gE/V*(ny-cos(Tet)); 
ff(3)=V*cos(Tet); 
ff(4)=V*sin(Tet); 
ff(5)=P_Teta*gE/V^2*(ny-cos(Tet))-P_x*cos(Tet)-P_y*sin(Tet); 
ff(6)=P_V*gE*cos(Tet)-P_Teta*gE/V*sin(Tet)+P_x*V*sin(Tet)-
P_y*V*cos(Tet); 
ff(7)=0; 
ff(8)=0; 
if OFF_Hamilton==1 
 if t==0 
 dem_Hamilton=1; 
 t_Hamilton(1)=0; 
 Hamilton 
(1)=P_V*ff(1)+P_Teta*ff(2)+P_x*ff(3)+P_y*ff(4)+(0.5*nx^2)/k1^2+1/2*ny
^2/k2^2; 
 nxx(1)=nx; 
 nyy(1)=ny; 
 VV(1)=V; 
 anfa_fa(1)=(2*ny*m0*gE)/(5.9*rho*V^2*S+2*P); 
 end; 
 if (t>=t_Hamilton(dem_Hamilton)+0.0001); 
 dem_Hamilton =dem_Hamilton+1; 
 t_Hamilton(dem_Hamilton)=t; 
 Hamilton 
(dem_Hamilton)=P_V*ff(1)+P_Teta*ff(2)+P_x*ff(3)+P_y*ff(4)+(0.5*nx^2)/
k1^2+1/2*ny^2/k2^2; 
 nxx(dem_Hamilton)=nx; 
 nyy(dem_Hamilton)=ny; 
 VV(dem_Hamilton)=V; 
anfa_fa(dem_Hamilton)=(2*nyy(dem_Hamilton)*m0*gE)/(5.9*rho*VV(dem
_Hamilton)^2*S+2*P); 
 end; 
end; 
end 
150 
Phụ lục 3. Sơ đồ mô phỏng bằng phần mềm Simulink 
Hình P3.1. Sơ đồ khối vòng điều khiển kín kênh dọc của UAV 
151 
Hình P3.2. Sơ đồ mô phỏng khối tham số hình học của UAV 
Hình P3.3. Sơ đồ mô phỏng khối thông tin về trái đất 
152 
Hình P3.4. Sơ đồ mô phỏng khối tính toán lực khí động 
Hình P3.5. Sơ đồ mô phỏng khối tính toán lực cản 
153 
Hình P3.6. Sơ đồ mô phỏng khối tính toán lực nâng và động áp q 
Hình P3.7. Sơ đồ mô phỏng khối điều khiển 
154 
Hình P3.8. Sơ đồ mô phỏng khối tính toán quá tải đứng ny 
Hình P3.9. Sơ đồ mô phỏng khối tính toán tốc độ bay 
155 
Hình P3.10. Sơ đồ mô phỏng khối tính toán góc nghiêng quỹ đạo 
Hình P3.11. Sơ đồ mô phỏng khối tính toán tốc độ góc chúc ngóc và góc chúc 
ngóc 
156 
Hình P3.12. Sơ đồ mô phỏng khối tính toán độ cao và cự ly 
Hình P3.13. Sơ đồ mô phỏng khối tính toán góc tấn