A dual-target synergistic lidar calibration method and related devices

Abstract

This application relates to the field of lidar technology, providing a dual-target collaborative lidar calibration method and related equipment. The method employs two targets with built-in inertial measurement units (IMUs) and ranging modules, and rapidly solves for extrinsic parameters using single-frame point cloud data combined with three geometric constraints. The calculation of lidar extrinsic parameters no longer relies on pre-stored target coordinates, but instead uses the relative attitude and distance between the two targets. This solves the calibration error problem caused by target position offset in traditional methods. Furthermore, the targets do not need to be precisely horizontal or directly facing the lidar; the IMU automatically compensates for attitude deviations. The two targets only need to be roughly placed within a common field of view, eliminating the need for precise manual alignment and reducing calibration difficulty.

Description

Technical Field

[0001] This invention relates to the field of lidar, specifically to a dual-target collaborative lidar calibration method and related equipment. Background Technology

[0002] As a core sensor for environmental perception, the accuracy of the external parameter calibration between lidar and the vehicle body or other sensors directly determines the overall performance of the perception system.

[0003] Traditional calibration methods mainly rely on manual targets (such as checkerboard patterns, circular reflective spheres, etc.), which require the collection of data from multiple frames of different poses. This process is cumbersome and time-consuming. Furthermore, the targets themselves may shift, causing inconsistencies between the default target coordinates stored in the system and the actual coordinates, resulting in poor calibration performance. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this application provides a dual-target collaborative lidar calibration method, the technical solution of which is as follows: In a first aspect, embodiments of this application provide a dual-target collaborative lidar calibration method, the method comprising: deploying a first target and a second target; wherein, both the first target and the second target are fixedly equipped with an inertial measurement unit and a ranging module, and both the surface of the first target and the second target are provided with at least three non-collinear feature points; The initial attitudes of the first target and the second target are obtained through the inertial measurement unit, and the target distance between the first target and the second target is determined through the ranging module. Based on the initial attitude of the first target, determine the first transformation matrix from the first target coordinate system to the global coordinate system corresponding to the first target; based on the initial attitude of the second target, determine the second transformation matrix from the second target coordinate system to the global coordinate system corresponding to the second target. The first target and the second target are simultaneously placed within the scanning field of view of the lidar to be calibrated, and the lidar is controlled to acquire a frame of point cloud data containing all feature points on the first target and the second target; Extract the three-dimensional coordinates of all feature points on the first target and the second target in the lidar coordinate system from the point cloud data, and denote them as the target observation point set; Establish a global coordinate system; The coordinates of all feature points on the first target in the global coordinate system are determined as a function containing a first unknown translation parameter; wherein, the first unknown translation parameter includes the first position vector of the center of the first target in the global coordinate system; The coordinates of all feature points on the second target in the global coordinate system are determined as a function containing a second unknown translation parameter; wherein, the second unknown translation parameter includes the second position vector of the center of the second target in the global coordinate system; An optimization function is constructed, which includes: a first constraint term constructed based on the target observation point set and the coordinates of all feature points in the global coordinate system; a second constraint term constructed based on the first transformation matrix and the second transformation matrix; and a third constraint term constructed based on the target distance. Solve the optimization function to determine the extrinsic parameters of the lidar.

[0005] Optionally, determining the coordinates of all feature points on the first target in the global coordinate system as a function containing a first unknown translation parameter, and determining the coordinates of all feature points on the second target in the global coordinate system as a function containing a second unknown translation parameter, includes: Define the first position vector of the center of the first target in the global coordinate system as: The coordinates of the i-th feature point on the first target in the global coordinate system for:

[0006] in, Let be the first transformation matrix. Let be the coordinates of the i-th feature point in the first target coordinate system; Define the second position vector of the center of the second target in the global coordinate system as: The coordinates of the j-th feature point on the second target in the global coordinate system for:

[0007] in, This is the second transformation matrix. Let be the coordinates of the j-th feature point in the second target coordinate system.

[0008] Optionally, the construction optimization function includes: Construct the following optimization function:

[0009] in, As the first constraint term, This is the second constraint term. This is the third constraint term. and This is a preset constant.

[0010] Optionally, the construction optimization function includes: The coordinates of the i-th feature point of the first target in the target observation point set in the lidar coordinate system are transformed to the global coordinate system using the lidar's extrinsic parameters:

[0011] in, Let R be the coordinates of the i-th feature point of the first target in the lidar coordinate system, R be the rotation matrix of the extrinsic parameter, and t be the translation matrix of the extrinsic parameter. The coordinates of the j-th feature point of the second target in the target observation point set, in the lidar coordinate system, are transformed to the global coordinate system using the lidar's extrinsic parameters.

[0012] in, Let be the coordinates of the j-th feature point of the second target in the lidar coordinate system, R be the rotation matrix of the extrinsic parameter, and t be the translation matrix of the extrinsic parameter; Construct the first constraint term The first constraint term The formula is:

[0013] in, This represents the total number of feature points on the first target. This represents the total number of feature points on the second target.

[0014] Optionally, the construction optimization function includes: Based on the coordinates of each feature point on the first target in the lidar coordinate system and the coordinates in the first target coordinate system, determine the first local rotation matrix from the first target coordinate system to the lidar coordinate system. Based on the coordinates of each feature point on the second target in the lidar coordinate system and the coordinates in the second target coordinate system, determine the second local rotation matrix from the second target coordinate system to the lidar coordinate system. The second constraint term is determined based on the rotation matrix, the first local rotation matrix, the second local rotation matrix, the first transformation matrix, and the second transformation matrix in the external parameters of the lidar. The second constraint term for:

[0015]

[0016]

[0017] Where R is the rotation matrix in the extrinsic parameters of the lidar. Let be the first local rotation matrix. Let be the second local rotation matrix.

[0018] Optionally, the construction optimization function includes: Calculate the global distance between the center of the first target and the center of the second target in the global coordinate system; The third constraint term is determined based on the global distance and the target distance. for:

[0019] in, The global distance is... The target distance is [value].

[0020] Optionally, solving the optimization function to determine the extrinsic parameters of the lidar includes: The extrinsic parameters of the lidar are determined by solving the optimization function using one of the following methods: Gauss-Newton method, Levenberg-Marquardt algorithm, Newton-Raphson method, and trust region method.

[0021] Optionally, after determining the extrinsic parameters of the lidar, the method further includes: The final root mean square error (RMS) of the first constraint term is calculated based on the extrinsic parameters, using the following formula:

[0022] Among them, the This is the minimum value of the optimized first constraint term determined based on the external parameters; Based on the final first position vector and the final second position vector determined after solving the optimization function, calculate the residual of the distance constraint. The formula is:

[0023] in, This is the final first position vector. This is the final second position vector; The rotation error is calculated based on the extrinsic parameters, the first local rotation matrix, the second local rotation matrix, the first transformation matrix, and the second transformation matrix. The formula is as follows:

[0024]

[0025]

[0026] If the first final root mean square error is less than the first preset threshold, the residual of the distance constraint is less than the second preset threshold, and the rotation error is less than the third preset threshold, the extrinsic parameters are saved; otherwise, a preset operation is performed.

[0027] Optionally, the first preset threshold is 0.02m, the second preset threshold is 0.05m, and the third preset threshold is 0.5°.

[0028] Secondly, this application provides a mobile device with a lidar, the mobile device with a lidar comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to enable the at least one processor to perform a lidar calibration method with dual targets as described in the first aspect.

[0029] Thirdly, this application provides a computer-readable storage medium storing an executable program, which is executed by a processor to implement the dual-target cooperative lidar calibration method as described in the first aspect.

[0030] Fourthly, this application provides a computer program product, which includes a computer program that, when executed by a processor, implements the dual-target collaborative lidar calibration method as described in the first aspect.

[0031] This application provides a dual-target coordinated lidar calibration method and related equipment, the advantages of which include: 1. By setting up an IMU and ranging module on the target, the calculation of the external parameters of the lidar no longer relies on the pre-stored coordinates of the target. Instead, it uses the relative attitude and relative distance between the two targets. On the one hand, this solves the problem of calibration error caused by target position offset in traditional methods. On the other hand, the target does not need to be precisely horizontal or directly facing the lidar. The IMU automatically compensates for attitude deviation. The two targets only need to be roughly placed in the common field of view, eliminating the need for manual fine alignment and reducing the difficulty of calibration.

[0032] 2. During the calibration process, the lidar only needs to collect one frame of point cloud data to complete the external parameter solution, reducing the calibration time to the second level, which is more than two orders of magnitude better than the traditional method (10-30 minutes).

[0033] 3. Strong robustness: The two targets verify each other. Even if the extraction of local feature points of one target is affected by noise, the overall optimization can still obtain the correct solution through the other target and multiple constraints. Attached Figure Description

[0034] Figure 1 This is a flowchart illustrating the dual-target collaborative lidar calibration method provided in the embodiments of this application.

[0035] Figure 2 This is a schematic diagram of the structure of a mobile device with lidar provided in an embodiment of this application.

[0036] Figure 3 This is a structural block diagram of a computer-readable storage medium provided in an embodiment of this application. Detailed Implementation

[0037] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0038] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0039] For ease of description of the first and second directions in the embodiments of this application, the first direction is the up-down direction in the figures, the second direction is the front-back direction in the figures, and the third direction is the left-right direction in the figures. The x-axis arrow direction is referred to as the "up" direction, the y-axis arrow direction as the "back" direction, and the z-axis arrow direction as the "right" direction, but these are not the sole limitations in the actual application of this application.

[0040] Please see Figure 1 , Figure 1 This is a schematic flowchart of the dual-target collaborative lidar calibration method provided in the embodiments of this application. Figure 1 As shown, the dual-target cooperative lidar calibration method 100 of this application includes steps 101 to 110.

