An underwater integrated navigation method and system based on speed prediction

Abstract

The application provides a kind of underwater integrated navigation method and system based on speed prediction, it is related to underwater navigation technical field, comprising: obtaining the observation data of inertial navigation system (INS), Doppler velocity log (DVL) and pressure sensor (PS), when the observation speed of Doppler velocity log (DVL) is invalid or meets the rough error condition, the observation speed is predicted based on the modified propeller speed;Based on the observation data, calculate the IMU pre-integration factor, DVL factor and PS factor, construct the factor graph, fuse the observation data through the factor graph optimization method, obtain the final underwater vehicle state, including position coordinates, speed, attitude, accelerometer and gyroscope zero bias error;The pseudo-DVL factor constructed by the speed prediction value replaces the DVL factor and participates in the navigation framework, which realizes the effective inhibition of navigation precision divergence in the DVL invalid 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, thereby effectively inhibiting the navigation precision divergence in the DVL failure state.

[0005] According to some embodiments, the present application adopts the following technical solutions:

[0006] The application discloses a speed prediction-based underwater integrated navigation method, and an underwater vehicle carries an inertial navigation system (INS), a Doppler velocity log (DVL) and a pressure sensor (PS) to perform underwater integrated navigation.

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

[0008] 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 through a factor graph optimization method, and finally, underwater vehicle states including position coordinates, speed, attitude, accelerometer and gyroscope zero bias errors are obtained.

[0009] 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 through the gross error threshold.

[0010] According to some embodiments, the application adopts the following technical scheme:

[0011] The application discloses a speed prediction-based underwater integrated navigation system, and an underwater vehicle carries an inertial navigation system (INS), a Doppler velocity log (DVL) and a pressure sensor (PS) to perform underwater integrated navigation.

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

[0013] 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 through a factor graph optimization method, and finally, obtain underwater vehicle states including position coordinates, speed, attitude, accelerometer and gyroscope zero bias errors.

[0014] 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 through the gross error threshold.

[0015] According to some embodiments, the application adopts the following technical scheme:

[0016] The application discloses a computer program product, including a computer program, which is executed by a processor to realize the speed prediction-based underwater integrated navigation method.

[0017] According to some embodiments, the present application adopts the technical solutions as follows:

[0018] A non-transitory computer readable storage medium for storing computer instructions, which, when executed by a processor, implement the underwater integrated navigation method based on speed prediction.

[0019] According to some embodiments, the present application adopts the technical solutions as follows:

[0020] An electronic device, comprising a processor, a memory and a computer program, wherein the processor is connected with the memory, and 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, so that the electronic device executes the underwater integrated navigation method based on speed prediction.

[0021] Compared with the prior art, the present application has the beneficial effects that:

[0022] The present application uses INS to recursively calculate the attitude, uses the relative changes of IMU pre-integrated position, velocity and attitude in the sampling interval, uses the PS depth observation and the NLS (nonlinear least squares) corrected propeller speed observation data as multi-feature input, predicts the DVL speed through the speed prediction model based on Transformer-LSTM, and replaces the DVL factor with the pseudo-DVL factor to participate in the navigation framework. The speed prediction model calculates the correlation between the input features in the speed prediction process by means of the self-attention (Self-Attention) mechanism in the Transformer, dynamically adjusts the weight of each feature, excellently captures the global context data relationship, and realizes effective inhibition of navigation precision divergence in the DVL failure state.

[0023] The present application proposes a pre-correction processing of the propeller speed data based on the NLS method, so that the propeller speed can accurately reflect the relationship between the propeller speed and the AUV speed in the normal operation state of the AUV, and when the change of the propeller power is detected, the simulation function is enabled until the speed is stable. Such processing steps are crucial for ensuring the accuracy and reliability of the model.

