Laser radar point cloud registration method for construction site robot

Abstract

The invention relates to a laser radar point cloud registration method for a construction site robot, and the method comprises the steps: inputting point cloud data obtained by a laser radar or processing data obtained through the processing of the point cloud data into a pre-trained deep neural network, and obtaining an initial transformation matrix; and then performing numerical iteration by taking the initial transformation matrix as an initial value of numerical iteration to obtain a precise transformation matrix meeting a preset optimization condition. An initial transformation matrix obtained by a machine learning algorithm is subjected to iterative optimization through a numerical iterative algorithm to obtain a precise transformation matrix with higher precision. At the moment, as the initial value is relatively close to a theoretical solution, the corresponding target value can be stably obtained in a relatively short time through numerical iteration. Therefore, the requirement of obtaining a reliable transformation result under a limited condition is met. The time requirement and the precision requirement can be met.

Description

Technical Field

[0001] The present invention relates to the technical field of laser radar data processing, and in particular to a laser radar point cloud registration method for a construction site robot. Background Art

[0002] Low-speed autonomous driving robots are an important part of future intelligent transportation and smart city construction. Applying low-speed autonomous driving construction site robots to construction site scenes for security inspection tasks is of great significance to improving the level of safety production on construction sites. LiDAR point cloud registration is a key module in autonomous driving technology and the basis of algorithms such as environmental perception and vehicle autonomous positioning. How to further improve the accuracy and speed of registration has always been a research hotspot at home and abroad.

[0003] The point clouds scanned by LiDAR in open construction sites are numerous and sparsely distributed. Traditional point cloud registration methods such as ICP, RANSAC and other iterative optimization algorithms have slow convergence speeds and cannot meet the requirements of mobile robot rapid response scenarios. However, with the rapid development of deep learning technology, the effect of point cloud registration using pre-trained neural networks has been significantly improved. Neural networks pre-trained with public data sets collected from the real world, such as KITTI, can directly receive LiDAR point cloud data and output rigid transformation matrices, saving a lot of iteration time required by traditional methods and improving algorithm efficiency.

[0004] However, the shortcomings of existing neural networks are that, firstly, the network is large in size and has many layers, which cannot meet the deployment requirements of mobile robots with low computing power; secondly, the network does not consider the sparse distribution characteristics of long-distance point clouds in open scenes, and the registration performance decreases as the distance of the point clouds increases. Finally, due to the environmental differences between public datasets and construction site scenes, the network trained on the basis of public datasets has poor robustness in the output results of actual application scenarios. Summary of the invention

[0005] The purpose of the present invention is to solve the problem of inaccurate registration of sparse laser radar point clouds of a construction site robot, and to provide a laser radar point cloud registration method for a construction site robot.

[0006] The technical solution of the present invention provides a laser radar point cloud registration method for a construction site robot. First, the point cloud data obtained by the laser radar or the processed data obtained by the laser radar is input into the pre-trained deep neural network to obtain the initial transformation matrix; Then, numerical iteration is performed using the initial transformation matrix as the initial value of the numerical iteration to obtain an accurate transformation matrix that meets the preset optimization conditions.

[0007] Preferably, the steps of performing numerical iteration with the initial transformation matrix as the initial value of numerical iteration to obtain an accurate transformation matrix that meets the preset optimization conditions are: The target frame point cloud is divided into uniform voxels with a side length of 0.3m, and the normal distribution parameters of each grid are traversed and calculated; the point cloud in each grid satisfies the Gaussian distribution, and the mean is calculated and mean square error :

[0008]

[0009] Initialize transformation parameters ,For the original frame point cloud to be registered, through the transformation matrix Convert it to the target point cloud coordinate system , the transformation matrix It can be expressed as

[0010] The initial transformation matrix is ​​used as the initial transformation parameter. The initial transformation parameters are used to perform a matrix transformation on the original frame so that the original frame and the target frame are in the same coordinate system, and the point cloud of the original frame is transformed into the voxels of the point cloud of the target frame; the probability density of each point in each voxel is calculated. :

[0011] The probability density of each voxel is added to get the NDT registration score :

[0012] Iteratively optimize the NDT registration score according to the Gauss-Newton optimization algorithm; The iteration ends when the set optimization conditions are met, and the optimized precise transformation matrix is ​​saved.

