Auv combined navigation method and system based on heteroscedastic perception and manifold filtering

Abstract

The present application relates to underwater autonomous vehicle integrated navigation technology field, especially to a kind of AUV integrated navigation method and system based on heteroscedasticity perception and manifold filtering, including obtaining high-frequency inertial measurement data and low-frequency local velocity measurement data, construct time sequence sliding window, input pre-training completed heteroscedasticity pseudo-observation neural network model to multi-source time sequence observation data, after reconstruction and augmentation, generate augmented pseudo-observation vector and augmented heteroscedasticity covariance matrix;Construct Lie group accompanied transformation matrix, generate global pseudo-new information vector and global observation covariance matrix;The normalized new information square value is calculated using global pseudo-new information vector, the filter gain is solved based on the modified global observation covariance matrix, the Lie algebra tangent space state update quantity is calculated in combination with global pseudo-new information vector, and the posteriori pose state and posteriori error covariance are output.The present application realizes low drift rate, high stability positioning under long-time navigation complex hydrographic environment.

Description

Technical Field

[0001] This invention relates to the field of underwater autonomous vehicle integrated navigation technology, and in particular to an AUV integrated navigation method and system based on heteroscedasticity sensing and manifold filtering. Background Technology

[0002] In existing technologies, the application scenarios of autonomous underwater vehicles (AUVs) in deep-sea exploration and operations continue to expand. AUV integrated navigation technology, by fusing data from multiple heterogeneous sensors such as inertial measurement units (IMUs) and Doppler logs (DVLs), calculates the three-dimensional pose information of the vehicle in real time, providing highly reliable state awareness support for underwater operations. Currently, AUV integrated navigation mainly employs traditional extended Kalman filtering (EKF), pure machine learning positioning, and learning-enhanced filtering.

[0003] Traditional EKF performs recursive estimation based on kinematic models and linearization assumptions; pure machine learning systems use deep neural networks to perform end-to-end training on multi-source sensor data and directly regress pose; learning-enhanced filtering combines neural networks with classical filtering and constructs pseudo-observations through network output to correct filter updates. However, real deep-sea operating environments are generally subject to multiple interferences such as bubble obstruction, undercurrent impact, and non-uniform acoustic media. Traditional filtering systems are prone to significant linearization errors when continuously turning at large angles or maneuvering violently, leading to a sharp degradation in positioning accuracy or even filter divergence. Pure machine learning positioning systems lack physical constraints, are prone to cumulative drift during long-endurance operations, and cannot verify output reliability. Learning-enhanced filtering systems lack strict positive definite constraints in solving error covariance, which can easily lead to inaccurate underlying numerical calculations. In addition, the residuals output by the network modules and the filter state updates belong to different coordinate spaces, lacking a strict coordinate alignment mechanism based on Lie group adjoint mapping, resulting in misalignment of the physical meaning of state variables and distortion of geometric structure. When encountering non-Gaussian anomalies such as DVL acoustic multipath and flying point impacts, it is impossible to effectively remove harmful observations, ultimately leading to continuous distortion of positioning results. At present, there is a need for an AUV integrated navigation method and system based on heteroscedasticity sensing and manifold filtering. Summary of the Invention

[0004] To address the problems of degradation in positioning accuracy and insufficient system robustness caused by existing AUV integrated navigation methods in complex underwater environments due to linearization errors caused by large maneuvers, cumulative drift over long endurance, multi-spatial geometric mismatch, and abnormal observation interference, this invention provides an AUV integrated navigation method and system based on heteroscedasticity sensing and manifold filtering.

[0005] In a first aspect, the AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering provided by the present invention adopts the following technical solution: The AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering includes: Acquire high-frequency inertial measurement data and low-frequency local velocity measurement data, construct SE(2) manifold motion input, and complete the propagation of prior pose state and prior error covariance based on SE(2) exponential mapping; A time-series sliding window is constructed, and multi-source time-series observation data are input into a pre-trained heteroscedastic pseudo-observation neural network model. The output is a two-dimensional pseudo-residual mean and Cholesky covariance parameter. After reconstruction and augmentation, augmented pseudo-observation vectors and augmented heteroscedastic covariance matrix are generated. Based on the prior pose state, the Lie group adjoint transformation matrix is ​​constructed, and the augmented pseudo-observation vector and the augmented heteroscedasticity covariance matrix are synchronously mapped to the global Lie algebraic tangent space to generate the global pseudo-innovation vector and the global observation covariance matrix. The BaselineNIS detection baseline is constructed based on the nominal observation covariance of physical priors. The normalized squared innovation value is calculated using the global pseudo-innovation vector. Based on the comparison results, the global observation covariance matrix is ​​corrected by elastic expansion. The filter gain is calculated based on the corrected global observation covariance matrix, and the Lie algebraic tangent space state update is calculated in combination with the global pseudo-innovation vector. The posterior pose state and posterior error covariance are updated in the manifold space by the SE(2) exponential mapping, and the posterior pose state and posterior error covariance are output.

[0006] Furthermore, the propagation of prior pose state and prior error covariance based on SE(2) exponential mapping includes time synchronization alignment and displacement integration of the system velocity measured by the Doppler log to obtain the system displacement increment; and integration of the heading axis angular velocity measured by the inertial measurement unit to obtain the heading angle increment. The displacement increment and heading angle increment of the machine system are constructed as motion inputs in the tangent space of the SE(2) Lie algebra, and the manifold pose increment is generated by the SE(2) exponential mapping. The manifold pose increment is applied to the posterior pose state of the previous time step based on the right multiplication form to obtain the prior pose state of the current time step. Based on the state error propagation Jacobian matrix and the process noise input Jacobian matrix, the posterior error covariance of the previous time step is propagated prior to obtain the prior error covariance of the current time step. The expression for the prior pose state is: , in, Let be the prior pose state at time k. Let be the posterior pose state at time k-1. The motion input is the Lie algebra matrix transformed by the wedge operator. For the SE(2) exponential mapping operator.

[0007] Furthermore, the output of the two-dimensional pseudo residual mean and Cholesky covariance parameter includes constructing a time-series sliding window covering the current time and historical observation data. The multi-source time-series observation data in the time-series sliding window includes high-frequency inertial data from the inertial measurement unit, local velocity data from the Doppler log, sampling time interval, and attitude auxiliary quantities. The multi-source time-series observation data within the time-series sliding window is input into the pre-trained heteroscedastic pseudo-observation neural network model. The heteroscedastic pseudo-observation neural network model consists of a feature embedding layer, a multi-scale time-series convolutional layer, a bidirectional time-series memory layer, a multi-head time-series attention layer, a shared latent variable layer, a pseudo residual mean output head, and a heteroscedastic covariance output head. The two-dimensional pseudo-residual mean is output from the pseudo-residual mean output head of the system, and the Cholesky covariance parameter is output from the heteroscedasticity covariance output head. The output of the heteroscedastic pseudo-observation neural network model satisfies: , in, Let be the mean of the two-dimensional pseudo-residuals in the system at time k. Let be the Cholesky covariance parameter vector at time k. This is a heteroscedastic pseudo-observation neural network model. For the k-th time-series sliding window.

[0008] Furthermore, the process of generating augmented pseudo-observation vectors and augmented heteroscedasticity covariance matrix after reconstruction and augmentation includes constructing a Cholesky lower triangular matrix after constraining the Cholesky covariance parameters with a positive activation function, and reconstructing the two-dimensional heteroscedasticity covariance matrix by multiplying the Cholesky lower triangular matrix with its transpose. The two-dimensional pseudo residual mean and two-dimensional heteroscedasticity covariance matrix under the machine system are augmented to the three-dimensional error state space of SE(2) by translation and embedding matrix, generating augmented pseudo-observation vectors and augmented heteroscedasticity covariance matrix. The expression of the two-dimensional heteroscedasticity covariance matrix is ​​as follows: , in, Let the two-dimensional heteroscedasticity covariance matrix of the system at time k be denoted as . Let be the Cholesky lower triangular matrix at time k. for The transpose of the matrix, For covariance regularization, It is a two-dimensional identity matrix.

