A global positioning method for large-scale scenes based on multivariate statistical information

Abstract

The present invention provides a global positioning method for a large-scale scene based on multivariate statistical information. First, a three-dimensional point cloud map of the scene is grid-divided to obtain a three-dimensional grid map; then, the point cloud falling into each three-dimensional grid in the three-dimensional grid map is counted to obtain a point cloud set of each three-dimensional grid, and the maximum frequency of the point cloud in all point cloud sets is counted to calculate the mean and variance of each three-dimensional grid; then, each three-dimensional grid is refined in the z direction and calculated as the score after the point cloud to be matched hits the target grid; finally, point cloud registration and positioning are performed. The present invention uses multivariate statistical information to describe the characteristics of the three-dimensional scene, divides the scene into grids of different resolution sizes, and uses a global search method to achieve global positioning.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a large-scale scene global positioning method based on multivariate statistical information. Background Art

[0002] In the field of autonomous driving, positioning is an important infrastructure. It generally uses high-precision inertial navigation to globally anchor the vehicle's position, and cooperates with laser radar or cameras, using SLAM (simultaneous localization and mapping) technology based on high-precision maps to achieve global positioning of multi-sensor fusion. On the one hand, it provides "where" information for unmanned vehicles; on the other hand, it provides assistance for perceiving the environment, thereby helping vehicles make decisions and plans. In particular, in the case of a lack of hardware equipment, high-precision inertial navigation equipment cannot be equipped, and global positioning based on high-precision maps plays a vital role.

[0003] Existing global positioning generally includes two solutions: one is global positioning based on the global positioning system or the Beidou system; the other is multi-sensor fusion global positioning based on high-precision maps. Multi-sensor fusion global positioning based on high-precision maps requires global initialization positioning of a large range of scenes under the condition of large initial errors. It usually uses vehicle-mounted sensors, such as lidar, to perceive the surrounding environment, and then aligns it with the high-precision map to achieve global high-precision positioning. Gradient descent-based methods such as ICP, GICP, NDT, etc. are often used; or global search-based methods such as CSM are used.

[0004] Solutions such as ICP, GICP, and NDT based on gradient descent methods need to provide an initial value close to the true value in order to successfully align, otherwise the final result will fall into the local minimum. Methods such as CSM based on global search usually do not fall into the local minimum problem, but the feature description of the environment is not rich enough, and are often used in 2D or 2.5D scenes. They are easily affected by noise in the environment, resulting in low robustness and easy registration failure. Summary of the invention

[0005] The technical problem to be solved by the present invention is: in order to overcome the deficiencies in the prior art, the present invention provides a large-scale scene global positioning method based on multivariate statistical information to solve the global alignment problem in a large-scale three-dimensional scene.

[0006] The technical solution adopted by the present invention to solve the technical problem is: a global positioning method for a large-scale scene based on multivariate statistical information, using a multi-line laser radar to achieve global positioning with a large initial error. Multivariate statistical information is used to describe the characteristics of the three-dimensional scene, the scene is divided into grids of different resolutions, and a global search method is used to achieve global positioning.

[0007] The specific steps include:

[0008] Step 1: Divide the three-dimensional point cloud map of the scene into grids in the x, y, and z directions to obtain a three-dimensional grid map;

[0009] Step 2: Count the point clouds that fall into each three-dimensional grid in the three-dimensional grid map, and obtain the point cloud set V of each three-dimensional grid i , then V i ={p1,p2,…,p ni}, and count the maximum frequency of point clouds in all point cloud sets max p , then max p =max{n1,n2,…,n k}; At the same time, Gaussian fitting is performed on each three-dimensional grid in the z direction to calculate the mean and variance of each three-dimensional grid; where V i Represents the point cloud set of the i-th three-dimensional grid, n i Represents the set V i The number of points in the 3D grid. Different sets have different numbers of points. i represents the sequence number of the 3D grid, i = 1, 2, ... k, k represents the number of grids divided into the 3D point cloud map.

[0010] Among them, frequency, mean and variance all represent the characteristics of the three-dimensional grid map. Frequency is a distribution feature, which represents the probability that the points scanned by the multi-line laser radar fall into the three-dimensional grid map; mean and variance are statistical features, which represent the similarity between the points scanned by the multi-line laser radar and the points in the three-dimensional grid when they fall into the three-dimensional grid map.

[0011] Step 3: Each 3D grid is transformed in the z direction with a certain resolution Z res Subdivided, and Z res <R z , the score after clicking on the subdivided grid is calculated according to the following formula:

[0012]

