Multi-sensor underwater navigation positioning method and system based on extended Kalman filtering

Abstract

The invention relates to the technical field of underwater navigation and positioning, and discloses a multi-sensor underwater navigation and positioning method and system based on extended Kalman filtering, and the method comprises the steps: collecting the acceleration of underwater equipment through an inertial measurement unit, obtaining the longitudinal speed and transverse speed of the underwater equipment through a Doppler velocimeter, acquiring position information of the underwater equipment through the ultra-short baseline system; preprocessing the acquired data to obtain a data type required by extended Kalman filtering; position estimation, prediction and updating are carried out through the preprocessed data of the inertial measurement unit and the Doppler velocimeter; the position information of the underwater equipment of the ultra-short baseline system is preprocessed, and then observation values of the inertial measurement unit and the Doppler velocimeter are corrected; and on the basis of the corrected observation value, the updated information is predicted in an optimized manner, and an optimal state estimation value is obtained. Error accumulation of the inertial navigation system is effectively inhibited, and long-term positioning precision is improved.

Description

Technical Field

[0001] The present invention relates to the field of underwater navigation and positioning technology, and more particularly to a high-precision underwater navigation method and system based on extended Kalman filtering and fusion of multi-source sensor data. Background Art

[0002] In traditional underwater navigation technology, inertial navigation systems (INS) can provide high-frequency position, velocity, and attitude estimates, but their reliance on integration and accumulation can lead to cumulative error drift over long periods of operation. Doppler velocimeters (DVLs) can provide velocity information to aid in INS corrections, but their reliability significantly decreases in areas with heavy sediment and poor bottom reflectivity. Ultra-short baseline systems (USBLs) can provide intermittent absolute position information, but their frequency is low and their accuracy is significantly affected by the environment. Existing systems struggle to balance high-frequency dynamic response with low-frequency drift correction, and multi-source data fusion still suffers from issues such as imperfect filtering models and inadequate observation delay processing.

[0003] Therefore, developing a navigation and positioning method that can efficiently integrate inertial navigation, DVL and USBL data is of great significance for improving the real-time performance and accuracy of underwater navigation. Summary of the Invention

[0004] In light of this, the present invention provides a multi-sensor underwater navigation and positioning method and system based on an extended Kalman filter. This method uses the robot's geophysical position and heading angle to define a simplified linear equation for the system. The accurate position information obtained by USBL is used to correct the position information derived through multiple integration, which has large errors. The USBL observations are then combined to optimize the system's positioning accuracy. This method solves the existing problem of inaccurate underwater robot positioning due to the cumulative error generated by integration, significantly improving the underwater robot's positioning accuracy.

[0005] In order to achieve the above object, the present invention adopts the following technical solutions:

[0006] A multi-sensor underwater navigation and positioning method based on extended Kalman filtering, comprising:

[0007] The acceleration of the underwater device is collected through the inertial measurement unit, the longitudinal and lateral velocities of the underwater device are obtained through the Doppler velocimeter, and the position information of the underwater device is obtained through the ultra-short baseline system;

[0008] Preprocessing the collected data to obtain the data type required by the extended Kalman filter;

[0009] Position estimation and prediction updates are performed using pre-processed inertial measurement unit and Doppler velocimeter data;

[0010] The position information of the underwater equipment of the ultra-short baseline system is pre-processed to correct the observation values of the inertial measurement unit and the Doppler velocimeter;

[0011] The updated information is optimized based on the revised observations to obtain the optimal state estimate.

[0012] Preferably, the pretreatment step specifically includes:

[0013] Integrate and double-integrate the acceleration of the underwater device collected by the inertial measurement unit to obtain the position information of the underwater device, including the north position and the east position;

[0014] The velocity collected by the Doppler velocimeter in the local coordinate system is mapped to the global coordinate system, and its rotation matrix is:

[0015]

[0016] Among them, v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater device, θ is the compass heading angle, and v is the velocity in the global coordinate system;

[0017] The position information of underwater equipment obtained by the ultra-short baseline system is processed to obtain the geophysical position.

