inertial radar, inertial, vision fusion navigation method

Abstract

The application relates to a radar, inertial and visual fusion navigation method of an inertial attention mechanism, comprising collecting and processing visual / inertial / laser radar navigation data, decomposing time / space features of double-frame data of images and radars by using a separated space-time attention mechanism, and then extracting inertial guided motion information features; by means of an inertial guided attention strategy network, key weights are generated by inertial features through a long short-term memory network hidden state, interaction is carried out under a multi-modal attention framework, binary decision values are generated by a Gumbel-Sigmoid method; according to the decision result, selective masking is carried out on visual and laser radar features, and after fusion with inertial features, the features are input into an LSTM pose estimation network, and high-precision six-degree-of-freedom pose is output. By selectively extracting visual and radar information, the application realizes robust, energy-saving and interpretable modal selection and accurate pose estimation.

Description

TECHNICAL FIELD

[0001] The present application relates to a multi-source information fusion navigation method, in particular to an inertial attention mechanism guided radar, inertial, vision fusion navigation method, belonging to the technical field of computer vision, robot navigation and automatic driving. BACKGROUND

[0002] In mobile robots, unmanned vehicles and aircraft platforms, vision (camera), inertial (IMU) and laser radar (LiDAR) are often used together for pose estimation. Different modalities perform differently in different environments: vision performance decreases in low light, rain, and fog conditions; LiDAR may also be limited in near-obstacle / raindrop reflection or sparse echo scenes; IMU can provide short-time motion information but cumulative error increases with time. Traditional methods mostly use fixed or simple rule fusion, which cannot intelligently select sensors in short-time dynamic scenes, resulting in performance degradation or resource waste. Therefore, a strategy network capable of adaptively selecting modalities in time and space and having an interpretable attention mechanism is needed to guide modality use, thereby improving robustness and saving computing resources. SUMMARY

[0003] The present application solves the problem: overcome the shortcomings of the prior art, provide an inertial attention mechanism guided radar, inertial, vision fusion navigation method and device, through the inertial attention mechanism guided strategy network for dynamic modality selection and multi-modal interaction, realize high-precision, high-robustness pose estimation in complex environment.

[0004] The technical solution of the present application: an inertial attention mechanism guided radar, inertial, vision fusion navigation method and device, characterized by comprising the following steps:

[0005] First, collect and process the navigation data of vision / inertial / laser radar, use the separated space-time attention mechanism to perform time / space feature decomposition on the dual-frame data of image and radar, and then extract the inertial-guided motion information features;

[0006] Second, through the inertial attention strategy network, generate key weights from the hidden state of the long short-term memory network (LSTM) with inertial features, interact in the multi-modal attention framework, and generate binary decision values through the Gumbel-Sigmoid method;

[0007] Third, according to the decision result, selectively mask the vision and laser radar features, input the fused inertial features into the LSTM pose estimation network, and output the high-precision six-degree-of-freedom pose.

[0008] In the first step, navigation data of vision / inertial / laser radar is collected and processed, time / space feature decomposition is performed on double-frame data of image and radar by using a separate space-time attention mechanism, and then inertial guided motion information features are extracted, and the implementation steps are as follows:

[0009] (a1) The collected image data is processed and a sample sequence is generated, adjacent two frames of images are stacked along the channel dimension, channel separation and patch block processing are performed by a double-frame embedding layer, and a single-frame patch number is obtained by formula Patchsize is the patch size, and are the height and width of the image respectively. Then the dimension is reshaped and global features (CLS Token) and position / time embedding are added, and then the space-time features are extracted by a multi-layer separate space-time attention module to output the CLS Token feature corresponding to each time step;

[0010] (a2) The collected 3D laser radar point cloud data is projected into a two-dimensional image and a sample sequence is generated, adjacent two frames of LiDAR images are stacked, and when the double-frame embedding layer of laser radar is used, padding calculation is first performed by formula and to ensure that the size is divisible, wherein , is the patch height and width, and are the height and width of the laser radar two-dimensional image respectively, then the dimension is reshaped and global features CLS Token and position / time embedding are added, and then the space-time features are extracted by a multi-layer separate space-time attention module to output the CLS Token feature of each time step;

