Laser radar point-to-line weighted matching method based on edge point quality evaluation

Abstract

The invention relates to a laser radar point-to-line weighted matching method based on edge point quality evaluation, and belongs to the technical field of measurement. The method solves the problems that in traditional point-to-line matching, due to the fact that the quality of edge points is not evaluated, the pose estimation precision is low, and the robustness of an unstructured scene is poor. According to the technical scheme, a preprocessing module screens candidate edge points through projection point cloud, target segmentation and roughness calculation; the position judgment module classifies the candidate points as middle, left or right edge points according to the category consistency of the adjacent points; the direction consistency judgment module calculates a direction standard deviation and screens effective edge points; and the dynamic weighting module allocates weights according to an exponential attenuation function, and optimizes the pose through weighting residual errors. The method has the advantages that the pose estimation precision is remarkably improved, the weak structure environment adaptability is enhanced, the calculation efficiency is optimized, and redundant iteration is avoided.

Description

Technical Field

[0001] The present invention belongs to the field of measurement technology and relates to a laser radar point-to-line weighted matching method based on edge point quality assessment. Background Art

[0002] Lidar odometry is one of the mainstream technical approaches for achieving simultaneous localization and mapping (SLAM), and is widely used in fields such as intelligent driving and mobile robotics. The LOAM (Lidar Odometry And Mapping) algorithm, a classic example, has become a benchmark for feature-matching-based Lidar odometry due to its strong real-time performance and clear structure. Its core process consists of two steps: first, edge feature points and surface feature points are filtered based on the curvature of the point cloud; second, inter-frame pose transformation estimation is achieved by minimizing the geometric distances from edge points to lines and surface points to surfaces. Most subsequent improved algorithms have continued this process, relying on roughness to extract feature points and minimizing distance residuals.

[0003] However, these methods only select feature points based on roughness without further evaluating their quality. This results in a large number of low-quality edge points being matched, significantly impacting pose estimation accuracy. Edge points are often distributed across the surfaces of various objects and are susceptible to interference from structural type and sampling noise, making them particularly sensitive to the matching process. For example, edge points on grass or cylindrical surfaces are prone to introducing matching errors due to their ambiguous geometric representation.

[0004] To improve the above problems, existing research has proposed improvement strategies. A 2021 study, "EnhanceAccuracy:Sensitivity and Uncertainty Theory in LiDAR Odometry and Mapping," introduced sensitivity and uncertainty theory to analyze edge point residuals after initial matching. However, it did not evaluate the quality characteristics of edge points before matching, resulting in increased computational time and insufficient robustness in weakly structured environments. Edge points of different qualities contribute significantly to the residuals, and low-quality points even produce negative interference. Therefore, there is an urgent need to dynamically evaluate the quality of edge points before matching and optimize the residuals based on the quality weight to improve system accuracy and adaptability. Summary of the Invention

[0005] In view of this, the purpose of the present invention is to provide a laser radar point-to-line weighted matching method based on edge point quality assessment. The method includes a preprocessing module, a position judgment module, a direction consistency judgment module, and a dynamic weighting module. Compared with the existing point-to-line matching method, the present invention introduces an edge point quality assessment mechanism, which dynamically adjusts its weight in residual optimization based on indicators such as the position characteristics and direction consistency of the edge points, so that high-quality edge points contribute more to the matching process, while the influence of low-quality edge points is effectively suppressed. While improving the overall matching accuracy, this strategy also enhances the robustness of the system in unstructured environments, thereby improving the pose estimation accuracy of the laser radar SLAM system.

[0006] In order to achieve the above object, the present invention provides the following technical solutions:

[0007] A laser radar point-to-line weighted matching method based on edge point quality assessment includes the following steps:

[0008] Preprocessing module: obtains the 3D coordinates of the original LiDAR point cloud, projects the point cloud into a 2D image, generates point cloud clusters with category labels through target segmentation, calculates the local roughness of each point, and screens candidate edge points;

[0009] Position judgment module: Classify the candidate edge point into a candidate middle edge point, a candidate left edge point, a candidate right edge point, or an abnormal edge point based on the consistency of the category labels of the candidate edge point and its left and right adjacent points;

[0010] Direction consistency judgment module: constructs direction vectors for the three types of candidate edge points, calculates the direction standard deviation, and selects valid edge points that meet the direction consistency standards;

[0011] Dynamic weighting module: Assigns weights based on the directional standard deviation of valid edge points and solves pose estimation through weighted residual optimization.

[0012] Furthermore, the point cloud projection satisfies:

[0013] Projected point cloud in:

[0014] r α is the row number, r α ∈[0,N-1], N is the number of laser radar beams;

[0015] c β is the column number, c β ∈[0,M-1], M is the number of single beam scans;

[0016] ∏(·) is the projection operation, is a point in the original point cloud.

[0017] Furthermore, the target segmentation adopts a fast segmentation algorithm to output the point cloud cluster category label L(r α ,c β )=ω, where ω∈[1,W] represents the category label and W is the total number of labels.

[0018] Furthermore, the classification rules of the position determination module are:

[0019] If the candidate edge point is consistent with the categories of the adjacent points on the left and right sides, it is determined to be a candidate middle edge point;

[0020] If the category is consistent only with the adjacent point on the left, it is determined to be a candidate left edge point;

[0021] If the category is consistent only with the adjacent point on the right, it is determined to be a candidate right edge point;

[0022] If it is inconsistent with the categories on both sides, it is judged as an abnormal edge point and removed.

[0023] Furthermore, the direction consistency judgment module performs:

[0024] Calculate the standard deviation of the left and right directions for the candidate middle edge point If they are all less than the threshold σ0, then retain and calculate the average standard deviation

[0025] Calculate the left direction standard deviation for the candidate left edge point If it is less than σ0, it is retained;

[0026] Calculate the right direction standard deviation for the candidate right edge point If it is less than σ0, it is retained.

[0027] Furthermore, the weight distribution of the dynamic weighting module satisfies:

[0028] The weight function is defined as Where: σ is the standard deviation of the direction of the effective edge points, the middle edge points are Left side Right side λ is the decay coefficient hyperparameter.

[0029] Furthermore, the pose estimation is achieved by minimizing the weighted residual:

[0030]

[0031] Where: R, t are rotation matrices and translation vectors; is the valid edge point after direction consistency judgment in the current frame; l s,k 、l e,k are the starting and ending points of the corresponding edge segment in the previous frame; w kis the edge point weight, D is the total number of valid edge points involved in matching; × represents vector cross product.

[0032] Furthermore, the local roughness calculation satisfies:

[0033]

[0034] Where: μ k for point The local roughness of ; S is the set of neighboring points, |S| is the set size; d k is the distance value of the current point, d k+γ is the distance value of the neighboring points.

[0035] Furthermore, the direction standard deviation calculation satisfies:

[0036]

[0037] Where: θ is the standard deviation in the unilateral direction; n is the number of adjacent points involved in the calculation; Δθ j is the angle between the jth direction vector and the mean.

[0038] A laser radar simultaneous positioning and mapping SLAM system adopts the weighted matching method to perform inter-frame pose optimization.

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

[0040] (1) Traditional methods assign equal weights to edge points, which results in edge points with ambiguous geometric expressions interfering with pose solution. This invention uses a dual screening mechanism of position feature classification and direction consistency detection:

[0041] Eliminate edge points with abnormal positions based on category labels;

[0042] Filter out structurally unstable edge points based on the directional standard deviation;

[0043] Make all points involved in matching high-confidence feature points;

[0044] (2) Improve environmental adaptability

[0045] In unstructured scenes such as vegetation and curved surfaces:

[0046] The dynamic weighting module assigns higher weights to high-quality edge points;

[0047] The exponential decay function effectively suppresses the influence of low-quality points;

[0048] The system's tolerance to sampling noise and weak structure interference is significantly improved;

[0049] (3) Optimize computing performance

[0050] Compared with the posterior screening strategy:

[0051] Complete quality assessment before matching to avoid redundant iterative calculations;

[0052] The position judgment module quickly eliminates abnormal points;

[0053] Lightweight calculation of direction detection standard deviation;

[0054] Achieve a balance between accuracy improvement and computational burden.

[0055] Other advantages, objects, and features of the present invention will be described in part in the following description and, in part, will be apparent to those skilled in the art upon examination of the following description or may be learned from practice of the present invention. The objects and other advantages of the present invention may be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below with reference to the accompanying drawings, in which:

[0057] Figure 1 This is a framework diagram of the present invention;

[0058] Figure 2 Scan the front view of the LiDAR point cloud;

[0059] Figure 3 Scan the top view for the LiDAR point cloud;

[0060] Figure 4 Schematic diagram of point cloud projection;

[0061] Figure 5 Schematic diagram for determining the position of candidate edge points;

[0062] Figure 6 Schematic diagram of direction consistency detection. DETAILED DESCRIPTION

