A point cloud registration method based on area array laser measurement

Abstract

This invention discloses a point cloud registration method based on area array laser measurement, belonging to the field of 3D point cloud registration and 3D reconstruction technology. It acquires an area array laser depth map sequence and synchronous IMU data, obtains initial relative pose values ​​of adjacent keyframes through IMU pre-integration, and uses these initial values ​​to remove motion distortion from the depth map. Multiple frames of distorted depth maps within a preset time window are fused into a local TSDF sub-map, and the sub-map point cloud is extracted. Using the initial relative pose value as the initial transformation, a normal distribution transformation model is constructed for the target point cloud, and NDT coarse registration is performed on the source point cloud to obtain a preliminary rigid body transformation matrix. Using the preliminary rigid body transformation matrix as the initial value, GICP fine registration is performed on the source and target point clouds to obtain an accurate rigid body transformation matrix. This invention can converge even under large rotation and low overlap conditions, balancing robustness and high accuracy in registration. It does not require a cooperative target and is suitable for mobile measurement and large-scale dynamic 3D reconstruction.

Description

Technical Field

[0001] This invention belongs to the field of 3D point cloud registration and 3D reconstruction technology, specifically involving a highly robust point cloud registration method based on area array laser measurement and fusion of inertial information. Background Technology

[0002] This invention relates to the field of 3D point cloud registration technology, specifically to a highly robust point cloud registration method based on area laser measurement and fusion of inertial information. While area laser sensors can acquire dense depth maps in a single scan, their single-frame field of view is typically small (e.g., horizontal field of view less than 90°). During continuous scanning, the point cloud overlap between adjacent frames is low. Furthermore, when the sensor undergoes large-scale rotation or rapid changes in viewing angle, traditional registration algorithms based on Iterative Closest Point (ICP) or its variants are prone to getting trapped in local optima due to large initial pose deviations, leading to registration failure or a significant decrease in accuracy. In addition, significant motion distortion exists within the depth map during motion, and direct inter-frame matching further deteriorates the registration quality. Therefore, how to achieve high-precision, robust point cloud registration under conditions of low overlap, large viewing angle, and large rotational differences is a pressing technical problem to be solved in this field. Summary of the Invention

[0003] To address the aforementioned issues, this invention provides a point cloud registration method based on area array laser measurement. This method can still converge under conditions of large rotation and low overlap, balancing robustness and high precision in registration. It does not require a cooperative target and is suitable for mobile measurement and large-scale dynamic 3D reconstruction.

[0004] The technical solution adopted in this invention is as follows: This invention proposes a point cloud registration method based on area array laser measurement, comprising the following steps: S1. Acquire the continuous depth map sequence collected by the area array laser sensor and the synchronous inertial measurement unit data, pre-integrate the inertial measurement unit data to obtain the initial relative pose value between adjacent keyframes, and use the initial relative pose value to remove motion distortion from the depth map sequence to obtain the distortion-free depth map sequence. S2. The multi-frame distortion-free depth maps within the preset time window are fused into a truncated symbolic distance function sub-map based on the initial relative pose or the optimized pose, and the sub-map point cloud is extracted from the sub-map. S3. Take the current sub-map point cloud as the source point cloud and the reference sub-map point cloud with overlapping areas as the target point cloud. Construct a normal distribution transformation model for the target point cloud and use the initial relative pose value as the initial transformation to perform a normal distribution transformation coarse registration on the source point cloud to obtain the preliminary rigid body transformation matrix. S4. Using the initial rigid body transformation matrix as the initial value, perform generalized iterative nearest-point fine registration on the source point cloud and the target point cloud to obtain the accurate rigid body transformation matrix.

[0005] Furthermore, the pre-integration specifically includes: Between two adjacent keyframe moments, the raw measurements of the inertial measurement unit are pre-integrated to obtain a pre-integrated result that includes relative rotation increment, velocity increment, and position increment.

