A real-time model updating method for LiDAR space target pose measurement

Abstract

The present invention discloses a real-time model updating method for laser radar space target posture measurement, belonging to the field of laser radar technology. The method comprises using a guided filter to filter the target point cloud acquired by the laser radar, aligning the filtered point cloud with the pre-stored target reference model data, and calculating the relative posture of the target relative to the reference model; calculating the distance and overlap rate between the registered target point cloud and the reference model; voxelizing the overlapping area point cloud, weightedly fusing the laser radar point cloud and the pre-stored reference point cloud of the target voxel by voxel; and re-entering the posture solution module with the fused point cloud as a new model reference point cloud. Aiming at the problem of inconsistency between the target prior model and the laser radar data accuracy and local geometric features in the relative posture measurement of non-cooperative targets in space, the present invention proposes an on-orbit intelligent fusion method, which updates the reference model of the target in real time while estimating the posture, and can improve the accuracy and reliability of posture measurement.

Description

Technical Field

[0001] The present invention belongs to the technical field of laser radar, and in particular relates to a real-time updating method of a model for laser radar space target posture measurement. Background Art

[0002] In the position and posture measurement of non-cooperative targets in space based on LiDAR, the model matching algorithm is usually used to solve the position and posture of the target. The basic process of the model matching algorithm is to match the real-time three-dimensional point cloud data of the target with the pre-stored target prior model data to obtain the translation and rotation relationship between the two, that is, the relative position and posture of the target. Commonly used algorithms include iterative closest point algorithm (ICP), normal distributions transform (NDT), feature matching algorithm (FSR), etc. Among them, the ICP algorithm is more commonly used and performs well in the process of non-cooperative target measurement in space, and can achieve centimeter-level and sub-degree-level position and posture accuracy. The premise of this algorithm is that the reference model of the target is known, which is usually pre-stored in the LiDAR device. The known reference model generally comes from the design model CAD of the target or the three-dimensional reconstruction before orbit. However, after being on orbit, due to the influence of the external environment, its own activities and the aging effect of long-term orbit, the structure and motion characteristics of the target may change, and there are different degrees of inconsistency with the known target design model. For example, solar panels fail during deployment, and the solar panels of the Hubble telescope have cracks and perforations due to thermal expansion and contraction and micrometeoroid impacts. These factors will cause the pose measurement sensor to not always accurately match the actual point cloud data of the target, thereby reducing the accuracy of pose measurement. Therefore, it is necessary to adapt to new data through real-time model update methods to ensure the accuracy and robustness of the matching process between the two. Summary of the invention

[0003] In order to solve the above technical problems, the present invention provides a real-time model updating method for laser radar space target pose measurement, which estimates the matching overlap rate and distance error by cross-matching the target three-dimensional point cloud data with the reference model. Based on the pre-stored model, for the points in the overlapping area, the point cloud data acquired by the laser radar in real time is fused with the prior target model using a fusion method based on feature intensity to obtain an updated target reference model, thereby improving the target pose measurement accuracy.

[0004] To achieve the above purpose, the technical solution adopted by the present invention is as follows:

[0005] A real-time model updating method for laser radar space target pose measurement, the method comprising:

[0006] Step 1: Perform guided filtering on the point cloud data acquired by the laser radar;

[0007] Step 2: ICP registration of the guided filtered point cloud data with the pre-stored point cloud data of the target reference model;

[0008] Step 3, calculating the distance and overlapping area between the registered point cloud data and the point cloud data of the target reference model;

[0009] Step 4: Voxelize the point cloud in the overlapping area, perform weighted fusion of the registered point cloud data and the point cloud data of the pre-stored target reference model voxel by voxel, use the fused point cloud data as the point cloud data of the new target reference model, re-ICP-register it with the point cloud data after guided filtering, and calculate the relative pose of the target.

[0010] The beneficial effects of the present invention are:

[0011] In the present invention, guided filtering is first used to smooth the point cloud acquired by the laser radar to increase its similarity with the reference point cloud. Then, a point cloud weighted fusion method based on voxel filtering is used to fuse the point cloud acquired by the radar and the reference point cloud voxel by voxel, and finally generate an updated model point cloud. The updated model point cloud has higher consistency with the point cloud acquired by the laser radar in real time in terms of local details and accuracy, so it can improve the registration accuracy and achieve higher pose solution accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] Figure 1 A flow chart of a real-time updating method of a model for measuring the position and posture of a space target using a laser radar according to the present invention;

[0013] Figure 2 Comparison of LiDAR point clouds before and after filtering;

[0014] Figure 3 This is a comparison diagram of the target model point cloud before and after updating based on the method of the present invention. DETAILED DESCRIPTION

[0015] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0016] like Figure 1 As shown, a flow chart of a real-time model updating method for laser radar space target pose measurement provided by the present invention includes the following steps:

[0017] Step 1: Point cloud guided filtering. There are differences in accuracy and local details between the LiDAR point cloud and the ideal reference point cloud. Therefore, guided filtering is first performed on the point cloud data obtained by the LiDAR to achieve data smoothing. and the point before filtering There is the following linear relationship:

[0018] ,

[0019] in, and For point The filtering parameters can be obtained by solving the following optimization function.

[0020] ,

[0021] in, For Centered is the neighborhood point set of radius, for The specific steps include:

[0022] Step 1.1: Point cloud acquired by LiDAR The Points , by finding is the center and the radius is All points in the neighborhood of get its neighborhood point set :

[0023] ,

[0024] in, Represents a point set midpoint, Indicate point and Point The Euclidean distance between .

[0025] Using neighborhood point sets , calculate its mean and variance :

[0026] ,

[0027] ,

[0028] in, express 's points.

[0029] Step 1.2: Use the mean and variance , calculation point The linear filter coefficients and :

[0030] ,

[0031] ,

[0032] in, is the regularization parameter of the filter, which controls the smoothness of the filter.

[0033] Step 1.3, calculate the points The coordinates of the filtered points :

[0034] ,

[0035] For each point After the above filtering processing, the final filtered point cloud is obtained. .

[0036] Step 2: Point cloud registration. Point cloud registration uses the reference model point cloud As the benchmark, the point cloud after filtering in step 1 Perform ICP registration for the point cloud to be registered. The specific steps include:

[0037] Step 2.1: Point pair search. For the filtered point cloud Each point in , find the reference model point cloud The distance to its closest point :

[0038] ,

[0039] in, Represents the Euclidean distance between two points.

[0040] Step 2.2, calculate the centroid of the point cloud. Calculate the filtered point cloud separately and reference model point cloud The centroid and :

[0041] ,

[0042] ,

[0043] Step 2.3: Calculate the covariance matrix :

[0044] ,

[0045] Step 2.4: Calculate the rotation matrix and translation vectors . Decompose the covariance matrix using singular value decomposition (SVD) , and then use the decomposition result to calculate the rotation matrix and translation vectors :

[0046] ,

[0047] ,

[0048] ,

[0049] in, is the left singular vector after singular value decomposition, is a diagonal matrix, and the elements on the diagonal are the matrices The singular values ​​of is the right singular vector after singular value decomposition, represents its transposed vector, and the superscript t represents the transposed operation.

[0050] Step 2.5: Transform the filtered point cloud using the calculated rotation matrix and translation vector , calculate q and the reference model point cloud The distance function between :

[0051] ,

[0052] Repeat steps 2.1 to 2.5 until the distance function between point clouds is If it no longer decreases, the iteration stops, and the optimal rotation and translation matrix result can be obtained.

[0053] Step 2.6: Use the final rotation matrix and translation vectors Perform point cloud transformation to obtain the registered point cloud :

[0054] .

[0055] Step 3: Calculate the registered point cloud The distance and overlap area between the target model point cloud Q and the target model point cloud Q. After registration, first use the registered target point cloud as a reference to calculate the distance between each point and the model point cloud. Then, use the model point cloud as a reference to calculate the distance between each point and the target point cloud. Finally, select a suitable threshold, and the point set whose two distances are both less than the threshold is the overlap area of ​​the two point clouds.

[0056] Step 3.1: For the target point cloud after registration Each point in ,turn up The closest point , calculate the distance between each pair of closest points :

[0057] ,

[0058] ,

[0059] Step 3.2: For the model point cloud Each point in ,turn up The closest point in , and calculate the distance between each pair of points:

[0060] ,

[0061] ,

[0062] Step 3.3: Set the threshold , overlapping area Defined as:

[0063] .

