A Point Cloud Registration Method and Application for the Positioning of Engineering Equipment at Bulk Cargo Terminals

Abstract

The present invention relates to a point cloud registration method and application for the positioning of bulk cargo terminal engineering equipment. Based on lidar, laser point clouds are obtained, and the laser point clouds are screened multiple times to obtain the initial pose. Based on the initial pose, the point cloud information for point cloud registration is screened to complete the ICP registration from point cloud to plane, and it is applied to the positioning of bulk cargo terminal engineering equipment in a high-dynamic closed environment. The present invention uses a variety of point cloud denoising methods to ensure the usability of the point cloud and remove random noise points; based on an adaptive point cloud downsampling method, the downsampling parameters are quickly determined to improve the stability of the algorithm; a dynamic target recognition method more suitable for high-dynamic closed environments such as coal bunkers is designed, and the voxel occupancy comparison method is used to reduce the impact of dynamic coal piles and incorrect associations on point cloud registration, greatly improving the accuracy and stability of the algorithm; aiming at the deficiency of the nearest neighbor strategy of the ICP algorithm, a method for removing incorrect associations based on normal vectors is designed to improve the accuracy of the algorithm.

Description

Technical Field

[0001] The present invention relates to the technical field of general image data processing or generation, and particularly relates to a point cloud registration method and application for positioning engineering equipment at a bulk cargo terminal in an unmanned state. Background Art

[0002] A bulk cargo terminal refers to a special terminal for loading and unloading bulk cargo such as coal, ore, sand and gravel, or grain. The tank cleaning operation is a necessary working procedure on the cargo ship at the bulk cargo terminal. In addition to cleaning the inevitable cargo residues in the cargo hold and pipeline system after the ship unloads the cargo, it can effectively ensure the safety of transportation, the accuracy of transportation data, and prevent cross-contamination between materials. Taking the bulk cargo terminal in a large thermal power plant as an example, the tank cleaning operation of its cargo ship usually relies on manual labor, but manual operation has problems such as low work efficiency and poor visibility. Moreover, the coal bunker environment has problems such as poor air fluidity and high dust concentration. Working in such an environment for a long time will harm the physical and mental health of personnel.

[0003] With the development of society and the improvement of technology, due to considerations such as personnel safety and work efficiency, there is a need for the technological development of engineering equipment at bulk cargo terminals for tank cleaning operations. As a part of the positioning process of engineering equipment at bulk cargo terminals, the importance of the point cloud registration method is self-evident.

[0004] Chinese Patent with publication number CN114581619A discloses a coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping. It uses the neighborhood curvature and the angle between the horizontal line and the vertical line to distinguish the laser point clouds of the bulkhead and the coal pile, and then uses the laser point cloud of the bulkhead for point cloud registration; but in fact, not all the laser point clouds of the coal pile are useless for point cloud registration. The part of the coal pile that has not moved actually makes a positive contribution to point cloud registration. That is, in fact, the calculation amount is increased here but the registration efficiency is reduced.

[0005] The technical solutions disclosed in Chinese Patent "A Method for Automatically Analyzing 3D Point Cloud Registration Error Based on Stereo Grid" with publication number CN107038717A, "A Global Optimization and Overall Registration Method for Multi-view Three-dimensional Laser Point Cloud" with publication number CN104463894A, etc. do not handle dynamic objects and are not applicable to the high-dynamic closed environment of the coal bunker.

[0006] Although the Chinese patent "A Laser SLAM Localization Method and Device Integrating Object Detection and Tracking" with the publication number CN116299500A eliminates the negative impact of dynamic objects on point cloud registration through object detection and tracking methods, the object detection method based on lidar is difficult to be used for objects such as coal piles. In terms of point cloud voxel downsampling, Chinese patents such as "A Laser Point Cloud Transmission Line Lead Fitting Method for Live Working Robots in Distribution Networks" with the publication number CN113468706A need multiple iterations to obtain suitable voxel parameters, and some existing technologies cannot provide appropriate initial values; there are also some patents that adopt fixed voxel downsampling methods, and this way of using fixed parameters cannot adapt to the changes in different environments, so it cannot meet the requirements for different coal bunkers. Summary of the Invention

