An Underwater Tightly Coupled Navigation Method for SINS / DVL / USBL Based on Centralized Filtering

Abstract

An underwater tightly-coupled navigation method for SINS / DVL / USBL based on centralized filtering, the method comprising the following steps: (1) Select the state variables of the SINS / DVL / USBL navigation algorithm and construct the state equation of centralized filtering; (2) Convert and calculate the original beam frequency shift information in the DVL coordinate system according to the carrier velocity information output by the SINS, and convert and calculate the angle measurement information and slant range information in the USBL acoustic array coordinate system according to the carrier position information and the position information of the transponder; (3) Construct the SINS / DVL tightly-coupled measurement equation based on frequency shift measurement and the SINS / USBL tightly-coupled measurement equation based on relative measurement information; (4) Design the DVL and USBL fault detection and processing mechanisms, and design to isolate the DVL and USBL fault data. The present invention provides an underwater navigation algorithm for SINS / DVL / USBL based on centralized filtering, which can effectively solve the accuracy loss caused by the failure of DVL or USBL data during the navigation and positioning process, and improve the underwater positioning accuracy.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater navigation, and particularly to an underwater tightly integrated navigation method based on centralized filtering for SINS / DVL / USBL. Background Art

[0002] The ocean contains a large amount of marine biological resources, chemical resources, and valuable mineral resources such as rich oil and natural gas. As an important tool for exploring the ocean, an Autonomous Underwater Vehicle (AUV) is undoubtedly essential for the future development of ocean resources and the expansion of the marine military field. Underwater navigation and positioning technology provides accurate navigation information for AUVs, which is crucial for AUVs to conduct marine scientific research and investigation operations safely and effectively.

[0003] Integrated navigation is to utilize the advantages of each navigation device, make up for each other's deficiencies, and effectively combine multiple navigation devices to achieve high-precision navigation of AUVs, improving fault tolerance and reliability. SINS / DVL integrated navigation and SINS / USBL integrated navigation are currently relatively mature and widely used underwater integrated navigation methods. However, SINS / DVL integrated navigation can suppress the divergence of speed errors, but the high-precision speed information provided by DVL will also be affected by its own speed measurement errors and the underwater environment, and finally manifests as a position error that accumulates over time; although SINS / USBL integrated navigation has a relatively high overall positioning accuracy and can suppress the divergence of position errors, due to the need to deploy sound source transmitters underwater, there are regional distance limitations, and it cannot meet the needs of AUVs to achieve wide-area navigation and positioning. Therefore, in underwater navigation, combining SINS, DVL, and USBL through integrated navigation technology helps to make up for each other's deficiencies of each navigation device, suppress the divergence of speed errors and position errors, and improve the long-term navigation and positioning accuracy.

[0004] The patent with the application number CN202210492631.4 (SINS / DVL / USBL underwater tightly integrated navigation method and device, underwater vehicle control equipment) proposes to use the original beam velocity data of DVL and the original slant range and azimuth information of USBL under the condition of SINS / DVL / USBL combination, and use federated filtering to implement a tightly integrated navigation method and solve the problem of missing DVL or USBL information. However, this method only considers the beam zero bias of DVL and does not consider other main errors of DVL and the main errors of USBL, and cannot achieve the mutual correction effect between navigation devices. The estimation obtained by the non-reset federated filtering structure adopted is not globally optimal. To address these issues, the underwater tightly integrated navigation method of SINS / DVL / USBL based on centralized filtering proposed in the present invention fully considers the influence of error sources of the three navigation devices, constructs measurement quantities based on centralized filtering using the original beam frequency shift data of DVL and the original slant range and azimuth information of USBL, and the centralized filtering method can achieve globally optimal estimation and obtain higher positioning accuracy. And the measurement matrix is adaptively updated and adjusted in the centralized filter to solve the problem of invalid DVL and USBL information.

[0005] The information filtering model of centralized SINS / DVL / USBL integrated navigation proposed in the patent with the application number CN202211086240.9 (an underwater multi-source navigation and positioning method based on undersea oil and gas pipeline detection) is realized by adopting a loose integration method in the construction of the observation model, that is, selecting the SINS / DVL velocity observation model and the SINS / USBL position observation model. While the centralized filtering structure adopted in the present invention is realized by using a tight integration method, constructing a SINS / DVL tightly integrated measurement equation based on frequency shift measurement and a SINS / USBL tightly integrated measurement equation based on relative measurement information as the measurement of centralized filtering. By adopting the tight integration method, the overall positioning accuracy is higher and the robustness is also stronger.