[0018] Preferably, position estimation and prediction update are performed using pre-processed inertial measurement unit and Doppler velocimeter data, including:

[0019] (1) The state vector x of the system is determined based on the position information of the underwater equipment obtained by the preprocessed inertial measurement unit and the longitudinal velocity and lateral velocity of the underwater equipment obtained by the Doppler velocimeter: x = [N, E, θ, v x ,v y ,b θ ] T

[0020] Where N is the geophysical north position of the underwater equipment, E is the geophysical east position of the underwater equipment, θ is the compass heading angle, and v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater equipment, b θ is the compass zero deflection angle;

[0021] (2) The prior estimate of the state vector x is calculated based on the velocity of the global coordinate system. The calculation formula of the prior estimate is as follows:

[0022]

[0023] Where, is the prior estimate of the system state vector, is the optimal estimate at the previous moment, B is the interference matrix of the system, u k-1 is the disturbance vector of the system, ω k is the noise of the system in the prediction stage;

[0024] After simplifying and combining the above formulas, we get:

[0025]

[0026] Where θ is the compass heading angle, v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater equipment, and t is the update time of the system;

[0027] (3) Determine the Jacobian matrix of the system:

[0028]

[0029] Where:

[0030]

[0031] Compute the prior estimated covariance based on the Jacobian matrix:

[0032]

[0033] Where, is the prior estimate of the covariance, is the value of the optimal covariance at the previous moment, Q is the process noise of the system and it obeys the normal distribution. The value of Q is as follows:

[0034]

[0035] Where n N is the noise at the north position, n E is the eastward position noise, n θ is the heading angle noise, is the longitudinal position noise, is the lateral position noise, is the compass bias noise;

[0036] (4) Determine the Kalman gain of the system based on the prior estimated covariance:

[0037]

[0038] Where K k is the Kalman gain, H is the observation matrix of the system, and its value is:

[0039]

[0040] The data obtained by the inertial measurement unit and Doppler velocimeter are used to calculate the compass heading angle θ and the compass zero deviation angle b. θ , longitudinal velocity v of the underwater equipment x and the lateral velocity v of the underwater device y Observe the value of

[0041] R is the observation noise, and its value is:

[0042]

[0043] (5) Update the state vector and covariance. The obtained value is the optimal estimate for each update. The iterative formula is as follows:

[0044]

[0045] Where, is the optimal covariance obtained for each update, is the optimal state vector obtained for each update, I is the unit matrix, z k is the observation vector of the system, and its expression is as follows:

[0046]

[0047] Where G c is the Gaussian noise of the compass, and is the Gaussian noise of the longitudinal and lateral velocities measured by the Doppler velocimeter.

[0048] Preferably, the position information of the underwater device of the ultra-short baseline system is pre-processed to correct the observation values of the inertial measurement unit and the Doppler velocimeter, including:

[0049] Adding the geophysical position obtained by the ultra-short baseline system, the revised observation matrix is:

[0050]

[0051] The corrected observation noise is:

[0052]

[0053] The corrected observation vector is:

[0054]

[0055] in, and is the Gaussian noise of the northing and easting positions measured by the ultra-short baseline system, N′ is the geophysical northing position measured by the ultra-short baseline system, and E′ is the geophysical easting position measured by the ultra-short baseline system;

[0056] The low-frequency observations of the ultra-short baseline system are timestamp aligned and abnormal data are removed.

[0057] Preferably, it also includes:

[0058] The optimal state estimation value obtained from each correction is stored in sequence.

[0059] A multi-sensor underwater navigation and positioning system based on extended Kalman filtering, comprising:

[0060] Data acquisition module: The inertial measurement unit is used to collect the acceleration of the underwater device, the Doppler velocimeter is used to obtain the longitudinal and lateral velocities of the underwater device, and the ultra-short baseline system is used to obtain the position information of the underwater device;

[0061] Data processing module: pre-processes the collected data to obtain the data type required by the extended Kalman filter;

[0062] Extended Kalman filter module: uses pre-processed inertial measurement unit and Doppler velocimeter data to estimate position and predict updates;

