A high-precision laser point cloud map production method and system

Abstract

The present invention discloses a method and system for producing a high-precision laser point cloud map, the method comprising: obtaining point cloud data collected by a laser radar and constructing a global point cloud map; obtaining a current frame point cloud and registering it with a global point cloud map based on an NDT algorithm, and adding the current frame point cloud after successful registration to the global point cloud map; performing loop detection on the current frame point cloud and the global point cloud map, and when it is detected that the current frame point cloud and the global point cloud map form a closed loop, the global point cloud map is optimized based on a closed-loop factor and a factor graph optimization method to obtain an optimized global point cloud map. The system comprises: a construction module, a registration module, and a detection and optimization module. By using the present invention, the accuracy of the point cloud map can be improved and the problem of cumulative errors can be alleviated. As a method and system for producing a high-precision laser point cloud map, the present invention can be widely used in the field of positioning technology for autonomous driving vehicles.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology for autonomous driving vehicles, and in particular to a method and system for producing a high-precision laser point cloud map. Background Art

[0002] In autonomous driving, SLAM can be used to create point cloud maps. High-precision point cloud maps ensure the positioning accuracy and driving safety of autonomous vehicles. A complete SLAM framework consists of two parts, the front-end and the back-end. Usually, the front-end part acts as an odometer and calculates the movement of the carrier when processing data from the SLAM system sensors. The back-end part optimizes the results of the front-end output.

[0003] LiDAR has high measurement accuracy. Self-driving cars can be equipped with LiDAR to produce point cloud maps and perform high-precision positioning. In the process of point cloud map production, cumulative errors and drift problems are prone to occur, especially when point cloud maps are produced in large-scale scenes with closed loops. If there are obvious cumulative errors and obvious drift in the point cloud map production results, the same location in the real world will appear in two different locations in the point cloud map, which will mislead vehicle navigation and cause accidents. Therefore, back-end optimization and loop closure detection are of great significance in SLAM. In the process of loop closure detection, not all path environments can form a loop. If the number of loop closure detections is not limited, this step will be continuously performed throughout the SLAM cycle, which will cause a waste of computing resources and the risk of misidentification of loop closure detection. Summary of the invention

[0004] In order to solve the above technical problems, the purpose of the present invention is to provide a high-precision laser point cloud map production method and system, which can improve the accuracy of point cloud maps and alleviate the problem of cumulative errors.

[0005] The first technical solution adopted by the present invention is: a method for producing a high-precision laser point cloud map, comprising the following steps:

[0006] Obtain point cloud data collected by LiDAR and construct a global point cloud map;

[0007] Get the current frame point cloud and register it with the global point cloud map based on the NDT algorithm, and add the current frame point cloud after successful registration to the global point cloud map;

[0008] A loop detection is performed on the current frame point cloud and the global point cloud map. If it is detected that the current frame point cloud and the global point cloud map form a closed loop, the global point cloud map is optimized based on the closed loop factor and factor graph optimization method to obtain an optimized global point cloud map.

[0009] Furthermore, it also includes downsampling and filtering the point cloud data collected by the lidar.

[0010] Furthermore, the step of obtaining the current frame point cloud and registering it with the global point cloud map based on the NDT algorithm, and adding the successfully registered current frame point cloud to the global point cloud map specifically includes:

[0011] The point cloud data collected by the LiDAR is used as the source point cloud, and the source point cloud is projected into each grid of a specified size, and the Gaussian distribution within each grid is calculated to obtain the probability density of the registration;

[0012] Get the current frame point cloud as the target point cloud, estimate the relative position change of the motion vector between the target point cloud and the source point cloud, and obtain the transformation matrix;

[0013] The coordinates of the target point cloud are transformed according to the transformation matrix, and the probability density of the source point cloud and the target point cloud is calculated in combination with the probability density of the registration;

[0014] The relative probability density of the source point cloud and the target point cloud is judged based on the preset value. If the relative probability density of the source point cloud and the target point cloud meets the preset value, the target point cloud and the source point cloud are spliced ​​with the transformation matrix and added to the global point cloud map.

[0015] Furthermore, it also includes:

[0016] If it is determined that the probability density of the source point cloud and the target point cloud relative to each other does not meet the preset value, the target point cloud is reselected and the probability density is calculated and judged.

