A precise navigation method for underwater autonomous vehicles based on multi-sensor fusion

Abstract

The invention discloses a precise navigation method for an underwater autonomous vehicle based on multi-sensor fusion, belonging to the field of navigation technology. The underwater vehicle adopts an INS / DVL combined navigation mode. Based on the DVL speed correction coefficient and installation deflection angle calibrated in a lake before deep-sea operation, the calculated DVL speed correction coefficient and installation deflection angle are compared with the lake test result through calibration operation in the deep sea, the calibration parameters are updated, and verification navigation is carried out based on the updated calibration parameters until the error meets the requirements, thereby effectively improving the accuracy of the INS / DVL combined navigation of the underwater vehicle.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a method for realizing precise navigation of an autonomous vehicle by utilizing multi-sensor fusion. Background Art

[0002] In the exploration and development of marine resources, the navigation accuracy and stability of autonomous underwater vehicles (AUVs) as important underwater detection tools are crucial. However, due to the special working environment of AUVs (such as high pressure and low temperature in the deep sea) and the errors of the navigation equipment itself, a single navigation device often cannot meet the long-term and high-precision navigation needs. Traditional navigation systems, such as Doppler VelocityLog (DVL), depth sensors, ultra-short baseline positioning systems (USBL) for hydroacoustic ranging, and global satellite navigation systems (GNSS), all have their own limitations when used alone. For example, DVL is easily affected by changes in the speed of sound, resulting in measurement errors; USBL has problems such as limited range, communication delays, and lack of real-time performance.

[0003] In recent years, integrated navigation technology has gradually become an important means to improve the navigation accuracy of AUVs. However, existing integrated navigation methods, such as the Inertial Navigation System (INS) / DVL combination and the INS / USBL combination, still have some technical bottlenecks. The INS / DVL combination will lead to the accumulation of position errors, while the INS / USBL combination does not accumulate position errors, but is limited by the range and communication delay of USBL, the real-time position accuracy calculation is delayed, and packet loss and point jumping are prone to occur, affecting navigation accuracy and stability. Summary of the invention

[0004] The present invention provides a method for improving the fusion navigation accuracy of an autonomous underwater vehicle (INS) / Doppler velocity measurement (DVL), thereby improving the navigation accuracy and efficiency of underwater operations.

[0005] In deep-sea environments, changes in temperature, salinity, pressure and other factors will cause changes in sound velocity, which is inconsistent with the results of the calibration parameters in the lake test, thus affecting the accuracy of the DVL. In order to improve the accuracy of the DVL, a series of technical measures need to be taken to correct the impact of changes in sound velocity, such as the use of sound velocity correction tables, sound line correction methods, etc. However, these methods have certain technical challenges and limitations in practical applications. The measurement error of DVL in a certain water area usually includes velocity correction coefficient, constant deviation, etc. Generally speaking, the velocity correction coefficient is the main error source of DVL, and the relative change of the installation deflection angle is not large, so the DVL velocity correction coefficient is mainly corrected.

[0006] In this paper, the ultra-short baseline positioning system USBL on the mother ship is used to determine the two-point position of the AUV, a method for calibrating the DVL speed correction coefficient and installation deflection in the deep sea is given, and the basis for selecting the calibration parameters is given, so as to improve the INS / DVL combined navigation accuracy.

[0007] The underwater autonomous vehicle precise navigation method based on multi-sensor fusion provided by the present disclosure is based on the following equipment:

[0008] The mother ship is integrated with GNSS, inertial navigation system INS1 and USBL array, while the AUV is integrated with inertial navigation system INS2, Doppler velocimeter DVL, depth gauge, transponder and other equipment. Through the coordinated work of these devices, more accurate positioning and navigation can be achieved in complex underwater environments; the mother ship follows the direction of the AUV, and the AUV does not exceed the USBL effective range.

[0009] The specific steps include:

[0010] (1) Before the AUV operates in the deep sea, the speed correction coefficient and installation angle should be calibrated in the lake.

[0011] (2) At sea, before the AUV starts to dive, perform inertial navigation alignment;

[0012] After the inertial navigation alignment is completed, the AUV starts to dive. At this time, the DVL is ineffective for the bottom, but the USBL can monitor the position of the AUV and ensure that the AUV is within the monitoring range.

[0013] (3) After arriving at the operating area, the AUV performs scanning and deep-sea calibration operations:

[0014] When it comes to deep-sea operations, the lawnmower path mode is generally used for surveying and scanning operations, which are basically straight-line navigation. This situation is suitable for calibrating the DVL speed correction factor and installation deflection angle.

