Luận án Xây dựng bộ ổn định và thuật toán điều khiển bám quỹ đạo cho UAV cánh bằng

 0
 a*
 (2.118) 
 Kp . z z z 0
 a* 2 1 2
 p
Từ: K. z z z 0 , ta có: 
 a* 2 1 2
 p
 z2 (.) Ka z 2 z 1 (2.119) 
 * 
 p
 p p  des K a.( p p des ) z1 (2.120) 
 * 
 p
 p Ka.( p p des ) z1 p  des (2.121) 
 * 
Sau khi biến đổi nhận được: 
 p p   p
 p ( KKpKK ). ( . 1). zKKBB ( ).  (2.122) 
 a****** a a a1 a a 
Từ (2.96) nhận được: 
 51 
 p p 
 (.CrCpqCM1 2 .). 3 .roll CM 4 . yaw ( K a KpKK a ).(. a a 1). z 1 
 **** (2.123) 
 (KKBB p ).  
 a** a
Từ đó ta có: 
 p p 
 C3. Mroll ( K a K a ). p ( K a . K a 1). z 1 
 **** (2.124) 
 (K Kp ). BB  ( CrCpqCM . . ). . 
 a** a1 2 4 yaw
 (KKKKKKp  ) ( p .  1) (  p ) 1
 M a****** a... p a a z a a B B 
 roll CCCC1
Hay: 3 3 3 3 (2.125) 
 (..)C r C p C
 1 2..q 4 M
 CC yaw
 3 3 
Với các thành phần tham số xác định theo (2.98), (2.100) và (2.110): 
 1 2
 Mroll V a SbC l p dyn... S b C l (2.126) 
 2 
 b
 Cl C l...(..) C l  a C l  r C l p C l r (2.127) 
  a  r p r
 2.Va
 B ( q .sin r .cos  ).tan  (2.128) 
Từ đó ta nhận được: 
 b 
 Mroll pSbCpSbC dyn.........(..) l dyn l C l  a C l  r CpCr l l (2.129) 
  a  r p r 
 2.Va 
Như vậy tác động tổng hợp của các cánh lái sẽ là: 
 M roll b 
 Cl...(..) a C l  r C l  C l p C l r (2.130) 
 a  r p. S . b  2. V p r
 dyn a 
Khi không sử dụng cánh lái hướng trong điều khiển bám góc nghiêng ta có: 
 (KKKKKKp  ) ( p .  1) (  p )
 a****** a a a a a
 Cl.... a p z1 B 
 a
 CpSb3........dyn CpSb 3 dyn CpSb 3 dyn
 1  (..)C1 r C 2 p C 4
 B .. q M yaw (2.131) 
 CpSb3dyn........ CpSb 3 dyn CpSb 3 dyn
 b 
 C.(..) C p C r
 l lp l r 
 2.Va
Đặt: D C3 pdyn.. S b ta có: 
 52 
 (KKKKKKp  ) ( p .  1) (  p )
 a****** a a a a a
 a ...p z1 B 
 CDCDCDl... l l
 a  a  a
 1  (..)C1 r C 2 p C 4
 B .. q M yaw (2.132) 
 CDCDCDl... l l
 a  a  a
 1 b 
 C.(..) C p C r
 l lp l r 
 CVl. 2. a 
 a 
 1 (KKKKp  )b C ( p .  1)
  ..... a**** a C 2 q p a a z 
 a lp 1
 CDVDCDl2. a l .
 a  a
 ()KK p 1 C. r
 a** a  1
 ..B B q (2.133) 
 CDCDCDl... l l
 a  a  a
 C 1
 4 ...)M C C r
 yaw l lr 
 CDCl. l
 a  a
 Từ (2.99) và (2.101), ta có: 
 1 2 D b
 Myaw V a SbC n .( C n .  C n .  r C n . r ) (2.134) 
 2CV r 2. r
 3 a 
 Không sử dụng cánh lái hướng (2.134) trở thành: 
 D b
 M .( C . C . r ) (2.135) 
 yaw n nr
 CV3 2. a
 p 
 1 ()KK b C 
 Đặt: Kp ... a** a C 2 q (2.136) 
 aCDVD 2. lp 
 l a 
 a 
 p 
 (KK . 1)
  a** a
 Ka (2.137) 
 CDl .
 a 
 Từ (2.133) có tính đến (2.135) nhận được: 
 ()KK p 1
 p  a** a 
 a K a.. p K a z1 B B 
 CDCDl.. l
 a  a (2.138) 
 C. r C b 1
 1.q 4 .( C . C . r ) C .  C . r )
 n nr l  l r 
 CDCCVCl. l .3 2. a l
 a  a  a 
 Thông thường các máy bay cũng như UAV cánh bằng có hệ số: J xz nhỏ so 
