A point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization

Abstract

The application discloses a point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization, relates to the technical field of point cloud processing, and aims to solve the problems of motion distortion and self-occlusion of a cleaning machine in a scanning process; the method comprises the following steps: S1, filtering the point cloud of the cleaning machine itself, and performing point cloud motion distortion compensation based on linear difference of time stamps; S2, optimizing the point cloud by using a self-adaptive voxel filtering method, wherein the self-adaptive voxel filtering method comprises point cloud storage based on hashing and adaptive filtering size selection; S3, constructing an angle prediction method under boundary constraint conditions, and specifically calculating a minimum circumscribed rectangle and minimizing a boundary angle to obtain a predicted value of a rotation angle; and S4, registering the point cloud by using a generalized registration method based on hashing lookup, and generating pose data. The point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization can improve the quality of the point cloud and meet the demand for high-precision positioning.

Description

TECHNICAL FIELD

[0001] The application relates to the technical field of point cloud processing, and particularly relates to a point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization. BACKGROUND

[0002] The ship cabin cleaning machine needs to work in a closed space with coal dust and complex structure. The mechanical laser radar (such as Speedten 32 / 64 line) carried by the ship cabin cleaning machine has low scanning frequency (10 Hz), motion distortion and self-occlusion, which leads to poor point cloud quality, and traditional point cloud processing methods cannot meet the high-precision positioning requirements.

[0003] A push rake machine positioning method based on ship unloading machine laser cooperative positioning is disclosed in Chinese Patent No. CN118363032A, which uses multi-radar cooperative registration and relies on external equipment (ship unloading machine), and does not involve the problem of single-device point cloud quality optimization, and cannot solve the registration divergence problem of motion distortion and heading angle mutation. SUMMARY

[0004] The application solves the problems of motion distortion and self-occlusion in the scanning process of the cleaning machine, and proposes a point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization, which improves the quality of point cloud and meets the high-precision positioning requirements.

[0005] In order to achieve the above purpose, the application adopts the following technical scheme: a point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization, comprising the following steps:

[0006] S1, filtering the point cloud of the cleaning machine itself, and compensating for the motion distortion of the point cloud based on the linear difference of the time stamp;

[0007] S2, optimizing the point cloud by an adaptive voxel filtering method, the adaptive voxel filtering method comprising point cloud storage based on hash and adaptive filter size selection;

[0008] S3, constructing an angle prediction method under boundary constraints, and specifically calculating the prediction value of the rotation angle by the minimum circumscribed rectangle calculation and the minimization of the boundary angle;

[0009] S4, using a generalized registration method based on hash lookup to register the point cloud and generate pose data.

[0010] The point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization mainly includes the following processes: cubic self-occlusion point cloud filtering based on the 3D size of the cleaning machine; motion distortion compensation based on linear interpolation of the time stamp; adaptive filtering of the voxel size through closed-loop feedback control; heading angle prediction by fusing the geometric constraints of the boundary rectangle; and covariance weighted generalized registration based on hash neighborhood search.

[0011] The application is further provided that: the filtering of the point cloud of the stripping machine itself comprises:

[0012] According to the three-dimensional appearance of the stripping machine, the spatial size range thereof relative to the set radar coordinate system is obtained, denoted as a first range;

[0013] It is judged whether the scanned point cloud is within the first range, and the point cloud within the first range is removed, so as to filter the point cloud of the stripping machine body.

[0014] In the technical solution, the laser radar installed on the stripping machine cannot capture part of the point cloud data due to the shielding area of the stripping machine body, and the filtering operation is used to remove the part of the point cloud data.

[0015] The application is further provided that: the point cloud motion distortion compensation based on linear difference value of time stamp comprises:

[0016] The time stamp of each scanning point in the frame period is analyzed to establish a normalized time proportion relationship with the starting time of the frame; a linear displacement interpolation model is constructed according to the adjacent frame pose transformation matrix, the instantaneous pose parameters corresponding to each time proportion are solved, and finally all the point clouds are mapped to the world coordinate system by transformation.

[0017] In the technical solution, the motion of the stripping machine causes the scanning point cloud to be deformed, so motion distortion correction is needed.

[0018] The application is further provided that: the point cloud storage based on hash comprises:

[0019] The total size of the down-sampled point cloud is accurately calculated by multiplying the point cloud statistics in the voxel grid by the number of voxel grids, and the rapid counting of the number of point clouds is realized by hash key value mapping.

[0020] In the technical solution, the adaptive voxel filtering method needs to dynamically evaluate the number of point clouds under different voxel sizes, and an efficient point cloud statistics and storage mechanism needs to be established. Therefore, the application designs a point cloud storage method based on hash index to realize efficient point cloud number statistics.

[0021] The application is further provided that: the adaptive filtering size selection comprises:

[0022] The filtering size L1 at the initial time is calculated according to the input point cloud data and the specified output point cloud number Np;

[0023] The number of point clouds N1 is calculated by using the point cloud storage based on hash;

[0024] The filter size is iterated to calculate the filter size Lk and the number of point clouds Nk. It is then determined whether the error between Np and Nk is less than the error stopping threshold Es. If so, the final filter size is obtained, where k is the number of iterations.

[0025] The present invention is further configured such that: the calculation of the minimum bounding rectangle includes: performing planar projection on the original point cloud, constructing the minimum closed region based on the convex hull construction algorithm; and finally obtaining the minimum bounding rectangle through angle rotation optimization to achieve the extraction of boundary features.

[0026] The present invention is further configured such that: minimizing the boundary angle includes: extracting the azimuth angles of the circumscribed rectangle boundary vectors of the current point cloud and the target point cloud respectively, establishing an angle difference optimization function, and minimizing the optimization function to obtain the angle prediction value.

[0027] The present invention is further configured such that the generalized registration method based on hash lookup includes the following steps:

[0028] Q1, find the point q in the adjacent voxel block of the target point cloud Q of point p in the input point cloud by means of the nearest neighbor search;

[0029] Q2, calculate the covariance of the point sets near p and q respectively;

[0030] Q3. Point cloud registration is performed using p, q, and their covariance.

[0031] The present invention is further configured such that step Q3 includes:

[0032] An objective function is constructed, which minimizes the alignment error between the input point cloud and the target point cloud by optimizing the rigid transformation parameters, including the rotation matrix R and the translation vector t. The optimal transformation parameters are obtained through iteration.

[0033] The present invention is further configured such that step Q1 includes:

[0034] The current corrected input point cloud is P, and the target point cloud in the world coordinate system is Q. For each point p in P, calculate its coordinates in voxel space; traverse multiple voxel blocks centered at this coordinate, search for a point q in Q in each adjacent voxel block, and calculate the distance between p and q; select the point q with the smallest distance as the candidate nearest corresponding point of p; if this distance is less than the distance threshold, then p and q are considered as a corresponding point pair.

[0035] The present invention can bring the following beneficial effects:

[0036] The point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization of the application can solve the problems of motion distortion and self-occlusion, improve the quality of point cloud, and meet the demand of high-precision positioning; the self-occlusion point cloud is completely removed through point cloud filtering; the retention rate of cabin corner features is greatly improved through the adaptive voxel filtering method. BRIEF DESCRIPTION OF DRAWINGS

[0037] Figure 1 is a general schematic diagram of the point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization of the application.

[0038] Figure 2 is a flowchart of step S1 of the point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization of the application.

[0039] Figure 3 is a point cloud distribution schematic diagram of the point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization of the application.

[0040] Figure 4 is a flowchart of the adaptive voxel filtering method of the application.

[0041] Figure 5 is a flowchart of the angle prediction method based on boundary constraints of the application.

[0042] Figure 6 is a flowchart of the minimum circumscribed rectangle calculation of the application.

