Point cloud registration method for positioning bulk cargo terminal engineering equipment and application

Abstract

The invention relates to a point cloud registration method for bulk cargo wharf engineering equipment positioning and application, and the method comprises the steps: obtaining laser point clouds based on a laser radar, carrying out the multi-round screening of the laser point clouds, and obtaining an initial pose; point cloud information used for point cloud registration is screened based on the initial pose, and ICP registration from point cloud to surface is completed; the method is applied to positioning of bulk cargo wharf engineering equipment in a high-dynamic closed environment. According to the method, the availability of point clouds is ensured by using multiple point cloud denoising modes, and random noise points are removed; on the basis of a self-adaptive point cloud downsampling method, downsampling parameters are quickly determined, and the algorithm stability is improved; a dynamic target identification method which is more suitable for high-dynamic closed environments such as a coal cabin is designed, the influence of dynamic coal piles and wrong association on point cloud registration is reduced in a voxel occupation comparison mode, and the precision and stability of an algorithm are greatly improved; the error correlation removal method based on the normal vector is designed by aiming at the defects of the nearest neighbor strategy of the ICP algorithm, so that the accuracy of the algorithm is improved.

Description

Technical Field

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

[0002] Bulk cargo terminal refers to a special terminal used for loading and unloading bulk cargo such as coal, ore, sand and gravel or grain. Tank cleaning is a necessary work procedure on cargo ships at bulk cargo terminals. It can effectively ensure the safety of transportation, the accuracy of transportation data, and the prevention of mutual contamination between materials by cleaning and removing the inevitable cargo residues in the cargo hold and pipe system after the ship has unloaded the cargo. Taking the bulk cargo terminal in a large thermal power plant as an example, the tank cleaning operation of its cargo ship is usually carried out manually, but manual operation has problems such as low work efficiency and poor vision. In addition, the coal bunker environment has problems such as poor air flow and high dust concentration. Working in such an environment for a long time will cause harm to the physical and mental health of personnel.

[0003] With the development of society and the improvement of technology, there is a demand for the technical development of bulk cargo terminal engineering equipment for cargo hold clearing operations due to considerations such as personnel safety and work efficiency. The point cloud registration method is a part of the positioning process of bulk cargo terminal engineering equipment, and its importance is self-evident.

[0004] Chinese patent publication number CN114581619A discloses a coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping. The method uses 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. However, in fact, not all laser point clouds of the coal pile are useless for point cloud registration. In fact, the part of the coal pile that has not moved makes a positive contribution to the point cloud registration. That is, the amount of calculation is actually increased here but the registration efficiency is reduced.

[0005] The disclosed technical solutions such as the Chinese patent with publication number CN107038717A "A method for automatically analyzing 3D point cloud registration errors based on stereo grid" and the Chinese patent with publication number CN104463894A "A multi-view three-dimensional laser point cloud global optimization overall registration method" do not process dynamic objects and are not suitable for the highly dynamic closed environment of the coal bunker.

[0006] Although the Chinese patent "A laser SLAM positioning method and device integrating target detection and tracking" with publication number CN116299500A removes the negative impact of dynamic objects on point cloud registration through target detection methods and tracking methods, the target detection method based on laser radar is difficult to use for objects such as coal piles. In terms of point cloud voxel downsampling, the Chinese patent "Laser point cloud transmission line lead fitting method for distribution network live working robot" with publication number CN113468706A requires multiple iterations to obtain suitable voxel parameters, and some existing technologies cannot provide suitable initial values; some other patents use fixed voxel downsampling methods, which cannot adapt to changes in different environments and therefore cannot meet the needs of different coal bunkers. Summary of the invention

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

[0008] The technical solution adopted by the present invention is a point cloud registration method for positioning engineering equipment of a bulk terminal. The method obtains laser point cloud based on a laser radar, performs multiple rounds of screening on the laser point cloud, and obtains an initial pose; based on the initial pose, the point cloud information used for point cloud registration is screened to complete the ICP registration of point cloud to surface.

[0009] Preferably, obtaining the initial posture comprises the following steps:

[0010] S11 filters the laser point cloud in the area where the non-lidar accuracy decreases;

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

[0012] S13 filters random noise points;

[0013] S14 filters the laser point cloud whose angle between the normal vector and the laser radar emission direction is greater than a preset value;

