An adaptive search laser SLAM method based on information entropy

Abstract

The present invention relates to the field of SLAM technology, and specifically to an adaptive search laser SLAM method based on information entropy. The method comprises the following steps: step 1, initializing the laser radar; step 2, obtaining and preprocessing the initial point cloud data; step 3, adaptive search scan matching based on information entropy; step 4, updating the grid map and iterative cycle. The present invention dynamically identifies high-information areas through the point cloud density information of the grid map, thereby improving the accuracy and efficiency of matching; adjusting the search window size in different entropy value areas, effectively allocating computing resources, and avoiding wasting time in low-feature areas; the method utilizes the point cloud density distribution of the grid map, dynamically calculates the information entropy, and adaptively adjusts the search strategy, thereby realizing an efficient and accurate scan matching process. Guided by the information entropy, the matching algorithm prioritizes feature-rich areas, while avoiding the computational waste caused by the fixed search window, thereby meeting the high-precision positioning requirements of the SLAM front end.

Description

Technical Field

[0001] The invention belongs to the technical field of SLAM, in particular to an adaptive search laser SLAM method based on information entropy. Background Art

[0002] With the rapid development of mobile robot technology, the application of robots in various fields has gradually become popular, from warehousing, logistics, service industry to manufacturing, medical treatment, agriculture and other fields. Mobile robots are gradually becoming key equipment in intelligent and automated scenes. Whether mobile robots can achieve efficient and stable autonomous navigation and positioning directly depends on the autonomous positioning and mapping (SLAM) technology they use.

[0003] SLAM technology refers to the process in which a robot obtains environmental information through its own sensors (such as lidar, camera, etc.) in an unknown or partially known environment, and simultaneously estimates its own posture and builds an environmental map. SLAM technology enables robots to move autonomously in complex environments, obtain map information, and plan paths. It has important theoretical research value and broad application prospects.

[0004] Among the many SLAM technologies, SLAM based on LiDAR has the advantages of high precision, good stability, and little influence by light, making it the mainstream choice for mobile robot applications. The laser SLAM system mainly relies on LiDAR sensors to obtain point cloud data of the surrounding environment, and estimates the robot's posture changes through matching between continuous point clouds, and gradually builds an environmental map.

[0005] The workflow of the laser SLAM system usually includes two key parts: front-end matching, which uses the scan matching method to achieve pose estimation between consecutive laser frames to ensure real-time positioning; back-end optimization, which uses graph optimization and other methods to adjust the global pose, correct accumulated errors, and improve the overall positioning and mapping accuracy. Front-end matching is the basis of the entire laser SLAM system, and its results directly affect the accuracy of back-end optimization. If errors or drifts occur in the front-end matching, it will be difficult for the back-end optimization to achieve global correction. Therefore, improving the accuracy and efficiency of the scan matching algorithm is the focus of current laser SLAM technology research.

[0006] In the laser SLAM system, the core of the front-end matching is laser point cloud registration, that is, by comparing the spatial relationship between the current frame point cloud and the reference frame (or map) point cloud, the current posture of the robot is calculated. However, mobile robots usually use motion models (such as odometers) to predict posture, but there is a certain degree of uncertainty and noise in the motion model, resulting in errors between the predicted posture and the actual posture. The matching results obtained by point cloud registration often depend on the estimated posture of the previous frame. If there is a matching error in the previous frame and it is not corrected in time, the error will be accumulated continuously, eventually leading to positioning drift.

[0007] In order to improve the matching accuracy, a large amount of calculation is usually required in the search space, especially in high-resolution point clouds, to perform matching scores for each possible pose candidate solution, which is time-consuming and not conducive to real-time requirements. Summary of the invention

[0008] To solve the above problems, the present invention provides an adaptive search laser SLAM method based on information entropy, which can reduce errors and improve the positioning accuracy of laser radar and the positioning capability of SLAM front end.

[0009] The technical solution of the present invention is described as follows in conjunction with the accompanying drawings:

[0010] An adaptive search laser SLAM method based on information entropy comprises the following steps:

[0011] Step 1: Initialize the laser radar;

[0012] Step 2: Obtain initial point cloud data and preprocess it;

[0013] Step 3: Adaptive search scan matching based on information entropy;

[0014] Step 4: Update the grid map and iterate the loop.