với Jx , Jz cho nên: 
 53 
 J ()JJJJx y z xz
 C xz 0 , C 0 ( J 0). 
 4  2  xz
 p 
 q 
   Mô hình r 
ref + K + a
 a ϕ 
 _ _ UAV 
  
 Va 
 β 
 p
 + Ka 
 + 
 Bộ tính toán các 
 tham số bộ điều 
 khiển bám góc 
 nghiêng 
 Tham số mô hình khí 
 ρ Mô hình 
 động học của UAV 
 C ,C , C , khí quyển 
 l l p l 
 a 
 Hình 2.2. Sơ đồ khối bộ điều khiển bám góc nghiêng của UAV 
Ta có được luật điều khiển cánh lái liệng là: 
 p 
 a K a. p K a .(   ref ) (2.139) 
  p
 ()KKC 1 C. r Cl
 Với: a** aB B 1 ... q   lr r (2.140) 
 CDCDCDCCl... l l l l
 a  a  a  a  a
  p
 Điều kiện: K 0 , K 0 (2.141) 
 a* a* 
 1 ()KKp  b 
 KCp .. a** a (2.142) 
 a lp 
 CDVl2. a
 a 
 (KKp . 1) 
  a** a
 Ka (2.143) 
 CDl .
 a 
 D C... p S b (2.144) 
 3 dyn 
 54 
 B ( q .sin r .cos  ).tan  (2.145) 
 Các góc cánh lái a được giới hạn trong khoảng nhất định, ví dụ một số 
 0 0
UAV các góc cánh lái nằm trong khoảng  a 20  20 . 
 Sơ đồ khối bộ điều khiển bám góc nghiêng tổng hợp theo Backstepping theo 
luật điều khiển (2.139) được thể hiện trên hình 2.2. 
 - Nhận xét: 
 Trong trường hợp UAV có tốc độ dạt và góc dạt nhỏ ( r 0, 0 ) luật điều 
khiển nhận được là: 
 ()KK p 1
 p  a** a 
 a K a. p K a .(   ref ) B B (2.146) 
 CDCDl.. l
 a  a 
 Trường hợp UAV bay với góc chúc ngóc đủ nhỏ và tốc độ chúc ngóc nhỏ để 
có được thành phần q.tan 0 , khi đó: BB 0, 0 khi đó luật điều khiển bám góc 
nghiêng UAV có cấu trúc trở về tương tự dạng bộ điều khiển trong bộ tự động lái 
CAУ-23. Luật điều khiển bám góc nghiêng xác định: 
  Kp. p K . z K p . p K  .(   ) (2.147) 
 a a a1 a a ref 
 Với hệ số K p , K  được xác định theo: (2.141)÷ (2.144). 
 a a 
2.3. Mô phỏng đánh giá bộ điều khiển được tổng hợp 
2.3.1. Mô phỏng bộ điều theo kênh dọc (bám góc nghiêng quỹ đạo) 
 - Điều kiện tiến hành mô phỏng: 
 Thực hiện mô phỏng, đánh giá trên cở sở mô hình phi tuyến đầy đủ của 
UAV sử dụng bộ tham số mô hình máy bay MiG-21 có chương trình mô phỏng 
được xây dựng trong phụ lục C của luận án. 
 0
 Điều kiện ban đầu của UAV: Maircraft=6800kg, Va=600km/h, α=2.75 , 
 0 0 0
=2.5 , =0 , =40 , e=0, a=0, r=0, lực đẩy được điều khiển bằng bộ điều 
khiển PI với tốc độ đặt là V 600 km / h , vị trí ban đầu P0 (x=0, y=0, h =2000m). 
 ad
 - Kịch bản: 
 Kịch bản 1: Thực hiện mô phỏng cho UAV bám các góc nghiêng quỹ đạo đặt 
 55 
dạng bước nhảy: ban đầu thực hiện với góc nghiêng quỹ đạo ref = 0, tiếp theo đó 
 0 0 0 0