[0024] The present application uses a sliding window combined with propeller speed to improve the existing IQR method, realizes dynamic adjustment of the threshold value, sets the sliding window, and reacts to the current carrier motion state through the propeller speed data in the window. If it is in a complex motion state, the threshold value is increased; if it is in a simple motion state, the threshold value is decreased. The FGO method based on factor graph optimization can better reduce the influence of abnormal values on subsequent estimation, globally optimize, and correct the historical state by using the late information, and is more robust than the filtering system. BRIEF DESCRIPTION OF DRAWINGS

[0025] The accompanying drawings, which form a part of this specification, are included to provide a further understanding of the application, and are incorporated into and constitute a part of this specification. The embodiments of the application, and their

[0026] Figure 1 Flow chart of underwater integrated navigation method for example 1.

[0027] Figure 2 Structure diagram of NLS-Transformer-LSTM model for example 1.

[0028] Figure 3 INS / DVL / PS optimization framework diagram for example 1. DETAILED DESCRIPTION

[0029] The application will be further described below in conjunction with the drawings and embodiments.

[0030] It should be noted that the following detailed description is exemplary in nature and is intended to provide further description of the application. Unless otherwise defined, 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 application belongs.

[0031] It is also to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments in accordance with the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, steps, operations, elements, components, and / or groups thereof.

[0032] Table 1: Glossary of terms

[0033]

[0034] Example 1

[0035] In one embodiment of the present application, a speed prediction based underwater integrated navigation method is provided, in which an underwater vehicle performs underwater integrated navigation by means of an onboard inertial navigation system (INS), a Doppler velocity log (DVL), and a pressure sensor (PS), comprising:

[0036] Step S1: obtaining observation data of the inertial navigation system (INS), the Doppler velocity log (DVL), and the pressure sensor (PS), wherein when the Doppler velocity log (DVL) observation speed is invalid or the observation speed meets the gross error condition, the observation speed is predicted based on the corrected propeller speed;

[0037] Step S2: based on the observation data, calculating the IMU pre-integration factor, DVL factor and PS factor, constructing the factor graph, fusing the observation data through the factor graph optimization method, and obtaining the final underwater vehicle state including position coordinates, velocity, attitude, accelerometer and gyroscope zero bias error;

[0038] The gross error condition is based on the IQR method, and the gross error threshold is dynamically adjusted using a sliding window combined with the propeller speed, and the gross error condition is judged by the gross error threshold.

[0039] As an embodiment, the underwater integrated navigation method based on speed prediction of the application replaces the DVL factor with the pseudo-DVL factor constructed by the speed prediction value to participate in the navigation framework, which realizes effective inhibition of navigation precision divergence in the DVL failure state. The specific implementation process is described below.

[0040] The application scenario of the embodiment is in a complex underwater environment. The inertial navigation system (INS) is the core sensor carried by the AUV. Because of its strong autonomy and the ability to provide continuous full-dimensional navigation parameters, it becomes the core sensor. However, its navigation error will accumulate and diverge over time, so it must rely on external observation information for correction. Therefore, the AUV will also carry sensors such as DVL and PS. The Doppler log (DVL) can provide high-precision velocity vectors, and the pressure sensor (PS) can provide absolute depth reference. The combination of the two with the INS has become the mainstream solution. Through information fusion, the error accumulation of the INS is inhibited. The most common information fusion method in underwater navigation is the extended Kalman filter (EKF), unscented Kalman filter (UKF), and other Kalman filter variants. This method is based on the state estimation at the previous time, and realizes real-time fusion of sensor data through the "prediction-update" recursive framework, which has the advantages of high computational efficiency and easy engineering implementation.

[0041] Due to the complexity of the marine environment, DVL observations are affected by factors such as exceeding the detection range, complex seabed topography and bottom material, unfavorable hydrological conditions, and extreme sea conditions, which lead to DVL observation failure. For DVL failure, the embodiment proposes a method of using INS recursive attitude, IMU pre-integrated position, velocity, and attitude relative changes within the sampling interval, PS depth observation, and NLS (nonlinear least squares) corrected propeller speed observation data as multi-feature input to train and predict DVL speed using the Transformer-LSTM model. The pseudo-DVL factor replaces the DVL factor to participate in the navigation framework, which realizes effective inhibition of navigation precision divergence in the DVL failure state, and is more accurate, stable, and persistent than other existing methods.

