3D point cloud continuous registration method and system

Abstract

The application belongs to the technical field of point cloud continuous registration, and provides a 3D point cloud continuous registration method and system, initialization setting is carried out; 3D point cloud data is acquired, motion compensation and distortion removal are carried out; double downsampling is carried out, pixel position elements of an index cache array are searched, the nearest neighbor point of each point is searched, each point and the corresponding nearest neighbor point thereof are taken as candidate corresponding points, and a candidate corresponding point set is formed; a correction amount of an iterative nearest point transformation matrix of a past set frame is extracted, the maximum displacement of each frame corresponding to the correction amount is calculated, a point pair with a distance less than a filtering threshold is filtered from the candidate corresponding point set, an effective corresponding point set is obtained, robust transformation estimation is carried out, a target function of the iterative nearest point is constructed, the target function is solved, a transformation matrix is iteratively updated, and a global pose of a target frame is calculated according to the final transformation matrix; and a local map is updated based on the global pose. The application realizes the efficiency, accuracy and robustness of 3D point cloud continuous registration.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of point cloud continuous registration, and particularly relates to a 3D point cloud continuous registration method and system. BACKGROUND

[0002] The statements in this section merely provide background information related to the application and do not necessarily constitute prior art.

[0003] 3D point cloud registration is a core technology for realizing robot autonomous navigation and environment modeling, and the goal is to calculate the 6-degree-of-freedom transformation matrix between two or more point clouds, so that the point clouds are aligned in a unified coordinate system.

[0004] The Iterative Closest Point (ICP) algorithm is the current mainstream registration method, and the core process is "corresponding point search → transformation matrix estimation → iterative convergence", but the existing technology still has many key defects: first, the standard ICP algorithm relies on data structures such as KD trees in the corresponding point search stage, and does not utilize the spatial locality and temporal correlation of continuous frame point clouds, resulting in repeated calculation of neighboring points, especially in continuous registration, the frame rate is low, usually only 2-3fps, which cannot meet the real-time control requirements of automatic driving and robots.

[0005] On the other hand, existing ICP variants require manual tuning of a large number of parameters, such as adjusting the feature extraction threshold, containing 30 motion model parameters, and having 107 system parameters, and the parameter adaptability is poor, and the parameters optimized for a certain type of sensor or a certain type of scene cannot be directly migrated to other sensors or other scenes.

[0006] In addition, in order to compensate for the point cloud distortion caused by sensor motion, some existing methods need to rely on additional equipment, increasing the system complexity and hardware cost, and if such equipment is lacking, the distortion compensation effect will be significantly reduced, thereby affecting the registration accuracy; traditional ICP uses a fixed distance threshold to filter corresponding points, which cannot adapt to changes in the motion state of the robot, and is prone to introducing outliers due to a threshold that is too large or losing effective corresponding points due to a threshold that is too small, resulting in registration drift or divergence. SUMMARY

[0007] In order to solve the above problems, the application provides a 3D point cloud continuous registration method and system, which realizes the efficiency, accuracy and robustness of 3D point cloud continuous registration by fusing local subtree cache acceleration, double downsampling and motion compensation, and adaptive threshold robust optimization.

[0008] According to some embodiments, the application adopts the following technical solutions:

[0009] A 3D point cloud continuous registration method, comprising the following steps:

[0010] KD tree, local map and cache array initialization settings are performed;

[0011] 3D point cloud data is acquired, motion compensation and distortion removal are performed, and complete point cloud is obtained;

[0012] The complete point cloud is subjected to double downsampling, wherein the first stage of downsampling uses a voxel filtering function for filtering, and the first acquired original point in each voxel is retained to obtain an intermediate point cloud, and the second stage of downsampling uses a set voxel size to filter the intermediate point cloud again to obtain a registration point cloud;

[0013] For each point in the registration point cloud, the pixel position element of the cache array is indexed according to the pixel position of the original point cloud data, the sub-tree node of the KD tree pointed to by the element is acquired, and the nearest neighbor point of each point is searched within the sub-tree range corresponding to the sub-tree node, and each point and its corresponding nearest neighbor point are taken as candidate corresponding points to form a candidate corresponding point set;

