A SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion

Abstract

The present invention proposes a SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion, which belongs to the field of automatic navigation and mobile robot technology. The method uses a lightweight improved LoFTR network to design a visual odometer factor to reduce the amount of calculation and improve the accuracy and robustness of the visual robot's positioning and mapping in an unstructured and weakly textured environment. The present invention designs and constructs a multi-sensor dynamic fusion model based on a bird's-eye view, integrating IMU factors, visual odometer factors, laser odometer factors, and loop detection factors. By introducing an adaptive weighting mechanism, features from different sensors are weighted and spliced ​​to achieve the optimal combination of feature information. The algorithm of the present invention reduces the amount of calculation, improves computing efficiency, and enables the SLAM system to achieve real-time or near real-time positioning and mapping. In addition, the adaptive weighting strategy can be dynamically adjusted according to the actual environment and sensor status, which improves the adaptability and flexibility of the algorithm in different scenarios.

Description

Technical Field

[0001] The present invention relates to the field of automatic navigation and mobile robot technology, and proposes a simultaneous localization and mapping (SLAM) method based on a bird's eye view (BEV) model, which integrates end-to-end visual odometer, lidar odometer, inertial measurement unit (IMU) and loop detection information. Background Art

[0002] With the rapid development of fields such as robot navigation and autonomous driving, the performance requirements for SLAM technology are getting higher and higher. As two mainstream environmental perception technologies, visual odometry and laser odometry each have unique advantages and limitations. For example, cameras perform well in visually rich environments, while lidar can provide accurate distance measurements in structured environments. In addition, the real-world environment is usually dynamic and complex, containing various elements from static obstacles to dynamic entities. A single sensor often has difficulty providing stable and reliable perception information when facing complex environments.

[0003] In order to solve the limitations of single-sensor SLAM systems, multi-sensor fusion SLAM technology has become a research hotspot. This technology aims to achieve more comprehensive and accurate environmental perception and robot positioning by integrating data from multiple sensors such as lidar, visual cameras, and IMU. However, traditional visual mileage calculation methods, such as ORB-SLAM and SVO, although they perform well in texture-rich environments, their performance will drop significantly in scenes with lack of texture or drastic changes in lighting. Although laser odometers have advantages in acquiring depth information, they have limited perception capabilities for dynamic objects and are relatively high in cost and power consumption. In addition, IMUs can provide high-frequency motion information, but their cumulative errors are large and need to be fused with other sensor data to improve positioning accuracy. Summary of the invention

[0004] In order to overcome the shortcomings of the above-mentioned prior art, the present invention proposes a SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion, which adopts an end-to-end visual odometer design based on an improved LoFTR matching algorithm. By combining the improved Transformer encoding and decoding framework with the adaptive aggregation attention mechanism, the visual odometer can provide more robust feature extraction and matching performance in dynamic and complex environments, greatly improving the positioning accuracy of the system, especially in scenes with changing lighting and scarce textures. Secondly, the present invention adopts a multi-sensor fusion method based on a bird's-eye view to uniformly convert the information of the camera, lidar, IMU and loop detection into a shared bird's-eye view space. This fusion method maps 2D image features and 3D point cloud data to a unified three-dimensional space through the Lift-Splat-Shoot perspective transformation strategy and voxelization processing, and then adaptively optimizes the weight distribution between each sensor through a dynamic fusion mechanism, thereby effectively improving the adaptability and perception accuracy of the system in complex environments. Combining the advantages of visual odometry and lidar, the system can provide high-precision pose estimation in dynamic and unstructured environments, significantly improving the accuracy and robustness of SLAM system positioning and mapping in unstructured and weakly textured environments.

[0005] The technical solution adopted by the present invention is specifically as follows:

[0006] A SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion, comprising the following steps:

[0007] Step 1: Based on the camera capturing two consecutive frames of environment images, the end-to-end visual odometry based on the improved LoFTR algorithm obtains the sub-pixel matching result, which is used as the visual flow input in the bird's-eye view fusion framework;

[0008] Step 2: Obtain multimodal data from laser odometer, inertial measurement unit (IMU), and loop closure detection;

[0009] Step 3: Convert the multimodal data from the camera, laser odometer, IMU and loop closure detection into a shared bird's-eye view space so that the multimodal data are in the same coordinate system. Then use the dynamic fusion module to stitch the image features from the visual odometer, the point cloud features from the lidar, the dynamic features from the IMU and the features of the loop closure factor based on the adaptive weighting mechanism to obtain the fused features. Then, combine the sub-pixel matching results to accurately determine the relative position between the images, and infer the relative position of the vehicle through the spatial correspondence of the feature points.

[0010] The end-to-end visual odometer based on the improved LoFTR algorithm includes a feature extraction module, a Transformer module, a coarse-grained matching module, and a sub-pixel level refinement module. Specifically, the sub-pixel matching result is obtained by the following steps:

[0011] Step 1.1: Use the feature extraction module to extract two consecutive frames of environment images , Perform feature extraction and extract the first coarse feature map of the middle 1 / 8 downsampling , the second coarse feature map , 1 / 4 resolution and 1 / 2 resolution fine feature maps;

[0012] Step 1.2: The Transformer module transforms the first coarse feature map , the second coarse feature map Perform self-attention and cross-attention alternation to output a set of high-resolution feature maps , , as follows:

[0013] Step 1.2.1: First, the first coarse feature map The feature labels in are adaptively aggregated to obtain the first aggregated feature ; For the second coarse feature map The feature markers in the image are applied with the maximum pooling operation to obtain the second significant feature. ;

[0014] Step 1.2.2: Refer to cross attention, take the first aggregate feature as the query, and the second salient feature as the key and value, and pass through the self-attention layer. The self-attention mechanism is expressed as:

[0015]

[0016] in, is the dimension of the key, The function is used to normalize the attention weights, and T represents transposition; Represent the query, key and value vectors in the self-attention mechanism respectively. Specifically, By respectively , , Applied to the first aggregate feature The second significant feature Get, that is:

[0017]

[0018]

[0019]

[0020] Then query and key Perform relative position encoding;

[0021] Step 1.2.3: Transform the position-encoded query and key The vector is input into the original attention layer, the attention weight based on the encoded tag is calculated, and the value vector is obtained by the weight pair Perform weighted summation to obtain the output of the original attention layer; then the output of the original attention layer is combined with the first coarse feature map Feature markers in the second coarse feature map The feature markers in the first splicing are then upsampled and the output result is combined with the first coarse feature map Feature markers in the second coarse feature map The feature labels in the second splicing are then processed by a multi-layer perceptron, and the output of the multi-layer perceptron is added to the features after the second splicing to obtain the final high-resolution feature map group. , ;

[0022] Step 1.3: Use the coarse matching module to match the high-resolution feature maps by calculating the score matrix between the feature maps. , Perform preliminary matching to obtain a rough matching set ;

[0023] Step 1.4: Use the sub-pixel refinement module to further match the coarse matching result to obtain the sub-pixel matching result; specifically, the high-resolution feature map is combined into , The fine feature map group is obtained by fusing the 1 / 4 resolution and 1 / 2 resolution fine feature maps extracted by the feature extraction module. , and then use the rough matching set As a guide, the fine feature map group Dense correlation calculations, correlation and expectation calculations are performed to achieve sub-pixel accurate matching and obtain sub-pixel matching results.

[0024] Compared with the prior art, the present invention has the following beneficial effects:

[0025] (1) The lightweight feature extraction network and effective aggregation attention mechanism reduce the amount of computation and improve the matching efficiency. For unstructured and weakly textured environments, this paper proposes a visual odometry design method that integrates the LoFTR matching network to enhance the robustness of the system in dynamic and unstructured environments.

[0026] (2) The design of the multi-sensor fusion model based on the bird's-eye view in the present invention further improves the adaptability of the algorithm to complex environments and reduces the dependence of the existing technology on visual laser fusion, and has strong robustness and generalization ability. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 This is a schematic diagram of the overall design framework.

[0028] Figure 2 Schematic diagram of the aggregated attention network.

[0029] Figure 3 Schematic diagram of IMU-assisted laser point cloud dedistortion.

[0030] Figure 4 Schematic diagram of the multi-sensor fusion framework based on the bird's-eye view. DETAILED DESCRIPTION

[0031] The framework of the method of the present invention is as follows Figure 1 As shown, the end-to-end visual odometer, lidar odometer, IMU data and loop detection information based on the improved LoFTR algorithm are integrated. The multi-sensor data is uniformly converted into a shared bird's-eye view and fused using an adaptive weighting mechanism. The present invention is further described below in conjunction with the accompanying drawings.

[0032] Step 1: Two consecutive frames of environment images captured by a monocular camera , ,The end-to-end visual odometry based on the improved LoFTR algorithm obtains sub-pixel matching results , applied to the bird's-eye view fusion framework as visual stream input;

[0033] The present invention adopts an improved LoFTR matching algorithm in visual odometry. This method is based on the improved transformer codec framework and combines the cross-attention mechanism to extract features from the acquired camera image. It is mainly composed of four parts: feature extraction module, transformer module, coarse-grained matching module, and sub-pixel level refinement module. The specific construction is as follows:

[0034] First, use the feature extraction module to extract features. The first coarse feature map , the second coarse feature map (referred to as coarse feature map group , ) and fine feature maps of 1 / 4 resolution and 1 / 2 resolution for coarse-to-fine matching. The coarse feature map group is also combined using the Transformer module , Perform self-attention and cross-attention alternation to transform into a high-resolution feature map group , , in order to improve the discriminability.

