Calibration method based on laser radar and camera extrinsic parameters

Abstract

The invention relates to the technical field of sensor calibration, in particular to a calibration method based on laser radar and camera external parameters, and the method comprises the steps: obtaining laser observation data and camera observation data; constructing a plane constraint equation of a laser point on the two-dimensional code calibration plate through a conversion relation among a calibration plate coordinate system, a laser coordinate system and a camera coordinate system; constructing a linear equation set based on the plane constraint equation, and performing singular value decomposition and rotation matrix orthogonalization on the linear equation set to obtain an external parameter initial value; and optimizing the initial values of the external parameters by adopting a nonlinear least square method to obtain optimal estimated values of the external parameters, and completing calibration of the external parameters of the laser radar and the camera. According to the method, the angular point information of the camera is rapidly extracted by using the high contrast of the two-dimensional code and the regular pattern, and the precision and robustness of external parameter calibration are improved in cooperation with the laser radar data of the plane constraint.

Description

Technical Field

[0001] The present invention relates to the field of sensor calibration technology, and in particular to a calibration method based on laser radar and camera extrinsic parameters. Background Art

[0002] In recent years, with the development of intelligent technology, a single sensor cannot fully and accurately perceive environmental information, so multimodal sensor fusion technology has been widely used. In the fields of autonomous driving, robotics, and three-dimensional space reconstruction, the combination of LiDAR and cameras has shown significant advantages. LiDAR can provide accurate distance and depth information, while cameras can provide high-resolution color images and texture data. In order to achieve effective fusion of the two sensor data, the external parameters between LiDAR and camera, that is, their relative position and direction, must be accurately calibrated.

[0003] Existing calibration methods can generally be divided into two categories: one is a method based on geometric targets, which performs calibration through the geometric constraints of point clouds and targets. Common methods include using targets of checkerboard, ring or specific geometric shapes for calibration. However, the sparsity of point clouds and the accuracy of calibration targets often affect the accuracy of calibration; the other is a method based on image stereo reconstruction and point cloud matching, which performs calibration through feature point matching and iterative closest point (ICP) algorithm, but this method has high requirements on accuracy and has problems of data sparsity and noise influence. Therefore, the present invention proposes a calibration method based on lidar and camera extrinsic parameters. Summary of the invention

[0004] The purpose of the present invention is to solve the problems of error accumulation and interference factors in existing calibration methods, and to provide a calibration method based on lidar and camera extrinsic parameters. The high contrast and regular pattern of the two-dimensional code are used to quickly extract the corner point information of the camera, and the accuracy and robustness of the extrinsic parameter calibration are improved by combining the lidar data with plane constraints.

[0005] To achieve the above object, the present invention provides the following solutions:

[0006] The calibration method based on LiDAR and camera external parameters includes:

[0007] Acquire laser observation data and camera observation data;

[0008] Through the conversion relationship between the calibration plate coordinate system, laser coordinate system and camera coordinate system, the plane constraint equation of the laser point on the QR code calibration plate is constructed;

[0009] Constructing a linear equation system based on the plane constraint equation, and performing singular value decomposition and rotation matrix orthogonalization on the linear equation system to obtain initial values ​​of external parameters;

[0010] The nonlinear least squares method is used to optimize the initial values ​​of the extrinsic parameters, obtain the optimal estimated values ​​of the extrinsic parameters, and complete the calibration of the laser radar and camera extrinsic parameters.

[0011] Optionally, the conversion relationship between the calibration plate coordinate system, the laser coordinate system, and the camera coordinate system includes:

[0012] Set the parameter of the calibration plate plane in the camera coordinate system to π C =[n C , d]∈R 4 , where n C ∈R 3 , is the three-dimensional normal vector of the calibration plate plane, R 4 is a four-dimensional vector, R 3 is a three-dimensional vector, d C is the distance from the origin of the camera coordinate system to the calibration plate plane. The coordinates of a 3D point on the calibration plate plane in the camera coordinate system are P C , the three-dimensional points on the calibration plate plane satisfy in is the transposed matrix of the three-dimensional normal vector of the calibration plate plane.

