Laser scanning radar mapping method flexibly combining NDT and ICP

Through a laser scanning radar mapping method that flexiblely combines NDT and ICP, the adaptive selection algorithm is used to improve the accuracy and robustness of point cloud registration, and the problem of matching complexity in unstructured scenarios is solved and efficient point Cloud registration and map building.

CN118938250APending Publication Date: 2024-11-12WUHU SIMBA NETWORK TECH CO LTD
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202411137951.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-19
Publication Date
2024-11-12

AI Technical Summary

Technical Problem

In the field of autonomous driving, laser SLAM algorithms face interference from factors such as noise, occlusion and surface changes in unstructured scenarios, resulting in increased point cloud matching complexity, reduced positioning accuracy and map construction reliability, and high computational complexity of existing algorithms , affects processing speed.

Method used

A laser scanning radar mapping method that flexible combination of NDT and ICP is adopted, through point cloud feature extraction and inter-frame matching, adaptive selection algorithm: pre-registration is used in unstructured scenarios, and through ICP algorithm Accurate registration, using scene recognition factor and singular value decomposition method to improve registration accuracy and stability.

Benefits of technology

It improves the accuracy and robustness of point cloud registration, adapts to different scenario requirements, reduces calculation complexity and time consumption, effectively corrects the odometer drift problem, and improves the practicality and reliability of the system.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118938250A_ABST
    Figure CN118938250A_ABST
Patent Text Reader

Abstract

The invention discloses a laser scanning radar mapping method flexibly combining NDT and ICP, and belongs to the technical field of map construction. The method comprises the following steps: feature extraction of a point cloud: performing motion distortion removal and outlier removal on original laser data, and outputting the processed data as a feature point cloud; inter-frame matching: carrying out inter-frame matching through the feature point cloud, carrying out iteration twice, calculating a scene recognition factor during the first iteration, and judging and determining whether to use an NDT algorithm to carry out pre-registration of the point cloud, using an ICP algorithm during the second iteration, updating point cloud information of a reference frame, and obtaining laser odometer data through matching of a current frame and a previous frame; and back-end optimization registration: pre-registration is carried out through an NDT algorithm, and then precise registration is carried out through an ICP algorithm. According to the invention, the registration precision in an unstructured scene is improved; the error and the calculation amount in the registration process are reduced; and the accumulated drift error of the speedometer is reduced.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of map construction, and in particular to a laser scanning radar mapping method that flexibly combines NDT and ICP. Background Art

[0002] With the development of autonomous driving technology, simultaneous localization and mapping (SLAM) has become a research hotspot in recent years. Compared with other sensors, laser radar has outstanding advantages in obstacle detection, high-precision distance measurement and insensitivity to light texture. Therefore, laser SLAM algorithms with laser radar as sensors have very broad application prospects in the field of autonomous driving. The widely used laser SLAM algorithm framework uses the iterative closest point (ICP) algorithm for point cloud registration. The ICP algorithm aligns point cloud data of different frames to the same coordinate system through continuous iterative optimization to achieve accurate matching and fusion. However, when facing unstructured scenes, the ICP algorithm often faces challenges because it relies on stable and reliable feature extraction. In these scenes, factors such as noise, occlusion and surface changes can easily interfere with the accurate recognition of features, thereby increasing the complexity of point cloud matching and may reduce the accuracy of positioning and the reliability of map construction. In order to overcome these difficulties, the Normal Distributions Transform (NDT) algorithm based on probability distribution can be used. The NDT algorithm extracts features from point cloud data and uses a probabilistic model to optimize the matching process. The advantage of this method is that it can adapt to non-rigid deformations and handle partial occlusions, and it also shows potential in improving registration accuracy.

[0003] Although the NDT algorithm is highly regarded for its high accuracy and robustness in point cloud registration, it is not wise to use this algorithm for pre-registration in all cases. Although the NDT algorithm can provide more accurate registration results, its high computational complexity may affect the overall processing speed and efficiency. Especially when dealing with structured scenes, the ICP algorithm shows its unique advantages. Since the features in structured scenes are usually obvious and regular, the ICP algorithm can quickly and effectively match point clouds, thereby optimizing the operating efficiency and accuracy. Therefore, in these scenarios, giving priority to the ICP algorithm can not only improve the processing speed, but also ensure the accuracy of the registration. Therefore, in the face of different types of scenes, how to intelligently select the most appropriate algorithm to achieve the best point cloud registration effect is still a technical challenge. Summary of the invention

