Lidar calibration method, apparatus, and storage medium
By employing point cloud preprocessing and matching methods, the problem of poor calibration results for low-overlapping fields of view in solid-state LiDAR was solved, achieving high-precision and fast LiDAR calibration, which is suitable for mobile robots.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SUZHOU INTELEZHEN INTELLIGENT TECH CO LTD
- Filing Date
- 2022-11-22
- Publication Date
- 2026-04-24
AI Technical Summary
Existing LiDAR calibration methods have a small overlapping field of view for solid-state LiDAR, resulting in poor calibration results or inability to calculate results. Furthermore, traditional methods have high hardware requirements or rely on deep learning, making them unsuitable for mobile robots.
By employing point cloud preprocessing and point cloud matching methods, overlapping point clouds are extracted from the initial pose transformation matrix H0 rotating sub-LiDAR point cloud. The pose transformation is optimized using a weighted sum of Euclidean distance error functions. Combined with KD tree search and sampling techniques, the calibration accuracy and speed are improved.
It achieves high-precision lidar calibration under low-overlap field-of-view conditions, improves calibration speed and robustness, is applicable to 2D and 3D lidar, and reduces hardware requirements.
Smart Images
Figure CN116299353B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of radar calibration, specifically to lidar calibration methods, devices, and storage media. Background Technology
[0002] The rise of new energy vehicles and drones has made Simultaneous Localization and Mapping (SLAM) an important research topic. LiDAR plays a crucial role in the SLAM process. Currently, LiDAR is mainly divided into two categories: 360° mechanically rotating LiDAR offers better detection performance but is extremely expensive and not durable, easily damaged. Solid-state LiDAR is less expensive but has a smaller field of view (FoV). Therefore, the common solution in the industry is to combine multiple solid-state LiDARs together to achieve the same level of environmental observation as a 360° rotating LiDAR.
[0003] LiDAR systems inherently contain manufacturing errors. Furthermore, human error can occur during sensor installation and measurement in practical applications. The accumulation of these errors will prevent the combined point cloud from closing. Clearly, the raw point cloud cannot reflect reality, affecting the accuracy of localization and mapping in SLAM. Therefore, we aim to obtain a more consistent and realistic point cloud through pre-calibrated LiDAR systems.
[0004] Most commercially available lidar calibration methods are based on calibrating point clouds with a large overlap of FoV (Field of Value). Typically, the FoV overlap of the point clouds needs to be above 65% to complete the calibration. However, solid-state lidar only has a small overlap of 5%-15%. In this case, using traditional calibration methods will result in very poor performance or even make it impossible to calculate results.
[0005] The paper "PREDATOR: Registration of 3D Point Clouds with Low Overlap" proposes a low-overlap point cloud matching model. However, this model is not suitable for mobile robots. It uses deep learning, thus placing high demands on hardware. Mobile robots need to control costs to achieve mass production feasibility. The paper "Extrinsic Calibration of Multiple LiDARs of Small FoV in Targetless Environments" provides a calibration method without a calibration reference, solving the low-overlap FoV problem by rotating the robot 360° around its center. However, this method still requires LOAM (Lidar Odometry and Mapping) to provide initial pose transformation information, and LOAM does not support 2D LiDAR. Summary of the Invention
[0006] To overcome the shortcomings of most commercially available lidar calibration methods described above, which rely on registering point clouds with significant FoV overlap (FoV), typically requiring over 65% FoV overlap for calibration, while solid-state lidar only has a small overlap of 10%-25%, the present invention addresses the poor performance of traditional calibration methods in such cases.
[0007] To achieve the above objectives, the present invention adopts the following technical solution:
[0008] A first aspect of the present invention provides a lidar calibration method, comprising point cloud preprocessing and point cloud matching.
[0009] The point cloud preprocessing includes the following steps:
[0010] S1: Obtain the pose transformation information between the parent lidar L1 and the child lidar L2. Define the initial value of the pose transformation information as a matrix H0 that includes translation and rotation. The matrix H0 is used to describe the initial pose transformation information between the parent lidar L1 and the child lidar L2. Using a matrix can facilitate subsequent numerical calculations.
[0011] S2: The point cloud P2 scanned by the sub-LiDAR L2 is rotated and transformed into point cloud P2' using the matrix H0, while keeping the point cloud P1 scanned by the parent LiDAR L1 unchanged. Since point cloud P2 uses L2 as its coordinate system in the viewpoint of LiDAR L2, it is necessary to rotate P2 and transform it to the L1 coordinate system to obtain P2' in order to obtain the true overlapping area in step S3.
[0012] S3: Extract the overlapping points O1 and O2 of point cloud P2' and point cloud P1 respectively;
[0013] S4: Based on the number of overlapping point clouds, sample the overlapping point cloud O1 to obtain the sampled overlapping point cloud O1', and sample the overlapping point cloud O2 to obtain the sampled overlapping point cloud O2'. Typically, the number of point clouds in O1 and O2 is quite large. To speed up the algorithm, downsampling significantly reduces the number of point clouds in O1 and O2.
[0014] The point cloud matching includes the following steps:
[0015] S5: Find the nearest point to each point in O1' in O2' and make them one-to-one corresponding point pairs;
[0016] S6: Weight the point pairs using the reciprocal of the absolute value of the distance between the point pairs as the weight value. Weighting point pairs can make the matching results more reliable;
[0017] S7: Discard point pairs whose absolute distance exceeds a distance threshold λ between point pairs, and point pairs located at boundaries. λ is an adaptive threshold adjusted based on whether the matching results converge. These point pairs are too far apart, so they are ignored as they are meaningless.
[0018] S8: Set the error function between points as the Euclidean distance between the points, and solve for the transformed pose information H from O1' to O2'. k H k To minimize the total error function of all point pairs, the pose transformation information H k Defined as a matrix including translation and rotation, where k represents the number of iterations. If the current transformation pose information H... k The transformed pose information H generated by the previous S5-S7 cycle k-1 If the difference is within a certain range, output H k If the result is positive, proceed to the next S5-S7 cycle; otherwise, continue.
[0019] The calibration method of this invention can calibrate both 2D and 3D lidar, and while ensuring high-precision calibration results, it can also improve calibration speed and robustness.
[0020] In some possible implementations, the matrix H0 is used to describe the initial pose transformation information between the parent lidar L1 and the child lidar L2, and H0 is a 4*4 homogeneous transformation matrix.
[0021] Where R0 is a 3x3 rotation matrix, R0 = R z (α)*R y (β)*R x (γ), R z (α), R y (β) and R x (γ) represents the angles of rotation around axes α, β, and γ from parent lidar L1 to child lidar L2; T0 is the displacement matrix, as detailed below;
[0022] T0 is a 3*1 displacement matrix, and x0, y0, and z0 are the measured displacement distances of the parent and child lidars along the x, y, and z axes, respectively.
[0023] In some possible implementations, in S2, the point cloud P2 scanned by the sub-LiDAR L2 is rotated and transformed into a point cloud P2' by the matrix H0, where P2' = P2*R0 + T0.
[0024] In some possible implementations, in S3, the overlapping points O1 and O2 of point clouds P2' and P1 are extracted respectively as follows: traverse each point in point cloud P1 or point cloud P2', then search for the nearest point of each point in point cloud P2' or point cloud P1, and determine whether the minimum distance between each point and its nearest point is less than a threshold λ. This threshold λ is an adaptively adjusted threshold based on whether the matching results converge. If it is less than λ, then the point is extracted.
[0025] In some possible implementations, during the search for the nearest point to a given point, a KD-tree model is constructed for the point cloud P2' and the point cloud P1. The KD-tree search process is as follows:
[0026] Perform a KD-tree search on P1 to determine if there exists a point in P1 within a range of λ for any given point p1. If so, add p' to O2. Perform a KD-tree search on P2' to determine if there exists a point in P2' within a range of λ for any given point p2. If so, add p2 to O2. The specific formula is as follows:
[0027]
[0028]
[0029] Where (P1,m) and (P2',m') are points with index m in point clouds P1 and P2', respectively; λ is an adaptive threshold that is adjusted based on whether the matching results converge; if the overlapping point clouds O1 and O2 tend to converge, then λ will gradually decrease according to the step size m, and vice versa.
[0030] In some possible implementations, another threshold T is set. Since the value of λ gradually decreases, the matching result always tends to be correct. When λ > T, only the overlapping point cloud of the child lidar is extracted, while the point cloud of the parent lidar is not processed. This can significantly increase the matching speed of the algorithm.
[0031] In some possible implementations, the sampling method is upsampling, specifically linear interpolation.
[0032] In some possible implementations, the sampling method downsampling is specifically voxel grid filtering.
[0033] In some possible implementations, in S5, finding the nearest point of each point in O1' in O2' is specifically as follows: for O1' and O2', the KD-tree search algorithm is used to accelerate the search for the nearest point of each point.
[0034] In some possible implementations, in S8, the error function e for a single point pair and the error function e for all point pairs are... all They are as follows:
[0035]
[0036] e all =∑e
[0037] Where ΔT is the Euclidean distance between pairs of points in S8; Δx, Δy, and Δz are the differences in the coordinates x, y, and z between pairs of points, respectively.
[0038] Using nonlinear optimization, the total error function e of all point pairs is solved. all The smallest transformation pose information H from O1' to O2' k .
[0039] A second aspect of the present invention provides a lidar calibration device, comprising a point cloud preprocessing module and a point cloud matching module.
[0040] The working steps of the point cloud preprocessing module are as follows:
[0041] S1: Obtain the pose transformation information between the parent lidar L1 and the child lidar L2. Define the initial value of the pose transformation information as a matrix H0 that includes translation and rotation. The matrix H0 is used to describe the initial pose transformation information between the parent lidar L1 and the child lidar L2. Using a matrix can facilitate subsequent numerical calculations.
[0042] S2: The point cloud P2 scanned by the sub-LiDAR L2 is rotated and transformed into point cloud P2' using the matrix H0, while keeping the point cloud P1 scanned by the parent LiDAR L1 unchanged. Since point cloud P2 uses L2 as its coordinate system in the viewpoint of LiDAR L2, it is necessary to rotate P2 and transform it to the L1 coordinate system to obtain P2' in order to obtain the true overlapping area in step S3.
[0043] S3: Extract the overlapping points O1 and O2 of point cloud P2' and point cloud P1 respectively;
[0044] S4: Based on the number of overlapping point clouds, sample the overlapping point cloud O1 to obtain the sampled overlapping point cloud O1', and sample the overlapping point cloud O2 to obtain the sampled overlapping point cloud O2'. Typically, the number of point clouds in O1 and O2 is quite large. To speed up the algorithm, downsampling significantly reduces the number of point clouds in O1 and O2.
[0045] The working steps of the point cloud pre-matching module are as follows:
[0046] S5: Find the nearest point to each point in O1' in O2' and make them one-to-one corresponding point pairs;
[0047] S6: Weight the point pairs using the reciprocal of the absolute value of the distance between the point pairs as the weight value. Weighting point pairs can make the matching results more reliable;
[0048] S7: Discard point pairs whose absolute distance exceeds a distance threshold λ between point pairs, and point pairs located at boundaries. λ is an adaptive threshold adjusted based on whether the matching results converge. These point pairs are too far apart, so they are ignored as they are meaningless.
[0049] S8: Set the error function between points as the Euclidean distance between the points, and solve for the transformed pose information H from O1' to O2'. k H k To minimize the total error function of all point pairs, the pose transformation information H k Defined as a matrix including translation and rotation, where k represents the number of iterations. If the current transformation pose information H... k The transformed pose information H generated by the previous S5-S7 cycle k-1 If the difference is within a certain range, output H kIf the result is positive, proceed to the next S5-S7 cycle; otherwise, continue.
[0050] In some possible implementations, the matrix H0 is used to describe the initial pose transformation information between the parent lidar L1 and the child lidar L2, and H0 is a 4*4 homogeneous transformation matrix.
[0051] Where R0 is a 3x3 rotation matrix, R0 = R z (α)*R y (β)*R x (γ), R z (α), R y (β) and R x (γ) represents the angles of rotation around axes α, β, and γ from parent lidar L1 to child lidar L2; T0 is the displacement matrix, as detailed below;
[0052] T0 is a 3*1 displacement matrix, and x0, y0, and z0 are the measured displacement distances of the parent and child lidars along the x, y, and z axes, respectively.
[0053] In some possible implementations, in S2, the point cloud P2 scanned by the sub-LiDAR L2 is rotated and transformed into a point cloud P2' by the matrix H0, where P2' = P2*R0 + T0.
[0054] In some possible implementations, in S3, the overlapping points O1 and O2 of point clouds P2' and P1 are extracted respectively as follows: traverse each point in point cloud P1 or point cloud P2', then search for the nearest point of each point in point cloud P2' or point cloud P1, and determine whether the minimum distance between each point and its nearest point is less than a threshold λ. This threshold λ is an adaptively adjusted threshold based on whether the matching results converge. If it is less than λ, then the point is extracted.
[0055] In some possible implementations, during the search for the nearest point to a given point, a KD-tree model is constructed for the point cloud P2' and the point cloud P1. The KD-tree search process is as follows:
[0056] Perform a KD-tree search on P1 to determine if there exists a point in P1 within a range of λ for any given point p1. If so, add p' to O2. Perform a KD-tree search on P2' to determine if there exists a point in P2' within a range of λ for any given point p2. If so, add p2 to O2. The specific formula is as follows:
[0057]
[0058]
[0059] Where (P1,m) and (P2',m') are points with index m in point clouds P1 and P2', respectively; λ is an adaptive threshold that is adjusted based on whether the matching results converge; if the overlapping point clouds O1 and O2 tend to converge, then λ will gradually decrease according to the step size m, and vice versa.
[0060] In some possible implementations, another threshold T is set. Since the value of λ gradually decreases, the matching result always tends to be correct. When λ > T, only the overlapping point cloud of the child lidar is extracted, while the point cloud of the parent lidar is not processed. This can significantly increase the matching speed of the algorithm.
[0061] In some possible implementations, the sampling method is upsampling, specifically linear interpolation.
[0062] In some possible implementations, the sampling method downsampling is specifically voxel grid filtering.
[0063] In some possible implementations, in S5, finding the nearest point of each point in O1' in O2' is specifically as follows: for O1' and O2', the KD-tree search algorithm is used to accelerate the search for the nearest point of each point.
[0064] In some possible implementations, in S8, the error function e for a single point pair and the error function e for all point pairs are... all They are as follows:
[0065]
[0066] e all =∑e
[0067] Where ΔT is the Euclidean distance between pairs of points in S8; Δx, Δy, and Δz are the differences in the coordinates x, y, and z between pairs of points, respectively.
[0068] Using nonlinear optimization, the total error function e for all point pairs is solved. all The smallest transformation pose information H from O1' to O2' k .
[0069] A third aspect of the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the above-described lidar calibration method. Attached Figure Description
[0070] Figure 1 This is a flowchart illustrating the point cloud preprocessing steps in an embodiment of the present invention.
[0071] Figure 2 This is a flowchart illustrating the point cloud matching steps in an embodiment of the present invention;
[0072] Figure 3 This is a schematic diagram of the overlapping area of the 2D lidar scan in an embodiment of the present invention.
[0073] Figure 4 This is a schematic diagram of the structure of a lidar calibration device according to an embodiment of the present invention. Detailed Implementation
[0074] The preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby providing a clearer and more explicit definition of the scope of protection of the present invention.
[0075] To address the problems existing in the prior art, this invention provides a lidar calibration method, including point cloud preprocessing and point cloud matching.
[0076] See appendix Figure 1 As shown, the point cloud preprocessing includes the following steps:
[0077] S1: Obtain the pose transformation information between the parent lidar L1 and the child lidar L2. Define the initial value of the pose transformation information as a matrix H0 that includes translation and rotation. The matrix H0 is used to describe the initial pose transformation information between the parent lidar L1 and the child lidar L2. The matrix H0 is a 4*4 homogeneous transformation matrix.
[0078] Where R0 is a 3x3 rotation matrix, R0 = R z (α)*R y (β)*R x (γ), R z (α), R y (β) and R x (γ) represents the angles of rotation around axes α, β, and γ from parent lidar L1 to child lidar L2; T0 is the displacement matrix, as detailed below;
[0079] T0 is a 3*1 displacement matrix, and x0, y0, and z0 are the measured displacement distances of the parent and child lidars along the x, y, and z axes, respectively.
[0080] The aforementioned rotations and displacements can be measured using 4PCS (4-point sets), NDT (normal distribution transform), or manually.
[0081] S2: The point cloud P2 scanned by the sub-LiDAR L2 is rotated and transformed into point cloud P2' using the matrix H0, where P2' = P2 * R0 + T0, while keeping the point cloud P1 scanned by the parent LiDAR L1 unchanged. Since point cloud P2 uses L2 as its coordinate system in the viewpoint of LiDAR L2, it is necessary to rotate P2 and transform it to the L1 coordinate system to obtain P2', which is essential to obtain the true overlapping area through S3.
[0082] S3: Extract the overlapping points O1 and O2 of point cloud P2' and point cloud P1 respectively, as follows;
[0083] Traverse each point in point cloud P1 or point cloud P2', and then search for the nearest point of each point in point cloud P2' or point cloud P1. Determine whether the minimum distance between each point and its nearest point is less than the distance threshold λ between the point pairs. This threshold λ is an adaptively adjusted threshold based on whether the matching results converge. If it is less than λ, then extract the point.
[0084] In the process of searching for the nearest point to a given point, constructing a KD-tree model for the point clouds P2' and P1 can reduce the complexity of the search and greatly accelerate the search speed. The KD-tree search process is as follows:
[0085] Perform a KD-tree search on P1 to determine if there exists a point in P1 within a range of λ for any given point p1. If so, add p' to O2. Perform a KD-tree search on P2' to determine if there exists a point in P2' within a range of λ for any given point p2. If so, add p2 to O2. The specific formula is as follows:
[0086]
[0087]
[0088] Where (P1,m) and (P2',m') are points with index m in point clouds P1 and P2', respectively; λ is an adaptive threshold that is adjusted based on whether the matching results converge; if the overlapping point clouds O1 and O2 tend to converge, then λ will gradually decrease according to the step size m, and vice versa.
[0089] When the matching result is closer to reality, the point cloud tends to overlap more, requiring a smaller number of points to reduce interference from other points besides the corresponding one. When the matching result deviates significantly from reality, the point cloud tends to be more dispersed, requiring a larger number of points to expand the range of selected points for better alignment. Therefore, the point cloud numbers of O1 and O2 are used for balancing.
[0090] When the number of point clouds O1 and O2 obtained in the current cycle (S5-S8) is greater than that in the previous cycle, the point clouds are tending to overlap and decrease according to the reduction factor m.
[0091] When the number of point clouds O1 and O2 obtained in the current loop (S5-S8) is less than that in the previous loop, the point clouds may have been incorrectly registered. Therefore, the number of point clouds is increased according to the increment factor n.
[0092] Ideally, the registration result should be that the point cloud converges. Therefore, the increment factor n is set to 50% of the decrement factor m and decremented globally to achieve a more accurate match.
[0093] In some implementations, to speed up overlap extraction, another threshold T is set. Since the value of λ gradually decreases, the matching result always tends to be correct. When λ > T, only the overlapping point cloud of the child LiDAR is extracted, while the point cloud of the parent LiDAR is not processed. This can significantly increase the matching speed of the algorithm.
[0094] S4: Based on the number of overlapping point clouds, sample the overlapping point cloud O1 to obtain the sampled overlapping point cloud O1', and sample the overlapping point cloud O2 to obtain the sampled overlapping point cloud O2'.
[0095] The sampling method can be upsampling, specifically linear interpolation. When the number of point clouds in O1 and O2 is small, linear interpolation can sample more point clouds, preventing the number of sampled point clouds from being too small, which would lead to inaccurate and unreliable calculations.
[0096] The sampling method can be downsampling, specifically voxel grid filtering. When the number of point clouds in O1 and O2 is large, voxel grid filtering can be used to perform accurate sampling, reduce the sampling of a large number of point clouds, and prevent the sampling of too many point clouds from leading to increased computation and slower processing speed.
[0097] See appendix Figure 2 As shown, the point cloud matching includes the following steps:
[0098] S5: Find the nearest point of each point in O1' in O2', specifically as follows: Use the KD-tree algorithm to accelerate the search for the nearest point of each point in O1' and O2', and make up one-to-one corresponding point pairs. This can speed up the formation of point pairs.
[0099] S6: Weight the point pairs to mitigate gross errors in the point cloud data (remove outliers). Use the reciprocal of the absolute value of the distance between point pairs as the weight value. Weighting point pairs can make the matching results more reliable.
[0100] S7: Discard point pairs whose absolute distance exceeds the distance threshold λ between point pairs, and point pairs formed by points located at boundaries (e.g., vertices). λ is an adaptive threshold that is adjusted based on whether the matching results converge. λ can be set manually to adjust the speed and accuracy of point cloud matching. Discarding means ignoring point pairs that exceed the distance threshold λ only during the point cloud matching step loop. These point pairs are too far away and therefore meaningless.
[0101] S8: Set the error function between points as the Euclidean distance between the points, and solve for the transformed pose information H from O1' to O2'. k H k To minimize the total error function of all point pairs, the pose transformation information H k It is defined as a matrix that includes translation and rotation, where k represents the number of cycles.
[0102] Error function e for a single point pair and error function e for all point pairs all They are as follows:
[0103]
[0104] e all =∑e
[0105] Where ΔT is the Euclidean distance between the point pairs; Δx, Δy, and Δz are the differences in the coordinates x, y, and z between the point pairs, respectively;
[0106] Using nonlinear optimization, the total error function e for all point pairs is solved. all The smallest transformation pose information H from O1' to O2' k If the current pose transformation information H k The transformed pose information H generated by the previous S5-S7 cycle k-1 If the difference is within a certain range, output H k If the result is positive, proceed to the next S5-S7 cycle; otherwise, continue.
[0107] 2D LiDAR Test:
[0108] The customized robotic vehicle is equipped with a PC virtualization environment featuring an Intel i7 7600U processor and 16GB of RAM, and two VanJee-716 2D solid-state LiDARs. Each LiDAR has a resolution of 0.33°, a scanning frequency of 15Hz, and is set to a FoV of 210 degrees. See appendix. Figure 3 As shown in the figure, the 30° area is the overlapping area of the scans of the two solid-state lidars, and the lidars cannot detect the same obstacle within 90 cm.
[0109] A simulated scene was constructed using cardboard boxes, within which a robotic car performed a manually controlled mapping using Cartographer (an open-source SLAM mapping algorithm). The map resolution was 0.005 m. At the beginning and end of map construction, the straight-line distance between the robot's center point and the nearest surrounding obstacle was measured, defined as Dstart (measured at the start of mapping) and Dend (measured at the end of mapping). Dstart and Dend were compared as ground truth distances to the distances in the probabilistic grid map (built using the Cartographer algorithm). Each grid in the Cartographer probabilistic grid map stores the probability that the grid will be hit by a laser. We measured the distance between the robot's center point and the nearest obstacle point (where the probability of the obstacle's presence is greater than 50%) in the Cartographer probabilistic grid map. The closer this measurement is to the ground truth, the more accurate the map.
[0110] Table 1: Data collected from 2D LiDAR tests under different conditions.
[0111]
[0112]
[0113] Test results show that the Dend value of the calibrated 2D LiDAR mapping is closer to the true value. However, the proposed method cannot effectively optimize Dstart. This is because the algorithm ultimately publishes L2 TF messages, while Dstart is measured by L1, where L1 is the LiDAR fixed on the vehicle as a reference (i.e., the origin of the coordinate system). To avoid this problem, we again measured D after the car's rotation and D measured by L2. The difference between the uncalibrated and calibrated results is basically similar to that of Dend. Therefore, it can be concluded that the proposed method will enable Cartographer mapping to have better performance. Furthermore, the data in the table shows that using multiple sensors provides more accurate mapping results than using only LiDAR.
[0114] 3D LiDAR Test:
[0115] In Scenario 1, the parent and child LiDARs have over 70% similarity in their point clouds, representing LiDAR calibration in a 3D environment under conditions of high overlap. In Scenario 2, the overlap angle between the parent and child LiDARs is only 65 degrees, and the point clouds have only 12% similarity. Scenario 2 represents low-overlap FoV (field of view) LiDAR calibration in a 3D environment.
[0116] The parent-child lidar pose transformation information accurately measured in a real environment is considered the true value. The calibration algorithm proposed in this invention also calculates the parent-child lidar pose transformation information after execution. The translation error e is defined according to the literature "Overlap-based ICP tuning for robust localization of a humanoid robot". t and rotational error e r :
[0117]
[0118]
[0119] Wherein, ΔT is the difference between the displacement matrix T calculated by this invention and the true value; Δx, Δy and Δz are the differences between the x, y and z coordinates calculated by this invention and the true values, respectively; ΔR is the difference between the angle calculated by this invention and the true value;
[0120] Table 2 compares the calibration results of 3D LiDAR using other commercially available calibration methods and the calibration method proposed in this paper under real-world conditions.
[0121] Table 2: Calibration Results of LiDAR Using Different Calibration Methods
[0122]
[0123] Through translation error e t and rotational error e r According to the definition, if the translation error e t and rotational error e r The closer the result is to 0, the more accurate it is. In scenario 2, even with a good initial value H0, the ICP method has a relatively high failure rate, resulting in no calibration results. This comparison shows that the lidar calibration method of this invention reduces the dependence on initial values of the traditional ICP method. The data in the table also shows that the lidar calibration method of this invention has higher accuracy and improves calibration speed to some extent. Further reducing the threshold λ can further improve the running speed of the calibration algorithm of this invention.
[0124] In some implementations, see Appendix Figure 4 As shown, this embodiment provides a lidar calibration device, including a point cloud preprocessing module and a point cloud matching module. This device implements the steps in the lidar calibration method described above during operation.
[0125] The working steps of the point cloud preprocessing module are as follows:
[0126] S1: Obtain the pose transformation information between the parent lidar L1 and the child lidar L2. Define the initial value of the pose transformation information as a matrix H0 that includes translation and rotation. The matrix H0 is used to describe the initial pose transformation information between the parent lidar L1 and the child lidar L2. Using a matrix can facilitate subsequent numerical calculations.
[0127] S2: The point cloud P2 scanned by the sub-LiDAR L2 is rotated and transformed into point cloud P2' using the matrix H0, while keeping the point cloud P1 scanned by the parent LiDAR L1 unchanged. Since point cloud P2 uses L2 as its coordinate system in the viewpoint of LiDAR L2, it is necessary to rotate P2 and transform it to the L1 coordinate system to obtain P2' in order to obtain the true overlapping area in step S3.
[0128] S3: Extract the overlapping points O1 and O2 of point cloud P2' and point cloud P1 respectively;
[0129] S4: Based on the number of overlapping point clouds, sample the overlapping point cloud O1 to obtain the sampled overlapping point cloud O1', and sample the overlapping point cloud O2 to obtain the sampled overlapping point cloud O2'. Typically, the number of point clouds in O1 and O2 is quite large. To speed up the algorithm, downsampling significantly reduces the number of point clouds in O1 and O2.
[0130] The working steps of the point cloud pre-matching module are as follows:
[0131] S5: Find the nearest point to each point in O1' in O2' and make them one-to-one corresponding point pairs;
[0132] S6: Weight the point pairs using the reciprocal of the absolute value of the distance between the point pairs as the weight value. Weighting point pairs can make the matching results more reliable;
[0133] S7: Discard point pairs whose absolute distance exceeds a distance threshold λ between point pairs, and point pairs located at boundaries. λ is an adaptive threshold adjusted based on whether the matching results converge. These point pairs are too far apart, so they are ignored as they are meaningless.
[0134] S8: Set the error function between points as the Euclidean distance between the points, and solve for the transformed pose information H from O1' to O2'. k H k To minimize the total error function of all point pairs, the pose transformation information H k Defined as a matrix including translation and rotation, where k represents the number of iterations. If the current transformation pose information H... k The transformed pose information H generated by the previous S5-S7 cycle k-1 If the difference is within a certain range, output H kIf the result is positive, proceed to the next S5-S7 cycle; otherwise, continue.
[0135] In some implementations, the matrix H0 is used to describe the initial pose transformation information between the parent lidar L1 and the child lidar L2, and H0 is a 4*4 homogeneous transformation matrix.
[0136] Where R0 is a 3x3 rotation matrix, R0 = R z (α)*R y (β)*R x (γ), R z (α), R y (β) and R x (γ) represents the angles of rotation around axes α, β, and γ from parent lidar L1 to child lidar L2; T0 is the displacement matrix, as detailed below;
[0137] T0 is a 3*1 displacement matrix, and x0, y0, and z0 are the measured displacement distances of the parent and child lidars along the x, y, and z axes, respectively.
[0138] In some implementations, in S2, the point cloud P2 scanned by the sub-LiDAR L2 is rotated and transformed into a point cloud P2' by the matrix H0, where P2' = P2*R0 + T0.
[0139] In some implementations, in S3, the overlapping points O1 and O2 of point clouds P2' and P1 are extracted respectively as follows: traverse each point in point cloud P1 or point cloud P2', then search for the nearest point of each point in point cloud P2' or point cloud P1, and determine whether the minimum distance between each point and its nearest point is less than a threshold λ. This threshold λ is an adaptively adjusted threshold based on whether the matching results converge. If it is less than λ, then the point is extracted.
[0140] In some implementations, during the search for the nearest point to a given point, a KD-tree model is constructed for the point cloud P2' and the point cloud P1. The KD-tree search process is as follows:
[0141] Perform a KD-tree search on P1 to determine if there exists a point in P1 within a range of λ for any given point p1. If so, add p' to O2. Perform a KD-tree search on P2' to determine if there exists a point in P2' within a range of λ for any given point p2. If so, add p2 to O2. The specific formula is as follows:
[0142]
[0143]
[0144] Where (P1,m) and (P2',m') are points with index m in point clouds P1 and P2', respectively; λ is an adaptive threshold that is adjusted based on whether the matching results converge; if the overlapping point clouds O1 and O2 tend to converge, then λ will gradually decrease according to the step size m, and vice versa.
[0145] In some implementations, an additional threshold T is set. Since the value of λ gradually decreases, the matching result always tends to be correct. When λ > T, only the overlapping point cloud of the child lidar is extracted, while the point cloud of the parent lidar is not processed. This can significantly increase the matching speed of the algorithm.
[0146] In some implementations, the sampling method is upsampling, specifically linear interpolation.
[0147] In some implementations, the sampling method downsampling is specifically voxel grid filtering.
[0148] In some implementations, in S5, finding the nearest point of each point in O1' in O2' is specifically done as follows: the KD-tree search algorithm is used to accelerate the search for the nearest point of each point in O1' and O2' respectively.
[0149] In some implementations, in S8, the error function e for a single point pair and the error function e for all point pairs are... all They are as follows:
[0150]
[0151] e all =∑e
[0152] Where ΔT is the Euclidean distance between pairs of points in S8; Δx, Δy, and Δz are the differences in the coordinates x, y, and z between pairs of points, respectively.
[0153] Using nonlinear optimization, the total error function e of all point pairs is solved. all The smallest transformation pose information H from O1' to O2' k .
[0154] This embodiment also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the above-described lidar calibration method.
[0155] The storage medium stores program instructions capable of implementing all the above methods. These program instructions can be stored in the storage medium in the form of a software product, including several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks, or terminal devices such as computers, servers, mobile phones, and tablets.
[0156] The processor can also be called a CPU (Central Processing Unit). A processor may be an integrated circuit chip with signal processing capabilities. A processor can also be:
[0157] A DSP (Digital Signal Processor) is a processor composed of large-scale or very large-scale integrated circuit chips used to perform specific signal processing tasks. It gradually developed to meet the needs of high-speed, real-time signal processing tasks. With the development of integrated circuit technology and digital signal processing algorithms, the implementation methods of digital signal processors are constantly changing, and their processing capabilities are continuously improving and expanding.
[0158] ASIC (Application Specific Integrated Circuit) refers to an integrated circuit designed and manufactured to meet the specific requirements of a user and the needs of a specific electronic system.
[0159] FPGA (Field Programmable Gate Array) is a further development based on programmable devices such as PAL (Programmable Array Logic) and GAL (Generic Array Logic). It emerged as a semi-custom circuit in the field of Application-Specific Integrated Circuits (ASICs), solving the shortcomings of custom circuits while overcoming the limitation of the limited gate count of original programmable devices.
[0160] A general-purpose processor, which may be a microprocessor or any conventional processor.
[0161] Other programmable logic devices, discrete gate or transistor logic devices, and discrete hardware components, etc.
[0162] The above embodiments are only for illustrating the technical concept and features of the present invention. Their purpose is to enable those skilled in the art to understand the content of the present invention and implement it. They should not be used to limit the scope of protection of the present invention. All equivalent changes or modifications made in accordance with the spirit and essence of the present invention should be covered within the scope of protection of the present invention.
Claims
1. A lidar calibration method, characterized in that, This includes point cloud preprocessing and point cloud matching. The point cloud preprocessing includes the following steps: S1: Obtain the pose transformation information between the parent lidar L1 and the child lidar L2, and define the initial value of the pose transformation information as a matrix H0 including translation and rotation. The matrix H0 is used to describe the initial pose transformation information between the parent lidar L1 and the child lidar L2. S2: The point cloud P2 scanned by the sub-library radar L2 is rotated and transformed into point cloud P2' through the matrix H0, while keeping the point cloud P1 scanned by the parent lidar L1 unchanged. S3: Extract the overlapping points O1 and O2 of point cloud P2' and point cloud P1 respectively; S4: Based on the number of overlapping point clouds, sample the overlapping point cloud O1 to obtain the sampled overlapping point cloud O1', and sample the overlapping point cloud O2 to obtain the sampled overlapping point cloud O2'. The point cloud matching includes the following steps: S5: Find the nearest point to each point in O1' in O2' and make them one-to-one corresponding point pairs; S6: Weight the point pairs by using the reciprocal of the absolute value of the distance between the point pairs as the weight value; S7: Discard point pairs whose absolute distance exceeds the distance threshold λ between point pairs and point pairs formed by points located at the boundary; S8: Set the error function between points as the Euclidean distance between the points, and solve for the transformed pose information H from O1' to O2'. k H k To minimize the total error function of all point pairs, the pose transformation information H k Defined as a matrix including translation and rotation, where k represents the number of iterations, if the current transformation pose information H k The transformed pose information H generated by the previous S5-S8 cycle k-1 If the difference is within a certain range, output H k If the result is positive, proceed to the next S5-S8 cycle; otherwise, continue.
2. The lidar calibration method according to claim 1, characterized in that: The matrix H0 is used to describe the initial pose transformation information between the parent lidar L1 and the child lidar L2. H0 is a 4*4 homogeneous transformation matrix. Where R0 is a 3x3 rotation matrix, R0 = R z (α)*R y (β)*R x (γ), R z (α), R y (β) and R x (γ) represents the angles of rotation around axes α, β, and γ from parent lidar L1 to child lidar L2; T0 is the displacement matrix, as follows; T0 is a 3*1 displacement matrix, and x0, y0 and z0 are the measured displacement distances of the parent lidar and the child lidar on the x, y, and z axes, respectively. In S2, P2' = P2*R0 + T0.
3. The lidar calibration method according to claim 1, characterized in that: In S3, the overlapping points O1 and O2 of point clouds P2' and P1 are extracted respectively as follows: traverse each point in point cloud P1 or point cloud P2', and then search for the nearest point of each point in point cloud P2' or point cloud P1. Determine whether the minimum distance between each point and its nearest point is less than the distance threshold λ between point pairs. This threshold λ is an adaptively adjusted threshold based on whether the matching results converge. If it is less than λ, then extract the point.
4. The lidar calibration method according to claim 3, characterized in that: In the process of searching for the nearest point to a certain point, a KD-tree model is constructed for the point cloud P2' and the point cloud P1. The KD-tree search process is as follows: Perform a KD-tree search on P1 to determine if there exists a point in P1 within a range of λ for any given point p1. If so, add p' to O2. Perform a KD-tree search on P2' to determine if there exists a point in P2' within a range of λ for any given point p2. If so, add p2 to O2. The specific formula is as follows: Where (P1,m) and (P2',m') are points with index m in point clouds P1 and P2', respectively; λ is an adaptive threshold that is adjusted based on whether the matching results converge; if the overlapping point clouds O1 and O2 tend to converge, then λ will gradually decrease according to the step size m, and vice versa.
5. The lidar calibration method according to claim 4, characterized in that: Set another threshold T, and when λ>T, only extract the overlapping point cloud of the child lidar, while the point cloud of the parent lidar is not processed.
6. The lidar calibration method according to claim 1, characterized in that: The sampling method is either upsampling or downsampling. Specifically, upsampling is linear interpolation, and downsampling is voxel grid filtering.
7. The lidar calibration method according to claim 1, characterized in that: In S5, the process of finding the nearest point to each point in O1' within O2' is as follows: The KD-tree search algorithm is used to accelerate the search for the nearest point to each point in O1' and O2' respectively.
8. The lidar calibration method according to claim 1, characterized in that: In S8, the error function e for a single point pair and the error function e for all point pairs. all They are as follows: and all =∑e Where ΔT is the Euclidean distance between pairs of points in S8; Δx, Δy, and Δz are the differences in the coordinates x, y, and z between pairs of points, respectively. Using nonlinear optimization, the total error function e of all point pairs is solved. all The smallest transformation pose information H from O1' to O2' k .
9. A lidar calibration device, characterized in that, Includes a point cloud preprocessing module and a point cloud matching module. The working steps of the point cloud preprocessing module are as follows: S1: Obtain the pose transformation information between parent lidar L1 and child lidar L2. Define the initial value of the pose transformation information as a matrix H0 that includes translation and rotation. Matrix H0 describes the initial pose transformation information between parent lidar L1 and child lidar L2. Using a matrix facilitates subsequent numerical calculations. Matrix H0 is a 4*4 homogeneous transformation matrix. Where R0 is a 3x3 rotation matrix, R0 = R z (α)*R y (β)*R x (γ), R z (α), R y (β) and R x (γ) represents the angles of rotation around axes α, β, and γ from parent lidar L1 to child lidar L2; T0 is the displacement matrix, as follows; T0 is a 3*1 displacement matrix, and x0, y0 and z0 are the measured displacement distances of the parent lidar and the child lidar on the x, y, and z axes, respectively. S2: The point cloud P2 scanned by the sub-LiDAR L2 is rotated and transformed into point cloud P2' through the matrix H0, where P2' = P2*R0+T0, while keeping the point cloud P1 scanned by the parent LiDAR L1 unchanged. S3: Extract the overlapping points O1 and O2 of point clouds P2' and P1 respectively, as follows: Traverse each point in point cloud P1 or point cloud P2', then search for the nearest point of each point in point cloud P2' or point cloud P1. Determine if the minimum distance between each point and its nearest point is less than the distance threshold λ between point pairs. This threshold λ is adaptively adjusted based on whether the matching results converge. If it is less than λ, then extract the point. During the search for the nearest point of a point, construct a KD-tree model for point clouds P2' and P1. The KD-tree search process is as follows: Perform a KD-tree search on P1 to determine if there exists a point in P1 within a range of λ for any given point p1. If so, add p' to O2. Perform a KD-tree search on P2' to determine if there exists a point in P2' within a range of λ for any given point p2. If so, add p2 to O2. The specific formula is as follows: Where (P1,m) and (P2',m') are points with index m in point clouds P1 and point cloud P2' respectively; if the overlapping point clouds O1 and O2 tend to converge, then λ will gradually decrease according to the step size m, and conversely, λ will gradually increase according to the step size n; another threshold T can also be set. Since the value of λ will gradually decrease, the matching result will always tend to be correct. When λ>T, only the overlapping point cloud of the sub-LiDAR is extracted, while the point cloud of the parent LiDAR is not processed. S4: Based on the number of overlapping point clouds, the overlapping point cloud O1 is sampled to obtain the sampled overlapping point cloud O1', and the overlapping point cloud O2 is sampled to obtain the sampled overlapping point cloud O2'. The sampling method is upsampling or downsampling. The upsampling method is linear interpolation, and the downsampling method is voxel grid filtering. The working steps of the point cloud pre-matching module are as follows: S5: Find the nearest point of each point in O1' in O2', that is, use the KD tree search algorithm to accelerate the search for the nearest point of each point in O1' and O2' respectively, and make up a one-to-one corresponding point pair; S6: Weight the point pairs by using the reciprocal of the absolute value of the distance between the point pairs as the weight value; S7: Discard point pairs whose absolute distance exceeds the distance threshold λ between point pairs and point pairs formed by points located at the boundary; S8: Set the error function between points as the Euclidean distance between the points, and solve for the transformed pose information H from O1' to O2'. k H k To minimize the total error function of all point pairs, the pose transformation information H k Defined as a matrix that includes translation and rotation, where k represents the number of iterations. Error function e for a single point pair and error function e for all point pairs all They are as follows: and all =∑e Where ΔT is the Euclidean distance between pairs of points in S8; Δx, Δy, and Δz are the differences in coordinates x, y, and z between pairs of points, respectively; and nonlinear optimization is used to solve for the total error function e of all pairs of points. all The smallest transformation pose information H from O1' to O2' k ; If the current pose transformation information H k The transformed pose information H generated by the previous S5-S8 cycle k-1 If the difference is within a certain range, output H k If the result is positive, proceed to the next S5-S8 cycle; otherwise, continue.
10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program that, when executed by a processor, implements the steps of the lidar calibration method according to any one of claims 1-8.