[0035] Step 1.1: The feature extraction module uses a lightweight network MobileNetV3 to extract two consecutive frames of environmental images captured by a monocular camera. , Use the feature extraction module to extract features and extract the middle 1 / 8 downsampled coarse feature map group , , and fine feature maps at 1 / 4 resolution and 1 / 2 resolution;

[0036] Step 1.2: Traditional Transformer algorithms usually use linear attention instead of raw attention to reduce computational cost. However, due to the large label size of coarse features, computational efficiency is still limited. In addition, the use of linear attention leads to suboptimal model performance. Therefore, the Transformer module of the present invention adopts an effective aggregation attention network. This network significantly improves the algorithm's computational speed while maintaining matching accuracy by adaptively aggregating features and optimizing attention calculations.

[0037] Specifically, the aggregate attention network of the Transformer module is as follows Figure 2 As shown in the figure, by introducing feature aggregation and selection strategies in the self-attention and cross-attention processes, the amount of computation is effectively reduced and the efficiency and accuracy of feature matching are improved.

[0038] Step 1.2.1: In the aggregated attention network, the feature map is first adaptively aggregated. This step reduces the number of features by aggregating adjacent markers. It is implemented by the following formula:

[0039]

[0040] in, Represents the first coarse feature map The feature markers extracted from express The first aggregate feature after aggregation, Represents a depthwise convolution operation.

[0041] At the same time, by applying a max pooling operation to each feature token, the most significant tokens are selected to further reduce the number of tokens involved in the self-attention calculation:

[0042]

[0043] in, Indicates that from the second coarse feature map The feature markers extracted from express The second most significant feature after selection, Represents a max pooling operation.

[0044] Step 1.2.2: Then, cross attention is introduced on the aggregated features, and the first coarse feature map The first aggregate feature As a query, the coarse feature map The second significant feature As keys and values, the model can establish connections between two different images and capture global context information, thereby improving the accuracy of matching. Next, the self-attention mechanism is applied. Compared with the traditional original attention, this self-attention calculation takes into account the relationship between features while reducing the amount of calculation. The self-attention calculation can be expressed as:

[0045]

[0046] in, is the dimension of the key, The function is used to normalize the attention weights. They represent the query, key, and value vectors in the self-attention mechanism. Specifically, By respectively , , Applied to the first aggregate feature The second significant feature Get, that is:

[0047]

[0048]

[0049]

[0050] In the formula, the weight matrix , , are parameters learned during model training that transform raw feature tokens into queries, keys, and values ​​for use in the self-attention mechanism.

[0051] To further optimize the calculation, relative position encoding (RoPE) is introduced in the self-attention layer to and key Relative position encoding is performed to preserve the spatial relationship information between markers. This encoding method enhances the model's ability to perceive spatial structure by encoding position information into attention weights.

[0052] Step 1.2.3: Then, the position-encoded query and key Input the original attention layer, calculate the attention weight based on the encoded tag, and pass the weight pair value vector Perform weighted summation to obtain the output feature. Next, combine the original attention layer output with the original feature label The first concatenation is performed in the channel dimension and further processed by a linear layer to learn the complex relationship between the output features and extract richer information. Then, through the upsampling operation, the resolution of the original attention layer output features is increased to match the coarse feature map group. , The resolution of the coarse feature map is 100%, which ensures that the detail information of the coarse feature map in the spatial dimension is preserved. To further integrate the feature information of different stages, the upsampled feature map is merged with the feature map of the first splicing layer in the channel dimension through the second splicing operation. In addition, in order to integrate information and enhance the expression ability of the model, a multi-layer perceptron is used to process the spliced ​​features, and the output of the multi-layer perceptron is added to the feature map after the second splicing to obtain the final high-resolution feature map. , , achieving feature fusion and enhancement.

[0053] By combining the coarse feature map , Perform self-attention and cross-attention alternation to output a set of high-resolution feature maps , , not only fuses the feature information from the two images, but also retains the details of the spatial structure for coarse matching and sub-pixel refined matching to establish an accurate correspondence between images, thereby improving the accuracy of feature matching while maintaining computational efficiency.

[0054] Step 1.3: Obtain the converted high-resolution feature map group through the Transformer module , After that, use the coarse matching module to perform preliminary matching and obtain the coarse matching set ; Specifically, the rough matching area is determined by calculating the score matrix between the feature maps. This process can be expressed as:

[0055]

[0056] in, Represents a high-resolution feature map group , The position index of Represents the score matrix, each element of which Representing images Middle position The features and positions in image B The matching score between the features of is the adjusted score matrix The scale of the elements in is used to scale the result of the feature vector dot product, thereby affecting the size of the matching score; in order to ensure that the sum of the matching probabilities at each position is 1, by applying , we can get the matching probability matrix :