[0042] The most common information fusion methods in underwater navigation are extended Kalman filter (EKF), unscented Kalman filter (UKF) and other Kalman filter variants. However, the recursive filtering algorithm is locally optimal in nature, which is updated based on the current state and observation, leading to the accumulation of linearization errors over time, making the navigation error prone to divergence over a long period of time. More importantly, EKF is extremely sensitive to outliers in observation data, and a single outlier can contaminate the entire filtering state, leading to a serious deviation in the estimation result and making it difficult to recover. Finally, EKF strictly relies on sequential data processing, making it difficult to flexibly integrate asynchronous and multi-rate underwater observation constraints, which makes it less adaptable, less accurate and less robust than factor graph optimization (FGO) in complex underwater environments. FGO has shown many advantages over traditional Kalman filtering methods in the field of state estimation of robots and autonomous systems.

[0043] Therefore, the embodiment proposes an improved INS / DVL / PS factor graph optimization integrated navigation method based on the improved IQR method. Based on the improved IQR method, a sliding window is used in combination with the propeller speed to dynamically adjust the threshold to adapt to the change of observation state; in the outlier detection, the data in the window is updated according to the time, the graph optimization framework is built, and the IMU factor, DVL factor and PS factor are constructed.

[0044] In the complex underwater observation environment, there are more observation outliers. The FGO method can better reduce the influence of outliers on subsequent estimation. Global optimization can correct the historical state using the late information, and the filtering system is more robust. The system shows the performance not inferior to the extended Kalman filter method under the optimal window size. The actual sea trial test shows that the method can provide more persistent and reliable dead reckoning for the AUV.

[0045] Based on the above pseudo-DVL velocity prediction, improved IQR dynamic threshold adjustment and INS / DVL / PS factor graph optimization, the underwater integrated navigation method provided by the embodiment is as shown in Figure 1 The specific steps are as follows:

[0046] As shown in Figure 1 is the conversion matrix of the carrier coordinate system to the navigation coordinate system; is the rod error of the PS sensor in the carrier coordinate system to the INS; is the rod error of the DVL sensor in the carrier coordinate system to the INS; , and are the relative changes of attitude, velocity and position in the IMU pre-interval within the DVL observation interval, respectively; ​For INS to recursively update the attitude data; For the depth data after height compensation; For the speed data converted from the propeller rotation speed by the conversion coefficient.

[0047] I. Input

[0048] The speed observation collected by the DVL sensor;

[0049] The depth observation collected by the PS sensor;

[0050] The angular velocity and acceleration information collected by the inertial sensor gyroscope and accelerometer;

[0051] Initial attitude, initial speed, initial position, lever error, etc.

[0052] II. Data processing

[0053] The main correction is for the speed value. The timing of the correction has two: 1) observation interruption, Doppler DVL observation speed failure; 2) observation gross error, that is, the observation speed meets the gross error condition, which will be described below:

[0054] 1. Determine whether DVL speed prediction is needed

[0055] (1) Detect whether the current DVL observation speed is interrupted.

[0056] Through the interruption detection, it is judged whether the current DVL speed information is valid, if valid, the combined navigation based on INS / DVL / PS is carried out to realize the navigation output through the graph optimization method, if invalid, it is judged whether the observation speed meets the gross error condition.

[0057] (2) Determine whether the observation speed meets the gross error condition

[0058] The threshold value determined by the existing IQR (Interquartile Range) method is fixed and cannot be changed. A fixed value cannot adapt to the complex underwater observation environment and variable carrier motion state. In this embodiment, the IQR method is improved, and a sliding window is used to dynamically adjust the threshold value based on the propeller rotation speed to adapt to the change of observation state.

[0059] Specifically, in the gross error detection, the data in the window will be updated according to time, the DVL data sampling frequency is , the size of the sliding window is in seconds, which has:

[0060] (18)