[0014] S15 converts the Cartesian coordinates of the filtered laser point cloud into a spherical coordinate system, uses the horizontal and vertical resolutions of the laser radar as the length and width of the grid, and divides the laser point cloud into spherical grids according to the elevation and azimuth angles in the spherical coordinate system; the spherical grid division here is used to increase the calculation efficiency, reduce the recognition difficulty, and obtain a better solution when edge problems are involved;

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

[0016] S17 Integrate the IMU data between any two adjacent frames of laser point cloud to obtain velocity estimation , initial pose estimation and .

[0017] Preferably, in S13, and Indicates the distance between the kth and k-1th laser points and the emission point in the same scanning line beam, Represents the preset distance difference parameter between two consecutive points. If , the kth laser point is filtered; subsequently, the k+1th laser point before filtering is used as the new kth laser point.

[0018] Preferably, in S16,

[0019]

[0020]

[0021] in, and are the horizontal and vertical resolutions of the lidar, is the distance value of the first added laser point in each spherical grid, represents the expected number of laser point cloud points, represents the adaptive voxel downsampling parameters calculated for the i-th time, To adjust the parameters, Indicates the number of laser point cloud points after the i-1th downsampling.

[0022] Preferably, based on the initial pose, the current laser point cloud Convert to the map point cloud coordinate system, and then convert the current laser point cloud The map point cloud is divided into voxels of the same size as the voxels of the map point cloud; the map point cloud refers to the processed standard three-dimensional point cloud data in the environment.

[0023] Preferably, the occupancy of the laser point cloud voxels and the map point cloud voxels is compared, and for the same voxel position:

[0024] If the laser point cloud There is a laser point at this location, and the map point cloud is at this location and its If there is no laser point in the neighborhood, the laser point cloud at that location will not be added to the point cloud. middle;

[0025] If the laser point cloud There is no laser point at this location, but the map point cloud is at this location 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] An application of the point cloud registration method for bulk cargo terminal engineering equipment positioning is applied to bulk cargo terminal engineering equipment positioning 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. The method comprises the following steps: obtaining laser point cloud based on a laser radar, performing multiple rounds of screening on the laser point cloud, and obtaining an initial pose; screening point cloud information for point cloud registration based on the initial pose, and completing ICP registration of point cloud to surface; and applying the method to positioning engineering equipment at a bulk cargo terminal in a highly dynamic closed environment.

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

[0037] (1) Using a variety of point cloud denoising methods to ensure the availability of point clouds, and removing random noise points by filtering based on the change in distance between consecutive points;

[0038] (2) Based on the adaptive point cloud downsampling method, the downsampling parameters can be quickly determined in one calculation, which solves the problem that the downsampling parameters are difficult to adjust in different coal bunker environments and improves the stability of the algorithm;

[0039] (3) Design a dynamic target recognition method that is more suitable for highly dynamic closed environments such as coal bunkers, and use voxel occupancy comparison to reduce the impact of dynamic coal piles and erroneous associations on point cloud registration, greatly improving the accuracy and stability of the algorithm;

[0040] (4) Taking advantage of the shortcomings of the nearest neighbor strategy of the ICP algorithm, a normal vector-based error association removal method is designed to improve the accuracy of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 is a flow chart of the method of the present invention;

[0042] Figure 2 is a diagram of the present invention;

[0043] Figure 3 A flow chart for obtaining an initial posture for the present invention;

[0044] Figure 4 This is a flowchart of the ICP registration of point cloud to surface according to the present invention. DETAILED DESCRIPTION

[0045] The present invention is further described in detail below in conjunction with embodiments, but the protection scope of the present invention is not limited thereto.

[0046] like Figure 1As shown, the present invention relates to a point cloud registration method for positioning engineering equipment of a bulk cargo terminal, which obtains laser point cloud based on a laser radar, performs multiple rounds of screening on the laser point cloud, and obtains an initial pose; based on the initial pose, the point cloud information used for point cloud registration is screened to complete the ICP registration of point cloud to surface.

[0047] The present invention mainly includes data preprocessing, screening point cloud information for point cloud registration, and ICP registration based on point cloud to surface. Figure 2 This is a schematic diagram, and the different parts are explained below.

[0048] (1) Data preprocessing