[0015] Furthermore, the specific method of step one is as follows:

[0016] 11) Start the LiDAR and ensure that the device is in normal working condition, that the LiDAR sensor is connected normally, that the power supply is stable, and that data collection is correct;

[0017] 12) According to the technical specifications of the LiDAR, configure the resolution and scanning frequency to ensure that the LiDAR can provide point cloud data with appropriate density and accuracy;

[0018] 13) Perform external parameter calibration to determine the external parameter transformation relationship between the laser radar coordinate system and the robot coordinate system.

[0019] Furthermore, the specific method of step 2 is as follows:

[0020] 21) At the initial position of the robot, start the laser radar device to scan and generate point cloud data M t ;

[0021] 22) Preprocess the original point cloud to remove outliers;

[0022] 23) Calculate the initial pose P0 and build the surrounding map.

[0023] Further, the specific method of step 22) is as follows:

[0024] 221) For each point in the point cloud, first find the nearest n neighboring points, calculate the mean and standard deviation of the Euclidean distance from each point in the point cloud to the neighboring points, assuming that there are points p in the point cloud i , for each point p i :

[0025]

[0026] Where n is the number of neighborhood points; d ij For point p i The distance to the jth neighboring point;

[0027] 222) Compare the mean and standard deviation of the distance from each point to the neighboring points. If the distance exceeds the set range, the point is considered a noise point and is removed. The standard deviation of the neighborhood distance is:

[0028]

[0029] If the distance d ij <μ i -ασ i or ij >μ i +ασ i , α=1.0, then the point is considered as a noise point and is removed.

[0030] Further, the specific method of step 23) is as follows:

[0031] 231) The initial pose P0 is calculated using the correlation scan matching algorithm as shown below:

[0032] 2311) discretize the search range into a set of candidate poses, with a search step of 0.05 meters and a search angle of 0.017 radians;

[0033] 2312) For each candidate pose, project the current laser scanning point cloud onto the grid map according to the candidate pose;

[0034] 2313) Calculate the matching score between the projection point and the map:

[0035]

[0036] Where K is the number of points contained in the input point cloud; s k is the position of the kth point in the input point cloud; P T is the position of the robot on the map; U n is the grid value of the corresponding position of the point in the grid map;

[0037] 2314) traverse all candidate poses and select the pose with the highest matching score as the initial pose P0;

[0038] 232) Construct a surrounding map as follows:

[0039] 2321) Determine the resolution of the map, that is, the actual physical size represented by each grid is 0.05 meters, and initialize the occupancy probability of each grid to an unknown state;

[0040] 2322) converting the lidar data from the sensor coordinate system to the global map coordinate system;

[0041] 2323) The path from the robot position to the laser point is projected as free space, and the grid where the laser point itself is located is marked as occupied;

[0042] 2324) Use the Bayesian update formula to update the occupancy probability of each grid:

[0043]

[0044] In the formula, P(O ij ) is the prior occupancy probability of the grid; P(S|O ij ) is the likelihood of the LiDAR scan data;

[0045] 2325) outputs a final grid map where each grid stores an occupancy probability or state.

[0046] Furthermore, the specific method of step three is as follows:

[0047] 31) Projecting the laser point cloud into a two-dimensional plane and dividing it into grid cells of fixed size;

[0048] 32) Calculate the density of the laser point cloud in each grid cell to measure the feature richness of the grid;

[0049] 33) Based on the point cloud density distribution, the information entropy formula is used to calculate the entropy value of each grid:

[0050] H i =-(p i logp i +(1-p i )log(1-pi ))

[0051] In the formula, H i is the information entropy of the i-th grid cell; p i Normalize the probability of point cloud density in the grid: n i is the point cloud density in the i-th grid cell, n max It is the maximum value of the point cloud density of all grid cells in the entire grid map;

[0052] 34) Design an adaptive search window to prioritize the search for high entropy areas, i.e., H i ≥0.5 and narrow the search window to perform fine matching, and finally search the low entropy area, i.e., H i <0.5, expand the search window and perform a rough match;

[0053] 35) In the search window W, the matching score between the current frame point cloud and the grid map is calculated by the correlation scanning matching algorithm, as shown below:

[0054]