[0057]

[0058] Among them, P c ( i , j ) represents the probability of matching between position i and position j in the feature map, Represents the score matrix All columns and specific rows The relevant score collection, Represents the score matrix All rows and specific columns in A collection of related scores. Represents the score matrix No. OK The results are in The value of the row, Represents the score matrix No. OK The results are in The value of the row.

[0059] The two-way matching principle is satisfied, that is, only when two feature points When they are each other's nearest neighbors in each other's feature space, and are relatively high, the feature points are Only those matches greater than the specified threshold are considered valid matches. Composing a coarse matching set .

[0060] Step 1.4: Get a rough matching set After that, the sub-pixel level refinement module is used to further match the coarse matching results. First, the coarse feature map is grouped , The fine feature maps of 1 / 4 resolution and 1 / 2 resolution extracted by the lightweight network MobileNetV3 are fused through convolution and upsampling operations, and the feature maps of different resolutions are spliced ​​in the channel dimension to obtain the fine feature map , thereby retaining the rich detail information in the coarse feature map. Using a result in the coarse matching As a guide, from the fine feature map Cut out The center is a fine feature block of 8×8 for subsequent matching refinement. Then, the coarse matching is further refined by a two-stage correlation refinement layer.

[0061] The first stage is to Perform dense correlation calculation on the 8×8 fine feature block cropped from the center to generate a local score matrix . Then in Apply mutual nearest neighbor matching on The pixel-level matching formula is as follows:

[0062]

[0063] in, Represents the pixel-level matching result. represents the score matrix between fine feature maps, Represents a mutual nearest neighbor matching operation.

[0064] The second stage further refines the matching by performing correlation and expectation calculations in a small area (3×3) to achieve sub-pixel accurate matching. This process can be expressed as:

[0065]

[0066] in, represents the sub-pixel matching result, Represents correlation and expectation calculations performed in a local area.

[0067] Finally, the lightweight and improved LoFTR algorithm of the present invention outputs the sub-pixel matching result , and will be applied as the visual flow input to the bird's-eye view fusion framework.

[0068] Step 2: Obtain multimodal data of LiDAR, IMU, and loop closure detection;

[0069] Step 2.1: Acquisition of point cloud data based on laser odometer; In order to accurately capture the three-dimensional structural information of the surrounding environment, this study uses 3D laser radar to measure the distance in the horizontal and vertical directions by emitting multiple laser beams and receiving the reflected signals, and generates point cloud data containing the three-dimensional information of the environment;

[0070] Step 2.2 Point cloud data distortion correction based on IMU data;

[0071] First, the point cloud data is preprocessed, and the octree filtering strategy is used to remove noise and redundant data, retaining key environmental features. Specifically, the point cloud data is decomposed into eight sub-nodes according to the distribution by recursively segmenting the spatial area until the preset conditions are met (such as the number of points in the node is less than the threshold). Then, the local statistical method is applied to remove noise points based on the mean and standard deviation, simplify the data and reduce the subsequent calculation amount.

[0072] Secondly, IMU data is used to assist in point cloud distortion correction to solve the problem of data distortion caused by rapid movement during movement. This embodiment combines high-frequency IMU data to model and compensate for the movement during the laser radar scanning process, such as Figure 3 The time difference between points can be estimated by matching the timestamps of the lidar or by using its rotation frequency, i.e.:

[0073]

[0074] in, As the starting point, , are any two points in a frame, is the time difference between two points, is the time it takes for the laser radar to rotate once. / is the coordinate of the point on the X-axis / Y-axis.

[0075] Next, the IMU data is used to calculate the attitude change of each point relative to the reference time. If the acquisition time of the point does not completely correspond to the IMU data, the attitude transformation of the point is calculated by linear interpolation. This method unifies the point cloud data into the same coordinate system, achieves accurate distortion correction, ensures the consistency of all point cloud data in space, and improves the accuracy and reliability of the point cloud during movement.

[0076] Then, extract key geometric features. Extract key edge features and plane features from the preprocessed and distortion-corrected point cloud data. Filter out important feature points by calculating the curvature of the point cloud and comparing it with the set threshold. These feature points are used to accurately align multi-frame point cloud data and match them with the map to improve the accuracy of data registration.

[0077] After the edge features and plane features are extracted, feature matching and registration are realized. The present invention uses frame-to-subgraph matching technology to compare the edge and plane features in the current frame with the historical frames to accurately estimate the relative motion between frames. For edge features, feature alignment is completed by finding the nearest neighbor point and calculating the distance from the point to the line; for plane features, it is done by finding the plane neighborhood point and calculating the distance from the point to the plane. Finally, the distance from all feature points to the geometric model is used as the objective function to construct an optimization equation, thereby accurately calculating the position and posture transformation matrix of the lidar to ensure high accuracy and efficiency of registration.

