A method of inertial / Doppler mid-water layer navigation based on grid water velocity assistance

Abstract

An inertial / Doppler mid-water layer navigation method based on grid water velocity assistance includes: initializing the carrier attitude, speed, and position information and dividing the navigation area into staggered grids; when obtaining effective Doppler velocity meter water observation (DVL-WT), judging the grid area corresponding to each DVL-WT measurement based on the current carrier solution position and the DVL-WT water layer measurement depth; judging whether the water speed of the grid area corresponding to each DVL-WT measurement has been estimated; if it has been estimated, using the layer to measure and update the water speed measurement result, and using the combined high-precision result and the current DVL-WT measurement to estimate the corresponding grid water speed; between DVL-WT measurements, performing inertial strapdown solution, updating the attitude, speed, and position differential equations, and continuously obtaining the carrier attitude, speed, and position information; repeating the above steps until the combination process is completed. The present invention can fully utilize the DVL-WT information of the mid-water layer and improve the SINS / DVL navigation positioning accuracy in the mid-water layer area.

Description

Technical Field

[0001] The present invention belongs to the technology of integrated navigation system, is particularly applicable to the field of underwater integrated navigation, and specifically relates to an inertial / Doppler mid-water layer navigation method based on grid water velocity assistance. Background Art

[0002] To exploit the abundant ocean resources, ocean exploration has become an essential component of human development. This exploration relies on underwater vehicles, whose navigation and positioning capabilities are essential for mission effectiveness. Among numerous underwater navigation systems, the SINS (Strapdown Inertial Navigation System) / DVL (Doppler Velocimeter) integrated navigation system, with its autonomy, high accuracy, and simple deployment, has become a core module. However, existing SINS / DVL integrated navigation systems require the DVL to operate continuously in bottom tracking (DVL-BT) mode, leaving open the need for further research into navigation solutions in mid-water regions.

[0003] In deeper waters, SINS / DVL systems encounter mid-water regions where both GNSS and DVL-BT are ineffective during descent and ascent. In these regions, DVL can only obtain water velocity measurement (DVL-WT) data. To improve navigation accuracy in mid-water regions, a mid-water navigation method based on water velocity to assist SINS / DVL is urgently needed. Summary of the Invention

[0004] In order to improve the navigation accuracy of a carrier in the mid-water layer, the present invention proposes an inertial / Doppler mid-water layer navigation method based on grid water velocity assistance.

[0005] To achieve the above object, the technical solution adopted by the present invention is:

[0006] A method for inertial / Doppler mid-water layer navigation based on grid water velocity assistance, characterized by comprising the following steps:

[0007] S1: Based on the integrated navigation system being a SINS / GNSS or SINS / DVL-BT system, the carrier attitude, velocity, and position information are initialized; the navigation area is divided into staggered grids;

[0008] S2: When valid DVL-WT measurements are obtained, the grid area corresponding to each DVL-WT measurement is determined based on the current carrier solution position and the measured depth of each DVL-WT water layer;

[0009] S3: Determine whether the water velocity of the grid area corresponding to each DVL-WT measurement has been estimated. If it has been estimated, use the layer to update the water velocity measurement results; if it has not been estimated, do not update the measurement, and use the combined results and the current DVL-WT measurements to estimate the water velocity of the corresponding grid;

[0010] S4: Between DVL-WT measurements, perform inertial strapdown calculations to update attitude, velocity, and position differential equations, and continuously obtain the carrier's attitude, velocity, and position information;

[0011] S5: Repeat steps S2 to S4 until the combination process is completed.

[0012] As a further improvement of the present invention, step S1 specifically includes the following process:

[0013] When the carrier starts from the water surface, the SINS / GNSS combination result is used as the carrier initial state information; when the carrier starts from the DVL-BT effective water area, the SINS / DVL-BT combination result is used as the carrier initial state information;

