Vehicle positioning method based on laser radar point cloud data

Abstract

The invention provides a vehicle positioning method based on laser radar point cloud data. The method comprises the following steps: acquiring a source point cloud and a target point cloud; using an improved point-to-surface iterative nearest point algorithm to decentralize the source point cloud and the target point cloud, calculating a rotation matrix and a translation vector according to decentralized coordinates of the source point cloud and the target point cloud, and combining input disturbance and Lie algebra to obtain an initial registration conversion formula of the source point cloud; performing Gaussian Newton iteration on the initial registration conversion formula to obtain a registration conversion formula of the source point cloud, and performing registration on the source point cloud and the target point cloud to obtain registration point cloud data; and constructing a map by using the registered point cloud data, and positioning the vehicle. According to the invention, the technical problem that the algorithm precision is not high when vehicle positioning is carried out through point cloud data in the prior art can be solved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving technology, and in particular to a vehicle positioning method based on laser radar point cloud data. Background Art

[0002] In the field of unmanned driving, Lidar-based SLAM and visual SLAM are the two main SLAM technologies. Among them, Lidar SLAM technology relies on Lidar as the main sensor to determine the position and distance of surrounding objects by emitting laser pulses and measuring their reflection time. Lidar provides highly accurate distance measurement, so it is very accurate in map construction and positioning. This technology is not sensitive to lighting conditions and weather changes, and can operate stably in a variety of environments.

[0003] As the front-end and core step in laser SLAM, the laser odometry is used to determine the change in the vehicle position over time, which largely determines the accuracy and robustness of the laser SLAM system. Existing laser odometry will inevitably produce cumulative errors during long-term incremental scanning. Excessive cumulative errors will cause positioning drift during vehicle driving, making the constructed point cloud map impossible to use normally, and unable to guarantee the safe and reliable operation of the unmanned driving system.

[0004] Therefore, there is an urgent need for a more accurate vehicle positioning method to assist autonomous driving and complete the positioning of the vehicle when driving in an unknown environment. Summary of the invention

[0005] In view of the shortcomings of the prior art, the present invention proposes a vehicle positioning method based on laser radar point cloud data to solve the technical problem in the prior art that the algorithm accuracy is not high for vehicle positioning through point cloud data.

[0006] The technical solution adopted by the present invention is as follows:

[0007] In a first aspect, a vehicle positioning method based on laser radar point cloud data is provided, comprising:

[0008] Obtain source point cloud and target point cloud; use the improved point-to-plane iterative closest point algorithm to align the source point cloud and target point cloud to obtain aligned point cloud data; use the aligned point cloud data to build a map and locate the vehicle.

[0009] Furthermore, the source point cloud and the target point cloud are obtained in the following way:

[0010] A plurality of point cloud data are obtained by scanning at preset time intervals; for a point cloud at a certain moment, the point cloud is defined as a target point cloud of a point cloud at a previous moment and a source point cloud of a point cloud at a next moment.

[0011] Furthermore, before the source point cloud and the target point cloud are registered using the improved point-to-plane iterative closest point algorithm, the source point cloud and the target point cloud are preprocessed, including point cloud downsampling and point cloud denoising.

[0012] Furthermore, the source point cloud and the target point cloud are registered using an improved point-to-plane iterative closest point algorithm, including: separately decentralizing the source point cloud and the target point cloud to obtain the decentralized coordinates of the source point cloud and the decentralized coordinates of the target point cloud; calculating the rotation matrix and the translation vector according to the decentralized coordinates of the source point cloud and the decentralized coordinates of the target point cloud; calculating the registration transformation formula of the source point cloud according to the rotation matrix and the translation vector; and registering the point cloud data according to the registration transformation formula.

[0013] Furthermore, the rotation matrix and the translation vector are calculated according to the decentralized coordinates of the source point cloud and the decentralized coordinates of the target point cloud, including: obtaining the center of mass of the source point cloud and the center of mass of the target point cloud; decomposing the original matrix into a diagonal matrix and a first orthogonal matrix and a second orthogonal matrix; multiplying the first and second orthogonal matrices to obtain a rotation matrix; and calculating the translation vector according to the rotation matrix and the center of mass of the source point cloud and the center of mass of the target point cloud.

[0014] Furthermore, the registration transformation formula of the source point cloud is calculated according to the rotation matrix and the translation vector, including:

[0015] According to the rotation matrix and the translation vector, the initial registration transformation formula of the source point cloud is obtained by combining the input disturbance and Lie algebra; the initial registration transformation formula is subjected to Gauss-Newton iteration to obtain the registration transformation formula of the source point cloud.