[0055] Where K is the number of points contained in the input point cloud; s k is the position of the kth point in the input point cloud; P T is the position of the robot on the map; U n is the grid value of the corresponding position of the point in the grid map, scroe(P T ) is a scoring function used to evaluate the pose P T score;

[0056] 36) Select the best pose candidate solution based on the matching score. Based on the candidate pose with the highest matching score, use the optimal pose result of the current frame point cloud as the robot's position estimate at the current moment, as shown below:

[0057]

[0058] Where W is the search window; P ξ is a pose in W.

[0059] Furthermore, the specific method of step 4 is as follows:

[0060] 41) Project the current frame point cloud onto the grid map and update the point cloud density and occupancy probability distribution of the grid cells;

[0061] 42) Repeat step 3 and step 41) to perform scan matching and map update frame by frame.

[0062] The beneficial effects of the present invention are:

[0063] The present invention uses the point cloud density information of the grid map to dynamically identify high-information areas and improve the matching accuracy and efficiency; adjusts the search window size in different entropy value areas to effectively allocate computing resources and avoid wasting time in low-feature areas; this method uses the point cloud density distribution of the grid map to dynamically calculate information entropy and adaptively adjust the search strategy to achieve an efficient and accurate scanning and matching process. Guided by information entropy, the matching algorithm prioritizes feature-rich areas, while avoiding the computational waste caused by a fixed search window, meeting the high-precision positioning requirements of the SLAM front end. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0065] Figure 1 A schematic diagram of a process of an adaptive search laser SLAM method based on information entropy according to the present invention;

[0066] Figure 2 Schematic diagram of point cloud comparison before and after preprocessing of the present invention;

[0067] Figure 3 Schematic diagram of the environment map constructed for the present invention. DETAILED DESCRIPTION

[0068] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the present invention, rather than to limit the present invention. It should also be noted that, for ease of description, only parts related to the present invention, rather than all structures, are shown in the accompanying drawings.

[0069] Embodiment 1

[0070] See also Figure 1 , this embodiment 1 provides an adaptive search laser SLAM method based on information entropy, comprising the following steps:

[0071] Step 1: Initialize the laser radar, as follows:

[0072] 11) Start the LiDAR and ensure that the device is in normal working condition, that the LiDAR sensor is connected normally, that the power supply is stable, and that data collection is correct;

[0073] 12) According to the technical specifications of the LiDAR, configure the resolution and scanning frequency to ensure that the LiDAR can provide point cloud data with appropriate density and accuracy;

[0074] 13) Perform external parameter calibration to determine the external parameter transformation relationship between the laser radar coordinate system and the robot coordinate system.

[0075] Step 2: Obtain the initial point cloud data and preprocess it, as follows:

[0076] 21) At the initial position of the robot, start the laser radar device to scan and generate point cloud data M t , the schematic diagram of point cloud comparison before and after preprocessing is as follows Figure 2 As shown;

[0077] 22) Preprocess the original point cloud to remove outliers, as follows:

[0078] 221) For each point in the point cloud, first find the nearest n neighboring points, calculate the mean and standard deviation of the Euclidean distance from each point in the point cloud to the neighboring points, assuming that there are points p in the point cloud i , for each point p i :

[0079]

[0080] Where n is the number of neighborhood points; d ij For point p i The distance to the jth neighboring point;

[0081] 222) Compare the mean and standard deviation of the distance from each point to the neighboring points. If the distance exceeds the set range, the point is considered a noise point and is removed. The standard deviation of the neighborhood distance is:

[0082]

[0083] If the distance d ij <μ i -ασ i or ij >μ i +ασ i , α=1.0, then the point is considered as a noise point and is removed.

[0084] 23) Calculate the initial pose P0 and construct the surrounding map M as follows:

[0085] 231) The initial pose P0 is calculated using the correlation scan matching algorithm as shown below:

[0086] 2311) discretize the search range into a set of candidate poses, with a search step of 0.05 meters and a search angle of 0.017 radians;

[0087] 2312) For each candidate pose, project the current laser scanning point cloud onto the grid map according to the candidate pose;

[0088] 2313) Calculate the matching score between the projection point and the map:

[0089]

[0090] Where K is the number of points contained in the input point cloud; s k is the position of the kth point in the input point cloud; P T is the position of the robot on the map; U n is the grid value of the corresponding position of the point in the grid map;