[0014] The correction amount of the iterative nearest point transformation matrix of the past set of frames is extracted, the point maximum displacement corresponding to each frame correction amount is calculated, the samples with a point maximum displacement greater than a set value are retained to obtain an effective sample set, the point maximum displacement values in the effective sample set are fitted to a Gaussian distribution, the standard deviation is calculated, the screening threshold is determined based on the standard value, and the point pairs with a distance less than the screening threshold are screened from the candidate corresponding point set to obtain an effective corresponding point set; robust transformation estimation is performed based on the effective corresponding point set, a target function of the iterative nearest point is constructed, the target function is solved, the transformation matrix is iteratively updated, and the global pose of the target frame is calculated based on the final transformation matrix until a predetermined condition is met;

[0015] The local map is updated according to the global pose.

[0016] As an optional implementation, the process of KD tree, local map and cache array initialization settings includes: when a basic KD tree of a target scene is pre-constructed, if it is a known scene, a triangular mesh scene model is loaded, a model point cloud is generated at a sampling density of a set value, the KD tree is constructed, and the maximum number of points of a leaf node is set; if it is an unknown scene, the first frame point cloud is taken as an initial map point cloud, and the KD tree is directly constructed;

[0017] The local map is initialized, a voxel grid structure is adopted, the voxel size is calculated according to the product of a set coefficient and the maximum detection distance of a sensor, the maximum number of points stored in each voxel is set, and the local map is empty in the initial state;

[0018] A 2D cache array consistent with the resolution of the laser radar is constructed, the dimension of the cache array corresponds to the pixel of the laser radar, and all array elements initially point to the root node of the KD tree;

[0019] Initialization settings of related parameters are performed.

[0020] As a further step, the relevant parameters include subtree height, reset threshold, minimum deviation threshold, convergence threshold, and corresponding point threshold.

[0021] As an alternative implementation, the process of performing motion compensation and distortion removal to obtain a complete point cloud includes: Let the target frame be the... Frame, extract the first Frame and the Frame registration transformation matrix , , including rotation matrix , Translation vector , Calculated using a constant-rate model:

[0022] Translation speed: ;

[0023] Angular velocity: ;

[0024] For the first Each point in the frame Each point All include relative timestamps Calculate distortion removal points :

[0025] Perform rotational correction: ;

[0026] Perform translation correction: .

[0027] As an alternative implementation, the process of forming a set of candidate corresponding points by taking each point and its corresponding nearest neighbor as candidate corresponding points includes: if the distance between a point and its corresponding nearest neighbor is less than a set value, the cache is valid, the point and its corresponding nearest neighbor are retained as candidate corresponding points, and the pixel position elements of the cache array are updated so that they point to the parent node of the subtree above the leaf node where the corresponding nearest neighbor is located, with a height of the set value.

[0028] Otherwise, the cache becomes invalid, and the nearest neighbor is searched again from the root node of the KD tree;

[0029] Each candidate corresponding point forms a candidate corresponding point set.

[0030] As an alternative implementation, the process of calculating the maximum point displacement corresponding to the correction amount in each frame includes: maximum point displacement ,in , for The rotation angle; for The magnitude of the translation vector.

[0031] As an alternative implementation, the objective function is:

[0032] ;

[0033] wherein, is a 6-DOF transformation matrix to be estimated for aligning the registration point cloud to the local map; is transformed to the Euclidean distance with the corresponding point to quantify the positional deviation of the two points, which is the core error term of optimization; is a kernel function scale parameter, which is dynamically adjusted with to enhance robustness.

[0034] As an alternative implementation, the process of calculating the global pose of the target frame according to the final transformation matrix includes: combining the global pose of the previous frame and the predicted motion calculated by the constant velocity model to obtain the global pose of the frame and output, is the updated transformation matrix.

[0035] As an alternative implementation, the process of updating the local map and the cache array includes: converting the intermediate point cloud to the unified coordinate system according to the global pose and inserting it into the voxel grid of the local map; if the number of points in a certain voxel exceeds a certain threshold, the earliest inserted point is deleted, and the voxel exceeding the maximum detection distance of the sensor is also deleted.

[0036] A 3D point cloud continuous registration system includes:

[0037] An initialization module configured to perform KD tree, local map, and cache array initialization settings;

[0038] A motion compensation module configured to obtain 3D point cloud data, perform motion compensation and remove distortion to obtain complete point cloud;