[0013] Optionally, the plane constraint equation of the laser point on the two-dimensional code calibration plate is:

[0014]

[0015] Among them, R cl represents the rotation matrix between the laser coordinate system and the camera coordinate system, t cl Represents the translation matrix from the laser coordinate system to the camera coordinate system, P l Represents the laser point in the laser coordinate system.

[0016] Optionally, constructing a system of linear equations based on the plane constraint equation includes:

[0017] Using the normal vector and the position of the point in the camera coordinate system, the plane constraint equation of each laser point is constructed as a matrix A and a vector b;

[0018] All matrices A and vectors b are combined to construct the linear equation system.

[0019] Optionally, the matrix A is:

[0020]

[0021] The vector b is:

[0022]

[0023] The relationship between the matrix A and the vector b is:

[0024] AH = b;

[0025] in, are the coordinate values ​​of the three-dimensional normal vector of the calibration plate plane, are the three-dimensional coordinate values ​​of the laser point in the laser coordinate system, H is the rotation and translation information between the camera and the laser radar, and b is the constant term vector in the plane equation.

[0026] Optionally, performing singular value decomposition and rotation matrix orthogonalization on the linear equations to obtain initial values ​​of extrinsic parameters includes:

[0027] Calculate the least squares solution of the relationship between the matrix A and the vector b;

[0028] The least squares solution is optimized based on singular value decomposition and rotation matrix orthogonalization, and the initial value of the extrinsic parameter is extracted through the optimized least squares solution.

[0029] Optionally, the optimized least squares solution is:

[0030]

[0031] Among them, U A and V A is an orthogonal matrix, T is the transpose of the matrix, is the inverse matrix of the diagonal matrix, and b is the constant term vector in the plane equation.

[0032] Optionally, the method of optimizing the initial value of the external parameter using the nonlinear least squares method is:

[0033]

[0034] in, are the optimized rotation matrix from the laser coordinate system to the camera coordinate system and the translation matrix from the laser coordinate system to the camera coordinate system, R cl represents the rotation matrix between the laser coordinate system and the camera coordinate system, t cl Represents the translation matrix between the laser coordinate system and the camera coordinate system, is the transposed matrix of the three-dimensional normal vector of the calibration plate plane corresponding to the laser of the i-th frame, is the distance from the origin of the camera coordinate system corresponding to the i-th frame of laser to the calibration plate plane, N is the total number of laser frames, represents the mth laser point on the i-th frame laser, N i is the number of laser points on the laser in the i-th frame.

[0035] The beneficial effects of the present invention are:

[0036] The present invention proposes an extrinsic parameter calibration method based on a two-dimensional code calibration plate, which combines the advantages of lidar and camera, uses the high contrast and regular pattern of the two-dimensional code to quickly extract the corner point information of the camera, combines the plane constraint data of the lidar, and adopts optimization methods such as singular value decomposition and rotation matrix orthogonalization to improve the accuracy and robustness of multi-sensor fusion and extrinsic parameter calibration. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0038] Figure 1 It is a schematic diagram of the calibration structure of an embodiment of the present invention;

[0039] Figure 2 This is a flow chart of a calibration method based on laser radar and camera extrinsic parameters according to an embodiment of the present invention;

[0040] Figure 3 Schematic diagram of inter-frame calibration in four environments according to an embodiment of the present invention;

[0041] Figure 4 This is a visualization comparison effect diagram of the calibration results of an embodiment of the present invention;

[0042] Figure 5 Schematic diagram of point cloud data and plane fitting effect according to an embodiment of the present invention. DETAILED DESCRIPTION

[0043] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0044] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0045] The camera can calculate the plane of the calibration plate in the camera coordinate system through the QR code of the calibration plate plane. At the same time, the laser beam falls on the calibration plate plane. The coordinates of the laser point in the laser coordinate system and the coordinates of the plane equation in the camera coordinate system are used to construct the constraints of the point on the plane to solve the external parameters. The calibration structure is as follows: Figure 1 shown.

