A multi-modal hierarchical-based AUV three-dimensional pose joint estimation method and system

Abstract

The present application relates to underwater positioning technology field, especially in kind based on multimodal layered AUV three-dimensional position joint estimation method and system, the method, including the historical observation sequence is assigned to the corresponding independent convolutional neural network branch according to mode, local time sequence feature extraction is carried out through each branch, and the local time sequence feature of each mode is output;Based on the local time sequence feature of each mode, hierarchical feature fusion is carried out, global fusion feature is input into double branch regression head, and is decoded through position regression branch and attitude regression branch respectively, and three-dimensional position increment and attitude increment are output;Loss function is constructed by using three-dimensional position increment and attitude increment, and the optimal model is obtained by training and is deployed to AUV platform, the present application breaks through the limitation that traditional method only focuses on two-dimensional plane or separate estimation pose, and the full space positioning demand of AUV three-dimensional maneuvering task is completely covered, and the integrity and consistency of pose estimation are improved.

Description

Technical Field

[0001] This invention relates to the field of underwater positioning technology, and in particular to a method and system for joint estimation of AUV three-dimensional pose based on multimodal hierarchical structure. Background Technology

[0002] Autonomous underwater vehicles (AUVs) are important tools for exploring the ocean, developing resources, and safeguarding national security. Their autonomous navigation and positioning technologies have become key indicators for measuring a country's marine science and technology strength. High-precision underwater positioning is not only a prerequisite for AUVs to perform long-endurance and wide-range missions, but also a core supporting technology for promoting high-quality development of the marine economy, implementing marine environmental monitoring, and supporting cutting-edge scientific research.

[0003] In real-world marine environments, AUV navigation faces multiple challenges, including high noise levels, drastic environmental changes, and incomplete observation information. These factors reduce the overall accuracy and robustness of underwater positioning systems. In existing technologies, traditional state estimation methods remain the foundation for AUV navigation. Kalman filtering and its extensions, as well as unscented Kalman filtering, are widely used. To overcome the limitations of traditional filtering methods in complex noise modeling and strong nonlinear processing, several deep learning-based underwater navigation methods have emerged in recent years. For example, application number CN202411063429.5 discloses an underwater positioning method based on multimodal data fusion and ensemble learning, which employs Transfor... The Mer architecture jointly models data from the inertial measurement unit, Doppler velocimeter, and geomagnetic navigation sensor. However, while the Transformer-based multimodal localization method improves the fusion capability of multi-source information to some extent and is more suitable for handling nonlinear and noisy environments compared to traditional filtering methods, it still mainly focuses on two-dimensional position estimation and does not achieve joint modeling of three-dimensional position and attitude in a unified network. At the same time, it lacks a hierarchical decoupling and intramodal temporal modeling mechanism for sensor data with different physical characteristics. The modeling efficiency and robustness in long-endurance scenarios still have room for improvement. At present, there is a need for a multimodal hierarchical AUV three-dimensional pose joint estimation method and system. Summary of the Invention

[0004] To address the technical problems of incomplete 3D spatial perception and insufficient robustness of multimodal fusion in existing AUV pose estimation methods, this invention provides a joint 3D pose estimation method and system for AUVs based on multimodal layering.

[0005] In a first aspect, the present invention provides a joint estimation method for AUV three-dimensional pose based on multimodal hierarchical structure, which adopts the following technical solution:

[0006] A multimodal hierarchical AUV 3D pose joint estimation method includes:

[0007] Acquire multimodal sensor data from the AUV and preprocess the multimodal sensor data to form a structured historical observation sequence;

[0008] Historical observation sequences are assigned to corresponding independent convolutional neural network branches according to modality, and local temporal features are extracted through each branch to output the local temporal features of each modality;

[0009] Based on the local temporal features of each modality, hierarchical feature fusion is performed, including the construction of a three-level architecture consisting of independent modeling within modality, cross-modal gating interaction, and global temporal consistency optimization. The global fused features are output through the three-level architecture.

[0010] The global fusion features are input into the dual-branch regression head, and decoded through the position regression branch and the pose regression branch respectively, outputting the 3D position increment and pose increment;

[0011] A loss function is constructed using 3D position increments and pose increments. The optimal model is trained and deployed to the AUV platform. The real-time 3D pose of the AUV is obtained by using the pose increments output in real time by the optimal model.

[0012] Furthermore, the preprocessing of the multimodal sensor data includes defining the time axis as the alignment reference, using linear interpolation to time-align the multimodal sensor data, unifying the sampling frequency of all modal sensor data, using adaptive noise threshold filtering to remove interference data, dividing the interference-free multimodal sensor data into several time segments of consistent length using a fixed-length sliding window strategy, calculating the mean and standard deviation of each type of modal data, and performing standardization and normalization transformation on each modal data.

[0013] Furthermore, the output of local temporal features for each modality includes constructing independent convolutional neural network branches based on modality categories from historical observation sequences. Each branch employs a residual learning framework consisting of a backbone path and residual paths. The backbone path contains multiple cascaded one-dimensional convolutional layers, with each layer followed by layer normalization and ReLU activation functions. The feature dimension is progressively expanded by increasing the number of filters layer by layer. The residual paths dynamically adapt to the difference in the number of channels between the input and output of the backbone path to maintain a consistent number of channels. After dynamic adaptation, the input features are passed to the output to obtain the local temporal features for each modality. The expression for these local temporal features is:

[0014] ,

[0015] in, For the local temporal features of the i-th mode, This is the mapping function for the main path of the convolution branch. This is the residual path mapping function for the convolution branch. This is the preprocessed historical observation sequence for the i-th mode.

[0016] Furthermore, the intramodal independent modeling includes projecting the local temporal features of each modality through linear layers to obtain feature vectors adapted for intramodal modeling. An independent Mamba module is configured for each modality. The Mamba module adopts a residual block stacking structure, with a pre-normalization layer, a Mamba layer, and residual connections sequentially set within each residual block. When independently modeling the long-term temporal dependencies of each modality using the Mamba module, the Mamba layer first expands the input dimension through linear layers, then dynamically generates state space parameters using depthwise convolution and activation functions. After discretizing the continuous state parameters using zero-order preservation, a selective scanning mechanism is used to recursively update the temporal state, ultimately outputting the enhanced features of each modality.