[0064] Step 4, model update. For non-overlapping areas, the pre-stored reference point cloud is retained because the pre-stored reference point cloud usually has a complete target structure. The point cloud acquired by the lidar in real time is affected by the field of view, resolution, occlusion and target reflectivity, and the point cloud data usually has certain omissions or uneven density. For the point cloud in the overlapping area, voxelization is first performed to calculate the center coordinates of each voxel. Then, weighted filtering is performed voxel by voxel, and the points belonging to the reference point cloud and the points belonging to the target instantaneous point cloud in each voxel are weighted averaged according to a certain coefficient to obtain the centroid coordinates of the voxel. The weighted centroid coordinates of all voxels are the fused reference point cloud. Specifically including:

[0065] Step 4.1: Voxelization. Calculate the overlapping area point cloud The minimum bounding box coordinates , set the size of each voxel , divide the minimum bounding box into Calculate the center coordinates of each voxel .

[0066] ,

[0067] ,

[0068] Step 4.2: Find the point within the voxel. Calculate the point cloud acquired by the LiDAR The points in and the center coordinates of each voxel If the distance is less than the voxel size, the point cloud acquired by the lidar The points in the figure are located in the corresponding voxels, and the overlap area is obtained. Located in the corresponding voxel points and reference model point cloud In the voxel points.

[0069] Step 4.3: Estimation of weighted voxel centroid. For each voxel centroid , which is obtained by weighting all points in the voxel. is the set of points inside the voxel point, is the set of points inside the voxel point, is the weight coefficient.

[0070] ,

[0071] Step 4.4: Calculate the weighted center of all voxels, which is the fused reference point cloud. . Re-enter the model matching process and calculate the reference point cloud and target point cloud The relative position of the target.

[0072] Example

[0073] Taking the docking mechanism of an artificial spacecraft as an example, the target posture measurement is performed using a 64-line laser imaging radar through a ground docking simulation platform. In the experiment, the 64-line laser imaging radar is imaged at a distance of 13m from the target to obtain the point cloud data of the target. The reference model point cloud of the target is generated by the CAD design model of the target according to a certain sampling interval. The specific implementation steps are as follows:

[0074] Step 1: Use a 64-line laser radar to obtain the 3D point cloud of the current target at a distance of 13.5m , the true value of the measured position is (13.5m, 0, 0), and the true value of the measured angle is (30°, 0, 0). First, the target data is guided and filtered, and the filter neighborhood radius is set to 0.05m and the filter coefficient is set to 0.1. The point cloud data before and after filtering is compared. Figure 2 As shown in Figure 2, the target point cloud data is smooth after filtering, and the Gaussian noise is significantly improved.

[0075] Step 2: Use the iterative ICP algorithm to calculate the filtered target point cloud and reference point cloud The rotation matrix and translation vectors .

[0076] Step 3, use the above rotation matrix and translation vectors Transform the original target point cloud , calculate the transformed target point cloud and reference point cloud The nearest neighbor pair error and , set the distance threshold to 0.02 to get the overlapping area;

[0077] Step 4: voxelize the points in the overlapping area, and set the voxel size to 0.02m, and get For each voxel, extract the point sets belonging to the target point and the reference model point, and set the weight coefficient , calculate the weighted centroid coordinates of each voxel Finally, the centroids of all voxels form a new fused point cloud, such as Figure 3 As shown in Figure 2, the fused point cloud has the accuracy and local curvature features of the actual point cloud, while maintaining the global shape features of the target.

[0078] ,

[0079] The fused point cloud is used as a new reference point cloud to match the original measured point cloud data of the target and calculate the relative position and pose of the target. The target position and pose results before and after the update and the registration error calculation results are shown in Table 1. As can be seen from Table 1, after the update, the registration error is improved from 2.9cm to 1.6cm, and the registration accuracy is improved by 44.8%.

[0080] Table 1

[0081]

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

Claims