[0009] Furthermore, the training process of the heteroscedastic pseudo-observation neural network model includes: The machine system supervision residual is constructed based on the actual trajectory or high-precision reference trajectory of the underwater robot. The machine system supervision residual is used to characterize the deviation between the actual displacement increment and the Doppler log integrated displacement increment. Construct a heteroscedastic negative log-likelihood loss function based on the system monitoring residuals, the mean of two-dimensional pseudo residuals, and the two-dimensional heteroscedastic covariance matrix; The heteroscedastic pseudo-observation neural network model is trained using the heteroscedastic negative log-likelihood loss function, enabling it to learn the displacement correction caused by Doppler log errors, inertial measurement unit integral errors, and local flow field disturbances during underwater motion.

[0010] Furthermore, the generation of the global pseudo-innovation vector and the global observation covariance matrix includes: The SE(2) Lie group adjoint transformation matrix is ​​constructed based on the prior pose state. The SE(2) Lie group adjoint transformation matrix is ​​used to describe the coordinate transformation relationship between the elements of the SE(2) group and the Lie algebraic tangent space vector. Using the SE(2) Lie group adjoint transformation matrix to perform coordinate transformation on the augmented pseudo-observation vector, the augmented pseudo-observation vector under the machine system is projected onto the global Lie algebra tangent space to generate the global pseudo-innovation vector; Using the adjoint transformation matrix and its transpose of the SE(2) Lie group, a synchronous coordinate transformation is performed on the augmented heteroscedasticity covariance matrix. The augmented heteroscedasticity covariance matrix under the machine system is projected onto the global Lie algebraic tangent space to generate the global observation covariance matrix. The expressions for the global pseudo-innovation vector and the global observation covariance matrix are as follows: , , in, Let be the global pseudo-innovation vector at time k. Let be the adjoint transformation matrix of the SE(2) Lie group constructed from the prior pose state at time k. Let k be the augmented pseudo-observation vector at time k. Let be the global observation covariance matrix at time k. Let be the augmented heteroscedasticity covariance matrix at time k.

[0011] Further, the calculation of the normalized innovation squared value using the global pseudo-innovation vector includes projecting the physical nominal observation covariance under the machine system onto the global error space to obtain the physical nominal observation covariance in the global error space; constructing the BaselineNIS detection baseline covariance based on the observation matrix, the prior error covariance matrix, and the physical nominal observation covariance in the global error space; and calculating the normalized innovation squared value using the global pseudo-innovation vector and the BaselineNIS detection baseline covariance. The formula for calculating the normalized innovation squared value is as follows: , in, Let be the normalized squared innovation value at time k. Let be the global pseudo-innovation vector at time k. The baseline covariance is detected by BaselineNIS at time k.

[0012] Furthermore, the elastic expansion correction of the global observation covariance matrix based on the comparison results includes comparing the normalized innovation squared value with a preset anomaly detection threshold. When the normalized innovation squared value exceeds the preset anomaly detection threshold, a covariance elastic expansion coefficient is generated. The global observation covariance matrix is ​​then penalized and amplified using the covariance elastic expansion coefficient to obtain the corrected global observation covariance matrix. The corrected global observation covariance matrix satisfies the following: , in, Let $k$ be the global observation covariance matrix corrected at time k. Let be the covariance elastic expansion coefficient at time k. Let be the global observation covariance matrix at time k, and let the covariance elastic expansion coefficient satisfy: , in, This is the upper limit of the expansion coefficient. This is a preset anomaly detection threshold.

[0013] Furthermore, the output posterior pose state and posterior error covariance include constructing a posterior update covariance based on the corrected global observation covariance matrix, the prior error covariance matrix and the observation matrix. The observation matrix is ​​used to map the error state in the Lie algebra tangent space to the pseudo-observation space. The Kalman gain is calculated using the posterior updated covariance, and the state update in the Lie algebraic tangent space is calculated based on the Kalman gain and the global pseudo-innovation vector. The state update quantity is converted into a Lie algebra matrix through the wedge operator, and the right multiplication form consistent with the prior propagation is adopted. The state correction quantity is applied to the prior pose state through the SE(2) exponential mapping, and the prior pose state is updated. The posterior error covariance is updated using the JosephForm stable update method, and the posterior pose state and posterior error covariance are output.

[0014] Secondly, an AUV integrated navigation system based on heteroscedasticity sensing and manifold filtering includes: The manifold prior propagation module is used to construct the SE(2) manifold motion input from the measurement data collected by the IMU data acquisition module and the DVL data acquisition module, and to complete the propagation of the prior pose state and prior error covariance based on the SE(2) exponential mapping. The heteroscedastic pseudo-observation generation module is used to construct a time-series sliding window based on measurement data and generate augmented pseudo-observation vectors and augmented heteroscedastic covariance matrices. The adjoint projection module is used to construct the adjoint transformation matrix of the Lie group and generate the global pseudo-innovation vector and the global observation covariance matrix. The BaselineNIS elastic gating module is used to construct a detection baseline based on the nominal observation covariance of physical priors, calculate the normalized squared innovation value using the global pseudo-innovation vector, and perform elastic expansion correction on the global observation covariance matrix. The manifold posterior closed-loop update module is used to output the posterior pose state and the posterior error covariance.

[0015] In summary, the present invention has the following beneficial technical effects: 1. This invention constructs an SE(2) manifold state space and completes prior propagation and posterior state correction based on exponential mapping, thereby achieving strict maintenance of the geometric structure consistency between pose state and error state, and effectively suppressing linearization error accumulation and filter divergence under high-speed operation.

[0016] 2. This invention synchronously outputs the pseudo residual mean and Cholesky parameterized heteroscedastic covariance through a pre-trained temporal neural network, and reconstructs the covariance matrix using a positive activation function. This achieves an adaptive dynamic representation of complex time-varying noise in the deep sea and a fundamental guarantee of the strict positive definiteness of the covariance matrix, reducing the risk of system instability caused by ill-conditioned covariance.

[0017] 3. This invention constructs the SE(2) Lie group adjoint transformation matrix from the prior pose state, and synchronously projects the machine system pseudo residuals and their heteroscedasticity covariance to the global Lie algebra tangent space, realizing the closed-loop fusion of the learning pseudo-observation and filtering error state in a unified geometric space, eliminating the spatial geometric mismatch and physical meaning misalignment of multi-source data fusion.

[0018] 4. This invention constructs a BaselineNIS detection baseline based on the physical nominal observation covariance and performs elastic expansion correction on the global observation covariance according to the normalized innovation square value, thereby realizing the physical decoupling of anomaly detection and learning covariance representation, and endowing the system with adaptive penalty and active blocking capabilities for non-Gaussian anomaly observations.

[0019] 5. This invention achieves the preservation of symmetry and semidefiniteness of the covariance matrix under floating-point truncation error by using the JosephForm stable update form to iteratively calculate the error covariance in the posterior update stage, thereby improving the reliability of the algorithm in engineering deployment on resource-constrained embedded platforms.

[0020] 6. This invention achieves low drift rate and high stability positioning in complex hydrological environments with long navigation time by deeply integrating classical physical recursive equations with heteroscedastic pseudo-observation neural networks, using a physical model as a rigid prior base and a time-series network to adaptively remove time-varying noise and inherent biases of the system. Attached Figure Description

[0021] Figure 1 This is a general flowchart of the integrated navigation method according to an embodiment of the present invention.

[0022] Figure 2 This is a flowchart of the data input stage of the integrated navigation method according to an embodiment of the present invention.

[0023] Figure 3 This is a flowchart of the learning and prediction phase of the integrated navigation method according to an embodiment of the present invention.

[0024] Figure 4 This is a flowchart of the abnormal gating stage of the integrated navigation method according to an embodiment of the present invention.

[0025] Figure 5 This is a flowchart of the filtering and updating stage of the integrated navigation method according to an embodiment of the present invention.

[0026] Figure 6 These are comparative examples illustrating the implementation effects of several embodiments of the present invention.

[0027] Figure 7 This is a diagram showing the DVL anomaly detection results of an embodiment of the present invention.

[0028] Figure 8 This is a pseudo-measurement uncertainty heatmap according to an embodiment of the present invention.