[0017] Furthermore, the cross-modal gating interaction concatenates the enhanced features of each modality along the channel dimension to obtain initial cross-modal fusion features. A gating mechanism is introduced to adaptively filter the initial cross-modal fusion features. A gating weight vector is generated through an activation function, and the initial fusion features are weighted element-wise. The filtered features are linearly mapped to generate the query, key, and value vectors required by the self-attention mechanism. Cross-modal association weights are obtained by calculating the similarity between vectors. The value vectors are then weighted and fused based on the association weights. Finally, the cross-modal fusion features are output through residual connections and layer normalization. The calculation formula for the adaptive filtering is as follows:

[0018] ,

[0019] in, For cross-modal features after gating, This refers to the initial fused features after splicing the enhancement features from each modality. It is the Sigmoid activation function. For the gated weight matrix, This is the gated bias vector. This is for element-wise multiplication.

[0020] Furthermore, the global temporal consistency optimization includes inputting cross-modal fusion features into a global Mamba module. The structure of the global Mamba module is consistent with that of the intra-modal Mamba module. By adjusting the state dimension and regularization parameters, and utilizing the state-space modeling and selective scanning mechanism of the global Mamba module, unified modeling of the cross-modal fusion features across the entire temporal range is performed. Global optimization of the cross-modal fusion features is then performed, outputting the final global fusion feature. The output formula for the global fusion feature is:

[0021] ,

[0022] in, For the final global fusion features, Features of cross-modal gated interactive output, These are mapping functions for the global Mamba module. This is a layer normalization operation.

[0023] Furthermore, the decoding via position regression and pose regression branches includes constructing a dual-branch regression head containing both a position regression branch and a pose regression branch. Each regression branch consists of a linear layer, a ReLU activation function, and a Dropout layer connected in series. The linear layer performs feature dimension transformation and mapping, and the globally fused features are simultaneously input into both regression branches. The position regression branch performs layer-by-layer feature transformation and nonlinear mapping on the globally fused features, and then decodes and outputs the three-dimensional position increment. The pose regression branch decodes and outputs the three-dimensional pose increment through the same network structure logic. The regression formula for the three-dimensional position increment is:

[0024] ,

[0025] in, For three-dimensional position increment, and These are the two linear layers in the location regression branch. For regularization operations, This is the activation function.

[0026] Furthermore, the construction of the loss function using 3D position and pose increments includes acquiring reference pose data as sample labels, performing incremental calculations on the sample labels to obtain corresponding label values, and calculating the corresponding single-task loss using the Huber loss function for both the position regression and pose regression tasks. An adaptive weighting strategy based on task uncertainty is introduced, dynamically adjusting the weight ratio of the two single-task losses in the total loss based on the noise variance of the position regression and pose regression tasks. The Huber loss function calculation formula is as follows:

[0027] ,

[0028] in, This represents Huber's loss value. This refers to the 3D position or attitude increment output by the model. For the corresponding tag value, This is the threshold parameter of the loss function.

[0029] Furthermore, the method of constructing a loss function using 3D position and pose increments also includes constructing a total loss function for model training based on single-task Huber loss and task uncertainty weights. This involves weighting and summing the single-task losses using adaptive weights, adding a regularization term to avoid training imbalance, and using an adaptive optimizer to iteratively optimize the total loss function using gradient descent. All trainable parameters of the model are updated through gradient backpropagation. An early stopping mechanism is also introduced, terminating training when the validation set loss decreases below a set threshold. The expression for the total loss function is:

[0030] ,

[0031] in, and These represent the noise variances for the position regression task and the pose regression task, respectively. and The Huber loss is calculated for position regression and pose regression, respectively. and These are the 3D position increment and attitude increment output by the model, respectively. and These are the corresponding tag values.

[0032] Secondly, a multimodal hierarchical AUV 3D pose joint estimation system includes:

[0033] The data acquisition module is configured to acquire multimodal sensor data from the AUV and preprocess the multimodal sensor data to form a structured historical observation sequence.

[0034] The feature extraction module is configured to: assign historical observation sequences to corresponding independent convolutional neural network branches according to modality, extract local temporal features through each branch, and output the local temporal features of each modality;

[0035] The feature fusion module is configured to perform hierarchical feature fusion based on the local temporal features of each modality, including constructing a three-level architecture consisting of independent modeling within modality, cross-modal gating interaction, and global temporal consistency optimization, and outputting global fused features through the three-level architecture;

[0036] The joint regression module is configured to: input global fused features into the dual-branch regression head, decode them through the position regression branch and the pose regression branch respectively, and output the 3D position increment and pose increment;

[0037] The deployment module is configured to: construct a loss function using 3D position increments and pose increments, train the optimal model and deploy it to the AUV platform, and obtain the real-time 3D pose of the AUV using the pose increments output in real time by the optimal model.

[0038] In summary, the present invention has the following beneficial technical effects:

[0039] 1. This invention achieves the unification of multi-source data in terms of time dimension and numerical range by using the time axis of the high-frequency inertial measurement unit as a reference and the linear interpolation method to synchronize multi-modal sensor data, and by combining sliding window partitioning and standardized normalization preprocessing techniques. This provides structured and high-quality input data for subsequent feature extraction and pose estimation, and effectively eliminates error interference caused by data mismatch and dimensional differences.

[0040] 2. This invention constructs independent convolutional neural network branches, adopts a residual learning framework with backbone paths and residual paths, and combines a hierarchical design of one-dimensional convolutional layers, layer normalization, and ReLU activation functions to achieve deep decoupled extraction of local temporal features of data from different modalities. This not only expands the feature receptive field and enhances the feature representation ability, but also alleviates the gradient decay and feature degradation problems of deep networks through residual connections, thereby improving the extraction accuracy and stability of local features.

[0041] 3. This invention designs a three-level hierarchical fusion architecture that integrates intramodal independent modeling, cross-modal gating interaction, and global temporal consistency optimization. It utilizes the state-space modeling and selective scanning mechanism of the Mamba module to capture long-term temporal dependencies, uses gating mechanisms to filter reliable features and attention mechanisms to achieve cross-modal association alignment, and finally optimizes temporal consistency through the global Mamba module. This achieves deep fusion and efficient modeling of multimodal information, significantly improving the model's ability to process long-duration flight data and its robustness in multi-sensor disturbance scenarios.