[0043] Figure 7 is a schematic diagram of the angle prediction method of the application.

[0044] Figure 8 is a schematic diagram of the performance analysis of the adaptive voxel filtering algorithm of the application.

[0045] Figure 9 is a schematic diagram of the time-consuming performance analysis of the angle prediction of the application.

[0046] Figure 10 is a schematic diagram of the accuracy performance analysis of the angle prediction of the application.

[0047] Reference signs: 1, laser radar; 2, radar capture area; 3, cabin cleaning machine; 4, vehicle body occlusion area. DETAILED DESCRIPTION

[0048] Embodiment 1

[0049] This embodiment proposes a point cloud positioning method based on self-occlusion filtering and dynamic voxel optimization, referring to Figures 1 to 6 which mainly includes the following steps.

[0050] Step S1, first filter the point cloud of the stripping machine itself, and then compensate for the point cloud motion distortion based on the linear difference of the time stamp.

[0051] Reference Figure 2 and Figure 3 The process of filtering the point cloud of the stripping machine itself specifically includes the following two steps.

[0052] The first step is to obtain the spatial size range of the stripping machine relative to the set radar coordinate system according to the three-dimensional appearance of the stripping machine, denoted as the first range.

[0053] The second step is to determine whether the scanned point cloud is within the first range, and to remove the point cloud within the first range to achieve the filtering of the stripping machine body point cloud.

[0054] In this technical solution, the laser radar installed on the stripping machine cannot capture part of the point cloud data due to the blocking area of the stripping machine body. Therefore, the above filtering operation is used to remove this part of the point cloud data.

[0055] In order to improve the integrity of the ship cabin mapping, it is necessary to expand the effective sensing range of the sensor. The current acquisition technical solution mainly includes a large vertical field angle radar scheme and a high installation scheme. The high installation scheme causes the system mass center to deviate, which easily causes high-frequency noise interference of the point cloud.

[0056] Thanks to the development of solid-state laser radar technology, large field angle sensors have been implemented in engineering applications, but inevitably produce point cloud interference of the stripping machine body, Figure 3 The point cloud distribution captured by the radar installed on the stripping machine is shown in the figure. As can be seen from the figure, the laser radar 1 realizes 360-degree scanning by continuous rotation, and the radar capture area 2 is the data that can be captured by the laser radar. The body blocking area 4 is the data of the stripping machine 3 itself irradiated by the radar, which needs to be removed.

[0057] More specifically, first, the spatial size of the stripping machine relative to the radar coordinate system is obtained according to the three-dimensional appearance of the stripping machine, denoted as Xmin, Xmax, Ymin, Ymax, Zmix, and Zmax. Since the size of the stripping machine is known, the stripping machine body point cloud can be wrapped in a cube in the radar coordinate system. The point cloud distribution contained in the cube is within the above maximum and minimum values. Based on the size information of the stripping machine, the point cloud belonging to the stripping machine body is selected from the collected data, and then these selected point clouds are deleted from the original data, thereby achieving the filtering of the stripping machine body point cloud.

[0058] Compensating for the point cloud motion distortion based on the linear difference of the time stamp includes the following steps.

[0059] The time stamp of each scanning point in a frame period is analyzed to establish a normalized time proportion relationship with the starting time of the frame; a linear displacement interpolation model is constructed according to the adjacent frame pose transformation matrix, the instantaneous pose parameters corresponding to each time proportion are solved, and finally all point clouds are uniformly mapped to the world coordinate system through transformation.

[0060] In the technical solution, the movement of the cleaning machine causes the scanning point cloud to deform, so motion distortion correction is needed.