ref lần lượt là -1 , -2 , 2 , 1 và trở về bám góc nghiêng quỹ đạo đặt ref = 0. 
 Kịch bản 2: Thực hiện mô phỏng cho UAV bám góc nghiêng quỹ đạo đặt 
dạng sine; ban đầu thực hiện với góc nghiêng quỹ đạo đặt ref = 0, tiếp theo đó ref 
 0
là một hàm sine với biên độ là 15 chu kỳ là T=60s và kết thúc trở về bám ref = 0. 
 - Thực hiện mô phỏng kịch bản 1: 
 Kết quả mô phỏng được thể hiện trên hình 2.3 và hình 2.4. Trên hình 2.3 là 
quỹ đạo chuyển động của UAV trong không gian. Một số tham số đầu vào và tham 
số đáp ứng của UAV được thể hiện trên hình 2.4. Các tham số lần lượt là góc 
nghiêng quỹ đạo đặt ref, giá trị cánh lái lên xuống e và các đáp ứng của UAV: góc 
nghiêng quỹ đạo  (đặt trên cùng đồ thị với ref), tốc độ góc chúc ngóc q, sai số góc 
nghiêng quỹ đạo, độ cao, góc tấn α, tốc độ Va, góc chúc ngóc  và quá tải đứng nz. 
 Hình 2.3. Quỹ đạo của UAV khi thực hiện bám góc nghiêng quỹ đạo bước nhảy 
 56 
 Hình 2.4. Đáp ứng của UAV khi thực hiện bám góc nghiêng quỹ đạo bước nhảy 
 - Thực hiện mô phỏng kịch bản 2: 
 Kết quả mô phỏng theo kịch bản 2 được thể hiện trên hình 2.5 và hình 2.6. 
Trên hình 2.5 là quỹ đạo chuyển động của UAV trong không gian. Một số tham số 
đầu vào và tham số đáp ứng của UAV được thể hiện trên hình 2.6; các tham số lần 
lượt là góc nghiêng quỹ đạo đặt ref, giá trị điều khiển cánh lái lên xuống e và các 
đáp ứng của UAV: góc nghiêng quỹ đạo  (đặt trên cùng đồ thị với ref), tốc độ góc 
chúc ngóc q, sai số góc nghiêng quỹ đạo, độ cao, góc tấn α, tốc độ Va, góc chúc 
ngóc  và quá tải đứng nz. 
 Hình 2.5. Quỹ đạo của UAV khi thực hiện bám góc nghiêng quỹ đạo hàm sine với 
 chu kỳ T=60s biên độ 150. 
 57 
 Hình 2.6. Đáp ứng của UAV khi thực hiện bám góc nghiêng quỹ đạo hàm sine với 
 chu kỳ T=60s biên độ 150. 
 - Nhận xét: 
 Kết quả cho thấy: với góc nghiêng quỹ đạo mong muốn thay đổi đột ngột 
như hàm bước nhảy ở kịch bản 1, bộ điều khiển vẫn điều khiển UAV có tham số 
góc nghiêng quỹ đạo nhanh chóng bám theo góc nghiêng quỹ đạo đặt; với quỹ đạo 
mong muốn thay đổi theo hàm sine ở kịch bản 2, bộ điều khiển đã đáp ứng tốt với 
tham số góc nghiêng quỹ đạo bám đều theo góc nghiêng quỹ đạo đặt. Bộ điều khiển 
bám góc nghiêng quỹ đạo đảm bảo thực hiện điều khiển UAV bám và ổn định tốt 
theo góc nghiêng quỹ đạo mong muốn. Bộ điều khiển đủ điều kiện là bộ điều khiển 
vòng trong bám theo góc nghiêng quỹ đạo mong muốn cho các bộ điều khiển dẫn 
bám vòng ngoài khi thực hiện điều khiển bám quỹ đạo cần được xây dựng trong các 
chương tiếp theo. 
 58 
2.3.2. Mô phỏng bộ điều khiển theo kênh ngang (bám góc nghiêng) 
 - Điều kiện tiến hành mô phỏng: 
 Thực hiện mô phỏng, đánh giá trên cở sở mô hình phi tuyến đầy đủ của 
UAV sử dụng bộ tham số mô hình máy bay MiG-21 có chương trình mô phỏng 
được xây dựng trong phụ lục C của luận án. 
 0 0 0 0
 Điều kiện ban đầu của UAV: Maircraft=6800kg, α=0 , =2.5 , =0 , =0 , 