[0014] The water area to be sailed is divided into staggered grids. According to the water velocity measurement stratification characteristics of the DVL equipment, the water area is divided into b from the depth of 0m from top to bottom. h As the floor height is divided into layers. In the first floor, the east-facing b E 、Northward b N Combined layer height b h Divide the rectangle into sections to form a continuous grid splicing. Between different layers, the grid intersection point of the next layer moves eastward by a compared to the previous layer. E , northbound movement a N , and so on, forming a staggered grid water area division.

[0015] As a further improvement of the present invention, step S2 specifically includes the following process:

[0016] When valid DVL-WT observations are obtained, the grid numbers (i, j, k) corresponding to the DVL-WT observations in different water layers are determined by the following steps;

[0017] Take a certain water layer as an example: Assume that the water layer is the Nth layer of water velocity measured by the DVL device at the current moment.

[0018]

[0019]

[0020]

[0021] λ k =λ1+(k-1)a E

[0022] L k =L1+(k-1)a N

[0023] in To round down, λ, L and h are the longitude, latitude and altitude of the current carrier location, λ k and L k Calculate the starting longitude and latitude for the grid number of the kth grid layer of the water area, R M and R N They are the radii of the meridian and the meridian respectively.

[0024] As a further improvement of the present invention, step S3 specifically includes the following process:

[0025] S3.1 When valid DVL-WT observations are obtained, determine whether the water velocity in the grid area corresponding to each DVL-WT measurement has been estimated. If it has been estimated, establish a DVL water velocity measurement error model:

[0026]

[0027] in is the velocity observation value of DVL-WT for grid (i, j, k), is the corresponding true value of speed measurement, w d_C(i，j，k) is the corresponding observation noise, δK is the DVL scale factor error;

[0028] The water velocity measurement results of this layer are used to construct the measurement equation:

[0029]

[0030]

[0031] z WT =[z WT(i1，j1，k1) … z WT(in，jn，kn )] T

[0032] Where n is the navigation coordinate system, the geographic coordinate system is used as the navigation coordinate system, b is the carrier coordinate system, is the speed of the SINS strapdown solution, is the rotation matrix corresponding to the carrier attitude angle, from n system to b system, is the estimated water velocity of grid (i, j, k), is the rotation matrix corresponding to the misalignment angle, φ x ,φ y ,φ z is the misalignment angle of each axis, δ is the related error, v n is the carrier speed in the n-series, is the true value of the corresponding velocity, υ C(i，j，k) is the water velocity estimation noise of grid (i, j, k), φ n is the misalignment angle, (×) is the corresponding skew-symmetric matrix;

[0033] Based on the attitude, velocity, and position differential equations and error models calculated by the SINS strapdown method, as well as the error model of the DVL water velocity measurement, the SINS / DVL-WT system equation is established, and the state variables are selected:

[0034]

[0035] where δv E ,δv N ,δv U is the velocity error in the northeast direction, δL, δλ, δh are the position errors in each axis, is the acceleration error in each axis, ε x , ε y , ε z is the gyroscope error of each axis;

[0036] Construct the system state equation:

[0037]

[0038] F and G are obtained based on the SINS and DVL-WT error models;

[0039] Based on the measurement equation and the system equation, filter estimation is performed to obtain the estimated result of the state quantity x, which is then compensated to the SINS system to form feedback correction;

[0040] S3.2 Use the combined high-precision results and the current DVL-WT measurements to estimate the water velocity of the corresponding grid;

[0041] Construct grid water velocity state vector x C :

[0042]

[0043] in is the estimated value of water flow in each grid, N1, N2, N3 are the maximum values ​​of the grids in three directions;

[0044] Based on the SINS / GNSS or SINS / DVL-BT high-precision integrated navigation information and the DVL-WT multi-water layer measurement values, the grid water velocity observation equation is constructed:

[0045]

[0046]

[0047] The observation matrix H C(i，j，k) is the unit matrix, H C By H C(i，j，k) where m is the number of effective grid water speeds observed this time, and (im, jm, km) is the grid number (i, j, k) corresponding to the m-th observation.