[0029] Figure 9 This is a graph showing the changes in the filter update parameters according to an embodiment of the present invention.

[0030] Figure 10 This is a diagram showing the local error variation in an embodiment of the present invention. Detailed Implementation

[0031] The present invention will be further described in detail below with reference to the accompanying drawings.

[0032] Example 1 Reference Figure 1 The AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering in this embodiment includes: S1. Acquire high-frequency inertial measurement data and low-frequency local velocity measurement data, construct SE(2) manifold motion input, and complete the propagation of prior pose state and prior error covariance based on SE(2) exponential mapping; S2. Construct a time-series sliding window, input multi-source time-series observation data into a pre-trained heteroscedastic pseudo-observation neural network model, output two-dimensional pseudo-residual mean and Cholesky covariance parameters, and generate augmented pseudo-observation vectors and augmented heteroscedastic covariance matrix after reconstruction and augmentation. S3. Construct the Lie group adjoint transformation matrix based on the prior pose state, and synchronously map the augmented pseudo-observation vector and the augmented heteroscedasticity covariance matrix to the global Lie algebraic tangent space to generate the global pseudo-innovation vector and the global observation covariance matrix. S4. Construct the BaselineNIS detection baseline based on the nominal observation covariance of physical priors, calculate the normalized squared innovation value using the global pseudo-innovation vector, and perform elastic expansion correction on the global observation covariance matrix based on the comparison results. S5. Based on the corrected global observation covariance matrix, calculate the filtering gain and combine it with the global pseudo-innovation vector to calculate the Lie algebraic tangent space state update. Then, through the SE(2) exponential mapping, update the a priori pose state and the a priori error covariance in the manifold space and output the a priori pose state and the a priori error covariance.

[0033] Specifically, the AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering includes the following steps: Before performing step S1, in order to uniformly describe the position and heading state of the underwater robot in the two-dimensional plane, this embodiment uses a two-dimensional special Euclidean group SE(2) to represent the pose state of the underwater robot. SE(2) is composed of a two-dimensional rotation matrix and translation vector, and its group elements are represented as follows: , In the formula, Indicates the first The pose state matrix of the underwater robot at any given time. Indicates the heading angle The constructed two-dimensional rotation matrix, This represents the position vector of the underwater robot in the global coordinate system. This represents a zero vector with one row and two columns. This represents a two-dimensional special Euclidean group.

[0034] Wherein, the two-dimensional rotation matrix and position vector They are represented as follows: , In the formula, and These represent the two-dimensional position components of the underwater robot in the global coordinate system. The heading angle of an underwater robot can be represented in the above form, allowing for a unified expression of the underwater robot's two-dimensional position and planar heading. Within the manifold space, it provides a unified state representation for subsequent manifold propagation, associated projection, and manifold posterior update.

[0035] The corresponding Lie algebra is denoted as For any three-dimensional Lie algebra vector: , Its corresponding wedge operator is expressed as: , In the formula, and Denotes the translation components in the tangent space of the Lie algebra. A three-dimensional Lie algebra vector. Denotes the rotational components in the tangent space of the Lie algebra. This represents mapping a three-dimensional vector to... The wedge operator for the Lie algebra matrix. This representation is used to uniformly express translation and heading errors as Lie algebra increments that can be applied to the pose state via exponential mapping.

[0036] Exponential mapping operator Used to represent Lie algebra Incremental mapping in Pose increments on the group. For ,make Then we have: , In the formula, Indicates the rotation angle is Two-dimensional rotation matrix, For the translation coupling matrix in the exponential mapping, For the translation components in the tangent space of the Lie algebra and The translation vector formed, when hour, Represented as: , when When the absolute value is small, Taylor expansion can be used for approximate calculation to avoid division by zero and amplification of floating-point errors. In a preferred embodiment, it can be set that: , In the formula, This represents a two-dimensional identity matrix. Through the above exponential mapping, the mechanical displacement increments provided by DVL and the heading increments provided by IMU can be uniformly mapped to... The pose increment on the group ensures that state propagation and posterior updates are both completed within a legal manifold space.

[0037] like Figure 1 , Figure 2 As shown, S1, firstly, acquire high-frequency inertial measurement data collected by the underwater robot's inertial measurement unit (IMU) and low-frequency local velocity measurement data collected by the Doppler log (DVL). The IMU data includes at least three-axis angular velocity information, and the Doppler log data includes at least local velocity information in the body coordinate system.

[0038] Because of the difference in sampling frequencies between the inertial measurement unit (IMU) and the Doppler log, the body velocity measured by the Doppler log needs to be time-synchronized and aligned to ensure consistency with the IMU data in terms of timing. Displacement integration is performed based on the time interval between adjacent sampling moments to obtain the two-dimensional displacement increment in the body coordinate system at the current moment. , In the formula, Indicates the first The two-dimensional displacement increment in the body coordinate system at any given time. This represents the system velocity measured by DVL over continuous time. Indicates the first The velocity vector of the DVL machine system after time synchronization. Indicates the first Time to the The sampling time interval at time . and Indicates two adjacent filtering times, superscript This represents the machine's coordinate system.

[0039] Simultaneously, the angular velocity of the heading axis measured by the inertial measurement unit is integrated to obtain the heading angle increment: , In the formula, Indicates the first The increment of the heading angle at time , This represents the angular velocity of the heading axis measured by the IMU over a continuous time period. Indicates the first The angular velocity of the heading axis after time synchronization, as described above, represents the translational and rotational motions of the underwater robot in the body coordinate system, respectively. To unify these two expressions within the SE(2) manifold space, the aforementioned system displacement and heading angle increments are constructed as motion inputs in the SE(2) Lie algebra tangent space: , In the formula, Indicates the first The incremental motion of the machine system at any given moment. and They represent the mechanical systems obtained by DVL velocity integration, respectively. direction and Increment of directional displacement This represents the yaw angle increment obtained by integrating the yaw axis angular velocity of the IMU. This motion input is converted into a Lie algebra matrix by the wedge operator. And by generating manifold pose increments on the group through SE(2) exponential mapping, the discrete-time sensor measurements are converted into pose changes on the SE(2) group: , In the formula, Indicates the first The manifold pose increment, constructed from IMU / DVL measurements at each time step, is applied to the posterior pose state of the previous time step using a right-multiplication method, thus completing the prior propagation of the nominal pose state: , In the formula, Indicates the first The prior pose state at any given time. Indicates the first The posterior pose state at a given moment. This propagation method represents the current system motion increment acting on the posterior pose state of the previous moment to obtain the current prior pose state. Through this manifold propagation method, the pose state can be kept constant. Within the group structure, and providing a physically consistent prediction basis for subsequent heteroscedastic pseudo-observation fusion and manifold posterior update, while completing the pose state prior propagation, based on the state error propagation Jacobian matrix and the process noise input Jacobian matrix, the posterior error covariance of the previous time step is propagated a priori: , In the formula, Indicates the first The prior error covariance matrix at time t. This represents the Jacobian matrix representing the propagation of state errors. Indicates the first The posterior error covariance matrix at time t. The Jacobian matrix represents the input noise of the process. Represents the process noise covariance matrix. This indicates the matrix transpose.

[0040] Through the aforementioned SE(2) manifold prior propagation method, the pose state is always within the SE(2) group structure, avoiding the problem that traditional Euclidean space linearization propagation is prone to generating large linearization errors under conditions of continuous turning at large angles, sharp turns and complex maneuvers, thus ensuring the geometric consistency of pose quantities from the perspective of state propagation.

