An offline external parameter calibration method for dual single-line laser radar
By using the sphere with known radius and nonlinear least squares optimization method to fit the sphere center in double single-line lidar, combined with the ICP algorithm, the problem of the lack of relative posture calibration of double single-line lidar is solved, and high-precision three-dimensional data acquisition and autonomous navigation are achieved.
Patent Information
- Application Number
- CN202111669133.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-12-31
- Publication Date
- 2025-04-11
- Estimated Expiration
- 2041-12-31
AI Technical Summary
The prior art lacks a calibration method for determining the relative position of double single-line lidar, which limits its application in three-dimensional data acquisition and autonomous navigation.
An offline external parameter calibration method for double single-line lidar is proposed. By selecting a sphere with a known radius, fitting the center of the circle and the center of the sphere using a nonlinear least squares optimization method, mapping the two-dimensional point cloud data to the three-dimensional sphere, and combining the ICP algorithm to estimate the relative position of the double single-line lidar.
The accurate relative posture calibration of double single-line lidar is achieved, which overcomes the disadvantages of inability to collect three-dimensional data, improves its competitiveness in low-cost and high-efficiency markets, and improves the credibility of three-dimensional data acquisition.
Smart Images
Figure CN114518569B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of autonomous driving and robotics technology, and in particular to an off-line external parameter calibration method for a dual-single-line laser radar. Background Art
[0002] Dual single-line laser radar can make up for the shortcoming that single-line laser radar cannot obtain the third dimension information, realize three-dimensional data collection, and provide more effective information for subsequent robot autonomous navigation, simultaneous localization and mapping (SLAM) and other tasks. Compared with three-dimensional laser radar, dual single-line laser radar has lower price and less energy consumption, which conforms to the competitive trend of low cost and high efficiency in the civilian market. However, there is currently a lack of a calibration method that can determine the relative posture of two single-line laser radars, which greatly restricts the practicality of dual single-line laser radars. Therefore, in order to overcome the shortcomings of dual single-line laser radars lacking relative posture information and unable to realize three-dimensional data collection, the present invention proposes an offline external parameter calibration method for dual single-line laser radars, using a sphere with a known radius to provide constraints for solving the relative posture of two two-dimensional laser radars, and then solve the relative posture information of the two, thereby solving the problem of dual single-line laser radars collecting three-dimensional data, and providing a reliable foundation for low-cost, low-energy laser positioning systems. Summary of the invention
[0003] The purpose of the present invention is to address the deficiencies in the prior art and provide an offline extrinsic parameter calibration method for dual single-line laser radars, which can accurately calibrate the relative position information of the centers of the two laser radars when the two laser radars are working simultaneously.
[0004] The object of the present invention is achieved through the following technical solution: an offline external parameter calibration method for a dual single-line laser radar, the method comprising the following steps:
[0005] Step (1): Select a sphere with a radius of R. Set up two single-line laser radars L1 and L2 in an open environment. L1 is placed relative to L2 in a posture T. L1 is the primary radar and L2 is the secondary radar. The straight-line distance between the centers of the two radars does not exceed the diameter of the sphere, and ensure that the sphere can be scanned by L1 and L2 at the same time.
[0006] Step (2): Start data acquisition and obtain the laser point cloud data of two single-line laser radars scanning the same sphere several times. The single-line laser radar uses a polar coordinate system. are the radius and angle in the polar coordinate system respectively;
[0007] Step (3): transform the point cloud data into a Cartesian coordinate system, and then perform Gaussian filtering on the point cloud data to reduce the interference of random noise on the point cloud data, and obtain the laser point cloud coordinates expressed by formula (1):
[0008]
[0009] Where i represents the number of points scanned by the LiDAR, and j represents whether the point cloud data belongs to L1 or L2. Represents the point cloud coordinates in the Cartesian coordinate system;
[0010] Step (4): Based on the Cartesian coordinate point cloud data obtained in step (3), estimate the center position of the sphere with the center of L1 and L2 as the origin of the coordinate system, move the sphere to another different position, repeat steps (2)-(4) for a total of m times, and obtain m pairs of sphere center coordinates:
[0011]
[0012] Step (5): Use the ICP (Iterative Closest Point) algorithm to estimate the relative pose information of the two single-line laser radars.
[0013] Furthermore, the fitting process of the sphere center coordinates in step (4) is:
[0014] For L1 single-line laser radar, since the point cloud data comes from the scanning of the same sphere, the point clouds are all on the same circle. According to the function definition of the circle:
[0015]
[0016] Among them, x1 and y1 represent the center of the circle corresponding to the current point cloud data scan, and R1 represents the radius of the circle corresponding to the current point cloud data scan. Represents the point cloud coordinates of the current scanning result of the L1 single-line laser radar transformed into the Cartesian coordinate system; thus, a nonlinear least squares optimization problem can be constructed, and the target loss function is:
[0017]
[0018] When updating the radius R1, choose It is regarded as a whole to perform gradient update, so as to avoid the problem of gradient disappearance; solve the nonlinear least squares optimization problem expressed in equation (4) and fit the set of point cloud data The corresponding circle is used to establish an XYZ three-vertical coordinate system with the circular surface, and the Z axis is perpendicular to the circular surface; under the condition that the radius R of the scanned sphere is known, relative to the single-line laser radar coordinate system, the center of the scanned sphere and the fitted center of the circle are on the same Z axis, that is, the X-axis and Y-axis coordinates are equal, and the point cloud data obtained by the current scan are all on the same spherical surface. Considering that the single-line laser radar can only obtain two-dimensional coordinates, in order to be able to fit the coordinates of the center of the sphere, the position of the radar is set to the origin of the Z axis, that is, all point cloud coordinates are transformed into According to the definition of spherical function:
[0019]
[0020] Where (x1, y1, z1) represents the three-dimensional coordinates of the center of the sphere. After obtaining the center of the circle where the current point cloud data is located, the target loss function about the center of the sphere can be constructed:
[0021]
[0022] After setting the initial values of the parameters and performing nonlinear least squares optimization on this part of the target loss function, the three-dimensional coordinates of the center of the sphere relative to the lidar at the current position of the sphere can be obtained.
[0023] Furthermore, the process of ICP registration estimation of pose information in step (5) is as follows:
[0024] After obtaining the three-dimensional coordinates of m pairs of sphere centers, calculate the center of mass coordinates corresponding to the m sphere center coordinates in the L1 and L2 radar coordinate systems respectively.
[0025]
[0026] After removing the centroid of all spherical center coordinates, we get the centroid-removed spherical center coordinates
[0027]
[0028] Calculate the characteristic matrix M in the ICP algorithm:
[0029]
[0030] Using the SVD (Singular Value Decomposition) method to decompose the feature matrix M, we get:
[0031] M=U∑V T (10)
[0032] Where U, V represent orthogonal matrices, ∑ is the characteristic diagonal matrix;
[0033] The rotation matrix Q and relative displacement t between the two single-line laser radars are solved as follows:
[0034]
[0035] Finally, the relative position information of the two single-line laser radars expressed in a homogeneous expression is estimated. for:
[0036]
[0037] The beneficial effect of the present invention is that: the present invention utilizes a sphere with a known radius, and through a nonlinear least squares optimization method, first fits the center of the circle, and then fits the center of the sphere, and maps the point cloud data originally on the two-dimensional plane to the geometric center of the sphere belonging to the three-dimensional space. After multiple data fittings, combined with the ICP algorithm, the relative posture information of the dual-line laser radar is finally obtained, which overcomes the shortcoming that the dual-line laser radar cannot perform three-dimensional data collection due to the lack of relative posture information, improves the competitive position of the dual-line laser radar in the civilian market, and the calibrated posture information has high precision and good accuracy, which improves the credibility of the dual-line laser radar in collecting three-dimensional data. BRIEF DESCRIPTION OF THE DRAWINGS
[0038] In order to more clearly illustrate the technical solution of the present invention, the following will briefly introduce the drawings required for use in the embodiments. It should be understood that the following drawings only illustrate certain embodiments of the present invention and should not be regarded as limiting the scope of protection of the present invention. In each of the drawings, similar components are numbered similarly.
[0039] Figure 1 It is a flow chart of the offline external parameter calibration method of dual single-line laser radar;
[0040] Figure 2 This is a flow chart of the specific method of sphere center fitting in the process of using the dual single-line laser radar offline external parameter calibration method;
[0041] Figure 3 It is a flow chart of the specific method of posture calibration during the use of the dual single-line laser radar offline external parameter calibration method;
[0042] Figure 4a It is the RPE error curve of the offline external parameter calibration result of dual single-line laser radar;
[0043] Figure 4b It is the error curve of the three coordinate axes in the Cartesian coordinate system of the offline external parameter calibration result of the dual single-line laser radar. DETAILED DESCRIPTION
[0044] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.
[0045] The components of the embodiments of the present invention generally described and shown in the drawings herein may be arranged and designed in a variety of different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.
[0046] In the following, various embodiments of the present disclosure will be described more fully. The present disclosure may have various embodiments, and adjustments and changes may be made therein. However, it should be understood that there is no intention to limit the various embodiments of the present disclosure to the specific embodiments disclosed herein, but rather the present disclosure should be understood to cover all adjustments, equivalents and / or alternatives that fall within the spirit and scope of the various embodiments of the present disclosure.
[0047] Hereinafter, the terms "including", "having" and their cognates, which may be used in various embodiments of the present invention, are intended only to indicate specific features, numbers, steps, operations, elements, components or combinations of the foregoing items, and should not be understood as first excluding the existence of one or more other features, numbers, steps, operations, elements, components or combinations of the foregoing items or adding the possibility of one or more features, numbers, steps, operations, elements, components or combinations of the foregoing items.
[0048] Furthermore, the terms “first”, “second”, “third”, etc. are merely used for distinguishing descriptions and are not to be understood as indicating or implying relative importance.
[0049] Unless otherwise defined, all terms (including technical terms and scientific terms) used herein have the same meanings as those generally understood by those skilled in the art to which the various embodiments of the present invention belong. The terms (such as those defined in generally used dictionaries) will be interpreted as having the same meanings as the contextual meanings in the relevant technical field and will not be interpreted as having idealized meanings or overly formal meanings unless clearly defined in the various embodiments of the present invention.
[0050] Figure 1 The following is a flow chart of an offline external parameter calibration method for a dual single-line laser radar provided in an embodiment, the method comprising:
[0051] Step 110: A sphere with a radius of R = 1.5 m is selected. Two single-line laser radars L1 and L2 are set in an open environment. L1 is placed relative to L2 in a posture T. The posture T is in the form of a homogeneous expression. The specific value of T in this embodiment is:
[0052]
[0053] Among them, L1 is the main radar and L2 is the secondary radar. The straight-line distance between the centers of the two radars does not exceed the diameter of the sphere, and it is ensured that the sphere can be scanned by L1 and L2 at the same time.
[0054] Step 120: Start data collection and obtain point cloud data of two radars scanning the same sphere 10 times. The single-line laser radar uses a polar coordinate system. are the radius and angle in polar coordinates, respectively.
[0055] Step 130: transform the point cloud data into a Cartesian coordinate system, and then perform Gaussian filtering on the point cloud data to reduce the interference of random noise on the data, and obtain the laser point cloud coordinates expressed by formula (1):
[0056]
[0057] Where i represents the number of points scanned by the LiDAR, and j represents whether the point cloud data belongs to L1 or L2. Represents the point cloud coordinates in the Cartesian coordinate system.
[0058] Step 140: Based on the Cartesian coordinate point cloud data obtained in step 130, estimate the center position of the sphere with the center of L1 and L2 as the origin of the coordinate system, move the sphere to another different position, and repeat steps (2) to (4) 4 times to obtain 4 pairs of sphere center coordinates:
[0059]
[0060] Specific steps are as follows Figure 2 shown.
[0061] Step 141, initialize the coordinates of the two circle centers corresponding to the two radar scanning data to (0.5, 0.5), the circle radius to 0.5, and the Z-axis coordinate of the sphere center to 0.5.
[0062] Step 142, when the specified number of times is not reached, it is necessary to return to step 120 to change the position of the ball and re-perform data collection and preprocessing.
[0063] Step 143, taking the center of the single-line laser radar as the origin of the coordinate system, the process of estimating the coordinate position of the center of the circle and the radius of the circle is as follows:
[0064] Taking L1 single-line laser radar as an example, since the point cloud data comes from the scanning of the same sphere, the point clouds are all on the same circle. According to the function definition of the circle, we have:
[0065]
[0066] Among them, x1 and y1 represent the center of the circle corresponding to the current point cloud data scan, and R1 represents the radius of the circle corresponding to the current point cloud data scan. Represents the point cloud coordinates of the current scan result of the L1 single-line laser radar transformed into the Cartesian coordinate system. Therefore, a nonlinear least squares optimization problem can be constructed, and the objective loss function is:
[0067]
[0068] When updating the radius R1, choose It can be regarded as a whole to perform gradient update, thus avoiding the problem of gradient disappearance.
[0069] Step 144, taking the center of the single-line laser radar as the origin of the coordinate system, the three-dimensional coordinates of the center of the sphere are estimated. The specific process is as follows:
[0070] The radius R of the scanned sphere is known. Relative to the single-line laser radar coordinate system, the center of the scanned sphere and the fitted center of the circle are on the same Z axis, that is, the X-axis and Y-axis coordinates are equal. The point cloud data obtained by the current scan are all on the same sphere. Considering that the single-line laser radar can only obtain two-dimensional coordinates, in order to fit the coordinates of the center of the sphere, the position of the radar is set to the origin of the Z axis, that is, the coordinates of all point clouds are transformed into According to the definition of spherical function:
[0071]
[0072] Where (x1, y1, z1) represents the three-dimensional coordinates of the center of the sphere. After obtaining the center of the circle where the current point cloud data is located, the target loss function about the center of the sphere can be constructed:
[0073]
[0074] After setting the initial values of the parameters and performing nonlinear least squares optimization on this part of the target loss function, the three-dimensional coordinates of the center of the sphere relative to the lidar at the current position of the sphere can be obtained.
[0075] Step 145, store the sphere center fitting result, return to step 142 and restart the execution until it stops after executing 4 times.
[0076] Step 150, using the ICP algorithm to estimate the relative position information of the two single-line laser radars, the specific steps are as follows: Figure 3 shown.
[0077] Step 151, respectively calculate the centroid coordinates corresponding to the four pairs of sphere center positions in the L1 and L2 radar coordinate systems
[0078]
[0079] Step 152: remove the centroids from all the spherical center coordinates to obtain the centroid-removed spherical center coordinates
[0080]
[0081] Step 153, calculate the characteristic matrix M in the ICP algorithm:
[0082]
[0083] Step 154, decompose the feature matrix M using the SVD method to obtain:
[0084] M=U∑V T (10)
[0085] Where U, V represent orthogonal matrices, ∑ is the characteristic diagonal matrix;
[0086] Step 155, the rotation matrix Q and relative displacement t between the two single-line laser radars are obtained by solving:
[0087]
[0088] Finally, the relative position information of the two single-line laser radars expressed in a homogeneous expression is estimated. for:
[0089]
[0090] Taking into account the interference of random noise in the laser radar acquisition process, the RPE evaluation index and the deviation of the relative displacement in the three coordinate axes of the Cartesian coordinate system are used to evaluate the pose calibration accuracy of the present invention under different noise levels. Figure 4a , Figure 4b As shown, when the noise standard deviation is less than 0.08m, the error size is maintained at the millimeter level, which is close to the influence of environmental random noise in actual use. The noise level is further improved, and the calibration error of the present invention is still maintained at the centimeter level with high accuracy, which verifies the effectiveness of the method of the present invention.
[0091] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.
Claims
1. An offline external parameter calibration method for dual single-line laser radar, characterized in that: The following steps are involved: Step (1): Select a sphere with a radius of R. Set up two single-line laser radars L1 and L2 in an open environment. L1 is placed relative to L2 in a posture T. L1 is the primary radar and L2 is the secondary radar. The straight-line distance between the centers of the two radars does not exceed the diameter of the sphere, and ensure that the sphere can be scanned by L1 and L2 at the same time. Step (2): Start data acquisition and obtain laser point cloud data of two single-line laser radars scanning the same sphere several times, which are recorded as The single-line laser radar uses a polar coordinate system. i=1,...,n j ,j=1,2, are the radius and angle in the polar coordinate system respectively; Step (3): transform the point cloud data into a Cartesian coordinate system, perform Gaussian filtering on the point cloud data, and obtain the laser point cloud coordinates expressed by equation (1): Where i represents the number of points scanned by the LiDAR, and j represents whether the point cloud data belongs to L1 or L2. Represents the point cloud coordinates in the Cartesian coordinate system; Step (4): Based on the Cartesian coordinate point cloud data obtained in step (3), estimate the center position of the sphere with the center of L1 and L2 as the origin of the coordinate system, move the sphere to another different position, repeat steps (2)-(4) for a total of m times, and obtain m pairs of sphere center coordinates: The fitting process of the spherical center coordinates is: For L1 single-line laser radar, since the point cloud data comes from the scanning of the same sphere, the point clouds are all on the same circle. According to the function definition of the circle: Among them, x1 and y1 represent the center of the circle corresponding to the current point cloud data scan, and R1 represents the radius of the circle corresponding to the current point cloud data scan. i=1,...,n1 represents the point cloud coordinates of the current scan result of the L1 single-line laser radar transformed into the Cartesian coordinate system; thus, a nonlinear least squares optimization problem can be constructed, and the target loss function is: When updating radius R1, choose It is regarded as a whole to perform gradient update; solve the nonlinear least squares optimization problem expressed by equation (4), fit the circle corresponding to the group of point cloud data, and establish an XYZ three-vertical coordinate system with the circular surface, and the Z axis is perpendicular to the circular surface; under the condition that the radius R of the scanned sphere is known, relative to the single-line laser radar coordinate system, the center of the scanned sphere and the center of the fitted circle are on the same Z axis, that is, the X-axis and Y-axis coordinates are equal, and the point cloud data currently scanned are all on the same sphere. Considering that the single-line laser radar can only obtain two-dimensional coordinates, in order to fit the coordinates of the center of the sphere, the position of the radar is set to the origin of the Z axis, that is, the coordinates of all point clouds are transformed into i=1,...,n1, according to the definition of spherical function: Where (x1, y1, z1) represents the three-dimensional coordinates of the center of the sphere. After obtaining the center of the circle where the current point cloud data is located, the target loss function about the center of the sphere can be constructed: After setting the initial values of the parameters and performing nonlinear least squares optimization on this part of the target loss function, the three-dimensional coordinates of the center of the sphere relative to the laser radar at the current sphere placement position can be obtained; Step (5): Use the ICP algorithm to estimate the relative pose information of the two single-line laser radars.
2. The method according to claim 1, characterized in that The process of ICP registration estimation of pose information in step (5) is as follows: After obtaining the three-dimensional coordinates of m pairs of sphere centers, calculate the center of mass coordinates corresponding to the m sphere center coordinates in the L1 and L2 radar coordinate systems respectively. After removing the centroid of all spherical center coordinates, we get the centroid-removed spherical center coordinates Calculate the characteristic matrix M in the ICP algorithm: Using the SVD method to decompose the feature matrix M, we can get: M=U∑V T (10) Where U, V represent orthogonal matrices, ∑ is the characteristic diagonal matrix; The rotation matrix Q and relative displacement t between the two single-line laser radars are solved as follows: Finally, the relative position information of the two single-line laser radars expressed in a homogeneous expression is estimated. for: