A tightly coupled laser SLAM method and device based on redundant key frame removal

Abstract

This invention discloses a tightly coupled laser SLAM method and apparatus based on redundant keyframe removal, relating to the field of simultaneous localization and mapping (SLAM). The method includes: acquiring radar point cloud data; preprocessing the point cloud data to obtain preprocessed point cloud data; calculating the curvature of the preprocessed point cloud data to extract point cloud edge features and planar features; registering the point cloud edge features and planar features to obtain keyframes and outputting radar odometry; obtaining a dynamic threshold based on environmental information and filtering keyframes according to the dynamic threshold; performing loop closure detection on the filtered keyframes; and after loop closure detection, using the ERASOR algorithm to remove dynamic points, obtaining the tightly coupled laser simultaneous localization and mapping (SLAM) result based on redundant keyframe removal. This invention solves the problems of current SLAM algorithms having overly simplistic keyframe selection strategies, false loop closure detections, and excessive redundant keyframes leading to low algorithm accuracy and efficiency.

Description

Technical Field

[0001] This invention relates to the field of simultaneous localization and mapping (SLAM) technology, and in particular to a tightly coupled laser SLAM method and apparatus based on redundant keyframe removal. Background Technology

[0002] In this era of booming artificial intelligence technology, and with the rapid development of robotics, robots have gradually evolved from simply entering our lives to permeating them. Various service robots, both indoor and outdoor, have attracted widespread attention. In industrial production, mobile robots can replace humans in performing more complex and high-precision tasks, without being limited by physical strength, minimizing the impact of human factors and bringing greater convenience to production and daily life.

[0003] Whether indoors or outdoors, there are still many problems to be solved in the popularization of mobile robots, such as accurate map modeling, accurate positioning of the robot in the scene, and real-time path planning for the robot.

[0004] While GPS can be used for outdoor positioning and navigation, the situation becomes more complex indoors. Furthermore, GPS will fail in outdoor locations with no or poor signal. To address these issues, SLAM (Simultaneous Localization and Mapping) technology has emerged and gained significant attention. Clearly, SLAM technology consists of two parts: localization and mapping. It refers to the process by which an autonomous vehicle or other device equipped with specific sensors simultaneously builds an environmental model and estimates its own motion in an unfamiliar environment without prior information.

[0005] Compared to visual SLAM, laser SLAM technology is more mature and reliable. The introduction of the LOAM (Lidar Odometry and Mapping) algorithm laid the foundation for the development of laser SLAM. The LOAM framework is mainly divided into several modules: front-end radar odometry, back-end optimization, loop closure detection, and mapping and localization. By preprocessing the data and extracting features, the ICP (Iterative Closest Point) algorithm is used to register frames to obtain radar odometry. Back-end loop closure detection finds the same places the robot has passed through, constructs constraints for optimization, improves the correlation between current data and historical data, eliminates the cumulative error that occurs during the operation of the SLAM system, and constructs a globally consistent trajectory and map.

[0006] In laser SLAM, a common loop closure detection method involves constructing keyframes, storing the poses within these keyframes, and performing loop closure detection at a fixed frequency. Each time, the latest keyframe is processed, and a KD-Tree is used to find a keyframe from the historical keyframes that meets the distance and time requirements, thus establishing a loop closure. After the loop closure is established, a local map is constructed, and ICP matching is performed between the map and the current keyframe to solve for the pose transformation. Summary of the Invention

[0007] This invention addresses the problem of how to achieve a precise and efficient laser SLAM system.

[0008] To solve the above-mentioned technical problems, the present invention provides the following technical solution:

[0009] On one hand, the present invention provides a tightly coupled laser SLAM method based on redundant keyframe removal, which is implemented by an electronic device and includes:

[0010] S1. Acquire radar point cloud data, preprocess the point cloud data, and obtain preprocessed point cloud data.

[0011] S2. Perform curvature calculation on the preprocessed point cloud data to extract point cloud edge features and planar features.

[0012] S3. Register the point cloud edge features and planar features, obtain key frames, and output the radar odometer.

[0013] S4. Obtain dynamic thresholds through environmental information, and filter keyframes based on dynamic thresholds.

[0014] S5. Perform loop closure detection on the selected keyframes.

[0015] S6. After loop closure detection, the ERASOR algorithm is used to remove dynamic points, resulting in tightly coupled laser simultaneous localization and mapping (SLAM) results based on redundant keyframe removal.

[0016] Optionally, in S1, acquiring radar point cloud data and preprocessing the point cloud data to obtain preprocessed point cloud data includes:

[0017] S11. The inertial measurement unit (IMU) is pre-integrated to obtain the robot's pose change. Based on the pose change, the pose of each point in the point cloud data relative to the initial scan time is obtained, motion compensation is performed, and the IMU odometry is output.

[0018] S12. Perform ground point segmentation on the motion-compensated point cloud data and output the point cloud data after ground point segmentation.

[0019] Optionally, the curvature of the preprocessed point cloud data is calculated in S2, as shown in equation (1) below:

[0020]

[0021] Where S represents point A specified number of points nearby, Let L represent the coordinate system of the k-th scan, and let L be the position of the j-th point in the point cloud within L. Let L represent the coordinate system of the k-th scan, and let L be the position of the i-th point in the point cloud within L.

[0022] Optionally, after performing curvature calculation on the preprocessed point cloud data in S2, the following steps are also included:

[0023] Remove points that are parallel to the laser direction and points that are blocked.

[0024] After removing points, the point cloud is sorted according to the calculated curvature. Points with curvature greater than the threshold of 1 are designated as corner points, and points with curvature less than the threshold of 0.2 are designated as face points. Corner points and face points are stored and searched using an ikd-tree data structure.

[0025] Optionally, S3 performs registration of point cloud edge features and planar features to obtain keyframes and outputs radar odometry, including:

[0026] S31. Initialize the pose of the current frame and obtain the local map and the point cloud of the current frame.

[0027] S32. The pose of the current frame is matched and optimized by the nearest point iterative ICP algorithm.

[0028] S33. Determine whether the current frame after matching optimization is a key frame. If it is, perform factor graph optimization, update the pose of all variable nodes in the factor graph, and update the radar odometry trajectory.

[0029] Optionally, in S4, a dynamic threshold is obtained through environmental information, and keyframes are filtered based on the dynamic threshold, including:

[0030] S41. Obtain dynamic thresholds through environmental information.

[0031] S42. Obtain the number of features of corner points and face points in the current frame, and determine whether the number of features is greater than or equal to the dynamic threshold. If so, obtain the pose change of the robot in the current frame and the previous key frame. If the pose change exceeds the set pose change threshold, establish a new key frame; otherwise, end the judgment.

[0032] Optionally, the dynamic threshold in S41 is as shown in equation (2) below:

[0033]

[0034] Where, d key d represents the distance of historical keyframes. mid This represents the median depth of the laser points in the point cloud.

[0035] Optionally, loop closure detection is performed on the filtered keyframes in S5, including:

[0036] S51. Create a point cloud descriptor for each keyframe in the filtered keyframes.

[0037] S52. Perform point cloud descriptor calculation on the point cloud descriptor, extract an N-dimensional vector from the calculated point cloud descriptor, perform a search for similar keyframes based on the N-dimensional vector, compare the point cloud descriptor of the searched keyframe with the point cloud descriptor of the current keyframe, and if the comparison result is higher than the preset point cloud descriptor threshold, then a loop closure is found.

[0038] S53. After finding the loop, perform backend factor graph optimization.

[0039] Optionally, the ERASOR algorithm in S6 is used to remove dynamic points, including:

[0040] S61. Obtain the point cloud map containing dynamic objects, play each frame of the original point cloud in the point cloud map, divide the original point cloud and the corresponding sub-map into multiple grids in the same way, and calculate a descriptor representing the point cloud distribution for each grid.

[0041] S62. Obtain the ratio of the descriptor of the same cell in the original point cloud and the sub-image, and mark the cells with a ratio less than the preset descriptor threshold as suspected dynamic regions.

[0042] S63. Fit a ground plane in the suspected dynamic region and filter out the points on the ground plane as dynamic points.

[0043] On the other hand, the present invention provides a tightly coupled laser SLAM apparatus based on redundant keyframe removal, which is used to implement a tightly coupled laser SLAM method based on redundant keyframe removal. The apparatus includes:

[0044] The preprocessing module is used to acquire radar point cloud data, preprocess the point cloud data, and obtain preprocessed point cloud data.

[0045] The curvature calculation module is used to calculate the curvature of the preprocessed point cloud data and extract the edge and planar features of the point cloud.

[0046] The keyframe acquisition module is used to register point cloud edge features and planar features, acquire keyframes, and output radar odometry.

[0047] The filtering module is used to obtain dynamic thresholds through environmental information and filter keyframes based on these dynamic thresholds.

[0048] The loop closure detection module is used to perform loop closure detection on the filtered keyframes.

[0049] The output module is used to remove dynamic points after loop closure detection using the ERASOR algorithm, resulting in tightly coupled SLAM (Simultaneous Localization and Mapping) results based on redundant keyframe removal.

[0050] Optionally, the preprocessing module is further used for:

[0051] S11. The inertial measurement unit (IMU) is pre-integrated to obtain the robot's pose change. Based on the pose change, the pose of each point in the point cloud data relative to the initial scan time is obtained, motion compensation is performed, and the IMU odometry is output.

[0052] S12. Perform ground point segmentation on the motion-compensated point cloud data and output the point cloud data after ground point segmentation.

[0053] Optionally, the curvature of the preprocessed point cloud data is calculated as shown in equation (1):

[0054]

[0055] Where S represents point A specified number of points nearby, Let L represent the coordinate system of the k-th scan, and let L be the position of the j-th point in the point cloud within L. Let L represent the coordinate system of the k-th scan, and let L be the position of the i-th point in the point cloud within L.

[0056] Optionally, the curvature calculation module is further used for:

[0057] Remove points that are parallel to the laser direction and points that are blocked.

[0058] After removing points, the point cloud is sorted according to the calculated curvature. Points with curvature greater than the threshold of 1 are designated as corner points, and points with curvature less than the threshold of 0.2 are designated as face points. Corner points and face points are stored and searched using an ikd-tree data structure.

[0059] Optionally, the keyframe acquisition module is further used for:

[0060] S31. Initialize the pose of the current frame and obtain the local map and the point cloud of the current frame.

[0061] S32. The pose of the current frame is matched and optimized by the nearest point iterative ICP algorithm.

[0062] S33. Determine whether the current frame after matching optimization is a key frame. If it is, perform factor graph optimization, update the pose of all variable nodes in the factor graph, and update the radar odometry trajectory.

[0063] Optionally, the filtering module is further used for:

[0064] S41. Obtain dynamic thresholds through environmental information.

[0065] S42. Obtain the number of features of corner points and face points in the current frame, and determine whether the number of features is greater than or equal to the dynamic threshold. If so, obtain the pose change of the robot in the current frame and the previous key frame. If the pose change exceeds the set pose change threshold, establish a new key frame; otherwise, end the judgment.

[0066] Optionally, the dynamic threshold is as shown in equation (2):

[0067]

[0068] Where, d key d represents the distance of historical keyframes. mid This represents the median depth of the laser points in the point cloud.

[0069] Optionally, the loop closure detection module is further used for:

[0070] S51. Create a point cloud descriptor for each keyframe in the filtered keyframes.

[0071] S52. Perform point cloud descriptor calculation on the point cloud descriptor, extract an N-dimensional vector from the calculated point cloud descriptor, perform a search for similar keyframes based on the N-dimensional vector, compare the point cloud descriptor of the searched keyframe with the point cloud descriptor of the current keyframe, and if the comparison result is higher than the preset point cloud descriptor threshold, then a loop closure is found.

[0072] S53. After finding the loop, perform backend factor graph optimization.

[0073] Optionally, the output module is further used for:

[0074] S61. Obtain the point cloud map containing dynamic objects, play each frame of the original point cloud in the point cloud map, divide the original point cloud and the corresponding sub-map into multiple grids in the same way, and calculate a descriptor representing the point cloud distribution for each grid.

[0075] S62. Obtain the ratio of the descriptor of the same cell in the original point cloud and the sub-image, and mark the cells with a ratio less than the preset descriptor threshold as suspected dynamic regions.

[0076] S63. Fit a ground plane in the suspected dynamic region and filter out the points on the ground plane as dynamic points.

[0077] On one hand, an electronic device is provided, comprising a processor and a memory, wherein the memory stores at least one instruction, which is loaded and executed by the processor to implement the above-described tightly coupled laser SLAM method based on redundant keyframe removal.

[0078] On the one hand, a computer-readable storage medium is provided, wherein at least one instruction is stored in the storage medium, the at least one instruction being loaded and executed by a processor to implement the above-described tightly coupled laser SLAM method based on redundant keyframe removal.

[0079] The above technical solution has at least the following advantages compared with the existing technology:

[0080] The above scheme discloses a tightly coupled laser SLAM method based on redundant keyframe removal, which uses an adaptive threshold for keyframe selection. Compared with the method based on the robot moving beyond a set threshold, this strategy is more universal and can cope with different scenarios, reducing loop closure errors. On the other hand, it reduces the extraction of redundant keyframes, relieves memory pressure, improves the accuracy and speed of the algorithm, and effectively improves the real-time performance of the algorithm.

[0081] A ground segmentation module is added to the front end to reduce the computational cost of point cloud feature extraction and the impact of ground points on feature extraction and registration. An ikd-tree data structure is used to store corner and face points, improving accuracy and speed. A loop closure detection method using SC descriptors is added to the back end, resulting in higher efficiency and accuracy for loop closure detection, thus improving the system's precision and robustness. Finally, the ERASOR method is used to remove dynamic point clouds, optimizing the system's mapping performance. Attached Figure Description

[0082] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0083] Figure 1 This is a schematic diagram of the tightly coupled laser SLAM method based on redundant keyframe removal provided in an embodiment of the present invention;

[0084] Figure 2 This is a flowchart of a tightly coupled laser SLAM method based on redundant keyframe removal provided in an embodiment of the present invention;

[0085] Figure 3 This is a schematic diagram of the ground point extraction method provided in an embodiment of the present invention;

[0086] Figure 4 This is the keyframe filtering process provided in the embodiments of the present invention. Figure 1 ;

[0087] Figure 5 This is the keyframe filtering process provided in the embodiments of the present invention. Figure 2 ;

[0088] Figure 6 This is a flowchart of the loop closure detection provided in an embodiment of the present invention;

[0089] Figure 7 This is a block diagram of a tightly coupled laser SLAM device based on redundant keyframe removal provided in an embodiment of the present invention;

[0090] Figure 8 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present invention. Detailed Implementation

[0091] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the described embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0092] like Figure 1 As shown, this embodiment of the invention provides a tightly coupled laser SLAM method based on redundant keyframe removal, which can be implemented by an electronic device. Figure 1 The flowchart shown is for a tightly coupled laser SLAM method based on redundant keyframe removal. The processing flow of this method may include the following steps:

[0093] S1. Acquire radar point cloud data, preprocess the point cloud data, and obtain preprocessed point cloud data.

[0094] Optionally, step S1 above may include the following steps S11-S12:

[0095] S11. The inertial measurement unit (IMU) is pre-integrated to obtain the robot's pose change. Based on the pose change, the pose of each point in the point cloud data relative to the initial scan time is obtained, motion compensation is performed, and the IMU odometry is output.

[0096] In one feasible implementation, the point cloud data input from the radar is preprocessed, and the radar point cloud is distorted by IMU (Inertial Measurement Unit) data, while the IMU odometry is output.

[0097] The specific formula for IMU pre-integration in step S11 is as follows:

[0098] a = R bw (a t -g)+b a +n a (1)

[0099] ω=ω t +b ω +n ω (2)

[0100] Where a represents acceleration, ω represents angular velocity, and is affected by the slowly changing bias b and white noise n, R bw is the rotation matrix from the world coordinate system W to the robot's body coordinate system B, and g is the gravity vector fixed in the W coordinate system. The IMU can obtain the current angular velocity and acceleration values, which can be used to calculate the system state (position, velocity, attitude). For two consecutive keyframes b... k b k+1 The state transfer formula between them is as follows:

[0101]

[0102]

[0103]

[0104] On the one hand, IMU pre-integration constraints can be used in conjunction with other constraints to construct optimization problems. On the other hand, IMU pre-integration can remove motion distortion. The purpose of motion compensation is to reconcile all point clouds to a specific time point, thus unifying the point clouds collected over a past period to a single point in time. A common approach is to compensate to the initial time point, as follows:

[0105] P start =T start_current *P current (6)