[0041] like Figure 3 As shown in Figure S2, after completing the prior propagation, in order to estimate the pseudo residual at the current time using historical observation information, a time-series sliding window covering the current time and historical observation data is constructed. The time-series sliding window at time k is expressed as follows: , In the formula, Indicates the first A time-series sliding window that continuously inputs into the neural network model. Indicates the length of the sliding window. Indicates the first The multi-source observation feature vector at each time step, where the multi-source time-series observation data within the time-series sliding window consists of high-frequency inertial data from the inertial measurement unit, local velocity data from the Doppler log, sampling time intervals, and attitude auxiliary quantities, can be expressed as: , In the formula, This represents the three-axis acceleration collected by the IMU in the machine system. This represents the three-axis angular velocity collected by the IMU in the machine system. This represents the local velocity of DVL data acquisition in the machine system. Indicates the sampling time interval. and This represents a continuous attitude auxiliary feature constructed from the heading angle, using... and Indicates the heading angle, which can reduce the angle crossing. and To mitigate the discontinuous effects of time, the input features are normalized and embedded. , , In the formula, This represents the normalized observation characteristics. and Let represent the mean and standard deviation of the features in the training data, respectively. This represents a numeric regularization term to prevent division by zero. Represents the feature embedding vector. and These represent the weights and biases of the feature embedding layer, respectively. This represents a nonlinear activation function. Through this feature embedding layer, IMU and DVL multi-source observations with different dimensions and sampling characteristics can be mapped to a unified latent space.

[0042] The heteroscedastic pseudo-observation neural network model consists of a feature embedding layer, a multi-scale temporal convolutional layer, a bidirectional temporal memory layer, a multi-head temporal attention layer, a shared latent variable layer, a pseudo-residual mean output head, and a heteroscedastic covariance output head. The model uses multi-scale temporal convolutional layers to extract local motion changes, short-term sensor perturbations, and DVL local velocity variation features. The r-th temporal convolutional unit is represented as: , In the formula, Indicates the first Temporal features output by each temporal convolutional unit From feature embedding sequence constitute, Indicates the expansion rate One-dimensional temporal convolution operation, The activation function of the convolutional layer is represented by , the residual connection is used to maintain the stability of temporal feature propagation, and the dilated convolution is used to expand the receptive field of historical observations, enabling the model to extract motion patterns of both short-term perturbations and longer time scales.

[0043] The model incorporates a bidirectional temporal memory layer to model the temporal dependencies of IMU / DVL observation sequences within a sliding window. The computation process is as follows: , , , In the formula, Represents a forward sequential memory unit. Represents a reverse temporal memory unit. and These represent the forward and reverse hidden states, respectively. This represents the fused hidden state output by the bidirectional temporal memory layer. Since the sliding window contains both current and historical observation data, the inverse temporal memory layer is used to reorganize temporal dependencies within a known window, without introducing future measurement information outside the current window.

[0044] The model incorporates a multi-head temporal attention layer to weight the current pseudo-residual estimate based on the importance of different historical observation segments. Each attention head, its calculation process is expressed as follows: , , , In the formula, This represents the sequence of hidden states formed by the outputs of the bidirectional temporal memory layer. , and They represent the first The query matrix, key matrix, and value matrix corresponding to each attention head. , , and This represents the learnable weight matrix in the attention layer. Indicates the first Each attention head's feature dimensions Indicates the number of attention heads. This layer represents the temporal characteristics after multi-head attention fusion. It is used to highlight historical segments that contribute more to the current pseudo residual and covariance estimation, reducing the impact of short-term noise and local outlier observations on the network output.

[0045] Constructing the first based on attention output Shared latent variables at time: ,or: , In the formula, Indicates the first The shared latent variables used to output predictions at each moment. This indicates a time-series pooling operation. This represents the hidden state corresponding to the current time step in the attention output sequence. This shared hidden variable is simultaneously input to both the pseudo-residual mean output header and the heteroscedasticity covariance output header, allowing the two types of outputs to share the same underwater temporal motion representation.

[0046] The pseudo-residual mean output head is based on the two-dimensional pseudo-residual mean of the shared latent variable output machine system: , In the formula, Indicates the first Mean of two-dimensional pseudo residuals in the time-machining system and Representing the machine system direction and Mean of directional pseudo residuals and These represent the weights and biases of the pseudo-residual mean output header, respectively. The pseudo-residual mean is used to describe the learnable correction between the nominal propagation of the DVL / IMU and the actual displacement change, and serves as learned pseudo-observation information in subsequent filter updates.

[0047] The heteroscedasticity covariance output header outputs the Cholesky covariance parameters based on the shared latent variables: , In the formula, represents the Cholesky covariance parameter vector at time i. and These represent the weights and biases of the heteroscedasticity covariance output head, respectively. This output head is used to generate sample-level uncertainty parameters based on the current underwater observation quality, motion state, and historical time series characteristics, enabling the model to dynamically express the reliability of pseudo-residuals under different times, operating conditions, and observation qualities.

[0048] After constraining the Cholesky covariance parameters with a positive activation function, a Cholesky lower triangular matrix is ​​constructed: , In the formula, Indicates the first The Cholesky lower triangular matrix at time t. This represents a positive activation function. This represents the diagonal regularization term. Constraining the diagonal elements of the lower triangular matrix using a positive activation function can improve the positive definiteness and stability of subsequent covariance reconstruction. The two-dimensional heteroscedasticity covariance matrix of the machine system can be reconstructed through the product of the Cholesky lower triangular matrix and its transpose. ; In the formula, Indicates the first The two-dimensional heteroscedasticity covariance matrix of the time-machining system. Denotes the covariance regularization term. Representing a two-dimensional identity matrix, in Under these conditions, the matrix satisfies the positive numerical definiteness requirement. This covariance matrix is ​​used to characterize the sample-level uncertainty of the current pseudo-residual mean, and in subsequent steps, it is projected onto the global Lie algebraic tangent space via the Lie group adjoint mapping. The two types of outputs of the neural network model can then be uniformly represented as: , In the formula, This represents a heteroscedastic pseudo-observation neural network model consisting of a feature embedding layer, a multi-scale temporal convolutional layer, a bidirectional temporal memory layer, a multi-head temporal attention layer, a shared latent variable layer, a pseudo-residual mean output head, and a heteroscedastic covariance output head. This model synchronously outputs the pseudo-residual mean and Cholesky covariance parameters of the machine system, forming a learning-based heteroscedastic pseudo-observation engine for underwater IMU / DVL integrated navigation.

[0049] During the training phase, supervisory residuals can be constructed based on the underwater robot's actual trajectory or a high-precision reference trajectory. As a preferred implementation, the machine-system supervisory residuals are expressed as: , In the formula, This represents the residual of the machine system constructed during the training phase. This represents the actual displacement increment of the machine system obtained from the reference trajectory. This represents the system displacement increment obtained from the DVL velocity integral. Using this supervisory residual, the neural network can learn the system displacement corrections caused by DVL errors, IMU integration errors, local flow field disturbances, and observation degradation during underwater motion. The neural network model is trained using heteroscedasticity negative log-likelihood loss. , In the formula, Indicates the first Heteroscedasticity negative log-likelihood loss for each sample Indicates the residual of the machine system monitoring. This represents the mean of the pseudo residuals in the neural network output. This represents the two-dimensional heteroscedasticity covariance matrix obtained by the neural network output and Cholesky reconstruction. This represents the matrix determinant. This loss function is used to simultaneously constrain the accuracy of the pseudo-residual mean estimation and the ability to express covariance uncertainty.

[0050] To avoid the covariance output being too small or too large during training, a mean error term and a covariance regularization term can be added to the training objective: , In the formula, Indicates total training loss. , and Let represent the weights of the mean error term, the heteroscedasticity negative log-likelihood term, and the covariance regularization term, respectively. This indicates retrieving the diagonal elements of the matrix. This represents the logarithmic function. This combined loss allows for simultaneous constraints on pseudo-residual prediction accuracy, sample-level uncertainty estimation capability, and covariance range during the training phase.

[0051] Due to the pseudo-residual mean output by the heteroscedastic pseudo-observation neural network model and the two-dimensional heteroscedasticity covariance matrix This mainly corresponds to two-dimensional translational pseudo-observations in the body coordinate system, while The error states in manifold filtering include two-dimensional translation errors and one-dimensional heading errors. Therefore, this invention extends two-dimensional translation pseudo-observations and their covariance to three dimensions. An error state space is used to ensure dimensionality consistency in subsequent Lie group adjoint projection and manifold filtering updates.

