A robust lidar-camera self-calibration method

Abstract

The application discloses a kind of robust laser radar-camera joint self-calibration method.First, the data collected by laser radar is preprocessed to obtain dense point cloud map and reflection intensity map, geometric edge is extracted from point cloud map, and intensity edge is extracted from reflection intensity map;Then, pixel edge line is extracted from camera image.Through fast initial value search to calibration external parameter, the initial registration of two kinds of edges under the co-view area of laser radar and camera is realized.The points on laser radar edge line are projected to camera image, and by minimizing the point-line distance residual of radar edge point to image edge line, the external parameter between laser radar and camera is further optimized.The application realizes joint external parameter self-calibration by making full use of the correspondence of laser radar geometric and intensity edge and camera image edge, which is independent of calibration board, has good scene adaptability, high calibration accuracy and good reliability, and provides a basis for laser radar and camera fusion automatic driving technology.

Description

Technical Field

[0001] This invention relates to a LiDAR-camera self-calibration method in the field of multi-sensor fusion for intelligent vehicles, and particularly to a robust LiDAR-camera self-calibration method. Background Technology

[0002] With the rapid development of autonomous driving, multi-sensor fusion technology has become a major trend. LiDAR, capable of capturing 3D information in the environment in real time and measuring the spatial distance between objects and the LiDAR sensor, boasts high accuracy and a wide field of view, making it one of the mainstream sensors in current autonomous driving perception modules. Cameras, capable of capturing color information in the environment and obtaining rich and dense 2D environmental information within their field of view, are an indispensable component of current autonomous driving perception modules. The current challenge in autonomous driving perception technology lies in improving accuracy and robustness under various complex scenarios. The intuitive 3D information perception capability of LiDAR and the powerful 2D environmental perception capability of cameras complement each other well, making autonomous driving technology that integrates LiDAR and cameras a current research hotspot.

[0003] Calibration methods for LiDAR and cameras require determining the translational and rotational extrinsic parameters between them. As the most fundamental module of multi-sensor fusion technology, calibration methods need to possess high calibration accuracy and robustness. Current mainstream algorithms require data collection using artificially placed calibration boards (such as checkerboards) in specific scenarios to achieve a certain level of calibration effectiveness. However, in complex indoor environments or outdoor road conditions, without calibration boards, they cannot effectively complete the calibration task, leading to errors in subsequent multi-sensor fusion. Therefore, how to achieve high-precision online self-calibration of LiDAR and cameras in real-world natural scenarios without the need for calibration boards is an urgent problem to be solved. Summary of the Invention

[0004] To address the problems existing in the background technology, the purpose of this invention is to provide a robust lidar-camera joint calibration method based on scene edge features, applicable to ordinary indoor and outdoor road environments.

[0005] This invention introduces spatial edge features and reflection intensity edge features from the LiDAR point cloud during the calibration stage. When the spatial edge features of the LiDAR are weak, reflection intensity edge features are used as a supplement, which effectively improves feature acquisition in scenes with weak 3D structure and enhances the robustness of calibration. In spatial edge feature extraction, an adaptive grid side length strategy is adopted to preserve the integrity and richness of the fitting plane while improving feature extraction speed. Coarse calibration is used to obtain the initial LiDAR and camera extrinsic parameter matrices, effectively reducing the time-consuming nature of direct optimization or the possibility of non-convergence. During the optimization stage, 3D distance weights are introduced to reduce the impact of large matching pair errors at distant locations on calibration. Furthermore, in the later stages of iterative optimization, mismatched pairs with large residuals are removed, reducing the probability of incorrect optimization direction, thus enabling convergence to the optimal solution in various scenarios. The overall method exhibits high robustness, laying the foundation for improving the environmental perception performance of LiDAR-camera fusion.

[0006] The steps of the technical solution adopted in this invention are as follows:

[0007] 1) After preprocessing the data collected by the camera and LiDAR respectively, the image edge features and LiDAR edge features are obtained respectively;

[0008] 2) Based on image edge features and lidar edge features, the ICP registration error of the two types of edges is used to perform initial iterative optimization registration of the extrinsic parameter matrix between the camera and lidar, so as to obtain the initial calibration extrinsic parameter matrix with the minimum registration error.

[0009] 3) After projecting the edge features of the LiDAR onto the camera image coordinate system using the initial calibration extrinsic parameter matrix, the 2D LiDAR edge features are obtained. Based on the LiDAR edge features and the 2D LiDAR edge features, the extrinsic parameter matrix is ​​iteratively optimized and registered again to align the LiDAR edges with the image edges, thereby obtaining the final extrinsic parameter matrix.

[0010] Step 1) specifically refers to:

[0011] 1.1) Generate a reflection intensity map based on the point cloud data collected by the lidar, extract the edges on the reflection intensity map and recover the 3D coordinates of each edge point in the reflection intensity map, so as to serve as the edge features of lidar reflection intensity;

[0012] 1.2) Accumulate the point cloud data collected by the lidar to obtain a dense point cloud map;

[0013] 1.3) Divide the dense point cloud map into 3D grids, and adaptively adjust the grid side length by fitting a plane within each grid to obtain different planes. Then, calculate the boundary line between different planes and use it as the spatial edge. Statistically calculate the 3D coordinates of the spatial edge and use it as the spatial edge feature of the lidar.

[0014] 1.4) After aggregating the edge features of lidar reflection intensity and lidar spatial edge features, the lidar edge features are obtained;

[0015] 1.5) Fit the 2D edges of the camera image and denot them as image edge features.

[0016] The process of extracting edges from the reflection intensity map and recovering the 3D coordinates of each edge point in the reflection intensity map, thereby using them as edge features of the lidar reflection intensity, specifically involves:

[0017] First, a mask map covering the lidar point cloud is generated based on the lidar field of view and the image size of the reflection intensity map;

[0018] Next, the fill rate of the reflection intensity map within the mask is calculated. If the fill rate is less than the preset fill rate, the width W and height H of the reflection intensity map are reduced. Then, holes are filled in the pixels in the reduced reflection intensity map that are within the mask. The edges of the hole-filled reflection intensity map are then extracted and used as the reflection intensity edges. The 3D coordinates of the points on the reflection intensity edges are restored and used as the reflection intensity edge features of the LiDAR.

[0019] Specifically, 1.3) refers to:

[0020] 1.3.1) Set the initial grid side length and perform 3D voxel grid division on the dense point cloud map;

[0021] 1.3.2) Use the Random Sample Consensus (RANSAC) fitting method to fit the point cloud within each voxel grid to obtain a fitting plane. Calculate the proportion of points in the current fitting plane to the point cloud within the current grid and record it as the point proportion of the current fitting plane. If the point proportion of the current fitting plane is greater than or equal to the first threshold, then execute 1.3.3); otherwise, execute 1.3.4.

[0022] 1.3.3) Record the points occupied by the current fitted plane as the same plane and remove it from the current voxel grid. Then, continuously use the Random Sample Consensus (RANSAC) fitting method to fit the remaining point cloud in the current voxel grid to obtain different planes and remove the points corresponding to the fitted planes until the remaining point cloud in the current voxel grid is less than the second threshold, and obtain all the planes in the current voxel grid.

[0023] 1.3.4) Record the points occupied by the current fitted plane as the same plane and remove it from the current voxel grid. Then, divide the current voxel grid into equal parts. Based on the remaining point cloud in the current voxel grid, perform plane fitting on the point cloud in each sub-grid. If the proportion of points of the fitted plane in a sub-grid is greater than or equal to the first threshold, then execute 1.3.3). Otherwise, continue to divide the current sub-grid into equal parts until the threshold of the number of divisions is reached. Then execute 1.3.3) on all sub-grids to obtain all planes in the current voxel grid.

[0024] 1.3.5) Traverse the remaining voxel grids, repeating 1.3.2)-1.3.4) until all planes in all voxel grids are obtained;

[0025] 1.3.6) Calculate the angle between any two planes in each voxel grid. If the angle is within the preset angle range, fit the intersecting line segment of the two planes and use it as the edge in space. Traverse and calculate all planes to obtain all edges in space. Statistically calculate the 3D coordinates of all edges in space and use them as the spatial edge features of the LiDAR.

[0026] Specifically, 1.4) refers to:

[0027] In the lidar reflection intensity edge features, if there are lidar spatial edge feature points around each lidar reflection intensity edge feature point, then the lidar reflection intensity edge feature point is deleted, and the remaining lidar reflection intensity edge features together with all lidar spatial edge features form the lidar edge features.

[0028] Specifically, 2) refers to:

[0029] 2.1) Set the initial extrinsic parameter matrix, search radius, and search step size, and use one of the six parameters of the initial extrinsic parameter matrix as the initial search degree of freedom;

[0030] 2.2) Add the forward and reverse search step sizes to the current search degrees of freedom in the current extrinsic parameter matrix, keeping the other five parameters in the current extrinsic parameter matrix unchanged, to obtain a new extrinsic parameter matrix, which will be used as the extrinsic parameter matrix for the next search. Based on the new extrinsic parameter matrix, project the LiDAR edge features onto the 2D pixel coordinate system to obtain the 2D LiDAR edge features. If the pixel coordinates do not exceed the camera image range, calculate the ICP registration error between the 2D LiDAR edge features and the image edge features and record it as the ICP registration error corresponding to the new extrinsic parameter matrix, and then execute 2.3); otherwise, directly execute 2.3).

[0031] 2.3) Based on the current extrinsic matrix, repeat step 2.2) to search for the current degree of freedom until the search radius is reached. Obtain multiple extrinsic matrices under the current degree of freedom and the corresponding ICP registration errors. Take the degree of freedom in the extrinsic matrix with the smallest ICP registration error as the optimal degree of freedom and fix it.

[0032] 2.4) After changing the parameters in the extrinsic parameter matrix, new search degrees of freedom are obtained. Repeat 2.2)-2.3) to obtain the optimal search degrees of freedom corresponding to different search degrees of freedom, thereby obtaining the optimal extrinsic parameter matrix under the current search radius and search step size and using it as the initial extrinsic parameter matrix for the next search.

[0033] 2.5) Reduce the search radius and search step size. Based on the current initial extrinsic matrix, repeat steps 2.2)-2.4) until the search radius and search step size are reduced to the preset value or the preset number of reductions is reached. Obtain the final optimal extrinsic matrix and record it as the initial calibration extrinsic matrix.

[0034] Specifically, 3) refers to:

[0035] 3.1) Use the initial calibration extrinsic matrix as the initial extrinsic matrix;

[0036] 3.2) Project the edge features of the LiDAR onto the 2D pixel coordinate system using the current initial extrinsic parameter matrix to obtain the 2D LiDAR edge features; then convert the image edge features into a 2D image edge feature point cloud;

[0037] 3.3) In the 2D image edge feature point cloud, use K-nearest neighbors to find the i-th 2D lidar edge feature. The k nearest 2D image edge features within a preset pixel distance are used to construct an image edge line, and the midpoint of the image edge line is calculated. And covariance, perform eigenvalue decomposition on the covariance, and find the linear eigenvector corresponding to the largest eigenvalue. Let be the direction vector of the image edge line;