[0042] 4. This invention constructs a dual-branch regression head to synchronously decode and output the three-dimensional position increment and attitude increment within a unified network framework, thereby achieving joint estimation of position and attitude. This breaks through the limitations of traditional methods that only focus on two-dimensional planes or estimate pose separately, fully covering the full-space positioning requirements of AUV three-dimensional maneuvering missions and improving the completeness and consistency of pose estimation.

[0043] 5. This invention improves the model's resistance to outliers by adopting the Huber loss function and constructs the total loss function by combining an adaptive weighting strategy based on task uncertainty. This achieves a dynamic balance between the two tasks of position regression and pose regression. With the help of an adaptive optimizer, learning rate scheduling and early stopping mechanism, the training efficiency and generalization ability of the model are effectively improved, ensuring that the AUV can output high-precision and stable real-time three-dimensional pose information in complex marine environments after deployment. Attached Figure Description

[0044] Figure 1 This is a schematic diagram of the overall process of a multimodal hierarchical AUV three-dimensional pose joint estimation method according to an embodiment of the present invention.

[0045] Figure 2 This is a diagram of the Mamba module architecture according to an embodiment of the present invention.

[0046] Figure 3 This is a schematic diagram of the data processing flow according to an embodiment of the present invention.

[0047] Figure 4 This is a comparison chart of the positioning trajectories of various algorithms in the indoor water tank dataset in the embodiments of the present invention.

[0048] Figure 5 This is a comparison chart of the z-axis estimation results of various algorithms in the indoor pool dataset in the embodiments of the present invention.

[0049] Figure 6 This is a comparison chart of the positioning trajectories of various algorithms in the embodiment of the present invention on a certain segment of the trajectory in the seaside pool dataset. Detailed Implementation

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

[0051] Example 1: Refer to Figure 1 This embodiment of a multimodal hierarchical AUV three-dimensional pose joint estimation method includes:

[0052] S1. Acquire multimodal sensor data from the AUV and preprocess the multimodal sensor data to form a structured historical observation sequence;

[0053] S2. Assign historical observation sequences to corresponding independent convolutional neural network branches according to modality, extract local temporal features through each branch, and output the local temporal features of each modality;

[0054] S3. Perform hierarchical feature fusion based on the local temporal features of each modality, including constructing a three-level architecture consisting of independent modeling within modality, cross-modal gating interaction, and global temporal consistency optimization, and outputting global fused features through the three-level architecture;

[0055] S4. Input the global fusion features into the dual-branch regression head, decode them through the position regression branch and the pose regression branch respectively, and output the 3D position increment and pose increment.

[0056] S5. Construct a loss function using the 3D position increment and pose increment, train the optimal model and deploy it to the AUV platform, and use the pose increment output by the optimal model in real time to obtain the real-time 3D pose of the AUV.

[0057] Specifically, a multimodal hierarchical AUV 3D pose joint estimation method includes the following:

[0058] S1. Acquire multimodal sensor data from the AUV and preprocess the multimodal sensor data to form a structured historical observation sequence;

[0059] like Figure 1 , Figure 3 As shown, multimodal sensor data acquisition is first performed, and the triaxial linear acceleration data of the AUV is acquired in real time through a strapdown inertial measurement unit. Three-axis angular velocity data and attitude information ,in, This is the roll angle. The pitch angle, The yaw angle was then used to acquire the AUV's three-dimensional velocity information using an acoustic Doppler velocimeter (DVL). Depth information of AUVs is collected using high-precision depth gauges. This generates five core modal data types, encompassing motion acceleration, rotational angular velocity, attitude state, movement speed, and vertical depth, comprehensively characterizing the motion state of the AUV.

[0060] The data preprocessing process is then initiated. The first step is time synchronization and sampling frequency unification. The time axis of the high-frequency inertial measurement unit (IMU) is defined as the global alignment reference because it has the highest sampling frequency and strong data continuity, and can provide accurate time anchors for other modal data. To address the issue that the sampling frequency of the acoustic Doppler velocimeter (DVL) and depth gauge is lower than that of the IMU, linear interpolation is used to time-align their data. The core principle of linear interpolation is based on the difference between the values ​​and timestamps of two adjacent valid sampling points. By linear fitting, the value corresponding to the missing timestamp is calculated to ensure that each modal data has a corresponding observation value at each time node. Furthermore, the sampling frequency of all modal data is unified to a preset target frequency. In this embodiment, the target frequency is set to 100Hz, which can ensure that the temporal resolution of the data meets the requirements of long-endurance modeling, and will not cause data redundancy due to excessively high frequency.

[0061] To further enhance the data's robustness against interference, this embodiment incorporates adaptive noise threshold filtering. Based on the statistical characteristics of each modality's data, the local variance of each modality's data is calculated in real-time using a sliding window. And dynamically adjust the noise threshold based on the local variance. The threshold calculation formula is: ,in, This is an adaptive adjustment coefficient, ranging from 0.8 to 1.2, dynamically adjusted according to the actual marine environment. This represents the mean of the historical local variances. The standard deviation of historical local variance is used to identify and correct outliers in each modal data. When the value of a sampling point differs from the mean of the five adjacent sampling points by more than a threshold, the outlier is corrected. When an outlier is detected, it is corrected by a weighted average of adjacent sampling points. The weight decreases linearly with the distance from the outlier, meaning the closer the distance, the greater the weight. This effectively filters out sudden noise caused by strong instantaneous interference in the marine environment, while retaining the real data mutations generated by AUV maneuvers.

[0062] After noise filtering, a sliding window sequence is divided. A fixed-length sliding window strategy is used to perform structured processing on the unified multimodal data. The preset sliding window length is L and the sliding step size is s. By sliding the window, the continuous sensor data stream is divided into several time segments of the same length. Each time segment contains L consecutive sampling points. Each sampling point corresponds to the synchronous data of 5 modalities, forming a structured local observation window. This can capture motion features in a short time and provide basic data units for subsequent long-term time-series dependency modeling.

