A SLAM method and system for unstructured features in underground mines

Abstract

This invention relates to the field of underground mine positioning and navigation technology, solving the technical problem of difficult underground positioning and mapping in unstructured environments where GPS is ineffective. Specifically, it relates to a SLAM method for unstructured features in underground mines, comprising the following steps: acquiring point cloud information of the current laser frame from a lidar system; calculating the curvature of each point in the point cloud information of the current lidar frame, and extracting corner features and planar point features of each point based on the curvature; extracting information from the current frame using a camera, detecting corner points of the current frame using the FAST algorithm based on the current frame information, and determining whether it is a corner feature point; and tracking feature points of the current sliding window keyframe using the KLT optical flow tracking algorithm based on the current frame information. This invention achieves high-precision positioning in coal mines, balancing accuracy and computational load, and improving the accuracy and real-time performance of positioning and mapping in mine tunnels.

Description

Technical Field

[0001] This invention relates to the field of underground mine positioning and navigation technology, and in particular to a SLAM method and system for unstructured features in underground mines. Background Technology

[0002] Coal mine roadways and working faces are characterized by unstructured environments. GPS technology cannot be directly applied to underground coal mines, resulting in poor positioning accuracy and attitude perception of autonomous positioning systems for coal mining robots. This leads to frequent mining accidents and reduces the safety of coal mining. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention provides a SLAM method and system for unstructured underground mining environments, solving the technical problem of difficult underground positioning and mapping in unstructured environments where GPS is ineffective.

[0004] To address the aforementioned technical problems, this invention provides the following technical solution: a SLAM method for unstructured features in underground mines, comprising the following processes:

[0005] Obtain the point cloud information of the current laser frame from the lidar;

[0006] Calculate the curvature of each point in the current laser frame point cloud information of the lidar, and extract the corner features and planar point features of each point based on the curvature of each point;

[0007] The information of the current frame is extracted by the camera, and the corner points of the current frame are detected by the FAST algorithm based on the information of the current frame, and it is determined whether it is a corner feature point.

[0008] Based on the information of the current frame, the KLT optical flow method is used to track the feature points of the current sliding window keyframe.

[0009] The pose estimate of the current state is updated based on the reprojection error between the pose estimate from the IMU prior and the feature points tracking the current sliding window keyframe.

[0010] The pose estimation of the current state is used to update the state and obtain the latest visual features.

[0011] The latest visual features are matched with the point cloud information of the current laser frame of the lidar with corner feature point cloud data to obtain the lidar odometry factor and the camera odometry factor.

[0012] After incorporating the lidar odometer factor, camera odometer factor, and IMU pre-integration factor into the factor graph for optimization, a 3D map is created.

[0013] Furthermore, the step of calculating the curvature of each point in the current laser frame point cloud information of the lidar, and extracting the corner features and planar point features of each point based on the curvature of each point, specifically includes the following processes:

[0014] Subscribe to the point cloud information after motion distortion correction for the current laser frame;

[0015] Traverse the effective point cloud after motion distortion correction of the current laser frame, extract several points before and after the current laser point from the effective point cloud and calculate their corresponding curvature;

[0016] Establish a set of corner points and a set of planar points based on the curvature of several points before and after the current laser point;

[0017] Extract the corner features of each point from the corner feature set, and extract the planar feature of each point from the planar feature set.

[0018] Furthermore, the step of using the FAST algorithm to detect corner points in the current frame based on the information of the current frame and determining whether it is a corner feature point specifically includes the following process:

[0019] Subscribe to information about the current frame extracted by the current camera;

[0020] Select a pixel from the information in the current frame and determine whether the pixel is a corner feature point.

[0021] Furthermore, the step of selecting a pixel from the information of the current frame and determining whether the pixel is a corner feature point specifically includes the following process:

[0022] Set the brightness value of the selected pixel p to I. p The brightness threshold is set to t;

[0023] Define a discretized Bresenham circle with a radius of 3 pixels centered at pixel p. The Bresenham circle has 16 pixels on its boundary.