[0007] To solve the above technical problems, the present invention provides a point cloud registration method and application for the positioning of bulk cargo terminal engineering equipment.

[0008] The technical solution adopted by the present invention is a point cloud registration method for the positioning of bulk cargo terminal engineering equipment, which obtains laser point cloud based on lidar, performs multiple rounds of screening on the laser point cloud to obtain an initial pose; filters the point cloud information for point cloud registration based on the initial pose, and completes the ICP registration from point cloud to surface.

[0009] Preferably, obtaining the initial pose includes the following steps:

[0010] S11 Filter the laser point cloud within the area where the lidar accuracy does not decline;

[0011] S12 Filter the laser point cloud with an intensity outside the threshold range;

[0012] S13 Filter random noise points;

[0013] S14 Filter the laser point cloud with the included angle between the normal vector and the lidar emission direction greater than a preset value;

[0014] S15 Convert the Cartesian coordinates of the filtered laser point cloud into a spherical coordinate system, use the horizontal and vertical resolutions of the lidar as the length and width of the grid, and divide the laser point cloud into spherical grids according to the elevation angle and azimuth angle in the spherical coordinate system; dividing the spherical grid here is used to improve the calculation efficiency, reduce the recognition difficulty, and obtain a better solution when dealing with edge problems;

[0015] S16 Calculate the first adaptive voxel downsampling parameter , and adjust the adaptive voxel downsampling parameter according to the number of points in the previous frame of laser point cloud until the set target point number is reached;

[0016] S17 Integrate the IMU data between any two adjacent frames of lidar point clouds to obtain velocity estimates , initial pose estimates and .

[0017] Preferably, in S13, let and represent the distance values of the k-th and (k - 1)-th lidar points from the emission point in the same scan beam, and let represent the preset distance difference parameter between two consecutive points. If , then filter the k-th lidar point; subsequently, use the (k + 1)-th lidar point before filtering as the new k-th lidar point.

[0018] Preferably, in S16,

[0019]

[0020]

[0021] wherein, and are the horizontal resolution and vertical resolution of the lidar, is the distance value of the first lidar point added into each spherical grid, represents the expected number of lidar point clouds, represents the adaptive voxel downsampling parameter for the i-th calculation, is the adjustment parameter, represents the number of lidar point clouds after the (i - 1)-th downsampling.

[0022] Preferably, based on the initial pose, transform the current lidar point cloud into the map point cloud coordinate system, and then divide the current lidar point cloud into voxels with the same voxel size as that of the map point cloud; the map point cloud refers to the processed standard three-dimensional point cloud data in the environment.

[0023] Preferably, compare the occupancy of the lidar point cloud voxels and the map point cloud voxels. For the same voxel position:

[0024] If there is a lidar point in the lidar point cloud at this position, while there is no lidar point in the map point cloud at this position and its neighborhood, then the lidar point cloud at this position is not added to the point cloud ;

[0025] If there is no lidar point in the lidar point cloud at this position, while there is a lidar point in the map point cloud at this position and its If there is a laser point in the neighborhood, it is considered that the point cloud at that position has moved, and the point cloud in the voxel of the map point cloud is deleted;

[0026] If the laser point cloud and the map point cloud at that location or If there are laser points in the neighborhood, the laser point cloud at that location is added to , add the map point cloud at this location to .

[0027] Preferably, the ICP registration of point cloud to surface includes the following steps:

[0028] S31 will get The points in the grid are (re)divided into a spherical grid ,right Building a KD tree ;

[0029] S32 for For each point in , calculate its corresponding normal vector; At each point in Select the M nearest neighbors of each point within the range of R and calculate the normal vector of each point;

[0030] S33 For each The point in Select the nearest neighbor point from The points and When the normal vector difference of the nearest neighbor point selected in is greater than the preset value, the current point pair will not be added as the correct associated point pair to the subsequent point cloud registration;