[0061] The rotary mechanical laser radar installed on the cleaning machine has a scanning frequency of about 10Hz, which causes the time interval between adjacent data acquisition to be 0.1s; during the 0.1s interval between adjacent data acquisition, the point cloud data is not collected at the same time. The laser radar installed on the cleaning machine is a Speedy 32-line or 64-line radar, which works based on the TOF principle, that is, the distance between the target object and the sensor is calculated according to the flight time of the laser between the transmitting end and the receiving end. During the 0.1s capture period of the sensor, the laser data acquired at different times will change as the cleaning machine is in motion, thereby causing point cloud distortion.

[0062] The point cloud distortion phenomenon is specifically that as the cleaning machine moves, the coordinate systems corresponding to the points in a frame of point cloud are inconsistent. When the cleaning machine moves along the motion direction, the captured data begins to distort.

[0063] During dynamic operation, the point cloud of the cleaning machine collected by the radar forms a persistent interference source. At the same time, the movement of the cleaning machine causes the scanning point cloud to deform, so motion distortion correction is needed. Therefore, the present application sets up a preprocessing method as shown in Figure 2 , which includes point cloud filtering and motion compensation.

[0064] The specific process includes: first, using a straight-through filtering algorithm to remove the point cloud of the cleaning machine body, eliminating self-shielding interference. Then, motion distortion compensation is implemented, the scanning timing is calculated from the time stamp information of each point cloud data packet, a linear interpolation model between adjacent poses is established based on the assumption of uniform motion; by solving the acquisition time of each laser beam, the radar pose transformation matrix at the corresponding time is derived, and finally the linear pose interpolation is used to realize the de-distortion of all scanning points, and the point cloud data is uniformly mapped to the world coordinate system.

[0065] Reference Figure 4 , step S2, the point cloud is optimized by an adaptive voxel filtering method, which includes hash-based point cloud storage and adaptive filter size selection.

[0066] For hash-based point cloud storage, the following steps are included:

[0067] The total size of the down-sampled point cloud is accurately calculated by multiplying the point cloud statistics in the voxel grid with the number of voxel grids, and the point cloud number is quickly counted through the hash key value mapping.

[0068] In the technical solution, in order to dynamically evaluate the point cloud number under different voxel sizes for the adaptive voxel filtering method, an efficient point cloud statistics and storage mechanism needs to be established.

[0069] For the specific implementation of the hash storage method, as shown in the figure, Figure 8 The single-point voxel storage strategy is adopted, that is, each voxel unit only retains single representative point cloud data. The strategy directly calculates the global point cloud size through voxel grid statistics, thereby improving the filtering efficiency. The hash function design needs to avoid the hash collision problem to avoid the retrieval failure problem. The hash function construction scheme is adopted in the embodiment, and the robust mapping of three-dimensional coordinates to hash values is realized through prime multiplication, bit operation and mask operation.

[0070] For adaptive filtering size selection, the following process is included.

[0071] The filtering size L1 at the initial time is calculated according to the input point cloud data and the specified output point cloud number Np; the point cloud number N1 is calculated by using the hash-based point cloud storage process; the filtering size iteration is performed to calculate the filtering size Lk and the point cloud number Nk, and it is judged whether the error of Np and Nk is less than the error stop threshold Es, if yes, the final filtering size is obtained, and k is the iteration number.

[0072] The hash-based point cloud storage process provides a basis for the calculation and storage of the point cloud number, and the specified number of point clouds can be realized through a reasonable filtering size selection strategy. The adaptive filtering size selection method is designed by using the characteristics that the voxel filtering size is inversely proportional to the output number, that is, the larger the filtering size, the fewer the output point clouds. When the output number is less than the set value, the filtering size is reduced; when the output number is more than the set value, the voxel size is increased, so that the point cloud data close to the preset number is output.

[0073] The adaptive voxel filtering method of the present application does not need to specify the filtering size in advance, only needs to input the point cloud data, and the algorithm can automatically determine the filtering size according to the spatial distribution characteristics of the point cloud, and finally output the specified number of point cloud data. This way avoids poor filtering effect caused by improper parameter setting.

[0074] For the process of adaptive voxel filtering size confirmation, the following specific steps are included.