[0004] The object of the present invention is to provide a laser scanning radar mapping method that flexibly combines NDT and ICP to solve the problems raised in the above background technology.

[0005] In order to solve the above technical problems, the present invention provides the following technical solutions: a laser scanning radar mapping method that flexibly combines NDT and ICP, the method comprising the following steps:

[0006] S1. Feature extraction of point cloud: Perform motion distortion removal and outlier removal operations on the original laser data, and output the processed data as feature point cloud;

[0007] S2, inter-frame matching: inter-frame matching is performed through the extracted feature point cloud. The inter-frame matching algorithm is iterated twice. In the first iteration, the scene recognition factor is calculated, and it is determined whether to use the NDT algorithm for point cloud pre-registration. In the second iteration, the ICP algorithm is used to update the point cloud information of the reference frame, and the laser odometer data is obtained by matching the current frame with the previous frame.

[0008] S3, back-end optimization registration: pre-register using the NDT algorithm, and then use the ICP algorithm for fine registration to better correct the drift of the odometer.

[0009] According to the above scheme, step S1 includes:

[0010] S2-1, receiving point cloud data from the laser radar, estimating the position of the radar at the time of acquisition of each point, transforming all points in a frame to the same coordinate system, and performing linear interpolation to obtain the radar pose at the time of acquisition of each point;

[0011] S2-2, by analyzing the relationship between the point sets, identifying the plane points and edge points suitable as feature points;

[0012] S2-3. Divide the point cloud data into voxel units, select the point closest to the voxel center in each voxel unit as a sampling point, and combine the selected sampling points to form new point cloud data.

[0013] According to the above scheme, the estimated pose at each point acquisition moment is realized by linear interpolation;

[0014] Assume T k-1 It is expressed as the inter-frame motion of the k-1th frame. When dedistorting the point cloud of the kth frame, it is assumed that the motion of the lidar is uniform within the frame period. The formula is as follows:

[0015]

[0016] Among them, T k-1,i It is represented as the radar pose estimation of the i-th point in the k-1-th frame point cloud at the acquisition time; tk It is represented as the starting time of the k-1 frame point cloud cycle; t k-1 Represents the end time of the k-1 frame point cloud cycle; t k-1,i It is represented as the moment of the i-th point in the k-1 frame point cloud.

[0017] According to the above scheme, by calculating the curvature of each point in the point set S, suitable feature points are screened out to improve the accuracy of point cloud registration. The formula is as follows:

[0018]

[0019] in, It is represented as a point in the point set S; j is represented as the points around i, S is represented as the set of neighborhood points of the i-th point, and c represents the curvature;

[0020] Points with small curvature indicate a smooth surface and are suitable as plane feature points; points with large curvature indicate edge points or corner points; curvature calculation selects high-quality feature points to ensure that plane points and edge points with smooth surfaces are selected for registration; feature point extraction and selection improve the accuracy and robustness of point cloud registration.

[0021] According to the above scheme, step S2 includes:

[0022] S5-1, using feature matching, matching the current frame with the previously extracted feature point cloud by iteratively optimizing the residual;

[0023] S5-2, introducing scene recognition factors, can more comprehensively evaluate scene structure;

[0024] S5-3. In unstructured scenes, the NDT algorithm is used for pre-alignment, the point cloud features are mapped using normal distribution, and the sum of probabilities between point clouds is maximized by optimizing the transformation matrix; the ICP algorithm is used for fine alignment, in which the singular value decomposition method is used to ensure that the global optimal solution is found and the numerical stability is improved to achieve accurate alignment of point cloud data.

[0025] According to the above scheme, feature matching is performed on the feature points of the current frame with the feature point cloud extracted in the first stage, and the line features or surface features in the point cloud are used for matching; the residual is optimized in an iterative manner, and the residual is constructed by the distance from point to line or point to surface;

[0026] The distance calculation formula from the edge point in the origin point cloud to the edge line in the target point cloud and from the plane point in the origin point cloud to the plane in the target point cloud is as follows:

[0027]