[0048] Based on the assumption that the water velocity in the grid does not change much in a short period of time, the one-step prediction of the Kalman filter state vector and its covariance is calculated:

[0049]

[0050] P C(k，k-1) =P C(k-1))

[0051] Calculate the Kalman gain matrix:

[0052]

[0053] where R C is the measurement noise covariance matrix;

[0054] Calculate the Kalman filter residual:

[0055] ζ C(k) =z C(k) -H C(k) x C(k，k-1)

[0056] Calculate the Kalman filter updated state estimate and its covariance:

[0057]

[0058] P C(k) =(IK C(k) H C(k) )P C(k，k-1 ).

[0059] As a further improvement of the present invention, step S4 specifically includes the following process:

[0060] Establish SINS attitude, velocity, and position differential equations:

[0061]

[0062]

[0063]

[0064]

[0065]

[0066]

[0067]

[0068]

[0069] where g n is the projection of the gravity vector in the navigation coordinate system, is the angular velocity from the carrier coordinate system to the navigation coordinate system in the carrier coordinate system, is the projection of the earth's rotation vector in the navigation coordinate system, is the angular velocity of the carrier from the earth coordinate system to the navigation coordinate system in the navigation coordinate system, and f b is the output of the inertial device, including the gyroscope and accelerometer, representing the angular velocity and specific force information relative to the inertial system, Ω is the angular velocity of the earth's rotation, v N 、v E and v U are the north, east and celestial velocities respectively.

[0070] As a further improvement of the present invention, step S5 specifically includes the following process:

[0071] In step S5, steps S2 to S4 are repeated until the combination process is completed. During the process, if there is GNSS information or DVL-BT information, it will form a multi-information fusion system with the SINS / DVL-WT system, which can further improve the accuracy.

[0072] Beneficial effects:

[0073] The water velocity-assisted SINS / DVL mid-water layer navigation method constructed in the present invention makes full use of the DVL-WT information of the mid-water layer and improves the SINS / DVL navigation positioning accuracy in the mid-water layer area. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 This is a system navigation scheme diagram of the inertial / Doppler mid-water layer navigation method based on grid water flow velocity assistance of the present invention. DETAILED DESCRIPTION

[0075] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments:

[0076] The following further describes the technical solution of the present invention in conjunction with the accompanying drawings. It should be understood that the examples provided below are only intended to fully and comprehensively disclose the present invention and fully convey the technical concept of the present invention to those skilled in the art. The present invention can also be implemented in many different forms and is not limited to the embodiments described herein. Figure 1The terms in the exemplary embodiments are not intended to limit the present invention.

[0077] The inertial / Doppler mid-water layer navigation method based on grid water velocity assistance provided by the present invention mainly includes the following steps:

[0078] Step S1: Initialize the carrier's attitude, velocity, and position information based on a high-precision integrated navigation system (e.g., SINS / GNSS or SINS / DVL-BT system); and divide the navigation area into staggered grids. Specifically, the following steps are involved:

[0079] When the carrier starts from the water surface, the SINS / GNSS combination result is used as the carrier's initial state information; when the carrier starts from the DVL-BT valid waters, the SINS / DVL-BT combination result is used as the carrier's initial state information.

[0080] The water area to be sailed is divided into staggered grids. According to the water velocity measurement stratification characteristics of the DVL equipment, the water area is divided into b from the depth of 0m from top to bottom. h As the floor height is divided into layers. In the first floor, the east-facing b E 、Northward b N Combined layer height b h The rectangular block is divided to form a continuous extended grid splicing. Between different layers, the grid intersection point of the next layer moves eastward a compared to the previous layer. E , northward movement a N , and so on, forming a staggered grid water area division.

[0081] Step S2: When valid DVL-WT measurements are obtained, the grid area corresponding to each DVL-WT measurement is determined based on the current carrier solution position and the measured depth of each DVL-WT water layer. This specifically includes the following steps:

[0082] When valid DVL-WT observations are obtained, the grid numbers (i, j, k) corresponding to the DVL-WT observations in different water layers are determined by the following steps (taking a certain water layer as an example): Assume that the water layer is the Nth layer of the water velocity measurement by the DVL device at the current moment,