[0075] Selection of filter size at initial time: for the selection of the initial filter size, assuming that the input point cloud data is a regular cubic structure, the maximum and minimum values Xmin, Xmax, Ymin, Ymax, Zmix, Zmax of the input point cloud and the specified output point cloud number Np are input to calculate the filter size L1 at the initial time.

[0076] Point cloud number calculation of filter size at initial time: according to the filter size L1 at the initial time, the point cloud number N1 is calculated based on the hash-based point cloud storage.

[0077] Selection of filter size in iteration: since there is a gap between the point cloud number calculated by the initial filter size and the specified output point cloud number, it is necessary to continuously correct the output point cloud number, and the next iteration filter size Lk can be determined by the point cloud number Np, the number of points in the voxel with filter size Lk Nk, the difference Ek between the output point cloud number under filter size Lk and the specified output point cloud number, the difference Ek between the output point cloud number under filter size Lk-1 and the specified output point cloud number, and the scaling factor r (set to 0.1 in the present application).

[0078] Comparison of output point cloud number in iteration with specified number: according to the size of the filter size Lk, the corresponding output point cloud number Nk is obtained, and the error Ek with the specified point cloud number Np is calculated, if the error Ek is less than the error stop threshold Es, the final filter size is obtained, otherwise the iteration is returned.

[0079] After obtaining the final filter size, the whole adaptive voxel filtering algorithm can be obtained.

[0080] Reference Figure 5 , Figure 6 and Figure 7 , step S3, an angle prediction method under boundary constraint is constructed, and the prediction value of the rotation angle is obtained by minimum circumscribed rectangle calculation and minimum boundary angle.

[0081] The optimized point cloud generated by the point cloud preprocessing and adaptive voxel filtering of the stripping machine still has the problem of sudden change of the heading angle caused by instability of the stripping machine when moving in the cabin, although it has the characteristics of moderate features and low noise. The constant speed model prediction method is prone to local extreme convergence in the point cloud registration process, which further leads to the decrease of the accuracy of the matching odometer. Therefore, the present application proposes a heading angle prediction method based on boundary constraint, which includes two parts: minimum circumscribed rectangle calculation and minimum boundary angle.

[0082] Firstly, the minimum enclosing rectangle is generated based on the convex hull algorithm for the extraction of the cabin boundary features, thereby constructing the geometric constraints; then, the optimization function between the target point cloud and the boundary features is established by minimizing the boundary angle, and the optimal heading angle prediction value is obtained through iteration. This method can effectively deal with the interference of large-angle distortion on the point cloud matching of the ship unloader.

[0083] The minimum enclosing rectangle calculation mainly includes the following processes: plane projection of the original point cloud, construction of the minimum closed region based on the convex hull algorithm; and finally, the minimum enclosing rectangle is obtained through angle rotation optimization to realize the extraction of the boundary features.

[0084] The ship unloader operates on the slope bottom of the coal pile in the cabin, and the roll angle and pitch angle in its pose parameters remain relatively stable, while the yaw angle is significantly affected by the turning motion of the vehicle body, especially in the slope turning working condition, which easily causes motion instability and leads to sudden changes in the heading angle. Based on the analysis of the distribution characteristics of the point cloud, the minimum enclosing rectangle generated by projection can effectively represent the planar geometric constraints of the cabin boundary, and the non-key axial interference is eliminated through projection to reduce the calculation redundancy.

[0085] Reference Figure 7 The minimum boundary angle mainly includes the following processes: the boundary vector azimuth angles of the current point cloud ( Figure 7 in the left image) and the target point cloud ( Figure 7 in the right image) are extracted, an angle difference optimization function is established, and the angle prediction value is obtained by minimizing the optimization function.

[0086] After obtaining the minimum enclosing rectangle, the four corner points of the rectangle are first extracted, and the angle of each edge vector of the rectangle is calculated. Finally, the yaw angle is predicted by minimizing the angle difference of the rectangular edge after rotation of the input point cloud and the target point cloud.