[0046] Based on the above calibration structure, this embodiment provides a calibration method based on laser radar and camera external parameters, such as Figure 2 As shown, including:

[0047] Acquire laser observation data and camera observation data;

[0048] Through the conversion relationship between the calibration plate coordinate system, laser coordinate system and camera coordinate system, the plane constraint equation of the laser point on the QR code calibration plate is constructed;

[0049] Constructing a linear equation system based on the plane constraint equation, and performing singular value decomposition and rotation matrix orthogonalization on the linear equation system to obtain initial values ​​of external parameters;

[0050] The nonlinear least squares method is used to optimize the initial values ​​of the extrinsic parameters, obtain the optimal estimated values ​​of the extrinsic parameters, and complete the calibration of the laser radar and camera extrinsic parameters.

[0051] Specifically, this embodiment combines the advantages of lidar and camera, utilizes the high contrast and regular pattern of the QR code to quickly extract the corner point information of the camera, combines the plane constraint data of the lidar, and adopts optimization methods such as singular value decomposition and rotation matrix orthogonalization to improve the accuracy and robustness of multi-sensor fusion and external parameter calibration.

[0052] Furthermore, in this embodiment, the plane constraint equation of the laser point on the two-dimensional code calibration plate is constructed through the conversion relationship between the calibration plate coordinate system, the laser coordinate system, and the camera coordinate system, including:

[0053] Define the parameter of the QR code calibration plate plane in the camera coordinate system C as π C =[n C ,d]∈R 4 , where n C ∈R 3 is the 3D normal vector of the plane, and d is the distance from the origin of the camera coordinate system to the plane. The coordinates of a 3D point on the plane in the camera coordinate system are P C , the points on the plane satisfy:

[0054]

[0055] Assume that the rotation and translation between the laser coordinate system L and the camera coordinate system is R cl , t cl If we know a laser point P in the laser coordinate system l If it falls on the calibration plate, the constraint that the point is on the plane can be used to construct equations about the external parameters:

[0056]

[0057] Through multiple such constraints, a linear equation system can be constructed to solve the external parameters. Since the nonlinear least squares problem is sensitive to the initial value, after using the closed-form solution to provide the initial value, the calibration accuracy can be improved by optimizing using multiple frames of data.

[0058] Furthermore, constructing a linear equation system based on the plane constraint equation includes:

[0059] Using the normal vector and the position of the point in the camera coordinate system, the plane constraint equation of each laser point is constructed as a matrix A and a vector b;

[0060] All matrices A and vectors b are combined to construct the linear equation system.

[0061] Further, performing singular value decomposition and rotation matrix orthogonalization on the linear equations to obtain initial values ​​of extrinsic parameters includes:

[0062] Calculate the least squares solution of the relationship between the matrix A and the vector b;

[0063] The least squares solution is optimized based on singular value decomposition and rotation matrix orthogonalization, and the initial value of the extrinsic parameter is extracted through the optimized least squares solution.

[0064] Specifically, in this embodiment, solving the initial value of the extrinsic parameter includes:

[0065] For each laser point P on the plane l , we can use the normal vector and the position of the point in the camera coordinate system to construct the matrix A and vector b:

[0066]

[0067] The constraints of all laser points are combined to construct a system of linear equations.

[0068] For solving the least squares problem, this embodiment uses SVD decomposition instead of LDLT decomposition to directly solve the linear equation, which is more robust to numerical instability and small singular values. Because singular value decomposition is used, a wider range of matrix situations can be handled, especially when A T When the condition number of A is very poor. The formula for the least squares solution is:

[0069] H=(A T A) -1 A T b (4)

[0070] Formula (4) is used to calculate H that minimizes ‖AH-b‖2. The formula using SVD decomposition is as follows:

[0071]

[0072] Here A and V A is an orthogonal matrix, ∑ A is a diagonal matrix containing singular values. Through SVD decomposition, the least squares solution can be rewritten as:

[0073]