[0083]

[0084]

[0085]

[0086] λ k =λ1+(k-1)a E

[0087] L k =L1+(k-1)aN

[0088] in To round down, λ, L and h are the longitude, latitude and altitude of the current carrier location, λ k and L k Calculate the starting longitude and latitude for the grid number of the kth grid layer of the water area. M and R N They are the radii of the meridian and the meridian respectively.

[0089] Step S3 determines whether the water velocity in the grid area corresponding to each DVL-WT measurement has been estimated. If so, the water velocity measurement results are updated using this layer; if not, no update is performed. Simultaneously, the combined high-precision results and the current DVL-WT measurements are used to estimate the water velocity in the corresponding grid area. This specifically involves the following steps:

[0090] S3.1 When valid DVL-WT observations are obtained, determine whether the water velocity in the grid area corresponding to each DVL-WT measurement has been estimated. If it has been estimated, establish the DVL water velocity measurement error model:

[0091]

[0092] in is the velocity observation value of DVL-WT for grid (i, j, k), is the corresponding true value of speed measurement, w d_C(i，j，k) is the corresponding observation noise, and δK is the DVL scale factor error.

[0093] The water velocity measurement results of this layer are used to construct the measurement equation:

[0094]

[0095]

[0096] z WT =[z WT(i1，j1，k1) … z WT(in，jn，kn )] T

[0097] Where n is the navigation coordinate system (the geographic coordinate system is used as the navigation coordinate system), b is the carrier coordinate system, is the speed of S1NS strapdown solution, is the rotation matrix corresponding to the carrier attitude angle (n system to b system), is the estimated water velocity of grid (i, j, k), is the rotation matrix corresponding to the misalignment angle, φ x ,φ y ,φ zis the misalignment angle of each axis, δ is the related error, v n is the carrier speed in the n-series, is the true value of the corresponding velocity, υ C(i，j，k) is the water velocity estimation noise of grid (i, j, k), φ n is the misalignment angle, and (×) is the corresponding skew-symmetric matrix.

[0098] At the same time, the SINS error model is constructed:

[0099]

[0100]

[0101]

[0102]

[0103]

[0104] in is the projection of the earth's rotation vector in the navigation coordinate system, is the angular velocity of the carrier from the earth coordinate system to the navigation coordinate system in the navigation coordinate system, ε b is the gyroscope error, δ is the related quantity error, is the accelerometer error, f b is the accelerometer output, representing the specific force information relative to the inertial system, δv E ,δv N ,δv U is the velocity error in the northeast celestial direction, δL, δλ, and δh are the position errors in each axis.

[0105] The SINS / DVL-WT system equations are established based on the attitude, velocity, and position differential equations and error models calculated by the SINS strapdown, as well as the DVL water velocity measurement error model. Select the state variables:

[0106]

[0107] in is the acceleration error in each axis, ε x , ε y , ε z is the gyroscope error of each axis. Construct the system state equation:

[0108]

[0109] F and G are obtained based on the SINS and DVL-WT error models.

[0110] Based on the measurement equation and the system equation, filtering estimation is performed to obtain the estimated result of the state quantity x, which is then compensated to the SINS system to form feedback correction.

[0111] S3.2 uses the combined high-precision results and the current DVL-WT measurements to estimate the water velocity of the corresponding grid.

[0112] Construct grid water velocity state vector x C :

[0113]

[0114] in is the estimated value of water flow in each grid, and N1, N2, and N3 are the maximum values ​​of the grids in three directions.

[0115] Based on the SINS / GNSS or SINS / DVL-BT high-precision integrated navigation information and the DVL-WT multi-water layer measurement values, the grid water velocity observation equation is constructed:

[0116]

[0117]

[0118] The observation matrix H C(i，j，k) is the unit matrix, H C By H C(i，j，k) m is the number of effective grid water speeds observed this time. (im, jm, km) is the grid number (i, j, k) corresponding to the mth observation.

