A SINS / DVL integrated navigation method and system based on state prior Transformer

Abstract

The present application belongs to the technical field of underwater autonomous navigation, and particularly relates to a SINS / DVL integrated navigation method and system based on state prior Transformer; comprising constructing a reference fusion framework based on error state extended Kalman filtering, for fusing inertial navigation system and Doppler velocity log data; designing and training a state prior Transformer prediction model; during interruption of Doppler velocity log signal, inputting the virtual speed observation value generated in step S2 into the reference fusion framework as pseudo-measurement value for measurement updating, and utilizing the updated error state to perform closed-loop correction on navigation parameters. By introducing the optimal filtering speed at the last moment as prior Token, a discrete Markov constraint is established in the neural network, so that the prediction process becomes conditional incremental prediction rather than direct fitting of absolute state, and the phase lag and long tail error in sequence prediction are significantly inhibited.

Description

Technical Field

[0001] This invention belongs to the field of underwater autonomous navigation technology, specifically relating to a SINS / DVL integrated navigation method and system based on state prior Transformer. Background Technology

[0002] In long-endurance, highly stealthy missions, autonomous underwater vehicles (AUVs) typically employ a combination of a system of inertial navigation systems (SINS) and a Doppler velocimeter (DVL) using error-state Kalman filtering for positioning and navigation. However, in complex underwater environments, factors such as terrain obstruction and acoustic interference can cause frequent intermittent interruptions in the Doppler velocimeter signal. During periods of Doppler velocimeter signal loss, the inherent errors of the inertial navigation system accumulate rapidly, leading to a rapid divergence in the navigation solution.

[0003] In existing technologies, some data-driven methods attempt to use neural networks to establish a mapping from inertial measurement unit data to virtual velocity in order to bridge the blind spots of Doppler velocimeters. However, traditional end-to-end sequence prediction models lack explicit historical state feedback mechanisms, which easily leads to problems such as prediction phase lag and error cascading accumulation, making it difficult to meet the navigation requirements of high precision and long endurance. Summary of the Invention

[0004] The purpose of this invention is to provide a SINS / DVL integrated navigation method and system based on state prior Transformer, to solve at least one of the above-mentioned technical problems.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A SINS / DVL integrated navigation method based on state prior Transformer includes the following steps: Step S1: Construct a benchmark fusion framework based on error state extended Kalman filtering for fusing data from inertial navigation systems and Doppler velocimeters; Step S2: Design and train a state-prior Transformer prediction model, which is used to generate virtual velocity observations during Doppler velocimeter signal interruptions; the model generation process includes: The optimal filtering velocity output by the benchmark fusion framework described in the previous moment is encoded as a state prior token; Temporal features are extracted from the current inertial measurement unit data window using a single-scale convolutional neural network; The state prior token is concatenated with the temporal features and input into the Transformer network for autoregressive processing to predict the velocity increment at the current moment. Based on the optimal filtered velocity of the previous moment and the velocity increment, the virtual velocity observation value of the current moment is synthesized. Step S3: During the Doppler velocimeter signal interruption, the virtual velocity observation value generated in step S2 is used as a pseudo measurement value and input into the reference fusion framework in step S1 for measurement update, and the updated error state is used to perform closed-loop correction of navigation parameters.

[0006] Furthermore, the error state extended Kalman filter in step S1 maintains a 15-dimensional error state vector. The error state vector includes position error. Speed ​​error Attitude error Accelerometer zero bias and gyroscope zero bias : (1); By performing a first-order Taylor expansion of the SINS rigid body kinematic equations, the continuous error state equation can be expressed as: (2); Where w(t) represents a continuous Gaussian white noise vector containing sensor noise. Proceeding to discrete time step k, one-step pre-... The measured covariance is updated as follows: (3), The state transition matrix can be approximated as: Q k This is the discretized process noise covariance matrix.

[0007] Furthermore, the single-scale convolutional neural network in step S2 is a one-dimensional convolutional neural network with a kernel size of 5, and is sequentially connected with a batch normalization layer and a GELU activation function layer.

[0008] Furthermore, the process of encoding the state prior into a token specifically involves mapping the optimal filtering velocity vector of the previous time step to a latent dimension space with the same temporal features using a trainable linear projection matrix.

[0009] Furthermore, in the autoregressive processing of the Transformer network, the state prior token serves as the query vector for the multi-head self-attention mechanism, used to extract dynamic velocity increment information from subsequent temporal features.