[0063] Positioning correction module: This module pre-processes the position information of the underwater equipment of the ultra-short baseline system and corrects the observation values of the inertial measurement unit and Doppler velocimeter;

[0064] The updated information is optimized based on the revised observations to obtain the optimal state estimate.

[0065] Preferably, the specific processing process of the data processing module is:

[0066] Integrate and double-integrate the acceleration of the underwater device collected by the inertial measurement unit to obtain the position information of the underwater device, including the north position and the east position;

[0067] The velocity collected by the Doppler velocimeter in the local coordinate system is mapped to the global coordinate system, and its rotation matrix is:

[0068]

[0069] Among them, v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater device, θ is the compass heading angle, and v is the velocity in the global coordinate system;

[0070] The position information of underwater equipment obtained by the ultra-short baseline system is processed to obtain the geophysical position.

[0071] Preferably, the specific processing process of the extended Kalman filter module is:

[0072] (1) The state vector x of the system is determined based on the position information of the underwater equipment obtained by the preprocessed inertial measurement unit and the longitudinal velocity and lateral velocity of the underwater equipment obtained by the Doppler velocimeter: x = [N, E, θ, v x ,v y ,b θ ] T

[0073] Where N is the north position collected by the inertial measurement unit, E is the east position collected by the inertial measurement unit, θ is the compass heading angle, and v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater equipment, b θ is the compass zero deflection angle;

[0074] (2) The prior estimate of the state vector x is calculated based on the velocity of the global coordinate system. The calculation formula of the prior estimate is as follows:

[0075]

[0076] Where, is the prior estimate of the system state vector, is the optimal estimate at the previous moment, B is the interference matrix of the system, u k-1 is the disturbance vector of the system, ω k is the noise of the system in the prediction stage.

[0077] After simplifying and combining the above formulas, we get:

[0078]

[0079] Where θ is the compass heading angle, v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater equipment, and t is the update time of the system.

[0080] (3) Determine the Jacobian matrix of the system:

[0081]

[0082] Where:

[0083]

[0084] Compute the prior estimated covariance based on the Jacobian matrix:

[0085]

[0086] Where, is the prior estimate of the covariance, is the value of the optimal covariance at the previous moment, Q is the process noise of the system and it obeys the normal distribution. The value of Q is as follows:

[0087]

[0088] Where n N is the noise at the north position, n E is the eastward position noise, n θ is the heading angle noise, is the longitudinal position noise, is the lateral position noise, is the compass bias noise;

[0089] (4) Determine the Kalman gain of the system based on the prior estimated covariance:

[0090]

[0091] Where K k is the Kalman gain, H is the observation matrix of the system, and its value is:

[0092]

[0093] The data obtained by the inertial measurement unit and Doppler velocimeter are used to calculate the compass heading angle θ and the compass zero deviation angle b. θ , longitudinal velocity v of the underwater equipment x and the lateral velocity v of the underwater device y Observe the value of

[0094] R is the observation noise, and its value is:

[0095]

[0096] (5) Update the state vector and covariance. The obtained value is the optimal estimate for each update. The iterative formula is as follows:

[0097]

[0098] Where, is the optimal covariance obtained for each update, is the optimal state vector obtained for each update, I is the unit matrix, z k is the observation vector of the system, and its expression is as follows:

[0099]

[0100] Where G c is the Gaussian noise of the compass, and is the Gaussian noise of the longitudinal and lateral velocities measured by the Doppler velocimeter.

[0101] Preferably, the position information of the underwater device of the ultra-short baseline system is pre-processed to correct the observation values of the inertial measurement unit and the Doppler velocimeter, including:

[0102] Adding the geophysical position obtained by the ultra-short baseline system, the revised observation matrix is:

[0103]

[0104] The corrected observation noise is:

[0105]

[0106] The corrected observation vector is:

[0107]

[0108] in, and is the Gaussian noise of the northing and easting positions measured by the ultra-short baseline system, N′ is the geophysical northing position measured by the ultra-short baseline system, and E′ is the geophysical easting position measured by the ultra-short baseline system;

