AUV positioning and orientation method based on GNSS dual antenna / low-precision SINS / DVL

Abstract

The invention discloses an AUV positioning and orientation method based on GNSS dual antenna / low-precision SINS / DVL, belonging to the technical field of autonomous vehicle positioning and orientation. The method ensures the accuracy of the AUV heading on the water surface by combining a low-precision SINS / GNSS dual antenna; under the water surface, the method combines SINS and DVL by utilizing the characteristic of low-precision SINS having good heading keeping accuracy, obtains speed information through DVL, establishes a SINS / DVL combined navigation system filtering model, and estimates and corrects the position information of SINS / DVL, thereby ensuring the accuracy of positioning and orientation of underwater AUV during navigation at a relatively low cost.

Description

Technical Field

[0001] The invention relates to the field of autonomous vehicle positioning and orientation, and in particular to an AUV positioning and orientation method based on a combination of a global satellite navigation system dual antenna, a low-precision strapdown inertial navigation system and a Doppler velocimeter. Background Art

[0002] AUV (Autonomous Underwater Vehicle) needs high-precision navigation information to operate smoothly underwater to ensure the accuracy of its heading, position and attitude. At present, the most commonly used navigation method is to use SINS (Strap-down Inertial Navigation System). SINS can provide accurate position and attitude information in a short time and has good autonomy and concealment. However, the cost of high-precision SINS is relatively high, which limits its wide application to a certain extent.

[0003] Although GNSS can provide speed and position information, in underwater environments, due to the shielding effect of water, AUVs cannot directly receive GNSS signals, so the use of GNSS positioning systems underwater is limited. Traditional AUV navigation systems are usually composed of high-precision inertial navigation systems, DVL (Doppler Velocity Log) and GNSS. Although such a combination can achieve high-precision navigation, it is expensive and not conducive to large-scale application.

[0004] In addition, when the inertial navigation system is used in combination with the DVL, calibration is required to determine the installation deflection angle and correction coefficient of the DVL and the inertial navigation system. However, when the low-precision SINS is combined with the DVL, the calibration result consistency is also poor due to the poor accuracy of the low-precision SINS in aligning the heading. Therefore, in practical applications, the low-precision SINS is often not calibrated when combined with the DVL, which further limits its ability to provide stable and reliable navigation information. Summary of the invention

[0005] The present invention discloses a positioning and orientation method based on a GNSS dual antenna and a low-precision SINS / DVL combination, which aims to effectively correct the cumulative error of SINS by combining three devices while reducing costs, and provide AUV with high-precision navigation information such as heading attitude, speed and position.

[0006] The AUV positioning and orientation method based on GNSS dual antenna / low-precision SINS / DVL provided by the present invention can ensure the accuracy of the AUV heading after the low-precision SINS / GNSS dual antenna combination on the water surface; when there is no SINS / GNSS dual antenna combination under the water surface, the low-precision SINS has good heading keeping accuracy, and the SINS and DVL are combined. The speed information can be obtained by combining the DVL, and the SINS / DVL combined navigation system filtering model is established, and the position information of the SINS / DVL is estimated and corrected, thereby ensuring the accuracy of the underwater AUV's positioning and orientation during the navigation process.

[0007] When using SINS / DVL combined navigation, the installation error angle and speed correction coefficient need to be calibrated first. Traditional methods include discrete calibration or Kalman filter calibration, both of which rely on the SINS's heading. During calibration, the installation error angle and speed correction coefficient affect each other and need to be calibrated at the same time. However, low-precision SINS has poor alignment accuracy and cannot be calibrated.

[0008] Determination of installation error angle: The DVL installation hole position is kept consistent with the dual antenna hole position during processing to ensure that the installation error angle is as small as possible; the low-precision SINS is combined with the GNSS dual antenna, and the heading is consistent with the GNSS dual antenna, ensuring that the installation error angle between the low-precision SINS heading and the DVL is very small and basically negligible;

[0009] Calibration of DVL speed correction coefficient: Fix DVL and GNSS to the ship separately to ensure the output signal is valid. Use differential signal synchronization to trigger the system so that the GNSS speed and DVL speed are output simultaneously, with a trigger frequency of 1Hz, and calculate the speed accumulation and ratio, which is the speed correction coefficient. There is no need to calibrate the installation deflection and correction coefficient of SINS / DVL on the AUV, and multiple sets of DVL speed correction coefficients can be calibrated at the same time, saving costs.