[0017] Furthermore, the detection step of performing loop detection on the current frame point cloud and the global point cloud map specifically includes:

[0018] Project the point cloud data collected by the LiDAR into each grid according to the preset resolution;

[0019] Extract the point with the highest height in each grid and store it in a matrix;

[0020] Obtain key frame point cloud from the point cloud data collected by the LiDAR as historical frame point cloud for matching;

[0021] Translate the historical frame point cloud and the current frame point cloud corresponding to the matrix and calculate the relevant distance;

[0022] The relevant distance is judged based on the preset value. If it is judged that the relevant distance meets the preset value, the current frame point cloud and the corresponding frame in the global point cloud map form a closed loop;

[0023] The global point cloud map is optimized based on the closed-loop factor and factor graph optimization method to obtain an optimized global point cloud map.

[0024] Furthermore, it also includes:

[0025] If it is determined that the relevant distance does not meet the preset value, the loop detection is performed again.

[0026] Furthermore, the calculation formula of the relevant distance is as follows:

[0027]

[0028] In the above formula, N s Indicates the number of sectors in the sth column of the matrix, I q Represents the point cloud of the current frame, I c Represents the historical frame point cloud, Represents the same index column vector of the current frame point cloud in the jth row of the matrix, Represents the same index column vector of the historical frame point cloud in the j-th row of the matrix.

[0029] Furthermore, it also includes performing dynamic loop condition detection before loop detection, and the dynamic loop condition detection formula is as follows:

[0030]

[0031] |Δ|>180°;

[0032] In the above formula, θ i represents the relative angle offset between the i-th two adjacent frames, and Δ is the accumulated value of the relative angle offsets between adjacent frames.

[0033] The second technical solution adopted by the present invention is: a high-precision laser point cloud map making system, comprising:

[0034] A construction module is used to obtain point cloud data collected by the lidar and construct a global point cloud map;

[0035] The registration module is used to obtain the current frame point cloud and register it with the global point cloud map based on the NDT algorithm, and add the current frame point cloud after successful registration to the global point cloud map;

[0036] The detection and optimization module is used to perform loop detection on the current frame point cloud and the global point cloud map. When it is detected that the current frame point cloud and the global point cloud map form a closed loop, the global point cloud map is optimized based on the closed loop factor and factor graph optimization method to obtain an optimized global point cloud map.

[0037] The beneficial effects of the method and system of the present invention are as follows: first, the present invention improves the point cloud processing efficiency by downsampling and filtering the point cloud data collected by the laser radar; secondly, by using the NDT algorithm as the laser odometer, it does not rely on feature extraction, so that the registration efficiency is high and it has greater tolerance to slight changes in the environment; then, loop detection and back-end optimization modules are added to the NDT odometer to solve the problems of large cumulative errors and obvious drift in mapping of traditional NDT algorithms in large-scale scenarios; finally, a dynamic loop condition detection method is designed in the loop detection to determine whether the path meets the loop condition, so that the loop detection mechanism is only executed when the path meets the loop condition, avoiding the waste of computing resources and reducing the potential risk of misidentification. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 It is a flowchart of the steps of a method for making a high-precision laser point cloud map according to the present invention;

[0039] Figure 2 It is a structural block diagram of a high-precision laser point cloud map making system of the present invention;

[0040] Figure 3 This is a diagram of a high-precision laser point cloud map production process according to a specific embodiment of the present invention;

[0041] Figure 4 It is a schematic diagram of calculating the angle offset of adjacent frames according to a specific embodiment of the present invention. DETAILED DESCRIPTION

[0042] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. The step numbers in the following embodiments are only provided for the convenience of explanation and description, and the order between the steps is not limited in any way. The execution order of each step in the embodiment can be adaptively adjusted according to the understanding of those skilled in the art.

[0043] Reference Figure 1 and Figure 3 The present invention provides a method for producing a high-precision laser point cloud map, the method comprising the following steps:

[0044] S1, obtain the point cloud data collected by the laser radar and build a global point cloud map;

[0045] Specifically, the point cloud data collected by the lidar is first obtained, then the point cloud data is preprocessed, and finally the preprocessed point cloud data is used to construct a global point cloud map.