[0091] 2314) Traverse all candidate poses and select the pose with the highest matching score as the initial pose P0.

[0092] 232) See Figure 3 , the method of constructing the surrounding map (the laser radar scanning range is about 15 meters) is as follows:

[0093] 2321) Determine the resolution of the map, that is, the actual physical size represented by each grid is 0.05 meters, and initialize the occupancy probability of each grid to an unknown state;

[0094] 2322) converting the lidar data from the sensor coordinate system to the global map coordinate system;

[0095] 2323) The path from the robot position to the laser point is projected as free space, and the grid where the laser point itself is located is marked as occupied;

[0096] 2324) Use the Bayesian update formula to update the occupancy probability of each grid:

[0097]

[0098] P(O ij ) is the prior occupancy probability of the grid, P(S|O ij ) is the likelihood of the LiDAR scan data;

[0099] 2325) outputs a final grid map where each grid stores an occupancy probability or state.

[0100] Step 3: After the initial pose, adaptive search scan matching based on information entropy is performed as follows:

[0101] 31) Projecting the laser point cloud into a two-dimensional plane and dividing it into grid cells of fixed size;

[0102] 32) Calculate the density of the laser point cloud in each grid cell to measure the feature richness of the grid;

[0103] 33) Based on the point cloud density distribution, the information entropy formula is used to calculate the entropy value of each grid:

[0104] H i =-(p i logp i +(1-p i )log(1-p i ))

[0105] In the formula, H i is the information entropy of the i-th grid cell; p i Normalize the probability of point cloud density in the grid: n i is the point cloud density in the i-th grid cell, n max It is the maximum value of the point cloud density of all grid cells in the entire grid map;

[0106] 34) Design an adaptive search window to prioritize the search for high entropy areas, i.e., H i ≥0.5 and narrow the search window to perform fine matching (step size 0.05m, angle 0.017 radians), and finally search the low entropy area, i.e. H i <0.5, expand the search window and perform a rough match (10 times the fine match);

[0107] 35) In the search window W, the matching score between the current frame point cloud and the grid map is calculated by the correlation scanning matching algorithm, as shown below:

[0108]

[0109] Where K is the number of points contained in the input point cloud; s k is the position of the kth point in the input point cloud; P T is the position of the robot on the map; U n is the grid value of the corresponding position of the point in the grid map, scroe(P T ) is a scoring function used to evaluate the pose P T score;

[0110] 36) Select the best pose candidate solution based on the matching score. Based on the candidate pose with the highest matching score, use the optimal pose result of the current frame point cloud as the robot's position estimate at the current moment, as shown below:

[0111]

[0112] Where W is the search window; P ξ is a pose in W.

[0113] Step 4: Update the grid map and iterate the loop, as follows:

[0114] 41) Project the current frame point cloud onto the grid map and update the point cloud density and occupancy probability distribution of the grid cells;

[0115] 42) Repeat step 3 and step 41) to perform scan matching and map update frame by frame.

[0116] In summary, this method utilizes the point cloud density distribution of the raster map, dynamically calculates the information entropy and adaptively adjusts the search strategy, realizing an efficient and accurate scanning and matching process. Guided by the information entropy, the matching algorithm prioritizes point cloud-rich areas, while avoiding the computational waste caused by a fixed search window, effectively solving problems such as large matching errors, difficulty in matching low-feature areas, and high consumption of computing resources, thus meeting the high-precision positioning requirements of the SLAM front end.

[0117] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. An adaptive search laser SLAM method based on information entropy, characterized in that: The following steps are involved: Step 1: Initialize the laser radar; Step 2: Obtain initial point cloud data and preprocess it; Step 3: Adaptive search scan matching based on information entropy; Step 4: Update the grid map and iterate the loop.

2. An adaptive search laser SLAM method based on information entropy according to claim 1, characterized in that: The specific method of step one is as follows: 11) Start the LiDAR and ensure that the device is in normal working condition, that the LiDAR sensor is connected normally, that the power supply is stable, and that data collection is correct; 12) According to the technical specifications of the LiDAR, configure the resolution and scanning frequency to ensure that the LiDAR can provide point cloud data with appropriate density and accuracy; 13) Perform external parameter calibration to determine the external parameter transformation relationship between the laser radar coordinate system and the robot coordinate system.