[0013] Preferably, the deep neural network consists of a feature encoding module, a feature pairing module and an SVD decomposition module; The feature encoding module network is a fully connected structure, which uses the cross attention mechanism and the residual attention mechanism to encode the spatial geometric feature information of the point cloud; the feature matching module uses a shared multi-layer perceptron network to match the point cloud pairs of the original frame and the target frame through feature information; the SVD decomposition module performs singular value decomposition on the matched point cloud pairs, solves the transformation matrix, and records the initial transformation matrix.

[0014] Preferably, the processed data is obtained by filtering the ground point cloud data; and / or, obtaining processed data by dynamically filtering out obstacles from the point cloud data; And / or, the processed data is obtained by sampling the point cloud data.

[0015] Preferably, the ground point cloud filtering step is: Downsample the laser point cloud using a cubic voxel grid with a preset edge length; According to the uniform distribution of laser radar beams, a circular grid map of the mission area is established. According to the acquisition principle of the equipped mechanical laser radar, the mission scene is evenly divided into 360° / The fan-shaped area, Represents the fixed horizontal resolution of the lidar; for any grid ,in and Indicates the number of rings and lines of the grid; the highest point in the point cloud in the grid is represented by , the lowest point As the ground base point, according to the four-domain relationship between grids After correction, the estimated values ​​of the four-domain ground base points at the lowest point in the grid can be expressed as:

[0016] when and The absolute value of the difference is less than the set four-area height difference threshold When using replace The value of is used to complete the correction of the grid ground base point; Calculate the height difference between the highest and lowest points in the grid:

[0017] when Less than the set ground point height threshold , then Marked as ground area, All points in are marked as ground points, otherwise they are marked as obstacle points; Filter out the ground point cloud and keep the remaining point clouds as feature points and obstacle points; Preferably, the dynamic obstacle filtering step is: Perform clustering processing on the point cloud data to obtain the clustered obstacle target point cloud; Filter out dynamic obstacles based on inter-frame matching: Project the retained obstacle target point cloud onto a two-dimensional grid plane, establish a global grid map by setting the grid size, traverse each laser projection point in each frame, and calculate the distance between the laser projection point and the center of the vehicle :

[0018] Select the closest point , determine a sphere center at a position one voxel diagonal length away from the point on the laser light path, and make a sphere with a voxel diagonal length as the radius; all the projection points in the blocked area behind the sphere are taken as The neighbor points of The normal vector of The position of a voxel diagonal determines a plane, and then traverses all grids within the safe surface on the optical path of that point until the nearest points in all grids are processed; Static obstacles and dynamic obstacles are determined by the change in the distance between the point projected on the safety surface and the center of the vehicle between the two frames; the point cloud determined to be a dynamic obstacle is filtered out, and the point cloud and feature points of the static obstacle are retained.

[0019] Preferably, the point cloud sampling step is: Perform a small amount of random sampling on the processed lidar point cloud; Based on a small number of sampling points, the Gaussian attenuation function is used Calculate the distance weights of the remaining point clouds and perform weighted sampling based on the distance weights. It is expressed as follows:

[0020] in To set the parameters, is the distance between the sampling point and the center of the vehicle, is the standard deviation; according to the distribution law of the Gaussian function, the sampling points are evenly distributed in the task area.

[0021] Preferably, firstly, the ground point cloud is filtered out from the point cloud data to obtain the processed data; then, the dynamic obstacle is filtered out from the previous processing result to obtain the processed data; finally, the previous result is sampled to obtain the processed data.

[0022] The present invention uses a numerical iteration algorithm to iteratively optimize the initial transformation matrix obtained by the machine learning algorithm to obtain a more accurate transformation matrix with higher accuracy. At this time, since the initial value is relatively close to the theoretical solution, the numerical iteration may stably obtain the corresponding target value in a shorter time. Thus, the requirement of obtaining reliable transformation results under restricted conditions is met. It not only meets the time requirement but also meets the accuracy requirement. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 A schematic diagram of the steps of a distance weighted construction robot laser radar point cloud registration method according to some embodiments disclosed in the present application; Figure 2A system structure diagram of a distance weighted construction robot laser radar point cloud registration method according to some embodiments disclosed in this application; Figure 3 A schematic diagram of dynamic obstacle point cloud filtering in some embodiments disclosed in this application; Figure 4 This is a schematic diagram of distance-weighted lidar point cloud sampling for some embodiments disclosed in this application. DETAILED DESCRIPTION