[0028] Among them, d edgeIt is expressed as the distance from the edge point in the origin point cloud to the edge line in the target point cloud; d plane It is represented as the distance from the plane point in the original point cloud to the plane in the target point cloud; j, k, l, m are represented as their feature identifiers in the corresponding sets of edge points and plane points respectively; Represented as edge points or plane points of the K+1th frame in the original point cloud; Represented as the matching point of the kth frame in the target point cloud.

[0029] According to the above scheme, the influence of the feature matching ratio and the normal vector consistency on the scene structure is comprehensively considered;

[0030] The formula for the feature matching ratio is as follows:

[0031]

[0032] Among them, R feature Expressed as the ratio of feature matching; F n Represents the total number of feature matches before the ICP algorithm performs point cloud registration for each frame; PC n Represented as the total number of point clouds in the corresponding frame; 10K line It is represented as an amplification factor introduced to ensure that the feature matching maintains sufficient sensitivity; K line Represented as the harness of the lidar used;

[0033] The formula for the normal vector angle is as follows:

[0034]

[0035] Among them, θ represents the normal vector angle, n1 and n2 are the normal vectors of adjacent points;

[0036] A comprehensive evaluation index can be defined, namely the scene identification factor SIF, the formula is as follows:

[0037]

[0038] Where N represents the total number of adjacent point pairs for normal vector calculation; θ i It is expressed as the angle between the normal vectors of the i-th pair of adjacent points;

[0039] Compare the SIF value with the threshold V0. When SIF is greater than V0, the scene tends to be structured. When SIF is less than V0, it means that the motion speed is fast, ICP tends to converge to the local extreme value, and the unstructured scene tends to be more unstructured.

[0040] The scene identification factor SIF evaluates the scene structure and adaptively chooses whether to use the NDT algorithm for pre-registration, which helps to improve the registration effect in unstructured environments.

[0041] According to the above solution, the steps of using the NDT algorithm for pre-registration include:

[0042] S8-1,NDT algorithm divides the point cloud data into multiple cubic grids by setting a certain voxel size;

[0043] S8-2. In each grid, the coordinate values ​​of the point cloud are counted and their mean μ and covariance ε are calculated. These point cloud data are converted into a probability density function with normal distribution characteristics. The formula is as follows:

[0044]

[0045] Among them, p(x) represents the probability density at point x; x represents the vector of two coordinates; It is represented as a normalized constant; exp{} represents an exponential operation with e as the base;

[0046] S8-3. After the point cloud coordinates P are transformed by the initial rotation matrix R and the translation vector t, their position in the reference coordinate system is updated to P', and the formula is as follows

[0047] P'=R×P+t

[0048] S8-4. According to the probability density function of each corresponding grid, the probability of each point appearing in the corresponding grid is accumulated to obtain the registration score of the NDT algorithm. The formula is as follows:

[0049]

[0050] Where m represents the total number of point clouds in the registration frame, and the pose transformation α is expressed in algebraic form; p(P′ i ) is represented by the probability density function of the i-th point in the corresponding grid after transformation;

[0051] The function score(α) of α is set as the objective function of the Gauss-Newton method. The objective function of the Gauss-Newton method can be expressed as the error function E after point cloud transformation. The formula is as follows:

[0052]

[0053] Where Q represents the point corresponding to P' in the reference coordinate system, ‖·‖ represents the Euclidean norm;

[0054] S8-5. Calculate the adjustment amount of posture transformation according to the score. The formula is as follows:

[0055] R new =R old +ΔR

[0056] t new =t old +Δt

[0057] Among them, ΔR and Δt are the posture transformation parameters calculated according to Gauss-Newton iteration;

[0058] S8-6, update the posture transformation parameters, and check whether the iteration termination condition is met; if not, repeat steps S8-3 to S8-6 until the iteration termination condition is met and the optimal solution is reached.

[0059] According to the above scheme, the ICP algorithm is used for the second iteration to convert the distance d from the edge point in the original point cloud to the edge line in the target point cloud. edge The distance d from the plane point in the origin point cloud to the plane in the target point cloud plane The constraints are unified and the formula is as follows:

[0060]

[0061] Where R is the rotation matrix; t is the translation vector; loss is the loss function for calculating R and t;

[0062] The loss function unified with the distance function is used to calculate the rotation matrix R and translation vector t that minimize the pose transformation parameters. The formula is as follows:

[0063]

[0064] Among them, F n It is expressed as the number of matched feature points; arg min is expressed as the minimum value of the parameter;

