[0052] The following describes in detail with reference to the accompanying drawings and specific embodiments.
[0053] The present invention is a method for judging the correctness of ICP pose matching of a laser point cloud of a space non-cooperative target. Ground physical simulation experiments are carried out to obtain a measurement point cloud of the space non-cooperative target and a known model point cloud of the space non-cooperative target; ICP pose matching, obtain the laser point cloud after ICP matching that is close to the standard laser point cloud of the real space non-cooperative target in space; obtain the standard laser point cloud of the real space non-cooperative target after ICP matching between the laser point cloud and the real space non-cooperative target in space above, the three-dimensional Euclidean space distance of each group of corresponding points; perform histogram distribution statistics to obtain the standard histogram distribution H2 of the point cloud distance; judge whether the distribution trends of the two histograms are consistent. The algorithm of the invention has simple data processing and low computational complexity, is suitable for engineering applications of spatial non-cooperative target pose matching, fully considers the influence of point cloud measurement noise, and can determine in real time whether the ICP pose matching achieves consistent pose matching, Then, the measurement accuracy of pose matching can be effectively improved.
[0054] In engineering applications, the ICP algorithm is used in the fine registration process of non-cooperative target pose matching, which can effectively improve the pose registration accuracy. However, the ICP algorithm is inevitably affected by measurement noise, resulting in consistent matching of point clouds. The situation that is not the global minimum value of ICP occurs from time to time, causing a coarse pose matching error in the pose matching. The accuracy is reduced, and the expected measurement accuracy cannot be achieved.
[0055] Under the influence of noise, the consistent matching of point clouds is not the case of the global minimum of ICP. The principle basis is as follows:
[0056] In the spatial non-cooperative target pose measurement, there are two types of point cloud sets, one is the target model point cloud set, denoted as Y={y j},j=1,...,N, one type is the point cloud set with noise obtained by lidar measurement, denoted as X={x i},i=1,...,M, where, The set of all ordered ternary real numbers. Usually the ICP algorithm is to find or estimate the rotation matrix R and the translation matrix t, R∈SO(3), SO(3) is a three-dimensional orthogonal group of space, is the set of all ordered ternary real numbers, so that the kernel function is in L 2 The error E under the norm is the smallest, as shown in the following formula:
[0057]
[0058]
[0059] In the formula, e i (R, t) is each point in the point set X and the corresponding point residuals.
[0060] By observing the above formula, if the measurement point cloud set X has no noise interference, it only corresponds to the corresponding model point cloud set The three-dimensional orthogonal group and translation matrix in space differ by a rotation matrix R∈SO(3) The set of all ordered ternary real numbers, then the estimated rotation and translation matrices that minimize the error E must be consistently close to the true rotation matrix R and translation matrix t, so ideally, the goal of the ICP algorithm is to search for the minimum error E. think at this time The closest to the true rotation matrix R and translation matrix t.
[0061] In engineering applications, the measurement point cloud obtained by lidar measurement is noisy, that is, the measurement point cloud set X and the corresponding model point cloud set In addition to differing by a rotation matrix R∈SO(3) space 3D orthogonal group and translation matrix In addition to the set of all ordered ternary real numbers, there is a measurement error K={k i}, and the error K={k i The distribution of} is related to the measuring instrument, so the measurement error K={k is introduced i} is not guaranteed to minimize the error E Estimate the closest true rotation matrix R and translation matrix t, that is, under the influence of noise, the consistent matching of point clouds is not necessarily the case when the ICP global minimum.
[0062] figure 2 (a) is the case where the measured point cloud after ICP and the standard laser point cloud achieve consistent and optimal pose matching. At this time, the objective function of ICP is 0.266, and the pose matching error of the two point clouds is zero. (b) is The measured point cloud after ICP and the standard laser point cloud reach the extremum of the optimal objective function of ICP, but do not reach the consistent optimal pose matching. At this time, the objective function of ICP is 0.199, and the pose matching error of the two point clouds is 3.6 degrees.
[0063] For example figure 2 (a) and (b) show that under the influence of measurement noise, when the global minimum of the ICP pose matching algorithm occurs, the matching pose between the measured point cloud and the model point cloud cannot be guaranteed to be consistent. . figure 2 The extremum of the ICP optimal objective function of (b) is 0.199, which is less than figure 2 0.206 for (a), but figure 2 The pose matching angle error of (b) is 3.6 degrees, which is greater than figure 2 (a) at 0 degrees, when figure 2(a) Consistent pose matching is achieved, but the optimal objective function of ICP is not the smallest.
[0064] like figure 1 As shown, the present invention is implemented according to the following steps:
[0065] (1) Carry out ground physical simulation experiments, use laser position and attitude sensors to scan and measure the space non-cooperative targets under simulated actual working conditions on the ground, and obtain the measurement point clouds of the space non-cooperative targets and the space non-cooperative targets. Know the model point cloud;
[0066] The ground physical simulation test refers to: within the working distance of the laser pose sensor, place a target with a known position and attitude relative to the laser pose sensor measurement coordinate system to simulate a non-cooperative space target and form a The known model point cloud of the target, denoted as P i =(x i ,y i ,z i ), i∈Ω set of natural numbers, the laser pose sensor is used to scan and measure the target, and the measured laser point cloud of the target is obtained, that is, the laser point cloud of the space non-cooperative target that simulates the actual working conditions and the target’s Known model point cloud, denoted as Q i =(xx i ,yy i ,zz i ), i∈Ω set of natural numbers, where, xx i ,yy i ,zz i They are the three-axis components of x, y, and z in the coordinate system measured by the attitude sensor.
[0067] (2) According to the measurement point cloud of the space non-cooperative target and the standard point cloud of the space non-cooperative target obtained in step (1), determine the standard histogram distribution H1 of the laser pose sensor;
[0068] For each point in the measurement point cloud of the space non-cooperative target, find the closest point corresponding to it in the standard point cloud of the space non-cooperative target, and form a corresponding point cloud pair. Cloud pair, to find the three-dimensional Euclidean space distance, that is, subtract the point coordinates in the standard point cloud of the space non-cooperative target from the point coordinates in the measurement point cloud of the non-cooperative target in space, and find its absolute value.
[0069] The calculation formula for obtaining the distance between the laser point cloud of the non-cooperative target and the known model point cloud of the target simulating the actual working conditions is: The set of natural numbers, for the distance d(Q i ,P i ) for histogram statistics and standardization by:
[0070] First get the maximum distance d(Q i ,P i ) max , normalize the distance d(Q i ,P i )/d(Q i ,P i ) max ,i∈Ω set of natural numbers, divided into n equal parts in the [0,1] interval, and the number of distances falling into each interval λ is counted n , the number of distances λ for each interval n Normalized processing λ n /max(λ n ), max(λ n ) represents λ n The maximum value in the laser pose sensor forms the standard histogram distribution H1, such as image 3 shown. In practical applications, n is determined according to needs, and usually n is 10 or 15.
[0071] (3) Using the laser pose sensor to measure the real space non-cooperative target in space, and obtain the measurement laser point cloud of the non-cooperative target;
[0072] The laser pose sensor is applied to the actual work of the space non-cooperative target pose measurement. The laser pose sensor emits laser light to the space non-cooperative target and receives the laser echo reflected by the space non-cooperative target surface. The time between the laser emission and the reception of the laser echo, and then according to the propagation speed of the laser, and the azimuth and elevation angles of the laser emission, a measurement coordinate system of the space non-cooperative target relative to the laser pose sensor is calculated. According to this principle, the entire measurement field of the laser pose sensor completes the laser emission and laser echo reception work in turn in a measurement cycle, and a frame of spatial non-cooperative target measurement can be formed. Laser point cloud.
[0073] (4) According to the standard laser point cloud of the real space non-cooperative target in the space and the measured laser point cloud of the real space non-cooperative target in the space measured in step (3), perform ICP pose matching, and obtain a close to the space. The laser point cloud after ICP matching of the standard laser point cloud of the real space non-cooperative target;
[0074] The ICP matching algorithm is used to perform pose matching iteration on the measurement laser point cloud of the space non-cooperative target and the standard laser point cloud of the space non-cooperative target, and use the measurement laser point cloud of the space non-cooperative target and the standard laser point of the space non-cooperative target. The minimum sum of the squares of the absolute distances of each group of points corresponding to the cloud is the iterative target, and each iteration of pose matching is performed, the measurement laser point cloud of the non-cooperative target in space and the standard laser point cloud of the non-cooperative target in space correspond to each The sum of the squares of the absolute distances of the group points will shrink a little, and the pose of the measured laser point cloud of the space non-cooperative target will also be closer to that of the standard laser point cloud of the space non-cooperative target, until each group of the above two point clouds. The sum of the squares of the absolute distances of the points reaches the predetermined threshold for stopping the pose matching iteration. At this time, the measured laser point cloud of the spatial non-cooperative target with a new pose state after the pose matching iteration is defined as close to The laser point cloud after ICP matching of the standard laser point cloud of the real spatial non-cooperative target in space.
[0075] The standard laser point cloud of the real space non-cooperative target in the space is obtained by sampling and extracting the point cloud of the mechanical model of the non-cooperative target. The applied extraction software can be MeshLab, and then the sampled point cloud is written or uploaded in advance and injected into the laser In the control computer of the pose sensor, the standard laser point cloud of the real space non-cooperative target is formed for the target pose matching measurement application.
[0076] (5) obtaining the three-dimensional Euclidean space distance of each group of corresponding points on the laser point cloud after ICP matching obtained in step (4) and the standard laser point cloud of the real space non-cooperative target in space;
[0077] The calculation method of the three-dimensional Euclidean space distance of each group of corresponding points is to subtract and square the three-dimensional coordinate values of the corresponding two points of each group, then sum the squared results, and then sum The square root of the result, that is, the three-dimensional Euclidean space distance of each group of corresponding points is obtained. Expressed in mathematical formulas as:
[0078] The set of all ordered ternary real numbers.
[0079] (6) performing histogram distribution statistics on the three-dimensional Euclidean space distance of each group of corresponding points in step (5) to obtain the standard histogram distribution H2 of the point cloud distance;
[0080] Histogram statistics and standardization methods are the same as in step (2). The point cloud distance histogram distribution H2 is as follows Figure 4 shown.
[0081] (7) According to the standard histogram distribution H1 of the laser pose sensor in step (2) and the standard histogram distribution H2 of the point cloud distance in step (6), determine whether the distribution trends of the two histograms are consistent, and if they are consistent, determine step (4) ) ICP pose matching is correct, otherwise it is determined that step (4) ICP pose matching is incorrect.
[0082] Method 1 is: find the mean value of the sum of the squares of the difference between the standard histogram distribution H1 of the laser pose sensor and the standard histogram distribution H2 of the point cloud distance in the corresponding interval of the number of distances rms =[(γ 1 -λ 1 ) 2 +(γ 2 -λ 2 ) 2 +…+(γ n -λ n ) 2 ]/n, set the threshold ρ, when Δ rms ≤ρ, it is judged that the pose matching consistency is obtained; when Δ rmsρ, it is determined that the pose matching consistency is not obtained.
[0083] Method 2: Sort the number of distances that fall into each interval in the standard histogram distribution H1 of the laser pose sensor and the standard histogram distribution H2 of point cloud distances in descending order, and record the corresponding interval serial number. The standard histogram distribution H1 of the pose sensor and the standard histogram distribution H2 of point cloud distances are exactly the same as the interval numbers corresponding to the number of distances that are currently m larger than the number of distances. Preferably, m is 3 to 5, and it is determined that the pose matching is obtained. Consistency; otherwise, it is determined that the pose matching consistency is not obtained. like Figure 5 shown.
[0084] The present invention is a space non-cooperative target laser point cloud ICP position and attitude matching correctness judging system, comprising: a ground physical simulation test module, a point cloud determination module, a measurement module, a pose matching module, a distance determination module, a statistics module, and a judgment module ;
[0085] Ground physics simulation test module, carry out ground physics simulation test, use laser position and attitude sensor to scan and measure the space non-cooperative target under simulated actual working conditions on the ground, and obtain the measurement point cloud of the space non-cooperative target and the space non-cooperative target the known model point cloud of the target;
[0086] The point cloud determination module determines the standard histogram distribution H1 of the laser pose sensor according to the measured point cloud of the space non-cooperative target and the standard point cloud of the space non-cooperative target obtained by the ground physics simulation test module;
[0087] The measurement module uses a laser pose sensor to measure the real space non-cooperative target in space, and obtain the measurement laser point cloud of the non-cooperative target;
[0088] The pose matching module, according to the standard laser point cloud of the real space non-cooperative target in space and the measured laser point cloud of the real space non-cooperative target in the space measured by the measurement module, performs ICP pose matching, and obtains a close to the space. The laser point cloud after ICP matching of the standard laser point cloud of the real space non-cooperative target;
[0089] The distance determination module obtains the three-dimensional Euclidean space distance of each group of corresponding points on the ICP matching laser point cloud obtained by the pose matching module and the standard laser point cloud of the real space non-cooperative target in space;
[0090] The statistical module performs histogram distribution statistics on the three-dimensional Euclidean space distance of each group of corresponding points in the distance determination module, and obtains the standard histogram distribution H2 of the point cloud distance;
[0091] Judging module, according to the standard histogram distribution H1 of the laser pose sensor in the point cloud determination module and the standard histogram distribution H2 of the point cloud distance in the statistics module, to determine whether the distribution trends of the two histograms are consistent, and if they are consistent, determine the pose The ICP pose matching in the matching module is correct, otherwise it is determined that the ICP pose matching in the pose matching module is incorrect.
[0092]The core idea of the present invention is to use the point cloud measurement error distribution of the laser pose sensor to maintain the consistent characteristics of the distribution under ground test conditions and in the real laser point cloud measurement process. After the ICP pose matching, if the distribution of the standard histogram distribution H2 of the point cloud distance and the standard histogram distribution H1 of the laser pose sensor obtained under the ground test conditions still maintain the consistency of this distribution, it is determined that In order to obtain the pose matching consistency, otherwise it is determined that the pose matching consistency has not been obtained. This method can effectively discriminate that the ICP algorithm falls into a local minimum or fails to achieve the best pose matching under noise interference, making it possible to correct the pose matching subsequently, thereby improving the pose matching accuracy.
[0093] use figure 2 The point cloud simulation data in (a), (b) carries out a test verification of the algorithm of the present invention:
[0094] First, the standard histogram distribution H1 of the laser pose sensor is obtained, as Image 6 shown;
[0095] Then, as in figure 2 As shown in (b), the ICP algorithm is used to perform pose matching between the measured laser point cloud of the non-cooperative target obtained by one measurement and the standard laser point cloud of the non-cooperative target in space, and the optimal objective function extreme value of ICP is obtained as 0.199. When , according to the standard laser point cloud of the laser point cloud after ICP matching and the spatial non-cooperative target, the standard histogram distribution H2 of the distance to the point cloud is drawn, such as Figure 7 shown.
[0096] Use method 1 to determine whether the distribution trends of the two histograms are consistent, and get
[0097] Δ rms =[(0.94-1) 2 +(0.81-0.91) 2 +(1-0.69) 2
[0098] +(0.68-0.50) 2 +(0.36-0.34) 2 +(0.20-0.25) 2
[0099] +(0.20-0.20) 2 +(0.18-0.18) 2 +(0.10-0.10) 2 +(0.10-0.10) 2 ]/10
[0100] =0.0145
[0101] The threshold value is set to 0.01, and the reference basis for the threshold value is the square of the measurement accuracy of the laser pose sensor. In this simulation, the measurement accuracy of the laser pose sensor is set to 0.1m, so the threshold value is set to 0.01. Therefore judge figure 2 (b) The two sets of point clouds matched by ICP do not achieve pose matching consistency. pass figure 2 It can be seen from (a) and (b) of this method that the judgment of this method is effective. When the optimal objective function of ICP reaches the extreme 0.199, the angle error of the pose matching of the two point clouds is still 3.6 degrees, while the point The optimal matching pose of the cloud is 0 degrees. If there is no determination method of the present invention, a coarse pose matching error of 3.6 degrees will be introduced in the pose matching solution, thereby reducing the pose matching accuracy of the measurement system.
[0102] also in figure 2 On the basis of the data of (a) and (b), the same judgment result can also be obtained by using method 2 to judge whether the distribution trends of the two histograms are consistent. figure 2 (b) The two sets of point clouds matched by ICP do not achieve pose matching consistency.
[0103] The method of the present invention has been verified by computer simulation test and physical test of space non-cooperative target flying around, approaching, and capturing a full physical test platform. The test results show that the method of the present invention can compare the measurement noise distribution measured by the lidar point cloud with the ICP The consistency of the distance difference distribution between the corresponding points of the two point clouds is used as a criterion for evaluating whether the matching of the point clouds has reached a consistent matching. Subsequent corrective matching is possible, thereby improving the pose matching accuracy.
[0104] In the present invention, the instrument noise of point cloud measurement is considered, and the consistency between the measurement noise distribution of lidar point cloud measurement and the distance difference distribution between the corresponding points of the two point clouds after the ICP is used as the criterion for evaluating whether the point cloud matching has reached a consistent matching. It can effectively discriminate that the ICP algorithm falls into the local minimum or fails to achieve the best matching of the pose and pose under the interference of noise, making it possible to correct the subsequent matching, thereby improving the accuracy of pose matching. The method of the invention is feasible, and the influence of measurement noise on the algorithm is considered, so it has practicability. The present invention considers that in engineering application, the ICP algorithm is inevitably affected by the measurement noise, resulting in the consistent matching of point clouds not being the global minimum value of the ICP. The method of the present invention can identify such a situation in time, which is practical in engineering. Value.
[0105] The two methods for judging whether the distribution trends of two histograms are consistent or not provided in the present invention have the characteristics of simple method, low computational complexity, and effective determination of the consistency of the distribution trends, and are suitable for application under the condition of limited space on-orbit computing resources . Due to the low computational complexity, the method of the present invention can realize real-time ICP pose matching correctness judgment, timely find the situation that the ICP matching does not reach the best matching, which is beneficial to the subsequent algorithm to perform pose matching correction in time, and avoid the appearance of coarse poses matching error.