[0039] A double downsampling module configured to perform double downsampling on the complete point cloud, wherein the first level of downsampling uses a voxel filtering function to filter and retain the first collected original point in each voxel to obtain an intermediate point cloud, and the second level of downsampling uses a set voxel size to filter the intermediate point cloud again to obtain a registration point cloud;

[0040] The local sub-tree cache module is configured to, for each point in the registration point cloud, index a pixel position element of a cache array according to a pixel position of the original point cloud data, obtain a sub-tree node of the KD tree pointed to by the element, search for the nearest neighbor point of each point within a sub-tree range corresponding to the sub-tree node, take each point and its corresponding nearest neighbor point as a candidate corresponding point, and form a candidate corresponding point set;

[0041] The adaptive threshold robust optimization module is configured to extract a correction amount of an iterative closest point transformation matrix of a past set of frames, calculate a point maximum displacement corresponding to each frame correction amount, retain samples with a point maximum displacement greater than a set value to obtain an effective sample set, fit a Gaussian distribution to the point maximum displacement values in the effective sample set, calculate a standard deviation, determine a screening threshold based on the standard value, screen point pairs with a distance less than the screening threshold from the candidate corresponding point set to obtain an effective corresponding point set, perform robust transformation estimation based on the effective corresponding point set, construct an objective function of the iterative closest point, solve the objective function, and iteratively update the transformation matrix until a predetermined condition is met, and calculate a global pose of the target frame based on the final transformation matrix.

[0042] The local map and cache updating module is configured to update the local map based on the global pose.

[0043] Compared with the prior art, the present application has the following advantages:

[0044] The present application improves the search efficiency of corresponding points, improves the frame rate of continuous point cloud registration without loss of registration accuracy, and meets the real-time control requirements.

[0045] The present application reduces the number of parameters and improves the parameter adaptability, avoids repeated manual optimization for different sensors and scenes, and realizes cross-device and cross-scene generalization.

[0046] The present application reduces hardware dependence, effectively compensates for point cloud distortion caused by sensor motion without using additional equipment, and controls system complexity and cost.

[0047] The present application optimizes the corresponding point screening strategy, so that the threshold can adapt to changes in the robot motion state, reduce outlier interference and effective point loss, and improve the registration robustness.

[0048] The real-time performance of the present application is significantly improved, the local sub-tree cache utilizes the temporal and spatial correlation of continuous frame point clouds, reduces the kd-tree search range from the full tree to the sub-tree, avoids repeated calculation, and improves the continuous registration frame rate from 2-3fps of the prior art to 10-15fps, meeting the real-time control requirements of autonomous driving and robots.

[0049] The application has strong parameter adaptability, only needs a small amount of core parameters, does not need to be manually optimized for sensors or scenes, and can be directly adapted to different devices such as various laser radars and unmanned aerial vehicle sensors and different scenes such as highways, indoor and outdoor scenes.

[0050] The application has low hardware dependence, realizes motion compensation based on a constant speed model, does not need additional equipment, reduces system complexity and hardware cost, and in the case of no additional equipment, the distortion compensation effect is equivalent to the existing method relying on additional equipment, and the registration accuracy is not affected by the lack of hardware.

[0051] In order to make the above-mentioned purposes, features and advantages of the application more obvious and easy to understand, the following preferred embodiments are specifically described below, and the accompanying drawings are used for detailed description. BRIEF DESCRIPTION OF DRAWINGS

[0052] The drawings constituting a part of the specification of the application are used to provide further understanding of the application, the schematic embodiments of the application and the description thereof are used to explain the application, and do not constitute improper limitation on the application.

[0053] Figure 1 It is a flowchart of a 3D point cloud continuous registration method of an embodiment;

[0054] Figure 2 It is a double down-sampling and motion compensation flowchart of an embodiment;

[0055] Figure 3 It is a local sub-tree cache flowchart of an embodiment;

[0056] Figure 4 It is a robust optimization flowchart of an embodiment;

[0057] Figure 5 It is a map updating workflow diagram of an embodiment. DETAILED DESCRIPTION

[0058] The application will be further described below in combination with the drawings and embodiments.

[0059] It should be pointed out that the following detailed description is exemplary and is intended to provide further description of the application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as generally understood by those skilled in the art to which the application belongs.