[0061] Therefore, the upper and lower threshold values of the gross error in the sliding window are dynamically calculated, which is expressed by the formula:

[0062] (19)

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

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

[0065] 2. DVL speed prediction

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

[0067] The NLS-Transformer-LSTM model used to predict pseudo-DVL velocities is explained below:

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

[0069] (1) Propeller speed model

[0070] 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:

[0071] 1) Detect the decrease in AUV propulsion power.

[0072] When the propeller power is detected to drop, due to the limitation of observation conditions, the propeller speed data in the speed drop stage cannot be obtained, and the propeller speed data needs to be pre-processed so as to accurately reflect the relationship between the propeller speed and the AUV speed in the normal running state of the AUV.

[0073] When the AUV runs underwater, the resistance mainly comes from the interaction between the water flow and the AUV, which affects the movement of the ship and is closely related to the speed of the AUV. The water resistance is composed of friction resistance, shape resistance and wave resistance, etc. Among them, the friction resistance and the shape resistance are the dominant part, which is caused by water viscosity. When the ship moves in the water, there is always a layer of water adhering to the surface of the ship body and moving with the ship body. The energy consumed by the movement of water molecules is the energy consumed by the ship to overcome the friction resistance. As shown in the following formula, the size of the friction resistance is related to the wetted surface area of the ship body, the slip of the ship body surface and the speed.

[0074] (1)

[0075] wherein, is the density of water, is the forward speed under the load system, represents the wet surface area of the ship body, represents the friction resistance coefficient, which is generally obtained by the ITTC1957 friction resistance correlation line formula recommended by the International Ship Model Test Pool Conference:

[0076] (2)

[0077] (3)

[0078] wherein, is the Reynolds number, represents the viscosity coefficient of fluid movement, represents the length of the AUV.

[0079] When the AUV cruises at low speed in deep sea, the influence of the wave resistance is very small, and the shape resistance is basically proportional to the square of the speed in this state, and the shape resistance coefficient changes little.

[0080] When the AUV propeller stops providing power, the AUV speed gradually decreases due to the action of the resistance, and the speed differential equation function is established in this stage:

[0081] (4)

[0082] wherein, is the speed derivative, , and unknown parameters solved by nonlinear least squares method, AUV mass.

[0083] Based on the above speed differential equation function, a propeller speed model is constructed, specifically:

[0084] Through the AUV speed and corresponding time data in the power descent phase, the AUV speed is provided by the DVL in the normal observation state, that is, the v-t data, and the unknown parameter values in the differential equation can be obtained by fitting through the nonlinear least squares method, and the speed-time function relationship in the power descent phase is also obtained. When the propeller power is detected to be reduced, the speed value Vb can be obtained through the speed differential equation function, and the corrected propeller speed can be obtained by multiplying the speed value Vb by the conversion coefficient of the propeller speed and the speed obtained from a large number of experiments. At this time, the corrected propeller speed can be input into the Transformer-LSTM model to predict the DVL speed.

[0085] 2) Detecting the AUV thruster power rise.

[0086] When the AUV thruster power is raised, the feedback speed of the propeller rises faster than the actual speed change of the AUV. In this embodiment, a propeller speed model is constructed to establish a function relationship between the propeller speed and time. The thrust provided by the thruster is related to the current sea water density in the region, the propeller speed of the thruster, and the propeller diameter, and the thrust is obtained The empirical formula is:

[0087] (5)

[0088] wherein, is the thrust coefficient, is the current propeller speed, is the propeller diameter, and depends on the advance coefficient :

[0089] (6)

[0090] wherein, is the current target propeller speed under power, and many experimental data and empirical model formulas of different propeller models show that the relationship between the thrust coefficient and the advance coefficient can be approximated by a quadratic function:

[0091] (7)

[0092] wherein, , and For the unknown parameters solved by the nonlinear least squares method, there is a velocity differential equation function in the stage when the propeller power changes:

[0093] (8)