[0013] The subscript i represents the number of the three-dimensional grid, i = 1, 2, ... k; j represents the number of the grid after the i-th grid is subdivided in the z direction, α represents the constant coefficient; z represents the height, that is, the center height of each grid after being divided by Zres; R z Indicates the resolution in the z direction when the 3D point cloud map is gridded; Z res Indicates the resolution in the z direction of each 3D grid refinement time; μ i represents the mean of the three-dimensional grid with sequence number i; σ i Indicates the variance of the three-dimensional grid with sequence number i.

[0014] Among them, the refinement means that R z Z res The size is further subdivided, and then R z The Gaussian model and probability model are used to calculate the Z of each corresponding grid. res score.

[0015] It is the score after a single click in the point cloud to be matched hits the target grid. The divided grids are fitted with a univariate Gaussian and the point cloud density information is counted to achieve multivariate information fusion feature expression.

[0016] Among them, Step 1 is the first grid division, and Step 3 is based on the first grid division, and then refines the division in the z direction, that is, the second grid division. After this step, the three-dimensional space high-precision point cloud map can be divided into a series of three-dimensional grids, and the score of the point cloud to be matched after hitting the target grid is calculated.

[0017] Step 4: Search the point cloud to be registered according to the global search method of CSM, and select the transformation with the highest total score as the optimal result, that is, the final positioning result. The purpose of this step is to match the point cloud to the map. Search for the best transformation from the point cloud to the map in the set solution space, which includes translation and rotation. That is, the laser radar is translated and rotated so that the current point cloud and the map can accurately overlap. The process of finding translation and rotation is the search. The translation and rotation experienced from the beginning to the place where the most overlap is found is the optimal transformation, which should be three degrees of freedom (x, y, yaw).

[0018] The present invention adopts a multi-scale distribution feature and statistical feature to characterize the grid map of the scene. The multi-resolution and multi-level grid map can compress the point cloud map data on the one hand. Secondly, compared with the single-level grid map, the expression of environmental characteristics is more reasonable and the algorithm is more robust. Compared with semantic features, the use of semantic features requires semantic segmentation of the scene, which consumes more computing power. Secondly, the semantic information after segmentation still needs to be reduced in data dimension. Therefore, the calculation speed is slower than that of this solution.

[0019] The global search method of CSM is an existing technical solution, which is implemented by using a branch and bound algorithm in a multi-resolution form. It is not the innovative point that the present invention claims to protect, so it will not be described in detail here.

[0020] Furthermore, the specific process of grid division in Step 1 is as follows:

[0021] Step 1.1: Determine the resolution R in the x, y, and z directions respectively x , R y , R z ; Among them, the resolution R x , R y , R z To form a three-dimensional grid, the resolution of the divisions in each direction may not be the same;

[0022] Step 1.2: Determine the number of grids k to be divided according to the resolution and size of the 3D point cloud map;

[0023] Step 1.3: According to the resolution and the number of grids k, the three-dimensional point cloud map in the scene is grid-divided to obtain a three-dimensional grid map.

[0024] Further, in Step 1, a multi-line laser radar is used to obtain a three-dimensional point cloud of the scene, and a three-dimensional point cloud map of the scene is constructed based on the three-dimensional point cloud. Preferably, the three-dimensional point cloud map is constructed in advance, and the constructed three-dimensional point cloud map of the scene can be directly applied to the positioning method of the present invention.

[0025] Furthermore, in Step 3, the score of clicking on each subdivided grid is calculated in advance and combined into a score table. In this way, the three-dimensional point cloud map is pre-divided into a three-dimensional score lookup table. In subsequent calculations, the table lookup method can be directly used to accelerate the calculation. The grid of the present invention stores a probability distribution description of the features in the grid (single Gaussian fitting, in fact, multi-core Gaussian fitting is also possible) and a statistical feature. For calculating the score: after refining the grid, the height value of each grid center is used to replace all the points that may fall on this grid. The score is calculated in advance by the height value of the grid center. In the subsequent search process, there is no need to use a complex algorithm to calculate the actual score of each laser radar point falling on the corresponding grid. Calculating a score for each refined grid in advance can speed up the calculation and increase the speed of positioning.

[0026] Technical terms:

[0027] CSM is the abbreviation of Correlative Scan Matching, correlation scan matching.

[0028] SLAM is the abbreviation of Simultaneous Localization And Mapping.

[0029] ICP is the abbreviation of Iterative Closest Point, which means iterative closest point.

[0030] GICP is the abbreviation of Generalized ICP, generalized iterative closest point.

[0031] NDT is the abbreviation of Normal Distributions Transform.

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