[0063] Finally, a standardization and normalization transformation is performed, and the global mean of each modality data is calculated across all sliding windows. and global standard deviation , These correspond to five modalities, with the mean value being... N is the total number of sampling points for this modality data. (where the value is the k-th sampling point value of the i-th mode), standard deviation The modal data are standardized and normalized based on the mean and standard deviation, and the transformation formula is as follows: ,in, For the normalized i-th modal data, The original data for the i-th modality is processed to eliminate the differences in dimensions and numerical ranges between different modal data, so that all modal data are mapped to the range of a standard normal distribution with a mean of 0 and a standard deviation of 1, while preserving the relative characteristic differences of each modal data, and finally forming a structured historical observation sequence. Each historical observation sequence corresponds to normalized multimodal time series data within a sliding window.

[0064] S2. Assign historical observation sequences to corresponding independent convolutional neural network branches according to modality, extract local temporal features through each branch, and output the local temporal features of each modality;

[0065] Based on historical observation sequences, the modal data is categorized into five types: three-axis linear acceleration, three-axis angular velocity, attitude information, three-dimensional velocity, and depth information. Independent convolutional neural network branches are configured for each of these categories, denoted as... to Each branch extracts features only for the corresponding modality data, avoiding feature interference caused by differences in the physical characteristics of different modalities. All convolutional neural network branches adopt a unified residual learning framework. The core advantage of this framework is that it alleviates the gradient decay and feature degradation problems in the training process of deep networks through residual connections, while maintaining the resolution of the time dimension.

[0066] Each branch of the convolutional neural network consists of a backbone path and a residual path. The backbone path is responsible for the progressive extraction of local temporal features, specifically configured as multiple cascaded one-dimensional convolutional layers (Conv1D). The design of the one-dimensional convolutional layers is adapted to the characteristics of temporal data, capturing local correlation information between adjacent sampling points through convolution operations. Its calculation formula is as follows: ,in, The convolution output feature value at time t. For input feature values, Here, K represents the weight parameter at the k-th position of the convolution kernel, and K is the kernel size. To pad the offset, a "same" padding strategy is used to ensure that the output temporal length after convolution is consistent with the input. The parameter settings of each one-dimensional convolutional layer in the main path follow the principle of progressive expansion of feature dimensions: the convolutional kernel size is uniformly adopted to adapt to the size of local correlation capture of temporal data, ensuring that it can cover feature interactions within a reasonable time window; the convolutional stride is set to 1, through the formula... The stride is correlated to ensure that the temporal dimension of the output feature after each convolution operation is consistent with that of the input. The padding method is "same" padding, which adds values ​​to the edges of the input sequence to make the temporal length of the input and output of the convolution operation in the formula completely match.

[0067] In the main path, the number of filters in each one-dimensional convolutional layer increases sequentially from low to high. The number of filters in the initial layer is adapted to the basic feature extraction requirements of the original data. The number of filters in each subsequent convolutional layer is reasonably increased based on the previous layer. The corresponding feature dimension expansion is defined by the number of output channels of the convolution operation, and its mathematical expression is: ,in, The number of output feature channels of the l-th convolutional layer. Let be the number of filters in the l-th convolutional layer, and satisfy . By increasing the number of filters, the network can progressively extract high-dimensional local temporal features from the original low-dimensional data. After each one-dimensional convolutional operation, layer normalization and ReLU activation functions are sequentially connected. The core formula for layer normalization is... ,in, The mean of the features, The variance of the feature To prevent tiny constants with a denominator of zero, and , which are trainable scaling and translation parameters, are used to standardize the output features of the current layer, keeping the mean and variance of the features stable and accelerating the training convergence speed of the network; the ReLU activation function enhances the network's ability to fit complex nonlinear features by introducing a nonlinear mapping relationship, while suppressing the gradient vanishing problem and ensuring effective training of deep networks.

[0068] The residual path and the main path are set up in parallel. Their core function is to directly pass input features to the output of the main path through residual connections, achieving feature leap propagation and mitigating gradient decay and feature degradation caused by deep stacking. The core fusion formula is: ,in, These are the characteristics after residual fusion. The output features of the main path The output features of the residual path are then expressed using the formula. Calculate the number of input feature channels Cin and the number of output feature channels Cout of the main path respectively, and make a real-time determination; when At that time, the residual path directly transmits the input features as is, that is... Substituting the values ​​into the fusion formula and adding them element-wise with the output features of the main path ensures that the residual connections do not change the dimension and distribution of the features. At that time, due to the increasing number of filters in the main path layer by layer, the number of output channels increases. The residual path is set to an adaptation structure containing a one-dimensional convolutional layer and layer normalization. The kernel size of the one-dimensional convolutional layer is 1, and the stride is 1. Its core formula is: ,in, The weight matrix is ​​a 1×1 convolution with dimension . , As the bias vector, this formula is used to precisely align the number of channels of the input features, so that... Subsequently, the channel-aligned features are standardized using a layer normalization operation, with the formula being the same as the layer normalization for the main path. This ensures the numerical stability of the residual path output features and avoids introducing additional noise due to channel conversion. Substitute the values ​​into the fusion formula and add them element by element to the output of the main path to ensure the effectiveness of the residual connections and the rationality of feature fusion.

[0069] During feature extraction, the historical observation sequences of each modality obtained after preprocessing in step S1 are... (i=1,…,5, corresponding to 5 modalities) are input into the corresponding convolutional neural network branches. Through layer-by-layer processing of multi-layer one-dimensional convolutions, layer normalization, and ReLU activation functions in the backbone path, local abstract features of each modality are extracted. Simultaneously, after dynamic adaptation processing by the residual path, the input features are element-wise added to the local abstract features output from the backbone path, and then non-linearly transformed using the ReLU activation function, finally outputting the local temporal features of each modality. Its mathematical expression is:

[0070] ,in, For the local temporal features of the i-th mode, This is a mapping function for the backbone path in a convolutional neural network, representing a series of processing steps performed by the backbone path on the input features. It is the mapping function of the residual path in the branch of the convolutional neural network, which represents the dynamic adaptation and transmission process of the residual path to the input features.

[0071] S3. Perform hierarchical feature fusion based on the local temporal features of each modality, including constructing a three-level architecture consisting of independent modeling within modality, cross-modal gating interaction, and global temporal consistency optimization, and outputting global fused features through the three-level architecture;

