Robust SINS / DVL integrated navigation method, system and equipment based on Transform model

Abstract

The invention provides a robust SINS / DVL integrated navigation method, system and device based on a Transform model, and belongs to the technical field of underwater navigation. According to the underwater integrated navigation method, the underwater integrated navigation equipment and the underwater integrated navigation system, which are more accurate and robust, are provided by combining the M-estimated Kalman filtering and the Transform neural network. When a DVL bottom measurement signal is lost, the system can still continuously provide accurate navigation information by means of the prediction capability of the Transform. And secondly, an M estimation method based on a Huber algorithm is introduced, abnormal values in data are effectively inhibited, the stability and reliability of Kalman filtering are improved, and high-precision performance of the system in a complex environment is ensured.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater navigation, and particularly relates to a robust SINS / DVL integrated navigation method, system and device based on a Transformer model. Background Technique

[0002] High-precision navigation and positioning are necessary conditions for ensuring that underwater vehicles correctly carry out underwater activities and execute underwater tasks. The strapdown inertial navigation system (SINS) can provide complete navigation parameters and has a high update frequency, which is an important means to achieve underwater autonomous navigation. However, due to the accumulation of errors in the inertial navigation system over time, it is difficult to meet the requirements of long-duration high-precision navigation. Therefore, additional auxiliary sensors are needed. The Doppler velocity log (DVL) is an instrument that measures the velocity of a vehicle based on the Doppler effect and can exactly compensate for the problem of error accumulation in SINS during long-duration navigation. Therefore, to achieve long-duration and high-precision autonomous underwater navigation, an integrated navigation system in the form of SINS / DVL is mostly adopted, and integrated navigation is carried out in the form of Kalman filtering.

[0003] Under normal circumstances, the DVL operates in the bottom measurement mode, measuring the velocity of the underwater vehicle relative to the bottom of the water, and can provide relatively accurate velocity data. However, during deep diving, as the depth of the underwater vehicle increases, the bottom measurement mode may become unusable due to signal failure. At this time, the DVL will switch to the current measurement mode, measuring the velocity of the underwater vehicle relative to the water current. Therefore, the obtained velocity is a relative velocity and cannot directly reflect the absolute velocity of the underwater vehicle. Moreover, affected by the change of ocean current, the data will have errors, reducing the accuracy and reliability of the navigation system. In addition, sensor data may also produce outliers due to external interference, equipment errors or transmission distortion. If not processed, it will affect the system performance and reduce the navigation accuracy.

[0004] Therefore, it is really necessary to provide a robust SINS / DVL integrated navigation method, system and device based on a Transformer model to solve the above problems. Summary of the Invention

[0005] The present invention provides a robust SINS / DVL integrated navigation method, system and device based on the Transformer model. With its powerful global dependence modeling ability, the Transformer network can process the entire input sequence simultaneously, accelerate the training and inference speed, and can process complex data of long time series, avoiding the problems of gradient disappearance or gradient explosion. In addition, to solve the influence of outliers on the state estimation of the iterated cubature Kalman filter and model training, a robust M-estimation method using approximate least absolute deviation estimation is adopted. Through this combined method, while solving the problem of DVL bottom measurement signal loss, the robustness and reliability of the integrated navigation system in complex environments can be significantly improved, and at least one technical problem involved in the background technology can be effectively solved.

[0006] To solve the above technical problems, the present invention is implemented as follows:

[0007] A robust SINS / DVL integrated navigation method based on the Transformer model includes the following steps:

[0008] Step S1: Taking the errors of SINS and DVL as the state vector and the difference between the measured velocity in the DVL bottom measurement mode and the calculated velocity of SINS as the observation value, establish a discrete nonlinear state equation of the SINS / DVL integrated navigation system and construct a Kalman filter.

[0009] Step S2: Project the observation value into the state error space. When the observation residual is greater than the preset threshold, design a correction factor on the basis of the traditional M-estimation algorithm to correct the observation noise covariance, and use the approximate least absolute deviation estimation method to fully evaluate all dimensions of the observation residual to estimate the error state of the SINS / DVL integrated navigation system, suppressing the influence of abnormal observation residuals, and estimating the state of the SINS / DVL integrated navigation system based on the Kalman filter.

[0010] Step S3: Construct a data set, which includes the acceleration and angular velocity measured by SINS and the measured flow velocity measured by DVL. Take the system state data estimated by the Kalman filter as the label of the data at the corresponding time node in the data set, and send the data set and the corresponding label into the Transformer model for training to learn the mapping relationship from the data to the label.

[0011] Step S4: For any SINS / DVL navigation process, if the DVL can obtain the bottom measurement velocity, estimate the state of the SINS / DVL integrated navigation system based on the Kalman filter; if the DVL cannot obtain the bottom measurement velocity, send the measured flow velocity measured by the DVL and the acceleration and angular velocity measured by the SINS into the trained Transformer model to predict the state of the SINS / DVL integrated navigation system.

[0012] As a preferred improvement, the attitude error equation, velocity error equation, and position error equation of SINS are expressed as:

[0013]