[0031] S34 Calculated using the point-to-surface ICP point cloud registration method and The conversion relationship between them is used to convert the current laser point cloud to the map point cloud coordinate system and add it to the map point cloud.

[0032] Preferably, in S32, if The corresponding point of If there are more than 3 points in the spherical grid, the normal vector is calculated using the least squares method using the points in the grid. Otherwise, the range is expanded to the four neighborhoods, and the normal vector is calculated using the four neighborhoods and the corresponding points in the grid to increase the accuracy of the calculation.

[0033] Preferably, in S32, the least squares method is used to calculate The normal vectors of each point in and its M nearest neighbors; if there are less than M nearest neighbors within the preset range, the current point will not be added to the subsequent point cloud registration.

[0034] Application of a point cloud registration method for positioning engineering equipment at a bulk cargo terminal, which is applied to the positioning of engineering equipment at a bulk cargo terminal in a high-dynamic closed environment.

[0035] The present invention relates to a point cloud registration method and application for positioning engineering equipment at a bulk cargo terminal. Based on a lidar to obtain a laser point cloud, the laser point cloud is screened in multiple rounds to obtain an initial pose; based on the initial pose, the point cloud information for point cloud registration is screened to complete the ICP registration from point cloud to surface; it is applied to the positioning of engineering equipment at a bulk cargo terminal in a high-dynamic closed environment.

[0036] The beneficial effects of the present invention are as follows:

[0037] (1) Using various point cloud denoising methods to ensure the usability of the point cloud, and removing random noise points by a filtering method based on the change of continuous point distances;

[0038] (2) Based on an adaptive point cloud downsampling method, the downsampling parameters can be quickly determined in one calculation, solving the problem that it is difficult to set the downsampling parameters for different coal bunker environments, and improving the stability of the algorithm;

[0039] (3) Designing a dynamic target recognition method more suitable for high-dynamic closed environments such as coal bunkers, and using the method of voxel occupancy comparison to reduce the influence of dynamic coal piles and incorrect associations on point cloud registration, greatly improving the accuracy and stability of the algorithm;

[0040] (4) Utilizing the deficiency of the nearest neighbor strategy of the ICP algorithm, designing a method for removing incorrect associations based on normal vectors to improve the accuracy of the algorithm. Description of the Drawings

[0041] Figure 1 is the flowchart of the method of the present invention;

[0042] Figure 2 is the illustration of the present invention;

[0043] Figure 3 is the flowchart for obtaining the initial pose of the present invention;

[0044] Figure 4 is the flowchart of the ICP registration from point cloud to surface of the present invention. Detailed Embodiments

[0045] The following further describes the present invention in detail with reference to embodiments, but the protection scope of the present invention is not limited thereto.

[0046] As Figure 1As shown in the figure, the present invention relates to a point cloud registration method for the positioning of bulk terminal engineering equipment. Based on lidar, lidar point clouds are obtained, and the lidar point clouds are screened in multiple rounds to obtain an initial pose. Based on the initial pose, the point cloud information for point cloud registration is screened to complete the ICP registration from point cloud to surface.

[0047] In the present invention, it mainly includes several parts: data preprocessing, screening the point cloud information for point cloud registration, and ICP registration based on point cloud to surface. Taking Figure 2 as a schematic diagram, the following will explain for different parts.

[0048] (1) Data preprocessing

[0049] The purpose of data preprocessing here is to remove various lidar point cloud noises, ensure the usability of lidar point clouds, and design an adaptive voxel downsampling method to maintain the time-consuming of the algorithm stability and improve the adaptability to different environments. After the above preprocessing, the initial pose can be obtained using IMU.

[0050] As Figure 3 shown, specifically, it includes the following steps:

[0051] (1-1) Filter the lidar point clouds within the area where the lidar accuracy does not decline; the lidar points that satisfy Equation (1) will be excluded.

[0052] (1)

[0053] Among them, is the distance value of the lidar point cloud, is the minimum distance value of the retained point cloud, is the maximum distance value of the retained point cloud, represents "or"; in this embodiment, takes 1m, takes 100m.