e=0, a=0, r=0, vị trí ban đầu P0 (x=0, y=0, h =2000m). 
 - Kịch bản: 
 Cho bộ điều khiển bám góc nghiêng sử dụng phương pháp Backstepping 
thực hiện bám tín hiệu góc nghiêng tham chiếu là tín hiệu bậc thang với giá trị góc 
 0
nghiêng tham chiếu ban đầu là ref=40 , sau 5s cho góc nghiêng tham chiếu là 
 0
ref=0 . Thực hiện mô phỏng với điều kiện tương tự sử dụng bộ ổn định nguyên bản 
trên MiG-21. Mô phỏng các trường hợp với các tốc độ khác nhau, thực hiện so sánh 
kết quả mô phỏng sử dụng bộ điều khiển Backstepping và kết quả mô phỏng sử 
dụng bộ điều khiển nguyên bản trên MiG-21. 
 - Thực hiện mô phỏng với bộ điều khiển bám góc nghiêng sử dụng phương 
 pháp Backstepping: 
Hình 2.7. Đáp ứng điều khiển bám góc nghiêng sử dụng bộ điều khiển Backstepping 
 Va=600km/h 
 Thực hiện mô phỏng với tốc độ ban đầu và tốc độ đặt cho bộ điều khiển tốc 
độ là Va = 600km/h. Trên hình 2.7 thể hiện kết quả mô phỏng trong chế độ bám góc 
nghiêng, các tham số lần lượt là: góc nghiêng tham chiếu ref, đáp ứng của góc 
 59 
nghiêng , giá trị điều khiển cánh lái liệng a và tốc độ góc nghiêng p. 
 Thực hiện mô phỏng với các điều kiện trên, thay đổi tốc độ ban đầu và tốc độ 
đặt cho bộ điều khiển tốc độ là Va = 800km/h. Đáp ứng của UAV được thể hiện trên 
hình 2.8, trong đó các tham số lần lượt là: góc nghiêng tham chiếu ref, đáp ứng của 
góc nghiêng , giá trị điều khiển cánh lái liệng a và tốc độ góc nghiêng p. 
Hình 2.8. Đáp ứng điều khiển bám góc nghiêng sử dụng bộ điều khiển Backstepping 
 Va=800km/h 
 Thực hiện mô phỏng với các điều kiện trên thay đổi tốc độ ban đầu và tốc độ 
đặt cho bộ điều khiển tốc độ là Va = 900km/h. Đáp ứng của UAV được thể hiện trên 
hình 2.9, trong đó các tham số lần lượt là: góc nghiêng tham chiếu ref, đáp ứng của 
góc nghiêng , giá trị điều khiển cánh lái liệng a và tốc độ góc nghiêng p. 
Hình 2.9. Đáp ứng điều khiển bám góc nghiêng sử dụng bộ điều khiển Backstepping 
 Va=900km/h 
 Qua các trường hợp mô phỏng cho thấy bộ điều khiển nhanh chóng đưa 
UAV bám góc nghiêng mong muốn, góc nghiêng được giữ ổn định với giá trị mong 
muốn. Hệ thống điều khiển bám và ổn định tham số mong muốn với chất lượng tốt. 
 60 
 - Thực hiện mô phỏng với bộ điều khiển nguyên bản trên MiG-21: 
 Thực hiện mô phỏng với điều kiện tương tự, sử dụng bộ điều khiển nguyên 
bản trên MiG-21 lần lượt với các phương án tốc độ tương ứng ở trên. 
 Trên hình 2.10 là kết quả mô phỏng trong chế độ ổn định góc nghiêng với 
tốc độ ban đầu và tốc độ đặt cho bộ điều khiển tốc độ là Va=600km/h. Trong đó các 
tham số lần lượt là: tham số đầu vào đặt, đáp ứng của góc nghiêng, tham số cánh lái 
điều khiển và đáp ứng của tốc độ góc nghiêng p. 
 Hình 2.10. Đáp ứng tốc độ góc nghiêng và góc nghiêng trong chế độ ổn định góc 
 nghiêng Va=600km/h sử dụng bộ CAУ-23 
 Thực hiện tương tự với điều kiện trên, thay đổi tốc độ ban đầu và tốc độ đặt 
