A quaternional whale optimization calibration method for low-resolution lidar and camera

Abstract

The application relates to the field of automatic driving multi-sensor calibration, in particular to a quaternion whale optimization calibration method for low-resolution laser radar and a camera, the method collects synchronous data by controlling an automatic driving vehicle to follow a preset 'S' shaped track, constructs an adaptive voxel map by using a normal distribution transformation algorithm, generates a high-precision point cloud map by combining B-spline track optimization, extracts multi-modal edge features, and robustly estimates an initial rotation based on epipolar constraints, realizes cross-modal feature coarse alignment through a quaternion whale optimization algorithm, and obtains pixel-level precision external parameters through multi-frame joint nonlinear optimization, the application does not need artificial targets and prior external parameters, solves the industry pain point of insufficient calibration precision of low-resolution laser radar (the number of line bundles is less than or equal to 32), has scene robustness and sensor universality, and can meet the efficient deployment demand of an automatic driving system.

Description

Technical Field

[0001] This invention relates to the field of multi-sensor calibration for autonomous driving, specifically a quaternion-based whale-optimized calibration method for low-resolution LiDAR and cameras. Background Technology

[0002] With the rapid development of autonomous driving technology, multi-sensor fusion has become a core solution for environmental perception. LiDAR and cameras, as key sensors in autonomous vehicles, provide 3D point cloud and 2D image information respectively, jointly supporting core functions such as environmental perception, decision-making, and planning. Accurate calibration between sensors is a prerequisite for multimodal data fusion and directly affects key performance aspects such as target detection and positioning accuracy.

[0003] Currently, calibration methods for low-resolution lidar and cameras have significant limitations: on the one hand, traditional methods rely on manual target placement, which is not only cumbersome and time-consuming, but also difficult to deploy in large-scale outdoor scenarios; on the other hand, existing targetless methods are mostly designed for high-resolution lidar, and under the condition of sparse point clouds and insufficient feature information in low-resolution lidar, they are prone to problems such as low calibration accuracy and poor robustness, and are also sensitive to the initial extrinsic parameter estimation, easily getting trapped in local optima.

[0004] Therefore, for low-resolution lidar, researching target-free automatic calibration technology is of great significance for improving the practicality and deployability of autonomous driving systems. Summary of the Invention

[0005] This invention aims to solve the problems of "target dependence", "sensitivity to initial values" and "insufficient accuracy" in existing low-resolution lidar and camera calibration methods, and provides a quaternion-based whale-optimized calibration method for low-resolution lidar and cameras.

[0006] This invention solves the above-mentioned technical problems through the following technical solution: a quaternion-based whale optimization calibration method for low-resolution lidar and cameras, the method comprising the following steps:

[0007] S1. Data Acquisition: In a structured road environment, control the autonomous vehicle to drive along a preset "S" shaped trajectory, and simultaneously collect and store the point cloud data of low-resolution LiDAR and the image sequence data of the camera.

[0008] S2. Point cloud map construction and trajectory optimization: Based on the collected lidar point cloud data, an initial point cloud map is constructed using the normal distribution transformation algorithm, and a high-precision point cloud map and optimized lidar motion trajectory are generated using B-spline trajectory optimization.

[0009] S3. Feature Extraction and Initial Rotation Estimation: Extract depth-continuous edge features from high-precision point cloud maps, extract structured edge features from the first frame image of the camera, extract detailed edge features from each frame image, and robustly estimate the initial rotation based on the "S"-shaped trajectory motion epipolar constraint to determine the common field of view.

[0010] S4. Quaternion Whale Optimization Coarse Calibration: Given the initial extrinsic parameter estimate, the continuous edge features of the point cloud depth are projected onto the camera image coordinate system. The quaternion whale optimization algorithm is applied to optimize the solution of the initial extrinsic parameter matrix by maximizing the spatial consistency between the projected features and the structured edge features.

[0011] S5. Continuous Time Point-to-Line Fine Optimization: Using the initial extrinsic parameters obtained from the optimization, the continuous edge features of the point cloud depth corresponding to each frame are projected onto the corresponding camera image coordinate system. The Euclidean distance from the projection point to the image detail edge features is calculated. The distance error function of all image frames is jointly optimized to obtain a high-precision extrinsic parameter matrix.