[0065] The singular value decomposition method is used to directly solve the spatial transformation matrix of each iteration to improve numerical stability and accuracy;

[0066] When the number of iterations is greater than 2, the point cloud data of the reference frame is updated, and the laser odometer data is published by matching the current frame with the previous frame.

[0067] According to the above scheme, the NDT algorithm is used for preliminary non-rigid registration to correct the posture deviation caused by the odometer error, and the ICP algorithm is used for fine adjustment;

[0068] The pre-registration of the NDT algorithm helps to correct the large initial pose deviation, and the subsequent fine-registration by the ICP algorithm significantly reduces the problem of accumulated error of the odometer.

[0069] The ICP algorithm uses the singular value decomposition (SVD) method to solve the spatial transformation matrix, which improves the numerical stability and computational efficiency of the registration process. In unstructured scenes, the NDT algorithm improves the registration speed and accuracy through normal distribution mapping and probability density function optimization.

[0070] The output poses are fused and the map is updated, and finally the trajectory and map are output to provide support for subsequent path planning and navigation tasks, thereby improving the practicality and reliability of the entire system.

[0071] Compared with the prior art, the beneficial effects achieved by the present invention are:

[0072] 1. The present invention adaptively selects whether to use the NDT algorithm for pre-registration of point clouds by determining whether the scene is an unstructured scene; the NDT algorithm has good robustness and registration accuracy when processing unstructured scenes, and effectively improves the accuracy of registration;

[0073] 2. The present invention comprehensively considers the ratio of the number of feature matches to the total number of point clouds in the corresponding frame and the consistency of the point cloud data before registering each frame of point cloud, and compares it with the threshold to decide whether to use the NDT algorithm for pre-registration; avoids applying the NDT algorithm to structured scenes, reduces the introduction of errors, and reduces the amount of calculation and time consumption;

[0074] 3. The present invention adopts the NDT algorithm for preliminary pre-alignment to deal with the situation where the initial posture deviation is large; after the NDT algorithm provides a relatively accurate initial posture, the ICP algorithm is used for further refined alignment; this not only improves the success rate of alignment, but also optimizes the accuracy of the alignment results, and effectively corrects the drift problem of the odometer. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention.

[0076] In the attached picture:

[0077] Figure 1 It is a schematic flow chart of a laser scanning radar mapping method that flexibly combines NDT and ICP according to the present invention;

[0078] Figure 2 It is a schematic diagram of the process of inter-frame matching of a laser scanning radar mapping method that flexibly combines NDT and ICP in the present invention. DETAILED DESCRIPTION

[0079] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0080] See also Figure 1-Figure 2 , the present invention provides a technical solution:

[0081] According to one embodiment of the present invention, a laser scanning radar mapping method that flexibly combines NDT and ICP includes:

[0082] Receive point cloud data from the LiDAR, estimate the position of the LiDAR at the time of acquisition of each point, transform all points in a frame to the same coordinate system, and perform linear interpolation to obtain the LiDAR pose at the time of acquisition of each point;

[0083] Assume T k-1 It is expressed as the inter-frame motion of the k-1th frame. When dedistorting the point cloud of the kth frame, it is assumed that the motion of the lidar is uniform within the frame period. The formula is as follows:

[0084]

[0085] Among them, T k-1,i It is represented as the radar pose estimation of the i-th point in the k-1-th frame point cloud at the acquisition time; t k It is represented as the starting time of the k-1 frame point cloud cycle; t k-1 Represents the end time of the k-1 frame point cloud cycle; t k-1,i It is represented as the moment of the i-th point in the k-1 frame point cloud.

[0086] By calculating the curvature of each point in the point set S, suitable feature points are screened out to improve the accuracy of point cloud registration. The formula is as follows:

[0087]

[0088] in, It is represented as a point in the point set S; j is represented as the points around i, S is represented as the set of neighborhood points of the i-th point, and c represents the curvature;

[0089] Points with small curvature indicate a smooth surface and are suitable as plane feature points, while points with large curvature indicate edge points or corner points.

[0090] The point cloud data is divided into voxel units, and the point closest to the voxel center in each voxel unit is selected as the sampling point. The selected sampling points are combined to form new point cloud data.

[0091] Match the feature points of the current frame with the feature point cloud extracted in the first stage, using the line features or surface features in the point cloud for matching; optimize the residual by iterative method, the residual is constructed by the distance from point to line or point to surface;