[0038] 3.4) Based on the direction vector of the image edge line and the edge feature L of the i-th 2D LiDAR edgi and the midpoint of the image edge line The distance to construct the point-to-line distance residual The edge features of the i-th lidar and the midpoint of the corresponding image edge line Form the i-th feature matching pair;

[0039] 3.5) Repeat steps 3.3)-3.4) to iterate and calculate the feature matching pairs and point-to-line distance residuals corresponding to the remaining lidar edge features. Optimize the current initial extrinsic matrix based on all feature matching pairs and the corresponding point-to-line distance residuals to minimize the residuals between all feature matching pairs. Obtain the current optimal extrinsic matrix and use it as the initial extrinsic matrix for the next iteration.

[0040] 3.6) Based on the current initial extrinsic parameter matrix, repeat steps 3.2)-3.6) to perform multiple iterative optimizations of the extrinsic parameter matrix to obtain the final optimal extrinsic parameter matrix.

[0041] The point-to-line distance residual The formula is as follows:

[0042]

[0043] In section 3.5), the formula for the current optimal extrinsic parameter matrix z is as follows:

[0044]

[0045]

[0046] Where b is the number of matching pairs for all LiDAR and cameras; s i w represents the adjustment coefficient for the i-th matching pair. i d represents the distance weight of the i-th matching pair. th Represents the residual threshold, ||| 2 The expression represents the squaring operation of the numerical L2 norm, and || represents the absolute value operation.

[0047] In section 3.6), during the multiple iterative optimization of the extrinsic parameter matrix, after every several iterations, the residual threshold d... th It can be gradually reduced.

[0048] Compared with the prior art, the beneficial effects of the present invention are:

[0049] (1) Without the need for a specific calibration object, it can achieve online adaptive self-calibration of the external parameters between the lidar and the camera, which improves the long-term reliability of the multi-sensor fusion task performance in autonomous vehicles.

[0050] (2) The present invention extracts spatial edge and reflection intensity edge features simultaneously for calibration, thereby improving the stability of feature extraction and the robustness of calibration;

[0051] (3) In spatial edge extraction, an adaptive grid size is used to fit the planes within the grid from large to small, which improves the accuracy and speed of spatial edge feature extraction; by projecting a spherical coordinate 2D reflection intensity image into the point cloud, the reflection intensity edge map is extracted, which improves the feature extraction efficiency.

[0052] (4) In the iterative optimization stage, the present invention introduces distance weights. For the point-to-line distance residuals constructed by matching pairs that are far from the lidar and camera, there are large errors. By setting a small weight, the optimization stability is improved. At the same time, mismatched pairs with excessive residuals are eliminated to avoid guiding the optimization in the wrong direction, accelerate the convergence of iterative optimization, and ultimately improve the accuracy of the calibration method. Attached Figure Description

[0053] Figure 1 This is a flowchart of the method of the present invention;

[0054] Figure 2 This is a lidar reflection intensity diagram of an embodiment;

[0055] Figure 3 This is an edge map of the laser radar reflection intensity in an embodiment;

[0056] Figure 4 These are the spatial edge features of the lidar in the embodiment;

[0057] Figure 5 This is the projection result of the edge features of the LiDAR in the indoor scene of the embodiment;

[0058] Figure 6 This is the projection result of the edge features of the lidar in the outdoor scene of the embodiment;

[0059] Figure 7 These are the lidar point cloud projection images before and after indoor scene calibration in the embodiment.

[0060] Figure 8 This is a LiDAR point cloud projection image before and after outdoor scene calibration in an embodiment. Detailed Implementation

[0061] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0062] The calibration method of the present invention will be described more clearly by an example below.

[0063] The embodiments and implementation process of the complete method according to the invention are as follows:

[0064] like Figure 1 As shown, the present invention includes the following steps:

[0065] 1) After preprocessing the data collected by the camera and lidar respectively, image edge features and lidar edge features are obtained respectively. The lidar edge features include lidar spatial edge features and lidar reflection intensity edge features.

[0066] Step 1) specifically involves:

[0067] 1.1) Generate a reflection intensity map based on the point cloud data collected by the lidar, extract the edges on the reflection intensity map and recover the 3D coordinates of each edge point in the reflection intensity map, so as to serve as the edge features of lidar reflection intensity;

[0068] In section 1.1), a reflection intensity map is generated based on the point cloud data collected by the lidar. The reflection intensity map is as follows: Figure 2 As shown, specifically:

[0069] Specifically, the point cloud data collected by the lidar is first transformed from the Cartesian coordinate system to the spherical coordinate system to obtain the spherical coordinates. The coordinate transformation calculation formula is as follows:

[0070]

[0071]

[0072]

[0073] Where x, y, z are the 3D coordinates of each point in the point cloud in the Cartesian coordinate system, and r, θ, φ are the 3D coordinates of each point in the spherical coordinate system;

[0074] By combining spherical coordinates with the projection of the LiDAR point cloud onto a 2D plane, a reflection intensity map is obtained. The formula for calculating the coordinates of each pixel in the reflection intensity map is as follows:

[0075]

[0076] Where u and v are the two coordinate values ​​of a pixel, W and H are the width and height of the reflectance map, respectively, and fov W and fov H These are the horizontal and vertical field of view angles of the lidar. In this embodiment, the horizontal and vertical field of view angles of the lidar used are: fov W =70.4 and fov H =77.2.

[0077] After extracting edges from the reflection intensity map and recovering the 3D coordinates of each edge point in the reflection intensity map, these are used as the edge features of the lidar reflection intensity. Specifically:

[0078] First, a mask map covering the lidar point cloud is generated based on the known lidar field of view range and the preset image size of the reflection intensity map. In this embodiment, the horizontal and vertical field of view of the Livox Avia lidar are 70.4° and 77.2° respectively (its scanning coverage area is an ellipse), and the width and height of the reflection intensity image are set to W (1400) and H (1400) respectively.