[0024] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. In this specification, the size ratios in the drawings do not represent the actual size ratios, but are only used to reflect the relative position relationship and connection relationship between the components. Components with the same name or the same number represent similar or identical structures and are only for illustrative purposes.

[0025] Figure 1 The present invention is a flowchart of the laser radar point cloud registration method for the construction site robot. First, the point cloud data obtained by the laser radar or the data processed by the laser radar is input into the pre-trained deep neural network to obtain the initial transformation matrix. Due to the repeatability of machine learning and the statistical nature of the structure, although the error of the initial transformation matrix is ​​close to the usable value, the error is still large, and there are often problems such as poor stability in actual use. Therefore, based on the initial transformation matrix, it is numerically iterated to determine the precise transformation matrix that meets the accuracy requirements.

[0026] Taking the processed data as an example, the processed data is obtained through the original point cloud. The processed data is input into the deep neural network and the initial transformation matrix is ​​output.

[0027] Training deep neural networks: Use the KITTI Odemetry dataset to provide 11 sets of lidar data with true values ​​of relative transformation matrices. Use groups 1 to 6 as training datasets, groups 7 to 9 as validation datasets, and groups 10 and 11 as test datasets. Use the sampled point cloud as the input of the network, and use a high-performance computing platform equipped with a GPU as the training device.

[0028] Deep neural network structure: The neural network consists of a feature encoding module, a feature pairing module, and an SVD decomposition module; The feature encoding module network is a fully connected structure, which uses the cross attention mechanism and the residual attention mechanism to encode the spatial geometric feature information of the point cloud; the feature matching module uses a shared multi-layer perceptron network to match the point cloud pairs of the original frame and the target frame through feature information; the SVD decomposition module performs singular value decomposition on the matched point cloud pairs, solves the transformation matrix, and records the initial transformation matrix.

[0029] The input of the deep neural network is the original lidar point cloud or its processed data, and the output is the rotation matrix and translation matrix, that is, the initial transformation matrix.

[0030] The initial transformation matrix is ​​optimized by Gauss-Newton iterative optimization using fast NDT to obtain the precise transformation matrix, which is then used to align the original frame and the target frame.

[0031] The target frame point cloud is divided into uniform voxels with a side length of 0.3m, and the normal distribution parameters of each grid are calculated. The point cloud in each grid satisfies the Gaussian distribution, and the mean is calculated. and mean square error :

[0032]

[0033] Initialize transformation parameters ,For the original frame point cloud to be registered, through the transformation matrix Convert it to the target point cloud coordinate system , the transformation matrix It can be expressed as

[0034] The initial transformation matrix is ​​used as the initial transformation parameter. The initial transformation parameters are used to perform a matrix transformation on the original frame so that the original frame and the target frame are in the same coordinate system, and the point cloud of the original frame is transformed into the voxel of the point cloud of the target frame. The probability density of each point in each voxel is calculated. :

[0035] The probability density of each voxel is added to get the NDT registration score :

[0036] The NDT registration score is iteratively optimized according to the Gauss-Newton optimization algorithm.

[0037] After the set optimization conditions are met, the iteration ends and the optimized precise transformation matrix is ​​saved; The original frame and the target frame are registered using the precise transformation matrix to obtain a frame of point cloud in the same coordinate system.

[0038] Complete the construction site robot lidar point cloud registration with distance weight integration.

[0039] The method of obtaining the processed data obtained by processing the original point cloud may be one or a combination of the following methods.

[0040] 1. Ground point cloud filtering.

[0041] Use the queue structure to maintain a memory containing two consecutive frames of point cloud. The current frame is used as the target frame, and the previous frame is used as the original frame; While keeping the original distribution characteristics of the laser radar unchanged, the laser point cloud is downsampled using a cubic voxel grid with a side length of 0.3m; According to the uniform distribution of laser radar beams, a circular grid map of the mission area is established. According to the acquisition principle of the equipped mechanical laser radar, the mission scene is evenly divided into 360° / The fan-shaped area, Represents the fixed horizontal resolution of the LiDAR. ,in and Indicates the number of rings and lines of the grid. The highest point in the point cloud within the grid is represented by , the lowest point As the ground base point, according to the four-domain relationship between grids After correction, the estimated values ​​of the four-domain ground base points at the lowest point in the grid can be expressed as:

[0042] when and The absolute value of the difference is less than the set four-area height difference threshold When using replace The value of is used to complete the correction of the grid ground base point.

[0043] Calculate the height difference between the highest and lowest points in the grid:

[0044] when Less than the set ground point height threshold , then Marked as ground area, All points in are marked as ground points, otherwise they are marked as obstacle points.

[0045] Filter out the ground point cloud and keep the remaining point clouds as feature points and obstacle points.

[0046] 2. Dynamic obstacle filtering.

[0047] The clustering algorithm is used to cluster the obstacle points. First, the point distance threshold and line distance threshold are set. The clustering operation is performed according to the point distance and line distance on the annular grid plane, and the clustered obstacle target point cloud is saved.

[0048] Using the target clustering results, dynamic obstacles are filtered out according to inter-frame matching. The specific steps include: The retained obstacle target point cloud is projected onto a two-dimensional grid plane to create a global grid map, with the grid voxel size set to 0.3m. Each laser projection point in each frame is traversed to calculate the distance between the laser projection point and the center of the vehicle. :

[0049] Select the closest point ,like Figure 3 As shown in the figure, a sphere center is determined at a position one voxel diagonal length away from the point on the laser light path, and a sphere is made with a voxel diagonal length as the radius. All projection points in the blocked area behind the sphere are taken as The neighbor points of The normal vector of . The position of a voxel diagonal determines a plane, and then traverses all grids within the safe surface on the optical path of the point until the nearest points in all grids are processed.

[0050] Static obstacles and dynamic obstacles are judged by the change in the distance between the point projected on the safety surface and the center of the vehicle between the two frames.

[0051] The point clouds judged as dynamic obstacles are filtered out, and the point clouds and feature points of static obstacles are retained.

[0052] 3. Point cloud sampling.

[0053] Taking the vehicle as the center of the coordinate system, the lidar point cloud is distance-weighted sampled. The specific steps include: Perform a small amount of random sampling on the processed lidar point cloud; Based on a small number of sampling points, the Gaussian attenuation function is used Calculate the distance weights of the remaining point clouds, such as Figure 4 As shown, weighted sampling is performed according to the distance weight. It is expressed as follows:

[0054] in To set the parameters, is the distance between the sampling point and the center of the vehicle, is the standard deviation. According to the distribution law of Gaussian function, the sampling points are evenly distributed in the task area.

[0055] Input the sampling points into the deep neural network and output the initial transformation matrix. The specific steps include: Training deep neural network: Use KITTI Odemetry dataset to provide 11 sets of LiDAR data with true values ​​of relative transformation matrix. Use sets 1 to 6 as training data sets, sets 7 to 9 as validation data sets, and sets 10 and 11 as test data sets. Use S32 sampled point cloud as network input, and use high-performance computing platform equipped with GPU as training equipment.

[0056] The above content only describes the preferred implementation mode of the present invention, and does not limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solution of the present invention by ordinary technicians in this field should fall within the protection scope determined by the claims of the present invention.

Claims

1. A laser radar point cloud registration method for a construction site robot, characterized in that: First, the point cloud data obtained by the laser radar or the processed data obtained by the laser radar is input into the pre-trained deep neural network to obtain the initial transformation matrix; Then, numerical iteration is performed using the initial transformation matrix as the initial value of the numerical iteration to obtain an accurate transformation matrix that meets the preset optimization conditions.

2. The laser radar point cloud registration method for a construction site robot according to claim 1, characterized in that: The steps of performing numerical iteration with the initial transformation matrix as the initial value of numerical iteration to obtain the accurate transformation matrix that meets the preset optimization conditions are as follows: The target frame point cloud is divided into uniform voxels with preset side lengths, and the normal distribution parameters of each grid are traversed and calculated; the point cloud in each grid satisfies the Gaussian distribution, and the mean is calculated and mean square error : Initialize transformation parameters ,For the original frame point cloud to be registered, through the transformation matrix Convert it to the target point cloud coordinate system , the transformation matrix It can be expressed as The initial transformation matrix is ​​used as the initial transformation parameter, and the matrix transformation is performed on the original frame using the initial transformation parameter, so that the original frame and the target frame are located in the same coordinate system, and the point cloud of the original frame is transformed into the voxel of the point cloud of the target frame; Calculate the probability density of each point in each voxel : The probability density of each voxel is added to get the NDT registration score : Iteratively optimize the NDT registration score according to the Gauss-Newton optimization algorithm; The iteration ends when the set optimization conditions are met, and the optimized precise transformation matrix is ​​saved.