[0012] The positive and progressive effects of this invention are as follows:

[0013] 1. By constructing an adaptive NDT map and extracting deep continuous edges, high-precision calibration can still be achieved under the condition of sparse point clouds of low-resolution sensor lidar. Translation and rotation errors are relatively reduced, breaking through the dependence of existing methods on high-resolution radar. Through "S"-shaped trajectory design and QWOA algorithm, cross-modal feature alignment is automatically achieved, which greatly improves the calibration efficiency. No manual target placement or input of prior extrinsic parameters is required throughout the process.

[0014] 2. By integrating L-CNN structured edge detection with Canny detail edge detection, Tukey robust loss function, and elite cross-strategy, the projection residual is smaller in complex scenes, resulting in lower error compared to traditional target methods. It is compatible with mechanical and solid-state low-resolution LiDAR, and the calibration process does not require complex auxiliary equipment. It can be directly integrated into the sensor calibration system of autonomous vehicles to meet the needs of mass production deployment. Attached Figure Description

[0015] Figure 1 This is a flowchart of the method of the present invention.

[0016] Figure 2 These are schematic diagrams of the four experimental scenarios (urban roads, park roads, parking lots, and suburban roads) of this invention.

[0017] Figure 3 This is a box plot showing the average projection residuals between the optimized lidar feature points and image feature points in this invention.

[0018] Figure 4 The figure shows the fitness value iteration curves of the quaternion whale optimization algorithm of this invention in different scenarios.

[0019] Figure 5 This is a comparison chart of the RMSE (root mean square error) of the final calibration structure in four scenarios of this invention. Detailed Implementation

[0020] The present invention will be further illustrated by way of embodiments below, but the present invention is not limited to the scope of the embodiments.

[0021] See Figures 1 to 5 A quaternion-based whale optimization calibration method for low-resolution lidar and cameras, comprising the following steps:

[0022] S1, Data Acquisition

[0023] S11. Trajectory Planning and Execution: Controlling autonomous vehicles to travel along a preset "S" shaped trajectory in a structured road environment;

[0024] S12. Data preprocessing: During driving, the low-resolution lidar and camera are synchronously acquired through the time synchronization module, generating and storing point cloud data streams and image sequences with timestamps. The low-resolution lidar is a mechanical lidar or a solid-state lidar with ≤32 beams, and the lidar point cloud data must include the time offset of each laser point relative to the start time of its respective scan frame for subsequent point cloud motion distortion correction.

[0025] S2, Point Cloud Map Construction and Trajectory Optimization

[0026] S21. Initial NDT Point Cloud Map Construction: The point cloud map is divided into voxels of adaptive size using a voxel grid. Within each voxel, the point cloud covariance matrix is ​​calculated using principal component analysis.

[0027] ;

[0028] Decomposition to obtain eigenvalues and corresponding feature vectors ;

[0029] Set feature threshold ,like If the voxel conforms to a planar structure, then the planar parameters are recorded. ,in It is the normal vector. The intercept is... The centroid of a plane;

[0030] If a voxel does not meet the planar condition, it is divided into 8 smaller voxels. S1 is repeated until the number of voxel divisions reaches the set threshold to form an initial NDT point cloud map.

[0031] S22, B-spline initial trajectory fitting: The laser radar trajectory is modeled using cubic uniform B-splines (order d=3). The translation component p(t) of the trajectory is defined using a cumulative spline matrix, and the rotation component q(t) is also defined using a cumulative spline matrix, where:

[0032] ;

[0033] ;

[0034] For cumulative spline matrix, for Exponential mapping on, as well as These are the control points for the translation and rotation of the B-spline, respectively. for Logarithmic mapping on, This refers to quaternion multiplication. , Based on the odometry pose data provided by the NDT algorithm, a residual function is constructed to minimize the deviation between the odometry pose and the B-spline trajectory pose, and the initial B-spline trajectory is obtained by solving the problem.