[0041] Step 101: Deploy the first target and the second target; wherein, the first target and the second target are both fixedly equipped with an inertial measurement unit and a ranging module, and the surfaces of the first target and the second target are provided with at least three non-collinear feature points.

[0042] Optionally, the ranging module can be an ultra-wideband (UWB) ranging module.

[0043] Optionally, each target surface has at least three non-collinear feature points, which can be checkerboard corner points, circular reflective sphere centers, or ArUco marker centers; this application does not impose any restrictions.

[0044] The three-dimensional coordinates of the feature points in the corresponding target coordinate system are obtained through precise measurements at the time of manufacture.

[0045] Optionally, the origin of the target coordinate system for each target is the target center, the XY plane is parallel to the substrate, and the Z axis is perpendicular to the substrate and extends outward.

[0046] Step 102: Obtain the initial attitudes of the first target and the second target through the inertial measurement unit, and determine the target distance between the first target and the second target through the ranging module.

[0047] Optionally, the processing unit sends attitude acquisition commands to the first and second targets. The IMU of each target measures the direction of gravity in a stationary state, calculates the pitch and roll angles using an accelerometer, and obtains the yaw angle using a magnetometer.

[0048] Optionally, the processing unit controls the UWB ranging modules on the first and second targets to perform two-way time-of-flight (TOF) ranging. The ranging process is as follows: 1. The first target A sends a ranging request and records the timestamp t1.

[0049] 2. After receiving the request, the second target B replies with confirmation and records the timestamp t2.

[0050] 3. After receiving confirmation, the first target A records the timestamp t3.

[0051] 4. Calculate the signal flight time ,in The processing delay for the second target is a pre-defined, known constant.

[0052] 5. Target distance = c * TOF, where c is the speed of light.

[0053] Optionally, to avoid errors, multiple target distance measurements can be performed to obtain multiple target distances, and then the average value can be taken as the final distance.

[0054] Step 103: Determine the first transformation matrix from the first target coordinate system to the global coordinate system based on the initial attitude of the first target, and determine the second transformation matrix from the second target coordinate system to the global coordinate system based on the initial attitude of the second target.

[0055] Optionally, the pitch angle θ and roll angle measured by the target's IMU are used. The yaw angle ψ is used to determine the rotation matrices from the three coordinate axes of the target coordinate system to the three coordinate axes of the global coordinate system.

[0056] Rotation matrix corresponding to the Z-axis ; Rotation matrix corresponding to the Y-axis ; Rotation matrix corresponding to the X-axis ; Transformation matrix from target coordinate system to global coordinate system Define the first transformation matrix from the first target coordinate system to the global coordinate system as follows: The measurement results of the IMU of the first target were then introduced into... The calculation formula can be obtained. Define the second transformation matrix from the second target coordinate system to the global coordinate system corresponding to the second target as follows: Substitute the measurement results of the IMU of the second target into The calculation formula can be obtained. .

[0057] Optionally, the first relative rotation matrix of target B relative to target A is:

[0058] in, This describes the rotation required to align the coordinate system of target B with that of target A. Due to noise in the attitude angles measured by the IMU, this can be applied to multiple consecutive frames (e.g., 10 frames). Take the average to improve accuracy.

[0059] Step 104: Place the first target and the second target simultaneously within the scanning field of view of the lidar to be calibrated, and control the lidar to acquire a frame of point cloud data containing all feature points on the first target and the second target.

[0060] Optionally, the lidar outputs point cloud data for the frame, with each point containing three-dimensional coordinates (x, y, z) and echo intensity value I.

[0061] Optionally, outlier filtering can also be performed on the point cloud data: for each point, the number of points in its neighborhood (radius 0.1m) is calculated, and if there are fewer than 3, it is considered an outlier and removed.

[0062] Optionally, voxel downsampling (voxel side length 0.02m) can also be performed, retaining only one point within each voxel (usually the centroid) to homogenize the point cloud density and reduce computational load.

[0063] Step 105: Extract the three-dimensional coordinates of all feature points on the first target and the second target in the lidar coordinate system from the point cloud data, and denote them as the target observation point set.

[0064] Optionally, step 105 includes steps 1051 to 1057.

[0065] Step 1051: Obtain the echo intensity value of each point in the preprocessed point cloud, statistically analyze the intensity value distribution, and determine the high reflection threshold and low reflection threshold; classify points with intensity values ​​higher than the high reflection threshold into the high reflection point set, and classify points with intensity values ​​lower than the low reflection threshold into the low reflection point set.

[0066] Step 1052: Perform Euclidean clustering on the high-reflectivity point set and the low-reflectivity point set respectively, and group points whose spatial distance is less than the preset clustering radius into the same cluster to obtain multiple candidate point cloud clusters; calculate the bounding box size of each candidate point cloud cluster, retain the clusters whose bounding box side length is within the preset target size range, and select two clusters from them as the first candidate cluster and the second candidate cluster respectively.