[0078] Step 2.3: Based on the pose relationship between the loop frames, construct a loop detection factor to optimize the relative position relationship between the features;

[0079] After the visual odometer completes the coarse matching and fine matching, the present invention further generates a dense descriptor for each pixel. First, the current frame is copied and connected with the channel dimension as the input of the improved LoFTR network in step 1 to generate a dense feature descriptor for each pixel in the image. And extract specific feature vectors from the coarse and fine matching stages of the LoFTR network. The coarse level features are reshaped into a feature map with a scale of 1 / 8 of the original image and 256 channels, and then a coarse level descriptor is generated at each pixel position using bilinear interpolation. For the fine level descriptor, the fine level feature vector used to calculate the sub-pixel match is selected as the fine level descriptor at each pixel position. Finally, by connecting the coarse level descriptor and the fine level descriptor, a complete hierarchical descriptor that can capture the high and low level textures of the pixel is obtained. In this way, a descriptor is generated for each pixel, resulting in a dense descriptor with the same resolution as the original image.

[0080] Secondly, the final keypoints are selected by calculating the descriptor saliency scores. The dense descriptors of each channel are regarded as saliency maps for keypoint localization. To calculate the saliency scores, the local scores of each feature map are calculated, which requires exponential operation of the 9 local neighbors of each pixel and then normalization of these exponential scores. Next, the channel score is calculated by normalizing the descriptors of each channel. Then, the combined score of each pixel is obtained by multiplying the local score and the channel score and taking the maximum value. And the saliency score of each pixel is obtained by normalizing the combined scores of all pixels. Finally, the pixels with the highest 5% saliency scores are selected as keypoint candidates, and the final keypoint location is determined by using non-maximum suppression (NMS) within a 9×9 local window.

[0081] Then, the key points are clustered using the K-ary tree method to construct a visual dictionary, where each cluster center represents a type of feature, i.e., a "word". Subsequently, the image is represented by calculating the histogram of its visual vocabulary, which reflects the frequency distribution of various visual vocabulary in the image. Possible loops are identified by comparing the histogram similarity between the current frame and the historical frame, and the frame with the highest similarity is selected as the candidate loop frame.

[0082] Finally, in the coarse matching verification stage, the iterative closest point (ICP) algorithm is used to refine the pixels of the preliminary match, and the distance error between pixels is reduced through an iterative optimization process, thereby verifying and optimizing the coarse match. Next, the present invention accurately estimates the pose relationship between the loop frames and uses this relationship to construct a loop detection factor.

[0083] Step 3: Multi-sensor fusion based on shared bird’s-eye view.

[0084] The present invention adopts a multi-sensor fusion method based on bird's-eye view representation, such as Figure 4 As shown in the figure, the multimodal data from cameras, lidar, IMU and loop closure detection are converted into a shared bird's-eye view space to achieve accurate multimodal information fusion.

[0085] Step 3.1: First, the data of each sensor is converted into a unified bird’s-eye view space so that the multi-sensor information can be effectively fused in the same coordinate system.

[0086] In the process of processing the visual odometry factor, since the camera's perspective is perspective, this study uses the Lift-Splat-Shoot (LSS) method to transform the visual features, map the 2D image features to the 3D coordinate space, and then quantify and project these features to the bird's-eye view plane. Specifically, in order to convert the two-dimensional image features into a representation in three-dimensional space, it is necessary to predict the depth of each pixel. For this purpose, a view projection module is designed. Based on the image features extracted by the visual odometry, the depth value is densely predicted by classification. According to the camera's external parameters and the predicted depth value, the image view features can be mapped to the three-dimensional coordinate system, pseudo voxels are generated, and preparations are made for the subsequent bird's-eye view encoding.

[0087] After the depth value prediction is completed, the designed view projection module is used to convert the two-dimensional image features into a three-dimensional coordinate system through the predicted depth information. Specifically, a 2D to 3D projection method is adopted to generate feature points in the three-dimensional space by applying the depth information to the camera features. Next, it enters the bird's-eye view encoding module, which further encodes the obtained pseudo-voxel features into features of the bird's-eye view space. Unlike the traditional downsampling operation, the bird's-eye view encoding module of the present invention adopts a space-to-channel (S2C) operation to convert the 4D tensor into a 3D tensor by reshaping to retain semantic information and reduce computational costs. Subsequently, four 3×3 convolutional layers are used to gradually reduce the channel dimension and extract high-level semantic information. These depth information and the sub-pixel matching results in step 1 As the visual stream input, it is passed to the bird’s-eye view fusion module.