cho bộ điều khiển tốc độ là Va=800km/h. Kết quả mô phỏng được thể hiện trên hình 
2.11, trong đó các tham số lần lượt là: góc nghiêng tham chiếu ref, đáp ứng của góc 
nghiêng , giá trị điều khiển cánh lái liệng a và tốc độ góc nghiêng p. 
 Hình 2.11. Đáp ứng tốc độ góc nghiêng và góc nghiêng trong chế độ ổn định góc 
 nghiêng Va=800km/h sử dụng bộ CAУ-23 
 Thực hiện tương tự với điều kiện trên, thay đổi tốc độ ban đầu và tốc độ đặt 
 61 
cho bộ điều khiển tốc độ lên Va=900km/h. Kết quả mô phỏng được thể hiện trên 
hình 2.12, trong đó các tham số lần lượt là: góc nghiêng tham chiếu ref, đáp ứng 
của góc nghiêng , giá trị điều khiển cánh lái liệng a và tốc độ góc nghiêng p. 
 Hình 2.12. Đáp ứng tốc độ góc nghiêng và góc nghiêng trong chế độ ổn định góc 
 nghiêng Va=900km/h sử dụng bộ CAУ-23 
 Kết quả mô phỏng với các trường hợp trên cho thấy bộ ổn định trên CAУ-23 
vẫn đảm bảo ổn định góc nghiêng mong muốn. 
 - Nhận xét: 
 Bộ điều khiển bám tham số góc nghiêng mong muốn được tổng hợp theo 
phương pháp Backstepping (với các tham số của bộ điều khiển được tính toán thay 
đổi theo tham số mô hình) đã điều khiển UAV bám tốt tham số góc nghiêng đặt 
trước với các tốc độ khác nhau. So với bộ ổn định được xây dựng bằng phương 
pháp tuyến tính trong chương 1 (hình 2.10÷2.12) thì bộ điều khiển bám góc nghiêng 
được xây dựng ở trên (hình 2.7÷2.9) đã thực hiện với độ quá chỉnh nhỏ hơn với số 
lần đổi dấu của tốc độ góc nghiêng p ít hơn. 
2.4. Kết luận chương 2 
 Mô hình động học của UAV phù hợp với việc áp dụng thuật toán 
Backstepping để tổng hợp bộ điều khiển, đây chính là phương pháp tổng hợp phi 
tuyến với tham số của bộ điều khiển được tính toán theo tham số mô hình UAV 
đảm bảo điều khiển UAV ổn định với chất lượng điều khiển tốt, độ dự trữ ổn định 
cao, đáp ứng phù hợp với sự thay đổi tham số mô hình. 
 Việc tổng hợp hệ điều khiển Backstepping cho UAV đơn giản dễ dàng áp 
 62 
dụng cho máy bay và UAV cánh bằng. Kết quả mô phỏng ở một số trường hợp cho 
thấy tính ổn định của hệ thống và chất lượng điều khiển tốt hơn so với bộ điều 
khiển tổng hợp bằng phương pháp tuyến tính hóa truyền thống (PID). Đây là cơ sở 
để xây dựng các bộ điều khiển cho các dạng quỹ đạo hỗn hợp (quỹ đạo thực hiện 
nhiệm vụ đặc thù), trong đó tham số đầu ra của các bộ điều khiển sẽ là tham số đầu 
vào của các bộ điều khiển Backstepping được xây dựng ở trên. Các nội dung này sẽ 
được đề cập trong chương 3 của luận án. 
 63 
 CHƯƠNG 3: TỔNG HỢP BỘ ĐIỀU KHIỂN DẪN ĐƯỜNG BÁM QUỸ 
 ĐẠO BAY THỰC HIỆN NHIỆM VỤ CHO UAV CÁNH BẰNG 
 Để UAV có thể thực hiện nhiệm vụ chiến đấu chúng cần đảm bảo thực hiện 
