Underwater integrated navigation method and system based on speed prediction

Abstract

The invention provides an underwater integrated navigation method and system based on speed prediction, and relates to the technical field of underwater navigation.The underwater integrated navigation method comprises the steps that observation data of an inertial navigation system (INS), a Doppler log (DVL) and a pressure sensor (PS) are obtained, and when the observation speed of the Doppler log (DVL) fails or meets the gross error condition, the pressure sensor (PS) is determined; predicting the observation speed based on the corrected rotating speed of the propeller; based on the observation data, calculating an IMU pre-integration factor, a DVL factor and a PS factor, constructing a factor graph, and fusing the observation data through a factor graph optimization method to obtain a final underwater vehicle state, including position coordinates, speed, attitude, accelerometer and gyroscope zero offset errors; according to the method, the pseudo DVL factor constructed by the speed predicted value replaces the DVL factor to participate in the navigation framework, so that the navigation precision divergence is effectively inhibited in the DVL failure state.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of underwater navigation, in particular to an underwater integrated navigation method and system based on speed prediction. BACKGROUND

[0002] Radio waves in water, especially in seawater, are extremely attenuated and almost cannot effectively propagate beyond a depth of several meters. This fundamental limitation has given rise to an underwater positioning, navigation and timing (PNT) system relying on acoustic technology. With the development of artificial intelligence technology, navigation and control technology, sensor technology and energy technology, autonomous underwater vehicles (AUVs) are widely used in ocean environment detection, underwater reconnaissance and search, underwater communication and navigation. An inertial navigation system (INS) is a core sensor carried by an AUV and can provide continuous, high-frequency and full-dimensional navigation information completely autonomously without relying on external signals. However, the error of the INS will quickly diverge with time accumulation, so the AUV will also carry other positioning systems and sensors such as a long baseline positioning system (LBL), an ultra-short baseline positioning system (USBL), a Doppler velocity log (DVL) and a pressure sensor (PS). However, the deployment of the LBL transponder is complex and costly. The USBL is susceptible to external interference and is not suitable for use in dynamic motion and harsh sea conditions. The DVL can observe the speed of the AUV, and the PS provides depth observation with low operation difficulty, easy to carry and lower price. Therefore, INS / DVL / PS integrated navigation has become a research hotspot in the current underwater navigation field.

[0003] The INS / DVL / PS integrated navigation method also has some disadvantages. The marine observation environment is complex, and the DVL observation is affected by reasons such as exceeding the detection range, complex seabed topography and bottom material, adverse hydrological conditions and extreme sea conditions, resulting in DVL observation failure. Once the DVL fails, the system will lose the most critical speed observation information, and the error of the inertial navigation caused by the accelerometer bias and the gyro drift cannot be effectively corrected. The direct consequence is that the navigation system degenerates into a pure inertial dead reckoning mode, and the horizontal position error begins to diverge sharply. Even in the tactical IMU, it will accumulate to an unacceptable range within a few minutes, which seriously threatens the navigation safety and mission success rate of the underwater vehicle. SUMMARY

[0004] To solve the above problems, the present application provides an underwater integrated navigation method and system based on speed prediction. A pseudo-DVL factor constructed by a speed prediction value replaces the DVL factor to participate in the navigation framework, and the effective suppression of the navigation precision divergence in the DVL failure state is realized.

[0005] According to some embodiments, the present application adopts the following technical scheme: The application discloses a speed prediction-based underwater integrated navigation method, and underwater vehicles perform underwater integrated navigation by means of an inertial navigation system (INS), a Doppler velocity log (DVL) and a pressure sensor (PS). Obtaining observation data of the inertial navigation system (INS), the Doppler velocity log (DVL) and the pressure sensor (PS), wherein when the DVL observation speed is invalid or the DVL observation speed meets a gross error condition, the observation speed is predicted based on a corrected propeller rotating speed. Based on the observation data, an IMU pre-integration factor, a DVL factor and a PS factor are calculated, a factor graph is constructed, the observation data are fused by a factor graph optimization method, and finally, underwater vehicle states including position coordinates, speed, attitude, accelerometer and gyroscope zero bias errors are obtained. The gross error condition is based on a quartile range (IQR) method, a sliding window is used in combination with the propeller rotating speed to dynamically adjust a gross error threshold, and the gross error condition is judged by the gross error threshold.