[0109] The low-frequency observations of the ultra-short baseline system are timestamp aligned and abnormal data are removed.

[0110] Preferably, it also includes:

[0111] Data storage module: stores the optimal state estimation value obtained from each correction in sequence.

[0112] It can be seen from the above technical solutions that, compared with the prior art, the present invention discloses a multi-sensor underwater navigation and positioning method and system based on extended Kalman filtering, which has the following advantages:

[0113] 1) Effectively suppress the accumulation of inertial navigation system errors and improve long-term positioning accuracy;

[0114] 2) Robust handling of USBL intermittent and delayed observations is achieved, improving system stability;

[0115] 3) Integrate multi-source heterogeneous information to adapt to complex underwater environments and achieve high-reliability navigation;

[0116] 4) The system structure is expandable, making it easy to integrate other underwater sensors and platforms. BRIEF DESCRIPTION OF THE DRAWINGS

[0117] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0118] Figure 1 This is a flow chart of a multi-sensor underwater navigation and positioning method based on extended Kalman filtering in the present invention.

[0119] Figure 2 For extended Kalman filter data prediction, update flow chart.

[0120] Figure 3 This is the motion trajectory of the underwater robot under ideal conditions.

[0121] Figure 4 Trajectory plot generated for the optimal estimate of the extended Kalman filter output when there are no USBL observations.

[0122] Figure 5 Trajectory map generated for observation points observed by USBL when USBL observation is available.

[0123] Figure 6 Comparison diagram of the ideal trajectory, USBL observation trajectory and extended Kalman filter output trajectory with USBL observation.

[0124] Figure 7 This is a block diagram of a multi-sensor underwater navigation and positioning system based on extended Kalman filtering in the present invention. DETAILED DESCRIPTION

[0125] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0126] The embodiment of the present invention discloses a multi-sensor underwater navigation and positioning method based on extended Kalman filtering, such as Figure 1 As shown, including:

[0127] The acceleration of the underwater device is collected through the inertial measurement unit, the longitudinal and lateral velocities of the underwater device are obtained through the Doppler velocimeter, and the position information of the underwater device is obtained through the ultra-short baseline system;

[0128] Preprocessing the collected data to obtain the data type required by the extended Kalman filter;

[0129] Position estimation and prediction updates are performed using pre-processed inertial measurement unit and Doppler velocimeter data;

[0130] The position information of the underwater equipment of the ultra-short baseline system is pre-processed to correct the observation values of the inertial measurement unit and the Doppler velocimeter;

[0131] The updated position is optimized based on the corrected observations to obtain the optimal position.

[0132] The present invention defines the simplified linear equations of the system using the robot's geophysical position and heading angle, and uses the accurate position information obtained by USBL to correct the position information with large errors derived through multiple integration. The positioning accuracy of the system is optimized in combination with the observation values of USBL. The robot position calculated by the sensor with lower confidence is compared with the precise position information obtained by USBL measurement with higher confidence, and the predicted position of the robot is dynamically corrected to obtain the current optimal estimated position. This solves the problem of the existing technology that the underwater robot positioning is not accurate due to the cumulative error caused by integration, and significantly improves the positioning accuracy of the underwater robot.

[0133] In this embodiment, the pre-processing step specifically includes:

[0134] The acceleration of the underwater device collected by the inertial measurement unit (IMU) is integrated and double-integrated to obtain the position information of the underwater device, including the north position and the east position. Specifically, the double integration of the three-axis acceleration obtained by the IMU can obtain the three-axis distance difference between the current position and the previous position. Combined with the gyroscope of the IMU itself, the current position can be calculated based on the previous position. By iterating in this way, the unknown current position can be calculated from the known initial position, thereby obtaining the east position E and north position N of the underwater robot.

[0135] The velocity collected by the Doppler velocimeter in the local coordinate system is mapped to the global coordinate system and used as the observation value to update the prior estimate of the Kalman filter. The rotation matrix is:

[0136]

[0137] Among them, v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater device, θ is the compass heading angle, and v is the velocity in the global coordinate system;