[0092] The distance calculation formula from the edge point in the origin point cloud to the edge line in the target point cloud and from the plane point in the origin point cloud to the plane in the target point cloud is as follows:

[0093]

[0094] Among them, d edge It is expressed as the distance from the edge point in the origin point cloud to the edge line in the target point cloud; d plane It is represented as the distance from the plane point in the original point cloud to the plane in the target point cloud; j, k, l, m are represented as their feature identifiers in the corresponding sets of edge points and plane points respectively; Represented as edge points or plane points of the K+1th frame in the original point cloud; Represented as the matching point of the kth frame in the target point cloud.

[0095] Comprehensively consider the impact of feature matching ratio and normal vector consistency on scene structure;

[0096] The formula for the feature matching ratio is as follows:

[0097]

[0098] Among them, R feature Expressed as the ratio of feature matching; F n Represents the total number of feature matches before the ICP algorithm performs point cloud registration for each frame; PC n Represented as the total number of point clouds in the corresponding frame; 10K line It is represented as an amplification factor introduced to ensure that the feature matching maintains sufficient sensitivity; K line Represented as the harness of the lidar used;

[0099] The formula for the normal vector angle is as follows:

[0100]

[0101] Among them, θ represents the normal vector angle, n1 and n2 are the normal vectors of adjacent points;

[0102] A comprehensive evaluation index can be defined, namely the scene identification factor SIF, the formula is as follows:

[0103]

[0104] Where N represents the total number of adjacent point pairs for normal vector calculation; θ i It is expressed as the angle between the normal vectors of the i-th pair of adjacent points;

[0105] Compare the SIF value with the threshold V0. When SIF is greater than V0, the scene tends to be structured. When SIF is less than V0, it means that the motion speed is fast and ICP tends to converge to the local extreme value. The unstructured scene tends to be more unstructured.

[0106] In structured scenes, the CP algorithm is used alone for fine alignment; in unstructured scenes, the NDT algorithm is used for pre-alignment, the point cloud features are mapped using normal distribution, and the sum of probabilities between point clouds is maximized by optimizing the transformation matrix. The ICP algorithm is then used for fine alignment, in which the singular value decomposition method is used to ensure that the global optimal solution is found and the numerical stability is improved to achieve accurate alignment of point cloud data.

[0107] The implementation process of the NDT algorithm can be summarized into the following key steps:

[0108] S8-1,NDT algorithm divides the point cloud data into multiple cubic grids by setting a certain voxel size;

[0109] S8-2. In each grid, the coordinate values ​​of the point cloud are counted and their mean μ and covariance ε are calculated. These point cloud data are converted into a probability density function with normal distribution characteristics. The formula is as follows:

[0110]

[0111] Among them, p(x) represents the probability density at point x; x represents the vector of two coordinates; It is represented as a normalized constant; exp{} represents an exponential operation with e as the base;

[0112] S8-3. After the point cloud coordinates P are transformed by the initial rotation matrix R and the translation vector t, their position in the reference coordinate system is updated to P', and the formula is as follows

[0113] P'=R×P+t

[0114] S8-4. According to the probability density function of each corresponding grid, the probability of each point appearing in the corresponding grid is accumulated to obtain the registration score of the NDT algorithm. The formula is as follows:

[0115]

[0116] Where m represents the total number of point clouds in the registration frame, and the pose transformation α is expressed in algebraic form; p(P′ i ) is represented by the probability density function of the i-th point in the corresponding grid after transformation;

[0117] The function score(α) of α is set as the objective function of the Gauss-Newton method. The objective function of the Gauss-Newton method can be expressed as the error function E after point cloud transformation. The formula is as follows:

[0118]

[0119] Where Q represents the point corresponding to P' in the reference coordinate system, ‖·‖ represents the Euclidean norm;

[0120] S8-5. Calculate the adjustment amount of posture transformation according to the score. The formula is as follows:

[0121] R new =R old +ΔR

[0122] t new =t old +Δt

[0123] Among them, ΔR and Δt are the posture transformation parameters calculated according to Gauss-Newton iteration;

[0124] S8-6, update the posture transformation parameters, and check whether the iteration termination condition is met; if not, repeat steps S8-3 to S8-6 until the iteration termination condition is met and the optimal solution is reached.

