﻿ 基于水声传播时延补偿的惯导误差修正方法
 舰船科学技术  2016, Vol. 38 Issue (3): 97-100 PDF

INS positioning error correction based on acoustic propagation time compensation
CHEN Jian-hua, ZHU Hai, GUO Zheng-dong, LUAN Lu-yu
Navy Submarine Academy, Qingdao 266042, China
Abstract: In allusion to positioning error correction for underwater vehicle INS(Inertial Navigation System), the INS/multi-becons acoustic range measurements integrating method is proposed. The error result from vehicle motion and acoustic propagation time is mainly analyzed, and a correcting positioning error algorithm based on acoustic propagation time compensation is presented. In this algorithm, the EKF(Extended Kalman Filters) is adopted, and the measurement equation is reconstructed via pushing-forward the INS's position error states. As a result, the measurement equation and the system measurement become synchronous, and the error from time delay is eliminated. The simulation results indicate that the algorithm can significantly improve the accuracy of correcting INS positioning error with INS/multi-becons acoustic range measurements integrated navigation system.
Key words: INS    positioning error    EKF    acoustic propagation time
0 引言

1 时间延迟误差分析

 图 1 时间延迟误差示意图 Fig. 1 Schematic diagram of the time-delay error

 图 2 长基线定位系统 Fig. 2 The LBL positioning system
 ${(x - {x_{b1}})^2} + {(y - {y_{b1}})^2} + {(z - {z_{b1}})^2} = R_1^2$ (1)

 $\begin{gathered} \sqrt {{{({x_{{t_0}}} - {x_{b1}})}^2} + {{({y_{{t_0}}} - {y_{b1}})}^2} + {{(z - {z_{b1}})}^2}} = C \cdot \Delta {t_1} \hfill \\ \sqrt {{{({x_{{t_0}}} + \Delta {x_1} - {x_{b1}})}^2} + {{({y_{{t_0}}} + \Delta {y_1} - {y_{b1}})}^2} + {{(z - {z_{b1}})}^2}} = C \cdot \Delta {t_2} \hfill \\ \end{gathered}$ (2)

 $\begin{gathered} {({x_{{t_0}}} - {x_{b1}})^2} + {({y_{{t_0}}} - {y_{b1}})^2} + 2{(z - {z_{b1}})^2} + {({x_{{t_0}}} + \Delta {x_1} - {x_{b1}})^2} \hfill \\ + {({y_{{t_0}}} + \Delta {y_1} - {y_{b1}})^2} = {(C\Delta {t_1})^2} + {(C\Delta {t_2})^2} \hfill \\ \end{gathered}$ (3)

 $\begin{gathered} {({x_{{t_0}}} + \frac{1}{2}\Delta {x_1} - {x_{b1}})^2} + {({y_{{t_0}}} + \frac{1}{2}\Delta {y_1} - {y_{b1}})^2} + {(z - {z_{b1}})^2} = \hfill \\ \frac{1}{2}{C^2}(\Delta {t_1}^2 + \Delta {t_2}^2) - \frac{1}{4}\Delta x_1^2 - \frac{1}{4}\Delta y_1^2 \hfill \\ \end{gathered}$ (4)

 $\begin{gathered} {({x_{{t_0}}} + \frac{1}{2}\Delta {x_1} - {x_{b1}})^2} + {({y_{{t_0}}} + \frac{1}{2}\Delta {y_1} - {y_{b1}})^2} + {(z - {z_{b1}})^2} = \hfill \\ {(\frac{1}{2}C\Delta {T_1})^2} - \frac{1}{4}\Delta x_1^2 - \frac{1}{4}\Delta y_1^2 = R_1^2 - \frac{1}{4}\Delta x_1^2 - \frac{1}{4}\Delta y_1^2 \hfill \\ \end{gathered}$ (5)

 $E = \frac{{2 \cdot \max ({R_1}{\text{,}}{R_2}{\text{,}}{R_3}{\text{,}}{R_4}) \cdot V}}{C}$ (6)

2 考虑水声传播时间延迟的惯导定位误差修正算法 2.1 系统方程

 $\dot X = F \cdot \dot X + w$ (7)

