An improved laser SLAM method for coal mine rescue robots

Abstract

The present invention discloses an improved laser SLAM method for a coal mine rescue robot, comprising: using a coal mine rescue robot sensor module to obtain underground rescue environment point cloud data; completing downsampling of the current frame point cloud; performing inter-frame registration and registration optimization; eliminating the cumulative error of the pose estimation of the front-end odometer module to achieve globally consistent robot pose estimation and accurate mapping. The method of the present invention uses only a single laser radar sensor for underground rescue environment perception, avoiding the data inaccuracy problem caused by the use of multiple sensors such as IMU in the bumpy underground environment, using an adaptive filtering method to maintain a relatively stable number of points to be matched and an improved loop detection method to improve the accuracy of loop detection and correct the front-end odometer pose. The present invention enables the rescue robot to achieve high-precision positioning and mapping in underground rescue environments that are challenging for odometers.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot SLAM algorithms, and in particular to a laser SLAM method for a coal mine rescue robot. Background Art

[0002] In recent years, Shan T has proposed the LEGO-LOAM laser SLAM algorithm, addressing the shortcomings of the LOAM algorithm's feature extraction method. This algorithm achieves high-precision positioning and mapping by filtering out outliers using a BFS algorithm and employing a ground point optimization method. Chen K proposed a nano-gicp-based SLAM algorithm for challenging environments such as coal mine tunnels. However, it lacks back-end optimization and cannot achieve globally consistent pose estimation. Xu W et al. proposed FAST-LIO, a SLAM algorithm that tightly couples lidar feature points and an IMU, using an iterative extended Kalman filter. Experiments have demonstrated that this algorithm boasts high computational efficiency and stable performance.

[0003] The above algorithms are mainly targeted at urban and outdoor scenes. The underground coal mine rescue environment has weak textures, long tunnels with inconsistent narrowness, and self-repeating scenes with multiple environmental features, which pose great challenges to the above positioning algorithms and are prone to positioning drift or even failure.

[0004] Chinese patent application publication number CN115290073A proposes a coal mine underground SLAM method based on multi-sensor fusion, using factor graph optimization to optimize the lidar odometry factor, camera odometry factor, and IMU pre-integration factor. However, the underground coal mine rescue environment has bumpy roads, and the use of multi-sensor fusion methods such as IMUs is prone to data inaccuracy. The limited power supply of rescue robots and the low computing power of industrial computers pose a great challenge to the positioning algorithm, which is prone to positioning drift or even failure. Summary of the Invention

[0005] To solve the above problems, the present invention proposes an improved laser SLAM method for coal mine rescue robots, which can solve the problem that the current mainstream laser SLAM method is prone to positioning drift or even failure in the coal mine underground rescue environment.

[0006] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0007] An improved laser SLAM method for a coal mine rescue robot first installs a sensor module, a point cloud preprocessing module, a front-end odometer module, and a loop detection optimization module on the coal mine rescue robot. The specific method includes the following steps:

[0008] Step S1: Use the 16-line laser radar in the coal mine rescue robot sensor module to obtain point cloud data of the underground rescue environment;

[0009] Step S2: Use the point cloud preprocessing module to complete the downsampling of the current frame point cloud;

[0010] Step S3: Use the front-end odometry module to perform inter-frame registration (scan-to-scan) and registration optimization (scan-to-map);

[0011] Step S4: Use the loop closure detection optimization module to eliminate the cumulative error of the pose estimation of the front-end odometer module to achieve globally consistent robot pose estimation and accurate mapping.

[0012] Among them, in step S1, the sensor module is responsible for connecting to the external laser radar sensor and inputting point cloud data to the point cloud preprocessing module.

[0013] The point cloud preprocessing module in step S2 is used to downsample the point cloud of the current frame and extract the point cloud to be matched in the current frame;

[0014] In step S3, the front-end odometry module is used to perform inter-frame registration (scan-to-scan) and registration optimization (scan-to-map) based on the GICP registration method according to the point cloud after downsampling of the current frame.

