A laser radar loop-back detection method based on hierarchical frequency domain feature extraction
By extracting hierarchical frequency domain features from LiDAR point clouds and generating a frequency domain signature matrix, the problem of decreased positioning accuracy in LiDAR loop closure detection is solved, achieving high accuracy and robust loop closure detection.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- BEIJING INST OF TECH
- Filing Date
- 2026-03-25
- Publication Date
- 2026-06-26
AI Technical Summary
Existing lidar-based loop closure detection methods suffer from decreased positioning accuracy in large-scale or long-term operation, especially in complex environments where sensor errors lead to cumulative pose drift. Furthermore, existing methods are insufficient in terms of recognition accuracy and computational efficiency.
A method based on hierarchical frequency domain feature extraction is adopted. By performing height hierarchical, polar coordinate, and Fourier transform on the LiDAR point cloud, a frequency domain signature matrix is generated. The difference of the frequency domain signature matrix is used to determine the loop closure relationship and correct the robot pose.
It improves the accuracy and robustness of loop closure detection, simplifies computational complexity, ensures real-time and accurate detection, and adapts to loop closure detection with large angle differences.
Smart Images
Figure CN122284598A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of navigation guidance and control technology, specifically to a lidar loop closure detection method based on hierarchical frequency domain feature extraction. Background Technology
[0002] With the rapid development of new urbanization, intelligent transportation, and smart manufacturing, the autonomous localization and environmental perception capabilities of mobile robots, unmanned vehicles, and automated inspection equipment in complex environments have become key supporting technologies. Among them, Simultaneous Localization and Mapping (SLAM) technology based on lidar is widely used in complex indoor and outdoor scenarios due to its advantages such as insensitivity to changes in lighting and high ranging accuracy. However, in large-scale or long-term operating environments, sensor errors inevitably lead to cumulative pose drift, which in turn affects the overall positioning accuracy and stability of the system.
[0003] To suppress the aforementioned accumulated errors, existing SLAM systems typically introduce loop closure detection mechanisms. These mechanisms identify whether the robot has reached the same or similar spatial locations at different times. A factor graph optimization framework is used to incorporate loop closure constraint factors into the overall factor graph, globally correcting the historical trajectory. Based on the correction results, the robot is repositioned to guide subsequent path planning and tracking control. Therefore, the accuracy and robustness of loop closure detection directly determine the reliability of the localization system in complex scenarios, especially in scenarios involving large-scale inspections, long-term operation, and high environmental structural repetition.
[0004] Existing loop closure detection methods based on LiDAR point clouds mostly employ polar coordinate raster statistics or local feature descriptions to model the environment. However, these methods still have certain limitations in practical applications. On the one hand, traditional methods are insufficient in extracting environmental height structure information; on the other hand, the recognition accuracy decreases when encountering loops with large angle differences; furthermore, in practical engineering applications, loop closure detection algorithms not only need to have high recognition accuracy but also need to balance computational efficiency and storage overhead. Therefore, how to make fuller use of the spatial structure information of LiDAR point clouds while ensuring computational efficiency, and construct a loop closure detection method that combines rotation invariance, structural discriminancy, and engineering practicality, remains a pressing problem to be solved in the current technical field. Summary of the Invention
[0005] In view of this, the present invention provides a lidar loop closure detection method based on hierarchical frequency domain feature extraction, which can solve the technical problem of positioning point drift caused by algorithm error in long-term operation of lidar SLAM.
[0006] To solve the above-mentioned technical problems, the present invention is implemented as follows.
[0007] A lidar loop closure detection method based on hierarchical frequency domain feature extraction includes:
[0008] Step S1: During the movement, the robot collects point cloud data of the current driving environment through the onboard LiDAR sensor to obtain the original LiDAR point cloud. Each radar frame corresponds to one original LiDAR point cloud. Step S2: Divide the preprocessed raw lidar point cloud into multiple layers according to height, and represent the layered sub-point clouds in polar coordinates; Step S3: Rasterize the layered sub-point cloud represented in polar coordinates and calculate the corresponding polar coordinate distribution statistics map of the layered sub-point cloud; perform energy intensity normalization and Fourier transform on the polar coordinate distribution statistics map in sequence, and generate an amplitude information map based on the obtained layered point cloud spectrum map. Step S4: Divide the amplitude information map into multiple radial frequency bands, integrate the frequency band features of the radial frequency bands as the frequency domain signature vectors corresponding to the layered sub-point clouds; integrate all frequency domain signature vectors of the radar frame to obtain the frequency domain signature matrix; Step S5: Determine the difference between the frequency domain signature matrix corresponding to the current radar frame acquired by the robot and the frequency domain signature matrix corresponding to the radar frame to be matched in the historical trajectory; when the difference is less than a preset threshold, there is a loop closure relationship between the current radar frame and the radar frame to be matched, determine the loop closure constraint factor and correct the robot pose, and correct the robot's motion operation behavior based on the corrected robot pose.
[0009] Preferably, in step S2, the preprocessed original lidar point cloud is divided into multiple layers according to height, including: Step S21: Select multiple laser sampling points from the original lidar point cloud. For each laser sampling point p1, calculate its coordinates ( , y, z), calculate the horizontal radius r1 and the height distance h of the laser sampling point from the origin of the airborne lidar coordinate system, and retain the h value within the height range [ , Within ] and the value of r1 is within the radius interval [ , Laser sampling points within the range are used to obtain the point cloud after range filtering. , as the preprocessed raw lidar point cloud; The horizontal radius r1 and the vertical distance h of the laser sampling point from the radar coordinate origin are calculated using the following formulas:
[0010] y1 and z1 are the x-coordinate, y-coordinate, and height coordinates of the laser sampling point p1, respectively. , These represent the minimum and maximum heights of the LiDAR point cloud filtering range, respectively. , These represent the minimum and maximum radii of the LiDAR point cloud filtering range, respectively. The coordinates are the height coordinates of the origin of the airborne lidar coordinate system. Step S22: Set the height range [ , The preprocessed raw lidar point cloud within the [included area] is uniformly divided into Each height range has a span of [number] meters. for:
[0011] Among them, the kth height interval Represented as:
[0012] The portion of the original LiDAR point cloud corresponding to the kth altitude range is denoted as the kth layered sub-point cloud. The k-th layered sub-point cloud Represented as:
[0013] For any point in the layered sub-point cloud, They are respectively The x-coordinate, y-coordinate, and height coordinate.
[0014] Preferably, in step S2, the layered sub-point cloud is represented in polar coordinates, wherein the transformation formula is as follows:
[0015] Where r is the radial coordinate of a point in the layered sub-point cloud in polar coordinates. The angular coordinates of the midpoint of the layered sub-point cloud in the polar coordinate system; The kth layered sub-point cloud Point set in polar coordinate system Represented as:
[0016] in, for The corresponding two-dimensional polar coordinates.
[0017] Preferably, in step S3, the layered sub-point cloud represented in polar coordinates is rasterized and the corresponding polar coordinate distribution statistics map of the layered sub-point cloud is calculated, including: radius interval [ , Evenly divided into A radial interval, and an angular interval [0, Evenly divided into Calculate the values of each point in the layered sub-point cloud represented in polar coordinates within each angular interval. The formulas for the corresponding radial interval dividing zone index value r2 and the angular interval dividing zone index value s2 are as follows:
[0018] The hierarchical sub-point cloud represented in polar coordinates is rasterized, with each raster corresponding to a raster point set. It is expressed as follows:
[0019] in, R represents the angular coordinate value, and R represents the directional coordinate value. The number of points in each grid cell of the hierarchical sub-point cloud represented in polar coordinates is counted to generate a polar coordinate distribution chart of the corresponding hierarchical sub-point cloud. : .
[0020] Preferably, in step S3, the polar coordinate distribution statistical map is subjected to energy intensity normalization and Fourier transform sequentially, and an amplitude information map is generated based on the obtained layered point cloud spectrum map, including: The energy intensity is normalized by performing energy intensity normalization on the polar coordinate distribution statistical map to obtain the normalized distribution statistical map. ,
[0021] Normalized distribution statistics Perform a Fourier transform to obtain the normalized distribution statistics. Corresponding hierarchical point cloud spectrum ,
[0022] in, Represents the statistical graph of the normalized distribution. Perform a Fourier transform. For data points in a normalized distribution statistical graph, the horizontal axis is s2 and the vertical axis is r2. It is an imaginary number. v represents the horizontal and vertical coordinates in the two-dimensional frequency domain, respectively. Spectral map of layered point cloud Perform an equivalent Fourier transform centered on the zero-frequency component to generate a layered point cloud spectrum. Corresponding amplitude information graph : .
[0023] Preferably, in step S4, the amplitude information map is divided into multiple radial frequency bands, and the frequency band features of the radial frequency bands are integrated as the frequency domain signature vectors corresponding to the layered sub-point clouds; the frequency domain signature matrix is obtained by integrating all frequency domain signature vectors of the radar frame, including: Step S41: Obtain amplitude information graph center With the corresponding normalized radius ;
[0024] in, These are the x and y coordinates of the origin of a two-dimensional frequency domain coordinate system, respectively. The maximum spectral radius corresponding to the polar coordinate distribution statistical graph; the information matrix of the normalized radius is cropped, retaining the frequency bands in the information matrix of the normalized radius that are within [ Information between ]; , These represent the minimum and maximum values of the normalized radius distribution proportions, respectively. Step S42: Divide the amplitude information map evenly into For each radial frequency band, calculate the energy statistics of that radial frequency band. Use the energy statistics of the radial frequency band as the frequency band characteristic of the radial frequency band. Here, the energy statistics of the num-th radial frequency band... The calculation formula is as follows:
[0025] Let num be the set of all spectral points contained in the num-th radial band; Step S43: Integrate the frequency band features of the radial frequency band as the frequency domain signature vector corresponding to the layered sub-point cloud. Frequency domain signature vector corresponding to the layered sub-point cloud It is expressed as follows:
[0026] T stands for transpose; Step S44: Integrate all frequency domain signature vectors of the radar frame to obtain the frequency domain signature matrix. : .
[0027] Preferably, in step S5, determining the difference between the frequency domain signature matrix corresponding to the current radar frame acquired by the robot and the frequency domain signature matrix corresponding to the radar frame to be matched in the historical trajectory includes: Determine the frequency domain signature matrix corresponding to the current radar frame a acquired by the robot and the frequency domain signature matrix corresponding to the radar frame b to be matched in the historical trajectory. The difference between the frequency domain signature matrix of the current radar frame acquired by the robot and the frequency domain signature matrix of the radar frames to be matched in the historical trajectory. The calculation formula is as follows:
[0028] in, The weights for the difference in point cloud layers at the k-th layer are: The cosine distance between the signature vectors of the k-th layer point cloud;
[0029]
[0030] The function F(.) is a monotonically increasing function with a range of [0.8, 10]. The difference in the number of points in the k-th layer of the point cloud is given by the following: This represents the number of sub-point clouds in the k-th layer of the current radar frame a. This represents the number of sub-point clouds in the k-th layer of the radar frame b to be matched for historical trajectories. This is the frequency domain signature vector of the k-th layer sub-point cloud in the current radar frame a. is the frequency domain signature vector of the k-th layer sub-point cloud in the radar frame b to be matched for the historical trajectory.
[0031] Preferably, in step S5, when the difference is less than a preset threshold, there is a loop closure relationship between the current radar frame and the radar frame to be matched. The loop closure constraint factor is determined and the robot pose is corrected. Based on the corrected robot pose, the robot's motion operation behavior is corrected, including: Step S51: When the difference is less than a preset threshold, there is a loop closure relationship between the current radar frame and the radar frame to be matched; based on the loop closure relationship, the loop closure constraint factor is determined, and the loop closure constraint factor is expressed as follows:
[0032] Where i represents the time corresponding to the current radar frame, and j represents the time corresponding to the radar frame to be matched. This represents the relative pose observation between the current radar frame and the radar frame to be matched. and These are the current radar frame pose and the radar frame pose to be matched, respectively. The weights of the lapsing constraint factors, Let be the displacement error between time i and time j. This is the information matrix for closure constraints; Step S52: Obtain the current airborne LiDAR positioning information, correct the robot's current pose based on the loop closure constraint factor; determine the corrected robot pose. The joint probability model is as follows: , in, Let be the prior probability of the initial pose. The relative pose observations between the current radar frame and the radar frame to be matched are provided by the odometry factor. This is the set of constraint edges provided by the odometry factors in the factor graph. This is the set of constraint edges provided by the loop closure detection factor in the factor graph; Step S53: Correct the robot's motion behavior based on the corrected robot pose.
[0033] A lidar loop closure detection device based on hierarchical frequency domain feature extraction, comprising: Data acquisition module: Configured for the robot to collect point cloud data of the current driving environment through the onboard LiDAR sensor during the movement, and obtain the original LiDAR point cloud. Each radar frame corresponds to one original LiDAR point cloud. Layered module: configured to divide the pre-processed raw LiDAR point cloud into multiple layers according to height, and represent the layered sub-point clouds in polar coordinates; Information extraction module: configured to rasterize the layered sub-point cloud represented in polar coordinates and calculate the corresponding polar coordinate distribution statistics map of the layered sub-point cloud; to perform energy intensity normalization and Fourier transform on the polar coordinate distribution statistics map in sequence, and to generate an amplitude information map based on the obtained layered point cloud spectrum map; Feature acquisition module: configured to divide the amplitude information map into multiple radial frequency bands, integrate the frequency band features of the radial frequency bands as the frequency domain signature vectors corresponding to the layered sub-point clouds; integrate all frequency domain signature vectors of the radar frame to obtain the frequency domain signature matrix; Correction module: configured to determine the difference between the frequency domain signature matrix corresponding to the current radar frame acquired by the robot and the frequency domain signature matrix corresponding to the radar frame to be matched in the historical trajectory; when the difference is less than a preset threshold, there is a loop relationship between the current radar frame and the radar frame to be matched, determine the loop constraint factor and correct the robot pose, and correct the robot's motion operation behavior based on the corrected robot pose.
[0034] The present invention provides an electronic device, characterized in that the electronic device comprises: A processor is used to execute multiple instructions; Memory, used to store multiple instructions; The plurality of instructions are to be stored in the memory and loaded and executed by the processor as described above.
[0035] The present invention has the following beneficial effects: (1) This invention provides a method for judging motion loop behavior based on the hierarchical spectral characteristics of environmental point clouds and using this as a motion constraint to optimize historical poses. It has advantages such as high loop detection rate, high detection accuracy, strong detection robustness, and strong real-time performance. Environmental point cloud information is collected by lidar, and structural information of the point cloud is extracted through steps such as height hierarchical analysis, polar coordinate statistical analysis, and spectral analysis based on Fourier transform. The loop detection results are then filtered through feature difference threshold analysis weighted by the difference in the number of point clouds. This method has strong detection accuracy and stability.
[0036] (2) The present invention performs feature extraction with height information compression in the height layered point cloud, which preserves the height structure features of the point cloud as much as possible, and effectively improves the accuracy of lidar point cloud loop detection.
[0037] (3) This invention extracts the gridded polar coordinate features of the highly layered radar point cloud and extracts the frequency domain features of the point cloud through two-dimensional fast Fourier transform, so that the point cloud feature signature has extremely high rotation invariance, and thus has the ability to detect loop closures with large angle differences, effectively improving the robustness of lidar point cloud loop closure detection.
[0038] (4) This invention generates a single-frame feature signature matrix by extracting frequency domain features from highly layered point clouds, and calculates point cloud feature difference parameters based on the difference in the number of point clouds in the inter-frame similarity matching stage, which simplifies the time complexity of the calculation and effectively improves the real-time performance of lidar point cloud loop closure detection. Attached Figure Description
[0039] Figure 1 This is a schematic diagram of the lidar loop closure detection method based on hierarchical frequency domain feature extraction according to the present invention.
[0040] Figure 2 This is a schematic diagram of point cloud height layering according to the present invention.
[0041] Figure 3 This is a schematic diagram of the polar coordinate rasterization of the layered sub-point cloud according to the present invention.
[0042] Figure 4 This is a schematic diagram of the normalized layered sub-point cloud polar coordinate distribution statistical matrix of the present invention.
[0043] Figure 5 This is a schematic diagram of frequency band clipping and frequency band division in the layered sub-point cloud amplitude information map of the present invention.
[0044] Explanation of reference numerals in the attached figures: 3-1, Radial interval separation band of layered sub-point cloud polar coordinates; 3-2, Angular interval separation band of layered sub-point cloud polar coordinates; 5-1, Schematic diagram of high frequency threshold in frequency band in layered sub-point cloud amplitude information map; 5-2, Schematic diagram of low frequency threshold in frequency band in layered sub-point cloud amplitude information map; 5-3, Single frequency band segmentation band in layered sub-point cloud amplitude information map. Detailed Implementation
[0045] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.
[0046] like Figure 1 As shown, this invention proposes a lidar loop closure detection method based on hierarchical frequency domain feature extraction, comprising: Step S1: During the movement, the robot collects point cloud data of the current driving environment through the onboard LiDAR sensor to obtain the original LiDAR point cloud. Each radar frame corresponds to one original LiDAR point cloud. Step S2: Divide the preprocessed raw lidar point cloud into multiple layers according to height, and represent the layered sub-point clouds in polar coordinates; Step S3: Rasterize the layered sub-point cloud represented in polar coordinates and calculate the corresponding polar coordinate distribution statistics map of the layered sub-point cloud; perform energy intensity normalization and Fourier transform on the polar coordinate distribution statistics map in sequence, and generate an amplitude information map based on the obtained layered point cloud spectrum map. Step S4: Divide the amplitude information map into multiple radial frequency bands, integrate the frequency band features of the radial frequency bands as the frequency domain signature vectors corresponding to the layered sub-point clouds; integrate all frequency domain signature vectors of the radar frame to obtain the frequency domain signature matrix; Step S5: Determine the difference between the frequency domain signature matrix corresponding to the current radar frame acquired by the robot and the frequency domain signature matrix corresponding to the radar frame to be matched in the historical trajectory; when the difference is less than a preset threshold, there is a loop closure relationship between the current radar frame and the radar frame to be matched, determine the loop closure constraint factor and correct the robot pose, and correct the robot's motion operation behavior based on the corrected robot pose.
[0047] Further, in step S2, the preprocessed raw lidar point cloud is divided into multiple layers according to height, including: Step S21: Select multiple laser sampling points from the original lidar point cloud. For each laser sampling point p1, calculate its coordinates ( , y, z), calculate the horizontal radius r1 and the height distance h of the laser sampling point from the origin of the airborne lidar coordinate system, and retain the h value within the height range [ , Within ] and the value of r1 is within the radius interval [ , Laser sampling points within the range are used to obtain the point cloud after range filtering. , as the preprocessed raw lidar point cloud; The horizontal radius r1 and the vertical distance h of the laser sampling point from the radar coordinate origin are calculated using the following formulas:
[0048] y1 and z1 are the x-coordinate, y-coordinate, and height coordinates of the laser sampling point p1, respectively. , These represent the minimum and maximum heights of the LiDAR point cloud filtering range, respectively. , These represent the minimum and maximum radii of the LiDAR point cloud filtering range, respectively. The coordinates are the height coordinates of the origin of the airborne lidar coordinate system. Step S22: Set the height range [ , The preprocessed raw lidar point cloud within the [included area] is uniformly divided into Each height range has a span of [number] meters. for:
[0049] Among them, the kth height interval Represented as:
[0050] The portion of the original LiDAR point cloud corresponding to the kth altitude range is denoted as the kth layered sub-point cloud. The k-th layered sub-point cloud Represented as:
[0051] For any point in the layered sub-point cloud, They are respectively The x-coordinate, y-coordinate, and height coordinate.
[0052] In step S2, the layered sub-point cloud is represented in polar coordinates, and the transformation formula is as follows:
[0053] Where r is the radial coordinate of a point in the layered sub-point cloud in polar coordinates. The angular coordinates of the midpoint of the layered sub-point cloud in the polar coordinate system; The kth layered sub-point cloud Point set in polar coordinate system Represented as:
[0054] in, for The corresponding two-dimensional polar coordinates.
[0055] In step S3, the layered sub-point cloud represented in polar coordinates is rasterized and the corresponding polar coordinate distribution statistics map of the layered sub-point cloud is calculated, including: radius interval [ , Evenly divided into A radial interval, and an angular interval [0, Evenly divided into Calculate the values of each point in the layered sub-point cloud represented in polar coordinates within each angular interval. The formulas for the corresponding radial interval dividing zone index value r2 and the angular interval dividing zone index value s2 are as follows:
[0056] The hierarchical sub-point cloud represented in polar coordinates is rasterized, with each raster corresponding to a raster point set. It is expressed as follows:
[0057] in, R represents the angular coordinate value, and R represents the directional coordinate value. The number of points in each grid cell of the hierarchical sub-point cloud represented in polar coordinates is counted to generate a polar coordinate distribution chart of the corresponding hierarchical sub-point cloud. : .
[0058] In this invention, the grid point set The expression means that the grid point set consists of all points in the layered sub-point cloud that satisfy the condition that the angular coordinate value is equal to S and the radial coordinate value is equal to R.
[0059] In step S3, the polar coordinate distribution statistical map is subjected to energy intensity normalization and Fourier transform sequentially, and an amplitude information map is generated based on the obtained layered point cloud spectrum map, including: The energy intensity is normalized by performing energy intensity normalization on the polar coordinate distribution statistical map to obtain the normalized distribution statistical map. ,
[0060] Normalized distribution statistics Perform a Fourier transform to obtain the normalized distribution statistics. Corresponding hierarchical point cloud spectrum ,
[0061] in, Represents the statistical graph of the normalized distribution. Perform a Fourier transform. For data points in a normalized distribution statistical graph, the horizontal axis is s2 and the vertical axis is r2. It is an imaginary number. v represents the horizontal and vertical coordinates in the two-dimensional frequency domain, respectively. Spectral map of layered point cloud Perform an equivalent Fourier transform centered on the zero-frequency component to generate a layered point cloud spectrum. Corresponding amplitude information graph : .
[0062] In step S4, the amplitude information map is divided into multiple radial frequency bands, and the frequency band features of the radial frequency bands are integrated as the frequency domain signature vectors corresponding to the layered sub-point clouds; the frequency domain signature matrix is obtained by integrating all frequency domain signature vectors of the radar frame, including: Step S41: Obtain amplitude information graph center With the corresponding normalized radius ;
[0063] in, These are the x and y coordinates of the origin of a two-dimensional frequency domain coordinate system, respectively. The maximum spectral radius corresponding to the polar coordinate distribution statistical graph; the information matrix of the normalized radius is cropped, retaining the frequency bands in the information matrix of the normalized radius that are within [ Information between ]; , These represent the minimum and maximum values of the normalized radius distribution proportion, respectively.
[0064] In this invention, the proportion of distributions smaller than the minimum radius in the information matrix of the normalized radius is determined. A portion of this is used to filter out low-frequency and DC components, exceeding the maximum radius distribution ratio. A portion of it is used for high-frequency noise filtering.
[0065] Step S42: Divide the amplitude information map evenly into For each radial frequency band, calculate the energy statistics of that radial frequency band. Use the energy statistics of the radial frequency band as the frequency band characteristic of the radial frequency band. Here, the energy statistics of the num-th radial frequency band... The calculation formula is as follows:
[0066] Let num be the set of all spectral points contained in the num-th radial band; Step S43: Integrate the frequency band features of the radial frequency band as the frequency domain signature vector corresponding to the layered sub-point cloud. Frequency domain signature vector corresponding to the layered sub-point cloud It is expressed as follows:
[0067] T stands for transpose; Step S44: Integrate all frequency domain signature vectors of the radar frame to obtain the frequency domain signature matrix. : .
[0068] In step S5, determining the difference between the frequency domain signature matrix corresponding to the current radar frame acquired by the robot and the frequency domain signature matrix corresponding to the radar frame to be matched in the historical trajectory includes: Determine the frequency domain signature matrix corresponding to the current radar frame a acquired by the robot and the frequency domain signature matrix corresponding to the radar frame b to be matched in the historical trajectory. The difference between the frequency domain signature matrix of the current radar frame acquired by the robot and the frequency domain signature matrix of the radar frames to be matched in the historical trajectory. The calculation formula is as follows:
[0069] in, The weights for the difference in point cloud layers at the k-th layer are: The cosine distance between the signature vectors of the k-th layer point cloud;
[0070]
[0071] The function F(.) is a monotonically increasing function with a range of [0.8, 10]. The difference in the number of points in the k-th layer of the point cloud is given by the following: This represents the number of sub-point clouds in the k-th layer of the current radar frame a. This represents the number of sub-point clouds in the k-th layer of the radar frame b to be matched for historical trajectories. This is the frequency domain signature vector of the k-th layer sub-point cloud in the current radar frame a. is the frequency domain signature vector of the k-th layer sub-point cloud in the radar frame b to be matched for the historical trajectory.
[0072] Furthermore, the methods for determining the radar frames to be matched for the current radar frame and historical trajectories include: During the robot's movement, a distance threshold of 1m and an angle threshold of 10° are used. When the cumulative angle difference reaches the angle threshold or the cumulative displacement reaches the distance threshold, the current lidar point cloud is used as the current radar frame and marked as a key radar frame. All key radar frames recorded in the robot's historical trajectory are used as radar frames to be matched.
[0073] Further, in step S5, when the difference is less than a preset threshold, a loop closure relationship exists between the current radar frame and the radar frame to be matched. A loop closure constraint factor is determined and the robot pose is corrected. Based on the corrected robot pose, the robot's motion operation behavior is corrected, including: Step S51: When the difference is less than a preset threshold, there is a loop closure relationship between the current radar frame and the radar frame to be matched; based on the loop closure relationship, the loop closure constraint factor is determined, and the loop closure constraint factor is expressed as follows:
[0074] Where i represents the time corresponding to the current radar frame, and j represents the time corresponding to the radar frame to be matched. This represents the relative pose observation between the current radar frame and the radar frame to be matched. and These are the current radar frame pose and the radar frame pose to be matched, respectively. The weights of the lapsing constraint factors, Let be the displacement error between time i and time j. This is the information matrix for closure constraints; Step S52: Obtain the current airborne LiDAR positioning information, correct the robot's current pose based on the loop closure constraint factor; determine the corrected robot pose. The joint probability model is as follows: ,in, Let be the prior probability of the initial pose. The relative pose observations between the current radar frame and the radar frame to be matched are provided by the odometry factor. This is the set of constraint edges provided by the odometry factors in the factor graph. This is the set of constraint edges provided by the loop closure detection factor in the factor graph; Step S53: Correct the robot's motion behavior based on the corrected robot pose.
[0075] In this invention, it is assumed that the robot's poses at time i and time j are as follows:
[0076] in, Representing the rigid body transformation group in 3D space, including coordinates and orientation, the relative pose observations between two frames are obtained through direct point cloud matching:
[0077] The probabilistic model corresponding to the closure constraint factor is expressed as:
[0078] in, The normalization constant is For the closure constraint information matrix, The pose error between time i and time j is defined as:
[0079] Furthermore, the weights of the loop constraint factors can be determined by the descriptor matching difference:
[0080] in, The descriptor difference between the current radar frame and the radar frame to be matched. As the adjustment coefficient, the weighted laparoscopy constraint factor can be expressed as:
[0081] Obtain the current airborne LiDAR positioning information, correct the current airborne LiDAR positioning information based on the loop closure constraint factor, and let the robot's global pose set be:
[0082] By combining the odometer constraint factor and the loop closure detection constraint factor, a joint probability model is constructed: , in, This represents the relative pose transformation relationship between frames k and k+1 calculated from the laser odometry factor, transforming the above maximum a posteriori estimation problem into a nonlinear least squares problem:
[0083] in:
[0084] By solving the above optimization problem, the corrected robot pose is obtained. ; The robot's motion and operational behavior are corrected based on the corrected robot pose. First, the map update is guided by the corrected localization information. Let the point in the k-th frame of the point cloud be denoted as in the radar coordinate system. Its position in the world coordinate system It can be represented as:
[0085] The corrected pose will guide the map points to be updated. :
[0086] Based on the revised map, adjust the robot's motion control parameters and robot control inputs. It can be represented as:
[0087] Where M represents the prior map and G represents the target operation point.
[0088] like Figures 1-5 As shown, the present invention also provides a specific embodiment of a lidar loop closure detection method based on hierarchical frequency domain feature extraction.
[0089] This embodiment discloses a lidar loop closure detection method based on hierarchical frequency domain feature extraction, which can extract hierarchical frequency domain features of environmental point clouds in real time during robot movement, and use these features to detect loop closure relationships between movements. This facilitates the correction of positioning errors accumulated in long-term operation by the robot positioning system, and ensures the positioning robustness and accuracy of the robot system in long-term operation.
[0090] like Figure 1 As shown in the figure, the specific implementation steps of the lidar loop closure detection method based on hierarchical frequency domain feature extraction disclosed in this embodiment are as follows: S1, during the robot's movement, the original point cloud of the LiDAR... Preprocessing is performed. For each laser sampling point p, based on its coordinate values ( Calculate the horizontal radius r and the vertical distance h from the radar coordinate origin (y, z), retain points within the vertical range [-2m, 3m] and the radius range [1m, 80m], and obtain the range-filtered point cloud. .
[0091] S2, as Figure 2 As shown, for the range-filtered point cloud obtained in S1 The point cloud is divided into layers based on its height coordinates. The height range [-2m, 3m] is evenly divided into 5 height intervals according to the z-value of the coordinates. This allows for the layering of the filtered point cloud. The data is divided into 5 hierarchical sub-point clouds, where the k-th hierarchical sub-point cloud can be represented as:
[0092] S3, for each layer of sub-point cloud, transform its representation from a three-dimensional Cartesian coordinate system to a two-dimensional polar coordinate system. Taking the k-th layer of sub-point cloud as an example, for any point within it... Its corresponding two-dimensional polar coordinates It is calculated using the following formula:
[0093] Therefore, we can obtain the point set of the k-th layered sub-point cloud in polar coordinates. , can be represented as:
[0094] S4, as Figure 3 As shown in Figure 3-1, the two-dimensional polar coordinate representation of the layered sub-point cloud obtained in S3 is rasterized and a statistical matrix is calculated. The radius interval [1m, 80m] is uniformly divided into 60 radial intervals. As shown in Figure 3-2, the angular interval [0, ...] is divided into 60 radial intervals. The area is evenly divided into 60 angular intervals. For each point in the interval... Its radial index R and angular index S are defined as follows:
[0095] Based on the radial and angular indices, all points in the layered sub-point cloud are divided into raster point sets, each of which can be represented as:
[0096] The number of points within each grid point set is counted to form a hierarchical sub-point cloud polar coordinate distribution statistical map. , can be represented as:
[0097] S5, such as Figure 4 The statistical distribution of polar coordinates of the layered sub-point clouds obtained in S4. Energy intensity normalization is performed to obtain an energy-normalized height sub-point cloud statistical map. It can be calculated using the following formula:
[0098] Statistical plot of energy-normalized layered sub-point cloud Perform a two-dimensional Fast Fourier Transform (FFT) to obtain a hierarchical point cloud spectrum. It can be calculated using the following formula:
[0099] Layered point cloud spectrum Perform an equivalent Fourier transform centered on the zero-frequency component and extract its amplitude information graph. It can be calculated using the following formula:
[0100] S6, the amplitude information map of the layered point cloud spectrum obtained in S5. Perform frequency band division. Define the center of the amplitude information map. With normalized radius It can be calculated using the following formula:
[0101] in This represents the maximum normalized radius. For example... Figure 5 As shown, the information matrix is cropped, retaining only the mid-frequency band portion. ], where 5-1 represents the maximum frequency boundary of the mid-frequency band and 5-1 represents the minimum frequency boundary of the mid-frequency band.
[0102] S7: As Figure 5 As shown, the frequency bands in the amplitude information map of the layered point cloud spectrum obtained in S6 are divided into frequency bands, which are uniformly divided into 8 radial frequency bands as shown in 5-3. The energy statistics of a single radial frequency band are calculated as the current frequency band feature, where the energy statistics of the i-th radial frequency band are... It can be calculated using the following formula:
[0103] in Let represent the set of all spectral points contained in the i-th radial frequency band. The energy statistics of all frequency bands in the hierarchical sub-point cloud can be integrated into one. Frequency domain signature vector of dimension It can be calculated using the following formula:
[0104] The frequency domain signature vectors of all hierarchical sub-point clouds in the current frame can be represented as frequency domain signature matrices. , can be represented as:
[0105] S8, based on the frequency domain signature matrix obtained in S7 Calculate the inter-frame difference The similarity between any two frames a and b can be calculated using the following formula:
[0106] in The cosine distance between the signature vectors of the k-th layer point cloud is represented by the following formula:
[0107] and The weight representing the difference in the point cloud at layer k is calculated using the following formula:
[0108] Where F(.) is a monotonically increasing function with a range of [0.8, 10]. The difference in the number of points in the k-th layer of the point cloud is given by the following: This represents the number of sub-points in the k-th layer of the point cloud in frame a. This represents the number of sub-point clouds in the k-th layer of frame b.
[0109] S9. During the robot's movement, keyframes are constructed by filtering based on a distance threshold of 1m and an angle threshold of 10°. For each keyframe, a single-frame frequency domain signature matrix is calculated according to the methods described in steps one through seven. Whenever a new keyframe is generated, a weighted difference calculation is performed between the current keyframe frequency domain signature matrix and the frequency domain signature matrices of all historical keyframes. When the difference... If the value is greater than the threshold of 0.34, it is considered that there is no loop closure between the two frames; otherwise, it is considered that there is a loop closure between the two frames.
[0110] The present invention also provides a lidar loop closure detection device based on hierarchical frequency domain feature extraction, comprising: Data acquisition module: Configured for the robot to collect point cloud data of the current driving environment through the onboard LiDAR sensor during the movement, and obtain the original LiDAR point cloud. Each radar frame corresponds to one original LiDAR point cloud. Layered module: configured to divide the pre-processed raw LiDAR point cloud into multiple layers according to height, and represent the layered sub-point clouds in polar coordinates; Information extraction module: configured to rasterize the layered sub-point cloud represented in polar coordinates and calculate the corresponding polar coordinate distribution statistics map of the layered sub-point cloud; to perform energy intensity normalization and Fourier transform on the polar coordinate distribution statistics map in sequence, and to generate an amplitude information map based on the obtained layered point cloud spectrum map; Feature acquisition module: configured to divide the amplitude information map into multiple radial frequency bands, integrate the frequency band features of the radial frequency bands as the frequency domain signature vectors corresponding to the layered sub-point clouds; integrate all frequency domain signature vectors of the radar frame to obtain the frequency domain signature matrix; Correction module: configured to determine the difference between the frequency domain signature matrix corresponding to the current radar frame acquired by the robot and the frequency domain signature matrix corresponding to the radar frame to be matched in the historical trajectory; when the difference is less than a preset threshold, there is a loop relationship between the current radar frame and the radar frame to be matched, determine the loop constraint factor and correct the robot pose, and correct the robot's motion operation behavior based on the corrected robot pose.
[0111] The specific embodiments described above only illustrate the design principles of the present invention. The shapes and names of the components in this description may differ and are not limited. Therefore, those skilled in the art can modify or make equivalent substitutions to the technical solutions described in the foregoing embodiments; and these modifications and substitutions do not depart from the inventive spirit and technical solutions of the present invention, and should all fall within the protection scope of the present invention.
Claims
1. A lidar loop closure detection method based on hierarchical frequency domain feature extraction, characterized in that, include: Step S1: During the movement, the robot collects point cloud data of the current driving environment through the onboard LiDAR sensor to obtain the original LiDAR point cloud. Each radar frame corresponds to one original LiDAR point cloud. Step S2: Divide the preprocessed raw lidar point cloud into multiple layers according to height, and represent the layered sub-point clouds in polar coordinates; Step S3: Rasterize the layered sub-point cloud represented in polar coordinates and calculate the corresponding polar coordinate distribution statistics map of the layered sub-point cloud; perform energy intensity normalization and Fourier transform on the polar coordinate distribution statistics map in sequence, and generate an amplitude information map based on the obtained layered point cloud spectrum map. Step S4: Divide the amplitude information map into multiple radial frequency bands, integrate the frequency band features of the radial frequency bands as the frequency domain signature vectors corresponding to the layered sub-point clouds; integrate all frequency domain signature vectors of the radar frame to obtain the frequency domain signature matrix; Step S5: Determine the difference between the frequency domain signature matrix corresponding to the current radar frame acquired by the robot and the frequency domain signature matrix corresponding to the radar frame to be matched in the historical trajectory; when the difference is less than a preset threshold, there is a loop closure relationship between the current radar frame and the radar frame to be matched, determine the loop closure constraint factor and correct the robot pose, and correct the robot's motion operation behavior based on the corrected robot pose.
2. The method as described in claim 1, characterized in that, In step S2, the preprocessed raw lidar point cloud is divided into multiple layers according to height, including: Step S21: Select multiple laser sampling points from the original lidar point cloud. For each laser sampling point p1, calculate its coordinates ( , y, z), calculate the horizontal radius r1 and the height distance h of the laser sampling point from the origin of the airborne lidar coordinate system, and retain the h value within the height range [ , Within ] and the value of r1 is within the radius interval [ , Laser sampling points within the range are used to obtain the point cloud after range filtering. , as the preprocessed raw lidar point cloud; The horizontal radius r1 and the vertical distance h of the laser sampling point from the radar coordinate origin are calculated using the following formulas: y1 and z1 are the x-coordinate, y-coordinate, and height coordinates of the laser sampling point p1, respectively. , These represent the minimum and maximum heights of the LiDAR point cloud filtering range, respectively. , These represent the minimum and maximum radii of the LiDAR point cloud filtering range, respectively. The coordinates are the height coordinates of the origin of the airborne lidar coordinate system. Step S22: Set the height range [ , The preprocessed raw lidar point cloud within the [included area] is uniformly divided into Each height range has a span of [number] meters. for: Among them, the kth height interval Represented as: The portion of the original LiDAR point cloud corresponding to the kth altitude range is denoted as the kth layered sub-point cloud. The k-th layered sub-point cloud Represented as: For any point in the layered sub-point cloud, They are respectively The x-coordinate, y-coordinate, and height coordinate.
3. The method as described in claim 2, characterized in that, In step S2, the layered sub-point cloud is represented in polar coordinates, and the transformation formula is as follows: Where r is the radial coordinate of a point in the layered sub-point cloud in polar coordinates. The angular coordinates of the midpoint of the layered sub-point cloud in the polar coordinate system; The kth layered sub-point cloud Point set in polar coordinate system Represented as: in, for The corresponding two-dimensional polar coordinates.
4. The method as described in claim 3, characterized in that, In step S3, the layered sub-point cloud represented in polar coordinates is rasterized and the corresponding polar coordinate distribution statistics map of the layered sub-point cloud is calculated, including: radius interval [ , Evenly divided into A radial interval, and an angular interval [0, Evenly divided into Calculate the values of each point in the layered sub-point cloud represented in polar coordinates within each angular interval. The formulas for the corresponding radial interval dividing zone index value r2 and the angular interval dividing zone index value s2 are as follows: The hierarchical sub-point cloud represented in polar coordinates is rasterized, with each raster corresponding to a raster point set. It is expressed as follows: in, R represents the angular coordinate value, and R represents the directional coordinate value. The number of points in each grid cell of the hierarchical sub-point cloud represented in polar coordinates is counted to generate a polar coordinate distribution chart of the corresponding hierarchical sub-point cloud. : 。 5. The method as described in claim 4, characterized in that, In step S3, the polar coordinate distribution statistical map is subjected to energy intensity normalization and Fourier transform sequentially, and an amplitude information map is generated based on the obtained layered point cloud spectrum map, including: The energy intensity is normalized by performing energy intensity normalization on the polar coordinate distribution statistical map to obtain the normalized distribution statistical map. , Normalized distribution statistics Perform a Fourier transform to obtain the normalized distribution statistics. Corresponding hierarchical point cloud spectrum , in, Represents the statistical graph of the normalized distribution. Perform a Fourier transform. For data points in a normalized distribution statistical graph, the horizontal axis is s2 and the vertical axis is r2. It is an imaginary number. v represents the horizontal and vertical coordinates in the two-dimensional frequency domain, respectively. Spectral map of layered point cloud Perform an equivalent Fourier transform centered on the zero-frequency component to generate a layered point cloud spectrum. Corresponding amplitude information graph : 。 6. The method as described in claim 5, characterized in that, In step S4, the amplitude information map is divided into multiple radial frequency bands, and the frequency band features of the radial frequency bands are integrated as the frequency domain signature vectors corresponding to the layered sub-point clouds; the frequency domain signature matrix is obtained by integrating all frequency domain signature vectors of the radar frame, including: Step S41: Obtain amplitude information graph center With the corresponding normalized radius ; in, These are the x and y coordinates of the origin of a two-dimensional frequency domain coordinate system, respectively. The maximum spectral radius corresponding to the polar coordinate distribution statistical graph; the information matrix of the normalized radius is cropped, retaining the frequency bands in the information matrix of the normalized radius that are within [ Information between ]; , These represent the minimum and maximum values of the normalized radius distribution proportions, respectively. Step S42: Divide the amplitude information map evenly into For each radial frequency band, calculate the energy statistics of that radial frequency band. Use the energy statistics of the radial frequency band as the frequency band characteristic of the radial frequency band. Here, the energy statistics of the num-th radial frequency band... The calculation formula is as follows: Let num be the set of all spectral points contained in the num-th radial band; Step S43: Integrate the frequency band features of the radial frequency band as the frequency domain signature vector corresponding to the layered sub-point cloud. Frequency domain signature vector corresponding to the layered sub-point cloud It is expressed as follows: T stands for transpose; Step S44: Integrate all frequency domain signature vectors of the radar frame to obtain the frequency domain signature matrix. : .
7. The method as described in claim 6, characterized in that, In step S5, determining the difference between the frequency domain signature matrix corresponding to the current radar frame acquired by the robot and the frequency domain signature matrix corresponding to the radar frame to be matched in the historical trajectory includes: Determine the frequency domain signature matrix corresponding to the current radar frame a acquired by the robot and the frequency domain signature matrix corresponding to the radar frame b to be matched in the historical trajectory. The difference between the frequency domain signature matrix of the current radar frame acquired by the robot and the frequency domain signature matrix of the radar frames to be matched in the historical trajectory. The calculation formula is as follows: in, The weights for the difference in point cloud layers at the k-th layer are: The cosine distance between the signature vectors of the k-th layer point cloud; The function F(.) is a monotonically increasing function with a range of [0.8, 10]. The difference in the number of points in the k-th layer of the point cloud is given by the following: This represents the number of sub-point clouds in the k-th layer of the current radar frame a. This represents the number of sub-point clouds in the k-th layer of the radar frame b to be matched for historical trajectories. This is the frequency domain signature vector of the k-th layer sub-point cloud in the current radar frame a. is the frequency domain signature vector of the k-th layer sub-point cloud in the radar frame b to be matched for the historical trajectory.
8. The method as described in claim 7, characterized in that, In step S5, when the difference is less than a preset threshold, a loop closure relationship exists between the current radar frame and the radar frame to be matched. The loop closure constraint factor is determined and the robot pose is corrected. Based on the corrected robot pose, the robot's motion operation behavior is corrected, including: Step S51: When the difference is less than a preset threshold, there is a loop closure relationship between the current radar frame and the radar frame to be matched; based on the loop closure relationship, the loop closure constraint factor is determined, and the loop closure constraint factor is expressed as follows: Where i represents the time corresponding to the current radar frame, and j represents the time corresponding to the radar frame to be matched. This represents the relative pose observation between the current radar frame and the radar frame to be matched. and These are the current radar frame pose and the radar frame pose to be matched, respectively. The weights of the lapsing constraint factors, Let be the displacement error between time i and time j. This is the information matrix for closure constraints; Step S52: Obtain the current airborne LiDAR positioning information, correct the robot's current pose based on the loop closure constraint factor; determine the corrected robot pose. The joint probability model is as follows: ,in, Let be the prior probability of the initial pose. The relative pose observations between the current radar frame and the radar frame to be matched are provided by the odometry factor. This is the set of constraint edges provided by the odometry factors in the factor graph. This is the set of constraint edges provided by the loop closure detection factor in the factor graph; Step S53: Correct the robot's motion behavior based on the corrected robot pose.
9. A lidar loop closure detection device based on hierarchical frequency domain feature extraction, characterized in that, include: Data acquisition module: Configured for the robot to collect point cloud data of the current driving environment through the onboard LiDAR sensor during the movement, and obtain the original LiDAR point cloud. Each radar frame corresponds to one original LiDAR point cloud. Layered module: configured to divide the pre-processed raw LiDAR point cloud into multiple layers according to height, and represent the layered sub-point clouds in polar coordinates; Information extraction module: configured to rasterize the layered sub-point cloud represented in polar coordinates and calculate the corresponding polar coordinate distribution statistics map of the layered sub-point cloud; to perform energy intensity normalization and Fourier transform on the polar coordinate distribution statistics map in sequence, and to generate an amplitude information map based on the obtained layered point cloud spectrum map; Feature acquisition module: configured to divide the amplitude information map into multiple radial frequency bands, integrate the frequency band features of the radial frequency bands as the frequency domain signature vectors corresponding to the layered sub-point clouds; integrate all frequency domain signature vectors of the radar frame to obtain the frequency domain signature matrix; Correction module: configured to determine the difference between the frequency domain signature matrix corresponding to the current radar frame acquired by the robot and the frequency domain signature matrix corresponding to the radar frame to be matched in the historical trajectory; When the difference is less than a preset threshold, there is a loop relationship between the current radar frame and the radar frame to be matched. The loop constraint factor is determined and the robot pose is corrected. The robot's motion operation behavior is corrected based on the corrected robot pose.
10. An electronic device, characterized in that, The electronic device includes: A processor is used to execute multiple instructions; Memory, used to store multiple instructions; The plurality of instructions are to be stored in the memory and loaded by the processor and executed as described in any one of claims 1-8.