Search

CN-121784768-B - Laser radar SLAM map construction method and system based on hierarchical fusion

CN121784768BCN 121784768 BCN121784768 BCN 121784768BCN-121784768-B

Abstract

The invention relates to the technical field of laser radar map construction, discloses a laser radar SLAM map construction method and a system based on layered fusion, which solve the core problems of history observation information fracture, registration constraint unbalance caused by near-density and far-thinning of a laser radar, difficult coordination of retrieval efficiency and noise resistance robustness, out-of-control of long-term running memory and the like in the existing laser radar SLAM technology by constructing a coarse-fine double-layer voxel shared storage architecture and combining a time-space domain weighted fusion mechanism, a confidence level linkage neighborhood retrieval and a coarse granularity dynamic maintenance strategy, the high fidelity of the geometric details of the map is guaranteed through fine granularity voxels, the optimization of calculation efficiency and memory occupation is realized by means of reference aggregation and batch maintenance of coarse granularity voxels, meanwhile, multi-frame observation information is fused through multi-dimensional weighting factors, the accumulation of sensor noise and pose drift is obviously restrained, and the system can realize map construction with high precision, high efficiency and strong robustness in various scenes such as static, dynamic, weak textures and the like.

Inventors

  • Chang Dengxiang
  • XIAO YI
  • SUN CHAO
  • NIE JIANBO

Assignees

  • 安徽大学

Dates

Publication Date
20260508
Application Date
20260305

Claims (8)

  1. 1. The laser radar SLAM map construction method based on layered fusion is characterized by comprising the following steps of: s1, initializing a double-layer voxel map parameter according to a laser radar parameter and an application scene, wherein the parameter comprises fine-granularity voxel side length Edge length of coarse grain size voxel Weight threshold Map boundary threshold Neighborhood search radius ; S2, constructing a coarse-fine double-layer voxel structure based on a hash table, maintaining a reference list of subordinate non-empty fine-granularity voxels, and storing map point coordinates and weights by fine-granularity voxels to realize index linkage; in step S2, the mapping function from coordinates to voxel index is: ; Wherein, the For the map origin, V is the voxel side length of the corresponding level, M is the preset step constant, The voxel index is marked, and x, y and z are three-dimensional coordinates of the point cloud to be added under a world coordinate system; s3, performing motion distortion correction and self-adaptive downsampling on the original point cloud, and extracting representative points; S4, transforming the representative points to a world coordinate system based on the pose initial values, inquiring and obtaining a neighborhood point set of the representative points in the map by using coarse-granularity voxel indexes and a reference list, registering the representative points of the current frame by using the neighborhood point set, and optimizing the pose initial values to obtain accurate poses; s5, transforming the representative point to a world coordinate system based on the accurate pose, inquiring the corresponding fine-grained voxel, and updating the map point coordinate and the weight in the voxel through space-time weighted fusion if the voxel is occupied, wherein the space-time weighted fusion updates the map point coordinate and the weight in the voxel, and the formula is as follows: ; ; Wherein, the In order to update the coordinates of the object, In order for the weights to be updated, As the world coordinates of the historical fusion point, For the world coordinates of the new observation representative point, As the weight of the historical fusion point, As a time-decay factor, In order to spatially distribute the equalization factors, For the side length scaling factor of fine granularity and coarse granularity voxels, Observing confidence factors for new points S6, monitoring map boundaries in real time, and deleting out-of-date data in batches by taking coarse-granularity voxels as a unit when the map boundaries exceed a threshold value; And S7, periodically executing closed loop detection, and if the closed loop is detected, backtracking and correcting the weighted updating result of the latest multi-frame map point according to the pose of the closed loop.
  2. 2. The laser radar SLAM map construction method based on hierarchical fusion according to claim 1, wherein in step S4, the query process of the neighborhood point set comprises: Firstly, calculating corresponding coarse-granularity voxel indexes according to the representative points after the pose initial value transformation; then traversing the coarse-grained voxels and coarse-grained voxels within the adjacent 3 x 3 neighborhood; then accessing fine-grained voxels according to the reference list to obtain corresponding map points; And finally, carrying out spatial distance screening based on the neighborhood searching radius, namely calculating Euclidean distance between each candidate neighborhood point and the projection representative point, and only reserving the points with the distance smaller than or equal to the neighborhood searching radius to form a final neighborhood point set.
  3. 3. The method of claim 1, wherein in step S6, the map boundary monitoring calculates the offset of the map center relative to the initial origin by periodically counting the coordinate range of the update point by the independent thread And according to Batch release beyond preset map boundaries Coarse-grained voxel memory of (c).
  4. 4. The laser radar SLAM map construction method based on hierarchical fusion according to claim 1, wherein in step S1, a fine-grained voxel side length calculation formula is: ; Wherein, the The average distance between the laser radar point clouds is; the calculation formula of the coarse-grain voxel side length is as follows: ; ; Wherein, the For a neighborhood search radius, N is a voxel granularity scaling factor.
  5. 5. The laser radar SLAM map construction method based on hierarchical fusion according to claim 1, wherein in step S3, representative point extraction further comprises an outlier rejection step, and points with distances greater than 3 times of standard deviation are rejected by calculating the average distance and standard deviation of K nearest neighbors of each candidate representative point.
  6. 6. The method for constructing a laser radar SLAM map based on hierarchical fusion according to claim 1, wherein said spatial distribution equalization factor The calculation formula of (2) is as follows: ; Wherein, the For the point cloud density within the current voxel, As a global average density of the particles, Is a very small value.
  7. 7. The method for constructing a laser radar SLAM map based on hierarchical fusion according to claim 1, wherein in step S7, the object of backtracking correction is the map point of the last 5 frames, and the space-time weighted fusion updating in step S5 is re-executed through the optimized pose.
  8. 8. The laser radar SLAM map construction system based on layered fusion for realizing the method of any one of claims 1-7 is characterized by comprising a double-layer voxel map storage module, a point cloud preprocessing module, a space-time weighting updating module, a confidence level linkage neighborhood retrieval module, a coarse granularity dynamic maintenance module and a map maintenance module which are integrated on an embedded computing platform, wherein the modules realize real-time interaction through a data bus.