[0087] The minimum enclosing rectangle and the minimum boundary angle can obtain the angle prediction method based on boundary constraints.

[0088] Step S4, using a generalized registration method based on hash lookup to register the point cloud and generate pose data.

[0089] The generalized registration method based on hash lookup includes the following steps.

[0090] Step Q1, find the point p in the input point cloud in the adjacent voxel block about the point q of the target point cloud Q through adjacent point search; specifically, this step includes the following processes.

[0091] The current corrected input point cloud is P, and the target point cloud in the world coordinate system is Q. For each point p in P, the coordinates of p in the voxel space are calculated. A plurality of voxel blocks centered on the coordinates are traversed, and a point q of Q is searched in each adjacent voxel block. The distance between p and q is calculated. The point q with the smallest distance is selected as the candidate nearest corresponding point of p. If the distance is smaller than a distance threshold, p and q are taken as the corresponding point pair.

[0092] In step Q2, the covariances of the point sets near p and q are calculated respectively. The covariance of the target point cloud Q is Σq, and the covariance of the input point cloud P is Σp.

[0093] In step Q3, the point cloud registration is performed by using p, q, and the covariances of p and q.

[0094] Regarding the point cloud registration, a target function F is defined, and the objective is to minimize the alignment error between the input point cloud and the target point cloud by optimizing the rigid transformation parameters (rotation matrix R and translation vector t). The function is the sum of weighted squared distances, and the weights are determined by the covariance matrices Σq and Σp. The derivatives of the target function F with respect to R and t are calculated, and the rotation matrix increment ΔR and the translation vector increment Δt are solved by a linear equation system. The transformation parameters are updated.

[0095] The optimal transformation parameters are obtained by iteration, and the process includes the following steps.

[0096] 1. Initialization: Given the initial values R0 and to of the rotation matrix R and the translation vector t, set the maximum iteration number kmax and the convergence threshold.

[0097] 2. Iterative optimization: for the kth iteration, the linear equation system is constructed according to the gradient formula of the target function F with respect to R and t to solve the rotation matrix increment ΔRk and the translation vector increment Δtk.

[0098] 3. Convergence judgment: if ΔRk and Δtk are small enough (satisfy the set convergence threshold) or reach the maximum iteration number, stop iteration, and the Rk and tk at this time are the optimal transformation parameters; otherwise, continue iteration.

[0099] After obtaining the predicted value of the rotation angle, it is matched with the target point cloud to generate the pose data. The radar used by the ship unloader is a radar with a large vertical field of view angle, and its beam is relatively sparse. If the point-to-point matching operation is performed, it will cause incorrect correspondence relationship, resulting in deviation of the odometry information.

[0100] To solve the above problems, the application realizes the matching of the point cloud data of the stripping machine by a generalized registration method, and solves the problem of mismatching caused by the sparsity of the radar line bundle with a large field of view angle by a covariance weighted point-to-point registration mechanism. The generalized registration method effectively solves the influence of point cloud sparsity by means of the corresponding relationship of adjacent points and by a covariance weighted point-to-point registration method. When the generalized registration method is performed, adjacent points need to be found, and then covariance data is established.

[0101] In an embodiment, the searching of adjacent points is performed by using the hash storage method described above. When the nearest point search is performed, the hash keys near the query point are searched, instead of blindly traversing the entire hash table. In this way, the search range is greatly reduced, and large-scale invalid search operations are avoided.

[0102] Embodiment 2