[0106] Therefore, it is necessary to know the pose corresponding to each point at any given time. If a high-frequency odometry is available, the pose of each point relative to the start of the scan can be obtained relatively easily. If an IMU is available, the pose of each point relative to the starting point can be calculated directly. If no other sensors are available, a uniform motion model can be used, using the odometry result of the previous frame as the motion between the two frames. At the same time, assuming that the current frame is also a uniform motion, the pose of each point relative to the start time can also be estimated.

[0107] S12. Perform ground point segmentation on the motion-compensated point cloud data and output the point cloud data after ground point segmentation.

[0108] In one feasible implementation, ground point segmentation and dynamic point removal are performed on the point cloud data. Ground points do not participate in feature extraction, which reduces the amount of data processed during point cloud feature extraction and increases ground constraints to improve subsequent registration accuracy.

[0109] Specifically, IMU pre-integration also participates in the removal of dynamic points in the point cloud. The initial pose is obtained through IMU odometry, and the error between IMU pre-integration and frame matching is used to determine the initial resolution. This initial resolution is then used to construct range images from the current LiDAR scan and the corresponding sub-images. After constructing the range images, most of the dynamic points in the sub-images are removed by comparing their visibility.

[0110] Ground segmentation methods such as Figure 3 As shown, two adjacent scan beams in the same column hit points A and B on the map. By calculating their vertical height difference h and horizontal distance difference d, an angle is obtained. Ideally, the angle on a perfectly flat surface is 0 degrees. However, considering that the equipment and the ground are not perfectly level, this angle will be slightly greater than zero. On flat roads in cities, this value can be set to 5 degrees. The formulas for calculating the vertical height difference, horizontal distance difference, and angle are as follows:

[0111] h=|z0-z1| (7)

[0112]

[0113] θ=atan2(h,d) (9)

[0114] The calculated angle θ is compared with a threshold. If the angle is less than the threshold, the point is marked as a ground point.

[0115] S2. Perform curvature calculation on the preprocessed point cloud data to extract point cloud edge features and planar features.

[0116] In one feasible implementation, radar point cloud data with motion distortion removed and ground segmentation performed is first subscribed to, and then the curvature of each point in the point cloud data is calculated using the following formula:

[0117]

[0118] Where S represents point A specified number of points nearby, Let L represent the coordinate system of the k-th scan, and let L be the position of the j-th point in the point cloud within L. Let L represent the coordinate system of the k-th scan, and let L be the position of the i-th point in the point cloud within L.

[0119] Optionally, after performing curvature calculation on the preprocessed point cloud data in S2, the following steps are also included:

[0120] Remove points that are parallel to the laser direction and points that are blocked.

[0121] After removing points, the point cloud is sorted according to the calculated curvature. Points with curvature greater than the threshold of 1 are designated as corner points, and points with curvature less than the threshold of 0.2 are designated as face points. Corner points and face points are stored and searched using an ikd-tree data structure.

[0122] In one feasible implementation, after calculating the curvature, laser points parallel to the laser direction or those that are obscured are removed. First, the depth values ​​of the current point and the next point are extracted, and it is determined whether the two points are on the same ring (scan line). If the two points are on the same scan line, the depths of the two points are compared. If the point with the larger depth is obscured by the point with the smaller depth, the point with the larger depth is not included in subsequent feature extraction. Then, the depth difference between the current point and its two adjacent points is calculated, and it is determined whether the current point is on a plane parallel to the laser frame. If the depth difference between the current point and the previous point, as well as the depth difference between the current point and the next point, are both greater than a threshold, it means that this line or this plane is parallel to the laser frame, so the point is removed.

[0123] Furthermore, the point cloud is sorted from largest to smallest based on the calculated point cloud curvature, and corner points and face points are extracted by comparing them with a threshold. The extracted corner points and face points are stored and searched using an ikd-Tree data structure.

[0124] S3. Register the point cloud edge features and planar features, obtain key frames, and output the radar odometer.

[0125] Optionally, step S3 above may include the following steps S31-S33:

[0126] S31. Initialize the pose of the current frame and obtain the local map and the point cloud of the current frame.

[0127] S32. The pose of the current frame is matched and optimized by the nearest point iterative ICP algorithm.

[0128] The IMU pre-integration provides the initial values.

[0129] S33. Determine whether the current frame after matching optimization is a key frame. If so, perform factor graph optimization to update the poses of all variable nodes in the factor graph, which is the poses of all historical key frames, and update the odometry trajectory.

[0130] In one feasible implementation, the extracted features are registered using a Scan-to-Map method, IMU pre-integration provides initial values ​​for point cloud registration, and then the radar odometry is output.

[0131] The formula for the ICP algorithm is as follows:

[0132]

[0133] Where R represents the rotation matrix, t represents the translation matrix, and p i Let R represent the feature points to be registered. Then, calculate the error of the points to be registered, and then construct a least squares problem to find the pose R and t that minimize the sum of squared errors.

[0134] S4. Obtain dynamic thresholds through environmental information, and filter keyframes based on dynamic thresholds.

[0135] Optionally, step S4 above may include the following steps S41-S42:

[0136] S41. Obtain dynamic thresholds through environmental information.

[0137] S42. Obtain the number of corner and face features in the current frame, and determine whether the number of features is greater than or equal to the dynamic threshold. In this example, the threshold is set to 100. If yes, obtain the pose change of the robot in the current frame and the previous key frame. If the pose change exceeds the set pose change threshold, establish a new key frame; otherwise, end the judgment.

[0138] One feasible implementation method is, for example Figure 4 , 5 As shown, traditional keyframe extraction methods typically determine keyframes based on changes in distance and angle. A new keyframe is defined if the robot moves more than 1 meter or moves more than 0.2°. This invention uses dynamic thresholding to reduce the creation of redundant keyframes, thus saving computational resources.

[0139] Furthermore, the openness or confinement of the robot's environment is determined by the pose transformation and median point cloud depth between the first few keyframes. In rapidly changing, small scenes, the threshold is lowered and the keyframe extraction frequency is increased; in large scenes, the threshold is increased and the extraction frequency is decreased. The threshold is determined by the point cloud depth and the spacing between historical keyframes. A dynamic threshold is calculated by weighting the current scene's openness and the distances between historical keyframes. This dynamic value determines the scene threshold, categorizing scenes into open, relatively open, relatively confinement, and confinement, each corresponding to a different threshold. The calculated current frame is compared with the corresponding scene threshold; if the scene is small, the extraction frequency is increased; if the scene is open, the frequency is decreased.

[0140] The adaptive threshold calculation formula is as follows:

[0141]

[0142] Where, d key d represents the distance of historical keyframes. mid d represents the median depth of the laser points in the point cloud. key The average distance between the past three historical keyframes is calculated using a sliding window method. The current scene is judged by calculating the AD value; the larger the AD, the more open the scene. For open scenes, the threshold D is set to 2m, meaning the robot must move more than 2m to confirm a new keyframe; for relatively open scenes, the threshold D is set to 1.5m, meaning the robot must move more than 1.5m to confirm a new keyframe; for relatively narrow scenes, the threshold D is set to 1m, meaning the robot must move more than 1m to confirm a new keyframe; and for narrow scenes, the threshold D is set to 0.5m, meaning the robot must move more than 0.5m to confirm a new keyframe.

[0143] S5. Perform loop closure detection on the filtered keyframes. After detecting loop closures in the system, the backend constructs constraints and performs factor graph optimization to reduce accumulated errors.

[0144] Optionally, step S5 above may include the following steps S51-S53:

[0145] S51. Create a point cloud descriptor for each keyframe in the filtered keyframes.

[0146] In one feasible implementation, creating a point cloud descriptor for a keyframe first requires dividing the acquired laser point cloud into sectors along the azimuth direction. Then, along the radial direction, the point cloud is divided into rings. The intersection of a sector and a ring is called a bin. A resolution of 6° can be set, resulting in 60 sectors and 25 rings radially from 0 to 250m. A scancontext descriptor is then formed by statistically analyzing the maximum z-direction value of the point cloud in each bin. This is equivalent to compressing a three-dimensional image into a two-dimensional 60*25 matrix. It's easy to see from this division that bins that are farther away are slightly wider than those that are closer. The advantage of this division is that it allows for automatic dynamic adjustment of the point cloud density at different distances. Areas closer to the point cloud typically have higher density, so our bins are narrower; conversely, areas farther away have sparser point clouds, so wider bins can accommodate more points. The SC descriptor transforms a single frame of point cloud data into a two-dimensional matrix, where the values ​​within the matrix represent the maximum height of the laser points within the corresponding bin.

[0147] S52. Perform point cloud descriptor calculation on the point cloud descriptor, extract an N-dimensional vector from the calculated point cloud descriptor, perform a search for similar keyframes based on the N-dimensional vector, compare the point cloud descriptor of the searched keyframe with the point cloud descriptor of the current keyframe, and if the comparison result is higher than the preset point cloud descriptor threshold, then a loop closure is found.

[0148] One feasible implementation method is, for example Figure 6 As shown, the point cloud descriptor is calculated for the input single-frame point cloud. Then, an N-dimensional vector (consistent with the number of rings) is extracted from the SC (Scan Context, point cloud descriptor) of the point cloud of that frame to search for similar keyframes in the KD tree. The SC of the searched reference frame is compared with the current frame to be matched. If the comparison score is higher than a certain threshold, it is considered that a loop closure has been found.

[0149] The formula for similarity is:

[0150]

[0151] The two Scan-Contexts are I q ,I c The similarity is the sum of the comparisons of each column, with the cosine distance used to calculate the similarity of the column vectors. The method (note: the higher the similarity, the smaller the cosine distance). N s It is the column number, which is the number of sectors in a descriptor.

[0152] Will I c Translate n units along the column direction Then we can iterate through and move this column vector N. s This process is repeated, obtaining the similarity result after each move, and the minimum value is taken as I. q ,I c Similarity:

[0153]

[0154]

[0155] Theoretically, it is certain that the Scan-Context search in the current frame can be found, but this method is not feasible from an efficiency point of view. Therefore, the Ring-Key method was introduced.

[0156] Specifically, it is represented by a one-dimensional array k, where each element of the array... The encoding value for the i-th Ring is established starting from the origin and proceeding from the nearest to the farthest point.

[0157]

[0158] The following is The expression where ||r i ||0 represents r i The number of non-zero values ​​in the matrix.

[0159]

[0160] This approach is equivalent to reducing the original two-dimensional array search to one-dimensional. The data is first compressed and dimensionality reduced before the search, making the search on the two-dimensional array more efficient.

[0161] S53. After finding the loop, perform backend factor graph optimization.

[0162] In one feasible implementation, backend factor graph optimization involves calling the GTSAM library to add odometry factors, IMU factors, and loop closure factors to perform optimization, thereby obtaining the optimized pose and cumulative variance. After one graph optimization, not only the pose of the current frame changes, but the poses of other nodes may also change, so the pose needs to be updated again.

[0163] S6. After loop closure detection, the ERASOR algorithm is used to remove dynamic points, resulting in tightly coupled laser simultaneous localization and mapping (SLAM) results based on redundant keyframe removal.

[0164] Optionally, the ERASOR algorithm in S6 is used to remove dynamic points, including:

[0165] S61. Obtain the point cloud map containing dynamic objects, play each frame of the original point cloud in the point cloud map, divide the original point cloud and the corresponding sub-map into multiple grids in the same way, and calculate a descriptor representing the point cloud distribution for each grid.

[0166] In one feasible implementation, the input data, namely a point cloud map containing dynamic objects established by a SLAM system, is played, each frame of the original point cloud (scan) is played, the scan and the corresponding submap are divided into each bin in the same way, and a descriptor representing the distribution of the point cloud is calculated.

[0167] Specifically, in this example, we only consider the space within a radius of 60m from the origin, in the height range of -1m to +4m. Within this space, the point cloud data will be divided into individual grids according to the angle and radius, with an angle of 6° and a distance of 1m. The height difference in the z-direction of the point cloud data within each grid is calculated, which is the descriptor.

[0168] S62. Obtain the ratio of the descriptor of the same cell in the original point cloud and the sub-image, and mark the cells with a ratio less than the preset descriptor threshold as suspected dynamic regions.

[0169] In one feasible implementation, the descriptors of the same bin are compared between scan and submap. If the ratio of descriptor values ​​is less than a threshold, the bin is marked as a suspected dynamic region.

[0170] S63. Fit a ground plane in the suspected dynamic region and filter out the points on the ground plane as dynamic points.

[0171] In one feasible implementation, a ground plane is fitted in the suspected area. The specific method for fitting ground points is to first select several points with the lowest frequency as seed points, perform three regional growth operations upwards, and find the points as ground points. Then, the eigenvalues ​​and eigenvectors of the ground point cloud are calculated using the PCA principal component analysis method. The eigenvector with the smallest eigenvalue is most likely the normal vector of the ground. Based on this, the plane equation of the ground plane is calculated. Finally, the points on the ground plane are filtered out as dynamic points.

[0172] Current loop closure detection methods are prone to failure when there is significant cumulative drift outdoors. Compared to traditional loop closure detection methods, this invention uses a point cloud descriptor approach for loop closure detection. Based on 3D point cloud relocalization and scene recognition, the main idea is to compress the 3D information of the scene and transform the information from the Cartesian coordinate system to the polar coordinate system for calculation. The advantage of this strategy is that it efficiently utilizes the distribution characteristics of the scene point cloud and introduces a "rotation invariant" descriptor for fast search.

[0173] In laser SLAM algorithms, the rapid insertion frequency of keyframes leads to a rapid increase in redundant information and excessive computational resources. This invention proposes an improved keyframe selection strategy that reduces the redundancy of keyframe information without sacrificing accuracy and robustness, shortens the time for backend factor graph optimization, and improves the speed and accuracy of algorithm processing.

[0174] In this embodiment of the invention, a tightly coupled laser SLAM method based on redundant keyframe removal is disclosed. It uses an adaptive threshold for keyframe selection. Compared with the method based on the robot moving beyond a set threshold, this strategy is more universal and can cope with different scenarios, reducing loop closure errors. On the other hand, it reduces the extraction of redundant keyframes, relieves memory pressure, improves the accuracy and speed of the algorithm, and effectively improves the real-time performance of the algorithm.

[0175] A ground segmentation module is added to the front end to reduce the computational cost of point cloud feature extraction and the impact of ground points on feature extraction and registration. An ikd-tree data structure is used to store corner and face points, improving accuracy and speed. A loop closure detection method using SC descriptors is added to the back end, resulting in higher efficiency and accuracy for loop closure detection, thus improving the system's precision and robustness. Finally, the ERASOR method is used to remove dynamic point clouds, optimizing the system's mapping performance.

[0176] In this embodiment of the invention, a tightly coupled laser SLAM method based on redundant keyframe removal is disclosed. It uses an adaptive threshold for keyframe selection. Compared with the method based on the robot moving beyond a set threshold, this strategy is more universal and can cope with different scenarios, reducing loop closure errors. On the other hand, it reduces the extraction of redundant keyframes, relieves memory pressure, improves the accuracy and speed of the algorithm, and effectively improves the real-time performance of the algorithm.

[0177] A ground segmentation module is added to the front end to reduce the computational cost of point cloud feature extraction and the impact of ground points on feature extraction and registration. An ikd-tree data structure is used to store corner and face points, improving accuracy and speed. A loop closure detection method using SC descriptors is added to the back end, resulting in higher efficiency and accuracy for loop closure detection, thus improving the system's precision and robustness. Finally, the ERASOR method is used to remove dynamic point clouds, optimizing the system's mapping performance.

[0178] like Figure 7 As shown, this embodiment of the invention provides a tightly coupled laser SLAM device 700 based on redundant keyframe removal. This device 700 is used to implement a tightly coupled laser SLAM method based on redundant keyframe removal. The device 700 includes:

[0179] The preprocessing module 710 is used to acquire radar point cloud data, preprocess the point cloud data, and obtain preprocessed point cloud data.

[0180] The curvature calculation module 720 is used to calculate the curvature of the preprocessed point cloud data and extract the point cloud edge features and planar features.

[0181] The keyframe acquisition module 730 is used to register point cloud edge features and planar features, acquire keyframes, and output radar odometry.

[0182] The filtering module 740 is used to obtain dynamic thresholds through environmental information and to filter keyframes based on the dynamic thresholds.

[0183] The loop closure detection module 750 is used to perform loop closure detection on the filtered keyframes.

[0184] The output module 760 is used to remove dynamic points after loop closure detection using the ERASOR algorithm, and obtains tightly coupled laser simultaneous localization and mapping (SLAM) results based on redundant keyframe removal.

[0185] Optionally, the preprocessing module 710 is further used for:

[0186] S11. The inertial measurement unit (IMU) is pre-integrated to obtain the robot's pose change. Based on the pose change, the pose of each point in the point cloud data relative to the initial scan time is obtained, motion compensation is performed, and the IMU odometry is output.

[0187] S12. Perform ground point segmentation on the motion-compensated point cloud data and output the point cloud data after ground point segmentation.