[0119] Based on the assumption that the water velocity in the grid does not change much in a short period of time, the one-step prediction of the Kalman filter state vector and its covariance is calculated:

[0120]

[0121] P C(k，k-1) =P C(k-1)

[0122] Calculate the Kalman gain matrix:

[0123]

[0124] where R C is the measurement noise covariance matrix;

[0125] Calculate the Kalman filter residual:

[0126] ζ C(k) =z C ( k) -H C(k) x C(k，k-1)

[0127] Calculate the Kalman filter updated state estimate and its covariance:

[0128]

[0129] P C(k) =(IK C(k) H C(k) )P C(k，k-1)

[0130] Step S4: Perform inertial strapdown calculations between DVL-WT measurements, update the attitude, velocity, and position differential equations, and continuously obtain the carrier attitude, velocity, and position information. This specifically includes the following steps:

[0131] Establish SINS attitude, velocity, and position differential equations:

[0132]

[0133]

[0134]

[0135]

[0136]

[0137]

[0138]

[0139]

[0140] where g n is the projection of the gravity vector in the navigation coordinate system, is the angular velocity from the carrier coordinate system to the navigation coordinate system in the carrier coordinate system, is the gyroscope output, representing the angular velocity information relative to the inertial system, Ω is the angular velocity of the earth's rotation, v N 、v E and v U are the north, east and celestial velocities respectively.

[0141] Step S5: Repeat steps S2 to S4 until the combination process is complete. If GNSS and DVL-BT observations are used during the integrated navigation process, they can be combined with SINS / DVL-WT sub-filters to further improve accuracy. The specific process includes the following:

[0142] Repeat steps S2 to S4 until the integration process is complete. If GNSS and DVL-BT observations are available during the integrated navigation process, they can be combined with the SINS / DVL-WT sub-filter to further improve accuracy.

[0143] The above description is merely a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent variation based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A water layer navigation method based on grid water velocity assistance and inertial / Doppler, characterized in that: The following steps are involved: S1: Initialize the carrier attitude, velocity, and position information for the SINS / GNSS or SINS / DVL-BT system based on the integrated navigation system; divide the navigation area into staggered grids; S2: When valid DVL-WT measurements are obtained, the grid area corresponding to each DVL-WT measurement is determined based on the current carrier solution position and the measured depth of each DVL-WT water layer; S3: Determine whether the water velocity of the grid area corresponding to each DVL-WT measurement has been estimated. If it has been estimated, use this layer to measure and update the water velocity measurement results; If it is not estimated, no measurement update is performed. At the same time, the combined result and the current DVL-WT measurements are used to estimate the water velocity of the corresponding grid; S4: Between DVL-WT measurements, perform inertial strapdown calculations to update attitude, velocity, and position differential equations, and continuously obtain the carrier's attitude, velocity, and position information; S5: Repeat steps S2 to S4 until the combination process is completed.

2. The inertial / Doppler mid-water layer navigation method based on grid water velocity assistance according to claim 1 is characterized in that: The step S1 specifically The following processes are included: When the carrier starts from the water surface, the SINS / GNSS combination result is used as the carrier initial state information; when the carrier starts from the DVL-BT effective water area, the SINS / DVL-BT combination result is used as the carrier initial state information; The water area to be sailed is divided into staggered grids. According to the water velocity measurement stratification characteristics of the DVL equipment, the water area is divided from the depth of 0m from top to bottom. As the floor height is divided into layers, within one floor, the east , North Combined layer height Divide the rectangle into sections to form a continuous grid. Between different layers, the grid intersection points of the next layer move eastward compared to the previous layer. , Northbound Movement , and so on, forming a staggered grid water area division.