[0046] Among them, the preprocessing includes downsampling and filtering. First, the point cloud data collected by the lidar is obtained, and then the point cloud data is downsampled, which reduces a large amount of point cloud data and improves the quality of the point cloud data. Then, the downsampled point cloud data is filtered, which greatly reduces the filtering calculation and effectively improves the point cloud data processing efficiency.

[0047] S2, obtaining the current frame point cloud and registering it with the global point cloud map based on the NDT algorithm, and adding the current frame point cloud after successful registration to the global point cloud map;

[0048] Specifically, the NDT (Normal Distribution Transform) algorithm is a point cloud registration algorithm that can better obtain the posture change relationship and matching degree between the two front and rear targets, so it is often used for matching positioning, map construction, etc. The most classic application of NDT is the matching of laser point clouds to obtain posture transformation, that is, rotation and translation change parameters. The core idea is to divide the source point cloud into several grids, and solve the multidimensional normal distribution of each grid according to the set parameters and calculate its probability distribution model. When the target point cloud at the same coordinate enters, the probability of each conversion point in the corresponding grid is calculated according to the probability density distribution function parameters, and the probabilities of all grids are accumulated to obtain p. When this probability p reaches the maximum, the optimal matching relationship is found.

[0049] S2.1. Use the point cloud data collected by the LiDAR as the source point cloud, project the source point cloud into each grid of a specified size, and calculate the Gaussian distribution within each grid to obtain the probability density of the registration;

[0050] Specifically, the normal distribution is also called "normal distribution", also known as Gaussian distribution. First, the space is divided into grids. The grid size can be confirmed according to the environment. The source point cloud is projected into each grid of the specified size, and these grids are traversed to retain grids containing at least 5 points (the minimum number of points in this cell is 5 to ensure that the normal distribution is computable. In practice, a value greater than 5 can be taken. For example, in some outdoor environments, dozens to hundreds of points can be taken) to avoid inappropriate size; secondly, the mean and covariance matrix in each grid are calculated. The covariance matrix is ​​the discrete distribution in each grid. The normal distribution is constructed according to the mean and covariance matrix to obtain the probability density of the alignment.

[0051] S2.2, obtaining the current frame point cloud as the target point cloud, estimating the relative position change of the motion vectors of the target point cloud and the source point cloud, and obtaining the transformation matrix;

[0052] S2.3, transform the coordinates of the target point cloud according to the transformation matrix, and calculate the relative probability density of the source point cloud and the target point cloud in combination with the registered probability density;

[0053] Specifically, the target point cloud is transformed into the grid of the source point cloud through the transformation matrix, the normal distribution of each point is confirmed, the distribution of each mapping point is evaluated in combination with the registered probability density and the results are accumulated to obtain the relative probability density of the source point cloud and the target point cloud.

[0054] S2.4. The relative probability density of the source point cloud and the target point cloud is judged based on the preset value. If the relative probability density of the source point cloud and the target point cloud meets the preset value, the target point cloud and the source point cloud are spliced ​​with the transformation matrix and added to the global point cloud map.

[0055] Specifically, if it is determined that the relative probability density of the source point cloud and the target point cloud meets the preset value, the source point cloud and the target point cloud are connected using the transformation matrix, the target point cloud is updated and integrated into the next registration as a global point cloud map, and the successfully registered target point cloud is added to the global point cloud map; if it is determined that the relative probability density of the source point cloud and the target point cloud does not meet the preset value, the target point cloud is reselected and the probability density is calculated and judged.

[0056] S3, performing loop detection on the current frame point cloud and the global point cloud map;

[0057] Specifically, loop closure detection uses the Scan Context algorithm, whose core is to use point cloud data for scene recognition. It reduces the dimensionality of the original point cloud data and directly uses the data in a two-dimensional matrix format as a descriptor for similarity comparison.

[0058] First, the point cloud data collected by the lidar is projected into each grid according to a preset resolution; secondly, the z-axis data is used as the selection criterion to extract the point with the highest height in each grid and store it in a matrix; then, the key frame point cloud is obtained from the point cloud data collected by the lidar as the historical frame point cloud for subsequent matching; finally, the historical frame point cloud and the current frame point cloud corresponding to the matrix are translated to calculate the relevant distance; if the relevant distance meets the preset value, the current frame point cloud and the global point cloud map form a closed loop; if the relevant distance does not meet the preset value, the current frame point cloud and the global point cloud map do not form a closed loop, and loop detection and judgment need to be performed again.