[0138] The location information of the underwater equipment obtained by the ultra-short baseline system is processed to obtain the geophysical position, including the north position N' and the east position E'. These two data can be used to determine the absolute position of the underwater equipment. Specifically, the USBL device includes a transmitter and a transponder. The transmitter is placed on the mother ship and can obtain accurate positioning through GPS; the transponder is placed on the underwater equipment body and is used to receive the acoustic signal emitted by the transmitter. The two can measure the Euclidean distance between the two and the azimuth of the transponder relative to the mother ship through this acoustic signal. The Euclidean distance R and azimuth γ of the underwater equipment relative to the mother ship collected by USBL can accurately locate the current geophysical position of the robot.

[0139] In this embodiment, the position is estimated and updated by using the pre-processed inertial measurement unit and Doppler velocimeter data, such as Figure 2 Shown, including:

[0140] (1) The state vector x of the system is determined based on the position information of the underwater equipment obtained by the preprocessed inertial measurement unit and the longitudinal velocity and lateral velocity of the underwater equipment obtained by the Doppler velocimeter: x = [N, E, θ, v x ,v y ,b θ ] T

[0141] Where N is the geophysical north position of the underwater equipment, E is the geophysical east position of the underwater equipment, θ is the compass heading angle, and v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater equipment, b θ is the compass zero deflection angle;

[0142] (2) The prior estimate of the state vector x is calculated based on the velocity of the global coordinate system. The calculation formula of the prior estimate is as follows:

[0143]

[0144] Where, is the prior estimate of the system state vector, is the optimal estimate at the previous moment, B is the interference matrix of the system, u k-1 is the disturbance vector of the system, ω k is the noise of the system in the prediction stage.

[0145] After simplifying and combining the above formulas, we get:

[0146]

[0147] Where θ is the compass heading angle, v xis the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater equipment, and t is the update time of the system.

[0148] (3) Determine the Jacobian matrix of the system:

[0149]

[0150] In the formula

[0151]

[0152] Compute the a priori estimated covariance:

[0153]

[0154] Where, is the prior estimate of the covariance, is the value of the optimal covariance at the previous moment, F is the Jacobian matrix of the system, Q is the process noise of the system and it obeys the normal distribution, and the value of Q is as follows:

[0155]

[0156] Where n N is the noise at the north position, n E is the eastward position noise, n θ is the heading angle noise, is the longitudinal position noise, is the lateral position noise, is the compass bias noise, and the noise here is artificially set.

[0157] (4) Determine the Kalman gain of the system based on the prior estimated covariance:

[0158]

[0159] Where H is the observation matrix of the system, and its value is:

[0160]

[0161] The data obtained by IMU and DVL, the heading angle θ, the compass zero deflection angle b θ , longitudinal velocity v x , lateral velocity v y Observe the value of .

[0162] R is the observation noise, and its value is:

[0163]

[0164] (5) Update the state vector and covariance. The obtained value is the optimal estimate for each update. The iterative formula is as follows:

[0165]

[0166] Where, is the optimal covariance obtained for each update, is the optimal state vector obtained for each update, I is the unit matrix, z k is the observation vector of the system, and its expression is as follows:

[0167]

[0168] Where G c is the Gaussian noise of the compass, and is the Gaussian noise of the longitudinal and lateral velocities measured by DVL.

[0169] The motion model of the system simulation is to change the heading angle at a stable speed while the device is running at a constant speed, such as Figure 3 As shown in , it is the ideal trajectory of the equipment operation without external interference. Figure 4 As shown in Figure 2, the red line is the trajectory generated by the optimal estimate of the system without USBL correction.

[0170] In this embodiment, the position information of the underwater equipment of the ultra-short baseline system is pre-processed to correct the observation values of the inertial measurement unit and the Doppler velocimeter, including:

[0171] (1) After adding the geophysical position information N′ and E′ observed by USBL, as Figure 5 As shown, the corrected observation matrix is:

[0172]