3. The laser radar point cloud registration method for a construction site robot according to claim 1, characterized in that: The deep neural network consists of a feature encoding module, a feature pairing module and an SVD decomposition module; The feature encoding module network is a fully connected structure, using the cross attention mechanism and residual attention mechanism to encode the spatial geometric feature information of the point cloud; The feature matching module uses a shared multi-layer perceptron network to match the point cloud pairs of the original frame and the target frame through feature information; the SVD decomposition module performs singular value decomposition on the matched point cloud pairs, solves the transformation matrix, and records the initial transformation matrix.

4. The laser radar point cloud registration method for a construction site robot according to claim 1, characterized in that: Processed data is obtained by filtering ground point cloud from point cloud data; and / or, obtaining processed data by dynamically filtering out obstacles from the point cloud data; And / or, the processed data is obtained by sampling the point cloud data.

5. The laser radar point cloud registration method for a construction site robot according to claim 4, characterized in that: The ground point cloud filtering steps are as follows: Downsample the laser point cloud using a cubic voxel grid with a preset edge length; According to the uniform distribution relationship of the laser radar beam, a circular grid map of the mission area is established. According to the collection principle of the equipped mechanical laser radar, the mission scene is evenly divided into 360° / The fan-shaped area, Represents the fixed horizontal resolution of the lidar; for any grid ,in and Indicates the number of rings and lines of the grid; the highest point in the point cloud in the grid is represented by , the lowest point As the ground base point, according to the four-domain relationship between grids After correction, the estimated values ​​of the four-domain ground base points at the lowest point in the grid can be expressed as: when and The absolute value of the difference is less than the set four-area height difference threshold When using replace The value of is used to complete the correction of the grid ground base point; Calculate the height difference between the highest and lowest points in the grid: when Less than the set ground point height threshold , then Marked as ground area, All points in are marked as ground points, otherwise they are marked as obstacle points; Filter out the ground point cloud and keep the remaining point clouds as feature points and obstacle points.

6. The laser radar point cloud registration method for a construction site robot according to claim 4, characterized in that: The dynamic obstacle filtering steps are: Perform clustering processing on the point cloud data to obtain the clustered obstacle target point cloud; Filter out dynamic obstacles based on inter-frame matching: Project the retained obstacle target point cloud onto a two-dimensional grid plane, establish a global grid map by setting the grid size, traverse each laser projection point in each frame, and calculate the distance between the laser projection point and the center of the vehicle : Select the closest point , determine a sphere center at a position one voxel diagonal length away from the point on the laser light path, and make a sphere with a voxel diagonal length as the radius; all the projection points in the blocked area behind the sphere are taken as The neighbor points of The normal vector of The position of a voxel diagonal determines a plane, and then traverses all grids within the safe surface on the optical path of that point until the nearest points in all grids are processed; Static obstacles and dynamic obstacles are determined by the change in the distance between the point projected on the safety surface and the center of the vehicle between the two frames; the point cloud determined to be a dynamic obstacle is filtered out, and the point cloud and feature points of the static obstacle are retained.

7. The laser radar point cloud registration method for a construction site robot according to claim 4, characterized in that: The point cloud sampling steps are: Perform a small amount of random sampling on the processed lidar point cloud; Based on a small number of sampling points, the Gaussian attenuation function is used Calculate the distance weights of the remaining point clouds and perform weighted sampling based on the distance weights. It is expressed as follows: in To set the parameters, is the distance between the sampling point and the center of the vehicle, is the standard deviation; according to the distribution law of the Gaussian function, the sampling points are evenly distributed in the task area.

8. The laser radar point cloud registration method for a construction site robot according to claim 4, characterized in that: First, the ground point cloud is filtered out from the point cloud data to obtain the processed data; then, the dynamic obstacle is filtered out from the previous processing result to obtain the processed data; finally, the processed data is obtained by sampling the previous result.

Images