[0006] Further, in step S1, the initial relative pose between adjacent keyframes is solved using the pre-integration results, specifically including: Obtain the absolute pose, velocity, and gravity vector of the current keyframe i and the adjacent keyframe j in the world coordinate system, and calculate the relative rotation matrix based on the pre-integration results. and relative translation vector As the initial value for relative pose: ; ; in, This is the relative rotation matrix between two keyframes. Let be the absolute rotation matrices of the j-th frame and the i-th frame in the world coordinate system, respectively. This is the relative translation vector between two keyframes. Let be the absolute positions of the j-th frame and the i-th frame in the world coordinate system, respectively. Let be the speed of the i-th frame. The time interval between two keyframes. Let T be the vector of gravitational acceleration, and the superscript T is the transpose.

[0007] Furthermore, the motion distortion removal specifically includes: The sensor pose at the start and end times of the depth map is obtained using the pre-integration results. Spherical linear interpolation is performed on the rotation component, and linear interpolation is performed on the translation component to obtain the sensor pose at the imaging time of any pixel within the depth map. ; Each pixel is back-projected into three-dimensional space according to its imaging time to obtain a three-dimensional point. reuse Sensor pose at the reference time of the depth map ,Will Transform to the reference time coordinate system.

[0008] Further, in step S2, the fusion into a locally truncated symbolic distance function sub-map includes: A three-dimensional voxel mesh is constructed in a local reference coordinate system defined by a preset time window. Each voxel stores a cutoff distance value and a cumulative weight, with the initial values ​​of both being zero. For each frame of the distortion-free depth map within the window, it is projected into a 3D point cloud and transformed to a local reference coordinate system; along the ray direction from the sensor optical center to each 3D point, the voxels through which the ray passes are traversed, the sign distance value from the center of each voxel to the object surface is calculated, and the truncated distance value is updated by weighted averaging in combination with the cumulative weight, and the cumulative weight is further updated. The process continues until all frames within the window have been processed, resulting in a fused local truncation symbolic distance function submap.

[0009] Furthermore, in step S2, a dynamic voxel structure is used to distribute voxels only near the surface, and the fusion weights are adaptively adjusted according to the distance from the measurement point to the sensor optical center or the incident angle.

[0010] Furthermore, in step S3, during the coarse registration process of the normal distribution transformation, if the matching score is lower than a preset threshold, a multi-resolution strategy is initiated: first, a global search is performed at a larger grid size, and then the grid size is gradually refined for local optimization.

[0011] Further, in step S4, the generalized iterative nearest-point fine registration includes: Using the initial rigid body transformation matrix as the initial transformation, the source point cloud is transformed to the target point cloud coordinate system; For each point in the source point cloud and the target point cloud, search for its spherical neighborhood of radius r or the nearest K points to form a neighborhood point set, and calculate the local covariance matrix of that point. An initial point pair correspondence between the source point cloud and the target point cloud is established through nearest neighbor search. For each pair of matching points, its distance vector is calculated, and a weighted error term is constructed. Calculate the weighted error of all matching pairs. If the weighted error exceeds the threshold of the chi-square distribution with 3 degrees of freedom under the preset confidence level, the pair is determined to be an abnormal match and is removed. The remaining reliable point pairs are used to construct the optimization objective function. The incremental transformation is iteratively solved with the goal of minimizing the sum of all weighted errors, and the transformation matrix is ​​updated. The final accurate rigid body transformation matrix is ​​output when the rotation and translation components of the transformation increment are both less than the preset convergence threshold or the preset maximum number of iterations is reached.

[0012] Furthermore, the formula for the weighted error term is as follows: ; ; in, For weighted error, Let T be the distance vector between the i-th pair of matching points in space, with the superscript T being the transpose. Let i be the position vector of the target point. Let i be the position vector of the source point. This is the current rigid body transformation matrix. Let i be the local covariance matrix of the target point i. Let be the local covariance matrix of the source point i.

[0013] This invention proposes a point cloud registration system based on area array laser measurement, used to implement the aforementioned point cloud registration method based on area array laser measurement. The system includes: The IMU pre-integration and motion distortion removal module is used to acquire continuous depth map sequences collected by the area array laser sensor and synchronous inertial measurement unit data, pre-integrate the inertial measurement unit data to obtain the initial relative pose values ​​between adjacent keyframes, and use the initial relative pose values ​​to remove motion distortion from the depth map sequence to obtain a distortion-free depth map sequence. The TSDF submap construction module is used to fuse multiple frames of distortion-free depth maps within a preset time window into a truncated symbolic distance function submap based on the initial relative pose value or the optimized pose, and extract the submap point cloud from the submap. The NDT coarse registration module is used to take the current sub-map point cloud as the source point cloud and the reference sub-map point cloud with overlapping areas as the target point cloud. It constructs a normal distribution transformation model for the target point cloud and uses the initial relative pose value as the initial transformation to perform normal distribution transformation coarse registration on the source point cloud to obtain the preliminary rigid body transformation matrix. The GICP fine registration module is used to perform generalized iterative nearest-point fine registration on the source point cloud and the target point cloud using the preliminary rigid body transformation matrix as the initial value, so as to obtain the accurate rigid body transformation matrix.

[0014] Compared with the prior art, the present invention has the following significant advantages: 1. By fusing multi-frame area laser depth maps into a sub-map within the TSDF framework, the local point cloud coverage area for a single registration is multiplied, effectively solving the core challenges of small field of view and low inter-frame overlap in area laser mapping. Simultaneously, the fusion process effectively suppresses single-frame measurement noise, resulting in smoother and more complete surface geometry, providing high-quality input for subsequent registration.

[0015] 2. IMU pre-integration not only provides a reliable initial pose for TSDF fusion and NDT coarse registration, but also eliminates motion distortion of the depth map in real time. This allows the system to maintain convergence even under extreme conditions such as large rotations and rapid motion, and its robustness is significantly better than that of pure geometric registration methods.

[0016] 3. A two-stage strategy combining NDT probabilistic coarse registration and GICP geometric fine registration is adopted. Since NDT is based on probability distribution, its convergence radius is much larger than that of traditional ICP, which can effectively correct the residual bias caused by IMU drift. GICP uses local surface covariance weighting to achieve optimal point-to-plane or point-to-surface fitting under good initial values, with accuracy down to the millimeter level, balancing robustness and accuracy.

[0017] 4. The entire process requires no collaborative target or initial manual alignment, is highly automated, and can be directly applied to dynamic scenarios such as mobile measurement, robot navigation, and large-scale 3D reconstruction. Attached Figure Description

[0018] Figure 1 This is a flowchart illustrating the overall process of the point cloud registration method based on area array laser measurement according to the present invention.

[0019] Figure 2 The flowchart for IMU pre-integration and motion distortion removal.

[0020] Figure 3 A flowchart for constructing a TSDF submap.

[0021] Figure 4 The flowchart for NDT coarse registration.

[0022] Figure 5 A flowchart for GICP fine registration. Detailed Implementation

[0023] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings. It should be understood that the specific processes and parameters described herein are only for explaining the present invention and are not intended to limit the scope of protection of the present invention.

[0024] like Figure 1 As shown, the point cloud registration method based on area array laser measurement provided by this invention includes four sequentially executed steps: Step S1, IMU pre-integration and motion distortion removal; Step S2, TSDF sub-map construction; Step S3, NDT coarse registration; Step S4, GICP fine registration.

[0025] The system takes as input a sequence of area array laser depth maps and synchronized IMU data, and outputs precise inter-frame or inter-submap transformation matrices for subsequent 3D reconstruction or map building.

[0026] The following details the specific implementation of each step.

[0027] Step S1: IMU pre-integration and motion distortion removal.

[0028] like Figure 2 As shown, the purpose of this step is to estimate the relative motion between adjacent keyframes using high-frequency inertial measurement data and to remove motion distortion from the single-frame depth map.

[0029] In one specific embodiment of the present invention, the process is as follows: First, high-precision extrinsic parameter calibration and time synchronization are performed on the array laser sensor and the IMU to obtain the fixed-connection transformation matrix between the array laser sensor and the IMU. and hardware timestamp offset .

[0030] During system operation, the IMU continuously outputs the angular velocity vector at a frequency of 200Hz. and linear acceleration vector The area laser array outputs a depth map at a frequency of 30Hz. A keyframe selection strategy is defined: a new keyframe is created whenever the sensor translates more than 0.3 meters or rotates more than 15°. The time interval between two adjacent keyframes is recorded as... and .

[0031] In the interval Within this model, zero-bias compensation and integration are performed on the raw IMU measurements. To avoid redundant integration, a pre-integration model is used. The pre-integrated state variables include the relative rotation increment. Relative velocity increment and relative position increment Its calculation is based on the compensated measurements: The pre-integral incremental recursive formula is: in, The IMU sampling period The compensated angular velocity, for The angular velocity measured raw by the IMU at that moment. To achieve zero bias in the gyroscope, For the compensated acceleration, for The acceleration measured raw by the IMU at that moment. To achieve zero bias in the accelerometer, Let i be the relative rotation increment from time i to time k+1. Let i be the relative rotation increment from time i to time k. For matrix exponential mapping, Let i be the relative velocity increment from time i to time k+1. Let i be the relative velocity increment from time i to time k. Let i be the relative position increment from time i to time k+1. Let be the relative position increment from time i to time k.

[0032] Finally, the initial relative pose between the two keyframes is: in, This is the relative rotation matrix between two keyframes. Let be the absolute rotation matrices of the j-th frame and the i-th frame in the world coordinate system, respectively. This is the relative position vector between two keyframes. Let be the absolute positions of the j-th frame and the i-th frame in the world coordinate system, respectively. Let be the speed of the i-th frame. The time interval between two keyframes. This is the vector of gravitational acceleration.

[0033] The details of the velocity term and the influence of gravity are ignored here. In actual calculations, the velocity and gravity of the previous state will be combined for compensation to obtain a more accurate initial translation value.

[0034] Next, motion distortion removal is performed. The depth map of an area laser array is typically exposed line-by-line or pixel-by-pixel. Within the integration time of one frame, the sensor may have experienced significant motion. This invention utilizes the continuous sensor pose obtained through IMU pre-integration to process the depth map for each frame.

[0035] The specific steps are as follows: Obtain the start time of the depth map. and end time The corresponding IMU pose is used to obtain any timestamp within the frame through spherical linear interpolation (for rotation) and linear interpolation (for translation). Corresponding area array laser sensor pose For each pixel According to its imaging time Projecting it back onto three-dimensional space yields points. reuse Transform to the reference time of this frame (usually the center time is selected). In the coordinate system of ) This yields a distortion-free depth map, where all points are unified to the same coordinate system, eliminating point cloud stretching or distortion caused by motion.

[0036] Step S2: TSDF submap construction.

[0037] like Figure 3 As shown, this step aims to construct a local sub-map with more complete geometric information using the distortion-free continuous depth map, thereby increasing the overlapping area of ​​subsequent registration.

[0038] First, define the sub-map construction window. The window size can be dynamically adjusted based on movement speed and scene structure. For example, it can be fixed to contain 30 frames of depth maps, or the cumulative translation distance of the sensors within the window can be limited to no more than 2 meters, and the rotation can be limited to no more than 45°. The window sliding step size can be set to half the window size to ensure a certain degree of overlap between adjacent sub-maps.

[0039] For the current window's first The frame, using the pose obtained in step S1 (before global optimization, the pose provided by IMU pre-integration can be used directly. Note that IMU pre-integration has drift that accumulates over time, but within a short local window, the drift is usually small and its impact on the local TSDF is negligible), the point cloud of the area laser sensor coordinate system of this frame is used. Transform to the local reference coordinate system defined by the window Below. The local reference coordinate system is usually set as the coordinate system of the area array laser sensor in the middle frame or the first frame of the window.