Description

Laser radar SLAM map construction method and system based on hierarchical fusion Technical Field The invention relates to the technical field of laser radar map building, in particular to a laser radar SLAM map building method and system based on hierarchical fusion. Background Synchronous positioning and mapping (SLAM) are the basis for realizing autonomous mobile robot environment sensing, path planning and motion control, and a laser radar (LiDAR) has become a core sensor of an SLAM system because of the advantages of high-precision ranging capability, strong environment adaptability, insensitivity to illumination change and the like, and in a LIDAR SLAM method based on point cloud distribution registration, the quality of a map directly determines the precision and robustness of pose estimation, so that the calculation efficiency and map details are considered, and the existing system generally adopts a voxelized strategy to organize and compress point clouds. The method comprises the steps of calculating curvature of each point of single-frame laser radar scanning data, screening points with remarkable geometric structures according to preset high-low curvature thresholds to serve as laser point cloud characteristic data, carrying out de-distortion processing on the characteristic points by using pose data provided by an inertial sensor, specifically, carrying out pose increment between the scanning moment and the frame starting moment of the points, backing the points to correction positions under a unified timestamp to obtain laser point cloud correction data, downsampling the corrected characteristic points into a sparse point set, estimating an accurate pose matrix of equipment in a world coordinate system by a filtering or optimizing method in combination with current pose priori, projecting the corrected characteristic points to the world coordinate system through pose transformation, and adding the batched points to the laser point cloud map if the distance between the current equipment position and the position of the last map exceeds the preset threshold, wherein the sparse points are also used for constructing a sparse map for follow-up pose tracking. For the above and related art, there are often the following drawbacks: 1. Because the laser radar SLAM needs to be frequently subjected to dynamic switching of a hierarchical structure in long-time operation, and the physical characteristics of the laser radar such as close, close and far are not effectively controlled, the historical observation information of multi-frame space-time fusion is broken, and meanwhile, the registration is restrained in different areas to be unbalanced in strength, so that the situations of continuous decline of geometric accuracy of a map and insufficient long-time operation stability are finally caused; 2. because the neighborhood retrieval mechanism of the existing laser radar SLAM depends on static space division, the multi-frame fused point cloud confidence coefficient and the index are not dynamically bound, and strict resource limitation exists in application scenes such as an embedded platform, so that a large number of low-confidence point clouds need to be traversed during retrieval, single-frame noise interference cannot be avoided, the calculation cost of index maintenance is difficult to control, and finally the situation that the retrieval efficiency and the noise resistance robustness are difficult to cooperatively meet the actual application requirements is caused. Disclosure of Invention The technical problem to be solved by the invention is that the prior art has the defect that the laser radar SLAM structure switching unbinding confidence coefficient and the precision efficiency is difficult to reach the standard, and therefore, the invention provides a laser radar SLAM map construction method and system based on layered fusion. In order to achieve the purpose, the application adopts the following technical scheme that the laser radar SLAM map construction method based on layered fusion comprises the following steps: step 1, initializing a double-layer voxel map parameter according to laser radar parameters and application scenes, wherein the parameter comprises fine-granularity voxel side lengths Edge length of coarse grain size voxelWeight thresholdMap boundary thresholdNeighborhood search radius; Step 2, constructing a coarse-fine double-layer voxel structure based on a hash table, maintaining a reference list of subordinate non-empty fine-granularity voxels, and storing map point coordinates and weights by fine-granularity voxels to realize index linkage; step 3, performing motion distortion correction and self-adaptive downsampling on the original point cloud, and extracting representative points; step 4, transforming the representative points to a world coordinate system based on the pose initial values, and inquiring and acquiring a neighborhood point set of the representative points in th