[0024] Given a Bresenham circle of size 16 pixels with 9 consecutive pixels, if the pixel value of the 9 consecutive pixels is greater than or less than I... p If +t, then pixel p is determined to be a corner feature point;

[0025] If the pixel value of 9 consecutive pixels is equal to I p If +t, then pixel p is determined not to be a corner feature point.

[0026] Furthermore, the step of tracking feature points of the current sliding window keyframe using the KLT optical flow algorithm based on the information of the current frame specifically includes the following process:

[0027] Subscribe to information about the current frame extracted by the current camera;

[0028] Perform histogram equalization on the original image in the current frame information;

[0029] Extract the pyramid of the histogram-equalized image, and then extract the FAST feature points of the previous frame image.

[0030] LK optical flow tracking is performed on the FAST feature points of the previous frame image;

[0031] Determine the number of feature points in the previous frame. If the previous frame has fewer than 10 feature points, set the tracking as failed.

[0032] If the number of tracking points meets the preset threshold, LK optical flow tracking is performed directly.

[0033] Furthermore, the step of updating the pose estimation of the current state specifically includes the following processes:

[0034] The method of marginalization is determined by the parallax. If the parallax between the new frame and the previous frame is large, the oldest frame is marginalized; if the parallax is small, the previous frame is marginalized.

[0035] 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.

[0036] Establish a residual relationship by comparing the latest observation frame of a feature point with the IMU pose estimation of the oldest frame and the two frames corresponding to the oldest frame in the sliding window based on the reprojection error.

[0037] Furthermore, the step of updating the state by estimating the pose of the current state using ESIKF to obtain the latest visual features specifically includes the following process:

[0038] The plane of the current frame is calculated by parametric equations, and the plane of the current frame is matched with the plane points of the previous frame to obtain the same plane, which is the observation of pose transformation.

[0039] Using the IMU integral as a priori, EKF update is performed and the covariance is updated to obtain the optimal pose estimate of the error and the state vector.

[0040] When ESIKF ends and exits, the posterior covariance matrix is ​​updated to complete the pose estimation of the current state and update the state.

[0041] Add some point clouds from the new frame to the kd tree and update the marker for finding it on the map in the next frame;

[0042] The result is approximated by ESIKF through several iterations, and the current sliding window is updated with the latest ESIKF posterior.

[0043] Furthermore, the step of adding the lidar odometry factor, camera odometry factor, and IMU pre-integration factor to the factor graph for 3D map modeling specifically includes the following processes:

[0044] The lidar odometry factor, camera odometry factor, and IMU pre-integration factor were added to the factor graph for optimization.

[0045] Calculate the Jacobian matrices of the lidar odometer factor, camera odometer factor, and IMU pre-integration factor with respect to the state variables, and solve the state variables using the Levenberg-Marquardt method.

[0046] 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.

[0047] This invention also provides another technical solution: a system for implementing SLAM under unstructured features in underground mines. The system utilizes factor graph-optimized lidar odometry factors, camera odometry factors, and IMU pre-integration factors. It then uses the rgbdslam package configured on the map_server in a ROS system and combines all the graph-optimized data to perform 3D map modeling, including:

[0048] A point cloud information acquisition module, which is used to acquire the point cloud information of the current laser frame of the lidar;

[0049] The feature extraction module is used to calculate the curvature of each point in the current laser frame point cloud information of the lidar, and extract the corner features and planar point features of each point based on the curvature of each point.

[0050] The corner feature determination module is used to extract information of the current frame through the camera, use the FAST algorithm to detect the corners of the current frame based on the information of the current frame, and determine whether it is a corner feature point.

[0051] The feature point tracking module is used to track feature points of the current sliding window keyframe using the KLT optical flow tracking algorithm based on the information of the current frame.

[0052] The pose estimation update module is used to update the pose estimation of the current state based on the reprojection error between the prior pose estimation of the IMU and the feature points of the current sliding window keyframe.

[0053] A visual feature extraction module is used to update the state by estimating the pose of the current state using ESIKF to obtain the latest visual features.

[0054] The odometry factor generation module is used to perform feature matching between the latest visual features and the point cloud information of the current laser frame of the lidar with corner feature point cloud data to generate lidar odometry factor and camera odometry factor.

[0055] The factor graph optimization module is used to add the lidar odometer factor, camera odometer factor, and IMU pre-integration factor to the factor graph optimization for 3D map modeling.

[0056] By employing the above technical solution, the present invention provides a SLAM method and system for unstructured features in underground mines, which has at least the following beneficial effects:

[0057] 1. This invention uses lidar, camera, and IMU to achieve high-precision positioning in coal mines and improves attitude perception of the underground environment. This enables high-precision positioning in coal mines while balancing accuracy and computational load, improving the accuracy and real-time performance of positioning and mapping in mine tunnels. It effectively solves the problem of difficult underground positioning and mapping in unstructured environments where GPS cannot be used.

[0058] 2. This invention uses a SLAM method combining lidar, camera, and IMU to perform joint calculations on the point cloud features of lidar, IMU data, and the feature information extracted and optimized by the camera. This improves the accuracy of the entire system in unstructured environments. Furthermore, it performs keyframe matching to reduce the computational load of the entire system, achieving a balance between accuracy and computational load, and ensuring the accuracy and real-time performance of the system.

[0059] 3. This invention optimizes the entire system by extracting features from the point cloud of the lidar and matching the visual features provided by the camera with the point cloud features, thereby keeping the entire system stable. Attached Figure Description

[0060] 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:

[0061] Figure 1This is a flowchart of the SLAM method of the present invention;

[0062] Figure 2 This is a schematic diagram of point cloud feature extraction data transmission according to the present invention;

[0063] Figure 3 This is a schematic diagram of the data transmission for visual feature extraction according to the present invention;

[0064] Figure 4 This is a schematic diagram of visual odometry data transmission in the camera of the present invention;

[0065] Figure 5 This is a schematic diagram illustrating the factor graph optimization data transfer of the present invention.

[0066] In the diagram: 100, Point Cloud Information Acquisition Module; 200, Feature Extraction Module; 300, Corner Feature Judgment Module; 400, Feature Point Tracking Module; 500, Pose Estimation Update Module; 600, Visual Feature Derivation Module; 700, Odometry Factor Derivation Module; 800, Factor Map Optimization Module. Detailed Implementation

[0067] 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.

[0068] 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.

[0069] Please refer to Figures 1-5 This illustration demonstrates a specific implementation of this embodiment, fully considering 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. Subsequently, feature extraction of the point cloud is performed on the LiDAR, and the camera can also provide visual features to match the point cloud features, thereby optimizing the entire system and ensuring its stability.

[0070] A SLAM method for unstructured features in underground mines includes the following steps:

[0071] S1. Obtain the point cloud information of the current laser frame of the lidar.

[0072] S2. Calculate the curvature of each point in the current laser frame point cloud information of the lidar, and extract the corner features and planar point features of each point based on the curvature of each point.

[0073] Step S2 specifically includes the following steps:

[0074] S21. Subscribe to the point cloud information after motion distortion correction for the current laser frame.

[0075] S22. Traverse the effective point cloud after motion distortion correction of the current laser frame, extract several points before and after the current laser point from the effective point cloud and calculate their corresponding curvature. The square of the curvature distance difference at the corner of the current laser point is used as the curvature, and the curvature value of the point is stored at the same time. Taking parameter 5 as an example, take the five points before and five points after the current laser point, a total of ten points, and calculate the curvature of each point. The curvature of each point is the square of the curvature distance difference at the corner.

[0076] S23. Based on the curvature of several points before and after the current laser point, establish a corner point feature set and a planar point feature set. During this process, points belonging to the two cases of occlusion and parallelism are marked from several points and no feature extraction is performed. Then, traverse the scan line. The point cloud of each scan line is divided into 6 segments. Extract 20 corner points and an unlimited number of planar points for each segment. Add points with large curvature in each segment to the corner point feature set and points with small curvature to the planar point feature set. Finally, consider non-corner points as planar points and add them to the planar point feature set, and then perform downsampling.

[0077] S24. Extract the corner features of each point from the corner feature set and the planar point features of each point from the planar point feature set. During this process, publish the corner point cloud and planar point cloud of the corresponding point based on the corner features and planar point features, and at the same time publish the point cloud information of the current laser frame of the lidar with corner feature point cloud data.

[0078] S3. Extract information from the current frame using the camera. Based on the information from the current frame, use the FAST algorithm to detect corner points in the current frame and determine whether it is a corner feature point. Taking a mine shaft as an example, the camera captures an image of the current scene, and the image of the current frame contains information about the scene.

[0079] Step S3 specifically includes the following steps:

[0080] S31. Subscribe to the information of the current frame extracted by the current camera.

[0081] S32. Select a pixel from the information in the current frame and determine whether the pixel is a corner feature point.

[0082] In step S32, determining whether a pixel is a feature point includes the following steps:

[0083] S321. Set the brightness value of the selected pixel p to I. p The brightness threshold is set to t.

[0084] S322. Define a discretized Bresenham circle with a radius of 3 pixels centered on pixel p. The boundary of the Bresenham circle has 16 pixels.

[0085] Given a Bresenham circle of size 16 pixels with 9 consecutive pixels, if the pixel value of the 9 consecutive pixels is greater than or less than I... p If +t, then pixel p is determined to be a corner feature point.

[0086] If the pixel value of 9 consecutive pixels is equal to I p If +t, then pixel p is determined not to be a corner feature point.

[0087] S4. Based on the information of the current frame, the KLT optical flow method is used to track the feature points of the current sliding window key frame.

[0088] Step S4 specifically includes the following steps:

[0089] S41. Subscribe to the information of the current frame extracted by the current camera.

[0090] S42. Perform histogram equalization on the original image in the current frame information.

[0091] S43. Extract the pyramid of the image after histogram equalization, and then extract the FAST feature points of the previous frame image.

[0092] S44. Perform LK optical flow tracking on the FAST feature points of the previous frame image.

[0093] S45. Determine the number of feature points in the previous frame image. If the number of feature points in the previous frame image is less than 10, set it as tracking failure directly. If the number of tracking points meets the preset threshold, LK optical flow tracking is performed directly. At this time, the feature points of the original image are first distorted, and then the RANSAC algorithm is used to remove outliers. Set whether all feature points in the previous frame image are successfully tracked. Only points that simultaneously meet the requirements of optical flow tracking and RANSAC detection are marked as 1.

[0094] S5. Update the pose estimation of the current state based on the reprojection error between the pose estimation of the IMU prior and the feature points of the current sliding window keyframe. The pose estimation of the IMU prior is obtained from the IMU and is the initial pose estimation of the current frame of the image captured by the camera in the current scene.

[0095] Step S5 specifically includes the following steps:

[0096] S51. Determine whether the current frame is a key frame by judging the parallax. If the parallax between the new frame and the previous frame is large, the oldest frame is marginalized. If the parallax is small, the previous frame is marginalized.

[0097] S52. Add the prior of the new feature point to the IMU. According to the pose estimation of the IMU prior, add all the observations of a feature point to the triangulation calculation to obtain the latest observation frame.

[0098] S53. 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. At the beginning of each iteration, the pose estimation after the previous visual inertial system, i.e. the camera and IMU joint optimization system, is used as the initial value. The pose estimation of the current state is updated by recalculating the residual relationship through the reprojection error within a window.

[0099] S6. The pose estimation of the current state is updated using ESIKF to obtain the latest visual features;

[0100] Step S6 specifically includes the following steps:

[0101] S61. The plane of the current frame is calculated through parametric equations, and the plane of the current frame is matched with the plane points of the previous frame to obtain the observation of pose transformation, which is considered to be the same plane.

[0102] S62. Using the IMU integral as a prior, perform EKF update and update the covariance to obtain the optimal pose estimate of the error and the state vector. After solving the gain matrix K, obtain the posterior of the error amount, and then calculate its change. When the change is less than the threshold, it is judged to be converged.

[0103] S63. When ESIKF ends and exits, update the posterior covariance matrix to complete the pose estimation of the current state and update the state.

[0104] S64. Add some point clouds from the new frame to the kd tree and update the marker for finding it on the map in the next frame.

[0105] S65. Approximate the result through several iterations of ESIKF, and update the current sliding window with the latest ESIKF posterior.

[0106] S7. Perform feature matching between the latest visual features and the point cloud information of the current laser frame of the LiDAR with corner feature point cloud data to obtain the LiDAR odometry factor and the camera odometry factor.

[0107] S8. After optimizing the factor graph by adding the lidar odometer factor, camera odometer factor, and IMU pre-integration factor, a 3D map is created.

[0108] Add IMU pre-integration factors to the factor plot:

[0109] 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:

[0110]

[0111] 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.

[0112] 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.

[0113] Add camera odometry factor to factor graph: Consider 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:

[0114]

[0115]

[0116]

[0117] 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. Let the pixel of the l-th feature in the j-th image be the three-dimensional coordinate of the back projection onto the unit sphere using the camera intrinsic parameters. 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 from the world coordinate system to the IMU coordinate system is represented in three-dimensional coordinates. 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.

[0118] 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.

[0119] Add a lidar odometry factor to the factor plot: The lidar odometry factor is Where ΔT k,k+1 State node X k With X k+1 The relative transformation relationship between them.

[0120] Considering that using lidar and IMU in mine shafts can improve the accuracy of the entire system in unstructured environments, but this will increase the amount of computation, the implementation example performs key frame matching to achieve a balance between accuracy and computation, thus ensuring the accuracy and real-time performance of the system.

[0121] The SLAM method using LiDAR, camera, and IMU combines the point cloud features of LiDAR with the IMU data and the feature information extracted and optimized by the camera to improve the accuracy of the entire system in unstructured environments. However, this increases the computational load. Therefore, this embodiment performs keyframe matching to reduce the computational load of the entire system, achieving a balance between accuracy and computational load, and ensuring the accuracy and real-time performance of the system.

[0122] Step S8 specifically includes the following steps:

[0123] S81. The lidar odometer factor, camera odometer factor, and IMU pre-integration factor are added to the factor graph for optimization.

[0124] S82. Calculate the Jacobian matrices of the lidar odometer factor, camera odometer factor, and IMU pre-integration factor with respect to the state variables, and solve for the state variables using the Levenberg-Marquardt method.

[0125] S83. In the ROS system, the rgbdslam package is configured using the map_server server, and all the data after factor graph optimization are combined to perform 3D map modeling.

[0126] This embodiment uses lidar, cameras, and IMU to achieve high-precision positioning in coal mines and improves attitude perception of the underground environment. This enables high-precision positioning in coal mines while balancing accuracy and computational load, improving the accuracy and real-time performance of positioning and mapping in mine tunnels. It effectively solves the problem of difficult underground positioning and mapping in unstructured environments where GPS cannot be used.

[0127] This embodiment also provides a system for applying the SLAM method to the unstructured features of underground mines mentioned above. The system utilizes the optimized lidar odometry factors, camera odometry factors, and IMU pre-integration factors from the factor graph. It then uses the rgbdslam package configured on the map_server in the ROS system and combines all the optimized data to perform 3D map modeling, including:

[0128] Point cloud information acquisition module 100 is used to acquire point cloud information of the current laser frame of the lidar.

[0129] The feature extraction module 200 is used to calculate the curvature of each point in the current laser frame point cloud information of the lidar, and extract the corner features and planar point features of each point based on the curvature of each point.

[0130] The corner feature determination module 300 is used to extract information of the current frame through the camera, detect corners of the current frame using the FAST algorithm based on the information of the current frame, and determine whether it is a corner feature point.

[0131] Feature point tracking module 400 is used to track feature points of the current sliding window keyframe using the KLT optical flow tracking algorithm based on the information of the current frame.

[0132] The pose estimation update module 500 is used to update the pose estimation of the current state based on the reprojection error between the pose estimation of the IMU prior and the feature points of the current sliding window keyframe.

[0133] The visual feature extraction module 600 is used to update the state by estimating the pose of the current state using ESIKF to obtain the latest visual features.

[0134] The odometer factor generation module 700 is used to perform feature matching between the latest visual features and the point cloud information of the current laser frame of the lidar with corner feature point cloud data to generate lidar odometer factors and camera odometer factors.

[0135] The factor graph optimization module 800 is used to add the lidar odometer factor, camera odometer factor, and IMU pre-integration factor to the factor graph for optimization and then perform 3D map modeling.

[0136] In this embodiment, feature extraction of point cloud data is performed on the LiDAR, and the camera can also provide visual features to match the point cloud features, thereby optimizing the entire system and keeping it stable.

[0137] By employing lidar, cameras, and IMUs, high-precision positioning is achieved in underground coal mines, and attitude perception of the underground environment is improved. This enables high-precision positioning in underground coal mines while balancing accuracy and computational load, improving the accuracy and real-time performance of positioning and mapping in mine tunnels. It effectively solves the problem of difficult underground positioning and mapping in unstructured environments where GPS cannot be used.

[0138] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. Since the above embodiments are substantially similar to the method embodiments, their descriptions are relatively simple; relevant parts can be referred to the descriptions of the method embodiments.

[0139] 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 SLAM method for unstructured features in underground mines, characterized in that, The process includes the following: Obtain the point cloud information of the current laser frame from the lidar; Calculate the curvature of each point in the current laser frame point cloud information of the lidar, and extract the corner features and planar point features of each point based on the curvature of each point; The information of the current frame is extracted by the camera, and the corner points of the current frame are detected by the FAST algorithm based on the information of the current frame, and it is determined whether it is a corner feature point. Based on the information of the current frame, the KLT optical flow method is used to track the feature points of the current sliding window keyframe. The pose estimate of the current state is updated based on the reprojection error between the pose estimate from the IMU prior and the feature points tracking the current sliding window keyframe. The pose estimation of the current state is used to update the state and obtain the latest visual features. The latest visual features are matched with the point cloud information of the current laser frame of the lidar with corner feature point cloud data to obtain the lidar odometry factor and the camera odometry factor. After incorporating the lidar odometry factor, camera odometry factor, and IMU pre-integration factor into the factor graph for optimization, a 3D map model is created. In the step of updating the state by estimating the pose of the current state using ESIKF to obtain the latest visual features, specifically... The process includes the following: The plane of the current frame is calculated by parametric equations, and the plane of the current frame is matched with the plane points of the previous frame to obtain the same plane, which is the observation of pose transformation. Using the IMU integral as a priori, EKF update is performed and the covariance is updated to obtain the optimal pose estimate of the error and the state vector. When ESIKF ends and exits, the posterior covariance matrix is ​​updated to complete the pose estimation of the current state and update the state. Add some point clouds from the new frame to the kd tree and update the marker for finding it on the map in the next frame; The result is approximated by ESIKF through several iterations, and the current sliding window is updated with the latest ESIKF posterior.

2. The SLAM method according to claim 1, characterized in that: In the step of calculating the curvature of each point in the current laser frame point cloud information of the lidar, and extracting the corner features and planar point features of each point based on the curvature, specifically... The process includes the following: Subscribe to the point cloud information after motion distortion correction for the current laser frame; Traverse the effective point cloud after motion distortion correction of the current laser frame, extract several points before and after the current laser point from the effective point cloud and calculate their corresponding curvature; Establish a set of corner points and a set of planar points based on the curvature of several points before and after the current laser point; Extract the corner features of each point from the corner feature set, and extract the planar feature of each point from the planar feature set.

3. The SLAM method according to claim 1, characterized in that: In the step of using the FAST algorithm to detect corner points in the current frame based on the information of the current frame, and determining whether it is a corner feature point, specifically... The process includes the following: Subscribe to information about the current frame extracted by the current camera; Select a pixel from the information in the current frame and determine whether the pixel is a corner feature point.

