CN102323586B

CN102323586B - A UUV-assisted navigation method based on ocean current profile

Info

Publication number: CN102323586B
Application number: CN 201110196747
Authority: CN
Inventors: 边信黔; 周佳加; 李举峰; 张勋; 张宏瀚; 陈涛; 张伟; 徐健
Original assignee: Harbin Engineering University
Current assignee: Harbin Shipboard Intelligent Technology Partnership (L.P.)
Priority date: 2011-07-14
Filing date: 2011-07-14
Publication date: 2013-02-06
Anticipated expiration: 2031-07-14
Also published as: CN102323586A

Abstract

The invention provides a UUV auxiliary navigation method based on the ocean current profile. First establish the Kalman filter model of ocean current profile data, then determine the observation noise variance R and process noise variance Q in the Kalman equation according to the changes in the environment and the UUV’s own velocity, and finally establish a "UUV velocity-ocean current profile" relational database, using the The relationship database and the ocean current profile information measured by ADCP calculate the UUV speed when the DVL fails, and then obtain the UUV's navigation position through the ship's position reckoning method. The beneficial effect of the present invention is that when the UUV performs tasks underwater, once the DVL sonar data fails, the UUV speed can be calculated using the ADCP ocean current profile information, so that the UUV can continue to perform marine exploration tasks in complex ocean environments such as large depths, and improve the efficiency of the UUV. environmental adaptability.

Description

A UUV-assisted navigation method based on ocean current profile

技术领域 technical field

本发明涉及的是一种UUV导航方法，尤其涉及一种基于UUV搭载的DVL声纳数据失效情况下海流剖面辅助导航方法。The present invention relates to a UUV navigation method, in particular to an ocean current profile auxiliary navigation method based on the failure of DVL sonar data carried by the UUV.

背景技术 Background technique

随着水下无人航行器UUV(Unmanned Underwater Vehicle)在海洋开发中的应用越来越广泛，对UUV导航系统的精度要求也越来越高。传统航位推算法是UUV中目前最常用的导航方法之一，这种方法依赖于多普勒测速仪(DVL)采集的速度数据和姿态传感器采集的姿态数据。但是，当UUV在转弯、海水较深超过DVL底跟踪的最大范围等情况下可能发生失效，DVL一旦失效或者由于性能下降导致UUV的实时速度无法获得，只能采用DVL失效前几拍的速度信息，而UUV的速度在复杂海洋环境下是实时变化的，从而对UUV的导航产生较大的影响，推算的UUV位置信息偏差会越来越大。因此在DVL失效时，寻求一种能够有效避免因速度传感器失效或者故障导致航位推算产生较大导航误差的问题，就显得尤为迫切。As the underwater unmanned vehicle UUV (Unmanned Underwater Vehicle) is more and more widely used in ocean development, the accuracy requirements for the UUV navigation system are also getting higher and higher. Traditional dead reckoning is one of the most commonly used navigation methods in UUVs, which relies on velocity data collected by Doppler Velocimeters (DVL) and attitude data collected by attitude sensors. However, when the UUV may fail when it is turning or the sea water is deeper than the maximum range of DVL bottom tracking, once the DVL fails or the real-time speed of the UUV cannot be obtained due to performance degradation, only the speed information of the few beats before the DVL failure can be used , while the speed of UUV changes in real time in a complex ocean environment, which has a greater impact on the navigation of UUV, and the deviation of the estimated UUV position information will become larger and larger. Therefore, when the DVL fails, it is particularly urgent to seek a problem that can effectively avoid the large navigation error caused by dead reckoning due to the failure or failure of the speed sensor.

目前，国内外试图寻求一种有效的在UUV测速传感器失效情况下能够辅助UUV导航的手段。如莫军在文献《基于海流数据库的水下导航信息融合方法探讨》(系统仿真学报，2002年，第14卷第10期)中提出了基于海流数据库的水下导航信息融合方法，该方法需要大量的工作海域的海流信息，工作量大，难以推广。2009年，中国海军工程大学的吴太旗等人在文献《重力场辅助水下导航仿真及分析平台的构建》(舰船科学技术，2009年，第31卷第2期)中提出了利用重力场进行水下无人航行器辅助导航的方法，该方法需要增加重力传感器、重力基准图数据等软硬件，增加了UUV系统的复杂性。2007年，挪威