[0035] S23. Trajectory and Map Joint Optimization: Motion compensation is performed on each frame of the LiDAR point cloud using the initial B-spline trajectory to eliminate point cloud distortion caused by sensor motion. The corrected point cloud is transformed to the world coordinate system. The nearest neighbor plane corresponding to each point is searched in the initial NDT map. The distance error from the point to the plane is calculated. The Levenberg-Marquardt algorithm is used to minimize the sum of errors of all point cloud frames. The control points of the B-spline trajectory are iteratively updated until the average error from the point to the plane is less than a preset threshold or the number of iterations reaches the upper limit. The optimized high-precision point cloud map and LiDAR motion trajectory are output.

[0036] S3. Feature Extraction and Initial Rotation Estimation

[0037] S31. Continuous edge extraction of point cloud depth: In the optimized high-precision point cloud map, a neighborhood search radius of 0.3m is set. High feature value points are screened through PCA analysis. For each high feature value point, the point cloud data in its neighborhood is collected. The K-Means clustering algorithm is used to separate the neighborhood point cloud into different point clusters according to the plane to which it belongs. For each point cluster, the Random Sample Consensus (RANSAC) algorithm (maximum number of iterations 100, in-plane point distance threshold 0.03m) is used to fit the plane coefficients. The intersection of adjacent planes is calculated to generate candidate edge points. Statistical filtering (points with a distance of mean ± 2 standard deviations are removed) is used to remove unstable candidate points, resulting in a continuous edge feature set containing 3D position and edge direction vectors.

[0038] S32. Camera image edge feature extraction: Apply the L-CNN (Line-CNN) algorithm to the first frame image to extract structured edge features such as lane lines and building outlines, and filter out unstructured edges such as leaves and debris for subsequent coarse calibration. Apply the Canny edge detection algorithm (low threshold 50, high threshold 150) to all image frames to extract dense edge features with rich details for subsequent fine optimization.

[0039] S33. Initial relative rotation estimation: Select image pairs with sufficient parallax from the camera image sequence, calculate the relative rotation of the camera itself, and extract the relative rotation of the lidar with the same timestamp from the optimized B-spline trajectory;

[0040] Constructing epipolar constraint equations Therefore, there is Multiple sets of equations accumulate to form an overdetermined system of equations:

[0041] ;

[0042] in, The rotational component of the extrinsic parameters of the camera and lidar. and For the relative rotation of the camera and LiDAR to meet the threshold, and Defined as:

[0043] ;

[0044] in, Let be a skew-symmetric matrix of vectors, by... The left-hand matrix is ​​subjected to singular value decomposition, and the singular vector corresponding to the minimum singular value is used as the initial estimate of the extrinsic parameters. , , V represents the real components of natural numbers, and V is the imaginary vector.

[0045] S4, Quaternion Whale Optimized Coarse Calibration

[0046] S41. Initial Population Generation: Based on the initial rotation obtained in S33, apply a ±5° random rotation perturbation and a ±0.5m translation perturbation to generate 150-200 candidate extrinsic individuals, forming the initial candidate solution population for the quaternion whale optimization algorithm. Each "whale" in the algorithm represents a set of candidate extrinsic parameters, which are represented by a quaternion q representing rotation and a translation vector t. Specific steps include...

[0047] Rotation component initialization: Initial rotation estimated based on robust estimation in S33 Based on this, an initial rotating population is generated by applying small random three-dimensional angular and translational perturbations.

[0048] ;

[0049] in , That is, random small perturbations. for;

[0050] S42. Based on the encirclement mechanism and spiral update strategy of whale pod predation behavior, and combined with the characteristics of quaternion operations, independent update rules for rotation and translation components are designed. The specific steps are as follows:

[0051]

[0052] in This represents a random whale individual in the current generation. ,in , Let r represent the maximum number of iterations in the Q-WOA algorithm, where r is in the interval [0, 1]. random scalar on, It is in the interval Let b be a random scalar on the logarithmic spiral shape. Specifically, for the rotation and translation components, the above equation can be broken down into updates of the rotation and translation components:

[0053]

[0054]