[0059] Among them, the matrix is ​​formed by the Scan context after dimensionality reduction, and the value stored in the matrix is ​​the height value of the "landform"; this matrix is ​​the encoding of the scene, and because it contains global information, it is a global descriptor.

[0060] Due to the different viewing angles of the laser radar, that is, when the laser radar rotates a certain angle at the same location, the column vector value remains unchanged, but there will be an offset; the behavior of the row vector is that the order of the elements in the vector will change, but the row vector will not be offset. In order to be more effective for dynamic objects, column comparison is used. The calculation formula for the relevant distance is as follows:

[0061]

[0062] In the above formula, N s represents the number of sectors in the sth column of the matrix, q is the query, and represents the point cloud to be queried, that is, I q represents the current frame point cloud, c is candidate represents the candidate point cloud, that is, I c Represents the historical frame point cloud, Represents the same index column vector of the current frame point cloud in the jth row of the matrix, Represents the same index column vector of the historical frame point cloud in the j-th row of the matrix.

[0063] S4. Optimize the global point cloud map based on the closed-loop factor and factor graph optimization method to obtain an optimized global point cloud map.

[0064] Specifically, if and only if it is detected that the current frame point cloud forms a closed loop with the global point cloud map, the global point cloud map is optimized using GTSAM based on the closed loop factor to obtain an optimized global point cloud map.

[0065] Among them, GTSAM stands for Georgia Tech Smoothing and Mapping library, which is a C++ library file launched by researchers at the Georgia Institute of Technology based on factor graphs and Bayesian networks. Generally, factor graph optimization algorithms are used in engineering. The most common way is to use the GTSAM library to implement them. You only need to call the GTSAM library like calling other third-party libraries (such as openCV, PCL, etc.).

[0066] As a further preferred embodiment of the method, it further includes performing dynamic loop condition detection before loop detection, such as Figure 4 As shown in the figure, the loop detection mechanism is executed only when the dynamic loop condition detection is met, which avoids the waste of computing resources and reduces the potential risk of misidentification. The dynamic loop condition detection formula is as follows:

[0067]

[0068] |Δ|>180°;

[0069] In the above formula, θ irepresents the relative angle offset between the i-th adjacent frames, Δ is the accumulated value of the relative angle offset between each adjacent frame, and when the absolute value of Δ is greater than 180°, it is considered that the path meets the loop condition.

[0070] like Figure 2 As shown, a high-precision laser point cloud map making system includes:

[0071] A construction module is used to obtain point cloud data collected by the lidar and construct a global point cloud map;

[0072] The registration module is used to obtain the current frame point cloud and register it with the global point cloud map based on the NDT algorithm, and add the current frame point cloud after successful registration to the global point cloud map;

[0073] The detection and optimization module is used to perform loop detection on the current frame point cloud and the global point cloud map. If it is detected that the current frame point cloud and the global point cloud map form a closed loop, the global point cloud map is optimized based on the closed loop factor and factor graph optimization method to obtain an optimized global point cloud map.

[0074] The contents of the above method embodiments are all applicable to the present system embodiments. The functions specifically implemented by the present system embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0075] The above is a specific description of the preferred implementation of the present invention, but the invention is not limited to the embodiments. Those skilled in the art may make various equivalent modifications or substitutions without violating the spirit of the present invention. These equivalent modifications or substitutions are all included in the scope defined by the claims of this application.

Claims

1. A method for producing high-precision laser point cloud maps. It is characterized in that The following steps are involved: Obtain point cloud data collected by LiDAR and construct a global point cloud map; Get the current frame point cloud and register it with the global point cloud map based on the NDT algorithm, and add the current frame point cloud after successful registration to the global point cloud map; Perform loop detection on the current frame point cloud and the global point cloud map. If it is detected that the current frame point cloud and the global point cloud map form a closed loop, the global point cloud map is optimized based on the closed loop factor and factor graph optimization method to obtain an optimized global point cloud map. The detection step of performing loop detection on the current frame point cloud and the global point cloud map specifically includes: Project the point cloud data collected by the LiDAR into each grid according to the preset resolution; Extract the point with the highest height in each grid and store it in a matrix; Obtain key frame point cloud from the point cloud data collected by the LiDAR as historical frame point cloud for matching; Translate the historical frame point cloud and the current frame point cloud corresponding to the matrix and calculate the relevant distance; The relevant distance is judged based on the preset value. If it is judged that the relevant distance meets the preset value, the current frame point cloud and the corresponding frame in the global point cloud map form a closed loop; The global point cloud map is optimized based on the closed-loop factor and factor graph optimization method to obtain an optimized global point cloud map.