[0006] According to some embodiments, the application adopts the following technical scheme: The application discloses a speed prediction-based underwater integrated navigation system, and underwater vehicles perform underwater integrated navigation by means of an inertial navigation system (INS), a Doppler velocity log (DVL) and a pressure sensor (PS), comprising: The data acquisition module is configured to obtain observation data of the inertial navigation system (INS), the Doppler velocity log (DVL) and the pressure sensor (PS), wherein when the DVL observation speed is invalid or the DVL observation speed meets a gross error condition, the observation speed is predicted based on a corrected propeller rotating speed. The data fusion module is configured to, based on the observation data, calculate an IMU pre-integration factor, a DVL factor and a PS factor, construct a factor graph, fuse the observation data by a factor graph optimization method, and finally, obtain underwater vehicle states including position coordinates, speed, attitude, accelerometer and gyroscope zero bias errors. The gross error condition is based on a quartile range (IQR) method, a sliding window is used in combination with the propeller rotating speed to dynamically adjust a gross error threshold, and the gross error condition is judged by the gross error threshold.

[0007] According to some embodiments, the application adopts the following technical scheme: The application discloses a computer program product, comprising a computer program, which, when executed by a processor, realizes the speed prediction-based underwater integrated navigation method.

[0008] According to some embodiments, the application adopts the following technical scheme: A non-transitory computer-readable storage medium is provided for storing computer instructions, which, when executed by a processor, implement the aforementioned underwater integrated navigation method based on velocity prediction.

[0009] According to some embodiments, the present invention adopts the following technical solution: An electronic device includes a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to implement the underwater integrated navigation method based on speed prediction.

[0010] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention utilizes INS recursive attitude, IMU pre-integrated position within the sampling interval, relative changes in velocity and attitude, PS depth observations, and propeller rotation speed observations corrected by NLS (nonlinear least squares) as multi-feature inputs. A Transformer-LSTM-based velocity prediction model predicts DVL velocity, with a pseudo-DVL factor replacing the actual DVL factor in the navigation framework. Leveraging the self-attention mechanism in the Transformer, this velocity prediction model calculates the correlation between input features during velocity prediction, dynamically adjusts the weights of each feature, and effectively captures global contextual data relationships, thus effectively suppressing navigation accuracy divergence even in DVL failure states.

[0011] This invention proposes a method based on NLS to pre-correct propeller speed data, enabling it to accurately reflect the relationship between propeller speed and AUV speed under normal operating conditions. When a change in propeller power is detected, a simulation function is activated until the speed stabilizes. Such processing steps are crucial to ensuring the accuracy and reliability of the model.

[0012] This invention improves upon existing IQR methods by using a sliding window combined with propeller speed data. It achieves dynamic threshold adjustment by setting a sliding window and using propeller speed data within the window to reflect the current motion state of the carrier. The threshold is increased for complex motion states and decreased for simple motion states. Utilizing factor graph optimization of the FGO method better reduces the impact of outliers on subsequent estimations. Global optimization and correction of historical states using later information make it more robust than filtering systems. Attached Figure Description

[0013] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0014] Figure 1 This is a flowchart of the underwater integrated navigation method in Example 1. Figure 2 This is a structural diagram of the NLS-Transformer-LSTM model in Example 1.

[0015] Figure 3 This is the INS / DVL / PS diagram optimization framework for Example 1. Detailed Implementation

[0016] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0017] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0018] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0019] Table 1. Glossary of Terms

[0020] Example 1 One embodiment of the present invention provides an underwater integrated navigation method based on velocity prediction. The underwater vehicle performs underwater integrated navigation using an onboard inertial navigation system (INS), a Doppler log (DVL), and a pressure sensor (PS), including: Step S1: Acquire observation data from the inertial navigation system INS, the Doppler log (DVL), and the pressure sensor PS. When the Doppler log (DVL) fails to observe the velocity or the observed velocity meets the gross error condition, the observed velocity is predicted based on the corrected propeller speed. Step S2: Based on the observation data, calculate the IMU pre-integration factor, DVL factor and PS factor, construct a factor map, and fuse the observation data through the factor map optimization method to obtain the final underwater vehicle state, including position coordinates, velocity, attitude, accelerometer and gyroscope zero bias error; The gross error condition is based on the IQR method, which uses a sliding window combined with the propeller speed to dynamically adjust the gross error threshold, and the gross error condition is judged by the gross error threshold.

[0021] As one embodiment, the underwater integrated navigation method based on velocity prediction of the present invention uses a pseudo-DVL factor constructed from the velocity prediction value to replace the DVL factor in the navigation framework, thereby effectively suppressing the divergence of navigation accuracy in the DVL failure state. The specific implementation process is described below.