[0010] As a preference, the device indicators selected are as follows: GNSS dual-antenna heading accuracy is 0.02° / 4m, the accuracy of low-precision SINS is heading accuracy better than 0.5°*sec(φ) (1σ, φ is the local latitude), attitude accuracy is better than 0.05°(1σ), heading holding degree is 0.1*sec(φ)° / h, and attitude holding accuracy is better than 0.01°(1σ).

[0011] Compared with the prior art, the beneficial effects of the present invention are: (1) the heading accuracy of the low-precision inertial navigation system is low, but after being combined with the dual-antenna GNSS, it can ensure consistency with the dual-antenna GNSS heading, and at the same time, the heading retention degree is relatively high. By combining GNSS with low-precision SINS / DVL, high-precision directional positioning is achieved at a relatively low cost; (2) the heading accuracy is high (the heading error is corrected by the GNSS dual antenna), (3) the combined navigation accuracy is high (meeting the complex underwater navigation requirements), and (4) the calibration method is simple and suitable for batch calibration. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] The above and other objects, features and advantages of the present disclosure will become more apparent through a more detailed description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present disclosure.

[0013] Figure 1 The figure is a flow chart of GNSS dual-antenna / low-precision SINS / DVL integrated navigation according to the present disclosure. DETAILED DESCRIPTION

[0014] The preferred embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to make the present disclosure more thorough and complete, and to fully convey the scope of the present disclosure to those skilled in the art.

[0015] Figure 1 The flowchart of an exemplary embodiment according to the present disclosure mainly includes the following steps:

[0016] Step 1: Calibrate the DVL and GNSS speed correction coefficients. Fix the DVL and GNSS to the ship respectively, and use the differential signal to synchronize the trigger system so that the GNSS speed and the DVL speed are output simultaneously, and the speed information is guaranteed to be valid. The frequency is 1Hz, and the straight-line heading is a set distance (about 3km). Record the corresponding speed and calculate the speed accumulation value V of the two. DVL / V GNSS The ratio is the speed correction factor k. If there are multiple DVLs that can be calibrated at the same time, the corresponding speed correction factors can be calculated separately. The following are the detailed steps:

[0017] Data synchronization: Fix the DVL and GNSS to the ship separately. Use the differential signal of GNSS for time synchronization to ensure that the timestamps of the two are consistent. Record data at a frequency of 1Hz, that is, once per second.

[0018] Data Records:

[0019] During the straight-line driving of the set distance (about 3 km), the speed data of the DVL and GNSS are recorded. If there are multiple DVLs, the speed data of each DVL is recorded separately.

[0020] Speed ​​accumulative value calculation:

[0021] For each timestamp t, calculate the velocity accumulation value of DVL and GNSS. Assume represents the speed of DVL at the i-th timestamp, Indicates the speed of GNSS at the i-th timestamp.

[0022] Then the speed accumulation value of DVL is:

[0023]

[0024] Then the GNSS velocity accumulation value is:

[0025]

[0026] Where n is the total number of timestamps (at 1 Hz), the speed correction factor k is calculated.

[0027] For each DVL, calculate the ratio of the velocity accumulation values:

[0028] ;

[0029] If there are multiple DVLs, the speed correction coefficient corresponding to each DVL is calculated respectively. The speed correction coefficient k of each DVL is output as a result. The speed correction coefficient is used for subsequent correction of the DVL speed to improve the accuracy of the navigation system.

[0030] Step 2: Fix the GNSS dual antennas and SINS on the AUV. When designing, make sure that the fixing holes of the GNSS dual antennas are consistent with the holes of the DVL. Install the DVL calibrated in step 1 under the AUV.

[0031] Step 3: On the surface, the heading, attitude, position and speed information of the AUV system are obtained by combining the heading information, speed and position of the low-precision SINS and the GNSS dual antennas. The heading of the GNSS dual antenna is used as a reference to ensure that the heading of the inertial navigation system is consistent with the heading of the GNSS dual antennas and remains consistent after alignment and combination.

[0032] The Kalman system filter model is as follows.

[0033]

[0034] Among them: the state variable X is the following 12-dimensional

[0035]

[0036] In the above formula, , , is the attitude error, , is the speed error, , is the position error, , , is the gyro bias error, , is the accelerometer bias error.

[0037] The observed quantities are the heading, velocity and position error of SINS, namely:

[0038]