[0016] Furthermore, according to the rotation matrix, translation vector, input perturbation and Lie algebra, the initial registration transformation formula of the source point cloud is obtained, including:

[0017] Assume that a source point cloud p undergoes a transformation T, and obtains a spatial point Tp after matrix transformation. The corresponding Lie algebra is ξ. At this time, multiply T by a perturbation on the left to obtain:

[0018]

[0019] In the above formula, Tp represents the spatial point p after matrix transformation, δ represents the input perturbation, ξ represents the Lie algebra, I represents the unit matrix, R represents the rotation matrix, p represents the source point cloud, and t represents the translation vector;

[0020] Then, according to the above formula, the non-homogeneous coordinates of the source point cloud p are taken, and the initial registration transformation formula can be obtained as follows:

[0021]

[0022] In the above formula, Tp represents the spatial point p after matrix transformation, δ represents the input disturbance, I represents the unit matrix, R represents the rotation matrix, p represents the source point cloud, and t represents the translation vector; the Lie algebra ξ∈se(3), the first three dimensions represent the translation denoted by ρ, and the last three dimensions represent the rotation denoted by φ, has:

[0023]

[0024] In this embodiment, for the Lie algebra φ∈so(3), the symbol ^ is defined as follows:

[0025]

[0026] In the above formula, a1, a2, and a3 represent matrix conversion coefficients.

[0027] Furthermore, the initial registration transformation is subjected to Gauss-Newton iteration, including:

[0028] Solve the current descent vector Δx according to the incremental equation for the initial registration transformation k , if Δx k If it is less than the threshold, the iteration ends;

[0029] Otherwise, let x k+1 =x k +Δx, continue iterating until Δx k Less than the threshold.

[0030] In a second aspect, an electronic device is provided, comprising: one or more processors; a storage device for storing one or more programs; when the one or more programs are executed by one or more processors, the one or more processors implement the vehicle positioning method based on laser radar point cloud data provided in the first aspect.

[0031] In a third aspect, a computer program product is provided, comprising a computer program / instruction, which, when executed by a processor, implements the steps of the vehicle positioning method based on lidar point cloud data provided in the first aspect.

[0032] It can be seen from the above technical solution that the beneficial technical effects of the present invention are as follows:

[0033] For laser point cloud data, an improved point-to-plane iterative closest point algorithm is used to align the source point cloud and the target point cloud. This algorithm is more suitable for processing point cloud data containing planar areas. The normal information provides additional geometric constraints, making the alignment more precise and achieving better alignment results. Maps constructed using point cloud data aligned using this method can achieve more accurate vehicle positioning results. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following is a brief introduction to the drawings required for the specific embodiments or the prior art description. In all the drawings, similar elements or parts are generally identified by similar reference numerals. In the drawings, the elements or parts are not necessarily drawn according to the actual scale.

[0035] Figure 1 A schematic diagram of a vehicle positioning method flow chart according to an embodiment of the present invention;

[0036] Figure 2 It is a schematic diagram of the point cloud registration effect of an embodiment of the present invention. DETAILED DESCRIPTION

[0037] The following embodiments of the technical solution of the present invention are described in detail in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and are therefore only used as examples, and cannot be used to limit the protection scope of the present invention.

[0038] It should be noted that, unless otherwise specified, the technical terms or scientific terms used in this application should have the common meanings understood by those skilled in the art to which the present invention belongs.

[0039] Example

[0040] Step S1: Use laser odometer to obtain source point cloud and target point cloud

[0041] In a specific implementation, the laser odometer scans at a preset time interval (such as 1 second) to obtain multiple point cloud data. For a point cloud at a certain moment, the point cloud is defined as the target point cloud of the point cloud at the previous moment and the source point cloud of the point cloud at the next moment. Specifically, assuming that the point cloud scanned by the laser odometer is one point cloud data per second, then the point cloud of the first second is the source point cloud relative to the point cloud of the second second, and the point cloud of the second second is the target point cloud relative to the point cloud of the first second. By analogy, after another second, the point cloud of the second second is the source point cloud relative to the point cloud of the third second, and the point cloud of the third second is the target point cloud relative to the point cloud of the second second.

[0042] Optionally, the source point cloud is preprocessed, including point cloud downsampling and point cloud denoising.