[0022] The application scenario of this embodiment is in a complex underwater environment. The inertial navigation system (INS) is the core sensor carried by the AUV. It is the core sensor because of its strong autonomy and ability to provide continuous all-dimensional navigation parameters. However, its navigation error will accumulate and diverge over time, and it must rely on external observation information for correction. Therefore, the AUV is also equipped with sensors such as the Doppler velocity log (DVL) and pressure sensor (PS). The DVL can provide high-precision velocity vectors, and the pressure sensor (PS) can provide absolute depth reference. The combined system of the two and the INS has become the mainstream solution. The error accumulation of the INS is suppressed by information fusion. The most common information fusion methods in the field of underwater navigation include Kalman filtering (EKF) and unscented Kalman filtering (UKF) variations. These methods are based on the state estimation of the previous moment and realize the real-time fusion of sensor data through a "prediction-update" recursive framework. They have the advantages of high computational efficiency and ease of engineering implementation.

[0023] Due to the complexity of the marine environment, DVL observations are subject to factors such as exceeding the detection range, complex seabed topography and sediment, unfavorable hydrological conditions, and extreme sea states, leading to DVL observation failure. To address DVL failure, this embodiment proposes a method for training and predicting DVL velocities using Transformer-LSTM model data as multi-feature inputs. This method utilizes INS recursive attitude, IMU pre-integrated position within the sampling interval, relative changes in velocity and attitude, PS depth observations, and propeller rotation speed observations corrected by NLS (nonlinear least squares). A pseudo-DVL factor replaces the DVL factor in the navigation framework, effectively suppressing navigation accuracy divergence under DVL failure conditions. This method is more accurate, stable, and durable compared to other existing methods.

[0024] The most common information fusion methods in the field of underwater navigation include Kalman filtering (EKF) and unscented Kalman filtering (UKF) variations. However, recursive filtering algorithms are essentially locally optimal, updating only based on the current state and observations. This causes early linearization errors to accumulate over time, making navigation errors prone to divergence during long-duration navigation. More importantly, EKF is extremely sensitive to outliers in the observation data. A single outlier can contaminate the entire filtered state, leading to severe and difficult-to-recover deviations in the estimation results. Finally, EKF strictly relies on sequential data processing, making it difficult to flexibly incorporate asynchronous and multi-rate underwater observation constraints. This makes its adaptability, accuracy, and robustness in complex underwater environments inferior to factor graph methods, which can perform global optimization and have stronger outlier handling capabilities. Compared to traditional Kalman filtering methods, FGO has demonstrated several significant advantages in the field of state estimation for robots and autonomous systems.

[0025] Therefore, this embodiment proposes an improved IQR method based on INS / DVL / PS factor graph optimization combined navigation. Based on the improved IQR method, a sliding window combined with propeller speed is used to dynamically adjust the threshold to adapt to changes in the observed state. In gross error detection, the data within the window is continuously updated over time, establishing a graph optimization framework and constructing a matrix relating IMU factors, DVL factors, and PS factors to state variables.

[0026] In the complex underwater observation environment, there are more observational outliers. The FGO method can better reduce the impact of outliers on subsequent estimations. It is globally optimized and uses later information to correct historical states, making it more robust than filtering systems. The system demonstrates performance comparable to the extended Kalman filter method under the optimal window size. Field sea trials have verified that this method can provide more durable and reliable dead reckoning for AUVs.

[0027] Based on the aforementioned pseudo-DVL velocity prediction, improved IQR dynamic threshold adjustment, and INS / DVL / PS factor graph optimization, the underwater integrated navigation method provided in this embodiment, such as... Figure 1 As shown, the specific steps are as follows: like Figure 1 As shown: The transformation matrix is ​​used to transform the vehicle coordinate system to the navigation coordinate system. The lever arm error is the result of converting the PS sensor to the INS in the carrier coordinate system. The lever arm error is the result of converting the DVL sensor to the INS in the carrier coordinate system. , and These represent the relative changes in attitude, velocity, and position within the IMU pre-integration interval, respectively, during the DVL observation interval; For INS recursive attitude data; This is the depth data after height compensation; The speed data is obtained by converting the propeller rotation speed through a coefficient.

[0028] I. Input Velocity observations acquired by the DVL sensor; Depth observations acquired by the PS sensor; Angular velocity and acceleration information collected by inertial sensors, gyroscopes, and accelerometers; Information such as initial attitude, initial velocity, initial position, and lever error.