[0006] The information filtering model of centralized SINS / DVL / USBL integrated navigation proposed by it improves the state estimation accuracy by improving the DS-UKF algorithm and uses the outlier detection algorithm of weighted one-class SVM to solve the noise and jump point problems of USBL data. While in the centralized filtering structure adopted in the present invention, the fault detection of DVL and USBL is realized by the chi-square test method, and after detecting the faults of DVL and USBL, the dimension of the measurement equation is adaptively changed and the measurement equation is updated, which is simpler to implement and can also effectively handle the problem of accuracy degradation caused by invalid DVL and USBL information. Summary of the Invention

[0007] To solve the above technical problems, the present invention proposes an underwater tightly integrated navigation method for SINS / DVL / USBL based on centralized filtering, which realizes mutual calibration and complementary advantages among three navigation devices, and improves the navigation and positioning accuracy.

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

[0009] An underwater tightly integrated navigation method for SINS / DVL / USBL based on centralized filtering, comprising the following steps:

[0010] Step 1. Select the state variables of the SINS / DVL / USBL navigation algorithm and construct the state equation of centralized filtering;

[0011] Step 2. Convert and calculate the original beam frequency shift information in the DVL coordinate system according to the carrier velocity information output by the SINS, and convert and calculate the angle measurement information and slant range information in the USBL acoustic array coordinate system according to the carrier position information and the position information of the transponder;

[0012] Step 3. On the basis of Steps 1 and 2, construct the SINS / DVL tightly integrated measurement equation based on frequency shift measurement and the SINS / USBL tightly integrated measurement equation based on relative measurement information as the measurement of centralized filtering;

[0013] Step 4. Design the detection and processing mechanism for DVL and USBL faults, and design to isolate the DVL and USBL fault data and update the measurement of centralized filtering in Step 3.

[0014] As a further improvement of the present invention, Step 1 is specifically as follows:

[0015] Select the traditional 15-dimensional inertial navigation system error as the error state quantity of the SINS subsystem:

[0016]