[0072] like Figure 2 As shown, the local temporal features of each modality output in step S2 are... Independent linear projection layers are input separately and mapped to feature dimensions suitable for Mamba module modeling through linear transformation. An independent Mamba module is configured for each modality to avoid temporal interference between modalities. Each Mamba module adopts a residual block stacking structure, and each residual block contains a pre-normalization layer, a Mamba layer, and residual connections in sequence. The pre-normalization layer is defined by the formula... The input features are standardized to stabilize the training process. When modeling long-term temporal dependencies of each modality using the Mamba module, the Mamba layer first expands the input dimension through a linear layer. ,in, The modal features are adapted to different dimensions and divided into two parallel branches. The main branch is processed by one-dimensional depthwise convolution and SiLU activation function, and then... (The sentence is incomplete and requires more context to translate accurately.) The core parameters of the state space are generated dynamically, and the formulas are as follows:

[0073] ,in, It is a linear layer with bias. and For a linear layer without bias, The output state evolution step size parameter is used to dynamically adjust the update rate of the state space. Its non-negativity ensures the stability of the state evolution. The input projection parameters are used to map the input features at the current time step to the state space dimension. The output parameters are the state parameters used to map the updated temporal state back to the feature space. The Softplus function ensures... Non-negativity satisfies the state update constraint; the state matrix A is obtained through a trainable logarithmic parameter matrix. Parameterization, the formula is , Initialize to The repeating vector, where N is the state dimension, and matrix D is initialized as an all-1 vector to provide the underlying bias.

[0074] Since the state-space parameters are constructed based on the continuous-time system, they need to be discretized using the zero-order hold (ZOH) method to adapt to discrete-time data. The discretization formula is as follows: , Where I is the identity matrix; subsequently, a selective scanning mechanism is used to recursively update the temporal state, and the state update formula is: ,in, Let this be the current state vector. The state vector from the previous time step is ultimately expressed by the formula. Output the enhancement features of each mode. ,in, For input The gated vector, after independent linear projection, is used to dynamically adjust the output feature weights, highlighting effective temporal information. The input bias parameters are initialized as a vector of all 1s, and are trainable parameters. For status output parameters, It is the Sigmoid linear unit activation function.

[0075] Next, the second-level cross-modal gating interaction is executed to achieve alignment and complementarity of heterogeneous modal features. First, the enhanced features of the five modalities are concatenated along the channel dimension to obtain the initial cross-modal fusion features. Its dimensions are , Given the time series length, feature information from all modes is integrated. To suppress noise propagation from unreliable modes, a gating mechanism is introduced for adaptive filtering. The core formula is: ,in, For cross-modal features after gating, This refers to the initial fused features after splicing the enhancement features from each modality. The Sigmoid activation function maps the gate weights to the [0,1] interval. For the gated weight matrix, This is the gated bias vector. To achieve element-wise multiplication, this formula dynamically assigns different weights to the features of each channel, thereby suppressing channels with high noise levels.

[0076] Subsequently, the selected... Perform a linear mapping to generate the query (q), key (k), and value (v) vectors required by the self-attention mechanism. The mapping formulas are as follows:

[0077] ,in, , , The weight matrix is ​​randomly initialized, with dimensions adapted to the number of attention heads and the dimension of the key vectors. Cross-modal association weights are obtained by calculating the similarity between vectors. Based on these association weights, the value vectors are weighted and fused to capture the non-linear correlations between different modalities, achieving implicit feature alignment. Finally, the attention output is compared with... Perform residual joins and layer normalization, using the following formula:

[0078] ,

[0079] in, Regularization operations are used to suppress overfitting. For cross-modal fusion features, For layer normalization operation, The cross-modal features after gating are the initial fusion features after adaptive weighting by the Sigmoid gating mechanism.

[0080] Finally, a third-level global temporal consistency optimization is performed to improve the overall temporal stability of the fused features. This involves fusing cross-modal features. The input is the global Mamba module, whose structure is consistent with the intramodal Mamba module. Both adopt a stacked structure of pre-normalized layers, Mamba layers, and residual blocks with residual connections. By adjusting the state dimension and regularization parameters, the global temporal modeling capability is enhanced. The state-space modeling and selective scan mechanism of the global Mamba module are utilized to... A unified modeling process covering the entire time series is implemented, including parameter generation, discretization, and state update of Mamba modules within repetitive modes. Temporal fluctuations in cross-modal fusion features are smoothed to enhance the consistency and stability of range-level trajectories. Finally, global fusion features are output via formulas. :

[0081] ,

[0082] in, For the final global fusion features, Features of cross-modal gated interactive output, These are mapping functions for the global Mamba module. For layer normalization operations, the original cross-modal feature information is preserved through residual connections, and the feature distribution is standardized by layer normalization to ensure that the output global fusion features contain both effective information of each modality and cross-modal correlation information.

[0083] S4. Input the global fusion features into the dual-branch regression head, decode them through the position regression branch and the pose regression branch respectively, and output the 3D position increment and pose increment.

[0084] Global fusion features Synchronize the input position regression branch and pose regression branch. Including multimodal effective information, cross-modal correlation information, and global temporal consistency features, it provides a high-quality feature foundation for pose increment estimation. For the position regression branch, it first passes through the first-level linear layer. The core calculation formula for the linear layer is as follows: Dimensional transformation and depth mapping are performed on the globally fused features.

[0085] ,in, for The weight matrix, As the bias vector, this linear transformation maps the globally fused features to an intermediate feature dimension that adapts to nonlinear fitting, achieving further abstraction and integration of feature information. Subsequently, Inputting the ReLU activation function enhances the network's ability to fit complex pose changes while suppressing the gradient vanishing problem, ensuring the effective transfer of deep features.

[0086] After ReLU activation, a Dropout layer is applied for regularization. The Dropout layer randomly discards some feature channels during training (the discard probability is a preset hyperparameter), as expressed in the formula:

[0087] ,in, The features are ReLU activated, and p is the discard probability. This avoids the model's over-reliance on specific feature channels and improves the model's generalization ability in complex marine environments. Finally, a second-level linear layer is applied. The core formula for performing dimensionality compression and target mapping on the features after Dropout processing is as follows: ,in, for The weight matrix, As a bias vector, the intermediate feature dimension is mapped to the target dimension of the 3D position increment. The output dimension is 3, corresponding to the position increments in the x, y, and z directions, respectively. Finally, the decoded output is the 3D position increment. .