[0039] in, Head To calculate the heading, Head GNSS Represents the heading of the GNSS dual antenna. , The east speed and north speed obtained by solution are , Represents the east speed and north speed of GNSS respectively. , Respectively represent the calculated latitude and longitude, , Respectively represent the latitude and longitude of the calibration location.

[0040] The calculated heading, speed, position and the heading, speed and position difference of GNSS are used as observations. The heading, speed and position information are filtered and corrected using the Kalman algorithm to obtain the heading, attitude, speed and position information of the AUV system.

[0041] Step 4: After the AUV dives underwater, the GNSS signal is invalid. Since the heading retention of the low-precision SINS is relatively high within a certain period of time, the low-precision SINS can provide the AUV with heading and attitude.

[0042] Step 5: After the AUV has been submerged for a period of time and the DVL is effective against the bottom, the heading information of the low-precision SINS and the speed information of the DVL are combined, and the speed correction coefficient calculated in step 1 is used to correct the DVL speed to obtain the heading, attitude, speed, and position information of the AUV system.

[0043] The Kalman system filter model is as follows.

[0044]

[0045] Among them: the state variable is the following 12 dimensions.

[0046]

[0047] In the above formula, , , is the attitude error, , is the speed error, , is the position error, , , is the gyro bias error, , is the accelerometer bias error.

[0048] The observed quantity is the velocity error of SINS, that is:

[0049]

[0050] in, , The east speed and north speed obtained by solution are , represent the east speed and north speed of DVL respectively, and k is the speed correction coefficient.

[0051] Since the velocity of DVL is effective in the DVL / SINS process, the velocity error can be selected as the observed quantity of the system.

[0052] Then the observation matrix Z is expressed as:

[0053]

[0054] in, is the inertial navigation system speed, v is the speed of DVL, X is the state variable, V is the observation noise, and the observation matrix is

[0055]

[0056] Discretization of system state equations and measurement equations.

[0057] X k =Φ k，k-1 X k-1 +W k-1

[0058] Z k =H k X k +V k

[0059] In the formula, X k is the state vector at time k, that is, the estimated vector; Z k is the measurement sequence at time k; W k-1 is the system noise at time k-1; V k is the measurement noise sequence at time k; Φ k，k-1 is the one-step state transfer matrix from time k-1 to time k; H k is the measurement matrix at time k.

[0060] Using the standard Kalman filter equation to calculate the optimal estimate of the state

[0061] State one-step prediction vector

[0062] X k / k-1 =Φ k，k-1 X k-1

[0063] State estimation calculation

[0064] X k =X k / k-1 +K k (Z k -H k X k / k-1 )

[0065] Among them, K k is the filter gain:

[0066] -1

[0067] Among them, R k is the measurement noise matrix;

[0068] One-step forecast mean square error matrix P k / k-1 :

[0069]

[0070] in Q k-1 is the system noise matrix; the estimated mean square error equation is P k ;

[0071] Estimated mean square error equation P k :

[0072]

[0073] Correct the DVL combined velocity, and use the current optimal error estimate to correct the state quantity obtained from the SINS measurement data. The velocity correction can be corrected by simply subtracting the estimated value of the parameter from the estimated error of the inertial navigation system. The Kalman algorithm is used to filter and correct the velocity information to obtain the heading, attitude, velocity, and position information of the AUV system.

[0074] Step 6: After a set period of underwater navigation, the above steps 2 to 5 can be repeated. A low-cost and high-precision navigation method based on low-precision SINS can be realized; the directional positioning information based on the GNSS dual antenna / low-precision SINS / DVL combination is highly accurate and can meet the mission requirements of underwater navigation.

[0075] The above technical scheme is only an exemplary embodiment of the present invention. For those skilled in the art, it is easy to make various types of improvements or modifications based on the application methods and principles disclosed in the present invention, and it is not limited to the method described in the above specific embodiment of the present invention. Therefore, the method described above is only preferred and does not have a restrictive meaning.

Claims