[0103] Based on the above embodiment 1, the performance verification experiment of the adaptive voxel filtering method is performed, the deviation of the output point cloud quantity and the specified point cloud quantity is quantified, and the algorithm efficiency is evaluated by calculating the time consumption. A radar sensor is installed in the cabin to collect raw data, and a point cloud containing 20000 points is obtained. In order to evaluate the effectiveness of the filtering process, the number of point clouds after filtering is set to 2000, 4000, 6000, 8000 and 10000 points respectively, and the iteration termination error threshold is set to 0.05, 0.10 and 0.20. The termination error here is the percentage of the error value of the output point cloud and the specified point cloud. The time consumption and error rate of the algorithm are analyzed respectively. As shown in Figure 8 shown, the maximum calculation time consumption of the algorithm is less than 10 milliseconds, the processing time consumption is less than 5 milliseconds when the tolerance threshold is set to 10%, the average time consumption is reduced to 3 milliseconds when the tolerance threshold is increased to 20%, and the convergence characteristics of the algorithm in the preset error range are verified.

[0104] The embodiment also performs verification experiments of the performance of the angle prediction algorithm. A dynamic test scene is simulated by a self-rotation motion simulation of the ship cabin of the stripping machine. The stripping machine is deployed in the middle of the ship cabin to perform self-rotation motion, so as to evaluate the prediction performance of the rotation angle of the stripping machine. The difference between the true value and the predicted value when the stripping machine has a large heading angle deviation is calculated. In the experiment, the stripping machine is set to complete 360° rotation around the Z axis within 8 seconds. This rapid rotation is intended to test the angle prediction performance to generate a heading angle difference of more than 10° between consecutive scans, simulate the dynamic and uncertain conditions commonly encountered in cargo hold operations, and provide a test for the heading angle prediction algorithm. In each test, the sensor data of the stripping machine is directly superimposed into the map data system. In order to reduce data complexity and improve computational efficiency, voxel filtering with a unit size of 0.5 meters is applied in this experiment. The angle prediction experiment verifies the processing time and iteration number of the original algorithms ICP, GICP, FAST-GICP, and the ICP, GICP, and FAST-GICP algorithms deployed with the angle prediction method based on boundary constraints.

[0105] Figure 9 The influence of the angle prediction algorithm in the application on the iteration time of the evaluated registration algorithm is shown. The angle prediction mechanism significantly reduces the time consumption of iteration convergence. The iteration time of the three registration algorithms with angle prediction (ICP, GICP, and FAST-GICP) is reduced by more than 44%. The GICP algorithm with angle prediction reduces the computation time by 61.39% compared to the original GICP, highlighting that the angle prediction algorithm helps to improve the convergence speed. In addition, Figure 10 The prediction accuracy of the angle prediction algorithm is shown. As can be seen from the figure, the angle prediction algorithm can generate a heading angle estimate value that is very close to the true value within a delay of two milliseconds. When the heading angle of the stripping machine changes greatly, the angle prediction can provide a value closer to the initial value of registration.

[0106] The embodiment also performs an odometer accuracy experiment. In the verification experiment of the pose estimation accuracy of the stripping machine, two closed-loop path trajectories are designed, and the stripping machine is required to perform a closed-loop path motion around the middle of the ship cabin. Radar point cloud data and odometer true value data are collected synchronously, and the results are compared with those of the KISS-ICP algorithm. The pose estimation accuracy of the algorithm designed in this paper is evaluated through trajectory similarity analysis and error statistics. The experimental data shows that the displacement deviation of the algorithm in this paper in the X and Y axis directions is significantly lower than that of the KISS-ICP algorithm, and the maximum deviation is controlled within 0.12 meters. The quantitative analysis results show that the average translation error of the two experiments of the algorithm in this paper is less than 0.05 meters, which is better than the result of the KISS-ICP algorithm, and the maximum displacement deviation of the latter is 0.74 meters. Table 1 details the translation error statistics of the two experiments, further verifying that the designed method has good odometer accuracy:

[0107] Table 1 Match precision analysis evaluation (m)

[0108]

Claims