[0188] Optionally, the curvature of the preprocessed point cloud data is calculated as shown in equation (1):

[0189]

[0190] Where S represents point A specified number of points nearby, Let L represent the coordinate system of the k-th scan, and let L be the position of the j-th point in the point cloud within L. Let L represent the coordinate system of the k-th scan, and let L be the position of the i-th point in the point cloud within L.

[0191] Optionally, the curvature calculation module 720 is further used for:

[0192] Remove points that are parallel to the laser direction and points that are blocked.

[0193] After removing points, the point cloud is sorted according to the calculated curvature. Points with curvature greater than the threshold of 1 are designated as corner points, and points with curvature less than the threshold of 0.2 are designated as face points. Corner points and face points are stored and searched using an ikd-tree data structure.

[0194] Optionally, the keyframe acquisition module 730 is further used for:

[0195] S31. Initialize the pose of the current frame and obtain the local map and the point cloud of the current frame.

[0196] S32. The pose of the current frame is matched and optimized by the nearest point iterative ICP algorithm.

[0197] S33. Determine whether the current frame after matching optimization is a key frame. If it is, perform factor graph optimization, update the pose of all variable nodes in the factor graph, and update the radar odometry trajectory.

[0198] Optionally, the filtering module 740 is further used for:

[0199] S41. Obtain dynamic thresholds through environmental information.

[0200] S42. Obtain the number of features of corner points and face points in the current frame, and determine whether the number of features is greater than or equal to the dynamic threshold. If so, obtain the pose change of the robot in the current frame and the previous key frame. If the pose change exceeds the set pose change threshold, establish a new key frame; otherwise, end the judgment.

[0201] Optionally, the dynamic threshold is as shown in equation (2):

[0202]

[0203] Where, d key d represents the distance of historical keyframes. mid This represents the median depth of the laser points in the point cloud.

[0204] Optionally, the loop closure detection module 750 is further used for:

[0205] S51. Create a point cloud descriptor for each keyframe in the filtered keyframes.

[0206] S52. Perform point cloud descriptor calculation on the point cloud descriptor, extract an N-dimensional vector from the calculated point cloud descriptor, perform a search for similar keyframes based on the N-dimensional vector, compare the point cloud descriptor of the searched keyframe with the point cloud descriptor of the current keyframe, and if the comparison result is higher than the preset point cloud descriptor threshold, then a loop closure is found.

[0207] S53. After finding the loop, perform backend factor graph optimization.

[0208] Optionally, the output module 760 is further used for:

[0209] S61. Obtain the point cloud map containing dynamic objects, play each frame of the original point cloud in the point cloud map, divide the original point cloud and the corresponding sub-map into multiple grids in the same way, and calculate a descriptor representing the point cloud distribution for each grid.

[0210] S62. Obtain the ratio of the descriptor of the same cell in the original point cloud and the sub-image, and mark the cells with a ratio less than the preset descriptor threshold as suspected dynamic regions.

[0211] S63. Fit a ground plane in the suspected dynamic region and filter out the points on the ground plane as dynamic points.

[0212] In this embodiment of the invention, a tightly coupled laser SLAM method based on redundant keyframe removal is disclosed. It uses an adaptive threshold for keyframe selection. Compared with the method based on the robot moving beyond a set threshold, this strategy is more universal and can cope with different scenarios, reducing loop closure errors. On the other hand, it reduces the extraction of redundant keyframes, relieves memory pressure, improves the accuracy and speed of the algorithm, and effectively improves the real-time performance of the algorithm.

[0213] A ground segmentation module is added to the front end to reduce the computational cost of point cloud feature extraction and the impact of ground points on feature extraction and registration. An ikd-tree data structure is used to store corner and face points, improving accuracy and speed. A loop closure detection method using SC descriptors is added to the back end, resulting in higher efficiency and accuracy for loop closure detection, thus improving the system's precision and robustness. Finally, the ERASOR method is used to remove dynamic point clouds, optimizing the system's mapping performance.

[0214] Figure 8 This is a schematic diagram of the structure of an electronic device 800 provided in an embodiment of the present invention. The electronic device 800 can vary considerably due to different configurations or performance. It may include one or more central processing units (CPUs) 801 and one or more memories 802. The memory 802 stores at least one instruction, which is loaded and executed by the processor 801 to implement the following tightly coupled laser SLAM method based on redundant keyframe removal:

[0215] S1. Acquire radar point cloud data, preprocess the point cloud data, and obtain preprocessed point cloud data.

[0216] S2. Perform curvature calculation on the preprocessed point cloud data to extract point cloud edge features and planar features.

[0217] S3. Register the point cloud edge features and planar features, obtain key frames, and output the radar odometer.

[0218] S4. Obtain dynamic thresholds through environmental information, and filter keyframes based on dynamic thresholds.

[0219] S5. Perform loop closure detection on the selected keyframes.

[0220] S6. After loop closure detection, the ERASOR algorithm is used to remove dynamic points, resulting in tightly coupled laser simultaneous localization and mapping (SLAM) results based on redundant keyframe removal.

[0221] In an exemplary embodiment, a computer-readable storage medium is also provided, such as a memory including instructions that can be executed by a processor in a terminal to perform the aforementioned tightly coupled laser SLAM method based on redundant keyframe removal. For example, the computer-readable storage medium may be a ROM, random access memory (RAM), CD-ROM, magnetic tape, floppy disk, and optical data storage device, etc.

[0222] Those skilled in the art will understand that all or part of the steps of the above embodiments can be implemented by hardware or by a program instructing related hardware. The program can be stored in a computer-readable storage medium, such as a read-only memory, a disk, or an optical disk.

