Xây dựng thuật toán xác định tham số dẫn đường cho vũ khí chống ngầm

Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 31, 06 - 2014 117 XâY DựNG THUậT TOáN XáC địNH THAM Số DẫN đường CHO vũ khí chống NGầM TRươNG DUY TRUNG, NGUYễN QUANG VịNH Tóm tắt: Bài báo trình bày phương pháp ứng dụng bộ lọc Kalman phi tuyến mở rộng để xây dựng thuật toán xác định tham số dẫn đường cho vũ khí chống ngầm thả từ máy bay trên cơ sở kết hợp thông tin của thiết bị dẫn đường quán tính (gia tốc kế, con quay tốc độ góc) với từ kế, vận tốc kế và cảm biến áp suất khi chuyển động rơi cùng dù trong khí quyển và khi chuyển động trong môi trường nước. Từ khóa: Bộ lọc Kalman phi tuyến, Con quay tốc độ góc, Từ kế, Gia tốc kế, Vận tốc kế, Cảm biến áp suất. 1. ĐặT VấN Đề Vũ khí chống ngầm (ASWs) thả từ máy bay hoạt động trong thời gian ngắn và không gian hẹp nên hệ tọa độ địa lý 0 0 0 OX Y Z nơi máy bay thả vũ khí chống ngầm có thể xem là hệ tọa độ dẫn đường. Hệ tọa độ gắn liền b b b b G Y ZX có tâm trùng với tâm trọng lực b G (hình 1). Hình 1. Hệ tọa độ tham chiếu cho ASWs. Ma trận Côsin định hướng giữa hệ tọa độ gắn liền và hệ tọa độ địa lý được xác định thông qua 4 số Rodrig-Hamilton 0 1 2 3 , , ,    như sau [3]: 2 2 0 1 1 2 0 3 1 3 0 2 2 2 1 2 0 3 0 2 2 3 0 1 2 2 1 3 0 2 2 3 0 1 0 3 2 2 1 2 2 2 2 2 2 2 2 1 2 2 2 2 2 2 2 2 1 n b C                                                     (1) Các số Rodrig-Hamilton 0 1 2 3 , , ,    được xác định bằng cách giải các phương trình [3]: 0 1 2 3 1 0 3 2 2 3 0 1 3 2 1 0 2 - - - 2 - 2 - 2 - p q r p q r p q r p q r                             (2) trong đó, , ,p q r là các thành phần tốc độ góc quay tuyệt đối của hệ tọa độ gắn liền. Từ việc xác định được ma trận Côsin định hướng n b C sẽ xác định được thành phần gia tốc trong hệ tọa độ địa lý [3]: n b b f C a (3) trong đó, [ , , ]T N E D f f f f là thành phần của véc tơ gia tốc trong hệ tọa độ địa lý; [ , , ]T b bx by bz a a a a là chỉ số của gia tốc kế trong hệ tọa độ gắn liền. Vận tốc tâm khối của ASWs trong hệ tọa độ địa lý được xác định [3]: ; ; N N E E D D V f V f V f g      (4) Tọa độ tâm khối của ASWs trong hệ tọa độ địa lý được tính [3]: ; ; N E D x V y V z V     (5) Điều khiển & Tự động hóa T. D. Trung, N. Q. Vịnh, “Xây dựng thuật toán xác định vũ khí chống ngầm.” 118 Các góc định hướng của ASWs được xác định thông qua các tham số Rodrig-Hamilton [3]: 1 2 0 3 2 3 0 1 1 3 0 22 2 2 2 0 1 0 3 arcsin 2 2 2 2 ; (2 2 ); 2 2 1 2 2 1 arctgarctg                                          (6) Tuy nhiên các con quay vi cơ đo thành phần tốc độ góc thực tế thường cho các giá trị tham số đo được bao gồm thành phần tốc độ quay thực cộng với độ trôi và nhiễu, gia tốc kế cho giá trị đo bao gồm gia tốc chuyển động và nhiễu: 1 2 3 ; ; p p q q r r p b w q b w r b w           , (7) 4 5 6 ; ; x bx y by z bz a a w a a w a a w      , (8) trong đó, , , p q r b b b là các tham số biến đổi chậm thể hiện độ trôi của con quay và 1 2 3 4 5 6, , , , ,w w w w w w là các nhiễu đo thường có dạng tạp trắng (nhiễu Gauss). Việc giải các phương trình trên trong điều kiện con quay vi cơ và gia tốc kế cho giá trị đo như phương trình (7), (8) sẽ cho sai số và sai số này tăng theo thời gian. Để khắc phục vấn đề trên thực tế có nhiều phương pháp khác nhau [1], [2], [3]. Trong bài báo này nhóm tác giả ứng dụng bộ lọc Kalman phi tuyến mở rộng [4] xây dựng thuật toán dẫn đường trên cơ sở kết hợp giữa thiết bị đo quán tính (con quay vi cơ và gia tốc kế) với thiết bị đo không quán tính (từ kế, vận tốc kế, cảm biến áp suất) để ước lượng trực tiếp các tham số Rodrig - Hamilton. Phương pháp này cho phép khắc phục được sai số do nhiễu đo, độ trôi của con quay vi cơ và nhiễu đo gia tốc kế gây nên. 2. THUậT TOáN XáC ĐịNH THAM Số DẫN ĐƯờNG CHO Vũ KHí CHốNG NGầM giai đoạn chuyển động trong khí quyển Giai đoạn chuyển động rơi trong khí quyển ở chế độ có dù, vận tốc chuyển động của ASWs biến thiên nhỏ. Trong trường hợp này tác giả đề xuất phương án ứng dụng bộ lọc Kalman phi tuyến mở rộng ước lượng trực tiếp các tham số Rodrig - Hamilton trên cơ sở các thông tin quan sát do con quay vi cơ, gia tốc kế, từ kế cung cấp. Giả thiết trên vũ khí chống ngầm có ba con quay vi cơ, ba gia tốc kế, ba từ kế được gắn theo các phương của hệ tọa độ gắn liền (hình 2): 1 2 3 , ,T T T - các từ kế; 1 2 3 , ,A A A - các gia tốc kế; 1 2 3, ,C C C - các con quay vi cơ Hình 2. Bố trí con quay vi cơ đo tốc độ góc, gia tốc kế và từ kế. Từ (7) rút ra được: 1 2 3 , , , p p q q r r p b w q b w r b w           (9) Thay các phương trình (9) vào các phương trình (2) ta có: 0 1 1 2 2 3 3 2 ( ) ( ) ( ) p p q q r r b w b w b w                (10) 1 0 1 3 2 2 3 2 ( ) ( ) ( ) p p q q r r b w b w b w               (11) Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 31, 06 - 2014 119 2 3 1 0 2 1 3 2 ( ) ( ) ( ) p p q q r r b w b w b w               (12) 3 2 1 1 2 0 3 2 ( ) ( ) ( ) p p q q r r b w b w b w                (13) Nguyên lý lọc Kalman được mô tả như sau: Giả sử rằng chuyển động của vật thể được mô tả bởi hệ phương trình động học dưới dạng rời rạc như sau: 1 1 1( ) ( ) ( ), ( ) ( ).k k k k k kX F X G X k Z h X v k      (14) trong đó, 1,k kX X  là các giá trị của véc tơ trạng thái X (véc tơ n chiều) ở bước thứ k và bước thứ (k-1); G là ma trận hệ số nhiễu,  là véc tơ nhiễu động học một chiều có dạng tạp trắng; Véc tơ đầu ra kZ có thể đo được bằng các phương tiện đo; 1kF  là véc tơ hàm số F ở bước thứ k-1; f1, f2, ...fn là các hàm phi tuyến với biến số là véc tơ X ; kZ là giá trị của véc tơ đầu ra h (véctơ m chiều, thường m n ) ở bước thứ k được đo bởi thiết bị đo, 1 2( , ,....., ) T mh h h h ; v là véc tơ m chiều nhiễu đo có dạng tạp trắng với kỳ vọng toán học bằng 0.    ~ (0, ), ( ), ( ) , ( ) 0, ~ (0, ), ( ), ( ) , ( ) 0,T Tk k k kN Q E j k Q E k v N R E v j v k R E v k             ˆ ˆ( ) T k k k kkP E X X X X         trong đó, E là ký hiệu phép lấy kỳ vọng toán học. Thủ tục của thuật toán đánh giá trạng thái X trên cơ sở véc tơ quan sát Z theo Kalman như [4] và được thể hiện trên hình 3. Để thực hiện việc ứng dụng bộ lọc Kalman đánh giá các tham số 0 1 2 3 , , ,    tiến hành các bước sau: 2.1. Đặt các biến của véc tơ trạng thái X 1 0 2 1 3 2 4 3 5 6 7 8 9 10 ; ; ; ; ; ; ; ; ; p q r x x x x x p x q x r x b x b x b             (15) Hình 3: Sơ đồ xác định tham số dẫn đường khi kết hợp con quay vi cơ, gia tốc kế và từ kế. 2.2. Xây dựng hàm động học 1 1( )k kF X  Với cách đặt biến như (15) các phương trình (10) đến (13) được viết lại: Điều khiển & Tự động hóa T. D. Trung, N. Q. Vịnh, “Xây dựng thuật toán xác định vũ khí chống ngầm.” 120 1 2 8 1 3 9 2 4 10 3 2 8 3 9 4 10 2 1 3 2 4 3 2 ( ) ( ) ( ) ( ) ( ) ( ) p q r p q r x x x w x x w x x w x x x x x x x w x w x w                            (16) 2 1 8 1 3 10 3 4 9 2 1 8 3 10 4 9 1 1 3 3 4 2 2 ( ) ( ) ( ) ( ) ( ) ( ) p r q p r q x x x w x x w x x w x x x x x x x w x w x w                          (17) 3 1 9 2 2 10 3 4 8 1 1 9 2 10 4 8 1 2 2 3 4 1 2 ( ) ( ) ( ) ( ) ( ) ( ) q r p q r p x x x w x x w x x w x x x x x x x w x w x w                          (18) 4 1 10 3 2 9 2 3 8 1 1 10 2 9 3 8 1 3 2 2 3 1 2 ( ) ( ) ( ) ( ) ( ) ( ) r q p r q p x x x w x x w x x w x x x x x x x w x w x w                          (19) Tách các số hạng có thừa số nhiễu 1 2 3 , ,w w w và từ phương trình vi phân (16) đến (19), các hàm động học 1 2 3 4 , , ,f f f f viết dưới dạng rời rạc của chúng theo phương pháp Ơle là: 1 1 1 1 2 8 3 9 4 10 ( ) ( ) ( 1) / 2 [ ( 1)( ( 1)) ( 1) ( ( 1)) ( 1)( ( 1))] / 2 k p q r x k f X x k T x k x k x k x k x k x k                     (20) 2 2 1 2 1 8 3 10 4 9 ( ) ( ) ( 1) / 2 [ ( 1)( ( 1)) ( 1) ( ( 1)) ( 1)( ( 1))] / 2 k p r q x k f X x k x k x k x k x k x k x k T                    (21) 3 3 1 3 1 9 2 10 4 8 ( ) ( ) ( 1) [ ( 1)( ( 1)) ( 1)( ( 1)) ( 1)( ( 1))] / 2 k q r p x k f X x k T x k x k x k x k x k x k                    (22) 4 4 1 4 1 10 2 9 3 8 ( ) ( ) ( 1) / 2 [ ( 1)( ( 1)) ( 1)( ( 1)) ( 1)( ( 1))] / 2 k r q p x k f X x k T x k x k x k x k x k x k                    (23) ở đây, T là bước tính khi rời rạc hóa. Đối với bộ lọc Kalman phi tuyến mở rộng [4] các tham số không được mô tả bằng phương trình vi phân như các tốc độ góc , ,p q r sẽ được xác định như sau: 5 5 1 5 ( ) ( ) ( 1) k x k f X x k     ; 6 6 1 6 ( ) ( ) ( 1) k x k f X x k     ; 7 7 1 7 ( ) ( ) ( 1) k x k f X x k     (24) Độ trôi , ,p q rb b b cũng có thể viết dưới dạng: 8 8 1 8 ( ) ( ) ( 1) k x k f X x k     ; 9 9 1 9 ( ) ( ) ( 1) k x k f X x k     ; 10 10 1 10( ) ( ) ( 1)kx k f X x k   (25) Như vậy, từ các phương trình (20) đến (25) cho đầy đủ các hàm 1 2 10 , ,...,f f f của véc tơ hàm 1 1 ( ) k k F X   . Tiếp theo xác định ma trận chuyển vị 1 1 ˆ1 ( )k k k X X F X          bằng cách lấy đạo hàm riêng: 1 ij 1,10 1,10, ;k i j f i j x                (26) trong đó, 812 ( ( 1)) / 2p x k T    ; 913 ( ( 1)) / 2q x k T    ; 1014 ( ( 1)) / 2r x k T    ; 18 2 ( 1) / 2kx T  ; 19 3( 1) / 2x k T   ; 110 4 ( 1) / 2x k T   ; 821 ( ( 1)) / 2p x k T   ; 1023 ( ( 1)) / 2r x k T   ; 924 ( ( 1)) / 2q x k T    ; 28 1( 1) / 2x k T    ; 29 4 ( 1) / 2x k T   ; 210 3 ( 1) / 2x k T    ; 931 ( ( 1)) / 2q x k T   ; 1032 ( ( 1)) / 2r x k T    ; Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 31, 06 - 2014 121 834 ( ( 1)) / 2p x k T   ; 38 4 ( 1) / 2x k T    ; 39 1( 1) / 2x k T    ; 310 2 ( 1) / 2x k T   ; 1041 ( ( 1)) / 2r x k T   ; 942 ( ( 1)) / 2q x k T   ; 843 ( ( 1)) / 2p x k T    ; 48 3 ( 1) / 2x k T   ; 49 2 ( 1) / 2x k T    ; 410 1( 1) / 2x k T    ; ii 1  ; tất cả các ij đều bằng 0, 1,10 1,10;i j  . 2.3. Xác định ma trận hệ số nhiễu 1( )kG X  Từ các phương trình (16) đến (19) các hàm nhiễu động học được viết lại dưới dạng rời rạc theo phương pháp Ơle: 1 1 2 2 3 3 4 [ ( 1) ( 1) ( 1)] / 2g w x k w x k w x k T      (27) 2 1 1 3 3 2 4 [ ( 1) ( 1) ( 1)] / 2g w x k w x k w x k T       (28) 3 2 1 3 2 1 4 [ ( 1) ( 1) ( 1)] / 2g w x k w x k w x k T       (29) 4 1 3 2 2 3 1 [ ( 1) ( 1) ( 1)] / 2g w x k w x k w x k T      (30) Lấy đạo hàm riêng ,i j g w   nhận được ma trận hệ số nhiễu: ij 1,10 1,3, ;G g i j     (31) trong đó, 11 2 ( 1) / 2g x k T  ; 12 3 ( 1) / 2g x k T  ; 13 4 ( 1) / 2g x k T  ; 21 1 ( 1) / 2g x k T   ; 22 4 ( 1) / 2g x k T  ; 23 3 ( 1) / 2g x k T   ; 31 4 ( 1) / 2g x k T   ; 32 1( 1) / 2g x k T   ; 33 2 ( 1) / 2g x k T  ; 41 3 ( 1) / 2g x k T   ; 42 2 ( 1) / 2g x k T   ; 43 1 ( 1) / 2g x k T   ; các hệ số ij g khác đều bằng 0. Ma trận Q là ma trận đường chéo với các giá trị trên đường chéo là các phương sai của các con quay vi cơ, tức là phương sai của các nhiễu 1 2 3 , ,w w w . 2.4. Xây dựng các hàm quan sát ( )kh X Vấn đề tiếp theo là tìm các hàm quan sát i h . Như trên đã nêu trong ASWs có ba từ kế đo ba thành phần 1 2 3 , ,z z z trong hệ tọa độ gắn liền của véc tơ từ trường trái đất nơi ASWs đang hoạt động. Giả sử từ trường trái đất vùng ASWs hoạt động không thay đổi và có các giá trị lần lượt theo ba phương của hệ tọa độ địa lý như sau: , , x y z B B B (các giá trị này được đo một lần bằng các từ kế khi hệ tọa độ gắn liền trùng với hệ tọa độ địa lý hoặc được đo bằng cách nào đó). Dễ dàng nhận thấy giữa 1 2 3 , ,z z z và , , x y z B B B có quan hệ mật thiết với nhau qua ma trận Côsin chỉ phương, tức là: 1 2 3 [ , , ] ( ) [ , , ]T n T T b x y z z z z C B B B (32) Triển khai biểu thức (32) với ma trận nbC như (1) ta có: 2 2 1 0 1 1 2 0 3 1 3 0 2 (2 2 1) (2 2 ) (2 2 ) x y z z B B B                (33) 2 2 2 1 2 0 3 0 2 2 3 0 1 (2 2 ) (2 2 1)) (2 2 ) x y z z B B B                (34) 2 2 3 1 3 0 2 2 3 0 1 0 3 (2 2 ) (2 2 ) (2 2 1) x y z z B B B                (35) Với cách đặt biến (15), từ (33) đến (35) ta có ba hàm quan sát: Điều khiển & Tự động hóa T. D. Trung, N. Q. Vịnh, “Xây dựng thuật toán xác định vũ khí chống ngầm.” 122 2 2 1 1 1 2 2 3 1 4 2 4 1 3 ( ) ( ) (2 ( ) 2 ( ) 1) (2 ( ) ( ) 2 ( ) ( )) (2 ( ) ( ) 2 ( ) ( )) k x y z z k h X x k x k B x k x k x k x k B x k x k x k x k B         (36) 2 2 2 3 1 4 2 2 1 3 3 4 1 2 ( ) ( ) (2 ( ) ( ) 2 ( ) ( )) (2 ( ) 2 ( ) 1) (2 ( ) ( ) 2 ( ) ( )) k x y z z k h X x k x k x k x k B x k x k B x k x k x k x k B         (37) 3 3 2 4 1 3 2 2 3 4 1 2 1 4 ( ) ( ) (2 ( ) ( ) 2 ( ) ( )) (2 ( ) ( ) 2 ( ) ( )) (2 ( ) 2 ( ) 1) k x y z z k h X x k x k x k x k B x k x k x k x k B x k x k B         (38) Giả thiết trong vùng hoạt động của ASWs véc tơ gia tốc trọng trường có phương và giá trị không đổi. Chiếu véc tơ đó xuống hệ tọa độ địa lý có được các thành phần như sau 0 0 0, ,x y zg g g (các thành phần này có thể xác định bằng cách đọc gia tốc kế khi làm cho hệ tọa độ gắn liền trùng với hệ tọa độ địa lý và cho ASWs bất động). Gia tốc kế không đo gia tốc chuyển động mà cho chỉ số của gia tốc cảm nhận (gia tốc biểu kiến) [2], tức là: ; ; bx N x by E y bz D z a f g a f g a f g      , trong đó , ,N E Df f f là hình chiếu gia tốc tuyệt đối của ASWs xuống các trục của hệ tọa độ gắn liền, còn , ,x y zg g g là hình chiếu của véc tơ gia tốc trọng trường xuống các trục tương ứng của hệ tọa độ gắn liền. Dễ dàng nhận thấy trong trường hợp thả ASWs từ máy bay ở chế độ có dù thì 0; 0; 0N E Df f f   . Trong trường hợp không xét đến nhiễu đo, ba gia tốc kế đo ba thành phần 4 5 6[ , , ] [ , , ] [ , , ] T T T bx by bz x y zz z z a a a g g g  . Khi đó có thể coi: 4 5 6 0 0 0[ , , ] ( ) [ , , ] T n T T b x y zz z z C g g g (39) 2 2 4 4 1 2 0 2 3 1 4 0 2 4 1 3 0 ( ) ( ) (2 ( ) 2 ( ) 1) (2 ( ) ( ) 2 ( ) ( )) (2 ( ) ( ) 2 ( ) ( )) k x y z z k h X x k x k g x k x k x k x k g x k x k x k x k g        (40) 5 5 2 3 1 4 0 2 2 1 3 0 3 4 1 2 0 ( ) ( ) (2 ( ) ( ) 2 ( ) ( )) (2 ( ) 2 ( ) 1) (2 ( ) ( ) 2 ( ) ( )) k x y z z k h X x k x k x k x k g x k x k g x k x k x k x k g         (41) 6 6 2 4 1 3 0 2 2 3 4 1 2 0 1 4 0 ( ) ( ) (2 ( ) ( ) 2 ( ) ( )) (2 ( ) ( ) 2 ( ) ( )) (2 ( ) 2 ( ) 1) k x y z z k h X x k x k x k x k g x k x k x k x k g x k x k g         (42) Theo các biểu thức (7) trong trường hợp các con quay vi cơ không xét đến nhiễu đo, các chỉ số 7 8 9, ,z z z con quay vi cơ đo được sẽ là: 7 7 5 8 ( ) ( ) ( ) ( ) k p z k h X p b x k x k     (43) 8 8 6 9 ( ) ( ) ( ) ( ) k q z k h X q b x k x k     (44) 9 9 7 10 ( ) ( ) ( ) ( ) k r z k h X r b x k x k     (45) Từ (36 - 38), (40 - 42) và (43 - 45) ta có đầy đủ 9 hàm quan sát 1 2 9 ( , ,..., )h h h của véc tơ hàm ( ) k h X . Việc xác định ma trận đo ˆ ( )k k k X X h H X      bằng cách lấy đạo hàm riêng i j h x   nhận được: , 1,9; 1,10 k ij H h i j     . (46) trong đó, 11 1 4 34 ( ) 2 ( ) 2 ( )x y zh B x k B x k B x k   ; 12 2 3 44 ( ) 2 ( ) 2 ( )x y zh B x k B x k B x k   ; 13 2 12 ( ) 2 ( )y zh B x k B x k  ; 14 1 22 ( ) 2 ( )y zh B x k B x k  ; 22 3 12 ( ) 2 ( )x zh B x k B x k  Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 31, 06 - 2014 123 21 4 1 22 ( ) 4 ( ) 2 ( )x y zh B x k B x k B x k    ; 23 2 3 42 ( ) 4 ( ) 2 ( )x y zh B x k B x k B x k   ; 24 1 32 ( ) 2 ( )x zh B x k B x k   ; 31 3 2 12 ( ) 2 ( ) 4 ( )x y zh B x k B x k B x k   ; 32 4 12 ( ) 2 ( )x yh B x k B x k  ; 33 1 42 ( ) 2 ( )x yh B x k B x k  ; 41 0 32 ( );zh g x k  34 2 3 42 ( ) 2 ( ) 4 ( )x y zh B x k B x k B x k   ; 42 0 42 ( );zh g x k 43 0 12 ( );zh g x k  44 0 22 ( );zh g x k 51 0 22 ( )zh g x k ; 52 0 12 ( )zh g x k ; 53 0 42 ( )zh g x k ; 54 0 32 ( )zh g x k ; 61 0 14 ( )zh g x k ; 64 0 44 ( )zh g x k ; 1010 99 88 710 69 58 1h h h h h h      ; tất cả các ijh khác đều bằng 0. Giả sử nhiễu đo của các từ kế, gia tốc kế và con quay vi cơ là ồn trắng thì ta có: 1 1 1 ( ) k T Xz h v  ; 2 2 2 ( ) k T Xz h v  ; 3 3 3 ( ) k T Xz h v  ; 4 4 4 ( ) k Xz h w  ; 5 5 5 ( ) k Xz h w  ; 6 6 6 ( ) k Xz h w  ; 7 7 1 ( ) ; k z h X w  28 8 ( ) k X wz h  ; 9 9 3 ( ) . k z h X w  trong đó, 1 2 3, ,T T Tv v v là nhiễu đo ba từ kế. Nếu nhiễu đo của các từ kế, gia tốc kế và con quay vi cơ không tương quan với nhau thì ma trận R là ma trận đường chéo với các giá trị trên đường chéo là các phương sai của các từ kế, gia tốc kế và các con quay vi cơ. Như vậy đã có đầy đủ yếu tố để xác định ma trận kP và ma trận kK theo thủ tục lọc Kalman [4]. Kết quả ước lượng theo lưu đồ giải thuật bộ lọc Kalman phi tuyến mở rộng nhận được 4 tham số Rodrig-Hamilton 0 1 2 3, , ,    . Từ đó xác định 3 góc định hướng , ,   theo công thức (6) vận tốc theo công thức (4) và vị trí tâm khối của ASWs trong hệ tọa độ địa lý theo công thức (5) như sơ đồ hình 3, trong đó ma trận M được xác định:  4 4 4 6x xM I O trong đó, I là ma trận đơn vị; O là ma trận các phần tử bằng 0. 3. THUậT TOáN XáC ĐịNH THAM Số DẫN ĐƯờNG CHO Vũ KHí CHốNG NGầM giai đoạn chuyển động trong nước Trong trường hợp này, tác giả đề xuất phương án ứng dụng bộ lọc Kalman phi tuyến mở rộng ước lượng trực tiếp các tham số dẫn đường trên cơ sở các thông tin quan sát do con quay vi cơ, gia tốc kế, từ kế, vận tốc kế và cảm biến áp suất cung cấp. Thuật toán này cho phép kế thừa các thiết bị đo đã dùng trong giai đoạn chuyển động trong khí quyển (hình 2), chỉ thêm ba vận tốc kế đo 3 thành phần vận tốc trong hệ tọa độ gắn liền và cảm biến áp suất đo độ sâu của ASWs so với bề mặt nước. Thay (8), (3) vào (4) ta có: 2 2 0 1 1 2 0 3 1 3 0 2 2 2 0 1 4 1 2 0 3 5 1 3 0 2 6 V =(2 2 1)a (2 2 )a (2 2 )a (2 2 1) (2 2 ) (2 2 ) N x y z w w w                                   (47) 2 2 1 2 0 3 0 2 2 3 0 1 2 2 1 2 0 3 4 0 2 5 2 3 0 1 6 V =(2 2 )a (2 2 1)a (2 2 )a (2 2 ) -(2 2 1) (2 2 ) E x y z w w w                                  (48) 2 2 1 3 0 2 2 3 0 1 0 3 2 2 1 3 0 2 4 2 3 0 1 5 0 3 6 V =(2 2 )a (2 2 )a (2 2 1)a (2 2 ) (2 2 ) (2 2 1) D x y z w w w g                                    (49) Biến X được đặt: 1 0 2 1 3 2 4 3 5 6 7 8 9 10 11 ; ; ; ; ; ; ; ; ; ; p q r N E D x x x x x b x b x b x V x V x V x z              (50) Điều khiển & Tự động hóa T. D. Trung, N. Q. Vịnh, “Xây dựng thuật toán xác định vũ khí chống ngầm.” 124 Theo cách đặt biến (50), từ (10-13), từ (47-49) và từ (5) xây dựng được các hàm động học 1 2 11 , ,...,f f f và các hàm nhiễu động học 1 2 11 , ,...,g g g . Vận tốc kế đo các thành phần vận tốc trong hệ tọa độ gắn liền cho các hàm quan sát: T T 4 5 6 [ , , ] ( ) [ , , ]n T b N E D z z z C V V V (51) Cảm biến áp suất đo độ sâu của ASWs so với mặt nước cho hàm quan sát 7 z . Từ (32), (51) và hàm quan sát 7 z ta có đủ 7 hàm quan sát: 1 2 3 4 5 6 7 [ , , , , , , ]TZ z z z z z z z . Bằng cách đạo hàm riêng các hàm động học, hàm quan sát, hàm nhiễu động học xác định được ma trận chuyển vị, ma trận đo và ma trận hệ số nhiễu, từ đó áp dụng thuật toán lọc Kalman phi tuyến mở rộng tương tự giai đoạn chuyển động trong khí quyễn sẽ xác định được véc tơ trạng thái ước lượng gồm có các tham số Rodrig – Hamilton và các thành phần vận tốc, độ sâu của ASWs trong hệ tọa độ địa lý. Tọa độ tâm khối của ASWs trong mặt phẳng ngang được tính từ công thức (5) với vận tốc là giá trị ước lượng được. 4. KếT QUả MÔ PHỏNG Kết quả mô phỏng chỉ ra trên hình 4 cho thấy rằng các tham số Rodrig-Hamilton ước lượng được phù hợp với giá trị lý tưởng giả định. Hình 4. Đồ thị tham số 0 1 2 3, , ,    lý tưởng và kết quả ước lượng được. 5. KếT LUậN Giai đoạn vũ khí chống ngầm rơi trong khí quyển ở chế độ có dù, tốc độ chuyển động của vũ khí chống ngầm biến thiên nhỏ. Vì thế, có thể áp dụng bộ lọc Kalman phi tuyến mở rộng xây dựng thuật toán xác định tham số dẫn đường cho ASWs trên cơ sở kết hợp thông tin từ các từ kế, gia tốc kế, con quay vi cơ. Thuật toán đề xuất ước lượng trực tiếp bốn tham số Rodrig-Hamilton 0 1 2 3, , ,    . Từ các tham số Rodrig-Hamilton xác định được các góc định hướng và ma trận Côsin định hướng giữa hệ tọa độ gắn liền và hệ tọa độ địa lý. Từ đó Giá trị lý tưởng Kết quả ước lượng Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 31, 06 - 2014 125 xác định được các thành phần gia tốc trong hệ tọa độ dẫn đường (hệ tọa độ địa lý) để xác định vận tốc và tọa độ tâm khối ASWs trong hệ tọa độ địa lý. Giai đoạn ASWs chuyển động trong nước có tốc độ biến đổi vì vậy để ước lượng trực tiếp các tham số Rodrig-Hamilton 0 1 2 3, , ,    cần bổ sung các phần tử đo các thành phần véc tơ vận tốc và độ sâu của ASWs và củng từ đó xác định được tọa độ tâm khối của ASWs. Nhờ kết hợp các phương tiện đo bằng bộ lọc Kalman phi tuyến mở rộng mà độ chính xác của các tham số dẫn đường được nâng cao vì khắc phục được yếu tố trôi của con quay vi cơ và nhiễu của các phương tiện đo. TàI LIệU THAM KHảO [1]. D.H.Titterton, J.L.Weston, (2004), “Strapdown Inertial Navigation Technology”, 2nd Edition, The Institution of Electrical Engineers, ISBN 0863413587. [2]. O. S. Salychev, (1988), “Inertial systems in navigation and geophysics”, Press Moscow. [3]. Pаспопов В. Я (2010), “Мисро-системная авионика”, Тула. [4]. Леондес К. Т, (1980), “Фильтрация и стохастическое управление в динамических системах”, Мир, Москва. abstract Constructing an algorithm for determining navigation parameters for anti-submarine weapons This article describes a method using the nonlinear extended Kalman filter in order to construct an algorithm for determining navigation parameters for anti- submarine weapons which are dropped from the plane based on the combination of information of inertial navigation equipments (accelerometers, speed angle gyros) with magnetometers, speedometers and pressure sensors in the drop motion with a parasol in the atmosphere and in the motion in the water environment. Keywords: Extended nonlinear Kalman filter, Angular rate sensors, Magnetometers, Accelerometers, Speedometers and pressure sensors. Nhận bài ngày 10 thỏng 3 năm 2014 Hoàn thiện ngày 20 thỏng 5 năm 2014 Chấp nhận đăng ngày 25 thỏng 5 năm 2014 Địa chỉ: Viện KH&CN Quân sự