[0088] The complete regression formula for the three-dimensional position increment is:

[0089] ,

[0090] in, The three-dimensional position increment is a vector containing displacement changes in the x, y, and z directions, which is directly used for subsequent recursive calculations of the AUV's real-time pose. and These are the first and second linear layers in the location regression branch, both of which are biased linear transformation modules. The former is responsible for feature dimension expansion and abstraction, while the latter is responsible for target dimension mapping and decoding. Dropout is a regularization operation that suppresses overfitting by randomly discarding feature channels, and its dropout probability is an adjustable hyperparameter. ReLU is a non-linear activation function used to enhance the network's ability to fit complex features.

[0091] The pose regression branch adopts the same network structure logic as the location regression branch, configuring only independent trainable parameters to ensure training fairness and task adaptability between the two branches. The specific process is as follows: globally fused features... The first linear layer of the input pose regression branch Through formula ,in, for The weight matrix, The bias vector is used to achieve feature dimension transformation and abstraction; then, non-linearity is introduced through the ReLU activation function, and regularization is performed through a Dropout layer (with the dropout probability consistent with the position regression branch), resulting in... Finally, through the second-level linear layer Perform target dimension mapping, mapping intermediate features to the target dimension of the 3D pose increment, and decode to output the 3D pose increment. .

[0092] The complete formula for attitude regression is:

[0093] ,

[0094] Its parameter logic is consistent with the location regression formula, only the task objective is different: The output three-dimensional attitude increment is a vector containing changes in roll angle, pitch angle, and yaw angle; and The two-level linear layers, which are branches for pose regression, are adapted to the feature decoding requirements of pose estimation. Through parallel decoding of the two branches, the synchronous conversion of global fused features into 3D position increments and pose increments is realized.

[0095] S5. Construct a loss function using the 3D position increment and pose increment, train the optimal model and deploy it to the AUV platform, and use the pose increment output by the optimal model in real time to obtain the real-time 3D pose of the AUV.

[0096] First, sample labels are acquired and processed. AUV reference pose data provided by a high-precision reference system (such as a lab-developed high-precision SLAM system) is obtained. This data serves as the sample labels for loss calculation during model training, ensuring the accuracy and reliability of the labels. A time synchronization method consistent with that used for the multimodal sensor data in step S1 is employed to perform time alignment processing on the reference pose data, ensuring a one-to-one correspondence between the label data and the model input data in the time dimension. Then, according to the step size of the sliding window in step S1, incremental calculations are performed on the aligned reference pose data to obtain the label values ​​corresponding to the 3D position and pose increments output by the model. and .

[0097] Next, a loss function is constructed to optimize the model parameters. First, the Huber loss function is used to calculate the single-task loss for both the position regression and pose regression tasks. The core advantage of the Huber loss function is its strong robustness to outliers. Its calculation formula is as follows:

[0098] ,

[0099] in, This represents Huber's loss value. This refers to the 3D position increment or attitude increment output by the model, specifically representing the 3D position increment for a position regression task or the 3D attitude increment for a attitude regression task. The threshold parameter of the loss function is the value of the absolute value of the prediction error. Less than or equal to When the mean square error is used to calculate the loss, it is suitable for fine-grained optimization when the error is small. When the absolute value of the prediction error is greater than 100%, the loss is calculated using the mean square error method. At the same time, linear error is used to calculate the loss to avoid excessive loss due to outliers affecting the stability of model training. For the corresponding tag value, i.e. or .

[0100] To achieve a dynamic balance between position regression and pose regression tasks, an adaptive weighting strategy based on task uncertainty is introduced. This strategy dynamically adjusts the weight of single-task loss in the total loss by having the model autonomously learn the noise variance of the two tasks, thus avoiding the dominance of a single task in the training process. Based on the single-task Huber loss and adaptive weights, the total loss function for model training is constructed, as follows:

[0101] ,

[0102] in, and These represent the noise variances for the position regression task and the pose regression task, respectively. and The Huber loss is calculated for position regression and pose regression, respectively. and These are the 3D position increment and attitude increment output by the model, respectively. and These are the corresponding label values. , This is a regularization term used to prevent excessive noise variance in one task from causing training imbalance and to ensure the stability of dual-task training.

[0103] The model is then trained to obtain the optimal parameters using an adaptive optimizer, specifically the Adam optimizer in this embodiment. The total loss function L is iteratively optimized using gradient descent. This optimizer accelerates model convergence by adaptively adjusting the learning rate of each parameter, while avoiding getting trapped in local optima. During training, the gradient of the total loss relative to all trainable parameters of the model is calculated using the gradient backpropagation algorithm. The parameters are updated according to the gradient direction to continuously reduce the total loss value.

[0104] A learning rate scheduling strategy is employed to dynamically adjust the learning rate during training. The initial learning rate is set to the initial value suitable for model training, and it is gradually reduced as the training epochs increase until the minimum learning rate is reached, achieving fine-grained optimization of the model. Simultaneously, an early stopping mechanism is introduced, setting a validation set loss monitoring metric. When the validation set loss decreases by less than a set threshold within a preset number of consecutive epochs, training is immediately terminated to prevent overfitting and ensure the model's generalization ability. Through this training process, the model with optimal generalization ability is finally obtained, namely the optimal AUV 3D pose joint estimation model based on a multimodal hierarchical Mamba architecture.

[0105] After model training, the optimal model is deployed to the AUV's onboard embedded computing platform or host computer system. During deployment, it is ensured that the model adapts to the AUV's hardware resource constraints, guaranteeing the real-time performance and stability of model inference. During actual underwater navigation of the AUV, the real-time acquired multimodal sensor data undergoes time synchronization, noise filtering, sliding window partitioning, and normalization processing according to the preprocessing procedure in step S1, resulting in a structured real-time observation sequence. This sequence is then sequentially input into the deployed optimal model, and after multi-branch convolutional feature extraction, hierarchical Mamba feature fusion, and bi-branch regression decoding, the current three-dimensional position increment is output in real time. and attitude increment Set the initial three-dimensional position of the AUV. and initial posture Based on task requirements, the origin coordinates and zero attitude are set, i.e. Based on a recursive calculation method, the pose increment at the current moment is fused with the pose at the previous moment to obtain the current real-time 3D pose of the AUV. The recursive formula is as follows: ,in, These represent the current three-dimensional position and orientation, respectively. , These are the three-dimensional position and attitude of the previous moment, respectively. Through this recursive process, the real-time three-dimensional pose of the AUV can be continuously output throughout the entire flight, meeting the autonomous navigation and positioning requirements under long-endurance and complex maneuvering conditions.