[0054] (1-2) Filter the lidar point clouds with intensities outside the threshold range;

[0055] (1-3) Filter random noise points;

[0056] Taking and to represent the distance values of the kth and (k-1)th lidar points from the emission point in the same scan beam, and taking to represent the preset distance difference parameter between two consecutive points. If Equation (2) is satisfied, then the kth lidar point is filtered.

[0057] (2)

[0058] In this embodiment, is 5m.

[0059] (1-4) Filter the lidar point cloud where the included angle between the filtered normal vector and the lidar emission direction is greater than a preset value;

[0060] In actual processing, remove the points where the normal vector is almost perpendicular to the ray direction, satisfying Equation (3),

[0061] (3)

[0062] where, represents the angle threshold between the lidar beam and the plane, which is taken as 5° in this embodiment, refer to Figure 2 , The expression of

[0063] (4)

[0064] where, and represent the kth and (k-1)th points on the same scan beam, represents the distance value between the (k-1)th lidar point and the emission point in the same scan beam.

[0065] (1-5) Convert the Cartesian coordinates of the filtered lidar point cloud to spherical coordinates using Equation (5),

[0066] (5)

[0067] where, represents the radial distance, represents the elevation angle, represents the azimuth angle, and (x, y, z) represents the coordinates of the lidar point cloud;

[0068] Take the horizontal and vertical resolutions of the lidar as the length and width of the grid, and divide the lidar point cloud into spherical grids according to the elevation angle and azimuth angle in the spherical coordinate system;

[0069] (1-6) Calculate the first adaptive voxel downsampling parameter , and adjust the adaptive voxel downsampling parameter according to the number of points in the previous frame of lidar point cloud;

[0070] (6)

[0071] (7)

[0072] where, and are the horizontal resolution and vertical resolution of the lidar, which are taken as 0.02° and 2° respectively in this embodiment, The distance value of the first added laser point within each spherical grid, represents the expected number of laser point cloud points, which is taken as 1000 here, represents the adaptive voxel downsampling parameter for the i-th calculation, is the adjustment parameter, which is taken as 0.001 here, represents the number of laser point cloud points after the (i - 1)-th downsampling.

[0073] (1 - 7) Integrate the IMU data between any two frames of laser point clouds to obtain the velocity estimation and the initial pose estimation and , satisfying Equation (8),

[0074] (8)

[0075] where, and represent the acceleration and angular velocity measurement values at time t, represents the gravitational acceleration, represents the rotation matrix from the IMU coordinate system to the world coordinate system, and represent the biases of the accelerometer and gyroscope, and represent the measurement noises of the accelerometer and gyroscope; generally, it is considered that the derivatives of the biases and the measurement noises both follow Gaussian distributions.

[0076] (2) Screen the point cloud information for point cloud registration

[0077] The screening here refers to removing the dynamic parts that are unfavorable for point cloud registration through voxel occupancy comparison and removing the changed parts in the point cloud map, specifically including:

[0078] (2 - 1) Based on the initial pose, transform the current laser point cloud into the map point cloud coordinate system, and then divide the current laser point cloud into voxels with the same voxel size as that of the map point cloud;

[0079] In this embodiment, the voxel size is taken as 0.1 m.

[0080] (2 - 2) Compare the occupancy of the laser point cloud voxels and the map point cloud voxels. For the same voxel positions:

[0081] If there is a laser point in the laser point cloud at this position, while there is no laser point in the map point cloud at this position and its neighborhood, then the laser point cloud within this position is not added to the point cloud In; This problem may occur because the map point cloud has not covered this area, or the data of the map point cloud is sparse and does not include this area;

[0082] If there is no laser point at this position in the laser point cloud, and there are laser points in the map point cloud at this position and its neighborhood, it is considered that the point cloud at this position has moved, and the point cloud in the map point cloud within this voxel is deleted; This problem indicates that the laser point cloud sensor cannot fully cover some areas, and there may be blind spots, etc.;