[0017] Wherein, δV n =[δV E δV N δV U T are the velocity errors in the east, north, and up directions respectively; φ n =[φ E φ N φ U T are the attitude errors in the east, north, and up directions respectively; δP n =[δL δλ δh] T are the latitude error, longitude error, and altitude error of the inertial navigation respectively; ​​are the biases of the three axes of the SINS accelerometer respectively; ε = [ε x ε y ε z T are the drifts of the three axes of the SINS gyroscope respectively;

[0018] Select the DVL scale factor error and the DVL installation angle error as the error state variables of the DVL subsystem:

[0019] X DVL = [δθ dx δθ dy δθ dz δκ d T (2)

[0020] where δθ d = [δθ dx δθ dy δθ dz T is the installation error angle of the three coordinate axes between the DVL and the SINS; δκ d is the scale factor error of the DVL;

[0021] Select the installation error angle between the USBL and the SINS, the angle measurement error of the USBL, the ranging error coefficient, and the constant bias error of the depth gauge PS as the error state variables of the USBL subsystem:

[0022] X USBL = [δθ ux δθ uy δθ uz δαδβδκ u δb ps T (3)

[0023] where δθ u = [δθ ux δθ uy δθ uz T is the installation error angle of the three axes between the USBL and the SINS; δα is the azimuth angle measurement error of the USBL, δβ is the elevation angle measurement error of the USBL, δκ u is the ranging error coefficient of the USBL, δb ps is the constant bias error of the depth gauge PS;

[0024] When constructing the SINS / DVL / USBL centralized filtering system, the state variables selected are:

[0025]

[0026] ​​​​​The state equation of the centralized filter for the SINS / DVL / USBL tightly integrated navigation is established as follows:

[0027]

[0028] Wherein, the system matrix F SINS is the inertial navigation system matrix, F DVL = 0 4×4 is the system matrix of the DVL subsystem, F USBL = 0 7×7 is the system matrix of the USBL subsystem, and W is the correlation noise matrix.

[0029] As a further improvement of the present invention, the specific steps of step 2 are as follows:

[0030] Using the velocity information of the navigation system measured by the SINS, calculate the frequency shift values in the four-beam directions as:

[0031]

[0032] Wherein, f 0 is the acoustic wave emission frequency, c is the sound speed in water, α dvl = 70°, f d is the true value of the frequency shift, is the inertial navigation attitude matrix, and V n is the velocity in the navigation coordinate system;

[0033] According to the known transponder position P t e and the AUV vehicle position solved by the SINS, convert them to the earth coordinate system Calculate the azimuth angle and the slant range

[0034]

[0035] Wherein, [α β ρ] T is the true value of the azimuth angle and the slant range,

[0036] Wherein, is the attitude transformation matrix between the acoustic array coordinate system and the vehicle coordinate system, is the attitude transformation matrix between the navigation coordinate system and the earth coordinate system,

[0037]

[0038] The coordinates of the transponder under the u system L, λ, and h are the latitude, longitude, and altitude of the vehicle output by the SINS, and R N is the radius of curvature of the prime vertical, and e is the first eccentricity.

[0039] As a further improvement of the present invention, step 3 is specifically as follows:

[0040] Using the frequency shift value calculated by the SINS and the actual frequency shift of the DVL The difference as the measurement value, based on the SINS / DVL tightly coupled measurement equation of frequency shift measurement:

[0041]

[0042] The observation matrix is:

[0043]

[0044] Using the difference between the azimuth and slant range information calculated from the inertial navigation position information output by the SINS and the azimuth and slant range information output by the USBL as the measurement quantity:

[0045]

[0046] In the formula,

[0047] Using the difference between the height information of the SINS and the depth information of the PS as the height information measurement:

[0048] Z PS = δh - δb ps (11)

[0049] Based on the SINS / USBL tightly coupled measurement equation of relative measurement information:

[0050]

[0051] The measurement matrix is:

[0052]

[0053] The measurement quantity of the constructed SINS / DVL / USBL tightly coupled navigation system can be selected as:

[0054]

[0055] The corresponding measurement equation constructed is

[0056] Z = HX + V (15)

[0057] In the formula, the measurement matrix is V is the measurement noise correlation term.

[0058] As a further improvement of the present invention, the specific steps of step 4 are as follows:

[0059] Adopt χ 2 test method to determine whether the measured values of DVL and USBL are abnormal.

[0060] When it is judged that there are invalid data and outliers in DVL, data isolation processing is adopted, and the measurement equation of the system is simplified to:

[0061] Z = Z S / U = H S / U X 26×1 + V (16)

[0062] When outliers appear in USBL due to environmental influence, data isolation processing is adopted, and the measurement equation of the system is simplified to:

[0063] Z = Z S / D = H S / D X 26×1 + V (17).

[0064] This application has the following benefits:

[0065] The present invention adopts a centralized filtering structure design for the SINS / DVL / USBL tightly coupled navigation algorithm, which can achieve global optimal estimation and obtain higher positioning accuracy; in the selection of state variables of the tightly coupled navigation algorithm, the present invention fully considers the main error sources of each navigation device, which can realize mutual correction and complementary advantages between navigation devices; the present invention adds a fault detection and processing mechanism to the centralized filtering structure, which can improve the fault tolerance of the centralized filtering method and solve the problem of accuracy decline caused by the failure of DVL or USBL data. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 is a schematic structural diagram of an underwater tightly coupled navigation algorithm based on centralized filtering;

[0067] Figure 2 is a schematic diagram for obtaining the original acoustic frequency shift data of DVL;

[0068] Figure 3 is a schematic diagram for obtaining the original azimuth and slant range data of USBL;

[0069] Figure 4 is a comparison of the horizontal positioning errors between the centralized filtering and federated filtering of the SINS / DVL / USBL tightly coupled system. DETAILED DESCRIPTION OF THE INVENTION

[0070] The present invention will be further described in detail below in conjunction with the drawings and specific embodiments:

[0071] As Figure 1 shown, the underwater tightly-coupled navigation method based on centralized filtering for SINS / DVL / USBL provided by the specific embodiment of the present invention is as follows;

[0072] Step 1. Select the state variables of the SINS / DVL / USBL navigation algorithm and construct the state equation of centralized filtering;

[0073] Step 2. Convert and calculate the original beam frequency shift information in the DVL coordinate system according to the carrier velocity information output by the SINS, and convert and calculate the angle measurement information and slant range information in the USBL acoustic array coordinate system according to the carrier position information and the position information of the transponder;

[0074] Step 3. On the basis of Steps 1 and 2, construct the SINS / DVL tightly-coupled measurement equation based on frequency shift measurement and the SINS / USBL tightly-coupled measurement equation based on relative measurement information as the measurement of centralized filtering;

[0075] Step 4. Design the detection and processing mechanism for DVL and USBL faults, and design to isolate the DVL and USBL fault data and update the measurement of centralized filtering in Step 3.

[0076] The implementation of Step 1 is as follows:

[0077] Select the traditional 15-dimensional inertial navigation system error as the error state quantity of the SINS subsystem:

[0078]

[0079] wherein, δV n =[δV E δV N δV U T are the velocity errors in the east, north, and up directions respectively; φ n =[φ E φ N φ U T are the attitude errors in the east, north, and up directions respectively; δP n =[δL δλ δh] T are the latitude error, longitude error, and altitude error of the inertial navigation respectively; are the zero biases of the three axes of the SINS accelerometer respectively; ε = [ε x ε y ε z T are the zero drifts of the three axes of the SINS gyroscope respectively;

[0080] ​​​Select the DVL scale factor error and the DVL installation angle error as the error state variables of the DVL subsystem:

[0081] X DVL =[δθ dx δθ dy δθ dz δκ d T (2)

[0082] Wherein, δθ d =[δθ dx δθ dy δθ dz T is the installation error angle of the three coordinate axes between the DVL and the SINS; δκ d is the scale factor error of the DVL;

[0083] Select the installation error angle between the USBL and the SINS, the angle measurement error of the USBL, the ranging error coefficient, and the constant bias error of the depth gauge PS as the error state variables of the USBL subsystem:

[0084] X USBL =[δθ ux δθ uy δθ uz δαδβδκ u δb ps T (3)

[0085] Wherein, δθ u =[δθ ux δθ uy δθ uz T is the installation error angle of the three axes between the USBL and the SINS; δα is the azimuth measurement error of the USBL, δβ is the elevation angle measurement error of the USBL, δκ u is the ranging error coefficient of the USBL, δb ps is the constant bias error of the depth gauge PS;

[0086] When constructing the SINS / DVL / USBL integrated filtering system, the state variables selected are:

[0087]

[0088] Establish the state equation of the SINS / DVL / USBL tightly coupled navigation integrated filter as:

[0089]

[0090] Wherein, the system matrix F​​​​SINS is the inertial navigation system matrix, F DVL = 0 4×4 is the system matrix of the DVL subsystem, F USBL = 0 7×7 is the system matrix of the USBL subsystem, and W is the correlation noise matrix.

[0091] The specific implementation steps of Step 2 are as follows:

[0092] Using the velocity information of the navigation system measured by the SINS, calculate the frequency shift values in the four-beam directions as:

[0093]

[0094] In the formula, f 0 is the acoustic wave emission frequency, c is the sound speed in water, α dvl = 70°, f d is the true value of the frequency shift, is the inertial navigation attitude matrix, V n is the velocity in the navigation coordinate system;

[0095] According to the known transponder position P t e and the AUV vehicle position solved by the SINS are converted to the Earth coordinate system Calculate the azimuth angle of the transponder relative to the transponder installed on the vehicle and the slant range

[0096]

[0097] In the formula, [α β ρ] T is the true value of the azimuth angle and the slant range,

[0098] In the formula, is the attitude transformation matrix between the acoustic array coordinate system and the vehicle coordinate system, is the attitude transformation matrix between the navigation coordinate system and the Earth coordinate system,

[0099]

[0100]

[0101] The coordinates of the transponder in the u system L, λ, h are the latitude, longitude and altitude of the vehicle output by the SINS, R N is the radius of curvature of the prime vertical, and e is the first eccentricity.

[0102] The specific implementation steps of Step 3 are as follows:

[0103] As Figure 2 shown, for the Doppler frequency shifts in the four beam directions of the DVL transducer, the difference between the frequency shift value calculated by the SINS and the actual frequency shift of the DVL is used as the measurement value, and the SINS / DVL tightly coupled measurement equation based on the frequency shift measurement:

[0104]

[0105] The observation matrix is:

[0106]

[0107] As Figure 3 shown, the USBL obtains the azimuth angle and slant range based on the four hydrophones on the acoustic array, and the differences between the azimuth angle and slant range information calculated by converting the inertial navigation position information output by the SINS and the azimuth angle and slant range information output by the USBL are used as the measurement quantities:

[0108]

[0109] In the formula,

[0110] The difference between the altitude information output by the SINS and the depth information of the PS is used as the altitude information measurement:

[0111] Z PS =δh - δb ps (11)

[0112] The SINS / USBL tightly coupled measurement equation based on the relative measurement information:

[0113]

[0114] The measurement matrix is:

[0115]

[0116] The measurement quantities of the constructed SINS / DVL / USBL tightly coupled navigation system can be selected as:

[0117]

[0118] The corresponding constructed measurement equation is

[0119] Z = HX + V (15)

[0120] In the formula, the measurement matrix is V is the measurement noise correlation term.

[0121] The specific implementation steps 4 are as follows:

[0122] Adopt χ 2 test method to determine whether the measured values of DVL and USBL are abnormal.

[0123] Set the false alarm rate to 0.005 to determine the threshold T D = 14.86. When it is judged that there are invalid data and outliers in DVL, data isolation processing is adopted, and the measurement equation of the system is simplified to:

[0124] Z = Z S / U = H S / U X 26×1 + V (16)

[0125] Set the false alarm rate to 0.005 to determine the threshold T U = 12.84. When outliers appear in USBL due to environmental influence, data isolation processing is adopted, and the measurement equation of the system is simplified to:

[0126] Z = Z S / D = H S / D X 26×1 + V (17)

[0127] Overall process:

[0128] In the process of obtaining navigation and positioning information through integrated filtering of SINS / DVL / USBL, the main error sources of the three navigation devices are selected as the state variables of the filter to achieve calibration, and a state model is constructed; the frequency shift information of DVL and the relative measurement information of USBL are calculated based on the navigation information output by SINS, and then the frequency shift information measurement of the four-beam direction of SINS / DVL and the relative position information measurement of SINS / USBL are obtained, and a measurement model is constructed; the measurement information of DVL and USBL is subjected to fault detection, and the measurement model of the integrated filter is updated according to the fault situation; finally, SINS is calibrated based on the obtained state error estimation to obtain the speed, attitude and position information calculated by the navigation algorithm.

[0129] The beneficial effects of the present invention are verified through the following simulation:

[0130] Designed an AUV to perform a uniform lawn mower trajectory at a speed of 4 m / s on the horizontal plane, with a simulation time of 2185 s. In order to ensure the verification of the algorithm positioning accuracy, various errors are shielded, and the initial attitude angles are all set to 0, without initial velocity error, position error and swaying motion. The constant drift of the gyroscope is set to 0.02° / h, and the random walk is The constant drift of the accelerometer is set to 50 μg, and the random walk is The update frequency is 200 Hz; the DVL scale factor error is 0.5%, and the installation angle error is [0.3° -0.2° 0.6°]; the USBL azimuth error is 0.1°, the slant range error coefficient is 0.5%, and the installation angle error is [0.5° -0.5° 0.5°]; the PS constant bias is 0.05 m, and the random bias is 0.01 m. The simulation results are as Figure 4 shown.

[0131] It can be Figure 4 seen that, compared with the federated filtering structure, the underwater tightly-coupled navigation method of SINS / DVL / USBL based on centralized filtering has smaller horizontal positioning error and higher positioning accuracy.

[0132] The above are only the preferred embodiments of the present invention, and do not constitute any other form of limitation to the present invention. Any modification or equivalent change made according to the technical essence of the present invention still belongs to the scope protected by the present invention.

Claims

1. An underwater tightly-coupled navigation method for SINS / DVL / USBL based on centralized filtering, comprising the following steps: Characterized in that: Step 1. Select the state variables of the SINS / DVL / USBL navigation algorithm and construct the state equation of centralized filtering; Step 2. Convert and calculate the original beam frequency shift information in the DVL coordinate system according to the carrier velocity information output by the SINS, and convert and calculate the angle measurement information and slant range information in the USBL acoustic array coordinate system according to the carrier position information and the position information of the transponder; The specific steps of Step 2 are as follows: Using the velocity information of the navigation system measured by the SINS, calculate the frequency shift values in the four-beam directions as: where f 0 is the acoustic wave emission frequency, c is the sound speed in water, α dvl = 70°, f d is the true value of the frequency shift, is the inertial navigation attitude matrix, and V n is the velocity in the navigation coordinate system; According to the known transponder position The AUV vehicle position solved by SINS is converted to the Earth coordinate system Calculate the azimuth angle of the transponder relative to the transponder installed on the vehicle And the slant range where [α β ρ] T are the true values of the azimuth angle and the slant range, wherein, is the attitude transformation matrix between the acoustic array coordinate system and the vehicle coordinate system, is the attitude transformation matrix between the navigation coordinate system and the earth coordinate system, The coordinates of the transponder in the u system L, λ, and h are the latitude, longitude, and altitude of the carrier output by the SINS, and R N is the radius of curvature of the prime vertical, and e is the first eccentricity; Step 3. On the basis of Step 1 and Step 2, construct the SINS / DVL tightly-coupled measurement equation based on frequency shift measurement and the SINS / USBL tightly-coupled measurement equation based on relative measurement information as the measurement of centralized filtering; Step 4. Design the detection and processing mechanism for DVL and USBL faults, and design to isolate the DVL and USBL fault data and update the measurement of centralized filtering in Step 3.

2. An underwater tightly-coupled navigation method for SINS / DVL / USBL based on centralized filtering according to claim 1, Characterized in that, The specific steps of Step 1 are as follows: Select the traditional 15-dimensional inertial navigation system error as the error state quantity of the SINS subsystem: X SINS = [δV n φ n δP n ▽ε] T (1) where δV n = [δV E δV N δV U T are the velocity errors in the east, north, and up directions respectively; φ n = [φ E φ N φ U T are the attitude errors in the east, north, and up directions respectively; δP n = [δL δλ δh] T are the latitude error, longitude error, and altitude error of the inertial navigation respectively; ▽ = [▽ x ▽ y ▽ z T are the zero biases of the three axes of the SINS accelerometer respectively; ε = [ε x ε y ε z T are the zero drifts of the three axes of the SINS gyroscope respectively;​​​​ Select the DVL scale factor error and the DVL installation angle error as the error state quantity of the DVL subsystem: X DVL = [δθ dx δθ dy δθ dz δκ d T (2)​ where δθ d = [δθ dx δθ dy δθ dz T is the installation error angles of the three coordinate axes between the DVL and the SINS; δκ d is the scale factor error of the DVL;​ Select the installation error angle between the USBL and the SINS, the angle measurement error of the USBL, the ranging error coefficient, and the constant bias error of the depth gauge PS as the error state quantity of the USBL subsystem: X USBL = [δθ ux δθ uy δθ uz δαδβδκ u δb ps T (3)​ where δθ u = [δθ ux δθ uy δθ uz T are the installation error angles of three axes between USBL and SINS; δα is the azimuth measurement error of USBL, δβ is the elevation angle measurement error of USBL, δκ u is the ranging error coefficient of USBL, δb ps is the constant bias error of the depth gauge PS;​ When constructing the SINS / DVL / USBL centralized filtering system, select the state variables as: Establish the SINS / DVL / USBL tightly-coupled navigation centralized filtering state equation as: In the formula, the system matrix F SINS is the inertial navigation system matrix, and F DVL = 0 4×4 is the system matrix of the DVL subsystem, and F USBL = 0 7×7 is the system matrix of the USBL subsystem, and W is the correlation noise matrix.

3. An underwater tightly-coupled navigation method for SINS / DVL / USBL based on centralized filtering according to claim 1, Characterized in that, The specific steps of Step 3 are as follows: The frequency shift value calculated by SINS and the actual frequency shift of DVL The difference is used as the measurement value, and the SINS / DVL tightly coupled measurement equation based on the frequency shift measurement is as follows: The observation matrix is: Take the difference between the azimuth angle and slant range information calculated by converting the inertial navigation position information output by the SINS and the azimuth angle and slant range information output by the USBL as the measurement quantity: In the formula, Take the difference between the height information output by the SINS and the depth information of the PS as the height information measurement: Z PS = δh - δb ps (11) The SINS / USBL tightly-coupled measurement equation based on relative measurement information: The measurement matrix is: The measurement quantity of the constructed SINS / DVL / USBL tightly-coupled navigation system can be selected as: The corresponding constructed measurement equation is Z = HX + V (15) where the measurement matrix is V is the measurement noise correlation term.

4. An underwater tightly-coupled navigation method for SINS / DVL / USBL based on centralized filtering according to claim 1, Characterized in that, The specific steps of Step 4 are as follows: Use the χ 2 test method to determine whether the measured values of DVL and USBL are abnormal; When it is judged that there are invalid data and outliers in the DVL, take data isolation processing, and the measurement equation of the system is simplified to: Z = Z S / U = H S / U X 26×1 + V (16) When outliers appear in the USBL due to environmental influence, take data isolation processing, and the measurement equation of the system is simplified to: Z = Z S / D = H S / D X 26×1 + V(17).

Images