[0173] At the heading angle θ, the compass zero deflection angle b θ , longitudinal velocity v x , lateral velocity v y Based on the observation of the value of , the observation vectors of the north position N′ and the east position E′ are added.

[0174] The corrected observation noise is:

[0175]

[0176] The corrected observation vector becomes:

[0177]

[0178] Where, and is the Gaussian noise of the northing and easting positions measured by USBL, N′ is the geophysical northing position measured by USBL, and E′ is the geophysical easting position measured by USBL.

[0179] (2) Perform timestamp alignment on the low-frequency observations of USBL. For IMU and DVL, they are high-frequency signals, so let’s assume their frequencies are 10Hz, while USBL is a low-frequency signal, so let’s assume its frequency is 0.5Hz. Then, for every 20 updates of IMU and DVL data, USBL data is updated once. Figure 6 The red coordinate points in the figure are the optimal values of the extended Kalman filter. The data in the figure are corrected every 20 coordinate points.

[0180] (3) Eliminate outliers to improve the robustness of the system. For the abnormal data obtained by USBL, the abnormal data is eliminated and the IMU and DVL data are updated accordingly based on the number of eliminated data.

[0181] In this embodiment, it also includes:

[0182] The optimal position value obtained from each correction is stored in order, including:

[0183] The optimal state estimate obtained after each state update is continuously stored in chronological order, including information such as position, velocity, and heading. Based on this stored, time-stamped sequence of optimal state estimates, a motion trajectory diagram of the underwater robot is automatically generated and output. This stored data provides fundamental data support for subsequent trajectory playback, performance analysis, and algorithm optimization.

[0184] The embodiment of the present invention provides a multi-sensor underwater navigation and positioning system based on extended Kalman filtering, such as Figure 7 Shown, including:

[0185] Data acquisition module: The inertial measurement unit is used to collect the acceleration of the underwater device, the Doppler velocimeter is used to obtain the longitudinal and lateral velocities of the underwater device, and the ultra-short baseline system is used to obtain the position information of the underwater device;

[0186] Data processing module: pre-processes the collected data to obtain the data type required by the extended Kalman filter;

[0187] Extended Kalman filter module: uses pre-processed inertial measurement unit and Doppler velocimeter data to estimate position and predict updates;

[0188] Positioning correction module: This module pre-processes the position information of the underwater equipment of the ultra-short baseline system and corrects the observation values of the inertial measurement unit and Doppler velocimeter;

[0189] The updated information is optimized based on the revised observations to obtain the optimal state estimate.

[0190] In this embodiment, a data storage module is further included: storing the optimal state estimation value obtained from each correction in sequence.

[0191] The specific implementation process of each module is consistent with the method part and will not be repeated here. Please refer to the method part.

[0192] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Reference can be made to the common and similar parts between the various embodiments. For the devices disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple, and the relevant parts can be referred to the method description.

[0193] The above description of the disclosed embodiments is intended to enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention is not limited to the embodiments shown herein but is intended to conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A multi-sensor underwater navigation and positioning method based on extended Kalman filtering, characterized in that: include: The acceleration of the underwater device is collected through the inertial measurement unit, the longitudinal and lateral velocities of the underwater device are obtained through the Doppler velocimeter, and the position information of the underwater device is obtained through the ultra-short baseline system; Preprocessing the collected data to obtain the data type required by the extended Kalman filter; Position estimation and prediction updates are performed using pre-processed inertial measurement unit and Doppler velocimeter data; The position information of the underwater equipment of the ultra-short baseline system is pre-processed to correct the observation values of the inertial measurement unit and the Doppler velocimeter; The updated information is optimized based on the revised observations to obtain the optimal state estimate.

2. The multi-sensor underwater navigation and positioning method based on extended Kalman filtering according to claim 1, characterized in that: The preprocessing steps specifically include: Integrate and double-integrate the acceleration of the underwater device collected by the inertial measurement unit to obtain the position information of the underwater device, including the north position and the east position; The velocity collected by the Doppler velocimeter in the local coordinate system is mapped to the global coordinate system, and its rotation matrix is: Among them, v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater device, θ is the compass heading angle, and v is the velocity in the global coordinate system; The position information of underwater equipment obtained by the ultra-short baseline system is processed to obtain the geophysical position.