[0052] As one of the preferred embodiments, a translation embedding matrix is ​​defined as follows: , In the formula, This indicates embedding two-dimensional translation pseudo-observations into three-dimensional... The translation embedding matrix of the error state space augments the two-dimensional pseudo-residual mean output by the neural network into a three-dimensional pseudo-observation vector of the machine system: ,in: When expanded, it can be written as: In the formula, This represents the three-dimensional augmented pseudo-observation vector in the body coordinate system. This represents the mean of the two-dimensional translational pseudo-residuals output by the neural network. This represents the embedding vector along the heading dimension. This indicates a placeholder term for the pseudo-residual of the heading.

[0053] In a preferred embodiment, the heteroscedastic pseudo-observation neural network model only outputs two-dimensional translation pseudo-residuals and does not directly output heading pseudo-residuals. In this case, let... Therefore, the pseudo-observation vector of the three-dimensional machine system can be written as: , in, Let be the mean of the pseudo residuals in the x-direction of the system at time k. Let be the mean of the pseudo-residuals in the y-direction of the system at time k. Through the above augmentation method, the two-dimensional translational pseudo-residuals output by the neural network can be embedded. Three-dimensional error state space. The heading component is set to zero, indicating that in this embodiment, the learned pseudo-observations are mainly used to correct two-dimensional translation errors. The heading state is mainly constrained by IMU heading propagation, prior covariance propagation, and subsequent filtering closed-loop mechanism, which together constrain the two-dimensional heteroscedasticity covariance matrix. Augmented to the heteroscedasticity covariance matrix of a three-dimensional machine system: , In the formula, This represents the three-dimensional augmented heteroscedasticity covariance matrix in the body coordinate system. This represents the two-dimensional translational heteroscedasticity covariance matrix of the machine system reconstructed from the Cholesky parameters. This represents the pseudo-observation variance in the heading direction.

[0054] Further expansion yields: , If the two-dimensional heteroscedasticity covariance matrix is ​​represented as: , The augmented three-dimensional machine covariance matrix is ​​then expressed as: , In the formula, Indicator System Directional pseudo residual variance Indicator System Directional pseudo residual variance Indicator System direction and Covariance between directional pseudo residuals; and This indicates that no direct cross-covariance is set between the two-dimensional translation pseudo-observation and the heading placeholder term; The pseudo-observation variance represents the heading direction and is used to control the influence of the heading placeholder term in subsequent filter updates.

[0055] In a preferred embodiment, Fixed nominal values, empirically calibrated values, or time-varying values ​​determined based on the IMU heading error model can be used. If it is desired that the learned pseudo-observations primarily correct for two-dimensional translation errors and reduce the impact of heading placeholder terms on posterior state updates, then... Set to a relatively large positive value; if you want the heading direction to participate in the weak constraint update, you can construct it based on the IMU heading axis angular velocity noise and sampling time interval: , In the formula, This represents the variance of the IMU's angular velocity noise along its heading axis. Indicates the sampling time interval. This represents the heading variance regularization term, used to ensure that the variance of false observations in the heading direction is positive. Through the above settings, it is possible to maintain... While ensuring consistency in the three-dimensional error state dimensions, it reduces the risk of excessive constraints on the heading state imposed by two-dimensional translation pseudo-observations.

[0056] Compared to conventional end-to-end position regression models or time-series neural network models that only output the mean, the heteroscedastic pseudo-observation neural network model in this embodiment has undergone the following adaptation improvements: First, the model output is limited to the pseudo-residual mean under the machine system, so that the learning results can be used as a correction quantity for the physical propagation model and fed into the filter; Second, the model adopts a dual-output structure with a pseudo-residual mean output head and a heteroscedastic covariance output head, so that the pseudo-observation at each time step has a corresponding uncertainty expression; Third, through Cholesky parameterization and positive activation functions... The covariance regularization term constrains the numerical positive definiteness of the covariance matrix, reducing the risk of filtering instability caused by ill-conditioned covariance. Fourth, multi-scale temporal convolution, bidirectional temporal memory, and multi-head attention structures are used together to extract local perturbations, long-term dependencies, and key historical segments in underwater IMU / DVL sequences, making the model more adaptable to underwater high-maneuverability, DVL local anomalies, velocity measurement degradation, and time-varying noise conditions. Fifth, the pseudo-residuals and covariances output by the model are subsequently projected in pairs onto the global Lie algebraic tangent space through Lie group adjoint mapping, enabling the learned observations to be integrated with the model's data. Manifold filtering state updates form geometrically consistent closed-loop fusion.

[0057] Step S3: The augmented pseudo-observation vector and its augmented heteroscedasticity covariance matrix generated in step S2 are both defined in the body coordinate system, while the filter error state update needs to be completed in the global Lie algebra tangent space. The coordinate spaces of the two are inconsistent. Therefore, this step constructs the SE(2) Lie group adjoint transformation matrix based on the prior pose state obtained in step S1, and synchronously maps the body pseudo-observation and its uncertainty to the global Lie algebra tangent space, so as to realize the geometric space alignment between the learned observation and the filter state update.

[0058] For the prior pose state obtained in step S1 The SE(2) Lie group adjoint transformation matrix, consistent with the right-multiplication state propagation convention, is constructed as follows: , In the formula, Indicates by the first The adjoint transformation matrix constructed from the prior states at each time step. Indicates the prior heading angle, The adjoint matrix is ​​used to describe the prior position vector. The coordinate transformation relationship between group elements and their Lie algebraic tangent space vectors is adopted in this embodiment. Consistent right-multiplication state propagation convention.

[0059] Based on the aforementioned adjoint transformation matrix, the machine-system augmented pseudo-observation vector generated in step S2 is projected onto the global Lie algebra tangent space to generate a global pseudo-innovation vector: , In the formula, This represents the global pseudo-innovation vector projected onto the global Lie algebra tangent space. The augmented pseudo-observation vector in the machine system is indicated by the superscript. This represents the global Lie algebra tangent space or the global error representation. Through this mapping, the learned pseudo-residuals in the machine system are transformed into a geometric space consistent with the filtering error state.

[0060] Simultaneously, using the aforementioned adjoint transformation matrix and its transpose, a synchronous coordinate transformation is performed on the machine-system augmented heteroscedasticity covariance matrix generated in step S2 to generate the global observation covariance matrix: , In the formula, This represents the projected global observation covariance matrix. The augmented heteroscedasticity covariance matrix in the representation system. This indicates the transpose of the adjoint matrix. The formula projects the pseudo-residual mean and heteroscedastic covariance of the machine system in pairs, ensuring that the pseudo-observations and their uncertainties are expressed in the global Lie algebra tangent space, thereby improving the geometric consistency of learned observations when updating manifold filters.

[0061] like Figure 4 As shown, in step S4: After obtaining the global pseudo-innovation vector and the global observation covariance matrix, in order to identify and suppress the adverse effects of non-Gaussian anomalous observations on state updates, this step constructs a physical prior-based anomaly detection and elastic gating mechanism. This physically decouples the real anomaly detection from the learned covariance output by the neural network, preventing the system from passively masking anomalies solely by relying on the network covariance. First, the physical nominal observation covariance under the machine system is projected onto the global error space to obtain the physical nominal observation covariance in the global error space. This projection method is consistent with the projection method of the heteroscedastic covariance in step S3, ensuring that the physical nominal baseline and the learned global observation covariance are in the same geometric space. Based on the observation matrix, the prior error covariance matrix obtained in step S1, and the physical nominal observation covariance in the global error space, the BaselineNIS detection baseline covariance is constructed: , In the formula, Indicates the first BaselineNIS detects baseline covariance at any given time. Represents the observation matrix. Indicates the first The prior error covariance matrix at time t. Represents the physical nominal observation covariance in the global error space. Represents a numerical regularization term. express 3D identity matrix This indicates the pseudo-observation dimension. When using three-dimensional augmented pseudo-observations... The detection baseline is constructed using the physical nominal covariance, which separates the anomaly detection process from the learned covariance representation of the neural network output.