[0029] II. Data Processing The main focus is on the correction of velocity values. There are two timing points for correction: 1) Observation interruption, where the Doppler log (DVL) fails to observe velocity; 2) Gross errors occur, meaning the observed velocity meets the gross error conditions. These are explained below: 1. Determine if DVL speed prediction is needed. (1) Detect whether the current DVL observation speed is interrupted.

[0030] By detecting interruptions, it is determined whether the current DVL velocity information is valid. If it is valid, navigation is performed based on INS / DVL / PS using graph optimization methods to achieve navigation output. If it fails, it is determined whether the observed velocity meets the gross error conditions.

[0031] (2) Determine whether the observation speed meets the gross error conditions. The threshold determined by the existing IQR (Interquartile Range) method remains fixed. A fixed value cannot adapt to complex underwater observation environments and variable carrier motion states. This embodiment improves the IQR method by using a sliding window combined with dynamic threshold adjustment based on propeller speed to adapt to changes in observation conditions.

[0032] Specifically, in gross error detection, the data within the window is continuously updated over time, and the DVL data sampling frequency is [missing information]. The size of the sliding window In seconds, we have: (18) Therefore, the upper and lower bound thresholds of gross errors in the sliding window can be dynamically calculated using the following formula: (19) In the formula, Indicates correspondence The lower bound for time-varying error detection; for Upper bound for time-varying error detection; , They represent in The third and first quartiles of the dataset of differences between DVL observed velocities and system state velocities within a time window, where the system state velocities are predicted. The speed value of the underwater vehicle at any given moment; for and The difference; express The target rotational speed of the propeller at any given time is the stable rotational speed that can be achieved under the current power. Represents propeller The actual rotational speed was measured at any given time.

[0033] When the detected difference is outside the range of the lower and upper bounds of gross error, it is identified as a DVL observation gross error, and DVL velocity prediction is required.

[0034] 2. DVL speed prediction By using a trained NLS-Transformer-LSTM model, the pseudo-DVL speed is output based on multi-feature input data.

[0035] The NLS-Transformer-LSTM model used to predict pseudo-DVL velocities is explained below: The structure of the NLS-Transformer-LSTM model, such as Figure 2 As shown, it includes two models: a propeller speed model based on nonlinear least squares (NLS) and a DVL speed prediction model based on Transformer-LSTM. The propeller speed model is used to obtain the current propeller speed, and the DVL speed prediction model is used to predict pseudo-DVL speed.

[0036] (1) Propeller speed model The propeller speed model utilizes the propeller's power change detection results. When power increases or decreases, it corrects the propeller speed based on the nonlinear least squares (NLS) method. Specifically: 1) Detect the decrease in AUV propulsion power.

[0037] When a decrease in propeller power is detected, propeller speed data during the speed reduction phase cannot be obtained due to limitations in observation conditions. Therefore, this portion of propeller speed data needs to be pre-processed to accurately reflect the relationship between propeller speed and AUV speed under normal operating conditions.

[0038] When an AUV travels underwater, the resistance it encounters primarily stems from the interaction between the water flow and the AUV. This resistance affects the ship's motion and is closely related to the AUV's speed. Water resistance consists of frictional resistance, form resistance, and wave-making resistance, with frictional resistance and form resistance being the dominant components. Frictional resistance is caused by water viscosity; when a ship moves in water, a layer of water adheres to the hull surface and moves with the hull. The energy consumed by the ship's motion in moving water molecules is the energy consumed by the ship to overcome frictional resistance, as shown in the following formula: Frictional Resistance The size is related to the hull's submerged surface area, the hull's surface slipperiness, and the ship's speed: (1) in, For water density, Forward velocity under the load system This represents the wetted surface area of ​​the ship's hull. The frictional resistance coefficient is generally obtained using the ITTC 1957 frictional resistance correlation formula recommended by the International Ship Model Testing Tank Conference: (2) (3) in, The Reynolds number is... Represents the kinematic viscosity of a fluid. Indicates the length of the AUV.

[0039] When an AUV cruises at low speed in deeper waters, it is minimally affected by wave-making drag, and the form drag is basically proportional to the square of the speed in this state, with little change in the form drag coefficient.

[0040] When the AUV thrusters stop providing power, the AUV's speed gradually decreases due to drag. During this phase, the velocity differential equation function is established: (4) in, For the velocity derivative, , and For the unknown parameters obtained by the nonlinear least squares method, For AUV quality.