1. A method for positioning and orienting an AUV based on GNSS dual antennas / low-precision SINS / DVL, comprising the following steps: S1, calibrate DVL and GNSS velocity correction coefficients; S2, fix the GNSS dual antenna and the low-precision SINS on the AUV, the fixing hole position of the GNSS dual antenna is consistent with the DVL hole position, and install the DVL calibrated in step S1 under the AUV; S3, when on the water surface, the heading information, speed and position of the low-precision SINS and GNSS dual antennas are combined, and the heading, attitude, position and speed information of the AUV system are obtained through the Kalman algorithm; among them, the heading of the GNSS dual antennas is used as the reference to ensure that the heading of the inertial navigation system is consistent with the heading of the GNSS dual antennas, and they are consistent after alignment and combination; S4, after the AUV dives underwater, the GNSS signal is invalid, and the low-precision SINS first provides the AUV with heading and attitude; S5, after the AUV dives for a period of time, after the DVL is effective against the bottom, the heading information of the low-precision SINS and the speed information of the DVL are combined, and the speed correction coefficient calculated in step S1 is used to correct the DVL speed based on the Kalman filter algorithm to obtain the heading, attitude, speed, and position information of the AUV system; The method of step S1 comprises: Fix the DVL and GNSS to the ship respectively, and use the differential signal to synchronize the trigger system so that the GNSS speed and the DVL speed are output simultaneously, and the speed information is guaranteed to be valid. The frequency is 1Hz, the straight-line heading is set, the corresponding speed is recorded, and the ratio of the accumulated speed values ​​of the two is calculated, that is, the speed correction coefficient k; If there are multiple DVLs that can be calibrated simultaneously, just calculate the corresponding speed correction coefficients for each; The specific steps of step S1 include: Data synchronization: Fix the DVL and GNSS to the ship separately; use the differential signal of GNSS for time synchronization to ensure that the timestamps of the two are consistent; record data at a frequency of 1Hz, that is, once per second; Data Records: Drive the set distance in a straight line. During this process, record the speed data of DVL and GNSS. If there are multiple DVLs, record the speed data of each DVL separately. Speed ​​accumulative value calculation: For each timestamp t, calculate the velocity accumulation values ​​of DVL and GNSS; assuming represents the speed of DVL at the i-th timestamp, represents the speed of GNSS at the i-th timestamp; Then the speed accumulation value of DVL is: Then the GNSS velocity accumulation value is: Where n is the total number of timestamps; Calculate the speed correction factor k: For each DVL, calculate the ratio of the velocity accumulation values: ； If there are multiple DVLs, the speed correction coefficient corresponding to each DVL is calculated respectively, and the speed correction coefficient k of each DVL is output as a result.

2. The method according to claim 1, characterized in that: The step S3 specifically includes: The Kalman system filter model is as follows: Among them, the state variable X is the following 12 dimensions: In the above formula, , , is the attitude error, , is the speed error, , is the position error, , , is the gyro bias error, , is the accelerometer bias error; The observed quantities are the heading, velocity and position error of SINS, namely: in, Head To calculate the heading, Head GNSS Represents the heading of the GNSS dual antenna, , are the solved east and north speeds, , Represents the east speed and north speed of GNSS respectively. , Respectively represent the calculated latitude and longitude, , Respectively represent the latitude and longitude of the calibration location; The calculated heading, velocity, position and the heading, velocity and position difference of GNSS are used as observation quantities. The heading, velocity and position information are filtered and corrected by Kalman algorithm to obtain the heading, attitude, velocity and position information of the AUV system.

3. The method according to claim 2, characterized in that The step S5 specifically includes: In the Kalman system filter model, the observed quantity is the velocity error of SINS, that is: in, , are the solved east and north speeds, , They represent the east and north speeds of DVL respectively. k is the speed correction factor, , is the speed error; Since the speed of DVL is effective in the DVL / SINS integrated navigation process, the speed error is selected as the observation quantity of the system, and the observation quantity matrix Z is expressed as: in, is the speed of SINS, v is the speed of DVL, X is the state variable, V is the observation noise, and the observation matrix is ； Correction of DVL combined velocity: The state quantity obtained by SINS measurement data is corrected using the current optimal error estimate; the velocity correction is performed by simply subtracting the SINS estimated value of the parameter from the estimated error; The Kalman algorithm is used to filter and correct the velocity information to obtain the heading, attitude, velocity and position information of the AUV system.

4. The method according to any one of claims 1 to 3, characterized in that: The accuracy of the low-precision SINS is: The heading accuracy is better than 0.5°*sec(φ) / 1σ, φ is the local latitude, the attitude accuracy is better than 0.05° / 1σ, the heading holding accuracy is 0.1*sec(φ)° / h, and the attitude holding accuracy is better than 0.01° / 1σ.

5. The method according to claim 1, characterized in that The following steps are also included: S6, after a set time of underwater navigation, repeat the above steps S2-S5.

Images