[0094] wherein, is the velocity derivative, is the thrust, , and is the unknown parameter solved by the nonlinear least squares method, is the AUV mass.

[0095] Based on the above velocity differential equation function, a propeller speed model is constructed, specifically:

[0096] Through the DVL observed speed and corresponding time data in the power descent stage, that is, the v-t data, the unknown parameter values in the differential equation can be obtained by fitting through the nonlinear least squares method, and the speed-time function relationship in the power descent stage is also obtained. When the propeller power is detected to descend, the speed value Vb can be obtained through the velocity differential equation function. The corrected propeller speed can be obtained by multiplying the speed value Vb by the conversion coefficient of the propeller speed and the speed obtained from a large number of experiments. At this time, the corrected propeller speed can be input into the Transformer-LSTM model for DVL speed prediction.

[0097] (2) DVL speed prediction model.

[0098] The DVL speed prediction model is constructed based on Transformer-LSTM, wherein the IMU pre-integration only depends on the angular velocity and specific force information of the gyroscope and accelerometer within the adjacent key frame period, and the relative position increment , the velocity increment and the attitude increment are obtained by one-time integration; the navigation equation is a differential equation group integrating the angular velocity output by the gyroscope and the specific force output by the accelerometer into the carrier attitude, speed and position. Since the pressure gauge observes the depth relative to the water surface, and not in the coordinate system used by the navigation system, the pressure gauge depth observation must be first height compensated; the propeller speed-speed conversion coefficient is calculated from a large number of experiments; the speed obtained by the corrected propeller speed through the speed-speed conversion coefficient, the depth obtained by further height compensation of the PS observation, the attitude calculated from the IMU observation value through the navigation equation, the attitude change, the velocity increment and the position increment For input, DVL speed as output.

[0099] When the absolute observation value of propeller speed is introduced, the control of forward speed in the body coordinate system can be maintained for a long time; the absolute dynamic changes of the AUV in the current interval can be well reflected by combining the IMU pre-integrated speed, position increment and attitude change with the equal DVL sampling interval; the vertical direction speed control can be well solved by combining the high-precision observation of the pressure gauge in the depth direction; finally, the multi-feature input parameters of the learning model are formed by combining the attitude change calculated by the high-precision angle observation of the INS.

[0100] The DVL speed prediction model is trained based on the training set, and the training set is composed of the speed obtained by the propeller speed in the normal observation stage of DVL through the speed-velocity conversion coefficient , the depth information compensated by height , the attitude calculated by the IMU observation value through the navigation equation , and the attitude change, speed increment and position increment obtained by the IMU pre-integration as input, and the training target is the speed in the normal observation stage of DVL.

[0101] When the DVL is interrupted, the input of the model remains unchanged, and the data is sent into the trained Transformer-LSTM model; the model first dynamically weights the whole sequence by the self-attention mechanism of Transformer, captures the long-time dependence and mutation features; then the LSTM recursively outputs the three-dimensional speed increment in the future 1 s; the reference speed of the last moment and the speed increment are accumulated, and the speed prediction value of the current moment is obtained, so as to realize the continuous, smooth and high-precision speed estimation under the condition of DVL missing.

[0102] III. Optimization process

[0103] The graph optimization process is bounded by a sliding window, and a lightweight factor graph is constructed: nodes only retain the positions, velocities, poses, and gyroscope and accelerometer bias parameters of key frames within the window, and edges are connected by multi-source residual factors, forming a sparse and incrementally updatable optimization structure. High-rate motion constraints between adjacent nodes are generated using IMU pre-integration, and the residual includes position, velocity, and attitude. The covariance is recursively propagated according to the pre-integration uncertainty, ensuring that high-frequency dynamics are not smoothed out. During the normal period of the DVL, the DVL factor is established by the DVL velocity observation. During the failure period of the DVL, the instantaneous velocity predicted by the Transformer-LSTM is encapsulated as a "pseudo DVL factor", and the covariance matrix is adaptively compressed according to the model output uncertainty. After depth conversion, the pressure sensor provides a single-degree-of-freedom absolute depth constraint; the residual only acts on the vertical direction of the position state variable, and the covariance is given by the sensor calibration noise, which suppresses the drift of IMU integration in the vertical direction. The marginalized prior factor generated on the edge compresses the old state that slides out of the window into a linearized prior, ensuring that information is forgotten without loss. In the marginalization, the relevant state is executed every time a frame slides out, and a compact prior factor is constructed using the Schur complement to add to the graph. This process keeps the system dimension constant while preserving historical constraints, preventing errors from accumulating over time.