[0083] If there are laser points in both the laser point cloud and the map point cloud at this position or its neighborhood, that is, the position scanned by the laser point cloud coincides with the map point cloud, and the laser point cloud data at this position is consistent with the point cloud data in the map point cloud, then the laser point cloud at this position is added to and the map point cloud at this position is added to .

[0084] In this embodiment, Take 18.

[0085] (3) ICP registration based on point cloud to surface

[0086] Use the method of spherical grid division to quickly select the neighboring points for normal vector estimation, and remove the incorrect point-plane associations through the point-plane association strategy based on the normal vector.

[0087] As Figure 4 shown, specifically, it includes the following steps:

[0088] (3-1) Divide the points in the obtained into spherical grids , and build a KD tree for ;

[0089] (3-2) For each point in , calculate its corresponding normal vector;

[0090] If there are more than 3 points in the spherical grid corresponding to any point in , then use the points in the grid to calculate the normal vector by the least squares method, otherwise expand the range to the four-neighborhood, and calculate the normal vector through the points in the four-neighborhood and the corresponding grid;

[0091] For each point in , select M nearest neighbors within the range of R for each point in , and calculate the normal vector of each point;

[0092] Calculate by the least squares method the normal vectors of each point in

[0093] and its M nearest neighbors; it should be noted that if there are less than M nearest neighbors within the range R, the current point will not be added to the subsequent point cloud registration.

[0094] (3-3) For each point in select the nearest neighbor point in when the difference between the normal vectors of the point in and the selected nearest neighbor point in

[0095] (3-4) Calculate the transformation relationship between and using the point-to-plane ICP point cloud registration method, and transform the current laser point cloud into the map point cloud coordinate system and add it to the map point cloud.

[0096] The present invention also relates to an application of the point cloud registration method for the positioning of the bulk cargo terminal engineering equipment described above, which is applied to the positioning of the bulk cargo terminal engineering equipment in a high-dynamic closed environment.

[0097] The present invention also relates to a computer-readable storage medium, on which a point cloud registration program for the positioning of the bulk cargo terminal engineering equipment is stored, and when the program is executed by a processor, the point cloud registration method for the positioning of the bulk cargo terminal engineering equipment is implemented.

[0098] The present invention also relates to a computer device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, the point cloud registration method for the positioning of the bulk cargo terminal engineering equipment is implemented.

[0099] Those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0100] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present invention. It should be understood that each flow and / or block in the flowchart and / or block diagram, and the combinations of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices produce a means for implementing the functions specified in one flow Figure 1 or more flows and / or blocks Figure 1 or more blocks.

[0101] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory produce a manufactured article including an instruction means that implements the functions specified in one flow Figure 1 or more flows and / or blocks Figure 1 or more blocks.

[0102] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operational steps are executed on the computer or other programmable device to generate a computer-implemented process, so that the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one flow Figure 1 or more flows and / or blocks Figure 1 or more blocks.

[0103] Although the preferred embodiments of the present invention have been described, those skilled in the art can make additional changes and modifications once they learn the basic creative concepts. Therefore, the appended claims are intended to be construed to include the preferred embodiments as well as all changes and modifications falling within the scope of the present invention.

[0104] Obviously, those skilled in the art can make various changes and variations to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

Claims

1. A point cloud registration method for the positioning of bulk cargo terminal engineering equipment, characterized in that: Applied to high-dynamic closed environments; based on lidar to obtain lidar point clouds, and perform multiple rounds of screening on the lidar point clouds to obtain the initial pose, including the following steps: S11 Filter the lidar point clouds within the area where the lidar accuracy does not decline; S12 Filter the lidar point clouds with intensities outside the threshold range; S13 Filter random noise points; S14 Filter the lidar point clouds with the angle between the normal vector and the lidar emission direction greater than the preset value; S15 Convert the Cartesian coordinates of the filtered lidar point clouds into the spherical coordinate system, use the horizontal and vertical resolutions of the lidar as the length and width of the grid, and divide the lidar point clouds into spherical grids according to the elevation angle and azimuth angle in the spherical coordinate system; S16 Calculate the first adaptive voxel downsampling parameter , adjust the adaptive voxel downsampling parameter according to the number of points in the previous frame of lidar point cloud until the set target number of points is reached; S17 Integrate the IMU data between any two adjacent frames of lidar point clouds to obtain velocity estimation , initial pose estimation and ; Based on the initial pose, screen the point cloud information for point cloud registration, remove the dynamic parts that are unfavorable for point cloud registration by means of voxel occupancy comparison, and remove the changed parts in the point cloud map; Quickly select the neighboring points for normal vector estimation by means of spherical grid division, and remove the incorrect point-plane associations through the point-plane association strategy based on the normal vector to complete the point cloud to plane ICP registration.