[0049] The purpose of data preprocessing here is to remove various laser point cloud noises, ensure the availability of laser point clouds, and design an adaptive voxel downsampling method to maintain the algorithm's stability and improve its adaptability to different environments. Based on the above preprocessing, the IMU can be used to obtain the initial pose.

[0050] like Figure 3 As shown, specifically, the following steps are included:

[0051] (1-1) Filter the laser point cloud in the area where the non-lidar accuracy decreases; the laser points that satisfy equation (1) will be eliminated.

[0052] (1)

[0053] in, is the distance value of the lidar point cloud, is the minimum distance value to retain the point cloud, is the maximum distance value to retain the point cloud, In this embodiment, Take 1m, Take 100m.

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

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

[0056] by and Indicates the distance between the kth and k-1th laser points and the emission point in the same scanning line beam, represents the preset distance difference parameter between two consecutive points. If it satisfies formula (2), the kth laser point is filtered.

[0057] (2)

[0058] In this embodiment, is 5m.

[0059] (1-4) Filter the laser point cloud whose angle between the normal vector and the laser radar emission direction is greater than a preset value;

[0060] In actual processing, we remove the points whose normal vector is almost perpendicular to the ray direction, satisfying equation (3):

[0061] (3)

[0062] in, represents the angle threshold between the laser beam and the plane, which is 5° in this embodiment. Figure 2 , The expression of satisfies formula (4),

[0063] (4)

[0064] in, and represents the kth and k-1th points on the same scan line beam, Indicates the distance between the k-1th laser point and the emission point in the same scanning line beam.

[0065] (1-5) The Cartesian coordinates of the filtered laser point cloud are converted into a spherical coordinate system using formula (5):

[0066] (5)

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

[0068] The horizontal and vertical resolutions of the laser radar are used as the length and width of the grid, and the laser point cloud is divided 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 parameters , adjust the adaptive voxel downsampling parameters according to the number of points in the previous frame of laser point cloud ;

[0070] (6)

[0071] (7)

[0072] in, and are the horizontal resolution and vertical resolution of the laser radar, which are 0.02° and 2° respectively in this embodiment. is the distance value of the first added laser point in each spherical grid, Indicates the expected number of laser point cloud points, here it is 1000, represents the adaptive voxel downsampling parameters calculated for the i-th time, is the adjustment parameter, which is taken as 0.001. Indicates the number of laser point cloud points after the i-1th downsampling.

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

[0074] (8)

[0075] in, and represents the acceleration and angular velocity measurements at time t, represents the acceleration due to gravity, Represents the rotation matrix from the IMU coordinate system to the world coordinate system, and represents the zero bias of the accelerometer and gyroscope, and represents the measurement noise of the accelerometer and gyroscope; in general, the derivative of the zero bias and the measurement noise are both assumed to follow a Gaussian distribution.

[0076] (2) Filtering point cloud information for point cloud registration

[0077] The screening here refers to removing the dynamic parts that are not conducive to point cloud registration by voxel occupancy comparison, and removing the changed parts in the point cloud map, including:

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

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

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

[0081] If the laser point cloud There is a laser point at this location, and the map point cloud is at this location and its If there is no laser point in the neighborhood, the laser point cloud at that location will not be added to the point cloud. This problem may occur because the map point cloud has not yet covered the area, or the map point cloud data is sparse and has not yet included the area;

[0082] If the laser point cloud There is no laser point at this location, but the map point cloud is at this location and its If there is a laser point in the neighborhood, it is considered that the point cloud at that location has moved, and the point cloud in that voxel of the map point cloud is deleted; this problem means that the laser point cloud sensor cannot fully cover certain areas, and there may be blind spots, etc.

[0083] If the laser point cloud and the map point cloud at that location or There are laser points in the neighborhood, that is, the location scanned by the laser point cloud coincides with the map point cloud. If the laser point cloud data at this location matches the point cloud data in the map point cloud, then the laser point cloud at this location is added to the , add the map point cloud at this location to .

[0084] In this embodiment, Take 18.

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

[0086] A spherical grid-based method is used to quickly select neighboring points for normal vector estimation, and a point-to-surface association strategy based on normal vectors is used to remove erroneous point-to-surface associations.

[0087] like Figure 4 Specifically, the following steps are included:

[0088] (3-1) The obtained The points in the grid are divided into a spherical grid ,right Building a KD tree ;

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

[0090] like 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.

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

[0092] Calculated by least squares method The normal vectors of each point in and its M nearest neighbors; it should be noted that if there are less than M nearest neighbors in the range R, the current point will not be added to the subsequent point cloud registration.

[0093] In this embodiment, M is 5 and R is 0.2 m.

[0094] (3-3) 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;

[0095] (3-4) Calculation 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.

[0096] The present invention also relates to an application of the point cloud registration method for positioning bulk cargo terminal engineering equipment, which is applied to positioning 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 positioning engineering equipment for a bulk cargo terminal is stored. When the program is executed by a processor, the point cloud registration method for positioning engineering equipment for a bulk cargo terminal is implemented.

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

[0099] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may 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 flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes 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 a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0101] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0102] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0103] Although the preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0104] Obviously, those skilled in the art can make various changes and modifications 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 equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A point cloud registration method for positioning bulk cargo terminal engineering equipment, characterized by: The laser point cloud is obtained based on the laser radar, and the laser point cloud is screened multiple times to obtain the initial pose; the point cloud information used for point cloud registration is screened based on the initial pose to complete the ICP registration of the point cloud to the surface.

2. A point cloud registration method for positioning bulk cargo terminal engineering equipment according to claim 1, characterized in that: Obtaining the initial pose includes the following steps: S11 filters the laser point cloud in the area where the non-lidar accuracy decreases; S12 filters the laser point cloud with intensity outside the threshold range; S13 filters random noise points; S14 filters the laser point cloud whose angle between the normal vector and the laser radar emission direction is greater than a preset value; S15 converts the Cartesian coordinates of the filtered laser point cloud into a spherical coordinate system, uses the horizontal and vertical resolutions of the laser radar as the length and width of the grid, and divides the laser point cloud into spherical grids according to the elevation angle and azimuth angle in the spherical coordinate system; S16 Calculate the first adaptive voxel downsampling parameters , adjust the adaptive voxel downsampling parameters according to the number of points in the previous frame of laser point cloud until the set target number of points is reached; S17 Integrate the IMU data between any two adjacent frames of laser point cloud to obtain velocity estimation , initial pose estimation and .

3. A point cloud registration method for positioning bulk cargo terminal engineering equipment according to claim 2, characterized in that: In S13, and Indicates the distance between the kth and k-1th laser points and the emission point in the same scanning line beam, Represents the preset distance difference parameter between two consecutive points. If , then filter the kth laser point.

4. A point cloud registration method for positioning bulk cargo terminal engineering equipment according to claim 2, characterized in that: In S16, ， ， in, and are the horizontal and vertical resolutions of the lidar, is the distance value of the first added laser point in each spherical grid, represents the expected number of laser point cloud points, represents the adaptive voxel downsampling parameters calculated for the i-th time, To adjust the parameters, Indicates the number of laser point cloud points after the i-1th downsampling.

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

6. A point cloud registration method for positioning bulk cargo terminal engineering equipment according to claim 5, characterized in that: Compare the occupancy of laser point cloud voxels with that of map point cloud voxels. For the same voxel position: If the laser point cloud There is a laser point at this location, and the map point cloud is at this location and its If there is no laser point in the neighborhood, the laser point cloud at that location will not be added to the point cloud. middle; If the laser point cloud There is no laser point at this location, but the map point cloud is at this location 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; 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 .

7. A point cloud registration method for positioning bulk cargo terminal engineering equipment according to claim 6, characterized in that: The ICP registration of point cloud to surface includes the following steps: S31 will get The points in the grid are divided into a spherical grid ,right Building a KD tree ; 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; 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; 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 into the map point cloud coordinate system and add it to the map point cloud.

8. The point cloud registration method for positioning bulk cargo terminal engineering equipment according to claim 7, characterized in that: 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.

9. The point cloud registration method for positioning bulk cargo terminal engineering equipment according to claim 7, characterized in that: 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.

10. An application of the point cloud registration method for positioning bulk cargo terminal engineering equipment according to any one of claims 1 to 9, characterized in that: Applied to the positioning of bulk cargo terminal engineering equipment in highly dynamic closed environments.

Images