[0014] In the formula, represents the first-order differential of the SINS attitude error, indicating the rate of change of the attitude error with time; C w represents the coefficient matrix of the Euler platform error angle differential equation, represents the direction cosine matrix from the n-system to the n'-system; represents the angular velocity of the n-system projected onto the i-system; represents the angular velocity of the n-system projected onto the i-system; represents the angular velocity error of the n-system projected onto the i-system; represents the direction cosine matrix from the b-system to the n'-system; ε b represents the gyro zero bias in the b-system; represents the first-order differential of the SINS velocity error, which is the rate of change of the velocity error with time; I represents the identity matrix; T represents the transpose matrix; represents the specific force in the b-system; represents the accelerometer error in the b-system; represents the earth rate vector error in the n-system; represents the angular velocity error of the n-system projected onto the e-system; v n represents the SINS velocity measurement value; represents the earth rate vector in the n-system; represents the angular velocity of the n-system projected onto the e-system; δv n represents the SINS velocity error; g n represents the gravitational acceleration; represents the first-order differential of the SINS latitude error, which is the rate of change of the latitude error with time; RM represents the radius of curvature of the meridian; h represents the SINS altitude measurement value; δv N represents the SINS northward velocity error; δh represents the SINS altitude error; represents the first-order differential of the SINS longitude error, which is the rate of change of the SINS longitude error with time; L represents the SINS longitude measurement value; δL represents the SINS longitude error; δv E represents the SINS eastward velocity error; v E represents the SINS eastward velocity measurement value; RN represents the radius of curvature of the prime vertical; δL represents the SINS longitude error; δh represents the SINS altitude error; represents the first-order differential of the SINS altitude error, which is the rate of change of the SINS altitude error with time; δν Udenotes the SINS celestial velocity error; γ denotes the roll angle of the underwater vehicle, denotes the heading angle of the underwater vehicle;

[0015] The discrete nonlinear state equation of the SINS / DVL integrated navigation system is expressed as:

[0016] x k+1 = f(x k ) + w k ;

[0017] where, x k denotes the state vector of the system at the k-th moment, and x k+1 denotes the state vector at the (k + 1)-th moment; f(·) denotes the nonlinear function; w k denotes the noise vector; among which:

[0018]