[0033] (1) A three-dimensional point cloud is composed of a series of points. Points are three-dimensional and occupy a large amount of storage. After being divided into a grid map, only the scores of the grids need to be stored. First, the number of grids is several orders of magnitude less than the points. Second, what is stored is a series of scores for each grid, which is only one-dimensional. Therefore, this positioning method compresses the three-dimensional point cloud map to reduce the storage of data.

[0034] (2) Compared with two-dimensional grid division, the present invention divides the three-dimensional point cloud data into grids in the x, y, and z directions, retaining more three-dimensional feature information, making the three-dimensional features more complete, and effectively improving the positioning accuracy and success rate.

[0035] (3) The Gaussian model is used to fit each grid to extract its feature information. The density information of the points in each grid is also taken into account. Multivariate statistical information such as frequency and Gaussian fitting is used to express the point cloud features, which is more robust.

[0036] (4) This solution can achieve global positioning within a range of 20 meters, taking milliseconds. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0038] Figure 1 It is a flow chart of the global positioning method of the present invention.

[0039] Figure 2 It is a schematic diagram of the grid division of the three-dimensional point cloud map.

[0040] Figure 3 This is a schematic diagram of the global positioning and registration results based on the multi-distance statistical information. DETAILED DESCRIPTION

[0041] The present invention will now be described in detail with reference to the accompanying drawings. This figure is a simplified schematic diagram, which only illustrates the basic structure of the present invention in a schematic manner, and therefore only shows the components related to the present invention.

[0042] like Figure 1 As shown, a large-scale scene global positioning method based on multivariate statistical information of the present invention comprises the following steps:

[0043] Step 1: Rough grid division

[0044] A multi-line laser radar is used to obtain a 3D point cloud of the scene, and a 3D point cloud map of the scene is constructed based on the 3D point cloud. The 3D point cloud map of the scene is grid-divided in the x, y, and z directions to obtain a 3D grid map; Figure 2 As shown in the figure, the specific process of grid division is:

[0045] Step 1.1: Determine the resolution R in the x, y, and z directions respectively x , R y , R z ; The resolution R x , R y , R z To form a three-dimensional grid, the resolution of the divisions in each direction may not be the same;

[0046] When determining the resolution in the xoy plane, the resolutions in the x, y, and z directions can be the same or different. If the resolutions in the three dimensions remain different, it is relatively more flexible.

[0047] Step 1.2: Determine the number of grids k to be divided according to the resolution and size of the 3D point cloud map;

[0048] Step 1.3: According to the resolution and the number of grids k, the three-dimensional point cloud map in the scene is grid-divided to obtain a three-dimensional grid map.

[0049] The three-dimensional point cloud map in this embodiment may be constructed in advance, and the constructed three-dimensional point cloud map of the scene may be directly applied to the positioning method of the present invention, or it may be constructed according to a specific scene when used.

[0050] Step 2: Feature calculation

[0051] Count the point clouds that fall into each three-dimensional grid in the three-dimensional grid map, and obtain the point cloud set V of each three-dimensional grid i ,but And count the maximum frequency of point clouds in all point cloud sets max p , then max p =max{n1,n2,…,n k}; At the same time, Gaussian fitting is performed on each three-dimensional grid in the z direction to calculate the mean and variance of each three-dimensional grid; where V i Represents the point cloud set of the i-th three-dimensional grid, n i Represents the set V i The number of points in the 3D grid. Different sets have different numbers of points. i represents the sequence number of the 3D grid, i = 1, 2, ... k, k represents the number of grids divided into the 3D point cloud map.

[0052] Among them, frequency, mean and variance all represent the characteristics of the three-dimensional grid map. Frequency is a distribution feature, which represents the probability that the points scanned by the multi-line laser radar fall into the three-dimensional grid map; mean and variance are statistical features, which represent the similarity between the points scanned by the multi-line laser radar and the points in the three-dimensional grid when they fall into the three-dimensional grid map.

[0053] Step 3: Grid refinement in the z direction

[0054] like Figure 2 As shown, each 3D grid is divided into two parts in the z direction with a certain resolution Z res Subdivided, and Z res <R z , the score after clicking on the subdivided grid is calculated according to the following formula:

[0055]

[0056] The subscript i represents the sequence number of the three-dimensional grid, i = 1, 2, ... k, and j represents the sequence number of the grid after the i-th grid is subdivided in the z direction. α represents the constant coefficient, z represents the height, R z Indicates the resolution in the z direction when the 3D point cloud map is gridded. res Indicates the resolution in the z direction of each 3D grid refinement time segment, μ i represents the mean of the three-dimensional grid with sequence number i, σ i Represents the variance of the three-dimensional grid with sequence number i;