[0062] Based on the detection baseline covariance and the global pseudo-innovation vector obtained in step S3, the normalized squared innovation value at the current time is calculated: , In the formula, Indicates the first The normalized squared value of the innovation at time , Represents the global pseudo-innovation vector. This represents the inverse matrix of the detection baseline covariance. The normalized innovation squared value characterizes the degree of anomalousness of the current pseudo-observation relative to the physical nominal detection baseline. This normalized innovation squared value is compared with a preset anomaly detection threshold, which is determined by the chi-square distribution quantile, experimental calibration values, or empirical thresholds specific to the task scenario. When the normalized innovation squared value does not exceed the preset anomaly detection threshold, the current observation is considered valid, and the covariance of the learning-based global observation remains unchanged. When the normalized innovation squared value exceeds the preset anomaly detection threshold, the current observation is determined to have a non-Gaussian anomalous perturbation, and a covariance elastic expansion coefficient is automatically generated based on the degree of anomaly. , In the formula, Indicates the first The covariance elastic expansion coefficient at time t. This indicates the upper limit of the expansion coefficient. This indicates a preset anomaly detection threshold, which can be determined by the chi-square distribution quantile, experimental calibration value, or empirical threshold for the task scenario. When Not exceeded the threshold hour, The value is 1; when Exceeding the threshold hour, It increases with the degree of abnormality and is affected by... Constraints are applied by using the aforementioned covariance elastic expansion coefficient to penalize and amplify the global observation covariance matrix obtained in step S3, resulting in the corrected global observation covariance matrix: , In the formula, This represents the global observation covariance after BaselineNIS elastic gating correction. This indicates the inflation factor on the variance scale. This represents the global observation covariance obtained by projecting the learned heteroscedasticity covariance onto its adjoint form. This formula is used to penalize and amplify the learned global observation covariance under outlier conditions, thereby reducing the weight of outlier observations in subsequent Kalman updates.

[0063] like Figure 5 As shown, step S5: Based on the elastically corrected global observation covariance obtained in step S4, calculate the Kalman gain; combine the Kalman gain with the global pseudo-innovation to calculate the state update in the Lie algebra tangent space; through... The exponential mapping completes the posterior pose state update within the manifold space; and the JosephForm stable form is used to update the posterior error covariance, finally outputting the posterior pose state and posterior error covariance at the current time.

[0064] The posterior updated covariance combines the prior state uncertainty with the elastically corrected learned observation uncertainty for subsequent gain calculation: , In the formula, This represents the posterior updated covariance used for Kalman gain calculation. This represents the global observation covariance after BaselineNIS elastic gating correction. This represents the numerical regularization term in the Kalman gain calculation. It is an m-dimensional identity matrix. For the observation matrix, Let be the prior error covariance matrix at time k. This posterior update covariance combines the prior state uncertainty and the learned observation uncertainty after elastic correction, and is used for subsequent gain calculation.

[0065] Calculate the Kalman gain based on the above posterior updated covariance: , In the formula, Indicates the first The Kalman gain matrix at time t. This represents the inverse matrix of the posterior update covariance.

[0066] Using the Kalman gain and the global pseudo-innovation vector obtained in step S3, calculate the state update in the global Lie algebraic tangent space: ; In the formula, This represents the state update quantity in the global Lie algebra tangent space. Represents the Kalman gain matrix. This represents the global pseudo-innovation vector.

[0067] The state update is converted into a Lie algebra matrix using the wedge operator. And the a priori state update is performed using the right-multiplication form consistent with prior propagation: , In the formula, Indicates the first The posterior pose state at time t. Indicates the first The prior pose state at any given time. This represents the group increment generated by the state correction. This update method applies the filtered state correction to the prior pose state in the form of a group product, ensuring that the posterior state remains within the specified range. Within the manifold space, through this closed-loop approach, heteroscedastic spurious observation generation, paired projection, BaselineNIS elastic gating, and manifold filtering updates are all integrated into the same state estimation framework.

[0068] JosephForm is used to stably update the posterior error covariance: , In the formula, Indicates the first The posterior error covariance matrix at time t. Represents the identity matrix. This represents updating the transpose of the residual matrix. This represents the transpose of the Kalman gain matrix. This formula is used to improve the numerical stability of the posterior covariance update process. To improve the numerical stability in engineering implementation, the posterior error covariance can be symmetricized and subjected to positive definite perturbation: , In the formula, This represents the positive definite perturbation coefficient of the posterior covariance. This indicates a numerical assignment or modification operation. These are the posterior covariance positive definite perturbation coefficients, used to apply small positive definite perturbations to the covariance matrix. Since it is an identity matrix, this processing can reduce the risk that floating-point errors will cause the covariance matrix to be asymmetric or non-positive definite.

[0069] This completes the posterior state update for the current moment, and outputs the posterior pose state for the current moment. With posterior error covariance The posterior pose state contains the underwater robot's two-dimensional position and heading angle information in the global coordinate system, which can be directly used for the AUV's real-time navigation, positioning, path tracking, and autonomous control decisions. The posterior error covariance represents the uncertainty of the current pose estimation and can be used for error propagation and navigation accuracy assessment in subsequent time steps. Then, the posterior pose state and posterior error covariance are used as the input for the next time step, and steps S1 to S5 are executed to form a complete recursive navigation process.

[0070] Example 2 The difference between this embodiment and Embodiment 1 is that this embodiment provides experimental verification of the AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering; To verify the beneficial effects of the technical solution proposed in this application, a comparative test was conducted using underwater robot inertial navigation and DVL-assisted positioning data. The test included comparisons of the traditional EKF method, the traditional IEKF method, a scheme to remove accompanying mappings, and the proposed solution.

[0071] Among them, the traditional EKF method and the traditional IEKF method are simplified filtering comparison schemes built based on the same DVL and inertial input, used to reflect the reference performance of the traditional filtering structure under the same input conditions; the simplification scheme that removes the adjoint mapping retains the neural network pseudo-measure output, heteroscedastic dynamic variance modeling and Cholesky positive definite constraints, but does not use the Lie group adjoint mapping in this application for covariance consistency propagation. The scheme in this application fully adopts SE(2) manifold state estimation, heteroscedastic neural network pseudo-measure, Cholesky positive definite covariance constraints, Lie group adjoint mapping and elastic gating dynamic filtering weight adjustment mechanism.

[0072] like Figures 6 to 10 As shown, this embodiment further illustrates the implementation effect of the technical solution of this application through visual experimental results. For example... Figure 6 As shown, the comprehensive navigation accuracy comparison test in this embodiment is used to demonstrate the difference in positioning trajectory between the proposed solution and the traditional EKF method and the simplified neural network IEKF method on the same underwater robot navigation test trajectory. Figure 7 As shown, this corresponds to the DVL anomaly observation and identification process in this embodiment, illustrating the relationship between DVL x-axis velocity, DVL y-axis velocity, anomaly score, and severe anomaly threshold over time. Figure 8 As shown, this corresponds to the heteroscedastic pseudo-observation generation process in this embodiment, used to demonstrate the normalized distribution of pseudo-measurement uncertainties in the x-direction, y-direction, and correction direction of the organism. For example... Figure 9 As shown, corresponding to the filtering update process in this embodiment, it illustrates the relationship between cross-gain, x-axis gain, y-axis gain, and log-covariance trace over time. Figure 10As shown, this corresponds to the local error analysis process in this embodiment, illustrating the relationship between the local error norm, x-direction error, and y-direction error over time. Figures 6 to 10 Together with Tables 1 to 3 below, they are used to illustrate the implementation effects of the proposed solution in terms of positioning accuracy, anomaly observation suppression, dynamic uncertainty expression, filter gain adjustment, and local error control.

[0073] Table 1 Comparison of Overall Navigation Accuracy

[0074] As shown in Table 1, the proposed scheme exhibits superior overall performance in terms of average cumulative positioning error, high quantile error, error coverage, and maximum endpoint error. Compared to traditional EKF and traditional IEKF, the average cumulative positioning error of the proposed scheme is reduced by 26.76% and 26.73%, respectively; compared to the scheme that removes the accompanying mapping, the average cumulative positioning error of the proposed scheme is reduced by 41.14%.