[0040] exist A TSDF voxel field is created. In this embodiment, the voxel resolution is set to 0.02 meters and the cutoff distance is set to 0.1 meters. To reduce memory usage, a dynamic voxel structure based on a hash table can be used, allocating storage only for voxels near the surface. Each voxel contains two floating-point values: the cumulative cutoff distance. and cumulative weight The initial values ​​are all 0.

[0041] For point clouds Each point in (Corresponding to the optical center of the array laser sensor) Starting from the ray, traverse the voxels along the ray, and at the center of each voxel on the ray... Calculate the symbolic distance value: like If the value is less than 1 meter, the voxel is considered to be far from the surface and is not updated or updated to an empty region identifier. Otherwise, a weighted fusion update is performed: in, The symbolic distance value. For symbolic functions, This is the updated cumulative cutoff distance. For the updated weights, This represents the cumulative cutoff distance before the update. The weights before the update. The weights are the weights of the current observations. It can be set to a constant of 1, or adaptively based on the inverse square ratio of the distance from the measurement point to the array laser sensor, or the cosine of the incident angle of the ray, in order to suppress noise at large incident angles.

[0042] Once all frames in the window have been fused, isosurfaces can be extracted from the TSDF voxel field. This embodiment uses the classic moving cube algorithm to generate a triangular mesh, and then uniformly extracts point clouds from the mesh surface using voxel center sampling or Poisson disk sampling, serving as the point cloud representation of the current sub-map. This sub-map also records the initial transformation between its local reference coordinate system and the global coordinate system.

[0043] Step S3: NDT coarse registration.

[0044] like Figure 4 As shown, this step performs coarse alignment of the point clouds of two adjacent sub-maps, providing a good initial position for fine registration.

[0045] Select the source point cloud of the current submap. Target point cloud of reference submap The reference submap can be the previous submap or a local map formed by accumulating multiple registered submaps.

[0046] Perform NDT modeling on the target point cloud: The space is divided into sections with sides of length . cube grid ( Initially set to 0.5 meters). For each grid cell If the number of points is included If the value is greater than or equal to 5, then calculate its mean. Covariance Matrix : To prevent the covariance matrix from being ill-conditioned, it can be regularized: , It is a small positive number. It is an identity matrix.

[0047] Transform each point of the source point cloud The initial transformation value is the initial transformation given in step S1. The objective function is constructed as the sum of the negative log-likelihoods of all transformed points in the Gaussian distribution of each grid: in, It is a constant. For transformation, Let i be the position vector of the source point. The mean of the target point cloud position vector. for It represents a uniform probability distribution and is used to handle outliers.

[0048] Iterative solutions using Newton's method or the Levenberg-Marquardt method make Minimal transformation During the optimization process, if the matching score (i.e., the average negative log-likelihood) is low, it indicates that the initial value deviation may still be large. In this case, a multi-resolution strategy can be initiated: first, optimize quickly with a 1.0-meter grid, then refine with a 0.5-meter grid, and finally, perform local corrections with a 0.25-meter grid. This coarse-to-fine strategy greatly enhances the convergence robustness of the algorithm.

[0049] Step S4: GICP fine registration.

[0050] like Figure 5 As shown, based on NDT coarse registration, GICP further utilizes local geometric details to achieve high-precision alignment.

[0051] The result obtained in step S3 This serves as the initial transformation from the source point cloud to the target point cloud. For each point in both the source and target point clouds... Search its neighborhood (e.g., radius) Inside or near (points), calculate the local covariance matrix. : in, For point The neighborhood point set, is the average position vector of the neighborhood point set.

[0052] If the point lies on a plane, the smallest eigenvalue of the covariance matrix will be much smaller than the other two eigenvalues, and the corresponding eigenvector is the plane normal.

[0053] GICP, by maximizing the observation likelihood, is equivalent to minimizing the following weighted error: in, To match the distance between pairs of points, Let the local covariance matrix of the target point cloud be . The source point cloud local covariance matrix, For transformation. This error model can be understood as follows: when both the source point and the target point are located on a planar structure, it degenerates into a distance metric from the point to the plane; when both are located on a general curved surface, the surface uncertainties of both are considered together.