đầy đủ các đoạn bay, chế độ bay khác nhau. Đặc trưng ở chế độ này UAV không ở 
chế độ bay bằng khi thay đổi góc hướng và cũng không ở chế độ ổn định góc 
nghiêng khi thay đổi độ cao. Chính vì vậy cần tổng hợp bộ điều khiển dẫn đường 
bám quỹ đạo bay cho UAV, khi mà UAV vừa muốn bám theo hướng quỹ đạo mong 
muốn (thông qua góc nghiêng mong muốn) vừa muốn bám theo góc nghiêng quỹ 
đạo mong muốn. Nội dung trong chương 3 sẽ trình bày tổng hợp bộ điều khiển dẫn 
đường bám các quỹ đạo bay hỗn hợp mà UAV sẽ thực hiện trong quá trình thực 
hiện nhiệm vụ tấn công mục tiêu di động trên biển: bay hành trình, công kích mục 
tiêu và quay về sân bay hạ cánh. Bộ điều khiển dẫn đường này đóng vai trò là bộ 
điều khiển vòng ngoài cung cấp các tham số đặt cho bộ điều khiển vòng trong (bộ 
ổn định) là các bộ điều khiển Backstepping kênh dọc và kênh ngang đã được tổng 
hợp trong chương 2. 
3.1. Xây dựng bộ tự động bám quỹ đạo hành trình cho UAV cánh bằng 
 Với mục tiêu sử dụng UAV thực hiện nhiệm vụ chiến đấu thì công việc rất 
quan trọng là thiết lập các quỹ đạo bay và thực hiện điều khiển bám các đường quỹ 
đạo bay hành trình đến không vực thực hiện nhiệm vụ để thực hiện nhiệm vụ. Các 
vấn đề đó sẽ được trình bày và giải quyết sau đây. 
 Các quỹ đạo hành trình cơ bản được xây dựng bởi những điểm hành trình 
cần qua. Trong phạm vi của luận án không xây dựng phương pháp xác định các 
điểm hành trình bay, xác định đó là các điểm hành trình được đặt trước. Trên cơ sở 
các điểm hành trình có thể chọn phương pháp bám: bám theo điểm hoặc bám theo 
đường quỹ đạo. Hiện nay phương pháp bám theo đường quỹ đạo được sử dụng 
nhiều bởi vì nó vừa đảm bảo UAV đi qua những điểm hành trình mà còn đảm bảo 
bám theo đường quỹ đạo hành trình mong muốn. Do đó trong nội dung của luận án 
tiến hành xây dựng phương pháp bám theo đường quỹ đạo để thực hiện xây dựng 
bộ tự động bám quỹ đạo hành trình cho UAV. 
 64 
3.1.1. Hệ thống điều khiển bám đường quỹ đạo và thiết lập quỹ đạo 
 Từ tập hợp các điểm đích và các điểm đặt cho quá trình bay hành trình sẽ 
xây dựng được quỹ đạo mong muốn. Khi có được đường quỹ đạo mong muốn, tọa 
độ của UAV và hướng bay; vấn đề của bám đường quỹ đạo là xác định một phương 
hướng mong muốn để đưa UAV bám theo đường quỹ đạo. Với yêu cầu bài toán 
bám đường quỹ đạo đặt ra sẽ xây dựng hệ thống dẫn và bám đường quỹ đạo, trong 
đó lựa chọn phương pháp dẫn bám theo điểm đích ảo (VTP) là phương pháp mới 
đang được nhiều sự quan tâm. 
 Hệ thống dẫn và điều khiển bao gồm: sơ đồ dẫn VTP (Virtual target point - 
điểm đích ảo) cho kênh dọc và kênh ngang, bộ điều khiển tốc độ (theo tay ga) và bộ 
ổn định (điều khiển vòng trong) tác động vào cánh lái của UAV. Hệ thống được thể 
hiện trên hình 3.1. 
 Hệ thống dẫn (là vòng điều khiển ngoài) được chia làm hai phần, hệ thống 
dẫn kênh dọc và hệ thống dẫn kênh ngang. Sơ đồ tổng quan hệ thống dẫn được thể 
hiện trên hình 3.2. 
 Trên cơ sở sơ đồ cấu trúc hệ thống dẫn nêu trên, trên nền ý tưởng về điểm 
đích ảo tiến hành xây dựng hệ thống dẫn bám cho UAV trong không gian bằng 
phương pháp hình học. 
 a 
 VTP Bộ ổn 
 định: 
 kênh ngang d, (d) 
 Danh điều 
 Thiết r 
 sách khiển 
 các lập 
 VTP Back- e UAV 
 điểm quỹ stepping 
 kênh dọc  , ( ) 
 quỹ đạo d d theo hai 
 đạo kênh 
 Bộ điều khiển t 
 tốc độ 
 , Va 
 Hình 3.1. Sơ đồ khối cấu trúc hệ thống dẫn và điều khiển cho UAV 
 Bài toán đặt ra là xây dựng giải thuật bám đường quỹ đạo trong không gian 