[0041] Based on the above velocity differential equation function, a propeller speed model is constructed as follows: By analyzing the AUV's velocity and corresponding time data during the powered descent phase (the velocity at this time is provided by the DVL under normal observation conditions, i.e., the vt data), the unknown parameters in the differential equation can be obtained through fitting using a nonlinear least squares method. This also yields the velocity-time function relationship during the powered descent phase. When propeller powered descent is detected, the velocity value Vb can be obtained from the velocity differential equation function. Multiplying Vb by the propeller speed-to-velocity conversion coefficient obtained from numerous experiments yields the corrected propeller speed. This corrected propeller speed can then be input into the Transformer-LSTM model for DVL velocity prediction.

[0042] 2) Detect the AUV propulsion system's upward thrust.

[0043] When the thruster power of an AUV increases, the propeller's feedback rotational speed increases faster than the actual speed of the AUV. This embodiment establishes a propeller rotational speed model to define the functional relationship between propeller rotational speed and time. It is known that the thrust provided by the thruster is related to the current seawater density, the propeller rotational speed, and the propeller diameter. The empirical formula is: (5) in, It is the thrust coefficient. The current propeller speed, The propeller diameter depends on the advance coefficient. : (6) In the formula, Given the target propeller speed under current power, numerous experimental data and empirical model formulas for different propeller models indicate that the thrust coefficient... and advance coefficient The relationship between them can be approximated by a quadratic function: (7) in, , and For the unknown parameters to be solved using the nonlinear least squares method, the velocity differential equation function exists during the stage where the thruster provides changing power: (8) in, For the velocity derivative, For thrust, , and For the unknown parameters obtained by the nonlinear least squares method, For AUV quality.

[0044] Based on the above velocity differential equation function, a propeller speed model is constructed as follows: By using the observed velocity and corresponding time data (vt data) during the DVL (Dynamic Value Lie) phase of propeller descent, the unknown parameters in the differential equation can be obtained through fitting using a nonlinear least squares method. This also yields the velocity-time function relationship during the descent phase. When propeller power descent is detected, the velocity value Vb can be obtained from the velocity differential equation function. Multiplying Vb by the propeller speed-to-velocity conversion coefficient obtained from numerous experiments yields the corrected propeller speed. This corrected propeller speed can then be input into the Transformer-LSTM model for DVL velocity prediction.

[0045] (2) DVL velocity prediction model.

[0046] The DVL velocity prediction model is built on Transformer-LSTM, where the IMU pre-integration relies solely on the angular velocity and specific force information from the gyroscope and accelerometer within adjacent keyframe time intervals, and obtains the relative position increment for that time interval through a single integration. Speed ​​increment With attitude increment The navigation equations are a set of differential equations that integrate the angular velocity output from the gyroscope and the specific force output from the accelerometer to determine the carrier's attitude, velocity, and position. Since the pressure gauge observes depth relative to the water surface, not in the coordinate system used by the navigation system, height compensation must first be applied to the pressure gauge depth observations. The propeller speed-velocity conversion coefficient was obtained through extensive experimental calculations. The velocity is obtained by converting the corrected propeller speed using the speed-velocity conversion coefficient. Depth obtained by further height compensation from PS observations Attitude calculated from IMU observations using navigation equations Attitude changes, velocity increments, and position increments obtained from IMU pre-integration The input is DVL speed, and the output is DVL speed.

[0047] By introducing the absolute observation value of propeller speed, long-term control of forward velocity in the carrier coordinate system can be maintained; combined with IMU pre-integration velocity, position increment and attitude change with equal DVL sampling intervals, the absolute dynamic changes of AUV within the current interval can be well reflected; combined with high-precision observation in the depth direction of the pressure gauge, the vertical velocity control can be well solved; finally, combined with the attitude change calculated by the high-precision angle observation of the INS, a multi-feature input parameter of the learning model is formed.

[0048] The DVL speed prediction model is trained based on a training set, which consists of propeller rotation speeds during normal DVL observations, converted to speeds using a speed-to-velocity conversion coefficient. Highly compensated depth information Attitude calculated from IMU observations using navigation equations and attitude change, velocity increment, and position increment obtained by IMU pre-integration. The input is the speed of the DVL normal observation phase.

[0049] When DVL is interrupted, the model input remains unchanged, and the data is fed into the trained Transformer-LSTM model. The model first uses the Transformer's self-attention mechanism to dynamically weight the entire sequence to capture long-term dependencies and abrupt changes. Then, the LSTM recursively outputs the three-dimensional velocity increment within the next 1 second. The baseline velocity of the previous time step and the velocity increment are accumulated to obtain the velocity prediction value of the current time step, thus achieving continuous, smooth, and high-precision velocity estimation under DVL absence.