[0079] Next, the fill rate of the reflection intensity map within the mask image is calculated. If the fill rate is less than a preset fill rate (e.g., 80%), the width W and height H of the reflection intensity map are reduced. Then, holes are filled in the pixels of the reduced reflection intensity map that are within the mask image. The canny edges of the hole-filled reflection intensity map are then extracted and used as the reflection intensity edges. The 3D coordinates of the points on the reflection intensity edges are recovered and used as the LiDAR reflection intensity edge features. In practice, areas within the reflection intensity map that are not covered by LiDAR points (i.e., holes) will not be added to the reflection intensity edge features, even if they are edge features, because these points are created by hole filling and do not have corresponding actual 3D coordinates (x, y, z). The reflection intensity edge map is as follows: Figure 3 As shown.

[0080] 1.2) Accumulate the point cloud collected by the stationary LiDAR to obtain a dense point cloud map; due to the non-repetitive scanning characteristics of the LivoxAvia LiDAR, the point cloud of multiple frames of LiDAR can be superimposed by stationary LiDAR to obtain a dense point cloud map of the environment within its scanning range.

[0081] 1.3) Divide the dense point cloud map into 3D grids, and adaptively adjust the grid side length by fitting a plane within each grid to obtain different planes. Then, calculate the boundary line between different planes and use it as the spatial edge. Statistically calculate the 3D coordinates of the spatial edge and use it as the spatial edge feature of the lidar.

[0082] 1.3) Specifically:

[0083] 1.3.1) Set an initial relatively large and fixed grid side length (e.g., 3m) and perform 3D voxel grid division on the dense point cloud map;

[0084] 1.3.2) Use the Random Sample Consensus (RANSAC) fitting method to fit the point cloud within each voxel grid to obtain a fitting plane. Calculate the proportion of points in the current fitting plane to the point cloud within the current grid and record it as the point proportion of the current fitting plane. If the point proportion of the current fitting plane is greater than or equal to the first threshold, then execute 1.3.3); otherwise, execute 1.3.4).

[0085] 1.3.3) Record the points occupied by the current fitted plane as the same plane and remove it from the current voxel grid. Then, continuously use the Random Sample Consensus (RANSAC) fitting method to fit the remaining point cloud in the current voxel grid to obtain different planes and remove the points corresponding to the fitted planes until the remaining point cloud in the current voxel grid is less than the second threshold, and obtain all the planes in the current voxel grid.

[0086] 1.3.4) Explain that there are multiple planes in the current voxel. Record the points occupied by the currently fitted plane as the same plane and remove it from the current voxel. Then, divide the current voxel equally, for example, reduce the side length of the grid to half of the original, that is, divide the current grid space into 8 sub-grids. Based on the remaining point cloud in the current voxel, use the Random Sample Consensus (RANSAC) fitting method to fit the point cloud in each sub-grid. If the proportion of points of the fitted plane in a sub-grid is greater than or equal to the first threshold, then execute 1.3.3). Otherwise, continue to divide the current sub-grid equally until the threshold of the number of divisions is reached. In this embodiment, the division is performed a maximum of 3 times, that is, the sub-grid division process is performed a maximum of 3 times. Then execute 1.3.3) on all sub-grids to obtain all planes in the current voxel.

[0087] 1.3.5) Traverse the remaining voxel grids, repeating 1.3.2)-1.3.4) until all planes in all voxel grids are obtained;

[0088] 1.3.6) Calculate the angle between any two planes in each voxel grid. If the angle is within a preset angle range, fit the intersecting line segment of the two planes and use it as the spatial edge. Iterate through all planes to obtain all spatial edges, and statistically analyze the 3D coordinates of all spatial edges as the spatial edge features of the LiDAR. In this embodiment, the preset angle range is set to be greater than 30°. In the fitted planar structure, larger planes are fitted in larger grids, and smaller planes are fitted in smaller grids. The corresponding extracted edge features are as follows: Figure 4 As shown.

[0089] 1.4) After aggregating the edge features of lidar reflection intensity and lidar spatial edge features, the lidar edge features are obtained;

[0090] 1.4) Specifically:

[0091] In the lidar reflection intensity edge features, if there are lidar spatial edge feature points around each lidar reflection intensity edge feature point, then the lidar reflection intensity edge feature point is deleted. That is, all spatial edge features and some reflection intensity edge features are preserved as supplements. The remaining lidar reflection intensity edge features are combined with all lidar spatial edge features to form lidar edge features, and the attributes of each feature point (i.e., whether it is a reflection intensity edge or a spatial edge) are preserved.

[0092] 1.5) Use the Canny operator to fit and obtain the 2D edges of the camera image, and denote them as image edge features. Its coordinates are

[0093] 2) Based on image edge features and LiDAR edge features, the ICP (Iterative Closest Point) registration error of the two types of edges is used to perform initial iterative optimization registration of the extrinsic parameter matrix between the camera and the LiDAR, so as to obtain the initial calibration extrinsic parameter matrix with the smallest registration error;

[0094] 2) Specifically:

[0095] 2.1) Set the initial extrinsic parameter matrix, search radius (R), and search step size (angle step δr and translation step δt). Use one of the six parameters of the initial extrinsic parameter matrix as the initial search degrees of freedom. In practice, the default extrinsic parameter matrix is ​​selected as the initial extrinsic parameter matrix. The default extrinsic parameter matrix is ​​the extrinsic parameter matrix installed with the radar xyz orientation at the front left and the camera xyz orientation at the lower right. Alternatively, if a coarse extrinsic parameter matrix exists, it can also be used as the initial extrinsic parameter matrix. The initial extrinsic parameter has a large error, as shown in the projected edge map. Figure 5 and Figure 6 As shown, the thinnest black solid line dots represent camera edge features, the medium-thickness gray solid line dots represent lidar reflection intensity edge features, and the thickest black solid line dots represent lidar spatial edge features.