等人在文献《Comparison of Mathematical Models for the HUGIN 4500 AUV Based on ExperimentalData》(International Symposium on Underwater Technology)中提出了利用UUV动力学模型进行辅助导航的方法，而UUV动力学模型涉及的参数较多，难以准确确定UUV的动力学模型，从而无法保证导航的精度。At present, domestic and foreign attempts to find an effective means to assist UUV navigation in the event of UUV speed sensor failure. For example, Mo Jun proposed an underwater navigation information fusion method based on the current database in the document "Discussion on the Fusion Method of Underwater Navigation Information Based on the Ocean Current Database" (Journal of System Simulation, 2002, Volume 14, No. 10). There is a large amount of current information in the working sea area, and the workload is heavy and it is difficult to promote. In 2009, Wu Taiqi and others from China Naval University of Engineering proposed the use of gravity field A method for assisted navigation of an underwater unmanned vehicle. This method needs to add software and hardware such as gravity sensors and gravity reference map data, which increases the complexity of the UUV system. 2007, Norway

et al. proposed a method of using the UUV dynamic model to assist navigation in the document "Comparison of Mathematical Models for the HUGIN 4500 AUV Based on Experimental Data" (International Symposium on Underwater Technology), and the UUV dynamic model involves many parameters. It is difficult to accurately determine the dynamic model of UUV, so that the accuracy of navigation cannot be guaranteed.

发明内容 Contents of the invention

本发明的目的在于提供一种当UUV搭载的DVL(多普勒测速仪，Doppler Velocity Log)失效时能够利用ADCP(声学多普勒海流剖面仪，Acoustic Doppler Current Profile)测得的海流剖面数据进行辅助自身导航的于海流剖面的UUV辅助导航方法。The purpose of the present invention is to provide a kind of current profile data that can be measured by ADCP (Acoustic Doppler current profile instrument, Acoustic Doppler Current Profile) when the DVL (Doppler Velocity Log) carried by UUV fails. A UUV-assisted navigation method based on ocean current profiles to assist self-navigation.

本发明的目的是这样实现的：包括下列步骤：The object of the present invention is achieved like this: comprise the following steps:

(1)海流剖面数据和DVL速度获取(1) Ocean current profile data and DVL velocity acquisition

利用UUV携带的控制计算机控制ADCP发射一定频率的声波，利用多普勒效应获取下部紧贴UUV水层厚度的海流剖面；Use the control computer carried by the UUV to control the ADCP to emit sound waves of a certain frequency, and use the Doppler effect to obtain the ocean current profile that is close to the thickness of the UUV water layer;

利用UUV携带的控制计算机控制DVL发射一定频率的声波，利用多普勒效应获取DVL的速度，即UUV的航行速度；Use the control computer carried by the UUV to control the DVL to emit sound waves of a certain frequency, and use the Doppler effect to obtain the speed of the DVL, that is, the sailing speed of the UUV;

(2)海流剖面的Kalman滤波(2) Kalman filter of ocean current profile

根据海流环境以及UUV自身模型的特点，建立海流剖面数据Kalman滤波数据模型；并参照海流高频残差的数量级确定海流Kalman滤波数据模型中的观测噪声方差R，同时根据DVL测得的UUV速度信息动态调节过程噪声方差Q值的大小；According to the current environment and the characteristics of UUV's own model, the Kalman filter data model of the current profile data is established; and the observation noise variance R in the current Kalman filter data model is determined by referring to the order of magnitude of the high-frequency residual of the current, and the UUV velocity information measured by DVL is used Dynamically adjust the size of the process noise variance Q value;

(3)“UUV速度-海流信息”关系库建立(3) Establishment of "UUV speed-ocean current information" relationship database