2. A point cloud registration method for the positioning of bulk terminal engineering equipment according to claim 1, characterized in that: In S13, use and to represent the distance values of the k-th and (k - 1)-th laser points from the emission point in the same scanning beam. Use to represent the preset distance difference parameter between two consecutive points. If , then filter the k-th laser point.

3. A point cloud registration method for the positioning of bulk cargo terminal engineering equipment according to claim 1, characterized in that: In S16, ， ， wherein, and are the horizontal resolution and vertical resolution of the lidar, is the distance value of the first added laser point in each spherical grid, represents the expected number of laser points in the point cloud, represents the adaptive voxel downsampling parameter calculated in the i-th time, is the adjustment parameter, represents the number of laser points in the point cloud after the (i - 1)-th downsampling.

4. A point cloud registration method for the positioning of bulk cargo terminal engineering equipment according to claim 1, characterized in that: Based on the initial pose, transform the current laser point cloud into the map point cloud coordinate system, and then divide the current laser point cloud into voxels with the same voxel size as that of the map point cloud.

5. A point cloud registration method for the positioning of bulk cargo terminal engineering equipment according to claim 4, characterized in that: Compare the occupancy of the lidar point cloud voxels and the map point cloud voxels. For the same voxel position: If there is a laser point cloud at this position, and there is no laser point in the map point cloud at this position and its neighborhood, then the laser point cloud at this position is not added to the point cloud collection; If the laser point cloud has no laser points at this position, while the map point cloud has laser points at this position and its neighborhood, it is considered that the point cloud at this position has moved, and the point cloud of the map point cloud within this voxel is deleted; If there are laser point clouds and map point clouds at this position or in its neighborhood, then add the laser point cloud at this position to and add the map point cloud at this position to .

6. A point cloud registration method for the positioning of bulk terminal engineering equipment according to claim 5, characterized in that: The point cloud to plane ICP registration includes the following steps: S31 Divide the points obtained in into the spherical grid , and build a KD tree ; S32 For each point in , calculate its corresponding normal vector; for each point in , select M nearest neighbors of each point within the range of R in , and calculate the normal vector of each point; S33 For each point in select the nearest neighbor point in ; when the difference between the normal vectors of the points in and the selected nearest neighbor points is greater than the preset value, the current point pair is not added as a correct associated point pair to the subsequent point cloud registration; S34 Calculated using the point-to-surface ICP point cloud registration method and The conversion relationship between them is used to convert the current laser point cloud to the map point cloud coordinate system and add it to the map point cloud.

7. A point cloud registration method for the positioning of bulk cargo terminal engineering equipment according to claim 6, characterized in that: In S32, if there are more than 3 points in the spherical grid corresponding to any point in , then use the points in the grid to calculate the normal vector by the least squares method; otherwise, expand the range to the four-neighborhood, and calculate the normal vector through the points in the four-neighborhood and the corresponding grid.

8. A point cloud registration method for the positioning of bulk terminal engineering equipment according to claim 6, characterized in that: In S32, the normal vectors of each point in and its M nearest neighbors are calculated by the least squares method; if there are less than M nearest neighbors within the preset range, the current point is not added to the subsequent point cloud registration.

9. A point cloud registration method for the positioning of bulk terminal engineering equipment according to any one of claims 1 to 8, characterized in that: Applied to the positioning of bulk cargo terminal engineering equipment in high-dynamic closed environments.

Images