[0060] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0061] Where there is no conflict, the embodiments and features described in this application may be combined with each other.

[0062] Example 1

[0063] A continuous registration method for 3D point clouds is proposed, which integrates three core steps: local subtree caching acceleration, double downsampling and motion compensation, and adaptive threshold robust optimization. Figure 1 As shown, high efficiency, accuracy and robustness of continuous registration of 3D point clouds are achieved.

[0064] Includes the following steps:

[0065] Step 1: Initialization.

[0066] Before inputting the first frame of point cloud, initialization operations need to be completed to lay the foundation for subsequent continuous registration:

[0067] First, the KD tree and local map are initialized. When pre-constructing the basic KD tree of the target scene, if it is a known scene such as an indoor environment, a scene CAD model with a triangular mesh can be loaded, and a model point cloud can be generated with a sampling density of 0.01m. The KD tree is constructed using the PCL library (the maximum number of leaf nodes is set to 5-8 to balance search efficiency and accuracy). If it is an unknown scene, the KD tree is directly constructed using the first frame point cloud as the "initial map point cloud".

[0068] Simultaneously initialize the local map, using a voxel mesh structure, with voxel dimensions... according to The maximum detection distance of the sensor is calculated, and each voxel is set to store a maximum of 20 points to avoid excessive voxel points leading to computational redundancy. In the initial state, the local map is empty.

[0069] Next, the 2D cache array is initialized, constructing a 2D cache array with the same resolution as the LiDAR (dimension 256×256, corresponding to the LiDAR pixels), with all array elements initially pointing to the root node of the KD tree. Finally, the core parameters are initialized, fixing core parameters including the subtree height. (Controlling the search range of the cache subtree), resetting the threshold (Determine if the cache is valid), minimum deviation threshold (Selecting valid historical deviation samples), convergence threshold (whether the transformation matrix converges), the initial value of the dynamic parameter is set as τ0=2m to avoid missing effective points.

[0070] The specific values of the above parameters are only examples, and in other embodiments, the above values can be adjusted and changed.

[0071] Step two, as shown in Figure 2 , motion compensation and double downsampling.

[0072] Motion compensation is used to eliminate point cloud distortion, and the steps are as follows:

[0073] Step 1, calculate the motion speed of the first two frames. Extract the registration transformation matrix of the first frame and the second frame , , , which contains the rotation matrix , , the translation vector , , and calculate by constant speed model:

[0074] Translation speed: ;

[0075] Angular velocity: ;

[0076] Step 2, de-distort point by point, for each point of the first frame (including relative timestamp ), calculate the de-distorted point :

[0077] Rotation correction: ;

[0078] Translation correction: , to compensate for the position offset caused by sensor translation;

[0079] Step 3, output the complete point cloud after de-distortion .

[0080] Double downsampling is used to reduce the amount of calculation, and the steps are as follows:

[0081] Step 1, first level downsampling, use the voxel filter function of PCL library, set voxel size , filter the complete point cloud , keep the first collected original point in each voxel, get the intermediate point cloud ;

[0082] Step 2, second level downsampling, set voxel size , filter the intermediate point cloud Filtering again, get the registered point cloud ;

[0083] Step 3, output the registered point cloud .

[0084] Step three, as shown in Figure 3 , local sub-tree cache, including the following steps:

[0085] Step 1, pixel coordinate index. Get the registered point cloud after double downsampling and motion compensation , for each point , according to the pixel position in the original output of the laser radar, index the element of the 2D cache array, get the KD tree sub-tree node pointed to by the element, denoted as Node;

[0086] Step 2, search within the sub-tree. Call the KD tree nearest neighbor search function of the PCL library, search for the nearest neighbor point only in the sub-tree range corresponding to Node; ;

[0087] Step 3, cache validity judgment. If the Euclidean distance between is , determine that the cache is valid, keep as the candidate corresponding point, and update the cache array element to point to the parent node of the leaf node where is located to the upper layer (update the search starting point of the next frame); if the distance , determine that the cache is invalid, re-search the nearest neighbor point from the root node of the KD tree, keep , and reset the cache array element to the sub-tree node corresponding to ;

[0088] Step 4, output the candidate corresponding point set .

[0089] Step four, as shown in Figure 4 , adaptive threshold robust optimization process, including the following steps:

[0090] Step 1, extract the ICP transformation matrix correction amount of the past 10 frames , calculate the maximum displacement of the corresponding points of each frame , where , is the rotation angle of the correction amount ; For correction amount The magnitude of the translation vector;

[0091] Step 2, Keep The effective sample set is obtained by excluding minor biases. ,right In Values ​​fitted to a Gaussian distribution And calculate the standard deviation. Set the filtering threshold for the corresponding point in the current frame. , The principle ensures that 99.7% of valid points are retained;

[0092] Step 3: From the candidate corresponding point set Medium screening distance The point pairs are used to obtain the effective corresponding point set. .

[0093] Step 4: Perform robust transformation estimation based on the effective corresponding point set. Construct the objective function by using a point-to-point ICP objective function combined with robust kernel suppression of outliers:

[0094] ;

[0095] in The transformation matrix to be estimated is a 6-DOF matrix used to align the registered point cloud to the local map; for After transformation After and corresponding point The Euclidean distance, which quantifies the positional deviation between two points, is the core error term for optimization. The kernel function scaling parameter varies with... Dynamic adjustments are made to enhance robustness.

[0096] Step 5: Solve the least squares problem using the Eigen library and iteratively update the transformation matrix. until Change in rotation angle And the amount of translation change Stop iterating when the time comes.

[0097] Step 6: Finally, combine the global pose from the previous frame. Compared with the predicted motion calculated by the constant velocity model , obtained the Frame global pose And output it.

[0098] Step 5, as follows Figure 5 As shown, local map and cache updates.

[0099] Step 1, Local Map Update: Update the intermediate point cloud in global pose Convert to a unified coordinate system and insert into the voxel grid of the local map; if the number of points in a voxel exceeds 20, delete the earliest inserted point to maintain the lightness of the map; at the same time, delete the voxels beyond the maximum detection distance of the sensor to avoid the accumulation of invalid data;

[0100] Step 2, cache array update: real-time update has been completed in the "corresponding point search" step, and no additional operation is required;

[0101] Step 3, loop triggering: wait for the next frame of laser radar point cloud input, and repeat the above "motion compensation → double downsampling → corresponding point search → robust optimization → map update" process.

[0102] Embodiment two

[0103] A 3D point cloud continuous registration system, comprising:

[0104] An initialization module configured to perform KD tree, local map and cache array initialization settings;

[0105] A motion compensation module configured to obtain 3D point cloud data, perform motion compensation and remove distortion to obtain complete point cloud;

[0106] A double downsampling module configured to perform double downsampling on the complete point cloud, wherein the first level of downsampling uses a voxel filtering function to filter and retain the first collected original point in each voxel to obtain intermediate point cloud, and the second level of downsampling filters the intermediate point cloud again with a set voxel size to obtain registration point cloud;

[0107] A local subtree cache module configured to, for each point in the registration point cloud, index the pixel position element of the cache array according to the pixel position of the original point cloud data, obtain the subtree node of the KD tree pointed to by the element, search for the nearest neighbor of each point within the subtree range corresponding to the subtree node, take each point and its corresponding nearest neighbor as a candidate corresponding point, and form a candidate corresponding point set;

[0108] An adaptive threshold robust optimization module configured to extract the correction amount of the iterative closest point transformation matrix of the past set of frames, calculate the maximum displacement of each frame corresponding to the correction amount, retain samples with a maximum displacement greater than a set value to obtain an effective sample set, fit a Gaussian distribution to the maximum displacement values in the effective sample set, calculate the standard deviation, determine a screening threshold based on the standard value, screen point pairs with a distance less than the screening threshold from the candidate corresponding point set to obtain an effective corresponding point set, perform robust transformation estimation based on the effective corresponding point set, construct an objective function of the iterative closest point, solve the objective function, and iteratively update the transformation matrix until a predetermined condition is met, and calculate the global pose of the target frame based on the final transformation matrix;

[0109] The local map and cache updating module is configured to update the local map according to the global pose.

[0110] Each module is configured to perform the details of each of steps one through five of embodiment one, which are not repeated here.

[0111] Those skilled in the art will understand that embodiments of the present application can be provided as methods, systems, or computer program products. Thus, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application can take the form of a computer program product on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage devices, and the like) embodying computer readable program code. CD - ROM