[0096] 2.2) Add the forward search step size and the reverse search step size to the current search degrees of freedom in the current extrinsic matrix, while keeping the other five parameters in the current extrinsic matrix unchanged, to obtain a new extrinsic matrix and use it as the extrinsic matrix for the next search;

[0097] extrinsic parameter matrix It is a 4×4 matrix, divided into rotation matrices. Translation matrix The decomposition formula is shown below:

[0098]

[0099] During the calculation, to avoid homogenizing the coordinates, the 3D coordinates are rotated and translated sequentially along the edges. The transformation formula is as follows: P c The coordinates of the 3D point of the LiDAR projected onto the camera coordinate system.

[0100] Calculate the rotation matrix in the extrinsic parameter matrix, specifically the matrix R that rotates about the xyz rotation axes. x R y R z The definition is as follows:

[0101]

[0102]

[0103]

[0104] The extrinsic parameter matrix between the lidar and the camera is:

[0105] Transform the LiDAR 3D coordinates to the camera coordinate system:

[0106] Image normalization projection is performed on the coordinates of the converted 3D points at the edge of the lidar:

[0107] Using pre-calibrated camera intrinsic parameters (f) x ,f y ,c x ,c y Distortion correction is performed on k1, k2, k3, p1, p2.

[0108]

[0109]

[0110] Where, x o y o Let r be the normalized xy coordinates in the z-direction. o coordinates (x) o ,y o The distance from the origin, x C ,y C ,z C Let f be the coordinates of the 3D point projection from the lidar to the camera coordinate system, tx, ty, tz be the translation components of the extrinsic parameters between the camera and the lidar, and f be the intrinsic parameters of the pinhole camera. x ,f y ,c x ,c yThe sequence consists of k1, k2, k3, p1, p2, which needs to be pre-calibrated, where f x ,f y c is the camera focal length. x ,c y Let x be the center coordinates of the camera origin in the pixel coordinate system, k1, k2, k3, p1, p2 be the first to fifth distortion parameters of the pinhole camera, and x be the center coordinates of the camera origin in the pixel coordinate system. distorted and y distorted These are the camera coordinates after distortion correction.

[0111] Finally, the coordinates L of the 3D coordinates of the LiDAR edge points in the camera pixel coordinate system can be obtained. edge .

[0112] Based on a new extrinsic parameter matrix, the edge features of the LiDAR are projected onto a 2D pixel coordinate system to obtain the 2D edge features of the LiDAR. Its coordinates are If its pixel coordinates do not exceed the camera image range, then the ICP registration error between the 2D LiDAR edge features and the image edge features is calculated and recorded as the ICP registration error corresponding to the new extrinsic parameter matrix. For camera edge point C... edge Construct a 2D kd-tree (k-dimensional tree) for each LiDAR edge point. Find the nearest camera edge point Calculate the pixel distance residual d between two points ij ,satisfy If d ij <d th The current matching pair is considered valid, its residual is retained, and all LiDAR edge points L are accumulated. edge The linear velocity distance residual is used as the ICP registration error. Then execute 2.3); otherwise, skip this search and directly execute 2.3).

[0113] 2.3) Based on the current extrinsic matrix, repeat step 2.2) to search for the current degree of freedom until the search radius is reached. Obtain multiple extrinsic matrices under the current degree of freedom and the corresponding ICP registration errors. Take the degree of freedom in the extrinsic matrix with the smallest ICP registration error as the optimal degree of freedom and fix it.

[0114] 2.4) After changing the parameters in the extrinsic parameter matrix, new search degrees of freedom are obtained. Repeat steps 2.2)-2.3) to obtain the optimal search degrees of freedom corresponding to different search degrees of freedom, thereby obtaining the optimal extrinsic parameter matrix under the current search radius and search step size, which is used as the initial extrinsic parameter matrix for the next search. In this embodiment, the search is performed sequentially in the order of [roll, pitch, yaw, tx, ty, tz]. For rotation angles roll (roll angle around the x-axis), pitch (pitch angle around the y-axis), and yaw (yaw angle around the z-axis), positive and negative step sizes are added to one of the rotation angles each time, for example: roll = roll ± δr; the same applies to translation tx (translation in the x-direction), ty (translation in the y-direction), and tz (translation in the z-direction), for example: tx = tx ± δt. Following the order [roll, pitch, yaw, tx, ty, tz], the forward and reverse step sizes are added sequentially multiple times, searching only one degree of freedom each time, while the values ​​of the other 5 degrees of freedom remain fixed; until the search radius is reached, for example, the result of the last step size addition for tz is tz=tz±R×δt.

[0115] 2.5) Reduce the search radius and search step size. Based on the current initial extrinsic parameter matrix, repeat steps 2.2)-2.4) until the search radius and search step size are reduced to the preset value or the preset number of reductions is reached. Obtain the final optimal extrinsic parameter matrix and record it as the initial calibration extrinsic parameter matrix. In this embodiment, three rounds of search are performed. In the first round, the angle is 0-360° with a step size of 45 degrees and the translation is 0-2m with a step size of 0.5m. In the second round, the angle is 0-90° with a step size of 15 degrees and the translation is 0-0.5m with a step size of 0.1m. In the third round, the angle is 0-15° with a step size of 3 degrees and the translation is 0-0.1m with a step size of 0.02m.

[0116] 3) After projecting the edge features of the LiDAR onto the camera image coordinate system using the initial calibration extrinsic matrix, the 2D LiDAR edge features are obtained. Based on the LiDAR edge features and the 2D LiDAR edge features, a point-to-line pixel distance residual is constructed. The extrinsic matrix of the point-to-line pixel distance residual is iteratively optimized and registered again to align the LiDAR edge with the image edge, thereby obtaining the final extrinsic matrix.

[0117] 3) Specifically:

[0118] 3.1) Use the initial calibration extrinsic matrix as the initial extrinsic matrix;

[0119] 3.2) Project the edge features of the LiDAR onto the 2D pixel coordinate system using the current initial extrinsic parameter matrix to obtain the 2D LiDAR edge features; then convert the image edge features into a 2D image edge feature point cloud. Specifically, use the PCL (Point Cloud Library) point cloud processing library to construct a 2D kd-tree for the 2D image edge feature point cloud to find neighboring points using the K nearest neighbors method.

[0120] 3.3) In the 2D image edge feature point cloud, use K-nearest neighbors to find the i-th 2D lidar edge feature. The k nearest 2D image edge features within a preset pixel distance are selected. In this embodiment, k = 5, requiring at least 5 image feature points. An image edge line is formed by these k 2D image edge features, and the midpoint of the image edge line is calculated. And covariance, midpoint The coordinates are Perform eigenvalue decomposition on the covariance and extract the linear eigenvector corresponding to the largest eigenvalue. Let be the direction vector of the image edge line, where the line feature vector is denoted as . The coordinates are

[0121] 3.4) Based on the direction vector of the image edge line and the edge features of the i-th 2D LiDAR and the midpoint of the image edge line The distance to construct the point-to-line distance residual The edge features of the i-th lidar and the midpoint of the corresponding image edge line Form the i-th feature matching pair, whose attributes are set to

[0122] Point-to-line distance residual The formula is as follows:

[0123]

[0124] 3.5) Repeat steps 3.3)-3.4) to iterate and calculate the feature matching pairs and point-to-line distance residuals corresponding to the remaining lidar edge features. Optimize the current initial extrinsic matrix based on all feature matching pairs and the corresponding point-to-line distance residuals to minimize the residuals between all feature matching pairs. Obtain the current optimal extrinsic matrix and use it as the initial extrinsic matrix for the next iteration.

[0125] In 3.5), the formula for the current optimal extrinsic parameter matrix x is as follows:

[0126]

[0127]

[0128] Where x = [roll, pitch, yaw, tx, ty, tz], n is the number of matching pairs for all LiDARs and cameras; s i w represents the adjustment coefficient for the i-th matching pair. i d represents the distance weight of the i-th matching pair. th Represents the residual threshold, ||| 2 || represents the square of the numerical L2 norm, and || represents the absolute value operation.

[0129] Because the physical size of each pixel in a camera increases as the distance from the sensor increases—for example, an object of the same size will occupy a larger pixel size in the image when placed closer to the camera, but may only occupy a few pixels when placed further away—matching pairs that are far from the camera and LiDAR have a larger inherent error. This invention addresses this by using edge feature points of the LiDAR. Calculate distance weight w i Weighting the point-to-line distance residuals ensures that the residual weights of more distant matching pairs are smaller. Simultaneously, it's necessary to eliminate mismatches with excessively large residuals. When projecting LiDAR edge feature points onto the image pixel coordinate system, if the distance between the LiDAR edge point and the nearest camera edge point is significant, it indicates that the LiDAR edge features and camera edge features may not represent the same point in physical space. This signifies a mismatch, specifically manifested in the point-to-line distance residuals. Greater than the threshold d th The initial threshold is set to d. th =25, this invention adjusts the coefficient s i accomplish.

[0130] 3.6) Based on the current initial extrinsic parameter matrix, repeat steps 3.2)-3.6) to perform multiple iterative optimizations of the extrinsic parameter matrix to obtain the final optimal extrinsic parameter matrix.

[0131] In section 3.6), during the multiple iterative optimization of the extrinsic parameter matrix, to avoid large-error matching pairs affecting the optimization direction, the residual threshold d is set after every few iterations. th It can be gradually reduced. In this embodiment, d is optimized every 2 iterations. th =d th -1.

[0132] Using the calibrated extrinsic parameters, the edge of the LiDAR is projected onto the image, such as... Figure 5 The right image and Figure 6 As shown in the right image, the two types of edges almost completely overlap. Projecting the point cloud onto the image as follows... Figure 7 and Figure 8 As shown, the lidar point cloud and the image are basically overlapping.