[0074] in Only take the inverse of the non-zero singular values. Extract h1, h2, and h3 from the solved H as the basis columns of the rotation matrix. Calculate the third column of the rotation matrix h3 = h1 × h2 based on h1 and h2, and you can get R cl and t cl :

[0075] R cl =[h1 h2 h1×h2] T (7)

[0076] t cl =-[h1 h2 h1×h2] T h3 (8)

[0077] The formula for SVD decomposition is as follows:

[0078]

[0079] Here R and V R is an orthogonal matrix, ∑ R is a diagonal matrix containing singular values. In this embodiment, an orthogonalization step is introduced so that Ensure the orthogonality of the rotation matrix.

[0080] Add a rotation matrix orthogonalization step to ensure that the rotation matrix has orthogonality and a determinant of 1. The rotation matrix must satisfy the condition that the column or row vectors are orthogonal to each other and the norm is 1. If the directly calculated matrix does not meet this condition, it will cause rotation errors or system instability. Orthogonalization corrects numerical errors to ensure the stability and reliability of the results. During the calibration process, matrix operations may accumulate errors. Orthogonalization effectively corrects these deviations and ensures that the rotation matrix maintains a valid rigid transformation. This step greatly improves the accuracy of the calibration or optimization process and ensures that the results meet the physical constraints of rigid body motion.

[0081] Further, the initial value of the extrinsic parameter is optimized by using a nonlinear least square method, and obtaining the best estimated value of the extrinsic parameter includes:

[0082] In order to solve the problem of extrinsic parameter calibration of the camera and the laser, the accuracy of the initial value is crucial. After calculating the initial value of the extrinsic parameter, it can be used as the starting point for optimization, and the nonlinear least squares method can be used to jointly optimize multiple sets of data to further improve the calibration accuracy. After calculating the initial value of the extrinsic parameter in the above way, the N sets of collected data are jointly optimized to obtain the optimal estimate. Assuming that the laser in the i-th frame has N i The laser points fall on the calibration plate, and the plane equation of the calibration plate corresponding to the i-th frame laser is expressed in the camera coordinate system as The expected value formula of external parameter optimization is:

[0083]

[0084] N is the number of laser points. Through the Ceres optimization library, a loss function can be constructed that integrates the errors of all laser points relative to the calibration plane. The optimization process continuously adjusts the rotation matrix and translation vector to minimize these errors, ultimately obtaining a more accurate estimate of the extrinsic parameters.

[0085] Experimental verification:

[0086] This method is compared with the algorithm of Kuangshi Research Institute and He Bo algorithm in the experimental scenes of light, light + wind, natural light, and natural light + wind. The schematic diagram of inter-frame calibration under the four environments is shown in Figure 3 . It can be seen that in four environments, the QR code in each frame can be detected by slowly moving according to the yaw angle, roll angle, pitch angle and forward angle. Kalibr can efficiently use the AprilTag QR code calibration plate to calibrate the external parameters of the camera and lidar. Even when the viewing angle changes, the AprilTag QR code can still be accurately recognized. The tool has a high degree of automation and can automatically recognize QR codes, collect data and optimize external parameters. By globally optimizing multiple frames of data, Kalibr significantly improves the calibration accuracy and reduces dependence on manual intervention. In addition, it supports the joint optimization of camera and LiDAR, effectively solving the problem of temporal and spatial alignment, thereby enhancing the accuracy and stability of fusion calibration.

[0087] Figure 4 This method is compared visually with the algorithm of Kuangshi Research Institute and Hebo algorithm in four data sets in terms of laser hitting the calibration board and detecting the position of the calibration board. It can be seen that all three algorithms can hit the laser on the calibration board and identify the position of the calibration board. Under the influence of brightness and wind, the laser line hitting the calibration board of Kuangshi and Hebo algorithms appears tilted. This method can hit a straight line on the calibration board without tilt in all four environments, and is not affected by brightness and wind.