[0011] (a3) The collected IMU data is initially processed and a sample sequence is generated, the IMU original sequence is cut according to a fixed window length, a one-dimensional CNN encoder is input, and a projection is performed through a full connection, and the output is an IMU feature ;

[0012] (a4) The time attention of the above-mentioned separate space-time attention module first extracts features by time attention on the front and back frames at the same spatial position, and then uses spatial attention to perceive spatial features of each frame patch, so as to obtain overall features with time sequence correlation and spatial context awareness;

[0013] (a5) After the CLS Token of each time step is obtained by the image and radar, the multi-layer attention temporal encoder is used for temporal coding, and the data is processed in two levels of temporal coding. The first step separates the spatial attention module in the local (patch level) to make cross-frame attention, and then the second level of temporal coding network (TemporalTransformer) makes cross-time coding on the CLS Token (overall representation) of each time step to process the dependence between T steps, and finally obtains the visual features and lidar features .

[0014] In the second step, the inertia guided attention strategy network is used to generate key weights through the hidden state of the long short-term memory network (LSTM) with inertia features, interact in the multi-modal attention framework, and generate binary decision values through the Gumbel-Sigmoid method, and the implementation steps are as follows:

[0015] (b1) The strategy network (TransformerPolicyNet) uses the IMU features , visual features , lidar features and the hidden state of the LSTM at the last time to perform multi-modal attention calculation. The strategy network uses a dynamic weight generation mechanism, and the dynamic weight is dynamically generated according to the real-time motion state (IMU) and the historical context (LSTM state);

[0016] (b2) In the process of multi-modal attention calculation of the strategy network, the query vector and the key vector K are modulated by element-wise multiplication from to generate a feature sequence [ ]; The value vectors of the visual / lidar features are generated by respective linear transformations, i.e. , , , , where and are linear transformation matrices of the image / lidar ;

[0017] (b3) The modulated Q, K, and V are reshaped in the feature dimension, and the multi-modal attention matrix M is calculated through the attention formula. This matrix quantifies the intra- and inter-modal correlation of visual and lidar modalities, and the value vectors are weighted and fused by means of attention weight (M·V). Finally, the visual and lidar features with attention correlation and enhanced are outputted;

[0018] (b4) The attention enhanced output of each modality is respectively connected with residual connection and layer normalization (LayerNorm), fed into a feed-forward network (FFN) and a decision head network to obtain the decision logic value of each modality , and the visual modality selection decision is obtained by Gumbel-Sigmoid sampling and the lidar modality selection decision , wherein the binary decision , ∈{0,1}, the soft sample of Gumbel-Sigmoid is , if a hard decision is used, then , wherein is the decision logic value, is the Gumbel noise, is the temperature parameter, is the Sigmoid function.

[0019] In the third step, according to the decision result, the visual and lidar features are selectively masked, and the inertial features are fused to input the LSTM pose estimation network, and a high-precision six-degree-of-freedom pose is output to realize the following steps:

[0020] (c1) Gumbel-Sigmoid function combined with straight-through estimator is used to realize hard decision sampling, while maintaining the differentiability of gradient during training. The temperature parameter is set, and annealing scheduling is performed during training. A higher temperature value is used to encourage exploratory decisions at the beginning of training, and the temperature is gradually reduced to tend to a deterministic decision as the training progresses;

[0021] (c2) The visual decision mask and the lidar decision mask are respectively multiplied with the corresponding modality features to realize the selective use of features, i.e. , ;

[0022] (c3) At the first time step (i=0) of the sequence, it is forced that = =1, i.e. all modality features are used for initial pose estimation to ensure the correct initialization of the LSTM hidden state, facilitate the initial integrity of the policy network, and generate and utilize the LSTM hidden state at the next time step to provide reliable time sequence context information for subsequent policy decisions;

[0023] (c4) the pose estimation network adopts a three-layer long short-term memory network (LSTM) as a core time sequence modeling component, followed by a regressor network, which is a linear network composed of two fully connected layers;

[0024] (c5) the weighted visual features, lidar features and IMU features are spliced in the feature dimension to form a fusion vector , which is input into an LSTM pose estimator to output a high-precision 6-DOF pose incremental estimate at the current time step, and the hidden state of the LSTM is updated for the decision-making process at the next time step.