[0104] The factor graph optimization framework includes state variables to be optimized, and factors constructed by sensors, that is, each factor converts sensor observations into residual constraints between state variables; the sliding window only retains a part of the state variables to reduce the size of the optimization problem, and only the last N key frames are processed each time; marginalization preserves the influence of outdated state variables in new prior factors, and the historical information that slides out of the window is compressed into a linear prior through Schur complement; the prior factor includes the initial pose, initial velocity, initial position, and marginalization optimization information preserved by the historical state outside the sliding window; these priors provide invariant initial constraints and historical cumulative constraints for the current window.

[0105] The system first constructs an INS / DVL / PS graph optimization framework. Set the state variables, sliding window size, iteration number, prior factor, and other information. During the navigation process of the system, the window slides backward, and IMU factors, DVL factors, PS factors, and prior factor information are added to constrain the state variables within the window. And generate prior factors through marginalization and add them to the new sliding window, finally output the system state variables, the following will detail the factor graph optimization framework of the system, including the construction process of IMU factor, DVL factor and PS factor and the implementation principle of factor graph optimization method:

[0106] (1) The INS / DVL / PS graph optimization framework, as shown in Figure 3 is to introduce factor graph optimization into the integration of inertial navigation system, DVL and PS. The corresponding variable node represents the required state variable, and the time The state of an underwater vehicle can be represented as:

[0107] (10)

[0108] in, , , , and The zero-bias error of the position coordinates, velocity, attitude, accelerometer, and gyroscope in the northeast-central coordinate system.

[0109] (2) Constructing the IMU pre-integration factor:

[0110] (11)

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

[0112] (3) Constructing the DVL factor:

[0113] (12)

[0114] In the formula, DVL factor; Velocity observation; Velocity state variable; Coordinate transformation coefficient; the projection of the rotation angular velocity vector of the body coordinate system relative to the navigation coordinate system in the body system; the installation arm error between the centers of the IMU and DVL sensors.

[0115] Meanwhile, the DVL observation equation function needs to be constructed to establish the relationship between the DVL factor and the state variable. which can be expressed as:

[0116] (13)

[0117] (14)

[0118] (15)

[0119] wherein, is the Jacobian matrix of the linearized objective function, wherein corresponds to the attitude part, corresponds to the gyro bias; is the state variable; wherein corresponds to the attitude part, corresponds to the gyro bias; is the unit matrix; the projection of the rotation angular velocity vector of the navigation coordinate system relative to the inertial coordinate system in the navigation system; is the theoretical angular velocity in the body system.

[0120] (4) Construct the PS factor:

[0121] (16)

[0122] wherein, is the PS factor; is the pressure gauge depth observation; is the position state variable; is the coefficient matrix; is the arm error; ; represents the first eccentricity of the earth; represents the radius of the earth; represents the latitude of the current position of the AUV.

[0123] The relationship between the PS factor and the state variable needs to be established, and the PS observation equation function which can be expressed as:

[0124] (17)

[0125] Based on the observation data, the calculated IMU pre-integration factor, DVL factor and PS factor, the optimization algorithm is used to minimize the factor error, so as to obtain the optimal state variable. At the same time, in order to improve the calculation efficiency, combined with the sliding window and marginalization technology, the sliding window limits the number of state variables participating in the optimization, and the marginalization technology eliminates the variables that are no longer concerned, while retaining their influence to form the prior factor, so as to ensure the accuracy and calculation efficiency of the optimization. Through the above system, the final underwater vehicle state is obtained, including position coordinates, velocity, attitude, accelerometer and gyroscope zero bias error parameters.