[0067] Step 1053: Fit the plane to the first candidate cluster using the random sampling consensus algorithm to obtain the first plane model, and use the interior point set of the first plane model as the first target precise point cloud; perform the same operation on the second candidate cluster to obtain the second plane model and the second target precise point cloud.

[0068] The RANSAC algorithm randomly selects three points to determine a candidate plane, and then calculates the distance from each point in the candidate cluster to the candidate plane. If the distance from a point to the plane is less than a pre-set threshold, the point is considered an interior point; otherwise, it is an exterior point. The set of interior points is the collection of all interior points.

[0069] For example, the preset threshold is 0.005 meters.

[0070] Step 1054: Establish a first planar local coordinate system with the normal vector of the first planar model as the Z-axis and the projection of the centroid of the first target precise point cloud onto the first plane as the origin; project all points in the first target precise point cloud onto the first plane and transform them into the first planar local coordinate system to obtain a first two-dimensional projection point set; perform the same operation on the second target precise point cloud to obtain a second two-dimensional projection point set.

[0071] Step 1055: Determine the minimum and maximum values ​​of the first two-dimensional projection point set in the X and Y axes, divide it into regular grids, take the average echo intensity of all projection points in each grid as the pixel value of that grid, and perform neighborhood interpolation on empty grids to generate a first intensity image; perform the same operation on the second two-dimensional projection point set to generate a second intensity image.

[0072] Step 1056: On the first intensity image, according to the preset feature point type of the first target, the sub-pixel level two-dimensional coordinates of each feature point are extracted using a corner detection algorithm or a circle detection algorithm to obtain the first feature point pixel coordinate set; the same operation is performed on the second intensity image to obtain the second feature point pixel coordinate set.

[0073] Step 1057: Project each coordinate point in the first feature point pixel coordinate set back onto the first plane according to the transformation relationship between the first plane local coordinate system and the lidar coordinate system, calculate the corresponding three-dimensional spatial coordinates, and use them as the three-dimensional coordinates of each feature point on the first target in the lidar coordinate system, and output them as the first target observation point set; perform the same operation on the second feature point pixel coordinate set, and output them as the second target observation point set.

[0074] Step 106: Establish a global coordinate system.

[0075] Optionally, the world coordinate system referenced by the IMU can be used as the global coordinate system. The world coordinate system referenced by the IMU is usually the Northeast-Sky (ENU) coordinate system: the X-axis points to geographic east, the Y-axis points to geographic north, and the Z-axis is perpendicular to the horizontal plane and upward (opposite to the direction of gravity).

[0076] Step 107: Determine the coordinates of all feature points on the first target in the global coordinate system as a function containing a first unknown translation parameter; wherein, the first unknown translation parameter includes the first position vector of the center of the first target in the global coordinate system.

[0077] Optionally, step 107 includes steps 1071 to 1072.

[0078] Step 1071: Determine the first transformation matrix from the first target coordinate system to the global coordinate system based on the initial attitude of the first target. .

[0079] Specifically, The calculation method has been introduced in step 103 and will not be repeated here.

[0080] Step 1072: Define the first position vector of the center of the first target in the global coordinate system as... The coordinates of the i-th feature point on the first target in the global coordinate system for:

[0081] in, Let be the first transformation matrix. Let be the coordinates of the i-th feature point in the first target coordinate system.

[0082] in, This is an unknown value and needs to be determined later when solving the optimization function.

[0083] Step 108: Determine the coordinates of all feature points on the second target in the global coordinate system as a function containing a second unknown translation parameter; wherein, the second unknown translation parameter includes the second position vector of the center of the second target in the global coordinate system.

[0084] Optionally, step 108 includes steps 1081 to 1082.

[0085] Step 1081: Determine the second transformation matrix from the second target coordinate system to the global coordinate system based on the initial attitude of the second target. .

[0086] Specifically, The calculation method has been introduced in step 103 and will not be repeated here.

[0087] Step 1082: Define the second position vector of the center of the second target in the global coordinate system as... The coordinates of the j-th feature point on the second target in the global coordinate system for:

[0088] in, This is the second transformation matrix. Let be the coordinates of the j-th feature point in the second target coordinate system.

[0089] in, This is an unknown value and needs to be determined later when solving the optimization function.

[0090] Step 109: Construct an optimization function, which includes: a first constraint term constructed based on the target observation point set and the coordinates of all feature points in the global coordinate system; a second constraint term constructed based on the first relative rotation matrix; and a third constraint term constructed based on the target distance.

[0091] Alternatively, the optimization function can be constructed as follows: .

[0092] in, As the first constraint term, This is the second constraint term. This is the third constraint term. and This is a preset constant.

[0093] in, and This application does not impose any restrictions on the specific values; you can choose them as needed.