[0015] The loop detection optimization module in step S4 eliminates the cumulative error of the pose estimation of the front-end odometer module to achieve globally consistent robot pose estimation and accurate mapping.

[0016] Preferably, in step S1, the sensor module only uses a 16-line lidar sensor, which can avoid the inaccuracy problem of the rescue robot equipped with multiple sensors such as IMU in the bumpy environment of the coal mine and effectively alleviate the problems of small power distribution capacity and low computing power of the industrial computer of the coal mine rescue robot.

[0017] Preferably, step S2 uses the point cloud preprocessing module to adaptively adjust the voxel filter resolution according to the narrowness of the underground tunnel to complete the downsampling of the current frame point cloud, so as to avoid the problem that the fixed resolution voxel filtering method extracts a small number of point clouds in narrow tunnels, which easily causes the rescue robot to fail in positioning. Specifically, it includes:

[0018] Step S21: Use the RANSCA algorithm to remove external points, i.e. invalid points, from the point cloud of the current frame;

[0019] Step S22: Calculate the center of mass position p of the point cloud of the current frame according to the following formula:

[0020]

[0021] Among them, n represents the number of point clouds in the current frame, p i Represents a point in the point cloud.

[0022] Step S23: Decentralize the current frame point cloud and calculate the covariance matrix M of the current frame point cloud according to the following formula:

[0023]

[0024] Step S24: Obtain the eigenvalue λ of the covariance matrix M i ,i∈[0,3], and λ0≥λ1≥λ2;

[0025] Step S25: Obtain the “width” ω of the current frame point cloud according to the following formula:

[0026]

[0027] Step S26: Obtain the voxel filtering resolution of the current frame point cloud according to the size of ω, and then perform adaptive voxel filtering on the current frame point cloud according to the resolution size to obtain the downsampled point cloud X;

[0028] Preferably, step S3 uses the front-end odometry module to perform inter-frame registration (scan-to-scan) and registration optimization (scan-to-map) on the point cloud after downsampling of the current frame based on the GICP registration method.

[0029] Preferably, step S4 uses the loop detection optimization module to use a loop detection method that integrates key frame poses and Scan Context descriptors to solve the problem that the single loop detection method based on Scan Context descriptors is prone to false loops due to the large number of self-repeating scenes in the coal mine rescue environment, and eliminates the cumulative error of the pose estimation of the front-end odometer module, specifically including:

[0030] Step S41: Get the current posture according to the following formula: W T i Compared with the previous keyframe pose The change in posture △ W T ik When it exceeds a certain threshold, the current frame becomes a key frame:

[0031]

[0032] Step S42: If the current frame meets the key frame condition, extract the Scan Context descriptor of the current frame. q , according to the following formula, select the descriptor library from the current position of the robot W T iThe ScanContext descriptors of the nearest certain number of other key frames are evaluated for similarity. If I q With the historical frame descriptor I c Satisfy the loop condition, that is, D(I q ,I c ) is less than a certain threshold.

[0033]

[0034] Among them, n s Indicates the number of regions where the direction angle is divided along the circumferential direction for the point cloud of the current frame. Represents one of the columns of the current frame descriptor, Represents one column of the historical frame descriptor, ||△t|| represents the difference between the two frames △ W T ij Length of the translation part.

[0035] Step S43: If q with I c If the loop condition is met, the current frame and the historical frames that meet the loop condition are registered between frames (scan-to-scan) using the ICP registration method to obtain the initial relative pose value. Then, the submap consisting of the current frame and some historical frames near the historical frame is registered and optimized (scan-to-map) to optimize the initial relative pose value.

[0036] Step S44: If q with I c Satisfy the loop condition, use the sum of squares of the distances between the corresponding point clouds after registration as the evaluation result. If the calculated result is less than a certain threshold, the registration result satisfies the constraint and the loop factor △ is obtained. W T ij , that is, the transformation matrix between the two frames, connecting the i node and the j node.

[0037] Step S45: If the current frame meets the key frame condition, the current key frame k is converted to i and the previous key frame k i-1 Relative posture Connect two adjacent nodes as a front-end odometry factor:

[0038]

[0039] Step S46: If the current frame meets the key frame condition, perform factor graph optimization and update the current key frame pose to obtain a globally consistent pose estimate, and output the aligned current frame point cloud.

[0040] The beneficial effects of the present invention are: an adaptive filtering algorithm is proposed to solve the problem that when the rescue robot performs a task, the narrowness of the tunnels in the environment is not uniform. If voxel filtering with a fixed resolution size is used, a smaller number of point clouds will be generated in the narrower tunnels, which may easily cause the rescue robot to fail in positioning. A loop detection method that integrates key frame pose and ScanContext descriptor is proposed to solve the problem that when the rescue robot performs a task, there are many self-repeating scenes in the coal mine underground rescue environment, which causes a single loop detection method based on ScanContext descriptor to easily generate erroneous loops, and the problem that the front-end odometer of the rescue robot has a large cumulative pose estimation error during long-term and long-distance task execution. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0042] Figure 1 A flow chart of an improved laser SLAM method for a coal mine rescue robot provided by the present invention;

[0043] Figure 2 A flow chart of a loop detection method of an improved laser SLAM method for a coal mine rescue robot provided by the present invention;

[0044] Figure 3 This is the running result of the improved laser SLAM method for coal mine rescue robots provided by the present invention on the F, K, and N segments of the nebula odometry dataset. DETAILED DESCRIPTION

[0045] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0046] This example proposes an improved laser SLAM method for a coal mine rescue robot. First, a sensor module, a point cloud preprocessing module, a front-end odometry module, and a loop detection and optimization module are installed on the coal mine rescue robot. The specific steps include:

[0047] Step S1: Use the 16-line laser radar in the coal mine rescue robot sensor module to obtain point cloud data of the underground rescue environment;

[0048] Step S2: Use the point cloud preprocessing module to complete the downsampling of the current frame point cloud;

[0049] Step S3: Use the front-end odometry module to perform inter-frame registration and registration optimization;

[0050] Step S4: Use the loop closure detection optimization module to eliminate the cumulative error of the pose estimation of the front-end odometer module to achieve globally consistent robot pose estimation and accurate mapping.

[0051] like Figure 1 As shown, the laser SLAM method for a coal mine rescue robot provided by the present invention uses a sensor module, a point cloud preprocessing module, a front-end odometer module and a loop detection optimization module.

[0052] In step S1, the sensor module is responsible for accessing the external lidar sensor and inputting point cloud data to the point cloud preprocessing module.

[0053] The sensor module only uses a 16-line lidar sensor, which can avoid the inaccuracy problem of the rescue robot equipped with multiple sensors such as IMU in the bumpy environment of the coal mine and effectively alleviate the problems of small power distribution capacity and low computing power of the industrial computer of the coal mine rescue robot.

[0054] Step S2 uses the point cloud preprocessing module to downsample the point cloud of the current frame and extract the point cloud to be matched in the current frame;

[0055] Specifically, the point cloud preprocessing module is used to adaptively adjust the voxel filter resolution according to the narrowness of the underground tunnel to complete the downsampling of the current frame point cloud, so as to avoid the problem that the fixed-resolution voxel filter method extracts a small number of point clouds in narrow tunnels, which easily causes the rescue robot to fail in positioning. Specifically, the following steps are performed:

[0056] Step S21: Use the RANSCA algorithm to remove external points, i.e. invalid points, from the point cloud of the current frame;

[0057] Step S22: Calculate the centroid position of the current frame point cloud according to the following formula:

[0058]

[0059] Among them, n represents the number of point clouds in the current frame, p i Represents a point in the point cloud.

[0060] Step S23: Decentralize the current frame point cloud and calculate the covariance matrix M of the current frame point cloud according to the following formula:

[0061]

[0062] Step S24: Obtain the eigenvalue λ of the covariance matrix M i ,i∈[0,3], and λ0≥λ1≥λ2;

[0063] Step S25: Obtain the “width” ω of the current frame point cloud according to the following formula:

[0064]

[0065] Step S26: The voxel filter resolution of the current frame point cloud is obtained based on the size of ω, and then the current frame point cloud is adaptively voxel filtered based on the resolution to obtain the downsampled point cloud X. The relationship between the "width" used in this example and the voxel filter resolution is shown in the following formula:

[0066]

[0067] In step S3, the front-end odometry module is used to perform inter-frame registration (scan-to-scan) and registration optimization (scan-to-map) based on the point cloud after downsampling of the current frame based on the GICP registration method.

[0068] Step S4 uses the loop detection optimization module to eliminate the cumulative error of the pose estimation of the front-end odometer module to achieve globally consistent robot pose estimation and accurate mapping.

[0069] See also Figure 2 Specifically, the loop detection optimization module uses a loop detection method that integrates keyframe poses and ScanContext descriptors to solve the problem that the single loop detection method based on ScanContext descriptors is prone to false loops due to the large number of self-repeating scenes in the coal mine rescue environment, and eliminates the accumulated error in the pose estimation of the front-end odometer module. Specifically, the method includes:

[0070] Step S41: Get the current posture according to the following formula: W T i Compared with the previous keyframe pose The change in posture △ W T ik When it exceeds a certain threshold, the current frame becomes a key frame:

[0071]

[0072] In this example, the selected threshold is a translation change of 2.0 m or a rotation angle of 30°.

[0073] Step S42: If the current frame meets the key frame condition, extract the Scan Context descriptor of the current frame. q , according to the following formula, select the descriptor library from the current position of the robot W T i The ScanContext descriptors of a certain number of other key frames are used for similarity evaluation (in this example, 20 frames of other key frames are used for evaluation). q With the historical frame descriptor I c Satisfy the loop condition, that is, D(I q ,I c ) is less than a certain threshold (the threshold used in this example is 1.5, θ1=1, θ2=0.05).

[0074]

[0075] Among them, n s Indicates the number of regions where the direction angle is divided along the circumferential direction for the point cloud of the current frame. Represents one of the columns of the current frame descriptor, Represents one column of the historical frame descriptor, ||△t|| represents the difference between the two frames △ W T ij Length of the translation part.

[0076] Step S43: If q with I c If the loop condition is met, the current frame and the historical frames that meet the loop condition are registered using the ICP registration method (scan-to-scan) to obtain the initial relative pose value. Then, the submap consisting of the current frame and some historical frames near the historical frame is registered and optimized (scan-to-map) to optimize the initial relative pose value.

[0077] Step S44: If q with I c The loop closure condition is met, and the sum of square distances of the corresponding point clouds after registration is used as the evaluation result. If the calculated result is less than a certain threshold (the threshold set in this example is 0.3), the registration result meets the constraint and the loop closure factor △ is obtained. W T ij , that is, the transformation matrix between the two frames, connecting the i node and the j node;

[0078] Step S45: If the current frame meets the key frame condition, the current key frame k is converted to i and the previous key frame k i-1 Relative posture Connect two adjacent nodes as a front-end odometry factor:

[0079]

[0080] Step S46: If the current frame meets the key frame condition, perform factor graph optimization and update the current key frame pose to obtain a globally consistent pose estimate, and output the aligned current frame point cloud.

[0081] Figure 3 Figures ac in the middle show the positioning experiments of the method proposed in this paper on the F, K, and N segments of the nebula odometry dataset (long alley dataset).

[0082] Table 1 below shows the absolute pose error statistics of the laser SLAM method proposed in this paper, the LEGO-LOAM method, and the FAST-LIO method in the F, K, and N segments of the nebula odometry dataset.

[0083] Table 1 Statistics of absolute pose error

[0084]

[0085]

[0086] In summary, the present invention uses only lidar sensors to achieve high-precision positioning and mapping of tunnels and cave scenes, alleviating the problems of limited power supply and low computing power of industrial computers on rescue robots. It improves the robustness of underground rescue environments through adaptive filtering methods and improved loop detection methods, and achieves accurate positioning and mapping of underground rescue environments.

[0087] Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if such changes and modifications fall within the scope of the claims and their equivalents, the present invention is intended to include such changes and modifications.

Claims

1. An improved laser SLAM method for coal mine rescue robots, characterized by: The following steps are involved: Step S1: Acquire underground rescue environment point cloud data; Step S2: completing downsampling of the current frame point cloud; in step S2, a point cloud preprocessing module is installed on the coal mine rescue robot to adaptively adjust the voxel filter resolution size according to the narrowness of the underground tunnel to complete the downsampling of the current frame point cloud; Step S3: performing inter-frame registration and registration optimization; Step S4: Eliminate the accumulated error of the pose estimation of the front-end odometry module to achieve globally consistent robot pose estimation and accurate mapping; In step S4, a loop closure detection optimization module is installed on the coal mine rescue robot, and a loop closure detection method that integrates keyframe poses and ScanContext descriptors is used to eliminate the cumulative error of the pose estimation of the front-end odometer module, including the following steps: Step S41: Get the current posture according to the following formula: W T i Compared with the previous keyframe pose The pose change Δ W T ik , when Δ W T ik When it exceeds a certain threshold, the current frame becomes a key frame: Step S42: If the current frame meets the key frame condition, extract the Scan Context descriptor of the current frame. q , according to the following formula, select the descriptor library from the current position of the robot W T i The ScanContext descriptors of the nearest certain number of other key frames are evaluated for similarity. If I q With the historical frame descriptor I c Satisfy the loop condition, that is, D(I q ,I c ) is less than a certain threshold; Among them, n s Indicates the number of regions where the direction angle is divided along the circumferential direction for the point cloud of the current frame. Represents one of the columns of the current frame descriptor, Represents one column of the historical frame descriptor, ||Δt|| represents the pose difference Δ between two frames W T ij Length of translation part; Step S43: If q with I c If the loop condition is met, the current frame and the historical frames that meet the loop condition are registered using the ICP registration method to obtain the initial relative pose value. Then, the sub-map consisting of the current frame and some historical frames near the historical frame is registered to optimize the initial relative pose value. Step S44: If q with I c Satisfy the loop condition, use the sum of squares of the distances between the corresponding point clouds after registration as the evaluation result. If the calculated result is less than a certain threshold, the registration result satisfies the constraint and the loop factor Δ is obtained. W T ij , that is, the transformation matrix between the two frames, connecting the i node and the j node; Step S45: If the current frame meets the key frame condition, the current key frame k is converted to i and the previous key frame k i-1 Relative posture Connect two adjacent nodes as a front-end odometry factor: Step S46: If the current frame meets the key frame condition, perform factor graph optimization and update the current key frame pose to obtain a globally consistent pose estimate, and output the aligned current frame point cloud.

2. The improved laser SLAM method for a coal mine rescue robot according to claim 1, wherein: In step S1, a sensor module is installed on the coal mine rescue robot to obtain point cloud data of the underground rescue environment, specifically using a 16-line laser radar sensor.

3. The improved laser SLAM method for a coal mine rescue robot according to claim 2, wherein: In step S2, the following steps are specifically included: Step S21: Use the RANSCA algorithm to remove external points, i.e. invalid points, from the point cloud of the current frame; Step S22: Calculate the centroid position of the current frame point cloud according to the following formula: Among them, n represents the number of point clouds in the current frame, p i Represents a point in the point cloud; Step S23: Decentralize the current frame point cloud and calculate the covariance matrix M of the current frame point cloud according to the following formula: Step S24: Obtain the eigenvalue λ of the covariance matrix M i ,i∈[0,3], and λ0≥λ1≥λ2; Step S25: Obtain the "width" ω of the current frame point cloud according to the following formula: Step S26: Obtain the voxel filtering resolution of the current frame point cloud according to the size of ω, and then perform adaptive filtering on the current frame point cloud according to the resolution size to obtain the downsampled point cloud X.

4. The improved laser SLAM method for a coal mine rescue robot according to claim 3, wherein: In step S3, a front-end odometry module is installed on the coal mine rescue robot, and inter-frame registration and registration optimization are performed on the downsampled point cloud X based on the GICP registration method.

Images