2.2 重构量测方程

 R=\sqrt{\begin{align} & {{({{x}_{{{t}_{0}}}}+\frac{1}{2}\Delta {{x}_{1}}-{{x}_{b1}})}^{2}}+{{({{y}_{{{t}_{0}}}}+\frac{1}{2}\Delta {{y}_{1}}-{{y}_{b1}})}^{2}} \\ & +{{(z-{{z}_{b1}})}^{2}}+\frac{1}{4}\Delta x_{1}^{2}+\frac{1}{4}\Delta y_{1}^{2} \\ \end{align}} (8)

 $\begin{array}{*{20}{c}} {{x_{{t_0}}} = {x_k} - \Delta {x_4}{\text{,}}} \ {{y_{{t_0}}} = {y_k} - \Delta {y_4}} \end{array}$ (9)

 ${x_{Ik}} = {x_k} + \delta x{\text{,}}{y_{Ik}} = {y_k} + \delta y$ (10)

 $\begin{array}{*{20}{c}} {{x_{{t_0}}} = {x_{Ik}} - x - \Delta {x_4}{\text{,}}} \ {{y_{{t_0}}} = {y_{Ik}} - y - \Delta {y_4}} \end{array}$ (11)

 \begin{array}{*{35}{l}} \begin{align} & {{Z}_{k}}=h(\delta x,\delta y)+{{v}_{z}} \\ & =\sqrt{\begin{array}{*{35}{l}} \begin{align} & {{({{x}_{Ik}}-\delta x-\Delta {{x}_{4}}-{{x}_{bi}}+\frac{1}{2}\Delta {{x}_{i}})}^{2}}+ \\ & {{({{y}_{Ik}}-\delta y-\Delta {{y}_{4}}-{{y}_{bi}}+\frac{1}{2}\Delta {{y}_{i}})}^{2}} \\ & +{{(z-{{z}_{bi}})}^{2}}+\frac{1}{4}\Delta x_{i}^{2}+\frac{1}{4}\Delta y_{i}^{2}\text{,} \\ \end{align} \\ \end{array}} \\ & +{{v}_{z}}\ i=1\text{,}\cdots \text{,}4 \\ \end{align} \\ \end{array} (12)

 $E({v_z}v_z^{\text{T}}) = \left[{\begin{array}{*{20}{c}} {\sigma _{P_1^2}^2}&0&0&0 \ 0&{\sigma _{P_2^2}^2}&0&0 \ 0&0&{\sigma _{P_3^2}^2}&0 \ 0&0&0&{\sigma _{P_4^2}^2} \end{array}} \right]$ (13)

 $H_k^z = {\left. {\frac{{\partial h}}{{\partial {X_k}}}} \right|_{{{\hat X}_{k{\text{,}}k - 1}}}} = \left[{\begin{array}{*{20}{c}} {{e_{1x}}}&{{e_{1y}}} \ {{e_{2x}}}&{{e_{2y}}} \ {{e_{3x}}}&{{e_{3y}}} \ {{e_{4x}}}&{{e_{4y}}} \end{array}} \right]$ (14)

${e_{jx}} = \frac{{ - ({x_{Ik}} - \delta {{\hat x}_{k,k - 1}} - \Delta {x_4} - {x_{bj}} + \frac{1}{2}\Delta {x_j})}}{{\hat \rho {\text{ }}_{k,k - 1}^j}}$

${e_{jx}} = \frac{{ - ({x_{Ik}} - \delta {{\hat x}_{k,k - 1}} - \Delta {x_4} - {x_{bj}} + \frac{1}{2}\Delta {x_j})}}{{\hat \rho _{k,k - 1}^j}}$