[0106] Example 2: The difference between this example and Example 1 is that this example provides a specific experimental method for a joint estimation method of AUV three-dimensional pose based on multimodal hierarchical structure;

[0107] To verify the performance of a joint AUV 3D pose estimation method based on a multimodal hierarchical Mamba architecture, we conducted tests on a self-collected indoor pool dataset and a seaside pool dataset. The experiments tested data-driven deep learning algorithms and traditional filtering algorithms, including the state-of-the-art end-to-end localization method NavNet, the IMU-based pure inertial odometry method RNIN, and the Extended Kalman Filter (EKF), serving as a baseline for performance evaluation of the proposed multimodal hierarchical Mamba architecture-based joint AUV 3D pose estimation method.

[0108] Table 1. Evaluation results of each localization algorithm on a certain trajectory in the indoor pool dataset.

[0109]

[0110] like Figure 4As shown, the indoor pool dataset was collected in an ideal environment with high underwater visibility and still water, minimizing disturbance to the AUV. Table 1 shows the evaluation results of each localization algorithm on a specific trajectory within the indoor pool dataset, and a comparison of the localization trajectories is provided. Figure 4 Because the AUV rotates approximately 270 degrees at the beginning of the trajectory, and RNIN is based solely on IMU data without DVL data for correction, the overall trajectory deviates significantly after the sharp rotation. While NavNet and EKF can obtain a general trajectory, they suffer from scale limitations. The method proposed in this invention achieves the highest positioning accuracy among all compared algorithms, maintaining high heading accuracy even after multiple rotations.

[0111] A comparison of the z-axis estimation results of various localization algorithms on a certain trajectory in the indoor pool dataset is shown below. Figure 2 Existing deep learning methods generally lack effective estimation of z-axis displacement. RNIN only focuses on horizontal displacement; if IMU is used to estimate the z-axis, it is easily affected by accumulated errors, leading to severe drift in the results. Thanks to depthmeter data, NavNet, EKF, and the method proposed in this invention can all obtain relatively accurate z-axis estimation results. Among them, NavNet directly uses depthmeter data as the z-axis result, but its frequency is low, its accuracy is limited, and the result has obvious oscillations. Although EKF can estimate the z-axis relatively stably, it is affected by noise interference, resulting in spikes in the output. The method proposed in this invention has higher accuracy in z-axis estimation and a smoother trajectory.

[0112] Table 2 Evaluation results of each localization algorithm on a certain trajectory in the seaside pool dataset;

[0113]

[0114] like Figure 5 , Figure 6 As shown, the seaside pool dataset was collected in real sea area. The AUV's movement is affected by waves, resulting in greater noise in the collected data. Table 2 shows the evaluation results of various localization algorithms on a certain trajectory segment in the seaside pool dataset. A comparison of localization trajectories is also shown. Figure 6 All localization algorithms can obtain a complete trajectory, but the absolute trajectory error gradually increases over time. The method proposed in this invention exhibits a slower increase in absolute trajectory error and has a stronger ability to suppress error accumulation compared to other methods.

[0115] Example 3: A multimodal hierarchical AUV 3D pose joint estimation system, comprising:

[0116] The data acquisition module is configured to acquire multimodal sensor data from the AUV and preprocess the multimodal sensor data to form a structured historical observation sequence.

[0117] The feature extraction module is configured to: assign historical observation sequences to corresponding independent convolutional neural network branches according to modality, extract local temporal features through each branch, and output the local temporal features of each modality;

[0118] The feature fusion module is configured to perform hierarchical feature fusion based on the local temporal features of each modality, including constructing a three-level architecture consisting of independent modeling within modality, cross-modal gating interaction, and global temporal consistency optimization, and outputting global fused features through the three-level architecture;

[0119] The joint regression module is configured to: input global fused features into the dual-branch regression head, decode them through the position regression branch and the pose regression branch respectively, and output the 3D position increment and pose increment;

[0120] The deployment module is configured to: construct a loss function using 3D position increments and pose increments, train the optimal model and deploy it to the AUV platform, and obtain the real-time 3D pose of the AUV using the pose increments output in real time by the optimal model.

[0121] 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. A joint estimation method for three-dimensional pose of an AUV based on multimodal hierarchical structure, characterized in that, include: Acquire multimodal sensor data from an AUV and preprocess the multimodal sensor data to form a structured historical observation sequence. The multimodal sensor data includes motion acceleration, rotational angular velocity, attitude state, movement speed, and vertical depth. Historical observation sequences are assigned to corresponding independent convolutional neural network branches according to modality, and local temporal features are extracted through each branch to output the local temporal features of each modality; Based on the local temporal features of each modality, hierarchical feature fusion is performed, including the construction of a three-level architecture consisting of independent modeling within modality, cross-modal gating interaction, and global temporal consistency optimization. The global fused features are output through the three-level architecture. The intramodal independent modeling includes projecting the local temporal features of each modality into dimensions through linear layers to obtain feature vectors adapted for intramodal modeling. An independent Mamba module is configured for each modality. The Mamba module adopts a residual block stacking structure, and a pre-normalization layer, a Mamba layer, and residual connections are set sequentially inside the residual block. When the long temporal dependencies of each modality are independently modeled by the Mamba module, the Mamba layer first expands the input dimension through linear layers, and then dynamically generates state space parameters by combining depthwise convolution and activation functions. After discretizing the continuous state parameters using zero-order preservation, a selective scanning mechanism is used to realize the recursive update of the temporal state, and finally the enhanced features of each modality are output. The cross-modal gating interaction concatenates the enhanced features of each modality along the channel dimension to obtain initial cross-modal fusion features. A gating mechanism is introduced to adaptively filter the initial cross-modal fusion features. A gating weight vector is generated through an activation function. The initial fusion features are weighted element-wise. The filtered features are linearly mapped to generate the query, key, and value vectors required by the self-attention mechanism. Cross-modal association weights are obtained by calculating the similarity between vectors. The value vectors are weighted and fused based on the association weights. Finally, the cross-modal fusion features are output through residual connections and layer normalization. The global temporal consistency optimization includes inputting cross-modal fusion features into a global Mamba module. The structure of the global Mamba module is consistent with that of the intra-modal Mamba module. By adjusting the state dimension and regularization parameters, the global Mamba module's state space modeling and selective scanning mechanism are used to perform unified modeling of the cross-modal fusion features across the entire temporal range, perform global optimization of the cross-modal fusion features, and output the final global fusion features. The global fusion features are input into the dual-branch regression head, and decoded through the position regression branch and the pose regression branch respectively, outputting the 3D position increment and pose increment; A loss function is constructed using 3D position increments and pose increments. The optimal model is trained and deployed to the AUV platform. The real-time 3D pose of the AUV is obtained by using the pose increments output in real time by the optimal model.