[0055] Through these two independent update rules, the Q-WOA algorithm can efficiently and synchronously optimize rotation and translation components within a unified framework, thereby finding the globally optimal calibration result.

[0056] S43. Constructing the fitness evaluation function: Based on the alignment accuracy of multimodal features, a fitness evaluation function is constructed, specifically including the following steps: For the candidate extrinsic parameters represented by the i-th whale individual in the current generation... , and These are its rotation and translation components, respectively. The extracted point cloud edge feature set is projected onto the camera coordinate system through the current extrinsic candidate solution, and then mapped onto the pixel plane of the selected image frame using the camera intrinsic model to generate the projection point coordinates:

[0057] ;

[0058] in Edge features extracted by lidar Position components, for The direction vector, where t1 is the time. It is the actual camera projection function. , These represent the relative motion of the robot from time t0 to time t1 in the radar coordinate system. yes The corresponding rotation matrix is ​​used to search for the nearest L-CNN edge feature point to the projection point in the image frame, using the distance error term. Describes the spatial alignment deviation and calculates the edge direction vector. Consistent with the direction of the image edge normal vector, through vector dot product To evaluate geometric alignment, the sigmoid function is used to normalize and weight the distance error term and orientation consistency term, constructing a comprehensive fitness function:

[0059] ;

[0060] Among them, the projection position and direction The image position and normal vector are calculated from the current candidate solutions of the extrinsic parameters and are derived from L-CNN features;

[0061] Among them, based on the current candidate solutions for external parameters Edge features and the corresponding image features Calculate edge points Projection position and its direction vector The direction after projection Then compare with the corresponding pixels in the image. and its direction vector After matching, construct the fitness;

[0062] The normalized value of the distance error term increases with the increase of the projection deviation, and the normalized value of the orientation consistency term increases with the decrease of the vector angle. This function achieves a quantitative evaluation of the quality of candidate extrinsic solutions by simultaneously constraining spatial position deviation and geometric structure consistency. The lower the fitness value, the higher the multimodal feature alignment accuracy.

[0063] S44. To enhance population diversity and avoid premature convergence, a quaternion spherical interpolation crossover operation is performed on elite individuals, specifically including:

[0064] S441. Elite Selection: Sort the current generation of whales in ascending order of fitness value and select the top 30% of individuals as the elite population.

[0065] S442, Random Pairing: Randomly pair elite individuals into two groups, each group with a probability... Trigger crossover operation;

[0066] S443, Quaternion Spherical Interpolation: For paired individuals that trigger crossover. and Its rotational component generates a new solution through quaternion spherical linear interpolation (Slerp):

[0067] ;

[0068] in The angle between quaternions The coefficients are uniform random interpolation coefficients;

[0069] S444, Population Update: The newly generated solution... By replacing individuals with poor fitness in the original elite population, this mechanism explores potential optimization paths simultaneously in the unit quaternion hypersphere and Euclidean space through the probabilistic mixing of rotation and translation components among elite individuals, effectively improving global convergence capability.

[0070] S5, Continuous Time Point-to-Line Fine Optimization

[0071] S51. Residual Construction: Using the initial extrinsic parameters obtained from S4 optimization, the continuous edge features of the point cloud depth corresponding to each frame are projected onto its corresponding camera image coordinate system to obtain a projection point set. For each frame, the Euclidean distance from the points on the continuous edge of the projected point cloud depth to the detail-rich edge features extracted in that frame is calculated as the residual.

[0072] ;

[0073] in, Edge features extracted by lidar Position components, The output calibration results, For timestamps.

[0074] S52. Construct a global optimization objective function, use the Ceres Solver optimization library, and employ the Levenberg-Marquardt algorithm to solve for the optimal extrinsic parameters. Set the optimization termination condition to be that the residual change is less than 1e. -6 If the number of iterations exceeds 150, the final extrinsic parameter matrix with pixel-level precision will be output.

[0075] This invention is not limited to the embodiments described above. Any changes in shape or structure shall fall within the protection scope of this invention. The protection scope of this invention is defined by the appended claims. Those skilled in the art may make various changes or modifications to these embodiments without departing from the principles and essence of this invention, but all such changes and modifications shall fall within the protection scope of this invention.