[0094] Alternatively, the optimization function can also use the first constraint term. However, in this application, to achieve extrinsic parameter calibration using only one frame of data points, a sufficient number of independent constraints need to be introduced to replace the geometric changes provided by multiple frames of data. Therefore, using all three constraint terms together is the optimal solution. However, in some simplified applications (e.g., where 2-3 frames of data can be collected), the second or third constraint term can be omitted, and calibration can still be achieved, although accuracy and robustness will decrease.

[0095] The first constraint term is discussed below. Let me introduce it.

[0096] Optionally, the first constraint term The calculation method is as follows.

[0097] (1) Transform the coordinates of the i-th feature point of the first target in the target observation point set in the lidar coordinate system to the global coordinate system using the extrinsic parameters of the lidar:

[0098] in, Let R be the coordinates of the i-th feature point of the first target in the lidar coordinate system, R be the rotation matrix of the extrinsic parameter, and t be the translation matrix of the extrinsic parameter.

[0099] (2) Transform the coordinates of the j-th feature point of the second target in the target observation point set into the global coordinate system using the extrinsic parameters of the lidar:

[0100] in, Let be the coordinates of the j-th feature point of the second target in the lidar coordinate system, R be the rotation matrix of the extrinsic parameter, and t be the translation matrix of the extrinsic parameter.

[0101] (3) Construct the first constraint term The first constraint term The formula is:

[0102] in, This represents the total number of feature points on the first target. This represents the total number of feature points on the second target.

[0103] In the steps above, The coordinates of the i-th feature point obtained by transforming the first target coordinate system to the global coordinate system are: Let be the coordinates of the i-th feature point obtained by transforming the lidar coordinate system to the global coordinate system. Theoretically, if the lidar's extrinsic parameters are correct, then... The modulus is zero or close to zero. Similarly.

[0104] By minimizing the reprojection error between the theoretical feature point coordinates and the measured point cloud coordinates after extrinsic parameter transformation, the accurate solution of the extrinsic parameters is directly driven, ensuring the optimal alignment between the point cloud data and the target geometric model, which is the core guarantee of calibration accuracy.

[0105] The second constraint term is discussed below. Let me introduce it.

[0106] Optionally, the second constraint term The calculation method is as follows: (1) Determine the first local rotation matrix from the first target coordinate system to the lidar coordinate system based on the coordinates of each feature point on the first target in the lidar coordinate system and the coordinates of each feature point in the first target coordinate system.

[0107] (2) Determine the second local rotation matrix from the second target coordinate system to the lidar coordinate system based on the coordinates of each feature point on the second target in the lidar coordinate system and the coordinates of each feature point in the second target coordinate system.

[0108] (3) Determine the second constraint term based on the rotation matrix, the first local rotation matrix, the second local rotation matrix, the first transformation matrix, and the second transformation matrix in the external parameters of the lidar; The second constraint term for:

[0109]

[0110]

[0111] Where R is the rotation matrix in the extrinsic parameters of the lidar. Let be the first local rotation matrix. Let be the second local rotation matrix.

[0112] in,

[0113] in, This is the sum of squares of the elements of the matrix, used to calculate the difference between two rotation matrices.

[0114] Second constraint term By aligning the absolute target attitude measured independently by the IMU with the attitude calculated by the lidar, the solution space for rotational extrinsic parameters is effectively constrained, the uncertainty of rotational degrees of freedom under single-frame data is eliminated, and the rotational accuracy and overall robustness of the calibration results are significantly improved.

[0115] The third constraint term is discussed below. Let me introduce it.

[0116] Optionally, the third constraint term The calculation method is as follows.

[0117] (1) Calculate the global distance between the center of the first target and the center of the second target in the global coordinate system.

[0118] Among them, global distance .

[0119] (2) Determine the third constraint term based on the global distance and the target distance, wherein the third constraint term... for:

[0120] in, The global distance is... The target distance is [value].

[0121] Third distance constraint term Using the absolute distance between targets measured independently by the ranging module as a rigid scale benchmark effectively constrains the scaling degree of freedom of the translation extrinsic parameters, eliminates scale drift that may exist in lidar observation, and significantly improves the translation accuracy and global consistency of the calibration results.

[0122] Step 110: Solve the optimization function to determine the extrinsic parameters of the lidar.

[0123] Optionally, solving the optimization function refers to finding the minimum value of the optimization function, which is a nonlinear least squares optimization problem. The optimization function can be solved by any one of the Gauss-Newton method, Levenberg-Marquardt algorithm, Newton-Raphson method, and trust region method to determine the extrinsic parameters of the lidar.

[0124] This application does not impose any restrictions on the specific solution method used.