[0010] Furthermore, the loss function used by the state prior Transformer prediction model during the offline training phase is a weighted sum of mean squared error loss and L2 regularization term.

[0011] Furthermore, including: The reference fusion module is configured to run the error state extended Kalman filter algorithm to fuse data from the strapdown inertial navigation system and the Doppler velocimeter. The state prior Transformer prediction module is configured to perform the function of step S2 as described in claim 1, for generating virtual velocity observations when the Doppler velocimeter signal is interrupted. The signal monitoring and switching module is configured to monitor the validity of the Doppler velocimeter signal in real time, and when the signal is interrupted, replace the actual Doppler velocimeter measurement value with the output of the state prior Transformer prediction module and connect it to the reference fusion module.

[0012] Furthermore, the state prior Transformer prediction module includes: A single-scale one-dimensional convolutional neural network submodule is used to extract temporal features from inertial measurement unit data; A state prior encoding submodule is used to convert the optimal filtering speed of the previous time step into a token vector; A Transformer submodule is used to perform self-attention calculation on the concatenated state prior token and convolutional neural network features; A regression output submodule is used to predict the velocity increment based on the attention calculation results and synthesize the final virtual velocity observation.

[0013] An autonomous underwater vehicle comprising the SINS / DVL integrated navigation system based on state prior Transformer as described in any of the preceding claims.

[0014] The beneficial effects of adopting the technical solution of the present invention are: By introducing the optimal filtering speed from the previous time step as a prior token, a discrete Markov constraint is established in the neural network, making the prediction process a conditional incremental prediction rather than a direct fit of the absolute state, which significantly suppresses phase lag and long-tail error in sequence prediction.

[0015] In an extreme test environment simulating a periodic 15-second acoustic signal loss, the method of this invention reduces the root mean square error of velocity prediction to 0.0822 m / s and strictly controls the position drift error throughout the cycle to within 4.27 meters, which is significantly better than pure inertial navigation solution, traditional recurrent neural network and Transformer model without prior knowledge.

[0016] Employing a single-scale CNN design, it effectively controls computational complexity while ensuring the receptive field, making it suitable for deployment on embedded platforms with limited computing power. Attached Figure Description

[0017] Figure 1 The state-priority Transformer (SSCP-Transformer) network architecture diagram provided for this invention.

[0018] Figure 2 The graph shows the probability distribution and kernel density estimation of velocity prediction errors during different DVL interruptions for the present invention and the comparative method.

[0019] Figure 3 This is a comparison diagram of the two-dimensional position trajectories of the present invention and the comparative method under the condition of periodic DVL interruption. Detailed Implementation

[0020] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to specific examples. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0021] A SINS / DVL integrated navigation method based on state prior Transformer includes the following steps: Step S1: Construct a benchmark fusion framework based on error state extended Kalman filtering for fusing data from inertial navigation systems and Doppler velocimeters; Step S2: Design and train a state-prior Transformer prediction model, which is used to generate virtual velocity observations during Doppler velocimeter signal interruptions; the model generation process includes: The optimal filtering velocity output by the benchmark fusion framework described in the previous moment is encoded as a state prior token; Temporal features are extracted from the current inertial measurement unit data window using a single-scale convolutional neural network; The state prior token is concatenated with the temporal features and input into the Transformer network for autoregressive processing to predict the velocity increment at the current moment. Based on the optimal filtered velocity of the previous moment and the velocity increment, the virtual velocity observation value of the current moment is synthesized. Step S3: During the Doppler velocimeter signal interruption, the virtual velocity observation value generated in step S2 is used as a pseudo measurement value and input into the reference fusion framework in step S1 for measurement update, and the updated error state is used to perform closed-loop correction of navigation parameters.

[0022] Furthermore, the error state extended Kalman filter in step S1 maintains a 15-dimensional error state vector. The error state vector includes position error. Speed ​​error Attitude error Accelerometer zero bias and gyroscope zero bias : (1); By performing a first-order Taylor expansion of the SINS rigid body kinematic equations, the continuous error state equation can be expressed as: (2); Where w(t) represents a continuous Gaussian white noise vector containing sensor noise. Proceeding to discrete time step k, one-step pre-... The measured covariance is updated as follows: (3), The state transition matrix can be approximated as: Q k This is the discretized process noise covariance matrix.