[0054] In one specific embodiment of the present invention, an initial point pair correspondence is first established through nearest neighbor search. Then, the above calculation is performed for each point pair. ,like If the match exceeds a threshold corresponding to a certain quantile of the chi-square distribution (e.g., 95% confidence level), the pair is considered an anomaly and is removed. An optimization problem is constructed using the remaining interior points, and the incremental transformation is solved iteratively using the Levenberg-Marquardt method. and update The iteration terminates when the transformation increment is less than 1. (Determine rotation and translation components separately) or reach the maximum number of iterations (50). The final output is the exact transformation matrix. .

[0055] After completing the registration between all adjacent submaps, a pose graph can be constructed, where nodes represent the coordinate system poses of each submap, and edges represent... Constraints. Accumulated errors are further eliminated through global graph optimization, and the optimized pose is used to re-fuse TSDF, generating a globally consistent high-precision 3D model.

[0056] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A point cloud registration method based on area array laser measurement, characterized in that, Includes the following steps: S1. Acquire the continuous depth map sequence collected by the area array laser sensor and the synchronous inertial measurement unit data, pre-integrate the inertial measurement unit data to obtain the initial relative pose value between adjacent keyframes, and use the initial relative pose value to remove motion distortion from the depth map sequence to obtain the distortion-free depth map sequence. S2. The multi-frame distortion-free depth maps within the preset time window are fused into a truncated symbolic distance function sub-map based on the initial relative pose or the optimized pose, and the sub-map point cloud is extracted from the sub-map. S3. Take the current sub-map point cloud as the source point cloud and the reference sub-map point cloud with overlapping areas as the target point cloud. Construct a normal distribution transformation model for the target point cloud and use the initial relative pose value as the initial transformation to perform a normal distribution transformation coarse registration on the source point cloud to obtain the preliminary rigid body transformation matrix. S4. Using the initial rigid body transformation matrix as the initial value, perform generalized iterative nearest-point fine registration on the source point cloud and the target point cloud to obtain the accurate rigid body transformation matrix.

2. The point cloud registration method based on area array laser measurement according to claim 1, characterized in that, The pre-integration specifically refers to: Between two adjacent keyframe moments, the raw measurements of the inertial measurement unit are pre-integrated to obtain a pre-integrated result that includes relative rotation increment, velocity increment, and position increment.

3. The point cloud registration method based on area array laser measurement according to claim 2, characterized in that, In step S1, the initial relative pose between adjacent keyframes is solved using the pre-integration results, specifically including: Obtain the absolute pose, velocity, and gravity vector of the current keyframe i and the adjacent keyframe j in the world coordinate system, and calculate the relative rotation matrix based on the pre-integration results. and relative translation vector As the initial value for relative pose: ； ； in, This is the relative rotation matrix between two keyframes. Let be the absolute rotation matrices of the j-th frame and the i-th frame in the world coordinate system, respectively. This is the relative translation vector between two keyframes. Let be the absolute positions of the j-th frame and the i-th frame in the world coordinate system, respectively. Let be the speed of the i-th frame. The time interval between two keyframes. Let T be the vector of gravitational acceleration, and the superscript T is the transpose.

4. The point cloud registration method based on area array laser measurement according to claim 1, characterized in that, The motion distortion removal specifically refers to: The sensor pose at the start and end times of the depth map is obtained using the pre-integration results. Spherical linear interpolation is performed on the rotation component, and linear interpolation is performed on the translation component to obtain the sensor pose at the imaging time of any pixel within the depth map. ; Each pixel is back-projected into three-dimensional space according to its imaging time to obtain a three-dimensional point. reuse Sensor pose at the reference time of the depth map ,Will Transform to the reference time coordinate system.