Claims

1. A quaternion-based whale optimization calibration method for low-resolution lidar and camera, characterized in that, The method includes the following steps: S1. Data Acquisition: In a structured road environment, control the autonomous vehicle to drive along a preset "S" shaped trajectory, and simultaneously collect and store the point cloud data of low-resolution LiDAR and the image sequence data of the camera. S2. Point cloud map construction and trajectory optimization: Based on the collected lidar point cloud data, an initial point cloud map is constructed using the normal distribution transformation algorithm, and a high-precision point cloud map is generated using B-spline trajectory optimization. The process of constructing the initial point cloud map in S2 includes: The point cloud map is divided into voxels of adaptive size using a voxel grid. Within each voxel, the point cloud covariance matrix is ​​calculated using principal component analysis, and eigenvalues ​​are obtained through decomposition. and corresponding feature vectors ; Set feature threshold ,like If the voxel conforms to a planar structure, then the planar parameters are recorded. ,in It is the normal vector. The intercept is... The centroid of the plane; If a voxel does not meet the planar condition, it is divided into 8 smaller voxels, and S1 is repeated until the number of voxel divisions reaches the set threshold. S3. Feature Extraction and Initial Rotation Estimation: Extract depth-continuous edge features from high-precision point cloud maps, extract structured edge features from the first frame image of the camera, extract detailed edge features from each frame image, and robustly estimate the initial rotation based on the "S"-shaped trajectory motion epipolar constraint. The process of extracting depth continuous edge features in S3 includes: In a high-precision point cloud map, a neighborhood search radius is set, and high eigenvalue points are filtered out through principal component analysis. For each high eigenvalue point, a clustering algorithm is used to separate different planar point sets in the neighborhood point cloud. For each point set, a random sampling consensus algorithm is used to fit the plane. Calculate the intersection lines of adjacent planes to generate candidate edge points, and use statistical filtering to remove outliers to obtain the final point cloud edge feature set; S4. Quaternion Whale Optimization Coarse Calibration: Given the initial extrinsic parameter estimate, the continuous edge features of the point cloud depth are projected onto the camera image coordinate system. The quaternion whale optimization algorithm is applied to optimize the solution of the initial extrinsic parameter matrix by maximizing the spatial consistency between the projected features and the structured edge features. S4 specifically includes: S41: Based on the initial rotation, apply ±5° random rotation perturbation and ±0.5m translation perturbation to generate 150-200 candidate external parameter individuals, which constitute the initial population; S42: Based on the encirclement mechanism and spiral update strategy of whale pod predation behavior, the update of rotation components adopts a combination of quaternion multiplication and exponential mapping to ensure the closure and normality of rotation operations in three-dimensional space. S43: Constructing the fitness evaluation function: Constructing the fitness evaluation function based on the alignment accuracy of multimodal features, specifically including the following steps: For the candidate extrinsic parameters represented by the i-th whale individual in the current generation... , and These are its rotation and translation components, respectively. The extracted point cloud edge feature set is projected onto the camera coordinate system through the current extrinsic candidate solution, and then mapped onto the pixel plane of the selected image frame using the camera intrinsic model to generate the projection point coordinates: ； in Edge features extracted by lidar positional components for The direction vector, where t1 is the time. It is the actual camera projection function. , These represent the relative motion of the robot from time t0 to time t1 in the radar coordinate system. yes The corresponding rotation matrix is ​​used to search for the nearest L-CNN edge feature point to the projection point in the image frame, using the distance error term. Describes the spatial alignment deviation and calculates the edge direction vector. Consistent with the direction of the image edge normal vector, through vector dot product To evaluate geometric alignment, the sigmoid function is used to normalize and weight the distance error term and orientation consistency term, constructing a comprehensive fitness function: ； Among them, the projection position and direction The image position and normal vector are calculated from the current candidate solutions of the extrinsic parameters and are derived from L-CNN features; Among them, based on the current candidate solutions for external parameters Edge features and the corresponding image features Calculate edge points Projection position and its direction vector The direction after projection Then compare with the corresponding pixels in the image. and its direction vector After matching, construct the fitness; S44: The population is updated through an encirclement mechanism and a spiral update strategy. Every 10 iterations, the top 30% of elite individuals in fitness are subjected to a quaternion spherical interpolation crossover operation with a probability of 0.