[0023] Measurement Updates and Closed-Loop Feedback: During the effective period of the actual DVL measurements, or when relying on virtual predictions from neural networks during signal interruptions, the measurement equations are uniformly linearized as follows: (4), The corresponding observation matrix structure is shown in the following equation: (5), To maintain observability during DVL interruptions, the synthesis rate predicted by the SSCP-Transformer will be used as... The input is given to this update interface. We define the virtual measurement noise covariance as... ,in This represents the nominal DVL noise variance. The system ultimately achieves state updates using the standard Kalman equations: (6), (7), (8), The calculated optimal error state The error correction is then fed back to compensate the navigation equations and reset to zero after compensation, thus completing the closed loop of error correction. Example

[0024] This embodiment provides a SINS / DVL integrated navigation method based on state prior Transformer. Its overall process includes two stages: offline training and online application.

[0025] (1) Data preparation: Collect AUV navigation data containing SINS data and valid DVL data. Periodic DVL signal loss (e.g., once every 30 seconds for 15 seconds) is artificially introduced into the data to generate training samples.

[0026] (2) Baseline filtering: Run the standard ES-EKF using complete SINS / DVL data and save the optimal filtering speed at each time step. , as a priori label for the state.

[0027] (3) Model building and training: Building such as Figure 1 The SSCP-Transformer network shown.

[0028] Input: 6-dimensional IMU data within a causal time sliding window And the optimal filtering speed at the previous moment. , Feature extraction: The IMU data is processed using a single-scale 1D-CNN with a kernel size of k=5. After batch normalization and GELU activation, the feature matrix is ​​output. .

[0029] State prior tokenization: through linear projection Map the velocity vector to a dimension, d model =64.

[0030] Transformer processing: Concatenates the tokenized state prior with CNN features. After adding positional encoding, the input is processed by a Transformer. The attention mechanism uses V... token For the query, extract dynamic speed increment information from subsequent features.

[0031] Output and Loss: Extract the output corresponding to the first token and obtain the velocity increment prediction through MLP regression. A combination of MSE and L2 regularization is used as the loss function for network training.

[0032] Example 2: SSCP-Transformer Network Design The SSCP-Transformer architecture and its underlying state feedback fusion logic of this invention are as follows: Figure 1 As shown.

[0033] Following the hybrid modeling concept, the state prior token is concatenated with the IMU features extracted by CNN to achieve autoregressive conditional incremental prediction.

[0034] IMU signals acquired by underwater vehicles typically contain significant noise. To accommodate the computational limitations of embedded platforms, this architecture employs a compact, single-scale cascaded 1D-CNN. The system operates within a causal time sliding window. (N=100) The process is as follows: 1D-CNN is run on... (9), The single-scale design effectively expands the receptive field, while the GELU activation function smoothly truncates high-frequency pulses, resulting in a stable dynamic feature matrix. .

[0035] To apply physically meaningful autoregressive constraints, this invention extracts the optimal filtering velocity at time k−1. As a state prior, it is encoded as a StatePriorToken. Linear projection maps this physical quantity to a latent dimension d. model =64: (10) spliced ​​sequence After adding positional encoding, the input is performed in a Transformer for multi-head self-attention computation. Here, V... token Serving as the baseline query vector, actively from subsequent X cnn The dynamic velocity increment is extracted from the features, which makes the entire time-series feature extraction process subject to strict boundary constraints of the physical state of the previous moment.

[0036] Nonlinear regression and feedback loop After the attention mechanism is applied, the feature vector associated with the first token is extracted. Input the MLP regression layer to calculate the three-dimensional velocity increment at the current moment. The loss function used in the offline training phase combines mean squared error (MSE) with an L2 regularization term: (11), During online inference, this predicted velocity increment is combined with the baseline velocity to obtain the final virtual measurement: (12) Ultimately, the neural network output is used as a pseudo-measurement. The measurement update equations, which are fed back to the ES-EKF system in real time, complete a robust closed loop that includes residual prediction and correction based on state feedback.

[0037] To verify the effectiveness of this invention, rigorous trajectory-level physical isolation tests were conducted on the open-source A-KIT lake test dataset. The system's survivability was evaluated by artificially introducing an extreme boundary condition where a 15-second complete loss of the DVL signal occurs every 30 seconds. Experimental verification results: Trajectory simulations were performed under the extreme conditions of the periodic DVL interruption, and the results are shown in Table 1. Figure 2 , Figure 3 As shown. The method of the present invention (SSCP-TF) achieves optimal performance: The speed RMSE is 0.0822 m / s, which is significantly better than that of traditional RNN (0.1306 m / s) and pure Transformer (0.1394 m / s).

