A Pose Transformation Estimation Method Based on an Improved NDT Algorithm
By improving the NDT algorithm and combining corner matching and penalty coefficient η, the point cloud matching process is optimized, which solves the problem of insufficient accuracy of the NDT algorithm in environments with many corners. It achieves high-precision pose transformation estimation with low computational cost and is suitable for real-time operation in complex environments.
Patent Information
- Application Number
- CN202410027530.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-01-08
- Publication Date
- 2026-11-17
- Estimated Expiration
- 2044-01-08
AI Technical Summary
Existing NDT algorithms lack sufficient accuracy in pose transformation estimation in environments with many corners, and the computational load increases with the amount of point cloud data, affecting the accuracy and robustness of real-time operations.
An improved NDT algorithm is adopted, which optimizes the point cloud matching process by combining point-to-point and point-to-point fusion with corner matching and penalty coefficient η, thereby improving the estimation accuracy of corner environment. Furthermore, the grid point cloud is characterized by Gaussian distribution, reducing the amount of computation.
It improves the accuracy of pose transformation estimation in environments with many corners, while maintaining the algorithm performance in environments with fewer corners, reducing the amount of computation and improving the accuracy and robustness of real-time operation.
Smart Images

Figure CN117876986B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of mobile robot localization technology, specifically relating to a pose transformation estimation method based on an improved NDT algorithm. Background Technology
[0002] In the field of mobile robotics, continuous advancements in localization technology are crucial for achieving intelligent navigation and task execution. Within this field, the ICP and NDT algorithms are demonstrating remarkable superiority. The ICP algorithm, through iterative adjustments to transformation parameters, can align two sets of point clouds to their optimal state, minimizing the error between them. The NDT algorithm, by modeling the distribution of point cloud data, can more comprehensively capture environmental features. This transformation method, based on a normal distribution, makes the algorithm more suitable for handling complex and changing environments.
[0003] Generally, ICP results do not always guarantee globally optimal performance and are highly dependent on their initial guess. Given an incorrect initial pose, ICP-based methods can produce a locally optimal or incorrect solution. Furthermore, a drawback is that the computational cost increases proportionally with the amount of point cloud data. In contrast, the NDT algorithm, which utilizes a Gaussian probability distribution for scan matching, minimizes the impact of increased point cloud data, making it more accurate and robust than ICP in real-time operations.
[0004] In the classic NDT algorithm, all radar points are uniformly distributed when constructing a normal distribution. However, in areas with a large number of corner points, this point-to-point distribution method can be omitted, and a point-to-point method can be used to improve the accuracy of pose transformation estimation.
[0005] This invention designs a pose transformation estimation method based on an improved NDT algorithm. Based on the distributed point-to-point method of the classic NDT algorithm, it proposes a method that combines distributed point-to-point and point-to-point methods. This method can improve the pose transformation estimation accuracy in environments with many corner points, while the algorithm performance is the same as the classic NDT algorithm in environments with fewer corner points. Summary of the Invention
[0006] The purpose of this invention is to overcome the shortcomings of existing technologies and provide a pose transformation estimation method based on an improved NDT algorithm. This invention is achieved through the following technical solutions:
[0007] A pose transformation estimation method based on an improved NDT algorithm includes the following steps:
[0008] S1: Turn on the robot and acquire 2D radar data and 3D point cloud map;
[0009] S2: Perform initialization and calculate the initial pose guess;
[0010] S3: Get the corner points of the current point cloud and the corner points of the previously generated point map;
[0011] S4: Match the corner points of the current point cloud with the corner points of the previously generated point map;
[0012] S5: Set a penalty coefficient η and improve the NDT algorithm based on corner matching;
[0013] S6: By continuously accumulating the optimal transformation obtained in S5, the pose transformation relationship of the radar coordinate system with respect to the world coordinate system is obtained.
[0014] Preferably, the initial pose guess in S2 is obtained from the centroid and covariance matrix of the point cloud, wherein the formulas for obtaining the centroid C and the covariance matrix H are:
[0015]
[0016]
[0017] Where, p i Let N be the coordinates of the i-th point in the point cloud, and N be the total number of points in the point cloud.
[0018] More preferably, the initial pose guess is calculated in step S2, including the following steps:
[0019] S21: Extract features from the point cloud data of the current frame and the reference frame, including the centroid C and covariance matrix H of the point cloud, using the following formula:
[0020]
[0021]
[0022] Where, p i Let be the coordinates of the i-th point in the point cloud, and N be the total number of points in the point cloud;
[0023] S22: Calculate the initial pose transformation using the calculated features, where the rotation matrix R is obtained from singular value decomposition, and the translation vector t is calculated using the following formula:
[0024] t = C curr -C ref
[0025] Among them, C curr C is the centroid of the current frame. ref The centroid of the reference frame;
[0026] S23: Construct the initial pose transformation T init The formula is:
[0027]
[0028] Among them, T init It is the initial pose transformation matrix.
[0029] Preferably, the method for determining the corner points in S3 is as follows: determine whether a point is a corner point by calculating the curvature of each point.
[0030] More preferably, the method for determining the corner points in S3 includes the following steps:
[0031] S31: For each point p i Calculate the covariance matrix H i From the covariance matrix H i The eigenvalue decomposition yields the normal vector n i The covariance matrix H is calculated. i The formula is:
[0032]
[0033] in, It is the neighborhood N i The average of all points;
[0034] S32: Calculate the curvature k at each point i The formula is:
[0035]
[0036] Where λ1, λ2, λ3 are the eigenvalues of the normal vector;
[0037] S33: Set a threshold τ, when k i When the value is greater than τ, the point is determined to be a corner point;
[0038] S34: Following steps S31-S34, iteratively calculate the corner point p of the current point cloud. i,feature And the corner point p of the previously generated point map i,target .
[0039] Preferably, in step S4, the corner points of the current point cloud are matched with the corner points of the previously generated point map. A match is considered complete when the probability of a match between the two points is 1. The formula is as follows:
[0040]
[0041] in, Let P(p) be the inverse covariance matrix of the current point cloud corner points. i,feature ) represents the probability of a two-point match. When this value is 1, it is determined to be a two-point match.
[0042] Preferably, in step S5, a penalty coefficient η is set. When a point is a corner point, the value of η corresponding to that point is 0; otherwise, the value of η is 1.
[0043] Preferably, in step S5, the improved NDT algorithm based on corner matching calculates the probability distribution function f(p) corresponding to each radar point in the current frame. The specific calculation formula is as follows:
[0044]
[0045] in, For the mean, ∑ -1 It is the inverse of the covariance matrix. Let p be the inverse covariance matrix of the current point cloud corner points, and p represent the position of the radar point. target η is the corner point on the map that matches the radar point, and η is the penalty coefficient.
[0046] More preferably, in S5, the improved NDT algorithm based on corner matching divides the point cloud into different grid cells before calculating the probability distribution function f(p); then, a Gaussian distribution is used to characterize the distribution, mean, and covariance matrix of the point cloud in each grid cell.
[0047] More preferably, the improved NDT algorithm based on corner matching in S5 is optimized by minimizing the negative log-likelihood function to obtain the optimal transformation, as shown in the following formula:
[0048]
[0049] in, Indicates the use of attitude transformation Transformation point p k And find the probability distribution function of the transformed points. Attached Figure Description
[0050] The present invention will be further described with reference to the accompanying drawings, but the embodiments in the drawings do not constitute any limitation on the present invention. For those skilled in the art, other drawings can be obtained based on the following drawings without creative effort.
[0051] Figure 1 The flowchart below shows a pose transformation estimation method based on an improved NDT algorithm according to the present invention.
[0052] Figure 2 This is a corner extraction image for a pose transformation estimation method based on an improved NDT algorithm according to the present invention. Detailed Implementation
[0053] The following detailed description of a pose transformation estimation method based on an improved NDT algorithm, with reference to specific embodiments, is provided. These embodiments are for comparison and explanation purposes only, and the present invention is not limited to these embodiments.
[0054] Example 1 :
[0055] like Figure 1 As shown: A pose transformation estimation method based on an improved NDT algorithm includes the following steps:
[0056] S1: Turn on the robot and acquire 2D radar data and 3D point cloud map;
[0057] S2: Perform initialization and calculate the initial pose guess;
[0058] S3: Get the corner points of the current point cloud and the corner points of the previously generated point map;
[0059] S4: Match the corner points of the current point cloud with the corner points of the previously generated point map;
[0060] S5: Set a penalty coefficient η and improve the NDT algorithm based on corner matching;
[0061] S6: By continuously accumulating the optimal transformation obtained in S5, the pose transformation relationship of the radar coordinate system with respect to the world coordinate system is obtained.
[0062] The initial pose guess is calculated in step S2, including the following steps:
[0063] S21: Extract features from the point cloud data of the current frame and the reference frame, including the centroid C and covariance matrix H of the point cloud, using the following formula:
[0064]
[0065]
[0066] Where, p i Let be the coordinates of the i-th point in the point cloud, and N be the total number of points in the point cloud;
[0067] S22: Calculate the initial pose transformation using the calculated features, where the rotation matrix R is obtained from singular value decomposition, and the translation vector t is calculated using the following formula:
[0068] t = C curr -C ref
[0069] Among them, C curr C is the centroid of the current frame. ref The centroid of the reference frame;
[0070] S23: Construct the initial pose transformation T init The formula is:
[0071]
[0072] Among them, Tinit It is the initial pose transformation matrix.
[0073] The method for extracting the corner points in S3 includes the following steps:
[0074] S31: For each point p i Calculate the covariance matrix H i From the covariance matrix H i The eigenvalue decomposition yields the normal vector n i The covariance matrix H is calculated. i The formula is:
[0075]
[0076] in, It is the neighborhood N i The average of all points;
[0077] S32: Calculate the curvature k at each point i The formula is:
[0078]
[0079] Where λ1, λ2, λ3 are the eigenvalues of the normal vector;
[0080] S33: Set a threshold τ, when k i When the value is greater than τ, the point is determined to be a corner point, and the extracted corner point is as follows: Figure 2 As shown;
[0081] S34: Following steps S31-S34, iteratively calculate the corner point p of the current point cloud. i,feature And the corner point p of the previously generated point map i,target .
[0082] In step S4, the corner points of the current point cloud are matched with the corner points of the previously generated point map. A match is determined when the probability of a match between two points is 1. The formula is as follows:
[0083]
[0084] in, Let P(p) be the inverse covariance matrix of the current point cloud corner points. i,feature ) represents the probability of a two-point match. When this value is 1, it is determined to be a two-point match.
[0085] In S5, a penalty coefficient η is set. When a point is a corner point, the value of η corresponding to that point is 0; otherwise, the value of η is 1.
[0086] The improved NDT algorithm based on corner matching in S5 specifically includes the following steps:
[0087] S51: Perform grid cell segmentation on the spatial environment, dividing the point cloud into different grid cells;
[0088] S52: The distribution of the point cloud in each grid cell is characterized by a Gaussian distribution, with the mean and covariance matrices as follows:
[0089]
[0090]
[0091] in, Let p be the mean, ∑ be the covariance matrix, and p be the mean. k=1,…,m This represents the positions of all radar points in a grid;
[0092] S53: Based on the initial solution, convert the radar points in the current frame to the reference frame and determine which cell they belong to;
[0093] S54: Establish a penalty coefficient and calculate the probability distribution function for each radar point in the current frame, using the following formula:
[0094]
[0095] in, For the mean, ∑ -1 It is the inverse of the covariance matrix. Let p be the inverse covariance matrix of the current point cloud corner points, and p represent the position of the radar point. target Let η be the corner point on the map that matches the radar point.
[0096] Penalty coefficient;
[0097] S55: Minimize the negative log-likelihood function to obtain the optimal transformation, as shown in the following formula:
[0098]
[0099] in, Indicates the use of attitude transformation Transformation point p k And find the probability distribution function of the transformed points.
[0100] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.
Claims
1. A pose transformation estimation method based on an improved NDT algorithm, characterized in that, Includes the following steps: S1: Turn on the robot and acquire 2D radar data and 3D point cloud map; S2: Perform initialization and calculate the initial pose guess; S3: Get the corner points of the current point cloud and the corner points of the previously generated point map; S4: Match the corner points of the current point cloud with the corner points of the previously generated point map; S5: Establish a penalty coefficient An improved NDT algorithm based on corner matching; S6: Continuously accumulate the optimal transformation obtained in S5 to obtain the pose transformation relationship of the radar coordinate system with respect to the world coordinate system; The penalty coefficient is set in S5. When a point is a corner point, the corresponding point is... The value is 0, otherwise The value is 1; In S5, the improved NDT algorithm based on corner matching calculates the probability distribution function corresponding to each radar point in the current frame. The specific calculation formula is as follows: in, The mean, Let be the inverse covariance matrix of the current point cloud. Let be the inverse covariance matrix of the current point cloud corner points. p Indicates the location of the radar point. The penalty coefficient is... The corner point on the map that matches the radar point; The improved NDT algorithm based on corner matching in S5 calculates the probability distribution function. Previously, the point cloud was divided into different grid cells; then, a Gaussian distribution was used to characterize the distribution of the point cloud in each grid cell, with the mean and covariance matrix as shown in the following formula: in, The mean, Let covariance matrix be the variance matrix. This represents the location of all radar points in a grid.
2. The pose transformation estimation method based on the improved NDT algorithm according to claim 1, characterized in that, The initial pose guess in S2 is obtained from the centroid and covariance matrix of the point cloud, wherein the centroid is obtained... Covariance Matrix The formula is: in, For the first point cloud i The coordinates of the points N It is the total number of points in the point cloud.
3. The pose transformation estimation method based on the improved NDT algorithm according to claim 1, characterized in that, The initial pose guess is calculated in step S2, which specifically includes the following steps: S21: Extract features from the point cloud data of the current frame and the reference frame, including the centroid of the point cloud. covariance matrix The formula is: in, For the first point cloud i The coordinates of the points N It is the total number of points in the point cloud; S22: Calculate the initial pose transformation using the calculated features, where the rotation matrix... The translation vector is obtained from singular value decomposition. The calculation formula is: in, The centroid of the current frame. The centroid of the reference frame; S23: Construct the initial pose transformation The formula is: in, It is the initial pose transformation matrix.
4. The pose transformation estimation method based on the improved NDT algorithm according to claim 1, characterized in that, The method for determining the corner points in S3 is to determine whether a point is a corner point by calculating the curvature of each point.
5. The pose transformation estimation method based on the improved NDT algorithm according to claim 1, characterized in that, The method for extracting the corner points in S3 specifically includes the following steps: S31: For each point Calculate the covariance matrix From the covariance matrix Eigenvalue decomposition yields normal vectors The covariance matrix is calculated. The formula is: in, It is the neighborhood The average of all points; S32: Calculate the curvature at each point The formula is: in, These are the eigenvalues of the normal vector; S33: Set a threshold ,when When that point is a corner point; S34: Following steps S31-S34, iteratively calculate the corner points of the current point cloud. and the corner points of the previously generated point map .
6. The pose transformation estimation method based on the improved NDT algorithm according to claim 1, characterized in that, In step S4, the corner points of the current point cloud are matched with the corner points of the previously generated point map. A match is determined when the probability of a match between two points is 1. The probability is calculated using the following formula: in, Let be the inverse covariance matrix of the current point cloud corner points. This represents the probability of a two-point match. When this probability is 1, the two points are considered a match.
7. The pose transformation estimation method based on the improved NDT algorithm according to claim 1, characterized in that, The improved NDT algorithm based on corner matching in S5 optimizes by minimizing the negative log-likelihood function to obtain the optimal transformation, as shown in the following formula: in, Indicates the use of attitude transformation Conversion point And find the probability distribution function of the transformed points.
Citation Information
Patent Citations
A fast monocular vision odometer navigation and positioning method combining a feature point method and a direct method
CN109544636A
Grid map positioning method fusing NDT and ICP
CN112612029A