\hat{\rho }_{k,k-1}^{j}=\sqrt{\begin{array}{*{35}{l}} \begin{align} & {{({{x}_{Ik}}-\delta {{{\hat{x}}}_{k\text{,}k-1}}-\Delta {{x}_{4}}-{{x}_{bj}}+\frac{1}{2}\Delta {{x}_{j}})}^{2}}+\ \\ & {{({{y}_{Ik}}-\delta {{{\hat{y}}}_{k\text{,}k-1}}-\Delta {{y}_{4}}-{{y}_{bj}}+\frac{1}{2}\Delta {{y}_{j}})}^{2}}+{{(z-{{z}_{bj}})}^{2}} \\ & +\frac{1}{4}\Delta x_{j}^{2}+\frac{1}{4}\Delta y_{j}^{2} \\ \end{align} \\ \end{array}}

 ${H_k} = [{O_{4 \times 5}}\;\;H_k^z \cdot B_n^z]$ (15)
3 仿真分析

 图 3 惯导定位误差曲线 Fig. 3 INS positioning error curve

4 结语

 [1] 刘芙蓉, 陈辉. 自主式水下潜器研究开发综述[J]. 舰船科学技术, 2008, 30(5):20-23.LIU Fu-rong, CHEN Hui. A review of the development and research of autonomous underwater vehicles[J]. Ship Science and Technology, 2008, 30(5):20-23. [2] 冀大雄, 刘健, 周波, 等. 深水机器人低成本导航系统的位置估计方法研究[J]. 仪器仪表学报, 2009, 30(1):35-38.JI Da-xiong, LIU Jian, ZHOU Bo, et al. Position estimation method for deep water vehicle using low-cost navigation sensors[J]. Chinese Journal of Scientific Instrument, 2009, 30(1):35-38. [3] 张立川, 徐德民, 刘明雍. 双伪测量的多水下航行器移动长基线协同导航算法[J]. 火力与指挥控制, 2013, 38(1):34-36.ZHANG Li-chuan, XU De-min, LIU Ming-yong. Cooperative navigation method of multiple autonomous underwater vehicles based on double acoustic measurement[J]. Fire Control & Command Control, 2013, 38(1):34-36. [4] 张福斌, 马朋. 一种基于虚拟移动长基线的多AUV协同导航算法[J]. 鱼雷技术, 2013, 21(2):115-119.ZHANG Fu-bin, MA Peng. A cooperative navigation algorithm of multiple AUVs based on synthetic moving long baseline[J]. Torpedo Technology, 2013, 21(2):115-119. [5] ENGEL R, KALWA J. Relative positioning of multiple underwater vehicles in the GREX project[C]//Proceedings of the 2009-EUROPE Oceans. Bremen:IEEE, 2009:1-7. [6] WOITHE H C, KREMER U. Using slocum gliders for coordinated spatial sampling[C]//Proceedings of the 2011 IEEE-Spain Oceans. Santander:IEEE, 2011:1-8. [7] 刘明雍, 黄博, 蔡挺. 一种基于量测伪距的EKF移动长基线AUV协同导航方法[J]. 鱼雷技术, 2012, 20(6):432-436.LIU Ming-yong, HUANG Bo, CAI Ting. A cooperative navigation method of EKF moving long baseline for AUV based on pseudo-range measurements[J]. Torpedo Technology, 2012, 20(6):432-436. [8] 高伟, 杨建, 刘菊, 等. 基于水声通信延迟的多UUV协同定位算法[J]. 系统工程与电子技术, 2014, 36(3):539-545.GAO Wei, YANG Jian, LIU Ju, et al. Cooperative location of multiple UUVs based on hydro-acoustic communication delay[J]. Systems Engineering and Electronics, 2014, 36(3):539-545. [9] 陈云飞, 李桂娟, 贾兵, 等. 水下合作目标三维定位技术[J]. 舰船科学技术, 2010, 32(5):48-51.CHEN Yun-fei, LI Gui-juan, JIA Bing, et al. Underwater cooperate target 3D positioning[J]. Ship Science and Technology, 2010, 32(5):48-51. [10] 葛锡云, 申高展, 潘琼文, 等. 基于惯性导航/地磁的水下潜器组合导航定位方法[J]. 舰船科学技术, 2014, 36(11):120-124.GE Xi-yun, SHEN Gao-zhan, PAN Qiong-wen, et al. The underwater integrated navigation method of underwater vehicle based on INS/geomagnetic[J]. Ship Science and Technology, 2014, 36(11):120-124.