[0043] In some embodiments, point cloud downsampling may use voxel filtering. Voxel filtering is a sampling-based filtering method that divides point cloud data into small cubic units, called voxels, and then selects a representative point in each voxel to achieve downsampling of point cloud data; this method can effectively remove noise and details and retain the overall shape of the point cloud data. Specifically, the following steps are included: determining the voxel size and sampling density; dividing the point cloud data into voxel units; selecting a representative point in each voxel; and outputting the downsampled point cloud data.

[0044] In some embodiments, point cloud denoising can be performed using statistical filtering. Statistical filtering assumes that the distribution of the point cloud is a Gaussian distribution with a mean and a standard deviation, and treats all points whose average distance is outside the interval defined by the global distance mean and standard deviation as outliers and prunes them from the data set, which has a good removal effect on sparse outliers.

[0045] Step S2: Use the improved point-to-plane iterative closest point algorithm to align the source point cloud and the target point cloud to obtain the aligned point cloud data.

[0046] This step specifically includes the following sub-steps:

[0047] Step S21: Decentralize the source point cloud and the target point cloud respectively to obtain the decentralized coordinates of the source point cloud and the decentralized coordinates of the target point cloud.

[0048] The specific methods of decentralization are as follows:

[0049] First, the source point cloud P = {p1, ..., p n}, the center of mass p0, the target point cloud Q = {q1,…,q n The centroid q0 of}:

[0050]

[0051] In the above formula, p0 represents the centroid of the source point cloud, p i Represents each point cloud data in the source point cloud; q0 represents the centroid of the target point cloud, q i Represents the point cloud data in the target point cloud.

[0052] Then, the two point cloud coordinates are decentralized as follows to obtain the decentralized coordinates of the source point cloud P' = {p'1, ..., p' n}, the decentralized coordinates of the target point cloud Q' = {q'1, ...., q' n}:

[0053] p′ i =p i -p0

[0054] q′i =q i -q0

[0055] In the above formula, p0 represents the centroid of the source point cloud, p i Represents each point cloud data in the source point cloud; q0 represents the centroid of the target point cloud, q i Represents the point cloud data in the target point cloud; p′ i Represents the decentralized coordinates of the source point cloud, q′ i Represents the decentralized coordinates of the target point cloud.

[0056] Step S22: Calculate the rotation matrix and translation vector according to the decentralized coordinates of the source point cloud and the decentralized coordinates of the target point cloud.

[0057] First, according to the decentralized coordinates of the source point cloud and the decentralized coordinates of the target point cloud, the original matrix H is calculated as follows:

[0058]

[0059] In the above formula, p′ i Represents the decentralized coordinates of the source point cloud, q′ i Represents the decentralized coordinates of the target point cloud.

[0060] Then, the original matrix H is decomposed into a diagonal matrix A and a first orthogonal matrix U and a second orthogonal matrix V as follows:

[0061] H=UAV t

[0062] Then multiply the first and second orthogonal matrices to get the rotation matrix R, as follows:

[0063] R=VU t

[0064] Finally, according to the rotation matrix and the center of mass of the source point cloud and the center of mass of the target point cloud, the translation vector t is calculated as follows:

[0065] t=q0-Rp0

[0066] In the above formula, q0 represents the center of mass of the target point cloud, R is the rotation matrix, and p0 represents the center of mass of the source point cloud.

[0067] Step S23: Calculate the registration transformation formula of the source point cloud according to the rotation matrix and translation vector

[0068] This step S23 is divided into the following sub-steps:

[0069] Step S231: According to the rotation matrix, translation vector, input disturbance and Lie algebra, the initial registration transformation formula of the source point cloud is obtained.

[0070] First, suppose a source point cloud p undergoes a transformation T to obtain a spatial point Tp after matrix transformation. The corresponding Lie algebra is ξ. At this time, multiply T by a perturbation ΔT = exp(δξ^) on the left, and we can get:

[0071]

[0072] In the above formula, Tp represents the spatial point p after matrix transformation, δ represents the input perturbation, ξ represents the Lie algebra, I represents the unit matrix, R represents the rotation matrix, p represents the source point cloud, and t represents the translation vector.

[0073] Then, according to the above formula, the non-homogeneous coordinates of the source point cloud p are taken, and the initial registration transformation formula can be obtained as follows:

[0074]

[0075] In the above formula, Tp represents the spatial point p after matrix transformation, δ represents the input disturbance, I represents the unit matrix, R represents the rotation matrix, p represents the source point cloud, and t represents the translation vector; the Lie algebra ξ∈se(3), the first three dimensions represent the translation denoted by ρ, and the last three dimensions represent the rotation denoted by φ, has:

[0076]

[0077] In this embodiment, for the Lie algebra φ∈so(3), the symbol ^ is defined as follows:

[0078]

[0079] In the above formula, a1, a2, and a3 represent matrix conversion coefficients.

[0080] Step S232: Perform Gauss-Newton iteration on the initial registration transformation formula to obtain the registration transformation formula of the source point cloud.

[0081] When performing Gauss-Newton iteration on the initial registration transformation, the iteration function f(x k ) and the increment equation.

[0082] The process of iterating the initial registration transformation using the incremental equation is as follows:

[0083] According to the Taylor series expansion principle, the first-order Taylor expansion of any function f(x) is as follows:

[0084] f(x+Δx)≈f(x)+J(x)Δx

[0085] In the above formula, J(x) is the derivative of f(x) with respect to x, which corresponds to the initial registration transformation formula in this embodiment.

[0086] To find the descent vector such that ||f(x+Δx)|| 2 To achieve the minimum, we need to solve the following linear least squares problem:

[0087]

[0088] Taking the derivative of the above formula with respect to Δx and setting it equal to zero, we can obtain:

[0089] J(x) T J(x)Δx=-J(x) T f(x)

[0090] The above equation is the incremental equation, which is a linear equation and can be used to calculate the value of the descent vector.

[0091] Solve the current descent vector Δx according to the incremental equation k , if Δx k If x is less than the threshold, the iteration ends. Otherwise, let x k+1 =x k +Δx, continue iterating until Δx k The threshold is set according to the actual application environment.

[0092] The Gauss-Newton iteration linearizes the residual function with the help of the first-order Taylor expansion to approximate the nonlinear least squares problem. And through multiple iterations, the regression coefficient is corrected to gradually approach the optimal value, and the optimal registration transformation is obtained.

[0093] After Gauss-Newton iteration, for the improved point-to-plane iterative closest point algorithm in this embodiment, the source point cloud p to be matched i The registration transformation can be expressed as follows:

[0094] J i (T) = n i T [-I(Rp+t)^]

[0095] In the above formula, n i represents the surface normal vector of each target point cloud, I represents the unit matrix, p represents the source point cloud, R is the rotation matrix, and t is the translation vector.

[0096] In a specific implementation, the normal line is obtained by fitting the plane of the neighboring points around the target point through neighborhood search. i There will be a surface normal vector n i .

[0097] Step S23: register the point cloud data according to the registration transformation formula to obtain the registered point cloud data.

[0098] After the point cloud data is registered, the registration effect is as follows: Figure 2 As shown in the figure, before registration, there is usually an obvious misalignment between the source point cloud and the target point cloud. After registration, the source point cloud and the target point cloud are aligned, showing a more accurate alignment effect.

[0099] This step adopts an improved point-to-plane iterative closest point algorithm, which is more suitable for processing point cloud data containing planar areas (such as the ground, walls, etc.). The normal information provides additional geometric constraints, making the registration more accurate and achieving better alignment effects.

[0100] Step S3: Use the registered point cloud data to build a map and locate the vehicle

[0101] In this embodiment, the registration point cloud data is used to construct a map, and the implementation method of positioning the vehicle is not limited, and can be implemented in any achievable manner in the prior art. For example:

[0102] The sliding window method can be used to construct the map. The sliding window is in a low-resolution map, calculates the node score, saves the resolution map with the highest score among each node, continues to calculate the node score for the map with higher resolution, selects the node with high score and then recurses. Specifically, the sliding window needs to select an appropriate angle step size to improve the matching efficiency. This step size must consider both the amount of calculation and the pose accuracy. In this embodiment, the angle step size is calculated as:

[0103]

[0104] Where, d max is the maximum distance of the laser point, is the laser point distance, S ψ is the angle step and r is the grid size.

[0105] According to the constructed map, the vehicle is positioned by any achievable method in the prior art. Using the point cloud data registered by the above method of this embodiment to construct a map can achieve a more accurate vehicle positioning effect.

[0106] In some embodiments, an electronic device is provided, comprising: one or more processors; a storage device for storing one or more programs; when the one or more programs are executed by one or more processors, the one or more processors implement the vehicle positioning method based on lidar point cloud data as described above.

[0107] In some embodiments, a computer program product is provided, including a computer program / instruction, which, when executed by a processor, implements the steps of the vehicle positioning method based on lidar point cloud data described above.

[0108] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. These modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention, and they should all be included in the scope of the claims and specification of the present invention.

Claims