[0125] For example, this application provides an example process for solving the above optimization function using the Levenberg-Marquardt algorithm: (1) Define a state vector x, which contains the axis angle parameters corresponding to the rotation matrix of the lidar extrinsic parameters to be solved. , Translation vector of lidar extrinsic parameters The first position vector of the first target center in the global coordinate system And the second position vector of the second target center in the global coordinate system. Right now That is, a 12-dimensional real vector space. Initialize the state vector by setting the axis angle parameter and the translation vector to zero, setting the first position vector to the centroid coordinates of the first target observation point set, and setting the second position vector to the centroid coordinates of the second target observation point set; set the initial damping factor. Damping adjustment factor Convergence threshold and the maximum number of iterations .

[0126] (2) Based on the current state vector (where k is the current iteration number, initially k=0), construct a residual vector The sum of squares of the residual vectors is equal to the optimization function. .

[0127] (3) Calculate the Jacobian matrix of the total residual vector with respect to the state vector. Each row of the Jacobian matrix corresponds to the partial derivative of a residual component with respect to each state variable; a numerical difference method is used to apply a small perturbation to each state variable. The residual changes after the perturbation are calculated to approximate each column of the Jacobian matrix.

[0128] (4) Solve the following system of linear equations using the Choleski decomposition method to obtain the increment of the state vector. The system of linear equations is as follows:

[0129] in The current damping factor, initially I is the identity matrix; (5) The increment Superimposed on the current state vector, the candidate state vector is obtained. Based on the candidate state vector, recalculate the total residual vector according to step (9.2). and candidate error sum of squares .

[0130] (6) Compare the candidate sum of squared errors with the current sum of squared errors: like Then accept the candidate state vector and let and reduce the damping factor ; like If so, then reject the candidate state vector and maintain... and increase the damping factor Return to step (4) to recalculate the increment (without increasing the number of iterations).

[0131] (7) Determine if the convergence condition is met: If and or number of iterations k+1 reaches the maximum number of iterations If the iteration terminates, the axis-angle parameters in the current state vector are changed. The transformation is converted to a rotation matrix R using the Rodriguez formula, and the translation vector t is extracted as the extrinsic parameter output of the lidar; otherwise, let... Return to step (2) and continue iterating.

[0132] Optionally, after step 110, the obtained extrinsic parameters are also verified. In this case, the method 100 provided by this application further includes steps 111 to 114.

[0133] Step 111: Calculate the final root mean square error (RMS) of the first constraint term based on the extrinsic parameters, using the following formula:

[0134] Among them, the This is the minimum value of the optimized first constraint term determined based on the extrinsic parameters.

[0135] The division by 3 in the RMS formula is because each feature point has three independent coordinate components (x, y, z) in space, and each component contributes to a residual. RMS is defined as the root mean square of the residuals, which is the average error of each component; therefore, the denominator is the total number of residuals, 3. ).

[0136] Step 112: Calculate the residuals of the distance constraints based on the final first position vector and the final second position vector determined after solving the optimization function. The formula is:

[0137] in, This is the final first position vector. This is the final second position vector, where the optimal value is obtained simultaneously when solving the optimization function. Finally, the optimal solution is obtained. That is , Similarly.

[0138] Step 113: Calculate the rotation error based on the extrinsic parameters, the first local rotation matrix, the second local rotation matrix, the first transformation matrix, and the second transformation matrix. The formula is as follows:

[0139]

[0140]

[0141] Step 114: If the first final root mean square error is less than the first preset threshold, the residual of the distance constraint is less than the second preset threshold, and the rotation error is less than the third preset threshold, save the extrinsic parameters; otherwise, execute the preset operation.

[0142] Optionally, the first preset threshold is 0.02m, the second preset threshold is 0.05m, and the third preset threshold is 0.5°.

[0143] Optionally, the preset operations include sending information to the user management terminal, recalculating external parameters, and starting a self-test program, etc., which are not limited in this application.

[0144] The final root mean square error (RMS) quantifies the average spatial deviation between the theoretical projection and the measured point cloud after calibration, providing an intuitive accuracy evaluation index for the calibration results and facilitating the determination of whether the calibration meets application requirements.

[0145] The distance constraint residuals verified the consistency between the optimized center distance between the two targets and the measured distance by UWB. This can effectively identify scale anomalies in the solution of translational extrinsic parameters and enhance the reliability of the calibration results.

[0146] The rotation error is directly quantified by comparing the absolute target attitude calculated by the lidar with the attitude independently measured by the IMU. This provides a reliable basis for verifying the rotation accuracy and effectively prevents rotation drift caused by point cloud noise or feature extraction errors from going undetected.

[0147] By comprehensively using three indicators—root mean square error, distance constraint residual, and rotation error—the reliability of calibration results can be evaluated from three dimensions: position, distance, and orientation. This avoids the limitations of a single indicator and ensures that the calibration quality meets the requirements of engineering applications.

[0148] Please see Figure 2 , Figure 2 This is a schematic diagram of the structure of a mobile device with lidar provided in an embodiment of this application. Figure 2 As shown, the mobile device 200 with lidar includes: one or more processors 210 and memory 220. Figure 2 Take a processor 210 as an example.

[0149] In some implementations, the processor 210 and the memory 220 can be connected via a bus or other means. Figure 2 Taking the example of a connection between China and Israel via a bus.