1. A real-time model updating method for laser radar space target pose measurement, characterized in that: The method comprises: Step 1: Guide filtering the point cloud data acquired by the laser radar; the guided filtering includes filtering the points in the point cloud data so that the filtered points and the point before filtering There is the following linear relationship: ， in, is the filtered point cloud, The point cloud obtained by LiDAR, and is the point before filtering The filtering parameters are obtained by solving the following optimization function: ， in, is the point before filtering Centered on is the neighborhood point set of radius, Neighborhood point set Hit a point, is the regularization parameter of the filter, which controls the smoothness of the filter; Step 2: ICP registration of the guided filtered point cloud data with the pre-stored point cloud data of the target reference model; Step 3, calculating the distance and overlapping area between the registered point cloud data and the point cloud data of the target reference model; Step 4: Voxelize the point cloud in the overlapping area, perform weighted fusion of the registered point cloud data and the point cloud data of the pre-stored target reference model voxel by voxel, use the fused point cloud data as the point cloud data of the new target reference model, re-ICP-register it with the point cloud data after guided filtering, and calculate the relative pose of the target.

2. A real-time model updating method for laser radar space target pose measurement according to claim 1, characterized in that: The step 1 comprises: Step 1.1: Point cloud acquired by LiDAR The Points , by finding is the center and the radius is All points in the neighborhood of get its neighborhood point set : ， in, Represents the neighborhood point set midpoint, Indicate point and Point The Euclidean distance between Using neighborhood point sets , calculate the mean and variance : ， ， in, express The number of points; Step 1.2: Use the mean and variance , calculation point The linear filter coefficients and : ， ， in, is the regularization parameter of the filter, which controls the smoothness of the filter; Step 1.3, calculate the points The coordinates of the filtered points : ， right Each point in is filtered to obtain the filtered point cloud .

3. The real-time model updating method for laser radar space target pose measurement according to claim 2 is characterized in that: Step 2 includes taking a reference model point cloud As the benchmark, the filtered point cloud For the point cloud to be registered, perform ICP registration, which specifically includes the following steps: Step 2.1: Point pair search; for the filtered point cloud Each point in , find the reference model point cloud The distance to its closest point : ， in, Represents the Euclidean distance between two points; Step 2.2, calculate the centroid of the point cloud; calculate the filtered point cloud separately and reference model point cloud The centroid and : ， ， Step 2.3: Calculate the covariance matrix : ， Step 2.4: Calculate the rotation matrix and translation vectors ; Decompose the covariance matrix using singular value decomposition , use the decomposition result to calculate the rotation matrix and translation vectors : ， ， ， in, is the left singular vector after singular value decomposition, is a diagonal matrix, and the elements on the diagonal are the matrices The singular values ​​of is the right singular vector after singular value decomposition, and the superscript t represents the transposed vector; Step 2.5: Transform the filtered point cloud using the calculated rotation matrix and translation vector , and calculate its and the reference model point cloud The distance function between : ， Repeat steps 2.1 to 2.5 until the distance function If it no longer decreases, the iteration stops and the optimal rotation and translation matrix result is obtained; Step 2.6: Use the final rotation matrix and translation vectors Perform point cloud transformation to obtain the registered point cloud .

4. The real-time model updating method for laser radar space target pose measurement according to claim 3 is characterized in that: The step 3 comprises: Step 3.1: For the registered point cloud Each point in , find the reference model point cloud The closest point , calculate the distance between each pair of closest points : ， ， Step 3.2: For the reference model point cloud Each point in , find the registered point cloud The closest point in , and calculate the distance between each pair of points: ， ， Step 3.3: Set the threshold , overlapping area Defined as: 。 5. The real-time model updating method for laser radar space target pose measurement according to claim 1 is characterized in that: The step 4 comprises: Step 4.1: Calculate the overlap area Minimum bounding box coordinates of point cloud , set the size of each voxel , divide the minimum bounding box into Calculate the center coordinates of each voxel : ， ， Step 4.2: Find points within voxels; calculate the point cloud acquired by the LiDAR The points in and the center coordinates of each voxel If the distance is less than the voxel size, the point cloud acquired by the lidar The points in the figure are located in the corresponding voxels, and the overlap area is obtained. Located in the corresponding voxel points and reference model point cloud Located within a voxel points; Step 4.3: Weighted voxel centroid estimation: For each voxel centroid , which is obtained by weighting all points in the voxel, where Point cloud acquired by LiDAR within a voxel The corresponding points, Point cloud of the reference model within the voxel The corresponding points, is the weight coefficient: ， Step 4.4: Calculate the weighted center of all voxels, which is the fused reference point cloud. , re-enter the model matching process and calculate the fused reference point cloud And the point cloud obtained by LiDAR The relative position of the target.

Images