1. A vehicle positioning method based on laser radar point cloud data, characterized in that: include: Get source point cloud and target point cloud; Use the improved point-to-plane iterative closest point algorithm to align the source point cloud and the target point cloud to obtain the aligned point cloud data; Use the registered point cloud data to build maps and locate the vehicle.

2. The vehicle positioning method based on laser radar point cloud data according to claim 1, characterized in that: Use the following methods to obtain the source point cloud and the target point cloud: Scanning at preset time intervals to obtain multiple point cloud data; For a point cloud at a certain moment, define the point cloud as the target point cloud of the point cloud at the previous moment and the source point cloud of the point cloud at the next moment.

3. The vehicle positioning method based on laser radar point cloud data according to claim 1, characterized in that: Before using the improved point-to-plane iterative closest point algorithm to align the source point cloud and the target point cloud, the source point cloud and the target point cloud are preprocessed, including point cloud downsampling and point cloud denoising.

4. The vehicle positioning method based on laser radar point cloud data according to claim 1, characterized in that: The source point cloud and the target point cloud are registered using an improved point-to-plane iterative closest point algorithm, including: Decentralize the source point cloud and the target point cloud respectively to obtain the decentralized coordinates of the source point cloud and the decentralized coordinates of the target point cloud; Calculate the rotation matrix and translation vector according to the decentralized coordinates of the source point cloud and the decentralized coordinates of the target point cloud; Calculate the registration transformation formula of the source point cloud based on the rotation matrix and translation vector; The point cloud data is registered according to the registration transformation.

5. The vehicle positioning method based on laser radar point cloud data according to claim 4 is characterized in that: The rotation matrix and translation vector are calculated based on the decentralized coordinates of the source point cloud and the decentralized coordinates of the target point cloud, including: Get the source point cloud centroid and the target point cloud centroid; Decomposing the original matrix into a diagonal matrix, a first orthogonal matrix, and a second orthogonal matrix; Multiply the first and second orthogonal matrices to obtain a rotation matrix; Calculate the translation vector based on the rotation matrix and the center of mass of the source point cloud and the center of mass of the target point cloud.

6. The vehicle positioning method based on laser radar point cloud data according to claim 4, characterized in that: The registration transformation formula of the source point cloud is calculated based on the rotation matrix and translation vector, including: According to the rotation matrix, translation vector, input disturbance and Lie algebra, the initial registration transformation formula of the source point cloud is obtained; The initial registration transformation formula is iterated by Gauss-Newton to obtain the registration transformation formula of the source point cloud.

7. The vehicle positioning method based on laser radar point cloud data according to claim 6, characterized in that: According to the rotation matrix, translation vector, input perturbation and Lie algebra, the initial registration transformation formula of the source point cloud is obtained, including: Assume that a source point cloud p undergoes a transformation T, and obtains a spatial point Tp after matrix transformation. The corresponding Lie algebra is ξ. At this time, multiply T by a perturbation on the left to obtain: In the above formula, Tp represents the spatial point p after matrix transformation, δ represents the input perturbation, ξ represents the Lie algebra, I represents the unit matrix, R represents the rotation matrix, p represents the source point cloud, and t represents the translation vector; Then, according to the above formula, the non-homogeneous coordinates of the source point cloud p are taken, and the initial registration transformation formula can be obtained as follows: In the above formula, Tp represents the spatial point p after matrix transformation, δ represents the input disturbance, I represents the unit matrix, R represents the rotation matrix, p represents the source point cloud, and t represents the translation vector; the Lie algebra ξ∈se(3), the first three dimensions represent the translation denoted by ρ, and the last three dimensions represent the rotation denoted by φ, has: In this embodiment, for the Lie algebra φ∈so(3), the symbol ∧ is defined as follows: In the above formula, a1, a2, and a3 represent matrix conversion coefficients.

8. The vehicle positioning method based on laser radar point cloud data according to claim 6, characterized in that: Perform Gauss-Newton iteration on the initial registration transformation, including: Solve the current descent vector Δx according to the incremental equation for the initial registration transformation k , if Δx k If it is less than the threshold, the iteration ends; Otherwise, let x k+1 =x k +Δx, continue iterating until Δx k Less than the threshold.

9. An electronic device, characterized in that: include: one or more processors; A storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the vehicle positioning method based on lidar point cloud data as described in any one of claims 1-8.

10. A computer program product comprising a computer program / instructions, characterized in that When the computer program / instruction is executed by a processor, the steps of the vehicle positioning method based on laser radar point cloud data described in any one of claims 1-8 are implemented.

Images