[0150] In some embodiments, the processor 210 is used to deploy a first target and a second target; wherein both the first target and the second target are fixedly equipped with an inertial measurement unit and a ranging module, and both the first target and the second target have at least three non-collinear feature points on their surfaces; the initial attitudes of the first target and the second target are acquired through the inertial measurement unit, and the target distance between the first target and the second target is determined through the ranging module; a first transformation matrix from the first target coordinate system to the global coordinate system is determined based on the initial attitude of the first target, and a second transformation matrix from the second target coordinate system to the global coordinate system is determined based on the initial attitude of the second target; the first target and the second target are simultaneously placed within the scanning field of view of the lidar to be calibrated, and the lidar is controlled to acquire a frame of point cloud data containing all feature points on the first target and the second target; the feature points are extracted from the point cloud data respectively. The three-dimensional coordinates of all feature points on the first and second targets in the lidar coordinate system are denoted as the target observation point set. A global coordinate system is established. The coordinates of all feature points on the first target in the global coordinate system are determined as a function containing a first unknown translation parameter, wherein the first unknown translation parameter includes the first position vector of the center of the first target in the global coordinate system. The coordinates of all feature points on the second target in the global coordinate system are determined as a function containing a second unknown translation parameter, wherein the second unknown translation parameter includes the second position vector of the center of the second target in the global coordinate system. An optimization function is constructed, which includes: a first constraint term constructed based on the target observation point set and the coordinates of all feature points in the global coordinate system, a second constraint term constructed based on the first transformation matrix and the second transformation matrix, and a third constraint term constructed based on the target distance. The optimization function is solved to determine the extrinsic parameters of the lidar.

[0151] In some implementations, the memory 220 serves as a non-volatile computer-readable storage medium, used to store non-volatile software programs, non-volatile computer-executable programs, and modules, such as the program instructions / modules of the dual-target collaborative lidar calibration method in the embodiments of this application. The processor 210 executes various functional applications and data processing of the mobile device with lidar by running the non-volatile software programs, instructions, and modules stored in the memory 220, thereby implementing the dual-target collaborative lidar calibration method of the above-described method embodiments.

[0152] In some embodiments, memory 220 may include a program storage area and a data storage area, wherein the program storage area may store the operating system and applications required for at least one function; the data storage area may store data created based on the use of the lidar-equipped mobile device, etc. Furthermore, memory 220 may include high-speed random access memory and may also include non-volatile memory, such as at least one disk storage device, flash memory device, or other non-volatile solid-state storage device. In some embodiments, memory 220 may optionally include memory remotely located relative to processor 210, and these remote memories may be connected to the controller via a network. Examples of such networks include, but are not limited to, the Internet, corporate intranets, local area networks, mobile communication networks, and combinations thereof.

[0153] In some implementations, one or more modules are stored in memory 220 and, when executed by one or more processors 210, perform the dual-target collaborative lidar calibration method in any of the above method embodiments, for example, performing the method described above. Figure 1 Steps 101 to 110 in the method.

[0154] Please refer to Figure 3 , Figure 3 This is a structural block diagram of a computer-readable storage medium provided in an embodiment of this application. The computer-readable storage medium 300 stores program code 310, which can be called by a processor to execute the dual-target cooperative lidar calibration method described in the above method embodiments.

[0155] The computer-readable storage medium 300 may be an electronic memory such as flash memory, EEPROM (Electrically Erasable Programmable Read-Only Memory), EPROM, hard disk, or ROM. Optionally, the computer-readable storage medium includes a non-volatile computer-readable medium. The computer-readable storage medium 300 has storage space for program code that performs any of the method steps of the control method described above. This program code can be read from or written to one or more computer program products. The program code may, for example, be compressed in a suitable form.

[0156] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.

Claims

1. A dual-target coordinated lidar calibration method, characterized in that, The method includes: Deploy a first target and a second target; wherein, both the first target and the second target are fixedly equipped with an inertial measurement unit and a ranging module, and both the first target and the second target have at least three non-collinear feature points on their surfaces; The initial attitudes of the first target and the second target are obtained through the inertial measurement unit, and the target distance between the first target and the second target is determined through the ranging module. Based on the initial attitude of the first target, determine the first transformation matrix from the first target coordinate system to the global coordinate system corresponding to the first target; based on the initial attitude of the second target, determine the second transformation matrix from the second target coordinate system to the global coordinate system corresponding to the second target. The first target and the second target are simultaneously placed within the scanning field of view of the lidar to be calibrated, and the lidar is controlled to acquire a frame of point cloud data containing all feature points on the first target and the second target; Extract the three-dimensional coordinates of all feature points on the first target and the second target in the lidar coordinate system from the point cloud data, and denote them as the target observation point set; Establish a global coordinate system; The coordinates of all feature points on the first target in the global coordinate system are determined as a function containing a first unknown translation parameter; wherein, the first unknown translation parameter includes the first position vector of the center of the first target in the global coordinate system; The coordinates of all feature points on the second target in the global coordinate system are determined as a function containing a second unknown translation parameter; wherein, the second unknown translation parameter includes the second position vector of the center of the second target in the global coordinate system; An optimization function is constructed, which includes: a first constraint term constructed based on the target observation point set and the coordinates of all feature points in the global coordinate system; a second constraint term constructed based on the first transformation matrix and the second transformation matrix; and a third constraint term constructed based on the target distance. Solve the optimization function to determine the extrinsic parameters of the lidar.