[0015] 1) Selection of calibration area: AUV operates at low speed, and the target area is a flat area to ensure that the DVL data is valid throughout the process. If the DVL occasionally has invalid values, the previous frame of data can be used; if there are too many invalid data of the DVL speed, cancel this calibration. The speed correction coefficient is the main error source of the DVL, and the installation deflection angle can be used as a reference for the calibration result.

[0016] 2) Confirm the position of the calibrated start and end AUV through USBL:

[0017] The USBL on the mother ship detects the direction of the transponder and the depth information of the AUV to determine the position of the start and end points of the calibration AUV. If the transponder is directly below the USBL, then its direction should be consistent with the vertical direction of the USBL system, and the depth information should match the water depth where the transponder is located. At this time, the mother ship and the AUV are in the same position, but at different heights; therefore, the GNSS position information of the mother ship can be used as the start or end point of the AUV calibration in the deep sea. If the mother ship's GNSS is far from the three-dimensional coordinates of the USBL, the USBL arm needs to be compensated.

[0018] 3) The AUV will operate in the deep sea along the lawn mower track, and there will be multiple straight-line trajectories. It will sail in a straight line once and be calibrated at the same time. After the calibration result is verified and determined to be correct in the next voyage, it can directly operate afterwards.

[0019] Since the effective distance limitation and communication time of USBL directly affect its practicality, depth meter information calculation is introduced to assist in judging the accuracy of USBL information, screen out possible wild values ​​and jump point phenomena, and solve the problem of insufficient real-time performance of USBL.

[0020] Since the jump point of the position information provided by USBL is relatively large, jump points will occur when SINS / USBL is combined. Therefore, the present invention performs INS / DVL combination during calibration, and INS / USBL is not combined, so as to avoid jump points and maintain the accuracy of INS / DVL combination. This lays a good foundation for subsequent measurement and scanning operations.

[0021] In the above process, the inertial navigation system on the AUV opens two working modes at the same time: compass mode and navigation mode. During calibration, the compass attitude information is used together with the DVL for position calculation. The attitude information in the compass mode is relatively stable and not subject to external interference; the attitude information in the INS / DVL combination uses the attitude information in the inertial navigation mode. Since the attitude of the inertial navigation system will be corrected when the INS / DVL is combined, the influence of the calibration result when the INS / DVL combination is avoided.

[0022] Compared with the prior art, the beneficial effects of the present invention are as follows: (1) multiple navigation means are used and multi-sensor data fusion processing is performed, thereby improving the accuracy and stability of AUV underwater navigation; (2) USBL is used to realize deep-sea calibration, which is simple to implement and does not require continuous position signals. Only the starting point position and the end point position are required for calibration and navigation calculation, thereby improving the combined positioning accuracy; (3) by introducing depth meter information and the mother ship's attitude angle calculation to assist in judging the accuracy of USBL information, possible wild values ​​and jump point phenomena are screened out, and the problem of insufficient real-time performance of USBL is solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] 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.

[0024] Figure 1 is a flow chart of an exemplary embodiment according to the present disclosure;

[0025] FIG. 2 shows the operation trajectory of the AUV in an exemplary embodiment. DETAILED DESCRIPTION

[0026] 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.

[0027] The present disclosure proposes a method for realizing high-precision positioning of an AUV by combining sensors on a mother ship and an AUV. The mother ship integrates: an inertial navigation system 1 (INS1), a USBL array, and a GNSS, while the AUV integrates an inertial navigation system 2 (INS2), a DVL, a depth meter, and a transponder.

[0028] The exemplary embodiment flow chart is as follows Figure 1 As shown, the following steps are included:

[0029] Step 1: Calibration on the lake

[0030] The AUV is integrated with INS2, DVL, depth gauge and transponder. The speed correction factor and installation deflection angle are calibrated on the lake, and the calibration parameters are stored in INS2.

[0031] Step 2: Deep Sea Inertial Navigation Alignment

[0032] INS1 and INS2 are used to receive GNSS information for alignment, where INS1 operates in inertial navigation mode; INS2 operates in two working modes: inertial navigation mode and compass mode. The inertial navigation mode is combined with DVL to provide navigation information to the AUV; the compass mode provides heading information, which is combined with the speed information DVL to calibrate the installation deflection and speed correction coefficient.

[0033] After the inertial navigation alignment is completed, the AUV begins to dive. At this time, the DVL is ineffective for the bottom, but the USBL can monitor the position of the AUV and ensure that the AUV is within the monitoring range.