[0088] For the point cloud information from the laser odometry, the processed point cloud is mapped to the bird's-eye view space by learning a parameterized voxelization method. The original points are voxelized using a sparse 3D convolutional network (PointPillars) to generate the corresponding voxel feature representation. The obtained voxel features are further encoded into the bird's-eye view features. The space-to-channel (S2C) operation is used to adjust the dimension of the features, and finally the convolutional layer is used to extract high-level semantic information to ensure that key geometric information and environmental features are retained in the bird's-eye view.

[0089] To further enhance the fusion effect, the present invention also integrates IMU data into the bird's-eye view representation. The IMU data calculates the pose change at the current moment through interpolation and pre-integration, and time-aligns the timestamp of the IMU with the timestamp of the point cloud data. In this way, IMU data can be used together with image and point cloud features to generate the bird's-eye view space, ensuring that key dynamic information is not missed during the environmental perception process.

[0090] For the loop detection factor, the present invention uses the camera intrinsics and extrinsics of the loop frame to convert its feature coordinates from the image coordinate system to the world coordinate system during the feature conversion process. This process requires combining the two-dimensional pixel coordinates with the depth information, and the conversion formula is:

[0091]

[0092] in, Represents the three-dimensional coordinates in the pixel world coordinate system, is the camera intrinsic parameter matrix, is the coordinate of the pixel on the image plane, is the homogeneous coordinate representation of a two-dimensional pixel point. is the depth value.

[0093] The feature points converted by the loop detection factor are projected onto the bird's-eye view plane, and the three-dimensional coordinates of the features are Perform appropriate processing to obtain its representation in the bird's-eye view coordinate system In this process, the height information is usually Compress, ignore, or average to reduce dimensionality.

[0094] Step 3.2: Use the dynamic fusion module to fuse multimodal features to obtain high-precision pose estimation results.

[0095] After converting the data of each sensor into a unified bird's-eye view space, this embodiment uses a dynamic fusion module to effectively fuse the multimodal features from different sensors. By introducing an adaptive weighting mechanism, the image features from the visual odometer, the point cloud features from the lidar, the dynamic features from the IMU, and the features of the loop detection factor are spliced. The whole process can be expressed by the following formula:

[0096]

[0097] in, Represents a concatenation operation along the channel dimension. Respectively represent the image, lidar, IMU, and loop detection factor features converted to the unified bird's-eye view space. Through the static fusion function , compressing the concatenated feature dimensions to an appropriate number of channels. Static fusion can be achieved through a 3×3 convolutional layer to ensure that the information after feature fusion still has strong expressive power. After static fusion is completed, the channel attention mechanism is applied To select and emphasize important features. This mechanism maps features to importance scores through global average pooling and weights the features through the sigmoid activation function. Its formula is:

[0098]

[0099] in, is the linear transformation matrix. Represents the feature vector after being processed by the static fusion function. Specifically, Spliced ​​in the channel dimension. is the global average pooling operation, is the eigenvector Perform a global average pooling operation to obtain a global feature description. is the sigmoid function. In this way, the dynamic fusion module can adaptively select the most important features to ensure that the system can still maintain efficient perception capabilities in the presence of environmental changes and sensor noise.

[0100] In addition, the loop detection factor enhances the spatial consistency of the current fused features by providing spatial constraints on the historical frames. The integration of loop information not only helps to correct the system state, but also further optimizes the relative position relationship between features. This process is achieved in the following way:

[0101]

[0102] in, represents the output fusion features obtained by the dynamic fusion module, represents the final feature representation after integrating the loop detection factor, It is the influence weight of the loop detection factor on the final feature, ensuring that the feature fusion can be dynamically adjusted when performing loop detection, thereby improving the overall performance and reliability of the system.

[0103] Next, using the fusion features The visual features contained in , combined with the sub-pixel matching output by the LoFTR network in step 1 , to accurately determine the relative position between images, and infer the relative position of the vehicle through the spatial correspondence of feature points. At the same time, the geometric information provided by the lidar and the dynamic data of the IMU are integrated into the fusion feature This enables the system to stably estimate the pose of the robot or sensor in a dynamically changing environment, especially in the case of rapid motion and high-frequency vibration. By integrating vision, lidar and IMU features, high-precision pose estimation is achieved. The system can quickly obtain the vehicle's position and pose in real-time or offline mode.

[0104] Finally, based on the pose estimation results, the data from different sensors are accurately mapped into the environment map. Based on the rich information in the image, the point cloud data of the LiDAR can be accurately aligned and combined with the texture information in the visual data to gradually build a detailed 3D environment model. During the mapping process, the pose estimation provides a real-time spatial position reference for each sensor data, ensuring that each frame of sensor data can be accurately positioned in the world coordinate system, so that the environment map is continuously improved and updated. At the same time, the fusion features generated by the dynamic fusion module Guided by the loop detection factor, the map is optimized and corrected. The loop detection factor can determine whether the vehicle has returned to a known position by matching features between historical frames, and adjust the current pose estimate based on the spatial information of the historical map, thereby correcting the accumulated error and optimizing the position relationship in the map. This optimization process greatly improves the accuracy of mapping, avoids pose drift and map distortion caused by long-term operation, and ensures that the final generated map not only accurately reflects the real structure of the environment, but also has high consistency and robustness.

