Luận án Nghiên cứu nâng cao chất lượng hoạt động của đầu tự dẫn hồng ngoại cho thiết bị bay có điều khiển trên cơ sở hoàn thiện bộ định hướng và luật dẫn tiếp cận tỷ lệ tăng cường

Nội dung text: Luận án Nghiên cứu nâng cao chất lượng hoạt động của đầu tự dẫn hồng ngoại cho thiết bị bay có điều khiển trên cơ sở hoàn thiện bộ định hướng và luật dẫn tiếp cận tỷ lệ tăng cường

1. 108 tính quan sát được quỹ đạo của mục tiêu mà không cần hy sinh hiệu quả của phương pháp dẫn tỷ lệ. Hiệu quả của phương pháp dẫn tỷ lệ tăng cường (4.16) sẽ được thể hiện rõ khi kết hợp với bộ ước lượng trạng thái các tham số của mục tiêu. Kết hợp (4.16) với lệnh bù thích nghi dịch chuyển tâm điểm ngắm mục tiêu (4.15), ta sẽ thu được phương trình dẫn của phương pháp dẫn tiếp cận tỷ lệ tăng cường như sau: ˆN0 ˆ aTL N0 V tc φ p φ  tn aˆ MT FD q φ p , (4.17) 2 ˆ ˆ Trong đó, các tham số Vtc , Dq ,aˆMT là các giá trị ước lượng tương ứng được xác định từ bộ ước lượng trạng thái sử dụng phương pháp lọc Kalman mở rộng. Trong biểu thức (4.17), ta thấy rằng tham số lệnh bù thích nghi dịch chuyển tâm điểm ngắm mục tiêu φtn không ảnh hưởng trong pha đầu và pha giữa của quá trình tự dẫn. Ở pha cuối tự dẫn, tham số lệnh bù thích nghi dịch chuyển tâm điểm ngắm mục tiêu φtn sẽ di chuyển điểm ngắm đến trọng tâm của mục tiêu bay. Từ đó làm tăng xác suất tiêu diệt mục tiêu. 4.3. Giải pháp kỹ thuật để thực hiện được phương pháp dẫn tiếp cận tỷ lệ tăng cường 4.3.1. Luận giải vấn đề Từ biểu thức (4.17) có thể đưa ra những nhận xét như sau: Vế bên phải của biểu thức (4.17) bao gồm các tham số cần phải đo hoặc ước lượng được nhờ bộ định hướng con quay quang điện tử hoặc các thuật toán xử lý thông tin hiện đại trên đầu tự dẫn của tên lửa có điều khiển. Để thực hiện được phương pháp dẫn mới này, những tham số cần phải đo hoặc ước lượng bao gồm: φ p , Vtc , Dp , aMT . Trên đầu tự dẫn lớp tên lửa tự dẫn hồng ngoại chỉ có thể trực tiếp đo được tham số về tốc độ góc đường ngắm TL-MT φ p . Còn lại các tham số cự
2. 110 4.3.2. Xây dựng hệ phương trình trạng thái phi tuyến của mô hình động hình học TL-MT Từ (4.5) và (4.7), mô hình động hình học TL-MT có tính đến sự cơ động của MT được biểu diễn bởi hệ phương trình vi phân phi tuyến như sau: DD φ 2 ; p p p   2Dp φ p aMT a TL φ p ; (4.18) DDp p aMT aMT  MT . MT Trong hệ (4.18) tốc độ góc đường ngắm TL-MT được xác định bằng bộ định hướng con quay quang điện tử trên đầu tự dẫn của tên lửa và được thể hiện bởi phương trình đo lường tuyến tính sau: z φ  ; (4.19) φp φ  p trong đó:  – nhiễu đo lường, dạng tạp trắng Gauss với phương sai σ2 . φ p φ Ta định nghĩa các trạng thái sau cho bộ ước lượng trạng thái Kalman:  x1 Dp – cự ly tương đối TL-MT; x2 Dp – vận tốc tiếp cận TL-MT; x3 φ p – góc đường ngắm TL-MT; x4 φ p – tốc độ góc đường ngắm TL-MT; x5 aMT – gia tốc pháp tuyến của mục tiêu. Từ (4.18) và (4.19), hệ phương trình phi tuyến trong không gian trạng thái của mô hình động hình học TL-MT có thể được viết dưới dạng sau: x f x,; u ξx (4.20) z Hx ξz , T   trong đó: x Dp D pφ p φ p a MT – vector trạng thái; u aTL – vector điều khiển; z z – vector đo lường; H 0 0 0 1 0 – ma trận đo lường; φ p  
3. 112 1Ts 0 0 0 2 Ts x41 0 2 T s x 1 x 4 0 0 0 1T 0 f s AI()k T 2x x x a 2x 2 x 1 ; s x TTTT2 4 5 TL 40 1 2 sx2 sx s x s x 1 1 1 1 1 0 0 0 0 1 Ts MT HH(k )  0 0 1 0 0  . Mô hình tuyến tính hóa trong miền rời rạc (4.22) sẽ được sử dụng trong Thuật toán 2.2 của bộ lọc Kalman mở rộng để ước lượng tham số trạng thái mục tiêu trong vòng điều khiển tự dẫn TL-MT. Lưu đồ thuật toán tổng hợp bộ lọc Kalman mở rộng EKF sử dụng Thuật toán 2.2 được mô tả ở Hình 4.6. Hình 4.6. Lưu đồ thuật toán tổng bộ lọc Kalman mở rộng EKF ước lượng trạng thái của mục tiêu trong vòng điều khiển tự dẫn
4. 114 trong đó, Ka là hệ số lệnh của TL hay tỷ số giữa vận tốc góc của tên lửa và góc lật cánh lái; Ta là hằng số thời gian của hệ tự động lái;  là hệ số giảm chấn của hệ thống ổn định tên lửa tự dẫn. Sơ đồ khối hệ tự động lái được mô tả trên Hình 4.8. Hình 4.8. Mô hình khối tự động lái Khối điều khiển mô phỏng luật dẫn tiếp cận tỷ lệ tăng cường có sơ đồ cấu trúc được mô tả trên Hình 4.9. Hình 4.9. Khối điều khiển mô phỏng luật dẫn tiếp cận tỷ lệ tăng cường Bộ lọc Kalman mở rộng (EKF_Estimator) được sử dụng để ước lượng các tham số trạng thái của mục tiêu trong vòng điều khiển tự dẫn với vector T   6 trạng thái x Dp D pφ p φ p a MT và thời gian trích mẫu Ts 10 ( s ) . Khối chương trình mô phỏng bộ lọc Kalman mở rộng được viết trên code S- function và được minh họa trong phần phụ lục của luận án.
5. 116 o θMT 10 ; Ka 3; Ta 0.75( s );  0.707; tại thời điểm t 1 ( s ) mục tiêu cơ 2 động với gia tốc aMT 10 ( m / s ) . Kết quả mô phỏng với luật dẫn tiếp cận tỷ lệ tăng cường trong trường hợp bắn đuổi mục tiêu được thể hiện trên các hình (4.11) - (4.13). Quỹ đạo của tên lửa và mục tiêu trong trường hợp bắn đuổi mục tiêu máy bay phản lực được thể hiện trên Hình 4.11. Hình 4.11. Quỹ đạo TL-MT khi bắn đuổi mục tiêu máy bay phản lực Gia tốc của tên lửa (Hình 4.12) trong trường hợp này có dao động để tăng tính quan sát được cho bộ ước lượng trạng thái Kalman mở rộng. Hình 4.12. Đồ thị gia tốc của tên lửa khi bắn đuổi mục tiêu máy bay phản lực Đồ thị đặc tuyến lệnh bù thích nghi dịch tâm điểm ngắm mục tiêu trong phương pháp dẫn tiếp cận tỷ lệ tăng cường được thể hiện trên Hình 4.13.
6. 118 Hình 4.15. Đồ thị gia tốc của tên lửa khi bắn đón mục tiêu máy bay phản lực Gia tốc của tên lửa (Hình 4.15) trong trường hợp này có dao động để tăng tính quan sát được cho bộ ước lượng trạng thái Kalman mở rộng. Đồ thị đặc tuyến lệnh bù thích nghi dịch tâm điểm ngắm mục tiêu trong phương pháp dẫn tiếp cận tỷ lệ tăng cường được thể hiện trên Hình 4.16. Hình 4.16. Đồ thị đặc tuyến lệnh bù thích nghi dịch tâm điểm ngắm mục tiêu khi bắn đón mục tiêu máy bay phản lực Ta thấy trong pha đầu và pha giữa của quá trình tự dẫn thì lệnh bù thích nghi gần bằng không. Trong pha cuối, lệnh bù thích nghi tăng nhanh. Việc đánh giá chất lượng của phương pháp dẫn tỷ lệ tăng cường so với phương pháp dẫn tỷ lệ truyền thống được thực hiện qua việc so sánh độ trượt tức thời [1], [57]: 2 Dp h( t ) φ p . (4.24) Vtc
7. 120 2 2 phương sai nhiễu σMT 0.75(m / s ) ; tần số dao động riêng của vector đường ngắm TL-MT: F 3; biên độ lệnh bù thích nghi được giới hạn trong khoảng 15% của tốc độ góc lớn nhất của con quay đầu tự dẫn: φ tn 2(deg/ sec) [67- 68]. 4.4.3.1. Trường hợp bắn đuổi mục tiêu máy bay trực thăng: Các tham số mô phỏng ban đầu khi bắn đuổi mục tiêu máy bay trực thăng o như sau: Dp (0) 1500 ( m ) ; φp (0) 25 ; XMT (0) 0; YMT (0) 1000( m ); o o θTL 35 ; θMT 10 ; Ka 3; Ta 0.55( s );  0.707; tại thời điểm t 1 ( s ) 2 mục tiêu máy bay trực thăng cơ động với gia tốc aMT 5 ( m / s ) . Kết quả mô phỏng với luật dẫn tiếp cận tỷ lệ tăng cường trong trường hợp bắn đuổi mục tiêu được thể hiện trên các Hình (4.17) và Hình (4.18). Quỹ đạo của tên lửa và mục tiêu trong mặt phẳng thẳng đứng và gia tốc của tên lửa trong trường hợp bắn đuổi mục tiêu máy bay trực thăng được thể hiện trên Hình 4.17. Hình 4.17. Kết quả mô phỏng luật dẫn tỷ lệ tăng cường khi bắn đuổi mục tiêu máy bay trực thăng: a – Quỹ đạo TL-MT; b – Gia tốc của tên lửa. Đồ thị đặc tuyến lệnh bù thích nghi dịch tâm điểm ngắm mục tiêu trong phương pháp dẫn tiếp cận tỷ lệ tăng cường được thể hiện trên Hình 4.18. Ta thấy trong pha đầu và pha giữa của quá trình tự dẫn thì lệnh bù thích nghi gần bằng không. Trong pha cuối, lệnh bù thích nghi tăng nhanh.
8. 122 Hình 4.20. Đồ thị đặc tuyến lệnh bù thích nghi dịch tâm điểm ngắm mục tiêu khi bắn đón mục tiêu máy bay trực thăng Đồ thị đặc tuyến lệnh bù thích nghi dịch tâm điểm ngắm mục tiêu trong phương pháp dẫn tiếp cận tỷ lệ tăng cường được thể hiện trên Hình 4.20. Khảo sát đồ thị đặc tuyến lệnh bù thích nghi dịch tâm điểm ngắm trong hình 4.20, ta thấy trong pha đầu và pha giữa của quá trình tự dẫn thì lệnh bù thích nghi gần bằng không. Trong pha cuối khi tên lửa cơ động ở giây thứ 3 đến thứ 4 thì lệnh bù thích nghi tăng nhanh. Các kết quả mô phỏng trên các Hình 4.17 b và Hình 4.19 b về gia tốc pháp tuyến của tên lửa cho thấy sự cơ động của tên lửa cần có để tăng cường tính quan sát được cho hệ thống điều khiển trong quá trình tự dẫn. Các kết quả so sánh giá trị độ trượt tức thời của phương pháp dẫn tỷ lệ tăng cường với phương pháp dẫn tiếp cận tỷ lệ truyền thống theo (4.24) cho lớp tên lửa tự dẫn hồng ngoại tương ứng với hai trường hợp bắn đuổi và bắn đón mục tiêu máy bay trực thăng [68] được thể hiện trong bảng 4.2. Các kết quả so sánh giá trị độ trượt tức thời được mô tả bảng 4.2 cho thấy độ trượt tức thời tại điểm va chạm của luật dẫn tiếp cận tỷ lệ tăng cường nhỏ hơn nhiều so với luật dẫn tiếp cận tỷ lệ truyền thống trong cả hai chế độ bắn đuổi và bắn đón mục tiêu máy bay trực thăng.
9. 124 (4.21) trong Matlab/Simulink bằng lệnh “rank(G H )” cho kết quả bằng 5. Như vậy hệ thống có thể quan sát được hoàn toàn các trạng thái. Kết quả được in ra trong WorkSpace/Command window như trong Hình 4.21. Hình 4.21. Kiểm tra tính quan sát được của hệ thống Kết quả mô phỏng ước lượng tọa độ của mục tiêu và sai số trung bình bình phương (MSE – Mean Square Error) tương ứng được thể hiện trên các Hình 4.22 - 4.24: Hình 4.22. Ước lượng cự ly mục tiêu
10. 126 4.5. Kết luận chương 4 Trong chương 4, luận án đã xây dựng được mô hình phi tuyến động hình học tự dẫn TL-MT có tính đến sự cơ động của mục tiêu và sơ đồ cấu trúc vòng điều khiển kín tên lửa tự dẫn hồng ngoại. Luận án đã nghiên cứu cải tiến luật dẫn tiếp cận tỷ lệ truyền thống và tổng hợp được luật dẫn tiếp cận tỷ lệ tăng cường cho lớp tên lửa tự dẫn hồng ngoại có tính đến lượng bù dịch tâm điểm ngắm mục tiêu ở pha cuối của quá trình tự dẫn. Các kết quả mô phỏng với các trường hợp cơ động khác nhau của mục tiêu khi bắn đón, bắn đuổi mục tiêu máy bay phản lực, máy bay trực thăng đã chứng tỏ phương pháp dẫn tiếp cận tỷ lệ tăng cường có những ưu điểm vượt trội so với phương pháp dẫn tỷ lệ truyền thống như độ trượt tức thời nhỏ (Bảng 4.1; Bảng 4.2), ít nhạy cảm với sự cơ động của mục tiêu và làm tăng tính quan sát được cho bộ ước lượng trạng thái trong hệ thống điều khiển của lớp tên lửa tự dẫn hồng ngoại chỉ có một kênh đo tốc độ góc đường ngắm TL-MT. Luận án đã đưa ra giải pháp đo, đánh giá các tham số trạng thái của mục tiêu như cự ly tương đối, vận tốc tiếp cận và gia tốc của mục tiêu trong vòng điều khiển tự dẫn TL-MT. Điểm mới của giải pháp đo, đánh giá các tham số này là được xây dựng trên cơ sở ứng dụng bộ ước lượng trạng thái Kalman mở rộng cho mô hình phi tuyến. Các kết quả mô phỏng, đánh giá hiệu quả làm việc của bộ lọc Kalman mở rộng trong Matlab/Simulink đã chứng minh ưu điểm của phương pháp này tối ưu về sai số (δ 0.54(m ) ; ; Dˆ δˆ 1.2 (m / s ) p D p δ 0.1(m / s2 ) ), ổn định ngay cả khi mục tiêu có cơ động và đơn giản khi aˆMT thực tế hóa bằng các giải pháp xử lý tín hiệu trên đầu tự dẫn của tên lửa. Các kết quả phân tích, nghiên cứu của chương 4 đã được công bố trong các bài báo khoa học số [2], [3], [5] của danh mục các công trình khoa học đã công bố của nghiên cứu sinh.
11. 128 quả mô phỏng đã chứng tỏ phương pháp dẫn tiếp cận tỷ lệ tăng cường có những ưu điểm vượt trội so với phương pháp dẫn tỷ lệ truyền thống như độ trượt tức thời nhỏ, ít nhạy cảm với sự cơ động của mục tiêu. 4. Đã ứng dụng được thuật toán lọc tối ưu Kalman mở rộng để ước lượng các tham số của mục tiêu trong vòng điều khiển kín của tên lửa tự dẫn. Bộ lọc Kalman mà luận án đề xuất chứng minh cho khả năng hiện thực hóa phương pháp dẫn tiếp cận tỷ lệ tăng cường mới trên cơ sở sử dụng các hệ thống tính toán trên khoang của tên lửa tự dẫn hồng ngoại. Những đóng góp mới của luận án: + Đề xuất giải pháp nâng cao chất lượng hoạt động bộ định hướng trên cơ sở bộ lọc Kalman mở rộng, Kalman thích nghi và điều khiển LQR cải thiện chất lượng hệ đo bám; + Tổng hợp luật dẫn tiếp cận tỷ lệ tăng cường cải tiến đảm bảo độ chính xác cao, có tính đến bù dịch tâm điểm ngắm và khả năng cơ động của mục tiêu. Hướng nghiên cứu tiếp theo của luận án là tích hợp được các kết quả nghiên cứu của luận án về các thuật toán lọc và luật điều khiển lên đầu tự dẫn hồng ngoại để nâng cao chất lượng hoạt động và hiệu quả chiến đấu cho tên lửa tự dẫn hồng ngoại, đồng thời phục vụ cho công tác nghiên cứu khoa học, đảm bảo kỹ thuật và cải tiến vũ khí trang bị trong quân đội.
12. 130 TÀI LIỆU THAM KHẢO Tiếng Việt 1. Lê Anh Dũng, Nguyễn Hữu Độ, Nguyễn Xuân Căn, Huỳnh Lương Nghĩa (1999), Lý thuyết bay và hệ thống điều khiển tên lửa phòng không (tập 1,2,3), Học viện Kỹ thuật Quân sự, Hà Nội. 2. Nguyễn Công Định (2002), Phân tích và tổng hợp các hệ thống điều khiển bằng máy tính, Nhà xuất bản KH&KT Hà Nội. 3. Nguyễn Doãn Phước (2016), Tối ưu hóa trong điều khiển và điều khiển tối ưu, NXB Bách Khoa Hà Nội. 4. Tô Văn Dực (2003), Hệ thống xử lý tín hiệu tên lửa tự dẫn hồng ngoại, NXB Quân đội Nhân dân. 5. Nguyễn Đức Cương (2002), Mô hình hoá và mô phỏng chuyển động của các khí cụ bay tự động, NXB Quân đội Nhân dân, Hà Nội. 6. Đàm Hữu Nghị, Nguyễn Văn Quảng (2001), Động học các hệ thống điều khiển tên lửa (tập 1, 2), NXB Quân đội Nhân dân, Hà Nội. 7. Đoàn Thế Tuấn, Trần Quý (2010), Giáo trình cơ sở thiết kế các hệ đo bám tọa độ số, Học viện Kỹ thuật Quân sự. 8. Tô văn Dực, Nguyễn Văn Sơn, Phạm Vũ Uy (2006), Động học bay và nguyên lý dẫn đối tượng bay điều khiển một kênh, NXB Khoa học và kỹ thuật, Hà Nội. 9. Đỗ Quang Việt (1998), Các hệ thống điều khiển tên lửa, Học viện Phòng không – Không quân. 10. Nguyễn Quang Vịnh, Nguyễn Minh Tuấn, Phan Tương Lai (2017), Ứng dụng lý thuyết điều khiển tối ưu cho chuyển động của các thiết bị bay, NXB Khoa học và Kỹ thuật. 11. Phạm Minh Hà (2004), Kỹ thuật mạch điện tử, Nhà xuất bản Khoa học và Kỹ thuật. 12. Nguyễn Thị Phương Hà (2008), Lý thuyết điều khiển hiện đại, Nhà xuất bản Đại học quốc gia Tp. HCM. 13. Dương Quốc Khánh (2017), Nghiên cứu, phân tích hệ thống điều khiển tên lửa phòng không tầm thấp Igla, Báo cáo khoa học đề tài nghiên cứu cấp Bộ quốc phòng. 14. Nguyễn Trung Kiên (2015), Xây dựng phương pháp tổng hợp hệ thống điều khiển các đài quan sát tự động định vị từ xa các đối tượng di động, Luận án Tiến sĩ kỹ thuật, Viện KH-CN quân sự.
13. 132 26. Chen R. H., Speyer J. L., Lianos D. (2007), Homing missile guidance and estimation under agile target acceleration. Journal of guidance control and dynamics, Vol. 30(6), pp. 1577-1589. 27. Dryzek J., Ruebenbauer K. (1992) Planck’s constant determination from black‐body radiation. American journal of physics, Vol. 60(3), pp. 251- 253. 28. Gauggel R. (1981), Steering Device for Missiles, United States Patent- 4288050. 29. Grewal M. S. and Andrews A. P. (2001), Kalman filtering: Theory and Practice using MatLab, John Wiley & Sons. 30. Han S.H., Hong H.K., Choi J.S. (1997), Dynamic simulation of infrared reticle seekers and an efficient counter-countermeasure algorithm, Optical Engineering Vol. 36.8 pp. 2341-2345. 31. Hepner S. R., Geering H. P. (1990), Observability Analysis For Target Maneuver Estimation Via Bearing-Only and Bearing-Rate-Only Measurements, Journal of Guidance, Control, and Dynamics, Vol. 13, No. 6, pp. 977-983. 32. Huang Y., Song T. L. (2015), Iterated modified gain extended Kalman filter with applications to bearings only tracking. Journal of Automation and Control Engineering Vol. 3(6), pp. 475-479. 33. Kalman R.(1960), A new approach to linear filtering and prediction problems, Transactions ASME Journal of Basic Engineering 82, pp.35-44. 34. Kamen E. W., Su J. K. (1999), Introduction to optimal estimation. Springer Science & Business Media. 35. Kim D., Ha J., You K. (2011), Adaptive extended Kalman filter-based geolocation using TDOA/FDOA, International Journal of Control and Automation, Vol. 4(2), pp. 49-58. 36. Kim P., Jun B. E., Cho H. (2002), An Approximate Target Adaptive Guidance for IR Homing, In AIAA Guidance, Navigation, and Control Conference and Exhibit (p. 4842). 37. Kiong T. K., Qing G. W., Chieh H. C., Advances in PID control. Springer London, 1999. 38. Koruba Z., Krzysztofik I. (2017), A control with the use of LQR modified method in the gyroscope system of target tracking, Engineering Mechanics, pp. 486-489.
14. 134 51. Song T. L. (1997), Target Adaptive Guidance for Passive Homing Missiles, IEEE Transactions on Aerospace and Electronic Systems, Vol. 33, No. 1, pp. 312-316. 52. Song T. L. (1996), Observability of target tracking with bearings-only measurements, IEEE Transactions on Aerospace and Electronic Systems, pp. 1468-1472. 53. Taur D. R., Chern J. S. (1999), Passive ranging for dog-fight air-to-air IR missiles, In Guidance, Navigation, and Control Conference and Exhibit, pp. 4289-4295. 54. Taur D. R., Chern J. S. (2000), A modified proportional navigation guidance law for IR homing missiles. In AIAA Guidance, Navigation, and Control Conference and Exhibit, pp. 747-754. 55. Yang C. D, Chen H. Y. (1998), Nonlinear H robust guidance law for homing missiles, Journal of Guidance Control and Dynamics, Vol 21.6, pp. 882-890. 56. Yanushevsky R. (2018). Modern missile guidance. CRC Press. 57. Zarchan Paul, Tactical and strategic missile guidance, American Institute of Aeronautics and Astronautics, Inc., 2012. Tiếng Nga 58. Кашин В. М., Лифиц А. Л., Ефремов М. И. (2014). Основы проектирования переносных зенитных ракетных комплексов. Москва : МГТУ им. Н.Э. Баумана – 232 с. 59. Криксунов Л. З. (1991). Следящие системы с оптико-электронными координаторами. К.: Техника – 156 с. 60. Криксунов Л. З. (1978). Справочник по основам инфракрасной техники. 61. Казаков И. Е., Мишаков А. Φ. (1988), Авиационные управляемые ракеты, ВВИА им проф. Н.Е. Жуковского. 62. Кринецкий Е. И. (1970), Системы самонаведения, Москва "Машиностроение". 63. Магнус К. (1974), Гироскоп: Теория и применения: Пер. с нем. Мир. 64. Меркин Д. Р. (1987). Введение в теорию устойчивости движения (Vol. 297). М.: Наука. 65. Савинов В. А. (1993). Инфракрасная головка самонаведения. М. Издательство МАИ.
15. P-1 PHỤ LỤC Chương trình Matlab-Simulink mô phỏng vòng điều khiển kín tên lửa tự dẫn hồng ngoại 1. Sơ đồ mô phỏng trong môi trường Simulink sử dụng m-file S-function 2. Mô hình động hình học tên lửa – mục tiêu trong m-file S-function %=== % Mo hinh Dong hinh hoc TL-MT trong S-function %=== function [sys,x0,str,ts,simStateCompliance]=Missile_Target_Inter(t,x,u ,flag) switch flag, case 0, [sys,x0,str,ts,simStateCompliance] = mdlInitialize_Sizes; case {2,9} sys=[]; case 3, sys = mdl__Outputs(t,x,u); otherwise DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag)); end %===
16. P-3 XLAM=atan2(RTM2,RTM1); XLEAD=asin(VT*sin(BETA+XLAM)/VM); THET=XLAM+XLEAD; VM1=VM*cos(THET+HE); VM2=VM*sin(THET+HE); VTM1 = VT1 - VM1; VTM2 = VT2 - VM2; VC=-(RTM1*VTM1 + RTM2*VTM2)/RTM; while ~((VC =Thoigian)) t = k*ts[1]; if t<Tcd u = 0; elseif t<T_0CD u = (i-1)*lamda_cd*J_M; else u=0; end A_T(i,k)= XMT; RTM1 = RT1(i,k-1) - RM1(i,k-1); RTM2 = RT2(i,k-1) - RM2(i,k-1); Lamda = atan2(RTM2,RTM1); VTM1 = VT1 - VM1; VTM2 = VT2 - VM2; RTM = sqrt(RTM1^2 + RTM2^2); LAMDAD = (RTM1*VTM2 - RTM2*VTM1)/(RTM*RTM); VC = -(RTM1*VTM1 + RTM2*VTM2)/RTM; XNC = XNP*VC*LAMDAD; H = (RTM*RTM*LAMDAD)/VC; A_M(i,k) = XNC; VM(i,k) = VM(i,k-1); Teta(i,k) = Teta(i,k-1) + T*A_M(i,k)/VM(i,k); K(i,k) = A_M(i,k)/VM(i,k)*VM(i,k); BetaD = XMT/VT; Beta = Beta + T*BetaD; VT1 = - VT*cos(Beta); VT2 = VT*sin(Beta); VM1 = VM(i,k)*cos(Teta(i,k)); VM2 = VM(i,k)*sin(Teta(i,k)); RT1(i,k) = RT1(i,k-1)+ T*VT1; RT2(i,k) = RT2(i,k-1)+T*VT2; RM1(i,k) = RM1(i,k-1)+T*VM1; RM2(i,k) = RM2(i,k-1)+T*VM2; Do_Truot(i,k)=RTM(i); H_K(i,k) = Ht(i); k = k+1;
17. P-5 sizes.Feedthrough = 1; sizes.SampleTimes = 1; sys = sim_sizes(sizes); x0 = [0 0 0 0 0 ]; str = []; Ts = [1e-6 0]; sim_Compliance = 'Default_SimState'; % end mdlInitializeSizes %=== % mdlDerivatives function sys = mdl__Outputs(t,x,u) global Up0; global dU0; Up=[u(5); u(10)]; Utl=u(2); x1p=u(3); % vector trang thai x2p=u(4); % vector trang thai x3p=u(5); % vector trang thai x4p=u(2); % vector trang thai x5p=u(7); Xp =[x1p;x2p;x3p;x4p;x5p]; % vector trang thai tien nghiem Pp1=u(8); % hiep phuong sai tien nghiem Pp2=u(9); % hiep phuong sai tien nghiem Pp3=u(10); % hiep phuong sai tien nghiem Pp4=u(11); % hiep phuong sai tien nghiem Pp5=u(12); % hiep phuong sai tien nghiem Pp=[Pp1;Pp2;Pp3;Pp4;Pp5]; % Vector hiep phuong sai tien nghiem nxe=5; nye=1; Pkp=zeros(nxe,nxe); for i=1:nxe Pkp(i,i)=Pp(i,1); end % Ma tran trong luong Qe =blkdiag(0.05, 0.5, 0.2, 0.2, 0.1); % Ma tran trong luong he thong Re=0.05; % Ma tran trong luong cua nhieu %=== Aekf=zeros(5,5); Bekf=zeros(5,1);
18. P-7 4. Vẽ đồ các đồ thị cho mô hình điều khiển TL-MT close all; figure(1); plot(t,ArrayRT1(:,1),'b',t,ArrayRM1(:,1),'r','linewidth', 2); grid on; title('Quỹ đạo TL-MT); xlabel('Cự li ngang [km]'); ylabel('Độ cao bay [km]'); pause hold off close all; figure(2); plot(t,a_mis,'g:','linewidth',2); legend('Gia tốc tên lửa'); xlabel('Thời gian [sec]'); ylabel('Gia tốc tên lửa [m/sec^2]'); grid on; pause hold off close all; figure(3); plot(t,delta_a_tn,'b','linewidth',2); legend('Lệnh bù thích nghi'); xlabel('Thời gian [sec]'); ylabel('Tốc độ góc [Deg/sec]'); grid on; pause hold off close all; figure(4); plot(t,D_true,'r',t,D_EKF,'b','linewidth',2); legend('Ước lượng cự ly mục tiêu'); xlabel('Thời gian [s]'); ylabel('Cự ly MT, [Km]'); grid on; hold on; figure(5); plot(t,mse_D;'b','linewidth',2); legend('Sai số cự ly'); xlabel('Thời gian [s]');