5. The point cloud registration method based on area array laser measurement according to claim 1, characterized in that, In step S2, the fusion into a locally truncated symbolic distance function sub-map includes: A three-dimensional voxel mesh is constructed in a local reference coordinate system defined by a preset time window. Each voxel stores a cutoff distance value and a cumulative weight, with the initial values ​​of both being zero. For each frame of the distortion-free depth map within the window, it is projected into a 3D point cloud and transformed to a local reference coordinate system; along the ray direction from the sensor optical center to each 3D point, the voxels through which the ray passes are traversed, the sign distance value from the center of each voxel to the object surface is calculated, and the truncated distance value is updated by weighted averaging in combination with the cumulative weight, and the cumulative weight is further updated. The process continues until all frames within the window have been processed, resulting in a fused local truncation symbolic distance function submap.

6. The point cloud registration method based on area array laser measurement according to claim 5, characterized in that, In step S2, A dynamic voxel structure is used to distribute voxels only near the surface and adaptively adjust the fusion weights based on the distance from the measurement point to the sensor optical center or the incident angle.

7. The point cloud registration method based on area array laser measurement according to claim 1, characterized in that, In step S3, during the coarse registration process of the normal distribution transformation, if the matching score is lower than a preset threshold, a multi-resolution strategy is activated: first, a global search is performed at a larger grid size, and then the grid size is gradually refined for local optimization.

8. The point cloud registration method based on area array laser measurement according to claim 1, characterized in that, In step S4, the generalized iterative nearest point fine registration includes: Using the initial rigid body transformation matrix as the initial transformation, the source point cloud is transformed to the target point cloud coordinate system; For each point in the source point cloud and the target point cloud, search for its spherical neighborhood of radius r or the nearest K points to form a neighborhood point set, and calculate the local covariance matrix of that point. An initial point pair correspondence between the source point cloud and the target point cloud is established through nearest neighbor search. For each pair of matching points, its distance vector is calculated, and a weighted error term is constructed. Calculate the weighted error of all matching pairs. If the weighted error exceeds the threshold of the chi-square distribution with 3 degrees of freedom under the preset confidence level, the pair is determined to be an abnormal match and is removed. The remaining reliable point pairs are used to construct the optimization objective function. The incremental transformation is iteratively solved with the goal of minimizing the sum of all weighted errors, and the transformation matrix is ​​updated. The final accurate rigid body transformation matrix is ​​output when the rotation and translation components of the transformation increment are both less than the preset convergence threshold or the preset maximum number of iterations is reached.

9. The point cloud registration method based on area array laser measurement according to claim 8, characterized in that, The formula for the weighted error term is as follows: ； ； in, For weighted error, Let T be the distance vector between the i-th pair of matching points in space, with the superscript T being the transpose. Let i be the position vector of the target point. Let i be the position vector of the source point. This is the current rigid body transformation matrix. Let i be the local covariance matrix of the target point i. Let be the local covariance matrix of the source point i.

10. A point cloud registration system based on area array laser measurement, used to implement the point cloud registration method based on area array laser measurement as described in claim 1, characterized in that the system... include: The IMU pre-integration and motion distortion removal module is used to acquire continuous depth map sequences collected by the area array laser sensor and synchronous inertial measurement unit data, pre-integrate the inertial measurement unit data to obtain the initial relative pose values ​​between adjacent keyframes, and use the initial relative pose values ​​to remove motion distortion from the depth map sequence to obtain a distortion-free depth map sequence. The TSDF submap construction module is used to fuse multiple frames of distortion-free depth maps within a preset time window into a truncated symbolic distance function submap based on the initial relative pose value or the optimized pose, and extract the submap point cloud from the submap. The NDT coarse registration module is used to take the current sub-map point cloud as the source point cloud and the reference sub-map point cloud with overlapping areas as the target point cloud. It constructs a normal distribution transformation model for the target point cloud and uses the initial relative pose value as the initial transformation to perform normal distribution transformation coarse registration on the source point cloud to obtain the preliminary rigid body transformation matrix. The GICP fine registration module is used to perform generalized iterative nearest-point fine registration on the source point cloud and the target point cloud using the preliminary rigid body transformation matrix as the initial value, so as to obtain the accurate rigid body transformation matrix.

Images