[0105] The above description is only a specific implementation mode of the present invention. Any feature disclosed in this specification, unless otherwise stated, can be replaced by other equivalent or alternative features with similar purposes; all disclosed features, or all methods or steps in the process, except for mutually exclusive features and / or steps, can be combined in any way.

Claims

1. A SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion, characterized in that: The following steps are involved: Step 1: Based on the camera capturing two consecutive frames of environment images, the end-to-end visual odometry based on the improved LoFTR algorithm obtains the sub-pixel matching result, which is used as the visual flow input in the bird's-eye view fusion framework; Step 2: Obtain multimodal data from laser odometer, inertial measurement unit (IMU), and loop closure detection; Step 3: Convert the multimodal data from the camera, laser odometer, IMU and loop detection into a shared bird's-eye view space so that the multimodal data are in the same coordinate system. Then use the dynamic fusion module to stitch the image features from the visual odometer, the point cloud features from the lidar, the dynamic features from the IMU and the features of the loop detection factor based on the adaptive weighting mechanism to obtain the fusion features. Then combine the sub-pixel matching results to accurately determine the relative position between the images, and infer the relative position and posture of the vehicle through the spatial correspondence of the feature points. The workflow of the dynamic fusion module is as follows: First, the multimodal data is concatenated using an adaptive weighting mechanism, expressed as: F fused =f adaptive (f static ([F camera ;F Lidar ;F IMU ;F loop ])); Where [·; ·] represents the concatenation operation along the channel dimension; F camera 、F Lidar 、F IMU 、F loop They represent the image, lidar, IMU, and loop detection factor features converted from the bird's-eye view encoding module to the unified bird's-eye view space; f static represents the static fusion function; f adaptive represents the channel attention mechanism, F fused Represents fusion features; the bird's-eye view encoding module adopts space-to-channel operation and uses four convolutional layers to gradually reduce the channel dimension; In addition, the loop detection factor enhances the spatial consistency of the current fusion feature by providing spatial constraints on the historical frames. This process is achieved as follows: F final =F fused +αF loop ; Among them, F final represents the final feature representation after integrating the loop detection factor, and α is the influence weight of the loop detection factor on the final feature; Then, using the fusion feature F fused The visual features contained in the image are combined with the sub-pixel matching results to accurately determine the relative positions between images, and the relative position and posture of the vehicle are inferred through the spatial correspondence of the feature points. Based on the pose estimation results, the data from different sensors are accurately mapped to the environment map.

2. A SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion according to claim 1, characterized in that: The end-to-end visual odometer based on the improved LoFTR algorithm includes a feature extraction module, a Transformer module, a coarse-grained matching module, and a sub-pixel level refinement module. Specifically, the sub-pixel matching result is obtained by the following steps: Step 1.1: Use the feature extraction module to extract two consecutive frames of environment images I A ,I B Perform feature extraction and extract the first coarse feature map of the middle 1 / 8 downsampling The second coarse feature map Fine feature maps at 1 / 4 resolution and 1 / 2 resolution; Step 1.2: The Transformer module transforms the first coarse feature map The second coarse feature map Perform self-attention and cross-attention alternation to output a set of high-resolution feature maps The details are as follows: Step 1.2.1: First, the first coarse feature map The feature labels in are adaptively aggregated to obtain the first aggregated feature f i ′; for the second coarse feature map The feature markers in the image are applied with the maximum pooling operation to obtain the second significant feature f j ′; Step 1.2.2: Refer to cross attention, take the first aggregate feature as the query, and the second salient feature as the key and value, and pass through the self-attention layer. The self-attention mechanism is expressed as: Among them, d k is the dimension of the key, the softmax function is used to normalize the attention weights, T represents the transpose; Q, K, and V represent the query, key, and value vectors in the self-attention mechanism, respectively. Specifically, Q, K, and V are respectively obtained by transforming the weight matrix W Q , W K , W V Applied to the first aggregate feature f i ′, the second significant feature f j 'Get, that is: Q=W Q f i ′; K=W K f j ′; V=W V f j ′; Then query Q and key K for relative position encoding; Step 1.2.3: Input the position-encoded query Q and key K into the original attention layer, calculate the attention weight based on the encoded tag, and weighted sum the value vector V by the weight to get the output of the original attention layer; then the output of the original attention layer is combined with the first coarse feature map Feature markers in the second coarse feature map The feature markers in the first splicing are then upsampled and the output result is combined with the first coarse feature map Feature markers in the second coarse feature map The feature labels in the second splicing are then processed by a multi-layer perceptron, and the output of the multi-layer perceptron is added to the features after the second splicing to obtain the final high-resolution feature map group. Step 1.3: Use the coarse matching module to match the high-resolution feature maps by calculating the score matrix between the feature maps. Perform preliminary matching to obtain a rough matching set M c ; Step 1.4: Use the sub-pixel refinement module to further match the coarse matching result to obtain the sub-pixel matching result; specifically, the high-resolution feature map is combined into The fine feature map group is obtained by fusing the 1 / 4 resolution and 1 / 2 resolution fine feature maps extracted by the feature extraction module. Then, using the rough matching set M c As a guide, the fine feature map group Dense correlation calculations, correlation and expectation calculations are performed to achieve sub-pixel accurate matching and obtain sub-pixel matching results.

