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Ö<br /> <br /> <br /> X©Y DùNG THUËT TO¸N X¸C ®ÞNH THAM Sè<br /> DÉN ®­êng CHO vò khÝ chèng NGÇM<br /> TR­¬NG DUY TRUNG, NGUYÔN QUANG VÞNH<br /> 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 ®Ó<br /> 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<br /> 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<br /> 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<br /> chuyÓn ®éng trong m«i tr­êng n­íc.<br /> 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.<br /> <br /> 1. §ÆT VÊN §Ò<br /> 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<br /> hÑp nªn hÖ täa ®é ®Þa lý OX 0Y0 Z 0 n¬i m¸y bay th¶ vò khÝ chèng ngÇm cã thÓ xem lµ hÖ täa ®é<br /> dÉn ®­êng. HÖ täa ®é g¾n liÒn Gb X bYb Z b cã t©m trïng víi t©m träng lùc Gb (h×nh 1).<br /> <br /> <br /> <br /> <br /> H×nh 1. HÖ täa ®é tham chiÕu cho ASWs.<br /> 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<br /> th«ng qua 4 sè Rodrig-Hamilton 0 , 1 , 2 , 3 nh­ sau [3]:<br />  202  212  1 212  20 3 213  20 2 <br />   (1)<br /> Cbn   212  20 3 202  222  1 22 3  20 1 <br />  213  20 2 22 3  20 1 20  23  1<br /> 2 2<br /> <br /> <br /> <br /> 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]:<br /> 2  - p -  q -  r<br /> 0 1 2 3<br /> <br /> 21  0 p - 3q  2 r<br /> (2)<br /> 2   p   q -  r<br /> 2 3 0 1<br /> <br /> 23  -2 p  1q  0 r<br /> 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.<br /> Tõ viÖc x¸c ®Þnh ®­îc ma trËn C«sin ®Þnh h­íng Cbn sÏ x¸c ®Þnh ®­îc thµnh phÇn gia<br /> tèc trong hÖ täa ®é ®Þa lý [3]: f  Cbn ab (3)<br /> T<br /> trong ®ã, f  [ f N , f E , f D ] lµ thµnh phÇn cña vÐc t¬ gia tèc trong hÖ täa ®é ®Þa lý;<br /> ab  [ abx , aby , abz ]T lµ chØ sè cña gia tèc kÕ trong hÖ täa ®é g¾n liÒn.<br /> VËn tèc t©m khèi cña ASWs trong hÖ täa ®é ®Þa lý ®­îc x¸c ®Þnh [3]:<br /> V  f ; V  f ; V  f  g<br /> N N E E D D<br /> (4)<br /> Täa ®é t©m khèi cña ASWs trong hÖ täa ®é ®Þa lý ®­îc tÝnh [3]:<br /> x  VN ; y  VE ; z  VD (5)<br /> <br /> <br /> <br /> T¹p chÝ Nghiªn cøu KH&CN qu©n sù, Sè 31, 06 - 2014 117<br />  §iÒu khiÓn & Tù ®éng hãa<br /> <br /> C¸c gãc ®Þnh h­íng cña ASWs ®­îc x¸c ®Þnh th«ng qua c¸c tham sè Rodrig-Hamilton [3]:<br />  2   20 3   2   2  <br />   arctg  12 2 2  ;    arcsin(213  20 2 );   arctg  22 3 2 0 1  (6)<br />  20  21  1   20  23  1 <br /> 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Þ<br /> 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Õ<br /> cho gi¸ trÞ ®o bao gåm gia tèc chuyÓn ®éng vµ nhiÔu:<br />  p  p  bp  w1 ; q  q  bq  w2 ; r  r  br  w3 , (7)<br /> a x  abx  w4 ; a y  aby  w5 ; az  abz  w6 , (8)<br /> trong ®ã, bp , bq , br lµ c¸c tham sè biÕn ®æi chËm thÓ hiÖn ®é tr«i cña con quay vµ<br /> w1 , w2 , w3 , w4 , w5 , w6 lµ c¸c nhiÔu ®o th­êng cã d¹ng t¹p tr¾ng (nhiÔu Gauss).<br /> 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Þ<br /> ®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<br /> 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<br /> 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<br /> 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<br /> 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 -<br /> 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<br /> quay vi c¬ vµ nhiÔu ®o gia tèc kÕ g©y nªn.<br /> <br /> 2. THUËT TO¸N X¸C §ÞNH THAM Sè DÉN §¦êNG CHO Vò KHÝ CHèNG<br /> NGÇM giai ®o¹n chuyÓn ®éng trong khÝ quyÓn<br /> Giai ®o¹n chuyÓn ®éng r¬i trong khÝ quyÓn ë chÕ ®é cã dï, vËn tèc chuyÓn ®éng cña<br /> 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<br /> Kalman phi tuyÕn më réng ­íc l­îng trùc tiÕp c¸c tham sè Rodrig - Hamilton trªn c¬ së<br /> 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Ý<br /> 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Ö<br /> täa ®é g¾n liÒn (h×nh 2):<br /> <br /> <br /> <br /> <br /> T1 , T2 , T3 - c¸c tõ kÕ; A1 , A2 , A3 - c¸c gia tèc kÕ; C1 , C2 , C3 - c¸c con quay vi c¬<br /> H×nh 2. Bè trÝ con quay vi c¬ ®o tèc ®é gãc, gia tèc kÕ vµ tõ kÕ.<br /> Tõ (7) rót ra ®­îc: p   p  bp  w1 , q  q  bq  w2 , r  r  br  w3 , (9)<br /> Thay c¸c ph­¬ng tr×nh (9) vµo c¸c ph­¬ng tr×nh (2) ta cã:<br /> 20  1 ( p  b p  w1 )  2 (q  bq  w2 )  3 (r  br  w3 ) (10)<br /> 21  0 ( p  bp  w1 )  3 (q  bq  w2 )  2 (r  br  w3 ) (11)<br /> <br /> <br /> <br /> <br /> 118 T. D. Trung, N. Q. Vịnh, “X©y dùng thuËt to¸n x¸c ®Þnh … vò khÝ chèng ngÇm.”<br /> Nghiªn cøu khoa häc c«ng nghÖ<br /> <br /> 22  3 ( p  bp  w1 )  0 (q  bq  w2 )  1 (r  br  w3 ) (12)<br /> 23  2 ( p  bp  w1 )  1 (q  bq  w2 )  0 (r  br  w3 ) (13)<br /> Nguyªn lý läc Kalman ®­îc m« t¶ nh­ sau: Gi¶ sö r»ng chuyÓn ®éng cña vËt thÓ ®­îc<br /> m« t¶ bëi hÖ ph­¬ng tr×nh ®éng häc d­íi d¹ng rêi r¹c nh­ sau:<br /> X k  F k 1 ( X k 1 )  G ( X k 1 ) (k ), Z k  h( X k )  v(k ). (14)<br /> trong ®ã, X k , X k 1 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<br /> 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;<br /> VÐc t¬ ®Çu ra Z k cã thÓ ®o ®­îc b»ng c¸c ph­¬ng tiÖn ®o; F k 1 lµ vÐc t¬ hµm sè F ë b­íc<br /> thø k-1; f1, f2, ...fn lµ c¸c hµm phi tuyÕn víi biÕn sè lµ vÐc t¬ X ; Z k lµ gi¸ trÞ cña vÐc t¬ ®Çu ra<br /> h (vÐct¬ m chiÒu, th­êng m  n ) ë b­íc thø k ®­îc ®o bëi thiÕt bÞ ®o, h  (h1 , h2 ,....., hm )T ; v<br /> 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.<br />  ~ N (0, Qk ), E  ( j ),  T ( k )   Qk , E  ( k )   0, v ~ N (0, Rk ), E  v ( j ), vT (k )   Rk , E  v (k )  0,<br /> T<br /> Pk  E (  X k  Xˆ k   X k  Xˆ k  ) trong ®ã, E lµ ký hiÖu phÐp lÊy kú väng to¸n häc.<br /> 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<br /> 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¸<br /> c¸c tham sè 0 , 1 , 2 , 3 tiÕn hµnh c¸c b­íc sau:<br /> <br /> 2.1. §Æt c¸c biÕn cña vÐc t¬ tr¹ng th¸i X<br /> x1  0 ; x2  1 ; x3  2 ; x4  3 ; x5  p; x6  q; x7  r ; x8  bp ; x9  bq ; x10  br (15)<br /> <br /> <br /> <br /> <br /> 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Õ.<br /> <br /> 2.2. X©y dùng hµm ®éng häc F k 1 ( X k 1 )<br /> Víi c¸ch ®Æt biÕn nh­ (15) c¸c ph­¬ng tr×nh (10) ®Õn (13) ®­îc viÕt l¹i:<br /> <br /> <br /> <br /> <br /> T¹p chÝ Nghiªn cøu KH&CN qu©n sù, Sè 31, 06 - 2014 119<br />  §iÒu khiÓn & Tù ®éng hãa<br /> <br /> 2 x1   x2 ( p  x8  w1 )  x3 (q  x9  w2 )  x4 (r  x10  w3 )<br /> (16)<br />   x2 ( p  x8 )  x3 (q  x9 )  x4 (r  x10 )  x2 w1  x3 w2  x4 w3<br /> 2 x2  x1 ( p  x8  w1 )  x3 (r  x10  w3 )  x4 (q  x9  w2 )<br /> (17)<br />  x1 ( p  x8 )  x3 (r  x10 )  x4 (q  x9 )  x1 w1  x3 w3  x4 w2<br /> 2 x3  x1 (q  x9  w2 )  x2 (r  x10  w3 )  x4 ( p  x8  w1 )<br /> (18)<br />  x1 (q  x9 )  x2 (r  x10 )  x4 ( p  x8 )  x1 w2  x2 w3  x4 w1<br /> 2 x4  x1 (r  x10  w3 )  x2 (q  x9  w2 )  x3 ( p  x8  w1 )<br /> (19)<br />  x1 (r  x10 )  x2 (q  x9 )  x3 ( p  x8 )  x1 w3  x2 w2  x3 w1<br /> T¸ch c¸c sè h¹ng cã thõa sè nhiÔu w1 , w2 , w3 vµ tõ ph­¬ng tr×nh vi ph©n (16) ®Õn (19),<br /> c¸c hµm ®éng häc f1 , f 2 , f3 , f 4 viÕt d­íi d¹ng rêi r¹c cña chóng theo ph­¬ng ph¸p ¥le lµ:<br /> x1 ( k )  f1 ( X k 1 )  x1 ( k  1) / 2  T [  x2 (k  1)( p  x8 (k  1))<br /> (20)<br />  x3 (k  1) (q  x9 ( k  1))  x4 ( k  1)(r  x10 ( k  1))] / 2<br /> x2 ( k )  f 2 ( X k 1 )  x2 ( k  1) / 2  T [ x1 ( k  1)( p  x8 ( k  1))<br /> (21)<br />  x3 (k  1) (r  x10 ( k  1))  x4 ( k  1)(q  x9 ( k  1))] / 2<br /> x3 ( k )  f3 ( X k 1 )  x3 (k  1)  T [ x1 ( k  1)(q  x9 (k  1))<br /> (22)<br />  x2 (k  1)(r  x10 (k  1))  x4 ( k  1)( p  x8 (k  1))] / 2<br /> x4 ( k )  f 4 ( X k 1 )  x4 (k  1) / 2  T [ x1 (k  1)(r  x10 (k  1))<br /> (23)<br />  x2 (k  1)(q  x9 (k  1))  x3 (k  1)( p  x8 ( k  1))] / 2<br /> ë ®©y, T lµ b­íc tÝnh khi rêi r¹c hãa.<br /> §è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<br /> ph­¬ng tr×nh vi ph©n nh­ c¸c tèc ®é gãc p, q, r sÏ ®­îc x¸c ®Þnh nh­ sau:<br /> x5 (k )  f5 ( X k 1 )  x5 (k  1) ; x6 (k )  f6 ( X k 1 )  x6 (k 1) ; x7 ( k )  f 7 ( X k 1 )  x7 ( k  1) (24)<br /> §é tr«i bp , bq , br còng cã thÓ viÕt d­íi d¹ng:<br /> x8 (k )  f8 ( X k 1 )  x8 (k  1) ; x9 (k )  f9 ( X k 1 )  x9 (k  1) ; x10 (k )  f10 ( X k 1 )  x10 (k 1) (25)<br /> <br /> Nh­ vËy, tõ c¸c ph­¬ng tr×nh (20) ®Õn (25) cho ®Çy ®ñ c¸c hµm f1 , f 2 ,..., f10 cña vÐc t¬<br /> Fk 1<br /> hµm Fk 1 ( X k 1 ) . TiÕp theo x¸c ®Þnh ma trËn chuyÓn vÞ  k 1  X  Xˆ k 1 (  )<br /> b»ng c¸ch lÊy<br /> X<br />  f <br /> ®¹o hµm riªng: k 1   i   ij  , i  1,10; j  1,10 (26)<br />  x j <br /> trong ®ã, 12  ( p  x8 (k  1))T / 2 ; 13  (q  x9 ( k  1))T / 2 ;<br /> 14  (r  x10 (k  1))T / 2 ; 18  x2 (k  1)T / 2 ; 19  x3 (k  1)T / 2 ;<br /> 110  x4 (k  1)T / 2 ; 21  ( p  x8 (k  1))T / 2 ; 23  (r  x10 (k  1))T / 2 ;<br /> 24  (q  x9 ( k  1))T / 2 ; 28   x1 (k  1)T / 2 ; 29  x4 (k  1)T / 2 ;<br /> 210   x3 (k  1)T / 2 ; 31  (q  x9 ( k  1))T / 2 ; 32  (r  x10 ( k  1))T / 2 ;<br /> <br /> <br /> 120 T. D. Trung, N. Q. Vịnh, “X©y dùng thuËt to¸n x¸c ®Þnh … vò khÝ chèng ngÇm.”<br /> Nghiªn cøu khoa häc c«ng nghÖ<br /> <br /> 34  ( p  x8 (k  1))T / 2 ; 38   x4 (k  1)T / 2 ; 39   x1 (k  1)T / 2 ;<br /> 310  x2 (k  1)T / 2 ; 41  (r  x10 ( k  1))T / 2 ; 42  (q  x9 ( k  1))T / 2 ;<br /> 43  ( p  x8 (k  1))T / 2 ; 48  x3 (k  1)T / 2 ; 49   x2 (k  1)T / 2 ;<br /> 410   x1 (k  1)T / 2 ; ii  1 ; tÊt c¶ c¸c ij ®Òu b»ng 0, i  1,10; j  1,10 .<br /> 2.3. X¸c ®Þnh ma trËn hÖ sè nhiÔu G ( X k 1 )<br /> 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<br /> r¹c theo ph­¬ng ph¸p ¥le:<br /> g1  [ w1 x2 (k  1)  w2 x3 (k  1)  w3 x4 ( k  1)]T / 2 (27)<br /> g 2  [ w1 x1 ( k  1)  w3 x3 (k  1)  w2 x4 ( k  1)]T / 2 (28)<br /> g 3  [ w2 x1 (k  1)  w3 x2 ( k  1)  w1 x4 ( k  1)]T / 2 (29)<br /> g 4  [ w1 x3 ( k  1)  w2 x2 ( k  1)  w3 x1 ( k  1)]T / 2 (30)<br /> g i<br /> LÊy ®¹o hµm riªng , nhËn ®­îc ma trËn hÖ sè nhiÔu:<br /> w j<br /> G   gij  , i  1,10; j  1,3 (31)<br /> trong ®ã, g11  x2 ( k  1)T / 2 ; g12  x3 ( k  1)T / 2 ; g13  x4 (k  1)T / 2 ;<br /> g 21   x1 ( k  1)T / 2 ; g 22  x4 (k  1)T / 2 ; g 23   x3 (k  1)T / 2 ; g 31   x4 ( k  1)T / 2 ;<br /> g32   x1 (k  1)T / 2 ; g 33  x2 ( k  1)T / 2 ; g 41   x3 (k  1)T / 2 ; g 42   x2 (k  1)T / 2 ;<br /> g 43   x1 (k  1)T / 2 ; c¸c hÖ sè g ij kh¸c ®Òu b»ng 0.<br /> 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<br /> c¸c con quay vi c¬, tøc lµ ph­¬ng sai cña c¸c nhiÔu w1 , w2 , w3 .<br /> <br /> 2.4. X©y dùng c¸c hµm quan s¸t h( X k )<br /> VÊn ®Ò tiÕp theo lµ t×m c¸c hµm quan s¸t hi . Nh­ trªn ®· nªu trong ASWs cã ba tõ kÕ<br /> ®o ba thµnh phÇn z1 , z2 , z3 trong hÖ täa ®é g¾n liÒn cña vÐc t¬ tõ tr­êng tr¸i ®Êt n¬i ASWs<br /> ®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<br /> gi¸ trÞ lÇn l­ît theo ba ph­¬ng cña hÖ täa ®é ®Þa lý nh­ sau: Bx , By , Bz (c¸c gi¸ trÞ nµy<br /> ®­î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<br /> ®o b»ng c¸ch nµo ®ã). DÔ dµng nhËn thÊy gi÷a z1 , z2 , z3 vµ Bx , By , Bz cã quan hÖ mËt thiÕt<br /> víi nhau qua ma trËn C«sin chØ ph­¬ng, tøc lµ: [ z1 , z2 , z3 ]T  (Cbn )T [ Bx , By , Bz ]T (32)<br /> TriÓn khai biÓu thøc (32) víi ma trËn Cbn nh­ (1) ta cã:<br /> z1  (202  212  1) Bx  (212  20 3 ) By  (213  20 2 ) Bz (33)<br /> z2  (212  20 3 ) Bx  (202  222  1)) By  (22 3  20 1 ) Bz (34)<br /> 2 2<br /> z3  (213  20 2 ) Bx  (22 3  20 1 ) By  (2  2  1) Bz<br /> 0 3 (35)<br /> Víi c¸ch ®Æt biÕn (15), tõ (33) ®Õn (35) ta cã ba hµm quan s¸t:<br /> <br /> <br /> <br /> <br /> T¹p chÝ Nghiªn cøu KH&CN qu©n sù, Sè 31, 06 - 2014 121<br />  §iÒu khiÓn & Tù ®éng hãa<br /> <br /> z1 (k ) h 1 ( X k )  (2 x12 ( k )  2 x22 (k )  1) Bx  (2 x2 ( k ) x3 (k )<br /> (36)<br />  2 x1 ( k ) x4 ( k )) By  (2 x2 ( k ) x4 ( k )  2 x1 ( k ) x3 ( k )) Bz<br /> z2 (k )  h2 ( X k )  (2 x2 ( k ) x3 ( k )  2 x1 ( k ) x4 ( k )) Bx<br /> (37)<br />  (2 x12 ( k )  2 x32 ( k )  1) By  (2 x3 ( k ) x4 ( k )  2 x1 ( k ) x2 ( k )) Bz<br /> z3 (k ) h3 ( X k )  (2 x2 ( k ) x4 ( k )  2 x1 (k ) x3 ( k )) Bx<br /> (38)<br />  (2 x3 (k ) x4 ( k )  2 x1 ( k ) x2 ( k )) By  (2 x12 (k )  2 x42 ( k )  1) Bz<br /> 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¸<br /> 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<br /> g x 0 , g y 0 , g z 0 (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<br /> 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<br /> 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µ:<br /> abx  f N  g x ; aby  f E  g y ; abz  f D  g z , trong ®ã f N , f E , f D lµ h×nh chiÕu gia tèc tuyÖt<br /> ®èi cña ASWs xuèng c¸c trôc cña hÖ täa ®é g¾n liÒn, cßn g x , g y , g z lµ h×nh chiÕu cña vÐc<br /> 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<br /> trong tr­êng hîp th¶ ASWs tõ m¸y bay ë chÕ ®é cã dï th× f N  0; f E  0; f D  0 . Trong<br /> tr­êng hîp kh«ng xÐt ®Õn nhiÔu ®o, ba gia tèc kÕ ®o ba thµnh phÇn<br /> [ z4 , z5 , z6 ]T  [ abx , aby , abz ]T  [ g x , g y , g z ]T . Khi ®ã cã thÓ coi:<br /> [ z4 , z5 , z6 ]T  (Cbn )T [ g x 0 , g y 0 , g z 0 ]T (39)<br /> z4 (k )  h 4 ( X k )  (2 x12 ( k )  2 x22 (k )  1) g x 0  (2 x2 ( k ) x3 ( k )<br /> (40)<br />  2 x1 (k ) x4 (k )) g y 0  (2 x2 (k ) x4 (k )  2 x1 (k ) x3 (k )) g z 0<br /> z5 ( k )  h5 ( X k )  (2 x2 ( k ) x3 ( k )  2 x1 ( k ) x4 ( k )) g x 0<br /> (41)<br />  (2 x12 ( k )  2 x32 ( k )  1) g y 0  (2 x3 ( k ) x4 ( k )  2 x1 (k ) x2 (k )) g z 0<br /> z6 (k )  h6 ( X k ) (2 x2 (k ) x4 ( k )  2 x1 (k ) x3 (k )) g x 0<br /> (42)<br />  (2 x3 (k ) x4 (k )  2 x1 (k ) x2 (k )) g y 0  (2 x12 (k )  2 x42 (k )  1) g z 0<br /> 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<br /> chØ sè z7 , z8 , z9 con quay vi c¬ ®o ®­îc sÏ lµ:<br /> z7 (k )  h7 ( X k )  p  bp  x5 ( k )  x8 ( k ) (43)<br /> z8 (k )  h8 ( X k )  q  bq  x6 ( k )  x9 (k ) (44)<br /> z9 ( k )  h9 ( X k )  r  br  x7 ( k )  x10 ( k ) (45)<br /> Tõ (36 - 38), (40 - 42) vµ (43 - 45) ta cã ®Çy ®ñ 9 hµm quan s¸t ( h1 , h2 ,..., h 9 ) cña vÐc t¬<br /> hk hi<br /> hµm h ( X k ) . ViÖc x¸c ®Þnh ma trËn ®o H k  X  Xˆ k (  )<br /> b»ng c¸ch lÊy ®¹o hµm riªng<br /> X x j<br /> nhËn ®­îc: H k   hij  , i  1, 9; j  1,10 . (46)<br /> trong ®ã, h11  4 Bx x1 (k )  2By x4 (k )  2Bz x3 (k ) ; h12  4Bx x2 (k )  2By x3 (k )  2Bz x4 (k ) ;<br /> h13  2 By x2 (k )  2 Bz x1 (k ) ; h14  2 By x1 (k )  2 Bz x2 (k ) ; h22  2 Bx x3 (k )  2 Bz x1 (k )<br /> <br /> <br /> <br /> 122 T. D. Trung, N. Q. Vịnh, “X©y dùng thuËt to¸n x¸c ®Þnh … vò khÝ chèng ngÇm.”<br /> Nghiªn cøu khoa häc c«ng nghÖ<br /> <br /> h21  2 Bx x4 (k )  4 By x1 (k )  2 Bz x2 (k ) ; h23  2 Bx x2 (k )  4 By x3 (k )  2 Bz x4 (k ) ;<br /> h24  2 Bx x1 (k )  2 Bz x3 (k ) ; h31  2 Bx x3 (k )  2 By x2 (k )  4 Bz x1 (k ) ;<br /> h32  2 Bx x4 (k )  2 By x1 (k ) ; h33  2 Bx x1 (k )  2 By x4 (k ) ; h41  2 g z 0 x3 (k );<br /> h34  2 Bx x2 (k )  2 By x3 (k )  4 Bz x4 (k ) ; h42  2 g z 0 x4 (k ); h43  2 g z 0 x1 (k );<br /> h44  2 g z 0 x2 (k ); h51  2 g z 0 x2 (k ) ; h52  2 g z 0 x1 (k ) ; h53  2 g z 0 x4 (k ) ; h54  2 g z 0 x3 (k ) ;<br /> h61  4 g z 0 x1 (k ) ; h64  4 g z 0 x4 (k ) ; h1010  h99  h88  h710  h69  h58  1 ; tÊt c¶ c¸c hij<br /> kh¸c ®Òu b»ng 0.<br /> 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ã:<br /> z1  h1 ( X k )  vT 1 ; z2  h2 ( X k )  vT 2 ; z3  h3 ( X k )  vT 3 ; z4  h4 ( X k )  w4 ;<br /> z5  h5 ( X k )  w5 ; z6  h6 ( X k )  w6 ; z7  h7 ( X k )  w1 ; z8  h8 ( X k )  w2 ; z9  h9 ( X k )  w3 .<br /> trong ®ã, vT 1 , vT 2 , vT 3 lµ nhiÔu ®o ba tõ kÕ.<br /> 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×<br /> 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<br /> 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<br /> Pk vµ ma trËn K k theo thñ tôc läc Kalman [4]. KÕt qu¶ ­íc l­îng theo l­u ®å gi¶i thuËt<br /> bé läc Kalman phi tuyÕn më réng nhËn ®­îc 4 tham sè Rodrig-Hamilton 0 , 1 , 2 , 3 . Tõ<br /> ®ã 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Þ<br /> 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 ®ã<br /> ma trËn M ®­îc x¸c ®Þnh: M   I 4 x 4 O4 x 6  trong ®ã, I lµ ma trËn ®¬n vÞ; O lµ ma<br /> trËn c¸c phÇn tö b»ng 0.<br /> <br /> 3. THUËT TO¸N X¸C §ÞNH THAM Sè DÉN §¦êNG CHO Vò KHÝ CHèNG<br /> NGÇM giai ®o¹n chuyÓn ®éng trong n­íc<br /> 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ë<br /> 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<br /> 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Õ<br /> 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<br /> 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<br /> cña ASWs so víi bÒ mÆt n­íc.<br /> Thay (8), (3) vµo (4) ta cã:<br />  =(2 2  2 2  1)a  (2   2  )a  (2   2  )a<br /> VN 0 1 x 1 2 0 3 y 1 3 0 2 z<br /> (47)<br /> 2 2<br />  (20  21  1)w4  (212  20 3 ) w5  (213  20 2 ) w6<br />  =(2   2  )a  (2 2  2 2  1)a  (2   2  )a<br /> VE 1 2 0 3 x 0 2 y 2 3 0 1 z<br /> (48)<br /> 2 2<br />  (212  20 3 ) w4 -(20  22  1)w5  (22 3  20 1 ) w6<br />  =(2   2  )a  (2   2  )a  (2 2  2 2  1)a<br /> VD 1 3 0 2 x 2 3 0 1 y 0 3 z<br /> (49)<br /> 2 2<br />  (213  20 2 )w4  (22 3  20 1 ) w5  (20  23  1)w6  g<br /> BiÕn X ®­îc ®Æt:<br /> x1  0 ; x2  1 ; x3  2 ; x4  3 ; x5  bp ; x6  bq ; x7  br ; x8  VN ; x9  VE ; x10  VD ; x11  z (50)<br /> <br /> <br /> <br /> <br /> T¹p chÝ Nghiªn cøu KH&CN qu©n sù, Sè 31, 06 - 2014 123<br />  §iÒu khiÓn & Tù ®éng hãa<br /> <br /> 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<br /> häc f1 , f 2 ,..., f11 vµ c¸c hµm nhiÔu ®éng häc g1 , g 2 ,..., g11 .<br /> 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:<br /> [z4 , z5 , z6 ]T  (Cbn )T [VN , VE , VD ]T (51)<br /> <br /> 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 z7 .<br /> Tõ (32), (51) vµ hµm quan s¸t z7 ta cã ®ñ 7 hµm quan s¸t:<br /> T<br /> Z  [ z1 , z2 , z3 , z4 , z5 , z6 , z7 ] .<br /> 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<br /> ®Þ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<br /> 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<br /> ®­î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<br /> 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<br /> 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.<br /> <br /> 4. KÕT QU¶ M¤ PHáNG<br /> 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<br /> l­îng ®­îc phï hîp víi gi¸ trÞ lý t­ëng gi¶ ®Þnh.<br /> <br /> <br /> Gi¸ trÞ lý t­ëng KÕt qu¶ ­íc l­îng<br /> <br /> <br /> <br /> <br /> H×nh 4. §å thÞ tham sè 0 , 1 , 2 , 3 lý t­ëng vµ kÕt qu¶ ­íc l­îng ®­îc.<br /> <br /> 5. KÕT LUËN<br /> 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<br /> 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<br /> 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õ<br /> 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è<br /> Rodrig-Hamilton 0 , 1 , 2 , 3 . Tõ c¸c tham sè Rodrig-Hamilton x¸c ®Þnh ®­îc c¸c gãc<br /> ®Þ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õ ®ã<br /> <br /> <br /> 124 T. D. Trung, N. Q. Vịnh, “X©y dùng thuËt to¸n x¸c ®Þnh … vò khÝ chèng ngÇm.”<br /> Nghiªn cøu khoa häc c«ng nghÖ<br /> <br /> 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<br /> ®Þ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<br /> 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<br /> 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<br /> 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<br /> 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<br /> 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.<br /> <br /> TµI LIÖU THAM KH¶O<br /> [1]. D.H.Titterton, J.L.Weston, (2004), “Strapdown Inertial Navigation Technology”, 2nd<br /> Edition, The Institution of Electrical Engineers, ISBN 0863413587.<br /> [2]. O. S. Salychev, (1988), “Inertial systems in navigation and geophysics”, Press Moscow.<br /> [3]. Pаспопов В. Я (2010), “Мисро-системная авионика”, Тула.<br /> [4]. Леондес К. Т, (1980), “Фильтрация и стохастическое управление в динамических<br /> системах”, Мир, Москва.<br /> <br /> abstract<br /> Constructing an algorithm for determining navigation<br /> parameters for anti-submarine weapons<br /> This article describes a method using the nonlinear extended Kalman filter in<br /> order to construct an algorithm for determining navigation parameters for anti-<br /> submarine weapons which are dropped from the plane based on the combination of<br /> information of inertial navigation equipments (accelerometers, speed angle gyros)<br /> with magnetometers, speedometers and pressure sensors in the drop motion with a<br /> parasol in the atmosphere and in the motion in the water environment.<br /> Keywords: Extended nonlinear Kalman filter, Angular rate sensors, Magnetometers, Accelerometers,<br /> Speedometers and pressure sensors.<br /> <br /> <br /> <br /> Nhận bài ngày 10 tháng 3 năm 2014<br /> Hoàn thiện ngày 20 tháng 5 năm 2014<br /> Chấp nhận đăng ngày 25 tháng 5 năm 2014<br /> <br /> <br /> <br /> <br /> §Þa chØ: ViÖn KH&CN Qu©n sù<br /> <br /> <br /> <br /> <br /> T¹p chÝ Nghiªn cøu KH&CN qu©n sù, Sè 31, 06 - 2014 125<br />