当DVL导航声纳数据有效时，利用DVL测得的UUV速度、OCTANS姿态传感器测量得到的UUV的航向信息，ADCP测量得到的海流剖面数据，建立“UUV速度-海流信息”关系库；When the DVL navigation sonar data is valid, use the UUV speed measured by DVL, the UUV heading information measured by the OCTANS attitude sensor, and the ocean current profile data measured by ADCP to establish a "UUV speed-ocean current information" relationship library;

(4)海流剖面辅助导航(4) Sea current profile aided navigation

当DVL导航声纳数据失效时，利用“UUV速度-海流信息”关系库，以及ADCP声纳实时测得的海流剖面信息，推算UUV的导航速度；再根据初始UUV的经纬度和船位推算算法得到推算UUV的位置信息。When the DVL navigation sonar data fails, use the "UUV speed-ocean current information" relationship library and the current profile information measured by the ADCP sonar in real time to calculate the navigation speed of the UUV; then calculate it based on the initial UUV latitude and longitude and ship position calculation algorithm The location information of the UUV.

所述UUV的位置信息的获取方法为：The acquisition method of the position information of the UUV is:

推算的UUV导航速度在正东方向和正北方向的分量如下式：The components of the estimated UUV navigation speed in the direction of due east and due north are as follows:

$\{\begin{matrix} {v v}_{E E.} = = {v v}_{F f} sin sin H h - - {v v}_{L L} cos cos H h \\ {v v}_{N N} = = {v v}_{F f} cos cos H h + + {v v}_{L L} sin sin H h \end{matrix}$

式中v_E、v_N——分别为载体航行速度在正东方向和正北方向的分量；In the formula, v _E and v _N —— are the components of the carrier's navigation speed in the direction of due east and due north respectively;

v_F、v_L——分别为DVL测得的载体相对大地的前向速度和左向速度；v _F , v _L ——respectively, the forward velocity and leftward velocity of the carrier relative to the ground measured by DVL;

H——UUV载体的航向角，顺时针为正，逆时针为负，由罗经测得；H——The heading angle of the UUV carrier, clockwise is positive, counterclockwise is negative, measured by the compass;

UUV位置由下面的公式计算得到：The UUV position is calculated by the following formula:

$\{\begin{matrix} J J = = {J J}_{00} + + {Σ Σ}_{i i = = 11}^{n no} {v v}_{E E.,, ((i i - - 11))} Δt Δt / / {R R}_{M m,, ((i i - - 11))} \\ W W = = {W W}_{00} + + {Σ Σ}_{i i = = 11}^{n no} {v v}_{N N,, ((i i - - 11))} Δt Δt / / {R R}_{N N,, ((i i - - 11))} \end{matrix}$

式中Δt——DVL采样周期；In the formula, Δt——DVL sampling period;

J、W——分别为n时刻UUV载体所在位置的经度和纬度；J, W——respectively the longitude and latitude of the UUV carrier's location at n time;

J₀、W₀——分别为初始时刻载体的经度和纬度，由GPS接收机测得；J ₀ , W ₀ ——respectively the longitude and latitude of the carrier at the initial moment, measured by the GPS receiver;

v_E，(i-1)、v_N，(i-1)——分别为i-1时刻UUV航行速度在正东方向和正北方向的分量；v _{E, (i-1)} , v _{N, (i-1)} ——respectively the components of UUV navigation speed in the direction of due east and due north at time i-1;

R_M(i-1)、R_N，(i-1)——分别为i-1时刻地球子午曲率半径和纬度圈曲率半径。R _M(i-1) , R _N,(i-1) ——respectively, the radius of curvature of the earth's meridian and the radius of curvature of the latitude circle at time i-1.

本发明相对现有技术具有如下的优点及效果：The present invention has following advantage and effect relative to prior art:

本发明的有益效果在于UUV在水下执行任务时，一旦DVL声纳数据失效，可以使用ADCP海流剖面信息推算UUV速度，使得UUV在大深度等复杂海洋环境下可以继续执行海洋勘探任务，提高UUV的环境适应能力。The beneficial effect of the present invention is that when the UUV performs tasks underwater, once the DVL sonar data fails, the UUV speed can be calculated using the ADCP ocean current profile information, so that the UUV can continue to perform marine exploration tasks in complex ocean environments such as large depths, and improve the UUV environmental adaptability.

附图说明 Description of drawings

图1是本发明海流剖面辅助UUV导航流程图；Fig. 1 is the flow chart of the present invention's ocean current profile assisted UUV navigation;

图2是本发明的高频残差辅助海流剖面滤波效果图；Fig. 2 is the filtering effect diagram of the high-frequency residual auxiliary ocean current profile of the present invention;

图3是本发明的过程噪声方差Q对海流剖面Kalman滤波的影响；Fig. 3 is the influence of process noise variance Q of the present invention on ocean current profile Kalman filtering;

图4是本发明利用DVL速度动态确定Q值的Kalman滤波效果图；Fig. 4 is that the present invention utilizes the Kalman filtering effect diagram of DVL speed dynamic determination Q value;

图5是本发明UUV随船坐标系与北东固定坐标系关系示意图；Fig. 5 is a schematic diagram of the relationship between the UUV on-board coordinate system and the northeast fixed coordinate system of the present invention;

图6是本发明海试实验中UUV航迹图；Fig. 6 is a UUV track diagram in the sea trial experiment of the present invention;

图7是本发明海试实验中海流剖面长时无效时辅助UUV导航效果图。Fig. 7 is an effect diagram of auxiliary UUV navigation when the sea current profile is invalid for a long time in the sea trial experiment of the present invention.

具体实施方式 Detailed ways

下面结合附图对本发明作进一步的详细描述。The present invention will be described in further detail below in conjunction with the accompanying drawings.

UUV在水下一定深度航行时，ADCP(换能器头向下安装)在控制计算机控制下，发射一定频率的声波，利用多普勒效应获取下部紧贴UUV水层厚度的海流剖面，海流剖面数据通过串口(或网口)传输到控制计算机的存储介质进行存储，这样通过ADCP就可以获取到海流剖面数据。When the UUV is sailing at a certain depth underwater, the ADCP (installed with the transducer head downward) emits sound waves of a certain frequency under the control of the control computer, and uses the Doppler effect to obtain the ocean current profile close to the thickness of the UUV water layer. The data is transmitted to the storage medium of the control computer through the serial port (or network port) for storage, so that the ocean current profile data can be obtained through ADCP.

利用UUV携带的控制计算机控制DVL发射一定频率的声波，利用多普勒效应获取DVL的速度，DVL的速度信息通过串口或(或网口)传输到控制计算机的存储介质进行存储，这样通过DVL就可以获取到UUV的航行速度。Use the control computer carried by the UUV to control the DVL to emit sound waves of a certain frequency, and use the Doppler effect to obtain the speed of the DVL. The speed information of the DVL is transmitted to the storage medium of the control computer through the serial port or (or network port) for storage. You can get the sailing speed of UUV.

(2)海流剖面的Kalman实时滤波(2) Kalman real-time filtering of ocean current profile

海流剖面数据处理选用Kalman实时滤波技术，以增加数据的稳定性与可信度。Kalman real-time filtering technology is selected for ocean current profile data processing to increase data stability and reliability.

设线性离散系统的状态方程和观测方程为：Let the state equation and observation equation of the linear discrete system be:

$\{\begin{matrix} {X x}_{k k} = = {φ φ}_{k k,, k k - - 11} {X x}_{k k - - 11} + + {Γ Γ}_{k k,, k k - - 11} {W W}_{k k,, k k - - 11} \\ {Z Z}_{k k} = = {H h}_{k k} {X x}_{k k} + + {V V}_{k k} \end{matrix} - - - - - - ((11))$

式中，X_k和Z_k分别是k时刻的状态矢量和观测矢量；φ_k，k-1为n×n维非奇异状态转移矩阵；Γ_k，k-1是n×p维系统过程噪声输入矩阵；H_k是m×n维观测矩阵；W_k，k-1是p维系统随机过程噪声序列；V_k是m维系统随机观测噪声序列。设线性离线系统为白噪声序列，则随机线性离散系统Kalman滤波方程可以表示为如下几个递推方程：In the formula, X _k and Z _k are the state vector and observation vector at time k, respectively; φ _k,k-1 _is the n×n-dimensional non-singular state transition matrix; Input matrix; H _k is the m×n dimensional observation matrix; W _{k, k-1} is the p-dimensional system random process noise sequence; V _k is the m-dimensional system random observation noise sequence. Assuming that the linear offline system is a white noise sequence, the Kalman filter equation of the random linear discrete system can be expressed as the following recurrence equations:

状态预测： ${\hat{X}}_{k, k - 1} = φ_{k, k - 1} {\hat{X}}_{k - 1} - - - (2)$ Status Prediction: ${\hat{x}}_{k, k - 1} = φ_{k, k - 1} {\hat{x}}_{k - 1} - - - (2)$

状态估计： ${\hat{X}}_{k} = X_{k, k - 1} + K_{k} (Z_{k} - H_{k} {\hat{X}}_{k, k - 1}) - - - (3)$ State estimation: ${\hat{x}}_{k} = x_{k, k - 1} + K_{k} (Z_{k} - h_{k} {\hat{x}}_{k, k - 1}) - - - (3)$

滤波增益方程： $K_{k} = P_{k, k - 1} H_{k}^{T} {[H_{k} P_{k, k - 1} H_{k}^{T} + R_{k}]}^{- 1} - - - (4)$ Filter gain equation: $K_{k} = P_{k, k - 1} h_{k}^{T} {[h_{k} P_{k, k - 1} h_{k}^{T} + R_{k}]}^{- 1} - - - (4)$

预测误差方差方程： $P_{k, k - 1} = φ_{k, k - 1} P_{k - 1} φ_{k, k - 1}^{T} + Γ_{k, k - 1} Q_{k - 1} Γ_{k, k - 1}^{T} - - - (5)$ Forecast error variance equation: $P_{k, k - 1} = φ_{k, k - 1} P_{k - 1} φ_{k, k - 1}^{T} + Γ_{k, k - 1} Q_{k - 1} Γ_{k, k - 1}^{T} - - - (5)$

估计误差方差方程：P_k＝[1-K_kH_k]P_k，k-1 (6)Estimated error variance equation: P _k = [1-K _k H _k ]P _{k, k-1} (6)

在小范围海区中，可以将一个较短的时间内的海流剖面数据看成是一组平稳随机序列，海流剖面是一个缓变的数据变量，对于这种特征的数据，在进行Kalman滤波操作时，可以将其模型进行简化。由于海流是缓变过程，所以系统转移矩阵φ_k，k-1＝1，即认为当前海流数据与前一时刻海流数据相等；声纳传感器得到的数据便是系统的状态，所以观测矩阵H_k＝1，这样，系统状态维数为1，得到系统的过程噪声输入矩阵Γ_k，k-1＝1。这样，海流剖面Kalman滤波方程便简化为：In a small-scale sea area, the ocean current profile data in a short period of time can be regarded as a set of stationary random sequences, and the ocean current profile is a slowly changing data variable. For data with this characteristic, when performing Kalman filter operation , the model can be simplified. Since the ocean current is a slowly changing process, the system transfer matrix φ _{k, k-1} = 1, that is, the current ocean current data is considered to be equal to the previous moment ocean current data; the data obtained by the sonar sensor is the state of the system, so the observation matrix H _k =1, so the system state dimension is 1, and the process noise input matrix Γ _k,k-1 =1 of the system is obtained. In this way, the Kalman filter equation of the ocean current profile is simplified to:

状态预测： ${\hat{X}}_{k, k - 1} = {\hat{X}}_{k - 1} - - - (7)$ Status Prediction: ${\hat{x}}_{k, k - 1} = {\hat{x}}_{k - 1} - - - (7)$

状态估计： ${\hat{X}}_{k} = X_{k, k - 1} + K_{k} (Z_{k} - {\hat{X}}_{k, k - 1}) - - - (8)$ State estimation: ${\hat{x}}_{k} = x_{k, k - 1} + K_{k} (Z_{k} - {\hat{x}}_{k, k - 1}) - - - (8)$

滤波增益方程：K_k＝P_k，k-1[P_k，k-1+R]^-1 (9)Filter gain equation: K _k = P _{k, k-1} [P _{k, k-1} + R] ^-1 (9)

预测误差方差方程：P_k，k-1＝P_k-1+Q (10)Forecast error variance equation: P _{k, k-1} = P _k-1 + Q (10)

估计误差方差方程：P_k＝[1-K_k]P_k，k-1 (11)Estimation error variance equation: P _k = [1-K _k ]P _{k, k-1} (11)

从式(7)-(12)海流剖面Kalman滤波公式中可以看出，只要确定ADCP声纳系统的观测噪声方差R与过程噪声方差Q，便可以逐步计算出滤波后的海流剖面数据。It can be seen from the formulas (7)-(12) Kalman filtering formulas of the ocean current profile that as long as the observation noise variance R and the process noise variance Q of the ADCP sonar system are determined, the filtered ocean current profile data can be gradually calculated.

1)观测噪声方差R的确定1) Determination of observation noise variance R

首先分析海流数据Kalman方程中的观测噪声方差R：如图3所示，使用小波变换工具分析ADCP声纳采集到的海流数据，提取数据的高频残差部分，根据高频残差数据的数量级确定R值的大小，再将R代人公式(9)中，计算Kalman方程中的增益系数K。First analyze the observation noise variance R in the Kalman equation of ocean current data: as shown in Figure 3, use the wavelet transform tool to analyze the ocean current data collected by ADCP sonar, and extract the high-frequency residual part of the data, according to the order of magnitude of the high-frequency residual data Determine the size of the R value, and then substitute R into the formula (9) to calculate the gain coefficient K in the Kalman equation.

2)过程噪声方差Q的确定2) Determination of process noise variance Q