[0126] (5) Factor graph optimization FGO method

[0127] FGO is a directed graph composed of variable nodes and factor nodes. The variable nodes are state variables, including attitude, velocity, position, gyroscope and accelerometer zero bias. The factor nodes are the constructed IMU factor, DVL factor and PS factor. FGO is based on global estimation of "batch optimization", and the value of the variable node is optimized by minimizing the cost function of all factor nodes. The cost function is usually the error calculated by each factor node according to the sensor observation and theoretical model. FGO optimizes all states at one time, so as to fully utilize the information in the whole motion process. The following is the basic process of FGO method:

[0128] At time, the set of all observation values received is represented as:

[0129]

[0130] In the formula, represents the actual observation value obtained by different sensors at time , is the IMU observation data; is the DVL observation data; is the pressure gauge depth observation data.

[0131] If the observation data at each time node is independent, according to the factorization of the global function by Bayesian estimation, the joint posterior probability density of all state variables and observation data in the kth time node is:

[0132]

[0133] In the formula, represents the prior information of the initial state of all state variables; represents the state transition model using IMU recursion, ​Let i be the i-th and i-1-th state variables. This indicates that the DVL observation update model is being used. 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.

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

[0135] 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:

[0136]

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

[0138] According to nonlinear optimization theory, by adjusting the state variables To minimize it, at this time The optimal state estimation value can be obtained by using the optimal value of the cost function.

[0139] IV. Output

[0140] The solved underwater vehicle state output includes the position coordinates, velocity, attitude of the underwater vehicle, and the bias error parameters of the inertial sensor gyroscope and accelerometer carried by the vehicle.

[0141] Embodiment 2

[0142] In an embodiment of the present application, a speed prediction-based underwater integrated navigation system is provided, and an underwater vehicle performs underwater integrated navigation by means of an inertial navigation system (INS), a Doppler velocity log (DVL), and a pressure sensor (PS), comprising:

[0143] A data acquisition module is configured to acquire 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 observation speed meets a gross error condition, the observation speed is predicted based on the corrected propeller speed.

[0144] A data fusion module is configured to calculate IMU pre-integration factors, DVL factors, and PS factors based on the observation data, construct a factor graph, fuse the observation data through a factor graph optimization method, and obtain the final underwater vehicle state, including position coordinates, velocity, attitude, accelerometer bias error, and gyroscope bias error.

[0145] The gross error condition is based on the IQR method, uses a sliding window to dynamically adjust the gross error threshold value based on the propeller speed, and judges the gross error condition through the gross error threshold value.

[0146] Embodiment 3

[0147] In an embodiment of the present application, a computer program product is provided, comprising a computer program, which, when executed by a processor, implements the speed prediction-based underwater integrated navigation method.

[0148] Embodiment 4

[0149] In an embodiment of the present application, a non-transitory computer readable storage medium is provided for storing computer instructions, which, when executed by a processor, implement the speed prediction-based underwater integrated navigation method.

[0150] Embodiment 5

[0151] ​An embodiment of the present application provides an electronic device, comprising a processor, a memory and a computer program; wherein the processor is connected with the memory, and the computer program is stored in the memory; when the electronic device is running, the processor executes the computer program stored in the memory, so that the electronic device executes the method for underwater integrated navigation based on speed prediction.

[0152] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems) and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of the flows and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing devices to produce a machine, so that the instructions executed by the computer or other programmable data processing devices generate a device that realizes the functions specified in the flowcharts and / or block diagrams. Figure 1 The functions specified in one flow or multiple flows and / or blocks Figure 1 The functions specified in one flow or multiple flows and / or blocks

[0153] The computer program instructions can also be loaded to a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to produce a computer-implemented process, so that the instructions executed on the computer or other programmable device provide a device for realizing the functions specified in the flowcharts and / or block diagrams. Figure 1 The functions specified in one flow or multiple flows and / or blocks Figure 1 The functions specified in one flow or multiple flows and / or blocks