3. A SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion according to claim 2, characterized in that: The process of converting multimodal data from cameras, laser odometers, IMUs, and loop closure detection into a shared bird’s-eye view space is as follows: For the two-dimensional image features acquired by the camera, the depth value is densely predicted by classification, and the two-dimensional image features are converted into a three-dimensional coordinate system using the predicted depth value, and input into the bird's-eye view encoding module; For the point cloud information from the laser odometer, the processed point cloud is input into the bird's-eye view encoding module by learning the parameterized voxelization method; For IMU data, the current pose change is calculated through interpolation and pre-integration, and time alignment is performed based on the timestamp of the IMU and the timestamp of the point cloud data; For the loop detection factor, the camera intrinsic and extrinsic parameters of the loop frame are used to transform its feature coordinates from the image coordinate system to the world coordinate system and input them into the bird's-eye view encoding module.

4. A SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion according to claim 3, characterized in that: The channel attention mechanism maps features to importance scores through global average pooling and weights the features through the sigmoid activation function, and its formula is: adaptive (F) = σ(W·f avg (F))·F; Where W is the linear transformation matrix, F represents the feature vector after being processed by the static fusion function, and f avg is the global average pooling operation, and σ is the sigmoid function.

5. A SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion according to claim 4, characterized in that: The feature extraction module is specifically a lightweight network MobileNetV3.

6. A SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion according to claim 5, characterized in that: The rough matching module specifically performs preliminary matching through the following steps: The rough matching area is determined by calculating the score matrix between the feature maps. This process is expressed as: Among them, i and j represent high-resolution feature map groups. The position index of image I is S, which represents the score matrix, and each element S(i, j) represents the image I A The matching score between the feature at position i in image A and the feature at position j in image B; τ is a parameter; Further obtain the matching probability matrix P c : P c (i,j)=softmax(S(i,·)) j ·softmax(S(·,j)) i ; Among them, P c (i, j) represents the probability of a match between position i and position j in the feature map, S(i, ) represents the set of scores associated with a specific row i in all columns of the score matrix S, and S( , j) represents the set of scores associated with a specific column j in all rows of the score matrix S; softmax(S(i, )) j represents the value of the softmax result of the i-th row of the score matrix S in the j-th row, softmax(S(·, j)) i Represents the value of the softmax result of the j-th row of the score matrix S in the i-th row; P c The two-way matching principle is satisfied in the , that is, the feature point pair (i, j) is considered to be a valid match only when the two feature points i and j are the nearest neighbors of each other in the feature space, and the matching probability greater than the specified threshold is Composed of rough matching set M c .

7. A SLAM method based on end-to-end visual odometer and bird's-eye view multi-sensor fusion according to claim 6, characterized in that: The sub-pixel matching result is obtained specifically through the following steps: First, the high-resolution feature map is combined The fine feature maps of 1 / 4 resolution and 1 / 2 resolution are fused through convolution and upsampling operations, and the feature maps of different resolutions are spliced ​​in the channel dimension to obtain a fine feature map group. Using a result from a rough match As a guide, from the fine feature map group Cut out The coarse match is then further refined by a two-stage correlation refinement layer: The first stage is to Perform dense correlation calculation on the 8×8 fine feature block cropped from the center to generate the local score matrix S fine ; then in S fine Apply mutual nearest neighbor matching on The pixel-level matching formula is as follows: M pixel =MNN(S fine ); Among them, M pixel Represents the pixel-level matching result, S fine represents the score matrix between the fine feature graph groups, and MNN represents the mutual nearest neighbor matching operation; The second stage further refines the matching by performing correlation and expectation calculations in the local area to achieve sub-pixel accurate matching. This process is expressed as: M subpixel =Expectation(M pixel ); Among them, M subpixel Represents the sub-pixel matching result, and Expectation represents the correlation and expectation calculation performed in the local area.

Images