Xây dựng thuật toán xác định tọa độ góc của thiết bị bay trong hệ thống điều khiển từ xa trên cơ sở ứng dụng bộ lọc Kalman

Tên lửa & Thiết bị bay N. V. Bàng, , P. H. Long, “Xây dựng thuật toán xác định ứng dụng bộ lọc Kalman.” 180 XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH TỌA ĐỘ GÓC CỦA THIẾT BỊ BAY TRONG HỆ THỐNG ĐIỀU KHIỂN TỪ XA TRÊN CƠ SỞ ỨNG DỤNG BỘ LỌC KALMAN Nguyễn Văn Bàng1*, Đoàn Thế Tuấn2, Nguyễn Quang Hùng3, Phương Hữu Long4 Tóm tắt: Bài báo trình bày các kết quả nghiên cứu, phân tích, cánh thức xây dựng thuật toán xác định các tham số tọa độ góc của mục tiêu và thiết bị bay trong hệ thống điều khiển tên lửa từ xa trên cơ sở ứng dụng bộ lọc Kalman. Các kết quả mô phỏng cho thấy những ưu điểm khi sử dụng thuật toán. Do đó phương pháp này có thể được áp dụng để xây dựng hàm truyền của các hệ xác định tọa độ phục vụ cho việc tổng hợp vòng điều khiển tên lửa từ xa. Từ khóa: Thiết bị bay; Tên lửa; Mục tiêu; Bộ lọc Kalman. 1. MỞ ĐẦU Bài toán lọc số liệu trong trường hợp này được hiểu là bài toán xác định, đánh giá tối ưu các tọa độ pha của mục tiêu hoặc của thiết bị bay (TBB) theo các phép đo nhận được cho đến thời điểm hiện tại [1, 2]. Khi thiết kế các hệ bám trong hệ thống điều khiển tên lửa từ xa, ta có thể ứng dụng thuật toán lọc Kalman để xác định các tham số chuyển động của mục tiêu và của TBB. 2. XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH TỌA ĐỘ GÓC CỦA MỤC TIÊU VÀ THIẾT BỊ BAY 2.1. Tóm tắt thuật toán lọc Kalman Đối tượng điều khiển hoặc quá trình được mô tả bởi phương trình trạng thái [2]: x(t)= (t) (t)+ (t) (t)+ (t)x A x B u ξ (1) Phương trình quan sát có dạng: (t)= (t) (t)+ (t)zz H x ξ (2) Trong đó, ( )tx là véc tơ kích thước n x 1; u là véc tơ tín hiệu tiền định kích thước r x 1; ( )tz là véc tơ kích thước m x 1 ( nm  ); xξ , zξ là các tạp trắng Gauss, không tương quan với nhau. Thuật toán lọc Kalman sẽ cho đánh giá tối ưu theo tiêu chuẩn cực tiểu trung bình bình phương sai số đánh giá  ˆ ˆ( ) ( )TM  x x x x , trong đó, xˆ là giá trị ước lượng trạng thái. Thuật toán như sau: ˆ ˆ ˆ( )   x Ax Bu K z Hx , 0ˆ (0) x x ; (3) T -1K = PH G (4)  T T -1P = AP + PA + R - PH G HP , 0P(0) = P (5) Trong đó, P là ma trận tương quan sai số của bộ lọc; G và R tương ứng là ma trận cường độ của tạp quan sát z và tạp quá trình x . 2.2. Thuật toán xác định tọa độ góc của thiết bị bay - Xây dựng mô hình không gian trạng thái mô tả chuyển động của thiết bị bay Xét chuyển động của TBB trong mặt phẳng thẳng đứng (Hình 1). Trong đó, Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 181 thiết bị định hướng đặt tại O dùng để đo góc tà ε và cự ly r của TBB P; Thiết bị bay P có tốc độ V và gia tốc pháp tuyến Hj . Ta có: r = Vcos(θ - ε) (6) rε = Vsin(θ - ε) (7) Vi phân (7) và sử dụng (6) nhận được: rε +2rε =Vsin(θ - ε)+Vθcos(θ - ε)  (8) Do Hj =Vθ  , (8) có thể biến đổi về dạng: Trε +2rε = j  (9) Trong đó, T Hj = Vsin(θ - ε)+ j cos(θ - ε)  là hình chiếu gia tốc của TBB lên phương vuông góc với véc tơ cự ly r. Từ (9), đặt 1x  , 2x = ε , Hju = r và giả thiết cos(θ - ε) 1 . Khi này, mô hình chuyển động của TBB được biểu diễn như là một hệ Cauchy thông thường dạng: 1 2 2 2 x = x 2r x = - x +u+w r        (10) Trong đó w - là tạp trắng Gauss, tương ứng với thành phần V sin(θ - ε) r  . Thiết bị định hướng đo tọa độ góc của TBB (thành phần vị trí ε) với sai số nhất định. Sai số này được mô hình hóa bởi tạp trắng Gauss. Như vậy phương trình quan sát có dạng: 1z = x +η (11) Trong (10), (11) w và η là tạp trắng Gause độc lập với kỳ vọng bằng không, có cường độ tương ứng là R và G đã biết. - Ứng dụng thuật toán lọc Kalman xác định tọa độ góc của thiết bị bay Giả thiết, r và r nhận được từ hệ bám cự ly và tốc độ; u nhận được từ hệ lập lệnh, khi đó thuật toán lọc Kalman ứng dụng cho mô hình (10), (11), có dạng: Hình 1. Biểu diễn mối quan hệ động hình học của thiết bị bay trong mặt phẳng thẳng đứng. Y X 0 Tên lửa & Thiết bị bay N. V. Bàng, , P. H. Long, “Xây dựng thuật toán xác định ứng dụng bộ lọc Kalman.” 182 ˆ ˆ ˆ ˆ ˆ ˆ 1 2 1 1 2 2 2 1 x = x +k (z - x ) 2r x = - x +u+k (z - x ) r         (12) Trong đó hệ số k1 và k2 là các hệ số có thứ nguyên phù hợp được xác định từ việc giải phương trình Ricati:     T T -1 T -1 P = AP + PA + R - PH G HP K = PH G (13) K là véc tơ có các hệ số k1 và k2, kích thước (2 x 1); 0 1 = 2r 0 - r         A là ma trận động học của quá trình, kích thước 2 x 2; 0 0 0 R       R = là ma trận cường độ tạp quá trình, kích thước 2 x 2; GG là cường độ tạp quan sát, kích thước 1x1;  1 0H là ma trận quan sát, kích thước 1 x 2; P là ma trận tương quan sai số đánh giá, kích thước 2 x 2; Dạng vô hướng của phương trình (13) cho hệ (10) với giá trị quan sát được (11) có dạng: 2 11 12 11 12 22 12 11 12 2 22 22 12 p = 2p - p / G p = p - 2rp / r - p p / G p = -4rp / r - p / G+ R           (14) Thông thường để đơn giản tính toán trong thiết bị thực, K được xác định ở chế độ xác lập, tức là P = 0 . Khi này, nhận được nghiệm của (14) với r 0 r   như sau: 11 12 22p = 2G RG ; p = RG ; p = 2R RG ; (15) Khi đó, 1 11 2 12k = p / G = 2 R / G ;k = p / G = R / G . Ký hiệu 4ω = R / G nhận được 21 2k = 2ω ,k = ω . Với s là biến phức biểu thị hàm là ảnh Laplace, hàm truyền của bộ lọc có dạng: 2 1 2 2 2 2 1 1 2 ˆ ( ) ( ) ( ) 2 1 2 1 s x s z s u s s s s s              (16) Có thể thấy rằng, bộ lọc có tính chất của khâu dao động với tần số cộng hưởng  và hệ số suy giảm 0, 707  . 2.3. Thuật toán xác định tọa độ góc của mục tiêu - Xây dựng mô hình không gian trạng thái mô tả chuyển động của mục tiêu Tương tự như xác định tham số chuyển động của TBB, việc xác định tham số Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 183 chuyển động của mục tiêu cũng có thể sử dụng thuật toán (12), trong đó biến điều khiển u chưa biết nên coi nó là quá trình ngẫu nhiên tính gộp vào tạp w . Khi đó thuật toán hình thành bộ lọc đánh giá (ước lượng) tọa độ góc và tốc độ góc. Tuy vậy, một số phương pháp dẫn đòi hỏi phải đánh giá đạo hàm bậc hai của tọa độ góc [1]. Để giải quyết vấn đề này cần phải mở rộng hệ (10) đến bậc ba bằng cách vi phân phương trình (8) và chuyển các thành phần sang vế phải, ngoại trừ rε . Khi đó phương trình chuyển động của mục tiêu được viết như sau [3]: r.ε= w hay 1 ε = w r  (17) Trong đó w - hàm số, có thể được biểu diễn như là một quá trình ngẫu nhiên. Hệ phương trình ngẫu nhiên mô tả động học của mục tiêu có thể được viết như sau [4]: 1 2 2 3 3 x = x x = x 1 x = w r           (18) Trong đó,  1 2 3x = ε, x = ε, x = ε và phương trình quan sát tương tự (11). 1z = x +η (19) Với η có cường độ G và w có cường độ là R. Phương trình (18) được viết dưới dạng phương trình ma trận: 0 1 0 0 0 0 1 0 0 0 0 1/ r 1 1 2 2 3 3 x x x = x +w x x                                      (20) - Ứng dụng thuật toán lọc Kalman xác định tọa độ góc của mục tiêu Phương trình lọc Kalman mô hình (20), (19) có dạng: 1 2 3 1 2 3 4 1 3 5 1 ˆ ˆ ˆ( ) ˆ ˆ ˆ( ) ˆ ˆ( ) x = x +k z - x x = x +k z - x x = k z - x         (21) Trong đó hệ số k3, k4 và k5 xác định từ việc giải phương trình Ricati:     T T -1 T -1 P = AP + PA + R - PH G HP K = PH G (22) K là véc tơ các hệ số k3, k4 và k5, kích thước (3 x 1) 0 1 0 0 0 1 0 0 0          A là ma trận động học của quá trình, kích thước 3 x 3. Tên lửa & Thiết bị bay N. V. Bàng, , P. H. Long, “Xây dựng thuật toán xác định ứng dụng bộ lọc Kalman.” 184 * 0 0 0 0 0 0 0 0 R          R là ma trận cường độ tạp quá trình, kích thước 3 x 3; * 2/R R r *GG là ma trận cường độ tạp quan sát, kích thước 1 x 1; * 2/G G r  1 0 0H là ma trận quan sát, kích thước 1 x 3; P - ma trận tương quan sai số đánh giá, kích thước 3 x 3; Ở dạng vô hướng, phương trình (22) có thể viết lại như sau: 2 * 11 12 112 /p p p G  * 12 22 13 11 12 /p p p p p G   * 13 23 11 13 /p p p p G  2 * 22 23 122 /p p p G  * 23 33 12 13 /p p p p G  * 2 * 33 13 /p R p G  Ở chế độ xác lập ( P = 0 ), nghiệm của (22) nhận được: 3* *2 * 11 2p G G R ; 3 *2 * 12 2p G R ; * * 13p G R 3* *2 * * 11 6 6 3 * * * 2 2 2 2 p G G R R R k G G G G      * 212 3 3 4 * * 2 2 2 p R R k G G G     ; * 313 5 * * p R R k G G G     Giá trị thiết lập của các hệ số bộ lọc: 2 33 4 5 1/ 6k = 2ω ; k = 2ω ; k = ω ; ω = (R / G) (23) Hàm truyền của bộ lọc khi các hệ số thiết lập có dạng: 2 2 1 2 3 2 3 s s 1+2 +2 x (s) ω ω= s s sz(s) 1+2 +2 + ω ω ω (24) Có thể thấy rằng, cả thuật toán xác định tọa độ góc mục tiêu và TBB được xác định bởi một tham số duy nhất là  . Tuy vậy việc xây dựng mô hình không gian trạng thái để tổng hợp bộ lọc cũng tồn tại những áp đặt nhất định. Do đó việc hiệu chỉnh tham số cho bộ lọc cần được kiểm chứng và hiệu chỉnh qua thử nghiệm mô phỏng. 3. KẾT QUẢ MÔ PHỎNG VÀ ĐÁNH GIÁ Việc mô phỏng, khảo sát bộ lọc tọa độ góc mục tiêu, tên lửa được xem xét trong thành phần của vòng điều khiển tên lửa từ xa. Sơ đồ các khối mô phỏng được chỉ ra trên hình 2. Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 185 Hình 2. Sơ đồ khối vòng điều khiển tên lửa từ xa. + Trong đó; hệ lập lệnh sử dụng phương pháp tạo lệnh theo phương pháp dẫn 3 điểm. + Tham số mục tiêu: Vận tốc mục tiêu: Vmt = 500 m/s Cự li ngang của mục tiêu: Dmtx = 16200 m Độ cao bay của mục tiêu: H = 10100 m Thời gian mục tiêu bắt đầu cơ động: TCD = 5 s Thời gian mục tiêu kết thúc cơ động: T0CD = 8 s + Tên lửa có hàm số truyền bằng 1, có vận tốc Vp = 900 m/s - Trường hợp mục tiêu không cơ động (jmt =0): Đồ thị quỹ đạo tên lửa - mục tiêu, đồ thị sai số vị trí, tốc độ, gia tốc của mục tiêu và tên lửa được chỉ ra trên các Hình 3a, 3b, 3c, 3d, 3e, 3f. Hình 3a. Đồ thị quỹ đạo tên lửa - mục tiêu. Hình 3b. Đồ thị sai số vị trí góc mục tiêu. Hình 3c. Đồ thị sai số tốc độ góc mục tiêu. Hình 3d. Đồ thị sai số gia tốc góc mục tiêu. Tên lửa & Thiết bị bay N. V. Bàng, , P. H. Long, “Xây dựng thuật toán xác định ứng dụng bộ lọc Kalman.” 186 Hình 3e. Đồ thị sai số vị trí góc tên lửa. Hình 3f. Đồ thị sai số tốc độ góc tên lửa. - Trường hợp mục tiêu cơ động 3g (jmt = 3g): Đồ thị quỹ đạo tên lửa - mục tiêu, đồ thị sai số vị trí, tốc độ, gia tốc của mục tiêu và tên lửa được chỉ ra trên các Hình 4a, 4b, 4c, 4d, 4e, 4f. Hình 4a. Đồ thị quỹ đạo tên lửa - mục tiêu. Hình 4b. Đồ thị sai số vị trí góc mục tiêu. Hình 4c. Đồ thị sai số tốc độ góc mục tiêu. Hình 4d. Đồ thị sai số gia tốc góc mục tiêu. Hình 4e. Đồ thị sai số vị trí góc tên lửa. Hình 4f. Đồ thị sai số tốc độ góc tên lửa. Nhận xét: Sai số vị trí ( 47.10 rad của mục tiêu, 0,07 rad của tên lửa), tốc độ( 34.10 /rad s của mục tiêu, 0,03 /rad s của tên lửa) của mục tiêu và tên lửa, cũng như gia tốc thực tế của mục tiêu ( 4 23,5.10 /rad s ) so với đánh giá rất nhỏ, không đáng kể. Sai số tại điểm gặp của tên lửa và mục tiêu nhỏ. Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san TĐH, 04 - 2019 187 4. KẾT LUẬN Bằng kỹ thuật lọc tối ưu đã xây dựng được thuật toán xác định tọa độ góc mục tiêu và TBB làm cơ sở để hình thành hệ thống xác định tọa độ góc mục tiêu và TBB. Cấu trúc của bộ lọc đơn giản, việc lựa chọn các tham số cho bộ lọc dễ dàng (chỉ cần một tham số  ). Với việc xác định tọa độ TBB, thuật toán đã tính tới tín hiệu điều khiển u có được trên hệ lập lệnh nhằm tăng độ chính xác dẫn. Qua mô phỏng thấy rằng hệ thống làm việc với sai số nhỏ, ổn định. Bộ lọc Kalman (12), (21) - cung cấp đánh giá tọa độ pha của mục tiêu, TBB trong mặt phẳng đứng. Bộ lọc tương tự có thể được sử dụng cho mặt phẳng nghiêng và kênh cự ly. TÀI LIỆU THAM KHẢO [1]. D. Simon, “Kalman filtering with state constraints - a survey of linear and nonlinear algorithm”, Control Theory and Application (2010), IET, Vol.4, No. 8, pp. 1303 [2]. B. Uhrmeister, “Kalman filter for a missile with radar and/or imaging sensor”, J. Guid. Control Dyn, (1994). [3]. Толпегин О.А, Новиков В. Г,“Математические моделисистем наведения летательных аппаратов, Коломна, Издательство КИ (ф) МГОУ, (2011), 112с. [4]. Е.А. Федосов, В.Т. Бобронников, М.Н. Красилыщиков, В.И. Кухтенко, А.А. Лебедев, В.В. Малыщев, Е.В. Орлов, Б.В. Пучков, А.И. Силаев, В.А. Стефанов,“Динамическое проектирование систем управления автоматических маневренных летательных аппаратов”, Машиностроение, Москва, (1997), стр. 49. ABSTRACT AN ALGORITHM BASED ON KALMAN FILTER APPLICATION TO DETERMINE ANGLE COORDINATES OF FLYING DEVICE FOR THE REMOTE CONTROL SYSTEM The article presents a method suggesting an algorithm based on Kalman filter to determine angle coordinates of target and of flying device that are necessary to the remote controller system. Acording to the results obtained, this method can be used to determine transfer function of the objects' coordinates measuring system for the synthesis of remote control loop. Keywords: Flying device; Missile; Target; Kalman filter. Nhận bài ngày 30 tháng 01 năm 2019 Hoàn thiện ngày 08 tháng 3 năm 2019 Chấp nhận đăng ngày 15 tháng 3 năm 2019 Địa chỉ: 1 Khoa Kỹ thuật Điều khiển - Học viện KTQS; 2 Khoa Kỹ thuật Điều khiển - Học viện KTQS; 3 Viện Tự động hóa - Viện Khoa học và Công nghệ QS; 4 Khoa Kỹ thuật Hàng không - Học viện PKKQ. * Email: banghvpkkq@gmail.com.