A mapping method, system, and computer program based on incremental point cloud registration.
By using incremental point cloud registration and voxel map updates, the positioning accuracy problem of the AGV system when switching between indoor and outdoor environments was solved, achieving efficient and real-time positioning and environmental adaptation, and enhancing the autonomy and intelligence of the AGV.
Patent Information
- Application Number
- CN202510588744.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-05-08
- Publication Date
- 2026-11-10
AI Technical Summary
Existing AGV systems have low positioning accuracy when switching between indoor and outdoor environments. Existing 3D point cloud images cannot be updated in a timely manner, leading to positioning errors. Furthermore, existing methods cannot cope with unknown environments and have low scalability.
An incremental point cloud registration-based mapping method is adopted. By acquiring keyframes and constructing voxel maps, point clouds scanned by LiDAR are used for registration, the voxel maps are updated incrementally, and density clustering and quasi-Newton methods are combined to optimize vehicle pose.
It improves the positioning accuracy and operating efficiency of the AGV system, reduces computational consumption, achieves real-time positioning, adapts to environmental changes, and enhances the autonomy and intelligence of AGVs in unknown environments.
Smart Images

Figure CN123071579A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of visual processing, and in particular to a mapping method, system, and computer program based on incremental point cloud registration. Background Technology
[0002] In traditional AGV (Automated Guided Vehicle) systems, vehicle positioning modules mostly use magnetic strips, 2D laser navigation, or cameras to acquire two-dimensional images of the surrounding environment. In indoor environments, these two-dimensional images can achieve positioning normally; however, once switched to outdoor environments, these methods suffer from difficulties in deployment, susceptibility to failure, and low accuracy, severely impacting the normal operation of the entire AGV system.
[0003] Current AGV operating scenarios are highly variable, often requiring stable operation in both indoor and outdoor (dynamic and occluded) environments. Existing technologies typically achieve positioning by pre-collecting point cloud information and constructing a 3D point cloud image. However, this method requires pre-built maps, cannot cope with unknown environments, and has low scalability. Furthermore, when the surrounding environment changes, the 3D point cloud image cannot be updated in a timely manner, leading to positioning errors. Summary of the Invention
[0004] In view of the above-mentioned shortcomings of the current technology, the present invention provides a mapping method based on incremental point cloud registration, which obtains key frames by registering the point cloud scanned by lidar, thereby incrementally updating the voxel map.
[0005] To achieve the above objectives, the embodiments of the present invention adopt the following technical solutions: A mapping method based on incremental point cloud registration includes the following steps: Obtain the initial frame point cloud and the initial frame vehicle pose; Use the initial frame as a keyframe to construct an initial local voxel map; The next frame of point cloud is repeatedly acquired as the current frame. Based on point cloud registration and vehicle pose iteration optimization, key frames are acquired, and the voxel map is updated incrementally.
[0006] According to one aspect of the present invention, the mapping method based on incremental point cloud registration further includes: A density-based clustering method is used to denoise the point cloud data.
[0007] According to one aspect of the present invention, the denoising process of point cloud data using a density-based clustering method includes: Set the neighborhood radius and the minimum number of neighborhood points for the core point; Starting from any unvisited data point, traverse the dataset and determine whether the point is a core point or a noise point; Starting with a core point, recursively add all core points within its neighborhood radius to the cluster until it can no longer be expanded.
[0008] Remove noise points that are not in the cluster.
[0009] According to one aspect of the present invention, constructing an initial local voxel map by using the initial frame as a keyframe includes: The initial frame is used as the key frame, and the point cloud of the key frame is voxelized and filtered at a fixed resolution to construct an initial local voxel map.
[0010] According to one aspect of the present invention, the step of repeatedly acquiring the next frame of point cloud as the current frame, acquiring key frames based on point cloud registration and vehicle pose iterative optimization, and incrementally updating the voxel map includes: Obtain the point cloud of the next frame as the current frame, project its point cloud onto the voxel map, and obtain the equation to be optimized for the current pose; The quasi-Newton method is used to iterate the equation to be optimized for the current pose until convergence, and then the current pose is updated. Compare the current pose with the vehicle pose of the previous keyframe. If the change in the current pose exceeds a preset threshold, then the current frame is used as a keyframe and added to the voxel map. Repeat the above steps to incrementally update the voxel map.
[0011] According to one aspect of the present invention, obtaining the next frame point cloud as the current frame, projecting its point cloud onto a voxel map, and obtaining the current pose equation to be optimized includes: Obtain the Gaussian distribution of the point cloud in each voxel of the voxel map, and get the mean and variance of all voxels; Infer the predicted vehicle pose in the current frame based on the vehicle pose in the previous frame; Based on the predicted pose of the vehicle in the current frame, the mean and variance of all voxels, obtain the residual between the point cloud in the current frame and the corresponding voxels. Based on the predicted vehicle pose in the current frame and the residual between the current frame point cloud and the corresponding voxel, the equation to be optimized for the current pose is obtained.
[0012] According to one aspect of the present invention, the step of iterating the equation to be optimized for the current pose using a quasi-Newton method until convergence and then updating the current pose includes: Choose an initial point and calculate the gradient at that point; Update the gradient direction using an approximation of the Hessian matrix; Perform a line search along the updated gradient direction to find a new point; Update the approximate matrix of the Hessian matrix using the new points and gradient information; Repeatedly update the points and gradients until the convergence condition is met, and then take the current point as the optimal solution.
[0013] According to one aspect of the present invention, the preset threshold is: The threshold for the change in distance between the current frame and the previous keyframe is 1m, or the threshold for the change in angle is 15 degrees.
[0014] A mapping system based on incremental point cloud registration, based on the mapping method based on incremental point cloud registration as described above, includes: The initial data acquisition module is used to acquire the initial frame point cloud and the initial frame vehicle pose; The initial map building module is used to construct an initial local voxel map by using the initial frame as a keyframe. The iteration module is used to repeatedly acquire the next frame of point cloud as the current frame, and iteratively optimize the point cloud registration and vehicle pose to acquire key frames and incrementally update the voxel map.
[0015] A computer program, when executed, implements the steps of the mapping method based on incremental point cloud registration as described above.
[0016] Advantages of implementing this invention: This invention provides a mapping method based on incremental point cloud registration. By registering the point cloud scanned by LiDAR, key frames are obtained, thereby incrementally updating the voxel map. By selecting key frames to update the voxel map, the time spent finding nearest neighbors, extracting point cloud features, and reconstructing local maps is eliminated when there are many point clouds. This significantly reduces computing power consumption, improves the operating efficiency of the positioning module, and optimizes the speed, thus achieving real-time results and enabling AGVs to work more stably and efficiently. At the same time, due to the use of the voxelization method, high matching accuracy can still be achieved when the environment does not change significantly, and the positioning results fluctuate little. Attached Figure Description
[0017] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. Obviously, the 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.
[0018] Figure 1 This is a flowchart of a mapping method based on incremental point cloud registration as described in this invention; Figure 2 Keyframes are selected when the scene changes described in this invention exceed a threshold. Detailed Implementation
[0019] 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 embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0020] Example 1
[0021] like Figure 1 As shown, a mapping method based on incremental point cloud registration includes the following steps: S1: Obtain the initial frame point cloud and the initial frame vehicle pose.
[0022] In practical applications, the vehicle positioning module of an AGV (Automated Guided Vehicle) uses LiDAR to scan the surrounding environment and acquire point cloud data. A point cloud frame refers to the set of points formed after the LiDAR completes a full scan (usually 360 degrees). This point cloud data is obtained by the LiDAR receiving and processing the laser beam emitted by the LiDAR after it interacts with objects in the surrounding environment.
[0023] The initial frame vehicle pose consists of several pre-set fixed poses for the vehicle.
[0024] S2: Use the initial frame as a keyframe to construct an initial local voxel map.
[0025] When implementing this method, key frames are selected from the frames scanned and obtained by the vehicle positioning module to construct a voxel map.
[0026] Step S2 includes: The initial frame is used as a keyframe, and the point cloud of the keyframe is voxelized and filtered at a fixed resolution to construct an initial local voxel map. The fixed resolution can be set according to the actual situation.
[0027] The specific principle of voxelization filtering is to downsample and smooth point cloud data based on a voxel lattice. The three-dimensional space is divided into a series of fixed-size cubes (i.e., voxels), and then the point cloud data within each voxel is processed. Typically, a representative point within the voxel (such as the average point, the center point, or the point closest to the center) is selected as the representative point of that voxel. In this way, the amount of point cloud data can be effectively reduced, computational complexity can be lowered, and the main features of the point cloud can be preserved.
[0028] Voxel mapping represents objects and obstacles in an environment by dividing three-dimensional space into a series of fixed-size cubic units (voxels) and encoding the state of each voxel. Each voxel can store information about its occupancy state, color, weight, or other relevant attributes. In this way, voxel mapping can represent complex environments in three-dimensional space in a concise and efficient manner.
[0029] In the field of vehicle navigation and positioning, voxel mapping is a crucial method for describing 3D environments. By dividing 3D space into regular volumetric units and describing the 3D environment based on the occupancy status of these units, vehicles can perceive and understand their surroundings. Vehicles can acquire data about their environment through sensors (such as LiDAR and cameras) and convert this data into occupancy statuses in a voxel map. This allows vehicles to perceive changes in their surroundings in real time, including the position, shape, and size of objects. Based on voxel maps, vehicles can perform tasks such as path planning, navigation, positioning, and obstacle avoidance, improving their autonomy and intelligence.
[0030] In practical applications, when a vehicle faces an unknown environment, the vehicle system does not store a voxel map of the current environment. Therefore, it is necessary to construct a voxel map starting from the currently acquired initial frame. In addition, the vehicle may also store a partial map of the current environment, and there may still be unknown environments. This method can also incrementally update the voxel map based on the vehicle's existing voxel map.
[0031] S3: Repeatedly acquire the next frame of point cloud as the current frame, and acquire key frames based on point cloud registration and vehicle pose iteration optimization, and incrementally update the voxel map.
[0032] Step S3 includes: S31: Obtain the point cloud of the next frame as the current frame, project its point cloud onto the voxel map, and obtain the equation to be optimized for the current pose.
[0033] Step S31 includes: (1) Obtain the Gaussian distribution of the point cloud in each voxel of the voxel map, and obtain the mean μ of all voxels. i Sum of variances ∑ i .
[0034] The mean is the central location of a Gaussian distribution and represents the expected value of a random variable. Variance is a measure of the dispersion of data distribution, representing the average squared distance between each data point and the mean. Here, i represents the i-th voxel.
[0035] (2) Infer the predicted vehicle pose in the current frame based on the vehicle pose in the previous frame.
[0036] Typically, the initial predicted pose of the point cloud in the current frame is inferred using a uniform velocity model based on the pose of the point cloud in the previous frame, including the rotation R and translation t of the current frame in the initial local voxel map.
[0037] The uniform speed model assumes that the vehicle moves at a constant speed between consecutive frames. By obtaining the pose of the previous frame (usually including position and orientation), the vehicle speed is obtained, the time interval between two frames is determined, and then the uniform speed model is used to predict the position of the current frame.
[0038] (3) Based on the vehicle's predicted pose in the current frame and the mean μ of all voxels i Sum of variances ∑ i Obtain the residual e between the current frame point cloud and the corresponding voxel. i .
[0039] Specifically, the residual e of the i-th voxel i The calculation method is as follows:
[0040] Where R and t represent the rotation and translation of the current frame in the initial local voxel map, respectively; q i This represents a point in the registered frame point cloud.
[0041] (4) Based on the vehicle's predicted pose in the current frame, the residual e between the current frame point cloud and the corresponding voxel i We obtain the equation to be optimized for the current pose.
[0042] Specifically, the equation to be optimized is:
[0043] S32: Iterate the equation to be optimized for the current pose using the quasi-Newton method (Broyden–Fletcher–Goldfarb–Shanno, BFGS method) until convergence, and then update the current pose.
[0044] Step S32 includes: (1) Select an initial point and calculate the gradient at that point; (2) Update the gradient direction using an approximation of the Hesser matrix; (3) Perform a line search along the updated gradient direction to find a new point; (4) Update the approximate matrix of the Hessian matrix using the new point and gradient information; (5) Repeatedly update the point and gradient until the convergence condition is met, and take the current point as the optimal solution.
[0045] Specifically, the BFGS method is used to iterate the vehicle's predicted pose in the current frame. The pose is updated after the convergence condition is met. The following is an approximate method for calculating the second-order Hesser matrix to improve the overall algorithm efficiency: (1) Take , , , making .in, For Jacobi matrix, Let k be the pose and k be the number of iterations.
[0046] (2) Select the Heather matrix The approximate substitution matrix, with the iterative formula as follows:
[0047] in, ; I is the identity matrix.
[0048] S33: Compare the current pose with the vehicle pose of the previous keyframe. If the change in the current pose exceeds a preset threshold, then the current frame is added to the voxel map as a keyframe.
[0049] Using the current pose obtained in step S32, calculate the changes in distance and angle between the current pose and the vehicle pose in the previous keyframe. If the changes exceed a preset threshold, it is considered that the scene has changed significantly. The current frame is then used as a keyframe, and the keyframe point cloud is voxelized and filtered at a fixed resolution and added to the voxel map.
[0050] In practical applications, the pose change threshold can be set as follows: The threshold for distance change between the current frame and the previous keyframe is 1 meter, or the threshold for angle change is 15 degrees. When the distance change in vehicle pose exceeds 1 meter or the angle change exceeds 15 degrees, a keyframe is selected, and the voxel map is updated. For example, ... Figure 2 As shown, keyframes were selected at four pink points. The distance between the two pink points on the left exceeds 1m, and the angle between the two pink points on the right changes by more than 15 degrees.
[0051] When incrementally updating voxel maps using frame point clouds, only key frames with significant scene changes are selected. This can greatly reduce the amount of computation, improve the map update speed, and achieve a real-time effect.
[0052] S34: Repeat the above steps to incrementally update the voxel map.
[0053] After adding keyframes to the voxel map, update the voxel mean μ. i Sum of variances ∑ i This will facilitate the next update.
[0054] The formula for merging and updating voxel mean is:
[0055] The formula for combining and updating voxel variances is:
[0056] Where m and n are the sequence numbers of the existing voxels and the voxels to be updated, respectively; x and y are the existing voxels and the voxels to be updated, respectively; H and A are the existing voxels and the voxels to be updated, respectively.
[0057] In practical applications, when a vehicle is in a large, unknown environment, this method can determine its own position without relying on information from external measurements. Instead, it can rely on its own sensors to determine the increment of its own position, thereby determining the vehicle's position in the environmental map. At the same time, based on the position and the sensor data of the current position, an environmental voxel map is incrementally built.
[0058] The beneficial effects of this embodiment are as follows: This method, through incremental point cloud matching, can register the point cloud scanned by the LiDAR, obtain key frames, and thus incrementally update the voxel map; by selecting key frames to update the voxel map, the time spent finding nearest neighbors, extracting point cloud features, and reconstructing local maps when there are many point clouds can be eliminated, which can significantly reduce computing power consumption, improve the operating efficiency of the positioning module, and optimize the speed, thereby achieving real-time effect and allowing the AGV to work more stably and efficiently; at the same time, due to the use of the voxelization method, high matching accuracy can still be achieved when the environment does not change significantly, and the positioning results fluctuate little.
[0059] Example 2
[0060] like Figure 1 As shown, a mapping method based on incremental point cloud registration includes the following steps: S1: Obtain the initial frame point cloud and the initial frame vehicle pose.
[0061] In practical applications, the vehicle positioning module of an AGV (Automated Guided Vehicle) uses LiDAR to scan the surrounding environment and acquire point cloud data. A point cloud frame refers to the set of points formed after the LiDAR completes a full scan (usually 360 degrees). This point cloud data is obtained by the LiDAR receiving and processing the laser beam emitted by the LiDAR after it interacts with objects in the surrounding environment.
[0062] Due to factors such as instrument errors, environmental interference, and the characteristics of the scanned target itself, point cloud data will contain noisy points. These noisy points increase the data volume and reduce data quality. Denoising can remove these irrelevant noise points and improve the overall quality of the point cloud data.
[0063] In practical applications, methods such as bilateral filtering, KD-Tree, and the density-based clustering method DBSCAN (Density-Based Spatial Clustering of Applications with Noise) can be used to denoise point cloud data.
[0064] DBSCAN can discover clusters of arbitrary shapes and effectively handle noisy data. Specifically, the processing steps of DBSCAN include: (1) Set the neighborhood radius ε and the minimum number of neighborhood points MinPts of the core point.
[0065] The choice of ε and MinPts has a significant impact on the clustering results; improper selection may lead to poor clustering performance.
[0066] (2) Starting from any unvisited data point, traverse the dataset and determine whether the point is a core point or a noise point.
[0067] Specifically, if there are at least MinPts points within the neighborhood radius ε of a point, then the point is marked as a core point; otherwise, it is marked as a noise point (which may be marked as a boundary point later).
[0068] (3) Starting from the core point, recursively add all core points within its neighborhood radius to the cluster until it can no longer be expanded.
[0069] (4) Remove noise points that are not in the cluster.
[0070] The initial frame vehicle pose consists of several pre-set fixed poses for the vehicle.
[0071] S2: Use the initial frame as a keyframe to construct an initial local voxel map.
[0072] When implementing this method, key frames are selected from the frames scanned and obtained by the vehicle positioning module to construct a voxel map.
[0073] Step S2 includes: The initial frame is used as a keyframe, and the point cloud of the keyframe is voxelized and filtered at a fixed resolution to construct an initial local voxel map. The fixed resolution can be set according to the actual situation.
[0074] The specific principle of voxelization filtering is to downsample and smooth point cloud data based on a voxel lattice. The three-dimensional space is divided into a series of fixed-size cubes (i.e., voxels), and then the point cloud data within each voxel is processed. Typically, a representative point within the voxel (such as the average point, the center point, or the point closest to the center) is selected as the representative point of that voxel. In this way, the amount of point cloud data can be effectively reduced, computational complexity can be lowered, and the main features of the point cloud can be preserved.
[0075] Voxel mapping represents objects and obstacles in an environment by dividing three-dimensional space into a series of fixed-size cubic units (voxels) and encoding the state of each voxel. Each voxel can store information about its occupancy state, color, weight, or other relevant attributes. In this way, voxel mapping can represent complex environments in three-dimensional space in a concise and efficient manner.
[0076] In the field of vehicle navigation and positioning, voxel mapping is a crucial method for describing 3D environments. By dividing 3D space into regular volumetric units and describing the 3D environment based on the occupancy status of these units, vehicles can perceive and understand their surroundings. Vehicles can acquire data about their environment through sensors (such as LiDAR and cameras) and convert this data into occupancy statuses in a voxel map. This allows vehicles to perceive changes in their surroundings in real time, including the position, shape, and size of objects. Based on voxel maps, vehicles can perform tasks such as path planning, navigation, positioning, and obstacle avoidance, improving their autonomy and intelligence.
[0077] In practical applications, when a vehicle faces an unknown environment, the vehicle system does not store a voxel map of the current environment. Therefore, it is necessary to construct a voxel map starting from the currently acquired initial frame. In addition, the vehicle may also store a partial map of the current environment, and there may still be unknown environments. This method can also incrementally update the voxel map based on the vehicle's existing voxel map.
[0078] S3: Repeatedly acquire the next frame of point cloud as the current frame, and acquire key frames based on point cloud registration and vehicle pose iteration optimization, and incrementally update the voxel map.
[0079] Step S3 includes: S31: Obtain the point cloud of the next frame as the current frame, project its point cloud onto the voxel map, and obtain the equation to be optimized for the current pose.
[0080] Step S31 includes: (1) Obtain the Gaussian distribution of the point cloud in each voxel of the voxel map, and obtain the mean μ of all voxels. i Sum of variances ∑ i .
[0081] The mean is the central location of a Gaussian distribution and represents the expected value of a random variable. Variance is a measure of the dispersion of data distribution, representing the average squared distance between each data point and the mean. Here, i represents the i-th voxel.
[0082] (2) Infer the predicted vehicle pose in the current frame based on the vehicle pose in the previous frame.
[0083] Typically, the initial predicted pose of the point cloud in the current frame is inferred using a uniform velocity model based on the pose of the point cloud in the previous frame, including the rotation R and translation t of the current frame in the initial local voxel map.
[0084] The uniform speed model assumes that the vehicle moves at a constant speed between consecutive frames. By obtaining the pose of the previous frame (usually including position and orientation), the vehicle speed is obtained, the time interval between two frames is determined, and then the uniform speed model is used to predict the position of the current frame.
[0085] (3) Based on the vehicle's predicted pose in the current frame and the mean μ of all voxels i Sum of variances ∑ i Obtain the residual e between the current frame point cloud and the corresponding voxel. i .
[0086] Specifically, the residual e of the i-th voxel i The calculation method is as follows:
[0087] Where R and t represent the rotation and translation of the current frame in the initial local voxel map, respectively; q i This represents a point in the registered frame point cloud.
[0088] (4) Based on the vehicle's predicted pose in the current frame, the residual e between the current frame point cloud and the corresponding voxel i We obtain the equation to be optimized for the current pose.
[0089] Specifically, the equation to be optimized is:
[0090] S32: Iterate the equation to be optimized for the current pose using the quasi-Newton method (Broyden–Fletcher–Goldfarb–Shanno, BFGS method) until convergence, and then update the current pose.
[0091] Step S32 includes: (1) Select an initial point and calculate the gradient at that point; (2) Update the gradient direction using an approximation of the Hesser matrix; (3) Perform a line search along the updated gradient direction to find a new point; (4) Update the approximate matrix of the Hessian matrix using the new point and gradient information; (5) Repeatedly update the point and gradient until the convergence condition is met, and take the current point as the optimal solution.
[0092] Specifically, the BFGS method is used to iterate the vehicle's predicted pose in the current frame. The pose is updated after the convergence condition is met. The following is an approximate method for calculating the second-order Hesser matrix to improve the overall algorithm efficiency: (1) Take , , , making .in, For Jacobi matrix, Let k be the pose and k be the number of iterations.
[0093] (2) Select the Heather matrix The approximate substitution matrix, with the iterative formula as follows:
[0094] in, ; I is the identity matrix.
[0095] S33: Compare the current pose with the vehicle pose of the previous keyframe. If the change in the current pose exceeds a preset threshold, then the current frame is added to the voxel map as a keyframe.
[0096] Using the current pose obtained in step S32, calculate the changes in distance and angle between the current pose and the vehicle pose in the previous keyframe. If the changes exceed a preset threshold, it is considered that the scene has changed significantly. The current frame is then used as a keyframe, and the keyframe point cloud is voxelized and filtered at a fixed resolution and added to the voxel map.
[0097] In practical applications, the pose change threshold can be set as follows: The threshold for distance change between the current frame and the previous keyframe is 1 meter, or the threshold for angle change is 15 degrees. When the distance change in vehicle pose exceeds 1 meter or the angle change exceeds 15 degrees, a keyframe is selected, and the voxel map is updated. For example, ... Figure 2 As shown, keyframes were selected at four pink points. The distance between the two pink points on the left exceeds 1m, and the angle between the two pink points on the right changes by more than 15 degrees.
[0098] When incrementally updating voxel maps using frame point clouds, only key frames with significant scene changes are selected. This can greatly reduce the amount of computation, improve the map update speed, and achieve a real-time effect.
[0099] S34: Repeat the above steps to incrementally update the voxel map.
[0100] After adding keyframes to the voxel map, update the voxel mean μ. i Sum of variances ∑ i This will facilitate the next update.
[0101] The formula for merging and updating voxel mean is:
[0102] The formula for combining and updating voxel variances is:
[0103] Where m and n are the sequence numbers of the existing voxels and the voxels to be updated, respectively; x and y are the existing voxels and the voxels to be updated, respectively; H and A are the existing voxels and the voxels to be updated, respectively.
[0104] In practical applications, when a vehicle is in a large, unknown environment, this method can determine its own position without relying on information from external measurements. Instead, it can rely on its own sensors to determine the increment of its own position, thereby determining the vehicle's position in the environmental map. At the same time, based on the position and the sensor data of the current position, an environmental voxel map is incrementally built.
[0105] The beneficial effects of this embodiment are as follows: This method also utilizes the DBSCAN method to denoise the point cloud data, thereby improving the overall quality of the point cloud data and thus improving the accuracy of the voxel map.
[0106] Example 3
[0107] A mapping system based on incremental point cloud registration, based on the mapping method based on incremental point cloud registration as described in Embodiment 1 or 2, includes: The initial data acquisition module is used to acquire the initial frame point cloud and the initial frame vehicle pose; The initial map building module is used to construct an initial local voxel map by using the initial frame as a keyframe. The iteration module is used to repeatedly acquire the next frame of point cloud as the current frame, and iteratively optimize the point cloud registration and vehicle pose to acquire key frames and incrementally update the voxel map.
[0108] Preferably, the system further includes: The denoising module is used to denoise point cloud data using a density-based clustering method.
[0109] Example 4
[0110] A computer program, when executed, implements the steps of the mapping method based on incremental point cloud registration as described in Embodiment 1 or 2.
[0111] Example 5
[0112] A computer program product comprising a computer program that, when executed, implements the steps of the mapping method based on incremental point cloud registration as described in Embodiment 1 or 2.
[0113] Example 6
[0114] A readable storage medium storing a computer program as described in Embodiment 4, wherein when executed, the computer program implements the steps of the mapping method based on incremental point cloud registration as described in Embodiment 1 or 2.
[0115] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.
Claims
1. A mapping method based on incremental point cloud registration, characterized in that, Includes the following steps: Obtain the initial frame point cloud and the initial frame vehicle pose; Use the initial frame as a keyframe to construct an initial local voxel map; The next frame of point cloud is repeatedly acquired as the current frame. Based on point cloud registration and vehicle pose iteration optimization, key frames are acquired, and the voxel map is updated incrementally.
2. The mapping method based on incremental point cloud registration according to claim 1, characterized in that, The mapping method based on incremental point cloud registration also includes: A density-based clustering method is used to denoise the point cloud data.
3. The mapping method based on incremental point cloud registration according to claim 2, characterized in that, The denoising process for point cloud data using a density-based clustering method includes: Set the neighborhood radius and the minimum number of neighborhood points for the core point; Starting from any unvisited data point, traverse the dataset and determine whether the point is a core point or a noise point; Starting from the core point, recursively add all core points within its neighborhood radius to the cluster until it can no longer be expanded; Remove noise points that are not in the cluster.
4. The mapping method based on incremental point cloud registration according to claim 1, characterized in that, The step of using the initial frame as a keyframe to construct the initial local voxel map includes: The initial frame is used as the key frame, and the point cloud of the key frame is voxelized and filtered at a fixed resolution to construct an initial local voxel map.
5. The mapping method based on incremental point cloud registration according to claim 1, characterized in that, The process of repeatedly acquiring the next frame of point cloud as the current frame, acquiring key frames based on point cloud registration and vehicle pose iterative optimization, and incrementally updating the voxel map includes: Obtain the point cloud of the next frame as the current frame, project its point cloud onto the voxel map, and obtain the equation to be optimized for the current pose; The quasi-Newton method is used to iterate the equation to be optimized for the current pose until convergence, and then the current pose is updated. Compare the current pose with the vehicle pose of the previous keyframe. If the change in the current pose exceeds a preset threshold, then the current frame is used as a keyframe and added to the voxel map. Repeat the above steps to incrementally update the voxel map.
6. The mapping method based on incremental point cloud registration according to claim 5, characterized in that, The process of obtaining the next frame point cloud as the current frame, projecting its point cloud onto the voxel map, and obtaining the current pose equation to be optimized includes: Obtain the Gaussian distribution of the point cloud in each voxel of the voxel map, and get the mean and variance of all voxels; Infer the predicted vehicle pose in the current frame based on the vehicle pose in the previous frame; Based on the predicted pose of the vehicle in the current frame, the mean and variance of all voxels, obtain the residual between the point cloud in the current frame and the corresponding voxels. Based on the predicted vehicle pose in the current frame and the residual between the current frame point cloud and the corresponding voxel, the equation to be optimized for the current pose is obtained.
7. The mapping method based on incremental point cloud registration according to claim 5, characterized in that, The step of iterating the equation to be optimized for the current pose using the quasi-Newton method until convergence and then updating the current pose includes: Choose an initial point and calculate the gradient at that point; Update the gradient direction using an approximation of the Hessian matrix; Perform a line search along the updated gradient direction to find a new point; Update the approximate matrix of the Hessian matrix using the new points and gradient information; Repeatedly update the points and gradients until the convergence condition is met, and then take the current point as the optimal solution.
8. The mapping method based on incremental point cloud registration according to claim 5, characterized in that, The preset threshold is: The threshold for the change in distance between the current frame and the previous keyframe is 1m, or the threshold for the change in angle is 15 degrees.
9. A mapping system based on incremental point cloud registration, characterized in that, The mapping method based on incremental point cloud registration according to any one of claims 1 to 8 includes: The initial data acquisition module is used to acquire the initial frame point cloud and the initial frame vehicle pose; The initial map building module is used to construct an initial local voxel map by using the initial frame as a keyframe. The iteration module is used to repeatedly acquire the next frame of point cloud as the current frame, and iteratively optimize the point cloud registration and vehicle pose to acquire key frames and incrementally update the voxel map.
10. A computer program, characterized in that, When the computer program is executed, it implements the steps of the mapping method based on incremental point cloud registration as described in any one of claims 1 to 8.
