A deep learning-based multi-modal slam method
By using a deep learning-based multimodal SLAM method, combined with feature pyramid generation and fusion from cameras and LiDAR, the problem of accuracy and difficulty in feature extraction in unstructured environments for downhole SLAM was solved, enabling precise perception and localization of downhole robots.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- HEFEI TURING ERA TECH CO LTD
- Filing Date
- 2022-11-05
- Publication Date
- 2026-06-05
AI Technical Summary
In the unstructured, low-light, and textureless environment of underground mines, the accuracy and difficulty of feature extraction increase, which becomes the main challenge for underground robot perception and localization.
A deep learning-based multimodal SLAM method is adopted. By fusing camera and LiDAR, PWCNet and PointPWC-Net neural networks are used to generate image and point cloud feature pyramids. Combined with KLT optical flow tracking and IMU pre-integration, error state iterative Kalman filtering and factor graph optimization are performed to achieve feature extraction and map reconstruction.
This improved the accuracy of feature extraction in downhole SLAM, reduced the extraction difficulty, met the perception and localization needs of downhole robots, and ensured the accuracy of map reconstruction and the stability of the system.
Smart Images
Figure CN117029802B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of intelligent positioning and navigation technology in mines, and in particular to a multimodal SLAM method based on deep learning. Background Technology
[0002] Coal mine roadways and working faces are characterized by unstructured environments, and GPS technology cannot be directly applied underground, leading to frequent mining accidents. There is an urgent need for mechanization to replace manual labor, automation to reduce manpower, and to improve the level of mine intelligence. Developing an autonomous positioning system suitable for coal mine robots is crucial to solving problems such as precise positioning and attitude perception for underground robots. Rapidly breaking through the technology of precise perception and positioning of underground robots by fusing multiple information sources, including inertial navigation, lasers, and cameras, is key to achieving partial autonomy for underground robots.
[0003] However, due to the unstructured, low-light, and textureless environment of underground mines, the accuracy and difficulty of feature extraction in underground SLAM increase, becoming the main challenge for underground robot perception and localization. Summary of the Invention
[0004] To address the shortcomings of existing technologies, this invention provides a deep learning-based multimodal SLAM method that solves the technical problems of increasing feature extraction accuracy and difficulty in underground SLAM. It can improve the feature extraction accuracy of underground SLAM in unstructured, low-light, and textureless environments in mines, while reducing the extraction difficulty, thus meeting the perception and localization needs of underground robots.
[0005] To address the aforementioned technical problems, this invention provides the following technical solution: a deep learning-based multimodal SLAM method, comprising the following steps:
[0006] S1. Extract information from the current frame image using the camera to generate an image feature pyramid;
[0007] S2. Obtain the information of the current laser frame of the lidar, and generate a point cloud feature pyramid by performing inverse depth scaling on the point cloud using an inverse depth scaling algorithm.
[0008] S3. Input the image feature pyramid and the point cloud feature pyramid into the bidirectional camera-LiDAR fusion module to obtain the fused image and point cloud feature output image feature points.
[0009] S4. The obtained image feature points are tracked using the KLT optical flow tracking method, and the reprojection error is used to perform error state iteration and Kalman filter state update to obtain the latest camera pose.
[0010] The pose estimation of the lidar is updated by using IMU prior and error state iterative Kalman filtering, and then visual odometry factor and lidar odometry factor are generated.
[0011] S5. Optimize the visual odometry factor, lidar odometry factor, and IMU pre-integration factor by adding them to the factor graph.
[0012] S6. Perform 3D map modeling based on the optimized factor graph.
[0013] Further, in step S1, information from the current frame image is extracted using the camera to generate an image feature pyramid. The specific process includes the following steps:
[0014] S11. Subscribe to the information of two adjacent image frames obtained by the current camera;
[0015] S12. Generate image feature pyramids for images based on the PWCNet neural network structure.
[0016] Further, in step S2, the information of the current laser frame of the lidar is obtained, and the point cloud feature pyramid is generated after inverse depth scaling of the point cloud using an inverse depth scaling algorithm. The specific process includes the following steps:
[0017] S21. Subscribe to the information of the point cloud frame obtained by the current lidar;
[0018] S22. First, perform inverse depth scaling on the obtained point cloud to ensure that the image and the point cloud correspond one-to-one.
[0019] S23. Based on the PointPWC-Net neural network structure, generate a point cloud feature pyramid for the point cloud after inverse depth scaling.
[0020] Furthermore, in step S4, the specific process includes the following steps:
[0021] S41. Perform KLT optical flow tracking on the feature points of the image frames fused in step three.
[0022] S42. Establish a residual relationship by reprojecting the latest observation frame of a feature point with the IMU pose estimation corresponding to the oldest frame and the two frames in the sliding window. Finally, use error state Kalman filtering to update the state of the residual relationship established by the reprojection error.
[0023] S43, Release visual odometry factor;
[0024] S44. Use IMU priors to predict the pose of the lidar and update the pose estimate by iterative Kalman filtering based on error state;
[0025] S45, release radar odometer factor.
[0026] Furthermore, in step S5, the visual odometry factor, lidar odometry factor, and IMU pre-integration factor are added to the factor graph for optimization. The specific process includes:
[0027] S51. Add IMU pre-integration factors to the factor plot;
[0028] S52. Add a camera odometer factor to the factor graph;
[0029] S53. Add a lidar odometer factor to the factor graph;
[0030] S54. Calculate the Jacobian matrix of the lidar odometer factor, camera odometer factor, and IMU pre-integration factor with respect to the state variables, and use the Levenberg-Marquardt method to solve the state variables to complete the optimization of the factor graph.
[0031] Further, in step S6, a 3D map is modeled based on the optimized factor map. The specific process includes:
[0032] In the ROS system, the rgbdslam package is configured using the map_server server, and 3D map modeling is performed using all the data optimized by factor graph.
[0033] By employing the above technical solution, the present invention provides a multimodal SLAM method based on deep learning, which has at least the following beneficial effects:
[0034] This invention employs a deep learning-based SLAM method using LiDAR, camera, and IMU. It extracts point cloud features from LiDAR and image features from the camera through a deep neural network, solving the technical problems of increasing accuracy and difficulty in feature extraction in underground SLAM. This method can improve the accuracy of feature extraction in unstructured, low-light, and textureless environments in mines, while reducing the extraction difficulty, thus meeting the perception and localization needs of underground robots. Attached Figure Description
[0035] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings:
[0036] Figure 1 This is a flowchart of the steps of the multimodal SLAM method of the present invention;
[0037] Figure 2 This is a block diagram illustrating the principle of the multimodal SLAM method of the present invention;
[0038] Figure 3This is a schematic diagram of the image feature pyramid of the present invention;
[0039] Figure 4 This is a schematic diagram of the point cloud feature pyramid of the present invention;
[0040] Figure 5 This is a schematic diagram illustrating the fusion of the image feature pyramid and the point cloud feature pyramid by the bidirectional camera-LiDAR fusion module of the present invention.
[0041] Figure 6 This is a schematic diagram of the principle of the bidirectional camera-lidar fusion module of the present invention;
[0042] Figure 7 This is a schematic diagram of the fusion sensing interpolation algorithm of the present invention. Detailed Implementation
[0043] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. This will allow for a full understanding of how the present application uses technical means to solve technical problems and achieve technical effects, and to facilitate its implementation.
[0044] Those skilled in the art will understand that all or part of the steps in the methods of the above embodiments can be implemented by a program instructing related hardware. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Moreover, this application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.
[0045] This embodiment fully considers the unstructured environment of mine tunnels and the inability to directly utilize GPS technology. Due to the unstructured nature of the mine tunnel environment, LiDAR may not be able to collect sufficient feature point clouds under certain circumstances. However, IMU can exist independently of other sensors without relying on external information. Therefore, IMU can perform pre-integration when there are insufficient feature point clouds, compensating for the lack of sufficient feature point clouds. Then, feature extraction is performed on the LiDAR point cloud, and the camera can also provide visual features for matching with the point cloud features. Simultaneously, using neural networks to extract features from camera and LiDAR frames significantly improves the efficiency of feature extraction, thereby optimizing the entire system and ensuring its stability.
[0046] Please refer to Figures 1-7This embodiment illustrates a specific implementation method. In this embodiment, the point cloud features of the LiDAR and the image features of the camera are extracted and fused through a deep neural network and then optimized together. Combining the significant advantages of neural networks in feature extraction compared to traditional methods with a new bidirectional fusion pipeline for camera and radar data fusion in a new method for multimodal data joint processing, this embodiment effectively solves the difficulties in feature extraction and multimodal data fusion in SLAM problems in unstructured, low-light, and textureless environments in underground mines.
[0047] Please refer to Figure 1 and Figure 2 This embodiment proposes a deep learning-based multimodal SLAM method, which includes the following steps:
[0048] S1. Extract information from the current frame image using the camera to generate an image feature pyramid;
[0049] In step S1, information from the current frame image is extracted using the camera to generate an image feature pyramid. The specific process includes the following steps:
[0050] S11. Subscribe to the information of two adjacent image frames obtained by the current camera;
[0051] S12. Generate image feature pyramids based on the PWCNet neural network structure;
[0052] Generate a diagram as shown in the image. Figure 3 The image feature pyramid structure follows the original PWCNet neural network structure, with modifications in that for the image branch, the feedforward CNN is replaced with residual blocks, and batch normalization is performed on each convolutional layer.
[0053] The image feature pyramid takes the top level as input, and for each level, the image features are downsampled by a factor of 2 using residual blocks.
[0054] S2. Obtain the information of the current laser frame of the lidar, and generate a point cloud feature pyramid by performing inverse depth scaling on the point cloud using an inverse depth scaling algorithm.
[0055] In step S2, the information of the current laser frame of the lidar is obtained, and the point cloud feature pyramid is generated after inverse depth scaling of the point cloud using an inverse depth scaling algorithm. The specific process includes the following steps:
[0056] S21. Subscribe to the information of the point cloud frame obtained by the current lidar;
[0057] S22. First, perform inverse depth scaling on the obtained point cloud to ensure that the image and the point cloud correspond one-to-one.
[0058] Specifically, let (P)x ,P y ,P z ) and (P x ',P y ',P z ') represent the coordinates of the points before and after the transformation, respectively. The inverse depth scaling algorithm uses depth... The reciprocal of is scaled equally across all three dimensions, that is:
[0059]
[0060] Transformed coordinates (P) x ',P y ',P z The result can be obtained by integrating the above equation:
[0061]
[0062]
[0063]
[0064] Where C x and C y Set all to 0, C z Set it to 1 to avoid the depth being zero.
[0065] S23. Based on the PointPWC-Net neural network structure, generate a point cloud feature pyramid for the point cloud after inverse depth scaling.
[0066] Specifically, to generate a point cloud after inverse depth scaling, such as... Figure 4 The point cloud feature pyramid structure follows the original PointPWC-Net neural network architecture. The modification is that the original PointPWC-Net built a three-layer pyramid and downsampled the point cloud by a factor of 4. Now, a six-layer pyramid is built with a downsampling factor of 2 to match the levels of each level of the image feature pyramid. Then, the PointConv neural network is used to aggregate the features.
[0067] like Figure 5 As shown, the functions from Level 6 to Level 2 are fused by a two-way camera-LiDAR fusion module to pass supplementary information. Processing starts from the top level, executing a coarse-to-fine estimation scheme up to Level 2.
[0068] In this embodiment, because the point cloud distribution is sparse and does not conform to a regular network, while the image features are organized in a dense grid structure, it is difficult to guarantee a one-to-one correspondence between image pixels and point cloud data, posing a challenge to the fusion of the two modalities. Therefore, an inverse depth scaling operator is proposed to balance the distribution of the point cloud, which has a beneficial effect on the transformation of point cloud information and image information before fusion.
[0069] S3. Input the image feature pyramid and the point cloud feature pyramid into the bidirectional camera-LiDAR fusion module to obtain the fused image and point cloud feature output image feature points.
[0070] In step S3, the specific process includes:
[0071] Specifically, the image feature pyramid Point cloud feature pyramid And the point position P = {p i |i=1,...N}∈R N×3 As input Figure 5 The method shown is used to match and enter Figure 6 The diagram shows a bidirectional camera-LiDAR fusion module, where N is the number of points.
[0072] By fusing image features to point cloud features and fusing point cloud features to image features, the two branches respectively fuse image and point cloud features to obtain fused image and point cloud features, outputting image feature points.
[0073] Specifically, image features are fused into point cloud features:
[0074] First, project the point onto the image plane (denoted as X = {x}). i |i=1,...N}∈R N×2 To retrieve the corresponding two-dimensional features:
[0075]
[0076] Here, F(x) represents the image feature at point x. If the coordinates are not integers, they can be obtained through bilinear interpolation. Then, the extracted feature H is concatenated with the input 3D feature g, and finally, the fused 3D feature is dimensionality reduced by a 1×1 convolution.
[0077] Secondly, point cloud features are fused into image features:
[0078] Similarly, the point is first projected onto the image plane (denoted as X = {x}). i |i=1,...N}∈R N×2 ).
[0079] Due to the sparse nature of point clouds, a fusion-perceptual interpolation algorithm is proposed to create a dense feature map from sparse 3D features. Then, the interpolated point cloud features are concatenated with the input image features, and finally a 1×1 convolution is performed to reduce the feature dimension.
[0080] The details of the fusion-sensory interpolation algorithm are as follows: Figure 7 As shown.
[0081] For each target pixel, find its k nearest neighbors. Use a learnable MLP and MEAN to aggregate features.
[0082] like Figure 7 For each target pixel q in the dense graph, find its k nearest neighbors in the projection points on the image plane. Using MLP and MEAN to aggregate features can be expressed as:
[0083]
[0084] Where N q G represents all neighboring points. i Let represent the 3D features of point i, and [·] denote concatenation. The input to the MLP also includes a 2D similarity measure between q and its neighboring points, defined as:
[0085] S(q,x i )=F(q)·F(x i )
[0086] To better integrate camera and LiDAR feature information, this embodiment proposes a bidirectional camera-LiDAR fusion algorithm to improve the accuracy of the entire system in unstructured environments. Furthermore, due to the sparse point cloud, a perceptual fusion interpolation algorithm is proposed during fusion to enhance the sufficiency of camera and LiDAR information fusion, resulting in a more accurate pose estimate output by the entire system and ensuring the accuracy of map reconstruction.
[0087] S4. The obtained image feature points are tracked using the KLT optical flow tracking method, and the reprojection error is used to perform error state iteration and Kalman filter state update to obtain the latest camera pose.
[0088] The pose estimation of the lidar is updated by using IMU prior and error state iterative Kalman filtering, and then visual odometry factor and lidar odometry factor are generated.
[0089] In step S4, the specific process includes the following steps:
[0090] S41. Perform KLT optical flow tracking on the feature points of the image frames fused in step three.
[0091] Specifically, the number of feature points in the previous frame is determined. If the number of feature points in the previous frame is less than 10, the tracking is set to fail directly. If the number of tracking points meets the preset threshold, LK optical flow tracking is performed directly.
[0092] The method of marginalization is determined by the parallax difference. If the parallax difference between the new frame and the previous frame is large, the oldest frame is marginalized; if the parallax difference is small, the previous frame is marginalized.
[0093] The prior of a new feature point is added to the IMU, and the latest observation frame is obtained by adding all observations of a feature point to the triangulation calculation according to the pose estimation of the IMU prior.
[0094] S42. Establish a residual relationship by reprojecting the latest observation frame of a feature point with the IMU pose estimation corresponding to the oldest frame and the two frames in the sliding window. Finally, use error state Kalman filtering to update the state of the residual relationship established by the reprojection error.
[0095] The error state Kalman filter is then implemented: the general flow of one iteration cycle is as follows:
[0096] 1. Calculate the parametric equations of the plane. Match the calculated plane with the plane points of the previous frame to determine if they are the same plane, i.e., the observations of pose transformation. Use the IMU integral as a prior for error Kalman filtering update. Update the covariance to obtain the optimal estimate of the error and the state vector. After solving for the gain matrix K, obtain the posterior of the error, and then calculate its change. Convergence is determined when the change is less than a threshold.
[0097] 2. When the error state Kalman filter ends and exits, update the posterior covariance matrix.
[0098] 3. After completing this state estimation, add some point clouds from the new frame to the kd tree and update the markers for finding it on the map in the next frame.
[0099] 4. Approximate the result through several iterations, and update the sliding window with the latest error state Kalman filter posterior.
[0100] S43, Release visual odometry factor;
[0101] S44. Use IMU priors to predict the pose of the lidar and update the pose estimate by iterative Kalman filtering based on error state;
[0102] S45, release radar odometer factor.
[0103] S5. Optimize the visual odometry factor, lidar odometry factor, and IMU pre-integration factor by adding them to the factor graph.
[0104] In step S5, the visual odometry factor, lidar odometry factor, and IMU pre-integration factor are added to the factor graph for optimization. The specific process includes:
[0105] S51. Add IMU pre-integration factors to the factor plot;
[0106] Specifically, consider two consecutive frames b within the sliding window. k and b k-1 The IMU pre-integration measurement residuals between IMU measurements are defined as follows:
[0107]
[0108] in[.] xyz The vector part (imaginary part) of the quaternion q used for error state representation was extracted. It is a third-order error state representation of quaternions. It is the IMU pre-integrated measurement value taken only by the noisy accelerometer and gyroscope within the time interval of two consecutive image frames. The accelerometer and gyroscope bias is also included in the residual term for online correction.
[0109] In the above formula, Indicate the definition of residual; These are the differences between the pre-integral values in the position and velocity directions of the IMU; δb a δb g These are the differences in bias for IMU acceleration and angular velocity, respectively. These represent the position, velocity, and rotation of the IMU coordinate system in frame k-1 in the world coordinate system. These are the pre-integral estimates of position, velocity, and angular velocity between time k-1 and time k; g W It is the gravity vector in the world coordinate system; It is the bias of the IMU acceleration at time k; It is the offset of the IMU angular velocity at time k.
[0110] S52. Add a camera odometer factor to the factor graph;
[0111] Specifically, consider the i-th th The first observed l in the image th The feature, at the j-th th The residual of a feature observation in an image is defined as:
[0112]
[0113]
[0114]
[0115] In the above formula, The definition of visual residual is given by b1 and b2, which are two orthogonal bases of the tangent plane, that is, the vector a from the origin to the measurement unit sphere point, and the tangential unit vector on the sphere. The pixel of the l-th feature in the j-th image is back-projected onto the unit sphere using camera intrinsic parameters to form its three-dimensional coordinates. Let be the three-dimensional coordinates of the pixel of the l-th feature in the j-th image. The rotation transformation matrix from the IMU coordinate system to the camera coordinate system is represented in three-dimensional coordinates. The rotation transformation matrix represents the three-dimensional coordinate representation from the world coordinate system to the IMU coordinate system. The position transformation matrix from the camera coordinate system to the IMU coordinate system is represented in three-dimensional coordinates. The position transformation matrix from the IMU coordinate system to the world coordinate system is represented in three-dimensional coordinates.
[0116] in, It is a back projection function that uses camera intrinsics to transform pixel coordinates into unit vectors. It is the i-th th The lth image th The first observation of a feature. It is the jth th Observations of the same features in multiple images. Since the visual residual has 2 degrees of freedom, the residual vector is projected onto the tangent plane.
[0117] S53. Add a lidar odometer factor to the factor graph;
[0118] Specifically, the lidar odometry factor is Where ΔT k,k+1 State node X k With X k+1 The relative transformation relationship between them.
[0119] S54. Calculate the Jacobian matrix of the lidar odometer factor, camera odometer factor, and IMU pre-integration factor with respect to the state variables, and use the Levenberg-Marquardt method to solve the state variables to complete the optimization of the factor graph.
[0120] S6. Perform 3D map modeling based on the optimized factor graph.
[0121] In step S6, a 3D map is modeled based on the optimized factor map. The specific process includes:
[0122] In the ROS system, the rgbdslam package is configured using the map_server server, and 3D map modeling is performed using all the data optimized by factor graph.
[0123] The above embodiments provide a detailed description of the present invention. Specific examples have been used to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.
Claims
1. A deep learning-based multimodal SLAM method, characterized in that, Includes the following steps: S1. Extract information from the current frame image using the camera to generate an image feature pyramid; S2. Obtain the information of the current laser frame from the lidar, and generate a point cloud feature pyramid by performing inverse depth scaling on the point cloud using an inverse depth scaling algorithm. The specific process includes the following steps: S21. Subscribe to the information of the point cloud frame obtained by the current lidar; S22. First, perform inverse depth scaling on the obtained point cloud to ensure that the image and the point cloud correspond one-to-one. S23. Based on the PointPWC-Net neural network structure, generate a point cloud feature pyramid for the point cloud after inverse depth scaling. S3. Input the image feature pyramid and point cloud feature pyramid into the bidirectional camera-LiDAR fusion module to obtain the fused image and point cloud features and output the image feature points. S4. The obtained image feature points are tracked using the KLT optical flow tracking method, and the reprojection error is used to perform error state iteration and Kalman filter state update to obtain the latest camera pose. The pose estimation of the lidar is updated by using IMU prior and error state iterative Kalman filtering, and then visual odometry factor and lidar odometry factor are generated. S5. Optimize the visual odometry factor, lidar odometry factor, and IMU pre-integration factor by adding them to the factor graph. S6. Perform 3D map modeling based on the optimized factor graph.
2. The multimodal SLAM method according to claim 1, characterized in that: In step S1, information from the current frame image is extracted using the camera to generate an image feature pyramid. The specific process includes the following steps: S11. Subscribe to the information of two adjacent image frames obtained by the current camera; S12. Generate image feature pyramids for images based on the PWCNet neural network structure.
3. The multimodal SLAM method according to claim 1, characterized in that: In step S4, the specific process includes the following steps: S41. Perform KLT optical flow tracking on the feature points of the image frames fused in step three. S42. Establish a residual relationship by reprojecting the latest observation frame of a feature point with the IMU pose estimation corresponding to the oldest frame and the two frames in the sliding window. Finally, use error state iterative Kalman filtering to update the state of the residual relationship established by the reprojection error. S43, Release visual odometry factor; S44. Use IMU priors to predict the pose of the lidar and update the pose estimate by iterative Kalman filtering based on error state; S45, Release lidar odometer factor.
4. The multimodal SLAM method according to claim 1, characterized in that: In step S5, the visual odometry factor, lidar odometry factor, and IMU pre-integration factor are added to the factor graph for optimization. The specific process includes: S51. Add IMU pre-integration factors to the factor plot; S52. Add visual odometry factors to the factor plot; S53. Add a lidar odometer factor to the factor graph; S54. Calculate the Jacobian matrix of the lidar odometry factor, visual odometry factor, and IMU pre-integration factor with respect to the state variables, and use the Levenberg-Marquardt method to solve the state variables to complete the optimization of the factor graph.
5. The multimodal SLAM method according to claim 1, characterized in that: In step S6, a 3D map is modeled based on the optimized factor map. The specific process includes: In the ROS system, the rgbdslam package is configured using the map_server server, and 3D map modeling is performed using all the data optimized by factor graph.