1. A point cloud localization method based on self-occlusion filtering and dynamic voxel optimization, characterized in that, The method comprises the following steps: S1, filtering the point cloud of the cleaning machine itself, and performing point cloud motion distortion compensation based on linear interpolation of timestamps; S2, optimizing the point cloud by an adaptive voxel filtering method, the adaptive voxel filtering method comprising hash-based point cloud storage and adaptive filter size selection; The hash-based point cloud storage comprises: multiplying the number of point clouds in the voxel grid by the number of voxel grids to accurately calculate the total size of the down-sampled point cloud, and realizing fast counting of the number of point clouds through hash key value mapping; The adaptive filter size selection comprises: calculating the filter size L1 at the initial time from the input point cloud data and the specified output point cloud number Np; calculating the number of point clouds N1 by using the hash-based point cloud storage; performing filter size iteration to calculate the filter size Lk and the number of point clouds Nk, and judging whether the error of Np and Nk is less than the error stop threshold Es, if yes, the final filter size is obtained, and k is the iteration number; S3, constructing an angle prediction method under boundary constraint conditions, and specifically obtaining a predicted value of the rotation angle by minimum circumscribed rectangle calculation and minimum boundary angle; S4, using a generalized registration method based on hash lookup to register the point cloud and generate pose data.

2. The point cloud localization method based on self-occlusion filtering and dynamic voxel optimization according to claim 1, characterized in that, The filtering of the point cloud of the cleaning machine itself comprises: According to the three-dimensional appearance of the cleaning machine, the spatial size range thereof relative to the set radar coordinate system is obtained, denoted as a first range; It is judged whether the scanned point cloud is within the first range, and the point cloud within the first range is removed to filter the point cloud of the cleaning machine body.

3. The point cloud localization method based on self-occlusion filtering and dynamic voxel optimization according to claim 1 or 2, characterized in that, The point cloud motion distortion compensation based on linear interpolation of timestamps comprises: The timestamps of each scanning point in the frame period are analyzed, and a normalized time proportion relationship thereof with the starting time of the frame is established; a linear displacement interpolation model is constructed according to an adjacent frame pose transformation matrix, the instantaneous pose parameters corresponding to each time proportion are solved, and finally all the point clouds are uniformly mapped to the world coordinate system by transformation.

4. The method of claim 1, wherein, If the error of Np and Nk is greater than or equal to the error stop threshold Es, the iteration of the filter size is returned.

5. The method of claim 1, wherein, The minimum circumscribed rectangle calculation comprises: projecting the original point cloud on a plane, constructing a minimum closed region based on a convex hull construction algorithm, and finally obtaining a minimum circumscribed rectangle through angle rotation optimization to realize extraction of boundary features.

6. The method of claim 5, wherein, The minimum boundary angle comprises: respectively extracting the azimuth angles of the circumscribed rectangle boundary vectors of the current point cloud and the target point cloud, establishing an angle difference optimization function, and minimizing the optimization function to obtain an angle prediction value.

7. The method of claim 1 or 6, wherein, The generalized registration method based on hash lookup comprises the following steps: Q1, finding out a point p in the input point cloud in the adjacent voxel block with respect to a point q of the target point cloud Q through adjacent point search; Q2, calculating the covariances of the point sets near p and q respectively; Q3, performing point cloud registration by using p, q and the covariances of p and q. The step Q3 comprises:

8. The method of claim 7, wherein, Constructing an objective function, the objective function minimizing the alignment error of the input point cloud and the target point cloud by optimizing rigid transformation parameters, the rigid transformation parameters comprising a rotation matrix R and a translation vector t, and the optimal transformation parameters are obtained through iteration. The step Q1 comprises:

9. The method of claim 7, wherein, ​ The current corrected input point cloud is P, and the target point cloud in the world coordinate system is Q. For each point p in P, the coordinates of p in the voxel space are calculated. A plurality of voxel blocks centered on the coordinates are traversed, and a point q of Q is searched in each adjacent voxel block. The distance between p and q is calculated. The point q with the minimum distance is selected as the candidate nearest corresponding point of p. If the distance is less than a distance threshold, p and q are taken as the corresponding point pair.

Images