2. The method according to claim 1, characterized in that, The step of determining the coordinates of all feature points on the first target in the global coordinate system as a function containing a first unknown translation parameter, and determining the coordinates of all feature points on the second target in the global coordinate system as a function containing a second unknown translation parameter, includes: Define the first position vector of the center of the first target in the global coordinate system as: The coordinates of the i-th feature point on the first target in the global coordinate system for: in, Let be the first transformation matrix. Let be the coordinates of the i-th feature point in the first target coordinate system; Define the second position vector of the center of the second target in the global coordinate system as: The coordinates of the j-th feature point on the second target in the global coordinate system for: in, This is the second transformation matrix. Let be the coordinates of the j-th feature point in the second target coordinate system.

3. The method according to claim 2, characterized in that, The construction optimization function includes: Construct the following optimization function: in, As the first constraint term, This is the second constraint term. This is the third constraint term. and This is a preset constant.

4. The method according to claim 3, characterized in that, The construction optimization function includes: The coordinates of the i-th feature point of the first target in the target observation point set in the lidar coordinate system are transformed to the global coordinate system using the lidar's extrinsic parameters: in, Let R be the coordinates of the i-th feature point of the first target in the lidar coordinate system, R be the rotation matrix of the extrinsic parameter, and t be the translation matrix of the extrinsic parameter. The coordinates of the j-th feature point of the second target in the target observation point set, in the lidar coordinate system, are transformed to the global coordinate system using the lidar's extrinsic parameters. in, Let be the coordinates of the j-th feature point of the second target in the lidar coordinate system, R be the rotation matrix of the extrinsic parameter, and t be the translation matrix of the extrinsic parameter; Construct the first constraint term The first constraint term The formula is: in, This represents the total number of feature points on the first target. This represents the total number of feature points on the second target.

5. The method according to claim 4, characterized in that, The construction optimization function includes: Based on the coordinates of each feature point on the first target in the lidar coordinate system and the coordinates in the first target coordinate system, determine the first local rotation matrix from the first target coordinate system to the lidar coordinate system. Based on the coordinates of each feature point on the second target in the lidar coordinate system and the coordinates in the second target coordinate system, determine the second local rotation matrix from the second target coordinate system to the lidar coordinate system. The second constraint term is determined based on the rotation matrix, the first local rotation matrix, the second local rotation matrix, the first transformation matrix, and the second transformation matrix in the external parameters of the lidar. The second constraint term for: Where R is the rotation matrix in the extrinsic parameters of the lidar. Let be the first local rotation matrix. Let be the second local rotation matrix.

6. The method according to claim 5, characterized in that, The construction optimization function includes: Calculate the global distance between the center of the first target and the center of the second target in the global coordinate system; The third constraint term is determined based on the global distance and the target distance. for: in, The global distance is... The target distance is [value].

7. The method according to claim 6, characterized in that, Solving the optimization function to determine the extrinsic parameters of the lidar includes: The extrinsic parameters of the lidar are determined by solving the optimization function using one of the following methods: Gauss-Newton method, Levenberg-Marquardt algorithm, Newton-Raphson method, and trust region method.

8. The method according to claim 7, characterized in that, After determining the extrinsic parameters of the lidar, the method further includes: The final root mean square error (RMS) of the first constraint term is calculated based on the extrinsic parameters, using the following formula: Among them, the This is the minimum value of the optimized first constraint term determined based on the external parameters; Based on the final first position vector and the final second position vector determined after solving the optimization function, calculate the residual of the distance constraint. The formula is: in, This is the final first position vector. This is the final second position vector; The rotation error is calculated based on the extrinsic parameters, the first local rotation matrix, the second local rotation matrix, the first transformation matrix, and the second transformation matrix. The formula is as follows: If the first final root mean square error is less than the first preset threshold, the residual of the distance constraint is less than the second preset threshold, and the rotation error is less than the third preset threshold, the extrinsic parameters are saved; otherwise, a preset operation is performed.

9. The method according to claim 8, characterized in that, The first preset threshold is 0.02m, the second preset threshold is 0.05m, and the third preset threshold is 0.5°.

10. A mobile device with a lidar, characterized in that, The mobile device includes: At least one processor; and, A memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor to enable the at least one processor to perform the dual-target collaborative lidar calibration method as described in any one of claims 1 to 9.

Images