[0019] where, φ n denotes the SINS attitude error, φ n = [φ E , φ N , φ U T , φ E , φ N , φ U respectively denote the pitch angle error, roll angle error, and heading angle error; δvn = [δv E , δv N , δv U T; δp n denotes the SINS position error, δp n = [δλ, δL, δh] T , δλ denotes the SINS longitude error; ε b = [ε x , ε y , ε z T , ε x , ε y , ε z respectively denote the gyro zero biases in the x, y, and z axis directions of the b-frame; respectively denote the accelerometer errors in the x, y, and z axis directions of the b-frame; K d denotes the scale factor error of the DVL module;

[0020] The observation equation of the SINS / DVL integrated navigation system is expressed as:

[0021]

[0022] ​​wherein represents the velocity estimated by the SINS in the b frame; represents the velocity measured by the DVL in the b frame; represents the true velocity of the underwater vehicle during the DVL measurement in the b frame, i.e., the ideal value without error correction of the scale factor Kd.

[0023] As a preferred improvement, the observation residual ζ is defined as follows:

[0024]

[0025] wherein represents the system state estimation vector; z k represents the observation vector; H k represents the observation matrix;

[0026] Design a correction factor ψ k on the basis of the traditional M-estimation algorithm, and use the approximate least absolute function to construct the optimization objective of the observation residual, which is expressed as:

[0027]

[0028] wherein, ζ i represents the i-th observation residual between the model prediction value and the actual observation value; J(ζ i ) represents the optimization objective of the observation residual ζ i ; γ represents a preset threshold value, with a value of 1.345; σ represents the smallest real number used to ensure the continuity of the derivative of the cost function J(ζ i ); represents the average value of the observations during the t-th iteration at time k; R represents the observation noise covariance;

[0029] During each iteration, update the observation noise covariance, and the update process is as follows:

[0030]

[0031] wherein represents the corrected value of the observation covariance after the t-th iteration at time k; represents the weight matrix of the observation covariance during the t-th iteration at time k; represents the correction factor of the observation covariance during the t-th iteration at time k;

[0032] As a preferred improvement, the Transformer model includes an encoder and a decoder. The input data is processed by the multi-head attention sub-layer in the encoder. Multiple attention heads can capture the dependencies between SINS and DVL data in different sub-spaces. Subsequently, the representation ability of the model is further enhanced through a normalization layer and a fully connected feed-forward network sub-layer. The output of the encoder will be used as the input of the decoder. The decoder also includes a multi-head attention sub-layer and an encoder-decoder multi-head attention sub-layer. These sub-layers process the input data through residual connections and normalization layers, and finally the output layer generates the prediction result. To process the sequential information in time series data, the decoder input layer maps each data point to a dmodel-dimensional vector through a positional encoder and adds it to the positional encoding vector to learn the temporal dependencies of SINS and DVL data and accurately predict the system state.

[0033] As a preferred improvement, the loss function for training the Transformer model is expressed as:

[0034]

[0035] In the formula, represents the true value of the i-th sample, provided by external true data; represents the predicted value of the i-th sample, generated by the Transformer model; n represents the total number of samples.

[0036] A robust SINS / DVL integrated navigation system based on the Transformer model includes:

[0037] A Kalman filter module, which is used to establish a discrete non-linear state equation of the SINS / DVL integrated navigation system and construct a Kalman filter with the errors of SINS and DVL as the state vector and the difference between the measured velocity in the DVL bottom measurement mode and the estimated velocity of SINS as the observation value;

[0038] An observation residual optimization module, which is used to project the observation value into the state error space. When the observation residual is greater than the preset threshold, a correction factor is designed based on the traditional M-estimation algorithm to correct the observation noise covariance. The approximate least absolute deviation estimation method is used to fully evaluate all dimensions of the observation residual to estimate the error state of the SINS / DVL integrated navigation system, suppress the influence of abnormal observation residuals, and estimate the state of the SINS / DVL integrated navigation system based on the Kalman filter;

[0039] The Transformer model construction module is used to construct a dataset. The dataset includes the acceleration and angular velocity measured by SINS and the flow velocity measured by DVL. The system state data estimated by Kalman filter is used as the label of the data at the corresponding time node in the dataset. The dataset and the corresponding label are fed into the Transformer model for training to learn the mapping relationship from data to label;

[0040] The prediction module, for any SINS / DVL navigation process, if DVL can obtain the bottom velocity, then the state of the SINS / DVL integrated navigation system is estimated based on Kalman filter; if DVL cannot obtain the bottom velocity, then the flow velocity measured by DVL and the acceleration and angular velocity measured by SINS are fed into the trained Transformer model to predict the state of the SINS / DVL integrated navigation system.

[0041] A robust SINS / DVL integrated navigation device based on the Transformer model, including a memory and a processor. Among them, the memory stores executable instructions, and the processor is configured to execute the executable instructions in the memory to implement the steps of the above-mentioned robust SINS / DVL integrated navigation method based on the Transformer model.

[0042] The beneficial effects of the present invention are as follows:

[0043] (1) The present invention uses the M-estimation algorithm to preprocess the training data, effectively suppressing the influence of outliers on model training, and ensuring that the navigation system can still maintain high accuracy and stability in the presence of noise or data loss;

[0044] (2) By comprehensively evaluating each measurement dimension to adjust the measurement noise covariance, better numerical stability and higher accuracy can be obtained in most cases than the traditional M-estimation algorithm based on the Huber function. It not only provides better numerical stability than the M-estimation algorithm based on the Huber function in the early stage, but also can improve the training accuracy and speed of the model in the later stage;

[0045] (3) The present invention uses the Transformer network to replace the traditional LSTM network. By using the self-attention mechanism of the Transformer, it can capture the global dependencies in the data and better process long time series data. Compared with LSTM, the Transformer has higher efficiency and stronger stability in long time series modeling. The Transformer network can not only handle the temporal dependencies between SINS and DVL, but also improve the training speed and prediction performance through its parallel computing advantages. Brief Description of the Drawings

[0046] To more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings, where:

[0047] Figure 1 It represents the flow framework diagram of the robust SINS / DVL integrated navigation method provided by the present invention based on the Transformer model;

[0048] Figure 2 It represents the architecture diagram of the Transformer model;

[0049] Figure 3 It represents a usage state flow diagram of the robust SINS / DVL integrated navigation method provided by the present invention based on the Transformer model;

[0050] Figure 4 It represents another usage state flow diagram of the robust SINS / DVL integrated navigation method provided by the present invention based on the Transformer model. Specific Embodiments

[0051] The following will combine the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of them. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0052] As Figures 1-4 shown, this embodiment provides a robust SINS / DVL integrated navigation method based on the Transformer model, including the following steps:

[0053] Step S1: Taking the errors of SINS and DVL as the state vector, and the difference between the measured speed in the DVL bottom measurement mode and the estimated speed of SINS as the observation value, establish the discrete nonlinear state equation of the SINS / DVL integrated navigation system and construct the Kalman filter.

[0054] In this embodiment, the coordinate systems involved are defined as follows:

[0055] n system: Navigation coordinate system;

[0056] n' system: Navigation coordinate system with errors;

[0057] i system: Inertial coordinate system;

[0058] b system: body coordinate system;

[0059] e system: Earth-centered Earth-fixed coordinate system.

[0060] The above coordinate systems are marked in the upper right corner of each parameter in superscript form to represent the coordinate system in which the parameter is located.

[0061] SINS uses accelerometers and gyroscopes to measure the acceleration of an underwater vehicle and angular velocity and then estimates the measured values of the speed, position and attitude of the underwater vehicle. Due to the existence of SINS errors, there are certain deviations between the estimated measured values of speed, position and attitude and the true values; while DVL directly measures the speed of the underwater vehicle using the Doppler effect. In the bottom detection mode, DVL measures the absolute speed of the underwater vehicle. Therefore, the measured speed of DVL in the bottom detection mode is used to correct the estimation results of SINS.

[0062] The attitude error equation, speed error equation and position error equation of SINS are expressed as:

[0063]

[0064] In the formula, represents the first-order differential of the SINS attitude error, indicating the change rate of the attitude error with time; C w represents the coefficient matrix of the Euler platform error angle differential equation, represents the direction cosine matrix from the n system to the n' system; represents the angular velocity of the n system projected onto the i system; represents the angular velocity of the n system projected onto the i system; represents the angular velocity error of the n system projected onto the i system; represents the direction cosine matrix from the b system to the n' system; ε b represents the gyro zero bias in the b system; represents the first-order differential of the SINS speed error, which is the change rate of the speed error with time; I represents the identity matrix; T represents the transpose matrix; represents the specific force in the b system; represents the accelerometer error in the b system; represents the Earth rate vector error in the n system; represents the angular velocity error of the n system projected onto the e system; v n represents the SINS speed measurement value; represents the Earth rate vector in the n system; represents the angular velocity of the n system projected onto the e system; δv n represents the SINS speed error; g n represents the gravitational acceleration; represents the first-order differential of the SINS latitude error, which is the rate of change of the latitude error with respect to time; RM represents the radius of curvature of the meridian; h represents the SINS altitude measurement; δv N represents the SINS northward velocity error; δh represents the SINS altitude error; represents the first-order differential of the SINS longitude error, which is the rate of change of the SINS longitude error with respect to time; L represents the SINS longitude measurement; δL represents the SINS longitude error; δv E represents the SINS eastward velocity error; v E represents the SINS eastward velocity measurement; RN represents the radius of curvature of the prime vertical; δL represents the SINS longitude error; δh represents the SINS altitude error; represents the first-order differential of the SINS altitude error, which is the rate of change of the SINS altitude error with respect to time; δν U represents the SINS vertical velocity error; γ represents the roll angle of the underwater vehicle, represents the heading angle of the underwater vehicle.

[0065] The discrete nonlinear state equation of the SINS / DVL integrated navigation system is expressed as:

[0066] x k+1 = f(x k ) + w k ;

[0067] where, x k represents the state vector of the system at the k-th moment, and x k+1 represents the state vector at the (k + 1)-th moment; f(·) represents the nonlinear function; w k represents the noise vector; where:

[0068]

[0069] where, φ n represents the SINS attitude error, φ n = [φ E , φ N , φ U T , φ E , φ N , φ U represent the pitch angle error, roll angle error, and heading angle error respectively; δvn = [δv E , δv N , δv U T; δp n represents the SINS position error, δp n = [δλ, δL, δh] T ​, $\delta\lambda$ represents the longitude error of the SINS; $\varepsilon$ b = [$\varepsilon$ x , $\varepsilon$ y , $\varepsilon$ z T , $\varepsilon$ x , $\varepsilon$ y , $\varepsilon$ z respectively represent the gyro zero biases in the x, y, and z axis directions of the b-frame; respectively represent the accelerometer errors in the x, y, and z axis directions of the b-frame; K d represents the scale factor error of the DVL module.

[0070] The observation equation of the SINS / DVL integrated navigation system is expressed as:

[0071]

[0072] In the formula, represents the velocity deduced by the SINS in the b-frame; represents the velocity measured by the DVL in the b-frame; represents the true velocity of the underwater vehicle during the DVL measurement in the b-frame, that is, the ideal value without scale factor $K_d$ error correction.

[0073] Rewrite the observation equation into matrix form, which is expressed as:

[0074] z k = H k x k + v k ;

[0075] In the formula, z k represents the observation vector; H k represents the observation matrix; v k represents the observation noise.

[0076] The observation matrix H k is as follows:

[0077]

[0078] In the formula, represents the direction cosine matrix from the n-frame to the b-frame; C pq (p, q = 1, 2, 3) represents the elements in the matrix, reflecting the rotation relationship from the b-frame to the n-frame; $\nu$ N represents the northward velocity of the SINS; $\nu$ U represents the upward velocity of the SINS; represents the velocities of the DVL in the x, y, and z axis directions of the b-frame.

[0079] During the Kalman filter estimation process, the prior distribution of the system state is:

[0080]

[0081] where x k denotes the state vector at time k; Z k-1 denotes the set of all observed data up to time k - 1; p(x k |Z k-1 ) represents the prior probability density of state x k-1 given the historical observations Z k ; N(·) represents the Gaussian distribution; denotes the predicted mean of the state; P k|k-1 denotes the covariance matrix of the state prediction.

[0082] The likelihood distribution of the observation process is:

[0083] p(z k |x k ) = N(z k : h(x k ), R);

[0084] where z k denotes the observation vector at time k; p(z k |x k ) represents the likelihood probability density of the observation z k given state x k ; h(x k ) represents the observation function; R represents the observation noise covariance matrix.

[0085] During the Kalman filter estimation process, the posterior distribution of the system state is:

[0086]

[0087] where Z k denotes the set of all observed data up to time k;

[0088] Using the maximum a posteriori probability estimation to solve the posterior distribution of the state, the solution process is expressed as:

[0089]

[0090] where denotes the optimal state estimate value, which comprehensively considers the prior information of the system and the actual observation information, and balances the relationship between the two by minimizing the cost function; J(x k ) represents the cost function of state x k , which is the weighted sum of the prior error and the observation residual;

[0091] The result of the solution is represented in matrix form as:

[0092]

[0093] In the formula, w k represents the joint error vector of the state and the observation; Φ k represents the information matrix, which is used to represent the precision of the posterior distribution;

[0094] The final expression of the iterative solution obtained by using the Gauss-Newton method is as follows:

[0095]

[0096] In the formula, represents the state estimate value after the (t + 1)-th iteration; represents the cross-covariance matrix between the state x and the observation z at the t-th iteration; represents the self-covariance matrix of the observation z at the t-th iteration; represents the state estimate value based on the t-th iteration obtained from the observation value predicted by the observation model.

[0097] Step S2: Project the observation value into the state error space. When the observation residual is greater than the preset threshold, a correction factor is designed based on the traditional M-estimation algorithm to correct the observation noise covariance, and the approximate least absolute deviation estimation method is used to fully evaluate all dimensions of the observation residual to estimate the error state of the SINS / DVL integrated navigation system, suppress the influence of abnormal observation residuals, and estimate the state of the SINS / DVL integrated navigation system based on the Kalman filter.

[0098] The traditional Kalman filter is based on the following assumptions:

[0099] (1) The observation noise follows a Gaussian distribution: the residual (the difference between the observation value and the predicted value) is minimized by the squared loss (L2 norm), corresponding to the maximum likelihood estimation;

[0100] (2) Fixed covariance matrix: The observation noise covariance matrix R is assumed to be known and constant.

[0101] When there are outliers in the observation noise or it shows a non-Gaussian distribution, the squared loss is overly sensitive to large residuals, resulting in state estimation deviation, the assumption of the observation noise covariance matrix R fails, and further the Kalman gain is inaccurate, affecting the final navigation accuracy. To solve this problem, this application introduces the M-estimation algorithm, replaces the squared loss with a robust loss function, and suppresses the influence of outliers.

[0102] The observation residual is defined as follows:

[0103]

[0104] In the formula, represents the state estimation vector;

[0105] The M-estimation algorithm usually optimizes the observation residual in the way of maximum likelihood, and the optimization objective J(ζ i ) is expressed as:

[0106]

[0107] In the formula, m represents the measurement size; ζ i represents the i-th observation residual between the model prediction value and the actual observation value; ρ(ζ i ) represents the loss function.

[0108] The commonly used loss function of the M-estimation algorithm is the Huber function. The Huber function is defined by introducing a preset threshold. When the observation error is less than the preset threshold, the mean square error (MSE) is adopted. When the observation error is greater than the preset threshold, the mean absolute error (MAE) is adopted. It combines the smoothness of MSE and the robustness of MAE. The M-estimation algorithm based on the Huber function realizes the optimization of the observation residual through the iteratively reweighted least squares method. Its specific execution process is as follows: the weight function is constructed by the derivative of the Huber function, the weight function is applied to the observation residual to construct the weight matrix. In the observation update of the Kalman filter, the observation noise covariance is dynamically adjusted through the weight matrix. After introducing the weight matrix, the observation noise covariance corresponding to the outlier is amplified, resulting in a decrease in the corresponding component value in the Kalman gain, thereby suppressing the influence of the abnormal observation state on the update.

[0109] The Huber function is expressed as:

[0110]

[0111] In the formula, γ represents the preset threshold, and its value is 1.345;

[0112] Define the influence function φ(·) = ρ′(·), then:

[0113]

[0114] The weight function is expressed as:

[0115]

[0116] In the M-estimation algorithm based on the Huber function, outliers in several measurement dimensions are independently identified, that is, outliers are suppressed by evaluating the absolute value of individual residuals. However, not all measurement dimensions have large outliers. When the condition number of the weight matrix is very large, numerical stability problems will occur, which are caused by significantly different measurement residuals in different dimensions. To solve this problem, based on the idea of the equivalent weight function of the M-estimation algorithm using the Huber function, this application improves the optimization objective to fully evaluate all dimensions of the observation residuals. When there are large errors, approximate least absolute deviation estimation and correction factors are used to correct the observation error covariance, which can more comprehensively suppress the influence of outliers.

[0117] After the improvement, an approximate least absolute value function is used to construct the optimization objective, which is expressed as:

[0118]

[0119] In the formula, σ represents the smallest real number used to ensure the continuity of the derivative of the cost function; ψ k represents the correction factor, σ represents the smallest real number used to ensure the continuity of the derivative of the cost function; represents the average value of observations in the t-th iteration process at time k;

[0120] In each iteration process, the observation noise covariance is adjusted, and the adjustment process is as follows:

[0121]

[0122] In the formula, represents the weight matrix of the observation covariance in the t-th iteration process at time k; represents the correction factor of the observation covariance in the t-th iteration process at time k; represents the corrected value of the observation covariance after the t-th iteration at time k.

[0123] Step S3, construct a data set, which includes the acceleration and angular velocity measured by SINS and the flow velocity measured by DVL. Use the system state data estimated by Kalman filter as the label of the data at the corresponding time node in the data set, and send the data set and the corresponding label into the Transformer model for training to learn the mapping relationship from data to label.

[0124] The Transformer model includes an encoder and a decoder. The encoder is responsible for reading the input sequence and generating a continuous representation; the decoder then uses this representation to generate the output sequence. In each encoder and decoder, there are multiple self-attention layers and feed-forward neural network layers. The self-attention layers and feed-forward neural network layers work together to capture the mapping relationship from data to label in the input sequence.

[0125] The time series of N unit time is as follows:

[0126] ψ input ={ψ i-N+1 ,…ψ i-1 ,ψ i};

[0127] The training label is the 9 - dimensional information of DVL in the next M unit time:

[0128]

[0129] The Transformer model includes an encoder and a decoder. The encoder includes a stacked input layer, a position encoding layer, and four identical encoding layers. The four encoding layers include the first encoding layer, the second encoding layer, the third encoding layer, and the fourth encoding layer. In the input layer, the input time series ψ input is mapped into a vector with dimension dmodel through a fully - connected neural network. The position encoding layer performs position encoding on the vector with dimension dmodel using the sine - cosine position encoding method. Its position encoding formula is:

[0130]

[0131] In the formula, pos represents the position of the current object in the sequence at the current dimension; d pos represents the dimension where the position pos is located; 10000 2dpos / dmodel represents the frequency.

[0132] Each encoding layer includes two sub - layers, namely the multi - head self - attention sub - layer and the fully - connected feed - forward network sub - layer. A normalization layer is connected behind the multi - head attention sub - layer and the fully - connected feed - forward network sub - layer respectively. The multi - head attention sub - layer, the fully - connected feed - forward network sub - layer and their subsequent normalization layers are added in the form of residual connection.

[0133] The formula adopted by the multi - head attention sub - layer is:

[0134]

[0135] MultiHead(Q,K,V)=Concat(head1,head2,···,head h )W O ;

[0136] In the formula, please supplement the definition of the parameters; Q represents the query matrix; K represents the key matrix; V represents the value matrix; d k represents the factor for scaling the dot - product to prevent the value of the dot - product from being too large, resulting in the vanishing gradient of the softmax function; W represents the weight matrix.

[0137] The formula adopted by the feedforward neural network is as follows:

[0138] FFN(x) = max(0, xW1 + b1)W2 + b2;

[0139] In the formula, x represents the input feature matrix, b represents the bias vector of a certain layer, and FFN is the output of the feedforward neural network.

[0140] Residual connection and normalization formula:

[0141] Outlater = LaterNorm(x + Sublayer(x));

[0142] In the formula, Outlater represents the final output of each layer; LaterNorm(·) represents the layer normalization function, and Sublayer(·) represents the function implemented by each sublayer itself;

[0143] The Transformer neural network is used to process the complex nonlinear relationship between the SINS module and the DVL module. The input data is processed by the multi-head attention sublayer in the encoder. Multiple attention heads can capture the dependency relationships between the SINS and DVL data in different subspaces. Subsequently, the representation ability of the model is further enhanced through the normalization layer and the fully connected feedforward network sublayer. The output of the encoder will be used as the input of the decoder. The decoder part also includes the multi-head attention sublayer and the encoder-decoder multi-head attention sublayer. These sublayers process the input data through the residual connection and the normalization layer, and finally the output layer generates the prediction results. To process the sequential information in the time series data, the decoder input layer maps each data point to a dmodel-dimensional vector through the position encoder and adds it to the position encoding vector. This process enables the Transformer to effectively learn the temporal dependencies of the SINS and DVL data and accurately predict the attitude, velocity, and position of the carrier. When there is a bottom detection signal from the DVL, the Transformer network learns the nonlinear mapping between the input data and the target output through the self-attention mechanism; in the case of the loss of the DVL bottom detection signal, the Transformer can still predict the pose and velocity based on the state estimation provided by the Kalman filter, ensuring the stable operation of the system.

[0144] The predicted pose and velocity obtained through the Transformer model are compared with the true values, and the model is trained through the mean square error loss between the true values and the predicted values. The loss function is expressed as:

[0145]

[0146] In the formula, Denote the true value of the \(i\)-th sample, which is provided by external true data; Denote the predicted value of the \(i\)-th sample, which is generated by the Transformer model; \(n\) represents the total number of samples.

[0147] Step S4, for any SINS / DVL navigation process, if the DVL can obtain the bottom velocity, then estimate the state of the SINS / DVL integrated navigation system based on the Kalman filter; if the DVL cannot obtain the bottom velocity, then send the flow velocity measured by the DVL and the acceleration and angular velocity measured by the SINS into the trained Transformer model to predict the state of the SINS / DVL integrated navigation system.

[0148] When the depth of the underwater vehicle is relatively shallow, the DVL can obtain both the bottom velocity and the flow velocity The model training is as Figure 3 shown, and the specific steps are as follows:

[0149] 1. Data input:

[0150] SINS data: The acceleration and angular velocity data of the SINS carrier.

[0151] DVL data: The bottom velocity of the DVL and the flow velocity

[0152] 2. Establishment of the state equation and the measurement equation (SINS maneuvering process):

[0153] State equation: Establish a state equation including acceleration, angular velocity, position, velocity, attitude, etc., to describe the dynamic evolution of the system.

[0154] Measurement equation: According to the measurement data of the DVL, construct a measurement equation to represent the relationship between the measured value and the state variable.

[0155] 3. Introduction of M-estimation:

[0156] Perform robust estimation on the input data to weaken the influence of outliers. Ensure the quality of the input data and avoid the negative impact of outliers on the Kalman filter process.

[0157] 4. Kalman filter calculation:

[0158] Prediction step: Use the previous state estimate and control input (from the SINS) for prediction to update the state prediction value.

[0159] Update steps: Use the measurements of the DVL to calculate the Kalman gain and update the state estimate. At this time, the bottom measurement velocity provided by the DVL, together with the state prediction of the SINS, achieves the final state update through Kalman filtering, namely attitude, velocity, and position.

[0160] 5. Transformer model training:

[0161] Take the angular velocity of the SINS acceleration flow measurement velocity and the attitude φ of the carrier n velocity v n position p n as the training set. Through its self-attention mechanism, the Transformer network precisely learns the temporal dependencies and non-linear mappings between the data.

[0162] When the DVL cannot measure the bottom velocity, the model operates as Figure 4 shown, and the specific steps are as follows:

[0163] 1. Data input:

[0164] SINS data: The angular velocity provided by the SINS acceleration information.

[0165] DVL data: The DVL is the flow measurement velocity

[0166] 2. The Transformer neural network model predicts the final position, velocity, and attitude:

[0167] The input data obtains the complex non-linear mapping relationship between the data through the learning of the Transformer for historical data, and predicts the position, velocity, and attitude of the carrier.

[0168] During the training phase of the model. When the seabed can be covered by the DVL beam, at this time the DVL can simultaneously give the bottom measurement velocity and the flow measurement velocity We choose to fuse the information of the bottom measurement velocity and the estimated information of the SINS through Kalman filtering, and output the system state (position, velocity, attitude). This position, velocity, and attitude need to be used as the input of the Transformer. In addition, the flow measurement velocity also needs to be used as the input to supplement the disappearance of the subsequent bottom measurement velocity To improve the prediction accuracy of the model in the later stage, the acceleration of the SINS angular velocity These long-existing data are also used as inputs for training. During the prediction phase of the model, when the seabed cannot be covered by the DVL beam, the DVL can only give the measured flow velocity at this time. The SINS can give the acceleration angular velocity Using only the acceleration angular velocity and the measured flow velocity can also output the prediction of the system state (position, velocity, attitude).

[0169] This embodiment also provides a robust SINS / DVL integrated navigation system based on the Transformer model, including:

[0170] A Kalman filter module, which uses the errors of the SINS and DVL as the state vector, and the difference between the measured velocity in the DVL bottom measurement mode and the deduced velocity of the SINS as the observation value, to establish a discrete nonlinear state equation of the SINS / DVL integrated navigation system and construct a Kalman filter;

[0171] An observation residual optimization module, which projects the observation value into the state error space. When the observation residual is greater than the preset threshold, a correction factor is designed based on the traditional M-estimation algorithm to correct the observation noise covariance, and the approximate least absolute deviation estimation method is used to fully evaluate all dimensions of the observation residual to estimate the error state of the SINS / DVL integrated navigation system, suppress the influence of abnormal observation residuals, and estimate the state of the SINS / DVL integrated navigation system based on the Kalman filter;

[0172] A Transformer model construction module, which constructs a data set. The data set includes the acceleration and angular velocity measured by the SINS and the measured flow velocity measured by the DVL. The system state data estimated by the Kalman filter is used as the label of the data at the corresponding time node in the data set. The data set and the corresponding label are sent into the Transformer model for training to learn the mapping relationship from the data to the label;

[0173] A prediction module. For any SINS / DVL navigation process, if the DVL can obtain the bottom measurement velocity, the state of the SINS / DVL integrated navigation system is estimated based on the Kalman filter; if the DVL cannot obtain the bottom measurement velocity, the measured flow velocity measured by the DVL and the acceleration and angular velocity measured by the SINS are sent into the trained Transformer model to predict the state of the SINS / DVL integrated navigation system.

[0174] This embodiment also provides a robust SINS / DVL integrated navigation device based on the Transformer model, including a memory and a processor. Among them, the memory stores executable instructions, and the processor is configured to execute the executable instructions in the memory to implement the steps of the above-mentioned robust SINS / DVL integrated navigation method based on the Transformer model.

[0175] The embodiments of the present invention have been described above in conjunction with the accompanying drawings. However, the present invention is not limited to the above specific embodiments. The above specific embodiments are merely illustrative and not restrictive. Under the inspiration of the present invention, those of ordinary skill in the art can also make many forms without departing from the spirit and scope protected by the claims of the present invention, and all of them fall within the protection scope of the present invention.

Claims

1. A robust SINS / DVL integrated navigation method based on the Transformer model, characterized in that, It includes the following steps: Step S1: Taking the error between SINS and DVL as the state vector, and the difference between the measured velocity in the DVL bottom measurement mode and the deduced velocity of SINS as the observation value, establish the discrete nonlinear state equation of the SINS / DVL integrated navigation system and construct the Kalman filter; Step S2: Project the observation value into the state error space. When the observation residual is greater than the preset threshold, design a correction factor to correct the observation noise covariance based on the traditional M-estimation algorithm, and use the approximate least absolute deviation estimation method to fully evaluate all dimensions of the observation residual to estimate the error state of the SINS / DVL integrated navigation system, suppress the influence of abnormal observation residuals, and estimate the state of the SINS / DVL integrated navigation system based on the Kalman filter; Step S3: Construct a data set, which includes the acceleration and angular velocity measured by SINS and the measured flow velocity measured by DVL. Take the system state data estimated by the Kalman filter as the label of the data at the corresponding time node in the data set, and send the data set and the corresponding label into the Transformer model for training to learn the mapping relationship from the data to the label; Step S4: For any SINS / DVL navigation process, if the DVL can obtain the bottom measurement velocity, estimate the state of the SINS / DVL integrated navigation system based on the Kalman filter; if the DVL cannot obtain the bottom measurement velocity, send the measured flow velocity measured by the DVL and the acceleration and angular velocity measured by the SINS into the trained Transformer model to predict the state of the SINS / DVL integrated navigation system.

2. The robust SINS / DVL integrated navigation method based on the Transformer model according to claim 1, wherein The attitude error equation, velocity error equation and position error equation of SINS are expressed as: In the formula, represents the first-order differential of the SINS attitude error, indicating the rate of change of the attitude error with time; C w represents the coefficient matrix of the Euler platform error angle differential equation, represents the direction cosine matrix from the n system to the n' system; represents the angular velocity of the projection of the n system onto the i system; represents the angular velocity of the projection of the n system onto the i system; represents the angular velocity error of the projection of the n system onto the i system; represents the direction cosine matrix from the b system to the n' system; ε b represents the gyro zero bias in the b system; represents the first-order differential of the SINS velocity error, which is the rate of change of the velocity error with time; I represents the identity matrix; $T$ represents the transposed matrix; represents the specific force in the $b$ frame; $\nabla$ b represents the accelerometer error in the $b$ frame; represents the error of the Earth rate vector in the $n$ frame; represents the angular velocity error of the projection of the $n$ frame onto the $e$ frame; $v$ n represents the SINS velocity measurement; represents the Earth rate vector in the $n$ frame; represents the angular velocity of the projection of the $n$ frame onto the $e$ frame; δv n represents the SINS velocity error; g n represents the gravitational acceleration; represents the first - order differential of the SINS latitude error, which is the rate of change of the latitude error with time; RM represents the radius of curvature of the meridian; h represents the SINS altitude measurement value; δv N represents the SINS north - bound velocity error; δh represents the SINS altitude error; represents the first - order differential of the SINS longitude error, which is the rate of change of the SINS longitude error with time; L represents the SINS longitude measurement value; δL represents the SINS longitude error; δv E represents the SINS east - bound velocity error; v E represents the SINS east - bound velocity measurement value; RN represents the radius of curvature of the prime vertical; δL represents the SINS longitude error; δh represents the SINS altitude error; represents the first - order differential of the SINS altitude error, which is the rate of change of the SINS altitude error with time; δν U represents the SINS vertical velocity error; γ represents the roll angle of the underwater vehicle, represents the heading angle of the underwater vehicle; The discrete nonlinear state equation of the SINS / DVL integrated navigation system is expressed as: x k+1 = f(x k ) + w k ; where, x k represents the state vector of the system at the k-th moment, and x k+1 represents the state vector at the (k + 1)-th moment; f(·) represents a non-linear function; w k represents a noise vector; where: x k = [φ n , δv n , δp n , ε b , ▽ b , K d T ;​ where φ n represents the SINS attitude error, and φ n = [φ E , φ N , φ U T , and φ E , φ N , and φ U represent the pitch angle error, roll angle error, and heading angle error respectively; δvn = [δv E , δv N , δv U T; δp n represents the SINS position error, and δp n = [δλ, δL, δh] T , where δλ represents the SINS longitude error; ε b = [ε x , ε y , ε z T , and ε x , ε y , and ε z represent the gyro zero biases in the x, y, and z axis directions of the b-frame respectively; ▽ b = [▽ x , ▽ y , ▽ z T , and ▽ x , ▽ y , and ▽ z represent the accelerometer errors in the x, y, and z axis directions of the b-frame respectively; K d represents the scale factor error of the DVL module;​​​ The observation equation of the SINS / DVL integrated navigation system is expressed as: In the formula, represents the velocity calculated by the SINS in the b-frame; represents the velocity measured by the DVL in the b-frame; represents the true velocity of the underwater vehicle during the DVL measurement in the b-frame, that is, the ideal value without error correction of the scale factor Kd.

3. The robust SINS / DVL integrated navigation method based on the Transformer model according to claim 2, wherein The observation residual ζ is defined as follows: wherein, represents the system state estimation vector; z k represents the observation vector; H k represents the observation matrix; Design the correction factor ψ based on the traditional M-estimation algorithm k , and use the approximate least absolute function to construct the optimization objective of the observation residual, expressed as: where ζ i represents the i-th observation residual between the model predicted value and the actual observed value; J(ζ i ) represents the optimization objective of the observation residual ζ i ; γ represents a preset threshold with a value of 1.345; σ represents the smallest real number used to ensure the continuity of the derivative of the cost function J(ζ i ); represents the average value of observations in the t-th iteration at time k; R represents the observation noise covariance; In each iteration process, update the observation noise covariance, and the update process is as follows: In the formula, represents the corrected value of the observation covariance after the t-th iteration at time k; represents the weight matrix of the observation covariance during the t-th iteration at time k; represents the correction factor of the observation covariance during the t-th iteration at time k.

4. The robust SINS / DVL integrated navigation method based on the Transformer model according to claim 1, wherein The Transformer model includes an encoder and a decoder. The input data is processed through the multi-head attention sub-layer in the encoder. Multiple attention heads can capture the dependence relationship between SINS and DVL data in different sub-spaces. Subsequently, the representation ability of the model is further enhanced through the normalization layer and the fully connected feed-forward network sub-layer; the output of the encoder will be used as the input of the decoder. The decoder also includes a multi-head attention sub-layer and an encoder-decoder multi-head attention sub-layer. These sub-layers process the input data through residual connections and normalization layers, and finally the output layer generates the prediction result; in order to process the sequential information in the time series data, the decoder input layer maps each data point to a dmodel-dimensional vector through the position encoder and adds it to the position encoding vector to learn the temporal dependence of SINS and DVL data and accurately predict the system state.

5. The robust SINS / DVL integrated navigation method based on the Transformer model according to claim 4, wherein The loss function of the Transformer model training is expressed as: Wherein, represents the true value of the i-th sample, provided by external true data; represents the predicted value of the i-th sample, generated by the Transformer model; n represents the total number of samples.

6. A robust SINS / DVL integrated navigation system based on the Transformer model, characterized in that, It includes: The Kalman filter module is used to establish a discrete nonlinear state equation of the SINS / DVL integrated navigation system and construct a Kalman filter with the errors of the SINS and DVL as the state vector and the difference between the measured velocity in the DVL bottom measurement mode and the deduced velocity of the SINS as the observation value; The observation residual optimization module is used to project the observation value into the state error space. When the observation residual is greater than the preset threshold, a correction factor is designed based on the traditional M-estimation algorithm to correct the observation noise covariance. The approximate least absolute deviation estimation method is used to fully evaluate all dimensions of the observation residual to estimate the error state of the SINS / DVL integrated navigation system, suppress the influence of abnormal observation residuals, and estimate the state of the SINS / DVL integrated navigation system based on the Kalman filter; The Transformer model construction module is used to construct a data set, which includes the acceleration and angular velocity measured by the SINS and the flow velocity measured by the DVL. The system state data estimated by the Kalman filter is used as the label of the data at the corresponding time node in the data set. The data set and the corresponding label are sent into the Transformer model for training to learn the mapping relationship from the data to the label; The prediction module, for any SINS / DVL navigation process, if the DVL can obtain the bottom measurement velocity, the state of the SINS / DVL integrated navigation system is estimated based on the Kalman filter; if the DVL cannot obtain the bottom measurement velocity, the flow velocity measured by the DVL and the acceleration and angular velocity measured by the SINS are sent into the trained Transformer model to predict the state of the SINS / DVL integrated navigation system.

7. A robust SINS / DVL integrated navigation device based on the Transformer model, characterized in that, It includes a memory and a processor. Among them, the memory stores executable instructions, and the processor is configured to execute the executable instructions in the memory to implement the steps of the above-mentioned robust SINS / DVL integrated navigation method based on the Transformer model.

Images