[0034] Step 3: AUV performs scanning and deep-sea calibration operations

[0035] The AUV performs scanning operations in the deep sea along the trajectory of a lawn mower. There will be multiple straight-line driving trajectories. It will sail in a straight line once and be calibrated at the same time. After the calibration result is verified and determined to be correct on the next voyage, it can proceed directly to operation.

[0036] Figure 1 The AUV scanning trajectory diagram in the figure, the AUV operation starting point position information is confirmed according to step 4, and the position information of point A is determined ( , After driving in a straight line for about 5 km, the mother ship arrives at point B. The mother ship moves synchronously with the AUV and determines the position information of point B according to the method in step 4 ( , ).

[0037] During this process, INS2 and DVL enter the combined mode to provide navigation information to the AUV; and use the heading provided by the compass mode and DVL to perform position calculations and record the position in real time. The position information of point A is recorded as ( , ), record the push position information of point B ( , ), (the position is replaced at the calibrated starting point, and the position information determined by USBL is sent to the AUV to determine the position information of the AUV, that is, the position of the calibrated starting point is the same).

[0038] Each time the position of the AUV is confirmed by USBL, the position of the INS2 and DVL combined state must also be replaced synchronously to avoid affecting the position of the AUV combined positioning during the deep-sea calibration process.

[0039] Step 4: Determine the location information of the deep-sea AUV:

[0040] After arriving at the operation area, the AUV is in a hovering state. The USBL on the mother ship is used to detect the direction and relative position of the transponder. If the transponder is directly below the USBL, its direction should be consistent with the vertical direction of the USBL system, and the depth information should be the same as the water depth where the transponder is located. It is determined that the mother ship and the AUV are in the same position at this time, but at different heights.

[0041] At this time, the GNSS position information of the mother ship is used to determine the position of the AUV in the deep sea: the GNSS position information of the mother ship is used to determine the position of the USBL, and then the position relationship between the USBL and the transponder measured by the USBL can be used to determine the position of the AUV in the deep sea;

[0042] Among them, when the USBL position is determined using the GNSS position information of the mother ship, if the three-dimensional coordinates of the GNSS on the mother ship and the USBL are far apart, the arm of the USBL needs to be compensated. The compensation arm method is as follows:

[0043]

[0044]

[0045] is the attitude matrix of the inertial navigation system 1, is the three-dimensional coordinate distance between GNSS and USBL carrier system, It is the three-dimensional coordinate distance between GNSS and USBL geographic system.

[0046] Then the location of the USBL matrix ( , ) and GNSS position ( , ) are as follows:

[0047] .

[0048] Step 5: Calibrate the installation deflection angle and calculate the speed correction coefficient.

[0049] and It represents the real displacement of AUV in the east and north directions, which can be calculated using the end point and the starting point determined by USBL; and Indicates the displacement in the east and north directions measured by inference, which is performed using the heading and DVL provided by the compass mode. K is the angle between the actual navigation endpoint line and the north axis (information provided by USBL), It is the angle between the line connecting the end point of the navigation and the north axis obtained by dead reckoning measurement (the compass heading and the information of DVL dead reckoning).

[0050] If the inertial navigation attitude error is ignored, the projection relationship between the DVL measurement velocity in the carrier coordinate system and the true velocity of the carrier in the carrier coordinate system is:

[0051] (1-1)

[0052] (1-2)

[0053] At this time (1-2) The speed brought in is , is the actual speed of DVL output. , It is the upper right front coordinate system of the d system.

[0054] After simplification, the displacement between the starting point and the theoretical end point is: , It is the upper right front coordinate system of system b.

[0055] (1-3)

[0056] in, is the heading angle, then the angle between the line connecting the starting point and the theoretical end point and the north axis The tangent value of :

[0057] (1-4)

[0058] When there are installation deviations and Doppler log errors, there are:

[0059] (1-5)

[0060] According to equations (1-4) and (1-5), after integration, the horizontal position calculated by the integrated navigation system is:

[0061] (1-6)

[0062] (1-7)

[0063] The angle between the line connecting the starting point and the calculated end point and the north axis The tangent value of is:

[0064] (1-8)

[0065] According to the trigonometric formula: the heading installation deviation angle Φ is:

[0066] Φ= (1-9)

[0067] in, To calibrate the angle between the line connecting the starting point and the theoretical end point and the north axis; The angle between the line connecting the starting point and the calculation end point and the north axis;

[0068] , , , (1-10)

[0069] and represents the true displacement of the AUV in the east and north directions, calculated using the end and start points determined by USBL; and It represents the displacement in the east and north directions measured by position inference during INS2 / DVL integrated navigation. Arctan represents the inverse tangent of the trigonometric function.

[0070] Scale factor error, theoretical displacement:

[0071] (1-11)

[0072] From equations (1-6) and (1-7), we can calculate the displacement:

[0073] (1-12)

[0074] According to formula (1-10) and formula (1-11), the speed correction coefficient can be obtained:

[0075] (1-13)

[0076] In this embodiment, the calibration method is very simple to implement. It does not require continuous position signals, but only the starting point position and the end point position. The starting point position and the end point position can be determined by the azimuth and position information of the mother ship's USBL detection transponder and the depth meter information.

[0077] Step six: calibration parameter selection.

[0078] According to formula (1-13), the installation deflection angle is independent of the correction coefficient and will not affect each other during calibration. Affected by the complex terrain of the deep sea, the speed correction coefficient generally changes greatly, while the installation deflection angle changes slightly.

[0079] Therefore, the deep-sea calibration results are compared with the original lake test results. If the installation deflection angle is close, the speed correction coefficient of the lake test calibration is replaced; if the calibrated installation deflection angle deviation is large, the calibration result is unusable and needs to be recalibrated.

[0080] Step 7: The AUV uses the new calibration coefficients to verify and start working along the mower path from point C to point D, continuing the INS2 / DVL combined mode:

[0081] The Kalman filter is designed based on the navigation error model, and the noise model of the three gyroscopes and two accelerometers of the inertial component adopts the white noise model. The error differential equation of the strapdown inertial navigation system is:

[0082]

[0083] Since the speed of DVL is valid during the DVL / INS2 process, the speed error can be selected as the observed quantity of the system.

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

[0085]

[0086] in, is the velocity navigation result of the inertial navigation system, V is the observation noise, and the observation matrix is

[0087]

[0088] The noise driving array is

[0089]

[0090] The system noise matrix is

[0091] The state quantities of the above two equations are:

[0092] In the above formula, the physical quantities are position error, velocity error, attitude angle error, accelerometer bias error, and gyroscope bias error. The state quantity can also be written as:

[0093]

[0094] The system matrix F is a 12×12 matrix in the following form:

[0095]

[0096] Note: The same row has the same number of rows; the same column has the same number of columns.

[0097] The position error equation is:

[0098]

[0099] So the position error corresponds to the position

[0100]

[0101] Position error corresponding to speed

[0102]

[0103] The Kalman filter equation is used to calculate the optimal estimate of the state:

[0104] State one-step prediction vector

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

[0106] State estimation calculation

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

[0108] Among them, Kk is the filter gain.

[0109] K k = P k / k−1 H k T (H k P k / k−1 H k T +R k ) −1

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

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

[0112] P k / k−1 =Φ k,k−1 P k−1 Φ k,k−1 T + Q k−1

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

[0114] Estimated mean square error equation P k :

[0115] P k =(1− K k H k ) P k / k−1

[0116] The Kalman filter equation is used to calculate the position information of INS2 / DVL integrated navigation.

[0117] Compare the AUV position given by USBL at point D with the INS2 / DVL combined navigation position:

[0118] Point C is taken as the starting point, and the AUV position given by USBL is the same as the starting point of the INS2 / DVL integrated navigation. Figure 2 The AUV scanning trajectory diagram in the figure is shown in Figure 1. The AUV operation starting point position information is confirmed according to step 4 to determine the position information of point C ( , ). After driving in a straight line for about 5 km, the mother ship arrives at point D. The mother ship moves synchronously with the AUV and determines the position information of point D according to the method in step 4 ( , ). In this process, INS2 and DVL enter the combined mode to provide navigation information to the AUV; and use the INS2 / DVL combined navigation position to record the combined position in real time, and record the position information of point C as ( , ), record the combined position information of point D ( , ).

[0119] calculate( , )and( , ) as the combined error of INS2 and DVL; calculate ( , )and( , ) is taken as the driving distance. The comparison between the two is the positioning error accuracy. If the requirements are met, continue with INS2 / DVL combined navigation.

[0120] In the subsequent operation process, the above steps 3 to 7 can be repeated.

[0121] In this embodiment, the scheme of calibrating parameters while using INS2 / DVL combined navigation improves the accuracy of INS2 / DVL combined navigation. The mother ship uses USBL to detect the position of AUV, and the collected position of AUV is combined with the navigation information of inertial navigation system 2 for subsequent surveying and scanning puzzles. Such information can provide important support for marine scientific research, marine resource development, etc.

[0122] 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 precise navigation method for an underwater autonomous vehicle based on multi-sensor fusion, characterized in that: The AUV mother ship is equipped with: INS1, USBL, GNSS, and the AUV is equipped with: INS2, DVL, depth gauge and transponder; the mother ship follows the direction of the AUV, and the AUV does not exceed the USBL range; The following steps are involved: S1, calibrate the DVL speed correction factor and installation deflection angle in the lake; S2, the AUV performs inertial navigation alignment before deep-sea diving; after alignment, it dives; S3, after arriving at the operation area, the AUV performs calibration based on the lawn mower path, sailing in a straight line once and calibrating once at the same time, including: S31, the position of the AUV is obtained through GNSS and USBL at the start and end of the calibration operation; during the calibration operation, INS2 and DVL enter the integrated navigation mode; S32, calculation of DVL installation deflection angle and speed correction coefficient; S33, comparing the calibration result obtained in step S32 with the original lake test result, if the installation deflection angle is less than the first set threshold, replacing the speed correction coefficient calibrated in the lake test; if the installation deflection angle exceeds the first set threshold, recalibrating; S4, after the calibration is verified on the next voyage and the results are correct, the subsequent operations can be carried out directly, including: The AUV uses the new calibration parameters for verification and continues the INS2 / DVL combined mode; the AUV position given by the verification endpoint USBL is compared with the INS2 / DVL combined navigation position. If the positioning error accuracy meets the requirements, the INS2 / DVL combined navigation continues.

2. The method according to claim 1, characterized in that In step S2: INS1 and INS2 are used to receive GNSS information for alignment respectively. INS1 runs in inertial navigation mode; INS2 runs in two working modes: inertial navigation mode and compass mode. The inertial navigation mode is used to provide navigation information to the AUV in combination with DVL; the compass mode is used to provide heading information, which is combined with the speed information DVL to calibrate the installation deflection angle and speed correction coefficient.

3. The method according to claim 2, characterized in that In step S31, the step of obtaining the position of the AUV by using GNSS and USBL at the start and end of the calibration operation specifically includes: At the start and end of the calibration operation, the AUV is in a hovering state; The USBL on the mother ship is used to detect the direction and relative position of the transponder on the AUV. According to the azimuth and slant distance of the transponder given by the USBL, if the slant distance is the same as the value of the depth gauge on the AUV, and the azimuth is zero, it is determined that the transponder, i.e. the AUV, is directly below the USBL. It is determined that the mother ship and the AUV are in the same position at this time, but at different heights. At this time, the position of the USBL is determined using the GNSS position information of the mother ship, and then the position relationship between the USBL and the transponder is measured using the USBL to determine the position of the AUV in the deep sea; Among them, when the position of USBL is determined by using the GNSS position information of the mother ship, if the three-dimensional coordinate distance between the GNSS on the mother ship and the USBL exceeds the second set threshold, the arm of USBL needs to be compensated, and the compensation method is as follows: is the attitude array of INS1, is the three-dimensional coordinate distance between GNSS and USBL carrier system, is the three-dimensional coordinate distance between GNSS and USBL geographic system; Then the location of the USBL matrix ( , ) and GNSS position ( , ) is: is the principal radius of curvature of the meridian, is the principal curvature radius of the Maoyou circle, for The inverse of the function, is the local latitude.

4. The method according to claim 2 or 3, characterized in that: The specific calculation method of step S32 includes: The heading installation deviation angle Φ is: Φ= in, To calibrate the angle between the starting point and the theoretical end point and the north axis, The angle between the starting point and the end point and the north axis: ， ， in, and represents the true displacement of the AUV in the east and north directions, calculated using the end and start points determined by USBL; and Indicates the displacement in the east and north directions measured by position inference during INS2 / DVL integrated navigation, where the heading provided by the INS2 compass mode and the DVL are used for position inference calculation; The speed correction factor is: in, The theoretical displacement is: To calculate the displacement: 。 5. The method according to claim 2 or 3, characterized in that: The step S4 specifically includes: Set up the verification section: The AUV uses the new calibration parameters to move from point C to point D. During this process, the Kalman filter equation is used to calculate the position information of the INS2 / DVL integrated navigation; The smoothed USBL position of point D is taken as the true value and compared with the INS2 / DVL combined navigation position at point D. If the positioning error of the combined navigation meets the requirements, continue the INS2 / DVL combined navigation.

Images