3. The multi-sensor underwater navigation and positioning method based on extended Kalman filtering according to claim 2, characterized in that: Position estimation and prediction updates using pre-processed inertial measurement unit and Doppler velocimeter data, including: (1) The state vector x of the system is determined based on the position information of the underwater equipment obtained by the preprocessed inertial measurement unit and the longitudinal velocity and lateral velocity of the underwater equipment obtained by the Doppler velocimeter: x = [N, E, θ, v x ,v y ,b θ ] T Where N is the geophysical north position of the underwater equipment, E is the geophysical east position of the underwater equipment, θ is the compass heading angle, and v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater equipment, b θ is the compass zero deflection angle; (2) The prior estimate of the state vector x is calculated based on the velocity of the global coordinate system. The calculation formula of the prior estimate is as follows: Where, is the prior estimate of the system state vector, is the optimal estimate at the previous moment, B is the interference matrix of the system, u k-1 is the disturbance vector of the system, ω k is the noise of the system in the prediction stage; After simplifying and combining the above formulas, we get: Where t is the update time of the system; (3) Determine the Jacobian matrix of the system: Where: Compute the prior estimated covariance based on the Jacobian matrix: Where, is the prior estimate of the covariance, is the value of the optimal covariance at the previous moment, Q is the process noise of the system and it obeys the normal distribution. The value of Q is as follows: Where n N is the noise at the north position, n E is the eastward position noise, n θ is the heading angle noise, is the longitudinal position noise, is the lateral position noise, is the compass bias noise; (4) Determine the Kalman gain of the system based on the prior estimated covariance: Where K k is the Kalman gain, H is the observation matrix of the system, and its value is: The data obtained by the inertial measurement unit and Doppler velocimeter are used to calculate the compass heading angle θ and the compass zero deviation angle b. θ , longitudinal velocity v of the underwater equipment x and the lateral velocity v of the underwater device y Observe the value of R is the observation noise, and its value is: (5) Update the state vector and covariance. The obtained value is the optimal estimate for each update. The iterative formula is as follows: Where, is the optimal covariance obtained for each update, is the optimal state vector obtained for each update, I is the unit matrix, z k is the observation vector of the system, and its expression is as follows: Where G c is the Gaussian noise of the compass, and is the Gaussian noise of the longitudinal and lateral velocities measured by the Doppler velocimeter.

4. The multi-sensor underwater navigation and positioning method based on extended Kalman filtering according to claim 3 is characterized in that: The position information of the underwater equipment of the ultra-short baseline system is pre-processed to correct the observation values of the inertial measurement unit and Doppler velocimeter, including: Adding the geophysical position obtained by the ultra-short baseline system, the revised observation matrix is: The corrected observation noise is: The corrected observation vector is: in, and is the Gaussian noise of the northing and easting positions measured by the ultra-short baseline system, N′ is the geophysical northing position measured by the ultra-short baseline system, and E′ is the geophysical easting position measured by the ultra-short baseline system; The low-frequency observations of the ultra-short baseline system are timestamp aligned and abnormal data are removed.

5. The multi-sensor underwater navigation and positioning method based on extended Kalman filtering according to claim 1, characterized in that: Also includes: The optimal state estimation value obtained from each correction is stored in sequence.

6. A multi-sensor underwater navigation and positioning system based on extended Kalman filtering, characterized in that: include: Data acquisition module: The inertial measurement unit is used to collect the acceleration of the underwater device, the Doppler velocimeter is used to obtain the longitudinal and lateral velocities of the underwater device, and the ultra-short baseline system is used to obtain the position information of the underwater device; Data processing module: pre-processes the collected data to obtain the data type required by the extended Kalman filter; Extended Kalman filter module: uses pre-processed inertial measurement unit and Doppler velocimeter data to estimate position and predict updates; Positioning correction module: This module pre-processes the position information of the underwater equipment of the ultra-short baseline system and corrects the observation values of the inertial measurement unit and Doppler velocimeter; The updated information is optimized based on the revised observations to obtain the optimal state estimate.

