22 Ò Ä Vol. 22 No. 204 ý 2 TOPEDO TECHNOLOGY Feb. 204 70075) 一种无主惯导参数注入的姿态角在线修正算法 Ð,2, ³È, í,2, «ó,2 (. À ø Ÿ 705, ä, 70075; 2. w Œ æ æ, ä, : Æ lñ, j Œ k, Á œ êýž ¼œ û Š Êf, e Ú Á x, ê œêýž ê œ w, ê œú, f åü é Ú ø Á x, k Œ å Œ, ý Ñw š œêýž ¼œ m, Ñ e Áœê q ýž Ð, w e Æ Á Ú å Œ ºÀ : Æ ; œ ; Ú ; Á x f É ú: TJ630.33; TP3 h u: A ú: 673-948(204)0-0039-05 An On-line Modifying Algorithm of Lightweight Torpedo Attitude Angle without Data from MINS YANG Guan-jin-zi,2, HONG Jian-ying, WANG Li-wen,2, GUO Lin-na,2 (. The 705 esearch Institute, China Shipbuilding Industry Corporation, Xi an 70075, China; 2. Science and Technology on Underwater Information and Control Laboratory, Xi an 70075, China) Abstract: The sensors in a lightweight torpedo in service often employ framed gyroscope. However, some problems are br- ought to launch control system interface of lightweight torpedo launch platform in updating the framed gyroscope by strapdown inertial technology. In this paper, an on-line modifying algorithm of lightweight torpedo attitude angle is proposed. The algorithm doesn t need any navigation parameter data from launch platform before launch, extracts the attitude angle by accelerometer after launch, and on-line modifies the attitude angle error in steady motion segment in order to achieve stable control. And it supports the original launch control system interface of lightweight torpedo launch platform. Simulation result shows that this algorithm greatly reduces the torpedo preparation time on launch platform, and keeps steady control of lightweight torpedo attitude angle during running. Keywords: lightweight torpedo; launch control system; attitude angle; on-line modifying 0 Á Æ, Áœê u } œ á, Ê, w, üø q ýú w Á u œ, w œ j t w Ð Ð, f, œî, à ê æ Ú, j Œ k,, ê { 203-0-5; 203-2-. ~v á(988-),, Á Ï, ¼ ø Œê. www.yljszz.cn 39
204 ý 2 22 œêû Š [-3] ¼Áê üƒ Øé þ s n é, œêýžá å ƒå n Ú œp ê œ, é, œêýžk å Ú å, g ns œêýž ä q s Œ «[4-5], Êf n, ênéá œêýž ¼œ û øñ, éf Ð æ, ê œêýž ê w, j w Æ éð Ú Áw Æ éð, é Øé ê Ñ Ð j é œê qq å éœ ê qýžn ä, Êf œ Ú Á x, Á å 0, œxl, é, Øÿ ãýž, Á å, éú ø o, ýé Ú x, j Œ å Œ ~ Æ q ÊfÑýž œê,, u ê, é, Æ, ýú x, s é kœú ž é s Áé Ú x,  û «ž, Á  p l, êg Ñ Ú ø, k ä( 34.2) t,  Œ 2.4 / h, Á é Ø œ 0, æ 80 ø, ý 24.8 / h, é Ú ø ž f é œê, œ 0, ê  Á ý ø «},, Á Øÿ ãýž q Ðf Ø ø, Ú ød Ñ, Î, Ú ø Æ Œ, œ Ú œ, Á å é Ð œú, é Ú ø o ý kx ;, Á  q,  {Ê žk å,  Á øžk Œ ê, Ú x, Ú žk Œ å Œ, æ ê é [6] œú, ê é Ú x ÀÕÀ Fig. œê,,, œxl é ÿ ãýž, ý y ~ Block diagram of on-line modifying algorithm of attitude angle Ø Œ Úœ q Œ,ÿ ý œ n 40 Torpedo Technology www.yljszz.cn
204 ý 2 á, : êœ Ú Á x ¼ å q Ðf å, Á é q å Ø œú œ x Á x, 50 ms ë o Ò ø, 20 s éú x, x œú Æ q, q, Á, q å, ýá z g ç,, Á ÿ q ç ω v } ÁÚ Á x, Í éýž Ð Ñ, Œ ý ½Ïd Î,, é ý Ð, k ê ø «, o{ é ý Ð, ý œ a_ out(0) = K0 * a_ in(0) a_out() = K * a_out(0) + K2 * a_in() + K3 * a_ in(0) a_ out( k) = K2 * a_ out( k ) K22 * a_ out( k 2) + K23 * a_in( k) + K24 * a_in( k ) + K25 * a_in( k 2) (), _in( ) a k Œ ; a_out( k) Œ ý ; k û ( k = 2,3,,0) ; Kij {, ø œú θ ( k) = arcsin a γ ( k) arctan a x 2 2 y + z a ( k) a ( k) y a ( k) az ( k) a ( k) = : θ a ( k ), γ a ( k) Œ ý xl ; x ( ) az (2) k Œ a k, a ( k ), ( k k ) Œ q x, y, z 3 ý œú ý Õ 9 θacc () l = ( ) 0 θa l k k = 0 9 γacc () l = γa ( l k) 0 k = 0 θ, γ () y (3) : acc () l acc Œý xl l l Œ ë, o f Í é f ê œ û ÑÚ ø, é ø x, x Ú Œ f å Œ ë Æ, Á ä ø, k Ò ø ë Æ, xl Ú, œ Õ Ẋ = FX + W (4) VE 0 ( ωie sinl+ tan L) 0 VE ( ωie sin L+ tan L) 0 0 F = (5) 2V E 0 fu 0 (2ωiesinL+ tan L) VE V N fu 0 (2ωiesin L+ tan L) tan L œ Ú Ú θacc θ Z = + V (6) γ γ acc sin s sin s www.yljszz.cn 4
204 ý 2 22 T 3 0 0 2 2 T2 T2 = T23T32 T22T33 T3T22 T2T 23 0 0 2 2 2 2 T22 T23 T22 T + + 23 H (7) 2 Íöº{s j ãm  m Š Á ãm, e f ê œ û Ú ø, { æ, å Ø 0, xl œ é, 2 ê ø Áä é, i ê ; 2 Áê, eú Á x, 20 s ë {éú x é 2 Š 20 min m Š À 2 x x é [7] T Õ ÕÀ 3 À 4, œ æ Ø Œ Ø 06.766 72, Á 20 min ƒ Î, ê q Ð, Ám œ 0, jm žk, x ê xl Á áá 3 5 øÿ ø, Á x, Ú phins ø Œ 0.4 øÿ 3 Fig. 3 ±± Í z ý ~ z ( phins ±) Curves of calculated pitch angle errors for pure navigation and on-line modified navigation in vehicle experiment conditions(compared with phins) Q 2 {s z ( ) ý ~ ( k ±) Fig. 2 Curves of calculated roll angle errors for pure navigation and on-line modified navigation in simulation conditions(latitude is 34.2e) ÕÀ 2, Á l,, äâ ê 5 ø Ÿ ø, Ú Á x, ø êœ 0. øÿ j m, ê Ô ê ( À phins) ê g«ä Á, w e ê g«ê phins ä ø ŒÁ Àƒ, k phins Ê, å g«ê ø œ ( äâ 34.2 ) m 4 ±± Í z ý ~ ( phins ±) Fig. 4 Curves of calculated roll angle errors for pure navigation and on-line modified navigation in vehicle experiment conditions(compared with phins) m, Á x Á, Ñw Ú ƒ Ñ, žk å Œ ; f e Á œê qýž é,, é žkä Á, ÑÑü Áýž œê Ð 3 é Æ é kœñýž œê q, Á Æ Ê, 42 Torpedo Technology www.yljszz.cn
204 ý 2 á, : êœ Ú Á x Êf œ Ú Á x, ý é ê Á x m Š,, Ú Á x, Ñw Æ Á Ú å Œ Ú Á x, ê œê ýž êœ n é, ê å éú ø x Ñ Áœê qýž Ð,, jœê qýž œ êê w, pe Æ Øé é qýž w u, Æ éð õ h: []. åp [M]. h: À ø, 2009. [2] k. ê š [M]. h: ø, 987. [3]. ê [M]. h: ã, 2005. [4], x. ê n é œœî [J]. Ñ Œ. 2004, 22(5): 50-55. Liu Yi, Liu Zhi-jian. The esearch Status and Development Trend of Transfer Alignment for SINS[J]. Aerospace Control, 2004, 22(5): 50-55. [5] Zhao Lin, Gao Wei, Li Pu, et al. The Study on Transfer Alignment for SINS on Dynamic Base[C]//IEEE International Conference on Mechatronics and Automation. Niagara, Canada: IEEE, 2005: 38-322. [6] À Û, ˆ. åp ê [J]., 200, 9(): 27-28. [7],, ž. qƒ IMU ë [J]. Œêã, 2009, 29(3): 3-6. Hu Lian-gui, Zhang Jing, Li Zhao. Simulation of IMU Output Signal in Missile and esearch on Kalman Filter[J]. Journal of Projectiles, ockets, Missiles and Guidance, 2009, 29(3): 3-6. («{ ±: ) ê. œ Œ Á ¼ ÿ. g,, ä. 203, 2(6). 2. x., n, f. 203, 2(3). 3. f Œê Æ. ä, âù. 203, 2(). 4. ýž m. n,, ä. 202, 20(6). 5. é Á o ÿ., ä ñ. 202, 20(5). 6. Êf  ÑÚ ø ê øo., Ž,,. 202, 20(3). 7. xl ±90 Ú m., f,. 202, 20(3). 8. w x AUV åp ø.,. 202, 20(). 9. ê n é }.. 20, 9(4). 0. ÊfÀ~Ï SLAM Á AUV ê ÿ.,,. 20, 9(4).. Ï w o.,, fý. 20, 9(). 2. Êfx h Æ q.,, ä,. 200, 8(5). 3. Êfx Ã., ì ý,. 200, 8(2). 4. Êf MGEKF Ï m Æ œ.,. 200, 8(). 5. v œ l Œ. ˆ,. 2009, 7(5). 6. Á Ñ Ø ãýž ø AUV ê.,,. 2009, 7(2). 7. Ê. wä. 2007, 5(5). 8. ê Á øš.,,. 2007, 5(4). 9. Êf ÿšš FOG. ý, v,. 2007, 5(4). 20. ê p ê.. 2007, 5(3). www.yljszz.cn 43