3. An adaptive search laser SLAM method based on information entropy according to claim 1, characterized in that: The specific method of step 2 is as follows: 21) At the initial position of the robot, start the laser radar device to scan and generate point cloud data M t ; 22) Preprocess the original point cloud to remove outliers; 23) Calculate the initial pose P0 and build the surrounding map.

4. An adaptive search laser SLAM method based on information entropy according to claim 3, characterized in that: The specific method of step 22) is as follows: 221) For each point in the point cloud, first find the nearest n neighboring points, calculate the mean and standard deviation of the Euclidean distance from each point in the point cloud to the neighboring points, assuming that there are points p in the point cloud i , for each point p i : Where n is the number of neighborhood points; d ij For point p i The distance to the jth neighboring point; 222) Compare the mean and standard deviation of the distance from each point to the neighboring points. If the distance exceeds the set range, the point is considered a noise point and is removed. The standard deviation of the neighborhood distance is: If the distance d ij <μ i -ασ i or ij >μ i +ασ i , α=1.0, then the point is considered as a noise point and is removed.

5. An adaptive search laser SLAM method based on information entropy according to claim 3, characterized in that: The specific method of step 23) is as follows: 231) The initial pose P0 is calculated using the correlation scan matching algorithm as shown below: 2311) discretize the search range into a set of candidate poses, with a search step of 0.05 meters and a search angle of 0.017 radians; 2312) For each candidate pose, project the current laser scanning point cloud onto the grid map according to the candidate pose; 2313) Calculate the matching score between the projection point and the map: Where K is the number of points contained in the input point cloud; s k is the position of the kth point in the input point cloud; P T is the position of the robot on the map; U n is the grid value of the corresponding position of the point in the grid map; 2314) traverse all candidate poses and select the pose with the highest matching score as the initial pose P0; 232) Construct a surrounding map as follows: 2321) Determine the resolution of the map, that is, the actual physical size represented by each grid is 0.05 meters, and initialize the occupancy probability of each grid to an unknown state; 2322) converting the lidar data from the sensor coordinate system to the global map coordinate system; 2323) The path from the robot position to the laser point is projected as free space, and the grid where the laser point itself is located is marked as occupied; 2324) Use the Bayesian update formula to update the occupancy probability of each grid: In the formula, P(O ij ) is the prior occupancy probability of the grid; P(S|O ij ) is the likelihood of the LiDAR scan data; 2325) outputs a final grid map where each grid stores an occupancy probability or state.

6. An adaptive search laser SLAM method based on information entropy according to claim 1, characterized in that: The specific method of step three is as follows: 31) Projecting the laser point cloud into a two-dimensional plane and dividing it into grid cells of fixed size; 32) Calculate the density of the laser point cloud in each grid cell to measure the feature richness of the grid; 33) Based on the point cloud density distribution, the information entropy formula is used to calculate the entropy value of each grid: H i =-(p i logp i +(1-p i )log(1-p i )) In the formula, H i is the information entropy of the i-th grid cell; p i Normalize the probability of point cloud density in the grid: n i is the point cloud density in the i-th grid cell, n max It is the maximum value of the point cloud density of all grid cells in the entire grid map; 34) Design an adaptive search window to prioritize the search for high entropy areas, i.e., H i ≥0.5 and narrow the search window to perform fine matching, and finally search the low entropy area, i.e., H i <0.5, expand the search window and perform a rough match; 35) In the search window W, the matching score between the current frame point cloud and the grid map is calculated by the correlation scanning matching algorithm, as shown below: Where K is the number of points contained in the input point cloud; s k is the position of the kth point in the input point cloud; P T is the position of the robot on the map; U n is the grid value of the corresponding position of the point in the grid map, scroe(P T ) is a scoring function used to evaluate the pose P T score; 36) Select the best pose candidate solution based on the matching score. Based on the candidate pose with the highest matching score, use the optimal pose result of the current frame point cloud as the robot's position estimate at the current moment, as shown below: Where W is the search window; P ξ is a pose in W.

7. An adaptive search laser SLAM method based on information entropy according to claim 1, characterized in that: The specific method of step 4 is as follows: 41) Project the current frame point cloud onto the grid map and update the point cloud density and occupancy probability distribution of the grid cells; 42) Repeat step 3 and step 41) to perform scan matching and map update frame by frame.

Images