[0075] Meanwhile, the P95 error of this scheme is 6.48m, lower than the 10.213m of the traditional EKF, the 10.210m of the traditional IEKF, and the 13.733m of the scheme that removes the accompanying mapping. This scheme increases the percentage of errors within 10m to 100%, with a maximum endpoint error of 4.946m. The above results demonstrate that this scheme can effectively reduce cumulative errors, high-quantile errors, and endpoint drift during long-endurance underwater positioning.

[0076] Table 2 Comparison of Anomaly Observations and Resistance to Long-Tail Errors

[0077] Traditional gating methods are fixed threshold or inelastic gating control methods; the elastic gating method proposed in this embodiment is an elastic gating method based on observation residuals, uncertainty estimation, and adaptive adjustment of anomaly intensity. The above operating conditions are all extracted from a typical underwater robot complex navigation test trajectory: the high-risk section of the trajectory corresponds to the severe DVL flypoint condition; the cumulative drift pressure section at the end of the trajectory corresponds to the long-term DVL degradation condition, the long-term recursive condition, and the cumulative drift boundary condition; and the high angular velocity maneuvering section corresponds to the complex maneuvering interference condition. The convergence recovery time is defined as the time required for the positioning error to first fall back to within 5m after the complex maneuver and remain continuously for at least 2s; if this condition is not met within the observation window, it is recorded as being greater than the observation window length.

[0078] As shown in Table 2, under typical underwater anomaly conditions such as DVL flypoints, prolonged degradation, complex maneuvers, and prolonged recursion, the elastic gating method of this embodiment exhibits better error suppression and state recovery capabilities compared to traditional gating methods. Under severe DVL flypoint conditions, the elastic gating method of this invention reduces the maximum positioning deviation from 13.240m to 7.295m, a reduction of approximately 44.9%, indicating that this method can reduce the impact of instantaneous anomaly observations on the positioning state. Under prolonged DVL degradation conditions, the drift velocity decreases from 0.0056m / s to 0.0027m / s, a reduction of approximately 51.7%, indicating that this method can delay the cumulative error growth caused by long-term degradation observations. Under complex maneuvering interference conditions, the traditional gating method fails to meet the stable recovery condition within an 80.38s observation window, while the convergence recovery time of the elastic gating method of this invention is 33.75s, a reduction of at least 58.0%. The results demonstrate that the present invention can guide the system to recover a stable estimate more quickly after complex maneuvers.

[0079] Under both long-term recursive and cumulative drift boundary conditions, the elastic gating method in this embodiment reduced the P95 positioning error from 13.810m to 4.914m and the maximum positioning error from 13.824m to 4.928m, respectively, with a reduction of approximately 64.4%. This demonstrates that the elastic gating method of this invention can significantly compress long-tail errors and cumulative drift boundaries, reducing the divergence risk during long-endurance navigation of underwater robots.

[0080] Table 3. Configuration of Core Modules for Each Technical Solution

[0081] It is important to note that Table 3 illustrates the differences in core module configurations among the various technical solutions. Specifically, the Lie group adjoint mapping is used to achieve consistent propagation between the machine system's pseudo-measurement uncertainty and the global error state space; heteroscedastic dynamic variance is used to dynamically adjust the pseudo-measurement reliability based on observation quality; and Cholesky positive definite constraints are used to ensure the positive definiteness of the network's output covariance matrix.

[0082] As shown in Table 3, there are significant differences in the core module configurations of the various comparative schemes. Combined with the quantitative results in Table 1, it can be seen that the comparative schemes, which only possess basic manifold constraints or only employ partial heteroscedasticity mechanisms, are insufficient to simultaneously address long-tail errors, anomalous observation impacts, and endpoint drift issues in long-endurance underwater positioning. The scheme in this application fully configures SE(2) manifold estimation, Lie group adjoint mapping, heteroscedasticity dynamic variance, and Cholesky positive definite constraints, thus achieving a more stable error suppression effect under complex underwater motion conditions.

[0083] Example 3 This embodiment provides an AUV integrated navigation system based on heteroscedasticity sensing and manifold filtering; The module comprises an IMU data acquisition module, a DVL data acquisition module, a manifold prior propagation module, a heteroscedastic pseudo-observation generation module, an adjoint projection module, a Baseline NIS elastic gating module, and a manifold posterior closed-loop update module; each module is sequentially connected to each other and is used to execute the corresponding method logic.

[0084] The IMU data acquisition module is used to collect high-frequency inertial measurement data of the underwater robot; the DVL data acquisition module is used to collect low-frequency local velocity measurement data of the underwater robot; the manifold prior propagation module is used to time-align the IMU data and DVL data, and construct a time-series sliding window for inputting the neural network, calculating the prior state in the "SE(2)" manifold space based on the DVL displacement increment and the IMU heading increment; the heteroscedasticity pseudo-observation generation module is used to output the mean of the pseudo-residuals of the machine system and its heteroscedasticity covariance based on the multi-source time-series observation information; the adjoint projection module is used to synchronously project the pseudo-residuals of the machine system and its heteroscedasticity covariance to the global Lie algebra tangent space using the Lie group adjoint matrix constructed from the prior state; the Baseline NIS elastic gating module is used to construct an anomaly detection baseline based on the physical nominal covariance, and perform elastic expansion correction on the learned global observation covariance based on the normalized innovation square value; the manifold posterior closed-loop update module is used to calculate the Kalman gain based on the corrected global observation covariance, and in "SE(2)"... The final posterior pose state and posterior error covariance are output within the manifold space.

[0085] The heteroscedastic pseudo-observation generation module includes a feature embedding submodule, a multi-scale temporal convolution submodule, a bidirectional temporal memory submodule, a multi-head temporal attention submodule, a shared latent variable submodule, a pseudo-residual mean output submodule, and a heteroscedastic covariance output submodule. The pseudo-residual mean output submodule is used to output the two-dimensional pseudo-residual mean under the machine system, and the heteroscedastic covariance output submodule is used to output the Cholesky covariance parameter, and reconstruct the two-dimensional heteroscedastic covariance matrix of the generator system through a positive activation function and Cholesky. This module further augments the two-dimensional pseudo-residual mean and the two-dimensional heteroscedastic covariance matrix into a machine system pseudo-observation vector and an augmented heteroscedastic covariance matrix consistent with the error state dimension of "SE(2)", which are used for subsequent adjoint projection module and manifold posterior closed-loop update module.

[0086] The above are all preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Therefore, all equivalent changes made in accordance with the structure, shape and principle of the present invention should be covered within the scope of protection of the present invention.

Claims