当DVL声纳数据有效时，采用UUV速度变化率来动态确定过程噪声方差Q的大小。当UUV速度变化率较大时，动态增加Q值的大小，以提高海流观测数据的实时性；当UUV速度变化率较小时，则动态较小Q值的大小，以增加海流观测数据的稳定性。When the DVL sonar data is available, the UUV velocity change rate is used to dynamically determine the magnitude of the process noise variance Q. When the UUV speed change rate is large, dynamically increase the Q value to improve the real-time performance of ocean current observation data; when the UUV speed change rate is small, dynamically decrease the Q value to increase the stability of ocean current observation data .

再分析海流数据Kalman方程中的过程噪声方差Q：Q值偏大与偏小时得到的Kalman海流数据滤波曲线如图2所示，本专利采用DVL声纳数据动态关联Q值的方法。当UUV速度变化大时，动态增大Q值；反之则动态较小Q值。采用此方法得到的Kalman海流滤波数据如图3所示。将Q值代人公式(10)与公式(11)中，计算Kalman方程中的估计预测方差P。Then analyze the process noise variance Q in the Kalman equation of ocean current data: the filter curve of Kalman ocean current data obtained when the Q value is too large or too small is shown in Figure 2. This patent uses the method of dynamically correlating the Q value of DVL sonar data. When the UUV speed changes greatly, the Q value is dynamically increased; otherwise, the Q value is dynamically decreased. The Kalman current filtering data obtained by this method is shown in Fig. 3. Substitute the Q value into formula (10) and formula (11) to calculate the estimated prediction variance P in the Kalman equation.