[0050] III. Optimization Process The graph optimization process uses a sliding window as the boundary to construct a lightweight factor graph: nodes retain only the position, velocity, attitude, and gyroscope and accelerometer zero-bias parameters of keyframes within the window, while edges are connected by multi-source residual factors, forming a sparse, incrementally updatable optimization structure. High-rate motion constraints are generated between adjacent nodes using IMU pre-integration. The residuals include position, velocity, and attitude, and the covariance is recursively calculated in real-time based on the pre-integration uncertainty to ensure that high-frequency dynamics are not smoothed out. During normal DVL periods, DVL factors are established through DVL velocity observations. During DVL failure periods, the instantaneous velocity predicted by Transformer-LSTM is encapsulated as a "pseudo-DVL factor," and the covariance matrix is ​​adaptively reduced according to the model output uncertainty. After depth transformation, the pressure sensor provides a single-degree-of-freedom absolute depth constraint; the residuals only act on the vertical direction of the position state variables, and the covariance is given by the sensor calibration noise, suppressing IMU integral drift in the vertical direction. The prior factors generated by marginalization compress the old states that slide out of the window into linearized priors using Schur complements, ensuring lossless information loss. During marginalization, each frame that slides out is marginalized, and compact prior factors constructed using Schur complements are added to the graph. This process maintains a constant system dimension while preserving historical constraints to prevent errors from accumulating over time.

[0051] The factor graph optimization framework includes the state variables to be optimized, corresponding to the factors constructed by the sensors. In other words, each factor transforms the sensor observations into residual constraints between state variables. The sliding window retains only a portion of the state variables to reduce the scale of the optimization problem, and each optimization only processes the most recent N keyframes. Marginalization preserves the influence of outdated state variables in the new prior factors, and compresses the historical information outside the sliding window into linear priors through Shur complement. The prior factors include the initial attitude, initial velocity, initial position, and marginalization optimization information retained by the historical states outside the sliding window. These priors provide invariant initial constraints and historical cumulative constraints for the current window.

[0052] The system first constructs the corresponding INS / DVL / PS graph optimization framework. Information such as state variables, sliding window size, iteration count, and prior factors are set. During system navigation, the window continuously slides backward, adding IMU factors, DVL factors, PS factors, and prior factor information to constrain the state variables. Prior factors are generated through marginalization and added to subsequent new sliding windows, ultimately outputting the system state variables. The following details the factor graph optimization framework of this system, including the construction process of IMU factors, DVL factors, and PS factors, and the implementation principle of the factor graph optimization method: (1) INS / DVL / PS diagram optimization framework, such as Figure 3 As shown, this integrates factor graph optimization into inertial navigation systems, DVL, and PS. The corresponding variable nodes represent the required state variables in time. The state of an underwater vehicle can be represented as: (10) in, , , , and The zero-bias error of the position coordinates, velocity, attitude, accelerometer, and gyroscope in the northeast-central coordinate system.

[0053] (2) Constructing the IMU pre-integration factor: (11) In the formula, Indicates the IMU pre-integration factor; In order to be in Time and The change in displacement between moments; In order to be in Time and The change in velocity between moments; , and represent the position change and velocity change after adding the zero bias update, respectively; and They represent Time and Time navigation coordinate system The corresponding posture In the navigation coordinate system and The change in attitude between time points; Represented in the navigation coordinate system and The change in attitude between time points; and They represent in and The change in bias of the accelerometer and gyroscope between moments.

[0054] (3) Constructing the DVL factor: (12) In the formula, DVL factor; Velocity observation; Velocity state variable; Coordinate transformation coefficient; This represents the projection of the rotational angular velocity vector of the vehicle coordinate system relative to the navigation coordinate system onto the vehicle system. This indicates the mounting arm error between the centers of the IMU and DVL sensors.

[0055] Simultaneously, it is necessary to construct the observation equation function to establish the relationship between DVL factors and state variables. (DVL observation equation function) It can be represented as: (13) (14) (15) In the formula, Let Jacobian matrix be the linearized objective function, where Corresponding to the attitude part, Corresponding gyroscope bias; Let be the state variable; where Corresponding to the attitude part, Corresponding gyroscope bias; It is the identity matrix; The projection of the rotational angular velocity vector of the navigation coordinate system relative to the inertial coordinate system onto the navigation frame; The theoretical angular velocity is given by the load system.

[0056] (4) Construct the PS factor: (16) In the formula, PS factor; Pressure gauge depth observation; Position state variables; It is a coefficient matrix; lever arm error; ; This represents the Earth's first eccentricity; Indicates the Earth's radius; This indicates the latitude of the current location of the AUV.