[0088] This method was calibrated with the Kuangshi Research Institute algorithm and the Hebo algorithm on four sets of data sets, and the chi-square residual value of each algorithm on different data sets was used as the basis for evaluating the accuracy of the algorithm. The calibration results are compared in Table 1.

[0089] Table 1

[0090]

[0091] As shown in Table 1, only in the "natural light" scene did the Kuangshi algorithm fail to converge. This method converged in all four environments. The changes in the initial cost and final cost showed that the three algorithms effectively reduced the cost during the optimization process. The three algorithms differed in the number of iterations and calculation time required for convergence. This method required fewer iterations and took less time to calculate. The chi-square residual value reflects the fitting effect. This method showed lower chi-square residual values ​​in the four scenes, showing a better optimization effect.

[0092] According to the chi-square residual values ​​in Table 1, the performance improvement percentage of this method compared with other algorithms can be calculated. The smaller the chi-square residual value, the better the calibration effect. In natural light scenes, this method improves by about 10.26% compared with the Kuangshi algorithm. In natural light + wind scenes, it improves by about 20.71%. In lighting scenes, it improves by about 29.05%. In lighting + wind scenes, it improves by about 12.75%. In general, this method shows significant improvement in all environments, especially in lighting environments.

[0093] The three algorithms read the laser point cloud data in four environments to draw the point cloud, and draw the plane corresponding to the generated plane parameters. A tolerance range is used to define "on the plane" to filter out the points on the plane. The point cloud data obtained by running the three algorithms in four environments and the fitting effect of the plane in three-dimensional space can be seen in Figure 5 . It can be seen that the fitting effects of the Kuangshi algorithm and the Hebo algorithm fluctuate in different environments, especially in the case of wind interference (natural light + wind, light + wind), showing a more obvious weakening of fitting; while this method can concentrate the point cloud data in the four environments and show a stable fitting effect. Under natural light conditions, the point cloud data of the three algorithms perform well, concentrated and parallel to the plane; under natural light + wind conditions, the point cloud data of the Kuangshi algorithm and the Hebo algorithm begin to disperse, while this method still maintains a high degree of fit. Under lighting conditions, the point cloud data of the three algorithms are still relatively concentrated; but under lighting + wind conditions, the point cloud data of the Kuangshi and Hebo algorithms fluctuate more, and the dispersion increases significantly, while this method still shows the best stability.

[0094] It can be seen that this method maintains high stability in all four environments, making the point cloud data continuously concentrated. However, the stability of the Kuangshi and Hebo algorithms decreases in windy conditions, causing the point cloud to scatter and reduce the fit with the plane.

[0095] This method uses a two-dimensional code calibration plate as a calibration target and combines it with the plane constraint data provided by the laser radar to accurately calculate the external parameters between the camera and the laser radar. The two-dimensional code calibration plate can effectively provide high contrast and regular patterns, simplify the feature point extraction process, and ensure the stability and high accuracy of the calibration system. In order to solve the shortcomings of traditional algorithms in dealing with numerical instability, singular value decomposition (SVD) is used instead of LDLT decomposition to enhance the robustness of the algorithm to singular matrices and bad condition numbers. In addition, by introducing the orthogonalization step of the rotation matrix, the orthogonality and determinant of the rotation matrix are ensured to be 1, which solves the errors and instability that may occur in the matrix calculation process. The initial value of the external parameter is solved by a linear equation system, and the nonlinear least squares optimization is performed in combination with multi-frame data, thereby improving the calibration accuracy. This optimization method can effectively eliminate the influence of the external environment (such as light and wind interference) on the calibration results, and improve the calibration accuracy and robustness. Through comparative experiments, it is proved that this method can significantly improve the centralization of point cloud data, and has stronger robustness and anti-interference ability than existing methods. This method significantly improves the accuracy of external parameter calibration of lidar and camera, and exhibits excellent robustness and adaptability under various environmental conditions, and has broad application prospects.

[0096] The embodiments described above are only descriptions of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.

Claims

