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ệ<br /> <br /> <br /> X©y dùng ph­¬ng ph¸p vµ thuËt to¸n<br /> x¸c ®Þnh vËn tèc gãc b»ng hÖ ®a gia tèc<br /> kÕt hîp con quay vËn tèc gãc<br /> NGUYỄN HOÀNG MINH*, TRẦN ĐỨC THUẬN**, NGUYỄN SỸ LONG*,<br /> NGUYỄN VIỆT HƯNG***<br /> 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<br /> 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<br /> 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ệ<br /> 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<br /> thấp và trung bình.<br /> Từ khóa: Tên lửa, Dẫn đường quán tính, Hệ đa gia tốc kế.<br /> <br /> 1. ĐẶT VẤN ĐỀ<br /> 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,<br /> 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<br /> 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ó<br /> độ 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<br /> 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<br /> 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<br /> 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<br /> đề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<br /> 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<br /> 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<br /> 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<br /> 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<br /> 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<br /> ố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ị<br /> bay (tên lửa, máy bay, ...) từ nước ngoài.<br /> 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<br /> ư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ị<br /> 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<br /> 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<br /> 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<br /> 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<br /> 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<br /> hơn gia tốc kế MEMS nhiều lần (Nguồn Digikey và Mouser).<br /> 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<br /> 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<br /> 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<br /> với con quay vận tốc góc.<br /> 2. HỆ ĐA GIA TỐC KẾ<br /> Ở 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<br /> 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,<br /> 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ỉ<br /> <br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 3<br />  Tên lửa & Thiết bị bay<br /> <br /> 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<br /> 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.<br /> Để 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<br /> 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<br />   <br /> có gia tốc góc  , vận tốc góc  , gia tốc dài aO . Khi đó phương trình cơ bản mô tả gia<br /> tốc của một điểm P bất kỳ trên vật rắn như sau:<br />    <br /> aP  aO  aPht  aPtt (1)<br />    <br /> trong đó, aP , aO là gia tốc dài tại điểm P và O; aPht là gia tốc hướng tâm tại P; aPtt là gia<br /> tốc tiếp tuyến tại P; O là tâm khối của vật thể.<br /> <br /> aPtt<br /> P <br /> aO<br /> <br />  <br /> O aPht  <br /> <br /> <br /> Hình 1. Chuyển động phức hợp của vật rắn lý tưởng.<br /> Chúng ta biết rằng [6]:<br />   <br /> aPtt    rP 0 (2)<br />    <br /> aPht    (  rP 0 ) (3)<br /> từ (1), (2), (3) suy ra:<br />       <br /> aP  a0    rP 0    (  rP 0 ) (4)<br /> Theo [6], áp dụng công thức nhân 2 véc tơ có hướng ta có:<br /> 0 rz  ry  x 0 rz  ry  x<br />   (5)<br />   rP 0   rz 0 rx  y   rz 0 rx  y<br /> ry  rx 0 z ry  rx 0  z<br />  x2<br />  y2<br /> 0  rx  rx ry rz 0<br />     z2 (6)<br />   (  rP O )   ry 0  ry rx 0 rz<br />  x y<br />  rz  rz 0 0 rx ry<br />  x z<br />  y z<br /> 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<br /> 3 trục như sau:<br /> Z<br /> Y<br /> 4 3<br /> <br /> <br /> <br /> 2 O 1<br /> <br /> <br /> <br /> X<br /> <br /> <br /> 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.<br /> <br /> <br /> <br /> <br /> 4 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.”<br /> Nghiên cứu khoa học công nghệ<br /> <br /> 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<br /> song song với các trục tương ứng của hệ tọa độ liên kết.<br /> Giả sử, hệ gia tốc kế số 1 nằm cách tâm khối O khoảng cách:<br /> r1x  r ; r1 y  0; r1z  0 (7)<br /> Từ công thức (1), (5), (6) ta có chỉ số của hệ gia tốc kế số 1 như sau:<br /> A1 x a x0 0  r ( y2   z2 )<br /> A1 y  a 0y  r z  r x  y (8)<br /> A1 z a z0  r y r x  z<br /> trong đó, A1x ; A1 y ; A1z là các chỉ số chỉ số lý tưởng của hệ gia tốc kế số 1; ax0 ; a 0y ; az0 là các<br /> 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<br /> 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:<br /> r2 x   r; r2 y  0; r2 z  0 (9)<br /> Từ công thức (1), (5), (6) ta có chỉ số gia tốc kế số 2 như sau:<br /> A2 x a x0 0 r ( y2   z2 )<br /> A2 y  a y0   r z   r x y (10)<br /> A2 z a 0<br /> z r y  r x z<br /> trong đó, A2 x ; A2 y ; A2 z là các chỉ số chỉ số lý tưởng của hệ gia tốc kế số 2. Hệ gia tốc<br /> kế số 3 nằm cách tâm khối O khoảng cách:<br /> r3 x  0; r3 y  r ; r3 z  0 (11)<br /> Từ công thức (1), (5), (6) ta có chỉ số gia tốc kế số 3 như sau:<br /> A3 x a x0  r  z r x y<br /> A3 y  a y  0   r ( x2   z2 )<br /> 0 (12)<br /> A3 z a z0 r  x r y  z<br /> trong đó, A3 x ; A3 y ; A3 z 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ố<br /> 4 nằm cách tâm khối O khoảng cách:<br /> r4 x  0; r4 y  0; r4 z  r (13)<br /> Từ công thức (1), (5), (6) ta có chỉ số gia tốc kế số 4 như sau:<br /> A3 x a x0 r  y r x z<br /> A3 y  a y   r  x <br /> 0<br /> r y z (14)<br /> A3 z a z0 0  r ( x2   y2 )<br /> trong đó, A4 x ; A4 y ; A4 z 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<br /> phương trình (8) với phương trình (10) nhận được:<br /> a x0 A1 x  A2 x<br /> 0 1 (15)<br /> a y  A1 y  A2 y<br /> 2<br /> a z0 A1 z  A2 z<br /> Từ phương trình (15) ta xác định được gia tốc dài aO của vật thể. Trừ hai vế của<br /> phương trình (8) cho phương trình (10) nhận được:<br /> <br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 5<br />  Tên lửa & Thiết bị bay<br /> <br /> A1 x  A2 x 0  2 r ( y2   z2 )<br /> A1 y  A2 y  2 r z  2 r x y (16)<br /> A1 z  A2 z  2 r y 2 r x z<br />  m1   x ; m 2   y ; m 3   z ;<br /> Đặt  m   2 ; m   2 ; m   2 ; (17)<br /> 4 x 5 y 6 z<br /> <br /> m<br />  7   <br /> x y ; m 8   <br /> x z ; m 9   y z ;<br /> <br /> Từ phương trình (16), (17) suy ra:<br />  m5  m 6   ( A1 x  A2 x ) / 2 r<br />  (18)<br />  m3  m 7  ( A1 y  A2 y ) / 2 r<br /> <br />   m 2  m8  ( A1 z  A2 z ) / 2 r<br /> Từ phương trình (12), (15) và (17) có thể rút ra như sau:<br />  m 3  m 7  (2 A3 x  A1 x  A2 x ) / 2 r<br />  (19)<br />  m 4  m 6   (2 A3 y  A1 y  A2 y ) / 2 r<br /> <br />  m1  m 9  (2 A3 z  A1 z  A2 z ) / 2 r<br /> Tương tự, từ phương trình (14), (15) và (17) cũng có:<br />  m2  m8  (2 A4 x  A1 x  A2 x ) / 2 r<br />  (20)<br />   m1  m9  (2 A4 y  A1 y  A2 y ) / 2 r<br /> <br />  m4  m5   (2 A4 z  A1 z  A2 z ) / 2 r<br /> Giải hệ 9 phương trình tuyến tính (18), (19), (20) với 9 ẩn mi ; i  1..9 ta có:<br /> m1   x  (2 A3 z  2 A4 y  A1z  A2 z  A1 y  A2 y ) / 4r (21)<br /> m2   y  (2 A4 x  A1x  A2 x  A1 z  A2 z ) / 4r (22)<br /> m3   z  ( A1 y  A2 y  2 A3 x  A1x  A2 x ) / 4r (23)<br /> 2<br /> m4    ( A1x  A2 x  2 A3 y  A1 y  A2 y  2 A4 z  A1z  A2 z ) / 4r<br /> x (24)<br /> m5   y2  (2 A4 z  A1z  A2 z  A1x  A2 x  2 A3 y  A1 y  A2 y ) / 4r (25)<br /> 2<br /> m6    (2 A3 y  A1 y  A2 y  A1x  A2 x  2 A4 z  A1z  A2 z ) / 4r<br /> z (26)<br /> m7   x y  ( A1 y  A2 y  2 A3 x  A1x  A2 x ) / 4r (27)<br /> m8  xz  (2 A4 x  A1x  A2 x  A1z  A2 z ) / 4r (28)<br /> m9   yz  (2 A3 z  A1z  A2 z  2 A4 y  A1 y  A2 y ) / 4r (29)<br /> Từ (15) và các nghiệm mi ; i  1..9 ở trên ta thấy rằng, bằng một hệ đa gia tốc kế có<br /> 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<br /> 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:<br /> - Tích phân gia tốc góc, hay là tích phân các nghiệm m1 ; m2 ; m3 ;<br /> - 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<br /> m4 ; m5 ; m6 ; m7 ; m8 ; m9 ;<br /> <br /> <br /> <br /> <br /> 6 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.”<br /> Nghiên cứu khoa học công nghệ<br /> <br /> 3. PHƯƠNG PHÁP VÀ THUẬT TOÁN XÁC ĐỊNH VẬN TỐC GÓC BẰNG<br /> HỆ ĐA GIA TỐC KẾ KẾT HỢP CON QUAY VẬN TỐC GÓC<br /> 3.1. Bộ lọc Kalman<br /> 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ệ<br /> độ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<br /> 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<br /> rời rạc như sau:<br /> X k  Fk 1 ( X k 1 )  Gw k , F ( X k )  ( f1 ( X k ), f 2 ( X k ),..., f n ( X k ))T ,<br /> (30)<br /> Z k  C ( X k )   k , C ( X k )  (c1 ( X k ), c2 ( X k ),...cn ( X k ))T<br /> X k , X k 1 là các véc tơ trạng thái X (n chiều) ở bước thứ k và bước thứ (k-1), Z k là giá trị<br /> 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<br /> phương tiện đo. Fk 1 là véc tơ hàm số F (.) ở bước thứ k-1, f1 , f 2 ,..., f n là các hàm phi<br /> tuyến với biến số là véc tơ X. Z k là véc tơ hàm số C (.) ở bước thứ k, C  (c1 , c2 ,...cn )T ,<br /> 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<br /> bằng 0: G -ma trận hệ số nhiễu, w k ~ N (0, Qk ), k ~ N (0, Rk ) . Pk   ([X k  Xˆ k ][X k  Xˆ k ]T ) ,<br />  (.) 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ở<br /> véc tơ quan sát Z theo Kalman như sau:<br /> f C k<br /> Xˆ k (  )  f k 1 ( Xˆ k 1 (  )),  k 1  k 1 , Zˆ k  C k ( Xˆ k (  )), H k  ,<br /> X X  Xˆ k 1 (  ) X X  Xˆ k (  )<br /> (31)<br /> Pk (  )   k 1 Pk 1 (  )  Tk 1  G k 1Q k 1G k 1 , K k  Pk (  ) H kT ( H k Pk (  ) H kT  Rk )  1<br /> Xˆ (  )  Xˆ (  )  K ( Z  Zˆ ), P (  )  ( I  K H ) P (  )<br /> k k k k k k k k k<br /> <br /> Để á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:<br /> các hàm động học f1 ( X k ), f 2 ( X k ) , f3 ( X k ),..., f n ( X k ) ;các hàm quán sát c1 ( X k ), c2 ( X k ) ,<br /> c3 ( X k ),..., cm ( X k ) ; ma trận hệ số nhiễu G ; véc tơ nhiễu động học w k , véc tơ nhiễu<br /> 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<br /> 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.<br /> 3.2. Thuật toán kết hợp<br /> 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<br /> đị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ó<br /> 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<br /> 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<br /> 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<br /> 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.<br /> Để 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<br /> 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<br /> 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<br />  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<br /> 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<br /> 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<br /> 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<br /> 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<br /> 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:<br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 7<br />  Tên lửa & Thiết bị bay<br /> <br /> A ( k )  A ( k )  ba ( k )   a ( k ) (32)<br /> 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<br /> điểm k, ba ( 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<br /> gia tốc kế tại thời điểm k.<br /> Tương tự cho con quay vận tốc góc ta có<br />  (k )   (k )  bg (k )   g (k ) (33)<br /> 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<br /> thời điểm k, bg (k ) là độ trôi của con quay tương ứng tại thời điểm k,  g ( k ) là nhiễu đo<br /> 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<br /> trạng thái như sau:<br /> X  [x1 x2 x3 ...x18 ]T<br /> (34)<br />  [x  y  z bgx bgy bgz ba1x ba1 y ba1z ba 2 x ba 2 y ba 2 z ba 3 x ba 3 y ba 3 z ba 4 x ba 4 y ba 4 z ]T<br /> 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;<br /> bgx , bgz , bgz là độ trôi của con quay vận tốc góc theo 3 trục của hệ tọa độ liên<br /> kết; baix , baiy , baiz 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.<br /> 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<br /> về gia tốc góc cúa nó:<br />  x (k  1)   x (k )   x (k ).t (35)<br /> Từ (32), (21), (17) phương trình (35) sẽ có dạng như sau:<br /> x1 (k  1)   x (k  1)  f1 ( X (k ))  w1 (k )<br />   x (k )  t (2 A3 z  2 A 4 y  A1z  A 2 z  A1 y  A 2 y ) / 4r<br />  t (2ba 3 z  2ba 4 y  ba1z  ba 2 z  ba1 y  ba 2 y ) / 4r<br /> (36)<br />  t (2 a 3 z  2a 4 y  a1z   a 2 z  a1 y  a 2 y ) / 4r<br />  x1 (k )  t (2 A3 z  2 A 4 y  A1z  A2 z  A1 y  A2 y ) / 4r<br />  t (2 x15 (k )  2 x17 (k )  x9 (k )  x12 (k )  x8 (k )  x11 (k )) / 4r  w1 (k )<br /> trong đó, w1 ( k )  t (2 a 3 z  2 a 4 y   a1z   a 2 z   a1 y   a 2 y ) / 4r<br /> Tương tự cho  y và  z như sau:<br /> x2 ( k  1)   y (k  1)  f 2 ( X (k ))  w 2 (k )  x2 (k )<br />  t (2 A 4 x  A1x  A 2 x  A1z  A 2 z ) / 4r (37)<br />  t (2 x16  x7  x10  x9  x12 ) / 4r<br />  t (2 a 4 x   a1x   a 2 x  a1z  a 2 z ) / 4r<br /> x3 (k  1)  z (k  1)  f 3 ( X (k ))  w 3 (k )<br />  x3 (k )  t ( A1 y  A 2 y  2 A3 x  A1x  A 2 x ) / 4r (38)<br />  t ( x8  x11  2 x13  x7  x10 ) / 4r  t (a1 y   a 2 y  2a 3 x  a1x  a 2 x ) / 4r<br /> 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ó:<br /> +Ba độ trôi của ba con quay đo tốc độ góc:<br /> <br /> <br /> 8 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.”<br /> Nghiên cứu khoa học công nghệ<br /> <br /> bg (k  1)  bg (k ) (39)<br /> +Mười hai độ chệch không (độ trôi) của 12 gia tốc kế:<br /> ba (k  1)  ba (k ) (40)<br /> Khi đó, trạng thái thứ 4 đến thứ 18 của hệ sẽ có dạng sau:<br /> xi (k  1)  fi ( X (k ))  xi (k ) (41)<br /> với xi 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<br /> 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<br /> ma trận G sẽ là ma trận bậc [18  3] với các phần tử như sau: g11  g 22  g 33  1 còn các<br /> 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<br /> sát như sau:<br /> Z  [z1 z2 z3 z4 z5 z6 z7 z8 z9 ]T<br /> T (42)<br />   x  y  z m 4 m 5 m 6 m 7 m 8 m 9 <br /> trong đó,  x ,  y ,  z là các chỉ số con quay vận tốc góc theo 3 trục; m<br />  i là thành phần bậc<br /> hai của vận tốc góc tính theo chỉ số của các gia tốc kế A . Từ (33) ta có:<br /> i<br /> <br />  x (k )  x (k )  bgx   gx<br />  x1 (k )  x4 (k )   gx (43)<br />  c1 ( X (k ))   gx<br />  y (k )   y (k )  bgy   gy<br />  x2 (k )  x5 (k )   gy (44)<br />  c2 ( X (k ))   gy<br />  z (k )   z (k )  bgz   gz<br />  x3 (k )  x6 (k )   gz (45)<br />  c3 ( X (k ))   gz<br /> Từ (24) và (32) suy ra:<br /> m 4 (k )  ( A1x  A2 x  2 A3 y  A1 y  A2 y  2 A4 z  A1z  A 2 z ) / 4r<br />  ( A1 x  A2 x  2 A3 y  A1 y  A2 y  2 A4 z  A1z  A2 z ) / 4r<br />  (ba1x  ba 2 x  2ba 3 y  ba1 y  ba 2 y  2ba 4 z  ba1z  ba 2 z ) / 4r<br />  ( a1x   a 2 x  2a 3 y  a1 y   a 2 y  2 a 4 z   a1z   a 2 z ) / 4r (46)<br />  x12  ( x7  x10  2 x14  x8  x11  2 x18  x9  x12 ) / 4r<br />  ( a1x   a 2 x  2a 3 y  a1 y   a 2 y  2 a 4 z   a1z   a 2 z ) / 4r<br />  c4 ( X (k ))  4<br /> trong đó,<br /> x12  x2  ( A1x  A2 x  2 A3 y  A1 y  A2 y  2 A4 z  A1z  A2 z ) / 4r (theo (24))<br /> c4 ( X )  x12  ( x7  x10  2 x14  x8  x11  2 x18  x9  x12 ) / 4r<br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 9<br />  Tên lửa & Thiết bị bay<br /> <br />  4  ( a1x   a 2 x  2a 3 y  a1 y  a 2 y  2 a 4 z   a1z  a 2 z ) / 4r<br /> Tương tự áp dụng cho m  5 ; m 6 ; m 7 ; m 8 ; m 9<br /> m (k )  (2 A  A  A  A  A  2 A  A  A ) / 4r<br /> 5<br /> <br /> 4z 1z 2z 1x 2x 3y 1y 2y<br /> 2<br />  x  (2 x18  x9  x12  x7  x10  2 x14  x8  x11 ) / 4r<br /> 2<br /> (47)<br />  (2 a 4 z  a1z  a 2 z   a1x   a 2 x  2 a 3 y  a1 y   a 2 y ) / 4r<br />  c5 ( X (k ))   5<br /> m 6 (k )  (2 A3 y  A1 y  A2 y  A1x  A 2 x  2 A 4 z  A1z  A 2 z ) / 4r<br />  x32  (2 x14  x8  x11  x7  x10  2 x18  x9  x12 ) / 4r<br /> (48)<br />  (2 a 3 y   a1 y   a 2 y   a1x  a 2 x  2a 4 z   a1z  a 2 z ) / 4r<br />  c6 ( X (k ))   6<br /> m 7 (k )  ( A1 y  A 2 y  2 A3 x  A1x  A 2 x ) / 4r<br />  x1 x2  ( x8  x11  2 x13  x7  x10 ) / 4r<br /> (49)<br />  ( a1 y  a 2 y  2a 3 x   a1x   a 2 x ) / 4r<br />  c7 ( X (k ))  7<br /> m 8 (k )  (2 A 4 x  A1 x  A2 x  A1 z  A2 z ) / 4r<br />  x1 x3  (2 x16  x7  x10  x9  x12 ) / 4r<br /> (50)<br />  (2a 4 x  a1x  a 2 x   a1z  a 2 z ) / 4r<br />  c8 ( X (k ))  8<br /> <br /> m 9 (k )  (2 A4 x  A1z  A 2 z  2 A 4 y  A1 y  A 2 y ) / 4r<br />  x2 x3  (2 x16  x9  x12  2 x17  x8  x11 ) / 4r<br /> (51)<br />  (2a 4 x  a1z   a 2 z  2a 4 y   a1 y  a 2 y ) / 4r<br />  c9 ( X (k ))   9<br /> Lấy đạo hàm riêng f i ( X (k )), (i  1, 2,3,...,18) theo các biến số x j ( j  1, 2,3,...,18)<br /> f i<br /> có ma trận  (t ) kích thước (18x18) có các phần tử là x j . Lấy đạo hàm riêng<br /> ci ( X (k )), (i  1,...,9) theo các biến số x j ( j  1, 2,3,...,18) có ma trận H (t ) kích thức<br /> ci<br /> (9x18) có các phần tử là x j . Khi nhiễu đo không tương quan lẫn nhau thì ma trận hiệp<br /> 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à<br /> 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ử<br /> q11 , q22 , q33 khác 0. Từ biểu thức (36) có:<br /> t<br /> q11   (2 a23 z  2 a24 y   a21z  2 a22 z   a21 y   a22 y ) (52)<br /> 4r<br /> <br /> <br /> <br /> <br /> 10 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.”<br /> Nghiên cứu khoa học công nghệ<br /> <br /> trong đó,  a2 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),<br /> (38) sẽ xác định được q22 , q33 tương tự như (52). Còn ma trận R là ma trận có kích thước<br /> [9x9] có các phần tử rii , i  1, 2,3,..., 9 khác không. Từ biểu thức (43), (44), (45) có:<br /> r11   gx2 , r22   gy2 , r33   gz2 (53)<br /> 2<br /> trong đó,  gx ,  gy2 ,  gz2 là các phương sai nhiễu đo của các con quay đo tốc độ góc.<br /> Từ biểu thức (46) có:<br /> r44  ( a21x   a22 x  2 a23 y   a21 y   a22 y  2 a24 z   a21z   a22 z ) / (4r ) (54)<br /> Tương tự (54) từ các công thức (47), (48), (49). (50), (51) sẽ tính được các<br /> rii , i  5, 6, 7,8,9 .<br /> 4. KẾT QUẢ MÔ PHỎNG KIỂM NGHIỆM THUẬT TOÁN<br /> 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<br /> kế kết hợp với con quay có các tham số đầu vào như sau:<br /> - Khoảng cách từ tâm khối đến 4 gia tốc kế:<br /> + Gia tốc kế thứ nhất: r1x  0,1m; r1 y  0,1m; r1z  0,1m;<br /> + Gia tốc kế thứ hai: r2 x  0,1m; r2 y  0,1m; r2 z  0,1m;<br /> + Gia tốc kế thứ ba: r3 x  0,1m; r3 y  0,1m; r3 z  0,1m;<br /> + Gia tốc kế thứ tư: r4 x  0,1m; r4 y  0,1m; r4 z  0,1m;<br /> <br /> - Gia tốc góc chuẩn theo 3 trục của hệ tọa độ liên kết:<br /> + Trục x:  x (t )   .cos(t /16000) /180;( o / s 2 )<br /> + Trục y:  y (t )   .cos(t / 6000) / 180;(o / s 2 )<br /> + Trục z:  z (t )   .cos(t / 26000) /180;( o / s 2 )<br /> <br /> - Vận tốc góc chuẩn theo 3 trục hệ tọa độ liên kết:<br /> + Trục x:  x (t  t )  x (t )   x (t ).t<br /> + Trục y:  y (t  t )   y (t )   y (t ).t<br /> + Trục z:  z (t  t )   z (t )   z (t ).t<br /> <br /> 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,<br /> 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.<br /> <br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 11<br />  Tên lửa & Thiết bị bay<br /> <br /> 4<br /> Tin hieu<br /> 3<br /> Kalman<br /> 2 Chuan<br /> Omega X (do/s)<br /> <br /> <br /> <br /> <br /> 1<br /> <br /> 0<br /> <br /> -1<br /> <br /> -2<br /> <br /> -3<br /> <br /> -4<br /> 0 100 200 300 400 500 600 700 800 900 1000<br /> Thoi gian (s)<br /> Hình 3. Đồ thị Omega X.<br /> 2<br /> Tin hieu<br /> 1.5 Kalman<br /> Chuan<br /> 1<br /> Omega Y (do/s)<br /> <br /> <br /> <br /> <br /> 0.5<br /> <br /> 0<br /> <br /> -0.5<br /> <br /> -1<br /> <br /> -1.5<br /> <br /> -2<br /> 0 100 200 300 400 500 600 700 800 900 1000<br /> Thoi gian (s)<br /> Hình 4. Đồ thị Omega Y.<br /> 4<br /> <br /> 3<br /> <br /> 2<br /> Omaga Z (do/s)<br /> <br /> <br /> <br /> <br /> 1<br /> Tin hieu<br /> 0 Kalman<br /> Chuan<br /> -1<br /> <br /> -2<br /> <br /> -3<br /> <br /> -4<br /> 0 100 200 300 400 500 600 700 800 900 1000<br /> Thoi gian (s)<br /> Hình 5. Đồ thị Omega Z.<br /> 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<br /> 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.<br /> 5. KẾT LUẬN<br /> 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<br /> 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ử<br /> 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<br /> 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à<br /> 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ử<br /> <br /> <br /> <br /> 12 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.”<br /> Nghiên cứu khoa học công nghệ<br /> <br /> đ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<br /> lượng trung bình và thấp.<br /> Để 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<br /> 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<br /> 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 đó<br /> 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<br /> biến so với hệ tọa độ liên kết của hệ.<br /> <br /> TÀI LIỆU THAM KHẢO<br /> [1]. Trần Đức Thuận, Trương Duy Trung, Nguyễn Quang Vịnh, Nguyễn Sĩ Long, Trần<br /> 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<br /> 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 độ<br /> 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<br /> Quân sự, (25), tr 7-16.<br /> [2]. Trương Duy Trung, Nguyễn Quang Vịnh, Nguyễn Quang Hùng, Trần Đức Thuận,<br /> (2013), “Xây dựng thuật toán xác định tham số dẫn đường cho phương tiện chuyển<br /> độ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<br /> chí Nghiên cứu KH&CN Quân sự, Viện KH&CN Quân sự, (21), tr 3-12.<br /> [3]. Anshu Pihadia “GPS/INS Integration Aided with Gyroscope-Free IMU for Pedestrian<br /> Applications”. MSc Thesis, Department of Geomatics Engineering, University of<br /> Calgary, Canada, 12-2010.<br /> [4]. Cardou, P. and J. Angeles “Angular velocity estimation from the angular<br /> acceleration matrix” Journal of Applied Mechanics, vol. 75, no. 2, 2008.<br /> [5]. Brown, R. and P. Hwang “Introduction to Random Signals and Applied Kalman<br /> Filtering”. Wiley, 1997.<br /> [6]. Корн Г, Корн Т., Справочник по математике для научных работников и<br /> инженеров, Издательство “Наука”, Москва, 1978<br /> <br /> ABSTRACT<br /> CONSTRUCTING METHOD AND ALGORITHM FOR ANGULAR VELOCITY<br /> ESTIMATION BY MULTI-ACCELEROMETER WITH GYROSCOPE<br /> This paper describes concept of multi-accelerometer, method and algorithm for<br /> angular velicity estimation by multi-accelerometer with gyroscope using non-linear<br /> Kalman filter. This method and algorithm are serviceable when constructing<br /> strapdown inertial navigation system on the basis of low-cost sensors.<br /> Keywords: Missille, Inertial navigation system, Multi-accelerometer<br /> <br /> <br /> <br /> <br /> Nhận bài ngày 29 tháng 08 năm 2014<br /> Hoàn thiện ngày 25 tháng 09 năm 2014<br /> Chấp nhận đăng ngày 26 tháng 11 năm 2014<br /> <br /> Địa chỉ: * Viện Tên Lửa – Viện KH&CNQS,<br /> ** Phòng Đào Tạo – Viện KH&CNQS,<br /> *** Học viện Bưu chính Viễn thông.<br /> <br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số 34, 12 - 2014 13<br />