[0057] Establish the relationship between the PS factor and the state variables, and the PS observation equation function. It can be represented as: (17) Based on observational data, the calculated IMU pre-integration factor, DVL factor, and PS factor are used to minimize factor errors using an optimization algorithm, thereby obtaining the optimal state variables. To improve computational efficiency, a sliding window and marginalization techniques are combined. The sliding window limits the number of state variables involved in the optimization, while marginalization eliminates variables that are no longer of interest, while retaining their influence as prior factors, thus ensuring both optimization accuracy and computational efficiency. The final underwater vehicle state, including position coordinates, velocity, attitude, and accelerometer and gyroscope bias error parameters, is obtained through this system.

[0058] (5) Factor graph optimization method for FGO FGO is a directed graph consisting of variable nodes and factor nodes. Variable nodes represent state variables, including attitude, velocity, position, gyroscope and accelerometer bias. Factor nodes represent the constructed IMU factors, DVL factors, and PS factors. FGO is based on a global estimation using "batch optimization," optimizing the values ​​of variable nodes by minimizing the cost function of all factor nodes. The cost function is typically the error calculated by each factor node based on sensor observations and theoretical models. FGO optimizes all states at once, thus fully utilizing information throughout the entire motion process. The basic process of the FGO method is as follows: exist The set of all observations received at time t is represented as:

[0059] In the formula, express Actual observations obtained from different sensors at different times , For IMU observation data; This is DVL observation data; This is pressure gauge depth observation data.

[0060] If the observation data at each time point are independent, factoring the global function according to Bayesian estimation, we can obtain all state variables at the k-th time point. and observation data joint posterior probability density for:

[0061] In the formula, It represents the prior information of the initial state of all state variables; This indicates a state transition model using IMU recursion. Let i be the i-th and i-1-th state variables. This indicates the DVL observation update model. It is the j-th set of the i-th variable node in the DVL observation update model. For the j-th DVL observation data, This indicates that the DVL observation update model is used.

[0062] When constructing factor nodes, the prior information of the carrier state is first modeled as a prior factor. This prior factor is only related to the variable node at each time step and is a univariate factor. In the joint probability density function... In the factorization of , each factor represents an independent term, that is ;in, Let be the state variable corresponding to time i.

[0063] Based on maximum a posteriori probability estimation, the problem of calculating the optimal value of the state variable from the observed data is transformed into an equivalent least squares optimization problem, namely:

[0064] in, This represents the maximum a posteriori probability estimate. It represents the error function constructed from prior information, and is usually expressed as the difference between the observed value and the mean. Represents the square of the Mahalanobis distance. Represent the covariance matrix; For the IMU error function, The DVL error function; The pressure gauge error function; Find the most likely one through optimization The estimated value, i.e., the maximum a posteriori estimate; This is the prior error term, which is usually based on known prior signal information and represents the influence of a certain prior model on the error. Let be the Jacobian matrix of the prior information, representing the linearization of the prior model; This represents the initial state estimation vector; Represents the initial state The summation of all variables is used to optimize the state; The observation model function represents the initial state, which is usually the mapping relationship between the state and the observed values; This represents the weighted squared error, and it represents the relationship between the measurement error and the observation noise covariance. According to nonlinear optimization theory, by adjusting the state variables To minimize it, at this time The value can bring the state to the optimal estimate. This is also the solution for the state variable. The process.

[0065] IV. Output The solution outputs the underwater vehicle's state, including its position coordinates, velocity, attitude, and the zero-bias error parameters of the inertial sensors gyroscope and accelerometer carried by the vehicle.

[0066] Example 2 One embodiment of the present invention provides an underwater integrated navigation system based on velocity prediction. The underwater vehicle performs underwater integrated navigation using an onboard inertial navigation system (INS), a Doppler log (DVL), and a pressure sensor (PS), including: The data acquisition module is configured to acquire observation data from the inertial navigation system INS, the Doppler log (DVL), and the pressure sensor PS. When the Doppler log (DVL) fails to observe the velocity or the observed velocity meets the gross error condition, the observed velocity is predicted based on the corrected propeller speed. The data fusion module is configured to: calculate the IMU pre-integration factor, DVL factor and PS factor based on the observation data, construct a factor map, fuse the observation data through the factor map optimization method, and obtain the final underwater vehicle state, including position coordinates, velocity, attitude, accelerometer and gyroscope zero bias error; The gross error condition is based on the IQR method, which uses a sliding window combined with the propeller speed to dynamically adjust the gross error threshold, and the gross error condition is judged by the gross error threshold.