当DVL声纳数据有效时，根据其测得的UUV速度信息以及罗经测得的UUV航向信息，将ADCP测得的海流信息从随船坐标系转换到北东固定坐标系下，如图2所示。转换公式如下：When the DVL sonar data is valid, according to the UUV speed information measured by it and the UUV heading information measured by the compass, the ocean current information measured by ADCP is converted from the on-board coordinate system to the northeast fixed coordinate system, as shown in Figure 2 Show. The conversion formula is as follows:

$\{\begin{matrix} {V V}_{E E.} = = {V V}_{u u} \times \times cos cos ((θ θ)) - - {V V}_{v v} \times \times sin sin ((θ θ)) \\ {V V}_{N N} = = {V V}_{u u} \times \times sin sin ((θ θ)) + + {V V}_{v v} \times \times cos cos ((θ θ)) \end{matrix} - - - - - - ((1212))$

式中，V_E为北东固定坐标系中ξ方向海流速度，V_N为北东固定坐标系中ζ方向海流速度，V_u为随船坐标系中x方向海流速度，V_v为随船坐标系中y方向海流速度。海流速度坐标系转换之后，便可以将UUV速度与海流剖面进行关联，建立起“UUV速度-海流信息”关系库。In the formula, V _E is the ocean current velocity in the ξ direction in the northeast fixed coordinate system, V _N is the ocean current velocity in the ζ direction in the northeast fixed coordinate system, V _u is the ocean current velocity in the x direction in the ship coordinate system, and V _v is the ship coordinate The velocity of the ocean current in the y-direction of the system. After the ocean current velocity coordinate system is converted, the UUV velocity can be associated with the ocean current profile, and a "UUV velocity-ocean current information" relationship library can be established.

(4)海流剖面辅助导航(4) Ocean current profile aided navigation

当DVL导航声纳数据失效时，利用“UUV速度-海流信息”关系库，以及ADCP声纳实时测得的海流剖面信息，推算UUV的导航速度。再根据初始UUV的经纬度和船位推算算法得到推算UUV的位置信息，具体推算过程如下：When the DVL navigation sonar data fails, use the "UUV speed-ocean current information" relationship library and the real-time ocean current profile information measured by the ADCP sonar to calculate the UUV's navigation speed. Then according to the latitude and longitude of the initial UUV and the ship position calculation algorithm, the position information of the estimated UUV is obtained. The specific calculation process is as follows:

$\{\begin{matrix} {v v}_{E E.} = = {v v}_{F f} sin sin H h - - {v v}_{L L} cos cos H h \\ {v v}_{N N} = = {v v}_{F f} cos cos H h + + {v v}_{L L} sin sin H h \end{matrix} - - - - - - ((1313))$

H——UUV载体的航向角，顺时针为正，逆时针为负，可由罗经测得。H——The heading angle of the UUV carrier, clockwise is positive and counterclockwise is negative, which can be measured by the compass.

$\{\begin{matrix} J J = = {J J}_{00} + + {Σ Σ}_{i i = = 11}^{n no} {v v}_{E E.,, ((i i - - 11))} Δt Δt / / {R R}_{M m,, ((i i - - 11))} \\ W W = = {W W}_{00} + + {Σ Σ}_{i i = = 11}^{n no} {v v}_{N N,, ((i i - - 11))} Δt Δt / / {R R}_{N N,, ((i i - - 11))} \end{matrix} - - - - - - ((1313))$

式中Δt——DVL采样周期；In the formula, Δt——DVL sampling period;

J₀、W₀——分别为初始时刻载体的经度和纬度，可由GPS接收机测得；J ₀ , W ₀ ——respectively, the longitude and latitude of the carrier at the initial moment, which can be measured by the GPS receiver;

以下描述本发明的实施例。Embodiments of the present invention are described below.

为了验证本发明方法的有效性，进行了DLV辅助导航海试试验，本发明中选取UUV规划中的一段U形航迹进行说明，该航迹用时1小时18分钟，航迹轨迹如图6所示。假设DVL长时间得不到有效数据，那么海流剖面辅助导航将一直采用最后一拍关联好的DVL-海流剖面关联数据进行导航，图7为DVL长时间得不到有效数据时的海流剖面数据导航图，此次海流剖面辅助UUV导航全程6008m，相对偏差2.35％，结果表明海流剖面辅助导航方法DVL失效时具有极大的应用价值。In order to verify the effectiveness of the method of the present invention, a DLV-assisted navigation sea trial test was carried out. In the present invention, a section of U-shaped track in the UUV planning is selected for illustration. The track takes 1 hour and 18 minutes, and the track trajectory is shown in Figure 6. Show. Assuming that DVL cannot obtain effective data for a long time, the current profile auxiliary navigation will always use the last shot associated DVL-ocean current profile associated data for navigation. Figure 7 shows the current profile data navigation when DVL cannot obtain valid data for a long time As shown in the figure, the current profile assisted UUV navigation is 6008m, and the relative deviation is 2.35%. The results show that the current profile assisted navigation method DVL has great application value when DVL fails.

Claims

1. the UUV auxiliary navigation method based on ocean current profile is characterized in that comprising the following steps:

(1) ocean current profile data and DVL speed are obtained

The control computer control ADCP that utilizes UUV to carry launches the sound wave of certain frequency, utilizes Doppler effect to obtain the ocean current profile that the UUV water layer thickness is close in the bottom;

Utilize the sound wave of the control computer control DVL emission certain frequency that UUV carries, utilize Doppler effect to obtain the speed of DVL, the i.e. headway of UUV;

(2) the Kalman filtering of ocean current profile

According to the characteristics of ocean current environment and UUV self model, set up ocean current profile data Kalman filtering data model; And determine observation noise variance R in the ocean current Kalman filtering data model with reference to the order of magnitude of ocean current high frequency residual error, the size of the UUV velocity information Dynamic Regulating Process noise variance Q value that records according to DVL simultaneously;

(3) " UUV speed-Ocean current information " concerns storehouse foundation

When DVL navigation sonar data are effective, utilize the course information of UUV speed that DVL records, UUV that the OCTANS attitude sensor measures, the ocean current profile data that ADCP measures are set up " UUV speed-Ocean current information " and are concerned the storehouse;

(4) ocean current profile assisting navigation

When DVL navigation sonar data failure, utilize " UUV speed-Ocean current information " concern the storehouse, and the ocean current profile information that gets of ADCP sonar Real-time Measuring, the navigation speed of reckoning UUV; Again according to the longitude and latitude of initial UUV and the positional information that the dead reckoning algorithm obtains calculating UUV.

2. a kind of UUV auxiliary navigation method based on ocean current profile according to claim 1 is characterized in that the acquisition methods of the positional information of described UUV is:

The UUV navigation speed of calculating the component of due east direction and direct north as shown in the formula:

\{\begin{matrix} v_{E} = v_{F} \sin H - v_{L} \cos H \\ v_{N} = v_{F} \cos H + v_{L} \sin H \end{matrix}

V in the formula _E, v _N---be respectively UUV carrier headway at the component of due east direction and direct north;

v _F, v _L---be respectively UUV carrier forward speed and the left-hand speed relative to the earth that DVL records;

H---the course angle of UUV carrier for just, for negative, is recorded by compass clockwise counterclockwise;

The UUV position is calculated by following formula:

\{\begin{matrix} J = J_{0} + Σ_{i = 1}^{n} v_{E, (i - 1)} Δt / R_{M, (i - 1)} \\ W = W_{0} + Σ_{i = 1}^{n} v_{N, (i - 1)} Δt / R_{N, (i - 1)} \end{matrix}

Δ t in the formula---the DVL sampling period;

J, W---be respectively n constantly longitude and the latitude of UUV carrier position;

J ₀, W ₀---be respectively longitude and the latitude of initial time carrier, recorded by the GPS receiver;

v _{E, (i-1)}, v _{N, (i-1)}---be respectively i-1 moment UUV carrier headway at the component of due east direction and direct north;

R _{M, (i-1)}, R _{N, (i-1)}---be respectively i-1 constantly earth meridian radius-of-curvature and latitude circle radius-of-curvature.