4. The SLAM method according to claim 3, characterized in that: In the step of selecting a pixel from the information of the current frame and determining whether the pixel is a corner feature point, specifically... Including the following process: Selected pixels Set the brightness value to The brightness threshold is set to ; In pixels Define a discretized Bresenham circle with a radius of 3 pixels centered on the point, and the boundary of the Bresenham circle has 16 pixels. Given a Bresenham circle of size 16 pixels with 9 consecutive pixels, if the pixel value of the 9 consecutive pixels is greater than or less than 9, then the following condition is met: Then determine the pixel It is a corner feature point; If the pixel value of 9 consecutive pixels is equal to Then determine the pixel It is not a corner feature point.

5. The SLAM method according to claim 1, characterized in that: In the step of tracking feature points of the current sliding window keyframe using the KLT optical flow algorithm based on the information of the current frame, specifically... The process includes the following: Subscribe to information about the current frame extracted by the current camera; Perform histogram equalization on the original image in the current frame information; Extract the pyramid of the histogram-equalized image, and then extract the FAST feature points of the previous frame image. LK optical flow tracking is performed on the FAST feature points of the previous frame image; Determine the number of feature points in the previous frame. If the previous frame has fewer than 10 feature points, set the tracking as failed. If the number of tracking points meets the preset threshold, LK optical flow tracking is performed directly.

6. The SLAM method according to claim 1, characterized in that: In the step of updating the pose estimation of the current state, specifically... The process includes the following: The method of marginalization is determined by the parallax. If the parallax between the new frame and the previous frame is large, the oldest frame is marginalized; if the parallax is small, the previous frame is marginalized. 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. Establish a residual relationship by reprojecting the IMU pose estimation of the latest observation frame of a feature point with the oldest frame and the two frames corresponding to the oldest frame in the sliding window.

7. The SLAM method according to claim 1, characterized in that: In the step of adding LiDAR odometry factors, camera odometry factors, and IMU pre-integration factors to the factor map for optimization and then performing 3D map modeling, specifically... The process includes the following: The lidar odometry factor, camera odometry factor, and IMU pre-integration factor were added to the factor graph for optimization. Calculate the Jacobian matrices of the lidar odometer factor, camera odometer factor, and IMU pre-integration factor with respect to the state variables, and solve the state variables using the Levenberg-Marquardt method. 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.

8. A system for implementing the SLAM method for unstructured features in underground mines as described in any one of claims 1-7, comprising: optimizing the lidar odometry factor, camera odometry factor, and IMU pre-integration factor using a factor graph; and performing 3D map modeling by configuring the rgbdslam package in a ROS system using a map_server and combining all the optimized data; characterized in that... include: Point cloud information acquisition module (100), the point cloud information acquisition module (100) is used to acquire the point cloud information of the current laser frame of the lidar; The feature extraction module (200) is used to calculate the curvature of each point in the current laser frame point cloud information of the lidar, and extract the corner features and planar point features of each point based on the curvature of each point; Corner feature judgment module (300), the corner feature judgment module (300) is used to extract information of the current frame through the camera, use the FAST algorithm to detect the corner of the current frame according to the information of the current frame, and determine whether it is a corner feature point; Feature point tracking module (400), the feature point tracking module (400) is used to track the feature points of the current sliding window key frame according to the information of the current frame using the KLT optical flow tracking algorithm; Pose estimation update module (500), the pose estimation update module (500) is used to update the pose estimation of the current state according to the reprojection error between the pose estimation of the IMU prior and the feature points of the current sliding window keyframe. The visual feature extraction module (600) is used to update the state by estimating the pose of the current state using ESIKF to obtain the latest visual features. The odometer factor generation module (700) is used to perform feature matching between the latest visual features and the point cloud information of the current laser frame of the lidar with corner feature point cloud data to generate lidar odometer factor and camera odometer factor. The factor graph optimization module (800) is used to add the lidar odometer factor, camera odometer factor and IMU pre-integration factor to the factor graph optimization and then perform 3D map modeling.

Images