[0067] Example 3 One embodiment of the present invention provides a computer program product, including a computer program that, when executed by a processor, implements the aforementioned underwater integrated navigation method based on velocity prediction.

[0068] Example 4 In one embodiment of the present invention, a non-transitory computer-readable storage medium is provided for storing computer instructions, which, when executed by a processor, implement the underwater integrated navigation method based on velocity prediction.

[0069] Example 5 One embodiment of the present invention provides an electronic device, including: a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to implement the underwater integrated navigation method based on speed prediction.

[0070] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0071] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0072] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. An underwater integrated navigation method based on velocity prediction, characterized in that, The underwater vehicle utilizes an onboard inertial navigation system (INS), a Doppler log (DVL), and a pressure sensor (PS) for integrated underwater navigation, including: Acquire observation data from the inertial navigation system (INS), the Doppler log (DVL), and the pressure sensor (PS). When the Doppler log (DVL) fails to observe the velocity or the observed velocity meets the gross error condition, the observed velocity is predicted based on the corrected propeller speed. Based on the observation data, the IMU pre-integration factor, DVL factor and PS factor are calculated, a factor map is constructed, and the observation data is fused through the factor map optimization method to obtain the final underwater vehicle state, including position coordinates, velocity, attitude, accelerometer and gyroscope zero bias error. The gross error condition is based on the IQR method, which uses a sliding window combined with the propeller speed to dynamically adjust the gross error threshold, and the gross error condition is judged by the gross error threshold.

2. The underwater integrated navigation method based on velocity prediction as described in claim 1, characterized in that, The failure of the Doppler log DVL observation speed was determined through interruption detection; The observation speed satisfies the gross error condition, which is achieved by using a gross error threshold for gross error detection.

3. The underwater integrated navigation method based on velocity prediction as described in claim 1, characterized in that, The corrected propeller speed is based on the propeller power change detection results. When the power increases or decreases, the propeller speed is corrected based on the NLS method.

4. The underwater integrated navigation method based on velocity prediction as described in claim 3, characterized in that, The aforementioned power change detection uses velocity differential equations to determine the stages of power increase, power decrease, and power constancy.

5. The underwater integrated navigation method based on velocity prediction as described in claim 1, characterized in that, The prediction of the observation velocity is achieved by using a trained Transformer-LSTM model with inputs including INS recursive attitude, IMU pre-integrated position within the sampling interval, relative changes in velocity and attitude, PS depth observation, and corrected propeller rotation speed.

6. The underwater integrated navigation method based on velocity prediction as described in claim 1, characterized in that, The IQR-based method uses a sliding window combined with propeller speed to dynamically adjust the gross error threshold, which can be expressed by the following formula: (19) In the formula, Indicates correspondence The lower bound for time-varying error detection; for Upper bound for time-varying error detection; , They represent in The third and first quartiles of the dataset of differences between DVL observed velocities and system state velocities within a time window, where the system state velocities are predicted. The speed value of the underwater vehicle at any given moment; for and The difference; express The target rotational speed of the propeller at any given time; Represents propeller The actual rotational speed was measured at any given time.

7. An underwater integrated navigation system based on velocity prediction, characterized in that, The underwater vehicle utilizes an onboard inertial navigation system (INS), a Doppler log (DVL), and a pressure sensor (PS) for integrated underwater navigation, including: The data acquisition module is configured to acquire observation data from the inertial navigation system INS, the Doppler log (DVL), and the pressure sensor PS. When the Doppler log (DVL) fails to observe the velocity or the observed velocity meets the gross error condition, the observed velocity is predicted based on the corrected propeller speed. The data fusion module is configured to: calculate the IMU pre-integration factor, DVL factor and PS factor based on the observation data, construct a factor map, fuse the observation data through the factor map optimization method, and obtain the final underwater vehicle state, including position coordinates, velocity, attitude, accelerometer and gyroscope zero bias error; The gross error condition is based on the IQR method, which uses a sliding window combined with the propeller speed to dynamically adjust the gross error threshold, and the gross error condition is judged by the gross error threshold.

8. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the underwater integrated navigation method based on velocity prediction as described in any one of claims 1-6.

9. A non-transitory computer-readable storage medium, characterized in that, The non-transitory computer-readable storage medium is used to store computer instructions, which, when executed by a processor, implement an underwater integrated navigation method based on velocity prediction as described in any one of claims 1-6.

10. An electronic device, characterized in that, include: The device includes a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to perform an underwater integrated navigation method based on speed prediction as described in any one of claims 1-6.

Images