NDT single-frame point cloud registration and positioning method based on cylindrical coordinates

Abstract

The present invention discloses an NDT single-frame point cloud registration and positioning method based on cylindrical coordinates, comprising the following steps: S1, determining a registration target area, S2, dividing a cylindrical coordinate grid, S3, allocating a map point cloud to the grid, S4, generating a probability density function of each grid, S5, pre-converting the single-frame point cloud, S6, allocating the single-frame point cloud to the grid, S7, calculating a positioning score, S8, performing nonlinear optimization and solving, and S9, iteratively seeking an optimal solution. The present invention designs a cylindrical coordinate grid based on the distribution characteristics of the single-frame point cloud. Compared with a traditional cubic grid, this method can make the single-frame point cloud more evenly distributed in each grid, greatly reducing the number of invalid grids and improving positioning accuracy. At the same time, the cylindrical coordinate grid can cover a larger range with fewer grids, reducing resource consumption when querying the grid.

Description

Technical Field

[0001] The present invention relates to the field of positioning and navigation technology, and in particular to an NDT single-frame point cloud registration and positioning method based on cylindrical coordinates. Background Art

[0002] Currently, NDT positioning is based on a Cartesian coordinate system that evenly divides the positioning grid. When using a single-frame (360-degree) point cloud from a mechanical lidar to match the map, the scanning characteristics of the mechanical lidar result in extremely uneven distribution of the point cloud in each grid, resulting in a decrease in positioning accuracy. Summary of the Invention

[0003] The technical problem to be solved by the present invention is to provide an NDT single-frame point cloud registration and positioning method based on cylindrical coordinates with uniform point cloud distribution, fewer invalid grids and higher positioning accuracy.

[0004] To solve the above technical problems, the technical solution of the present invention is: an NDT single-frame point cloud registration and positioning method based on cylindrical coordinates, comprising the following steps:

[0005] S1. Determine the registration target area: determine the coordinate center O of the cylindrical coordinates;

[0006] S2. Divide the cylindrical coordinate grid: With the coordinate center O as the center and the laser radar scanning distance as the radius, make a cylinder. Intercept the single-frame point cloud of the positioning map within the cylinder as the registered target point cloud Q. Divide the target point cloud Q into multiple grids according to the three dimensions (α, r, h);

[0007] S3. Assigning map point clouds to grids: Calculate the corresponding cylindrical coordinates (α, r, h) based on the 3D coordinates (x, y, z) of all points in the target point cloud Q, and then find the corresponding grid;

[0008] S4. Generate probability density function of each grid: calculate the three-dimensional normal distribution parameters of each grid;

[0009] S5, single-frame point cloud pre-conversion: convert the single-frame point cloud to be registered into the inertial navigation coordinate system through calibration parameters, and then convert it into the positioning map coordinate system through the initial posture;

[0010] S6. Assigning grids to single-frame point clouds: Find the grid where each point cloud in the single-frame point cloud is located according to the definition in step S3;

[0011] S7. Calculate the positioning score: Calculate the probability density of each point cloud in the single-frame point cloud to be registered based on the normal distribution parameters calculated in step S4, and add the results calculated for each point cloud to obtain the cylindrical coordinate NDT registration score;

[0012] S8, nonlinear optimization solution: Use the Gauss-Newton method to optimize the socre function and find the optimal posture R and optimal position T to maximize the value of socre;

[0013] S9. Iterate to find the optimal solution: Bring the calculated R and T into step S5, and repeat steps S5-S7 until the change in R and T is less than the threshold. The final R and T obtained are the results of single-frame point cloud positioning.

[0014] As a preferred technical solution, the grid calculation method in step S3 is:

[0015]

[0016] h=z

[0017] Where (x, y, z) are three-dimensional coordinates and (α, r, h) are cylindrical coordinates.

[0018] As a preferred technical solution, the specific calculation method of the three-dimensional normal distribution parameters of the grid in step S4 is:

[0019]

[0020] Among them, p i is the coordinate of the i-th point in the grid, p q is the mean coordinate, ∑ is the covariance matrix. As a preferred technical solution, in step S5, the coordinate transformation formula is:

[0021] P'=RP+T

[0022] Among them, P' is the transformed point cloud.

[0023] As a preferred technical solution, the calculation method of the cylindrical coordinate NDT registration score in step S7 is:

[0024]

[0025] score=∑PDF

[0026] Among them, p i is a point in P′.

[0027] Due to the adoption of the above technical solution, the beneficial effects of the present invention are as follows: the present invention designs a cylindrical coordinate grid based on the distribution characteristics of a single-frame point cloud. Compared with the traditional cubic grid, this method can make the single-frame point cloud more evenly distributed in each grid, greatly reducing the number of invalid grids and improving positioning accuracy; at the same time, the cylindrical coordinate grid can cover a larger range with fewer grids, reducing resource consumption when querying the grid. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] The following drawings are intended only to illustrate and explain the present invention, and are not intended to limit the scope of the present invention.

[0029] Figure 1 is an algorithm flow chart of an embodiment of the present invention;

[0030] Figure 2 is a schematic diagram of a cylindrical coordinate grid according to an embodiment of the present invention; DETAILED DESCRIPTION

[0031] The present invention will be further described below with reference to the accompanying drawings and examples. In the following detailed description, certain exemplary embodiments of the present invention are described by way of illustration only. It is understood that those skilled in the art will recognize that the described embodiments may be modified in various ways without departing from the spirit and scope of the present invention. Therefore, the drawings and description are illustrative in nature and are not intended to limit the scope of the claims.

[0032] like Figure 1 As shown in FIG, the NDT single-frame point cloud registration and positioning method based on cylindrical coordinates includes the following steps:

[0033] S1. Determine the registration target area: Determine the coordinate center O of the cylindrical coordinates based on data from other position sensors (GNSS, etc.).

[0034] S2. Divide the cylindrical coordinate grid: With the coordinate center O as the center of the circle and the laser radar scanning distance (usually about 100m) as the radius, make a cylinder. Intercept the single-frame point cloud of the positioning map within the cylinder as the registered target point cloud Q. Divide the target point cloud Q into multiple grids according to the three dimensions (angle α, radial distance r, height h). The specific division method is as follows:

[0035] α dimension: 0° is due north, and north by east is positive. Each N in the α dimension is one unit. In this example, n = 45°

[0036] r dimension: Each r is a unit, in this example r = 1m

[0037] h dimension: Each h is a unit, in this example h = 1m

[0038] S3. Assigning map point clouds to grids: Calculate the corresponding cylindrical coordinates (α, r, h) based on the three-dimensional coordinates (x, y, z) of all points in the target point cloud Q, and then find the corresponding grid. The grid calculation method is:

[0039]

[0040] h=z

[0041] Where (x, y, z) are three-dimensional coordinates and (α, r, h) are cylindrical coordinates.

[0042] S4. Generate probability density function of each grid: calculate the three-dimensional normal distribution parameters of each grid. The specific calculation method of the three-dimensional normal distribution parameters of the grid is:

[0043]

[0044] Among them, p i is the coordinate of the i-th point in the grid, p q is the mean coordinate, and ∑ is the covariance matrix.

[0045] S5. Single-frame point cloud pre-conversion: The single-frame point cloud to be registered is converted to the inertial navigation coordinate system through the calibration parameters, and then converted to the positioning map coordinate system through the initial posture. The coordinate transformation formula is:

[0046] P'=RP+T

[0047] Among them, P' is the transformed point cloud.

[0048] S6. Assigning a grid to a single-frame point cloud: Find the grid where each point cloud in the single-frame point cloud is located according to the definition in step S3.

[0049] S7. Calculate the positioning score: Calculate the probability density of each point cloud in the single-frame point cloud to be registered based on the normal distribution parameters calculated in step S4, and add the calculated results of each point cloud to obtain the cylindrical coordinate NDT registration score. The cylindrical coordinate NDT registration score is calculated as follows:

[0050]

[0051] score=∑PDF

[0052] Among them, p i is a point in P′.

[0053] S8. Nonlinear optimization solution: Use the Gauss-Newton method to optimize the socre function and find the optimal posture R and optimal position T to maximize the value of socre.

[0054] S9. Iterate to find the optimal solution: Bring the calculated R and T into step S5, and repeat steps S5-S7 until the change in RT is less than the threshold. The final R and T obtained are the results of single-frame point cloud positioning.

[0055] See also Figure 2 Schematic diagram of the cylindrical coordinate grid.

[0056] The basic principles, main features, and advantages of the present invention are shown and described above. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions are merely illustrative of the principles of the present invention. Various changes and modifications may be made to the present invention without departing from the spirit and scope of the present invention. Such changes and modifications are intended to fall within the scope of the present invention. The scope of protection claimed in the present invention is defined by the appended claims and their equivalents.

Claims

1. The NDT single-frame point cloud registration and positioning method based on cylindrical coordinates is characterized by: The steps include: S1. Determine the registration target area: determine the coordinate center O of the cylindrical coordinates; S2. Divide the cylindrical coordinate grid: With the coordinate center O as the center and the laser radar scanning distance as the radius, make a cylinder. Intercept the single-frame point cloud of the positioning map within the cylinder as the registered target point cloud Q. Divide the target point cloud Q into multiple grids according to the three dimensions (α, r, h); S3. Assigning map point clouds to grids: Calculate the corresponding cylindrical coordinates (α, r, h) based on the 3D coordinates (x, y, z) of all points in the target point cloud Q, and then find the corresponding grid; S4. Generate probability density function of each grid: calculate the three-dimensional normal distribution parameters of each grid; S5, single-frame point cloud pre-conversion: convert the single-frame point cloud to be registered into the inertial navigation coordinate system through calibration parameters, and then convert it into the positioning map coordinate system through the initial posture; S6. Assigning grids to single-frame point clouds: Find the grid where each point cloud in the single-frame point cloud is located according to the definition in step S3; S7. Calculate the positioning score: Calculate the probability density of each point cloud in the single-frame point cloud to be registered based on the normal distribution parameters calculated in step S4, and add the results calculated for each point cloud to obtain the cylindrical coordinate NDT registration score; S8, nonlinear optimization solution: Use the Gauss-Newton method to optimize the socre function and find the optimal posture R and optimal position T to maximize the value of socre; S9, iteratively seek the optimal solution: bring the calculated R and T into step S5, and repeat steps S5-S7 until the changes in R and T are less than the threshold. The final R and T obtained are the results of single-frame point cloud positioning; The grid calculation method in step S3 is: h=z Where (x, y, z) are three-dimensional coordinates and (α, r, h) are cylindrical coordinates.

2. The cylindrical coordinate-based NDT single-frame point cloud registration and positioning method according to claim 1, characterized in that: The specific calculation method of the three-dimensional normal distribution parameters of the grid in step S4 is: Among them, p i is the coordinate of the i-th point in the grid, p q is the mean coordinate, and ∑ is the covariance matrix.

3. The cylindrical coordinate-based NDT single-frame point cloud registration and positioning method according to claim 2, characterized in that: In step S5, the coordinate transformation formula is: P'=RP+T Among them, P' is the transformed point cloud.

4. The cylindrical coordinate-based NDT single-frame point cloud registration and positioning method according to claim 3, characterized in that: The calculation method of the cylindrical coordinate NDT registration score in step S7 is: score=∑PDF Among them, p i is a point in P′.

Images