2. According to the method for producing a high-precision laser point cloud map according to claim 1, It is characterized in that It also includes downsampling and filtering of point cloud data collected by the lidar.

3. According to the method for producing a high-precision laser point cloud map as described in claim 1, It is characterized in that The step of obtaining the current frame point cloud and registering it with the global point cloud map based on the NDT algorithm, and adding the current frame point cloud after successful registration to the global point cloud map specifically includes: The point cloud data collected by the LiDAR is used as the source point cloud, and the source point cloud is projected into each grid of a specified size, and the Gaussian distribution within each grid is calculated to obtain the probability density of the registration; Get the current frame point cloud as the target point cloud, estimate the relative position change of the motion vector between the target point cloud and the source point cloud, and obtain the transformation matrix; The coordinates of the target point cloud are transformed according to the transformation matrix, and the probability density of the source point cloud and the target point cloud is calculated in combination with the probability density of the registration; The relative probability density of the source point cloud and the target point cloud is judged based on the preset value. If the relative probability density of the source point cloud and the target point cloud meets the preset value, the target point cloud and the source point cloud are spliced ​​with the transformation matrix and added to the global point cloud map.

4. According to the method for producing a high-precision laser point cloud map as described in claim 3, It is characterized in that Also includes: If it is determined that the probability density of the source point cloud and the target point cloud relative to each other does not meet the preset value, the target point cloud is reselected and the probability density is calculated and judged.

5. According to the method for producing a high-precision laser point cloud map as described in claim 1, It is characterized in that Also includes: If it is determined that the relevant distance does not meet the preset value, the loop detection is performed again.

6. According to the method for producing a high-precision laser point cloud map as described in claim 1, It is characterized in that The calculation formula of the relevant distance is as follows: In the above formula, N s Indicates the number of sectors in the sth column of the matrix, I q Represents the point cloud of the current frame, I c Represents the historical frame point cloud, Represents the same index column vector of the current frame point cloud in the jth row of the matrix, Represents the same index column vector of the historical frame point cloud in the j-th row of the matrix.

7. According to claim 1, a method for producing a high-precision laser point cloud map, It is characterized in that It also includes performing dynamic loop condition detection before loop detection. The dynamic loop condition detection formula is as follows: |Δ|>180°; In the above formula, θ i represents the relative angle offset between the i-th two adjacent frames, and Δ is the accumulated value of the relative angle offsets between adjacent frames.

8. A high-precision laser point cloud map production system, It is characterized in that include: A construction module is used to obtain point cloud data collected by the lidar and construct a global point cloud map; The registration module is used to obtain the current frame point cloud and register it with the global point cloud map based on the NDT algorithm, and add the current frame point cloud after successful registration to the global point cloud map; The detection and optimization module is used to perform loop detection on the current frame point cloud and the global point cloud map. If it is detected that the current frame point cloud and the global point cloud map form a closed loop, the global point cloud map is optimized based on the closed loop factor and factor graph optimization method to obtain an optimized global point cloud map. The detection step of performing loop detection on the current frame point cloud and the global point cloud map specifically includes: Project the point cloud data collected by the LiDAR into each grid according to the preset resolution; Extract the point with the highest height in each grid and store it in a matrix; Obtain key frame point cloud from the point cloud data collected by the LiDAR as the historical frame point cloud for matching; Translate the historical frame point cloud and the current frame point cloud corresponding to the matrix and calculate the relevant distance; The relevant distance is judged based on the preset value. If it is judged that the relevant distance meets the preset value, the current frame point cloud and the corresponding frame in the global point cloud map form a closed loop; The global point cloud map is optimized based on the closed-loop factor and factor graph optimization method to obtain an optimized global point cloud map.

Images