[0057] It is the score after a single click in the point cloud to be matched hits the target grid. The divided grids are fitted with a univariate Gaussian and the point cloud density information is counted to achieve multivariate information fusion feature expression.

[0058] Among them, Step 1 is the first grid division, and Step 3 is based on the first grid division, and then refines the division in the z direction, that is, the second grid division. After this step, the three-dimensional space high-precision point cloud map can be divided into a series of three-dimensional grids, and the score of the point cloud to be matched after hitting the target grid is calculated.

[0059] The score of each subdivided grid can be calculated in advance and combined into a score table. In this way, the three-dimensional point cloud map is pre-divided into a three-dimensional score lookup table. In subsequent calculations, the table lookup method can be directly used to speed up the calculation.

[0060] Step 4: Point cloud registration and positioning

[0061] The point cloud to be registered is searched according to the global search method of CSM, and the transformation with the highest total score is selected as the optimal result, that is, the final positioning result.

[0062] The global search method of CSM is an existing technical solution, which is implemented by using a branch and bound algorithm in a multi-resolution form. It is not the innovative point that the present invention claims to protect, so it will not be described in detail here.

[0063] The global positioning method based on multivariate statistical information of the present invention is used to process the point cloud map to realize the registration of the laser radar point cloud and the map. The registration result is as follows: Figure 3 As shown, the white point cloud is a high-precision vector map, the yellow point cloud is the initial point cloud to be matched, and the red point cloud is the point cloud after registration. The red point cloud can be completely matched with the map, and the yellow point cloud has no match, just to show that under huge rotation and displacement errors, this solution can also effectively achieve the matching of point cloud and map.

[0064] Based on the above ideal embodiments of the present invention, the relevant staff can make various changes and modifications without departing from the scope of the present invention through the above description. The technical scope of this invention is not limited to the contents of the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A global positioning method for a large-scale scene based on multivariate statistical information, characterized by: The following steps are involved: Step 1: Divide the three-dimensional point cloud map of the scene into grids in the x, y, and z directions to obtain a three-dimensional grid map; Step 2: Count the point clouds that fall into each three-dimensional grid in the three-dimensional grid map, and obtain the point cloud set V of each three-dimensional grid i ,but And count the maximum frequency of point clouds in all point cloud sets max p , then max p =max{n1,n2,…,n k }; At the same time, Gaussian fitting is performed on each three-dimensional grid in the z direction to calculate the mean and variance of each three-dimensional grid; Among them, V i Represents the point cloud set of the i-th three-dimensional grid, n i Represents the set V i The number of points in , i represents the sequence number of the three-dimensional grid, i = 1, 2, ... k, k represents the number of grids divided into three-dimensional point cloud maps; Step 3: Each 3D grid is transformed in the z direction with a resolution of Z res Subdivided, and Z res <R z , the score after clicking on the subdivided grid is calculated according to the following formula: The subscript i represents the sequence number of the three-dimensional grid, i = 1, 2, .... k; j represents the sequence number of the grid after the i-th grid is subdivided in the z direction, α represents the constant coefficient; z represents the height; R z Indicates the resolution in the z direction when the 3D point cloud map is gridded; Z res Indicates the resolution in the z direction when each 3D grid is finely divided; μ i represents the mean of the three-dimensional grid with sequence number i; σ i Represents the variance of the three-dimensional grid with sequence number i; It is the score after a single click in the point cloud to be matched hits the target grid; Step 4: Search the point cloud to be registered according to the global search method of CSM, and select the transformation with the highest total score as the optimal result, that is, the final positioning result.

2. The method for global positioning of a large-scale scene based on multivariate statistical information as claimed in claim 1, characterized in that: The specific process of grid division in Step 1 is: Step 1.1: Determine the resolution R in the x, y, and z directions respectively x , R y , R z ; Step 1.2: Determine the number of grids k to be divided according to the resolution and size of the 3D point cloud map; Step 1.3: According to the resolution and the number of grids k, the three-dimensional point cloud map in the scene is grid-divided to obtain a three-dimensional grid map.

3. The method for global positioning of a large-scale scene based on multivariate statistical information as claimed in claim 1, characterized in that: In Step 1, a multi-line laser radar is used to obtain a three-dimensional point cloud of the scene, and a three-dimensional point cloud map of the scene is constructed based on the three-dimensional point cloud.

4. The method for global positioning of a large-scale scene based on multivariate statistical information as claimed in claim 1, characterized in that: In Step 3, the score of each subdivided grid is calculated in advance and combined into a score table. In this way, the three-dimensional point cloud map is pre-divided into a three-dimensional score lookup table, and the table lookup method is directly used in subsequent calculations to accelerate the calculation.

Images