[0154] Although the specific embodiments of the present application are described above with reference to the drawings, the present application is not limited to the above embodiments, and various modifications or changes can be made on the basis of the technical solutions of the present application without creative labor, which are still within the protection scope of the present application.

Claims

1. A speed prediction based underwater integrated navigation method, characterized in that, The underwater vehicle performs underwater integrated navigation by means of an inertial navigation system INS, a Doppler velocity log DVL and a pressure sensor PS, comprising: Obtaining observation data of the inertial navigation system INS, the Doppler velocity log DVL and the pressure sensor PS, wherein when the observation speed of the Doppler velocity log DVL meets a gross error condition, the observation speed is predicted based on the modified propeller speed; Based on the observation data, the IMU pre-integration factor, the DVL factor and the PS factor are calculated, a factor graph is constructed, the observation data is fused through a factor graph optimization method, and the final underwater vehicle state including position coordinates, speed, attitude, accelerometer and gyroscope zero bias errors is obtained; The gross error condition is based on an IQR method, a sliding window is used in combination with the propeller speed to dynamically adjust the gross error threshold, and the gross error condition is judged by the gross error threshold. The IQR method uses a sliding window in combination with the propeller speed to dynamically adjust the gross error threshold, which is expressed by the formula: wherein represents the lower bound of the rough error detection at time represents the upper bound of the rough error detection at time , and , and represent the third and first quartiles of the dataset of the difference between the DVL observed velocity and the system state velocity within the time window at time represents the difference between the system state velocity and the DVL observed velocity at time represents the target propeller speed at time represents the actual measured speed of the propeller at time represents the actual measured speed of the propeller at time​​​​​​ 2. The velocity prediction based underwater integrated navigation method of claim 1, wherein, The observation speed of the Doppler velocity log DVL meeting the gross error condition is realized by gross error detection using the gross error threshold.

3. The velocity prediction based underwater integrated navigation method of claim 1, wherein, The modified propeller speed is based on the detection result of the power change of the propeller, and when the power is increased or decreased, the propeller speed is modified based on the NLS method.

4. A velocity prediction based underwater integrated navigation method as claimed in claim 3, wherein, The power change detection is performed by a speed differential equation to judge the power increase, power decrease and power unchanged stages.

5. The velocity prediction based underwater integrated navigation method of claim 1, wherein, The observation speed is predicted by a trained Transformer-LSTM model, taking the INS recursive attitude, the IMU pre-integration position, speed and attitude relative changes in the sampling interval, the PS depth observation and the modified propeller speed as inputs.

6. A velocity-prediction-based underwater integrated navigation system, characterized in that, The underwater vehicle performs 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 observation speed of the Doppler velocity log DVL meets a gross error condition, the observation speed is predicted based on the modified propeller speed; The data fusion module is configured to calculate the IMU pre-integration factor, the DVL factor and the PS factor based on the observation data, construct a factor graph, fuse the observation data through a factor graph optimization method, and obtain the final underwater vehicle state including position coordinates, speed, attitude, accelerometer and gyroscope zero bias errors. The gross error condition is based on an IQR method, a sliding window is used in combination with the propeller speed to dynamically adjust the gross error threshold, and the gross error condition is judged by the gross error threshold. The IQR method uses a sliding window in combination with the propeller speed to dynamically adjust the gross error threshold, which is expressed by the formula: 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. A computer program product comprising a computer program, characterized in that, The computer program is executed by the processor to realize the underwater integrated navigation method based on speed prediction of any one of claims 1-5.

8. A non-transitory computer-readable storage medium, comprising: The non-transitory computer readable storage medium is configured to store computer instructions, and the computer instructions are configured to be executed by a processor to implement the underwater integrated navigation method based on speed prediction according to any one of claims 1-5.

9. An electronic device, comprising: The method comprises: A processor, a memory and a computer program, wherein the processor is connected with 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 according to any one of claims 1-5.

Images