[0133] Figure 5 and Figure 6 The projection images of the feature edges before and after calibration are shown, corresponding to indoor scenes respectively. Figure 5 ) and outdoor scenes ( Figure 6 The image below visualizes the radar edge projected onto the image. The thinnest black solid line represents the camera edge features, the medium-thickness gray solid line represents the lidar reflection intensity edge features, and the thickest black solid line represents the lidar spatial edge features. The left image shows the effect before calibration, where there is some overlap error between the two types of edges. The right image shows the effect after calibration using this method, where the two types of edges almost completely overlap.

[0134] Figure 7 and Figure 8 The projection images of the LiDAR point cloud before and after calibration are shown. Uncalibrated sequences in the scene are selected, and the calibration results are used to represent the corresponding indoor scenes. Figure 7 ) and outdoor scenes ( Figure 8 The image below shows a visualization of radar points projected onto the image. The left image shows the result before calibration, and the right image shows the result after calibration using this method. Before calibration, the point cloud and the image did not largely overlap, and errors still existed in some areas, for example... Figure 7 The middle checkerboard area, and Figure 8 In the window area on the right, the window frame of the point cloud and the window frame of the image do not overlap; after calibration using this method, the LiDAR points and the image almost completely overlap, qualitatively demonstrating the accuracy of the calibration effect of this method.

[0135] The effectiveness of this method was verified by collecting data from a complex indoor environment. The sensors used included a Livox Avia series LiDAR and a HIKVERSION series camera, both rigidly connected and fixed to the same platform. Data was collected for approximately one minute by holding the platform rigidly and allowing it to remain stationary. The data was stored in rosbag format for offline calibration. Both the LiDAR and camera data were collected at a frequency of 10Hz, with time-synchronized acquisition. The entire dataset includes one indoor scene and one outdoor scene, both collected from the same handheld platform. The effectiveness of the method was evaluated by calibrating the two datasets and calculating the mean and standard deviation of the calibration results.

[0136] Table 1 lists the calibration results of this method under the complex indoor and outdoor environment datasets. Each column of the table represents the rotation and translation components of the calibration extrinsic parameter matrix. The calibration extrinsic parameters of this method are statistically analyzed under two complex environment datasets (indoor and outdoor), with results presented as mean ± standard deviation. Since the true values ​​of the calibration extrinsic parameters between the LiDAR and the camera are difficult to obtain, the standard deviation of calibration results across multiple scenes is typically used as an evaluation criterion to indirectly reflect the accuracy and robustness of the calibration method. The table shows that the translation calibration accuracy of this method can reach the centimeter level, and the rotation angle calibration accuracy is less than 1 degree.

[0137] Table 1 shows the self-calibration results of the method of the present invention in indoor and outdoor environments.

[0138] indoor -0.001±0.006 0.069±0.009 -0.07±0.004 90.21±0.03 -0.52±0.05 89.57±0.12 outdoor -0.009±0.014 0.048±0.024 -0.09±0.03 89.82±0.21 -0.63±0.11 89.06±0.08 average -0.007±0.011 0.059±0.013 -0.82±0.04 90.01±0.25 -0.58±0.10 89.3±0.31

[0139] Finally, it should be noted that the above embodiments and descriptions are only used to illustrate the technical solutions of the present invention and not to limit it. Those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the disclosure of the technical solutions of the present invention, and all such modifications and substitutions should be covered within the protection scope of the claims of the present invention.

Claims

1. A robust lidar-camera joint self-calibration method, characterized in that, The steps include the following: 1) After preprocessing the data collected by the camera and LiDAR respectively, the image edge features and LiDAR edge features are obtained respectively; Step 1) includes: 1.1) Generate a reflection intensity map based on the point cloud data collected by the lidar, extract the edges on the reflection intensity map and recover the 3D coordinates of each edge point in the reflection intensity map, so as to serve as the edge features of lidar reflection intensity; 1.2) Accumulate the point cloud data collected by lidar to obtain a dense point cloud map; 1.3) Divide the dense point cloud map into 3D grids, and adaptively adjust the grid side length by fitting a plane within each grid to obtain different planes. Then, calculate the boundary line between different planes and use it as the edge in space. Statistically calculate the 3D coordinates of the edge in space and use it as the spatial edge feature of the lidar. 1.4) After aggregating the edge features of lidar reflection intensity and lidar spatial edge features, the lidar edge features are obtained; Specifically, 1.4) refers to: In the lidar reflection intensity edge features, if there are lidar spatial edge feature points around each lidar reflection intensity edge feature point, then the lidar reflection intensity edge feature point is deleted, and the remaining lidar reflection intensity edge features together with all lidar spatial edge features form the lidar edge features. 2) Based on image edge features and lidar edge features, the ICP registration error of the two types of edges is used to perform initial iterative optimization registration of the extrinsic parameter matrix between the camera and lidar, so as to obtain the initial calibration extrinsic parameter matrix with the minimum registration error; 3) After projecting the edge features of the LiDAR onto the camera image coordinate system using the initial calibration extrinsic parameter matrix, the 2D LiDAR edge features are obtained; the extrinsic parameter matrix is ​​iteratively optimized and registered again based on the LiDAR edge features and the 2D LiDAR edge features, so that the LiDAR edge and the image edge are aligned, thereby obtaining the final extrinsic parameter matrix; Specifically, 3) refers to: 3.1) Use the initial calibration extrinsic matrix as the initial extrinsic matrix; 3.2) Project the edge features of the LiDAR onto the 2D pixel coordinate system using the current initial extrinsic parameter matrix to obtain the 2D LiDAR edge features; then convert the image edge features into a 2D image edge feature point cloud; 3.3) In the 2D image edge feature point cloud, use K-nearest neighbors to find the i-th 2D lidar edge feature. The k nearest 2D image edge features within a preset pixel distance are used to construct an image edge line, and the midpoint of the image edge line is calculated. And covariance, perform eigenvalue decomposition on the covariance, and find the linear eigenvector corresponding to the largest eigenvalue. Let be the direction vector of the image edge line; 3.4) Based on the direction vector of the image edge line and the edge features of the i-th 2D LiDAR and the midpoint of the image edge line The distance to construct the point-to-line distance residual The midpoint of the edge features of the i-th lidar and the corresponding image edge line Form the i-th feature matching pair; 3.5) Repeat steps 3.3)-3.4) to iterate and calculate the feature matching pairs and point-to-line distance residuals corresponding to the remaining lidar edge features. Optimize the current initial extrinsic matrix based on all feature matching pairs and the corresponding point-to-line distance residuals to minimize the residuals between all feature matching pairs. Obtain the current optimal extrinsic matrix and use it as the initial extrinsic matrix for the next iteration. 3.6) Based on the current initial extrinsic parameter matrix, repeat steps 3.2)-3.6) to perform multiple iterative optimizations of the extrinsic parameter matrix to obtain the final optimal extrinsic parameter matrix.