1. An AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering, characterized in that, include: Acquire high-frequency inertial measurement data and low-frequency local velocity measurement data, construct SE(2) manifold motion input, and complete the propagation of prior pose state and prior error covariance based on SE(2) exponential mapping; A time-series sliding window is constructed, and multi-source time-series observation data are input into a pre-trained heteroscedastic pseudo-observation neural network model. The output is a two-dimensional pseudo-residual mean and Cholesky covariance parameter. After reconstruction and augmentation, augmented pseudo-observation vectors and augmented heteroscedastic covariance matrix are generated. The output of the two-dimensional pseudo residual mean and Cholesky covariance parameters includes constructing a time-series sliding window covering the current time and historical observation data. The multi-source time-series observation data in the time-series sliding window includes high-frequency inertial data from the inertial measurement unit, local velocity data from the Doppler log, sampling time interval, and attitude auxiliary quantities. The multi-source time-series observation data within the time-series sliding window is input into the pre-trained heteroscedastic pseudo-observation neural network model. The heteroscedastic pseudo-observation neural network model consists of a feature embedding layer, a multi-scale time-series convolutional layer, a bidirectional time-series memory layer, a multi-head time-series attention layer, a shared latent variable layer, a pseudo residual mean output head, and a heteroscedastic covariance output head. The two-dimensional pseudo-residual mean is output from the pseudo-residual mean output head of the system, and the Cholesky covariance parameter is output from the heteroscedasticity covariance output head. The output of the heteroscedastic pseudo-observation neural network model satisfies: ， in, Let be the mean of the two-dimensional pseudo-residuals in the system at time k. Let be the Cholesky covariance parameter vector at time k. This is a heteroscedastic pseudo-observation neural network model. For the time-series sliding window at time k; Based on the prior pose state, the Lie group adjoint transformation matrix is ​​constructed, and the augmented pseudo-observation vector and the augmented heteroscedasticity covariance matrix are synchronously mapped to the global Lie algebraic tangent space to generate the global pseudo-innovation vector and the global observation covariance matrix. The generation of the global pseudo-innovation vector and the global observation covariance matrix includes: The SE(2) Lie group adjoint transformation matrix is ​​constructed based on the prior pose state. The SE(2) Lie group adjoint transformation matrix is ​​used to describe the coordinate transformation relationship between the elements of the SE(2) group and the Lie algebraic tangent space vector. Using the SE(2) Lie group adjoint transformation matrix to perform coordinate transformation on the augmented pseudo-observation vector, the augmented pseudo-observation vector under the machine system is projected onto the global Lie algebra tangent space to generate the global pseudo-innovation vector; Using the adjoint transformation matrix and its transpose of the SE(2) Lie group, a synchronous coordinate transformation is performed on the augmented heteroscedasticity covariance matrix. The augmented heteroscedasticity covariance matrix under the machine system is projected onto the global Lie algebraic tangent space to generate the global observation covariance matrix. The expressions for the global pseudo-innovation vector and the global observation covariance matrix are as follows: ， ， in, Let be the global pseudo-innovation vector at time k. Let be the adjoint transformation matrix of the SE(2) Lie group constructed from the prior pose state at time k. Let k be the augmented pseudo-observation vector at time k. Let be the global observation covariance matrix at time k. Let be the augmented heteroscedasticity covariance matrix at time k; The BaselineNIS detection baseline is constructed based on the nominal observation covariance of physical priors. The normalized squared innovation value is calculated using the global pseudo-innovation vector. Based on the comparison results, the global observation covariance matrix is ​​corrected by elastic expansion. The filter gain is calculated based on the corrected global observation covariance matrix, and the Lie algebraic tangent space state update is calculated in combination with the global pseudo-innovation vector. The posterior pose state and posterior error covariance are updated in the manifold space by the SE(2) exponential mapping, and the posterior pose state and posterior error covariance are output.

2. The AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering according to claim 1, characterized in that, The propagation of prior pose state and prior error covariance based on SE(2) exponential mapping includes time synchronization alignment and displacement integration of the system velocity measured by the Doppler log to obtain the system displacement increment. Integrate the angular velocity of the heading axis measured by the inertial measurement unit to obtain the heading angle increment; The displacement increment and heading angle increment of the machine system are constructed as motion inputs in the tangent space of the SE(2) Lie algebra, and the manifold pose increment is generated by the SE(2) exponential mapping. The manifold pose increment is applied to the posterior pose state of the previous time step based on the right multiplication form to obtain the prior pose state of the current time step. Based on the state error propagation Jacobian matrix and the process noise input Jacobian matrix, the posterior error covariance of the previous time step is propagated prior to obtain the prior error covariance of the current time step. The expression for the prior pose state is: ， in, Let be the prior pose state at time k. Let be the posterior pose state at time k-1. The motion input is the Lie algebra matrix transformed by the wedge operator. For the SE(2) exponential mapping operator.

3. The AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering according to claim 1, characterized in that, The process of generating augmented pseudo-observation vectors and augmented heteroscedasticity covariance matrix after reconstruction and augmentation includes constructing a Cholesky lower triangular matrix after constraining the Cholesky covariance parameters with a positive activation function, and reconstructing the two-dimensional heteroscedasticity covariance matrix by multiplying the Cholesky lower triangular matrix with its transpose. The two-dimensional pseudo residual mean and two-dimensional heteroscedasticity covariance matrix under the machine system are augmented to the three-dimensional error state space of SE(2) by translation and embedding matrix, generating augmented pseudo-observation vectors and augmented heteroscedasticity covariance matrix. The expression of the two-dimensional heteroscedasticity covariance matrix is ​​as follows: ， in, Let the two-dimensional heteroscedasticity covariance matrix of the system at time k be denoted as . Let be the Cholesky lower triangular matrix at time k. for The transpose of the matrix, For covariance regularization, It is a two-dimensional identity matrix.

4. The AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering according to claim 3, characterized in that, The training process of the heteroscedastic pseudo-observation neural network model includes: The machine system supervision residual is constructed based on the actual trajectory or high-precision reference trajectory of the underwater robot. The machine system supervision residual is used to characterize the deviation between the actual displacement increment and the Doppler log integrated displacement increment. Construct a heteroscedastic negative log-likelihood loss function based on the system monitoring residuals, the mean of two-dimensional pseudo residuals, and the two-dimensional heteroscedastic covariance matrix; The heteroscedastic pseudo-observation neural network model is trained using the heteroscedastic negative log-likelihood loss function, enabling it to learn the displacement correction caused by Doppler log errors, inertial measurement unit integral errors, and local flow field disturbances during underwater motion.

5. The AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering according to claim 1, characterized in that, The calculation of the normalized innovation squared value using the global pseudo-innovation vector includes projecting the physical nominal observation covariance under the machine system onto the global error space to obtain the physical nominal observation covariance in the global error space; constructing the BaselineNIS detection baseline covariance based on the observation matrix, the prior error covariance matrix, and the physical nominal observation covariance in the global error space; and calculating the normalized innovation squared value using the global pseudo-innovation vector and the BaselineNIS detection baseline covariance. The formula for calculating the normalized innovation squared value is as follows: ， in, Let be the normalized squared innovation value at time k. Let be the global pseudo-innovation vector at time k. The baseline covariance is detected by BaselineNIS at time k.

6. The AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering according to claim 5, characterized in that, The step of performing elastic expansion correction on the global observation covariance matrix based on the comparison results includes comparing the normalized innovation squared value with a preset anomaly detection threshold. When the normalized innovation squared value exceeds the preset anomaly detection threshold, a covariance elastic expansion coefficient is generated. This coefficient is then used to penalize and amplify the global observation covariance matrix, resulting in a corrected global observation covariance matrix. The corrected global observation covariance matrix satisfies the following: ， in, Let $k$ be the global observation covariance matrix corrected at time k. Let be the covariance elastic expansion coefficient at time k. Let be the global observation covariance matrix at time k, and let the covariance elastic expansion coefficient satisfy: ， in, This is the upper limit of the expansion coefficient. This is a preset anomaly detection threshold.

7. The AUV integrated navigation method based on heteroscedasticity sensing and manifold filtering according to claim 1, characterized in that, The output posterior pose state and posterior error covariance include constructing a posterior update covariance based on the corrected global observation covariance matrix, the prior error covariance matrix and the observation matrix. The observation matrix is ​​used to map the error state in the Lie algebra tangent space to the pseudo-observation space. The Kalman gain is calculated using the posterior updated covariance, and the state update in the Lie algebraic tangent space is calculated based on the Kalman gain and the global pseudo-innovation vector. The state update quantity is converted into a Lie algebra matrix through the wedge operator, and the right multiplication form consistent with the prior propagation is adopted. The state correction quantity is applied to the prior pose state through the SE(2) exponential mapping, and the prior pose state is updated. The posterior error covariance is updated using the JosephForm stable update method, and the posterior pose state and posterior error covariance are output.

8. An AUV integrated navigation system based on heteroscedasticity sensing and manifold filtering, executing the method described in claim 1, characterized in that, include: The manifold prior propagation module is used to construct the SE(2) manifold motion input from the measurement data collected by the IMU data acquisition module and the DVL data acquisition module, and to complete the propagation of the prior pose state and prior error covariance based on the SE(2) exponential mapping. The heteroscedastic pseudo-observation generation module is used to construct a time-series sliding window based on measurement data and generate augmented pseudo-observation vectors and augmented heteroscedastic covariance matrices. The adjoint projection module is used to construct the adjoint transformation matrix of the Lie group and generate the global pseudo-innovation vector and the global observation covariance matrix. The BaselineNIS elastic gating module is used to construct a detection baseline based on the nominal observation covariance of physical priors, calculate the normalized squared innovation value using the global pseudo-innovation vector, and perform elastic expansion correction on the global observation covariance matrix. The manifold posterior closed-loop update module is used to output the posterior pose state and the posterior error covariance.

Images