[0063] The following describes the embodiments of the present invention by means of specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0064] Among them, the accompanying drawings are only for illustrative purposes and represent only schematic diagrams rather than actual pictures, and should not be understood as limiting the present invention. In order to better illustrate the embodiments of the present invention, some parts of the accompanying drawings may be omitted, enlarged or reduced, and do not represent the dimensions of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions may be omitted in the accompanying drawings.

[0065] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "front", "back", etc. indicating directions or positional relationships, they are based on the directions or positional relationships shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operate in a specific direction. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0066] like Figure 1 As shown, the present invention proposes a lidar point-to-line weighted matching method based on edge point quality evaluation, and its specific implementation steps are as follows:

[0067] 1. Original point cloud projection:

[0068] like Figure 2 and Figure 3 As shown in Figure 1, the LiDAR scans the 3D world and generates a raw point cloud representing the 3D information of the environment. Each point in the point cloud represents 3D spatial coordinate information.

[0069] Assume that the original point cloud obtained at a certain moment is P, and the calculation method of its projected point cloud I is as follows:

[0070]

[0071] Among them, r α ,c β Represent the row number and column number of the laser scan, respectively, satisfying r α ∈[0,N-1],c β ∈[0,M-1], where N is the number of laser beams and M is the number of times a beam is transmitted or received in a frame scan. Π(·) represents the projection operation. is a point in the point cloud P.

[0072] The schematic diagram of the projected point cloud is as follows Figure 4 As shown in the figure, a black dot indicates that a laser point is projected onto that location, and a blank area indicates that no laser point is projected onto that location.

[0073] 2. Target segmentation and category label assignment:

[0074] Although there are many mature target segmentation methods currently, in order to meet the real-time requirements of target segmentation, this paper adopts the fast segmentation method proposed in the 2016 paper "Fast Range Image-Based Segmentation of Sparse 3D Laser Scansfor Online Operation" for processing.

[0075] After the projected point cloud is input into the segmentation algorithm, each laser point can be assigned a corresponding category label, which is specifically expressed as follows:

[0076] L(r α ,c β )=ω,ω∈[1,W]

[0077] Where L(·) represents the category label to which the laser point belongs, and W represents the total number of labels. Multiple laser points with the same category label constitute a point cloud cluster.

[0078] 3. Candidate edge point selection:

[0079] For each laser point in the projected point cloud, calculate the local roughness of the neighboring points within a certain range in the same scan line:

[0080]

[0081] Among them, μ k Indicates the current point The local roughness of , S represents the set of adjacent points used for calculation, |S| represents the number of points in the set S. k+γ and d k They respectively represent the distance between a point in the set S and the current point.

[0082] After sorting the point cloud in descending order according to the local roughness value, the following strategy is used to traverse and filter the candidate edge points until the number limit threshold is met, and then the traversal is terminated:

[0083] E←p k , μ k >μ0 and ψ<Q

[0084] Among them, E represents the set of candidate edge points, ← represents the point p k The operation of adding to the set, μ0 represents the minimum local roughness threshold that must be met to be considered a candidate edge point, ψ represents the number of current candidate edge points, and Q represents the preset upper limit threshold.

[0085] 4. Unilateral Category Consistency Judgment

[0086] Traverse the candidate edge point set E, for each candidate edge point E i In the projected point cloud I, find the adjacent laser points within a certain range on the left and right along the row where the candidate edge point is located, and obtain the category labels of these adjacent points from L. Determine whether the label of the left adjacent point is the same as E i , and whether the label of the right adjacent point is the same as E i , to determine the category consistency.

[0087] 5. Position determination

[0088] If the categories of the candidate edge point and the adjacent points on the left and right are consistent, it is determined to be a candidate middle edge point; if only the category of the left point is consistent, it is determined to be a candidate left edge point; if only the category of the right point is consistent, it is determined to be a candidate right edge point; if the categories of the two points are inconsistent, it is determined to be a candidate abnormal edge point and is discarded.

[0089] An example of candidate edge point position determination is shown in Figure 5 . The spatial distribution of multiple laser points on the same scanning line is shown in the figure. If adjacent laser points are found within a certain range on the left and right of the candidate edge point and category consistency is determined, then: laser point 2 and its left and right points are consistent in category, so it is determined to be a candidate middle edge point; laser point 4 is only consistent with its left point in category, so it is determined to be a candidate left edge point; laser point 5 is only consistent with its right point in category, so it is determined to be a candidate right edge point.

[0090] 6. Direction consistency detection

[0091] An example of direction consistency detection is shown in Figure 6 . Traverse the candidate edge point set E, for each candidate edge point E i , in the projected point cloud I, find the adjacent laser points within a certain range on the left and right along the row where the candidate edge point is located, and construct a direction vector pointing to the adjacent point with E i as the starting point. Then, the standard deviation of all direction vectors constructed on the left and right is calculated to evaluate the direction consistency. The standard deviation calculation formula of the direction vector is as follows:

[0092]

[0093] Where: σ θ represents the standard deviation of the direction vector on that side; n is the number of adjacent points participating in the calculation on that side; Δθ j represents the angle between the jth direction vector and the mean.

[0094] 7. Edge point screening

[0095] For candidate middle edge points, if the standard deviation of the direction on either side exceeds the set threshold, the point is considered to have inconsistent directions and is removed; otherwise, the average of the standard deviations on the left and right sides is recorded and marked as a valid middle edge point:

[0096]

[0097] Among them, E middle represents the valid intermediate edge point set, is the standard deviation in the left direction, is the right direction standard deviation, and σ0 is the direction standard deviation threshold.

[0098] For any middle edge point in the middle edge point set, its directional standard deviation is estimated by the average of the directional standard deviations on the left and right sides. The calculation formula is as follows:

[0099]

[0100] in, Indicates the directional standard deviation of the middle edge point.

[0101] For a candidate left edge point, if the left direction standard deviation exceeds the threshold, the point is discarded; otherwise, its direction standard deviation is recorded and marked as a valid left edge point:

[0102]

[0103] Among them, E left Represents the valid left edge point set.

[0104] For a candidate right edge point, if the right direction standard deviation exceeds the threshold, the point is discarded; otherwise, its direction standard deviation is recorded and marked as a valid right edge point:

[0105]

[0106] Among them, E right Represents the valid right edge point set.

[0107] 8. Edge point weight function

[0108] A larger directional standard deviation usually means that the local structure of the point has greater noise or higher geometric complexity, making it more difficult to accurately extract high-quality edge features. To this end, this paper uses an exponential decay weight function to assign different weights to each edge point to reflect the degree of influence of its directional consistency on subsequent optimization. The specific weight function is defined as follows:

[0109]

[0110] Among them, σ represents the standard deviation of the direction of the edge point. If it is an intermediate edge point, If it is the left edge point If it is the right edge point λ is a hyperparameter that controls the decay rate; ω represents the final weight of the edge point, which ranges from (0, 1].

[0111] 9. Dynamic weighted matching and solution of edge points

[0112] Weighted matching is performed on the edge points with weights to the lines, and all residuals are optimized as a whole through nonlinear optimization methods (such as least squares method) to solve the optimal pose estimation of the current frame.

[0113]

[0114] Where: R, t represents the rotation matrix and translation vector; The kth edge point in the current frame; l s,k 、l e,k Indicates the starting and ending points of the edge segment corresponding to the kth edge point in the previous frame; w k is the directional consistency weight of the kth edge point; D is the total number of edge points involved in the matching; ||·|| represents the vector modulus; × represents the vector cross product, which represents the vertical distance from the point to the line.

[0115] Example 1: Complete quality assessment process for urban road scenes

[0116] 1.1 Data Collection and Projection

[0117] Vehicle-mounted LiDAR collects urban road point clouds (including buildings, guardrails, and roadside trees)

[0118] Projection parameter settings: number of beams N = 64, number of single line scans M = 1024

[0119] Perform a projection operation:

[0120] 1.2 Target segmentation and candidate point screening

[0121] The projected point cloud is processed using a fast segmentation algorithm to generate 12 types of labels, ω∈[1,12];

[0122] Local roughness calculation: neighborhood range |S| = 10, threshold μ0 = 0.25;

[0123] Screen candidate edge points: retain μ k >The first 5% points of μ0.

[0124] 1.3 Position Classification and Direction Detection

[0125] (1) Position judgment:

[0126] Guardrail vertex → candidate left edge point;

[0127] Gap points between roadside trees → abnormal points.

[0128] (2) Direction consistency detection:

[0129] Building corner points:

[0130] Leaf edge points:

[0131] 1.4 Dynamic Weighted Matching

[0132] Building corner point weight:

[0133] Optimizing pose estimation:

[0134] Output vehicle pose: translation error <0.05m, rotation error <0.5°.

[0135] Example 2: Robustness Verification in Unstructured Field Scenarios

[0136] 2.1 Weak structure environment treatment

[0137] Input point cloud: grass, gravel road, isolated tree trunk

[0138] Target segmentation: Set vegetation cluster label ω = 8, ground label ω = 1

[0139] 2.2 Real-time elimination of abnormal points

[0140] Grass edge point: left label = 8 (vegetation), right label = 1 (ground) → outlier removal

[0141] Trunk edge points: labels on both sides = 8 → candidate middle edge points are retained