[0125] The ICP algorithm is used for the second iteration to calculate the distance d from the edge point in the original point cloud to the edge line in the target point cloud. edge The distance d from the plane point in the origin point cloud to the plane in the target point cloud plane The constraints are unified and the formula is as follows:

[0126]

[0127] Where R is the rotation matrix; t is the translation vector; loss is the loss function for calculating R and t;

[0128] The loss function unified with the distance function is used to calculate the rotation matrix R and translation vector t that minimize the pose transformation parameters. The formula is as follows:

[0129]

[0130] Among them, F n It is expressed as the number of matched feature points; arg min is expressed as the minimum value of the parameter;

[0131] The singular value decomposition method is used to directly solve the spatial transformation matrix of each iteration to improve numerical stability and accuracy;

[0132] When the number of iterations is greater than 2, the point cloud data of the reference frame is updated, and the laser odometer data is published by matching the current frame with the previous frame.

[0133] The NDT algorithm is used for preliminary non-rigid registration to correct the posture deviation caused by the odometer error, and the ICP algorithm is applied for fine adjustment. The output posture is fused and the map is updated. Finally, the trajectory and map are output to provide support for subsequent path planning and navigation tasks. This improves the practicality and reliability of the entire system.

[0134] In this example, the inter-frame matching of a laser scanning radar mapping method that flexibly combines NDT and ICP includes:

[0135] The residual is optimized by iterative method with the data after feature matching. The number of iterations is first determined. When the number of iterations is <2 and the number of iterations == 0, the scene identification factor SIF is calculated. The formula is as follows:

[0136]

[0137] Among them, R feature It is expressed as the ratio of feature matching; N is the total number of adjacent point pairs calculated for normal vector; θ i It is expressed as the angle between the normal vectors of the i-th pair of adjacent points;

[0138] When SIF is less than V0, it means that the movement speed is fast, ICP tends to converge to the local extreme value, and the unstructured scene tends to be more unstructured;

[0139] In unstructured scenes, the NDT algorithm is used for pre-registration. The implementation process of the NDT algorithm can be summarized into the following key steps:

[0140] S8-1,NDT algorithm divides the point cloud data into multiple cubic grids by setting a certain voxel size;

[0141] S8-2. In each grid, the coordinate values ​​of the point cloud are counted and their mean μ and covariance ε are calculated, and the point cloud data are converted into a probability density function with normal distribution characteristics;

[0142] S8-3, after the point cloud coordinates P are transformed by the initial rotation matrix R and the translation vector t, their position in the reference coordinate system is updated to P';

[0143] S8-4, according to the probability density function of each corresponding grid, the probability of each point appearing in the corresponding grid is accumulated to obtain the registration score of the NDT algorithm;

[0144] The function score(α) of α is set as the objective function of the Gauss-Newton method. The objective function of the Gauss-Newton method can be expressed as the error function E after the point cloud transformation.

[0145] S8-5, calculating the adjustment amount of posture transformation according to the score;

[0146] S8-6, update the posture transformation parameters, and check whether the iteration termination condition is met; if not, repeat steps S8-3 to S8-6 until the iteration termination condition is met and the optimal solution is reached.

[0147] When SIF is greater than V0, the scene tends to be structured. In structured scenes, the ICP algorithm is used for registration.

[0148] After NDT pre-registration or ICP algorithm registration, the number of iterations is increased by 1, and the number of iterations is judged again;

[0149] When the number of iterations is <2 and the number of iterations is ≠0, the ICP algorithm is used for fine registration;

[0150] The distance d from the edge point in the origin point cloud to the edge line in the target point cloud edge The distance d from the plane point in the origin point cloud to the plane in the target point cloud plane Constraint and unity;

[0151] The loss function unified with the distance function is used to calculate the rotation matrix R and translation vector t that minimize the pose transformation parameters; the singular value decomposition method is used to directly solve the spatial transformation matrix of each iteration to improve numerical stability and accuracy; and accurate alignment of point cloud data is achieved.

[0152] After the ICP algorithm is used for fine registration, the number of iterations is increased by 1, and the number of iterations is judged again;

[0153] At this time, the number of iterations == 2, then the point cloud data of the reference frame is updated, and the laser odometer data is published by matching the current frame with the previous frame.

[0154] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0155] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A laser scanning radar mapping method that flexibly combines NDT and ICP, characterized in that: The method comprises the following steps: S1. Feature extraction of point cloud: Perform motion distortion removal and outlier removal operations on the original laser data, and output the processed data as feature point cloud; S2, inter-frame matching: inter-frame matching is performed through the extracted feature point cloud. The inter-frame matching algorithm is iterated twice. In the first iteration, the scene recognition factor is calculated, and it is determined whether to use the NDT algorithm for point cloud pre-registration. In the second iteration, the ICP algorithm is used to update the point cloud information of the reference frame, and the laser odometer data is obtained by matching the current frame with the previous frame. S3, back-end optimization registration: pre-register using the NDT algorithm, and then use the ICP algorithm for fine registration to better correct the drift of the odometer.

2. The laser scanning radar mapping method of flexible combination of NDT and ICP according to claim 1 is characterized by: Step S1 includes: S2-1, receiving point cloud data from the laser radar, estimating the position of the radar at the time of acquisition of each point, transforming all points in a frame to the same coordinate system, and performing linear interpolation to obtain the radar pose at the time of acquisition of each point; S2-2, by analyzing the relationship between the point sets, identifying the plane points and edge points suitable as feature points; S2-3. Divide the point cloud data into voxel units, select the point closest to the voxel center in each voxel unit as a sampling point, and combine the selected sampling points to form new point cloud data.

3. The method for constructing a laser scanning radar map by flexibly combining NDT and ICP according to claim 2 is characterized in that: The estimated pose at each point acquisition moment is achieved using linear interpolation; Assume T k-1 It is expressed as the inter-frame motion of the k-1th frame. When dedistorting the point cloud of the kth frame, it is assumed that the motion of the LiDAR is uniform within the frame period. The formula is as follows: Among them, T k-1,i It is represented as the radar pose estimation of the i-th point in the k-1-th frame point cloud at the acquisition time; t k It is represented as the starting time of the k-1 frame point cloud cycle; t k-1 Represents the end time of the k-1 frame point cloud cycle; t k-1,i It is represented as the moment of the i-th point in the k-1 frame point cloud.

4. The method for constructing a laser scanning radar map by flexibly combining NDT and ICP according to claim 2 is characterized in that: By calculating the curvature of each point in the point set S, suitable feature points are screened out to improve the accuracy of point cloud registration. The formula is as follows: in, It is represented as a point in the point set S; j is represented as the points around i, S is represented as the set of neighborhood points of the i-th point, and c represents the curvature; Points with small curvature indicate a smooth surface and are suitable as plane feature points, while points with large curvature indicate edge points or corner points.

5. The laser scanning radar mapping method of flexible combination of NDT and ICP according to claim 1 is characterized by: Step S2 includes: S5-1, using feature matching, matching the current frame with the previously extracted feature point cloud by iteratively optimizing the residual; S5-2, introducing scene recognition factors, can more comprehensively evaluate scene structure; S5-3. In unstructured scenes, the NDT algorithm is used for pre-alignment, the point cloud features are mapped using normal distribution, and the sum of probabilities between point clouds is maximized by optimizing the transformation matrix; the ICP algorithm is used for fine alignment, in which the singular value decomposition method is used to ensure that the global optimal solution is found and the numerical stability is improved to achieve accurate alignment of point cloud data.

6. The method for constructing a laser scanning radar map by flexibly combining NDT and ICP according to claim 5 is characterized in that: Match the feature points of the current frame with the feature point cloud extracted in the first stage, using the line features or surface features in the point cloud for matching; optimize the residual by iterative method, the residual is constructed by the distance from point to line or point to surface; The distance calculation formula from the edge point in the origin point cloud to the edge line in the target point cloud and from the plane point in the origin point cloud to the plane in the target point cloud is as follows: Among them, d edge It is expressed as the distance from the edge point in the origin point cloud to the edge line in the target point cloud; d plane It is represented as the distance from the plane point in the original point cloud to the plane in the target point cloud; j, k, l, m are represented as their feature identifiers in the corresponding sets of edge points and plane points respectively; Represented as edge points or plane points of the K+1th frame in the original point cloud; Represented as the matching point of the kth frame in the target point cloud.

7. The method for constructing a laser scanning radar map by flexibly combining NDT and ICP according to claim 5 is characterized in that: Comprehensively consider the impact of feature matching ratio and normal vector consistency on scene structure; The formula for the feature matching ratio is as follows: Among them, R feature Expressed as the ratio of feature matching; F n Represents the total number of feature matches before the ICP algorithm performs point cloud registration for each frame; PC n Represented as the total number of point clouds in the corresponding frame; 10K line It is represented as an amplification factor introduced to ensure that the feature matching maintains sufficient sensitivity; K line Represented as the harness of the lidar used; The formula for the normal vector angle is as follows: Among them, θ represents the normal vector angle, n1 and n2 are the normal vectors of adjacent points; A comprehensive evaluation index can be defined, namely the scene identification factor SIF, the formula is as follows: Where N represents the total number of adjacent point pairs for normal vector calculation; θ i It is expressed as the angle between the normal vectors of the i-th pair of adjacent points; Compare the SIF value with the threshold V0. When SIF is greater than V0, the scene tends to be structured. When SIF is less than V0, it means that the motion speed is fast and ICP tends to converge to the local extreme value. The unstructured scene tends to be more unstructured.

8. The method for constructing a laser scanning radar map by flexibly combining NDT and ICP according to claim 5 is characterized in that: The pre-registration using the NDT algorithm comprises the following steps: S8-1,NDT algorithm divides the point cloud data into multiple cubic grids by setting a certain voxel size; S8-2. In each grid, the coordinate values ​​of the point cloud are counted and their mean μ and covariance ε are calculated. These point cloud data are converted into a probability density function with normal distribution characteristics. The formula is as follows: Among them, p(x) represents the probability density at point x; x represents the vector of two coordinates; It is represented as a normalized constant; exp{} represents an exponential operation with e as the base; S8-3. After the point cloud coordinates P are transformed by the initial rotation matrix R and the translation vector t, their position in the reference coordinate system is updated to P', and the formula is as follows P'=R×P+t S8-4. According to the probability density function of each corresponding grid, the probability of each point appearing in the corresponding grid is accumulated to obtain the registration score of the NDT algorithm. The formula is as follows: Where m represents the total number of point clouds in the registration frame, and the pose transformation α is expressed in algebraic form; p(P′ i ) is represented by the probability density function of the i-th point in the corresponding grid after transformation; The function score(α) of α is set as the objective function of the Gauss-Newton method. The objective function of the Gauss-Newton method can be expressed as the error function E after point cloud transformation. The formula is as follows: Where Q represents the point corresponding to P' in the reference coordinate system, ‖·‖ represents the Euclidean norm; S8-5. Calculate the adjustment amount of posture transformation according to the score. The formula is as follows: R new =R old +ΔR t new =t old +Δt Among them, ΔR and Δt are the posture transformation parameters calculated according to Gauss-Newton iteration; S8-6, update the posture transformation parameters, and check whether the iteration termination condition is met; if not, repeat steps S8-3 to S8-6 until the iteration termination condition is met and the optimal solution is reached.

9. The method for constructing a laser scanning radar map by flexibly combining NDT and ICP according to claims 6-8, characterized in that: The ICP algorithm is used for the second iteration to calculate the distance d from the edge point in the original point cloud to the edge line in the target point cloud. edge The distance d from the plane point in the origin point cloud to the plane in the target point cloud plane The constraints are unified and the formula is as follows: Where R is the rotation matrix; t is the translation vector; loss is the loss function for calculating R and t; The loss function unified with the distance function is used to calculate the rotation matrix R and translation vector t that minimize the pose transformation parameters. The formula is as follows: Among them, F n It is expressed as the number of matched feature points; arg min is expressed as the minimum value of the parameter; The singular value decomposition method is used to directly solve the spatial transformation matrix of each iteration to improve numerical stability and accuracy; When the number of iterations is greater than 2, the point cloud data of the reference frame is updated, and the laser odometer data is published by matching the current frame with the previous frame.

10. The laser scanning radar mapping method of flexible combination of NDT and ICP according to claim 1 is characterized by: Use the NDT algorithm for preliminary non-rigid registration, correct the posture deviation caused by the odometer error, and apply the ICP algorithm for fine adjustment; The output poses are fused and the map is updated, and finally the trajectory and map are output to provide support for subsequent path planning and navigation tasks.

Citation Information

Cited By

  • Model real-time updating method for laser radar space target pose measurement

    CN119471634A