3D. Với yêu cầu bài toán các điểm lựa chọn sẽ xây dựng nên các đường quỹ đạo, 
 65 
các đường quỹ đạo này sẽ bao gồm các đoạn thẳng và các đường cong. Tiến hành 
xây dựng phương pháp bám đường quỹ đạo theo đường thẳng và theo đường cong. 
 PPlane (xplane, yplane, zplane) 
 , p 
 Wi, Wi+1 
 Bám theo đích ảo d,(d) 
 Bộ ổn 
 kênh ngang , 
 định: điều 
 a 
 khiển Va 
 Thiết lập r 
 Back- UAV 
 quỹ đạo e 
 stepping 
 Bám theo đích ảo d,(d) theo hai 
 kênh dọc kênh 
 Wi, Wi+1
 PPlane (xplane, yplane, zplane) , q 
 Hình 3.2. Sơ đồ hệ thống dẫn theo kênh dọc và kênh ngang cho UAV 
 - Nhận xét: 
 Như vậy tham số đầu vào cho bộ điều khiển dẫn bám quỹ đạo hành trình là 
tọa độ các điểm đặt (điểm đích và các điểm đặt cho trước), tham số đầu ra là góc d 
và góc d cấp cho bộ ổn định được xây dựng ở chương 2. 
3.1.2. Xây dựng phương pháp bám đường quỹ đạo là đường thẳng 
 Bài toán cần xây dựng là xây dựng phương pháp bám đường quỹ đạo dọc 
theo hai điểm lựa chọn trong không gian 3D. Hệ tọa độ quán tính Oixiyizi, trong 
trường hợp này được viết gọn là Oxyz. Hai điểm lựa chọn lần lượt là Wi=[xi, yi, zi] 
và Wi+1=[xi+1, yi+1, zi+1], được thể hiện trên hình 3.3. 
 Đoạn quỹ đạo qua Wi và Wi+1 sẽ được xác định với góc hướng quỹ đạo của 
đoạn quỹ đạo f và góc nghiêng quỹ đạo của đoạn quỹ đạo f. Từ hệ trục Oxyz quay 
quanh trục Oz sao cho vị trí mới của trục Ox song song với mặt phẳng trực giao 
chứa WiWi+1, vị trí mới xác định hệ trục Ox1y1z1. Bài toán thực hiện trên cơ sở thuật 
toán bám đường quỹ đạo với phương pháp bám theo đích ảo. Điểm P là vị trí UAV 
trong không gian, Px1z1 là hình chiếu của P lên mặt phẳng vuông góc với xOy (mặt 
phẳng thẳng đứng song song với mặt phẳng Ox1z1 có thể coi mặt phẳng Ox1z1) chứa 
đường Wi Wi+1, chọn điểm T là đích ảo của UAV chạy dọc theo đường quỹ đạo, T 
 66 
cách điểm hình chiếu của Px1z1 lên đường quỹ đạo là Δ1, bài toán được xây dựng đưa 
về thực hiện UAV bám theo đích ảo T. Khoảng cách từ Px1z1 tới đường quỹ đạo (ev) 
là độ lệch tầm của UAV so với đường quỹ đạo. Pxy là hình chiếu của P lên mặt 
phẳng Oxy. Lxy là hình chiếu của đoạn quỹ đạo WiWi+1 lên mặt phẳng Oxy. Khoảng 
cách từ Pxy đến Lxy là ec chính là độ lệch cạnh của UAV so với đường quỹ đạo. 
 Wi+1 
 -z, -z1 
 x 
 Px z
 1 1 T 
 zPlane 
 e
 Δ1 
 R1 
 zref 
 Lxy P 
 R2 
 xi+1 f 
 W'i+1 
 Wi Txy 
 zplane xi 
 Δ2 
 Sxy ec 
 W'i 
 S1 
 xPlane Pxy 
 x1 
 f y1plane 
 x1plane 
 O yi yplane yi+1 y 
 y1 
 Hình 3.3. Xây dựng phương pháp dẫn bám theo điểm đích ảo trong không gian 
 Các góc hướng và góc nghiêng quỹ đạo của đường quỹ đạo được xác định 
theo công thức: 
  f ar