[0142] 2.3 Differentiated Weight Distribution

[0143] The differentiated weight distribution of different edge point types is shown in Table 1.

[0144] Table 1

[0145] Edge point type Directional standard deviation σ Weight ω The trunk has clear edges and corners 0.08 0.92 Blurred edges of rubble pile 0.22 0.41

[0146] 2.4 Anti-interference pose solution

[0147] The contribution of low-weighted points is reduced to 37% of the traditional method;

[0148] Vehicle trajectory drift is reduced by 20-40%.

[0149] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should all be included in the scope of the claims of the present invention.

Claims

1. A laser radar point-to-line weighted matching method based on edge point quality assessment, characterized by: The following steps are involved: Preprocessing module: obtains the 3D coordinates of the original LiDAR point cloud, projects the point cloud into a 2D image, generates point cloud clusters with category labels through target segmentation, calculates the local roughness of each point, and screens candidate edge points; Position judgment module: Classify the candidate edge point into a candidate middle edge point, a candidate left edge point, a candidate right edge point, or an abnormal edge point based on the consistency of the category labels of the candidate edge point and its left and right adjacent points; Direction consistency judgment module: constructs direction vectors for the three types of candidate edge points, calculates the direction standard deviation, and selects valid edge points that meet the direction consistency standards; Dynamic weighting module: Assigns weights based on the directional standard deviation of valid edge points and solves pose estimation through weighted residual optimization.

2. The laser radar point-to-line weighted matching method based on edge point quality assessment according to claim 1, characterized in that: The point cloud projection satisfies: Projected point cloud in: r α is the row number, r α ∈[0,N-1], N is the number of laser radar beams; c β is the column number, c β ∈[0,M-1], M is the number of single beam scans; Π(·) is the projection operation, is a point in the original point cloud.

3. The laser radar point-to-line weighted matching method based on edge point quality assessment according to claim 1, characterized in that: The target segmentation adopts a fast segmentation algorithm to output the point cloud cluster category label L(r α ,c β )=ω, where ω∈[1,W] represents the category label and W is the total number of labels.

4. The laser radar point-to-line weighted matching method based on edge point quality assessment according to claim 1, characterized in that: The classification rules of the position judgment module are: If the candidate edge point is consistent with the categories of the adjacent points on the left and right sides, it is determined to be a candidate middle edge point; If the category is consistent only with the adjacent point on the left, it is determined to be a candidate left edge point; If the category is consistent only with the adjacent point on the right, it is determined to be a candidate right edge point; If it is inconsistent with the categories on both sides, it is judged as an abnormal edge point and removed.

5. The laser radar point-to-line weighted matching method based on edge point quality assessment according to claim 1, characterized in that: The direction consistency judgment module performs: Calculate the standard deviation of the left and right directions for the candidate middle edge point If they are all less than the threshold σ0, then retain and calculate the average standard deviation Calculate the left direction standard deviation for the candidate left edge point If it is less than σ0, it is retained; Calculate the right direction standard deviation for the candidate right edge point If it is less than σ0, it is retained.

6. The laser radar point-to-line weighted matching method based on edge point quality assessment according to claim 1, characterized in that: The weight distribution of the dynamic weighting module satisfies: The weight function is defined as Where: σ is the standard deviation of the direction of the effective edge points, the middle edge points are Left side Right side λ is the decay coefficient hyperparameter.

7. The laser radar point-to-line weighted matching method based on edge point quality assessment according to claim 1, characterized in that: The pose estimation is achieved by minimizing the weighted residual: Where: R, t are rotation matrices and translation vectors; is the valid edge point after direction consistency judgment in the current frame; l s,k 、l e,k are the starting and ending points of the corresponding edge segment in the previous frame; w k is the edge point weight, D is the total number of valid edge points involved in matching; × represents vector cross product.

8. The laser radar point-to-line weighted matching method based on edge point quality assessment according to claim 1, characterized in that: The local roughness calculation satisfies: Where: μ k for point The local roughness of ; S is the set of neighboring points, |S| is the set size; d k is the distance value of the current point, d k+γ is the distance value of the neighboring points.

9. The laser radar point-to-line weighted matching method based on edge point quality assessment according to claim 5, characterized in that: The direction standard deviation calculation satisfies: Where: θ is the standard deviation in the unilateral direction; n is the number of adjacent points involved in the calculation; Δθ j is the angle between the jth direction vector and the mean.

10. A laser radar simultaneous positioning and mapping SLAM system, characterized by: The inter-frame pose optimization is performed using the weighted matching method described in any one of claims 1 to 9.

Images