2. The robust lidar-camera joint self-calibration method according to claim 1, characterized in that, Step 1) further includes: 1.5) Fit the 2D edges of the camera image and denot them as image edge features.

3. The robust lidar-camera joint self-calibration method according to claim 1, characterized in that, The process of extracting edges from the reflection intensity map and recovering the 3D coordinates of each edge point in the reflection intensity map, thereby using them as edge features of the lidar reflection intensity, specifically involves: First, a mask map covering the lidar point cloud is generated based on the lidar field of view and the image size of the reflection intensity map; Next, the fill rate of the reflection intensity map within the mask image is calculated. If the fill rate is less than a preset fill rate, the width of the reflection intensity map is reduced. and height Then, holes are filled in the pixels in the reduced reflection intensity map that are within the mask image. The edges of the hole-filled reflection intensity map are then extracted and used as the reflection intensity edges. The 3D coordinates of the points on the reflection intensity edges are restored and used as the reflection intensity edge features of the LiDAR.

4. A robust lidar-camera joint self-calibration method according to claim 1, characterized in that, Specifically, 1.3) refers to: 1.3.1) Set the initial grid side length and perform 3D voxel grid division on the dense point cloud map; 1.3.2) Use the Random Sample Consensus (RANSAC) fitting method to fit the point cloud within each voxel grid to obtain a fitting plane. Calculate the percentage of points in the current fitting plane relative to the point cloud within the current grid and record this as the percentage of points in the current fitting plane. If the percentage of points in the current fitting plane is greater than or equal to a first threshold, proceed to step 1.3.3; otherwise, proceed to step 1.3.

4. 1.3.3) Record the points occupied by the current fitted plane as the same plane and remove it from the current voxel grid. Then, continuously use the Random Sample Consensus (RANSAC) fitting method to fit the remaining point cloud in the current voxel grid to obtain different planes and remove the points corresponding to the fitted planes until the remaining point cloud in the current voxel grid is less than the second threshold, and obtain all the planes in the current voxel grid. 1.3.4) Record the points occupied by the current fitted plane as the same plane and remove it from the current voxel grid. Then, divide the current voxel grid into equal parts. Based on the remaining point cloud in the current voxel grid, perform plane fitting on the point cloud in each sub-grid. If the proportion of points of the fitted plane in a sub-grid is greater than or equal to the first threshold, then execute 1.3.3). Otherwise, continue to divide the current sub-grid into equal parts until the threshold of the number of divisions is reached. Then execute 1.3.3) on all sub-grids to obtain all planes in the current voxel grid. 1.3.5) Traverse the remaining voxel grids, repeating steps 1.3.2)-1.3.4) until all planes in all voxel grids are obtained; 1.3.6) Calculate the angle between any two planes in each voxel grid. If the angle is within the preset angle range, fit the intersecting line segment of the two planes and use it as the edge in space. Traverse and calculate all planes to obtain all edges in space. Statistically calculate the 3D coordinates of all edges in space and use them as the spatial edge features of the LiDAR.

5. A robust lidar-camera joint self-calibration method according to claim 1, characterized in that, Specifically, 2) refers to: 2.1) Set the initial extrinsic parameter matrix, search radius, and search step size, and use one of the six parameters of the initial extrinsic parameter matrix as the initial search degree of freedom; 2.2) Add the forward and reverse search step sizes to the current search degrees of freedom in the current extrinsic parameter matrix, keeping the other five parameters in the current extrinsic parameter matrix unchanged, to obtain a new extrinsic parameter matrix, which will be used as the extrinsic parameter matrix for the next search. Based on the new extrinsic parameter matrix, project the LiDAR edge features onto the 2D pixel coordinate system to obtain the 2D LiDAR edge features. If the pixel coordinates do not exceed the camera image range, calculate the ICP registration error between the 2D LiDAR edge features and the image edge features and record it as the ICP registration error corresponding to the new extrinsic parameter matrix, then execute 2.3); otherwise, directly execute 2.3). 2.3) Based on the current extrinsic matrix, repeat step 2.2) to search for the current search degrees of freedom until the search radius is reached. Obtain multiple extrinsic matrices under the current search degrees of freedom and the corresponding ICP registration errors. Take the search degrees of freedom in the extrinsic matrix with the smallest ICP registration error as the optimal search degrees of freedom and fix them. 2.4) After changing the parameters in the extrinsic parameter matrix, obtain the new search degrees of freedom. Repeat steps 2.2)-2.3) to obtain the optimal search degrees of freedom corresponding to different search degrees of freedom, thereby obtaining the optimal extrinsic parameter matrix under the current search radius and search step size and using it as the initial extrinsic parameter matrix for the next search. 2.5) Reduce the search radius and search step size. Based on the current initial extrinsic matrix, repeat steps 2.2)-2.4) until the search radius and search step size are reduced to the preset value or the preset number of reductions are reached. Obtain the final optimal extrinsic matrix and record it as the initial calibration extrinsic matrix.

6. A robust lidar-camera joint self-calibration method according to claim 1, characterized in that, The point-to-line distance residual The formula is as follows: 。 7. A robust lidar-camera joint self-calibration method according to claim 1, characterized in that, In section 3.5), the current optimal extrinsic parameter matrix The formula is as follows: in, The number of matching pairs for all LiDAR and cameras; Indicates the first The adjustment coefficient for the matched pair Indicates the first For the distance weight of the matching pair, Indicates the residual threshold. The square operation represents the numerical L2 norm. This indicates the absolute value operation.

8. A robust lidar-camera joint self-calibration method according to claim 1, characterized in that, In section 3.6), during the multiple iterative optimization of the extrinsic parameter matrix, after each series of iterations, the residual threshold is... It can be gradually reduced.

Images