[0223] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A tightly coupled laser SLAM method based on redundant keyframe removal, characterized in that, The method includes: S1. Acquire radar point cloud data, preprocess the point cloud data, and obtain preprocessed point cloud data. S2. Perform curvature calculation on the preprocessed point cloud data, and extract point cloud edge features and planar features; S3. Register the point cloud edge features and planar features, obtain key frames, and output the radar odometry. S4. Obtain a dynamic threshold through environmental information, and filter the keyframes according to the dynamic threshold; S5. Perform loop closure detection on the filtered keyframes; S6. After loop closure detection, the ERASOR algorithm is used to remove dynamic points, and the tightly coupled laser simultaneous localization and map construction SLAM results based on redundant keyframe removal are obtained. The step S4, which involves obtaining a dynamic threshold from environmental information and filtering the keyframes based on the dynamic threshold, includes: S41. Obtain dynamic thresholds through environmental information; S42. Obtain the number of corner points and face points in the current frame, and determine whether the number of features is greater than or equal to the dynamic threshold. If yes, obtain the pose change of the robot in the current frame and the previous key frame. If the pose change exceeds the set pose change threshold, establish a new key frame. If not, end the judgment. The dynamic threshold in S41 is shown in the following formula (2): （2） in, The distance to historical keyframes is calculated as the average distance between the past three historical keyframes using a sliding window method. This represents the median depth of the laser points in the point cloud.

2. The method according to claim 1, characterized in that, The step S1 involves acquiring radar point cloud data and preprocessing the point cloud data to obtain preprocessed point cloud data, including: S11. The inertial measurement unit (IMU) is pre-integrated to obtain the robot's pose change. Based on the pose change, the pose of each point in the point cloud data relative to the starting scan time is obtained, motion compensation is performed, and the IMU odometry is output. S12. Perform ground point segmentation on the motion-compensated point cloud data and output the point cloud data after ground point segmentation.

3. The method according to claim 1, characterized in that, The curvature calculation of the preprocessed point cloud data in S2 is shown in the following formula (1): （1） in, Point A specified number of points nearby, Indicates the first k The coordinate system for this scan is L The first point cloud j The point is at L The position in the middle, Indicates the first k The coordinate system for this scan is L The first point cloud i The point is at L The position in the middle.

4. The method according to claim 1, characterized in that, After performing curvature calculation on the preprocessed point cloud data in step S2, the method further includes: Remove points that are parallel to the laser direction and points that are blocked; After removing points, the point cloud is sorted according to the calculated curvature. Points with curvature greater than a threshold of 1 are designated as corner points, and points with curvature less than a threshold of 0.2 are designated as face points. The corner points and face points are stored and searched using an ikd-tree data structure.

5. The method according to claim 1, characterized in that, The step S3, which involves registering the point cloud edge features and planar features to obtain key frames and outputting radar odometry, includes: S31. Initialize the pose of the current frame and obtain the local map and the point cloud of the current frame; S32. The pose of the current frame is matched and optimized by the nearest point iterative ICP algorithm; S33. Determine whether the current frame after matching optimization is a key frame. If it is, perform factor graph optimization, update the pose of all variable nodes in the factor graph, and update the radar odometry trajectory.

6. The method according to claim 1, characterized in that, The loop closure detection in S5 of the filtered keyframes includes: S51. Create a point cloud descriptor for each keyframe in the filtered keyframes; S52. Perform point cloud descriptor calculation on the point cloud descriptor, extract an N-dimensional vector from the calculated point cloud descriptor, perform a search for similar keyframes based on the N-dimensional vector, compare the point cloud descriptor of the searched keyframe with the point cloud descriptor of the current keyframe, and if the comparison result is higher than the preset point cloud descriptor threshold, then a loop closure is found. S53. After finding the loop, perform backend factor graph optimization.

7. The method according to claim 1, characterized in that, The removal of dynamic points using the ERASOR algorithm in S6 includes: S61. Obtain the established point cloud map containing dynamic objects, play each frame of the original point cloud in the point cloud map, divide the original point cloud and the corresponding sub-map into multiple grids in the same way, and calculate a descriptor representing the point cloud distribution for each grid. S62. Obtain the ratio of the descriptor of the same cell in the original point cloud and the sub-image, and mark the cells with a ratio less than a preset descriptor threshold as suspected dynamic regions. S63. Fit a ground plane in the suspected dynamic region and filter out the points on the ground plane as dynamic points.

8. A tightly coupled laser SLAM device based on redundant keyframe removal, characterized in that, The device includes: The preprocessing module is used to acquire radar point cloud data, preprocess the point cloud data, and obtain preprocessed point cloud data. The curvature calculation module is used to calculate the curvature of the preprocessed point cloud data and extract point cloud edge features and planar features. The keyframe acquisition module is used to register the point cloud edge features and planar features, acquire keyframes, and output radar odometry. The filtering module is used to obtain a dynamic threshold through environmental information and filter the keyframes according to the dynamic threshold; The loop closure detection module is used to perform loop closure detection on the filtered keyframes. The output module is used to remove dynamic points after loop closure detection using the ERASOR algorithm, and obtain tightly coupled laser simultaneous localization and mapping (SLAM) results based on redundant keyframe removal. The step of obtaining a dynamic threshold through environmental information and filtering the keyframes based on the dynamic threshold includes: Dynamic thresholds are obtained through environmental information; Obtain the number of corner and face features in the current frame, and determine whether the number of features is greater than or equal to the dynamic threshold. If so, obtain the pose change between the current frame and the previous key frame. If the pose change exceeds the set pose change threshold, establish a new key frame; otherwise, end the judgment. The dynamic threshold is shown in equation (2) below: （2） in, The distance to historical keyframes is calculated as the average distance between the past three historical keyframes using a sliding window method. This represents the median depth of the laser points in the point cloud.

Images