[0038] The position RMSE is 4.27m, which is significantly better than pure inertial navigation (26.60m), pure Transformer (5.57m) and traditional RNN (5.08m).

[0039] The above experimental data strongly demonstrate the effectiveness and superiority of the state prior feedback mechanism and autoregressive conditional prediction architecture proposed in this invention.

[0040] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent claim. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A SINS / DVL integrated navigation method based on state prior Transformer, characterized in that, Includes the following steps: Step S1: Construct a benchmark fusion framework based on error state extended Kalman filtering for fusing data from inertial navigation systems and Doppler velocimeters; Step S2: Design and train a state-prior Transformer prediction model to generate virtual velocity observations during Doppler velocimeter signal interruptions; The model generation process includes: The optimal filtering velocity output by the benchmark fusion framework described in the previous moment is encoded as a state prior token; Temporal features are extracted from the current inertial measurement unit data window using a single-scale convolutional neural network; The state prior token is concatenated with the temporal features and input into the Transformer network for autoregressive processing to predict the velocity increment at the current moment. Based on the optimal filtered velocity of the previous moment and the velocity increment, the virtual velocity observation value of the current moment is synthesized. Step S3: During the Doppler velocimeter signal interruption, the virtual velocity observation value generated in step S2 is used as a pseudo measurement value and input into the reference fusion framework in step S1 for measurement update, and the updated error state is used to perform closed-loop correction of navigation parameters.

2. The method according to claim 1, characterized in that, The error state extended Kalman filter in step S1 maintains a 15-dimensional error state vector. The error state vector includes position error. Speed ​​error Attitude error Accelerometer zero bias and gyroscope zero bias : （1）； By performing a first-order Taylor expansion of the SINS rigid body kinematic equations, the continuous error state equation can be expressed as: （2）； Where w(t) represents a continuous Gaussian white noise vector containing sensor noise. Proceeding to discrete time step k, one-step pre-... The measured covariance is updated as follows: （3） The state transition matrix can be approximated as: Q k This is the discretized process noise covariance matrix.

3. The method according to claim 1, characterized in that, The single-scale convolutional neural network in step S2 is a one-dimensional convolutional neural network with a kernel size of 5, and is sequentially connected with a batch normalization layer and a GELU activation function layer.

4. The method according to claim 1, characterized in that, The process of encoding the state prior into a token is specifically as follows: using a trainable linear projection matrix, the optimal filtered velocity vector of the previous time step is mapped to a latent dimension space with the same temporal features.

5. The method according to claim 1, characterized in that, In the autoregressive processing of the Transformer network, the state prior token serves as the query vector for the multi-head self-attention mechanism, used to extract dynamic velocity increment information from subsequent temporal features.

6. The method according to claim 1, characterized in that, The state prior Transformer prediction model uses a loss function that is a weighted sum of mean squared error loss and L2 regularization term during the offline training phase.

7. A SINS / DVL integrated navigation system based on state prior Transformer, characterized in that, include: The reference fusion module is configured to run the error state extended Kalman filter algorithm to fuse data from the strapdown inertial navigation system and the Doppler velocimeter. The state prior Transformer prediction module is configured to perform the function of step S2 as described in claim 1, for generating virtual velocity observations when the Doppler velocimeter signal is interrupted. The signal monitoring and switching module is configured to monitor the validity of the Doppler velocimeter signal in real time, and when the signal is interrupted, replace the actual Doppler velocimeter measurement value with the output of the state prior Transformer prediction module and connect it to the reference fusion module.

8. The system according to claim 7, characterized in that, The state prior Transformer prediction module includes: A single-scale one-dimensional convolutional neural network submodule is used to extract temporal features from inertial measurement unit data; A state prior encoding submodule is used to convert the optimal filtering speed of the previous time step into a token vector; A Transformer submodule is used to perform self-attention calculation on the concatenated state prior token and convolutional neural network features; A regression output submodule is used to predict the velocity increment based on the attention calculation results and synthesize the final virtual velocity observation.

9. An autonomous underwater vehicle, characterized in that, The SINS / DVL integrated navigation system based on state prior Transformer as described in any one of claims 7 or 8.

Images