Efficient point cloud positioning initialization method

Abstract

The invention discloses an efficient point cloud positioning initialization method. The positioning initialization method comprises the following steps: extracting feature points from a point cloud map, and establishing feature descriptors for the feature points; extracting feature points from the to-be-matched point cloud and establishing feature descriptors; matching the feature points of the point cloud to be matched with the feature points of the point cloud map to obtain paired feature points; filtering out mismatching points from the matched feature points of the point cloud map; performing first registration on the feature points of the to-be-matched point cloud and the paired point cloud map feature points to obtain initial pose estimation; and on the basis of the initial pose estimation, performing second registration on the to-be-matched point cloud and the point cloud map to obtain accurate initial positioning. According to the invention, the unmanned vehicle or the mobile robot can efficiently carry out positioning initialization without the help of a GNSS (Global Navigation Satellite System).

Description

Technical Field

[0001] The present invention relates to the field of driverless, and particularly to an efficient method for initializing point cloud positioning. Background Art

[0002] An important prerequisite for a driverless vehicle or a mobile robot to perform tasks is to accurately position itself. Among the existing main positioning technologies, positioning based on lidar requires a good initial pose estimate. In an outdoor environment, currently, the Global Navigation Satellite System (GNSS) is generally used to obtain the initial pose estimate. However, in an occluded environment, such as when blocked by high-rise buildings or trees, the GNSS signal is weak or even there is no GNSS signal, and the GNSS module may not be initialized successfully, thus unable to perform the initial pose estimate. In an indoor environment, usually, the initial position and direction of the mobile robot are segmented at a certain resolution with reference to a map, and then the most likely combination of position and direction is found by using a traversal search method to perform the initial pose estimate. This method is very time-consuming and not very practical.

[0003] Therefore, there are defects in the prior art and improvement is needed. Summary of the Invention

[0004] The object of the present invention is to overcome the deficiencies of the prior art and provide an efficient method for initializing point cloud positioning, which can efficiently perform the initial pose estimation of an unmanned vehicle to solve the above problems.

[0005] The technical solution of the present invention is as follows: An efficient method for initializing point cloud positioning includes the following steps:

[0006] Extract feature points from the point cloud map and establish feature descriptors for the feature points; extract feature points from the point cloud to be matched and establish feature descriptors; match the feature points of the point cloud to be matched and the feature points of the point cloud map to obtain paired feature points; filter out mis-matched points from the matched feature points of the point cloud map; perform the first registration on the feature points of the point cloud to be matched and the paired feature points of the point cloud map to obtain an initial pose estimate; on the basis of this initial pose estimate, perform the second registration on the point cloud to be matched and the point cloud map to obtain an accurate initial positioning.

[0007] Preferably, extract feature points from the point cloud map and establish feature descriptors for the feature points. The specific process is as follows:

[0008] Use a point cloud map construction algorithm to construct the required point cloud map; in an offline environment, use a feature point detection algorithm to extract feature points from the point cloud map, establish feature descriptors for the feature points, and construct a map feature point search database.

[0009] Among them, the specific steps for establishing the feature descriptor are as follows:

[0010] Calculate the FPFH (Fast Point Feature Histogram) feature descriptor of the feature points as the first feature descriptor;

[0011] Calculate the coordinate distribution of the adjacent point clouds within a preset radius r of the feature points. Among them, T sub - intervals are used for each of the three coordinate axes of X, Y, and Z to statistically analyze the point cloud coordinate distribution in this direction, and the coordinate distribution histogram described by a 3*T - dimensional array is used as the second feature descriptor;

[0012] Fuse the first feature descriptor and the second feature descriptor as the feature descriptor of the feature points.

[0013] Preferably, extract feature points from the point cloud to be matched and establish feature descriptors. The specific process is as follows:

[0014] Receive a single - frame environmental point cloud scanned by a lidar, filter out the lidar noise, and convert the environmental point cloud from the lidar coordinate system to the vehicle body coordinate system to obtain the point cloud to be matched;

[0015] The calculation formula for converting the point cloud from the lidar coordinate system to the vehicle body coordinate system is:

[0016]

[0017] Among them, R 3×3 and T 3×1 are the extrinsic parameters of the lidar, representing the rotation matrix and translation vector of the lidar coordinate system relative to the vehicle body coordinate system respectively. O represents a matrix of all zeros, [X l , Y l , Z l is the coordinate value of each point in the single - frame environmental point cloud in the lidar coordinate system, and [X, Y, Z] is the coordinate value of each point in the single - frame environmental point cloud in the vehicle body coordinate system.

[0018] Use a feature point detection algorithm to extract feature points from the point cloud to be matched and establish feature descriptors for the feature points. The specific steps are as follows:

[0019] Calculate the FPFH feature descriptor of the feature points as the first feature descriptor;

[0020] Calculate the coordinate distribution of the adjacent point clouds within a preset radius r of the feature points. Among them, T sub - intervals are used for each of the three coordinate axes of X, Y, and Z to statistically analyze the point cloud coordinate distribution in this direction, and the coordinate distribution histogram described by a 3*T - dimensional array is used as the second feature descriptor;

[0021] Fuse the first feature descriptor and the second feature descriptor as the feature descriptor of the feature point.

[0022] Preferably, match the feature points of the point cloud to be matched and the feature points of the point cloud map to obtain paired feature points. The specific process is as follows:

[0023] Traverse the feature points of the point cloud to be matched, and use a search algorithm to find the feature point p of the point cloud to be matched in the database of the map feature points i 's optimal matching feature point p ic and sub-optimal matching feature point p ic* .

[0024] Preferably, filter out the mis-matched points from the matched point cloud map feature points. The specific process is as follows:

[0025] Calculate the distance ratio of the feature point of the point cloud to be matched to the optimal matching feature point and the sub-optimal matching feature point;

[0026] Assign weights to the optimal matching feature point and the sub-optimal matching feature point respectively according to the threshold range where the ratio is located;

[0027] Add the feature points with non-zero weights to the feature point set {P m}, and the feature points with weight 0 are considered mis-matched points;

[0028] Specifically, the calculation formula for weight assignment of feature points is:

[0029]

[0030] where dist(p i , p ic ) is the distance from the feature point p of the point cloud to be matched i to its optimal matching feature point p ic , dist(p i , p ic* ) is the distance from the feature point p of the point cloud to be matched i to its sub-optimal matching feature point p ic* , R dist1 is the first threshold, R dist2 is the second threshold, w1 is the first weight, w b is the second weight, w s is the third weight.

[0031] Before the first registration of the feature points of the point cloud to be matched and the paired point cloud map feature points, perform a second screening of the feature points. The specific steps are as follows:

[0032] For the feature point set {P mPerform clustering to divide the set of feature points into one or more feature point clusters;

[0033] Calculate the priority score of the current feature point cluster according to the number of points in each feature point cluster and the proportion of the optimal matching feature points and the sub - optimal matching feature points;

[0034] Specifically, the calculation formula for the priority score of each feature point cluster is:

[0035] Pri i = a1 * N ti + a2 * N bi / N ti + a3 * N si / N ti ;

[0036] Where N ti is the number of feature points in each feature point cluster, N bi is the number of optimal matching feature points in each feature point cluster, N si is the number of sub - optimal matching feature points in each feature point cluster, and a1, a2, a3 are preset coefficients for each sub - item.

[0037] Sort the priorities of all feature point clusters, and select the feature point clusters with a preset ratio R cluster and add them to the set {C m};

[0038] Calculate the distance from the feature points with weights w1 or w m in each feature point cluster in the set {C b} to the feature points of the paired point cloud to be matched;

[0039] Specifically, the distance calculation formula for the feature point pair is:

[0040] D(q,p)= ∑[(q k - p k ) 2 ;

[0041] Where q is the feature point of the feature point cluster, p is the feature point of the paired point cloud to be matched, q k is the value of the k - th dimension feature of the feature point in the feature point cluster, and p k is the value of the k - th dimension feature of the feature point of the paired point cloud to be matched.

[0042] Sort the distance values between the feature points included in each feature point cluster and the feature points of the paired point cloud to be matched in ascending order, and select the first M feature points with the smallest distances and add them to the subset of each feature point cluster ;

[0043] Preferably, perform first registration on the feature points of the point cloud to be matched and the paired point cloud map feature points to obtain an initial pose estimate. The specific steps are as follows:

[0044] Use a registration algorithm to register the feature points in each subset of the feature point clusters with the feature points of the paired point cloud to be matched, and obtain the pose transformation relationship {R i , T i} between the two sets of feature points. Among them, R i represents the rotation matrix, and T i represents the translation matrix;

[0045] Perform coordinate transformation on all feature points of the point cloud to be matched according to the transformation relationship;

[0046] Specifically, the coordinate transformation formula is:

[0047]

[0048] Among them, is the coordinate of the feature point of the point cloud to be matched in the vehicle body coordinate system, represents the coordinate of the feature point of the point cloud to be matched in the map coordinate system after attitude transformation.

[0049] Calculate the fitness distance from the coordinates of all feature points of the point cloud to be matched after attitude transformation to the coordinates of their paired optimal matching feature points. The smaller the fitness distance value, the higher the corresponding fitness;

[0050] Specifically, the calculation formula for the fitness distance is:

[0051]

[0052] Among them, X si , Y si , Z si are the X-axis, Y-axis, and Z-axis coordinates of the i-th feature point of the point cloud to be matched after attitude transformation respectively, X di , Y di , Z di are the X-axis, Y-axis, and Z-axis coordinates of the optimal matching feature point paired with the i-th feature point of the point cloud to be matched respectively, and N is the total number of feature points of the point cloud to be matched.

[0053] Select the pose transformation relationship corresponding to the subset of the feature point cluster with the smallest fitness distance, that is, the highest fitness, as the initial pose estimate.

[0054] Preferably, on the basis of this initial pose estimate, perform second registration on the point cloud to be matched and the point cloud map to obtain an accurate initial positioning.

[0055] Adopting the above solution, the present invention provides an efficient method for initializing point cloud positioning, which has the following beneficial effects:

[0056] 1. The initial pose of the unmanned vehicle or mobile robot in the map can be accurately calculated, providing a basis for the unmanned vehicle to perform subsequent tasks;

[0057] 2. In outdoor occluded environments or indoor environments, when it is impossible to obtain an initial pose estimate by means of technologies such as GNSS, the unmanned vehicle or mobile robot can still perform positioning initialization, with strong adaptability and a wide range of applications;

[0058] 3. In the case of a large-scale map, the initial pose estimate can still be efficiently carried out, with high efficiency and strong practicability; BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 It is a schematic flow chart of the method for initializing the positioning of the unmanned vehicle of the present invention;

[0060] Figure 2 It is a schematic flow chart of establishing feature descriptors of the present invention;

[0061] Figure 3 It is a schematic flow chart of filtering out mis-matched points from the feature points of the point cloud map that are matched in the present invention;

[0062] Figure 4 It is a schematic flow chart of the second screening of feature points in the present invention;

[0063] Figure 5 It is a schematic flow chart of performing the first registration on the feature points of the point cloud to be matched and the paired feature points of the point cloud map to obtain an initial pose estimate in the present invention;

[0064] Figure 6 It is a schematic diagram of the effect of feature point clustering in the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0065] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0066] Please refer to Figures 1 - 6 , the present invention provides an efficient method for initializing point cloud positioning, including the following steps:

[0067] S1: Extract feature points from the point cloud map and establish feature descriptors for the feature points. The specific process is as follows:

[0068] Construct the required point cloud map using the point cloud map construction algorithm; in the offline environment, use the feature point detection algorithm to extract feature points from the point cloud map, establish feature descriptors for the feature points, and construct a map feature point lookup database. In this embodiment, the Harris corner detection algorithm is used for feature point extraction, and the map feature point lookup database is implemented using KdTree.

[0069] Among them, the specific steps for establishing the feature descriptor are as follows:

[0070] S11: Calculate the FPFH (Fast Point Feature Histogram) feature descriptor of the feature point as the first feature descriptor;

[0071] S12: Calculate the coordinate distribution of the adjacent point cloud within a preset radius r of the feature point. Among them, T sub-intervals are used for each of the three coordinate axes of X, Y, and Z to statistically analyze the point cloud coordinate distribution in this direction, and the coordinate distribution histogram described by a 3*T-dimensional array is used as the second feature descriptor. In this embodiment, r = 0.3m and T = 10.

[0072] S13: Fuse the first feature descriptor and the second feature descriptor as the feature descriptor of the feature point.

[0073] S2: Extract feature points from the point cloud to be matched and establish feature descriptors. The specific process is as follows:

[0074] S21: Receive a single-frame environmental point cloud scanned by the lidar, filter out the lidar noise, and convert the environmental point cloud from the lidar coordinate system to the vehicle body coordinate system to obtain the point cloud to be matched;

[0075] The calculation formula for converting the point cloud from the lidar coordinate system to the vehicle body coordinate system is:

[0076]

[0077] Among them, R 3×3 and T 3×1 are the external parameters of the lidar, representing the rotation matrix and translation vector of the lidar coordinate system relative to the vehicle body coordinate system respectively. O represents a matrix of all zeros, [X l , Y l , Z l are the coordinate values of each point in the single-frame environmental point cloud in the lidar coordinate system, and [X, Y, Z] are the coordinate values of each point in the single-frame environmental point cloud in the vehicle body coordinate system. In this embodiment, R 3×3= [[0.53987, -0.839339, 0.0636353], [0.840798, 0.541309, 0.00660395], [-0.0399893, 0.0499392, 0.997951]], T 3×1 = [1.22, 0.03, 0.785] T .

[0078] S22: Use the feature point detection algorithm to extract feature points from the point cloud to be matched, and establish feature descriptors for the feature points. In this embodiment, the Harris corner detection algorithm is used for feature point extraction. The specific steps for establishing the feature descriptor are the same as those in S11 - S13.

[0079] S3: Match the feature points of the point cloud to be matched and the feature points of the point cloud map to obtain paired feature points. The specific process is as follows:

[0080] Traverse the feature points of the point cloud to be matched, and use the search algorithm to find the feature point p of the point cloud to be matched in the database of the map feature points i 's optimal matching feature point p ic and sub - optimal matching feature point p ic* . In this embodiment, the BBF (Best Bin First) search algorithm is used to find the optimal matching feature point and sub - optimal matching feature point of the feature points of the point cloud to be matched.

[0081] S4: Filter out the mis - matched points from the matched point cloud map feature points. The specific process is as follows:

[0082] S41: Calculate the distance ratio of the feature points of the point cloud to be matched to the optimal matching feature point and sub - optimal matching feature point;

[0083] S42: Assign weights to the optimal matching feature point and sub - optimal matching feature point respectively according to the threshold range where the ratio is located;

[0084] S43: Add the feature points with non - zero weights to the feature point set {P m}, and the feature points with weight 0 are considered mis - matched points;

[0085] Specifically, the calculation formula for weight assignment of feature points is:

[0086]

[0087] where dist(p i , p ic ) is the distance from the feature point p of the point cloud to be matched i to its optimal matching feature point p ic , dist(p i , pic* ) The distance from the feature point p of the point cloud to be matched i to the second-best matching feature point p ic* is R dist1 is the first threshold, R dist2 is the second threshold, w1 is the first weight, w b is the second weight, w s is the third weight. In this embodiment, R dist1 = 0.25, R dist2 = 0.67, w1 = 1, w b = 0.65, w s = 0.35.

[0088] S5: Before the first registration of the feature points of the point cloud to be matched and the paired point cloud map feature points, the feature points are screened for the second time. The specific steps are as follows:

[0089] S51: Cluster the set of feature points {P m}, and divide the set of feature points into one or more feature point clusters. In this embodiment, Euclidean clustering is used to cluster the set of feature points.

[0090] S52: Calculate the priority score of the current feature point cluster according to the number of points in each feature point cluster and the proportion of the optimal matching feature points and the second-best matching feature points;

[0091] Specifically, the formula for calculating the priority score of each feature point cluster is:

[0092] Pri i = a1 * N ti + a2 * N bi / N ti + a3 * N si / N ti ;

[0093] where N ti is the number of feature points in each feature point cluster, N bi is the number of optimal matching feature points in each feature point cluster, N si is the number of second-best matching feature points in each feature point cluster, and a1, a2, and a3 are preset coefficients for each sub-item. In this embodiment, a1 = 0.6, a2 = 0.3, and a3 = 0.1.

[0094] S53: Sort the priorities of all feature point clusters, and select the feature point clusters with a preset ratio R cluster in descending order of the priority score and add them to the set {C m}. In this embodiment, R cluster = 50%.

[0095] S54: Calculate the distance from each feature point cluster with weights of w1 or w m in the set {C b} to the feature points of its paired point cloud to be matched;

[0096] Specifically, the distance calculation formula for the feature point pair is:

[0097] D(q,p) = ∑[(q k - p k ) 2 ;

[0098] where q is the feature point of the feature point cluster, p is the feature point of its paired point cloud to be matched, q k is the value of the k-th dimension feature of the feature point in the feature point cluster, and p k is the value of the k-th dimension feature of the feature point of its paired point cloud to be matched.

[0099] S55: Sort the distance values between the feature points included in each feature point cluster and the feature points of the paired point cloud to be matched in ascending order, and select the first M feature points with the smallest distances to add to the subset of each feature point cluster. In this embodiment, M = 20.

[0100] S6: Perform the first registration on the feature points of the point cloud to be matched and the paired point cloud map feature points to obtain an initial pose estimate. The specific steps are as follows:

[0101] S61: Use a registration algorithm to register the feature points in the subset of each feature point cluster with the feature points of its paired point cloud to be matched to obtain the pose transformation relationship {R i , T i} between the two feature point sets. Among them, R i represents the rotation matrix, and T i represents the translation matrix. In this embodiment, the ICP algorithm is used for registration.

[0102] S62: Perform coordinate transformation on all feature points of the point cloud to be matched according to the transformation relationship;

[0103] Specifically, the coordinate transformation formula is:

[0104]

[0105] Among them, is the coordinate of the feature point of the point cloud to be matched in the vehicle body coordinate system, represents the coordinate of the feature point of the point cloud to be matched in the map coordinate system after pose transformation.

[0106] S63: Calculate the fitness distance between the coordinates of all feature points of the point cloud to be matched after pose transformation and the coordinates of their paired optimal matching feature points. The smaller the fitness distance value, the higher the corresponding fitness;

[0107] Specifically, the calculation formula for the fitness distance is:

[0108]

[0109] where X si , Y si , Z si are the X-axis, Y-axis, and Z-axis coordinates of the i-th feature point of the point cloud to be matched after pose transformation, respectively, and X di , Y di , Z di are the X-axis, Y-axis, and Z-axis coordinates of the optimal matching feature point paired with the i-th feature point of the point cloud to be matched, respectively, and N is the total number of feature points of the point cloud to be matched.

[0110] S64: Select the pose transformation relationship corresponding to the subset of the feature point cluster with the smallest fitness distance, that is, the highest fitness, as the initial pose estimate.

[0111] S7: Based on this initial pose estimate, perform a second registration on the point cloud to be matched and the point cloud map to obtain an accurate initial positioning. In this embodiment, the normal distribution transformation algorithm is used for the second registration.

[0112] In summary, an efficient method for initializing point cloud positioning provided by the present invention extracts feature points from the point cloud map offline and establishes feature descriptors for the feature points; extracts feature points from the point cloud to be matched and establishes feature descriptors; then matches the feature points of the point cloud to be matched and the feature points of the point cloud map to obtain paired feature points; filters out mis-matched points from the matched feature points of the point cloud map; then performs a first registration on the feature points of the point cloud to be matched and the paired feature points of the point cloud map to obtain an initial pose estimate; finally, based on this initial pose estimate, perform a second registration on the point cloud to be matched and the point cloud map to obtain an accurate initial positioning, providing a basis for the unmanned vehicle to perform subsequent tasks; in addition, without relying on technologies such as GNSS to obtain an initial pose estimate, the unmanned vehicle or mobile robot can also perform positioning initialization, with strong adaptability and wide application range; in the case of a large map scale, an initial pose estimate can still be efficiently obtained, with high efficiency and strong practicality.

[0113] The above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. An efficient method for initializing point cloud localization, characterized in that, It includes the following steps: Extract feature points from the point cloud map and establish feature descriptors for the feature points; extract feature points from the point cloud to be matched and establish feature descriptors; match the feature points of the point cloud to be matched with the feature points of the point cloud map to obtain paired feature points; filter out mis-matched points from the matched point cloud map feature points; perform a first registration on the feature points of the point cloud to be matched and the paired point cloud map feature points to obtain an initial pose estimate; based on this initial pose estimate, perform a second registration on the point cloud to be matched and the point cloud map to obtain an accurate initial positioning.

2. An efficient point cloud localization initialization method according to claim 1, characterized in that The specific process of extracting feature points from the point cloud map and establishing feature descriptors for the feature points is as follows: Use the point cloud map construction algorithm to construct the required point cloud map; in an offline environment, use the feature point detection algorithm to extract feature points from the point cloud map, establish feature descriptors for the feature points, and construct a map feature point search database; Among them, the specific steps of establishing the feature descriptor are: Calculate the FPFH feature descriptor of the feature point as the first feature descriptor; Calculate the coordinate distribution of the adjacent point cloud within a preset radius r of the feature point, where T sub-intervals are used in each of the three coordinate axes of X, Y, and Z to statistically analyze the point cloud coordinate distribution in this direction, and use the coordinate distribution histogram described by a 3*T-dimensional array as the second feature descriptor; Fuse the first feature descriptor and the second feature descriptor as the feature descriptor of the feature point.

3. An efficient point cloud localization initialization method according to claim 1, characterized in that, The specific process of extracting feature points from the point cloud to be matched and establishing feature descriptors is as follows: Receive the single-frame environmental point cloud scanned by the lidar, filter out the lidar noise, and convert the environmental point cloud from the lidar coordinate system to the vehicle body coordinate system to obtain the point cloud to be matched; The calculation formula for converting the point cloud from the lidar coordinate system to the vehicle body coordinate system is: Among them, R 3×3 and T 3×1 are the extrinsic parameters of the lidar, representing the rotation matrix and translation vector of the lidar coordinate system relative to the vehicle body coordinate system respectively. O represents a zero matrix, and [X l , Y l , Z l are the coordinate values of each point in a single-frame environmental point cloud in the lidar coordinate system, and [X, Y, Z] are the coordinate values of each point in a single-frame environmental point cloud in the vehicle body coordinate system; Use the feature point detection algorithm to extract feature points from the point cloud to be matched and establish feature descriptors for the feature points; the specific steps are: Calculate the FPFH feature descriptor of the feature point as the first feature descriptor; Calculate the coordinate distribution of the adjacent point cloud within a preset radius r of the feature point, where T sub-intervals are used in each of the three coordinate axes of X, Y, and Z to statistically analyze the point cloud coordinate distribution in this direction, and use the coordinate distribution histogram described by a 3*T-dimensional array as the second feature descriptor; Fuse the first feature descriptor and the second feature descriptor as the feature descriptor of the feature point.

4. An efficient point cloud localization initialization method according to claim 1, characterized in that The specific process of matching the feature points of the point cloud to be matched with the feature points of the point cloud map to obtain paired feature points is: Traverse the feature points of the point cloud to be matched, and use a search algorithm to find the feature point p of the point cloud to be matched in the database of map feature points i of the optimal matching feature point p ic and the sub-optimal matching feature point p ic* .

5. An efficient point cloud localization initialization method according to claim 1, characterized in that The specific process of filtering out mis-matched points from the matched point cloud map feature points is: Calculate the distance ratio of the feature points of the point cloud to be matched to the optimal matching feature point and the sub-optimal matching feature point; Assign weights to the optimal matching feature point and the sub-optimal matching feature point according to the threshold range where the ratio is located; Add the feature points with non-zero weights to the set of feature points {P m}, and the feature points with a weight of 0 are considered to be mismatched points; Specifically, the calculation formula for weight assignment of feature points is: Among them, dist(p i , p ic ) is the distance from the feature point p i of the point cloud to be matched to its optimal matching feature point p ic , dist(p i , p ic* ) is the distance from the feature point p i of the point cloud to be matched to its second-best matching feature point p ic* , R dist1 is the first threshold, R dist2 is the second threshold, w1 is the first weight, w b is the second weight, w s is the third weight.

6. An efficient point cloud localization initialization method according to claim 5, characterized in that Before performing the first registration on the feature points of the point cloud to be matched and the paired point cloud map feature points, the specific steps for the second screening of the feature points are: Cluster the set of feature points {P m}, and divide the set of feature points into one or more feature point clusters; Calculate the priority score of the current feature point cluster according to the number of points in each feature point cluster and the proportion of the optimal matching feature points and the sub-optimal matching feature points; Specifically, the calculation formula for the priority score of each feature point cluster is: Pri i = a1 * N ti + a2 * N bi / N ti + a3 * N si / N ti ; where N ti is the number of feature points in each feature point cluster, and N bi is the number of optimal matching feature points in each feature point cluster, and N si is the number of sub-optimal matching feature points in each feature point cluster, and a1, a2, and a3 are preset coefficients for each sub-item; Sort the priorities of all feature point clusters, and select the feature point clusters with a preset ratio R in the order from the highest to the lowest priority score cluster to add to the set {C m}; Calculate the distance from each feature point cluster in the set {C m} with a weight of w1 or w b to the feature point of its paired point cloud to be matched; Specifically, the distance calculation formula for the feature point pair is: D(q,p) = ∑[(q k - p k ) 2 ; where q is the feature point of the feature point cluster, p is the feature point of the point cloud to be matched paired with it, and q k is the value of the k-th dimension feature of the feature point in the feature point cluster, and p k is the value of the k-th dimension feature of the feature point of the point cloud to be matched paired with it; Sort the distance values between the feature points included in each cluster of feature points and the feature points of the paired point cloud to be matched in ascending order, and select the first M feature points with the smallest distances to add to the subset of each cluster of feature points in.

7. An efficient point cloud localization initialization method according to claim 6, characterized in that The specific steps for performing the first registration on the feature points of the point cloud to be matched and the feature points of the paired point cloud map to obtain the initial pose estimation are: Using a registration algorithm, register the feature points in each subset of the feature point clusters with the feature points of the point cloud to be matched that are paired with them, and obtain the pose transformation relationship {R i , T i} between the two sets of feature points; where R i represents the rotation matrix, and T i represents the translation matrix; Perform coordinate transformation on all feature points of the point cloud to be matched according to the transformation relationship; Specifically, the coordinate transformation formula is: Among them, is the coordinate of the feature point of the point cloud to be matched in the vehicle body coordinate system, represents the coordinate of the feature point of the point cloud to be matched in the map coordinate system after attitude transformation; Calculate the fitness distance from the coordinates of all feature points of the point cloud to be matched after the pose transformation to the coordinates of their paired optimal matching feature points; the smaller the fitness distance value, the higher the corresponding fitness; Specifically, the calculation formula for the fitness distance is: Among them, X si , Y si , Z si are the X-axis, Y-axis, and Z-axis coordinates of the i-th feature point of the point cloud to be matched after pose transformation, respectively. X di , Y di , Z di are the X-axis, Y-axis, and Z-axis coordinates of the optimal matching feature point paired with the i-th feature point of the point cloud to be matched, respectively. N is the total number of feature points of the point cloud to be matched; Select the pose transformation relationship corresponding to the subset of the feature point cluster with the smallest fitness distance, that is, the highest fitness, as the initial pose estimation.

8. An efficient point cloud localization initialization method according to claim 7, characterized in that, Based on this initial pose estimation, perform the second registration on the point cloud to be matched and the point cloud map to obtain an accurate initial positioning.

Images