1. The calibration method based on laser radar and camera extrinsic parameters is characterized by: include: Acquire laser observation data and camera observation data; Through the conversion relationship between the calibration plate coordinate system, laser coordinate system and camera coordinate system, the plane constraint equation of the laser point on the QR code calibration plate is constructed; Constructing a linear equation system based on the plane constraint equation, and performing singular value decomposition and rotation matrix orthogonalization on the linear equation system to obtain initial values ​​of external parameters; The nonlinear least squares method is used to optimize the initial values ​​of the extrinsic parameters, obtain the optimal estimated values ​​of the extrinsic parameters, and complete the calibration of the laser radar and camera extrinsic parameters.

2. The calibration method based on laser radar and camera extrinsic parameters according to claim 1, characterized in that: The conversion relationship between the calibration plate coordinate system, the laser coordinate system, and the camera coordinate system includes: Set the parameter of the calibration plate plane in the camera coordinate system to π C =[n C ,d C ]∈R 4 , where n C ∈R 3 , is the three-dimensional normal vector of the calibration plate plane, R 4 is a four-dimensional vector, R 3 is a three-dimensional vector, d C is the distance from the origin of the camera coordinate system to the calibration plate plane. The coordinates of a 3D point on the calibration plate plane in the camera coordinate system are P C , the three-dimensional points on the calibration plate plane satisfy in is the transposed matrix of the three-dimensional normal vector of the calibration plate plane.

3. The calibration method based on laser radar and camera extrinsic parameters according to claim 2, characterized in that: The plane constraint equation of the laser point on the two-dimensional code calibration plate is: Among them, R cl represents the rotation matrix between the laser coordinate system and the camera coordinate system, t cl Represents the translation matrix from the laser coordinate system to the camera coordinate system, P l Represents the laser point in the laser coordinate system.

4. The calibration method based on laser radar and camera extrinsic parameters according to claim 1, characterized in that: Constructing a linear equation system based on the plane constraint equation includes: Using the normal vector and the position of the point in the camera coordinate system, the plane constraint equation of each laser point is constructed as a matrix A and a vector b; All matrices A and vectors b are combined to construct the linear equation system.

5. The calibration method based on laser radar and camera extrinsic parameters according to claim 4 is characterized in that: The matrix A is: The vector b is: The relationship between the matrix A and the vector b is: AH=b; in, are the coordinate values ​​of the three-dimensional normal vector of the calibration plate plane, are the three-dimensional coordinate values ​​of the laser point in the laser coordinate system, H is the rotation and translation information between the camera and the laser radar, and b is the constant term vector in the plane equation.

6. The calibration method based on laser radar and camera extrinsic parameters according to claim 5, characterized in that: Performing singular value decomposition and rotation matrix orthogonalization on the linear equations to obtain initial values ​​of external parameters includes: Calculate the least squares solution of the relationship between the matrix A and the vector b; The least squares solution is optimized based on singular value decomposition and rotation matrix orthogonalization, and the initial value of the extrinsic parameter is extracted through the optimized least squares solution.

7. The calibration method based on laser radar and camera extrinsic parameters according to claim 6, characterized in that: The optimized least squares solution is: Among them, U A and V A is an orthogonal matrix, T is the transpose of the matrix, is the inverse matrix of the diagonal matrix, and b is the constant term vector in the plane equation.

8. The calibration method based on laser radar and camera extrinsic parameters according to claim 7, characterized in that: The method of optimizing the initial value of the external parameter by using the nonlinear least squares method is: in, are the optimized rotation matrix from the laser coordinate system to the camera coordinate system and the translation matrix from the laser coordinate system to the camera coordinate system, R cl represents the rotation matrix between the laser coordinate system and the camera coordinate system, t cl Represents the translation matrix between the laser coordinate system and the camera coordinate system, is the transposed matrix of the three-dimensional normal vector of the calibration plate plane corresponding to the laser of the i-th frame, is the distance from the origin of the camera coordinate system corresponding to the i-th frame of laser to the calibration plate plane, N is the total number of laser frames, represents the mth laser point on the i-th frame laser, N i is the number of laser points on the laser in the i-th frame.

Images