3. The inertial / Doppler mid-water layer navigation method based on grid water velocity assistance according to claim 1 is characterized in that: The step S2 specifically includes the following process: When valid DVL-WT observations are obtained, the grid numbers corresponding to the DVL-WT observations in different water layers Determine by following the steps below; Take a certain water layer as an example: Assume that the water layer is the Nth layer of water velocity measured by the DVL device at the current moment. ； ； ； ； ； in To round down, 、 and It represents the longitude, latitude and altitude of the current carrier location. and For navigable waters The grid number of the layer grid is used to calculate the starting longitude and latitude. and They are the radii of the meridian and the meridian respectively.

4. The inertial / Doppler mid-water layer navigation method based on grid water velocity assistance according to claim 1 is characterized in that: The step S3 specifically The following processes are included: S3.1 When valid DVL-WT observations are obtained, determine whether the water velocity in the grid area corresponding to each DVL-WT measurement has been estimated. If it has been estimated, establish a DVL water velocity measurement error model: ； in DVL-WT grid Speed ​​measurement observation value, is the corresponding true value of speed measurement, is the corresponding observation noise, is the DVL scale factor error; The water velocity measurement results of this layer are used to construct the measurement equation: ； ； ； in As the navigation coordinate system, the geographic coordinate system is used as the navigation coordinate system. is the carrier coordinate system, is the speed of the SINS strapdown solution, is the rotation matrix corresponding to the carrier attitude angle, Tie to Tie, Grid Estimated water velocity, is the rotation matrix corresponding to the misalignment angle, , , is the misalignment angle of each axis, is the correlation error, for System download speed, is the true value of the corresponding speed, Grid Water velocity estimation noise, is the misalignment angle, is the corresponding skew-symmetric matrix; Based on the attitude, velocity, and position differential equations and error models calculated by the SINS strapdown method, as well as the error model of the DVL water velocity measurement, the SINS / DVL-WT system equation is established, and the state variables are selected: ； in , , is the velocity error in the northeast sky direction, , , is the position error of each axis, , , is the acceleration error in each axis, , , is the gyroscope error of each axis; Construct the system state equation: ； and Obtained according to the SINS and DVL-WT error models; Based on the measurement equation and system equation, filter estimation is performed to obtain the state quantity The estimation results are compensated to the SINS system to form feedback correction; S3.2 Use the combined high-precision results and the current DVL-WT measurements to estimate the water velocity at the corresponding grid; Constructing grid water velocity state vector : ； in is the estimated value of water flow in each grid, , , is the maximum value of the grid in three directions; Based on the SINS / GNSS or SINS / DVL-BT high-precision integrated navigation information and the DVL-WT multi-water layer measurement values, the grid water velocity observation equation is constructed: ； ； The observation matrix is the unit matrix, Depend on composition, The number of effective grid water speeds observed this time, For the The grid number corresponding to the observation ; Based on the assumption that the water velocity in the grid does not change much in a short period of time, the one-step prediction of the Kalman filter state vector and its covariance is calculated: ； ； Calculate the Kalman gain matrix: ； in is the measurement noise covariance matrix; Calculate the Kalman filter residual: ； Calculate the Kalman filter updated state estimate and its covariance: ； 。 5. The inertial / Doppler mid-water layer navigation method based on grid water velocity assistance according to claim 1 is characterized in that: The step S4 specifically includes the following process: Establish SINS attitude, velocity, and position differential equations: ； ； ； ； ； ； ； ； in is the projection of the gravity vector in the navigation coordinate system, is the angular velocity from the carrier coordinate system to the navigation coordinate system in the carrier coordinate system, is the projection of the earth's rotation vector in the navigation coordinate system, is the angular velocity of the carrier from the earth coordinate system to the navigation coordinate system in the navigation coordinate system, and are the outputs of the gyroscope and accelerometer, representing the angular velocity and specific force information relative to the inertial system. is the angular velocity of the Earth's rotation, 、 and are the north, east and celestial velocities respectively.

6. According to the grid water velocity-assisted inertial / Doppler mid-water layer navigation method of claim 1, step S5 specifically comprises the following process: In step S5, steps S2 to S4 are repeated until the combination process is completed. During the process, if there is GNSS information or DVL-BT information, it forms a multi-information fusion system with the SINS / DVL-WT system.

Images