[0025] The application also discloses a radar, inertial and visual fusion navigation device with an inertial attention guiding mechanism, and has the characteristics that the device executes the above method.

[0026] The application also discloses a non-volatile storage medium, which has the characteristics that the non-volatile storage medium comprises a stored program, wherein the program controls a device in which the non-volatile storage medium is located to execute the above method when running.

[0027] The application also discloses a robot applying the above method to a dynamic environment and capable of realizing robust, high-precision and energy-saving pose estimation, and has the characteristics that the robot comprises a processor, a memory, a communication interface and a bus; the processor, the memory and the communication interface are connected through the bus and complete communication among each other; the memory stores executable program code; and the processor runs a program corresponding to the executable program code by reading the executable program code stored in the memory.

[0028] Compared with the prior art, the application has the following advantages:

[0029] (1) The application can extract image and lidar features through dual-frame input and decomposed spatial-temporal attention, and further adopt a Transformer for secondary time sequence modeling, so that short-time and long-time sequence information can be fully utilized, thereby improving the time sequence expression capability of modal features.

[0030] (2) The application takes inertial attention guiding as the core driving force, realizes dynamic modal selection in a multi-modal attention framework, and generates a differentiable binary decision through Gumbel-Sigmoid, so that the accuracy and interpretability of modal selection are effectively improved.

[0031] (3) The advantages of the present invention are due to the fact that it takes inertial guidance attention as the decision core, modulates multimodal attention interaction through IMU real-time motion information and LSTM hidden state, so that the policy network can adaptively select the most effective modal information based on the motion state of the environment, thereby enabling the system to significantly improve the pose estimation accuracy and robustness in complex environments. Attached Figure Description

[0032] Figure 1 This is an overall flowchart of a radar, inertial, and visual fusion navigation method based on an inertial guidance attention mechanism according to the present invention.

[0033] Figure 2 This is a schematic diagram of the radar, inertial, and visual fusion strategy network structure of an inertial guidance attention mechanism according to the present invention.

[0034] Figure 3 This is a comparison of the pose estimation accuracy of the method of the present invention on a publicly available test dataset. Detailed Implementation

[0035] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0036] like Figure 1 As shown in Figure 2, this invention discloses a radar, inertial, and visual fusion navigation method and device based on an inertial-guided attention mechanism. This method first constructs a robust multimodal spatiotemporal representation through dual-frame spatiotemporal attention and secondary temporal feature extraction. Then, it utilizes an inertial-guided dynamic modality selection mechanism to achieve adaptive sensor fusion, thereby achieving accurate and reliable pose estimation in complex environments. The specific steps of this method are as follows:

[0037] The first step is to collect and process navigation data from visual, inertial, and lidar systems. A spatiotemporal attention mechanism is used to decompose the temporal and spatial features of the two-frame data from the image and radar, thereby extracting motion information features for inertial guidance.

[0038] First, for a two-frame image, the two adjacent frames are stacked along the channel dimension to form a shape of [ ,T,6, , ], where B is the training batch size, T is the number of time steps, and 6 is the total number of channels in the two-frame image. and These represent the height and width of the image, respectively. The dual-frame data is separated into channels and patch blocks using a DualFramePatchEmbed layer. The total number of patches per frame is:

[0039] (1)

[0040] where, and are the height and width of the image, respectively. The size is then reshaped to [B*T, , embed_dim_image], where embed_dim_image is the dimension of each patch mapping. The patch sequence length for a dual frame is (two frames concatenated).

[0041] For LiDAR data, the 3D point cloud is first projected into a two-dimensional image , generating depth, intensity, and normal two-dimensional maps, where (x, y, z) and u, v represent 3D point cloud and 2D projected image coordinates, thus, represents the distance to the LiDAR. The 3D point cloud is projected into a 2D image according to the following equation:

[0042] (2)

[0043] where, and are the width and height of the 2D image. and represent the minimum and maximum values of the vertical field of view of the LiDAR sensor. In addition, represents and the sum of

[0044] For the normal, the nearest neighbor (pixel) in pixel units is found in the two-dimensional image, and the normal vector is calculated by taking the cross product of the x, y, z values between the pixel and the center pixel. To represent it in the form of an image, the x, y, and z values of the normal vector are matched with the r, g, and b values of the image, respectively:

[0045] (3)

[0046] where, and are the vectors connecting the center pixel and the nearest neighbor pixel. represents the angle between two vectors and . In addition, is the unit vector perpendicular to the plane containing the two vectors and .

[0047] The adjacent front and rear two frames of LiDAR data are stacked into [B, T, 10, , ], where the 10 channels correspond to the dual frame depth, intensity, and normal maps, and These are the height and width of the radar image, respectively. When using DualFramePatchEmbed on a LiDAR image, the LiDAR image is first processed... and Padding is calculated to obtain the values ​​at each scale. To ensure that the size is divisible:

[0048] (4)

[0049] in, , Here are the height and width of the patch. After padding, the number of patches per frame is:

[0050] (5)

[0051] Then reshape the size to [B*T, *2, embed_dim_lidar], where, The number of patches per frame after padding is given, and `embed_dim_lidar` is the laser patch embedding dimension. Then, Conv2d projection is used to obtain the embedding vector dimension of each patch: `embed_dim_image` (visual embedding dimension) or `embed_dim_lidar` (radar embedding dimension).

[0052] Next, a global table class token (CLS token) and location / time embedding are added to the data, forming a token sequence (with the CLS token added) at each single time step t. Time attention: For patch tokens with the CLS token removed, they are first rearranged according to spatial location as (B· , ,embed_dim_image) (in this single step) =2), performing temporal self-attention on consecutive frames at the same spatial location; spatial attention: reconstructing the patch sequence from the temporal attention output and copying / expanding the CLS token for each frame, then performing spatial attention (self-attention) on the spatial location within each frame to obtain a spatially enhanced representation and update the CLS. Cross-frame fusion at the patch level is achieved through multi-layer stacking. The focus of this stage is to capture the temporal consistency between consecutive frames and the spatial context within the frame at the local (patch level). Finally, CLS token features are preserved in a single time step. As a short-term overall representation of this time step, where M is the dimension of the extracted visual time step CLS Token.

[0053] The time attention module in the separation spatio-temporal attention processing of the above process is a first-level time sequence processing, and then a second-level time sequence coding is performed through a TemporalTransformer. The CLS Token vector set (length T) on each modality time sequence is respectively input into an independent TemporalTransformer module, which is composed of a multi-layer Transformer encoder, each layer containing a multi-head self-attention mechanism and a feedforward network, and realizing modeling of long sequence dependence through residual connection and layer normalization to stabilize the training process, and outputting a time sequence enhanced feature representation.

[0054] The TemporalTransformer performs second-level time sequence modeling on each time step CLS Token, which is used to capture long-range dependency relationships and time consistency between T steps, thereby supplementing the long time sequence information not covered by the first-level (local) spatio-temporal attention.

[0055] The IMU data is encoded by a one-dimensional convolutional encoder, and the IMU window corresponding to each time step is input into a Conv1d layer group, and then linearly projected to obtain IMU features where M is the length of the extracted IMU features. The feature dimensions of the three modalities are all M, and the unified feature dimension facilitates subsequent fusion.

[0056] In the second step, an inertia-guided attention strategy network is used to generate key weights from the inertia features through a long short-term memory (LSTM) hidden state, interact in a multi-modal attention framework, and generate binary decision values through a Gumbel-Sigmoid method.

[0057] Based on the IMU features and the pose estimator hidden state of the current time step t, the strategy network makes a binary decision on whether to use the visual and radar modalities, as shown in Figure 2 As shown, the TransformerPolicyNet strategy network receives the IMU features , visual features , lidar features , and the hidden state of the LSTM at the previous time The strategy network does not use fixed Q / K projection matrices, but dynamically generates weights to respond to the immediate motion situation and historical context. and are generated from the IMU features and the hidden state of the LSTM generated by their respective MLPs (IMU-to-Wq and hidden-to-Wk), where IMU-to-Wq and hidden-to-Wk are both composed of two linear layers, LayerNorm normalization layers, and ReLU activation functions, mapping IMU or hidden states to the same dimension as the modality features.

[0058] Then, multi-modal attention calculation is performed to splice the visual and lidar features into a dual-modal feature matrix and construct Query, Key, and Value:

[0059] (6)

[0060] wherein, , are two sets of linear projections (V calculation for vision and lidar, respectively). The modulated Q, K, and V are split and reshaped in the feature dimension to a dimension of [B, num_heads, 2, head_dim], where num_heads is the number of parallel calculation units, and head_dim is the split feature dimension, and the efficiency is improved through parallel calculation.

[0061] The modulated Q, K, and V are calculated through the following attention formula to obtain the multi-modal attention matrix M:

[0062] (7)

[0063] wherein, is the split feature dimension, and this matrix quantifies the correlation weight within and between modalities, and the weighted fusion of the value vector (M·V) through the attention weight obtains the enhanced attention visual and lidar features:

[0064] (8)

[0065] Then, the attention-enhanced output of each modality is respectively connected with a residual connection and LayerNorm, a feed-forward network (FFN), and a decision head network to obtain the decision logic value of each modality Then, Gumbel-Sigmoid is used for sampling to obtain the visual modality selection decision and the lidar modality selection decision wherein the binary decision .

[0066] The soft sample and hard sample of Gumbel-Sigmoid are defined as follows:

[0067] (9)

[0068] If a straight-through estimator (hard decision) is used:

[0069] (10)

[0070] and passing gradients through where is the decision logic value, is the Gumbel noise, is the temperature parameter, is the Sigmoid function.

[0071] Third, according to the decision result, the visual and laser radar features are selectively masked, and after fusion with the inertial features, they are input into the LSTM pose estimation network to output high-precision six-degree-of-freedom pose.

[0072] For the binary mask , a Gumbel-Sigmoid function is used in combination with a straight-through estimator (Straight-Through Estimator) to realize hard decision sampling, while maintaining gradient differentiability during training. Set the temperature parameter and perform annealing scheduling during training. A higher temperature value is used at the beginning of training to encourage exploratory decisions, and the temperature is gradually reduced to approach deterministic decisions as the training progresses.

[0073] After the policy network outputs the binary mask , the visual and radar features are masked element by element:

[0074] (11)

[0075] In addition, to ensure initialization stability, at the first time step (i=0) of the sequence , i.e. use all modal features for initial pose estimation to ensure correct initialization of the LSTM hidden state, facilitate the initial integrity of the policy network and the generation of subsequent decisions, and generate and utilize the LSTM hidden state at the next time step, providing reliable temporal context information for subsequent policy decisions.

[0076] The weighted visual features, laser radar features, and IMU features that are always retained are concatenated in the feature dimension to form a fusion vector , where M is the dimension of each three-modal feature extracted by the feature extractor, is the feature after selective fusion of the three modalities.

[0077] According to the decision result, the visual and laser radar features are selectively masked to obtain The input three-layer LSTM is used for time series regression, and a high-precision six-degree-of-freedom pose incremental estimation at a current time step is output (3 translation + 3 rotation), and the LSTM hidden state is updated for use in the next time step decision.

[0078] The application realizes dynamic selection and tight coupling fusion of visual, lidar and IMU three-modal data by combining decomposed spatio-temporal attention and multi-modal attention-based strategy network, effectively overcomes the defects of fixed fusion strategy, and realizes high-precision and high-robustness pose estimation in complex environments. Figure 3 The pose estimation error results of the 05 test data on the public KITTI dataset are given, and Table 1 is the pose estimation error results and modal usage percentage values of the 05, 07 and 10 sequence test data on the KITTI dataset.

[0079] Table 1 is the pose estimation error results of the test data set of the application

[0080] Seq t_rel r_rel t_rmse r_rmse Vis_usage Lidar_usage 05 2.2311 0.7014 0.0352 0.0543 64.9040 80.6954 07 1.7569 0.6400 0.0412 0.0524 63.5786 83.9237 10 2.7643 0.6482 0.0715 0.0634 65.2789 83.4305

[0081] t_rel and r_rel in the table are average translation error (%) and average rotation error (%) obtained from various segment lengths of 100m-800m, t_rmse is the position root mean square error, and r_rmse is the rotation root mean square error.

[0082] The basic principles, main features and advantages of the application are shown and described above. Those skilled in the art should understand that the application is not limited to the above examples, and the above examples and descriptions in the specification are only the principles of the application. Without departing from the spirit and scope of the application, various changes and improvements can be made to the application, and these changes and improvements all fall within the scope of the claimed application. The scope of protection required by the application is defined by the appended claims and their equivalents.

Claims

1. A radar, inertial, vision fusion navigation method with inertial attention mechanism, characterized in that Comprise the following steps: The first step, collecting and processing visual / inertial / laser radar navigation data, using a separate space-time attention mechanism to decompose the time / space features of double-frame data of image and radar, and then extracting the inertial guided motion information features; The second step, through the inertial guided attention strategy network, the inertial features pass through the long short-term memory network LSTM hidden state to generate key weight, interact in the multi-modal attention framework, and generate binary decision value through Gumbel-Sigmoid method; The third step, according to the decision result, the visual and laser radar features are selectively masked, and then input into the LSTM pose estimation network after fusion with the inertial features, and output high-precision six-degree-of-freedom pose; The third step is implemented as follows: (c1) using Gumbel-Sigmoid function combined with straight-through estimator to realize hard decision sampling, while keeping the gradient differentiable during training; (c2) the visual decision mask and the laser radar decision mask are respectively multiplied with the corresponding modal features to realize the selective use of the features; (c3) all modal features are used for initial pose estimation at the first time step of the sequence to ensure the correct initialization of the LSTM hidden state, and the generation and use of the LSTM hidden state at the next time step; (c4) the pose estimation network adopts three-layer long short-term memory network LSTM as the core time sequence modeling component, followed by a regressor network composed of two fully connected layers; (c5) the weighted visual features, laser radar features and IMU features are spliced in the feature dimension to form a fusion vector, which is input into the LSTM pose estimator to output the high-precision multi-degree-of-freedom pose increment estimation at the current time step, and the LSTM hidden state is updated for the decision process at the next time step.

2. The radar, inertial, vision fusion navigation method of claim 1, wherein: The first step includes the following contents: (a1) processing the collected image data and generating sample sequence, stacking adjacent two frames of image along channel dimension, and performing channel separation and block processing through double-frame embedding layer; (a2) projecting the collected 3D laser radar point cloud data into two-dimensional image and generating sample sequence; (a3) initial processing of the collected inertial IMU data and generation of sample sequence, cutting the IMU original sequence according to fixed window length, inputting one-dimensional CNN encoder and outputting through full connection projection; (a4) time attention of the separated space-time attention module to obtain overall features with time sequence correlation and spatial context awareness; (a5) after the image and radar obtain the CLS Token at each time step, the multi-layer attention time sequence encoder is used for time sequence coding, and the data is processed by two-level time sequence to obtain the final visual features and laser radar features.

3. The radar, inertial, vision fusion navigation method of claim 1, wherein: The second step is implemented as follows: (b1) the strategy network uses the current features and the hidden state of the last time LSTM to perform multi-modal attention calculation; (b2) during the course of the multi-modal attention computation of the policy network, the query vector generated by modulating the sequence of features with the key vector K through element-wise multiplication, the value vector of the visual / lidar features generated by respective linear transformations; (b3) the modulated Q, K and V are segmented and reshaped in the feature dimension, and finally the visual and laser radar features with attention correlation and enhanced are output; (b4) the attention enhanced output of each modality is respectively connected with a residual connection and layer normalization, and then fed into a decision head network to obtain a decision logic value of each modality, and a visual modality selection decision and a lidar modality selection decision are obtained by sampling.

4. Radar, inertial, vision fusion navigation device with inertial attention mechanism characterized by: The apparatus performs the method of any one of claims 1-3.

5. A non-volatile storage medium, characterized by, The non-volatile storage medium comprises a stored program, wherein the program controls the device in which the non-volatile storage medium is located to perform the method of any one of claims 1-3 when the program is executed.

6. The radar, inertial, vision fusion navigation method of claim 1-3 applied to dynamic environment, enabling a robot with robust, high-precision and energy-efficient pose estimation, The device comprises a processor, a memory, a communication interface and a bus; the processor, the memory and the communication interface are connected through the bus and complete communication with each other; the memory stores executable program code; the processor runs the program corresponding to the executable program code by reading the executable program code stored in the memory.

Images