7. The multi-sensor underwater navigation and positioning system based on extended Kalman filtering according to claim 6, characterized in that: The specific processing process of the data processing module is as follows: Integrate and double-integrate the acceleration of the underwater device collected by the inertial measurement unit to obtain the position information of the underwater device, including the north position and the east position; The velocity collected by the Doppler velocimeter in the local coordinate system is mapped to the global coordinate system, and its rotation matrix is: Among them, v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater device, θ is the compass heading angle, and v is the velocity in the global coordinate system; The position information of underwater equipment obtained by the ultra-short baseline system is processed to obtain the geophysical position.

8. The multi-sensor underwater navigation and positioning system based on extended Kalman filtering according to claim 7, characterized in that: The specific processing process of the extended Kalman filter module is as follows: (1) The state vector x of the system is determined based on the position information of the underwater equipment obtained by the preprocessed inertial measurement unit and the longitudinal velocity and lateral velocity of the underwater equipment obtained by the Doppler velocimeter: x = [N, E, θ, v x ,v y ,b θ ] T Where N is the geophysical north position of the underwater equipment, E is the geophysical east position of the underwater equipment, θ is the compass heading angle, and v x is the longitudinal velocity of the underwater equipment, v y is the lateral velocity of the underwater equipment, b θ is the compass zero deflection angle; (2) The prior estimate of the state vector x is calculated based on the velocity of the global coordinate system. The calculation formula of the prior estimate is as follows: Where, is the prior estimate of the system state vector, is the optimal estimate at the previous moment, B is the interference matrix of the system, u k-1 is the disturbance vector of the system, ω k is the noise of the system in the prediction stage; After simplifying and combining the above formulas, we get: Where t is the update time of the system; (3) Determine the Jacobian matrix of the system: Where: Compute the prior estimated covariance based on the Jacobian matrix: Where, is the prior estimate of the covariance, is the value of the optimal covariance at the previous moment, Q is the process noise of the system and it obeys the normal distribution. The value of Q is as follows: Where n N is the noise at the north position, n E is the eastward position noise, n θ is the heading angle noise, is the longitudinal position noise, is the lateral position noise, is the compass bias noise; (4) Determine the Kalman gain of the system based on the prior estimated covariance: Where K k is the Kalman gain, H is the observation matrix of the system, and its value is: The data obtained by the inertial measurement unit and Doppler velocimeter are used to calculate the compass heading angle θ and the compass zero deviation angle b. θ , longitudinal velocity v of the underwater equipment x and the lateral velocity v of the underwater device y Observe the value of R is the observation noise, and its value is: (5) Update the state vector and covariance. The obtained value is the optimal estimate for each update. The iterative formula is as follows: Where, is the optimal covariance obtained for each update, is the optimal state vector obtained for each update, I is the unit matrix, z k is the observation vector of the system, and its expression is as follows: Where G c is the Gaussian noise of the compass, and is the Gaussian noise of the longitudinal and lateral velocities measured by the Doppler velocimeter.

9. The multi-sensor underwater navigation and positioning system based on extended Kalman filtering according to claim 8, characterized in that: The position information of the underwater equipment of the ultra-short baseline system is pre-processed to correct the observation values of the inertial measurement unit and Doppler velocimeter, including: Adding the geophysical position obtained by the ultra-short baseline system, the revised observation matrix is: The corrected observation noise is: The corrected observation vector is: in, and is the Gaussian noise of the northing and easting positions measured by the ultra-short baseline system, N′ is the geophysical northing position measured by the ultra-short baseline system, and E′ is the geophysical easting position measured by the ultra-short baseline system; The low-frequency observations of the ultra-short baseline system are timestamp aligned and abnormal data are removed.

10. The multi-sensor underwater navigation and positioning system based on extended Kalman filtering according to claim 6, characterized in that: Also includes: Data storage module: stores the optimal state estimation value obtained from each correction in sequence.

Images