Xây dựng phương pháp và thuật toán xác định vận tốc góc bằng hệ đa gia tốc kết hợp con quay vận tốc góc

Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 3 X©y dùng ph­¬ng ph¸p vµ thuËt to¸n x¸c ®Þnh vËn tèc gãc b»ng hÖ ®a gia tèc kÕt hîp con quay vËn tèc gãc NGUYỄN HOÀNG MINH*, TRẦN ĐỨC THUẬN**, NGUYỄN SỸ LONG*, NGUYỄN VIỆT HƯNG*** Tóm tắt: Bài báo trình bày phương pháp và thuật toán sử dụng bộ lọc Kalman phi tuyến mở rộng để xác định vận tốc góc bằng hệ đa gia tốc kế kết hợp con quay vận tốc góc. Phương pháp và thuật toán có giá trị khi xây dựng hệ thống dẫn đường quán tính không đế dựa trên các loại cảm biến có độ chính xác thấp và trung bình. Từ khóa: Tên lửa, Dẫn đường quán tính, Hệ đa gia tốc kế. 1. ĐẶT VẤN ĐỀ Hệ dẫn đường quán tính không đế sử dụng hệ cảm biến gồm ba con quay vận tốc góc, ba gia tốc kế và có thể kết hợp thêm ba từ kế hoặc GPS để xác định tất cả các trạng thái chuyển động của vật thể. Tuy nhiên, nó còn nhiều nhược điểm: giá thành cao (cảm biến có độ chính xác cao), độ trôi lớn (cảm biến có độ chính xác thấp), Ở nước ngoài và ở Việt Nam đã có một số bài báo đưa ra các phương pháp để nâng cao độ chính xác của hệ thống nhưng vẫn còn nhiều hạn chế. Trong bài báo [1,3] đưa phương pháp kết hợp con quay vận tốc góc với gia tốc kế và từ kế chỉ áp dụng được trong trường hợp vật thể chuyển động gần đều. Trong bài báo [2,4] đưa ra phương pháp kết hợp hệ con quay, gia tốc kế, từ kế với vận tốc kế. Phương pháp này có thể áp dụng được trong mọi trường hợp, nhưng để triển khai trong thực tế còn gặp nhiều khó khăn. Ví dụ, để xác định vận tốc chuyển động cho vật thể bay có thể sử dụng tín hiệu từ GPS hoặc sử dụng ống Pi tô. Nếu sử dụng tín hiệu GPS thì chỉ ứng dụng được cho vật thể chuyển động trên mặt đất vì sự cung cấp hạn chế chất lượng GPS của Bộ Quốc Phòng Mỹ. Mặt khác, hiện nay ở Việt Nam chưa chế tạo được ống Pi tô, bên cạnh đó rất khó khăn khi mua được ống Pi tô có dải đo phù hợp cho thiết bị bay (tên lửa, máy bay, ...) từ nước ngoài. Ngày nay, công nghệ vi cơ điện tử (MEMS) đã sản xuất được các loại cảm biến có nhiều ưu điểm như kích thước rất nhỏ, giá thành hạ, công suất tiêu thụ thấp và dễ dàng mua trên thị trường. Thế nên, cảm biến MEMS là sự lựa chọn tiềm năng để xây dựng hệ dẫn đường quán tính không đế tại Việt Nam. Với công nghệ hiện nay, gia tốc kế MEMS ngày càng chính xác, nó đã gần đạt đến độ chính xác cấp chiến thuật. Nhưng chất lượng con quay MEMS vẫn còn thấp: độ trôi lớn, giá trị trôi khởi tạo còn thay đổi lớn sau mỗi lần bật tắt, khả năng chống sốc còn hạn chế, độ bền còn kém, ... Bên cạnh đó, giá thành của con quay MEMS còn đắt hơn gia tốc kế MEMS nhiều lần (Nguồn Digikey và Mouser). Vì vậy, giải pháp xác định vận tốc góc bằng hệ đa gia tốc kế có triển vọng sẽ chính xác hơn khi chỉ đo bằng con quay vận tốc góc đơn thuần. Trong bài báo này sẽ trình bày phương pháp và thuật toán xác định vận tốc góc của vật thể bằng hệ đa gia tốc kế kết hợp với con quay vận tốc góc. 2. HỆ ĐA GIA TỐC KẾ Ở nước ngoài, giải pháp ước lượng vận tốc góc của vật thể bằng nhiều gia tốc kế được sử dụng bởi nhiều nhà nghiên cứu. Từ những năm 60 [5], Shuler đã mô tả cấu hình gồm 6, 8, 9 gia tốc kế kết hợp để xác định vận tốc góc. Nhưng hầu hết những công trình đó chỉ Tên lửa & Thiết bị bay N. H. Minh, T. Đ. Thuận,..., “Xây dựng phương pháp và thuật toán ... vận tốc góc.” 4 thông báo kết quả, không nêu rõ thuật toán. Trong bài báo này, tác giả xem xét cấu hình cảm biến gồm có 4 hệ gia tốc kế 3 trục kết hợp 1 hệ con quay vận tốc góc 3 trục. Để hiểu rõ bản chất việc xác định tốc độ góc trên cơ sở hệ đa gia tốc kế, ta xem xét chuyển động của một vật rắn lý tưởng như hình 1. Giả sử, vật rắn chuyển động phức hợp có gia tốc góc   , vận tốc góc   , gia tốc dài Oa  . Khi đó phương trình cơ bản mô tả gia tốc của một điểm P bất kỳ trên vật rắn như sau: P O Pht Ptta a a a       (1) trong đó, ,P Oa a   là gia tốc dài tại điểm P và O; Phta  là gia tốc hướng tâm tại P; Ptta  là gia tốc tiếp tuyến tại P; O là tâm khối của vật thể. Hình 1. Chuyển động phức hợp của vật rắn lý tưởng. Chúng ta biết rằng [6]: 0Ptt Pa r    (2) 0( )Pht Pa r       (3) từ (1), (2), (3) suy ra: 0 0 0( )P P Pa a r r              (4) Theo [6], áp dụng công thức nhân 2 véc tơ có hướng ta có: 0 0 0 0 0 0 0 z y x z y x P z x y z x y y x z y x z r r r r r r r r r r r r r                      (5) 2 2 2 0 0 ( ) 0 0 0 0 x y x x y z z PO y y x z x y z z x y x z y z r r r r r r r r r r r r r                        (6) Xem xét mô hình hệ cảm biến gồm 4 hệ gia tốc kế 3 trục và 1 hệ con quay vận tốc góc 3 trục như sau: Hình 2. Mô hình 4 hệ gia tốc kế 3 trục kết hợp 1 hệ con quay vận tốc góc 3 trục. O P     Ptta  Phta  Oa  X 1 2 3 4 Y Z O Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 5 Hệ tọa độ OXYZ là hệ tọa độ liên kết của vật thể bay, tất cả các cảm biến có trục nhạy song song với các trục tương ứng của hệ tọa độ liên kết. Giả sử, hệ gia tốc kế số 1 nằm cách tâm khối O khoảng cách: 1 1 1; 0; 0x y zr r r r   (7) Từ công thức (1), (5), (6) ta có chỉ số của hệ gia tốc kế số 1 như sau: 0 2 2 1 0 1 0 1 0 ( )x x y z y y z x y z z y x z A a r A a r r A a r r                 (8) trong đó, 1 1 1; ;x y zA A A là các chỉ số chỉ số lý tưởng của hệ gia tốc kế số 1; 0 0 0; ;x y za a a là các gia tốc dài của vật thể; ; ;x y z   là các vận tốc góc của vật thể; ; ;x y z     là các gia tốc góc của vật thể. Hệ gia tốc kế số 2 nằm cách tâm khối O khoảng cách: 2 2 2; 0; 0x y zr r r r    (9) Từ công thức (1), (5), (6) ta có chỉ số gia tốc kế số 2 như sau: 0 2 2 2 0 2 0 2 0 ( )x x y z y y z x y z z y x z A a r A a r r A a r r                  (10) trong đó, 2 2 2; ;x y zA A A là các chỉ số chỉ số lý tưởng của hệ gia tốc kế số 2. Hệ gia tốc kế số 3 nằm cách tâm khối O khoảng cách: 3 3 30; ; 0x y zr r r r   (11) Từ công thức (1), (5), (6) ta có chỉ số gia tốc kế số 3 như sau: 0 3 0 2 2 3 0 3 0 ( ) x x z x y y y x z z z x y z A a r r A a r A a r r                 (12) trong đó, 3 3 3; ;x y zA A A là các chỉ số chỉ số lý tưởng của hệ gia tốc kế số 3. Hệ gia tốc kế số 4 nằm cách tâm khối O khoảng cách: 4 4 40; 0;x y zr r r r   (13) Từ công thức (1), (5), (6) ta có chỉ số gia tốc kế số 4 như sau: 0 3 0 3 0 2 2 3 0 ( ) x x y x z y y x y z z z x y A a r r A a r r A a r                 (14) trong đó, 4 4 4; ;x y zA A A là các chỉ số chỉ số lý tưởng của hệ gia tốc kế số 4. Cộng 2 vế của phương trình (8) với phương trình (10) nhận được: 0 1 2 0 1 2 0 1 2 1 2 x x x y y y z z z a A A a A A a A A     (15) Từ phương trình (15) ta xác định được gia tốc dài Oa của vật thể. Trừ hai vế của phương trình (8) cho phương trình (10) nhận được: Tên lửa & Thiết bị bay N. H. Minh, T. Đ. Thuận,..., “Xây dựng phương pháp và thuật toán ... vận tốc góc.” 6 2 2 1 2 1 2 1 2 0 2 ( ) 2 2 2 2 x x y z y y z x y z z y x z A A r A A r r A A r r                   (16) Đặt 1 2 3 2 2 2 4 5 6 7 8 9 ; ; ; ; ; ; ; ; ; x y z x y z x y x z y z m m m m m m m m m                           (17) Từ phương trình (16), (17) suy ra: 5 6 1 2 3 7 1 2 2 8 1 2 ( ) / 2 ( ) / 2 ( ) / 2 x x y y z z m m A A r m m A A r m m A A r               (18) Từ phương trình (12), (15) và (17) có thể rút ra như sau: 3 7 3 1 2 4 6 3 1 2 1 9 3 1 2 (2 ) / 2 (2 ) / 2 (2 ) / 2 x x x y y y z z z m m A A A r m m A A A r m m A A A r                 (19) Tương tự, từ phương trình (14), (15) và (17) cũng có: 2 8 4 1 2 1 9 4 1 2 4 5 4 1 2 (2 ) / 2 (2 ) / 2 (2 ) / 2 x x x y y y z z z m m A A A r m m A A A r m m A A A r                  (20) Giải hệ 9 phương trình tuyến tính (18), (19), (20) với 9 ẩn ; 1..9im i  ta có: 1 3 4 1 2 1 2(2 2 ) / 4x z y z z y ym A A A A A A r       (21) 2 4 1 2 1 2(2 ) / 4y x x x z zm A A A A A r      (22) 3 1 2 3 1 2( 2 ) / 4z y y x x xm A A A A A r      (23) 2 4 1 2 3 1 2 4 1 2( 2 2 ) / 4x x x y y y z z zm A A A A A A A A r         (24) 25 4 1 2 1 2 3 1 2( 2 2 ) / 4y z z z x x y y ym A A A A A A A A r          (25) 26 3 1 2 1 2 4 1 2( 2 2 ) / 4z y y y x x z z zm A A A A A A A A r          (26) 7 1 2 3 1 2( 2 ) / 4x y y y x x xm A A A A A r       (27) 8 4 1 2 1 2(2 ) / 4x z x x x z zm A A A A A r       (28) 9 3 1 2 4 1 2(2 2 ) / 4y z z z z y y ym A A A A A A r        (29) Từ (15) và các nghiệm ; 1..9im i  ở trên ta thấy rằng, bằng một hệ đa gia tốc kế có thể xác định được gia tốc dài, gia tốc góc và thành phần bậc hai vận tốc góc của vật thể khi chuyển động phức hợp. Để xác định vận tốc góc của vật thể có thể dùng hai cách sau đây: - Tích phân gia tốc góc, hay là tích phân các nghiệm 1 2 3; ; ;m m m - Rút ra từ các thành phần bậc hai của vận tốc góc, chính là rút ra từ các nghiệm 4 5 6 7 8 9; ; ; ; ; ;m m m m m m Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 7 3. PHƯƠNG PHÁP VÀ THUẬT TOÁN XÁC ĐỊNH VẬN TỐC GÓC BẰNG HỆ ĐA GIA TỐC KẾ KẾT HỢP CON QUAY VẬN TỐC GÓC 3.1. Bộ lọc Kalman Bộ lọc Kalman là một công cụ toán học giúp cho việc đánh giá véc tơ trạng thái hệ động học trên cơ sở thông tin quan sát véc tơ tín hiệu đầu ra. Giả sử quá trình chuyển động của vật thể hoặc quá trình công nghệ đượ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 2 1 2 ( ) w , ( ) ( ( ), ( ),..., ( )) , ( ) , ( ) ( ( ), ( ),... ( )) T k k k k k k k n k T k k k k k k n k X F X G F X f X f X f X Z C X C X c X c X c X        (30) 1,k kX X  là các véc tơ trạng thái X (n chiều) ở bước thứ k và bước thứ (k-1), kZ là giá trị của véc tơ đầu ra (m chiều, thường m n ). Véc tơ đầu ra này 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, 1 2, ,..., nf f f là các hàm phi tuyến với biến số là véc tơ X. kZ là véc tơ hàm số (.)C ở bước thứ k, 1 2( , ,... ) T nC c c c , w ,k k là các véc tơ nhiễu động học và nhiễu đo có dạng ồn trắng với kỳ vọng toán học bằng 0: G -ma trận hệ số nhiễu, w ~ (0, ), ~ (0, )k k k kN Q N R . ˆ ˆ([ ][ ] ) T k k k k kP X X X X   , (.) là ký hiệu 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ư sau: 1 1 1 1 1 ˆ ˆ( ) ( ) 1 1 1 1 1 1 1 ˆ ˆ ˆ ˆ( ) ( ( )), , ( ( )), , ( ) ( ) , ( ) ( ( ) ) ˆ ˆ ˆ( ) ( ) ( ), ( ) ( ) ( ) k k k k k k k k k k k k X X X X T T T k k k k k k k k k k k k k k k k k k k k k k k f C X f X Z C X H X X P P G Q G K P H H P H R X X K Z Z P I K H P                                                (31) Để áp dụng được thuật toán lọc Kalman phi tuyến mở rộng cần phải xác định được: các hàm động học 1 2( ), ( )k kf X f X , 3( ),..., ( )k n kf X f X ;các hàm quán sát 1 2( ), ( )k kc X c X , 3 ( ),..., ( )k m kc X c X ; ma trận hệ số nhiễu G ; véc tơ nhiễu động học wk , véc tơ nhiễu quan sát k và các ma trận cường độ nhiễu ,Q R . Sau đây bài báo trình bày các vấn đề này cho bài toán xác định véc tơ tốc độ góc quay của vật thể chuyển động. 3.2. Thuật toán kết hợp Từ phần luận giải ở mục 2, ta thấy rằng khi sử dụng hệ đa gia tốc kế thì có thể xác định được vận tốc góc của vật thể theo hai cách. Tuy nhiên, cả hai cách nêu trên đều có những hạn chế. Nếu dùng cách thứ hai, rút ra từ thành phần bậc hai vận tốc góc thì chúng ta chỉ có thể biết được độ lớn của vận tốc góc  chứ không biết được hướng. Nếu dùng cách thứ nhất, tích phân gia tốc góc  , thì nếu không biết vận tốc góc khởi tạo 0 cũng không xác định được. Hơn nữa tính theo cách này sẽ có sai số tích lũy theo thời gian. Để giải quyết vấn đề trên, ta xem xét giải pháp đưa vào một con quay vận tốc góc ba trục. Con quay dùng để khởi tạo vận tốc góc ban đầu và sẽ làm hội tụ nhanh hơn phép tính toán, giới hạn các sai số. Nó đặc biệt hữu dụng khi chất lượng phép đánh giá gia tốc góc  và thành phần bậc hai vận tốc góc quá tồi, ví dụ, sai số lớn khi vật thể chuyển động biến đổi chậm. Nên việc kết hợp con quay vận tốc góc vào hệ đa gia tốc kế giúp cho việc xác định gia tốc được chính xác hơn, vì khi có nhiều gia tốc kế thì nhiễu sẽ được trung bình. Nếu con quay cho tín hiệu chất lượng thấp hoặc khi vật thể chuyển động biến đổi nhanh, tín hiệu con quay khi đó không còn tin cậy vì giới hạn của dải đo thì khi đó hệ đa gia tốc kế sẽ cho chất lượng tốt hơn. Ta có chỉ số gia tốc kế đo được như sau: Tên lửa & Thiết bị bay N. H. Minh, T. Đ. Thuận,..., “Xây dựng phương pháp và thuật toán ... vận tốc góc.” 8 ( ) ( ) ( ) ( )a aA k A k b k k    (32) trong đó, ( )A k là chỉ số của các gia tốc kế tại thời điểm k, ( )A k là gia tốc thực tại thời điểm k, ( )ab k là độ trôi của gia tốc kế tương ứng tại thời điểm k, ( )a k là nhiễu đo của gia tốc kế tại thời điểm k. Tương tự cho con quay vận tốc góc ta có ( ) ( ) ( ) ( )g gk k b k k     (33) trong đó, ( )k là chỉ số của các con quay tại thời điểm k, ( )k là vận tốc góc thực tại thời điểm k, ( )gb k là độ trôi của con quay tương ứng tại thời điểm k, ( )g k là nhiễu đo của con quay tại thời điểm k. Mô hình trạng thái của hệ thống được xây dựng dựa trên 18 trạng thái như sau: 1 2 3 18 1 1 1 2 2 2 3 3 3 4 4 4 [ ... ] [ ] T T x y z gx gy gz a x a y a z a x a y a z a x a y a z a x a y a z X x x x x b b b b b b b b b b b b b b b     (34) trong đó, , ,x y z   là các vận tốc góc của vật thể chiếu lên 3 trục của hệ tọa độ liên kết; , ,gx gz gzb b b là độ trôi của con quay vận tốc góc theo 3 trục của hệ tọa độ liên kết; , ,aix aiy aizb b b là độ trôi của gia tốc kế thứ i theo 3 trục của hệ tọa độ liên kết với i=1...4. Ta có thể tính vận tốc góc theo trục x của vật thể tại thời điểm (k+1) khi biết thông tin về gia tốc góc cúa nó: ( 1) ( ) ( ).x x xk k k t      (35) Từ (32), (21), (17) phương trình (35) sẽ có dạng như sau: 1 1 1 3 4 1 2 1 2 3 4 1 2 1 2 3 4 1 2 1 2 1 3 4 1 2 1 2 ( 1) ( 1) ( ( )) w ( ) ( ) (2 2 ) / 4 (2 2 ) / 4 (2 2 ) / 4 ( ) (2 2 ) / 4 x x z y z z y y a z a y a z a z a y a y a z a y a z a z a y a y z y z z y y x k k f X k k k t A A A A A A r t b b b b b b r t r x k t A A A A A A r                                                       15 17 9 12 8 11 1(2 ( ) 2 ( ) ( ) ( ) ( ) ( )) / 4 w ( )t x k x k x k x k x k x k r k      (36) trong đó, 1 3 4 1 2 1 2w ( ) (2 2 ) / 4a z a y a z a z a y a yk t r            Tương tự cho y và z như sau: 2 2 2 2 4 1 2 1 2 16 7 10 9 12 4 1 2 1 2 ( 1) ( 1) ( ( )) w ( ) ( ) (2 ) / 4 (2 ) / 4 (2 ) / 4 y x x x z z a x a x a x a z a z x k k f X k k x k t A A A A A r t x x x x x r t r                                  (37) 3 3 3 3 1 2 3 1 2 8 11 13 7 10 1 2 3 1 2 ( 1) ( 1) ( ( )) w ( ) ( ) ( 2 ) / 4 ( 2 ) / 4 ( 2 ) / 4 z y y x x x a y a y a x a x a x x k k f X k k x k t A A A A A r t x x x x x r t r                                   (38) Bên cạnh đó, nếu coi độ trôi của các cảm biến là không đổi theo thời gian thì ta có: +Ba độ trôi của ba con quay đo tốc độ góc: Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 9 ( 1) ( )g gb k b k  (39) +Mười hai độ chệch không (độ trôi) của 12 gia tốc kế: ( 1) ( )a ab k b k  (40) Khi đó, trạng thái thứ 4 đến thứ 18 của hệ sẽ có dạng sau: ( 1) ( ( )) ( )i i ix k f X k x k   (41) với ix là trạng thái thứ i của hệ, i=4...18. Ba phương trình (36), (37), (38) cùng với 15 phương trình dạng (41) sẽ tạo thành hệ động học của bộ lọc Kalman. Dễ dàng nhận thấy ma trận G sẽ là ma trận bậc [18 3] với các phần tử như sau: 11 22 33 1g g g   còn các phần tử khác đều bằng 0. Mô hình quan sát của hệ thống được xây dựng dựa trên 9 quan sát như sau: 1 2 3 4 5 6 7 8 9 4 5 6 7 8 9 [ ]T T x y z Z z z z z z z z z z m m m m m m                (42) trong đó, , ,x y z     là các chỉ số con quay vận tốc góc theo 3 trục; im là thành phần bậc hai của vận tốc góc tính theo chỉ số của các gia tốc kế iA  . Từ (33) ta có: 1 4 1 ( ) ( ) ( ) ( ) ( ( )) x x gx gx gx gx k k b x k x k c X k               (43) 2 5 2 ( ) ( ) ( ) ( ) ( ( )) y y gy gy gy gy k k b x k x k c X k               (44) 3 6 3 ( ) ( ) ( ) ( ) ( ( )) z z gz gz gz gz k k b x k x k c X k               (45) Từ (24) và (32) suy ra: 4 1 2 3 1 2 4 1 2 1 2 3 1 2 4 1 2 1 2 3 1 2 4 1 2 1 2 3 1 2 4 1 2 2 1 7 10 ( ) ( 2 2 ) / 4 ( 2 2 ) / 4 ( 2 2 ) / 4 ( 2 2 ) / 4 ( 2 x x y y y z z z x x y y y z z z a x a x a y a y a y a z a z a z a x a x a y a y a y a z a z a z m k A A A A A A A A r A A A A A A A A r b b b b b b b b r r x x x                                                     14 8 11 18 9 12 1 2 3 1 2 4 1 2 4 4 2 ) / 4 ( 2 2 ) / 4 ( ( )) a x a x a y a y a y a z a z a z x x x x x x r r c X k                         (46) trong đó, 2 21 1 2 3 1 2 4 1 2( 2 2 ) / 4 ( (24))x x x y y y z z zx A A A A A A A A r theo         24 1 7 10 14 8 11 18 9 12( ) ( 2 2 ) / 4c X x x x x x x x x x r         Tên lửa & Thiết bị bay N. H. Minh, T. Đ. Thuận,..., “Xây dựng phương pháp và thuật toán ... vận tốc góc.” 10 4 1 2 3 1 2 4 1 2( 2 2 ) / 4a x a x a y a y a y a z a z a z r                Tương tự áp dụng cho 5 6 7 8 9; ; ; ;m m m m m     5 4 1 2 1 2 3 1 2 2 2 18 9 12 7 10 14 8 11 4 1 2 1 2 3 1 2 5 5 ( ) ( 2 2 ) / 4 ( 2 2 ) / 4 ( 2 2 ) / 4 ( ( )) z z z x x y y y a z a z a z a x a x a y a y a y m k A A A A A A A A r x x x x x x x x x r r c X k                                                (47) 6 3 1 2 1 2 4 1 2 2 3 14 8 11 7 10 18 9 12 3 1 2 1 2 4 1 2 6 6 ( ) ( 2 2 ) / 4 ( 2 2 ) / 4 ( 2 2 ) / 4 ( ( )) y y y x x z z z a y a y a y a x a x a z a z a z m k A A A A A A A A r x x x x x x x x x r r c X k                                                (48) 7 1 2 3 1 2 1 2 8 11 13 7 10 1 2 3 1 2 7 7 ( ) ( 2 ) / 4 ( 2 ) / 4 ( 2 ) / 4 ( ( )) y y x x x a y a y a x a x a x m k A A A A A r x x x x x x x r r c X k                              (49) 8 4 1 2 1 2 1 3 16 7 10 9 12 4 1 2 1 2 8 8 ( ) (2 ) / 4 (2 ) / 4 (2 ) / 4 ( ( )) x x x z z a x a x a x a z a z m k A A A A A r x x x x x x x r r c X k                              (50) 9 4 1 2 4 1 2 2 3 16 9 12 17 8 11 4 1 2 4 1 2 9 9 ( ) (2 2 ) / 4 (2 2 ) / 4 (2 2 ) / 4 ( ( )) x z z y y y a x a z a z a y a y a y m k A A A A A A r x x x x x x x x r r c X k                                   (51) Lấy đạo hàm riêng ( ( )), ( 1,2,3,...,18)if X k i  theo các biến số ( 1, 2,3,...,18)jx j  có ma trận ( )t kích thước (18x18) có các phần tử là i j f x   . Lấy đạo hàm riêng ( ( )), ( 1,...,9)ic X k i  theo các biến số ( 1,2,3,...,18)jx j  có ma trận ( )H t kích thức (9x18) có các phần tử là i j c x   . Khi nhiễu đo không tương quan lẫn nhau thì ma trận hiệp phương sai nhiễu trạng thái Q và ma trận hiệp phương sai nhiễu quan sát R của hệ thống là ma trận đường chéo. Ma trận Q là ma trận có kích thước [3x3] và có các phần tử 11 22 33, ,q q q khác 0. Từ biểu thức (36) có: 2 2 2 2 2 211 3 4 1 2 1 2(2 2 2 ) 4 a z a y a z a z a y a y t q r               (52) Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 11 trong đó, 2a  là các phương sai nhiễu đo của các gia tốc kế tương ứng. Từ biểu thức (37), (38) sẽ xác định được 22 33,q q tương tự như (52). Còn ma trận R là ma trận có kích thước [9x9] có các phần tử , 1,2,3,...,9iir i  khác không. Từ biểu thức (43), (44), (45) có: 2 2 211 22 33, ,gx gy gzr r r     (53) trong đó, 2 2 2, ,gx gy gz   là các phương sai nhiễu đo của các con quay đo tốc độ góc. Từ biểu thức (46) có: 2 2 2 2 2 2 2 244 1 2 3 1 2 4 1 2( 2 2 ) / (4 )a x a x a y a y a y a z a z a zr r               (54) Tương tự (54) từ các công thức (47), (48), (49). (50), (51) sẽ tính được các , 5,6,7,8,9iir i  . 4. KẾT QUẢ MÔ PHỎNG KIỂM NGHIỆM THUẬT TOÁN Sử dụng thuật toán đã đề xuất, mô phỏng trên phần mềm Matlab mô hình hệ 4 gia tốc kế kết hợp với con quay có các tham số đầu vào như sau: - Khoảng cách từ tâm khối đến 4 gia tốc kế: + Gia tốc kế thứ nhất: 1 1 10,1 ; 0,1 ; 0,1 ;x y zr m r m r m   + Gia tốc kế thứ hai: 2 2 20,1 ; 0,1 ; 0,1 ;x y zr m r m r m      + Gia tốc kế thứ ba: 3 3 30,1 ; 0,1 ; 0,1 ;x y zr m r m r m     + Gia tốc kế thứ tư: 4 4 40,1 ; 0,1 ; 0,1 ;x y zr m r m r m     - Gia tốc góc chuẩn theo 3 trục của hệ tọa độ liên kết: + Trục x: 2( ) .cos( /16000) /180;( / )ox t t s  + Trục y: 2( ) .cos( / 6000) /180;( / )oy t t s  + Trục z: 2( ) .cos( / 26000) /180;( / )oz t t s  - Vận tốc góc chuẩn theo 3 trục hệ tọa độ liên kết: + Trục x: ( ) ( ) ( ).x x xt t t t t       + Trục y: ( ) ( ) ( ).y y yt t t t t       + Trục z: ( ) ( ) ( ).z z zt t t t t       Trên hình 3, 4, 5 mô tả đồ thị vận tốc góc chuẩn, tín hiệu của con quay vận tốc góc, vận tốc góc sau khi lọc Kalman trên 3 trục của hệ tọa độ liên kết. Tên lửa & Thiết bị bay N. H. Minh, T. Đ. Thuận,..., “Xây dựng phương pháp và thuật toán ... vận tốc góc.” 12 0 100 200 300 400 500 600 700 800 900 1000 -4 -3 -2 -1 0 1 2 3 4 Thoi gian (s) O m e g a X ( d o /s ) Tin hieu Kalman Chuan Hình 3. Đồ thị Omega X. 0 100 200 300 400 500 600 700 800 900 1000 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 Thoi gian (s) O m e g a Y ( d o /s ) Tin hieu Kalman Chuan Hình 4. Đồ thị Omega Y. 0 100 200 300 400 500 600 700 800 900 1000 -4 -3 -2 -1 0 1 2 3 4 Thoi gian (s) O m a g a Z ( d o /s ) Tin hieu Kalman Chuan Hình 5. Đồ thị Omega Z. Từ kết quả mô phỏng thấy rằng hệ thống đáp ứng được yêu cầu, nhiễu ước lượng vận tốc góc sau khi lọc Kalman giảm khoảng 4 lần so với nhiễu đo từ con quay vận tốc góc. 5. KẾT LUẬN Khi sử dụng hệ đa gia tốc kế kết hợp con quay vận tốc góc cho ước lượng tốt hơn so với việc chỉ đo vận tốc góc bằng con quay có chất lượng trung bình và thấp. Có thể sử dụng hệ này để xây dựng hệ thống dẫn đường quán tính không đế có chất lượng tốt từ các cảm biến gia tốc, cảm biến con quay có chất lượng trung bình và thấp. Phương pháp và thuật toán này đặc biệt có giá trị trong điều kiện nước ta chưa sản xuất được các phần tử Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 13 đo của thiết bị dẫn đường quán tính và chỉ mua được ở nước ngoài các phần tử đo có chất lượng trung bình và thấp. Để có thể áp dụng mô hình này vào thiết bị thực, khi thiết kế cần chú ý tính toán tâm khối của hệ để xác định chính xác các khoảng cách từ tâm khối hệ đến các cảm biến. Sai số của khoảng cách này ảnh hưởng lớn đến kết quả ước lượng vận tốc góc. Bên cạnh đó cần chú ý đến việc hiệu chỉnh các cảm biến và xác định sai số lắp đặt của từng cụm cảm biến so với hệ tọa độ liên kết của hệ. TÀI LIỆU THAM KHẢO [1]. Trần Đức Thuận, Trương Duy Trung, Nguyễn Quang Vịnh, Nguyễn Sĩ Long, Trần Xuân Kiên, Bùi Hồng Huế, Nguyễn Văn Diên, (2013), “Xây dựng thuật toán xác định tham số định hướng cho phương tiện chuyển động trên cơ sở kết hợp con quay tốc độ góc với từ kế và gia tốc kế”, Tạp chí Nghiên cứu KH&CN Quân sự, Viện KH&CN Quân sự, (25), tr 7-16. [2]. Trương Duy Trung, Nguyễn Quang Vịnh, Nguyễn Quang Hùng, Trần Đức Thuận, (2013), “Xây dựng thuật toán xác định tham số dẫn đường cho phương tiện chuyển động trên cơ sở kết hợp con quay tốc độ góc với từ kế, gia tốc kế và vận tốc kế”, Tạp chí Nghiên cứu KH&CN Quân sự, Viện KH&CN Quân sự, (21), tr 3-12. [3]. Anshu Pihadia “GPS/INS Integration Aided with Gyroscope-Free IMU for Pedestrian Applications”. MSc Thesis, Department of Geomatics Engineering, University of Calgary, Canada, 12-2010. [4]. Cardou, P. and J. Angeles “Angular velocity estimation from the angular acceleration matrix” Journal of Applied Mechanics, vol. 75, no. 2, 2008. [5]. Brown, R. and P. Hwang “Introduction to Random Signals and Applied Kalman Filtering”. Wiley, 1997. [6]. Корн Г, Корн Т., Справочник по математике для научных работников и инженеров, Издательство “Наука”, Москва, 1978 ABSTRACT CONSTRUCTING METHOD AND ALGORITHM FOR ANGULAR VELOCITY ESTIMATION BY MULTI-ACCELEROMETER WITH GYROSCOPE This paper describes concept of multi-accelerometer, method and algorithm for angular velicity estimation by multi-accelerometer with gyroscope using non-linear Kalman filter. This method and algorithm are serviceable when constructing strapdown inertial navigation system on the basis of low-cost sensors. Keywords: Missille, Inertial navigation system, Multi-accelerometer Nhận bài ngày 29 tháng 08 năm 2014 Hoàn thiện ngày 25 tháng 09 năm 2014 Chấp nhận đăng ngày 26 tháng 11 năm 2014 Địa chỉ: * Viện Tên Lửa – Viện KH&CNQS, ** Phòng Đào Tạo – Viện KH&CNQS, *** Học viện Bưu chính Viễn thông.