3. S5. Continuous Time Point-to-Line Fine Optimization: Using the initial extrinsic parameters obtained from the optimization, the continuous edge features of the point cloud depth corresponding to each frame are projected onto the corresponding camera image coordinate system. The Euclidean distance from the projection point to the image detail edge features is calculated. The distance error function of all image frames is jointly optimized to obtain a high-precision extrinsic parameter matrix.

2. The quaternion-based whale optimization calibration method for low-resolution lidar and camera as described in claim 1, characterized in that: S1 specifically includes: S11. Trajectory Planning and Execution: Controlling autonomous vehicles to travel along a preset "S"-shaped trajectory in a structured road environment; S12. Data preprocessing: During driving, the low-resolution lidar and camera are synchronously acquired through the time synchronization module, generating and storing point cloud data streams and image sequences with timestamps. The low-resolution lidar is a mechanical lidar or a solid-state lidar with ≤32 beams, and the lidar point cloud data must include the time offset of each laser point relative to the start time of its respective scan frame for subsequent point cloud motion distortion correction.

3. The quaternion-based whale optimization calibration method for low-resolution lidar and camera as described in claim 1, characterized in that: The process of optimizing the B-spline trajectory parameters in S2 includes: Motion compensation for lidar point clouds is performed using the initial B-spline trajectory to eliminate point cloud distortion; The corrected point cloud is transformed to the world coordinate system, and the nearest neighbor plane is searched in the initial point cloud map and the distance error from the point to the plane is calculated. The Levenberg-Marquardt algorithm is used to minimize the sum of errors of all point cloud frames, and the B-spline trajectory control points are updated until the average point-to-plane error is less than a preset threshold or the number of iterations reaches the upper limit.

4. The quaternion-based whale optimization calibration method for low-resolution lidar and camera as described in claim 1, characterized in that: The structured edge features in S3 are extracted using the L-CNN algorithm, while the detailed edge features are extracted using the Canny edge detection algorithm.

5. The quaternion-based whale optimization calibration method for low-resolution lidar and camera as described in claim 4, characterized in that: The process of estimating the initial relative rotation in S3 includes: Select image pairs with sufficient parallax from the camera image sequence, calculate the relative rotation of the camera itself, and extract the relative rotation of the LiDAR with the same timestamp from the optimized B-spline trajectory; Constructing epipolar constraint equations Therefore, there is Multiple sets of equations accumulate to form an overdetermined system of equations: ； in, The rotational component of the extrinsic parameters of the camera and lidar. and For the relative rotation of the camera and LiDAR to meet the threshold, and Defined as: ； in, Let be a skew-symmetric matrix of vectors, by... The left-hand matrix is ​​subjected to singular value decomposition, and the singular vector corresponding to the minimum singular value is used as the initial estimate of the extrinsic parameters. , , V represents the real components of natural numbers, and V is the imaginary vector.

6. The quaternion-based whale optimization calibration method for low-resolution lidar and camera as described in claim 1, characterized in that: S5 specifically includes: S51: Project the edge features of the point cloud of each frame to the camera image coordinate system using the initial extrinsic parameters to obtain the projection point set. Fit the edge line segment set in the Canny edge of each frame image and calculate the Euclidean distance from the projection point to the nearest edge line segment as the residual. S52: Construct a global optimization objective function, use the Ceres Solver optimization library, and employ the Levenberg-Marquardt algorithm to solve for the optimal extrinsic parameters. The optimization termination condition is set to the residual change being less than 1e. -6 Or the number of iterations exceeds 150.

7. The quaternion-based whale optimization calibration method for low-resolution lidar and camera as described in claim 6, characterized in that: The Euclidean distance residual from the edge of the projected point cloud to the edge of the Canny is: ； in, Edge features extracted by lidar positional components The output calibration results, For timestamps.

Images