[0112] The present application is described in reference to the flowchart and / or block diagrams of the method, apparatus (system) and computer program product according to embodiments of the present application. It should be understood that each flow and / or block in the flowchart and / or block diagrams, as well as combinations of flows and / or blocks in the flowchart and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general purpose computer, a special purpose computer, an embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart and / or block diagrams block or blocks. Figure 1 The flowchart and / or block diagrams can also be implemented by the functional equivalent of one or more of the flows or blocks and / or combinations of flows and / or blocks specified in the flowchart and / or block diagrams. Figure 1 The flowchart and / or block diagrams can also be implemented by the functional equivalent of one or more of the flows or blocks and / or combinations of flows and / or blocks specified in the flowchart and / or block diagrams.

[0113] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart and / or block diagrams block or blocks. Figure 1 The flowchart and / or block diagrams can also be implemented by the functional equivalent of one or more of the flows or blocks and / or combinations of flows and / or blocks specified in the flowchart and / or block diagrams. Figure 1 The flowchart and / or block diagrams can also be implemented by the functional equivalent of one or more of the flows or blocks and / or combinations of flows and / or blocks specified in the flowchart and / or block diagrams.

[0114] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart and / or block diagrams block or blocks. Figure 1 The flowchart and / or block diagrams can also be implemented by the functional equivalent of one or more of the flows or blocks and / or combinations of flows and / or blocks specified in the flowchart and / or block diagrams. Figure 1 The flowchart and / or block diagrams can also be implemented by the functional equivalent of one or more of the flows or blocks and / or combinations of flows and / or blocks specified in the flowchart and / or block diagrams.

[0115] ​The above merely describes the preferred embodiments of the present application and is not used to limit the present application. Any modification, equivalent replacement, improvement, etc. made by those skilled in the art without departing from the spirit and principle of the present application shall fall within the protection scope of the present application.

Claims

1. A method for continuous registration of 3D point clouds, characterized in that, The method comprises the following steps: KD tree, local map and cache array initialization settings are performed; 3D point cloud data is acquired, motion compensation and distortion removal are performed, and complete point cloud is obtained; Double downsampling is performed on the complete point cloud, wherein the first level of downsampling uses a voxel filter function for filtering, and the first collected original point in each voxel is retained to obtain intermediate point cloud, and the second level of downsampling uses a set voxel size to filter the intermediate point cloud again to obtain registration point cloud; For each point in the registration point cloud, according to the pixel position of the original point cloud data, the pixel position element of the cache array is indexed, the sub-tree node of the KD tree pointed by the element is acquired, the nearest neighbor point of each point is searched in the sub-tree range corresponding to the sub-tree node, each point and its corresponding nearest neighbor point are taken as candidate corresponding points to form a candidate corresponding point set; The correction amount of the iterative closest point transformation matrix of the past set of frames is extracted, the point maximum displacement corresponding to each frame correction amount is calculated, samples with a point maximum displacement greater than a set value are retained to obtain an effective sample set, a Gaussian distribution is fitted to the point maximum displacement values in the effective sample set, a standard deviation is calculated, a screening threshold is determined based on the standard value, and points pairs with a distance less than the screening threshold are screened from the candidate corresponding point set to obtain an effective corresponding point set; robust transformation estimation is performed based on the effective corresponding point set, a target function of the iterative closest point is constructed, the target function is solved, the transformation matrix is iteratively updated, and the final transformation matrix is calculated to obtain the global pose of the target frame until a predetermined condition is met; The local map is updated according to the global pose.

2. The 3D point cloud continuous registration method of claim 1, wherein, The process of KD tree, local map and cache array initialization settings comprises the following steps: when a basic KD tree of a target scene is pre-constructed, if it is a known scene, a triangular mesh scene model is loaded, a model point cloud is generated according to a set sampling density, the KD tree is constructed, and the maximum number of points of a leaf node is set; if it is an unknown scene, the first frame point cloud is taken as an initial map point cloud, and the KD tree is directly constructed; The local map is initialized, a voxel grid structure is adopted, the voxel size is calculated according to the product of a set coefficient and the maximum detection distance of a sensor, the maximum number of points stored in each voxel is set, and the initial state of the local map is empty; A 2D cache array consistent with the resolution of the laser radar is constructed, the dimension of the cache array corresponds to the pixel of the laser radar, and all array elements initially point to the root node of the KD tree; Initialization settings of related parameters are performed.

3. The method of claim 2, wherein the 3D point cloud continuous registration method is characterized by, The related parameters comprise a sub-tree height, a reset threshold, a minimum deviation threshold, a convergence threshold and a corresponding point threshold.

4. The method of claim 1, wherein, The process of motion compensation and distortion removal to obtain the complete point cloud includes: setting the target frame as the first frame, extracting the registration transformation matrix of the first frame and the second frame , , including a rotation matrix , , a translation vector , , and calculating by a constant speed model: Translation speed: ; Angular velocity: ; To the first frame each point , each point contains a relative timestamp , the de-warped point is calculated Rotation correction is performed: ; Performing a translation correction: .

5. The method of claim 1, wherein, The process of taking each point and its corresponding nearest neighbor point as a candidate corresponding point to form a candidate corresponding point set comprises the following steps: if the distance between a certain point and its corresponding nearest neighbor point is less than a set value, the cache is valid, the point and its corresponding nearest neighbor point are retained as candidate corresponding points, and the pixel position element of the cache array is updated to point to the parent node of the leaf node where the corresponding nearest neighbor point is located and whose sub-tree height is a set value; Otherwise, the cache is invalid, and the nearest neighbor point is searched again from the root node of the KD tree; Each candidate corresponding point forms a candidate corresponding point set.

6. The method of claim 1, wherein, The process of calculating the point maximum displacement corresponding to the correction amount of each frame comprises: The point maximum displacement , , is the rotation angle of the correction amount ; is the translation vector module length of the correction amount .

7. The method of claim 1, wherein, The target function is: ； in The transformation matrix to be estimated is a 6-DOF matrix used to align the registration point cloud to the local map; for After transformation After and corresponding point The Euclidean distance, which quantifies the positional deviation between two points, is the core error term for optimization. The kernel function scaling parameter varies with... Dynamic adjustments are made to enhance robustness.

8. The method of claim 1, wherein, The process of calculating the global pose of the target frame according to the final transformation matrix comprises: combining the global pose of the previous frame with the predicted motion calculated by the constant velocity model to obtain the global pose of the first frame and output, as the updated transformation matrix.

9. The method of claim 1, wherein, The process of updating the local map includes: inserting the intermediate point cloud into the voxel grid of the local map in the global pose converted to the unified coordinate system; if the number of points in a voxel exceeds a set threshold, deleting the earliest inserted point and deleting the voxel exceeding the maximum detection distance of the sensor.

10. A 3D point cloud continuous registration system, characterized in that it comprises: An initialization module configured to initialize KD tree, local map and cache array; A motion compensation module configured to obtain 3D point cloud data, perform motion compensation and remove distortion to obtain complete point cloud; A double downsampling module configured to perform double downsampling on the complete point cloud, wherein the first level downsampling utilizes a voxel filter function to filter and retain the first collected original point in each voxel to obtain intermediate point cloud, and the second level downsampling sets a voxel size to filter the intermediate point cloud again to obtain registration point cloud; A local sub-tree cache module configured to, for each point in the registration point cloud, index a pixel position element of a cache array according to a pixel position of the original point cloud data, obtain a sub-tree node of the KD tree pointed by the element, search for the nearest neighbor of each point within a sub-tree range corresponding to the sub-tree node, take each point and its corresponding nearest neighbor as a candidate corresponding point, and form a candidate corresponding point set; An adaptive threshold robust optimization module configured to extract a correction amount of an iterative closest point transformation matrix of a past set of frames, calculate a point maximum displacement corresponding to each frame correction amount, retain samples with a point maximum displacement greater than a set value to obtain an effective sample set, fit a Gaussian distribution to the point maximum displacement values in the effective sample set, calculate a standard deviation, determine a screening threshold based on the standard value, screen point pairs with a distance less than the screening threshold from the candidate corresponding point set to obtain an effective corresponding point set, perform robust transformation estimation based on the effective corresponding point set, construct an objective function of the iterative closest point, solve the objective function, iteratively update the transformation matrix until a predetermined condition is met, and calculate a global pose of a target frame according to the final transformation matrix; A local map and cache updating module configured to update the local map according to the global pose.

Images