2. The method for joint estimation of AUV three-dimensional pose based on multimodal hierarchical structure according to claim 1, characterized in that, The preprocessing of multimodal sensor data includes defining the time axis as the alignment reference, using linear interpolation to align the multimodal sensor data in time, unifying the sampling frequency of all modal sensor data, using adaptive noise threshold filtering to remove interference data, dividing the interference-free multimodal sensor data into several time segments of consistent length using a fixed-length sliding window strategy, calculating the mean and standard deviation of each type of modal data, and performing standardization and normalization transformation on each modal data.

3. The AUV 3D pose joint estimation method based on multimodal hierarchical structure according to claim 1, characterized in that, The output local temporal features of each modality include independent convolutional neural network branches constructed based on modality categories from historical observation sequences. Each branch employs a residual learning framework consisting of a backbone path and residual paths. The backbone path contains multiple cascaded one-dimensional convolutional layers, each followed by layer normalization and ReLU activation functions. The feature dimension is progressively expanded by increasing the number of filters layer by layer. The residual paths dynamically adapt to the difference in the number of channels between the input and output of the backbone path, ensuring a consistent number of channels. After dynamic adaptation, the input features are passed to the output to obtain the local temporal features of each modality. The expression for the local temporal features is: ， in, For the local temporal features of the i-th mode, This is the mapping function for the main path of the convolution branch. This is the residual path mapping function for the convolution branch. This is the preprocessed historical observation sequence for the i-th mode.

4. The method for joint estimation of AUV three-dimensional pose based on multimodal hierarchical structure according to claim 1, characterized in that, The calculation formula for the adaptive filtering is: ， in, For cross-modal features after gating, This refers to the initial fused features after splicing the enhancement features from each modality. It is the Sigmoid activation function. For the gated weight matrix, This is the gated bias vector. This is for element-wise multiplication.

5. The AUV 3D pose joint estimation method based on multimodal hierarchical structure according to claim 1, characterized in that, The output formula for the global fusion feature is: ， in, For the final global fusion features, Features of cross-modal gated interactive output, These are mapping functions for the global Mamba module. This is a layer normalization operation.

6. The method for joint estimation of AUV three-dimensional pose based on multimodal hierarchical structure according to claim 5, characterized in that, The decoding process, involving separate position and pose regression branches, includes constructing a dual-branch regression head containing both branches. Each regression branch consists of a linear layer, a ReLU activation function, and a Dropout layer sequentially connected in series. The linear layer performs feature dimension transformation and mapping, simultaneously inputting the global fused features into both regression branches. The position regression branch performs layer-by-layer feature transformation and non-linear mapping on the global fused features, then decodes and outputs the 3D position increment. The pose regression branch, using the same network structure logic, decodes and outputs the 3D pose increment. The regression formula for the 3D position increment is: ， in, For three-dimensional position increment, and These are the two linear layers in the location regression branch. For regularization operations, This is the activation function.

7. The AUV 3D pose joint estimation method based on multimodal hierarchical structure according to claim 1, characterized in that, The method of constructing a loss function using 3D position and pose increments includes acquiring reference pose data as sample labels, performing incremental calculations on the sample labels to obtain corresponding label values, and calculating the corresponding single-task loss using the Huber loss function for both position regression and pose regression tasks. An adaptive weighting strategy based on task uncertainty is introduced, dynamically adjusting the weight ratio of the two single-task losses in the total loss based on the noise variance of the position regression and pose regression tasks. The Huber loss function calculation formula is as follows: ， in, This represents Huber's loss value. This refers to the 3D position or attitude increment output by the model. For the corresponding tag value, This is the threshold parameter of the loss function.

8. The AUV 3D pose joint estimation method based on multimodal hierarchical structure according to claim 7, characterized in that, The method of constructing a loss function using 3D position and pose increments also includes constructing a total loss function for model training based on single-task Huber loss and task uncertainty weights. This involves weighting and summing the single-task losses using adaptive weights, adding a regularization term to avoid training imbalance, and using an adaptive optimizer to iteratively optimize the total loss function using gradient descent. All trainable parameters of the model are updated through gradient backpropagation. An early stopping mechanism is also introduced, terminating training when the validation set loss decreases below a set threshold. The expression for the total loss function is: ， in, and These represent the noise variances for the position regression task and the pose regression task, respectively. and These are the Huber losses for position regression and pose regression, respectively. and These are the 3D position increment and attitude increment output by the model, respectively. and These are the corresponding tag values.

9. A multimodal hierarchical AUV three-dimensional pose joint estimation system, executed according to claim 1, characterized in that, include: The data acquisition module is configured to acquire multimodal sensor data from the AUV and preprocess the multimodal sensor data to form a structured historical observation sequence. The feature extraction module is configured to: assign historical observation sequences to corresponding independent convolutional neural network branches according to modality, extract local temporal features through each branch, and output local temporal features of each modality; The feature fusion module is configured to perform hierarchical feature fusion based on the local temporal features of each modality, including constructing a three-level architecture consisting of independent modeling within modality, cross-modal gating interaction, and global temporal consistency optimization, and outputting global fused features through the three-level architecture; The joint regression module is configured to: input global fused features into the dual-branch regression head, decode them through the position regression branch and the pose regression branch respectively, and output the 3D position increment and pose increment; The deployment module is configured to: construct a loss function using 3D position increments and pose increments, train the optimal model and deploy it to the AUV platform, and obtain the real-time 3D pose of the AUV using the pose increments output in real time by the optimal model.

Images